From 288d6e925e68b6a6bee2a5a0026173086e53b626 Mon Sep 17 00:00:00 2001 From: Francesco Romano Date: Mon, 25 Sep 2017 12:10:57 +0200 Subject: [PATCH 01/89] Migrated old DH conversion code from embedded to iDynTree --- toolbox/CMakeLists.txt | 3 +- toolbox/src/InverseKinematics.cpp | 149 ++++++------------------------ 2 files changed, 27 insertions(+), 125 deletions(-) diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index e648f87f6..52c60000f 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -40,8 +40,7 @@ add_install_rpath_support(BIN_DIRS ${CMAKE_INSTALL_PREFIX}/bin find_package(Eigen3 REQUIRED) find_package(wholeBodyInterface 0.2.5 REQUIRED) find_package(yarpWholeBodyInterface 0.3.2 REQUIRED) -# Workaround section, aka, this stuff should be defined by imported libraries -find_package(iDynTree REQUIRED) +find_package(iDynTree 0.7.2 REQUIRED) #On new versions of Clang, MATLAB requires C++11. #I enable it on all Clangs diff --git a/toolbox/src/InverseKinematics.cpp b/toolbox/src/InverseKinematics.cpp index 1ad048a52..43c45a7e0 100644 --- a/toolbox/src/InverseKinematics.cpp +++ b/toolbox/src/InverseKinematics.cpp @@ -14,20 +14,12 @@ #include #include -//These includes are to convert from URDF to DH -#include -#include -#include -#include -#include -#include -#include -//End of temporary include section +#include +#include +#include namespace wbt { - static bool iKinLimbFromUrdfFile(const std::string &urdf_file_name, const std::string &base_link_name, const std::string& end_effector_link_name, iCub::iKin::iKinLimb &convertedChain, std::vector& jointNames); - struct InverseKinematics::InverseKinematicsPimpl { bool m_firstTime; //input buffers @@ -204,11 +196,32 @@ namespace wbt { if (!m_piml->m_leg) return false; std::vector jointNames; - if (!iKinLimbFromUrdfFile(urdfFile, baseLink, endEffectorLink, *((iCub::iKin::iKinLimb*)m_piml->m_leg), jointNames)) { - if (error) error->message = "Cannot convert urdf to iKin chain"; + iDynTree::ModelLoader loader; + loader.loadModelFromFile(urdfFile); + if (!loader.isValid()) { + if (error) error->message = "Cannot load urdf file"; + return false; + } + + iDynTree::DHChain dhChain; + if (!iDynTree::ExtractDHChainFromModel(loader.model(), + baseLink, + endEffectorLink, + dhChain)) { + if (error) error->message = "Cannot extract DH parameters from model"; + return false; + } + if (!iDynTree::iKinLimbFromDHChain(dhChain, *((iCub::iKin::iKinLimb*)m_piml->m_leg))) { + if (error) error->message = "Cannot convert DH parameters to iKin chain"; return false; } + // Retrieve joint names + jointNames.reserve(dhChain.getNrOfDOFs()); + for (size_t i = 0; i < dhChain.getNrOfDOFs(); ++i) { + jointNames.push_back(dhChain.getDOFName(i)); + } + m_piml->m_solver = new iCub::iKin::iKinIpOptMin(*m_piml->m_leg, IKINCTRL_POSE_XYZ, 1e-3, 1e-6, 100); if (!m_piml->m_solver) return false; @@ -329,114 +342,4 @@ namespace wbt { return false; } - - bool iKinLimbFromUrdfFile(const std::string &urdf_file_name, const std::string &base_link_name, const std::string& end_effector_link_name, iCub::iKin::iKinLimb &convertedChain, std::vector& jointNames) { - - KDL::Tree kdl_tree; - KDL::Chain kdl_chain; - std::vector joint_names; - KDL::JntArray min,max; - - // - // URDF --> KDL::Tree - // - bool root_inertia_workaround = true; - if (!iDynTree::treeFromUrdfFile(urdf_file_name,kdl_tree,root_inertia_workaround)) - { - std::cerr << "Could not parse urdf robot model" << std::endl; - std::cerr << "Please open an issue at https://github.com/robotology-playground/idyntree/issues " << std::endl; - - return false; - } - - // - // URDF --> position ranges - // - if (!iDynTree::jointPosLimitsFromUrdfFile(urdf_file_name,joint_names,min,max)) - { - std::cerr << "Could not parse urdf robot model limits" << std::endl; - return false; - } - - if (joint_names.size() != min.rows() || - joint_names.size() != max.rows() || - joint_names.size() == 0) - { - std::cerr << "Inconsistent joint limits got from urdf (nr of joints extracted: " << joint_names.size() << " ) " << std::endl; - return false; - } - - // - // KDL::Tree --> KDL::CoDyCo::UndirectedTree - // (for extracting arbitrary chains, - // using KDL::Tree you can just get chains where the base of the chain - // is proximal to the tree base with respect to the end effector. - // - KDL::CoDyCo::UndirectedTree undirected_tree(kdl_tree); - - KDL::Tree kdl_rotated_tree = undirected_tree.getTree(base_link_name); - - bool result = kdl_rotated_tree.getChain(base_link_name,end_effector_link_name,kdl_chain); - if (!result) - { - std::cerr << "Impossible to find " << base_link_name << " or " - << end_effector_link_name << " in the URDF." << std::endl; - return false; - } - - // - // Copy the limits extracted from the URDF to the chain - // - int nj = kdl_chain.getNrOfJoints(); - KDL::JntArray chain_min(nj), chain_max(nj); - - jointNames.clear(); - jointNames.reserve(nj); - - size_t seg_i, jnt_i; - for (seg_i = 0,jnt_i = 0; seg_i < kdl_chain.getNrOfSegments(); seg_i++) - { - const KDL::Segment & seg = kdl_chain.getSegment(seg_i); - if( seg.getJoint().getType() != KDL::Joint::None ) - { - std::string jnt_name = seg.getJoint().getName(); - // std::cerr << "searching for joint " << jnt_name << std::endl; - int tree_jnt = 0; - for(tree_jnt = 0; tree_jnt < joint_names.size(); tree_jnt++ ) - { - //std::cerr << "joint_names[ " << tree_jnt << "] is " << joint_names[tree_jnt] << std::endl; - if( joint_names[tree_jnt] == jnt_name ) - { - chain_min(jnt_i) = min(tree_jnt); - chain_max(jnt_i) = max(tree_jnt); - jnt_i++; - jointNames.push_back(jnt_name); - break; - } - } - if( tree_jnt == joint_names.size() ) - { - std::cerr << "Failure in converting limits from tree to chain, unable to find joint " << jnt_name << std::endl; - return false; - } - } - } - - if (jnt_i != nj) - { - std::cerr << "Failure in converting limits from tree to chain" << std::endl; - return false; - } - - // - // Convert the chain and the limits to an iKin chain (i.e. DH parameters) - // - result = iDynTree::iKinLimbFromKDLChain(kdl_chain,convertedChain,chain_min,chain_max); - if (!result) - { - std::cerr << "Could not export KDL::Tree to iKinChain" << std::endl; - return false; - } - return true; - } } From aa70f727dc7cd15db3f52f4d1eae4e16324edfbc Mon Sep 17 00:00:00 2001 From: Francesco Romano Date: Mon, 25 Sep 2017 17:38:09 +0200 Subject: [PATCH 02/89] Fixed bug introduced with toolbox refactoring SetReferences subList parameter was erroneously converted to a boolean, while it is actually the index of the selected item in the Simulink mask dropdown. Being the index >=1 the boolean conversion resulted always in a true value. Fix https://github.com/robotology/WB-Toolbox/issues/57 --- toolbox/src/SetReferences.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 8a8f4464e..4f3e4c97d 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -33,7 +33,10 @@ namespace wbt { unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_fullControl = blockInfo->getScalarParameterAtIndex(WBIBlock::numberOfParameters() + 2).booleanData(); + // In Toolbox mask the options are the following: + // - 1 => full control + // - 2 => sublist control + m_fullControl = blockInfo->getScalarParameterAtIndex(WBIBlock::numberOfParameters() + 2).int32Data() == 1; if (!m_fullControl) { //sublist From 6716aaac2de5dea93ac48e3dc3bfed4f07e9287f Mon Sep 17 00:00:00 2001 From: Francesco Romano Date: Tue, 24 Oct 2017 17:12:26 +0200 Subject: [PATCH 03/89] Temporary fix: included iostream Note: actually the std::cerr are warnings for the user, this is why they are not returned as an error. We have to find a smarter way to return the warning to the user. --- toolbox/src/InverseKinematics.cpp | 2 ++ toolbox/src/SetReferences.cpp | 2 ++ toolbox/src/YarpRead.cpp | 1 + 3 files changed, 5 insertions(+) diff --git a/toolbox/src/InverseKinematics.cpp b/toolbox/src/InverseKinematics.cpp index 43c45a7e0..f8f6e61ff 100644 --- a/toolbox/src/InverseKinematics.cpp +++ b/toolbox/src/InverseKinematics.cpp @@ -18,6 +18,8 @@ #include #include +#include + namespace wbt { struct InverseKinematics::InverseKinematicsPimpl { diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 4f3e4c97d..397bd6020 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -7,6 +7,8 @@ #include #include +#include + namespace wbt { std::string SetReferences::ClassName = "SetReferences"; diff --git a/toolbox/src/YarpRead.cpp b/toolbox/src/YarpRead.cpp index 9fd15c3ca..82fa279f4 100644 --- a/toolbox/src/YarpRead.cpp +++ b/toolbox/src/YarpRead.cpp @@ -10,6 +10,7 @@ #include #include +#include #define PARAM_IDX_1 1 // port name #define PARAM_IDX_2 2 // Size of the port you're reading From 22954931291da1a88e6951361b7ea74405b6ea1e Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 30 Oct 2017 10:17:22 +0000 Subject: [PATCH 04/89] New Matlab WBToolboxConfig class This class contains and validates the configuration passed to every wbt::WBBlock object. ValidConfiguration is a Dependent property and it is set to 1 only if the basic checks currently implemented in other properties don't fail. From this state, it is possible to call getSimulinkParameters() in order to obtain the struct which is passed to the S-Functions and then parsed in C++. This class also contains utilities functions for obtainng a simple string representation of 1D vectors and 1D chars, required to serialize the data in the Config Block's mask. The deserialization is performed using eval(). --- +WBToolbox/WBToolboxConfig.m | 143 +++++++++++++++++++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100644 +WBToolbox/WBToolboxConfig.m diff --git a/+WBToolbox/WBToolboxConfig.m b/+WBToolbox/WBToolboxConfig.m new file mode 100644 index 000000000..20374e498 --- /dev/null +++ b/+WBToolbox/WBToolboxConfig.m @@ -0,0 +1,143 @@ +classdef WBToolboxConfig < matlab.mixin.Copyable + %WBTOOLBOXCONFIG Summary of this class goes here + % Detailed explanation goes here + + % PROPERTIES + % ========== + + properties + % Robot state + RobotName (1,:) char + UrdfFile (1,:) char + ControlledJoints (1,:) cell + ControlBoardsNames (1,:) cell + LocalName (1,:) char + % Other Variables + GravityVector (1,3) double = [0, 0, -9.81] + end + + properties (Dependent) + ValidConfiguration (1,1) logical + end + + properties (Hidden, Dependent) + SimulinkParameters (1,1) struct + end + + % METHODS + % ======= + + methods + + % SET METHODS + % ----------- + + % Dependent properties + % -------------------- + + function set.ValidConfiguration(~,~) + error(['ValidConfiguration is set to 1 automatically ', ... + 'when the configuration is valid.']); + end + + function set.SimulinkParameters(~,~) + error(['SimulinkParameters is created automatically ', ... + 'when the configuration is valid.']); + end + + % Other properties + % ---------------- + + function set.ControlledJoints(obj, value) + if (~iscellstr(value)) + error(['ControlledJoints must be a cell array of ', ... + 'charachter vectors']); + end + obj.ControlledJoints = value; + end + + function set.ControlBoardsNames(obj, value) + if (~iscellstr(value)) + error(['ControlBoardsNames must be a cell array of ', ... + 'charachter vectors']); + end + obj.ControlBoardsNames = value; + end + + % GET METHODS + % ----------- + + % Dependent properties + % -------------------- + + function value = get.ValidConfiguration(obj) + % Check if all the properties have been set + value = ... + ~isempty(obj.RobotName) && ... + ~isempty(obj.UrdfFile) && ... + ~isempty(obj.ControlledJoints) && ... + ~isempty(obj.ControlBoardsNames) && ... + ~isempty(obj.LocalName) && ... + ~isequal(obj.GravityVector,[0, 0, 0]); + end + + function value = get.SimulinkParameters(obj) + % Create and populate the struct + value = struct(); + value.RobotName = obj.RobotName; + value.UrdfFile = obj.UrdfFile; + value.LocalName = obj.LocalName; + value.ControlledJoints = obj.ControlledJoints; + value.ControlBoardsNames = obj.ControlBoardsNames; + value.GravityVector = obj.GravityVector; + end + + % Other properties + % ---------------- + + % OTHER METHODS + % ============= + + function value = getSimulinkParameters(obj) + if (obj.ValidConfiguration) + value = obj.SimulinkParameters; + else + error('The configuration is not complete'); + end + end + end + + methods(Static) + + function cellArraySerialized = serializeCellArray1D(cellArray) + [m,n] = size(cellArray); + if (m>1 && n>1) + error('The input cell must be 1D'); + end + cellArraySerialized = '{'; + for i=1:length(cellArray) + cellArraySerialized = strcat(cellArraySerialized,"'",cellArray{i},"'"); + if i ~= length(cellArray) + cellArraySerialized = strcat(cellArraySerialized,','); + end + end + cellArraySerialized = strcat(cellArraySerialized,'}'); + end + + function vectorSerialized = serializeVector1D(vector) + [m,n] = size(vector); + if (m>1 && n>1) + error('The input vector must be 1D'); + end + vectorSerialized = '['; + for i=1:length(vector) + vectorSerialized = strcat(vectorSerialized,num2str(vector(i))); + if i ~= length(vector) + vectorSerialized = strcat(vectorSerialized,','); + end + end + vectorSerialized = strcat(vectorSerialized,']'); + end + + end +end From be52d5bdd48d0818540583600c50be9a19e895f3 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 30 Oct 2017 10:21:52 +0000 Subject: [PATCH 05/89] New Anytype interface and MxAnyType implementation Initial version of the library, which will become independent in the future. MxAnyType handles the C / C++ mxArray pointer that represents any generic data type in Matlab. It wraps most of the C APIs, and it has optional support of strict type checking. --- CMakeLists.txt | 1 + MxAnyType/CMakeLists.txt | 56 +++++++++ MxAnyType/include/AnyType.h | 67 +++++++++++ MxAnyType/include/MxAnyType.h | 99 ++++++++++++++++ MxAnyType/src/MxAnyType.cpp | 213 ++++++++++++++++++++++++++++++++++ 5 files changed, 436 insertions(+) create mode 100644 MxAnyType/CMakeLists.txt create mode 100644 MxAnyType/include/AnyType.h create mode 100644 MxAnyType/include/MxAnyType.h create mode 100644 MxAnyType/src/MxAnyType.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index acd0156d3..034310442 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ include(YCMDefaultDirs) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) set(WB-TOOLBOX_SHARE_DIR "${CMAKE_INSTALL_PREFIX}/share/WB-Toolbox") +add_subdirectory(MxAnyType) add_subdirectory(toolbox) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/images DESTINATION ${WB-TOOLBOX_SHARE_DIR}) diff --git a/MxAnyType/CMakeLists.txt b/MxAnyType/CMakeLists.txt new file mode 100644 index 000000000..8d3250642 --- /dev/null +++ b/MxAnyType/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_policy(SET CMP0048 NEW) +project(MxAnyType LANGUAGES CXX VERSION 0.1) +cmake_minimum_required(VERSION 3.0.2) + +# Configure the project +# ===================== +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +include(GNUInstallDirs) +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +# Use a general prefix for the project +set(VARS_PREFIX ${PROJECT_NAME}) +# set(VARS_PREFIX "MxAnyType") + +# Build the library +# ================= + +# Export the includes needed to who inherits this library +# Set this up properly for handling either BUILD and INSTALL trees +set(${VARS_PREFIX}_INCLUDE_BUILD ${CMAKE_CURRENT_SOURCE_DIR}/include) +set(${VARS_PREFIX}_INCLUDE_INSTALL ${CMAKE_INSTALL_INCLUDEDIR}/${VARS_PREFIX}) + +# Add the target +add_library(${VARS_PREFIX} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/include/AnyType.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/MxAnyType.h + ${CMAKE_CURRENT_SOURCE_DIR}/src/MxAnyType.cpp) + + +# TODO: temporary, waiting the library becomes a shared +if (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") + set_target_properties(${VARS_PREFIX} PROPERTIES COMPILE_FLAGS "-fPIC") +endif() + +# Find Eigen +find_package(Eigen3 REQUIRED) +target_include_directories(${VARS_PREFIX} PUBLIC "${EIGEN3_INCLUDE_DIR}") + +# Find Matlab resources +find_package(Matlab REQUIRED MX_LIBRARY) +# target_include_directories(${VARS_PREFIX} SYSTEM PUBLIC "${Matlab_INCLUDE_DIRS}") // TODO: when committing +target_include_directories(${VARS_PREFIX} PUBLIC "${Matlab_INCLUDE_DIRS}") + +# Setup the include directories +# TODO why in the how to was INTERFACE? +target_include_directories(${VARS_PREFIX} PUBLIC + $ + $) + +# Assign some useful properties +# set_target_properties(${VARS_PREFIX} PROPERTIES VERSION ${PROJECT_VERSION} +# PUBLIC_HEADER MxAnyType.h) + +# Build tests +# =========== +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/tests) diff --git a/MxAnyType/include/AnyType.h b/MxAnyType/include/AnyType.h new file mode 100644 index 000000000..1637b38b8 --- /dev/null +++ b/MxAnyType/include/AnyType.h @@ -0,0 +1,67 @@ +#ifndef ANYTYPE_H +#define ANYTYPE_H + +#include +#include +#include +#include + +class AnyType; + +typedef std::shared_ptr AnyTypeSPtr; +typedef std::vector AnyCell; +typedef std::unordered_map AnyStruct; + +class AnyType +{ +protected: + // void* m_AnyData; + +public: + // AnyType() : m_AnyData(nullptr) {}; + AnyType() = default; + // virtual void* getAnyDataPtr() const = 0; + + virtual ~AnyType() = default; + + // Integers + virtual bool asInt(int& i) = 0; + // virtual bool asInt8(int8_t& i) = 0; + // virtual bool asInt16(int16_t& i) = 0; + virtual bool asInt32(int32_t& i) = 0; + // virtual bool asInt64(int64_t& i) = 0; + + // Unsigned Integers + virtual bool asUInt(unsigned& i) = 0; + // virtual bool asUInt8(uint8_t& i) = 0; + // virtual bool asUInt16(uint16_t& i) = 0; + // virtual bool asUInt32(uint32_t& i) = 0; + // virtual bool asUInt64(uint64_t& i) = 0; + + // Boolean + virtual bool asBool(bool& b) = 0; + + // Floating-point + // virtual bool asFloat(float& f) = 0; + virtual bool asDouble(double& d) = 0; + + // Characters + virtual bool asString(std::string& s) = 0; + + // Struct + virtual bool asAnyStruct(AnyStruct& map) = 0; + + // Cell array + virtual bool asAnyCell(AnyCell& map) = 0; + + // Matrix + // TODO: constraint max 2-dimension + // virtual bool asMatrixFloat(Eigen::MatrixXf mat) = 0; + // virtual bool asMatrixDouble(Eigen::MatrixXd mat) = 0; + + // Vector + virtual bool asVectorDouble(Eigen::VectorXd& vec) = 0; + virtual bool asVectorDouble(std::vector& vec) = 0; +}; + +#endif /* ANYTYPE_H */ diff --git a/MxAnyType/include/MxAnyType.h b/MxAnyType/include/MxAnyType.h new file mode 100644 index 000000000..21fc7090f --- /dev/null +++ b/MxAnyType/include/MxAnyType.h @@ -0,0 +1,99 @@ +#ifndef MXANYTYPE_H +#define MXANYTYPE_H + +#include +#include +#include +#include +#include "AnyType.h" +#include "matrix.h" + +class MxAnyType; + +// If needed in the future +// class MxAnyCell : public AnyCell {}; +// class MxAnyStruct : public AnyStruct {}; + +struct MxArrayMetadata { + mxClassID id; + bool isScalar; + size_t rows; + size_t cols; + size_t nElem; + size_t nDims; + std::vector dims; +}; + +class MxAnyType : public AnyType +{ +private: + const mxArray* mx; + MxArrayMetadata md; + bool validate; + + // TODO: https://it.mathworks.com/help/matlab/apiref/mxgetscalar.html returns a double always + bool asScalar(double& d); + bool validateClassId(mxClassID id1, mxClassID id2); + + // mxArray* getMxArrayPtr() const { return static_cast(getAnyDataPtr()); } + +public: + // void* getAnyDataPtr() const override { + // return (void*) mx; + // } + + MxAnyType(); + MxAnyType(const mxArray* m, bool validateId = false); + ~MxAnyType() = default; + MxAnyType(const MxAnyType& mxAnyType); + + void enableClassIDValidation(); + + // STRING / CHARS + // ============== + + bool asString(std::string& s) override; + + // SCALAR TYPES + // ============ + + // Generic casting + // --------------- + + bool asInt(int& i) override; + bool asUInt(unsigned& i) override; + + // Specific casting + // ---------------- + + bool asInt32(int32_t& i) override; + + // TODO: complete with all the other scalar types + // bool asInt64(int64_t& i) override + // { + // double buffer; + // if (!asScalar(buffer)) return false; + // i = static_cast(buffer); + // return validateClassId(md.id, mxINT64_CLASS); + // } + + bool asDouble(double& d) override; + bool asBool(bool& b) override; + + // COMPOSITE DATA TYPES + // ==================== + + bool asAnyStruct(AnyStruct& s) override; + bool asAnyCell(AnyCell& cell) override; + + // MATRIX + // ====== + + // VECTOR + // ====== + + bool asVectorDouble(Eigen::VectorXd& vec) override; + bool asVectorDouble(std::vector& vec) override; +}; + +#endif /* MXANYTYPE_H */ diff --git a/MxAnyType/src/MxAnyType.cpp b/MxAnyType/src/MxAnyType.cpp new file mode 100644 index 000000000..77a2f10f4 --- /dev/null +++ b/MxAnyType/src/MxAnyType.cpp @@ -0,0 +1,213 @@ +#include "MxAnyType.h" +#include + +// PRIVATE METHODS +// =============== + +bool MxAnyType::asScalar(double& d) +{ + if (!mx) return false; + if (!mxIsScalar(mx)) return false; // 1x1 + if (!mxIsNumeric(mx)) return false; // Types: https://it.mathworks.com/help/matlab/apiref/mxisnumeric.html + + // Cast to double since even a mxINT8_CLASS is returned as double: + // https://it.mathworks.com/help/matlab/apiref/mxgetscalar.html + d = static_cast(mxGetScalar(mx)); + return true; +} + +bool MxAnyType::validateClassId(mxClassID id1, mxClassID id2) +{ + if (validate) + return id1 == id2; + else + return true; +} + +// PUBLIC METHODS +// ============== + +// Constructors +// ============ + +MxAnyType::MxAnyType() : mx(nullptr), validate(false) +{ + md.id = mxUNKNOWN_CLASS; +} + +MxAnyType::MxAnyType(const mxArray* m, bool validateId) : mx(m), validate(validateId) +{ + assert(mx); + + // Get the ID + md.id = mxGetClassID(mx); + assert(md.id != mxVOID_CLASS); + assert(md.id != mxUNKNOWN_CLASS); + + // Get the other metadata + md.isScalar = mxIsScalar(mx); + md.rows = static_cast(mxGetN(mx)); + md.cols = static_cast(mxGetM(mx)); + md.nElem = static_cast(mxGetNumberOfElements(mx)); + md.nDims = static_cast(mxGetNumberOfDimensions(mx)); + + if (md.isScalar) + assert(md.rows == md.cols == md.nElem == 1); + + // TODO: only 2 dims currently supported + assert(md.nDims == 2); + assert(md.rows * md.cols == md.nElem); + + const size_t* size = mxGetDimensions(mx); + for (unsigned dim = 0; dim < md.nDims; ++dim) { + md.dims.push_back(static_cast(size[dim])); + } + assert(md.dims.size() == 2); +} + +MxAnyType::MxAnyType(const MxAnyType& mxAnyType) +: mx(mxAnyType.mx) +, md(mxAnyType.md) +, validate(mxAnyType.validate) +{} + +void MxAnyType::enableClassIDValidation() +{ + validate = true; +} + +// STRING / CHARS +// ============== + +bool MxAnyType::asString(std::string& s) +{ + if (!mx) return false; + if (md.id != mxCHAR_CLASS) return false; + char* buffer = mxArrayToString(mx); + s = std::string(buffer); + mxFree(buffer); + return true; +} + +// SCALAR TYPES +// ============ + +// Generic casting +// --------------- + +bool MxAnyType::asInt(int& i) +{ + double buffer; + if (!asScalar(buffer)) return false; + i = static_cast(buffer); + return true; +} + +bool MxAnyType::asUInt(unsigned& i) +{ + double buffer; + if (!asScalar(buffer)) return false; + i = static_cast(buffer); + return true; +} + +// Specific casting +// ---------------- + +bool MxAnyType::asInt32(int32_t& i) +{ + double buffer; + if (!asScalar(buffer)) return false; + i = static_cast(buffer); + return validateClassId(md.id, mxINT32_CLASS); +} + +// TODO: complete with all the other scalar types +// bool asInt64(int64_t& i) +// { +// double buffer; +// if (!asScalar(buffer)) return false; +// i = static_cast(buffer); +// return validateClassId(md.id, mxINT64_CLASS); +// } + +bool MxAnyType::asDouble(double& d) +{ + bool ok = asScalar(d); + return ok && validateClassId(md.id, mxDOUBLE_CLASS); +} + +bool MxAnyType::asBool(bool& b) + { + if (!mx) return false; + if (!mxIsLogicalScalar(mx)) return false; + b = mxIsLogicalScalarTrue(mx); + return true; +} + +// COMPOSITE DATA TYPES +// ==================== + +bool MxAnyType::asAnyStruct(AnyStruct& s) +{ + if (!mx) return false; + if (md.id != mxSTRUCT_CLASS) return false; + + for (unsigned i = 0; i < mxGetNumberOfFields(mx); ++i) { + const char* fieldName = mxGetFieldNameByNumber(mx,i); + // TODO multidimensional struct + mxArray* fieldContent = mxGetFieldByNumber(mx,0,i); + if (fieldName == nullptr) return false; + if (fieldContent == nullptr) return false; + s[std::string(fieldName)] = std::make_shared(fieldContent); + } + return true; +} + +bool MxAnyType::asAnyCell(AnyCell& cell) +{ + if (!mx) return false; + if (md.id != mxCELL_CLASS) return false; + + // TODO: AnyCell then will have a operator()(3,4) method; + for (unsigned i=0; i < mxGetNumberOfElements(mx); ++i) { + mxArray* cellContent = mxGetCell(mx, i); + if (!cellContent) return false; + cell.push_back(std::make_shared(cellContent)); + } + return true; +} + +// MATRIX +// ====== + +// VECTOR +// ====== + +// TODO tests with other types (uint8) https://it.mathworks.com/help/matlab/apiref/mxgetdata.html (Descriptio) +bool MxAnyType::asVectorDouble(Eigen::VectorXd& vec) +{ + if (!mx) return false; + if (!mxIsDouble(mx)) return false; + + if (md.rows > 1 && md.cols > 1) { + return false; + } + + // TODO add method for complex vectors (and move the check into md) + if (mxIsComplex(mx)) return false; + + double* buffer = mxGetPr(mx); + if (!buffer) return false; + + vec = Eigen::Map(buffer, md.nElem); + return true; +} + +bool MxAnyType::asVectorDouble(std::vector& vec) +{ + Eigen::VectorXd vecEigen; + if (!asVectorDouble(vecEigen)) return false; + vec.assign(vecEigen.data(), vecEigen.data() + vecEigen.rows() * vecEigen.cols()); + return true; +} From 705766370b9eac6ea2160fff06fdc045f1318390 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 30 Oct 2017 10:30:52 +0000 Subject: [PATCH 06/89] Error class has been enhanced and renamed to Log wbt::Log is the new class that provides log support to the WBToolbox. It now supports both Errors and Warnings. For being more flexible, wbt::Log is a singleton. This approach simplifies dramatically the handling of warnings, and can be called from every method without the need of passing it though arguments. Since singletons cannot be deleted, wbt::Log state is reset in the early stage of the processing. [Squash] toolbox.cpp --- toolbox/CMakeLists.txt | 4 +- toolbox/include/base/Error.h | 19 ---- toolbox/include/base/Log.h | 42 ++++++++ toolbox/include/base/toolbox.h | 2 +- toolbox/src/base/Error.cpp | 0 toolbox/src/base/Log.cpp | 71 ++++++++++++++ toolbox/src/base/toolbox.cpp | 174 +++++++++++++++++++++------------ 7 files changed, 230 insertions(+), 82 deletions(-) delete mode 100644 toolbox/include/base/Error.h create mode 100644 toolbox/include/base/Log.h delete mode 100644 toolbox/src/base/Error.cpp create mode 100644 toolbox/src/base/Log.cpp diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 52c60000f..9838ee86f 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -62,7 +62,7 @@ configure_block(BLOCK_NAME "Base" src/base/Block.cpp src/base/BlockInformation.cpp src/base/WBIBlock.cpp - src/base/Error.cpp + src/base/Log.cpp src/base/WBInterface.cpp src/base/factory.cpp src/base/WBIModelBlock.cpp @@ -72,7 +72,7 @@ configure_block(BLOCK_NAME "Base" include/base/Block.h include/base/BlockInformation.h include/base/WBIBlock.h - include/base/Error.h + include/base/Log.h include/base/WBInterface.h include/base/WBIModelBlock.h include/base/SimulinkBlockInformation.h #this is temp diff --git a/toolbox/include/base/Error.h b/toolbox/include/base/Error.h deleted file mode 100644 index f11dc413c..000000000 --- a/toolbox/include/base/Error.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef WBT_ERROR_H -#define WBT_ERROR_H - -#include - -namespace wbt { - class Error; -} - -/** - * Basic Error class - */ -class wbt::Error { -public: - std::string message; /**< a string representing the error message */ -}; - - -#endif /* end of include guard: WBT_ERROR_H */ diff --git a/toolbox/include/base/Log.h b/toolbox/include/base/Log.h new file mode 100644 index 000000000..699c431b6 --- /dev/null +++ b/toolbox/include/base/Log.h @@ -0,0 +1,42 @@ +#ifndef WBT_LOG_H +#define WBT_LOG_H + +#include +#include + +namespace wbt { + class Log; +} + +/** + * Basic Log class + */ +class wbt::Log +{ +private: + std::vector errors; + std::vector warnings; + std::string prefix; + + static std::string serializeVectorString(std::vector v, std::string prefix=""); +public: + + static wbt::Log& getSingleton(); + + void error(std::string errorMessage); + void warning(std::string warningMessage); + + void resetPrefix(); + void setPrefix(std::string prefixMessage); + + std::string getErrors() const; + std::string getWarnings() const; + + void clearErrors(); + void clearWarnings(); + + void clear(); +}; + + +#endif /* end of include guard: WBT_LOG_H */ diff --git a/toolbox/include/base/toolbox.h b/toolbox/include/base/toolbox.h index a61e5e74e..a1ff7505f 100644 --- a/toolbox/include/base/toolbox.h +++ b/toolbox/include/base/toolbox.h @@ -1,4 +1,4 @@ -#include "Error.h" +#include "Log.h" //General Yarp utilities #include "YarpRead.h" #include "YarpWrite.h" diff --git a/toolbox/src/base/Error.cpp b/toolbox/src/base/Error.cpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/toolbox/src/base/Log.cpp b/toolbox/src/base/Log.cpp new file mode 100644 index 000000000..9ee6abdaa --- /dev/null +++ b/toolbox/src/base/Log.cpp @@ -0,0 +1,71 @@ +#include "Log.h" +#include +#include + +using namespace wbt; + +Log& Log::getSingleton() +{ + static Log logInstance; + return logInstance; +} + +void Log::error(std::string errorMessage) +{ + errors.push_back(errorMessage); +} + +void Log::warning(std::string warningMessage) +{ + warnings.push_back(warningMessage); +} + +void Log::setPrefix(std::string prefixMessage) +{ + prefix = prefixMessage; +} + +void Log::resetPrefix() +{ + prefix.clear(); +} + +std::string Log::serializeVectorString(std::vector v, std::string prefix) +{ + std::stringstream output; + std::ostream_iterator output_iterator(output, "\n"); + std::copy(v.begin(), v.end(), output_iterator); + + if (prefix.empty()) { + return output.str(); + } + else { + return prefix + "\n" + output.str(); + } +} + +std::string Log::getErrors() const +{ + return serializeVectorString(errors, prefix); +} + +std::string Log::getWarnings() const +{ + return serializeVectorString(warnings, prefix); +} + +void Log::clearWarnings() +{ + warnings.clear(); +} + +void Log::clearErrors() +{ + errors.clear(); +} + +void Log::clear() +{ + clearErrors(); + clearWarnings(); +} diff --git a/toolbox/src/base/toolbox.cpp b/toolbox/src/base/toolbox.cpp index 5162f3505..2e2d9037f 100644 --- a/toolbox/src/base/toolbox.cpp +++ b/toolbox/src/base/toolbox.cpp @@ -26,11 +26,74 @@ #include "toolbox.h" #include "Block.h" +#include "Log.h" #include "SimulinkBlockInformation.h" #include #include +static void catchLogMessages(bool status, SimStruct *S, std::string prefix) +{ + // Initialize static buffers + const unsigned bufferLen = 1024; + static char errorBuffer[bufferLen]; + static char warningBuffer[bufferLen]; + + // Notify warnings + if (!wbt::Log::getSingleton().getWarnings().empty()) { + // Get the singleton + wbt::Log& log = wbt::Log::getSingleton(); + + // Handle the prefix + std::string warningMsg; + if (!prefix.empty()) { + log.setPrefix(prefix); + warningMsg = log.getWarnings(); + log.resetPrefix(); + } + else { + warningMsg = log.getWarnings(); + } + + // Trim the message if needed + if (warningMsg.length() >= bufferLen) { + warningMsg = warningMsg.substr(0, bufferLen-1); + } + + // Forward to Simulink + sprintf(warningBuffer, "%s", warningMsg.c_str()); + ssWarning(S, warningBuffer); + log.clearWarnings(); + } + + // Notify errors + if (!status) { + // Get the singleton + wbt::Log& log = wbt::Log::getSingleton(); + + // Handle the prefix + std::string errorMsg; + if (!prefix.empty()) { + log.setPrefix(prefix); + errorMsg = log.getErrors(); + log.resetPrefix(); + } + else { + errorMsg = log.getErrors(); + } + + // Trim the message if needed + if (errorMsg.length() >= bufferLen) { + errorMsg = errorMsg.substr(0, bufferLen-1); + } + + // Forward to Simulink + sprintf(errorBuffer, "%s", errorMsg.c_str()); + ssSetErrorStatus(S, errorBuffer); + return; + } +} + // Function: MDL_CHECK_PARAMETERS #define MDL_CHECK_PARAMETERS #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) @@ -65,10 +128,12 @@ static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, // block's characteristics (number of inputs, s, states, etc.). static void mdlInitializeSizes(SimStruct *S) { - static char errorBuffer[512]; + // Initialize the Log singleton + wbt::Log::getSingleton().clear(); + if (ssGetSFcnParamsCount(S) < 1) { - sprintf(errorBuffer, "%s", "The block type parameter must be specified"); - ssSetErrorStatus(S, errorBuffer); + wbt::Log::getSingleton().error("The block type parameter must be specified"); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); return; } char *classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); @@ -78,43 +143,39 @@ static void mdlInitializeSizes(SimStruct *S) //We cannot save data in PWork during the initializeSizes phase ssSetNumPWork(S, 1); - + + // Notify errors if (!block) { - sprintf(errorBuffer, "Could not create an object of type %s", className.c_str()); - ssSetErrorStatus(S, errorBuffer); + wbt::Log::getSingleton().error("Could not create an object of type " + className); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); return; } - + ssSetNumSFcnParams(S, 1 + block->numberOfParameters()); ssSetSFcnParamTunable(S, 0, false); for (unsigned i = 1; i < ssGetNumSFcnParams(S); ++i) { bool tunable = false; block->parameterAtIndexIsTunable(i - 1, tunable); ssSetSFcnParamTunable(S, i, tunable); - } - - + + #if defined(MATLAB_MEX_FILE) - if(ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)){ + if(ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { mdlCheckParameters(S); - if(ssGetErrorStatus(S)){ + if(ssGetErrorStatus(S)) { return; } - } else{ - sprintf(errorBuffer, "%s", "Number of parameters different from those defined"); - ssSetErrorStatus(S, errorBuffer); + } else { + wbt::Log::getSingleton().error("Number of parameters different from those defined"); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); return; } #endif - wbt::Error error; wbt::SimulinkBlockInformation blockInfo(S); - if (!block->configureSizeAndPorts(&blockInfo, &error)) { - sprintf(errorBuffer, "%s", error.message.substr(0, 511).c_str()); - ssSetErrorStatus(S, errorBuffer); - return; - } + bool ok = block->configureSizeAndPorts(&blockInfo); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); ssSetNumSampleTimes(S, 1); @@ -134,17 +195,15 @@ static void mdlInitializeSizes(SimStruct *S) std::vector additionalOptions = block->additionalBlockOptions(); - for (std::vector::const_iterator it = additionalOptions.begin(); - it < additionalOptions.end(); ++it) { - wbt::Data option; - if (blockInfo.optionFromKey(*it, option)) { - options |= option.uint32Data(); + for (auto additionalOption: additionalOptions) { + double option; + if (blockInfo.optionFromKey(additionalOption, option)) { + options |= static_cast(option); } } ssSetOptions(S, options); delete block; - } // Function: mdlInitializeSampleTimes ========================================= @@ -173,15 +232,12 @@ static void mdlStart(SimStruct *S) wbt::Block *block = wbt::Block::instantiateBlockWithClassName(className); ssSetPWorkValue(S, 0, block); - wbt::Error error; wbt::SimulinkBlockInformation blockInfo(S); - if (!block || !block->initialize(&blockInfo, &error)) { - yError() << "[mdlStart]" << error.message; - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlStart]%s", error.message.substr(0, 1023 - strlen("[mdlStart]")).c_str()); - ssSetErrorStatus(S, errorBuffer); + bool ok = false; + if (block) { + ok = block->initialize(&blockInfo); } - + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); } @@ -192,13 +248,13 @@ static void mdlUpdate(SimStruct *S, int_T tid) UNUSED_ARG(tid); if (ssGetNumPWork(S) > 0) { wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); - wbt::Error error; + wbt::SimulinkBlockInformation blockInfo(S); - if (!block || !block->updateDiscreteState(&blockInfo, &error)) { - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlOutputs]%s", error.message.substr(0, 1023 - strlen("[mdlOutputs]")).c_str()); - ssSetErrorStatus(S, errorBuffer); + bool ok = false; + if (block) { + ok = block->updateDiscreteState(&blockInfo); } + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); } } #endif @@ -210,13 +266,13 @@ static void mdlInitializeConditions(SimStruct *S) { if (ssGetNumPWork(S) > 0) { wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); - wbt::Error error; + wbt::SimulinkBlockInformation blockInfo(S); - if (!block || !block->initializeInitialConditions(&blockInfo, &error)) { - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlInitializeConditions]%s", error.message.substr(0, 1023 - strlen("[mdlInitializeConditions]")).c_str()); - ssSetErrorStatus(S, errorBuffer); + bool ok = false; + if (block) { + ok = block->initializeInitialConditions(&blockInfo); } + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); } } #endif @@ -240,35 +296,33 @@ static void mdlOutputs(SimStruct *S, int_T tid) UNUSED_ARG(tid); if (ssGetNumPWork(S) > 0) { wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); - wbt::Error error; + wbt::SimulinkBlockInformation blockInfo(S); - if (!block || !block->output(&blockInfo, &error)) { - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlOutputs]%s", error.message.substr(0, 1023 - strlen("[mdlOutputs]")).c_str()); - ssSetErrorStatus(S, errorBuffer); + bool ok = false; + if (block) { + ok = block->output(&blockInfo); } + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); } - } static void mdlTerminate(SimStruct *S) { if (ssGetNumPWork(S) > 0 && ssGetPWork(S)) { wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); - wbt::Error error; + wbt::SimulinkBlockInformation blockInfo(S); + bool ok = false; if (block) { - if (block->terminate(&blockInfo, &error)) { - delete block; - ssSetPWorkValue(S, 0, NULL); - } else { - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlTerminate]%s", error.message.substr(0, 1023 - strlen("[mdlTerminate]")).c_str()); - ssSetErrorStatus(S, errorBuffer); - } + ok = block->terminate(&blockInfo); } - } + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); + if (ok) { + delete block; + ssSetPWorkValue(S, 0, NULL); + } + } } // Required S-function trailer From b4beb26830179ee9299ca4cabc1d3a7374463caa Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 30 Oct 2017 10:37:45 +0000 Subject: [PATCH 07/89] Removed WBIModelBlock Its functionality are implemented by the new generic WBBlock, which is a refactored version of WBIBlock that access robot resources in a lazy way. This approach allows avoiding the previous separation between WBIBlock and WBIModelBlock. --- toolbox/CMakeLists.txt | 2 -- toolbox/include/base/WBIModelBlock.h | 36 ---------------------------- toolbox/src/base/WBIModelBlock.cpp | 27 --------------------- 3 files changed, 65 deletions(-) delete mode 100644 toolbox/include/base/WBIModelBlock.h delete mode 100644 toolbox/src/base/WBIModelBlock.cpp diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 9838ee86f..7ae729c4a 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -65,7 +65,6 @@ configure_block(BLOCK_NAME "Base" src/base/Log.cpp src/base/WBInterface.cpp src/base/factory.cpp - src/base/WBIModelBlock.cpp src/base/SimulinkBlockInformation.cpp #this is temp src/base/Signal.cpp HEADERS include/base/toolbox.h @@ -74,7 +73,6 @@ configure_block(BLOCK_NAME "Base" include/base/WBIBlock.h include/base/Log.h include/base/WBInterface.h - include/base/WBIModelBlock.h include/base/SimulinkBlockInformation.h #this is temp include/base/Signal.h ) diff --git a/toolbox/include/base/WBIModelBlock.h b/toolbox/include/base/WBIModelBlock.h deleted file mode 100644 index ff054e655..000000000 --- a/toolbox/include/base/WBIModelBlock.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef WBT_WBIMODELBLOCK_H -#define WBT_WBIMODELBLOCK_H - -#include "WBIBlock.h" - -namespace wbt { - class WBIModelBlock; - class BlockInformation; -} - -/** - * Basic class for WBI related blocks. - * This class (the whole toolbox in reality) assumes the block represent - * an instantaneous system (i.e. not a dynamic system). - * - * You can create a new block by deriving this class and implementing at least - * the output method. - * - * This block implements the following default behaviours: - * - it ask for 4 parameters (robot name, local (module) name, wbi config file and wbi list) - * - It initializes the yarp network and the whole body interface object - * - During terminate it closes and release the interface object and terminate the yarp network - * - * @Note: Usually you want to call this class implementations at some point in your - * method overridings, unless you want to completely change the code (but at that point - * you probabily want to derive from Block instead) - */ -class wbt::WBIModelBlock : public wbt::WBIBlock { - -public: - virtual ~WBIModelBlock(); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); -}; - -#endif /* WBT_WBIMODELBLOCK_H */ diff --git a/toolbox/src/base/WBIModelBlock.cpp b/toolbox/src/base/WBIModelBlock.cpp deleted file mode 100644 index 3921b3cf2..000000000 --- a/toolbox/src/base/WBIModelBlock.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "WBIModelBlock.h" - -#include "WBInterface.h" -#include "Error.h" - -wbt::WBIModelBlock::~WBIModelBlock() {} - -bool wbt::WBIModelBlock::initialize(wbt::BlockInformation *blockInfo, wbt::Error *error) -{ - if (!configureWBIParameters(blockInfo, error)) - return false; - - if (!WBInterface::sharedInstance().initializeModel()) { - if (error) error->message = "Failed to initialize WBI"; - return false; - } - return true; -} - -bool wbt::WBIModelBlock::terminate(wbt::BlockInformation *blockInfo, wbt::Error *error) -{ - if (!WBInterface::sharedInstance().terminateModel()) { - if (error) error->message = "Failed to terminate WBI"; - return false; - } - return true; -} From aee4eed5b118ee4b2e9ca556bf9ae3d75d528386 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 30 Oct 2017 10:43:49 +0000 Subject: [PATCH 08/89] New Configuration class This class contains the data passed from Simulink. It matches the data structure of Matlab's WBToolboxConfig.getSimulinkParameters() struct. --- toolbox/CMakeLists.txt | 2 + toolbox/include/base/Configuration.h | 162 +++++++++++++++++++++++++++ toolbox/src/base/Configuration.cpp | 123 ++++++++++++++++++++ 3 files changed, 287 insertions(+) create mode 100644 toolbox/include/base/Configuration.h create mode 100644 toolbox/src/base/Configuration.cpp diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 7ae729c4a..25a304fa7 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -67,6 +67,7 @@ configure_block(BLOCK_NAME "Base" src/base/factory.cpp src/base/SimulinkBlockInformation.cpp #this is temp src/base/Signal.cpp + src/base/Configuration.cpp HEADERS include/base/toolbox.h include/base/Block.h include/base/BlockInformation.h @@ -75,6 +76,7 @@ configure_block(BLOCK_NAME "Base" include/base/WBInterface.h include/base/SimulinkBlockInformation.h #this is temp include/base/Signal.h + include/base/Configuration.h ) configure_block(BLOCK_NAME "Inverse Kinematics" diff --git a/toolbox/include/base/Configuration.h b/toolbox/include/base/Configuration.h new file mode 100644 index 000000000..192656cdf --- /dev/null +++ b/toolbox/include/base/Configuration.h @@ -0,0 +1,162 @@ +#ifndef WBT_TOOLBOXCONFIG_H +#define WBT_TOOLBOXCONFIG_H + +#include +#include + +namespace wbt { + class Configuration; +} + +/** + * \class Configuration Configuration.h + * + * This class stores in a C++ object the content of the WBToolboxConfig Matlab class. + * + * @see WBToolboxConfig Matlab Class + * @see RobotInterface + */ +class wbt::Configuration +{ +// TODO: check how localName is used +private: + std::string m_robotName; ///< Name of the robot + std::string m_urdfFile; ///< Name of the file containing the urdf model + std::string m_localName; ///< Prefix appended to the opened ports + std::vector m_controlledJoints; ///< Subset of controlled joints + std::vector m_controlBoardsNames; ///< Names of the used ControlBoard names + std::vector m_gravityVector; ///< The gravity vector + size_t m_dofs; //< DoFs extracted my m_controlBoardsNames vector + +public: + Configuration(); + ~Configuration() = default; + + // SET METHODS + // =========== + + /** + * Initialize the Configuration object + * + * @param robotName Name of the robot + * @param urdfFile Name of the file containing the urdf model + * @param controlledJoints Subset of controlled joints + * @param controlBoardsNames Names of the used ControlBoard names + * @param localName Prefix appended to the opened ports + * @param gravityVector The gravity vector + */ + void setParameters(std::string robotName, + std::string urdfFile, + std::vector controlledJoints, + std::vector controlBoardsNames, + std::string localName, + std::vector gravityVector); + + /** + * Set the name of the robot + * + * @param robotName Name of the robot + */ + void setRobotName(const std::string& robotName); + + /** + * Set the name of the file containing the urdf model + * + * @param urdfFile Name of the file containing the urdf model + */ + void setUrdfFile(const std::string& urdfFile); + + /** + * Set the subset of controlled joints + * @param controlledJoints Subset of controlled joints + */ + void setControlledJoints(const std::vector& controlledJoints); + + /** + * Set the names of the used ControlBoard names + * @param controlBoardsNames Names of the used ControlBoard names + */ + void setControlBoardsNames(const std::vector& controlBoardsNames); + + /** + * Set the prefix appended to the opened ports + * @param localName Prefix appended to the opened ports + */ + void setLocalName(const std::string& localName); + + /** + * Set the gravity vector + * @param gravityVector The gravity vector + */ + void setGravityVector(const std::vector& gravityVector); + + // GET METHODS + // =========== + + /** + * Set the name of the robot + * + * @return Name of the robot + */ + const std::string& getRobotName() const; + + /** + * Set the name of the file containing the urdf model + * + * @return Name of the file containing the urdf model + */ + const std::string& getUrdfFile() const; + + /** + * Set the subset of controlled joints + * + * @return Subset of controlled joints + */ + const std::vector& getControlledJoints() const; + + /** + * Set the names of the used ControlBoard names + * + * @return Names of the used ControlBoard names + */ + const std::vector& getControlBoardsNames() const; + + /** + * Set the prefix appended to the opened ports + * + * @return Prefix appended to the opened ports + */ + const std::string& getLocalName() const; + + /** + * Set the gravity vector + * + * @return The gravity vector + */ + const std::vector& getGravityVector() const; + + /** + * Get the configured number of DoFs + * + * @return The configured number of DoFs + */ + const size_t& getNumberOfDoFs() const; + + // OTHER METHODS + // ============= + + /** + * Checks if the stored configuration is valid, i.e. all the required fields have + * been saved successfully + * + * @return True if the configuration is valid + */ + bool isValid() const; + + // OPERATORS OVERLOADING + // ===================== + + bool operator==(const Configuration &config) const; +}; + +#endif // WBT_TOOLBOXCONFIG_H diff --git a/toolbox/src/base/Configuration.cpp b/toolbox/src/base/Configuration.cpp new file mode 100644 index 000000000..d28857016 --- /dev/null +++ b/toolbox/src/base/Configuration.cpp @@ -0,0 +1,123 @@ +#include "Configuration.h" + +using namespace wbt; + +Configuration::Configuration() +: m_dofs(0) +{} + +// SET METHODS +// =========== + +void Configuration::setParameters(std::string robotName, + std::string urdfFile, + std::vector controlledJoints, + std::vector controlBoardsNames, + std::string localName, + std::vector gravityVector) +{ + setRobotName(robotName); + setUrdfFile(urdfFile); + setControlledJoints(controlledJoints); + setControlBoardsNames(controlBoardsNames); + setLocalName(localName); + setGravityVector(gravityVector); +} + +void Configuration::setRobotName(const std::string& robotName) +{ + m_robotName = robotName; +} + +void Configuration::setUrdfFile(const std::string& urdfFile) +{ + m_urdfFile = urdfFile; +} + +void Configuration::setControlledJoints(const std::vector& controlledJoints) +{ + m_controlledJoints = controlledJoints; + m_dofs = controlledJoints.size(); +} + +void Configuration::setControlBoardsNames(const std::vector& controlBoardsNames) +{ + m_controlBoardsNames = controlBoardsNames; +} + +void Configuration::setLocalName(const std::string& localName) +{ + m_localName = localName; +} + +void Configuration::setGravityVector(const std::vector& gravityVector) +{ + m_gravityVector = gravityVector; +} + +// GET METHODS +// =========== + +const std::string& Configuration::getRobotName() const +{ + return m_robotName; +} + +const std::string& Configuration::getUrdfFile() const +{ + return m_urdfFile; +} + +const std::vector& Configuration::getControlledJoints() const +{ + return m_controlledJoints; +} + +const std::vector& Configuration::getControlBoardsNames() const +{ + return m_controlBoardsNames; +} + +const std::string& Configuration::getLocalName() const +{ + return m_localName; +} + +const std::vector& Configuration::getGravityVector() const +{ + return m_gravityVector; +} + +const size_t& Configuration::getNumberOfDoFs() const +{ + return m_dofs; +} + +// OTHER METHODS +// ============= + +bool Configuration::isValid() const +{ + bool status = !m_robotName.empty() && + !m_urdfFile.empty() && + !m_localName.empty() && + !m_controlledJoints.empty() && + !m_controlBoardsNames.empty() && + !m_gravityVector.empty() && + m_dofs > 0; + return status; +} + +// OPERATORS OVERLOADING +// ===================== + +bool Configuration::operator==(const Configuration &config) const +{ + return this->m_robotName == config.m_robotName && + this->m_urdfFile == config.m_urdfFile && + this->m_localName == config.m_localName && + this->m_controlledJoints == config.m_controlledJoints && + this->m_controlBoardsNames == config.m_controlBoardsNames && + this->m_gravityVector == config.m_gravityVector && + this->m_dofs == config.m_dofs; +} From 8a68ac718e9a75d4d9e8952ea02be3f3842ddfe3 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 30 Oct 2017 10:47:50 +0000 Subject: [PATCH 09/89] New RobotInterface class This class contains all the required data structures to operate on a robot, either through the model or the interfaces to the hardware / gazebo. From the Simulink point of view, there is one instance of RobotInterface for every Configuration block added in the model. In fact, RobotInterface extends by composition the Configuration object. It handles the pointers to its members in a lazy way, initializing on the fly the asked resource. --- toolbox/CMakeLists.txt | 2 + toolbox/include/base/RobotInterface.h | 228 +++++++++++++ toolbox/src/base/RobotInterface.cpp | 447 ++++++++++++++++++++++++++ 3 files changed, 677 insertions(+) create mode 100644 toolbox/include/base/RobotInterface.h create mode 100644 toolbox/src/base/RobotInterface.cpp diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 25a304fa7..f084744c7 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -68,6 +68,7 @@ configure_block(BLOCK_NAME "Base" src/base/SimulinkBlockInformation.cpp #this is temp src/base/Signal.cpp src/base/Configuration.cpp + src/base/RobotInterface.cpp HEADERS include/base/toolbox.h include/base/Block.h include/base/BlockInformation.h @@ -77,6 +78,7 @@ configure_block(BLOCK_NAME "Base" include/base/SimulinkBlockInformation.h #this is temp include/base/Signal.h include/base/Configuration.h + include/base/RobotInterface.h ) configure_block(BLOCK_NAME "Inverse Kinematics" diff --git a/toolbox/include/base/RobotInterface.h b/toolbox/include/base/RobotInterface.h new file mode 100644 index 000000000..83da54cde --- /dev/null +++ b/toolbox/include/base/RobotInterface.h @@ -0,0 +1,228 @@ +#ifndef WBT_ROBOTINTERFACE_H +#define WBT_ROBOTINTERFACE_H + +#include "Configuration.h" +#include +#include +#include +#include +#include +#include + +namespace yarp { + namespace dev { + class PolyDriver; + class IPositionControl; + class IPositionDirect; + class IVelocityControl; + class ITorqueControl; + class IPWMControl; + class IControlMode2; + class ICurrentControl; + class IEncoders; + class IControlLimits2; + class IPidControl; + } +} + +namespace iDynTree { + class KinDynComputations; +} + +namespace wbt { + class RobotInterface; + struct YarpDevices; + + typedef int jointIdx_yarp; + typedef int jointIdx_iDynTree; + typedef unsigned cb_idx; + typedef std::unordered_map> JointsMapIndex; + typedef std::unordered_map> JointsMapString; +} + +/** + * \struct wbt::YarpDevices RobotInterface.h + * + * This struct contains shared_ptrs to the devices which are (lazy) asked from the blocks. + * + * @note The shared_ptr is owned only by wbt::RobotInterface. All the blocks will receive a + * weak_ptr. + * @remark Right now only asking / setting the whole joint set of the current Configuration is + * supported. If in the future the support of operating on a subset will be implemented, + * IPositionControl2 and IVelocityControl2 should be implemented. For the time being, + * a possible workaround for this situation is creating a new configuration block + * containing only the reduced set in a deeper-hierarchy Simulink's subsystem. + * @see RobotInterface::getDevice + */ +struct wbt::YarpDevices +{ + std::shared_ptr iControlMode2; + std::shared_ptr iPositionControl; + std::shared_ptr iPositionDirect; + std::shared_ptr iVelocityControl; + std::shared_ptr iTorqueControl; + std::shared_ptr iPWMControl; + std::shared_ptr iCurrentControl; + std::shared_ptr iEncoders; + std::shared_ptr iControlLimits2; + std::shared_ptr iPidControl; +}; + +// TODO o pensare come evitare di avere due conf con es position e torque nei setref con lo stesso robot. +// e' un casino -> aggiungere max un warning o solo documentazione. +// Solo un setReference attivo per configuration.Se ci sono due setReference con lo stesso +// set di giunti attivi contemporaneamente. Oppure due stessi blocchi setReference con lo stesso blocco config. + +/** + * \class wbt::RobotInterface RobotInterface.h + * + * This class holds the configuration used by one or more blocks, and all the objects to operate + * with the specified robot (real or model). + * + * @see wbt::Configuration + * @see wbt::YarpDevices + * @see iDynTree::KinDynComputations + */ +class wbt::RobotInterface +{ +private: + std::shared_ptr m_robotDevice; + std::shared_ptr m_kinDynComp; + wbt::YarpDevices m_yarpDevices; + + // Maps used to store infos about yarp's and idyntree's internal joint indexing + std::shared_ptr m_jointsMapIndex; + std::shared_ptr m_jointsMapString; + // std::unordered_map, joint_name> m_yarp2joint; + + // Configuration from Simulink Block's parameters + const wbt::Configuration m_config; + + // Counters for resource allocation / deallocation + unsigned m_robotDeviceCounter; + + // INITIALIZATION HELPERS + // ====================== + + /** + * Initialize the iDynTree::KinDynComputations with the information contained + * in wbt::Configuration. It finds the urdf file and stores the object to operate on it. + * If the joint list contained in RobotInterface::m_config is not complete, it loads a + * reduced model of the robot + * + * @return True if success + */ + bool initializeModel(); + + /** + * Configure a RemoteControlBoardRemapper device in order to allow + * interfacing the toolbox with the robot (real or in Gazebo). + * + * @return True if success + */ + bool initializeRemoteControlBoardRemapper(); + + // OTHER PRIVATE METHODS + // ===================== + + /** + * Gather a device from the RemoteControlBoardRemapper as a \c weak_ptr + * + * @param device The RemoteControlBoardRemapper device + * @return The dynamic(ally)_cast device + * @tparam T The type of the retured device + */ + template std::weak_ptr getDevice(std::shared_ptr device); + + /** + * Creates the map between joints (specified as either names or idyntree indices) and + * their YARP representation, which consist in a pair: Control Board index and joint index inside + * the its Control Board. + * + * @see getJointsMapString + * @see getJointsMapIndex + * + * @return True if the map has been created successfully + */ + bool mapDoFs(); +public: + + // CONSTRUCTOR / DESTRUCTOR + // ======================== + + RobotInterface() = delete; + RobotInterface(const wbt::Configuration& c); + ~RobotInterface(); + + // GET METHODS + // =========== + + /** + * Get the current configuration + * + * @return A reference of the Configuration object containing the current configuraton + */ + const wbt::Configuration& getConfiguration() const; + + /** + * Get the map between model joint namesand the YARP representation (Control Board and + * joint index) + * + * @return The joint map + */ + const std::shared_ptr getJointsMapString(); + + /** + * Get the map between model joint indices and the YARP representation (Control Board and + * joint index) + * + * @return The joint map + */ + const std::shared_ptr getJointsMapIndex(); + + /** + * Get the object to operate on the configured model + * + * @return A \c shared_ptr to the KinDynComputations object + */ + const std::shared_ptr getKinDynComputations(); + + /** + * Get a \c weak_ptr to an interface from the RemoteControlBoardRemapper + * + * param interface [out] A \c weak_ptr to the interface + * @return True if the \c weak_ptr is valid + * @tparam T The type of interface + */ + template + bool getInterface(std::weak_ptr& interface); + + // LAZY EVALUATION + // =============== + + /** + * Handles the internal counter for using the RemoteControlBoardRemapper + * + * @attention All the blocks which need to use any of the interfaces provided by + * wbt::YarpDevices must call this function in their initialize() method. + * @see releaseRemoteControlBoardRemapper + * + * @return True if success + */ + bool retainRemoteControlBoardRemapper(); + + /** + * Handles the internal counter for using the RemoteControlBoardRemapper. After the call + * from the last instance which retained the object, the RemoteControlBoardRemapper and all + * the allocated drivers get destroyed. + * + * @note On the contrary of retainRemoteControlBoardRemapper, this method is already + * called in wbt::~WBBlock + * @see retainRemoteControlBoardRemapper + * + * @return True if success + */ + bool releaseRemoteControlBoardRemapper(); +}; + +#endif /* end of include guard: WBT_ROBOTINTERFACE_H */ diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp new file mode 100644 index 000000000..59633f63b --- /dev/null +++ b/toolbox/src/base/RobotInterface.cpp @@ -0,0 +1,447 @@ +#include "RobotInterface.h" +#include "Log.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wbt { + static std::string vectorToString(std::vector v, std::string prefix=""); + + // The declaration of the following template specializations are required only by GCC + using namespace yarp::dev; + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(std::weak_ptr& interface); +} + +using namespace wbt; + +// CONSTRUCTOR / DESTRUCTOR +// ======================== + + +RobotInterface::RobotInterface(const wbt::Configuration& c) +: m_config(c) +, m_robotDeviceCounter(0) +{} + +RobotInterface::~RobotInterface() +{ + // Asserts for debugging purpose. + + // - 1 if at least one block asked the model. At this point only the shared_ptr + // of m_kinDynComp should be still alive (--> 1) + // - 0 if no block asked for the model. m_kinDynComp was never allocated. + assert(m_kinDynComp.use_count() <= 1); + + // m_robotDevice should be destroyed by the last releaseCB() + assert(!m_robotDevice); + assert(m_robotDeviceCounter == 0); +} + +bool RobotInterface::mapDoFs() +{ + std::unique_ptr controlBoard; + std::vector> iAxisInfos; + yarp::os::Property options; + + for (unsigned cbNum = 0; cbNum < m_config.getControlBoardsNames().size(); ++cbNum) { + // Configure the single control board + options.clear(); + options.put("device","remotecontrolboard"); + const std::string prefix = "/" + m_config.getRobotName() + "/"; + const std::string remoteName = prefix + m_config.getControlBoardsNames().at(cbNum); + options.put("remote", remoteName); + options.put("localPortPrefix", "WBTtmp"); + + // Try to open the control board + if (!controlBoard->open(options) || !controlBoard->isValid()) { + Log::getSingleton().error("Unable to open RemoteControlBoard " + remoteName); + return false; + } + + // Get an IAxisInfo object from the device + std::unique_ptr iAxisInfo; + yarp::dev::IAxisInfo* iAxisInfoPtr = iAxisInfo.get(); + controlBoard->view(iAxisInfoPtr); + if (!iAxisInfoPtr) { + Log::getSingleton().error("Unable to open IAxisInfo from " + remoteName); + return false; + } + + // Get an IEncoders object from the device + // This is used to get how many joints the control board contains + std::unique_ptr iEnc; + yarp::dev::IEncoders* iEncPtr = iEnc.get(); + controlBoard->view(iEncPtr); + int numAxes; + if (!iEncPtr || !iEncPtr->getAxes(&numAxes)) { + Log::getSingleton().error("Unable to open IEncoders from " + remoteName); + return false; + } + + // Iterate all the joints in the selected Control Board + for (unsigned axis = 0; axis < numAxes; ++axis) { + std::string axisName; + if (!iAxisInfoPtr->getAxisName(axis, axisName)) { + Log::getSingleton().error("Unable to get AxisName from " + remoteName); + return false; + } + // Look if axisName is a controlledJoint + bool found = false; + for (const auto& controlledJoint : m_config.getControlledJoints()) { + if (controlledJoint == axisName) { + // Get the iDynTree index + const auto& model = getKinDynComputations()->model(); + iDynTree::LinkIndex iDynLinkIdx = model.getLinkIndex(axisName); + if (iDynLinkIdx == iDynTree::LINK_INVALID_INDEX) { + Log::getSingleton().error("Joint " + axisName + " exists in the " + + remoteName + "control board but not in the model."); + return false; + } + // If this is the first entry to add, allocate the objects + if (!m_jointsMapIndex) { + m_jointsMapIndex = std::make_shared(); + } + if (!m_jointsMapString) { + m_jointsMapString = std::make_shared(); + } + // Create a new entry in the map objects + m_jointsMapString->at(controlledJoint) = {cbNum, axis}; + m_jointsMapIndex->at(static_cast(iDynLinkIdx)) = {cbNum, axis}; + found = true; + break; + } + } + // Notify that the control board just checked is not used by any joint + // of the controlledJoints list + if (!found) { + Log::getSingleton().warning("No controlled joints found in " + + m_config.getControlBoardsNames().at(cbNum) + + " control board. It might be unsed."); + } + } + } + + if (!controlBoard->close()) { + Log::getSingleton().error("Unable to close the device of the Control Board."); + return false; + } + return true; +} + +// GET METHODS +// =========== + +const wbt::Configuration& RobotInterface::getConfiguration() const +{ + return m_config; +} + +const std::shared_ptr RobotInterface::getJointsMapString() +{ + if (m_jointsMapString->empty()) { + if (!mapDoFs()) { + Log::getSingleton().error("Failed to create the Yarp - iDynTree joint map."); + return nullptr; + } + } + + assert (m_jointsMapString); + return m_jointsMapString; +} + +const std::shared_ptr RobotInterface::getJointsMapIndex() +{ + if (m_jointsMapIndex->empty()) { + if (!mapDoFs()) { + Log::getSingleton().error("Failed to create the Yarp - iDynTree joint map."); + return nullptr; + } + } + + assert (m_jointsMapIndex); + return m_jointsMapIndex; +} + +const std::shared_ptr RobotInterface::getKinDynComputations() +{ + if (m_kinDynComp) { + return m_kinDynComp; + } + + // Otherwise, initialize a new object + if (initializeModel()) { + return m_kinDynComp; + } + + // Return an empty shared_ptr (implicitly initialized) + return nullptr; +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iControlMode2); + return !interface.expired(); +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iPositionControl); + return !interface.expired(); +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iPositionDirect); + return !interface.expired(); +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iVelocityControl); + return !interface.expired(); +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iTorqueControl); + return !interface.expired(); +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iPWMControl); + return !interface.expired(); +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iCurrentControl); + return !interface.expired(); +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iEncoders); + return !interface.expired(); +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iControlLimits2); + return !interface.expired(); +} + +template <> +bool RobotInterface::getInterface(std::weak_ptr& interface) +{ + interface = getDevice(m_yarpDevices.iPidControl); + return !interface.expired(); +} + +// LAZY EVALUATION +// =============== + +bool RobotInterface::retainRemoteControlBoardRemapper() +{ + if (m_robotDeviceCounter > 0) { + m_robotDeviceCounter++; + return true; + } + + assert(!m_robotDevice); + if (m_robotDevice) { + m_robotDevice.reset(); + } + + if (!initializeRemoteControlBoardRemapper()) { + return false; + } + + m_robotDeviceCounter++; + return true; +} + +bool RobotInterface::releaseRemoteControlBoardRemapper() +{ + // The RemoteControlBoardRemapper has not been used + if (m_robotDeviceCounter == 0) { + return true; + } + + // If there are at most 2 blocks with th CB still used + if (m_robotDeviceCounter > 1) { + m_robotDeviceCounter--; + return true; + } + + // This should be executed by the last block which uses CB (m_robotDeviceCounter=1) + assert(m_robotDevice); + if (m_robotDevice) { + // Free all the drivers + m_yarpDevices.iControlMode2.reset(); + m_yarpDevices.iPositionControl.reset(); + m_yarpDevices.iPositionDirect.reset(); + m_yarpDevices.iVelocityControl.reset(); + m_yarpDevices.iTorqueControl.reset(); + m_yarpDevices.iPWMControl.reset(); + m_yarpDevices.iCurrentControl.reset(); + m_yarpDevices.iEncoders.reset(); + m_yarpDevices.iControlLimits2.reset(); + m_yarpDevices.iPidControl.reset(); + // Close the device + m_robotDevice->close(); + // Free the object + m_robotDevice.reset(); + } + + m_robotDeviceCounter = 0; + return true; +} + +// INITIALIZATION HELPERS +// ====================== + +bool RobotInterface::initializeModel() +{ + assert (!m_kinDynComp); + + // Allocate the object + m_kinDynComp = std::make_shared(); + if (!m_kinDynComp) return false; + + // Use RF to load the urdf file + // ---------------------------- + + // Initialize RF + // Workaround for the fact that ResourceFinder initializes the network by itself. See YARP#1014 + using namespace yarp::os; + Network network; + ResourceFinder& rf = ResourceFinder::getResourceFinderSingleton(); + rf.configure(0, 0); + + // Get the absolute path of the urdf file + std::string urdf_file = getConfiguration().getUrdfFile(); + std::string urdf_file_path = rf.findFile(urdf_file.c_str()); + + // Load the reduced model into KinDynComputations + // ---------------------------------------------- + + // Load the joint list + std::vector controlledJoints = getConfiguration().getControlledJoints(); + + // Use ModelLoader to load the reduced model + iDynTree::ModelLoader mdlLoader; + if (!mdlLoader.loadReducedModelFromFile(urdf_file_path, controlledJoints)) { + Log::getSingleton().error("ToolboxSingleton: impossible to load " + urdf_file + "."); + Log::getSingleton().error("Probably the joint list contains an entry not present in the urdf model."); + return false; + } + + // Add the loaded model to the KinDynComputations object + return m_kinDynComp->loadRobotModel(mdlLoader.model()); +} + +bool RobotInterface::initializeRemoteControlBoardRemapper() +{ + // Setup the RemoteControlBoardRemapper + yarp::os::Property options; + options.put("device", "remotecontrolboardremapper"); + options.put("axesNames", vectorToString(m_config.getControlledJoints())); + std::string prefix = "/" + m_config.getRobotName() + "/"; + options.put("remoteControlBoards", vectorToString(m_config.getControlBoardsNames(), prefix)); + options.put("localPortPrefix", m_config.getLocalName()); + yarp::os::Property& remoteCBOpts = options.addGroup("REMOTE_CONTROLBOARD_OPTIONS"); + remoteCBOpts.put("writeStrict","on"); + + // Allocate the device driver + m_robotDevice = std::make_shared(); + + if (!m_robotDevice) { + return false; + } + + // Open the device driver + if (m_robotDevice->open(options) && m_robotDevice->isValid()) { + return true; + } + + // Remove garbage if the opening fails + m_robotDevice.reset(); + return false; +} + +// OTHER METHODS +// ============= + +template +std::weak_ptr RobotInterface::getDevice(std::shared_ptr device) +{ + if (!device) { + // Blocks which require the RemoteControlBoardRemapper need to retain / release it + // in their initialization / terminate phase; + assert(m_robotDevice); + if (!m_robotDevice) { + // Return an empty weak pointer + return std::weak_ptr(); + } + + T* ptr = nullptr; + if (!m_robotDevice->view(ptr)) { + // Return an empty weak_ptr + return std::weak_ptr(); + } + // Store ptr into the smart pointer + device.reset(ptr); + } + // Implicit conversion from shared_ptr to weak_ptr + return device; +} + +/** + * Converts a vector of strings into a string format \code (element1 element2 element3) \endcode. + * It optioanlly support appending a prefix to the elements. e.g. + * \code ({prefix}element1 {prefix}element2 {prefix}element3) + * + * @param v The vector of strings + * @param prefix The optional element prefix + * @return The serialized string + */ +std::string wbt::vectorToString(std::vector v, std::string prefix) +{ + // Add the prefix if specified + if (!prefix.empty()) { + for (auto& str : v) { + str = prefix + str; + } + } + + // Create a string in a format which YARP likes: (item1 item2 item3) + std::stringstream output; + std::ostream_iterator output_iterator(output, " "); + std::copy(v.begin(), v.end(), output_iterator); + return "(" + output.str() + ")"; +} From af1fa4106a4597254b28b4f3eaecb41261600124 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 11:14:43 +0000 Subject: [PATCH 10/89] ClockServer thrift messages are now a static library This is a temporary fix. The cpp file should be generated by its thrift source, which should be found already installed in the system. A new CMake option e.g. USE_GAZEBO should be added. --- toolbox/CMakeLists.txt | 10 +++-- toolbox/autogenerated/CMakeLists.txt | 38 +++++++++++++++++++ .../autogenerated/src/thrift/ClockServer.cpp | 4 +- 3 files changed, 45 insertions(+), 7 deletions(-) create mode 100644 toolbox/autogenerated/CMakeLists.txt diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index f084744c7..8da7e2e47 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -143,13 +143,11 @@ configure_block(BLOCK_NAME "Yarp Clock" SOURCES src/YarpClock.cpp HEADERS include/YarpClock.h) -# TODO: this should be removed with a proper inclusion of the library -include_directories(SYSTEM "autogenerated/include/") configure_block(BLOCK_NAME "Simulator Synchronizer" GROUP "Utilities" LIST_PREFIX WBT - SOURCES src/SimulatorSynchronizer.cpp autogenerated/src/thrift/ClockServer.cpp - HEADERS include/SimulatorSynchronizer.h autogenerated/include/thrift/ClockServer.h) + SOURCES src/SimulatorSynchronizer.cpp + HEADERS include/SimulatorSynchronizer.h) configure_block(BLOCK_NAME "Mass Matrix" GROUP "Model" @@ -275,6 +273,10 @@ else() target_link_libraries(WBToolbox ${LINKED_LIBRARIES} ${Matlab_LIBRARIES}) endif() +# Link with ClockServer library +add_subdirectory(autogenerated/) +target_link_libraries(WBToolbox PUBLIC ClockServer) + # Remote Inverse Kinematics requires C++11 if (CMAKE_VERSION VERSION_LESS "3.1") if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR diff --git a/toolbox/autogenerated/CMakeLists.txt b/toolbox/autogenerated/CMakeLists.txt new file mode 100644 index 000000000..3ce96c1c5 --- /dev/null +++ b/toolbox/autogenerated/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_policy(SET CMP0048 NEW) +project(ClockServer LANGUAGES CXX VERSION 0.1) +cmake_minimum_required(VERSION 3.0.2) + +# Configure the project +# ===================== +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +include(GNUInstallDirs) +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +# Use a general prefix for the project +set(VARS_PREFIX ${PROJECT_NAME}) + +# Build the library +# ================= + +# Export the includes needed to who inherits this library +# Set this up properly for handling either BUILD and INSTALL trees +set(${VARS_PREFIX}_INCLUDE_BUILD ${CMAKE_CURRENT_SOURCE_DIR}/include) +set(${VARS_PREFIX}_INCLUDE_INSTALL ${CMAKE_INSTALL_INCLUDEDIR}/${VARS_PREFIX}) + +# Add the target +add_library(${VARS_PREFIX} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/include/thrift/ClockServer.h + ${CMAKE_CURRENT_SOURCE_DIR}/src/thrift/ClockServer.cpp) + +if (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") + set_target_properties(${VARS_PREFIX} PROPERTIES COMPILE_FLAGS "-fPIC") +endif() + +# Setup the include directories +target_include_directories(${VARS_PREFIX} PUBLIC + $ + $) + +# YARP +find_package(YARP REQUIRED) +target_include_directories(${VARS_PREFIX} PUBLIC ${YARP_INCLUDE_DIRS}) diff --git a/toolbox/autogenerated/src/thrift/ClockServer.cpp b/toolbox/autogenerated/src/thrift/ClockServer.cpp index 30ced2c9d..64d4cf3e8 100644 --- a/toolbox/autogenerated/src/thrift/ClockServer.cpp +++ b/toolbox/autogenerated/src/thrift/ClockServer.cpp @@ -1,7 +1,7 @@ // This is an automatically-generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. -#include +#include "thrift/ClockServer.h" #include namespace gazebo { @@ -382,5 +382,3 @@ std::vector ClockServer::help(const std::string& functionName) { return helpString; } } // namespace - - From 1d71a8628a3721213555da85780ce7f6aecd7c6a Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 12:18:16 +0000 Subject: [PATCH 11/89] Substituted C-style cast with static_cast in Signal.cpp This commit fixes also some error in computing the address used in memcpy calls --- toolbox/src/base/Signal.cpp | 92 +++++++++++++++++++++++++++++-------- 1 file changed, 73 insertions(+), 19 deletions(-) diff --git a/toolbox/src/base/Signal.cpp b/toolbox/src/base/Signal.cpp index de33aaf15..86c40a89d 100644 --- a/toolbox/src/base/Signal.cpp +++ b/toolbox/src/base/Signal.cpp @@ -42,38 +42,92 @@ namespace wbt { Data data; switch (portType) { case PortDataTypeDouble: - data.doubleData(isContiguous ? ((double*)contiguousData)[index] : *((double **)nonContiguousData)[index]); + if (isContiguous) { + data.doubleData((static_cast(contiguousData))[index]); + } + else { + const double* buffer = static_cast(*nonContiguousData); + data.doubleData(static_cast(buffer[index])); + } break; case PortDataTypeSingle: - data.floatData(isContiguous ? ((float*)contiguousData)[index] : *((float **)nonContiguousData)[index]); + if (isContiguous) { + data.floatData((static_cast(contiguousData))[index]); + } + else { + const float* buffer = static_cast(*nonContiguousData); + data.floatData(static_cast(buffer[index])); + } break; case PortDataTypeInt8: - data.int8Data(isContiguous ? ((int8_t*)contiguousData)[index] : *((int8_t **)nonContiguousData)[index]); + if (isContiguous) { + data.int8Data((static_cast(contiguousData))[index]); + } + else { + const int8_t* buffer = static_cast(*nonContiguousData); + data.int8Data(static_cast(buffer[index])); + } break; case PortDataTypeUInt8: - data.uint8Data(isContiguous ? ((uint8_t*)contiguousData)[index] : *((uint8_t **)nonContiguousData)[index]); + if (isContiguous) { + data.uint8Data((static_cast(contiguousData))[index]); + } + else { + const uint8_t* buffer = static_cast(*nonContiguousData); + data.uint8Data(static_cast(buffer[index])); + } break; case PortDataTypeInt16: - data.int8Data(isContiguous ? ((int16_t*)contiguousData)[index] : *((int16_t **)nonContiguousData)[index]); + if (isContiguous) { + data.int16Data((static_cast(contiguousData))[index]); + } + else { + const int16_t* buffer = static_cast(*nonContiguousData); + data.int16Data(static_cast(buffer[index])); + } break; case PortDataTypeUInt16: - data.uint8Data(isContiguous ? ((uint16_t*)contiguousData)[index] : *((uint16_t **)nonContiguousData)[index]); + if (isContiguous) { + data.uint16Data((static_cast(contiguousData))[index]); + } + else { + const uint16_t* buffer = static_cast(*nonContiguousData); + data.uint16Data(static_cast(buffer[index])); + } break; case PortDataTypeInt32: - data.int8Data(isContiguous ? ((int32_t*)contiguousData)[index] : *((int32_t **)nonContiguousData)[index]); + if (isContiguous) { + data.int32Data((static_cast(contiguousData))[index]); + } + else { + const int32_t* buffer = static_cast(*nonContiguousData); + data.int32Data(static_cast(buffer[index])); + } break; case PortDataTypeUInt32: - data.uint8Data(isContiguous ? ((uint32_t*)contiguousData)[index] : *((uint32_t **)nonContiguousData)[index]); + if (isContiguous) { + data.uint32Data((static_cast(contiguousData))[index]); + } + else { + const uint32_t* buffer = static_cast(*nonContiguousData); + data.uint32Data(static_cast(buffer[index])); + } break; case PortDataTypeBoolean: - data.booleanData(isContiguous ? ((bool*)contiguousData)[index] : *((bool **)nonContiguousData)[index]); + if (isContiguous) { + data.booleanData((static_cast(contiguousData))[index]); + } + else { + const bool* buffer = static_cast(*nonContiguousData); + data.booleanData(static_cast(buffer[index])); + } } return data; } void* Signal::getContiguousBuffer() { - if (!isContiguous) return 0; + if (!isContiguous) return nullptr; return this->contiguousData; } @@ -110,13 +164,13 @@ namespace wbt { case PortDataTypeDouble: { dataSize = sizeof(double); - address = data + startIndex; + address = static_cast(address) + startIndex; break; } case PortDataTypeSingle: { dataSize = sizeof(float); - address = ((float*)data) + startIndex; + address = static_cast(address) + startIndex; break; } default: @@ -164,19 +218,19 @@ namespace wbt { case PortDataTypeInt32: { dataSize = sizeof(int32_t); - address = ((int32_t*)data) + startIndex; + address = static_cast(address) + startIndex; break; } case PortDataTypeInt16: { dataSize = sizeof(int16_t); - address = ((int16_t*)data) + startIndex; + address = static_cast(address) + startIndex; break; } case PortDataTypeInt8: { dataSize = sizeof(int8_t); - address = ((int8_t*)data) + startIndex; + address = static_cast(address) + startIndex; break; } default: @@ -223,19 +277,19 @@ namespace wbt { case PortDataTypeUInt32: { dataSize = sizeof(uint32_t); - address = ((uint32_t*)data) + startIndex; + address = data + startIndex; break; } case PortDataTypeUInt16: { dataSize = sizeof(uint16_t); - address = ((uint16_t*)data) + startIndex; + address = data + startIndex; break; } case PortDataTypeUInt8: { dataSize = sizeof(uint8_t); - address = ((uint8_t*)data) + startIndex; + address = data + startIndex; break; } default: @@ -255,7 +309,7 @@ namespace wbt { { if (isConstPort) return; unsigned dataSize = sizeof(bool); - const void * address = ((bool*)data) + startIndex; + const void* address = static_cast(data) + startIndex; memcpy(contiguousData, address, dataSize * length); } From 19cf4d6fe51b7e4a8df2df5a6413bb538d53c691 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 13:49:23 +0000 Subject: [PATCH 12/89] Switch to AnyType interface to handle data read by BlockInformation Furthermore: - All methods that read parameters return a bool value - optionFromKey doesn't use anymore wbt::Data - setNumberOfOuputPorts() became setNumberOfOutputPorts() --- toolbox/include/base/BlockInformation.h | 31 ++++++---- .../include/base/SimulinkBlockInformation.h | 13 +++-- toolbox/src/base/BlockInformation.cpp | 2 +- toolbox/src/base/SimulinkBlockInformation.cpp | 57 +++++++++++-------- 4 files changed, 63 insertions(+), 40 deletions(-) diff --git a/toolbox/include/base/BlockInformation.h b/toolbox/include/base/BlockInformation.h index b16461d89..f49ba843c 100644 --- a/toolbox/include/base/BlockInformation.h +++ b/toolbox/include/base/BlockInformation.h @@ -1,6 +1,7 @@ #ifndef WBT_BLOCKINFORMATION_H #define WBT_BLOCKINFORMATION_H +#include "AnyType.h" #include #include @@ -20,8 +21,8 @@ namespace wbt { PortDataTypeBoolean, } PortDataType; - - struct Data { + class Data + { private: double buffer; public: @@ -59,7 +60,8 @@ class wbt::BlockInformation { public: virtual ~BlockInformation(); - //Block Options methods + // Block Options methods + // ===================== /** * Convert a block option from its Toolbox identifier to a specific implementation @@ -68,10 +70,12 @@ class wbt::BlockInformation { * @param [out] option implementation-specific block option * @return true if the option has been converted. False otherwise */ - virtual bool optionFromKey(const std::string& key, Data &option) const; + virtual bool optionFromKey(const std::string& key, double& option) const; + + // Parameters methods + // ================== - //Parameters methods /** * Reads the parameter at the specified index and interpret it as a string * @@ -81,12 +85,16 @@ class wbt::BlockInformation { * @return true if success, false otherwise */ virtual bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) = 0; - virtual Data getScalarParameterAtIndex(unsigned parameterIndex) = 0; + virtual bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) = 0; + virtual bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) = 0; + virtual bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) = 0; + virtual bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) = 0; + + // Port information methods + // ======================== - //Port information methods virtual bool setNumberOfInputPorts(unsigned numberOfPorts) = 0; - virtual bool setNumberOfOuputPorts(unsigned numberOfPorts) = 0; - //If portSize == -1, then it is dynamic + virtual bool setNumberOfOutputPorts(unsigned numberOfPorts) = 0; virtual bool setInputPortVectorSize(unsigned portNumber, int portSize) = 0; virtual bool setInputPortMatrixSize(unsigned portNumber, int rows, int columns) = 0; virtual bool setOutputPortVectorSize(unsigned portNumber, int portSize) = 0; @@ -103,12 +111,13 @@ class wbt::BlockInformation { virtual bool setInputPortType(unsigned portNumber, PortDataType portType) = 0; virtual bool setOutputPortType(unsigned portNumber, PortDataType portType) = 0; - //Port data + // Port data + // ========= + virtual unsigned getInputPortWidth(unsigned portNumber) = 0; virtual unsigned getOutputPortWidth(unsigned portNumber) = 0; virtual wbt::Signal getInputPortSignal(unsigned portNumber) = 0; virtual wbt::Signal getOutputPortSignal(unsigned portNumber) = 0; - }; #endif /* end of include guard: WBT_BLOCKINFORMATION_H */ diff --git a/toolbox/include/base/SimulinkBlockInformation.h b/toolbox/include/base/SimulinkBlockInformation.h index 40b3c36c6..9c26b23b1 100644 --- a/toolbox/include/base/SimulinkBlockInformation.h +++ b/toolbox/include/base/SimulinkBlockInformation.h @@ -2,8 +2,8 @@ #define WBT_SIMULINKBLOCKINFORMATION_H #include "BlockInformation.h" - #include "simstruc.h" +#include "AnyType.h" namespace wbt { class SimulinkBlockInformation; @@ -18,21 +18,24 @@ class wbt::SimulinkBlockInformation : public wbt::BlockInformation { virtual ~SimulinkBlockInformation(); - virtual bool optionFromKey(const std::string& key, Data &option) const; + bool optionFromKey(const std::string& key, double& option) const override; //Parameters methods - virtual bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter); - virtual Data getScalarParameterAtIndex(unsigned parameterIndex); + bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) override; + bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) override; + bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) override; + bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) override; + bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) override; //Port information methods virtual bool setNumberOfInputPorts(unsigned numberOfPorts); - virtual bool setNumberOfOuputPorts(unsigned numberOfPorts); virtual bool setInputPortVectorSize(unsigned portNumber, int portSize); virtual bool setInputPortMatrixSize(unsigned portNumber, int rows, int columns); virtual bool setOutputPortVectorSize(unsigned portNumber, int portSize); virtual bool setOutputPortMatrixSize(unsigned portNumber, int rows, int columns); virtual bool setInputPortType(unsigned portNumber, PortDataType portType); virtual bool setOutputPortType(unsigned portNumber, PortDataType portType); + bool setNumberOfOutputPorts(unsigned numberOfPorts) override; //Port data virtual unsigned getInputPortWidth(unsigned portNumber); diff --git a/toolbox/src/base/BlockInformation.cpp b/toolbox/src/base/BlockInformation.cpp index c2fc14f0d..5fa160eef 100644 --- a/toolbox/src/base/BlockInformation.cpp +++ b/toolbox/src/base/BlockInformation.cpp @@ -3,4 +3,4 @@ const std::string wbt::BlockOptionPrioritizeOrder = "wbt.BlockOptionPrioritizeOrder"; wbt::BlockInformation::~BlockInformation() {} -bool wbt::BlockInformation::optionFromKey(const std::string& key, wbt::Data &option) const { return false; } +bool wbt::BlockInformation::optionFromKey(const std::string& key, double& option) const { return false; } diff --git a/toolbox/src/base/SimulinkBlockInformation.cpp b/toolbox/src/base/SimulinkBlockInformation.cpp index 85dc6593d..9f1f69f71 100644 --- a/toolbox/src/base/SimulinkBlockInformation.cpp +++ b/toolbox/src/base/SimulinkBlockInformation.cpp @@ -1,8 +1,10 @@ #include "SimulinkBlockInformation.h" #include "Signal.h" - +#include "MxAnyType.h" #include "simstruc.h" +#include +#include namespace wbt { @@ -11,10 +13,10 @@ namespace wbt { SimulinkBlockInformation::~SimulinkBlockInformation() {} - bool SimulinkBlockInformation::optionFromKey(const std::string& key, Data &option) const + bool SimulinkBlockInformation::optionFromKey(const std::string& key, double& option) const { if (key == wbt::BlockOptionPrioritizeOrder) { - option.uint32Data(SS_OPTION_PLACE_ASAP); + option = SS_OPTION_PLACE_ASAP; return true; } @@ -24,26 +26,33 @@ namespace wbt { //Parameters methods bool SimulinkBlockInformation::getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) { - int_T buflen, status; - char *buffer = NULL; + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asString(stringParameter); + } - //robot name - buflen = (1 + mxGetN(ssGetSFcnParam(simstruct, parameterIndex))) * sizeof(mxChar); - buffer = static_cast(mxMalloc(buflen)); - status = mxGetString((ssGetSFcnParam(simstruct, parameterIndex)), buffer, buflen); - if (status) { - return false; - } - stringParameter = buffer; - mxFree(buffer); buffer = NULL; - return true; + bool SimulinkBlockInformation::getStructAtIndex(unsigned parameterIndex, AnyStruct& map) + { + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asAnyStruct(map); + } + + + bool SimulinkBlockInformation::getVectorAtIndex(unsigned parameterIndex, std::vector& vec) + { + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asVectorDouble(vec); + } + + bool SimulinkBlockInformation::getScalarParameterAtIndex(unsigned parameterIndex, double& value) + { + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asDouble(value); } - Data SimulinkBlockInformation::getScalarParameterAtIndex(unsigned parameterIndex) + bool SimulinkBlockInformation::getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) { - Data data; - data.doubleData(mxGetScalar(ssGetSFcnParam(simstruct, parameterIndex))); - return data; + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asBool(value); } //Port information methods @@ -52,7 +61,7 @@ namespace wbt { return ssSetNumInputPorts(simstruct, numberOfPorts); } - bool SimulinkBlockInformation::setNumberOfOuputPorts(unsigned numberOfPorts) + bool SimulinkBlockInformation::setNumberOfOutputPorts(unsigned numberOfPorts) { return ssSetNumOutputPorts(simstruct, numberOfPorts); } @@ -170,7 +179,8 @@ namespace wbt { { Signal signal; InputPtrsType port = ssGetInputPortSignalPtrs(simstruct, portNumber); - signal.initSignalType(PortDataTypeDouble, true); + bool isConstPort = true; + signal.initSignalType(PortDataTypeDouble, isConstPort); signal.setNonContiguousBuffer(port); return signal; } @@ -178,9 +188,10 @@ namespace wbt { wbt::Signal SimulinkBlockInformation::getOutputPortSignal(unsigned portNumber) { Signal signal; - signal.initSignalType(PortDataTypeDouble, false); + bool isConstPort = false; + signal.initSignalType(PortDataTypeDouble, isConstPort); signal.setContiguousBuffer(ssGetOutputPortSignal(simstruct, portNumber)); return signal; } - + } From f0973693c675e28491c50091288126b730985464 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 14:26:05 +0000 Subject: [PATCH 13/89] WBIBlock refactored and renamed to WBBlock This new WBBlock implements the same functionalities of the former WBIBlock and WBIModelBlock blocks. Resources are lazy-handled, and it is not anymore necessary having two different setups. Furthermore: - The parameters from Simulink are stored in the ToolboxSingleton in the configureSizeAndPorts() instead of initialize(). This is possible because singletons are not deleted by Simulink between these two steps (Blocks instead are deleted). --- toolbox/CMakeLists.txt | 4 +- toolbox/include/base/WBBlock.h | 82 +++++++++++ toolbox/include/base/WBIBlock.h | 48 ------- toolbox/src/base/WBBlock.cpp | 241 ++++++++++++++++++++++++++++++++ toolbox/src/base/WBIBlock.cpp | 140 ------------------- 5 files changed, 325 insertions(+), 190 deletions(-) create mode 100644 toolbox/include/base/WBBlock.h delete mode 100644 toolbox/include/base/WBIBlock.h create mode 100644 toolbox/src/base/WBBlock.cpp delete mode 100644 toolbox/src/base/WBIBlock.cpp diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 8da7e2e47..855625d45 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -61,7 +61,7 @@ configure_block(BLOCK_NAME "Base" SOURCES src/base/toolbox.cpp src/base/Block.cpp src/base/BlockInformation.cpp - src/base/WBIBlock.cpp + src/base/WBBlock.cpp src/base/Log.cpp src/base/WBInterface.cpp src/base/factory.cpp @@ -72,7 +72,7 @@ configure_block(BLOCK_NAME "Base" HEADERS include/base/toolbox.h include/base/Block.h include/base/BlockInformation.h - include/base/WBIBlock.h + include/base/WBBlock.h include/base/Log.h include/base/WBInterface.h include/base/SimulinkBlockInformation.h #this is temp diff --git a/toolbox/include/base/WBBlock.h b/toolbox/include/base/WBBlock.h new file mode 100644 index 000000000..cee7d7d11 --- /dev/null +++ b/toolbox/include/base/WBBlock.h @@ -0,0 +1,82 @@ +#ifndef WBT_WBIBLOCK_H +#define WBT_WBIBLOCK_H + +#include "Block.h" +#include +#include +#include +#include +#include +#include + +namespace wbt { + class WBBlock; + class Configuration; + class BlockInformation; + class RobotInterface; +} + +namespace iDynTree { + class MatrixDynSize; +} + +/** + * \struct iDynTreeRobotState WBBlock.h + * + * This struct contains the iDynTree objects used to configure the + * state of iDynTree::KinDynComputations objects. + */ +struct iDynTreeRobotState { + iDynTree::Twist m_baseVelocity; + iDynTree::Vector3 m_gravity; + iDynTree::Transform m_world_T_base; + iDynTree::VectorDynSize m_jointsVelocity; + iDynTree::VectorDynSize m_jointsPosition; + + iDynTreeRobotState() = default; + ~iDynTreeRobotState() = default; + + iDynTreeRobotState(const unsigned& dofs, const std::vector& gravity); +}; + +/** + * Basic class for Whole-Body related blocks. + * This class (the whole toolbox in reality) assumes the block represent + * an instantaneous system (i.e. not a dynamic system). + * + * You can create a new block by deriving this class and implementing at least + * the output method. + * + * This block implements the following default behaviours: + * - it ask for 4 parameters (robot name, local (module) name, names of the remote control boards, + * and the list of joints which should all belong to one of the remote control boards) + * - It initializes the yarp network and the whole body interface object + * - During terminate it closes and release the interface object and terminate the yarp network + * + * @note Usually you want to call this class implementations at some point in your + * method overridings, unless you want to completely change the code (but at that point + * you probabily want to derive from Block instead) + */ +class wbt::WBBlock : public wbt::Block +{ +private: + static const unsigned ConfigurationParameterIndex; + static const unsigned ConfBlockNameParameterIndex; + +protected: + std::string confKey; + iDynTreeRobotState robotState; + bool getWBToolboxParameters(Configuration& config, BlockInformation* blockInfo); + const std::shared_ptr getRobotInterface(); + const Configuration& getConfiguration(); + +public: + WBBlock() = default; + ~WBBlock() override = default; + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; +}; + +#endif /* end of include guard: WBT_WBIBLOCK_H */ diff --git a/toolbox/include/base/WBIBlock.h b/toolbox/include/base/WBIBlock.h deleted file mode 100644 index 33ffe9c36..000000000 --- a/toolbox/include/base/WBIBlock.h +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef WBT_WBIBLOCK_H -#define WBT_WBIBLOCK_H - -#include "Block.h" -#include - -namespace wbt { - class WBIBlock; - class BlockInformation; -} - -/** - * Basic class for WBI related blocks. - * This class (the whole toolbox in reality) assumes the block represent - * an instantaneous system (i.e. not a dynamic system). - * - * You can create a new block by deriving this class and implementing at least - * the output method. - * - * This block implements the following default behaviours: - * - it ask for 4 parameters (robot name, local (module) name, wbi config file and wbi list) - * - It initializes the yarp network and the whole body interface object - * - During terminate it closes and release the interface object and terminate the yarp network - * - * @note Usually you want to call this class implementations at some point in your - * method overridings, unless you want to completely change the code (but at that point - * you probabily want to derive from Block instead) - */ -class wbt::WBIBlock : public wbt::Block { - -protected: - - std::string m_wbiConfigurationFileName; - std::string m_wbiListName; - - bool configureWBIParameters(BlockInformation *blockInfo, wbt::Error *error); - -public: - WBIBlock(); - virtual ~WBIBlock(); - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); -}; - - -#endif /* end of include guard: WBT_WBIBLOCK_H */ diff --git a/toolbox/src/base/WBBlock.cpp b/toolbox/src/base/WBBlock.cpp new file mode 100644 index 000000000..27f9913f2 --- /dev/null +++ b/toolbox/src/base/WBBlock.cpp @@ -0,0 +1,241 @@ +#include "WBBlock.h" + +#include "BlockInformation.h" +#include "Log.h" +#include "ToolboxSingleton.h" +#include "Configuration.h" +#include "RobotInterface.h" +#include "AnyType.h" +#include +#include +#include +#include +#include +#include + +using namespace wbt; + +const unsigned WBBlock::ConfigurationParameterIndex = 1; // Struct from Simulink +const unsigned WBBlock::ConfBlockNameParameterIndex = 2; // Absolute name of the block containing the + // configuration + +iDynTreeRobotState::iDynTreeRobotState(const unsigned& dofs, const std::vector& gravity) +: m_gravity(gravity.data(), 3) +, m_jointsVelocity(dofs) +, m_jointsPosition(dofs) +{ + m_jointsPosition.zero(); + m_jointsVelocity.zero(); +} + +unsigned WBBlock::numberOfParameters() { return 2; } + +bool WBBlock::getWBToolboxParameters(Configuration& config, BlockInformation* blockInfo) +{ + // Infos + // ===== + // + // For what concern initialization, the callback order is: + // + // -> configureSizeAndPorts() + // -> initialize() + // -> initializeInitialConditions() + // + // Despite class objects after the configureSizeAndPorts() are destroyed and + // reallocated before the initialize(), the ToolboxSingleton remains alive and + // can store from this stage the configuration created by reading the parameters. + // + + // Gather the parameters from the Mask of the Simulink Block + // ========================================================= + + // Get the struct containing the parameters + AnyStruct s; + if (!blockInfo->getStructAtIndex(ConfigurationParameterIndex, s)) { + Log::getSingleton().error("Failed to retrieve the struct with parameters."); + return false; + } + + // Check the existence of all the fields + try { + s.at("RobotName"); + s.at("UrdfFile"); + s.at("ControlledJoints"); + s.at("ControlBoardsNames"); + s.at("LocalName"); + s.at("GravityVector"); + } + catch (const std::out_of_range& e) { + Log::getSingleton().error("Cannot retrieve one or more parameter from config struct."); + return false; + } + + // RobotName + std::string robotName; + if (!s["RobotName"]->asString(robotName)) { + Log::getSingleton().error("Cannot retrieve string from RobotName parameter."); + return false; + } + + // UrdfFile + std::string urdfFile; + if (!s["UrdfFile"]->asString(urdfFile)) { + Log::getSingleton().error("Cannot retrieve string from UrdfFile parameter."); + return false; + } + + // ControlledJoints + AnyCell cellCJ; + if (!s["ControlledJoints"]->asAnyCell(cellCJ)) { + Log::getSingleton().error("Cannot retrieve ControlledJoints parameter."); + return false; + } + + std::vector controlledJoints; + for (auto cell: cellCJ) { + std::string joint; + if (!cell->asString(joint)) { + Log::getSingleton().error("Failed to convert ControlledJoints from cell to strings."); + return false; + } + controlledJoints.push_back(joint); + } + + // ControlBoardsNames + AnyCell cellCBN; + if (!s["ControlBoardsNames"]->asAnyCell(cellCBN)) { + Log::getSingleton().error("Cannot retrieve ControlBoardsNames parameter."); + return false; + } + + std::vector controlBoardsNames; + for (auto cell: cellCBN) { + std::string joint; + if (!cell->asString(joint)) { + Log::getSingleton().error("Failed to convert ControlBoardsNames from cell to string."); + return false; + } + controlBoardsNames.push_back(joint); + } + + // LocalName + std::string localName; + if (!s["LocalName"]->asString(localName)) { + Log::getSingleton().error("Cannot retrieve string from LocalName parameter."); + return false; + } + + std::vector gravityVector; + if (!s["GravityVector"]->asVectorDouble(gravityVector)) { + Log::getSingleton().error("Cannot retrieve vector from GravityVector parameter."); + return false; + } + + // Create the ToolboxConfig object + // =============================== + + // Populate a Property object that stores the input parameters + config.setRobotName(robotName); + config.setUrdfFile(urdfFile); + config.setControlledJoints(controlledJoints); + config.setControlBoardsNames(controlBoardsNames); + config.setLocalName(localName); + config.setGravityVector(gravityVector); + + return true; +} + +const Configuration& WBBlock::getConfiguration() +{ + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + return interface.getConfiguration(confKey); +} + +const std::shared_ptr WBBlock::getRobotInterface() +{ + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + return interface.getRobotInterface(confKey); +} + +bool WBBlock::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Remember not allocate / save any data in this function, it won't be persistent. + + // Setup the ToolboxSingleton + // ========================== + + // Setup the DOFs (required by childs for configuring port sizes) + // -------------------------------------------------------------- + + // Initialize the ToolboxSingleton + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + + // Get the absolute name of the block from which the configuration has been retrieved. + // This will be the key of the map containing all the configurations of the ToolboxSingleton. + if (!blockInfo->getStringParameterAtIndex(ConfBlockNameParameterIndex, confKey)) { + Log::getSingleton().error("Failed to retrieve the struct with parameters."); + return false; + } + + // Get the configuration struct from the block + Configuration config; + if (!getWBToolboxParameters(config, blockInfo)) { + return false; + } + + // Check if the configuration is valid + if (!config.isValid()) { + Log::getSingleton().error("The provided configuration is not valid."); + return false; + } + + // Store the configuration inside the ToolboxSingleton + if (!interface.storeConfiguration(confKey, config)) { + Log::getSingleton().error("Failed to configure ToolboxSingleton."); + return false; + } + + // Check if the DoFs are positive (and if the config has been stored successfully) + const Configuration& configFromSingleton = getConfiguration(); + + if (configFromSingleton.getNumberOfDoFs() < 1) { + Log::getSingleton().error("Failed to configure WBBlock. Read 0 DoFs."); + return false; + } + + return true; +} + +bool WBBlock::initialize(BlockInformation* blockInfo) +{ + // CONFIGURE the ToolboxSingleton + // ============================== + + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + + // Gather again confKey + // Note: despite data for blocks is not persistent between configureSizeAndPorts() + // and initialize() (e.g. confKey), the singleton is not destroyed. + // This means that the stored configurations are still there. + if (!blockInfo->getStringParameterAtIndex(ConfBlockNameParameterIndex, confKey)) { + Log::getSingleton().error("Failed to retrieve the struct with parameters."); + return false; + } + + // Check if the key is valid + if (!interface.isKeyValid(confKey)) { + Log::getSingleton().error("Failed to retrieve the configuration object from the singleton."); + return false; + } + + // Initialize the iDynTreeRobotState struct + const unsigned& dofs = interface.numberOfDoFs(confKey); + robotState = iDynTreeRobotState(dofs, getConfiguration().getGravityVector()); + + return true; +} + +bool WBBlock::terminate(BlockInformation* /*blockInfo*/) +{ + return true; +} diff --git a/toolbox/src/base/WBIBlock.cpp b/toolbox/src/base/WBIBlock.cpp deleted file mode 100644 index 475ffee97..000000000 --- a/toolbox/src/base/WBIBlock.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "WBIBlock.h" - -#include "BlockInformation.h" -#include "Error.h" -#include "WBInterface.h" -#include - -#define PARAM_IDX_1 1 // robot name -#define PARAM_IDX_2 2 // local (module) name -#define PARAM_IDX_3 3 // wbi config file -#define PARAM_IDX_4 4 // wbi list - -wbt::WBIBlock::WBIBlock() -: m_wbiConfigurationFileName("") -, m_wbiListName("") {} - -wbt::WBIBlock::~WBIBlock() { } - -unsigned wbt::WBIBlock::numberOfParameters() { return 4; } - -bool wbt::WBIBlock::configureWBIParameters(BlockInformation *blockInfo, wbt::Error *error) -{ - //parameters needed by this block: - // - YARP_ROBOT_NAME: needed by resource finder for resource lookup (for now it is taken by the environment) - // - robot: robot port. If defined overrides the one specified by wbi file - // - moduleName: local (opened) ports. - // - wbi config file name (default: yarpWholeBodyInterface.ini): specifies the wbi config file - // - wbi list (default ROBOT_TORQUE_CONTROL_JOINTS): specifies the WBI list . - // It it is a normal string, it is interpreted as a named list of joints that is loaded from the - // yarpWholeBodyInterface.ini file. If instead it s - - - //robot name - std::string robotName; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_1, robotName)) { - if (error) error->message = "Cannot retrieve string from robot parameter"; - return false; - } - - //local name - std::string localName; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_2, localName)) { - if (error) error->message = "Cannot retrieve string from localName parameter"; - return false; - } - //default local name - if (localName.empty()) localName = "WBIT"; - - //wbi config file - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_3, m_wbiConfigurationFileName)) { - if (error) error->message = "Cannot retrieve string from WBI config file parameter"; - return false; - } - - //default config file: - if (m_wbiConfigurationFileName.empty()) m_wbiConfigurationFileName = "yarpWholeBodyInterface.ini"; - - //wbi list name - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_4, m_wbiListName)) { - if (error) error->message = "Cannot retrieve string from WBI list parameter"; - return false; - } - - //default list: - if (m_wbiListName.empty()) m_wbiListName = "ROBOT_TORQUE_CONTROL_JOINTS"; - - WBInterface &interface = WBInterface::sharedInstance(); - - Error interfaceError; - if (!interface.configure(robotName, localName, m_wbiConfigurationFileName, m_wbiListName, &interfaceError)) { - if (error) error->message = "Failed to configure WholeBodyInterface with error " + interfaceError.message; - return false; - } - return true; -} - -bool wbt::WBIBlock::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) -{ - //wbi config file - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_3, m_wbiConfigurationFileName)) { - if (error) error->message = "Could not read WBI configuration file parameter"; - return false; - } - - //default config file: - if (m_wbiConfigurationFileName.empty()) m_wbiConfigurationFileName = "yarpWholeBodyInterface.ini"; - - //wbi list name - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_4, m_wbiListName)) { - if (error) error->message = "Could not read WBI list parameter"; - return false; - } - - //default list: - if (m_wbiListName.empty()) m_wbiListName = "ROBOT_TORQUE_CONTROL_JOINTS"; - - WBInterface &interface = WBInterface::sharedInstance(); - - int dofs = interface.dofsForConfigurationFileAndList(m_wbiConfigurationFileName, m_wbiListName); - if (dofs == -1) { - if (error) error->message = "Failed to configure WholeBodyInterface. Could not load WBI properties from file"; - return false; - } else if (dofs == -2) { - if (error) error->message = "Failed to configure WholeBodyInterface. WBI joint list not found or failed to configure. Check if joint list exists."; - return false; - } - - return true; -} - -bool wbt::WBIBlock::initialize(BlockInformation *blockInfo, wbt::Error *error) -{ - using namespace yarp::os; - Network::init(); - if (!Network::initialized() || !Network::checkNetwork(5.0)) { - if (error) error->message = "YARP server wasn't found active"; - return false; - } - - if (!configureWBIParameters(blockInfo, error)) { - return false; - } - - WBInterface &interface = WBInterface::sharedInstance(); - if (!interface.initialize()) { - if (error) error->message = "Failed to initialize WBI"; - return false; - } - return true; -} - -bool wbt::WBIBlock::terminate(BlockInformation */*S*/, wbt::Error *error) -{ - if (!WBInterface::sharedInstance().terminate()) { - if (error) error->message = "Failed to terminate WBI"; - return false; - } - yarp::os::Network::fini(); - return true; -} From 04531f146dd55050a5c11e9b270558c540c8a552 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 10 Nov 2017 14:47:40 +0000 Subject: [PATCH 14/89] Minor edits of base/ classes - Removed old Error parameter from methods - Small documentation changes - Small style edits --- toolbox/include/base/Block.h | 31 ++++++------- toolbox/include/base/BlockInformation.h | 4 +- toolbox/include/base/Signal.h | 19 ++++---- .../include/base/SimulinkBlockInformation.h | 33 +++++++------- toolbox/src/base/Block.cpp | 8 ++-- toolbox/src/base/Signal.cpp | 43 +++++++++--------- toolbox/src/base/SimulinkBlockInformation.cpp | 8 ++-- toolbox/src/base/factory.cpp | 2 +- toolbox/src/base/toolbox.cpp | 44 +++++++++---------- 9 files changed, 92 insertions(+), 100 deletions(-) diff --git a/toolbox/include/base/Block.h b/toolbox/include/base/Block.h index b65f52a7b..a233859e6 100644 --- a/toolbox/include/base/Block.h +++ b/toolbox/include/base/Block.h @@ -6,7 +6,6 @@ namespace wbt { class Block; - class Error; class BlockInformation; } @@ -15,14 +14,15 @@ namespace wbt { * Basic abstract class for all the blocks. * This class (the whole toolbox in reality) assumes the block represent * an instantaneous system (i.e. not a dynamic system). - * + * * You can create a new block by deriving this class and implementing at least * all the pure virtual methods. * * @Note: if you need to implement a block which uses the WBI, you should derive * from WBIBlock as it already provides some facilities. */ -class wbt::Block { +class wbt::Block +{ public: /** * Create and returns a new Block object of the specified class. @@ -39,7 +39,7 @@ class wbt::Block { * */ virtual ~Block(); - + /** * Returns the number of configuration parameters needed by this block * @@ -59,7 +59,7 @@ class wbt::Block { * Returns the number of discrete states of the block. * * The base implementation returns 0, i.e. no discrete states - * @note if you return a number > 0, you should implement the + * @note if you return a number > 0, you should implement the * updateDiscreteState function * @return the number of discrete states */ @@ -82,7 +82,7 @@ class wbt::Block { * @param S the SimStruct structure * @return true for success, false otherwise */ - virtual bool updateDiscreteState(BlockInformation *blockInfo, wbt::Error *error); + virtual bool updateDiscreteState(BlockInformation* blockInfo); /** * Not called for now @@ -90,7 +90,7 @@ class wbt::Block { * @param S the SimStruct structure * @return true for success, false otherwise */ - virtual bool stateDerivative(BlockInformation *blockInfo, wbt::Error *error); + virtual bool stateDerivative(BlockInformation* blockInfo); /** @@ -111,21 +111,19 @@ class wbt::Block { * their size and configuration. * @Note: you should not save any object in this method because it will not persist * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. Check if the pointer exists before dereference it. * * @return true for success, false otherwise */ - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) = 0; + virtual bool configureSizeAndPorts(BlockInformation* blockInfo) = 0; /** * Never called. * * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. Check if the pointer exists before dereference it. * * @return true for success, false otherwise */ - virtual bool checkParameters(BlockInformation *blockInfo, wbt::Error *error); + virtual bool checkParameters(BlockInformation* blockInfo); /** * Initialize the object for the simulation @@ -133,11 +131,10 @@ class wbt::Block { * This method is called at model startup (i.e. during mdlStart) * @Note: you can save and initialize your object in this method * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. * * @return true for success, false otherwise */ - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error) = 0; + virtual bool initialize(BlockInformation* blockInfo) = 0; /** * Called to initialize the initial conditions @@ -146,11 +143,10 @@ class wbt::Block { * @note this function is also called on a reset event * @note if you need to perform initialization only once, than implement initialize * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. * * @return true for success, false otherwise */ - virtual bool initializeInitialConditions(BlockInformation *blockInfo, wbt::Error *error); + virtual bool initializeInitialConditions(BlockInformation* blockInfo); /** * Perform model cleanup. @@ -158,11 +154,10 @@ class wbt::Block { * This method is called at model termination (i.e. during mdlTerminate). * You should release all the resources you requested during the initialize method * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. * * @return true for success, false otherwise */ - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error) = 0; + virtual bool terminate(BlockInformation* blockInfo) = 0; @@ -175,7 +170,7 @@ class wbt::Block { * * @return true for success, false otherwise */ - virtual bool output(BlockInformation *blockInfo, wbt::Error *error) = 0; + virtual bool output(BlockInformation* blockInfo) = 0; public: diff --git a/toolbox/include/base/BlockInformation.h b/toolbox/include/base/BlockInformation.h index f49ba843c..a4ea9da0a 100644 --- a/toolbox/include/base/BlockInformation.h +++ b/toolbox/include/base/BlockInformation.h @@ -66,9 +66,9 @@ class wbt::BlockInformation { /** * Convert a block option from its Toolbox identifier to a specific implementation * - * @param [in] key identifier of the block option + * @param [in] key identifier of the block option * @param [out] option implementation-specific block option - * @return true if the option has been converted. False otherwise + * @return true if the option has been converted. False otherwise */ virtual bool optionFromKey(const std::string& key, double& option) const; diff --git a/toolbox/include/base/Signal.h b/toolbox/include/base/Signal.h index 97f567669..b46d70ecd 100644 --- a/toolbox/include/base/Signal.h +++ b/toolbox/include/base/Signal.h @@ -7,7 +7,9 @@ namespace wbt { class Signal; } -class wbt::Signal { +class wbt::Signal +{ +private: PortDataType portType; bool isContiguous; bool isConstPort; @@ -16,31 +18,30 @@ class wbt::Signal { void* contiguousData; public: - Signal(); + Signal() = default; void initSignalType(wbt::PortDataType type, bool constPort); - void setContiguousBuffer(void* buffer); void setContiguousBuffer(const void* buffer); void setNonContiguousBuffer(void** buffer); - void setNonContiguousBuffer(const void* const * buffer); + void setNonContiguousBuffer(const void* const* buffer); const Data get(unsigned index) const; void* getContiguousBuffer(); - + //the missing are cast void set(unsigned index, double data); - void setBuffer(const double *data, const unsigned length, unsigned startIndex = 0); + void setBuffer(const double* data, const unsigned length, unsigned startIndex = 0); void set(unsigned index, int32_t data); - void setBuffer(const int32_t *data, const unsigned length, unsigned startIndex = 0); + void setBuffer(const int32_t* data, const unsigned length, unsigned startIndex = 0); void set(unsigned index, uint32_t data); - void setBuffer(const uint32_t *data, const unsigned length, unsigned startIndex = 0); + void setBuffer(const uint32_t* data, const unsigned length, unsigned startIndex = 0); void set(unsigned index, bool data); - void setBuffer(const bool *data, const unsigned length, unsigned startIndex = 0); + void setBuffer(const bool* data, const unsigned length, unsigned startIndex = 0); }; diff --git a/toolbox/include/base/SimulinkBlockInformation.h b/toolbox/include/base/SimulinkBlockInformation.h index 9c26b23b1..2334d32a4 100644 --- a/toolbox/include/base/SimulinkBlockInformation.h +++ b/toolbox/include/base/SimulinkBlockInformation.h @@ -10,13 +10,14 @@ namespace wbt { class Signal; } -class wbt::SimulinkBlockInformation : public wbt::BlockInformation { - SimStruct *simstruct; +class wbt::SimulinkBlockInformation : public wbt::BlockInformation +{ +private: + SimStruct* simstruct; public: - SimulinkBlockInformation(SimStruct *simstruct); - - virtual ~SimulinkBlockInformation(); + SimulinkBlockInformation(SimStruct* simstruct); + ~SimulinkBlockInformation() override = default; bool optionFromKey(const std::string& key, double& option) const override; @@ -28,20 +29,20 @@ class wbt::SimulinkBlockInformation : public wbt::BlockInformation { bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) override; //Port information methods - virtual bool setNumberOfInputPorts(unsigned numberOfPorts); - virtual bool setInputPortVectorSize(unsigned portNumber, int portSize); - virtual bool setInputPortMatrixSize(unsigned portNumber, int rows, int columns); - virtual bool setOutputPortVectorSize(unsigned portNumber, int portSize); - virtual bool setOutputPortMatrixSize(unsigned portNumber, int rows, int columns); - virtual bool setInputPortType(unsigned portNumber, PortDataType portType); - virtual bool setOutputPortType(unsigned portNumber, PortDataType portType); + bool setNumberOfInputPorts(unsigned numberOfPorts) override; bool setNumberOfOutputPorts(unsigned numberOfPorts) override; + bool setInputPortVectorSize(unsigned portNumber, int portSize) override; + bool setInputPortMatrixSize(unsigned portNumber, int rows, int columns) override; + bool setOutputPortVectorSize(unsigned portNumber, int portSize) override; + bool setOutputPortMatrixSize(unsigned portNumber, int rows, int columns) override; + bool setInputPortType(unsigned portNumber, PortDataType portType) override; + bool setOutputPortType(unsigned portNumber, PortDataType portType) override; //Port data - virtual unsigned getInputPortWidth(unsigned portNumber); - virtual unsigned getOutputPortWidth(unsigned portNumber); - virtual wbt::Signal getInputPortSignal(unsigned portNumber); - virtual wbt::Signal getOutputPortSignal(unsigned portNumber); + unsigned getInputPortWidth(unsigned portNumber) override; + unsigned getOutputPortWidth(unsigned portNumber) override; + wbt::Signal getInputPortSignal(unsigned portNumber) override; + wbt::Signal getOutputPortSignal(unsigned portNumber) override; }; #endif /* end of include guard: WBT_SIMULINKBLOCKINFORMATION_H */ diff --git a/toolbox/src/base/Block.cpp b/toolbox/src/base/Block.cpp index 396e1084d..b7d36396a 100644 --- a/toolbox/src/base/Block.cpp +++ b/toolbox/src/base/Block.cpp @@ -4,12 +4,12 @@ wbt::Block::~Block() {} std::vector wbt::Block::additionalBlockOptions() { return std::vector(); } void wbt::Block::parameterAtIndexIsTunable(unsigned /*index*/, bool &tunable) { tunable = false; } -bool wbt::Block::checkParameters(wbt::BlockInformation */*blockInfo*/, wbt::Error */*error*/) { return true; } +bool wbt::Block::checkParameters(wbt::BlockInformation* /*blockInfo*/) { return true; } unsigned wbt::Block::numberOfDiscreteStates() { return 0; } unsigned wbt::Block::numberOfContinuousStates() { return 0; } -bool wbt::Block::updateDiscreteState(wbt::BlockInformation */*blockInfo*/, wbt::Error */*error*/) { return true; } -bool wbt::Block::stateDerivative(wbt::BlockInformation */*blockInfo*/, wbt::Error */*error*/) { return true; } +bool wbt::Block::updateDiscreteState(wbt::BlockInformation* /*blockInfo*/) { return true; } +bool wbt::Block::stateDerivative(wbt::BlockInformation* /*blockInfo*/) { return true; } -bool wbt::Block::initializeInitialConditions(wbt::BlockInformation */*blockInfo*/, wbt::Error */*error*/) { return true; } +bool wbt::Block::initializeInitialConditions(wbt::BlockInformation* /*blockInfo*/) { return true; } diff --git a/toolbox/src/base/Signal.cpp b/toolbox/src/base/Signal.cpp index 86c40a89d..5d57938fc 100644 --- a/toolbox/src/base/Signal.cpp +++ b/toolbox/src/base/Signal.cpp @@ -4,9 +4,6 @@ namespace wbt { - - Signal::Signal() {} - void Signal::initSignalType(wbt::PortDataType type, bool constPort) { this->portType = type; @@ -30,7 +27,7 @@ namespace wbt { this->isContiguous = false; } - void Signal::setNonContiguousBuffer(const void* const * buffer) + void Signal::setNonContiguousBuffer(const void* const* buffer) { nonContiguousData = const_cast(buffer); this->isContiguous = false; @@ -139,13 +136,13 @@ namespace wbt { switch (portType) { case PortDataTypeDouble: { - double *buffer = static_cast(contiguousData); + double* buffer = static_cast(contiguousData); buffer[index] = data; break; } case PortDataTypeSingle: { - float *buffer = static_cast(contiguousData); + float* buffer = static_cast(contiguousData); buffer[index] = data; break; } @@ -154,11 +151,11 @@ namespace wbt { } } - void Signal::setBuffer(const double *data, const unsigned length, unsigned startIndex) + void Signal::setBuffer(const double* data, const unsigned length, unsigned startIndex) { if (isConstPort) return; unsigned dataSize = 0; - const void * address = data; + const void* address = data; switch (portType) { case PortDataTypeDouble: @@ -187,19 +184,19 @@ namespace wbt { switch (portType) { case PortDataTypeInt32: { - int32_t *buffer = static_cast(contiguousData); + int32_t* buffer = static_cast(contiguousData); buffer[index] = data; break; } case PortDataTypeInt16: { - int16_t *buffer = static_cast(contiguousData); + int16_t* buffer = static_cast(contiguousData); buffer[index] = data; break; } case PortDataTypeInt8: { - int8_t *buffer = static_cast(contiguousData); + int8_t* buffer = static_cast(contiguousData); buffer[index] = data; break; } @@ -208,11 +205,11 @@ namespace wbt { } } - void Signal::setBuffer(const int32_t *data, const unsigned length, unsigned startIndex) + void Signal::setBuffer(const int32_t* data, const unsigned length, unsigned startIndex) { if (isConstPort) return; unsigned dataSize = 0; - const void * address = data; + const void* address = data; switch (portType) { case PortDataTypeInt32: @@ -237,7 +234,7 @@ namespace wbt { break; } - memcpy(contiguousData, address, dataSize * length); + memcpy(contiguousData, address, dataSize* length); } void Signal::set(unsigned index, uint32_t data) @@ -246,19 +243,19 @@ namespace wbt { switch (portType) { case PortDataTypeUInt32: { - uint32_t *buffer = static_cast(contiguousData); + uint32_t* buffer = static_cast(contiguousData); buffer[index] = data; break; } case PortDataTypeUInt16: { - uint16_t *buffer = static_cast(contiguousData); + uint16_t* buffer = static_cast(contiguousData); buffer[index] = data; break; } case PortDataTypeUInt8: { - uint8_t *buffer = static_cast(contiguousData); + uint8_t* buffer = static_cast(contiguousData); buffer[index] = data; break; } @@ -267,11 +264,11 @@ namespace wbt { } } - void Signal::setBuffer(const uint32_t *data, const unsigned length, unsigned startIndex) + void Signal::setBuffer(const uint32_t* data, const unsigned length, unsigned startIndex) { if (isConstPort) return; unsigned dataSize = 0; - const void * address = data; + const void* address = data; switch (portType) { case PortDataTypeUInt32: @@ -296,22 +293,22 @@ namespace wbt { break; } - memcpy(contiguousData, address, dataSize * length); + memcpy(contiguousData, address, dataSize* length); } void Signal::set(unsigned index, bool data) { - bool *buffer = static_cast(contiguousData); + bool* buffer = static_cast(contiguousData); buffer[index] = data; } - void Signal::setBuffer(const bool *data, const unsigned length, unsigned startIndex) + void Signal::setBuffer(const bool* data, const unsigned length, unsigned startIndex) { if (isConstPort) return; unsigned dataSize = sizeof(bool); const void* address = static_cast(data) + startIndex; - memcpy(contiguousData, address, dataSize * length); + memcpy(contiguousData, address, dataSize* length); } diff --git a/toolbox/src/base/SimulinkBlockInformation.cpp b/toolbox/src/base/SimulinkBlockInformation.cpp index 9f1f69f71..48ccdde51 100644 --- a/toolbox/src/base/SimulinkBlockInformation.cpp +++ b/toolbox/src/base/SimulinkBlockInformation.cpp @@ -1,5 +1,4 @@ #include "SimulinkBlockInformation.h" - #include "Signal.h" #include "MxAnyType.h" #include "simstruc.h" @@ -8,10 +7,9 @@ namespace wbt { - SimulinkBlockInformation::SimulinkBlockInformation(SimStruct *S) - : simstruct(S) {} - - SimulinkBlockInformation::~SimulinkBlockInformation() {} + SimulinkBlockInformation::SimulinkBlockInformation(SimStruct* S) + : simstruct(S) + {} bool SimulinkBlockInformation::optionFromKey(const std::string& key, double& option) const { diff --git a/toolbox/src/base/factory.cpp b/toolbox/src/base/factory.cpp index 9ff5aac03..69a962e08 100644 --- a/toolbox/src/base/factory.cpp +++ b/toolbox/src/base/factory.cpp @@ -3,7 +3,7 @@ wbt::Block* wbt::Block::instantiateBlockWithClassName(std::string blockClassName) { wbt::Block *block = NULL; - + if (blockClassName == wbt::YarpRead::ClassName) { block = new wbt::YarpRead(); } else if (blockClassName == wbt::YarpWrite::ClassName) { diff --git a/toolbox/src/base/toolbox.cpp b/toolbox/src/base/toolbox.cpp index 2e2d9037f..ba9c86cb5 100644 --- a/toolbox/src/base/toolbox.cpp +++ b/toolbox/src/base/toolbox.cpp @@ -32,7 +32,7 @@ #include #include -static void catchLogMessages(bool status, SimStruct *S, std::string prefix) +static void catchLogMessages(bool status, SimStruct* S, std::string prefix) { // Initialize static buffers const unsigned bufferLen = 1024; @@ -97,7 +97,7 @@ static void catchLogMessages(bool status, SimStruct *S, std::string prefix) // Function: MDL_CHECK_PARAMETERS #define MDL_CHECK_PARAMETERS #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) -static void mdlCheckParameters(SimStruct *S) +static void mdlCheckParameters(SimStruct* S) { UNUSED_ARG(S); //TODO: still to find a way to call Block implementation @@ -105,8 +105,8 @@ static void mdlCheckParameters(SimStruct *S) #endif /*MDL_CHECK_PARAMETERS*/ #define MDL_SET_INPUT_PORT_DIMENSION_INFO -static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, - const DimsInfo_T *dimsInfo) +static void mdlSetInputPortDimensionInfo(SimStruct* S, int_T port, + const DimsInfo_T* dimsInfo) { //TODO: for now accept the proposed size. //If we want to change the behaviour we have to implement some callbacks @@ -114,8 +114,8 @@ static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, } #define MDL_SET_OUTPUT_PORT_DIMENSION_INFO -static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, - const DimsInfo_T *dimsInfo) +static void mdlSetOutputPortDimensionInfo(SimStruct* S, int_T port, + const DimsInfo_T* dimsInfo) { //TODO: for now accept the proposed size. //If we want to change the behaviour we have to implement some callbacks @@ -126,7 +126,7 @@ static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, // Abstract: // The sizes information is used by Simulink to determine the S-function // block's characteristics (number of inputs, s, states, etc.). -static void mdlInitializeSizes(SimStruct *S) +static void mdlInitializeSizes(SimStruct* S) { // Initialize the Log singleton wbt::Log::getSingleton().clear(); @@ -136,10 +136,10 @@ static void mdlInitializeSizes(SimStruct *S) catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); return; } - char *classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); + char* classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); std::string className(classNameStr); mxFree(classNameStr); - wbt::Block *block = wbt::Block::instantiateBlockWithClassName(className); + wbt::Block* block = wbt::Block::instantiateBlockWithClassName(className); //We cannot save data in PWork during the initializeSizes phase ssSetNumPWork(S, 1); @@ -211,7 +211,7 @@ static void mdlInitializeSizes(SimStruct *S) // This function is used to specify the sample time(s) for your // S-function. You must register the same number of sample times as // specified in ssSetNumSampleTimes. -static void mdlInitializeSampleTimes(SimStruct *S) +static void mdlInitializeSampleTimes(SimStruct* S) { ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME); ssSetOffsetTime(S, 0, 0.0); @@ -224,12 +224,12 @@ static void mdlInitializeSampleTimes(SimStruct *S) // have states that should be initialized once, this is the place // to do it. #define MDL_START -static void mdlStart(SimStruct *S) +static void mdlStart(SimStruct* S) { - char *classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); + char* classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); std::string className(classNameStr); mxFree(classNameStr); - wbt::Block *block = wbt::Block::instantiateBlockWithClassName(className); + wbt::Block* block = wbt::Block::instantiateBlockWithClassName(className); ssSetPWorkValue(S, 0, block); wbt::SimulinkBlockInformation blockInfo(S); @@ -243,11 +243,11 @@ static void mdlStart(SimStruct *S) #define MDL_UPDATE #if defined(MDL_UPDATE) && defined(MATLAB_MEX_FILE) -static void mdlUpdate(SimStruct *S, int_T tid) +static void mdlUpdate(SimStruct* S, int_T tid) { UNUSED_ARG(tid); if (ssGetNumPWork(S) > 0) { - wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); + wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); wbt::SimulinkBlockInformation blockInfo(S); bool ok = false; @@ -262,10 +262,10 @@ static void mdlUpdate(SimStruct *S, int_T tid) //Initialize the state vectors of this C MEX S-function #define MDL_INITIALIZE_CONDITIONS #if defined(MDL_INITIALIZE_CONDITIONS) && defined(MATLAB_MEX_FILE) -static void mdlInitializeConditions(SimStruct *S) +static void mdlInitializeConditions(SimStruct* S) { if (ssGetNumPWork(S) > 0) { - wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); + wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); wbt::SimulinkBlockInformation blockInfo(S); bool ok = false; @@ -280,7 +280,7 @@ static void mdlInitializeConditions(SimStruct *S) #define MDL_DERIVATIVES #if defined(MDL_DERIVATIVES) && defined(MATLAB_MEX_FILE) -static void mdlDerivatives(SimStruct *S) +static void mdlDerivatives(SimStruct* S) { /* Add mdlDerivatives code here */ } @@ -291,11 +291,11 @@ static void mdlDerivatives(SimStruct *S) // Abstract: // In this function, you compute the outputs of your S-function // block. -static void mdlOutputs(SimStruct *S, int_T tid) +static void mdlOutputs(SimStruct* S, int_T tid) { UNUSED_ARG(tid); if (ssGetNumPWork(S) > 0) { - wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); + wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); wbt::SimulinkBlockInformation blockInfo(S); bool ok = false; @@ -306,10 +306,10 @@ static void mdlOutputs(SimStruct *S, int_T tid) } } -static void mdlTerminate(SimStruct *S) +static void mdlTerminate(SimStruct* S) { if (ssGetNumPWork(S) > 0 && ssGetPWork(S)) { - wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); + wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); wbt::SimulinkBlockInformation blockInfo(S); bool ok = false; From 639fb718b461f1bbf0d02f8d922026111dd30b33 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 14:37:43 +0000 Subject: [PATCH 15/89] WBInterface refactored and renamed to ToolboxSingleton The new ToolboxSingleton class contains in a persistent wat Configuration and RobotInterface objects. Most of the logic previously contained in WBInterface has been moved in these two new classes. --- toolbox/CMakeLists.txt | 4 +- toolbox/include/base/ToolboxSingleton.h | 143 +++++++++++ toolbox/include/base/WBInterface.h | 189 -------------- toolbox/src/base/ToolboxSingleton.cpp | 122 +++++++++ toolbox/src/base/WBInterface.cpp | 318 ------------------------ 5 files changed, 267 insertions(+), 509 deletions(-) create mode 100644 toolbox/include/base/ToolboxSingleton.h delete mode 100644 toolbox/include/base/WBInterface.h create mode 100644 toolbox/src/base/ToolboxSingleton.cpp delete mode 100644 toolbox/src/base/WBInterface.cpp diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 855625d45..d9fcf56eb 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -63,7 +63,7 @@ configure_block(BLOCK_NAME "Base" src/base/BlockInformation.cpp src/base/WBBlock.cpp src/base/Log.cpp - src/base/WBInterface.cpp + src/base/ToolboxSingleton.cpp src/base/factory.cpp src/base/SimulinkBlockInformation.cpp #this is temp src/base/Signal.cpp @@ -74,7 +74,7 @@ configure_block(BLOCK_NAME "Base" include/base/BlockInformation.h include/base/WBBlock.h include/base/Log.h - include/base/WBInterface.h + include/base/ToolboxSingleton.h include/base/SimulinkBlockInformation.h #this is temp include/base/Signal.h include/base/Configuration.h diff --git a/toolbox/include/base/ToolboxSingleton.h b/toolbox/include/base/ToolboxSingleton.h new file mode 100644 index 000000000..1140a2d76 --- /dev/null +++ b/toolbox/include/base/ToolboxSingleton.h @@ -0,0 +1,143 @@ +#ifndef WBT_ToolboxSingleton_H +#define WBT_ToolboxSingleton_H + +#include +#include +#include +#include +#include "Configuration.h" + +namespace wbt { + class ToolboxSingleton; + class RobotInterface; +} + +namespace iDynTree { + class KinDynComputations; +} + +// TODO: check class doxygen macro +/** + * \class ToolboxSingleton ToolboxSingleton.h + * + * This class handles and contains configuration and devices used by all the blocks + * allocated by a running model. It is possible obtaining a singleton of this class + * by calling WBToolbox::sharedInstance. + * + * @see wbt::RobotInterface + * @see wbt::Configuration + * @see wbt::yarpDevices + * + */ +class wbt::ToolboxSingleton { // TODO: wbt::ToolboxSingleton +private: + /// Object that stores all the configurations labelled by the name of the Simulink Block's + /// name. + /// @see wbt::RobotInterface + std::unordered_map> m_interfaces; + +public: + + // CONSTRUCTOR / DESTRUCTOR + // ======================== + + ToolboxSingleton(); + ~ToolboxSingleton(); + + ToolboxSingleton(const ToolboxSingleton&) = delete; + ToolboxSingleton& operator=(const ToolboxSingleton&) = delete; + + // UTILITIES + // ========= + + /** + * Check if a Configuration labelled by confKey exists and is a + * valid object. + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @return True if the configuration is valid + */ + bool isKeyValid(const std::string& confKey); + + /** + * Returns the DoFs associated with the configuration labelled by confKey + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @return The number of degrees of freedom. It returns -1 when failing. + */ + int numberOfDoFs(const std::string& confKey); + + // GET METHODS + // =========== + + /** + * Returns the singleton instance to this object. + * + * @note The singleton stays in memory until the entire matlab is closed. The WBToolbox + * is designed in such a way that assures to reach a clean state when all blocks + * have been terminated. + * + * @return the singleton instance + */ + static wbt::ToolboxSingleton& sharedInstance(); + + // TODO: check if doxygen ref work + + /** + * Returns the Configuration object labelled by confKey. + * This object is contained into the wbt::RobotInterface object. + * + * @see ToolboxSingleton::getRobotInterface + * @param confKey The key describing the configuration (name of the Simulink block) + * @return A constant reference to the Configuration object + */ + const Configuration& getConfiguration(const std::string& confKey); + + /** + * Returns a \c shared_ptr to the RobotInterface object containing the configuration + * labelled by confKey and all the objects used to gather and set robot data + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @return A \c shared_ptr to the RobotInterface of the requested configuration + */ + const std::shared_ptr getRobotInterface(const std::string& confKey); + + /** + * Returns a \c shared_ptr to the KinDynComputations object used to perform calculation + * on the provided model + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @return A \c shared_ptr to the iDynTree::KinDynComputations of the requested + * configuration + */ + const std::shared_ptr getModel(const std::string& confKey); + + // TOOLBOXSINGLETON CONFIGURATION + // ============================== + + /*! Saves in the singleton a new configuration \c config. + * + * If the config is valid and hasn't been already stored, it creates a new entry + * in ToolboxSingleton::m_interfaces. If a configuration with matching confKey is found, + * if the Configuration object is the same it does nothing, otherwise it overrides it. + * + * @note Since confKey is the name of the block, the overriding can happen only after + * subsequent compilation in Simulink, when the block's parameters have been + * changed. + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @param config The wbt::Configuration object parsed from Simulink's parameters + * @return Returns \c true if configure is successful, \c false otherwise + * @see ToolboxSingleton::isKeyValid + */ + bool storeConfiguration(const std::string& confKey, const Configuration& config); + + /** + * Delete the wbt::RobotInterface referred by confKey. No-op if it doesn't exist. + * + * @param confKey The key describing the configuration (name of the Simulink block) + */ + void eraseConfiguration(const std::string& confKey); +}; + +#endif /* end of include guard: WBT_ToolboxSingleton_H */ diff --git a/toolbox/include/base/WBInterface.h b/toolbox/include/base/WBInterface.h deleted file mode 100644 index bfae00cc9..000000000 --- a/toolbox/include/base/WBInterface.h +++ /dev/null @@ -1,189 +0,0 @@ -#ifndef WBT_WBINTERFACE_H -#define WBT_WBINTERFACE_H - -#include - -namespace wbt { - class WBInterface; - class Error; -} - -namespace wbi { - class wholeBodyInterface; - class iWholeBodyModel; - class IDList; -} -namespace yarp { - namespace os { - class Property; - } -} - -/** - * This class holds a reference counted handle to the whole body interface object. - * You can obtain the singleton reference to this object by calling the sharedInstance method. - * @note at the current state, and because of how the wbi is structured, the model and the interface are - * two different objects, even if this should not lead to any problem - */ -class wbt::WBInterface { - - // Private constructor, destructor, copy constructor and assignemnt operator - WBInterface(); - WBInterface(const WBInterface&); - WBInterface& operator=(const WBInterface&); - ~WBInterface(); - - - wbi::wholeBodyInterface* m_interface; /**< Reference to the interface object */ - wbi::iWholeBodyModel* m_model; /**< Reference to the model object */ - - wbi::IDList *m_robotList; /**< Reference to the joint list object */ - yarp::os::Property *m_configuration; /**< Reference to the configuration used to configure the interface */ - - bool m_initialized; /**< true if the interface has been initialized */ - bool m_modelInitialized; /**< true if the model has been initialized */ - bool m_configured; /**< true if the interface has been configured */ - int m_dofs; /**< dofs modelled by the interface */ - - std::string m_wbiFilePath; - std::string m_wbiListName; - - static unsigned s_referenceCount; /**< number of blocks currently initialized */ - static unsigned s_modelReferenceCount; /**< number of model blocks currently initialized */ - -public: - /** - * Returns the singleton instance to this object. - * - * This is the only way to obtain a reference to an instance of this class - * @return the singleton instance - */ - static wbt::WBInterface& sharedInstance(); - - /** - * Returns the constant pointer to the whole body interface object - * - * @return the whole body interface object - */ - wbi::wholeBodyInterface * const interface(); - - /** - * @return a weak pointer to the model; - */ - wbi::iWholeBodyModel * const model(); - - /** - * @return the current configuration used for the interface and/or model - */ - const yarp::os::Property * const currentConfiguration() const; - - /** - * Returns the degrees of freedom associated with the interface object - * - * @return degrees of freedom of the WBI object - */ - int numberOfDoFs() const; - - /** - * Load the used wbi::IDList from the WBI configuration and the list parameter - * - * @param wbiConfigFile wbi configuration file - * @param list either the name of joint list to be found in the wbi configuration file, - * or directly a list in the format (value1 value2 value3) - * @return true if loading the list was successful, false otherwise. - */ - static bool wbdIDListFromConfigPropAndList(const yarp::os::Property & wbiConfigProp, - const std::string & list, - wbi::IDList & idList); - - /** - * Returns the degrees of freedom for the specified configuration file and the list parameter - * - * @note this method also save the dofs in the internal state. It is thus possible to retrieve it - * by calling numberOfDofs. - * @todo: maybe we should transform this method into const? - * @param wbiConfigFile wbi configuration file - * @param list either the name of joint list to be found in the wbi configuration file, - * or directly a list in the format (Value1 value2 value3) - * - * @return the degrees of freedom of the specified list - */ - int dofsForConfigurationFileAndList(const std::string & wbiConfigFile, const std::string & list); - - /** - * Configure the interface with the specified paramters. - * - * @Note: the interface is configured only once. Subsequent calls are ignored. - * Call clearConfiguration() to clear the configuration. - * @param robotName name of the robot (The model will connect to port with this name as prefix). - * empty the one specified in the configuration file is taken - * @param localName name of the model (ports will be open with this name) - * @param wbiConfigFile name of the wbi configuration file. The file will be looked for by using - * yarp::os::ResourceFinder policies - * @param list either the name of joint list to be found in the wbi configuration file, - * or directly a list in the format (Value1 value2 value3) - * @param [out]error Pointer to the error object to be filled in case of error. - * Check for NULL before using it - * - * @return true if configure is successful, false otherwise. - */ - bool configure(const std::string &robotName, - const std::string & localName, - const std::string & wbiConfigFile, - const std::string & list, - wbt::Error *error); - - /** - * Clear the current configuration, so that configure can be called again. - */ - void clearConfiguration(); - - /** - * Initialize the whole body interface. - * - * This call has effect only the first time. Subsequent calls only increase the reference count - * @Note: Each initialize call should be matched by a subsequent call to terminate. - * @return: true if configure is successful, false otherwise. - */ - bool initialize(); - - /** - * Initialize the model. - * - * This call has effect only the first time. Subsequent calls only increase the reference count - * @Note: Each initialize call should be matched by a subsequent call to terminateModel. - * @return: true if configure is successful, false otherwise. - */ - bool initializeModel(); - - /** - * Release the resources associated with the whole body interface - * - * @Note: resources are released only if the call is made by the last object using the interface - * otherwise the reference count is decreased and no resources are relased. - * @return true if configure is successful, false otherwise. - */ - bool terminate(); - - /** - * Release the resources associated with the model - * - * @Note: resources are released only if the call is made by the last object using the interface - * otherwise the reference count is decreased and no resources are relased. - * @return true if configure is successful, false otherwise. - */ - bool terminateModel(); - - /** - * Returns true if the interface is initialized. False otherwise - * - * @return true if the interface is initialized, false otherwise - */ - bool isInterfaceInitialized() const; - - const wbi::IDList* wbiList() const; - const std::string& wbiFilePath() const; - const std::string& wbiListName() const; -}; - -#endif /* end of include guard: WBT_WBINTERFACE_H */ diff --git a/toolbox/src/base/ToolboxSingleton.cpp b/toolbox/src/base/ToolboxSingleton.cpp new file mode 100644 index 000000000..a3d7981b5 --- /dev/null +++ b/toolbox/src/base/ToolboxSingleton.cpp @@ -0,0 +1,122 @@ +#include "ToolboxSingleton.h" + +#include "Log.h" +#include "RobotInterface.h" +#include +#include +#include +#include +#include + +// TODO: remove nesting +namespace wbt { + + // CONSTRUCTOR / DESTRUCTOR + // ======================== + + ToolboxSingleton::ToolboxSingleton() {} + ToolboxSingleton::~ToolboxSingleton() {} + + // UTILITIES + // ========= + + int ToolboxSingleton::numberOfDoFs(const std::string& confKey) + { + if (!isKeyValid(confKey)) + return -1; + else + return m_interfaces[confKey]->getConfiguration().getNumberOfDoFs(); + } + + bool ToolboxSingleton::isKeyValid(const std::string& confKey) + { + if (m_interfaces.find(confKey) != m_interfaces.end()) { + if (m_interfaces[confKey]) + return true; + else + return false; + } + else { + return false; + } + } + + // GET METHODS + // =========== + + ToolboxSingleton& ToolboxSingleton::sharedInstance() + { + static ToolboxSingleton instance; + return instance; + } + + const Configuration& ToolboxSingleton::getConfiguration(const std::string& confKey) + { + return getRobotInterface(confKey)->getConfiguration(); + } + + const std::shared_ptr ToolboxSingleton::getRobotInterface(const std::string& confKey) + { + if (!isKeyValid(confKey)) { + return nullptr; + } + + return m_interfaces[confKey]; + } + + const std::shared_ptr ToolboxSingleton::getModel(const std::string& confKey) + { + if (!isKeyValid(confKey)) { + return nullptr; + } + + return m_interfaces[confKey]->getKinDynComputations(); + } + + // TOOLBOXSINGLETON CONFIGURATION + // ============================== + + bool ToolboxSingleton::storeConfiguration(const std::string& confKey, const Configuration& config) + { + if (!config.isValid()) { + return false; + } + + // Add the new Configuration object and override an existing key if it already exist. + // Note: Simulink doesn't flush memory unless Matlab is closed, and static objects stay in memory. + // This may cause problems if the config block's mask is changed after the first compilation. + if (m_interfaces.find(confKey) == m_interfaces.end()) { + m_interfaces[confKey] = std::make_shared(config); + return static_cast(m_interfaces[confKey]); + } + + if (!(m_interfaces[confKey]->getConfiguration() == config)) { + assert(m_interfaces[confKey]); + + // Check that at top there are 2 instances of the model: + // the private member of RobotInterface and the one returned + // by the model() called by the assert itself + assert(getModel(confKey).use_count() <= 2); + // TODO: and also m_robotDeviceCounter should be max 1 + // + // TODO: domani: quando finisce la simulazione (terminate) dovrei avere + // dentro tutti i Configuration al massimo 1 sia di model che di + // device (e le interfacce tutte chiuse?) + + // Delete the old configuration (calling the destructor for cleaning garbage) + m_interfaces[confKey].reset(); + m_interfaces.erase(confKey); + + // Allocate a new configuration + m_interfaces[confKey] = std::make_shared(config); + return static_cast(m_interfaces[confKey]); + } + + return true; + } + + void ToolboxSingleton::eraseConfiguration(const std::string& confKey) + { + m_interfaces.erase(confKey); + } +} diff --git a/toolbox/src/base/WBInterface.cpp b/toolbox/src/base/WBInterface.cpp deleted file mode 100644 index befd4051d..000000000 --- a/toolbox/src/base/WBInterface.cpp +++ /dev/null @@ -1,318 +0,0 @@ -#include "WBInterface.h" - -#include "Error.h" -#include -#include - -namespace wbt { - - unsigned WBInterface::s_referenceCount = 0; - unsigned WBInterface::s_modelReferenceCount = 0; - - WBInterface::WBInterface() - : m_interface(0) - , m_model(0) - , m_robotList(0) - , m_configuration(0) - , m_initialized(false) - , m_modelInitialized(false) - , m_configured(false) - , m_dofs(-1) {} - - WBInterface::WBInterface(const WBInterface&) {} - WBInterface::~WBInterface() {} - WBInterface& WBInterface::operator=(const wbt::WBInterface &) { return *this; } - - WBInterface& WBInterface::sharedInstance() - { - static WBInterface instance; - return instance; - //TODO: check if the destructor get called at simulink exit - } - - wbi::wholeBodyInterface * const WBInterface::interface() { return m_interface; } - - wbi::iWholeBodyModel * const WBInterface::model() - { - if (!m_model) - return ((yarpWbi::yarpWholeBodyInterface*)m_interface)->wholeBodyModel(); - return m_model; - } - - const yarp::os::Property * const WBInterface::currentConfiguration() const - { - return m_configuration; - } - - - int WBInterface::numberOfDoFs() const { return m_dofs; } - - bool WBInterface::wbdIDListFromConfigPropAndList(const yarp::os::Property& wbiConfigProp, - const std::string& list, wbi::IDList& idList) { - // There are two ways of specifying the list throuth the mask parameter: - // either the specified parameter is a list (in the sense of YARP configuration file list, - // so something like (joint1,joint2,joint3) ) and it that case it is - // considered to by directly the list of joints to load, or otherwise - // the wbi list is just a string, and it is considered the name of the list - // in the yarpWholeBodyInterface.ini file - - // Reset the idList - idList.removeAllIDs(); - - // Check if the list string is actually a list - yarp::os::Value listAsValue; - listAsValue.fromString(list.c_str()); - - if (listAsValue.isList()) { - // If the list param is a (YARP) list, load the IDList from it - for (int jnt = 0; jnt < listAsValue.asList()->size(); jnt++) - { - yarp::os::ConstString jntName = listAsValue.asList()->get(jnt).asString(); - idList.addID(wbi::ID(jntName.c_str())); - } - } else { - // Otherwise consider the list to be a - if (!yarpWbi::loadIdListFromConfig(list, wbiConfigProp, idList)) { - return false; - } - } - return true; - } - - int WBInterface::dofsForConfigurationFileAndList(const std::string & wbiConfigFile, const std::string & list) - { - using namespace yarp::os; - - //Workaround for the fact that ResourceFinder initializes the network by itself. See YARP#1014 - Network network; - - ResourceFinder &resourceFinder = ResourceFinder::getResourceFinderSingleton(); - resourceFinder.configure(0, 0); - - Property configurations; - //loading defaults from configuration file - if (!configurations.fromConfigFile(resourceFinder.findFile(wbiConfigFile))) { - return -1; - } - - wbi::IDList jointList; - //parse the file to get the joint list - if (!WBInterface::wbdIDListFromConfigPropAndList(configurations,list,jointList)) { - return -2; - } - - m_dofs = jointList.size(); - - return m_dofs; - } - - bool WBInterface::configure(const std::string &robotName, - const std::string & localName, - const std::string & wbiConfigFile, - const std::string & list, - wbt::Error *error) - { - if (m_configured) return true; - - using namespace yarp::os; - - //Workaround for the fact that ResourceFinder initializes the network by itself. See YARP#1014 - Network network; - ResourceFinder &resourceFinder = ResourceFinder::getResourceFinderSingleton(); - resourceFinder.configure(0, 0); - - //parameters needed by this block: - // - YARP_ROBOT_NAME: needed by resource finder for resource lookup (for now it is taken by the environment) - // - robot: robot port. If defined overrides the one specified by wbi file - // - moduleName: local (opened) ports. - // - wbi config file name (default: yarpWholeBodyInterface.ini): specifies the wbi config file - // - wbi list (default ROBOT_TORQUE_CONTROL_JOINTS): specifies the WBI list. - // If it is a list [of style: (value1 value2 value3)] it specifies directly the list of joints to control, - // otherwise its value specifies the name of list present the wbi config file. - - if (m_configuration) { - delete m_configuration; - m_configuration = 0; - } - m_configuration = new Property(); - //loading defaults - m_wbiFilePath = resourceFinder.findFile(wbiConfigFile); - if (!m_configuration->fromConfigFile(m_wbiFilePath)) { - if (error) error->message = "Could not load the WBI configuration file"; - return false; - } - - //overwriting values - if (!robotName.empty()) - m_configuration->put("robot", robotName); - - m_configuration->put("localName", localName); - - //loading joint list - if (m_robotList) { - delete m_robotList; - m_robotList = 0; - } - - m_robotList = new wbi::IDList(); - - if (!WBInterface::wbdIDListFromConfigPropAndList(*m_configuration,list,*m_robotList)) { - if(error) error->message = "Could not load the specified WBI list (list param: " + list + " )"; - Network::fini(); - return false; - } - m_wbiListName = list; - - m_dofs = m_robotList->size(); - - m_configured = true; - - return true; - } - - void WBInterface::clearConfiguration() - { - m_configured = false; - if (m_robotList) { - delete m_robotList; - m_robotList = 0; - } - if (m_configuration) { - delete m_configuration; - m_configuration = 0; - } - } - - bool WBInterface::initialize() - { - if (m_initialized) { - s_referenceCount++; - return true; - } - - if (!m_configuration || !m_robotList) return false; - - if (m_interface) { - m_interface->close(); - delete m_interface; - m_interface = 0; - } - - m_interface = new yarpWbi::yarpWholeBodyInterface(m_configuration->find("localName").toString().c_str(), *m_configuration); - if (!m_interface) return false; - - s_referenceCount = 1; - - if (!m_interface->addJoints(*m_robotList)) { - terminate(); - return false; - } - - if (!m_interface->init()) { - terminate(); - return false; - } - - m_initialized = true; - return true; - } - - bool WBInterface::initializeModel() - { - if (m_modelInitialized) { - s_modelReferenceCount++; - return true; - } - - if (!m_configuration || !m_robotList) return false; - - if (m_model) { - m_model->close(); - delete m_model; - m_model = 0; - } - - m_model = new yarpWbi::yarpWholeBodyModel(m_configuration->find("localName").toString().c_str(), *m_configuration); - if (!m_model) return false; - - s_modelReferenceCount = 1; - - if (!m_model->addJoints(*m_robotList)) { - terminateModel(); - return false; - } - - if (!m_model->init()) { - terminateModel(); - return false; - } - - m_modelInitialized = true; - return true; - } - - bool WBInterface::terminate() - { - if (s_referenceCount > 1) { - s_referenceCount--; - return true; - } - - bool result = true; - if (m_interface) { - result = m_interface->close(); - if (result) { - delete m_interface; - m_interface = 0; - } - } - - //clean also configuration if also model has been removed - if (!m_modelInitialized) clearConfiguration(); - m_initialized = false; - s_referenceCount = 0; - return result; - } - - bool WBInterface::terminateModel() - { - if (s_modelReferenceCount > 1) { - s_modelReferenceCount--; - return true; - } - - bool result = true; - if (m_model) { - result = m_model->close(); - if (result) { - delete m_model; - m_model = 0; - } - } - - //clean also configuration if also interface has been removed - if (!m_initialized) clearConfiguration(); - m_modelInitialized = false; - s_modelReferenceCount = 0; - return result; - } - - bool WBInterface::isInterfaceInitialized() const { return m_initialized; } - - const wbi::IDList* WBInterface::wbiList() const - { - if (m_robotList) - return m_robotList; - return NULL; - } - - const std::string& WBInterface::wbiFilePath() const - { - return m_wbiFilePath; - } - - const std::string& WBInterface::wbiListName() const - { - return m_wbiListName; - } -} From 42a06f4546a9411de615eb96f5cdc71dfadcf7cc Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:14:11 +0000 Subject: [PATCH 16/89] CentroidalMomentum refactored --- toolbox/include/CentroidalMomentum.h | 37 +++-- toolbox/src/CentroidalMomentum.cpp | 237 ++++++++++++++------------- 2 files changed, 142 insertions(+), 132 deletions(-) diff --git a/toolbox/include/CentroidalMomentum.h b/toolbox/include/CentroidalMomentum.h index c4c5c121d..d3abb34bb 100644 --- a/toolbox/include/CentroidalMomentum.h +++ b/toolbox/include/CentroidalMomentum.h @@ -1,35 +1,36 @@ #ifndef WBT_CENTROIDALMOMENTUM_H #define WBT_CENTROIDALMOMENTUM_H -#include "WBIModelBlock.h" +#include "WBBlock.h" namespace wbt { class CentroidalMomentum; } -class wbt::CentroidalMomentum : public wbt::WBIModelBlock { +namespace iDynTree { + class SpatialMomentum; +} - double *m_basePose; - double *m_centroidalMomentum; +class wbt::CentroidalMomentum : public wbt::WBBlock +{ +private: + iDynTree::SpatialMomentum* m_centroidalMomentum; - //input buffers - double *m_basePoseRaw; - double *m_configuration; - double *m_baseVelocity; - double *m_jointsVelocity; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned INPUT_IDX_BASE_VEL; + static const unsigned INPUT_IDX_JOINT_VEL; + static const unsigned OUTPUT_IDX_CENTRMOM; public: - static std::string ClassName; + static const std::string ClassName; CentroidalMomentum(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + bool configureSizeAndPorts(BlockInformation* blockInfo) override; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - - + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_CENTROIDALMOMENTUM_H */ +#endif /* WBT_CENTROIDALMOMENTUM_H */ diff --git a/toolbox/src/CentroidalMomentum.cpp b/toolbox/src/CentroidalMomentum.cpp index 35a707f68..ca93b9308 100644 --- a/toolbox/src/CentroidalMomentum.cpp +++ b/toolbox/src/CentroidalMomentum.cpp @@ -2,158 +2,167 @@ #include "BlockInformation.h" #include "Signal.h" -#include "Error.h" -#include "WBInterface.h" -#include -#include +#include "Log.h" +#include "RobotInterface.h" +#include +#include +#include +#include #include namespace wbt { - std::string CentroidalMomentum::ClassName = "CentroidalMomentum"; + const std::string CentroidalMomentum::ClassName = "CentroidalMomentum"; + + const unsigned CentroidalMomentum::INPUT_IDX_BASE_POSE = 0; + const unsigned CentroidalMomentum::INPUT_IDX_JOINTCONF = 1; + const unsigned CentroidalMomentum::INPUT_IDX_BASE_VEL = 2; + const unsigned CentroidalMomentum::INPUT_IDX_JOINT_VEL = 3; + const unsigned CentroidalMomentum::OUTPUT_IDX_CENTRMOM = 0; CentroidalMomentum::CentroidalMomentum() - : m_basePose(0) - , m_centroidalMomentum(0) - , m_basePoseRaw(0) - , m_configuration(0) - , m_baseVelocity(0) - , m_jointsVelocity(0) {} - - bool CentroidalMomentum::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + : m_centroidalMomentum(nullptr) + {} + + bool CentroidalMomentum::configureSizeAndPorts(BlockInformation* blockInfo) { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + // Memory allocation / Saving data not allowed here - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // 4) Joints velocity (1xDoFs vector) + // + // Number of inputs if (!blockInfo->setNumberOfInputPorts(4)) { - if (error) error->message = "Failed to configure the number of input ports"; + Log::getSingleton().error("Failed to configure the number of input ports."); return false; } - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); //base pose - success = success && blockInfo->setInputPortVectorSize(1, dofs); //joint configuration - success = success && blockInfo->setInputPortVectorSize(2, 6); //base velocity - success = success && blockInfo->setInputPortVectorSize(3, dofs); //joints velocitity + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); - blockInfo->setInputPortType(2, PortDataTypeDouble); - blockInfo->setInputPortType(3, PortDataTypeDouble); + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); + + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); if (!success) { - if (error) error->message = "Failed to configure input ports"; + Log::getSingleton().error("Failed to configure input ports."); return false; } - // Output port: - // - 6 vector representing the centroidal momentum - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; + // OUTPUTS + // ======= + // + // 1) Vector representing the centroidal momentum (1x6) + // + + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); return false; } - success = blockInfo->setOutputPortVectorSize(0, 6); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + // Size and type + success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_CENTRMOM, 6); + blockInfo->setOutputPortType(OUTPUT_IDX_CENTRMOM, PortDataTypeDouble); return success; } - bool CentroidalMomentum::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool CentroidalMomentum::initialize(BlockInformation* blockInfo) { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_centroidalMomentum = new double[6]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; - m_baseVelocity = new double[6]; - m_jointsVelocity = new double[dofs]; - - return m_basePose && m_centroidalMomentum && m_basePoseRaw && m_configuration && m_baseVelocity && m_jointsVelocity; + if (!WBBlock::initialize(blockInfo)) return false; + + // OUTPUT + // ====== + + m_centroidalMomentum = new iDynTree::SpatialMomentum(); + return m_centroidalMomentum; } - bool CentroidalMomentum::terminate(BlockInformation *blockInfo, wbt::Error *error) + bool CentroidalMomentum::terminate(BlockInformation* blockInfo) { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } if (m_centroidalMomentum) { - delete [] m_centroidalMomentum; + delete m_centroidalMomentum; m_centroidalMomentum = 0; } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - if (m_baseVelocity) { - delete [] m_baseVelocity; - m_baseVelocity = 0; - } - if (m_jointsVelocity) { - delete [] m_jointsVelocity; - m_jointsVelocity = 0; - } - return WBIModelBlock::terminate(blockInfo, error); + return WBBlock::terminate(blockInfo); } - bool CentroidalMomentum::output(BlockInformation *blockInfo, wbt::Error */*error*/) + bool CentroidalMomentum::output(BlockInformation* blockInfo) { - //get input - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - Signal baseVelocity = blockInfo->getInputPortSignal(2); - Signal jointsVelocity = blockInfo->getInputPortSignal(3); - - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(2); ++i) { - m_baseVelocity[i] = baseVelocity.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(3); ++i) { - m_jointsVelocity[i] = jointsVelocity.get(i).doubleData(); - } - - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; - - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); - - interface->computeCentroidalMomentum(m_configuration, - frame, - m_jointsVelocity, - m_baseVelocity, - m_centroidalMomentum); - - Signal output = blockInfo->getOutputPortSignal(0); - output.setBuffer(m_centroidalMomentum, blockInfo->getOutputPortWidth(0)); - - return true; + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; } - return false; + + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== + + unsigned signalWidth; + + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + + // Base velocity + Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); + double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); + robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), + AngVelocity(m_baseVelocityBuffer+3, 3)); + + // Joints velocity + Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); + robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); + + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + // OUTPUT + // ====== + + // Calculate the centroidal momentum + *m_centroidalMomentum = model->getCentroidalTotalMomentum(); + + // Forward the output to Simulink + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_CENTRMOM); + output.setBuffer(toEigen(*m_centroidalMomentum).data(), + blockInfo->getOutputPortWidth(OUTPUT_IDX_CENTRMOM)); + return true; } } From aab17af561e3022a1ea6c0ada7bfd5305425af10 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:15:08 +0000 Subject: [PATCH 17/89] DotJNu refactored --- toolbox/include/DotJNu.h | 44 ++++--- toolbox/src/DotJNu.cpp | 278 ++++++++++++++++++++++----------------- 2 files changed, 177 insertions(+), 145 deletions(-) diff --git a/toolbox/include/DotJNu.h b/toolbox/include/DotJNu.h index 343b8b15c..d844bb510 100644 --- a/toolbox/include/DotJNu.h +++ b/toolbox/include/DotJNu.h @@ -1,38 +1,40 @@ #ifndef WBT_DOTJDOTQ_H #define WBT_DOTJDOTQ_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include +#include namespace wbt { class DotJNu; } -class wbt::DotJNu : public wbt::WBIModelBlock { +class wbt::DotJNu : public wbt::WBBlock +{ +private: + // Output + iDynTree::Vector6* m_dotJNu; - double *m_basePose; - double *m_dotJNu; + // Other variables + bool m_frameIsCoM; + iDynTree::FrameIndex m_frameIndex; - //input buffers - double *m_basePoseRaw; - double *m_configuration; - double *m_baseVelocity; - double *m_jointsVelocity; - - int m_frameIndex; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned INPUT_IDX_BASE_VEL; + static const unsigned INPUT_IDX_JOINT_VEL; + static const unsigned OUTPUT_IDX_DOTJ_NU; public: - static std::string ClassName; + static const std::string ClassName; DotJNu(); - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - - + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_DOTJDOTQ_H */ +#endif /* WBT_DOTJDOTQ_H */ diff --git a/toolbox/src/DotJNu.cpp b/toolbox/src/DotJNu.cpp index 1dfaef8e8..db9264ab2 100644 --- a/toolbox/src/DotJNu.cpp +++ b/toolbox/src/DotJNu.cpp @@ -1,186 +1,216 @@ #include "DotJNu.h" -#include "Error.h" -#include "WBInterface.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" -#include -#include +#include "RobotInterface.h" +#include +#include +#include #include namespace wbt { - std::string DotJNu::ClassName = "DotJNu"; + const std::string DotJNu::ClassName = "DotJNu"; + + const unsigned DotJNu::INPUT_IDX_BASE_POSE = 0; + const unsigned DotJNu::INPUT_IDX_JOINTCONF = 1; + const unsigned DotJNu::INPUT_IDX_BASE_VEL = 2; + const unsigned DotJNu::INPUT_IDX_JOINT_VEL = 3; + const unsigned DotJNu::OUTPUT_IDX_DOTJ_NU = 0; DotJNu::DotJNu() - : m_basePose(0) - , m_dotJNu(0) - , m_basePoseRaw(0) - , m_configuration(0) - , m_baseVelocity(0) - , m_jointsVelocity(0) - , m_frameIndex(-1) {} + : m_dotJNu(nullptr) + , m_frameIsCoM(false) + , m_frameIndex(iDynTree::FRAME_INVALID_INDEX) + {} unsigned DotJNu::numberOfParameters() { - return WBIBlock::numberOfParameters() + 1; + return WBBlock::numberOfParameters() + 1; } - bool DotJNu::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool DotJNu::configureSizeAndPorts(BlockInformation* blockInfo) { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + // Memory allocation / Saving data not allowed here - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // 4) Joints velocity (1xDoFs vector) + // + // Number of inputs if (!blockInfo->setNumberOfInputPorts(4)) { - if (error) error->message = "Failed to configure the number of input ports"; + Log::getSingleton().error("Failed to configure the number of input ports."); return false; } - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); //base pose - success = success && blockInfo->setInputPortVectorSize(1, dofs); //joint configuration - success = success && blockInfo->setInputPortVectorSize(2, 6); //base velocity - success = success && blockInfo->setInputPortVectorSize(3, dofs); //joints velocitity + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); - blockInfo->setInputPortType(2, PortDataTypeDouble); - blockInfo->setInputPortType(3, PortDataTypeDouble); + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); + + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); if (!success) { - if (error) error->message = "Failed to configure input ports"; + Log::getSingleton().error("Failed to configure input ports."); return false; } - // Output port: - // - 6-d vector representing the \dot{J} \dot{q} vector - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; + // OUTPUTS + // ======= + // + // 1) Vector representing the \dot{J} \nu vector (1x6) + // + + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); return false; } - success = blockInfo->setOutputPortVectorSize(0, 6); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + // Size and type + success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_DOTJ_NU, 6); + blockInfo->setOutputPortType(OUTPUT_IDX_DOTJ_NU, PortDataTypeDouble); return success; } - bool DotJNu::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool DotJNu::initialize(BlockInformation* blockInfo) { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; + if (!WBBlock::initialize(blockInfo)) return false; + + // INPUT PARAMETERS + // ================ - int parentParameters = WBIBlock::numberOfParameters() + 1; std::string frame; - if (!blockInfo->getStringParameterAtIndex(parentParameters, frame)) { - if (error) error->message = "Cannot retrieve string from frame parameter"; + int parentParameters = WBBlock::numberOfParameters(); + + if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + Log::getSingleton().error("Cannot retrieve string from frame parameter."); return false; } - //here obtain joint list and get the frame - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (!interface) { - if (error) error->message = "Cannot retrieve handle to WBI interface"; + + // Check if the frame is valid + // --------------------------- + + const auto& model = getRobotInterface()->getKinDynComputations(); + if (!model) { + Log::getSingleton().error("Cannot retrieve handle to KinDynComputations."); return false; } - wbi::IDList frames = interface->getFrameList(); + if (frame != "com") { - if (!frames.idToIndex(wbi::ID(frame), m_frameIndex)) { - if (error) error->message = "Cannot find " + frame + " in the frame list"; + m_frameIndex = model->getFrameIndex(frame); + if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { + Log::getSingleton().error("Cannot find " + frame + " in the frame list."); return false; } - } else { - m_frameIndex = wbi::wholeBodyInterface::COM_LINK_ID; } - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_dotJNu = new double[6]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; - m_baseVelocity = new double[6]; - m_jointsVelocity = new double[dofs]; - - return m_basePose && m_dotJNu && m_basePoseRaw && m_configuration && m_baseVelocity && m_jointsVelocity; + else { + m_frameIsCoM = true; + m_frameIndex = iDynTree::FRAME_INVALID_INDEX; + } + + // OUTPUT + // ====== + m_dotJNu = new iDynTree::Vector6(); + m_dotJNu->zero(); + + return m_dotJNu; } - bool DotJNu::terminate(BlockInformation *blockInfo, wbt::Error *error) + bool DotJNu::terminate(BlockInformation* blockInfo) { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } + // Output if (m_dotJNu) { - delete [] m_dotJNu; + delete m_dotJNu; m_dotJNu = 0; } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - if (m_baseVelocity) { - delete [] m_baseVelocity; - m_baseVelocity = 0; - } - if (m_jointsVelocity) { - delete [] m_jointsVelocity; - m_jointsVelocity = 0; - } - return WBIModelBlock::terminate(blockInfo, error); + + return WBBlock::terminate(blockInfo); } - bool DotJNu::output(BlockInformation *blockInfo, wbt::Error */*error*/) + bool DotJNu::output(BlockInformation* blockInfo) { - //get input - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - Signal baseVelocity = blockInfo->getInputPortSignal(2); - Signal jointsVelocity = blockInfo->getInputPortSignal(3); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(2); ++i) { - m_baseVelocity[i] = baseVelocity.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(3); ++i) { - m_jointsVelocity[i] = jointsVelocity.get(i).doubleData(); - } - - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); + const auto& model = getRobotInterface()->getKinDynComputations(); - interface->computeDJdq(m_configuration, - frame, - m_jointsVelocity, - m_baseVelocity, - m_frameIndex, - m_dotJNu); - - Signal output = blockInfo->getOutputPortSignal(0); - output.setBuffer(m_dotJNu, blockInfo->getOutputPortWidth(0)); + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } - return true; + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== + + unsigned signalWidth; + + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + + // Base velocity + Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); + double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); + robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), + AngVelocity(m_baseVelocityBuffer+3, 3)); + + // Joints velocity + Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); + robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); + + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + // OUTPUT + // ====== + + // Compute the dot{J}*\nu + Vector6 biasAcc; + if (!m_frameIsCoM) { + biasAcc = model->getFrameBiasAcc(m_frameIndex); + } + else { + Vector3 comBiasAcc = model->getCenterOfMassBiasAcc(); + toEigen(biasAcc).segment<3>(0) = iDynTree::toEigen(comBiasAcc); + toEigen(biasAcc).segment<3>(3).setZero(); } - return false; + + // Forward the output to Simulink + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_DOTJ_NU); + output.setBuffer(m_dotJNu->data(), + blockInfo->getOutputPortWidth(OUTPUT_IDX_DOTJ_NU)); + return true; } } From 2881b1b08176df3e8cae95773bfeeefafdf78a3a Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:16:12 +0000 Subject: [PATCH 18/89] ForwardKinematics refactored --- toolbox/include/ForwardKinematics.h | 37 +++-- toolbox/src/ForwardKinematics.cpp | 228 ++++++++++++++++------------ 2 files changed, 147 insertions(+), 118 deletions(-) diff --git a/toolbox/include/ForwardKinematics.h b/toolbox/include/ForwardKinematics.h index 6ed0e5bf9..147f2c7b4 100644 --- a/toolbox/include/ForwardKinematics.h +++ b/toolbox/include/ForwardKinematics.h @@ -1,36 +1,33 @@ #ifndef WBT_FORWARDKINEMATICS_H #define WBT_FORWARDKINEMATICS_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include namespace wbt { class ForwardKinematics; } -class wbt::ForwardKinematics : public wbt::WBIModelBlock { +class wbt::ForwardKinematics : public wbt::WBBlock +{ +private: + bool m_frameIsCoM; + iDynTree::FrameIndex m_frameIndex; - double *m_basePose; - double *m_frameForwardKinematics; - - //input buffers - double *m_basePoseRaw; - double *m_configuration; - - int m_frameIndex; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned OUTPUT_IDX_FW_FRAME; public: - static std::string ClassName; + static const std::string ClassName; ForwardKinematics(); - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_FORWARDKINEMATICS_H */ +#endif /* WBT_FORWARDKINEMATICS_H */ diff --git a/toolbox/src/ForwardKinematics.cpp b/toolbox/src/ForwardKinematics.cpp index 81f7dee14..052713745 100644 --- a/toolbox/src/ForwardKinematics.cpp +++ b/toolbox/src/ForwardKinematics.cpp @@ -2,160 +2,192 @@ #include "BlockInformation.h" #include "Signal.h" -#include "Error.h" -#include "WBInterface.h" -#include -#include +#include "Log.h" +#include "RobotInterface.h" +#include +#include +#include #include namespace wbt { - std::string ForwardKinematics::ClassName = "ForwardKinematics"; + const std::string ForwardKinematics::ClassName = "ForwardKinematics"; + + const unsigned ForwardKinematics::INPUT_IDX_BASE_POSE = 0; + const unsigned ForwardKinematics::INPUT_IDX_JOINTCONF = 1; + const unsigned ForwardKinematics::OUTPUT_IDX_FW_FRAME = 0; ForwardKinematics::ForwardKinematics() - : m_basePose(0) - , m_frameForwardKinematics(0) - , m_basePoseRaw(0) - , m_configuration(0) - , m_frameIndex(-1) {} + : m_frameIsCoM(false) + , m_frameIndex(iDynTree::FRAME_INVALID_INDEX) + {} unsigned ForwardKinematics::numberOfParameters() { - return WBIBlock::numberOfParameters() + 1; + return WBBlock::numberOfParameters() + 1; } - bool ForwardKinematics::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool ForwardKinematics::configureSizeAndPorts(BlockInformation* blockInfo) { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + // Memory allocation / Saving data not allowed here - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // + // Number of inputs if (!blockInfo->setNumberOfInputPorts(2)) { - if (error) error->message = "Failed to configure the number of input ports"; + Log::getSingleton().error("Failed to configure the number of input ports."); return false; } + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Size and type bool success = true; - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); - success = success && blockInfo->setInputPortVectorSize(1, dofs); + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); if (!success) { - if (error) error->message = "Failed to configure input ports"; + Log::getSingleton().error("Failed to configure input ports."); return false; } - // Output port: - // - (4)x(4) matrix representing the homogenous transformation between the specified frame and the world frame - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; + // OUTPUTS + // ======= + // + // 1) Homogeneous transformation between the world and the specified frame (4x4 matrix) + // + + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); return false; } - success = blockInfo->setOutputPortMatrixSize(0, 4, 4); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + // Size and type + success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_FW_FRAME, 4, 4); + blockInfo->setOutputPortType(OUTPUT_IDX_FW_FRAME, PortDataTypeDouble); return success; } - bool ForwardKinematics::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool ForwardKinematics::initialize(BlockInformation* blockInfo) { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; + if (!WBBlock::initialize(blockInfo)) return false; + + // INPUT PARAMETERS + // ================ - int parentParameters = WBIBlock::numberOfParameters() + 1; std::string frame; - if (!blockInfo->getStringParameterAtIndex(parentParameters, frame)) { - if (error) error->message = "Cannot retrieve string from frame parameter"; + int parentParameters = WBBlock::numberOfParameters(); + + if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + Log::getSingleton().error("Cannot retrieve string from frame parameter."); return false; } - //here obtain joint list and get the frame - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (!interface) { - if (error) error->message = "Cannot retrieve handle to WBI interface"; + // Check if the frame is valid + // --------------------------- + + const auto& model = getRobotInterface()->getKinDynComputations(); + if (!model) { + Log::getSingleton().error("Cannot retrieve handle to WBI model."); return false; } - wbi::IDList frames = interface->getFrameList(); + if (frame != "com") { - if (!frames.idToIndex(wbi::ID(frame), m_frameIndex)) { - if (error) error->message = "Cannot find " + frame + " in the frame list"; + m_frameIndex = model->getFrameIndex(frame); + if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { + Log::getSingleton().error("Cannot find " + frame + " in the frame list."); return false; } - } else { - m_frameIndex = wbi::wholeBodyInterface::COM_LINK_ID; + } + else { + m_frameIsCoM = true; + m_frameIndex = iDynTree::FRAME_INVALID_INDEX; } - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_frameForwardKinematics = new double[4 * 4]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; - - return m_basePose && m_frameForwardKinematics && m_basePoseRaw && m_configuration; + return true; } - bool ForwardKinematics::terminate(BlockInformation *blockInfo, wbt::Error *error) + bool ForwardKinematics::terminate(BlockInformation* blockInfo) { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } - if (m_frameForwardKinematics) { - delete [] m_frameForwardKinematics; - m_frameForwardKinematics = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - return WBIModelBlock::terminate(blockInfo, error); + return WBBlock::terminate(blockInfo); } - bool ForwardKinematics::output(BlockInformation *blockInfo, wbt::Error */*error*/) + bool ForwardKinematics::output(BlockInformation* blockInfo) { - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1) - ; - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; + typedef Matrix Matrix4diDynTree; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } + + // Get the signals and convert them to iDynTree objects + // ==================================================== - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; + unsigned signalWidth; - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - wbi::Frame outputFrame; - interface->computeH(m_configuration, frame, m_frameIndex, outputFrame); - outputFrame.get4x4Matrix(m_frameForwardKinematics); + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - Eigen::Map > frameRowMajor(m_frameForwardKinematics); + // TODO: the other 3 inputs are taken from the previous block's setRobotState(). + // How to handle it? What happens if a nullptr is passed? - Signal output = blockInfo->getOutputPortSignal(0); - Eigen::Map > frameColMajor((double*)output.getContiguousBuffer(), 4, 4); - frameColMajor = frameRowMajor; - return true; + // Update the robot status + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + // Output + // ====== + + iDynTree::Transform world_H_frame; + + // Compute the transform to the selected frame + if (!m_frameIsCoM) { + world_H_frame = model->getWorldTransform(m_frameIndex); + } + else { + world_H_frame.setPosition(model->getCenterOfMassPosition()); } - return false; + + // Get the output signal memory location + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_FW_FRAME); + + // Allocate objects for row-major -> col-major conversion + Map world_H_frame_RowMajor = toEigen(world_H_frame.asHomogeneousTransform()); + Map world_H_frame_ColMajor((double*)output.getContiguousBuffer(), + 4, 4); + + // Forward the buffer to Simulink transforming it to ColMajor + world_H_frame_ColMajor = world_H_frame_RowMajor; + return true; } } From 881f382909b74732c819d136d2105446ee6940e2 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:17:50 +0000 Subject: [PATCH 19/89] GetLimits refactored --- toolbox/include/GetLimits.h | 41 ++++--- toolbox/src/GetLimits.cpp | 209 +++++++++++++++++++++++------------- 2 files changed, 163 insertions(+), 87 deletions(-) diff --git a/toolbox/include/GetLimits.h b/toolbox/include/GetLimits.h index 539386a67..81570ee24 100644 --- a/toolbox/include/GetLimits.h +++ b/toolbox/include/GetLimits.h @@ -1,30 +1,41 @@ #ifndef WBT_GETLIMITS_H #define WBT_GETLIMITS_H -#include "WBIBlock.h" +#include "WBBlock.h" +#include namespace wbt { class GetLimits; + struct Limit; } -class wbt::GetLimits : public wbt::WBIBlock { +struct wbt::Limit +{ + std::vector min; + std::vector max; - struct Limit; - struct Limit *m_limits; - -public: - static std::string ClassName; - GetLimits(); + Limit(unsigned size = 0) + : min(size) + , max(size) + {} +}; - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); +class wbt::GetLimits : public wbt::WBBlock +{ +private: + std::unique_ptr m_limits; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); +public: + static const std::string ClassName; + GetLimits() = default; + ~GetLimits() override = default; + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_GETLIMITS_H */ +#endif /* WBT_GETLIMITS_H */ diff --git a/toolbox/src/GetLimits.cpp b/toolbox/src/GetLimits.cpp index 8ad4a6093..bc2f55b10 100644 --- a/toolbox/src/GetLimits.cpp +++ b/toolbox/src/GetLimits.cpp @@ -1,131 +1,196 @@ #include "GetLimits.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" -#include "WBInterface.h" -#include -#include +#include "RobotInterface.h" +#include "iDynTree/KinDynComputations.h" +#include "iDynTree/Model/Model.h" +#include +#include +#include namespace wbt { - std::string GetLimits::ClassName = "GetLimits"; - - struct GetLimits::Limit { - double *min; - double *max; - - Limit(unsigned size) : min(0), max(0) - { - min = new double[size](); - max = new double[size](); - } - - ~Limit() - { - if (min) { - delete [] min; - min = 0; - } - if (max) { - delete [] max; - max = 0; - } - } - }; - - GetLimits::GetLimits() - : m_limits(0) {} + const std::string GetLimits::ClassName = "GetLimits"; unsigned GetLimits::numberOfParameters() { - return WBIBlock::numberOfParameters() + 1; + return WBBlock::numberOfParameters() + 1; } - bool GetLimits::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool GetLimits::configureSizeAndPorts(BlockInformation* blockInfo) { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + // INPUTS + // ====== + // + // No inputs + // if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to configure the number of input ports"; + Log::getSingleton().error("Failed to configure the number of input ports."); return false; } - // Output port: - // - DoFs vector with the information asked - if (!blockInfo->setNumberOfOuputPorts(2)) { - if (error) error->message = "Failed to configure the number of output ports"; + // OUTPUTS + // ======= + // + // 1) vector with the information asked (1xDoFs) + // + + if (!blockInfo->setNumberOfOutputPorts(2)) { + Log::getSingleton().error("Failed to configure the number of output ports."); return false; } - bool success = blockInfo->setOutputPortVectorSize(0, dofs); //Min limit - success = success && blockInfo->setOutputPortVectorSize(1, dofs); //Max limit + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + bool success = true; + success = success && blockInfo->setOutputPortVectorSize(0, dofs); // Min limit + success = success && blockInfo->setOutputPortVectorSize(1, dofs); // Max limit blockInfo->setOutputPortType(0, PortDataTypeDouble); blockInfo->setOutputPortType(1, PortDataTypeDouble); if (!success) { - if (error) error->message = "Failed to configure output ports"; + Log::getSingleton().error("Failed to configure output ports."); return false; } - return true; } - bool GetLimits::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool GetLimits::initialize(BlockInformation* blockInfo) { using namespace yarp::os; - if (!WBIBlock::initialize(blockInfo, error)) return false; - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - wbi::wholeBodyInterface * const interface = WBInterface::sharedInstance().interface(); + if (!WBBlock::initialize(blockInfo)) return false; - // Reading the control type + // Read the control type std::string limitType; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 1, limitType)) { - if (error) error->message = "Could not read estimate type parameter"; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, limitType)) { + Log::getSingleton().error("Could not read estimate type parameter."); return false; } - bool success = true; - if (limitType == "Position") { - m_limits = new Limit(dofs); - if (m_limits) { - success = interface->getJointLimits(m_limits->min, m_limits->max); + // Initialize the structure that stores the limits + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_limits.reset(new Limit(dofs)); + + // Initializes some buffers + double min = 0; + double max = 0; + + // From the RemoteControlBoardRemapper + // =================================== + // + // In the next methods, the values are asked using joint index and not string. + // The ControlBoardRemapper internally uses the same joints ordering of its + // initialization. In this case, it matches 1:1 the joint vector. It is hence + // possible using i to point to the correct joint. + + // Get the RemoteControlBoardRemapper and IControlLimits2 interface if needed + std::weak_ptr iControlLimits2; + if (limitType == "ControlBoardPosition" || limitType == "ControlBoardVelocity") { + // Retain the control board remapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); + return false; + } + // Get the interface + if (!getRobotInterface()->getInterface(iControlLimits2)) { + Log::getSingleton().error("Failed to get IControlLimits2 interface."); + return false; + } + } + + if (limitType == "ControlBoardPosition") { + for (auto i = 0; i < dofs; ++i) { + if (!iControlLimits2.lock()->getLimits(i, &min, &max)) { + Log::getSingleton().error("Failed to get limits from the interface."); + return false; + } + m_limits->min[i] = min; + m_limits->max[i] = max; + } + } + else if (limitType == "ControlBoardVelocity") { + for (auto i = 0; i < dofs; ++i) { + if (!iControlLimits2.lock()->getVelLimits(i, &min, &max)) { + Log::getSingleton().error("Failed to get limits from the interface."); + return false; + } + m_limits->min[i] = min; + m_limits->max[i] = max; } - } else { - if (error) error->message = "Limit type not supported"; + } + + // From the URDF model + // =================== + // + // For the time being, only position limits are supported. + + else if (limitType == "ModelPosition") { + iDynTree::IJointConstPtr p_joint; + const iDynTree::Model model = getRobotInterface()->getKinDynComputations()->model(); + + for (auto i = 0; i < dofs; ++i) { + // Get the joint name + std::string joint = getConfiguration().getControlledJoints()[i]; + + // Get its index + iDynTree::LinkIndex jointIndex = model.getLinkIndex(joint); + + // Get the joint from the model + p_joint = model.getJoint(jointIndex); + + if (!p_joint->hasPosLimits()) { + Log::getSingleton().warning("Joint " + joint + " has no model limits."); + // TODO: test how these values are interpreted by Simulink + min = -std::numeric_limits::infinity(); + max = std::numeric_limits::infinity(); + } + else { + p_joint->getPosLimits(0, min, max); + } + } + } + // TODO + // else if (limitType == "ModelVelocity") { + // } + // else if (limitType == "ModelEffort") { + // } + else { + Log::getSingleton().error("Limit type " + limitType + " not recognized."); return false; } - return m_limits && success; + return true; } - bool GetLimits::terminate(BlockInformation *blockInfo, wbt::Error *error) + bool GetLimits::terminate(BlockInformation* blockInfo) { - if (m_limits) { - delete m_limits; - m_limits = 0; + // Release the RemoteControlBoardRemapper + bool ok = true; + ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); } - return WBIBlock::terminate(blockInfo, error); + + return ok && WBBlock::terminate(blockInfo); } - bool GetLimits::output(BlockInformation *blockInfo, wbt::Error */*error*/) + bool GetLimits::output(BlockInformation* blockInfo) { if (!m_limits) return false; Signal minPort = blockInfo->getOutputPortSignal(0); Signal maxPort = blockInfo->getOutputPortSignal(1); - for (int i = 0; i < blockInfo->getOutputPortWidth(0); ++i) { - minPort.set(i, m_limits->min[i]); - maxPort.set(i, m_limits->max[i]); - } + minPort.setBuffer(m_limits->min.data(), getConfiguration().getNumberOfDoFs()); + maxPort.setBuffer(m_limits->max.data(), getConfiguration().getNumberOfDoFs()); + return true; } } From fade9bb24c14769233ee31ed8da81f62bfaccd9e Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:19:28 +0000 Subject: [PATCH 20/89] InverseDynamics refactored --- toolbox/include/InverseDynamics.h | 51 +++-- toolbox/src/InverseDynamics.cpp | 340 +++++++++++++++--------------- 2 files changed, 195 insertions(+), 196 deletions(-) diff --git a/toolbox/include/InverseDynamics.h b/toolbox/include/InverseDynamics.h index 58e796131..2b035663c 100644 --- a/toolbox/include/InverseDynamics.h +++ b/toolbox/include/InverseDynamics.h @@ -1,41 +1,46 @@ #ifndef WBT_INVERSEDYNAMICS_H #define WBT_INVERSEDYNAMICS_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include namespace wbt { class InverseDynamics; } -class wbt::InverseDynamics : public wbt::WBIModelBlock { +namespace iDynTree { + class VectorDynSize; + class FreeFloatingGeneralizedTorques; +} - double *m_basePose; - double *m_torques; +class wbt::InverseDynamics : public wbt::WBBlock +{ +private: + iDynTree::Vector6* m_baseAcceleration; + iDynTree::VectorDynSize* m_jointsAcceleration; - bool m_explicitGravity; + // Output + iDynTree::FreeFloatingGeneralizedTorques* m_torques; - //input buffers - double *m_basePoseRaw; - double *m_configuration; - double *m_baseVelocity; - double *m_jointsVelocity; - double *m_baseAcceleration; - double *m_jointsAcceleration; - double m_gravity[3]; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned INPUT_IDX_BASE_VEL; + static const unsigned INPUT_IDX_JOINT_VEL; + static const unsigned INPUT_IDX_BASE_ACC; + static const unsigned INPUT_IDX_JOINT_ACC; + static const unsigned OUTPUT_IDX_TORQUES; public: - static std::string ClassName; + static const std::string ClassName; InverseDynamics(); - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - - -}; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; +}; -#endif /* end of include guard: WBT_INVERSEDYNAMICS_H */ +#endif /* WBT_INVERSEDYNAMICS_H */ diff --git a/toolbox/src/InverseDynamics.cpp b/toolbox/src/InverseDynamics.cpp index 45999d7cc..28b8a70c7 100644 --- a/toolbox/src/InverseDynamics.cpp +++ b/toolbox/src/InverseDynamics.cpp @@ -1,221 +1,215 @@ #include "InverseDynamics.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" -#include "WBInterface.h" -#include -#include +#include "RobotInterface.h" +#include +#include +#include +#include #include -#define PARAM_IDX_1 5 - +// TODO: Substitute with using namespace wbt { - std::string InverseDynamics::ClassName = "InverseDynamics"; + const std::string InverseDynamics::ClassName = "InverseDynamics"; + + const unsigned InverseDynamics::INPUT_IDX_BASE_POSE = 0; + const unsigned InverseDynamics::INPUT_IDX_JOINTCONF = 1; + const unsigned InverseDynamics::INPUT_IDX_BASE_VEL = 2; + const unsigned InverseDynamics::INPUT_IDX_JOINT_VEL = 3; + const unsigned InverseDynamics::INPUT_IDX_BASE_ACC = 4; + const unsigned InverseDynamics::INPUT_IDX_JOINT_ACC = 5; + const unsigned InverseDynamics::OUTPUT_IDX_TORQUES = 0; InverseDynamics::InverseDynamics() - : m_basePose(0) - , m_torques(0) - , m_explicitGravity(false) - , m_basePoseRaw(0) - , m_configuration(0) - , m_baseVelocity(0) - , m_jointsVelocity(0) - , m_baseAcceleration(0) - , m_jointsAcceleration(0) - { - m_gravity[0] = 0; - m_gravity[1] = 0; - m_gravity[2] = -9.81; - } + : m_baseAcceleration(nullptr) + , m_jointsAcceleration(nullptr) + , m_torques(nullptr) + {} unsigned InverseDynamics::numberOfParameters() { - return WBIBlock::numberOfParameters() + 1; + return WBBlock::numberOfParameters(); } - bool InverseDynamics::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool InverseDynamics::configureSizeAndPorts(BlockInformation* blockInfo) { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // 4) Joints velocity (1xDoFs vector) + // 5) Base frame acceleration (1x6 vector) + // 6) Joints acceleration (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(6)) { + Log::getSingleton().error("Failed to configure the number of input ports."); return false; } - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + // Get the DoFs + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations - - m_explicitGravity = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).booleanData(); - - int portNumber = 6; - if (m_explicitGravity) portNumber++; - - if (!blockInfo->setNumberOfInputPorts(portNumber)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } + // Size and type bool success = true; - - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); //base pose - success = success && blockInfo->setInputPortVectorSize(1, dofs); //joint configuration - success = success && blockInfo->setInputPortVectorSize(2, 6); //base velocity - success = success && blockInfo->setInputPortVectorSize(3, dofs); //joints velocitity - success = success && blockInfo->setInputPortVectorSize(4, 6); //base acceleration - success = success && blockInfo->setInputPortVectorSize(5, dofs); //joints acceleration - if (m_explicitGravity) - success = success && blockInfo->setInputPortVectorSize(6, 3); //gravity acceleration - - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); - blockInfo->setInputPortType(2, PortDataTypeDouble); - blockInfo->setInputPortType(3, PortDataTypeDouble); - blockInfo->setInputPortType(4, PortDataTypeDouble); - blockInfo->setInputPortType(5, PortDataTypeDouble); - - if (m_explicitGravity) - blockInfo->setInputPortType(6, PortDataTypeDouble); + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_ACC, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_ACC, dofs); + + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_ACC, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_ACC, PortDataTypeDouble); if (!success) { - if (error) error->message = "Failed to configure input ports"; + Log::getSingleton().error("Failed to configure input ports."); return false; } - // Output port: - // - DoFs + 6) vector representing the torques - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; + // OUTPUTS + // ======= + // + // 1) Vector representing the torques (1x(DoFs+6)) + // + + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); return false; } - success = blockInfo->setOutputPortVectorSize(0, dofs + 6); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + // Size and type + success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_TORQUES, dofs + 6); + blockInfo->setOutputPortType(OUTPUT_IDX_TORQUES, PortDataTypeDouble); return success; } - bool InverseDynamics::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool InverseDynamics::initialize(BlockInformation* blockInfo) { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; - - m_explicitGravity = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).booleanData(); - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_torques = new double[6 + dofs]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; - m_baseVelocity = new double[6]; - m_jointsVelocity = new double[dofs]; - m_baseAcceleration = new double[6]; - m_jointsAcceleration = new double[dofs]; - - return m_basePose && m_torques && m_basePoseRaw && m_configuration && m_baseVelocity && m_jointsVelocity && m_baseAcceleration && m_jointsAcceleration; + if (!WBBlock::initialize(blockInfo)) return false; + + // OUTPUT / VARIABLES + // ================== + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + m_baseAcceleration = new iDynTree::Vector6(); + m_baseAcceleration->zero(); + m_jointsAcceleration = new iDynTree::VectorDynSize(dofs); + m_jointsAcceleration->zero(); + + const auto& model = getRobotInterface()->getKinDynComputations()->model(); + m_torques = new iDynTree::FreeFloatingGeneralizedTorques(model); + + return m_baseAcceleration && m_jointsAcceleration && m_torques; } - bool InverseDynamics::terminate(BlockInformation *blockInfo, wbt::Error *error) + bool InverseDynamics::terminate(BlockInformation* blockInfo) { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } - if (m_torques) { - delete [] m_torques; - m_torques = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - if (m_baseVelocity) { - delete [] m_baseVelocity; - m_baseVelocity = 0; - } - if (m_jointsVelocity) { - delete [] m_jointsVelocity; - m_jointsVelocity = 0; - } - if (m_baseAcceleration) { - delete [] m_baseAcceleration; - m_baseAcceleration = 0; + delete m_baseAcceleration; + m_baseAcceleration = nullptr; } - if (m_jointsAcceleration) { - delete [] m_jointsAcceleration; - m_jointsAcceleration = 0; + delete m_jointsAcceleration; + m_jointsAcceleration = nullptr; + } + if (m_torques) { + delete m_torques; + m_torques = nullptr; } - return WBIModelBlock::terminate(blockInfo, error); + return WBBlock::terminate(blockInfo); } - bool InverseDynamics::output(BlockInformation *blockInfo, wbt::Error */*error*/) + bool InverseDynamics::output(BlockInformation* blockInfo) { - //get input - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - Signal baseVelocity = blockInfo->getInputPortSignal(2); - Signal jointsVelocity = blockInfo->getInputPortSignal(3); - Signal baseAcceleration = blockInfo->getInputPortSignal(4); - Signal jointsAcceleration = blockInfo->getInputPortSignal(5); - - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(2); ++i) { - m_baseVelocity[i] = baseVelocity.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(3); ++i) { - m_jointsVelocity[i] = jointsVelocity.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(4); ++i) { - m_baseAcceleration[i] = baseAcceleration.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(5); ++i) { - m_jointsAcceleration[i] = jointsAcceleration.get(i).doubleData(); - } - - if (m_explicitGravity) { - Signal gravity = blockInfo->getInputPortSignal(6); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(6); ++i) { - m_gravity[i] = gravity.get(i).doubleData(); - } - } - - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; - - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); - - interface->inverseDynamics(m_configuration, - frame, - m_jointsVelocity, - m_baseVelocity, - m_jointsAcceleration, - m_baseAcceleration, - m_gravity, - m_torques); - - Signal output = blockInfo->getOutputPortSignal(0); - output.setBuffer(m_torques, blockInfo->getOutputPortWidth(0)); - - return true; + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; } - return false; + + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== + + unsigned signalWidth; + + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + + // Base velocity + Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); + double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); + robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), + AngVelocity(m_baseVelocityBuffer+3, 3)); + + // Joints velocity + Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); + robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); + + // Base acceleration + Signal baseAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_ACC); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_ACC); + m_baseAcceleration->fillBuffer(baseAccelerationSignal.getStdVector(signalWidth).data()); + + // Joints acceleration + Signal jointsAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_ACC); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_ACC); + m_jointsAcceleration->fillBuffer(jointsAccelerationSignal.getStdVector(signalWidth).data()); + + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + // OUTPUT + // ====== + + // Calculate the inverse dynamics (assuming zero external forces) + model->inverseDynamics(*m_baseAcceleration, + *m_jointsAcceleration, + LinkNetExternalWrenches(model->getNrOfLinks()), // TODO + *m_torques); + + // Forward the output to Simulink + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_TORQUES); + output.setBuffer(m_torques->jointTorques().data(), + blockInfo->getOutputPortWidth(OUTPUT_IDX_TORQUES)); + return true; } } From 58eecea9eb6c207136d589d0afe5eb3314220d2f Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:20:13 +0000 Subject: [PATCH 21/89] Jacobian refactored --- toolbox/include/Jacobian.h | 44 ++++--- toolbox/src/Jacobian.cpp | 251 +++++++++++++++++++++++-------------- 2 files changed, 183 insertions(+), 112 deletions(-) diff --git a/toolbox/include/Jacobian.h b/toolbox/include/Jacobian.h index 0c5307a19..b0726e900 100644 --- a/toolbox/include/Jacobian.h +++ b/toolbox/include/Jacobian.h @@ -1,36 +1,44 @@ #ifndef WBT_JACOBIAN_H #define WBT_JACOBIAN_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include namespace wbt { class Jacobian; } -class wbt::Jacobian : public wbt::WBIModelBlock { +namespace iDynTree { + class MatrixDynSize; +} + +class wbt::Jacobian : public wbt::WBBlock +{ +private: + // Support variables + iDynTree::MatrixDynSize* m_jacobianCOM; - double *m_basePose; - double *m_jacobian; + // Output + iDynTree::MatrixDynSize* m_jacobian; - //input buffers - double *m_basePoseRaw; - double *m_configuration; + // Other variables + bool m_frameIsCoM; + iDynTree::FrameIndex m_frameIndex; - int m_frameIndex; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned OUTPUT_IDX_FW_FRAME; public: - static std::string ClassName; + static const std::string ClassName; Jacobian(); - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_JACOBIAN_H */ +#endif /* WBT_JACOBIAN_H */ diff --git a/toolbox/src/Jacobian.cpp b/toolbox/src/Jacobian.cpp index eefcc8d1c..8988cf4bc 100644 --- a/toolbox/src/Jacobian.cpp +++ b/toolbox/src/Jacobian.cpp @@ -1,161 +1,224 @@ #include "Jacobian.h" -#include "Error.h" -#include "WBInterface.h" +#include "Log.h" +#include "RobotInterface.h" #include "BlockInformation.h" #include "Signal.h" -#include -#include +#include +#include +#include #include namespace wbt { - std::string Jacobian::ClassName = "Jacobian"; + const std::string Jacobian::ClassName = "Jacobian"; + + const unsigned Jacobian::INPUT_IDX_BASE_POSE = 0; + const unsigned Jacobian::INPUT_IDX_JOINTCONF = 1; + const unsigned Jacobian::OUTPUT_IDX_FW_FRAME = 0; Jacobian::Jacobian() - : m_basePose(0) - , m_jacobian(0) - , m_basePoseRaw(0) - , m_configuration(0) - , m_frameIndex(-1) {} + : m_jacobianCOM(nullptr) + , m_jacobian(nullptr) + , m_frameIsCoM(false) + , m_frameIndex(iDynTree::FRAME_INVALID_INDEX) + {} unsigned Jacobian::numberOfParameters() { - return WBIBlock::numberOfParameters() + 1; + return WBBlock::numberOfParameters() + 1; } - bool Jacobian::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool Jacobian::configureSizeAndPorts(BlockInformation* blockInfo) { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + // Memory allocation / Saving data not allowed here - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // + // Number of inputs if (!blockInfo->setNumberOfInputPorts(2)) { - if (error) error->message = "Failed to configure the number of input ports"; + Log::getSingleton().error("Failed to configure the number of input ports."); return false; } + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Size and type bool success = true; - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); - success = success && blockInfo->setInputPortVectorSize(1, dofs); + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); if (!success) { - if (error) error->message = "Failed to configure input ports"; + Log::getSingleton().error("Failed to configure input ports."); return false; } - // Output port: - // - (6)x(6+dofs) matrix - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; + // OUTPUTS + // ======= + // + // 1) Matrix representing the Jacobian (6x(DoFs+6)) + // + + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); return false; } - success = blockInfo->setOutputPortMatrixSize(0, 6, 6 + dofs); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + // Size and type + success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_FW_FRAME, 6, 6 + dofs); + blockInfo->setOutputPortType(OUTPUT_IDX_FW_FRAME, PortDataTypeDouble); return success; } - bool Jacobian::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool Jacobian::initialize(BlockInformation* blockInfo) { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; + if (!WBBlock::initialize(blockInfo)) return false; + + // INPUT PARAMETERS + // ================ - int parentParameters = WBIBlock::numberOfParameters() + 1; - //robot name std::string frame; - if (!blockInfo->getStringParameterAtIndex(parentParameters, frame)) { - if (error) error->message = "Cannot retrieve string from frame parameter"; + int parentParameters = WBBlock::numberOfParameters(); + + if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + Log::getSingleton().error("Cannot retrieve string from frame parameter."); return false; } - //here obtain joint list and get the frame - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (!interface) { - if (error) error->message = "Cannot retrieve handle to WBI interface"; + // Check if the frame is valid + // --------------------------- + + const auto& model = getRobotInterface()->getKinDynComputations(); + if (!model) { + Log::getSingleton().error("Cannot retrieve handle to KinDynComputations."); return false; } - wbi::IDList frames = interface->getFrameList(); + if (frame != "com") { - if (!frames.idToIndex(wbi::ID(frame), m_frameIndex)) { - if (error) error->message = "Cannot find " + frame + " in the frame list"; + m_frameIndex = model->getFrameIndex(frame); + if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { + Log::getSingleton().error("Cannot find " + frame + " in the frame list."); return false; } - } else { - m_frameIndex = wbi::wholeBodyInterface::COM_LINK_ID; } + else { + m_frameIsCoM = true; + m_frameIndex = iDynTree::FRAME_INVALID_INDEX; + } + + // OUTPUT / VARIABLES + // ================== - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_jacobian = new double[6 * (6 + dofs)]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - return m_basePose && m_jacobian && m_basePoseRaw && m_configuration; + m_jacobianCOM = new iDynTree::MatrixDynSize(3, 6 + dofs); + m_jacobianCOM->zero(); + + // Output + m_jacobian = new iDynTree::MatrixDynSize(6, 6 + dofs); + m_jacobian->zero(); + + return m_jacobianCOM && m_jacobian; } - bool Jacobian::terminate(BlockInformation *blockInfo, wbt::Error *error) + bool Jacobian::terminate(BlockInformation* blockInfo) { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; + if (m_jacobianCOM) { + delete m_jacobianCOM; + m_jacobianCOM = nullptr; } if (m_jacobian) { - delete [] m_jacobian; - m_jacobian = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; + delete m_jacobian; + m_jacobian = nullptr; } - return WBIModelBlock::terminate(blockInfo, error); + + return WBBlock::terminate(blockInfo); } - bool Jacobian::output(BlockInformation *blockInfo, wbt::Error */*error*/) + bool Jacobian::output(BlockInformation* blockInfo) { - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; + typedef Matrix MatrixXdSimulink; + typedef Matrix MatrixXdiDynTree; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } + + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; + unsigned signalWidth; - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - interface->computeJacobian(m_configuration, frame, m_frameIndex, m_jacobian); + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - Eigen::Map > jacobianRowMajor(m_jacobian, 6, 6 + dofs); + // TODO: what about the other inputs of setRobotState? - Signal output = blockInfo->getOutputPortSignal(0); - Eigen::Map > jacobianColMajor((double*)output.getContiguousBuffer(), 6, 6 + dofs); + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); - jacobianColMajor = jacobianRowMajor; - return true; + // OUTPUT + // ====== + + iDynTree::Transform world_H_frame; + + // Compute the jacobian + bool ok = false; + if (!m_frameIsCoM) { + world_H_frame = model->getWorldTransform(m_frameIndex); + ok = model->getFrameFreeFloatingJacobian(m_frameIndex, *m_jacobian); } - return false; - } + else { + world_H_frame.setPosition(model->getCenterOfMassPosition()); + ok = model->getCenterOfMassJacobian(*m_jacobianCOM); + int cols = m_jacobianCOM->cols(); + toEigen(*m_jacobian).block(0,0,3,cols) = toEigen(*m_jacobianCOM); + toEigen(*m_jacobian).block(3,0,3,cols).setZero(); + } + + // Get the output signal memory location + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_FW_FRAME); + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Allocate objects for row-major -> col-major conversion + Map jacobianRowMajor = toEigen(*m_jacobian); + Map jacobianColMajor((double*)output.getContiguousBuffer(), + 6, 6 + dofs); + + // Forward the buffer to Simulink transforming it to ColMajor + jacobianColMajor = jacobianRowMajor; + return true; + } } From 648aef94d8071bcaf6e95d66d65a2336f0d1e23b Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:21:07 +0000 Subject: [PATCH 22/89] MassMatrix refactored --- toolbox/include/MassMatrix.h | 34 +++--- toolbox/src/MassMatrix.cpp | 198 ++++++++++++++++++++--------------- 2 files changed, 132 insertions(+), 100 deletions(-) diff --git a/toolbox/include/MassMatrix.h b/toolbox/include/MassMatrix.h index ea0d58ce0..6529d4504 100644 --- a/toolbox/include/MassMatrix.h +++ b/toolbox/include/MassMatrix.h @@ -1,33 +1,35 @@ #ifndef WBT_MASSMATRIX_H #define WBT_MASSMATRIX_H -#include "WBIModelBlock.h" +#include "WBBlock.h" namespace wbt { class MassMatrix; } -class wbt::MassMatrix : public wbt::WBIModelBlock { +namespace iDynTree { + class MatrixDynSize; +} - double *m_basePose; - double *m_massMatrix; +class wbt::MassMatrix : public wbt::WBBlock +{ +private: + iDynTree::MatrixDynSize* m_massMatrix; - //input buffers - double *m_basePoseRaw; - double *m_configuration; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned OUTPUT_IDX_MASS_MAT; public: - static std::string ClassName; + static const std::string ClassName; MassMatrix(); + ~MassMatrix() = default; - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_MASSMATRIX_H */ +#endif /* WBT_MASSMATRIX_H */ diff --git a/toolbox/src/MassMatrix.cpp b/toolbox/src/MassMatrix.cpp index 2644213ee..4fcbf7510 100644 --- a/toolbox/src/MassMatrix.cpp +++ b/toolbox/src/MassMatrix.cpp @@ -2,130 +2,160 @@ #include "BlockInformation.h" #include "Signal.h" -#include "Error.h" -#include "WBInterface.h" -#include -#include +#include "Log.h" +#include "RobotInterface.h" +#include +#include +#include #include +// TODO: remove wbt nesting; namespace wbt { - std::string MassMatrix::ClassName = "MassMatrix"; + const std::string MassMatrix::ClassName = "MassMatrix"; + + const unsigned MassMatrix::INPUT_IDX_BASE_POSE = 0; + const unsigned MassMatrix::INPUT_IDX_JOINTCONF = 1; + const unsigned MassMatrix::OUTPUT_IDX_MASS_MAT = 0; MassMatrix::MassMatrix() - : m_basePose(0) - , m_massMatrix(0) - , m_basePoseRaw(0) - , m_configuration(0) {} + : m_massMatrix(nullptr) + {} - bool MassMatrix::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool MassMatrix::configureSizeAndPorts(BlockInformation* blockInfo) { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) { return false; } - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // + // Number of inputs if (!blockInfo->setNumberOfInputPorts(2)) { - if (error) error->message = "Failed to configure the number of input ports"; + Log::getSingleton().error("Failed to configure the number of input ports."); return false; } + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Size and type bool success = true; - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); - success = success && blockInfo->setInputPortVectorSize(1, dofs); - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); if (!success) { - if (error) error->message = "Failed to configure input ports"; + Log::getSingleton().error("Failed to configure input ports."); return false; } - // Output port: - // - (DoFs + 6)x(DoFs + 6) matrix representing the mass matrix - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; + // OUTPUTS + // ======= + // + // 1) Matrix epresenting the mass matrix (DoFs+6)x(DoFs+6) + // + + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); return false; } - success = blockInfo->setOutputPortMatrixSize(0, dofs + 6, dofs + 6); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + // Size and type + success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_MASS_MAT, dofs + 6, dofs + 6); + blockInfo->setOutputPortType(OUTPUT_IDX_MASS_MAT, PortDataTypeDouble); return success; } - bool MassMatrix::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool MassMatrix::initialize(BlockInformation* blockInfo) { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; + if (!WBBlock::initialize(blockInfo)) return false; - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_massMatrix = new double[(6 + dofs)*(6 + dofs)]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - return m_basePose && m_massMatrix && m_basePoseRaw && m_configuration; + // Output + m_massMatrix = new iDynTree::MatrixDynSize(6 + dofs, 6 + dofs); + m_massMatrix->zero(); + + return m_massMatrix; } - bool MassMatrix::terminate(BlockInformation *blockInfo, wbt::Error *error) + bool MassMatrix::terminate(BlockInformation* blockInfo) { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } if (m_massMatrix) { - delete [] m_massMatrix; - m_massMatrix = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; + delete m_massMatrix; + m_massMatrix = nullptr; } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - return WBIModelBlock::terminate(blockInfo, error); + + return WBBlock::terminate(blockInfo); } - bool MassMatrix::output(BlockInformation *blockInfo, wbt::Error */*error*/) + bool MassMatrix::output(BlockInformation* blockInfo) { - //get input - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } - - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; - - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); - - interface->computeMassMatrix(m_configuration, frame, m_massMatrix); - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - Eigen::Map > massMatrixRowMajor(m_massMatrix, 6 + dofs, 6 + dofs); - - Signal output = blockInfo->getOutputPortSignal(0); - Eigen::Map > massMatrixColMajor((double*)output.getContiguousBuffer(), 6 + dofs, 6 + dofs); - massMatrixColMajor = massMatrixRowMajor; - return true; + using namespace Eigen; + using namespace iDynTree; + typedef Matrix Matrix4dSimulink; + typedef Matrix MatrixXdSimulink; + typedef Matrix MatrixXdiDynTree; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; } - return false; + + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== + + unsigned signalWidth; + + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + // OUTPUT + // ====== + + // Compute the Mass Matrix + model->getFreeFloatingMassMatrix(*m_massMatrix); + + // Get the output signal memory location + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_MASS_MAT); + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Allocate objects for row-major -> col-major conversion + Map massMatrixRowMajor = toEigen(*m_massMatrix); + Map massMatrixColMajor((double*)output.getContiguousBuffer(), + 6 + dofs, 6 + dofs); + + // Forward the buffer to Simulink transforming it to ColMajor + massMatrixColMajor = massMatrixRowMajor; + return true; } } From 6953aa23b16fdd2aed19d0ddc32729dc39481118 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:22:20 +0000 Subject: [PATCH 23/89] MinimumJerkTrajectoryGenerator refactored --- .../include/MinimumJerkTrajectoryGenerator.h | 35 ++-- .../src/MinimumJerkTrajectoryGenerator.cpp | 159 ++++++++++++------ 2 files changed, 128 insertions(+), 66 deletions(-) diff --git a/toolbox/include/MinimumJerkTrajectoryGenerator.h b/toolbox/include/MinimumJerkTrajectoryGenerator.h index ba0415e9f..8bf0f29b7 100644 --- a/toolbox/include/MinimumJerkTrajectoryGenerator.h +++ b/toolbox/include/MinimumJerkTrajectoryGenerator.h @@ -22,19 +22,19 @@ namespace yarp { class wbt::MinimumJerkTrajectoryGenerator : public wbt::Block { public: - static std::string ClassName; - + static const std::string ClassName; + MinimumJerkTrajectoryGenerator(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; + private: - iCub::ctrl::minJerkTrajGen *m_generator; + iCub::ctrl::minJerkTrajGen* m_generator; int m_outputFirstDerivativeIndex; int m_outputSecondDerivativeIndex; @@ -45,9 +45,18 @@ class wbt::MinimumJerkTrajectoryGenerator : public wbt::Block { bool m_explicitInitialValue; bool m_externalSettlingTime; bool m_resetOnSettlingTimeChange; - yarp::sig::Vector *m_initialValues; - yarp::sig::Vector *m_reference; - + yarp::sig::Vector* m_initialValues; + yarp::sig::Vector* m_reference; + + static const unsigned PARAM_IDX_SAMPLE_TIME; // Sample Time (double) + static const unsigned PARAM_IDX_SETTLING_TIME; // Settling Time (double) + static const unsigned PARAM_IDX_OUTPUT_1ST_DERIVATIVE; // Output first derivative (boolean) + static const unsigned PARAM_IDX_OUTPUT_2ND_DERIVATIVE; // Output second derivative (boolean) + static const unsigned PARAM_IDX_INITIAL_VALUE; // Initial signal value as input (boolean) + static const unsigned PARAM_IDX_EXT_SETTLINGTIME; // Control if the settling time comes from + // external port or static parameter + static const unsigned PARAM_IDX_RESET_CHANGEST; // True if the block should reset the traj + // generator in case settling time changes }; #endif /* end of include guard: WBT_MINJERKTRAJGENERATOR_H */ diff --git a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp index 65832b2bd..2760a7624 100644 --- a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp +++ b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp @@ -1,6 +1,6 @@ #include "MinimumJerkTrajectoryGenerator.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" @@ -13,48 +13,76 @@ #include -#define PARAM_IDX_1 1 //Sample Time (double) -#define PARAM_IDX_2 2 //Settling Time (double) -#define PARAM_IDX_3 3 //Output first derivative (boolean) -#define PARAM_IDX_4 4 //Output second derivative (boolean) -#define PARAM_IDX_5 5 //Initial signal value as input (boolean) -#define PARAM_IDX_6 6 //Control if the settling time comes from external port or static parameter -#define PARAM_IDX_7 7 //True if the block should reset the traj generator in case settling time changes - namespace wbt { - - std::string MinimumJerkTrajectoryGenerator::ClassName = "MinimumJerkTrajectoryGenerator"; + + const std::string MinimumJerkTrajectoryGenerator::ClassName = "MinimumJerkTrajectoryGenerator"; + + const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_SAMPLE_TIME = 1; + const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_SETTLING_TIME = 2; + const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_OUTPUT_1ST_DERIVATIVE = 3; + const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_OUTPUT_2ND_DERIVATIVE = 4; + const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_INITIAL_VALUE = 5; + const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_EXT_SETTLINGTIME = 6; + const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_RESET_CHANGEST = 7; MinimumJerkTrajectoryGenerator::MinimumJerkTrajectoryGenerator() - : m_generator(0) + : m_generator(nullptr) , m_outputFirstDerivativeIndex(-1) , m_outputSecondDerivativeIndex(-1) , m_firstRun(true) , m_explicitInitialValue(false) , m_externalSettlingTime(false) , m_resetOnSettlingTimeChange(false) - , m_initialValues(0) - , m_reference(0) {} - + , m_initialValues(nullptr) + , m_reference(nullptr) + {} + unsigned MinimumJerkTrajectoryGenerator::numberOfParameters() { return 7; } - bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation* blockInfo) { - bool outputFirstDerivative = blockInfo->getScalarParameterAtIndex(PARAM_IDX_3).booleanData(); - bool outputSecondDerivative = blockInfo->getScalarParameterAtIndex(PARAM_IDX_4).booleanData(); - bool explicitInitialValue = blockInfo->getScalarParameterAtIndex(PARAM_IDX_5).booleanData(); - bool externalSettlingTime = blockInfo->getScalarParameterAtIndex(PARAM_IDX_6).booleanData(); + // PARAMETERS + // ========== + // + // 1) Sample time (double) + // 2) Settling time (double) + // 3) Enable the output of 1st derivative (bool) + // 4) Enable the output of 2nd derivative (bool) + // 5) Enable the input with the initial conditions (bool) + // 6) Enable the input with the external settling time (bool) + // 7) Reset the trajectory generator when settling time changes (bool) + // + + bool outputFirstDerivative; + bool outputSecondDerivative; + bool explicitInitialValue; + bool externalSettlingTime; + bool ok = true; + + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, outputFirstDerivative); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, outputSecondDerivative); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, explicitInitialValue); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, externalSettlingTime); + + if (!ok) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } int numberOfInputPorts = 1; - if (explicitInitialValue) - numberOfInputPorts++; - if (externalSettlingTime) - numberOfInputPorts++; + numberOfInputPorts += static_cast(explicitInitialValue); + numberOfInputPorts += static_cast(externalSettlingTime); - // Specify I/O // INPUTS + // ====== + // + // 1) The reference signal (1xn) + // 2) The optional initial conditions + // 3) The optional settling time + // + if(!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { - if (error) error->message = "Failed to set input port number"; + Log::getSingleton().error("Failed to set input port number."); return false; } @@ -69,20 +97,26 @@ namespace wbt { portIndex++; } - if (externalSettlingTime) - { + if (externalSettlingTime) { blockInfo->setInputPortVectorSize(portIndex, 1); blockInfo->setInputPortType(portIndex, PortDataTypeDouble); portIndex++; } // OUTPUTS + // ======= + // + // 1) The calculated trajectory + // 2) The optional 1st derivative + // 3) The optional 2nd derivative + // + int numberOfOutputPorts = 1; - if (outputFirstDerivative) numberOfOutputPorts++; - if (outputSecondDerivative) numberOfOutputPorts++; + numberOfOutputPorts += static_cast(outputFirstDerivative); + numberOfOutputPorts += static_cast(outputSecondDerivative); - if (!blockInfo->setNumberOfOuputPorts(numberOfOutputPorts)) { - if (error) error->message = "Failed to set output port number"; + if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { + Log::getSingleton().error("Failed to set output port number."); return false; } @@ -94,32 +128,50 @@ namespace wbt { return true; } - bool MinimumJerkTrajectoryGenerator::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool MinimumJerkTrajectoryGenerator::initialize(BlockInformation* blockInfo) { - //Save parameters - bool outputFirstDerivative = blockInfo->getScalarParameterAtIndex(PARAM_IDX_3).booleanData(); - bool outputSecondDerivative = blockInfo->getScalarParameterAtIndex(PARAM_IDX_4).booleanData(); + // Get the additional parameters + bool outputFirstDerivative; + bool outputSecondDerivative; + bool ok = true; + + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, + outputFirstDerivative); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, + outputSecondDerivative); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, m_explicitInitialValue); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, m_externalSettlingTime); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_RESET_CHANGEST, + m_resetOnSettlingTimeChange); + + if (!ok) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } + + if (outputFirstDerivative) { + m_outputFirstDerivativeIndex = 1; + } - if (outputFirstDerivative) m_outputFirstDerivativeIndex = 1; if (outputSecondDerivative) { m_outputSecondDerivativeIndex = outputFirstDerivative ? 2 : 1; } - double sampleTime = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).doubleData(); - double settlingTime = blockInfo->getScalarParameterAtIndex(PARAM_IDX_2).doubleData(); + double sampleTime; + double settlingTime; - m_explicitInitialValue = blockInfo->getScalarParameterAtIndex(PARAM_IDX_5).booleanData(); - m_externalSettlingTime = blockInfo->getScalarParameterAtIndex(PARAM_IDX_6).booleanData(); - m_resetOnSettlingTimeChange = blockInfo->getScalarParameterAtIndex(PARAM_IDX_7).booleanData(); + ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_SAMPLE_TIME, sampleTime); + ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_SETTLING_TIME, settlingTime); - unsigned size = blockInfo->getInputPortWidth(0); + unsigned signalSize = blockInfo->getInputPortWidth(0); - m_generator = new iCub::ctrl::minJerkTrajGen(size, sampleTime, settlingTime); + m_generator = new iCub::ctrl::minJerkTrajGen(signalSize, sampleTime, settlingTime); m_previousSettlingTime = settlingTime; - m_initialValues = new yarp::sig::Vector(size); - m_reference = new yarp::sig::Vector(size); + m_initialValues = new yarp::sig::Vector(signalSize); + m_reference = new yarp::sig::Vector(signalSize); + if (!m_generator || !m_initialValues || !m_reference) { - if (error) error->message = "Could not allocate memory for trajectory generator"; + Log::getSingleton().error("Could not allocate memory for trajectory generator."); return false; } @@ -127,24 +179,24 @@ namespace wbt { return true; } - bool MinimumJerkTrajectoryGenerator::terminate(BlockInformation *blockInfo, wbt::Error *error) + bool MinimumJerkTrajectoryGenerator::terminate(BlockInformation* blockInfo) { if (m_generator) { delete m_generator; - m_generator = 0; + m_generator = nullptr; } if (m_initialValues) { delete m_initialValues; - m_initialValues = 0; + m_initialValues = nullptr; } if (m_reference) { delete m_reference; - m_reference = 0; + m_reference = nullptr; } return true; } - - bool MinimumJerkTrajectoryGenerator::output(BlockInformation *blockInfo, wbt::Error *error) + + bool MinimumJerkTrajectoryGenerator::output(BlockInformation* blockInfo) { if (!m_generator) return false; @@ -198,6 +250,7 @@ namespace wbt { Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputFirstDerivativeIndex); derivativeOutput.setBuffer(derivative.data(), blockInfo->getOutputPortWidth(m_outputFirstDerivativeIndex)); } + if (m_outputSecondDerivativeIndex != -1) { const yarp::sig::Vector &derivative = m_generator->getAcc(); Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputSecondDerivativeIndex); From c931213cffe5b515603b50151730494488a4a938 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:24:36 +0000 Subject: [PATCH 24/89] RealTimeSyncronizer refactored --- toolbox/include/RealTimeSynchronizer.h | 25 ++++++---- toolbox/src/RealTimeSynchronizer.cpp | 69 ++++++++++++++++---------- 2 files changed, 58 insertions(+), 36 deletions(-) diff --git a/toolbox/include/RealTimeSynchronizer.h b/toolbox/include/RealTimeSynchronizer.h index 3438d727f..5a2bc601d 100644 --- a/toolbox/include/RealTimeSynchronizer.h +++ b/toolbox/include/RealTimeSynchronizer.h @@ -7,24 +7,27 @@ namespace wbt { class RealTimeSynchronizer; } -class wbt::RealTimeSynchronizer : public wbt::Block { +class wbt::RealTimeSynchronizer : public wbt::Block +{ public: - static std::string ClassName; - + static const std::string ClassName; + RealTimeSynchronizer(); - virtual ~RealTimeSynchronizer(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + ~RealTimeSynchronizer() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; + private: double m_period; double m_initialTime; unsigned long m_counter; + + static const unsigned PARAM_PERIOD; // Period }; #endif /* end of include guard: WBT_REALTIMESYNCHRONIZER_H */ diff --git a/toolbox/src/RealTimeSynchronizer.cpp b/toolbox/src/RealTimeSynchronizer.cpp index 678350493..752674d44 100644 --- a/toolbox/src/RealTimeSynchronizer.cpp +++ b/toolbox/src/RealTimeSynchronizer.cpp @@ -1,69 +1,88 @@ #include "RealTimeSynchronizer.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include -#define PARAM_IDX_1 1 // Period - namespace wbt { - - std::string RealTimeSynchronizer::ClassName = "RealTimeSynchronizer"; + + const std::string RealTimeSynchronizer::ClassName = "RealTimeSynchronizer"; + + const unsigned RealTimeSynchronizer::PARAM_PERIOD = 1; // Period RealTimeSynchronizer::RealTimeSynchronizer() : m_period(0.01) , m_initialTime(0) - , m_counter(0) {} + , m_counter(0) + {} - RealTimeSynchronizer::~RealTimeSynchronizer() {} - unsigned RealTimeSynchronizer::numberOfParameters() { return 1; } - bool RealTimeSynchronizer::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool RealTimeSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) { + // INPUTS + // ====== + // + // No inputs + // + if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to set input port number to 0"; + Log::getSingleton().error("Failed to set input port number to 0."); return false; } - - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to set output port number"; + + // OUTPUTS + // ======= + // + // No outputs + // + + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to set output port number."); return false; } return true; } - bool RealTimeSynchronizer::initialize(BlockInformation *blockInfo, wbt::Error */*error*/) + bool RealTimeSynchronizer::initialize(BlockInformation* blockInfo) { - m_period = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).doubleData(); + if (!blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period)) { + Log::getSingleton().error("Failed to get input parametes."); + return false; + } + + if (m_period < 0) { + Log::getSingleton().error("Period must be greater than 0."); + return false; + } + m_counter = 0; - return m_period > 0; + return true; } - - bool RealTimeSynchronizer::terminate(BlockInformation *blockInfo, wbt::Error */*error*/) + + bool RealTimeSynchronizer::terminate(BlockInformation* blockInfo) { return true; } - - bool RealTimeSynchronizer::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - using namespace yarp::os; + bool RealTimeSynchronizer::output(BlockInformation* blockInfo) + { if (m_counter == 0) { m_initialTime = yarp::os::Time::now(); } //read current time double currentTime = yarp::os::Time::now() - m_initialTime; - double desiredTime = m_counter * m_period; + double desiredTime = m_counter* m_period; double sleepPeriod = desiredTime - currentTime; //sleep for the remaining time - if (sleepPeriod > 0) + if (sleepPeriod > 0) { yarp::os::Time::delay(sleepPeriod); - + } + m_counter++; return true; From 0bc69bd31e400d7ce8801fb4bec39b025041f669 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 10 Nov 2017 14:39:46 +0000 Subject: [PATCH 25/89] SetLowLevelPID refactored --- toolbox/include/SetLowLevelPID.h | 70 +++--- toolbox/include/base/toolbox.h | 4 +- toolbox/src/SetLowLevelPID.cpp | 373 ++++++++++++++----------------- toolbox/src/base/factory.cpp | 7 +- 4 files changed, 199 insertions(+), 255 deletions(-) diff --git a/toolbox/include/SetLowLevelPID.h b/toolbox/include/SetLowLevelPID.h index e0d154a27..ff781a885 100644 --- a/toolbox/include/SetLowLevelPID.h +++ b/toolbox/include/SetLowLevelPID.h @@ -1,65 +1,49 @@ #ifndef WBT_SETLOWLEVELPID_H_ #define WBT_SETLOWLEVELPID_H_ -#include "WBIBlock.h" -#include -#include +#include "WBBlock.h" +#include +#include namespace wbt { class SetLowLevelPID; -} - -namespace wbi { - class wholeBodyInterface; - class iWholeBodyActuators; -} - -namespace yarpWbi { - class PIDList; + enum PidDataIndex { + PGAIN = 0, + IGAIN = 1, + DGAIN = 2 + }; + typedef std::tuple PidData; } namespace yarp { - namespace os { - class Value; + namespace dev { + class Pid; } } -typedef std::map PidMap; - -class wbt::SetLowLevelPID : public wbt::WBIBlock { +class wbt::SetLowLevelPID : public wbt::WBBlock +{ +private: + std::vector m_appliedPidValues; + std::vector m_defaultPidValues; + std::unordered_map m_pidJointsFromParameters; - bool m_firstRun; - PidMap m_pids; - int m_lastGainSetIndex; - wbi::ControlMode m_controlMode; + yarp::dev::PidControlTypeEnum m_controlType; - bool loadLowLevelGainsFromFile(std::string filename, - const yarpWbi::PIDList &originalList, - wbi::wholeBodyInterface& interface, - yarpWbi::PIDList &loadedPIDs); - - bool loadGainsFromValue(const yarp::os::Value &gains, - PidMap &pidMap, - wbi::wholeBodyInterface& interface); - - bool setCurrentGains(const PidMap &pidMap, - std::string key, - wbi::iWholeBodyActuators& actuators); + bool readWBTPidConfigObject(BlockInformation* blockInfo); public: - static std::string ClassName; - SetLowLevelPID(); + static const std::string ClassName; - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + SetLowLevelPID() = default; + ~SetLowLevelPID() override = default; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; - + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; }; - - #endif /* end of include guard: WBT_SETLOWLEVELPID_H_ */ diff --git a/toolbox/include/base/toolbox.h b/toolbox/include/base/toolbox.h index a1ff7505f..71a034c28 100644 --- a/toolbox/include/base/toolbox.h +++ b/toolbox/include/base/toolbox.h @@ -16,13 +16,11 @@ #include "DotJNu.h" #include "GetLimits.h" #include "CentroidalMomentum.h" +#include "SetLowLevelPID.h" #ifdef WBT_USES_ICUB #include "MinimumJerkTrajectoryGenerator.h" #endif #ifdef WBT_USES_IPOPT #include "InverseKinematics.h" -#endif #include "RemoteInverseKinematics.h" -#ifdef WBT_USES_CODYCO_COMMONS -#include "SetLowLevelPID.h" #endif diff --git a/toolbox/src/SetLowLevelPID.cpp b/toolbox/src/SetLowLevelPID.cpp index b772cb742..9448b4eb0 100644 --- a/toolbox/src/SetLowLevelPID.cpp +++ b/toolbox/src/SetLowLevelPID.cpp @@ -1,275 +1,240 @@ #include "SetLowLevelPID.h" -#include "Error.h" -#include "WBInterface.h" +#include "Log.h" +#include "RobotInterface.h" #include "BlockInformation.h" -#include -#include -#include -#include -#include -#include +#include +#include namespace wbt { - static const std::string TorquePIDInitialKey = "__ORIGINAL_PIDs__"; - static const std::string TorquePIDDefaultKey = "__DEFAULT_PIDs__"; + const std::string SetLowLevelPID::ClassName = "SetLowLevelPID"; -#pragma mark - SetLowLevelPID class implementation - - std::string SetLowLevelPID::ClassName = "SetLowLevelPID"; + unsigned SetLowLevelPID::numberOfParameters() + { + return WBBlock::numberOfParameters() + + 1 // WBTPIDConfig object + + 1; // Control type + } - SetLowLevelPID::SetLowLevelPID() - : m_firstRun(true) - , m_lastGainSetIndex(-1) - , m_controlMode(wbi::CTRL_MODE_TORQUE) {} + bool SetLowLevelPID::configureSizeAndPorts(BlockInformation* blockInfo) + { + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + // INPUTS + // ====== + // + // No inputs + // - bool SetLowLevelPID::loadLowLevelGainsFromFile(std::string filename, - const yarpWbi::PIDList &originalList, - wbi::wholeBodyInterface& interface, - yarpWbi::PIDList &loadedPIDs) - { - //List of list. Each element has a key: joint name, and a list of pairs: kp, kd, ki and its respective value - using namespace yarp::os; - yarp::os::ResourceFinder resourceFinder = yarp::os::ResourceFinder::getResourceFinderSingleton(); - Property file; - std::string fileName = resourceFinder.findFileByName(filename); - if (fileName.empty()) return false; - file.fromConfigFile(fileName); - - Bottle externalList; - externalList.fromString(file.toString()); - - bool result = true; - wbi::IDList jointList = interface.getJointList(); - for (int i = 0; i < externalList.size(); ++i) { - if (!externalList.get(i).isList()) continue; - Bottle *jointConfig = externalList.get(i).asList(); - if (jointConfig->size() < 2 || !jointConfig->get(0).isString()) continue; - wbi::ID jointID(jointConfig->get(0).asString()); - int jointIndex = -1; - if (!jointList.idToIndex(jointID, jointIndex)) continue; - if (jointIndex < 0 || jointIndex >= jointList.size()) { - yWarning("Specified joint %s index is outside joint list size", jointID.toString().c_str()); - continue; - } + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } - loadedPIDs.pidList()[jointIndex] = originalList.pidList()[jointIndex]; - loadedPIDs.motorParametersList()[jointIndex] = originalList.motorParametersList()[jointIndex]; + // OUTPUTS + // ======= + // + // No outputs + // - result = result && yarpWbi::pidFromBottleDescription(*jointConfig, loadedPIDs.pidList()[jointIndex]); - if (m_controlMode == wbi::CTRL_MODE_TORQUE) { - result = result && yarpWbi::motorTorqueParametersFromBottleDescription(*jointConfig, loadedPIDs.motorParametersList()[jointIndex]); - } + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; } - return result; + + return true; } - bool SetLowLevelPID::loadGainsFromValue(const yarp::os::Value &gains, - PidMap &pidMap, - wbi::wholeBodyInterface& interface) + bool SetLowLevelPID::readWBTPidConfigObject(BlockInformation* blockInfo) { - pidMap.clear(); - - yarpWbi::yarpWholeBodyInterface *yarpInterface = dynamic_cast(&interface); - if (!yarpInterface) { + AnyStruct s; + if (!blockInfo->getStructAtIndex(WBBlock::numberOfParameters() + 1, s)) { + Log::getSingleton().error("Failed to retrieve the struct with parameters."); return false; } - //Load original gains from controlboards and save them to the original key. - yarpWbi::PIDList originalGains(interface.getDoFs()); - yarpWbi::yarpWholeBodyActuators *actuators = yarpInterface->wholeBodyActuator(); - if (!actuators) { + // Check the existence of all the fields + try { + s.at("P"); + s.at("I"); + s.at("D"); + s.at("jointList"); + } + catch (const std::out_of_range& e) { + Log::getSingleton().error("Cannot retrieve one or more parameter from parameter's struct."); return false; } - //TODO: should be made not limited to torque - actuators->getPIDGains(originalGains.pidList(), wbi::CTRL_MODE_TORQUE); - actuators->getMotorTorqueParameters(originalGains.motorParametersList()); - pidMap.insert(PidMap::value_type(TorquePIDInitialKey, originalGains)); - - //Now load additional gains - bool result = true; - if (gains.isString()) { - yarpWbi::PIDList pids(interface.getDoFs()); - result = loadLowLevelGainsFromFile(gains.asString(), originalGains, interface, pids); - pidMap.insert(PidMap::value_type(TorquePIDDefaultKey, pids)); - } else if (gains.isList()) { - using namespace yarp::os; - Bottle *list = gains.asList(); - - //list of files. gains will be saved as integer-values - for (int i = 0; i < list->size(); ++i) { - if (!list->get(i).isString()) continue; - std::string filename = list->get(i).asString(); - yarpWbi::PIDList pids(interface.getDoFs()); - result = loadLowLevelGainsFromFile(filename, originalGains, interface, pids); - - if (result) { - pidMap.insert(PidMap::value_type(yarpWbi::stringFromInt(i + 1), pids)); - } - } + + // Proportional gains + std::vector Pvector; + if (!s["P"]->asVectorDouble(Pvector)) { + Log::getSingleton().error("Cannot retrieve vector from P parameter."); + return false; } - return result; - } - //TODO: for performance it is probably better to change the map so that the index is an integer - //i.e. a non continuous vector - bool SetLowLevelPID::setCurrentGains(const PidMap &pidMap, - std::string key, - wbi::iWholeBodyActuators& actuators) - { - yarpWbi::yarpWholeBodyActuators *yarpActuators = static_cast(&actuators); - if (!yarpActuators) return false; + // Integral gains + std::vector Ivector; + if (!s["I"]->asVectorDouble(Ivector)) { + Log::getSingleton().error("Cannot retrieve vector from I parameter."); + return false; + } - PidMap::const_iterator found = pidMap.find(key); - //Treat one exception: pidMap with size == 2, the default can be set to either the string or the num 1 - if (found == pidMap.end() && key == TorquePIDDefaultKey && pidMap.size() == 2) { - found = pidMap.find("1"); + // Derivative gains + std::vector Dvector; + if (!s["D"]->asVectorDouble(Dvector)) { + Log::getSingleton().error("Cannot retrieve vector from D parameter."); + return false; } - if (found == pidMap.end()) return false; - bool result = yarpActuators->setPIDGains(found->second.pidList(), m_controlMode); //to be made generic (torque, position, etc) - if (m_controlMode == wbi::CTRL_MODE_TORQUE) { - result = result && yarpActuators->setMotorTorqueParameters(found->second.motorParametersList()); + // Considered joint names + AnyCell jointPidsCell; + if (!s["jointList"]->asAnyCell(jointPidsCell)) { + Log::getSingleton().error("Cannot retrieve string from jointList parameter."); + return false; } - return result; - } -#pragma mark - overloaded methods + // From AnyCell to vector + std::vector jointNamesFromParameters; + for (auto cell: jointPidsCell) { + std::string joint; + if (!cell->asString(joint)) { + Log::getSingleton().error("Failed to convert jointList from cell to strings."); + return false; + } + jointNamesFromParameters.push_back(joint); + } - unsigned SetLowLevelPID::numberOfParameters() - { - return WBIBlock::numberOfParameters() - + 1 // pid parameters file - + 1;// control method - } + assert(Pvector.size() == Ivector.size() == Dvector.size() == jointNamesFromParameters.size()); - bool SetLowLevelPID::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + // Store this data into a private member map + for (unsigned i = 0; i < jointNamesFromParameters.size(); ++i) { + // Check the processed joint is actually a controlledJoint + const auto& controlledJoints = getConfiguration().getControlledJoints(); + auto findElement = std::find(std::begin(controlledJoints), + std::end(controlledJoints), + jointNamesFromParameters[i]); + if (findElement != std::end(controlledJoints)) { + m_pidJointsFromParameters[jointNamesFromParameters[i]] = {Pvector[i], Ivector[i], Dvector[i]}; + } + else { + Log::getSingleton().warning("Attempted to set PID of joint " + jointNamesFromParameters[i] + + " non currently controlled. Skipping it."); + } - // Specify I/O - if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; } - // Output port: - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; + if (m_pidJointsFromParameters.size() != jointNamesFromParameters.size()) { + Log::getSingleton().warning("PID have been passed only for a subset of the controlled joints."); } return true; } - bool SetLowLevelPID::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool SetLowLevelPID::initialize(BlockInformation* blockInfo) { - using namespace yarp::os; - if (!WBIBlock::initialize(blockInfo, error)) return false; + if (!WBBlock::initialize(blockInfo)) return false; // Reading the control type std::string controlType; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 2, controlType)) { - if (error) error->message = "Could not read control type parameter"; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 2, controlType)) { + Log::getSingleton().error("Could not read control type parameter."); return false; } - m_controlMode = wbi::CTRL_MODE_UNKNOWN; if (controlType == "Position") { - m_controlMode = wbi::CTRL_MODE_POS; - } else if (controlType == "Torque") { - m_controlMode = wbi::CTRL_MODE_TORQUE; - } else { - if (error) error->message = "Control Mode not supported"; + m_controlType = yarp::dev::VOCAB_PIDTYPE_POSITION; + } + else if (controlType == "Torque") { + m_controlType = yarp::dev::VOCAB_PIDTYPE_TORQUE; + } + else { + Log::getSingleton().error("Control type not recognized."); return false; } + // Reading the WBTPIDConfig matlab class + if (!readWBTPidConfigObject(blockInfo)) { + Log::getSingleton().error("Failed to parse the WBTPIDConfig object."); + return 1; + } - // Reading the PID specification parameter - std::string pidParameter; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 1, pidParameter)) { - if (error) error->message = "Could not read PID file specification parameter"; + // Retain the RemoteControlBoardRemapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to retain the control board remapper."); return false; } - Value value; - value.fromString(pidParameter.c_str()); - m_pids.clear(); + const unsigned& dofs = getConfiguration().getNumberOfDoFs(); - yarpWbi::yarpWholeBodyInterface * const interface = dynamic_cast(WBInterface::sharedInstance().interface()); - if (!interface) { - if (error) error->message = "This block currently work only with YARP-WBI implementation"; + // Initialize the vector size to the number of dofs + m_defaultPidValues.resize(dofs); + + // Get the interface + std::weak_ptr iPidControl; + if (!getRobotInterface()->getInterface(iPidControl)) { + Log::getSingleton().error("Failed to get IPidControl interface."); return false; } - if (!loadGainsFromValue(value, m_pids, *interface)) { - m_pids.clear(); - if (error) error->message = "Error while loading PIDs configuration"; - yError("Error while loading PIDs configuration"); + + // Store the default gains + if (!iPidControl.lock()->getPids(m_controlType, m_defaultPidValues.data())) { + Log::getSingleton().error("Failed to get default data from IPidControl."); return false; - } else { - yInfo("Loaded PIDs configuration"); } - m_lastGainSetIndex = -1; - m_firstRun = true; - return true; - } + // Initialize the vector of the applied pid gains with the default gains + m_appliedPidValues = m_defaultPidValues; - bool SetLowLevelPID::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - //static_cast as the dynamic has been done in the initialize - //and the pointer should not change - yarpWbi::yarpWholeBodyInterface * const interface = static_cast(WBInterface::sharedInstance().interface()); - - if (interface) { - yarpWbi::yarpWholeBodyActuators *actuators = interface->wholeBodyActuator(); - if (actuators && m_pids.size() > 1) { - setCurrentGains(m_pids, TorquePIDInitialKey, *actuators); + // Override the PID with the gains specified as block parameters + for (unsigned i = 0; i < dofs; ++i) { + const std::string jointName = getConfiguration().getControlledJoints()[i]; + // If the pid has been passed, set the new gains + if (m_pidJointsFromParameters.find(jointName) != m_pidJointsFromParameters.end()) { + PidData gains = m_pidJointsFromParameters[jointName]; + m_appliedPidValues[i].setKp(std::get(gains)); + m_appliedPidValues[i].setKi(std::get(gains)); + m_appliedPidValues[i].setKd(std::get(gains)); } } - m_pids.clear(); - m_lastGainSetIndex = -1; + // Apply the new pid gains + if (!iPidControl.lock()->setPids(m_controlType, m_appliedPidValues.data())) { + Log::getSingleton().error("Failed to set PID values."); + return false; + } - return WBIBlock::terminate(blockInfo, error); + return true; } - bool SetLowLevelPID::output(BlockInformation *blockInfo, wbt::Error *error) + bool SetLowLevelPID::terminate(BlockInformation* blockInfo) { - //static_cast as the dynamic has been done in the initialize - //and the pointer should not change - yarpWbi::yarpWholeBodyInterface * const interface = static_cast(WBInterface::sharedInstance().interface()); - if (interface) { - if (m_firstRun) { - m_firstRun = false; - - yarpWbi::yarpWholeBodyActuators *actuators = interface->wholeBodyActuator(); - if (!actuators) { - if (error) error->message = "Failed to retrieve the interface to the actuators"; - return false; - } - - //First case: only one element - if (m_lastGainSetIndex == -1 && m_pids.size() == 2) { - //just switch to the only existing set - setCurrentGains(m_pids, TorquePIDDefaultKey, *actuators); - m_lastGainSetIndex = 0; - } else { -// InputPtrsType u = ssGetInputPortSignalPtrs(S, 0); -// InputInt8PtrsType uPtrs = (InputInt8PtrsType)u; -// if (*uPtrs[0] != lastGainIndex) { -// wbitoolbox::setCurrentGains(*pids, *uPtrs[0], *((yarpWbi::yarpWholeBodyInterface*)robot->wbInterface)); -// info[0] = *uPtrs[0]; -// } - } + bool ok = true; - } - return true; + // Get the IPidControl interface + std::weak_ptr iPidControl; + ok = ok & getRobotInterface()->getInterface(iPidControl); + if (!ok) { + Log::getSingleton().error("Failed to get IPidControl interface."); + } + + // Reset default pid gains + ok = ok & iPidControl.lock()->setPids(m_controlType, m_defaultPidValues.data()); + if (!ok) { + Log::getSingleton().error("Failed to set default PID values."); } - return false; + + // Release the RemoteControlBoardRemapper + ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + } + + return ok && WBBlock::terminate(blockInfo); + } + + bool SetLowLevelPID::output(BlockInformation* blockInfo) + { + return true; } } diff --git a/toolbox/src/base/factory.cpp b/toolbox/src/base/factory.cpp index 69a962e08..dcbe75d60 100644 --- a/toolbox/src/base/factory.cpp +++ b/toolbox/src/base/factory.cpp @@ -34,6 +34,8 @@ wbt::Block* wbt::Block::instantiateBlockWithClassName(std::string blockClassName block = new wbt::GetLimits(); } else if (blockClassName == wbt::CentroidalMomentum::ClassName) { block = new wbt::CentroidalMomentum(); + } else if (blockClassName == wbt::SetLowLevelPID::ClassName) { + block = new wbt::SetLowLevelPID(); } #ifdef WBT_USES_ICUB else if (blockClassName == wbt::MinimumJerkTrajectoryGenerator::ClassName) { @@ -48,11 +50,6 @@ wbt::Block* wbt::Block::instantiateBlockWithClassName(std::string blockClassName else if (blockClassName == wbt::RemoteInverseKinematics::ClassName) { block = new wbt::RemoteInverseKinematics(); } -#ifdef WBT_USES_CODYCO_COMMONS - else if (blockClassName == wbt::SetLowLevelPID::ClassName) { - block = new wbt::SetLowLevelPID(); - } -#endif return block; From 0eada49c511af56815b6593eeb3b2cab9254b990 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:37:51 +0000 Subject: [PATCH 26/89] SetReferences refactored --- toolbox/include/SetReferences.h | 32 ++-- toolbox/src/SetReferences.cpp | 328 +++++++++++++++++++------------- 2 files changed, 212 insertions(+), 148 deletions(-) diff --git a/toolbox/include/SetReferences.h b/toolbox/include/SetReferences.h index ca3e1465f..c7c4c2ed8 100644 --- a/toolbox/include/SetReferences.h +++ b/toolbox/include/SetReferences.h @@ -1,36 +1,32 @@ #ifndef WBT_SETREFERENCES_H #define WBT_SETREFERENCES_H -#include "WBIBlock.h" -#include +#include "WBBlock.h" #include namespace wbt { class SetReferences; } -class wbt::SetReferences : public wbt::WBIBlock { - - double *m_references; - wbi::ControlMode m_controlMode; - bool m_fullControl; - std::vector m_controlledJoints; +class wbt::SetReferences : public wbt::WBBlock +{ +private: + std::vector m_controlModes; bool m_resetControlMode; + static void rad2deg(std::vector& v); public: - static std::string ClassName; - SetReferences(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + static const std::string ClassName; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initializeInitialConditions(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); + SetReferences(); + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool initializeInitialConditions(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; }; - #endif /* end of include guard: WBT_SETREFERENCES_H */ diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 397bd6020..3858a55a5 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -1,206 +1,274 @@ #include "SetReferences.h" -#include "Error.h" -#include "WBInterface.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" -#include -#include +#include "RobotInterface.h" +#include -#include +#define _USE_MATH_DEFINES +#include namespace wbt { - std::string SetReferences::ClassName = "SetReferences"; + const std::string SetReferences::ClassName = "SetReferences"; SetReferences::SetReferences() - : m_references(0) - , m_controlMode(wbi::CTRL_MODE_UNKNOWN) - , m_fullControl(true) - , m_resetControlMode(true) {} + : m_resetControlMode(true) + {} + + void rad2deg(std::vector& v) + { + const double rad2deg = 180.0 / (2 * M_PI); + for (auto& element : v) { + element *= rad2deg; + } + } unsigned SetReferences::numberOfParameters() { - // 1 - Control Type - // 2 - Full/Sublist type - // 3 - (only if sublist) controlled joints - return WBIBlock::numberOfParameters() + 3; + return WBBlock::numberOfParameters() + 1; } - bool SetReferences::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + // Memory allocation / Saving data not allowed here - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - // In Toolbox mask the options are the following: - // - 1 => full control - // - 2 => sublist control - m_fullControl = blockInfo->getScalarParameterAtIndex(WBIBlock::numberOfParameters() + 2).int32Data() == 1; + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - if (!m_fullControl) { - //sublist - std::string controlledList; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 3, controlledList)) { - if (error) error->message = "Could not read control type parameter"; - return false; - } - const yarp::os::Property * controlledListProperty = WBInterface::sharedInstance().currentConfiguration(); + // INPUTS + // ====== + // + // 1) Joint refereces (1xDoFs vector) + // - wbi::IDList idList; - WBInterface::wbdIDListFromConfigPropAndList(*controlledListProperty, - controlledList, idList); - dofs = idList.size(); - } - - // Specify I/O + // Number of inputs if (!blockInfo->setNumberOfInputPorts(1)) { - if (error) error->message = "Failed to configure the number of input ports"; + Log::getSingleton().error("Failed to configure the number of input ports."); return false; } + // Size and type bool success = blockInfo->setInputPortVectorSize(0, dofs); blockInfo->setInputPortType(0, PortDataTypeDouble); if (!success) { - if (error) error->message = "Failed to configure input ports"; + Log::getSingleton().error("Failed to configure input ports."); return false; } - // Output port: - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to configure the number of output ports"; + // OUTPUTS + // ======= + // + // No outputs + // + + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of output ports."); return false; } return true; } - bool SetReferences::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool SetReferences::initialize(BlockInformation* blockInfo) { - using namespace yarp::os; - if (!WBIBlock::initialize(blockInfo, error)) return false; + if (!WBBlock::initialize(blockInfo)) return false; // Reading the control type std::string controlType; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 1, controlType)) { - if (error) error->message = "Could not read control type parameter"; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, + controlType)) { + Log::getSingleton().error("Could not read control type parameter."); return false; } - m_controlMode = wbi::CTRL_MODE_UNKNOWN; + // Initialize the std::vectors + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_controlModes.assign(dofs, VOCAB_CM_UNKNOWN); + + // IControlMode.h if (controlType == "Position") { - m_controlMode = wbi::CTRL_MODE_POS; - } else if (controlType == "Position Direct") { - m_controlMode = wbi::CTRL_MODE_DIRECT_POSITION; - } else if (controlType == "Velocity") { - m_controlMode = wbi::CTRL_MODE_VEL; - } else if (controlType == "Torque") { - m_controlMode = wbi::CTRL_MODE_TORQUE; - } else if (controlType == "Open Loop") { - m_controlMode = wbi::CTRL_MODE_MOTOR_PWM; - } else { - if (error) error->message = "Control Mode not supported"; + m_controlModes.assign(dofs, VOCAB_CM_POSITION); + } + else if (controlType == "Position Direct") { + m_controlModes.assign(dofs, VOCAB_CM_POSITION_DIRECT); + } + else if (controlType == "Velocity") { + m_controlModes.assign(dofs, VOCAB_CM_VELOCITY); + } + else if (controlType == "Torque") { + m_controlModes.assign(dofs, VOCAB_CM_TORQUE); + } + else if (controlType == "PWM") { + m_controlModes.assign(dofs, VOCAB_CM_PWM); + } + else if (controlType == "Current") { + m_controlModes.assign(dofs, VOCAB_CM_CURRENT); + } + else { + Log::getSingleton().error("Control Mode not supported."); return false; } - //Read if full or sublist control - m_fullControl = blockInfo->getScalarParameterAtIndex(WBIBlock::numberOfParameters() + 2).booleanData(); - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - if (!m_fullControl) { - //sublist - std::string controlledList; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 3, controlledList)) { - if (error) error->message = "Could not read control type parameter"; - return false; - } - const yarp::os::Property * controlledListProperty = WBInterface::sharedInstance().currentConfiguration(); - - wbi::IDList idList; - bool result = WBInterface::wbdIDListFromConfigPropAndList(*controlledListProperty, - controlledList, idList); - wbi::IDList fullList = WBInterface::sharedInstance().interface()->getJointList(); - m_controlledJoints.clear(); - m_controlledJoints.reserve(idList.size()); - if (result) { - for (int i = 0; i < idList.size(); i++) { - int index; - if (fullList.idToIndex(idList.at(i), index)) - m_controlledJoints.push_back(index); - else - std::cerr << "Joint " << idList.at(i).toString() << " not found\n"; - } - } - dofs = idList.size(); + // Retain the ControlBoardRemapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); + return false; } - m_references = new double[dofs]; m_resetControlMode = true; - return m_references; + return true; } - bool SetReferences::terminate(BlockInformation *blockInfo, wbt::Error *error) + bool SetReferences::terminate(BlockInformation* blockInfo) { - wbi::wholeBodyInterface * const interface = WBInterface::sharedInstance().interface(); - if (interface) { - if (m_fullControl) { - interface->setControlMode(wbi::CTRL_MODE_POS); - } else { - for (int i = 0; i < m_controlledJoints.size(); i++) { - interface->setControlMode(wbi::CTRL_MODE_POS, 0, m_controlledJoints[i]); - } - } + using namespace yarp::dev; + bool ok = true; + + // Get the interface + std::weak_ptr icmd2; + ok = ok & getRobotInterface()->getInterface(icmd2); + if (!ok) { + Log::getSingleton().error("Failed to get the IControlMode2 interface."); } - if (m_references) { - delete [] m_references; - m_references = 0; + + // Set all the controlledJoints VOCAB_CM_POSITION + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_controlModes.assign(dofs, VOCAB_CM_POSITION); + + ok = ok & icmd2.lock()->setControlModes(m_controlModes.data()); + if (!ok) { + Log::getSingleton().error("Failed to set control mode."); + } + + // Release the RemoteControlBoardRemapper + ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); } - return WBIBlock::terminate(blockInfo, error); + + return ok && WBBlock::terminate(blockInfo); } - bool SetReferences::initializeInitialConditions(BlockInformation */*blockInfo*/, wbt::Error */*error*/) + bool SetReferences::initializeInitialConditions(BlockInformation* /*blockInfo*/) { - //Simply reset the variable m_resetControlMode - //It will be read at the first cycle of output + // Simply reset the variable m_resetControlMode. + // It will be read at the first cycle of output. m_resetControlMode = true; return true; } - bool SetReferences::output(BlockInformation *blockInfo, wbt::Error */*error*/) + bool SetReferences::output(BlockInformation* blockInfo) { - //get input - wbi::wholeBodyInterface * const interface = WBInterface::sharedInstance().interface(); - if (!interface) return false; - - Signal references = blockInfo->getInputPortSignal(0); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_references[i] = references.get(i).doubleData(); - } + using namespace yarp::dev; + // Set the control mode at the first run if (m_resetControlMode) { m_resetControlMode = false; + // Get the interface + std::weak_ptr icmd2; + if (!getRobotInterface()->getInterface(icmd2)) { + Log::getSingleton().error("Failed to get the IControlMode2 interface."); + return false; + } + // Set the control mode to all the controlledJoints + if (!icmd2.lock()->setControlModes(m_controlModes.data())) { + Log::getSingleton().error("Failed to set control mode."); + return false; + } + } + + // Get the signal + Signal references = blockInfo->getInputPortSignal(0); + unsigned signalWidth = blockInfo->getInputPortWidth(0); + std::vector referencesVector = references.getStdVector(signalWidth); - //now switch control mode - if (m_fullControl) { - interface->setControlMode(m_controlMode, m_references); - } else { - for (int i = 0; i < m_controlledJoints.size(); i++) { - interface->setControlMode(m_controlMode, &m_references[i], m_controlledJoints[i]); + bool ok = false; + // TODO: here only the first element is checked + switch (m_controlModes.front()) { + case VOCAB_CM_UNKNOWN: + Log::getSingleton().error("Control mode has not been successfully set."); + return false; + break; + case VOCAB_CM_POSITION: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get IPositionControl interface."); + return false; + } + // Convert from rad to deg + rad2deg(referencesVector); + // Set the references + ok = interface.lock()->positionMove(referencesVector.data()); + break; + } + case VOCAB_CM_POSITION_DIRECT: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get IPositionDirect interface."); + return false; + } + // Convert from rad to deg + rad2deg(referencesVector); + // Set the references + ok = interface.lock()->setPositions(referencesVector.data()); + break; + } + case VOCAB_CM_VELOCITY: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get IVelocityControl interface."); + return false; + } + // Convert from rad to deg + rad2deg(referencesVector); + // Set the references + ok = interface.lock()->velocityMove(referencesVector.data()); + break; + } + case VOCAB_CM_TORQUE: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get ITorqueControl interface."); + return false; } + ok = interface.lock()->setRefTorques(referencesVector.data()); + break; + } + case VOCAB_CM_PWM: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get IPWMControl interface."); + return false; + } + ok = interface.lock()->setRefDutyCycles(referencesVector.data()); + break; + } + case VOCAB_CM_CURRENT: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get ICurrentControl interface."); + return false; + } + ok = interface.lock()->setRefCurrents(referencesVector.data()); + break; } - return true; } - if (m_fullControl) { - interface->setControlReference(m_references); - } else { - for (int i = 0; i < m_controlledJoints.size(); i++) { - interface->setControlReference(&m_references[i], m_controlledJoints[i]); - } + if (!ok) { + Log::getSingleton().error("Failed to set references."); + return false; } + return true; } } From 520266f1097cdadf4f25ad21c5755a2ed30e23e7 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:39:05 +0000 Subject: [PATCH 27/89] SimulatorSynchronizer refactored --- toolbox/include/SimulatorSynchronizer.h | 26 +++++---- toolbox/src/SimulatorSynchronizer.cpp | 78 +++++++++++++++---------- 2 files changed, 61 insertions(+), 43 deletions(-) diff --git a/toolbox/include/SimulatorSynchronizer.h b/toolbox/include/SimulatorSynchronizer.h index 863fc6067..aa5c3eced 100644 --- a/toolbox/include/SimulatorSynchronizer.h +++ b/toolbox/include/SimulatorSynchronizer.h @@ -7,27 +7,31 @@ namespace wbt { class SimulatorSynchronizer; } -class wbt::SimulatorSynchronizer : public wbt::Block { +class wbt::SimulatorSynchronizer : public wbt::Block +{ public: - static std::string ClassName; + static const std::string ClassName; SimulatorSynchronizer(); - virtual ~SimulatorSynchronizer(); + ~SimulatorSynchronizer() override = default; - virtual unsigned numberOfParameters(); - virtual std::vector additionalBlockOptions(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); + unsigned numberOfParameters() override; + std::vector additionalBlockOptions() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; private: - double m_period; bool m_firstRun; struct RPCData; - RPCData *m_rpcData; + RPCData* m_rpcData; + + static const unsigned PARAM_PERIOD; // Period + static const unsigned PARAM_GZCLK_PORT; // Gazebo clock port + static const unsigned PARAM_RPC_PORT; // RPC client port name }; #endif /* end of include guard: WBT_SIMULATORSYNCHRONIZER_H */ diff --git a/toolbox/src/SimulatorSynchronizer.cpp b/toolbox/src/SimulatorSynchronizer.cpp index 6ed88c3e4..741a48ad6 100644 --- a/toolbox/src/SimulatorSynchronizer.cpp +++ b/toolbox/src/SimulatorSynchronizer.cpp @@ -1,17 +1,12 @@ #include "SimulatorSynchronizer.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" +#include "thrift/ClockServer.h" #include #include -#include #include -#define PARAM_IDX_1 1 // Period -#define PARAM_IDX_2 2 // Gazebo clock port -#define PARAM_IDX_3 3 // RPC client port name - - namespace wbt { struct SimulatorSynchronizer::RPCData @@ -26,16 +21,19 @@ namespace wbt { yarp::os::Port clientPort; gazebo::ClockServer clockServer; }; - - std::string SimulatorSynchronizer::ClassName = "SimulatorSynchronizer"; + + const std::string SimulatorSynchronizer::ClassName = "SimulatorSynchronizer"; + + const unsigned SimulatorSynchronizer::PARAM_PERIOD = 1; + const unsigned SimulatorSynchronizer::PARAM_GZCLK_PORT = 2; + const unsigned SimulatorSynchronizer::PARAM_RPC_PORT = 3; SimulatorSynchronizer::SimulatorSynchronizer() : m_period(0.01) , m_firstRun(true) - , m_rpcData(0) {} + , m_rpcData(0) + {} - SimulatorSynchronizer::~SimulatorSynchronizer() {} - unsigned SimulatorSynchronizer::numberOfParameters() { return 3; } std::vector SimulatorSynchronizer::additionalBlockOptions() @@ -43,43 +41,57 @@ namespace wbt { return std::vector(1, wbt::BlockOptionPrioritizeOrder); } - bool SimulatorSynchronizer::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool SimulatorSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) { - // Specify I/O // INPUTS + // ====== + // + // No inputs + // + if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to set input port number to 0"; + Log::getSingleton().error("Failed to set input port number to 0."); return false; } - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to set output port number"; + // OUTPUT + // ====== + // + // No outputs + // + + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to set output port number."); return false; } return true; } - bool SimulatorSynchronizer::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool SimulatorSynchronizer::initialize(BlockInformation* blockInfo) { - m_period = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).doubleData(); std::string serverPortName; std::string clientPortName; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_2, serverPortName) || !blockInfo->getStringParameterAtIndex(PARAM_IDX_3, clientPortName)) { - if (error) error->message = "Error reading RPC parameters"; + bool ok = true; + ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period); + ok = ok & blockInfo->getStringParameterAtIndex(PARAM_GZCLK_PORT, serverPortName); + ok = ok & blockInfo->getStringParameterAtIndex(PARAM_RPC_PORT, clientPortName); + + if (!ok) { + Log::getSingleton().error("Error reading RPC parameters."); return false; } yarp::os::Network::init(); if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork()) { - if (error) error->message = "Error initializing Yarp network"; + Log::getSingleton().error("Error initializing Yarp network."); return false; } m_rpcData = new struct RPCData(); if (!m_rpcData) { - if (error) error->message = "Error creating RPC data structure"; + Log::getSingleton().error("Error creating RPC data structure."); return false; } @@ -90,32 +102,34 @@ namespace wbt { return true; } - - bool SimulatorSynchronizer::terminate(BlockInformation */*S*/, wbt::Error *error) + + bool SimulatorSynchronizer::terminate(BlockInformation* /*S*/) { if (m_rpcData) { if (m_rpcData->clientPort.isOpen()) { m_rpcData->clockServer.continueSimulation(); - if (!yarp::os::Network::disconnect(m_rpcData->configuration.clientPortName, m_rpcData->configuration.serverPortName)) { - if (error) error->message = "Error disconnecting from simulator clock server"; + if (!yarp::os::Network::disconnect(m_rpcData->configuration.clientPortName, + m_rpcData->configuration.serverPortName)) { + Log::getSingleton().error("Error disconnecting from simulator clock server."); } m_rpcData->clientPort.close(); } delete m_rpcData; - m_rpcData = 0; + m_rpcData = nullptr; } yarp::os::Network::fini(); return true; } - bool SimulatorSynchronizer::output(BlockInformation */*S*/, wbt::Error *error) + bool SimulatorSynchronizer::output(BlockInformation* /*S*/) { if (m_firstRun) { m_firstRun = false; - if (!m_rpcData->clientPort.open(m_rpcData->configuration.clientPortName) - || !yarp::os::Network::connect(m_rpcData->configuration.clientPortName, m_rpcData->configuration.serverPortName)) { - if (error) error->message = "Error connecting to simulator clock server"; + if (!m_rpcData->clientPort.open(m_rpcData->configuration.clientPortName) || + !yarp::os::Network::connect(m_rpcData->configuration.clientPortName, + m_rpcData->configuration.serverPortName)) { + Log::getSingleton().error("Error connecting to simulator clock server."); return false; } From d23242cbfa8c7c63aa357186984d48a3d7c71731 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:39:44 +0000 Subject: [PATCH 28/89] YarpClock refactored --- toolbox/include/YarpClock.h | 16 ++++++++-------- toolbox/src/YarpClock.cpp | 36 +++++++++++++++++++++++------------- 2 files changed, 31 insertions(+), 21 deletions(-) diff --git a/toolbox/include/YarpClock.h b/toolbox/include/YarpClock.h index 9d0e551e5..000229579 100644 --- a/toolbox/include/YarpClock.h +++ b/toolbox/include/YarpClock.h @@ -7,16 +7,16 @@ namespace wbt { class YarpClock; } -class wbt::YarpClock : public wbt::Block { +class wbt::YarpClock : public wbt::Block +{ public: - static std::string ClassName; - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); + static const std::string ClassName; + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; }; #endif /* end of include guard: WBT_YARPCLOCK_H */ diff --git a/toolbox/src/YarpClock.cpp b/toolbox/src/YarpClock.cpp index f20ba9b5b..b504287df 100644 --- a/toolbox/src/YarpClock.cpp +++ b/toolbox/src/YarpClock.cpp @@ -1,6 +1,6 @@ #include "YarpClock.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" @@ -9,22 +9,31 @@ namespace wbt { - std::string YarpClock::ClassName = "YarpClock"; + const std::string YarpClock::ClassName = "YarpClock"; unsigned YarpClock::numberOfParameters() { return 0; } - bool YarpClock::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool YarpClock::configureSizeAndPorts(BlockInformation* blockInfo) { - // Specify I/O // INPUTS + // ====== + // + // No inputs + // + if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to set input port number to 0"; + Log::getSingleton().error("Failed to set input port number to 0."); return false; } - // OUTPUTS - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to set output port number"; + // OUTPUT + // ====== + // + // 1) The yarp time. In short, it streams yarp::os::Time::now(). + // + + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to set output port number."); return false; } @@ -34,24 +43,25 @@ namespace wbt { return true; } - bool YarpClock::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool YarpClock::initialize(BlockInformation* blockInfo) { yarp::os::Network::init(); - if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)){ - if (error) error->message = "YARP server wasn't found active!! \n"; + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); return false; } return true; } - bool YarpClock::terminate(BlockInformation */*S*/, wbt::Error */*error*/) + + bool YarpClock::terminate(BlockInformation* /*S*/) { yarp::os::Network::fini(); return true; } - bool YarpClock::output(BlockInformation *blockInfo, wbt::Error */*error*/) + bool YarpClock::output(BlockInformation* blockInfo) { Signal signal = blockInfo->getOutputPortSignal(0); signal.set(0, yarp::os::Time::now()); From 525a62002670342302f73103d234d5e904551e3b Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:41:26 +0000 Subject: [PATCH 29/89] YarpRead refactored --- toolbox/include/YarpRead.h | 32 +++++---- toolbox/src/YarpRead.cpp | 133 ++++++++++++++++++++++++------------- 2 files changed, 108 insertions(+), 57 deletions(-) diff --git a/toolbox/include/YarpRead.h b/toolbox/include/YarpRead.h index 14edd2564..bc79bcb61 100644 --- a/toolbox/include/YarpRead.h +++ b/toolbox/include/YarpRead.h @@ -17,25 +17,33 @@ namespace yarp { } } -class wbt::YarpRead : public wbt::Block { +class wbt::YarpRead : public wbt::Block +{ public: - static std::string ClassName; - + static const std::string ClassName; + YarpRead(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; + private: bool m_autoconnect; bool m_blocking; bool m_shouldReadTimestamp; bool m_errorOnMissingPort; - - yarp::os::BufferedPort *m_port; + + yarp::os::BufferedPort* m_port; + + static const unsigned PARAM_IDX_PORTNAME; // port name + static const unsigned PARAM_IDX_PORTSIZE; // Size of the port you're reading + static const unsigned PARAM_IDX_WAITDATA; // boolean for blocking reading + static const unsigned PARAM_IDX_READ_TS; // boolean to stream timestamp + static const unsigned PARAM_IDX_AUTOCONNECT; // Autoconnect boolean + static const unsigned PARAM_IDX_ERR_NO_PORT; // Error on missing port if autoconnect is on boolean }; #endif /* end of include guard: WBT_YARPREAD_H */ diff --git a/toolbox/src/YarpRead.cpp b/toolbox/src/YarpRead.cpp index 82fa279f4..5542b9a42 100644 --- a/toolbox/src/YarpRead.cpp +++ b/toolbox/src/YarpRead.cpp @@ -1,6 +1,6 @@ #include "YarpRead.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" @@ -12,55 +12,79 @@ #include #include -#define PARAM_IDX_1 1 // port name -#define PARAM_IDX_2 2 // Size of the port you're reading -#define PARAM_IDX_3 3 // boolean for blocking reading -#define PARAM_IDX_4 4 // boolean to stream timestamp -#define PARAM_IDX_5 5 // Autoconnect boolean -#define PARAM_IDX_6 6 // Error on missing port if autoconnect is on boolean - namespace wbt { - - std::string YarpRead::ClassName = "YarpRead"; + + const std::string YarpRead::ClassName = "YarpRead"; + + const unsigned YarpRead::PARAM_IDX_PORTNAME = 1; // port name + const unsigned YarpRead::PARAM_IDX_PORTSIZE = 2; // Size of the port you're reading + const unsigned YarpRead::PARAM_IDX_WAITDATA = 3; // boolean for blocking reading + const unsigned YarpRead::PARAM_IDX_READ_TS = 4; // boolean to stream timestamp + const unsigned YarpRead::PARAM_IDX_AUTOCONNECT = 5; // Autoconnect boolean + const unsigned YarpRead::PARAM_IDX_ERR_NO_PORT = 6; // Error on missing port if autoconnect is on boolean YarpRead::YarpRead() : m_autoconnect(false) , m_blocking(false) , m_shouldReadTimestamp(false) , m_errorOnMissingPort(true) - , m_port(0) {} + , m_port(0) + {} unsigned YarpRead::numberOfParameters() { return 6; } - bool YarpRead::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool YarpRead::configureSizeAndPorts(BlockInformation* blockInfo) { - // Specify I/O // INPUTS + // ====== + // + // No inputs + // + if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to set input port number to 0"; + Log::getSingleton().error("Failed to set input port number to 0."); return false; } - + // OUTPUTS - bool shouldReadTimestamp = blockInfo->getScalarParameterAtIndex(PARAM_IDX_4).booleanData(); - int signalSize = blockInfo->getScalarParameterAtIndex(PARAM_IDX_2).int32Data(); - bool autoconnect = blockInfo->getScalarParameterAtIndex(PARAM_IDX_5).booleanData(); + // ======= + // + // 1) Vector with the data read from the port (1 x signalSize) + // 2) Optional: Timestamp read from the port + // 3) Optional: If autoconnect is disabled, this output becomes 1 when data starts arriving + // (and hence it means that the user connected manually the port) + // + + bool shouldReadTimestamp; + bool autoconnect; + double signalSize; + + bool ok = false; + + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, shouldReadTimestamp); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, autoconnect); + ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_PORTSIZE, signalSize); + + if (!ok) { + Log::getSingleton().error("Failed to read input parameters."); + return false; + } if (signalSize < 0) { - if (error) error->message = "Signal size must be non negative"; + Log::getSingleton().error("Signal size must be non negative."); return false; } int numberOfOutputPorts = 1; - if (shouldReadTimestamp) numberOfOutputPorts++; //timestamp is the second port - if (!autoconnect) numberOfOutputPorts++; //!autoconnect => additional port with 1/0 depending on the connection status + numberOfOutputPorts += static_cast(shouldReadTimestamp); //timestamp is the second port + numberOfOutputPorts += static_cast(!autoconnect); //!autoconnect => additional port with 1/0 depending on the connection status - if (!blockInfo->setNumberOfOuputPorts(numberOfOutputPorts)) { - if (error) error->message = "Failed to set output port number"; + if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { + Log::getSingleton().error("Failed to set output port number."); return false; } - blockInfo->setOutputPortVectorSize(0, signalSize); + blockInfo->setOutputPortVectorSize(0, static_cast(signalSize)); blockInfo->setOutputPortType(0, PortDataTypeDouble); int portIndex = 1; @@ -77,76 +101,92 @@ namespace wbt { return true; } - bool YarpRead::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool YarpRead::initialize(BlockInformation* blockInfo) { using namespace yarp::os; using namespace yarp::sig; Network::init(); - if (!Network::initialized() || !Network::checkNetwork(5.0)){ - if (error) error->message = "YARP server wasn't found active!! \n"; + if (!Network::initialized() || !Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); return false; } - m_shouldReadTimestamp = blockInfo->getScalarParameterAtIndex(PARAM_IDX_4).booleanData(); - m_autoconnect = blockInfo->getScalarParameterAtIndex(PARAM_IDX_5).booleanData(); - m_blocking = blockInfo->getScalarParameterAtIndex(PARAM_IDX_3).booleanData(); - m_errorOnMissingPort = blockInfo->getScalarParameterAtIndex(PARAM_IDX_6).booleanData(); + bool ok = true; + + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, m_shouldReadTimestamp); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_WAITDATA, m_blocking); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, m_errorOnMissingPort); + + if (!ok) { + Log::getSingleton().error("Failed to read input parameters."); + return false; + } std::string portName; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_1, portName)) { - if (error) error->message = "Cannot retrieve string from port name parameter"; + if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_PORTNAME, portName)) { + Log::getSingleton().error("Cannot retrieve string from port name parameter."); return false; } std::string sourcePortName; std::string destinationPortName; + // Autoconnect: the block opens a temporary input port ..., and it connects to an existing + // port portName (which is streaming data). if (m_autoconnect) { sourcePortName = portName; destinationPortName = "..."; - } else { + } + // Manual connection: the block opens an input port portName, and waits a manual connection to an + // output port. + else { destinationPortName = portName; } m_port = new BufferedPort(); if (!m_port || !m_port->open(destinationPortName)) { - if (error) error->message = "Error while opening yarp port"; + Log::getSingleton().error("Error while opening yarp port."); return false; } if (m_autoconnect) { if (!Network::connect(sourcePortName, m_port->getName())) { - std::cerr << "Failed to connect " << sourcePortName << " to " << m_port->getName() << "\n"; + Log::getSingleton().warning("Failed to connect " + + sourcePortName + + " to " + + m_port->getName()); if (m_errorOnMissingPort) { - if (error) error->message = "ERROR connecting ports"; + Log::getSingleton().error("Failed connecting ports."); return false; } } } return true; } - bool YarpRead::terminate(BlockInformation */*S*/, wbt::Error */*error*/) + + bool YarpRead::terminate(BlockInformation* /*blockInfo*/) { if (m_port) { m_port->close(); delete m_port; - m_port = 0; + m_port = nullptr; } yarp::os::Network::fini(); return true; } - - bool YarpRead::output(BlockInformation *blockInfo, wbt::Error */*error*/) + + bool YarpRead::output(BlockInformation* blockInfo) { int timeStampPortIndex = 1; int connectionStatusPortIndex = 1; - yarp::sig::Vector *v = m_port->read(m_blocking); // Read from the port. Waits until data arrives. - if (v) - { + yarp::sig::Vector* v = m_port->read(m_blocking); // Read from the port. Waits until data arrives. + + if (v) { if (m_shouldReadTimestamp) { connectionStatusPortIndex++; yarp::os::Stamp timestamp; @@ -155,9 +195,11 @@ namespace wbt { Signal pY1 = blockInfo->getOutputPortSignal(timeStampPortIndex); pY1.set(0, timestamp.getCount()); pY1.set(1, timestamp.getTime()); - } + Signal signal = blockInfo->getOutputPortSignal(0); + + // Crop the buffer if it exceeds the OutputPortWidth. signal.setBuffer(v->data(), std::min(blockInfo->getOutputPortWidth(0), (unsigned)v->size())); if (!m_autoconnect) { @@ -170,6 +212,7 @@ namespace wbt { //we set the port to zero again } } + return true; } } From 3ff628603e27845fd832bb04af6d1ec379c7b90e Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:43:00 +0000 Subject: [PATCH 30/89] YarpWrite refactored --- toolbox/include/YarpWrite.h | 27 ++++++----- toolbox/src/YarpWrite.cpp | 93 +++++++++++++++++++++++++------------ 2 files changed, 79 insertions(+), 41 deletions(-) diff --git a/toolbox/include/YarpWrite.h b/toolbox/include/YarpWrite.h index b7ea198ce..807b774cd 100644 --- a/toolbox/include/YarpWrite.h +++ b/toolbox/include/YarpWrite.h @@ -17,24 +17,29 @@ namespace yarp { } } -class wbt::YarpWrite : public wbt::Block { +class wbt::YarpWrite : public wbt::Block +{ public: - static std::string ClassName; - + static const std::string ClassName; + YarpWrite(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; + private: bool m_autoconnect; bool m_errorOnMissingPort; std::string m_destinationPortName; - yarp::os::BufferedPort *m_port; + yarp::os::BufferedPort* m_port; + + static const unsigned PARAM_IDX_PORTNAME; // Port name + static const unsigned PARAM_IDX_AUTOCONNECT; // Autoconnect boolean + static const unsigned PARAM_IDX_ERR_NO_PORT; // Error on missing port if autoconnect is true }; #endif /* end of include guard: WBT_YARPWRITE_H */ diff --git a/toolbox/src/YarpWrite.cpp b/toolbox/src/YarpWrite.cpp index a6f920c04..f557eccbb 100644 --- a/toolbox/src/YarpWrite.cpp +++ b/toolbox/src/YarpWrite.cpp @@ -1,6 +1,6 @@ #include "YarpWrite.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" @@ -9,39 +9,53 @@ #include #include -#define PARAM_IDX_1 1 // port name -#define PARAM_IDX_2 2 // Autoconnect boolean -#define PARAM_IDX_3 3 // Error on missing port if autoconnect is on boolean - namespace wbt { - - std::string YarpWrite::ClassName = "YarpWrite"; + + const std::string YarpWrite::ClassName = "YarpWrite"; + + const unsigned YarpWrite::PARAM_IDX_PORTNAME = 1; // Port name + const unsigned YarpWrite::PARAM_IDX_AUTOCONNECT = 2; // Autoconnect boolean + const unsigned YarpWrite::PARAM_IDX_ERR_NO_PORT = 3; // Error on missing port if autoconnect is true YarpWrite::YarpWrite() : m_autoconnect(false) , m_errorOnMissingPort(true) , m_destinationPortName("") - , m_port(0) {} - + , m_port(nullptr) + {} + unsigned YarpWrite::numberOfParameters() { return 3; } - bool YarpWrite::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) + bool YarpWrite::configureSizeAndPorts(BlockInformation* blockInfo) { + // INPUT + // ===== + // + // 1) The signal to stream to the specified yarp port + // + if (!blockInfo->setNumberOfInputPorts(1)) { - if (error) error->message = "Failed to set input port number to 0"; + Log::getSingleton().error("Failed to set input port number to 0."); return false; } blockInfo->setInputPortVectorSize(0, -1); blockInfo->setInputPortType(0, PortDataTypeDouble); - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to set output port number"; + // OUTPUT + // ====== + // + // No outputs + // + + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to set output port number."); return false; } + return true; } - bool YarpWrite::initialize(BlockInformation *blockInfo, wbt::Error *error) + bool YarpWrite::initialize(BlockInformation* blockInfo) { using namespace yarp::os; using namespace yarp::sig; @@ -49,70 +63,89 @@ namespace wbt { Network::init(); if (!Network::initialized() || !Network::checkNetwork(5.0)){ - if (error) error->message = "YARP server wasn't found active!! \n"; + Log::getSingleton().error("YARP server wasn't found active!!"); return false; } - m_autoconnect = blockInfo->getScalarParameterAtIndex(PARAM_IDX_2).booleanData(); - m_errorOnMissingPort = blockInfo->getScalarParameterAtIndex(PARAM_IDX_3).booleanData(); + bool ok = true; + + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, + m_errorOnMissingPort); + + if (!ok) { + Log::getSingleton().error("Failed to read input parameters."); + return false; + } std::string portParameter; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_1, portParameter)) { - if (error) error->message = "Error reading port name parameter"; + if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_PORTNAME, portParameter)) { + Log::getSingleton().error("Error reading port name parameter."); return false; } std::string sourcePortName; + // Autoconnect: the block opens a temporary output port ..., and it connects to an existing + // port portName (which will receive data). if (m_autoconnect) { sourcePortName = "..."; m_destinationPortName = portParameter; - } else { + } + // Manual connection: the block opens an output port portName, and waits a manual connection to an + // input port. + else { sourcePortName = portParameter; } m_port = new BufferedPort(); if (!m_port || !m_port->open(sourcePortName)) { - if (error) error->message = "Error while opening yarp port"; + Log::getSingleton().error("Error while opening yarp port."); return false; } if (m_autoconnect) { if (!Network::connect(m_port->getName(), m_destinationPortName)) { + Log::getSingleton().warning("Failed to connect " + + m_port->getName() + + " to " + + m_destinationPortName); if (m_errorOnMissingPort) { - if (error) error->message ="Failed to connect " + m_port->getName() + " to " + m_destinationPortName; + Log::getSingleton().error("Failed connecting ports."); return false; } } } - //prepare the first object allocation - yarp::sig::Vector &outputVector = m_port->prepare(); + // Initialize the size of the internal buffer handled by m_port + yarp::sig::Vector& outputVector = m_port->prepare(); outputVector.resize(blockInfo->getInputPortWidth(0)); return true; } - bool YarpWrite::terminate(BlockInformation */*S*/, wbt::Error */*error*/) + bool YarpWrite::terminate(BlockInformation* /*S*/) { if (m_port) { - if (m_autoconnect) + if (m_autoconnect) { yarp::os::Network::disconnect(m_port->getName(), m_destinationPortName); + } m_port->close(); delete m_port; - m_port = 0; + m_port = nullptr; } yarp::os::Network::fini(); return true; } - - bool YarpWrite::output(BlockInformation *blockInfo, wbt::Error */*error*/) + + bool YarpWrite::output(BlockInformation* blockInfo) { if (!m_port) return false; - yarp::sig::Vector &outputVector = m_port->prepare(); + yarp::sig::Vector& outputVector = m_port->prepare(); outputVector.resize(blockInfo->getInputPortWidth(0)); //this should be a no-op Signal signal = blockInfo->getInputPortSignal(0); + for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { outputVector[i] = signal.get(i).doubleData(); } From 0f36c3399635085221f34c6a7dbc0dd64a87aea4 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:47:46 +0000 Subject: [PATCH 31/89] GetEstimate refactored and renamed to GetMeasurement --- toolbox/CMakeLists.txt | 4 +- toolbox/include/GetEstimate.h | 31 ------ toolbox/include/GetMeasurement.h | 38 +++++++ toolbox/include/base/toolbox.h | 2 +- toolbox/src/GetEstimate.cpp | 106 ------------------ toolbox/src/GetMeasurement.cpp | 183 +++++++++++++++++++++++++++++++ toolbox/src/base/factory.cpp | 4 +- 7 files changed, 226 insertions(+), 142 deletions(-) delete mode 100644 toolbox/include/GetEstimate.h create mode 100644 toolbox/include/GetMeasurement.h delete mode 100644 toolbox/src/GetEstimate.cpp create mode 100644 toolbox/src/GetMeasurement.cpp diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index d9fcf56eb..730222497 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -200,8 +200,8 @@ configure_block(BLOCK_NAME "Set Low-Level PIDs" configure_block(BLOCK_NAME "Get Estimate" GROUP "State" LIST_PREFIX WBT - SOURCES src/GetEstimate.cpp - HEADERS include/GetEstimate.h) + SOURCES src/GetMeasurement.cpp + HEADERS include/GetMeasurement.h) configure_block(BLOCK_NAME "Get Limits" GROUP "State" diff --git a/toolbox/include/GetEstimate.h b/toolbox/include/GetEstimate.h deleted file mode 100644 index a376d1484..000000000 --- a/toolbox/include/GetEstimate.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef WBT_GETESTIMATE_H -#define WBT_GETESTIMATE_H - -#include "WBIBlock.h" -#include - -namespace wbt { - class GetEstimate; -} - -class wbt::GetEstimate : public wbt::WBIBlock { - - double *m_estimate; - wbi::EstimateType m_estimateType; - -public: - static std::string ClassName; - GetEstimate(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - - -}; - - -#endif /* end of include guard: WBT_GETESTIMATE_H */ diff --git a/toolbox/include/GetMeasurement.h b/toolbox/include/GetMeasurement.h new file mode 100644 index 000000000..8673874f7 --- /dev/null +++ b/toolbox/include/GetMeasurement.h @@ -0,0 +1,38 @@ +#ifndef WBT_GETMEASUREMENT_H +#define WBT_GETMEASUREMENT_H + +#include "WBBlock.h" + +namespace wbt { + class GetMeasurement; + enum EstimateType { + ESTIMATE_JOINT_POS, + ESTIMATE_JOINT_VEL, + ESTIMATE_JOINT_ACC, + ESTIMATE_JOINT_TORQUE + }; +} + + +class wbt::GetMeasurement : public wbt::WBBlock +{ +private: + std::vector m_estimate; + wbt::EstimateType m_estimateType; + static void deg2rad(std::vector& v); + +public: + static const std::string ClassName; + + GetMeasurement() = default; + ~GetMeasurement() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; +}; + +#endif /* end of include guard: WBT_GETMEASUREMENT_H */ diff --git a/toolbox/include/base/toolbox.h b/toolbox/include/base/toolbox.h index 71a034c28..fe4ea5680 100644 --- a/toolbox/include/base/toolbox.h +++ b/toolbox/include/base/toolbox.h @@ -11,7 +11,7 @@ #include "RealTimeSynchronizer.h" #include "SimulatorSynchronizer.h" #include "Jacobian.h" -#include "GetEstimate.h" +#include "GetMeasurement.h" #include "InverseDynamics.h" #include "DotJNu.h" #include "GetLimits.h" diff --git a/toolbox/src/GetEstimate.cpp b/toolbox/src/GetEstimate.cpp deleted file mode 100644 index ec2ed17c9..000000000 --- a/toolbox/src/GetEstimate.cpp +++ /dev/null @@ -1,106 +0,0 @@ -#include "GetEstimate.h" - -#include "Error.h" -#include "WBInterface.h" -#include "BlockInformation.h" -#include "Signal.h" -#include -#include - -namespace wbt { - - std::string GetEstimate::ClassName = "GetEstimate"; - - GetEstimate::GetEstimate() - : m_estimate(0) - , m_estimateType(wbi::ESTIMATE_JOINT_POS) {} - - unsigned GetEstimate::numberOfParameters() - { - return WBIBlock::numberOfParameters() + 1; - } - - bool GetEstimate::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - - if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - - // Output port: - // - DoFs vector with the information asked - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } - - bool success = blockInfo->setOutputPortVectorSize(0, dofs); - blockInfo->setOutputPortType(0, PortDataTypeDouble); - if (!success) { - if (error) error->message = "Failed to configure output ports"; - return false; - } - - - return true; - } - - bool GetEstimate::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIBlock::initialize(blockInfo, error)) return false; - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_estimate = new double[dofs]; - - // Reading the control type - std::string informationType; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 1, informationType)) { - if (error) error->message = "Could not read estimate type parameter"; - return false; - } - - if (informationType == "Joints Position") { - m_estimateType = wbi::ESTIMATE_JOINT_POS; - } else if (informationType == "Joints Velocity") { - m_estimateType = wbi::ESTIMATE_JOINT_VEL; - } else if (informationType == "Joints Acceleration") { - m_estimateType = wbi::ESTIMATE_JOINT_ACC; - } else if (informationType == "Joints Torque") { - m_estimateType = wbi::ESTIMATE_JOINT_TORQUE; - } else { - if (error) error->message = "Estimate not supported"; - return false; - } - return m_estimate; - } - - bool GetEstimate::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_estimate) { - delete [] m_estimate; - m_estimate = 0; - } - return WBIBlock::terminate(blockInfo, error); - } - - bool GetEstimate::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - //get input - wbi::wholeBodyInterface * const interface = WBInterface::sharedInstance().interface(); - if (interface) { - interface->getEstimates(m_estimateType, m_estimate); - Signal signal = blockInfo->getOutputPortSignal(0); - signal.setBuffer(m_estimate, blockInfo->getOutputPortWidth(0)); - - return true; - } - return false; - } -} diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp new file mode 100644 index 000000000..c0e0f01f6 --- /dev/null +++ b/toolbox/src/GetMeasurement.cpp @@ -0,0 +1,183 @@ +#include "GetMeasurement.h" + +#include "Log.h" +#include "BlockInformation.h" +#include "Signal.h" +#include "RobotInterface.h" +#include +#include + +#define _USE_MATH_DEFINES +#include + +namespace wbt { + + const std::string GetMeasurement::ClassName = "GetMeasurement"; + + void deg2rad(std::vector& v) + { + const double deg2rad = (2 * M_PI) / 180.0; + for (auto& element : v) { + element *= deg2rad; + } + } + + unsigned GetMeasurement::numberOfParameters() + { + return WBBlock::numberOfParameters() + 1; + } + + bool GetMeasurement::configureSizeAndPorts(BlockInformation* blockInfo) + { + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // No inputs + // + + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } + + // OUTPUTS + // ======= + // + // 1) Vector with the information asked (1xDoFs) + // + + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + bool success = blockInfo->setOutputPortVectorSize(0, dofs); + blockInfo->setOutputPortType(0, PortDataTypeDouble); + if (!success) { + Log::getSingleton().error("Failed to configure output ports."); + return false; + } + + return true; + } + + bool GetMeasurement::initialize(BlockInformation* blockInfo) + { + if (!WBBlock::initialize(blockInfo)) return false; + + // Reading the control type + std::string informationType; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, informationType)) { + Log::getSingleton().error("Could not read estimate type parameter."); + return false; + } + + if (informationType == "Joints Position") { + m_estimateType = wbt::ESTIMATE_JOINT_POS; + } else if (informationType == "Joints Velocity") { + m_estimateType = wbt::ESTIMATE_JOINT_VEL; + } else if (informationType == "Joints Acceleration") { + m_estimateType = wbt::ESTIMATE_JOINT_ACC; + } else if (informationType == "Joints Torque") { + m_estimateType = wbt::ESTIMATE_JOINT_TORQUE; + } else { + Log::getSingleton().error("Estimate not supported."); + return false; + } + + // Initialize the size of the output vector + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_estimate.reserve(dofs); + + // Retain the ControlBoardRemapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); + return false; + } + + return true; + } + + bool GetMeasurement::terminate(BlockInformation* blockInfo) + { + // Release the RemoteControlBoardRemapper + bool ok = true; + ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + } + + return ok && WBBlock::terminate(blockInfo); + } + + bool GetMeasurement::output(BlockInformation* blockInfo) + { + bool ok; + switch (m_estimateType) { + case ESTIMATE_JOINT_POS: { + // Get the interface + std::weak_ptr iEncoders; + if (!getRobotInterface()->getInterface(iEncoders)) { + Log::getSingleton().error("Failed to get IPidControl interface."); + return false; + } + // Get the measurement + ok = iEncoders.lock()->getEncoders(m_estimate.data()); + deg2rad(m_estimate); + break; + } + case ESTIMATE_JOINT_VEL: { + // Get the interface + std::weak_ptr iEncoders; + if (!getRobotInterface()->getInterface(iEncoders)) { + Log::getSingleton().error("Failed to get IEncoders interface."); + return false; + } + // Get the measurement + ok = iEncoders.lock()->getEncoderSpeeds(m_estimate.data()); + deg2rad(m_estimate); + break; + } + case ESTIMATE_JOINT_ACC: { + // Get the interface + std::weak_ptr iEncoders; + if (!getRobotInterface()->getInterface(iEncoders)) { + Log::getSingleton().error("Failed to get IEncoders interface."); + return false; + } + // Get the measurement + ok = iEncoders.lock()->getEncoderAccelerations(m_estimate.data()); + deg2rad(m_estimate); + break; + } + case ESTIMATE_JOINT_TORQUE: { + // Get the interface + std::weak_ptr iTorqueControl; + if (!getRobotInterface()->getInterface(iTorqueControl)) { + Log::getSingleton().error("Failed to get ITorqueControl interface."); + return false; + } + // Get the measurement + ok = iTorqueControl.lock()->getTorques(m_estimate.data()); + break; + } + default: + Log::getSingleton().error("Estimate type not recognized."); + return false; + } + + if (!ok) { + Log::getSingleton().error("Failed to get estimate."); + return false; + } + + Signal signal = blockInfo->getOutputPortSignal(0); + signal.setBuffer(m_estimate.data(), blockInfo->getOutputPortWidth(0)); + + return true; + } +} diff --git a/toolbox/src/base/factory.cpp b/toolbox/src/base/factory.cpp index dcbe75d60..0320676bc 100644 --- a/toolbox/src/base/factory.cpp +++ b/toolbox/src/base/factory.cpp @@ -24,8 +24,8 @@ wbt::Block* wbt::Block::instantiateBlockWithClassName(std::string blockClassName block = new wbt::SimulatorSynchronizer(); } else if (blockClassName == wbt::Jacobian::ClassName) { block = new wbt::Jacobian(); - } else if (blockClassName == wbt::GetEstimate::ClassName) { - block = new wbt::GetEstimate(); + } else if (blockClassName == wbt::GetMeasurement::ClassName) { + block = new wbt::GetMeasurement(); } else if (blockClassName == wbt::InverseDynamics::ClassName) { block = new wbt::InverseDynamics(); } else if (blockClassName == wbt::DotJNu::ClassName) { From 028dbd603947979aded515f44cfa3bed1b41903c Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:55:03 +0000 Subject: [PATCH 32/89] YARPWBIConverter refactored and renamed to ModelPartitioner --- toolbox/CMakeLists.txt | 6 +- toolbox/include/ModelPartitioner.h | 33 ++++ toolbox/include/YARPWBIConverter.h | 29 ---- toolbox/include/base/toolbox.h | 2 +- toolbox/src/ModelPartitioner.cpp | 197 +++++++++++++++++++++ toolbox/src/YARPWBIConverter.cpp | 268 ----------------------------- toolbox/src/base/factory.cpp | 4 +- 7 files changed, 236 insertions(+), 303 deletions(-) create mode 100644 toolbox/include/ModelPartitioner.h delete mode 100644 toolbox/include/YARPWBIConverter.h create mode 100644 toolbox/src/ModelPartitioner.cpp delete mode 100644 toolbox/src/YARPWBIConverter.cpp diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 730222497..99625ae0d 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -131,11 +131,11 @@ configure_block(BLOCK_NAME "Real Time Synchronizer" SOURCES src/RealTimeSynchronizer.cpp HEADERS include/RealTimeSynchronizer.h) -configure_block(BLOCK_NAME "YARP - WBI Converter" +configure_block(BLOCK_NAME "Model Partitioner" GROUP "Utilities" LIST_PREFIX WBT - SOURCES src/YARPWBIConverter.cpp - HEADERS include/YARPWBIConverter.h) + SOURCES src/ModelPartitioner.cpp + HEADERS include/ModelPartitioner.h) configure_block(BLOCK_NAME "Yarp Clock" GROUP "Utilities" diff --git a/toolbox/include/ModelPartitioner.h b/toolbox/include/ModelPartitioner.h new file mode 100644 index 000000000..929da6c7e --- /dev/null +++ b/toolbox/include/ModelPartitioner.h @@ -0,0 +1,33 @@ +#ifndef WBT_MODELPARTITIONER_H +#define WBT_MODELPARTITIONER_H + +#include "WBBlock.h" +#include "RobotInterface.h" +#include +#include + +namespace wbt { + class ModelPartitioner; +} + +class wbt::ModelPartitioner : public wbt::WBBlock +{ +private: + bool m_yarp2WBI; + + std::shared_ptr m_jointsMapString; + +public: + static const std::string ClassName; + ModelPartitioner() = default; + ~ModelPartitioner() override = default; + + unsigned numberOfParameters() override; + + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; + bool terminate(BlockInformation* blockInfo) override; + bool output(BlockInformation* blockInfo) override; +}; + +#endif /* end of include guard: WBT_MODELPARTITIONER_H */ diff --git a/toolbox/include/YARPWBIConverter.h b/toolbox/include/YARPWBIConverter.h deleted file mode 100644 index 37f334057..000000000 --- a/toolbox/include/YARPWBIConverter.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef WBT_YARP2DOFS_H -#define WBT_YARP2DOFS_H - -#include "WBIModelBlock.h" - -namespace wbt { - class YARPWBIConverter; -} - -class wbt::YARPWBIConverter : public wbt::WBIModelBlock { - - struct YARPWBIConverterPimpl; - YARPWBIConverterPimpl *m_pimpl; - -public: - static std::string ClassName; - YARPWBIConverter(); - - virtual unsigned numberOfParameters(); - - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - -}; - - -#endif /* end of include guard: WBT_YARP2DOFS_H */ diff --git a/toolbox/include/base/toolbox.h b/toolbox/include/base/toolbox.h index fe4ea5680..cc0750e51 100644 --- a/toolbox/include/base/toolbox.h +++ b/toolbox/include/base/toolbox.h @@ -2,7 +2,7 @@ //General Yarp utilities #include "YarpRead.h" #include "YarpWrite.h" -#include "YARPWBIConverter.h" +#include "ModelPartitioner.h" #include "YarpClock.h" //WBI-related stuff #include "MassMatrix.h" diff --git a/toolbox/src/ModelPartitioner.cpp b/toolbox/src/ModelPartitioner.cpp new file mode 100644 index 000000000..bedee09c9 --- /dev/null +++ b/toolbox/src/ModelPartitioner.cpp @@ -0,0 +1,197 @@ +#include "ModelPartitioner.h" + +#include "BlockInformation.h" +#include "Configuration.h" +#include "RobotInterface.h" +#include "Signal.h" +#include "Log.h" + +namespace wbt { + + const std::string ModelPartitioner::ClassName = "ModelPartitioner"; + + unsigned ModelPartitioner::numberOfParameters() + { + return 1; + } + + bool ModelPartitioner::configureSizeAndPorts(BlockInformation* blockInfo) + { + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // PARAMETERS + // ========== + // + // 1) Boolean specifying if yarp2WBI (true) or WBI2yarp (false) + // + + bool yarp2WBI; + if (!blockInfo->getBooleanParameterAtIndex(1, yarp2WBI)) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } + + // TODO: update to new logic + // INPUTS + // ====== + // + // yarp2WBI + // -------- + // + // 1) Map of joints / control boards (1 x nJoints) + // 2) Vector containing the full set of robot's joints (1 x nJoints) + // + // WBI2yarp + // -------- + // + // 1) Map of joints / control boards (1 x nJoints) + // 2) Torso Control Board Signals + // 3) Head Control Board Signals + // 4) Left Arm Control Board Signals + // 5) Right Arm Control Board Signals + // 6) Left Leg Control Board Signals + // 7) Right Leg Control Board Signals + // + + bool ok; + unsigned numberOfInputs; + unsigned controlBoardsNumber = getConfiguration().getControlBoardsNames().size(); + + if (yarp2WBI) { + numberOfInputs = 1; + ok = blockInfo->setNumberOfInputPorts(numberOfInputs); + blockInfo->setInputPortVectorSize(0, getConfiguration().getNumberOfDoFs()); + } + else { + numberOfInputs = controlBoardsNumber; + ok = blockInfo->setNumberOfInputPorts(numberOfInputs); + // Set the size as dynamic + for (unsigned i = 0; i < numberOfInputs; ++i) { + blockInfo->setInputPortType(i, PortDataTypeDouble); + } + } + + if (!ok) { + Log::getSingleton().error("Failed to set input port number."); + return false; + } + + + // TODO: update + // OUTPUTS + // ======= + // + // yarp2WBI + // -------- + // + // 1) Torso Control Board Signals + // 2) Head Control Board Signals + // 3) Left Arm Control Board Signals + // 4) Right Arm Control Board Signals + // 5) Left Leg Control Board Signals + // 6) Right Leg Control Board Signals + // + // WBI2yarp + // -------- + // + // 1) Vector containing the full set of robot's joints (1 x nJoints) + // + + unsigned numberOfOutputs; + + if (yarp2WBI) { + numberOfOutputs = controlBoardsNumber; + ok = blockInfo->setNumberOfOutputPorts(numberOfOutputs); + // Set the size as dynamic + for (unsigned i = 0; i < numberOfOutputs; ++i) { + blockInfo->setOutputPortVectorSize(i, -1); + blockInfo->setOutputPortType(i, PortDataTypeDouble); + } + } + else { + numberOfOutputs = 1; + ok = blockInfo->setNumberOfOutputPorts(numberOfOutputs); + blockInfo->setOutputPortVectorSize(0, getConfiguration().getNumberOfDoFs()); + blockInfo->setOutputPortType(0, PortDataTypeDouble); + } + + if (!ok) { + Log::getSingleton().error("Failed to set output port number."); + return false; + } + + return true; + } + + bool ModelPartitioner::initialize(BlockInformation* blockInfo) + { + if (!WBBlock::initialize(blockInfo)) return false; + + if (!blockInfo->getBooleanParameterAtIndex(1, m_yarp2WBI)) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } + + m_jointsMapString = getRobotInterface()->getJointsMapString(); + + if (!m_jointsMapString) { + return false; + } + + return true; + } + + bool ModelPartitioner::terminate(BlockInformation* blockInfo) + { + return WBBlock::terminate(blockInfo); + } + + bool ModelPartitioner::output(BlockInformation* blockInfo) + { + if (m_yarp2WBI) { + // Input + Signal jointListSignal = blockInfo->getInputPortSignal(0); + + // Outputs + Signal ithControlBoardSignal; + + for (unsigned ithJoint = 0; ithJoint < blockInfo->getInputPortWidth(0); ++ithJoint) { + // Get the ControlBoard number the ith joint belongs, and its index into the CB itself + std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; + const auto& mappedJointInfos = m_jointsMapString->at(ithJointName); + cb_idx controlBoardOfJoint = mappedJointInfos.first; + jointIdx_yarp yarpIndexOfJoint = mappedJointInfos.second; + + // Get the data to forward + Data value = jointListSignal.get(ithJoint); + + // Forward the value to the correct output / output index + ithControlBoardSignal = blockInfo->getOutputPortSignal(controlBoardOfJoint); + ithControlBoardSignal.set(yarpIndexOfJoint, value.doubleData()); + } + } + else { + // Inputs + Signal ithControlBoardSignal; + + // Output + Signal jointListSignal = blockInfo->getOutputPortSignal(0); + + for (unsigned ithJoint = 0; ithJoint < blockInfo->getOutputPortWidth(0); ++ithJoint) { + // Get the ControlBoard number the ith joint belongs, and its index into the CB itself + std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; + const auto& mappedJointInfos = m_jointsMapString->at(ithJointName); + cb_idx controlBoardOfJoint = mappedJointInfos.first; + jointIdx_yarp yarpIndexOfJoint = mappedJointInfos.second; + + // Get the data to forward + ithControlBoardSignal = blockInfo->getInputPortSignal(controlBoardOfJoint); + Data value = ithControlBoardSignal.get(ithJoint); + + // Forward the value to the correct output index + jointListSignal.set(yarpIndexOfJoint, value.doubleData()); + } + } + return true; + } +} diff --git a/toolbox/src/YARPWBIConverter.cpp b/toolbox/src/YARPWBIConverter.cpp deleted file mode 100644 index 0cca8ff77..000000000 --- a/toolbox/src/YARPWBIConverter.cpp +++ /dev/null @@ -1,268 +0,0 @@ -#include "YARPWBIConverter.h" - -#include "BlockInformation.h" -#include "Signal.h" -#include "Error.h" -#include "WBInterface.h" -#include -#include -#include -#include - - -namespace wbt { - namespace hidden { - struct Joint; - - struct Part { - std::string partName; - std::vector joints; - }; - - struct Joint { - unsigned yarpIndex; - unsigned dofIndex; - std::string name; - }; - - /** - * Parse the configuration file to extract parts, joints and indices - * - * @param filepath full path and name to the configuration file - * @return the YARP parts representation - */ - static bool parseConfigurationFileForPartsConfiguration(const std::string& filepath, const wbi::IDList& wbiList, std::vector &parts); - - } -} - -namespace wbt { - - struct YARPWBIConverter::YARPWBIConverterPimpl { - std::vector parts; - bool yarpToWBI; - }; - - std::string YARPWBIConverter::ClassName = "YARPWBIConverter"; - - YARPWBIConverter::YARPWBIConverter() {} - - unsigned YARPWBIConverter::numberOfParameters() { return wbt::WBIModelBlock::numberOfParameters() + 1; } - bool YARPWBIConverter::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - //This is a check that list, dofs and files are ok - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } - - //This specify the output port size - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - - using namespace wbt::hidden; - //Parse the yarpwbi file - //Need to retrieve both parameters, i.e. wbiFile and list - //The following is to avoid to configure the WBInterface during compilation - //(why I don't know :D ) - std::vector parts; - - //Workaround for the fact that ResourceFinder initializes the network by itself. See YARP#1014 - yarp::os::Network network; - yarp::os::ResourceFinder resourceFinder = yarp::os::ResourceFinder::getResourceFinderSingleton(); - - std::string configFilePath = resourceFinder.findFile(m_wbiConfigurationFileName); - - yarp::os::Property configurations; - //loading defaults from configuration file - if (!configurations.fromConfigFile(configFilePath)) { - if (error) error->message = "Failed to parse WBI configuration file"; - return false; - } - - wbi::IDList jointList; - //parse the file to get the joint list - if (!WBInterface::wbdIDListFromConfigPropAndList(configurations, m_wbiListName, jointList)) { - if (error) error->message = "Failed to parse Joint list"; - return false; - } - - if (!parseConfigurationFileForPartsConfiguration(configFilePath, jointList, parts)) { - return false; - } - - blockInfo->getScalarParameterAtIndex(wbt::WBIModelBlock::numberOfParameters() + 1).int8Data(); - - bool yarpToWBI = blockInfo->getScalarParameterAtIndex(wbt::WBIModelBlock::numberOfParameters() + 1).int8Data() == 1; - - // Specify I/O - bool success = true; - //Input - if (yarpToWBI) { - if (!blockInfo->setNumberOfInputPorts(parts.size())) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - - for (unsigned i = 0 ; i < parts.size(); ++i) { - success = success && blockInfo->setInputPortVectorSize(i, parts[i].joints.size()); - blockInfo->setInputPortType(i, PortDataTypeDouble); - } - - } else { - if (!blockInfo->setNumberOfInputPorts(1)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - - success = success && blockInfo->setInputPortVectorSize(0, dofs); - blockInfo->setInputPortType(0, PortDataTypeDouble); - } - - if (!success) { - if (error) error->message = "Failed to configure input ports"; - return false; - } - - if (yarpToWBI) { - // Output port: - // - one port of size dofs - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } - - success = blockInfo->setOutputPortVectorSize(0, dofs); - blockInfo->setOutputPortType(0, PortDataTypeDouble); - } else { - if (!blockInfo->setNumberOfOuputPorts(parts.size())) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - - for (unsigned i = 0 ; i < parts.size(); ++i) { - success = success && blockInfo->setOutputPortVectorSize(i, parts[i].joints.size()); - blockInfo->setOutputPortType(i, PortDataTypeDouble); - } - } - - if (!success) { - if (error) error->message = "Failed to configure output ports"; - return false; - } - - return success; - } - - bool YARPWBIConverter::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace wbt::hidden; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; - - m_pimpl = new YARPWBIConverterPimpl(); - if (!m_pimpl) return false; - - m_pimpl->yarpToWBI = blockInfo->getScalarParameterAtIndex(wbt::WBIModelBlock::numberOfParameters() + 1).int8Data() == 1; - return parseConfigurationFileForPartsConfiguration(WBInterface::sharedInstance().wbiFilePath(), *WBInterface::sharedInstance().wbiList(), m_pimpl->parts); - } - - bool YARPWBIConverter::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - delete m_pimpl; - m_pimpl = 0; - - return WBIModelBlock::terminate(blockInfo, error); - } - - bool YARPWBIConverter::output(BlockInformation *blockInfo, wbt::Error *error) - { - if (!m_pimpl) return false; - using namespace wbt::hidden; - - if (m_pimpl->yarpToWBI) { - //From YARP to WBI: - //Multiple inputs, single output - - Signal output = blockInfo->getOutputPortSignal(0); - - for (unsigned partIdx = 0; partIdx < m_pimpl->parts.size(); ++partIdx) { - Signal partPort = blockInfo->getInputPortSignal(partIdx); - - Part part = m_pimpl->parts[partIdx]; - for (unsigned jointIdx = 0; jointIdx < part.joints.size(); ++jointIdx) { - Joint joint = part.joints[jointIdx]; - output.set(joint.dofIndex, partPort.get(joint.yarpIndex).doubleData()); - } - } - - } else { - //From WBI to YARP - //Single Input port, multiple outputs - Signal inputPort = blockInfo->getInputPortSignal(0); - - for (unsigned partIdx = 0; partIdx < m_pimpl->parts.size(); ++partIdx) { - Signal output = blockInfo->getOutputPortSignal(partIdx); - - Part part = m_pimpl->parts[partIdx]; - for (unsigned jointIdx = 0; jointIdx < part.joints.size(); ++jointIdx) { - Joint joint = part.joints[jointIdx]; - output.set(joint.yarpIndex, inputPort.get(joint.dofIndex).doubleData()); - } - - } - } - - return true; - } - - namespace hidden { - - bool parseConfigurationFileForPartsConfiguration(const std::string& filepath, const wbi::IDList& wbiList, std::vector &parts) - { - parts.clear(); - yarp::os::Property configurationFile; - configurationFile.fromConfigFile(filepath); - //retrieve joints/yarp mapping - yarp::os::Bottle &mapping = configurationFile.findGroup("WBI_YARP_JOINTS"); - if (mapping.isNull() || mapping.size() <= 0) { - return false; - } - - //for every joint in the list - for (unsigned i = 0; i < wbiList.size(); ++i) { - const wbi::ID &id = wbiList.at(i); - //look in the configuration file for that key - yarp::os::Value &jointList = mapping.find(id.toString()); - - if (!jointList.isNull() && jointList.isList() && jointList.asList()->size() == 2) { - yarp::os::Bottle *list = jointList.asList(); - Joint joint; - std::string partName = list->get(0).asString(); - joint.yarpIndex = list->get(1).asInt(); - joint.dofIndex = i; - joint.name = id.toString(); - - std::vector::iterator found = parts.end(); - //look for the part in the vector.. a map would be nicer - for (std::vector::iterator part = parts.begin(); - part != parts.end(); ++part) { - if (part->partName == partName) { - found = part; - break; - } - } - - if (found != parts.end()) { - found->joints.push_back(joint); - } else { - Part newPart; - newPart.partName = partName; - newPart.joints.push_back(joint); - parts.push_back(newPart); - } - } - } - return true; - } - } -} - - diff --git a/toolbox/src/base/factory.cpp b/toolbox/src/base/factory.cpp index 0320676bc..e30b81379 100644 --- a/toolbox/src/base/factory.cpp +++ b/toolbox/src/base/factory.cpp @@ -8,8 +8,8 @@ wbt::Block* wbt::Block::instantiateBlockWithClassName(std::string blockClassName block = new wbt::YarpRead(); } else if (blockClassName == wbt::YarpWrite::ClassName) { block = new wbt::YarpWrite(); - } else if (blockClassName == wbt::YARPWBIConverter::ClassName) { - block = new wbt::YARPWBIConverter(); + } else if (blockClassName == wbt::ModelPartitioner::ClassName) { + block = new wbt::ModelPartitioner(); } else if (blockClassName == wbt::YarpClock::ClassName) { block = new wbt::YarpClock(); } else if (blockClassName == wbt::MassMatrix::ClassName) { From 1ad29cb87074262914631647fd41d1d42b90ac54 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:58:26 +0000 Subject: [PATCH 33/89] Removed support of CMake < 3.0 --- toolbox/CMakeLists.txt | 52 +++++------------------------------------- 1 file changed, 6 insertions(+), 46 deletions(-) diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 99625ae0d..3b0487bdb 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(Matlab REQUIRED ENG_LIBRARY MAIN_PROGRAM) - if(NOT Matlab_FOUND) message(FATAL_ERROR "Matlab not found") endif() @@ -228,50 +227,11 @@ if (WBT_USES_ICUB) endif() #Adding files used for the generation of the dynamic library -if (CMAKE_VERSION GREATER 3.0.0) - matlab_add_mex( - NAME WBToolbox - SRC ${ALL_HEADERS} ${ALL_SOURCES} - LINK_TO ${LINKED_LIBRARIES} - ) -else() - #Adding files used for the generation of the dynamic library - add_library(WBToolbox SHARED ${ALL_HEADERS} ${ALL_SOURCES}) - - set(SFUNCTION_SUFFIX ${Matlab_MEX_EXTENSION}) - #Removing default dynamic library extensions (.dll for Windows, .so for UNIX) and prefixes (lib) otherwise automatically prepended to the mex file. - set_target_properties(WBToolbox PROPERTIES SUFFIX .${SFUNCTION_SUFFIX}) - set_target_properties(WBToolbox PROPERTIES PREFIX "") - - # This seems to be the most reliable method to date to know installed Matlab arch - STRING(REGEX MATCH ..$ MATLAB_ARCH Matlab_MEX_EXTENSION 64) - - #In the following specific flags will be set to ensure compliance with the default settings of Matlab's mex compiler - if(WIN32) - if(NOT "${MATLAB_ARCH}" STREQUAL "64") - message("Flags have been passed to compiler for Matlab 32bits on a Windows computer") - elseif() - message("Flags have been passed to compiler for Matlab 64bits on a Windows computer") - endif() - if(MSVC) - #Setting Compiler Flags - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Z7 /nologo /W3 /WX- /O2 /Oy- /D_REENTRANT /D_CRT_SECURE_NO_DEPRECATE /D_SCL_SECURE_NO_DEPRECATE /D_SECURE_SCL=0 /DyWrite_EXPORTS /Gm- /EHs /Zp8 /GS /fp:precise /Zc:wchar_t /Zc:forScope /GR /Gd /TP /showIncludes") - #Setting Linker Flags - set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /INCREMENTAL:NO /NOLOGO /MANIFEST /debug /MAP.mexw64.map /MACHINE:X64 /EXPORT:mexFunction") - if(NOT "${MATLAB_ARCH}" STREQUAL "64") - #Changing some flags according to system/MATLAB version (32, 64 bits) - set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /MAP.mexw32.map /MACHINE:X32") - endif() - elseif(MSVC) - message("You are not compiling using Microsoft Visual Studio, ERRORS might occur") - endif(MSVC) - elseif(WIN32) - message("No particular flags have been passed to compiler for Matlab 64bits on a UNIX computer") - endif(WIN32) - - # Linking Libraries - target_link_libraries(WBToolbox ${LINKED_LIBRARIES} ${Matlab_LIBRARIES}) -endif() +matlab_add_mex( + NAME WBToolbox + SRC ${ALL_HEADERS} ${ALL_SOURCES} + LINK_TO ${LINKED_LIBRARIES} +) # Link with ClockServer library add_subdirectory(autogenerated/) @@ -279,7 +239,7 @@ target_link_libraries(WBToolbox PUBLIC ClockServer) # Remote Inverse Kinematics requires C++11 if (CMAKE_VERSION VERSION_LESS "3.1") - if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR + if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang") target_compile_options(WBToolbox PUBLIC "-std=c++11") endif() From 0c2ecfd249f7b00d58ee721eeaf51c6b7f835992 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 15:59:10 +0000 Subject: [PATCH 34/89] iDynTree is now a global dependency --- toolbox/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 3b0487bdb..1ce390bc9 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -219,10 +219,11 @@ include_directories(SYSTEM ${Matlab_INCLUDE_DIRS} "${Matlab_ROOT_DIR}/simulink/i include_directories(SYSTEM ${YARP_INCLUDE_DIRS} ${yarpWholeBodyInterface_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) list(APPEND LINKED_LIBRARIES ${YARP_LIBRARIES} ${yarpWholeBodyInterface_LIBRARIES}) +list(APPEND LINKED_LIBRARIES ${iDynTree_LIBRARIES}) if (WBT_USES_ICUB) list(APPEND LINKED_LIBRARIES ctrlLib) if (${ICUB_USE_IPOPT}) - list(APPEND LINKED_LIBRARIES iKin ${iDynTree_LIBRARIES}) + list(APPEND LINKED_LIBRARIES iKin) endif() endif() From d73ea70195f07513a962c3d36003b81acfeb0a5a Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 16:00:21 +0000 Subject: [PATCH 35/89] Link the project against the MxAnyType library --- toolbox/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 1ce390bc9..93f3a71b7 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -234,6 +234,10 @@ matlab_add_mex( LINK_TO ${LINKED_LIBRARIES} ) +# Link with MxAnyType library +# TODO: this will become an external project sooner or later +target_link_libraries(WBToolbox PUBLIC MxAnyType) + # Link with ClockServer library add_subdirectory(autogenerated/) target_link_libraries(WBToolbox PUBLIC ClockServer) From d1bc77597006ee70acdb04f58be57cd31e500985 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 16:04:49 +0000 Subject: [PATCH 36/89] New PID and WBTPIDConfig matlab classes These classes are used to store PID gains for the SetLowLevelPID block --- +WBToolbox/PID.m | 50 +++++++++++++++++++++++++++++++ +WBToolbox/WBTPIDConfig.m | 63 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 113 insertions(+) create mode 100644 +WBToolbox/PID.m create mode 100644 +WBToolbox/WBTPIDConfig.m diff --git a/+WBToolbox/PID.m b/+WBToolbox/PID.m new file mode 100644 index 000000000..33badfea0 --- /dev/null +++ b/+WBToolbox/PID.m @@ -0,0 +1,50 @@ +classdef PID < handle + %PID Class to store pid parameters for a single joint. + % PID('jointName') creates an object for the joint 'jointName' with + % all pid gains set to 0. + % + % PID('jointName', P, I, D) creates an object for the joint 'jointName' + % initializing the gains respectively to P, I, D. + % + % PID Properties: + % P - Proportional gain + % I - Integral gain + % D - Derivative gain + % joint - Reference joint + % + % See also: WBTOOLBOX.WBTPIDCONFIG + + properties + P (1,1) double = 0 + I (1,1) double = 0 + D (1,1) double = 0 + joint (1,:) char + end + + methods + % function obj = PID(jointName) + % obj.joint = jointName; + % end + + function obj = PID(jointName, P, I, D) + if (nargin == 4) + obj.joint = jointName; + obj.P = P; + obj.I = I; + obj.D = D; + elseif (nargin == 1) + obj.joint = jointName; + else + error(... + [... + 'Wrong number of arguments: ', ... + 'You can either specify 1 argument (joint) ', ... + 'or 4 arguments (joint, P, I, D).',... + ]... + ); + end + end + + end +end + diff --git a/+WBToolbox/WBTPIDConfig.m b/+WBToolbox/WBTPIDConfig.m new file mode 100644 index 000000000..9d23b523f --- /dev/null +++ b/+WBToolbox/WBTPIDConfig.m @@ -0,0 +1,63 @@ +classdef WBTPIDConfig < handle + % WBTPIDCONFIG Class to store a set of PID config for many joints. + % WBTPIDCONFIG(WBToolbox.PID('jointName', P, I, D)) adds a new PID + % for joint 'jointName'. + % + % WBTPIDCONFIG Properties: + % P - Proportional gains + % I - Integral gains + % D - Derivative gains + % jointList - List of joints + % + % WBTPIDCONFIG Methods: + % addPID - Add a new joint PID + % removePID - Remove a stored joint PID + % + % See also: WBTOOLBOX.PID + + % Read-Only properties + properties (SetAccess = private) + P (1,:) double + I (1,:) double + D (1,:) double + jointList (1,:) cell + end + + methods + function addPID(obj, pid) + % Add + if ~isa(pid,'WBToolbox.PID') + error('The argument must be a WBToolbox.PID object.') + end + + matches = find(strcmp(obj.jointList, pid.joint), 1); + if ~isempty(matches) + error('The PID''s joint has been already processed.') + end + + obj.jointList{1, end+1} = pid.joint; + obj.P(1, end+1) = pid.P; + obj.I(1, end+1) = pid.I; + obj.D(1, end+1) = pid.D; + end + + function removePID(obj, jointName) + if ~ischar(jointName) + error('The argument must be a char containing the joint name.') + end + + idx = find(strcmp(obj.jointList, jointName), 1); + + if isempty(idx) + error('The specified joint has no configuration stored') + end + + obj.jointList(:,idx) = []; + obj.P(:,idx) = []; + obj.I(:,idx) = []; + obj.D(:,idx) = []; + end + end + +end + From 82b760ba3a00fa12de55047d1b25e6ba514a23fb Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 8 Nov 2017 16:20:42 +0000 Subject: [PATCH 37/89] Matlab functions used by Blocks which inherit from WBBlock class - BlockInitialization: Find the Config block hierarchically closer to the caller block - Mask2WBToolboxConfig: Read the mask of a config block and generates from it a WBToolboxConfig object, validating also its data - WBToolboxConfig2Mask: Read a WBToolboxConfig object and populates the mask of a Config block --- +WBToolbox/BlockInitialization.m | 61 +++++++++++++++++++++ +WBToolbox/Mask2WBToolboxConfig.m | 39 ++++++++++++++ +WBToolbox/WBToolboxConfig2Mask.m | 90 +++++++++++++++++++++++++++++++ 3 files changed, 190 insertions(+) create mode 100644 +WBToolbox/BlockInitialization.m create mode 100644 +WBToolbox/Mask2WBToolboxConfig.m create mode 100644 +WBToolbox/WBToolboxConfig2Mask.m diff --git a/+WBToolbox/BlockInitialization.m b/+WBToolbox/BlockInitialization.m new file mode 100644 index 000000000..5783432e0 --- /dev/null +++ b/+WBToolbox/BlockInitialization.m @@ -0,0 +1,61 @@ +function [configBlock, WBTConfig] = BlockInitialization(currentBlock, currentSystem) +%BLOCKINITIALIZATION Initialize blocks that read a Config block +% This function is used to initialize the mask of block that need to +% gather the WBToolbox configuration from a Config block. + +try + % Get the name of this block from the model's root + %blockAbsoluteName = gcb; + + % Get the top level name + %blockNameSplit = strsplit(blockAbsoluteName,'/'); + blockNameSplit = strsplit(currentBlock,'/'); + topLevel = blockNameSplit{1}; + + % Get all the blocks from the top level of the system + blocks_system = find_system(topLevel,'LookUnderMasks','on'); + + % Get the name of the block's subsystem + %[blockScopeName,~] = fileparts(blockAbsoluteName); + %blockScopeName = gcs; + blockScopeName = currentSystem; +catch + error('[%s::Initialization] Failed to process model''s tree',char(blockAbsoluteName)) +end + +% Look a config block from the block's scope going up in the tree +analyzedScope = blockScopeName; +configBlock = char; +while ~isempty(analyzedScope) + rule = strcat(strrep(analyzedScope,'/','\/'),'\/\w+\/ImConfig'); + idx = cellfun(@(x) ~isequal(regexp(x,rule),[]), blocks_system); + if (sum(idx) > 1) + error('[%s::Initialization] Found more than one configuration',char(blockAbsoluteName)); + elseif (sum(idx) == 1) + configBlock = blocks_system{idx}; + configBlock = fileparts(configBlock); + break; + end + analyzedScope = fileparts(analyzedScope); +end + +if isempty(configBlock) + error('[%s::Initialization] No configuration found',char(blockAbsoluteName)); +end + +configSource = get_param(configBlock,'ConfigSource'); + +if strcmp(configSource,'Mask') + % Read the mask + WBTConfig = WBToolbox.Mask2WBToolboxConfig(configBlock); +elseif strcmp(configSource,'Workspace') + % Get the object name and populate the mask + configObjectName = get_param(configBlock,'ConfigObject'); + WBToolbox.WBToolboxConfig2Mask(configBlock, configObjectName); + % Read the mask + WBTConfig = WBToolbox.Mask2WBToolboxConfig(configBlock); +else + error('[%s::Initialization] Read ConfigSource from Config block not recognized',char(blockAbsoluteName)); +end + +end diff --git a/+WBToolbox/Mask2WBToolboxConfig.m b/+WBToolbox/Mask2WBToolboxConfig.m new file mode 100644 index 000000000..0396a46a3 --- /dev/null +++ b/+WBToolbox/Mask2WBToolboxConfig.m @@ -0,0 +1,39 @@ +function [ WBTConfig ] = Mask2WBToolboxConfig(configBlock) +%MASK2WBTOOLBOXCONFIG Summary of this function goes here +% Detailed explanation goes here + +defaultString = '''NonValid'''; + +if (... + strcmp(char(get_param(configBlock,'RobotName')),defaultString) && ... + strcmp(char(get_param(configBlock,'UrdfFile')),defaultString) && ... + strcmp(char(get_param(configBlock,'LocalName')),defaultString) && ... + strcmp(char(get_param(configBlock,'GravityVector')),defaultString) && ... + strcmp(char(get_param(configBlock,'ControlledJoints')),defaultString) && ... + strcmp(char(get_param(configBlock,'ControlBoardsNames')),defaultString) ... + ) + error('[Mask2WBToolboxConfig] The provided config is NonValid.'); +end + +WBTConfig = WBToolbox.WBToolboxConfig; + +WBTConfig.RobotName = stripApices(char(get_param(configBlock,'RobotName'))); +WBTConfig.UrdfFile = stripApices(char(get_param(configBlock,'UrdfFile'))); +WBTConfig.LocalName = stripApices(char(get_param(configBlock,'LocalName'))); + +ControlledJointsChar = stripApices(char(get_param(configBlock,'ControlledJoints'))); +WBTConfig.ControlledJoints = eval('base',ControlledJointsChar); + +ControlBoardsNamesChar = stripApices(char(get_param(configBlock,'ControlBoardsNames'))); +WBTConfig.ControlBoardsNames = eval('base',ControlBoardsNamesChar); + +GravityVectorChar = stripApices(char(get_param(configBlock,'GravityVector'))); +WBTConfig.GravityVector = eval('base',GravityVectorChar); + +end + +function str = stripApices(str) + if (strcmp(str(1),'''') && strcmp(str(end),'''')) + str = str(2:end-1); + end +end diff --git a/+WBToolbox/WBToolboxConfig2Mask.m b/+WBToolbox/WBToolboxConfig2Mask.m new file mode 100644 index 000000000..1e7e3b227 --- /dev/null +++ b/+WBToolbox/WBToolboxConfig2Mask.m @@ -0,0 +1,90 @@ +function WBToolboxConfig2Mask(configBlock, WBTConfigObjectName) +%WBTOOLBOXCONFIG2MASK Summary of this function goes here +% Detailed explanation goes here + +% Assign names to labels +item_ConfigSource = 1; +item_ConfigObject = 2; +item_RobotName = 3; +item_UrdfFile = 4; +item_ControlledJoints = 5; +item_ControlBoardsNames = 6; +item_LocalName = 7; +item_GravityVector = 8; + +% Get the mask of the Config Block +try + mask = Simulink.Mask.get(configBlock); +catch + error('[WBToolboxConfig2Mask] Impossible to gather Config Block: %s', configBlock); +end + + +% Remove apices if present +if (strcmp(WBTConfigObjectName(1),'''') && strcmp(WBTConfigObjectName(end),'''')) + WBTConfigObjectName = WBTConfigObjectName(2:end-1); +end + +% Try to get the object from the workspace +try + WBTConfigHandle = evalin('base', WBTConfigObjectName); % Get the handle + WBTConfigObject = copy(WBTConfigHandle); % Shallow copy the handle +catch + error('[WBToolboxConfig2Mask] Failed to get %s object from the workspace', WBTConfigObjectName); +end + +try + if (isa(WBTConfigObject,'WBToolbox.WBToolboxConfig') && WBTConfigObject.ValidConfiguration) + SetParamFromReadOnly(... + configBlock,'RobotName',... + strcat('''',WBTConfigObject.RobotName,''''),... + mask.Parameters(item_RobotName)); + SetParamFromReadOnly(... + configBlock,'UrdfFile',... + strcat('''',WBTConfigObject.UrdfFile,''''),mask.Parameters(item_UrdfFile)); + SetParamFromReadOnly(... + configBlock,'ControlledJoints',... + WBTConfigObject.serializeCellArray1D(WBTConfigObject.ControlledJoints),mask.Parameters(item_ControlledJoints)); + SetParamFromReadOnly(... + configBlock,'ControlBoardsNames',... + WBTConfigObject.serializeCellArray1D(WBTConfigObject.ControlBoardsNames),mask.Parameters(item_ControlBoardsNames)); + SetParamFromReadOnly(... + configBlock,'GravityVector',... + WBTConfigObject.serializeVector1D(WBTConfigObject.GravityVector),mask.Parameters(item_GravityVector)) + SetParamFromReadOnly(... + configBlock,'LocalName',... + strcat('''',WBTConfigObject.LocalName,''''),mask.Parameters(item_LocalName)); + else + error('[WBToolboxConfig2Mask] Failed to get the %s object', WBTConfigObjectName); + end +catch + SetParamFromReadOnly(... + configBlock,'RobotName',... + '''NonValid''',mask.Parameters(item_RobotName)); + SetParamFromReadOnly(... + configBlock,'UrdfFile',... + '''NonValid''',mask.Parameters(item_UrdfFile)); + SetParamFromReadOnly(... + configBlock,'ControlledJoints',... + '''NonValid''',mask.Parameters(item_ControlledJoints)); + SetParamFromReadOnly(... + configBlock,'ControlBoardsNames',... + '''NonValid''',mask.Parameters(item_ControlBoardsNames)); + SetParamFromReadOnly(... + configBlock,'LocalName',... + '''NonValid''',mask.Parameters(item_LocalName)); + SetParamFromReadOnly(... + configBlock,'GravityVector',... + '''NonValid''',mask.Parameters(item_GravityVector)); +end + +end + +function SetParamFromReadOnly(block, parameterName, parameterValue, maskParameter) +if ~strcmp(get_param(block,parameterName),parameterValue) + maskParameter.ReadOnly = 'off'; + maskParameter.Enabled = 'on'; + set_param(block,parameterName,parameterValue); + maskParameter.ReadOnly = 'on'; +end +end From 68dc0100644e8e7a03be0b617ba8762c5b247314 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 9 Nov 2017 10:59:31 +0000 Subject: [PATCH 38/89] New getStdVector() method in Signal This method returns a std::vector either if the signal is contiguous or non-contiguous. Considering that all the input signals are const and non-contiguous, this function assures makes the situation clear encapsulating data in a std::vector. --- toolbox/include/base/Signal.h | 2 ++ toolbox/src/base/Signal.cpp | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/toolbox/include/base/Signal.h b/toolbox/include/base/Signal.h index b46d70ecd..0fbe27f3c 100644 --- a/toolbox/include/base/Signal.h +++ b/toolbox/include/base/Signal.h @@ -2,6 +2,7 @@ #define WBT_SIGNAL_H #include "BlockInformation.h" +#include namespace wbt { class Signal; @@ -29,6 +30,7 @@ class wbt::Signal const Data get(unsigned index) const; void* getContiguousBuffer(); + std::vector getStdVector(unsigned length) const; //the missing are cast void set(unsigned index, double data); diff --git a/toolbox/src/base/Signal.cpp b/toolbox/src/base/Signal.cpp index 5d57938fc..9d5f6adbc 100644 --- a/toolbox/src/base/Signal.cpp +++ b/toolbox/src/base/Signal.cpp @@ -128,6 +128,15 @@ namespace wbt { return this->contiguousData; } + std::vector Signal::getStdVector(unsigned length) const + { + std::vector v(length); + for (unsigned i = 0; i < length; ++i) { + v[i] = get(i).doubleData(); + } + return v; + } + //the missing are cast void Signal::set(unsigned index, double data) { From 3bbad1f782e7dea6ed3b92acea6aa3dd23f8c802 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 10 Nov 2017 17:10:04 +0000 Subject: [PATCH 39/89] BlockInformation is now a const pointer, changed indentation in cpp --- toolbox/include/CentroidalMomentum.h | 6 +- toolbox/include/DotJNu.h | 6 +- toolbox/include/ForwardKinematics.h | 6 +- toolbox/include/GetLimits.h | 6 +- toolbox/include/GetMeasurement.h | 6 +- toolbox/include/InverseDynamics.h | 6 +- toolbox/include/Jacobian.h | 6 +- toolbox/include/MassMatrix.h | 6 +- .../include/MinimumJerkTrajectoryGenerator.h | 6 +- toolbox/include/ModelPartitioner.h | 6 +- toolbox/include/RealTimeSynchronizer.h | 6 +- toolbox/include/SetLowLevelPID.h | 8 +- toolbox/include/SetReferences.h | 8 +- toolbox/include/SimulatorSynchronizer.h | 6 +- toolbox/include/YarpClock.h | 6 +- toolbox/include/YarpRead.h | 6 +- toolbox/include/YarpWrite.h | 6 +- toolbox/include/base/Block.h | 14 +- toolbox/include/base/BlockInformation.h | 23 +- .../include/base/SimulinkBlockInformation.h | 18 +- toolbox/include/base/WBBlock.h | 6 +- toolbox/src/CentroidalMomentum.cpp | 289 +++++---- toolbox/src/DotJNu.cpp | 341 ++++++----- toolbox/src/ForwardKinematics.cpp | 276 +++++---- toolbox/src/GetLimits.cpp | 297 +++++---- toolbox/src/GetMeasurement.cpp | 277 +++++---- toolbox/src/InverseDynamics.cpp | 376 ++++++------ toolbox/src/Jacobian.cpp | 329 +++++----- toolbox/src/MassMatrix.cpp | 274 +++++---- .../src/MinimumJerkTrajectoryGenerator.cpp | 418 +++++++------ toolbox/src/ModelPartitioner.cpp | 312 +++++----- toolbox/src/RealTimeSynchronizer.cpp | 123 ++-- toolbox/src/SetLowLevelPID.cpp | 373 ++++++------ toolbox/src/SetReferences.cpp | 425 +++++++------ toolbox/src/SimulatorSynchronizer.cpp | 213 ++++--- toolbox/src/YarpClock.cpp | 93 ++- toolbox/src/YarpRead.cpp | 329 +++++----- toolbox/src/YarpWrite.cpp | 241 ++++---- toolbox/src/base/Block.cpp | 20 +- toolbox/src/base/BlockInformation.cpp | 6 +- toolbox/src/base/Signal.cpp | 562 +++++++++--------- toolbox/src/base/SimulinkBlockInformation.cpp | 328 +++++----- toolbox/src/base/ToolboxSingleton.cpp | 166 +++--- toolbox/src/base/WBBlock.cpp | 6 +- toolbox/src/base/factory.cpp | 76 +-- 45 files changed, 3136 insertions(+), 3181 deletions(-) diff --git a/toolbox/include/CentroidalMomentum.h b/toolbox/include/CentroidalMomentum.h index d3abb34bb..754e4249b 100644 --- a/toolbox/include/CentroidalMomentum.h +++ b/toolbox/include/CentroidalMomentum.h @@ -28,9 +28,9 @@ class wbt::CentroidalMomentum : public wbt::WBBlock bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* WBT_CENTROIDALMOMENTUM_H */ diff --git a/toolbox/include/DotJNu.h b/toolbox/include/DotJNu.h index d844bb510..9c70fe864 100644 --- a/toolbox/include/DotJNu.h +++ b/toolbox/include/DotJNu.h @@ -32,9 +32,9 @@ class wbt::DotJNu : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* WBT_DOTJDOTQ_H */ diff --git a/toolbox/include/ForwardKinematics.h b/toolbox/include/ForwardKinematics.h index 147f2c7b4..4a683e761 100644 --- a/toolbox/include/ForwardKinematics.h +++ b/toolbox/include/ForwardKinematics.h @@ -25,9 +25,9 @@ class wbt::ForwardKinematics : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* WBT_FORWARDKINEMATICS_H */ diff --git a/toolbox/include/GetLimits.h b/toolbox/include/GetLimits.h index 81570ee24..fcabddeb0 100644 --- a/toolbox/include/GetLimits.h +++ b/toolbox/include/GetLimits.h @@ -33,9 +33,9 @@ class wbt::GetLimits : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* WBT_GETLIMITS_H */ diff --git a/toolbox/include/GetMeasurement.h b/toolbox/include/GetMeasurement.h index 8673874f7..101aed7af 100644 --- a/toolbox/include/GetMeasurement.h +++ b/toolbox/include/GetMeasurement.h @@ -30,9 +30,9 @@ class wbt::GetMeasurement : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* end of include guard: WBT_GETMEASUREMENT_H */ diff --git a/toolbox/include/InverseDynamics.h b/toolbox/include/InverseDynamics.h index 2b035663c..63c8e8a3e 100644 --- a/toolbox/include/InverseDynamics.h +++ b/toolbox/include/InverseDynamics.h @@ -37,9 +37,9 @@ class wbt::InverseDynamics : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; diff --git a/toolbox/include/Jacobian.h b/toolbox/include/Jacobian.h index b0726e900..5db8f6d95 100644 --- a/toolbox/include/Jacobian.h +++ b/toolbox/include/Jacobian.h @@ -36,9 +36,9 @@ class wbt::Jacobian : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* WBT_JACOBIAN_H */ diff --git a/toolbox/include/MassMatrix.h b/toolbox/include/MassMatrix.h index 6529d4504..4ebea06b8 100644 --- a/toolbox/include/MassMatrix.h +++ b/toolbox/include/MassMatrix.h @@ -27,9 +27,9 @@ class wbt::MassMatrix : public wbt::WBBlock bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* WBT_MASSMATRIX_H */ diff --git a/toolbox/include/MinimumJerkTrajectoryGenerator.h b/toolbox/include/MinimumJerkTrajectoryGenerator.h index 8bf0f29b7..5d175e7ab 100644 --- a/toolbox/include/MinimumJerkTrajectoryGenerator.h +++ b/toolbox/include/MinimumJerkTrajectoryGenerator.h @@ -28,9 +28,9 @@ class wbt::MinimumJerkTrajectoryGenerator : public wbt::Block { unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; private: diff --git a/toolbox/include/ModelPartitioner.h b/toolbox/include/ModelPartitioner.h index 929da6c7e..2a8b5c435 100644 --- a/toolbox/include/ModelPartitioner.h +++ b/toolbox/include/ModelPartitioner.h @@ -25,9 +25,9 @@ class wbt::ModelPartitioner : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* end of include guard: WBT_MODELPARTITIONER_H */ diff --git a/toolbox/include/RealTimeSynchronizer.h b/toolbox/include/RealTimeSynchronizer.h index 5a2bc601d..26a797589 100644 --- a/toolbox/include/RealTimeSynchronizer.h +++ b/toolbox/include/RealTimeSynchronizer.h @@ -17,9 +17,9 @@ class wbt::RealTimeSynchronizer : public wbt::Block unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; private: double m_period; diff --git a/toolbox/include/SetLowLevelPID.h b/toolbox/include/SetLowLevelPID.h index ff781a885..96b8384d6 100644 --- a/toolbox/include/SetLowLevelPID.h +++ b/toolbox/include/SetLowLevelPID.h @@ -30,7 +30,7 @@ class wbt::SetLowLevelPID : public wbt::WBBlock yarp::dev::PidControlTypeEnum m_controlType; - bool readWBTPidConfigObject(BlockInformation* blockInfo); + bool readWBTPidConfigObject(const BlockInformation* blockInfo); public: static const std::string ClassName; @@ -41,9 +41,9 @@ class wbt::SetLowLevelPID : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* end of include guard: WBT_SETLOWLEVELPID_H_ */ diff --git a/toolbox/include/SetReferences.h b/toolbox/include/SetReferences.h index c7c4c2ed8..d599a1410 100644 --- a/toolbox/include/SetReferences.h +++ b/toolbox/include/SetReferences.h @@ -23,10 +23,10 @@ class wbt::SetReferences : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool initializeInitialConditions(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool initializeInitialConditions(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* end of include guard: WBT_SETREFERENCES_H */ diff --git a/toolbox/include/SimulatorSynchronizer.h b/toolbox/include/SimulatorSynchronizer.h index aa5c3eced..f307c8fc7 100644 --- a/toolbox/include/SimulatorSynchronizer.h +++ b/toolbox/include/SimulatorSynchronizer.h @@ -18,9 +18,9 @@ class wbt::SimulatorSynchronizer : public wbt::Block unsigned numberOfParameters() override; std::vector additionalBlockOptions() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; private: double m_period; diff --git a/toolbox/include/YarpClock.h b/toolbox/include/YarpClock.h index 000229579..c4857e8e0 100644 --- a/toolbox/include/YarpClock.h +++ b/toolbox/include/YarpClock.h @@ -14,9 +14,9 @@ class wbt::YarpClock : public wbt::Block unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* end of include guard: WBT_YARPCLOCK_H */ diff --git a/toolbox/include/YarpRead.h b/toolbox/include/YarpRead.h index bc79bcb61..6fed985ad 100644 --- a/toolbox/include/YarpRead.h +++ b/toolbox/include/YarpRead.h @@ -26,9 +26,9 @@ class wbt::YarpRead : public wbt::Block unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; private: bool m_autoconnect; diff --git a/toolbox/include/YarpWrite.h b/toolbox/include/YarpWrite.h index 807b774cd..4d9e8238b 100644 --- a/toolbox/include/YarpWrite.h +++ b/toolbox/include/YarpWrite.h @@ -26,9 +26,9 @@ class wbt::YarpWrite : public wbt::Block unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; - bool output(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; private: bool m_autoconnect; diff --git a/toolbox/include/base/Block.h b/toolbox/include/base/Block.h index a233859e6..65f8447b6 100644 --- a/toolbox/include/base/Block.h +++ b/toolbox/include/base/Block.h @@ -82,7 +82,7 @@ class wbt::Block * @param S the SimStruct structure * @return true for success, false otherwise */ - virtual bool updateDiscreteState(BlockInformation* blockInfo); + virtual bool updateDiscreteState(const BlockInformation* blockInfo); /** * Not called for now @@ -90,7 +90,7 @@ class wbt::Block * @param S the SimStruct structure * @return true for success, false otherwise */ - virtual bool stateDerivative(BlockInformation* blockInfo); + virtual bool stateDerivative(const BlockInformation* blockInfo); /** @@ -123,7 +123,7 @@ class wbt::Block * * @return true for success, false otherwise */ - virtual bool checkParameters(BlockInformation* blockInfo); + virtual bool checkParameters(const BlockInformation* blockInfo); /** * Initialize the object for the simulation @@ -134,7 +134,7 @@ class wbt::Block * * @return true for success, false otherwise */ - virtual bool initialize(BlockInformation* blockInfo) = 0; + virtual bool initialize(const BlockInformation* blockInfo) = 0; /** * Called to initialize the initial conditions @@ -146,7 +146,7 @@ class wbt::Block * * @return true for success, false otherwise */ - virtual bool initializeInitialConditions(BlockInformation* blockInfo); + virtual bool initializeInitialConditions(const BlockInformation* blockInfo); /** * Perform model cleanup. @@ -157,7 +157,7 @@ class wbt::Block * * @return true for success, false otherwise */ - virtual bool terminate(BlockInformation* blockInfo) = 0; + virtual bool terminate(const BlockInformation* blockInfo) = 0; @@ -170,7 +170,7 @@ class wbt::Block * * @return true for success, false otherwise */ - virtual bool output(BlockInformation* blockInfo) = 0; + virtual bool output(const BlockInformation* blockInfo) = 0; public: diff --git a/toolbox/include/base/BlockInformation.h b/toolbox/include/base/BlockInformation.h index a4ea9da0a..06e2c37c1 100644 --- a/toolbox/include/base/BlockInformation.h +++ b/toolbox/include/base/BlockInformation.h @@ -58,7 +58,8 @@ namespace wbt { class wbt::BlockInformation { public: - virtual ~BlockInformation(); + BlockInformation() = default; + virtual ~BlockInformation() = default; // Block Options methods // ===================== @@ -84,11 +85,13 @@ class wbt::BlockInformation { * * @return true if success, false otherwise */ - virtual bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) = 0; - virtual bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) = 0; - virtual bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) = 0; - virtual bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) = 0; - virtual bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) = 0; + virtual bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const = 0; + virtual bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) const = 0; + virtual bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const = 0; + // virtual bool getAnyTypeAtIndex(unsigned parameterIndex, AnyType* data) = 0; + // virtual bool getCellAtIndex(unsigned parameterIndex, AnyCell& map) = 0; + virtual bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const = 0; + virtual bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const = 0; // Port information methods // ======================== @@ -114,10 +117,10 @@ class wbt::BlockInformation { // Port data // ========= - virtual unsigned getInputPortWidth(unsigned portNumber) = 0; - virtual unsigned getOutputPortWidth(unsigned portNumber) = 0; - virtual wbt::Signal getInputPortSignal(unsigned portNumber) = 0; - virtual wbt::Signal getOutputPortSignal(unsigned portNumber) = 0; + virtual unsigned getInputPortWidth(unsigned portNumber) const = 0; + virtual unsigned getOutputPortWidth(unsigned portNumber) const = 0; + virtual wbt::Signal getInputPortSignal(unsigned portNumber) const = 0; + virtual wbt::Signal getOutputPortSignal(unsigned portNumber)const = 0; }; #endif /* end of include guard: WBT_BLOCKINFORMATION_H */ diff --git a/toolbox/include/base/SimulinkBlockInformation.h b/toolbox/include/base/SimulinkBlockInformation.h index 2334d32a4..0da7f51c5 100644 --- a/toolbox/include/base/SimulinkBlockInformation.h +++ b/toolbox/include/base/SimulinkBlockInformation.h @@ -22,11 +22,11 @@ class wbt::SimulinkBlockInformation : public wbt::BlockInformation bool optionFromKey(const std::string& key, double& option) const override; //Parameters methods - bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) override; - bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) override; - bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) override; - bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) override; - bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) override; + bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const override; + bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) const override; + bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const override; + bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const override; + bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const override; //Port information methods bool setNumberOfInputPorts(unsigned numberOfPorts) override; @@ -39,10 +39,10 @@ class wbt::SimulinkBlockInformation : public wbt::BlockInformation bool setOutputPortType(unsigned portNumber, PortDataType portType) override; //Port data - unsigned getInputPortWidth(unsigned portNumber) override; - unsigned getOutputPortWidth(unsigned portNumber) override; - wbt::Signal getInputPortSignal(unsigned portNumber) override; - wbt::Signal getOutputPortSignal(unsigned portNumber) override; + unsigned getInputPortWidth(unsigned portNumber) const override; + unsigned getOutputPortWidth(unsigned portNumber) const override; + wbt::Signal getInputPortSignal(unsigned portNumber) const override; + wbt::Signal getOutputPortSignal(unsigned portNumber) const override; }; #endif /* end of include guard: WBT_SIMULINKBLOCKINFORMATION_H */ diff --git a/toolbox/include/base/WBBlock.h b/toolbox/include/base/WBBlock.h index cee7d7d11..b3d5cf33c 100644 --- a/toolbox/include/base/WBBlock.h +++ b/toolbox/include/base/WBBlock.h @@ -66,7 +66,7 @@ class wbt::WBBlock : public wbt::Block protected: std::string confKey; iDynTreeRobotState robotState; - bool getWBToolboxParameters(Configuration& config, BlockInformation* blockInfo); + bool getWBToolboxParameters(Configuration& config, const BlockInformation* blockInfo); const std::shared_ptr getRobotInterface(); const Configuration& getConfiguration(); @@ -75,8 +75,8 @@ class wbt::WBBlock : public wbt::Block ~WBBlock() override = default; unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; }; #endif /* end of include guard: WBT_WBIBLOCK_H */ diff --git a/toolbox/src/CentroidalMomentum.cpp b/toolbox/src/CentroidalMomentum.cpp index ca93b9308..1294c7e19 100644 --- a/toolbox/src/CentroidalMomentum.cpp +++ b/toolbox/src/CentroidalMomentum.cpp @@ -10,159 +10,158 @@ #include #include -namespace wbt { - - const std::string CentroidalMomentum::ClassName = "CentroidalMomentum"; - - const unsigned CentroidalMomentum::INPUT_IDX_BASE_POSE = 0; - const unsigned CentroidalMomentum::INPUT_IDX_JOINTCONF = 1; - const unsigned CentroidalMomentum::INPUT_IDX_BASE_VEL = 2; - const unsigned CentroidalMomentum::INPUT_IDX_JOINT_VEL = 3; - const unsigned CentroidalMomentum::OUTPUT_IDX_CENTRMOM = 0; - - CentroidalMomentum::CentroidalMomentum() - : m_centroidalMomentum(nullptr) - {} - - bool CentroidalMomentum::configureSizeAndPorts(BlockInformation* blockInfo) - { - // Memory allocation / Saving data not allowed here - - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - - // INPUTS - // ====== - // - // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) - // 2) Joints position (1xDoFs vector) - // 3) Base frame velocity (1x6 vector) - // 4) Joints velocity (1xDoFs vector) - // - - // Number of inputs - if (!blockInfo->setNumberOfInputPorts(4)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } - - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - - // Size and type - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); - - blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); - - if (!success) { - Log::getSingleton().error("Failed to configure input ports."); - return false; - } - - // OUTPUTS - // ======= - // - // 1) Vector representing the centroidal momentum (1x6) - // - - // Number of outputs - if (!blockInfo->setNumberOfOutputPorts(1)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } - - // Size and type - success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_CENTRMOM, 6); - blockInfo->setOutputPortType(OUTPUT_IDX_CENTRMOM, PortDataTypeDouble); - - return success; +using namespace wbt; + +const std::string CentroidalMomentum::ClassName = "CentroidalMomentum"; + +const unsigned CentroidalMomentum::INPUT_IDX_BASE_POSE = 0; +const unsigned CentroidalMomentum::INPUT_IDX_JOINTCONF = 1; +const unsigned CentroidalMomentum::INPUT_IDX_BASE_VEL = 2; +const unsigned CentroidalMomentum::INPUT_IDX_JOINT_VEL = 3; +const unsigned CentroidalMomentum::OUTPUT_IDX_CENTRMOM = 0; + +CentroidalMomentum::CentroidalMomentum() +: m_centroidalMomentum(nullptr) +{} + +bool CentroidalMomentum::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // 4) Joints velocity (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(4)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - bool CentroidalMomentum::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - // OUTPUT - // ====== + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); - m_centroidalMomentum = new iDynTree::SpatialMomentum(); - return m_centroidalMomentum; + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); + + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; } - bool CentroidalMomentum::terminate(BlockInformation* blockInfo) - { - if (m_centroidalMomentum) { - delete m_centroidalMomentum; - m_centroidalMomentum = 0; - } + // OUTPUTS + // ======= + // + // 1) Vector representing the centroidal momentum (1x6) + // - return WBBlock::terminate(blockInfo); + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; } - bool CentroidalMomentum::output(BlockInformation* blockInfo) - { - using namespace iDynTree; - using namespace Eigen; - typedef Matrix Matrix4dSimulink; - - const auto& model = getRobotInterface()->getKinDynComputations(); - - if (!model) { - Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); - return false; - } - - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== - - unsigned signalWidth; - - // Base pose - Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - - // Base velocity - Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); - double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); - robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), - AngVelocity(m_baseVelocityBuffer+3, 3)); - - // Joints velocity - Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); - robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); - - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); - - // OUTPUT - // ====== - - // Calculate the centroidal momentum - *m_centroidalMomentum = model->getCentroidalTotalMomentum(); - - // Forward the output to Simulink - Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_CENTRMOM); - output.setBuffer(toEigen(*m_centroidalMomentum).data(), - blockInfo->getOutputPortWidth(OUTPUT_IDX_CENTRMOM)); - return true; + // Size and type + success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_CENTRMOM, 6); + blockInfo->setOutputPortType(OUTPUT_IDX_CENTRMOM, PortDataTypeDouble); + + return success; +} + +bool CentroidalMomentum::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; + + // OUTPUT + // ====== + + m_centroidalMomentum = new iDynTree::SpatialMomentum(); + return m_centroidalMomentum; +} + +bool CentroidalMomentum::terminate(const BlockInformation* blockInfo) +{ + if (m_centroidalMomentum) { + delete m_centroidalMomentum; + m_centroidalMomentum = 0; } + + return WBBlock::terminate(blockInfo); +} + +bool CentroidalMomentum::output(const BlockInformation* blockInfo) +{ + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } + + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== + + unsigned signalWidth; + + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + + // Base velocity + Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); + double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); + robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), + AngVelocity(m_baseVelocityBuffer+3, 3)); + + // Joints velocity + Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); + robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); + + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + // OUTPUT + // ====== + + // Calculate the centroidal momentum + *m_centroidalMomentum = model->getCentroidalTotalMomentum(); + + // Forward the output to Simulink + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_CENTRMOM); + output.setBuffer(toEigen(*m_centroidalMomentum).data(), + blockInfo->getOutputPortWidth(OUTPUT_IDX_CENTRMOM)); + return true; } diff --git a/toolbox/src/DotJNu.cpp b/toolbox/src/DotJNu.cpp index db9264ab2..7c82c2c39 100644 --- a/toolbox/src/DotJNu.cpp +++ b/toolbox/src/DotJNu.cpp @@ -9,208 +9,207 @@ #include #include -namespace wbt { +using namespace wbt; - const std::string DotJNu::ClassName = "DotJNu"; +const std::string DotJNu::ClassName = "DotJNu"; - const unsigned DotJNu::INPUT_IDX_BASE_POSE = 0; - const unsigned DotJNu::INPUT_IDX_JOINTCONF = 1; - const unsigned DotJNu::INPUT_IDX_BASE_VEL = 2; - const unsigned DotJNu::INPUT_IDX_JOINT_VEL = 3; - const unsigned DotJNu::OUTPUT_IDX_DOTJ_NU = 0; +const unsigned DotJNu::INPUT_IDX_BASE_POSE = 0; +const unsigned DotJNu::INPUT_IDX_JOINTCONF = 1; +const unsigned DotJNu::INPUT_IDX_BASE_VEL = 2; +const unsigned DotJNu::INPUT_IDX_JOINT_VEL = 3; +const unsigned DotJNu::OUTPUT_IDX_DOTJ_NU = 0; - DotJNu::DotJNu() - : m_dotJNu(nullptr) - , m_frameIsCoM(false) - , m_frameIndex(iDynTree::FRAME_INVALID_INDEX) - {} +DotJNu::DotJNu() +: m_dotJNu(nullptr) +, m_frameIsCoM(false) +, m_frameIndex(iDynTree::FRAME_INVALID_INDEX) +{} - unsigned DotJNu::numberOfParameters() - { - return WBBlock::numberOfParameters() + 1; +unsigned DotJNu::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} + +bool DotJNu::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // 4) Joints velocity (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(4)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - bool DotJNu::configureSizeAndPorts(BlockInformation* blockInfo) - { - // Memory allocation / Saving data not allowed here + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); - // INPUTS - // ====== - // - // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) - // 2) Joints position (1xDoFs vector) - // 3) Base frame velocity (1x6 vector) - // 4) Joints velocity (1xDoFs vector) - // + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); - // Number of inputs - if (!blockInfo->setNumberOfInputPorts(4)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; + } - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + // OUTPUTS + // ======= + // + // 1) Vector representing the \dot{J} \nu vector (1x6) + // - // Size and type - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } - blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); + // Size and type + success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_DOTJ_NU, 6); + blockInfo->setOutputPortType(OUTPUT_IDX_DOTJ_NU, PortDataTypeDouble); - if (!success) { - Log::getSingleton().error("Failed to configure input ports."); - return false; - } + return success; +} - // OUTPUTS - // ======= - // - // 1) Vector representing the \dot{J} \nu vector (1x6) - // +bool DotJNu::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - // Number of outputs - if (!blockInfo->setNumberOfOutputPorts(1)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } + // INPUT PARAMETERS + // ================ - // Size and type - success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_DOTJ_NU, 6); - blockInfo->setOutputPortType(OUTPUT_IDX_DOTJ_NU, PortDataTypeDouble); + std::string frame; + int parentParameters = WBBlock::numberOfParameters(); - return success; + if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + Log::getSingleton().error("Cannot retrieve string from frame parameter."); + return false; } - bool DotJNu::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; - - // INPUT PARAMETERS - // ================ - - std::string frame; - int parentParameters = WBBlock::numberOfParameters(); + // Check if the frame is valid + // --------------------------- - if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { - Log::getSingleton().error("Cannot retrieve string from frame parameter."); - return false; - } - - // Check if the frame is valid - // --------------------------- + const auto& model = getRobotInterface()->getKinDynComputations(); + if (!model) { + Log::getSingleton().error("Cannot retrieve handle to KinDynComputations."); + return false; + } - const auto& model = getRobotInterface()->getKinDynComputations(); - if (!model) { - Log::getSingleton().error("Cannot retrieve handle to KinDynComputations."); + if (frame != "com") { + m_frameIndex = model->getFrameIndex(frame); + if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { + Log::getSingleton().error("Cannot find " + frame + " in the frame list."); return false; } - - if (frame != "com") { - m_frameIndex = model->getFrameIndex(frame); - if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { - Log::getSingleton().error("Cannot find " + frame + " in the frame list."); - return false; - } - } - else { - m_frameIsCoM = true; - m_frameIndex = iDynTree::FRAME_INVALID_INDEX; - } - - // OUTPUT - // ====== - m_dotJNu = new iDynTree::Vector6(); - m_dotJNu->zero(); - - return m_dotJNu; + } + else { + m_frameIsCoM = true; + m_frameIndex = iDynTree::FRAME_INVALID_INDEX; } - bool DotJNu::terminate(BlockInformation* blockInfo) - { - // Output - if (m_dotJNu) { - delete m_dotJNu; - m_dotJNu = 0; - } + // OUTPUT + // ====== + m_dotJNu = new iDynTree::Vector6(); + m_dotJNu->zero(); + + return m_dotJNu; +} - return WBBlock::terminate(blockInfo); +bool DotJNu::terminate(const BlockInformation* blockInfo) +{ + // Output + if (m_dotJNu) { + delete m_dotJNu; + m_dotJNu = 0; } - bool DotJNu::output(BlockInformation* blockInfo) - { - using namespace iDynTree; - using namespace Eigen; - typedef Matrix Matrix4dSimulink; + return WBBlock::terminate(blockInfo); +} - const auto& model = getRobotInterface()->getKinDynComputations(); +bool DotJNu::output(const BlockInformation* blockInfo) +{ + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; - if (!model) { - Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); - return false; - } + const auto& model = getRobotInterface()->getKinDynComputations(); - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== - - unsigned signalWidth; - - // Base pose - Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - - // Base velocity - Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); - double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); - robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), - AngVelocity(m_baseVelocityBuffer+3, 3)); - - // Joints velocity - Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); - robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); - - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); - - // OUTPUT - // ====== - - // Compute the dot{J}*\nu - Vector6 biasAcc; - if (!m_frameIsCoM) { - biasAcc = model->getFrameBiasAcc(m_frameIndex); - } - else { - Vector3 comBiasAcc = model->getCenterOfMassBiasAcc(); - toEigen(biasAcc).segment<3>(0) = iDynTree::toEigen(comBiasAcc); - toEigen(biasAcc).segment<3>(3).setZero(); - } + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } - // Forward the output to Simulink - Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_DOTJ_NU); - output.setBuffer(m_dotJNu->data(), - blockInfo->getOutputPortWidth(OUTPUT_IDX_DOTJ_NU)); - return true; + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== + + unsigned signalWidth; + + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + + // Base velocity + Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); + double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); + robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), + AngVelocity(m_baseVelocityBuffer+3, 3)); + + // Joints velocity + Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); + robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); + + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + // OUTPUT + // ====== + + // Compute the dot{J}*\nu + Vector6 biasAcc; + if (!m_frameIsCoM) { + biasAcc = model->getFrameBiasAcc(m_frameIndex); } + else { + Vector3 comBiasAcc = model->getCenterOfMassBiasAcc(); + toEigen(biasAcc).segment<3>(0) = iDynTree::toEigen(comBiasAcc); + toEigen(biasAcc).segment<3>(3).setZero(); + } + + // Forward the output to Simulink + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_DOTJ_NU); + output.setBuffer(m_dotJNu->data(), + blockInfo->getOutputPortWidth(OUTPUT_IDX_DOTJ_NU)); + return true; } diff --git a/toolbox/src/ForwardKinematics.cpp b/toolbox/src/ForwardKinematics.cpp index 052713745..a34f5c72b 100644 --- a/toolbox/src/ForwardKinematics.cpp +++ b/toolbox/src/ForwardKinematics.cpp @@ -9,185 +9,181 @@ #include #include -namespace wbt { +using namespace wbt; - const std::string ForwardKinematics::ClassName = "ForwardKinematics"; +const std::string ForwardKinematics::ClassName = "ForwardKinematics"; - const unsigned ForwardKinematics::INPUT_IDX_BASE_POSE = 0; - const unsigned ForwardKinematics::INPUT_IDX_JOINTCONF = 1; - const unsigned ForwardKinematics::OUTPUT_IDX_FW_FRAME = 0; +const unsigned ForwardKinematics::INPUT_IDX_BASE_POSE = 0; +const unsigned ForwardKinematics::INPUT_IDX_JOINTCONF = 1; +const unsigned ForwardKinematics::OUTPUT_IDX_FW_FRAME = 0; - ForwardKinematics::ForwardKinematics() - : m_frameIsCoM(false) - , m_frameIndex(iDynTree::FRAME_INVALID_INDEX) - {} +ForwardKinematics::ForwardKinematics() +: m_frameIsCoM(false) +, m_frameIndex(iDynTree::FRAME_INVALID_INDEX) +{} - unsigned ForwardKinematics::numberOfParameters() - { - return WBBlock::numberOfParameters() + 1; - } - - bool ForwardKinematics::configureSizeAndPorts(BlockInformation* blockInfo) - { - // Memory allocation / Saving data not allowed here +unsigned ForwardKinematics::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; +bool ForwardKinematics::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here - // INPUTS - // ====== - // - // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) - // 2) Joints position (1xDoFs vector) - // + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - // Number of inputs - if (!blockInfo->setNumberOfInputPorts(2)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - - // Size and type - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(2)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } - blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - if (!success) { - Log::getSingleton().error("Failed to configure input ports."); - return false; - } + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - // OUTPUTS - // ======= - // - // 1) Homogeneous transformation between the world and the specified frame (4x4 matrix) - // + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - // Number of outputs - if (!blockInfo->setNumberOfOutputPorts(1)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; + } - // Size and type - success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_FW_FRAME, 4, 4); - blockInfo->setOutputPortType(OUTPUT_IDX_FW_FRAME, PortDataTypeDouble); + // OUTPUTS + // ======= + // + // 1) Homogeneous transformation between the world and the specified frame (4x4 matrix) + // - return success; + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; } - bool ForwardKinematics::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; + // Size and type + success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_FW_FRAME, 4, 4); + blockInfo->setOutputPortType(OUTPUT_IDX_FW_FRAME, PortDataTypeDouble); - // INPUT PARAMETERS - // ================ + return success; +} - std::string frame; - int parentParameters = WBBlock::numberOfParameters(); +bool ForwardKinematics::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { - Log::getSingleton().error("Cannot retrieve string from frame parameter."); - return false; - } + // INPUT PARAMETERS + // ================ - // Check if the frame is valid - // --------------------------- + std::string frame; + int parentParameters = WBBlock::numberOfParameters(); - const auto& model = getRobotInterface()->getKinDynComputations(); - if (!model) { - Log::getSingleton().error("Cannot retrieve handle to WBI model."); - return false; - } + if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + Log::getSingleton().error("Cannot retrieve string from frame parameter."); + return false; + } - if (frame != "com") { - m_frameIndex = model->getFrameIndex(frame); - if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { - Log::getSingleton().error("Cannot find " + frame + " in the frame list."); - return false; - } - } - else { - m_frameIsCoM = true; - m_frameIndex = iDynTree::FRAME_INVALID_INDEX; - } + // Check if the frame is valid + // --------------------------- - return true; + const auto& model = getRobotInterface()->getKinDynComputations(); + if (!model) { + Log::getSingleton().error("Cannot retrieve handle to WBI model."); + return false; } - bool ForwardKinematics::terminate(BlockInformation* blockInfo) - { - return WBBlock::terminate(blockInfo); + if (frame != "com") { + m_frameIndex = model->getFrameIndex(frame); + if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { + Log::getSingleton().error("Cannot find " + frame + " in the frame list."); + return false; + } + } + else { + m_frameIsCoM = true; + m_frameIndex = iDynTree::FRAME_INVALID_INDEX; } - bool ForwardKinematics::output(BlockInformation* blockInfo) - { - using namespace iDynTree; - using namespace Eigen; - typedef Matrix Matrix4dSimulink; - typedef Matrix Matrix4diDynTree; + return true; +} - const auto& model = getRobotInterface()->getKinDynComputations(); +bool ForwardKinematics::terminate(const BlockInformation* blockInfo) +{ + return WBBlock::terminate(blockInfo); +} - if (!model) { - Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); - return false; - } +bool ForwardKinematics::output(const BlockInformation* blockInfo) +{ + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; + typedef Matrix Matrix4diDynTree; - // Get the signals and convert them to iDynTree objects - // ==================================================== + const auto& model = getRobotInterface()->getKinDynComputations(); - unsigned signalWidth; + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } - // Base pose - Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + // Get the signals and convert them to iDynTree objects + // ==================================================== - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + unsigned signalWidth; - // TODO: the other 3 inputs are taken from the previous block's setRobotState(). - // How to handle it? What happens if a nullptr is passed? + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - // Update the robot status - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - // Output - // ====== + // Update the robot status + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); - iDynTree::Transform world_H_frame; + // Output + // ====== - // Compute the transform to the selected frame - if (!m_frameIsCoM) { - world_H_frame = model->getWorldTransform(m_frameIndex); - } - else { - world_H_frame.setPosition(model->getCenterOfMassPosition()); - } + iDynTree::Transform world_H_frame; - // Get the output signal memory location - Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_FW_FRAME); + // Compute the transform to the selected frame + if (!m_frameIsCoM) { + world_H_frame = model->getWorldTransform(m_frameIndex); + } + else { + world_H_frame.setPosition(model->getCenterOfMassPosition()); + } - // Allocate objects for row-major -> col-major conversion - Map world_H_frame_RowMajor = toEigen(world_H_frame.asHomogeneousTransform()); - Map world_H_frame_ColMajor((double*)output.getContiguousBuffer(), - 4, 4); + // Get the output signal memory location + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_FW_FRAME); - // Forward the buffer to Simulink transforming it to ColMajor - world_H_frame_ColMajor = world_H_frame_RowMajor; - return true; - } + // Allocate objects for row-major -> col-major conversion + Map world_H_frame_RowMajor = toEigen(world_H_frame.asHomogeneousTransform()); + Map world_H_frame_ColMajor((double*)output.getContiguousBuffer(), + 4, 4); + + // Forward the buffer to Simulink transforming it to ColMajor + world_H_frame_ColMajor = world_H_frame_RowMajor; + return true; } diff --git a/toolbox/src/GetLimits.cpp b/toolbox/src/GetLimits.cpp index bc2f55b10..4923021e7 100644 --- a/toolbox/src/GetLimits.cpp +++ b/toolbox/src/GetLimits.cpp @@ -10,187 +10,186 @@ #include #include -namespace wbt { +using namespace wbt; - const std::string GetLimits::ClassName = "GetLimits"; +const std::string GetLimits::ClassName = "GetLimits"; - unsigned GetLimits::numberOfParameters() - { - return WBBlock::numberOfParameters() + 1; - } - - bool GetLimits::configureSizeAndPorts(BlockInformation* blockInfo) - { - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; +unsigned GetLimits::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} - // INPUTS - // ====== - // - // No inputs - // +bool GetLimits::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - if (!blockInfo->setNumberOfInputPorts(0)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } + // INPUTS + // ====== + // + // No inputs + // - // OUTPUTS - // ======= - // - // 1) vector with the information asked (1xDoFs) - // + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } - if (!blockInfo->setNumberOfOutputPorts(2)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } + // OUTPUTS + // ======= + // + // 1) vector with the information asked (1xDoFs) + // - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + if (!blockInfo->setNumberOfOutputPorts(2)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } - bool success = true; - success = success && blockInfo->setOutputPortVectorSize(0, dofs); // Min limit - success = success && blockInfo->setOutputPortVectorSize(1, dofs); // Max limit + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - blockInfo->setOutputPortType(0, PortDataTypeDouble); - blockInfo->setOutputPortType(1, PortDataTypeDouble); + bool success = true; + success = success && blockInfo->setOutputPortVectorSize(0, dofs); // Min limit + success = success && blockInfo->setOutputPortVectorSize(1, dofs); // Max limit - if (!success) { - Log::getSingleton().error("Failed to configure output ports."); - return false; - } + blockInfo->setOutputPortType(0, PortDataTypeDouble); + blockInfo->setOutputPortType(1, PortDataTypeDouble); - return true; + if (!success) { + Log::getSingleton().error("Failed to configure output ports."); + return false; } - bool GetLimits::initialize(BlockInformation* blockInfo) - { - using namespace yarp::os; - if (!WBBlock::initialize(blockInfo)) return false; + return true; +} + +bool GetLimits::initialize(const BlockInformation* blockInfo) +{ + using namespace yarp::os; + if (!WBBlock::initialize(blockInfo)) return false; + + // Read the control type + std::string limitType; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, limitType)) { + Log::getSingleton().error("Could not read estimate type parameter."); + return false; + } - // Read the control type - std::string limitType; - if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, limitType)) { - Log::getSingleton().error("Could not read estimate type parameter."); + // Initialize the structure that stores the limits + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_limits.reset(new Limit(dofs)); + + // Initializes some buffers + double min = 0; + double max = 0; + + // From the RemoteControlBoardRemapper + // =================================== + // + // In the next methods, the values are asked using joint index and not string. + // The ControlBoardRemapper internally uses the same joints ordering of its + // initialization. In this case, it matches 1:1 the joint vector. It is hence + // possible using i to point to the correct joint. + + // Get the RemoteControlBoardRemapper and IControlLimits2 interface if needed + std::weak_ptr iControlLimits2; + if (limitType == "ControlBoardPosition" || limitType == "ControlBoardVelocity") { + // Retain the control board remapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); return false; } + // Get the interface + if (!getRobotInterface()->getInterface(iControlLimits2)) { + Log::getSingleton().error("Failed to get IControlLimits2 interface."); + return false; + } + } - // Initialize the structure that stores the limits - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - m_limits.reset(new Limit(dofs)); - - // Initializes some buffers - double min = 0; - double max = 0; - - // From the RemoteControlBoardRemapper - // =================================== - // - // In the next methods, the values are asked using joint index and not string. - // The ControlBoardRemapper internally uses the same joints ordering of its - // initialization. In this case, it matches 1:1 the joint vector. It is hence - // possible using i to point to the correct joint. - - // Get the RemoteControlBoardRemapper and IControlLimits2 interface if needed - std::weak_ptr iControlLimits2; - if (limitType == "ControlBoardPosition" || limitType == "ControlBoardVelocity") { - // Retain the control board remapper - if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { - Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); + if (limitType == "ControlBoardPosition") { + for (auto i = 0; i < dofs; ++i) { + if (!iControlLimits2.lock()->getLimits(i, &min, &max)) { + Log::getSingleton().error("Failed to get limits from the interface."); return false; } - // Get the interface - if (!getRobotInterface()->getInterface(iControlLimits2)) { - Log::getSingleton().error("Failed to get IControlLimits2 interface."); + m_limits->min[i] = min; + m_limits->max[i] = max; + } + } + else if (limitType == "ControlBoardVelocity") { + for (auto i = 0; i < dofs; ++i) { + if (!iControlLimits2.lock()->getVelLimits(i, &min, &max)) { + Log::getSingleton().error("Failed to get limits from the interface."); return false; } + m_limits->min[i] = min; + m_limits->max[i] = max; } + } - if (limitType == "ControlBoardPosition") { - for (auto i = 0; i < dofs; ++i) { - if (!iControlLimits2.lock()->getLimits(i, &min, &max)) { - Log::getSingleton().error("Failed to get limits from the interface."); - return false; - } - m_limits->min[i] = min; - m_limits->max[i] = max; - } - } - else if (limitType == "ControlBoardVelocity") { - for (auto i = 0; i < dofs; ++i) { - if (!iControlLimits2.lock()->getVelLimits(i, &min, &max)) { - Log::getSingleton().error("Failed to get limits from the interface."); - return false; - } - m_limits->min[i] = min; - m_limits->max[i] = max; - } - } + // From the URDF model + // =================== + // + // For the time being, only position limits are supported. - // From the URDF model - // =================== - // - // For the time being, only position limits are supported. - - else if (limitType == "ModelPosition") { - iDynTree::IJointConstPtr p_joint; - const iDynTree::Model model = getRobotInterface()->getKinDynComputations()->model(); - - for (auto i = 0; i < dofs; ++i) { - // Get the joint name - std::string joint = getConfiguration().getControlledJoints()[i]; - - // Get its index - iDynTree::LinkIndex jointIndex = model.getLinkIndex(joint); - - // Get the joint from the model - p_joint = model.getJoint(jointIndex); - - if (!p_joint->hasPosLimits()) { - Log::getSingleton().warning("Joint " + joint + " has no model limits."); - // TODO: test how these values are interpreted by Simulink - min = -std::numeric_limits::infinity(); - max = std::numeric_limits::infinity(); - } - else { - p_joint->getPosLimits(0, min, max); - } + else if (limitType == "ModelPosition") { + iDynTree::IJointConstPtr p_joint; + const iDynTree::Model model = getRobotInterface()->getKinDynComputations()->model(); + + for (auto i = 0; i < dofs; ++i) { + // Get the joint name + std::string joint = getConfiguration().getControlledJoints()[i]; + + // Get its index + iDynTree::LinkIndex jointIndex = model.getLinkIndex(joint); + + // Get the joint from the model + p_joint = model.getJoint(jointIndex); + + if (!p_joint->hasPosLimits()) { + Log::getSingleton().warning("Joint " + joint + " has no model limits."); + // TODO: test how these values are interpreted by Simulink + min = -std::numeric_limits::infinity(); + max = std::numeric_limits::infinity(); + } + else { + p_joint->getPosLimits(0, min, max); } } - // TODO - // else if (limitType == "ModelVelocity") { - // } - // else if (limitType == "ModelEffort") { - // } - else { - Log::getSingleton().error("Limit type " + limitType + " not recognized."); - return false; - } - - return true; + } + // TODO + // else if (limitType == "ModelVelocity") { + // } + // else if (limitType == "ModelEffort") { + // } + else { + Log::getSingleton().error("Limit type " + limitType + " not recognized."); + return false; } - bool GetLimits::terminate(BlockInformation* blockInfo) - { - // Release the RemoteControlBoardRemapper - bool ok = true; - ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); - if (!ok) { - Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); - } + return true; +} - return ok && WBBlock::terminate(blockInfo); +bool GetLimits::terminate(const BlockInformation* blockInfo) +{ + // Release the RemoteControlBoardRemapper + bool ok = true; + ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); } - bool GetLimits::output(BlockInformation* blockInfo) - { - if (!m_limits) return false; + return ok && WBBlock::terminate(blockInfo); +} + +bool GetLimits::output(const BlockInformation* blockInfo) +{ + if (!m_limits) return false; - Signal minPort = blockInfo->getOutputPortSignal(0); - Signal maxPort = blockInfo->getOutputPortSignal(1); + Signal minPort = blockInfo->getOutputPortSignal(0); + Signal maxPort = blockInfo->getOutputPortSignal(1); - minPort.setBuffer(m_limits->min.data(), getConfiguration().getNumberOfDoFs()); - maxPort.setBuffer(m_limits->max.data(), getConfiguration().getNumberOfDoFs()); + minPort.setBuffer(m_limits->min.data(), getConfiguration().getNumberOfDoFs()); + maxPort.setBuffer(m_limits->max.data(), getConfiguration().getNumberOfDoFs()); - return true; - } + return true; } diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp index c0e0f01f6..65cc1a49e 100644 --- a/toolbox/src/GetMeasurement.cpp +++ b/toolbox/src/GetMeasurement.cpp @@ -10,174 +10,173 @@ #define _USE_MATH_DEFINES #include -namespace wbt { +using namespace wbt; - const std::string GetMeasurement::ClassName = "GetMeasurement"; +const std::string GetMeasurement::ClassName = "GetMeasurement"; - void deg2rad(std::vector& v) - { - const double deg2rad = (2 * M_PI) / 180.0; - for (auto& element : v) { - element *= deg2rad; - } - } - - unsigned GetMeasurement::numberOfParameters() - { - return WBBlock::numberOfParameters() + 1; +void deg2rad(std::vector& v) +{ + const double deg2rad = (2 * M_PI) / 180.0; + for (auto& element : v) { + element *= deg2rad; } +} - bool GetMeasurement::configureSizeAndPorts(BlockInformation* blockInfo) - { - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; +unsigned GetMeasurement::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} - // INPUTS - // ====== - // - // No inputs - // +bool GetMeasurement::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - if (!blockInfo->setNumberOfInputPorts(0)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } + // INPUTS + // ====== + // + // No inputs + // - // OUTPUTS - // ======= - // - // 1) Vector with the information asked (1xDoFs) - // + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } - if (!blockInfo->setNumberOfOutputPorts(1)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } + // OUTPUTS + // ======= + // + // 1) Vector with the information asked (1xDoFs) + // - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } - bool success = blockInfo->setOutputPortVectorSize(0, dofs); - blockInfo->setOutputPortType(0, PortDataTypeDouble); - if (!success) { - Log::getSingleton().error("Failed to configure output ports."); - return false; - } + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - return true; + bool success = blockInfo->setOutputPortVectorSize(0, dofs); + blockInfo->setOutputPortType(0, PortDataTypeDouble); + if (!success) { + Log::getSingleton().error("Failed to configure output ports."); + return false; } - bool GetMeasurement::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; + return true; +} - // Reading the control type - std::string informationType; - if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, informationType)) { - Log::getSingleton().error("Could not read estimate type parameter."); - return false; - } +bool GetMeasurement::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - if (informationType == "Joints Position") { - m_estimateType = wbt::ESTIMATE_JOINT_POS; - } else if (informationType == "Joints Velocity") { - m_estimateType = wbt::ESTIMATE_JOINT_VEL; - } else if (informationType == "Joints Acceleration") { - m_estimateType = wbt::ESTIMATE_JOINT_ACC; - } else if (informationType == "Joints Torque") { - m_estimateType = wbt::ESTIMATE_JOINT_TORQUE; - } else { - Log::getSingleton().error("Estimate not supported."); - return false; - } + // Reading the control type + std::string informationType; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, informationType)) { + Log::getSingleton().error("Could not read estimate type parameter."); + return false; + } - // Initialize the size of the output vector - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - m_estimate.reserve(dofs); + if (informationType == "Joints Position") { + m_estimateType = wbt::ESTIMATE_JOINT_POS; + } else if (informationType == "Joints Velocity") { + m_estimateType = wbt::ESTIMATE_JOINT_VEL; + } else if (informationType == "Joints Acceleration") { + m_estimateType = wbt::ESTIMATE_JOINT_ACC; + } else if (informationType == "Joints Torque") { + m_estimateType = wbt::ESTIMATE_JOINT_TORQUE; + } else { + Log::getSingleton().error("Estimate not supported."); + return false; + } - // Retain the ControlBoardRemapper - if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { - Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); - return false; - } + // Initialize the size of the output vector + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_estimate.reserve(dofs); - return true; + // Retain the ControlBoardRemapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); + return false; } - bool GetMeasurement::terminate(BlockInformation* blockInfo) - { - // Release the RemoteControlBoardRemapper - bool ok = true; - ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); - if (!ok) { - Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); - } + return true; +} - return ok && WBBlock::terminate(blockInfo); +bool GetMeasurement::terminate(const BlockInformation* blockInfo) +{ + // Release the RemoteControlBoardRemapper + bool ok = true; + ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); } - bool GetMeasurement::output(BlockInformation* blockInfo) - { - bool ok; - switch (m_estimateType) { - case ESTIMATE_JOINT_POS: { - // Get the interface - std::weak_ptr iEncoders; - if (!getRobotInterface()->getInterface(iEncoders)) { - Log::getSingleton().error("Failed to get IPidControl interface."); - return false; - } - // Get the measurement - ok = iEncoders.lock()->getEncoders(m_estimate.data()); - deg2rad(m_estimate); - break; - } - case ESTIMATE_JOINT_VEL: { - // Get the interface - std::weak_ptr iEncoders; - if (!getRobotInterface()->getInterface(iEncoders)) { - Log::getSingleton().error("Failed to get IEncoders interface."); - return false; - } - // Get the measurement - ok = iEncoders.lock()->getEncoderSpeeds(m_estimate.data()); - deg2rad(m_estimate); - break; + return ok && WBBlock::terminate(blockInfo); +} + +bool GetMeasurement::output(const BlockInformation* blockInfo) +{ + bool ok; + switch (m_estimateType) { + case ESTIMATE_JOINT_POS: { + // Get the interface + std::weak_ptr iEncoders; + if (!getRobotInterface()->getInterface(iEncoders)) { + Log::getSingleton().error("Failed to get IPidControl interface."); + return false; } - case ESTIMATE_JOINT_ACC: { - // Get the interface - std::weak_ptr iEncoders; - if (!getRobotInterface()->getInterface(iEncoders)) { - Log::getSingleton().error("Failed to get IEncoders interface."); - return false; - } - // Get the measurement - ok = iEncoders.lock()->getEncoderAccelerations(m_estimate.data()); - deg2rad(m_estimate); - break; + // Get the measurement + ok = iEncoders.lock()->getEncoders(m_estimate.data()); + deg2rad(m_estimate); + break; + } + case ESTIMATE_JOINT_VEL: { + // Get the interface + std::weak_ptr iEncoders; + if (!getRobotInterface()->getInterface(iEncoders)) { + Log::getSingleton().error("Failed to get IEncoders interface."); + return false; } - case ESTIMATE_JOINT_TORQUE: { - // Get the interface - std::weak_ptr iTorqueControl; - if (!getRobotInterface()->getInterface(iTorqueControl)) { - Log::getSingleton().error("Failed to get ITorqueControl interface."); - return false; - } - // Get the measurement - ok = iTorqueControl.lock()->getTorques(m_estimate.data()); - break; + // Get the measurement + ok = iEncoders.lock()->getEncoderSpeeds(m_estimate.data()); + deg2rad(m_estimate); + break; + } + case ESTIMATE_JOINT_ACC: { + // Get the interface + std::weak_ptr iEncoders; + if (!getRobotInterface()->getInterface(iEncoders)) { + Log::getSingleton().error("Failed to get IEncoders interface."); + return false; } - default: - Log::getSingleton().error("Estimate type not recognized."); + // Get the measurement + ok = iEncoders.lock()->getEncoderAccelerations(m_estimate.data()); + deg2rad(m_estimate); + break; + } + case ESTIMATE_JOINT_TORQUE: { + // Get the interface + std::weak_ptr iTorqueControl; + if (!getRobotInterface()->getInterface(iTorqueControl)) { + Log::getSingleton().error("Failed to get ITorqueControl interface."); return false; + } + // Get the measurement + ok = iTorqueControl.lock()->getTorques(m_estimate.data()); + break; } - - if (!ok) { - Log::getSingleton().error("Failed to get estimate."); + default: + Log::getSingleton().error("Estimate type not recognized."); return false; - } - - Signal signal = blockInfo->getOutputPortSignal(0); - signal.setBuffer(m_estimate.data(), blockInfo->getOutputPortWidth(0)); + } - return true; + if (!ok) { + Log::getSingleton().error("Failed to get estimate."); + return false; } + + Signal signal = blockInfo->getOutputPortSignal(0); + signal.setBuffer(m_estimate.data(), blockInfo->getOutputPortWidth(0)); + + return true; } diff --git a/toolbox/src/InverseDynamics.cpp b/toolbox/src/InverseDynamics.cpp index 28b8a70c7..5aa635a93 100644 --- a/toolbox/src/InverseDynamics.cpp +++ b/toolbox/src/InverseDynamics.cpp @@ -10,206 +10,204 @@ #include #include -// TODO: Substitute with using -namespace wbt { - - const std::string InverseDynamics::ClassName = "InverseDynamics"; - - const unsigned InverseDynamics::INPUT_IDX_BASE_POSE = 0; - const unsigned InverseDynamics::INPUT_IDX_JOINTCONF = 1; - const unsigned InverseDynamics::INPUT_IDX_BASE_VEL = 2; - const unsigned InverseDynamics::INPUT_IDX_JOINT_VEL = 3; - const unsigned InverseDynamics::INPUT_IDX_BASE_ACC = 4; - const unsigned InverseDynamics::INPUT_IDX_JOINT_ACC = 5; - const unsigned InverseDynamics::OUTPUT_IDX_TORQUES = 0; - - InverseDynamics::InverseDynamics() - : m_baseAcceleration(nullptr) - , m_jointsAcceleration(nullptr) - , m_torques(nullptr) - {} - - unsigned InverseDynamics::numberOfParameters() - { - return WBBlock::numberOfParameters(); +using namespace wbt; + +const std::string InverseDynamics::ClassName = "InverseDynamics"; + +const unsigned InverseDynamics::INPUT_IDX_BASE_POSE = 0; +const unsigned InverseDynamics::INPUT_IDX_JOINTCONF = 1; +const unsigned InverseDynamics::INPUT_IDX_BASE_VEL = 2; +const unsigned InverseDynamics::INPUT_IDX_JOINT_VEL = 3; +const unsigned InverseDynamics::INPUT_IDX_BASE_ACC = 4; +const unsigned InverseDynamics::INPUT_IDX_JOINT_ACC = 5; +const unsigned InverseDynamics::OUTPUT_IDX_TORQUES = 0; + +InverseDynamics::InverseDynamics() +: m_baseAcceleration(nullptr) +, m_jointsAcceleration(nullptr) +, m_torques(nullptr) +{} + +unsigned InverseDynamics::numberOfParameters() +{ + return WBBlock::numberOfParameters(); +} + +bool InverseDynamics::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // 4) Joints velocity (1xDoFs vector) + // 5) Base frame acceleration (1x6 vector) + // 6) Joints acceleration (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(6)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - bool InverseDynamics::configureSizeAndPorts(BlockInformation* blockInfo) - { - // Memory allocation / Saving data not allowed here - - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - - // INPUTS - // ====== - // - // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) - // 2) Joints position (1xDoFs vector) - // 3) Base frame velocity (1x6 vector) - // 4) Joints velocity (1xDoFs vector) - // 5) Base frame acceleration (1x6 vector) - // 6) Joints acceleration (1xDoFs vector) - // - - // Number of inputs - if (!blockInfo->setNumberOfInputPorts(6)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } - - // Get the DoFs - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - - // Size and type - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_ACC, 6); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_ACC, dofs); - - blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_BASE_ACC, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINT_ACC, PortDataTypeDouble); - - if (!success) { - Log::getSingleton().error("Failed to configure input ports."); - return false; - } - - // OUTPUTS - // ======= - // - // 1) Vector representing the torques (1x(DoFs+6)) - // - - // Number of outputs - if (!blockInfo->setNumberOfOutputPorts(1)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } - - // Size and type - success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_TORQUES, dofs + 6); - blockInfo->setOutputPortType(OUTPUT_IDX_TORQUES, PortDataTypeDouble); - - return success; + // Get the DoFs + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_ACC, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_ACC, dofs); + + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_ACC, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_ACC, PortDataTypeDouble); + + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; } - bool InverseDynamics::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; + // OUTPUTS + // ======= + // + // 1) Vector representing the torques (1x(DoFs+6)) + // - // OUTPUT / VARIABLES - // ================== + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + // Size and type + success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_TORQUES, dofs + 6); + blockInfo->setOutputPortType(OUTPUT_IDX_TORQUES, PortDataTypeDouble); - m_baseAcceleration = new iDynTree::Vector6(); - m_baseAcceleration->zero(); - m_jointsAcceleration = new iDynTree::VectorDynSize(dofs); - m_jointsAcceleration->zero(); + return success; +} - const auto& model = getRobotInterface()->getKinDynComputations()->model(); - m_torques = new iDynTree::FreeFloatingGeneralizedTorques(model); +bool InverseDynamics::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - return m_baseAcceleration && m_jointsAcceleration && m_torques; - } + // OUTPUT / VARIABLES + // ================== - bool InverseDynamics::terminate(BlockInformation* blockInfo) - { - if (m_baseAcceleration) { - delete m_baseAcceleration; - m_baseAcceleration = nullptr; - } - if (m_jointsAcceleration) { - delete m_jointsAcceleration; - m_jointsAcceleration = nullptr; - } - if (m_torques) { - delete m_torques; - m_torques = nullptr; - } - - return WBBlock::terminate(blockInfo); + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + m_baseAcceleration = new iDynTree::Vector6(); + m_baseAcceleration->zero(); + m_jointsAcceleration = new iDynTree::VectorDynSize(dofs); + m_jointsAcceleration->zero(); + + const auto& model = getRobotInterface()->getKinDynComputations()->model(); + m_torques = new iDynTree::FreeFloatingGeneralizedTorques(model); + + return m_baseAcceleration && m_jointsAcceleration && m_torques; +} + +bool InverseDynamics::terminate(const BlockInformation* blockInfo) +{ + if (m_baseAcceleration) { + delete m_baseAcceleration; + m_baseAcceleration = nullptr; + } + if (m_jointsAcceleration) { + delete m_jointsAcceleration; + m_jointsAcceleration = nullptr; } + if (m_torques) { + delete m_torques; + m_torques = nullptr; + } + + return WBBlock::terminate(blockInfo); +} + +bool InverseDynamics::output(const BlockInformation* blockInfo) +{ + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; - bool InverseDynamics::output(BlockInformation* blockInfo) - { - using namespace iDynTree; - using namespace Eigen; - typedef Matrix Matrix4dSimulink; - - const auto& model = getRobotInterface()->getKinDynComputations(); - - if (!model) { - Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); - return false; - } - - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== - - unsigned signalWidth; - - // Base pose - Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - - // Base velocity - Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); - double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); - robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), - AngVelocity(m_baseVelocityBuffer+3, 3)); - - // Joints velocity - Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); - robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); - - // Base acceleration - Signal baseAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_ACC); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_ACC); - m_baseAcceleration->fillBuffer(baseAccelerationSignal.getStdVector(signalWidth).data()); - - // Joints acceleration - Signal jointsAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_ACC); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_ACC); - m_jointsAcceleration->fillBuffer(jointsAccelerationSignal.getStdVector(signalWidth).data()); - - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); - - // OUTPUT - // ====== - - // Calculate the inverse dynamics (assuming zero external forces) - model->inverseDynamics(*m_baseAcceleration, - *m_jointsAcceleration, - LinkNetExternalWrenches(model->getNrOfLinks()), // TODO - *m_torques); - - // Forward the output to Simulink - Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_TORQUES); - output.setBuffer(m_torques->jointTorques().data(), - blockInfo->getOutputPortWidth(OUTPUT_IDX_TORQUES)); - return true; + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; } + + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== + + unsigned signalWidth; + + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + + // Base velocity + Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); + double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); + robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), + AngVelocity(m_baseVelocityBuffer+3, 3)); + + // Joints velocity + Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); + robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); + + // Base acceleration + Signal baseAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_ACC); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_ACC); + m_baseAcceleration->fillBuffer(baseAccelerationSignal.getStdVector(signalWidth).data()); + + // Joints acceleration + Signal jointsAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_ACC); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_ACC); + m_jointsAcceleration->fillBuffer(jointsAccelerationSignal.getStdVector(signalWidth).data()); + + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + // OUTPUT + // ====== + + // Calculate the inverse dynamics (assuming zero external forces) + model->inverseDynamics(*m_baseAcceleration, + *m_jointsAcceleration, + LinkNetExternalWrenches(model->getNrOfLinks()), // TODO + *m_torques); + + // Forward the output to Simulink + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_TORQUES); + output.setBuffer(m_torques->jointTorques().data(), + blockInfo->getOutputPortWidth(OUTPUT_IDX_TORQUES)); + return true; } diff --git a/toolbox/src/Jacobian.cpp b/toolbox/src/Jacobian.cpp index 8988cf4bc..4901c7ebc 100644 --- a/toolbox/src/Jacobian.cpp +++ b/toolbox/src/Jacobian.cpp @@ -9,216 +9,215 @@ #include #include -namespace wbt { +using namespace wbt; - const std::string Jacobian::ClassName = "Jacobian"; +const std::string Jacobian::ClassName = "Jacobian"; - const unsigned Jacobian::INPUT_IDX_BASE_POSE = 0; - const unsigned Jacobian::INPUT_IDX_JOINTCONF = 1; - const unsigned Jacobian::OUTPUT_IDX_FW_FRAME = 0; +const unsigned Jacobian::INPUT_IDX_BASE_POSE = 0; +const unsigned Jacobian::INPUT_IDX_JOINTCONF = 1; +const unsigned Jacobian::OUTPUT_IDX_FW_FRAME = 0; - Jacobian::Jacobian() - : m_jacobianCOM(nullptr) - , m_jacobian(nullptr) - , m_frameIsCoM(false) - , m_frameIndex(iDynTree::FRAME_INVALID_INDEX) - {} +Jacobian::Jacobian() +: m_jacobianCOM(nullptr) +, m_jacobian(nullptr) +, m_frameIsCoM(false) +, m_frameIndex(iDynTree::FRAME_INVALID_INDEX) +{} - unsigned Jacobian::numberOfParameters() - { - return WBBlock::numberOfParameters() + 1; - } +unsigned Jacobian::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} - bool Jacobian::configureSizeAndPorts(BlockInformation* blockInfo) - { - // Memory allocation / Saving data not allowed here +bool Jacobian::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - // INPUTS - // ====== - // - // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) - // 2) Joints position (1xDoFs vector) - // 3) Base frame velocity (1x6 vector) - // + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // - // Number of inputs - if (!blockInfo->setNumberOfInputPorts(2)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(2)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - // Size and type - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - if (!success) { - Log::getSingleton().error("Failed to configure input ports."); - return false; - } + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; + } - // OUTPUTS - // ======= - // - // 1) Matrix representing the Jacobian (6x(DoFs+6)) - // + // OUTPUTS + // ======= + // + // 1) Matrix representing the Jacobian (6x(DoFs+6)) + // - // Number of outputs - if (!blockInfo->setNumberOfOutputPorts(1)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } - // Size and type - success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_FW_FRAME, 6, 6 + dofs); - blockInfo->setOutputPortType(OUTPUT_IDX_FW_FRAME, PortDataTypeDouble); + // Size and type + success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_FW_FRAME, 6, 6 + dofs); + blockInfo->setOutputPortType(OUTPUT_IDX_FW_FRAME, PortDataTypeDouble); - return success; - } + return success; +} - bool Jacobian::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; +bool Jacobian::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - // INPUT PARAMETERS - // ================ + // INPUT PARAMETERS + // ================ - std::string frame; - int parentParameters = WBBlock::numberOfParameters(); + std::string frame; + int parentParameters = WBBlock::numberOfParameters(); - if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { - Log::getSingleton().error("Cannot retrieve string from frame parameter."); - return false; - } + if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + Log::getSingleton().error("Cannot retrieve string from frame parameter."); + return false; + } - // Check if the frame is valid - // --------------------------- + // Check if the frame is valid + // --------------------------- + + const auto& model = getRobotInterface()->getKinDynComputations(); + if (!model) { + Log::getSingleton().error("Cannot retrieve handle to KinDynComputations."); + return false; + } - const auto& model = getRobotInterface()->getKinDynComputations(); - if (!model) { - Log::getSingleton().error("Cannot retrieve handle to KinDynComputations."); + if (frame != "com") { + m_frameIndex = model->getFrameIndex(frame); + if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { + Log::getSingleton().error("Cannot find " + frame + " in the frame list."); return false; } + } + else { + m_frameIsCoM = true; + m_frameIndex = iDynTree::FRAME_INVALID_INDEX; + } - if (frame != "com") { - m_frameIndex = model->getFrameIndex(frame); - if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { - Log::getSingleton().error("Cannot find " + frame + " in the frame list."); - return false; - } - } - else { - m_frameIsCoM = true; - m_frameIndex = iDynTree::FRAME_INVALID_INDEX; - } + // OUTPUT / VARIABLES + // ================== - // OUTPUT / VARIABLES - // ================== + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_jacobianCOM = new iDynTree::MatrixDynSize(3, 6 + dofs); + m_jacobianCOM->zero(); - m_jacobianCOM = new iDynTree::MatrixDynSize(3, 6 + dofs); - m_jacobianCOM->zero(); + // Output + m_jacobian = new iDynTree::MatrixDynSize(6, 6 + dofs); + m_jacobian->zero(); - // Output - m_jacobian = new iDynTree::MatrixDynSize(6, 6 + dofs); - m_jacobian->zero(); + return m_jacobianCOM && m_jacobian; +} - return m_jacobianCOM && m_jacobian; +bool Jacobian::terminate(const BlockInformation* blockInfo) +{ + if (m_jacobianCOM) { + delete m_jacobianCOM; + m_jacobianCOM = nullptr; } - - bool Jacobian::terminate(BlockInformation* blockInfo) - { - if (m_jacobianCOM) { - delete m_jacobianCOM; - m_jacobianCOM = nullptr; - } - if (m_jacobian) { - delete m_jacobian; - m_jacobian = nullptr; - } - - return WBBlock::terminate(blockInfo); + if (m_jacobian) { + delete m_jacobian; + m_jacobian = nullptr; } - bool Jacobian::output(BlockInformation* blockInfo) - { - using namespace iDynTree; - using namespace Eigen; - typedef Matrix Matrix4dSimulink; - typedef Matrix MatrixXdSimulink; - typedef Matrix MatrixXdiDynTree; + return WBBlock::terminate(blockInfo); +} - const auto& model = getRobotInterface()->getKinDynComputations(); +bool Jacobian::output(const BlockInformation* blockInfo) +{ + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; + typedef Matrix MatrixXdSimulink; + typedef Matrix MatrixXdiDynTree; - if (!model) { - Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); - return false; - } + const auto& model = getRobotInterface()->getKinDynComputations(); - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } - unsigned signalWidth; + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== - // Base pose - Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + unsigned signalWidth; - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - // TODO: what about the other inputs of setRobotState? + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); + // TODO: what about the other inputs of setRobotState? - // OUTPUT - // ====== + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); - iDynTree::Transform world_H_frame; + // OUTPUT + // ====== - // Compute the jacobian - bool ok = false; - if (!m_frameIsCoM) { - world_H_frame = model->getWorldTransform(m_frameIndex); - ok = model->getFrameFreeFloatingJacobian(m_frameIndex, *m_jacobian); - } - else { - world_H_frame.setPosition(model->getCenterOfMassPosition()); - ok = model->getCenterOfMassJacobian(*m_jacobianCOM); - int cols = m_jacobianCOM->cols(); - toEigen(*m_jacobian).block(0,0,3,cols) = toEigen(*m_jacobianCOM); - toEigen(*m_jacobian).block(3,0,3,cols).setZero(); - } + iDynTree::Transform world_H_frame; + + // Compute the jacobian + bool ok = false; + if (!m_frameIsCoM) { + world_H_frame = model->getWorldTransform(m_frameIndex); + ok = model->getFrameFreeFloatingJacobian(m_frameIndex, *m_jacobian); + } + else { + world_H_frame.setPosition(model->getCenterOfMassPosition()); + ok = model->getCenterOfMassJacobian(*m_jacobianCOM); + int cols = m_jacobianCOM->cols(); + toEigen(*m_jacobian).block(0,0,3,cols) = toEigen(*m_jacobianCOM); + toEigen(*m_jacobian).block(3,0,3,cols).setZero(); + } - // Get the output signal memory location - Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_FW_FRAME); - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + // Get the output signal memory location + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_FW_FRAME); + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - // Allocate objects for row-major -> col-major conversion - Map jacobianRowMajor = toEigen(*m_jacobian); - Map jacobianColMajor((double*)output.getContiguousBuffer(), - 6, 6 + dofs); + // Allocate objects for row-major -> col-major conversion + Map jacobianRowMajor = toEigen(*m_jacobian); + Map jacobianColMajor((double*)output.getContiguousBuffer(), + 6, 6 + dofs); - // Forward the buffer to Simulink transforming it to ColMajor - jacobianColMajor = jacobianRowMajor; - return true; - } + // Forward the buffer to Simulink transforming it to ColMajor + jacobianColMajor = jacobianRowMajor; + return true; } diff --git a/toolbox/src/MassMatrix.cpp b/toolbox/src/MassMatrix.cpp index 4fcbf7510..96f841253 100644 --- a/toolbox/src/MassMatrix.cpp +++ b/toolbox/src/MassMatrix.cpp @@ -9,153 +9,151 @@ #include #include -// TODO: remove wbt nesting; -namespace wbt { - - const std::string MassMatrix::ClassName = "MassMatrix"; - - const unsigned MassMatrix::INPUT_IDX_BASE_POSE = 0; - const unsigned MassMatrix::INPUT_IDX_JOINTCONF = 1; - const unsigned MassMatrix::OUTPUT_IDX_MASS_MAT = 0; - - MassMatrix::MassMatrix() - : m_massMatrix(nullptr) - {} - - bool MassMatrix::configureSizeAndPorts(BlockInformation* blockInfo) - { - // Memory allocation / Saving data not allowed here - - if (!WBBlock::configureSizeAndPorts(blockInfo)) { - return false; - } - - // INPUTS - // ====== - // - // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) - // 2) Joints position (1xDoFs vector) - // - - // Number of inputs - if (!blockInfo->setNumberOfInputPorts(2)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } - - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - - // Size and type - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); - success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - - blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); - blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - - if (!success) { - Log::getSingleton().error("Failed to configure input ports."); - return false; - } - - // OUTPUTS - // ======= - // - // 1) Matrix epresenting the mass matrix (DoFs+6)x(DoFs+6) - // - - // Number of outputs - if (!blockInfo->setNumberOfOutputPorts(1)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } - - // Size and type - success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_MASS_MAT, dofs + 6, dofs + 6); - blockInfo->setOutputPortType(OUTPUT_IDX_MASS_MAT, PortDataTypeDouble); - - return success; +using namespace wbt; + +const std::string MassMatrix::ClassName = "MassMatrix"; + +const unsigned MassMatrix::INPUT_IDX_BASE_POSE = 0; +const unsigned MassMatrix::INPUT_IDX_JOINTCONF = 1; +const unsigned MassMatrix::OUTPUT_IDX_MASS_MAT = 0; + +MassMatrix::MassMatrix() +: m_massMatrix(nullptr) +{} + +bool MassMatrix::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) { + return false; + } + + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(2)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - bool MassMatrix::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - // Output - m_massMatrix = new iDynTree::MatrixDynSize(6 + dofs, 6 + dofs); - m_massMatrix->zero(); + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - return m_massMatrix; + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; } - bool MassMatrix::terminate(BlockInformation* blockInfo) - { - if (m_massMatrix) { - delete m_massMatrix; - m_massMatrix = nullptr; - } + // OUTPUTS + // ======= + // + // 1) Matrix epresenting the mass matrix (DoFs+6)x(DoFs+6) + // - return WBBlock::terminate(blockInfo); + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; } - bool MassMatrix::output(BlockInformation* blockInfo) - { - using namespace Eigen; - using namespace iDynTree; - typedef Matrix Matrix4dSimulink; - typedef Matrix MatrixXdSimulink; - typedef Matrix MatrixXdiDynTree; - - const auto& model = getRobotInterface()->getKinDynComputations(); - - if (!model) { - Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); - return false; - } - - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== - - unsigned signalWidth; - - // Base pose - Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); - - // OUTPUT - // ====== - - // Compute the Mass Matrix - model->getFreeFloatingMassMatrix(*m_massMatrix); - - // Get the output signal memory location - Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_MASS_MAT); - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - - // Allocate objects for row-major -> col-major conversion - Map massMatrixRowMajor = toEigen(*m_massMatrix); - Map massMatrixColMajor((double*)output.getContiguousBuffer(), - 6 + dofs, 6 + dofs); - - // Forward the buffer to Simulink transforming it to ColMajor - massMatrixColMajor = massMatrixRowMajor; - return true; + // Size and type + success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_MASS_MAT, dofs + 6, dofs + 6); + blockInfo->setOutputPortType(OUTPUT_IDX_MASS_MAT, PortDataTypeDouble); + + return success; +} + +bool MassMatrix::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Output + m_massMatrix = new iDynTree::MatrixDynSize(6 + dofs, 6 + dofs); + m_massMatrix->zero(); + + return m_massMatrix; +} + +bool MassMatrix::terminate(const BlockInformation* blockInfo) +{ + if (m_massMatrix) { + delete m_massMatrix; + m_massMatrix = nullptr; } + + return WBBlock::terminate(blockInfo); +} + +bool MassMatrix::output(const BlockInformation* blockInfo) +{ + using namespace Eigen; + using namespace iDynTree; + typedef Matrix Matrix4dSimulink; + typedef Matrix MatrixXdSimulink; + typedef Matrix MatrixXdiDynTree; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } + + // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS + // ==================================================== + + unsigned signalWidth; + + // Base pose + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); + fromEigen(robotState.m_world_T_base, + Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); + + // Joints position + Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); + robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + + // UPDATE THE ROBOT STATUS + // ======================= + model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + // OUTPUT + // ====== + + // Compute the Mass Matrix + model->getFreeFloatingMassMatrix(*m_massMatrix); + + // Get the output signal memory location + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_MASS_MAT); + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Allocate objects for row-major -> col-major conversion + Map massMatrixRowMajor = toEigen(*m_massMatrix); + Map massMatrixColMajor((double*)output.getContiguousBuffer(), + 6 + dofs, 6 + dofs); + + // Forward the buffer to Simulink transforming it to ColMajor + massMatrixColMajor = massMatrixRowMajor; + return true; } diff --git a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp index 2760a7624..80505a592 100644 --- a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp +++ b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp @@ -13,251 +13,249 @@ #include -namespace wbt { - - const std::string MinimumJerkTrajectoryGenerator::ClassName = "MinimumJerkTrajectoryGenerator"; - - const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_SAMPLE_TIME = 1; - const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_SETTLING_TIME = 2; - const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_OUTPUT_1ST_DERIVATIVE = 3; - const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_OUTPUT_2ND_DERIVATIVE = 4; - const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_INITIAL_VALUE = 5; - const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_EXT_SETTLINGTIME = 6; - const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_RESET_CHANGEST = 7; - - MinimumJerkTrajectoryGenerator::MinimumJerkTrajectoryGenerator() - : m_generator(nullptr) - , m_outputFirstDerivativeIndex(-1) - , m_outputSecondDerivativeIndex(-1) - , m_firstRun(true) - , m_explicitInitialValue(false) - , m_externalSettlingTime(false) - , m_resetOnSettlingTimeChange(false) - , m_initialValues(nullptr) - , m_reference(nullptr) - {} - - unsigned MinimumJerkTrajectoryGenerator::numberOfParameters() { return 7; } - - bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation* blockInfo) - { - // PARAMETERS - // ========== - // - // 1) Sample time (double) - // 2) Settling time (double) - // 3) Enable the output of 1st derivative (bool) - // 4) Enable the output of 2nd derivative (bool) - // 5) Enable the input with the initial conditions (bool) - // 6) Enable the input with the external settling time (bool) - // 7) Reset the trajectory generator when settling time changes (bool) - // - - bool outputFirstDerivative; - bool outputSecondDerivative; - bool explicitInitialValue; - bool externalSettlingTime; - bool ok = true; - - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, outputFirstDerivative); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, outputSecondDerivative); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, explicitInitialValue); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, externalSettlingTime); - - if (!ok) { - Log::getSingleton().error("Failed to get input parameters."); - return false; - } +using namespace wbt; + +const std::string MinimumJerkTrajectoryGenerator::ClassName = "MinimumJerkTrajectoryGenerator"; + +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_SAMPLE_TIME = 1; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_SETTLING_TIME = 2; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_OUTPUT_1ST_DERIVATIVE = 3; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_OUTPUT_2ND_DERIVATIVE = 4; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_INITIAL_VALUE = 5; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_EXT_SETTLINGTIME = 6; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_RESET_CHANGEST = 7; + +MinimumJerkTrajectoryGenerator::MinimumJerkTrajectoryGenerator() +: m_generator(nullptr) +, m_outputFirstDerivativeIndex(-1) +, m_outputSecondDerivativeIndex(-1) +, m_firstRun(true) +, m_explicitInitialValue(false) +, m_externalSettlingTime(false) +, m_resetOnSettlingTimeChange(false) +, m_initialValues(nullptr) +, m_reference(nullptr) +{} + +unsigned MinimumJerkTrajectoryGenerator::numberOfParameters() { return 7; } + +bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // PARAMETERS + // ========== + // + // 1) Sample time (double) + // 2) Settling time (double) + // 3) Enable the output of 1st derivative (bool) + // 4) Enable the output of 2nd derivative (bool) + // 5) Enable the input with the initial conditions (bool) + // 6) Enable the input with the external settling time (bool) + // 7) Reset the trajectory generator when settling time changes (bool) + // + + bool outputFirstDerivative; + bool outputSecondDerivative; + bool explicitInitialValue; + bool externalSettlingTime; + bool ok = true; + + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, outputFirstDerivative); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, outputSecondDerivative); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, explicitInitialValue); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, externalSettlingTime); + + if (!ok) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } - int numberOfInputPorts = 1; - numberOfInputPorts += static_cast(explicitInitialValue); - numberOfInputPorts += static_cast(externalSettlingTime); - - // INPUTS - // ====== - // - // 1) The reference signal (1xn) - // 2) The optional initial conditions - // 3) The optional settling time - // - - if(!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { - Log::getSingleton().error("Failed to set input port number."); - return false; - } + int numberOfInputPorts = 1; + numberOfInputPorts += static_cast(explicitInitialValue); + numberOfInputPorts += static_cast(externalSettlingTime); + + // INPUTS + // ====== + // + // 1) The reference signal (1xn) + // 2) The optional initial conditions + // 3) The optional settling time + // + + if(!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { + Log::getSingleton().error("Failed to set input port number."); + return false; + } - blockInfo->setInputPortVectorSize(0, -1); - blockInfo->setInputPortType(0, PortDataTypeDouble); + blockInfo->setInputPortVectorSize(0, -1); + blockInfo->setInputPortType(0, PortDataTypeDouble); - unsigned portIndex = 1; + unsigned portIndex = 1; - if (explicitInitialValue) { - blockInfo->setInputPortVectorSize(portIndex, -1); - blockInfo->setInputPortType(portIndex, PortDataTypeDouble); - portIndex++; - } - - if (externalSettlingTime) { - blockInfo->setInputPortVectorSize(portIndex, 1); - blockInfo->setInputPortType(portIndex, PortDataTypeDouble); - portIndex++; - } + if (explicitInitialValue) { + blockInfo->setInputPortVectorSize(portIndex, -1); + blockInfo->setInputPortType(portIndex, PortDataTypeDouble); + portIndex++; + } - // OUTPUTS - // ======= - // - // 1) The calculated trajectory - // 2) The optional 1st derivative - // 3) The optional 2nd derivative - // - - int numberOfOutputPorts = 1; - numberOfOutputPorts += static_cast(outputFirstDerivative); - numberOfOutputPorts += static_cast(outputSecondDerivative); - - if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { - Log::getSingleton().error("Failed to set output port number."); - return false; - } + if (externalSettlingTime) { + blockInfo->setInputPortVectorSize(portIndex, 1); + blockInfo->setInputPortType(portIndex, PortDataTypeDouble); + portIndex++; + } - for (int i = 0; i < numberOfOutputPorts; ++i) { - blockInfo->setOutputPortVectorSize(i, -1); - blockInfo->setOutputPortType(i, PortDataTypeDouble); - } + // OUTPUTS + // ======= + // + // 1) The calculated trajectory + // 2) The optional 1st derivative + // 3) The optional 2nd derivative + // + + int numberOfOutputPorts = 1; + numberOfOutputPorts += static_cast(outputFirstDerivative); + numberOfOutputPorts += static_cast(outputSecondDerivative); + + if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { + Log::getSingleton().error("Failed to set output port number."); + return false; + } - return true; + for (int i = 0; i < numberOfOutputPorts; ++i) { + blockInfo->setOutputPortVectorSize(i, -1); + blockInfo->setOutputPortType(i, PortDataTypeDouble); } - bool MinimumJerkTrajectoryGenerator::initialize(BlockInformation* blockInfo) - { - // Get the additional parameters - bool outputFirstDerivative; - bool outputSecondDerivative; - bool ok = true; - - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, - outputFirstDerivative); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, - outputSecondDerivative); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, m_explicitInitialValue); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, m_externalSettlingTime); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_RESET_CHANGEST, - m_resetOnSettlingTimeChange); - - if (!ok) { - Log::getSingleton().error("Failed to get input parameters."); - return false; - } + return true; +} - if (outputFirstDerivative) { - m_outputFirstDerivativeIndex = 1; - } +bool MinimumJerkTrajectoryGenerator::initialize(const BlockInformation* blockInfo) +{ + // Get the additional parameters + bool outputFirstDerivative; + bool outputSecondDerivative; + bool ok = true; + + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, + outputFirstDerivative); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, + outputSecondDerivative); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, m_explicitInitialValue); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, m_externalSettlingTime); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_RESET_CHANGEST, + m_resetOnSettlingTimeChange); + + if (!ok) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } - if (outputSecondDerivative) { - m_outputSecondDerivativeIndex = outputFirstDerivative ? 2 : 1; - } + if (outputFirstDerivative) { + m_outputFirstDerivativeIndex = 1; + } - double sampleTime; - double settlingTime; + if (outputSecondDerivative) { + m_outputSecondDerivativeIndex = outputFirstDerivative ? 2 : 1; + } - ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_SAMPLE_TIME, sampleTime); - ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_SETTLING_TIME, settlingTime); + double sampleTime; + double settlingTime; - unsigned signalSize = blockInfo->getInputPortWidth(0); + ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_SAMPLE_TIME, sampleTime); + ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_SETTLING_TIME, settlingTime); - m_generator = new iCub::ctrl::minJerkTrajGen(signalSize, sampleTime, settlingTime); - m_previousSettlingTime = settlingTime; - m_initialValues = new yarp::sig::Vector(signalSize); - m_reference = new yarp::sig::Vector(signalSize); + unsigned signalSize = blockInfo->getInputPortWidth(0); - if (!m_generator || !m_initialValues || !m_reference) { - Log::getSingleton().error("Could not allocate memory for trajectory generator."); - return false; - } + m_generator = new iCub::ctrl::minJerkTrajGen(signalSize, sampleTime, settlingTime); + m_previousSettlingTime = settlingTime; + m_initialValues = new yarp::sig::Vector(signalSize); + m_reference = new yarp::sig::Vector(signalSize); - m_firstRun = true; - return true; + if (!m_generator || !m_initialValues || !m_reference) { + Log::getSingleton().error("Could not allocate memory for trajectory generator."); + return false; } - bool MinimumJerkTrajectoryGenerator::terminate(BlockInformation* blockInfo) - { - if (m_generator) { - delete m_generator; - m_generator = nullptr; - } - if (m_initialValues) { - delete m_initialValues; - m_initialValues = nullptr; - } - if (m_reference) { - delete m_reference; - m_reference = nullptr; - } - return true; + m_firstRun = true; + return true; +} + +bool MinimumJerkTrajectoryGenerator::terminate(const BlockInformation* blockInfo) +{ + if (m_generator) { + delete m_generator; + m_generator = nullptr; } + if (m_initialValues) { + delete m_initialValues; + m_initialValues = nullptr; + } + if (m_reference) { + delete m_reference; + m_reference = nullptr; + } + return true; +} - bool MinimumJerkTrajectoryGenerator::output(BlockInformation* blockInfo) - { - if (!m_generator) return false; +bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) +{ + if (!m_generator) return false; - if (m_externalSettlingTime) { - unsigned portIndex = 1; - if (m_explicitInitialValue) portIndex++; - Signal externalTimePort = blockInfo->getInputPortSignal(portIndex); - double externalTime = externalTimePort.get(0).doubleData(); + if (m_externalSettlingTime) { + unsigned portIndex = 1; + if (m_explicitInitialValue) portIndex++; + Signal externalTimePort = blockInfo->getInputPortSignal(portIndex); + double externalTime = externalTimePort.get(0).doubleData(); - if (std::abs(m_previousSettlingTime - externalTime) > 1e-5) { - m_previousSettlingTime = externalTime; + if (std::abs(m_previousSettlingTime - externalTime) > 1e-5) { + m_previousSettlingTime = externalTime; - m_generator->setT(externalTime); - if (m_resetOnSettlingTimeChange && !m_firstRun) { - m_generator->init(m_generator->getPos()); - } + m_generator->setT(externalTime); + if (m_resetOnSettlingTimeChange && !m_firstRun) { + m_generator->init(m_generator->getPos()); } } + } - if (m_firstRun) { - m_firstRun = false; - Signal initialValues; - unsigned portIndex = 0; - if (m_explicitInitialValue) { - portIndex = 1; - } - initialValues = blockInfo->getInputPortSignal(portIndex); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(portIndex); ++i) { - (*m_initialValues)[i] = initialValues.get(i).doubleData(); - } - m_generator->init(*m_initialValues); + if (m_firstRun) { + m_firstRun = false; + Signal initialValues; + unsigned portIndex = 0; + if (m_explicitInitialValue) { + portIndex = 1; + } + initialValues = blockInfo->getInputPortSignal(portIndex); + for (unsigned i = 0; i < blockInfo->getInputPortWidth(portIndex); ++i) { + (*m_initialValues)[i] = initialValues.get(i).doubleData(); } + m_generator->init(*m_initialValues); + } - Signal references = blockInfo->getInputPortSignal(0); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - (*m_reference)[i] = references.get(i).doubleData(); - } - m_generator->computeNextValues(*m_reference); + Signal references = blockInfo->getInputPortSignal(0); + for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { + (*m_reference)[i] = references.get(i).doubleData(); + } + m_generator->computeNextValues(*m_reference); - const yarp::sig::Vector &signal = m_generator->getPos(); - Signal output = blockInfo->getOutputPortSignal(0); - output.setBuffer(signal.data(), signal.size()); + const yarp::sig::Vector &signal = m_generator->getPos(); + Signal output = blockInfo->getOutputPortSignal(0); + output.setBuffer(signal.data(), signal.size()); // for (unsigned i = 0; i < blockInfo->getOutputPortWidth(0); ++i) { // output.set(i, signal[i]); // } - //note: index of the port is not known a priori. - //I should save it in the initialization phase - if (m_outputFirstDerivativeIndex != -1) { - const yarp::sig::Vector &derivative = m_generator->getVel(); - Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputFirstDerivativeIndex); - derivativeOutput.setBuffer(derivative.data(), blockInfo->getOutputPortWidth(m_outputFirstDerivativeIndex)); - } - - if (m_outputSecondDerivativeIndex != -1) { - const yarp::sig::Vector &derivative = m_generator->getAcc(); - Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputSecondDerivativeIndex); - derivativeOutput.setBuffer(derivative.data(), blockInfo->getOutputPortWidth(m_outputSecondDerivativeIndex)); - } + //note: index of the port is not known a priori. + //I should save it in the initialization phase + if (m_outputFirstDerivativeIndex != -1) { + const yarp::sig::Vector &derivative = m_generator->getVel(); + Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputFirstDerivativeIndex); + derivativeOutput.setBuffer(derivative.data(), blockInfo->getOutputPortWidth(m_outputFirstDerivativeIndex)); + } - return true; + if (m_outputSecondDerivativeIndex != -1) { + const yarp::sig::Vector &derivative = m_generator->getAcc(); + Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputSecondDerivativeIndex); + derivativeOutput.setBuffer(derivative.data(), blockInfo->getOutputPortWidth(m_outputSecondDerivativeIndex)); } + return true; } diff --git a/toolbox/src/ModelPartitioner.cpp b/toolbox/src/ModelPartitioner.cpp index bedee09c9..2c54458bd 100644 --- a/toolbox/src/ModelPartitioner.cpp +++ b/toolbox/src/ModelPartitioner.cpp @@ -6,192 +6,176 @@ #include "Signal.h" #include "Log.h" -namespace wbt { +using namespace wbt; - const std::string ModelPartitioner::ClassName = "ModelPartitioner"; +const std::string ModelPartitioner::ClassName = "ModelPartitioner"; - unsigned ModelPartitioner::numberOfParameters() - { - return 1; - } +unsigned ModelPartitioner::numberOfParameters() +{ + return 1; +} - bool ModelPartitioner::configureSizeAndPorts(BlockInformation* blockInfo) - { - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; +bool ModelPartitioner::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - // PARAMETERS - // ========== - // - // 1) Boolean specifying if yarp2WBI (true) or WBI2yarp (false) - // + // PARAMETERS + // ========== + // + // 1) Boolean specifying if yarp2WBI (true) or WBI2yarp (false) + // - bool yarp2WBI; - if (!blockInfo->getBooleanParameterAtIndex(1, yarp2WBI)) { - Log::getSingleton().error("Failed to get input parameters."); - return false; - } + bool yarp2WBI; + if (!blockInfo->getBooleanParameterAtIndex(1, yarp2WBI)) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } - // TODO: update to new logic - // INPUTS - // ====== - // - // yarp2WBI - // -------- - // - // 1) Map of joints / control boards (1 x nJoints) - // 2) Vector containing the full set of robot's joints (1 x nJoints) - // - // WBI2yarp - // -------- - // - // 1) Map of joints / control boards (1 x nJoints) - // 2) Torso Control Board Signals - // 3) Head Control Board Signals - // 4) Left Arm Control Board Signals - // 5) Right Arm Control Board Signals - // 6) Left Leg Control Board Signals - // 7) Right Leg Control Board Signals - // - - bool ok; - unsigned numberOfInputs; - unsigned controlBoardsNumber = getConfiguration().getControlBoardsNames().size(); - - if (yarp2WBI) { - numberOfInputs = 1; - ok = blockInfo->setNumberOfInputPorts(numberOfInputs); - blockInfo->setInputPortVectorSize(0, getConfiguration().getNumberOfDoFs()); - } - else { - numberOfInputs = controlBoardsNumber; - ok = blockInfo->setNumberOfInputPorts(numberOfInputs); - // Set the size as dynamic - for (unsigned i = 0; i < numberOfInputs; ++i) { - blockInfo->setInputPortType(i, PortDataTypeDouble); - } + // INPUTS + // ====== + // + // yarp2WBI + // -------- + // + // 1) Vector containing the data vector (1 x DoFs) + // + // WBI2yarp + // -------- + // + // n signals) The n ControlBoards configured from the config block + // + + bool ok; + unsigned numberOfInputs; + unsigned controlBoardsNumber = getConfiguration().getControlBoardsNames().size(); + + if (yarp2WBI) { + numberOfInputs = 1; + ok = blockInfo->setNumberOfInputPorts(numberOfInputs); + blockInfo->setInputPortVectorSize(0, getConfiguration().getNumberOfDoFs()); + } + else { + numberOfInputs = controlBoardsNumber; + ok = blockInfo->setNumberOfInputPorts(numberOfInputs); + // Set the size as dynamic + for (unsigned i = 0; i < numberOfInputs; ++i) { + blockInfo->setInputPortType(i, PortDataTypeDouble); } + } - if (!ok) { - Log::getSingleton().error("Failed to set input port number."); - return false; + if (!ok) { + Log::getSingleton().error("Failed to set input port number."); + return false; + } + + // OUTPUTS + // ======= + // + // yarp2WBI + // -------- + // + // n signals) The n ControlBoards configured from the config block + // + // WBI2yarp + // -------- + // + // 1) Vector containing the data vector (1 x DoFs) + // + + unsigned numberOfOutputs; + + if (yarp2WBI) { + numberOfOutputs = controlBoardsNumber; + ok = blockInfo->setNumberOfOutputPorts(numberOfOutputs); + // Set the size as dynamic + for (unsigned i = 0; i < numberOfOutputs; ++i) { + blockInfo->setOutputPortVectorSize(i, -1); + blockInfo->setOutputPortType(i, PortDataTypeDouble); } + } + else { + numberOfOutputs = 1; + ok = blockInfo->setNumberOfOutputPorts(numberOfOutputs); + blockInfo->setOutputPortVectorSize(0, getConfiguration().getNumberOfDoFs()); + blockInfo->setOutputPortType(0, PortDataTypeDouble); + } + if (!ok) { + Log::getSingleton().error("Failed to set output port number."); + return false; + } - // TODO: update - // OUTPUTS - // ======= - // - // yarp2WBI - // -------- - // - // 1) Torso Control Board Signals - // 2) Head Control Board Signals - // 3) Left Arm Control Board Signals - // 4) Right Arm Control Board Signals - // 5) Left Leg Control Board Signals - // 6) Right Leg Control Board Signals - // - // WBI2yarp - // -------- - // - // 1) Vector containing the full set of robot's joints (1 x nJoints) - // - - unsigned numberOfOutputs; - - if (yarp2WBI) { - numberOfOutputs = controlBoardsNumber; - ok = blockInfo->setNumberOfOutputPorts(numberOfOutputs); - // Set the size as dynamic - for (unsigned i = 0; i < numberOfOutputs; ++i) { - blockInfo->setOutputPortVectorSize(i, -1); - blockInfo->setOutputPortType(i, PortDataTypeDouble); - } - } - else { - numberOfOutputs = 1; - ok = blockInfo->setNumberOfOutputPorts(numberOfOutputs); - blockInfo->setOutputPortVectorSize(0, getConfiguration().getNumberOfDoFs()); - blockInfo->setOutputPortType(0, PortDataTypeDouble); - } + return true; +} - if (!ok) { - Log::getSingleton().error("Failed to set output port number."); - return false; - } +bool ModelPartitioner::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - return true; + if (!blockInfo->getBooleanParameterAtIndex(1, m_yarp2WBI)) { + Log::getSingleton().error("Failed to get input parameters."); + return false; } - bool ModelPartitioner::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; + m_jointsMapString = getRobotInterface()->getJointsMapString(); - if (!blockInfo->getBooleanParameterAtIndex(1, m_yarp2WBI)) { - Log::getSingleton().error("Failed to get input parameters."); - return false; - } + if (!m_jointsMapString) { + return false; + } - m_jointsMapString = getRobotInterface()->getJointsMapString(); + return true; +} - if (!m_jointsMapString) { - return false; - } +bool ModelPartitioner::terminate(const BlockInformation* blockInfo) +{ + return WBBlock::terminate(blockInfo); +} - return true; - } +bool ModelPartitioner::output(const BlockInformation* blockInfo) +{ + if (m_yarp2WBI) { + // Input + Signal jointListSignal = blockInfo->getInputPortSignal(0); - bool ModelPartitioner::terminate(BlockInformation* blockInfo) - { - return WBBlock::terminate(blockInfo); - } + // Outputs + Signal ithControlBoardSignal; - bool ModelPartitioner::output(BlockInformation* blockInfo) - { - if (m_yarp2WBI) { - // Input - Signal jointListSignal = blockInfo->getInputPortSignal(0); - - // Outputs - Signal ithControlBoardSignal; - - for (unsigned ithJoint = 0; ithJoint < blockInfo->getInputPortWidth(0); ++ithJoint) { - // Get the ControlBoard number the ith joint belongs, and its index into the CB itself - std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; - const auto& mappedJointInfos = m_jointsMapString->at(ithJointName); - cb_idx controlBoardOfJoint = mappedJointInfos.first; - jointIdx_yarp yarpIndexOfJoint = mappedJointInfos.second; - - // Get the data to forward - Data value = jointListSignal.get(ithJoint); - - // Forward the value to the correct output / output index - ithControlBoardSignal = blockInfo->getOutputPortSignal(controlBoardOfJoint); - ithControlBoardSignal.set(yarpIndexOfJoint, value.doubleData()); - } + for (unsigned ithJoint = 0; ithJoint < blockInfo->getInputPortWidth(0); ++ithJoint) { + // Get the ControlBoard number the ith joint belongs, and its index into the CB itself + std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; + const auto& mappedJointInfos = m_jointsMapString->at(ithJointName); + cb_idx controlBoardOfJoint = mappedJointInfos.first; + jointIdx_yarp yarpIndexOfJoint = mappedJointInfos.second; + + // Get the data to forward + Data value = jointListSignal.get(ithJoint); + + // Forward the value to the correct output / output index + ithControlBoardSignal = blockInfo->getOutputPortSignal(controlBoardOfJoint); + ithControlBoardSignal.set(yarpIndexOfJoint, value.doubleData()); } - else { - // Inputs - Signal ithControlBoardSignal; - - // Output - Signal jointListSignal = blockInfo->getOutputPortSignal(0); - - for (unsigned ithJoint = 0; ithJoint < blockInfo->getOutputPortWidth(0); ++ithJoint) { - // Get the ControlBoard number the ith joint belongs, and its index into the CB itself - std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; - const auto& mappedJointInfos = m_jointsMapString->at(ithJointName); - cb_idx controlBoardOfJoint = mappedJointInfos.first; - jointIdx_yarp yarpIndexOfJoint = mappedJointInfos.second; - - // Get the data to forward - ithControlBoardSignal = blockInfo->getInputPortSignal(controlBoardOfJoint); - Data value = ithControlBoardSignal.get(ithJoint); - - // Forward the value to the correct output index - jointListSignal.set(yarpIndexOfJoint, value.doubleData()); - } + } + else { + // Inputs + Signal ithControlBoardSignal; + + // Output + Signal jointListSignal = blockInfo->getOutputPortSignal(0); + + for (unsigned ithJoint = 0; ithJoint < blockInfo->getOutputPortWidth(0); ++ithJoint) { + // Get the ControlBoard number the ith joint belongs, and its index into the CB itself + std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; + const auto& mappedJointInfos = m_jointsMapString->at(ithJointName); + cb_idx controlBoardOfJoint = mappedJointInfos.first; + jointIdx_yarp yarpIndexOfJoint = mappedJointInfos.second; + + // Get the data to forward + ithControlBoardSignal = blockInfo->getInputPortSignal(controlBoardOfJoint); + Data value = ithControlBoardSignal.get(ithJoint); + + // Forward the value to the correct output index + jointListSignal.set(yarpIndexOfJoint, value.doubleData()); } - return true; } + return true; } diff --git a/toolbox/src/RealTimeSynchronizer.cpp b/toolbox/src/RealTimeSynchronizer.cpp index 752674d44..62701a38c 100644 --- a/toolbox/src/RealTimeSynchronizer.cpp +++ b/toolbox/src/RealTimeSynchronizer.cpp @@ -4,87 +4,86 @@ #include -namespace wbt { +using namespace wbt; - const std::string RealTimeSynchronizer::ClassName = "RealTimeSynchronizer"; +const std::string RealTimeSynchronizer::ClassName = "RealTimeSynchronizer"; - const unsigned RealTimeSynchronizer::PARAM_PERIOD = 1; // Period +const unsigned RealTimeSynchronizer::PARAM_PERIOD = 1; // Period - RealTimeSynchronizer::RealTimeSynchronizer() - : m_period(0.01) - , m_initialTime(0) - , m_counter(0) - {} +RealTimeSynchronizer::RealTimeSynchronizer() +: m_period(0.01) +, m_initialTime(0) +, m_counter(0) +{} - unsigned RealTimeSynchronizer::numberOfParameters() { return 1; } +unsigned RealTimeSynchronizer::numberOfParameters() { return 1; } - bool RealTimeSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) - { - // INPUTS - // ====== - // - // No inputs - // +bool RealTimeSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUTS + // ====== + // + // No inputs + // - if (!blockInfo->setNumberOfInputPorts(0)) { - Log::getSingleton().error("Failed to set input port number to 0."); - return false; - } - - // OUTPUTS - // ======= - // - // No outputs - // + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; + } - if (!blockInfo->setNumberOfOutputPorts(0)) { - Log::getSingleton().error("Failed to set output port number."); - return false; - } + // OUTPUTS + // ======= + // + // No outputs + // - return true; + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to set output port number."); + return false; } - bool RealTimeSynchronizer::initialize(BlockInformation* blockInfo) - { - if (!blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period)) { - Log::getSingleton().error("Failed to get input parametes."); - return false; - } - - if (m_period < 0) { - Log::getSingleton().error("Period must be greater than 0."); - return false; - } + return true; +} - m_counter = 0; - return true; +bool RealTimeSynchronizer::initialize(const BlockInformation* blockInfo) +{ + if (!blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period)) { + Log::getSingleton().error("Failed to get input parametes."); + return false; } - bool RealTimeSynchronizer::terminate(BlockInformation* blockInfo) - { - return true; + if (m_period < 0) { + Log::getSingleton().error("Period must be greater than 0."); + return false; } - bool RealTimeSynchronizer::output(BlockInformation* blockInfo) - { - if (m_counter == 0) { - m_initialTime = yarp::os::Time::now(); - } + m_counter = 0; + return true; +} - //read current time - double currentTime = yarp::os::Time::now() - m_initialTime; - double desiredTime = m_counter* m_period; +bool RealTimeSynchronizer::terminate(const BlockInformation* blockInfo) +{ + return true; +} - double sleepPeriod = desiredTime - currentTime; +bool RealTimeSynchronizer::output(const BlockInformation* blockInfo) +{ + if (m_counter == 0) { + m_initialTime = yarp::os::Time::now(); + } - //sleep for the remaining time - if (sleepPeriod > 0) { - yarp::os::Time::delay(sleepPeriod); - } + //read current time + double currentTime = yarp::os::Time::now() - m_initialTime; + double desiredTime = m_counter* m_period; - m_counter++; + double sleepPeriod = desiredTime - currentTime; - return true; + //sleep for the remaining time + if (sleepPeriod > 0) { + yarp::os::Time::delay(sleepPeriod); } + + m_counter++; + + return true; } diff --git a/toolbox/src/SetLowLevelPID.cpp b/toolbox/src/SetLowLevelPID.cpp index 9448b4eb0..b2d9772c4 100644 --- a/toolbox/src/SetLowLevelPID.cpp +++ b/toolbox/src/SetLowLevelPID.cpp @@ -6,235 +6,234 @@ #include #include -namespace wbt { +using namespace wbt; - const std::string SetLowLevelPID::ClassName = "SetLowLevelPID"; +const std::string SetLowLevelPID::ClassName = "SetLowLevelPID"; - unsigned SetLowLevelPID::numberOfParameters() - { - return WBBlock::numberOfParameters() - + 1 // WBTPIDConfig object - + 1; // Control type - } +unsigned SetLowLevelPID::numberOfParameters() +{ + return WBBlock::numberOfParameters() + + 1 // WBTPIDConfig object + + 1; // Control type +} - bool SetLowLevelPID::configureSizeAndPorts(BlockInformation* blockInfo) - { - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; +bool SetLowLevelPID::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - // INPUTS - // ====== - // - // No inputs - // + // INPUTS + // ====== + // + // No inputs + // - if (!blockInfo->setNumberOfInputPorts(0)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } - // OUTPUTS - // ======= - // - // No outputs - // + // OUTPUTS + // ======= + // + // No outputs + // - if (!blockInfo->setNumberOfOutputPorts(0)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } + + return true; +} - return true; +bool SetLowLevelPID::readWBTPidConfigObject(const BlockInformation* blockInfo) +{ + AnyStruct s; + if (!blockInfo->getStructAtIndex(WBBlock::numberOfParameters() + 1, s)) { + Log::getSingleton().error("Failed to retrieve the struct with parameters."); + return false; } - bool SetLowLevelPID::readWBTPidConfigObject(BlockInformation* blockInfo) - { - AnyStruct s; - if (!blockInfo->getStructAtIndex(WBBlock::numberOfParameters() + 1, s)) { - Log::getSingleton().error("Failed to retrieve the struct with parameters."); - return false; - } + // Check the existence of all the fields + try { + s.at("P"); + s.at("I"); + s.at("D"); + s.at("jointList"); + } + catch (const std::out_of_range& e) { + Log::getSingleton().error("Cannot retrieve one or more parameter from parameter's struct."); + return false; + } - // Check the existence of all the fields - try { - s.at("P"); - s.at("I"); - s.at("D"); - s.at("jointList"); - } - catch (const std::out_of_range& e) { - Log::getSingleton().error("Cannot retrieve one or more parameter from parameter's struct."); - return false; - } + // Proportional gains + std::vector Pvector; + if (!s["P"]->asVectorDouble(Pvector)) { + Log::getSingleton().error("Cannot retrieve vector from P parameter."); + return false; + } - // Proportional gains - std::vector Pvector; - if (!s["P"]->asVectorDouble(Pvector)) { - Log::getSingleton().error("Cannot retrieve vector from P parameter."); - return false; - } + // Integral gains + std::vector Ivector; + if (!s["I"]->asVectorDouble(Ivector)) { + Log::getSingleton().error("Cannot retrieve vector from I parameter."); + return false; + } - // Integral gains - std::vector Ivector; - if (!s["I"]->asVectorDouble(Ivector)) { - Log::getSingleton().error("Cannot retrieve vector from I parameter."); - return false; - } + // Derivative gains + std::vector Dvector; + if (!s["D"]->asVectorDouble(Dvector)) { + Log::getSingleton().error("Cannot retrieve vector from D parameter."); + return false; + } - // Derivative gains - std::vector Dvector; - if (!s["D"]->asVectorDouble(Dvector)) { - Log::getSingleton().error("Cannot retrieve vector from D parameter."); - return false; - } + // Considered joint names + AnyCell jointPidsCell; + if (!s["jointList"]->asAnyCell(jointPidsCell)) { + Log::getSingleton().error("Cannot retrieve string from jointList parameter."); + return false; + } - // Considered joint names - AnyCell jointPidsCell; - if (!s["jointList"]->asAnyCell(jointPidsCell)) { - Log::getSingleton().error("Cannot retrieve string from jointList parameter."); + // From AnyCell to vector + std::vector jointNamesFromParameters; + for (auto cell: jointPidsCell) { + std::string joint; + if (!cell->asString(joint)) { + Log::getSingleton().error("Failed to convert jointList from cell to strings."); return false; } + jointNamesFromParameters.push_back(joint); + } - // From AnyCell to vector - std::vector jointNamesFromParameters; - for (auto cell: jointPidsCell) { - std::string joint; - if (!cell->asString(joint)) { - Log::getSingleton().error("Failed to convert jointList from cell to strings."); - return false; - } - jointNamesFromParameters.push_back(joint); - } - - assert(Pvector.size() == Ivector.size() == Dvector.size() == jointNamesFromParameters.size()); - - // Store this data into a private member map - for (unsigned i = 0; i < jointNamesFromParameters.size(); ++i) { - // Check the processed joint is actually a controlledJoint - const auto& controlledJoints = getConfiguration().getControlledJoints(); - auto findElement = std::find(std::begin(controlledJoints), - std::end(controlledJoints), - jointNamesFromParameters[i]); - if (findElement != std::end(controlledJoints)) { - m_pidJointsFromParameters[jointNamesFromParameters[i]] = {Pvector[i], Ivector[i], Dvector[i]}; - } - else { - Log::getSingleton().warning("Attempted to set PID of joint " + jointNamesFromParameters[i] + - " non currently controlled. Skipping it."); - } + assert(Pvector.size() == Ivector.size() == Dvector.size() == jointNamesFromParameters.size()); + // Store this data into a private member map + for (unsigned i = 0; i < jointNamesFromParameters.size(); ++i) { + // Check the processed joint is actually a controlledJoint + const auto& controlledJoints = getConfiguration().getControlledJoints(); + auto findElement = std::find(std::begin(controlledJoints), + std::end(controlledJoints), + jointNamesFromParameters[i]); + if (findElement != std::end(controlledJoints)) { + m_pidJointsFromParameters[jointNamesFromParameters[i]] = {Pvector[i], Ivector[i], Dvector[i]}; } - - if (m_pidJointsFromParameters.size() != jointNamesFromParameters.size()) { - Log::getSingleton().warning("PID have been passed only for a subset of the controlled joints."); + else { + Log::getSingleton().warning("Attempted to set PID of joint " + jointNamesFromParameters[i] + + " non currently controlled. Skipping it."); } - return true; } - bool SetLowLevelPID::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; - - // Reading the control type - std::string controlType; - if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 2, controlType)) { - Log::getSingleton().error("Could not read control type parameter."); - return false; - } + if (m_pidJointsFromParameters.size() != jointNamesFromParameters.size()) { + Log::getSingleton().warning("PID have been passed only for a subset of the controlled joints."); + } - if (controlType == "Position") { - m_controlType = yarp::dev::VOCAB_PIDTYPE_POSITION; - } - else if (controlType == "Torque") { - m_controlType = yarp::dev::VOCAB_PIDTYPE_TORQUE; - } - else { - Log::getSingleton().error("Control type not recognized."); - return false; - } + return true; +} - // Reading the WBTPIDConfig matlab class - if (!readWBTPidConfigObject(blockInfo)) { - Log::getSingleton().error("Failed to parse the WBTPIDConfig object."); - return 1; - } +bool SetLowLevelPID::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - // Retain the RemoteControlBoardRemapper - if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { - Log::getSingleton().error("Failed to retain the control board remapper."); - return false; - } + // Reading the control type + std::string controlType; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 2, controlType)) { + Log::getSingleton().error("Could not read control type parameter."); + return false; + } - const unsigned& dofs = getConfiguration().getNumberOfDoFs(); + if (controlType == "Position") { + m_controlType = yarp::dev::VOCAB_PIDTYPE_POSITION; + } + else if (controlType == "Torque") { + m_controlType = yarp::dev::VOCAB_PIDTYPE_TORQUE; + } + else { + Log::getSingleton().error("Control type not recognized."); + return false; + } - // Initialize the vector size to the number of dofs - m_defaultPidValues.resize(dofs); + // Reading the WBTPIDConfig matlab class + if (!readWBTPidConfigObject(blockInfo)) { + Log::getSingleton().error("Failed to parse the WBTPIDConfig object."); + return 1; + } - // Get the interface - std::weak_ptr iPidControl; - if (!getRobotInterface()->getInterface(iPidControl)) { - Log::getSingleton().error("Failed to get IPidControl interface."); - return false; - } + // Retain the RemoteControlBoardRemapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to retain the control board remapper."); + return false; + } - // Store the default gains - if (!iPidControl.lock()->getPids(m_controlType, m_defaultPidValues.data())) { - Log::getSingleton().error("Failed to get default data from IPidControl."); - return false; - } + const unsigned& dofs = getConfiguration().getNumberOfDoFs(); - // Initialize the vector of the applied pid gains with the default gains - m_appliedPidValues = m_defaultPidValues; - - // Override the PID with the gains specified as block parameters - for (unsigned i = 0; i < dofs; ++i) { - const std::string jointName = getConfiguration().getControlledJoints()[i]; - // If the pid has been passed, set the new gains - if (m_pidJointsFromParameters.find(jointName) != m_pidJointsFromParameters.end()) { - PidData gains = m_pidJointsFromParameters[jointName]; - m_appliedPidValues[i].setKp(std::get(gains)); - m_appliedPidValues[i].setKi(std::get(gains)); - m_appliedPidValues[i].setKd(std::get(gains)); - } - } + // Initialize the vector size to the number of dofs + m_defaultPidValues.resize(dofs); - // Apply the new pid gains - if (!iPidControl.lock()->setPids(m_controlType, m_appliedPidValues.data())) { - Log::getSingleton().error("Failed to set PID values."); - return false; - } + // Get the interface + std::weak_ptr iPidControl; + if (!getRobotInterface()->getInterface(iPidControl)) { + Log::getSingleton().error("Failed to get IPidControl interface."); + return false; + } - return true; + // Store the default gains + if (!iPidControl.lock()->getPids(m_controlType, m_defaultPidValues.data())) { + Log::getSingleton().error("Failed to get default data from IPidControl."); + return false; } - bool SetLowLevelPID::terminate(BlockInformation* blockInfo) - { - bool ok = true; + // Initialize the vector of the applied pid gains with the default gains + m_appliedPidValues = m_defaultPidValues; - // Get the IPidControl interface - std::weak_ptr iPidControl; - ok = ok & getRobotInterface()->getInterface(iPidControl); - if (!ok) { - Log::getSingleton().error("Failed to get IPidControl interface."); + // Override the PID with the gains specified as block parameters + for (unsigned i = 0; i < dofs; ++i) { + const std::string jointName = getConfiguration().getControlledJoints()[i]; + // If the pid has been passed, set the new gains + if (m_pidJointsFromParameters.find(jointName) != m_pidJointsFromParameters.end()) { + PidData gains = m_pidJointsFromParameters[jointName]; + m_appliedPidValues[i].setKp(std::get(gains)); + m_appliedPidValues[i].setKi(std::get(gains)); + m_appliedPidValues[i].setKd(std::get(gains)); } + } - // Reset default pid gains - ok = ok & iPidControl.lock()->setPids(m_controlType, m_defaultPidValues.data()); - if (!ok) { - Log::getSingleton().error("Failed to set default PID values."); - } + // Apply the new pid gains + if (!iPidControl.lock()->setPids(m_controlType, m_appliedPidValues.data())) { + Log::getSingleton().error("Failed to set PID values."); + return false; + } - // Release the RemoteControlBoardRemapper - ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); - if (!ok) { - Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); - } + return true; +} + +bool SetLowLevelPID::terminate(const BlockInformation* blockInfo) +{ + bool ok = true; + + // Get the IPidControl interface + std::weak_ptr iPidControl; + ok = ok & getRobotInterface()->getInterface(iPidControl); + if (!ok) { + Log::getSingleton().error("Failed to get IPidControl interface."); + } - return ok && WBBlock::terminate(blockInfo); + // Reset default pid gains + ok = ok & iPidControl.lock()->setPids(m_controlType, m_defaultPidValues.data()); + if (!ok) { + Log::getSingleton().error("Failed to set default PID values."); } - bool SetLowLevelPID::output(BlockInformation* blockInfo) - { - return true; + // Release the RemoteControlBoardRemapper + ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); } + + return ok && WBBlock::terminate(blockInfo); +} + +bool SetLowLevelPID::output(const BlockInformation* blockInfo) +{ + return true; } diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 3858a55a5..c739b0e44 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -9,266 +9,265 @@ #define _USE_MATH_DEFINES #include -namespace wbt { +using namespace wbt; - const std::string SetReferences::ClassName = "SetReferences"; +const std::string SetReferences::ClassName = "SetReferences"; - SetReferences::SetReferences() - : m_resetControlMode(true) - {} +SetReferences::SetReferences() +: m_resetControlMode(true) +{} - void rad2deg(std::vector& v) - { - const double rad2deg = 180.0 / (2 * M_PI); - for (auto& element : v) { - element *= rad2deg; - } +void rad2deg(std::vector& v) +{ + const double rad2deg = 180.0 / (2 * M_PI); + for (auto& element : v) { + element *= rad2deg; } +} + +unsigned SetReferences::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} + +bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here - unsigned SetReferences::numberOfParameters() - { - return WBBlock::numberOfParameters() + 1; + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // INPUTS + // ====== + // + // 1) Joint refereces (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) - { - // Memory allocation / Saving data not allowed here + // Size and type + bool success = blockInfo->setInputPortVectorSize(0, dofs); + blockInfo->setInputPortType(0, PortDataTypeDouble); - if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; + } - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + // OUTPUTS + // ======= + // + // No outputs + // - // INPUTS - // ====== - // - // 1) Joint refereces (1xDoFs vector) - // + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } - // Number of inputs - if (!blockInfo->setNumberOfInputPorts(1)) { - Log::getSingleton().error("Failed to configure the number of input ports."); - return false; - } + return true; +} - // Size and type - bool success = blockInfo->setInputPortVectorSize(0, dofs); - blockInfo->setInputPortType(0, PortDataTypeDouble); +bool SetReferences::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - if (!success) { - Log::getSingleton().error("Failed to configure input ports."); - return false; - } + // Reading the control type + std::string controlType; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, + controlType)) { + Log::getSingleton().error("Could not read control type parameter."); + return false; + } - // OUTPUTS - // ======= - // - // No outputs - // + // Initialize the std::vectors + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_controlModes.assign(dofs, VOCAB_CM_UNKNOWN); - if (!blockInfo->setNumberOfOutputPorts(0)) { - Log::getSingleton().error("Failed to configure the number of output ports."); - return false; - } + // IControlMode.h + if (controlType == "Position") { + m_controlModes.assign(dofs, VOCAB_CM_POSITION); + } + else if (controlType == "Position Direct") { + m_controlModes.assign(dofs, VOCAB_CM_POSITION_DIRECT); + } + else if (controlType == "Velocity") { + m_controlModes.assign(dofs, VOCAB_CM_VELOCITY); + } + else if (controlType == "Torque") { + m_controlModes.assign(dofs, VOCAB_CM_TORQUE); + } + else if (controlType == "PWM") { + m_controlModes.assign(dofs, VOCAB_CM_PWM); + } + else if (controlType == "Current") { + m_controlModes.assign(dofs, VOCAB_CM_CURRENT); + } + else { + Log::getSingleton().error("Control Mode not supported."); + return false; + } - return true; + // Retain the ControlBoardRemapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); + return false; } - bool SetReferences::initialize(BlockInformation* blockInfo) - { - if (!WBBlock::initialize(blockInfo)) return false; + m_resetControlMode = true; + return true; +} - // Reading the control type - std::string controlType; - if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, - controlType)) { - Log::getSingleton().error("Could not read control type parameter."); - return false; - } +bool SetReferences::terminate(const BlockInformation* blockInfo) +{ + using namespace yarp::dev; + bool ok = true; - // Initialize the std::vectors - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - m_controlModes.assign(dofs, VOCAB_CM_UNKNOWN); + // Get the interface + std::weak_ptr icmd2; + ok = ok & getRobotInterface()->getInterface(icmd2); + if (!ok) { + Log::getSingleton().error("Failed to get the IControlMode2 interface."); + } - // IControlMode.h - if (controlType == "Position") { - m_controlModes.assign(dofs, VOCAB_CM_POSITION); - } - else if (controlType == "Position Direct") { - m_controlModes.assign(dofs, VOCAB_CM_POSITION_DIRECT); - } - else if (controlType == "Velocity") { - m_controlModes.assign(dofs, VOCAB_CM_VELOCITY); - } - else if (controlType == "Torque") { - m_controlModes.assign(dofs, VOCAB_CM_TORQUE); - } - else if (controlType == "PWM") { - m_controlModes.assign(dofs, VOCAB_CM_PWM); - } - else if (controlType == "Current") { - m_controlModes.assign(dofs, VOCAB_CM_CURRENT); - } - else { - Log::getSingleton().error("Control Mode not supported."); - return false; - } + // Set all the controlledJoints VOCAB_CM_POSITION + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_controlModes.assign(dofs, VOCAB_CM_POSITION); - // Retain the ControlBoardRemapper - if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { - Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); - return false; - } + ok = ok & icmd2.lock()->setControlModes(m_controlModes.data()); + if (!ok) { + Log::getSingleton().error("Failed to set control mode."); + } - m_resetControlMode = true; - return true; + // Release the RemoteControlBoardRemapper + ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); } - bool SetReferences::terminate(BlockInformation* blockInfo) - { - using namespace yarp::dev; - bool ok = true; + return ok && WBBlock::terminate(blockInfo); +} + +bool SetReferences::initializeInitialConditions(const BlockInformation* /*blockInfo*/) +{ + // Simply reset the variable m_resetControlMode. + // It will be read at the first cycle of output. + m_resetControlMode = true; + return true; +} + +bool SetReferences::output(const BlockInformation* blockInfo) +{ + using namespace yarp::dev; + // Set the control mode at the first run + if (m_resetControlMode) { + m_resetControlMode = false; // Get the interface std::weak_ptr icmd2; - ok = ok & getRobotInterface()->getInterface(icmd2); - if (!ok) { + if (!getRobotInterface()->getInterface(icmd2)) { Log::getSingleton().error("Failed to get the IControlMode2 interface."); + return false; } - - // Set all the controlledJoints VOCAB_CM_POSITION - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - m_controlModes.assign(dofs, VOCAB_CM_POSITION); - - ok = ok & icmd2.lock()->setControlModes(m_controlModes.data()); - if (!ok) { + // Set the control mode to all the controlledJoints + if (!icmd2.lock()->setControlModes(m_controlModes.data())) { Log::getSingleton().error("Failed to set control mode."); + return false; } - - // Release the RemoteControlBoardRemapper - ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); - if (!ok) { - Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); - } - - return ok && WBBlock::terminate(blockInfo); } - bool SetReferences::initializeInitialConditions(BlockInformation* /*blockInfo*/) - { - // Simply reset the variable m_resetControlMode. - // It will be read at the first cycle of output. - m_resetControlMode = true; - return true; - } + // Get the signal + Signal references = blockInfo->getInputPortSignal(0); + unsigned signalWidth = blockInfo->getInputPortWidth(0); + std::vector referencesVector = references.getStdVector(signalWidth); - bool SetReferences::output(BlockInformation* blockInfo) - { - using namespace yarp::dev; - - // Set the control mode at the first run - if (m_resetControlMode) { - m_resetControlMode = false; + bool ok = false; + // TODO: here only the first element is checked + switch (m_controlModes.front()) { + case VOCAB_CM_UNKNOWN: + Log::getSingleton().error("Control mode has not been successfully set."); + return false; + break; + case VOCAB_CM_POSITION: { // Get the interface - std::weak_ptr icmd2; - if (!getRobotInterface()->getInterface(icmd2)) { - Log::getSingleton().error("Failed to get the IControlMode2 interface."); + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get IPositionControl interface."); return false; } - // Set the control mode to all the controlledJoints - if (!icmd2.lock()->setControlModes(m_controlModes.data())) { - Log::getSingleton().error("Failed to set control mode."); + // Convert from rad to deg + rad2deg(referencesVector); + // Set the references + ok = interface.lock()->positionMove(referencesVector.data()); + break; + } + case VOCAB_CM_POSITION_DIRECT: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get IPositionDirect interface."); return false; } + // Convert from rad to deg + rad2deg(referencesVector); + // Set the references + ok = interface.lock()->setPositions(referencesVector.data()); + break; } - - // Get the signal - Signal references = blockInfo->getInputPortSignal(0); - unsigned signalWidth = blockInfo->getInputPortWidth(0); - std::vector referencesVector = references.getStdVector(signalWidth); - - bool ok = false; - // TODO: here only the first element is checked - switch (m_controlModes.front()) { - case VOCAB_CM_UNKNOWN: - Log::getSingleton().error("Control mode has not been successfully set."); + case VOCAB_CM_VELOCITY: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get IVelocityControl interface."); return false; - break; - case VOCAB_CM_POSITION: { - // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { - Log::getSingleton().error("Failed to get IPositionControl interface."); - return false; - } - // Convert from rad to deg - rad2deg(referencesVector); - // Set the references - ok = interface.lock()->positionMove(referencesVector.data()); - break; - } - case VOCAB_CM_POSITION_DIRECT: { - // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { - Log::getSingleton().error("Failed to get IPositionDirect interface."); - return false; - } - // Convert from rad to deg - rad2deg(referencesVector); - // Set the references - ok = interface.lock()->setPositions(referencesVector.data()); - break; - } - case VOCAB_CM_VELOCITY: { - // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { - Log::getSingleton().error("Failed to get IVelocityControl interface."); - return false; - } - // Convert from rad to deg - rad2deg(referencesVector); - // Set the references - ok = interface.lock()->velocityMove(referencesVector.data()); - break; } - case VOCAB_CM_TORQUE: { - // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { - Log::getSingleton().error("Failed to get ITorqueControl interface."); - return false; - } - ok = interface.lock()->setRefTorques(referencesVector.data()); - break; - } - case VOCAB_CM_PWM: { - // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { - Log::getSingleton().error("Failed to get IPWMControl interface."); - return false; - } - ok = interface.lock()->setRefDutyCycles(referencesVector.data()); - break; + // Convert from rad to deg + rad2deg(referencesVector); + // Set the references + ok = interface.lock()->velocityMove(referencesVector.data()); + break; + } + case VOCAB_CM_TORQUE: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get ITorqueControl interface."); + return false; } - case VOCAB_CM_CURRENT: { - // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { - Log::getSingleton().error("Failed to get ICurrentControl interface."); - return false; - } - ok = interface.lock()->setRefCurrents(referencesVector.data()); - break; + ok = interface.lock()->setRefTorques(referencesVector.data()); + break; + } + case VOCAB_CM_PWM: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get IPWMControl interface."); + return false; } + ok = interface.lock()->setRefDutyCycles(referencesVector.data()); + break; } - - if (!ok) { - Log::getSingleton().error("Failed to set references."); - return false; + case VOCAB_CM_CURRENT: { + // Get the interface + std::weak_ptr interface; + if (!getRobotInterface()->getInterface(interface)) { + Log::getSingleton().error("Failed to get ICurrentControl interface."); + return false; + } + ok = interface.lock()->setRefCurrents(referencesVector.data()); + break; } + } - return true; + if (!ok) { + Log::getSingleton().error("Failed to set references."); + return false; } + + return true; } diff --git a/toolbox/src/SimulatorSynchronizer.cpp b/toolbox/src/SimulatorSynchronizer.cpp index 741a48ad6..20f43456b 100644 --- a/toolbox/src/SimulatorSynchronizer.cpp +++ b/toolbox/src/SimulatorSynchronizer.cpp @@ -7,140 +7,139 @@ #include #include -namespace wbt { +using namespace wbt; - struct SimulatorSynchronizer::RPCData - { - struct { - std::string clientPortName; - std::string serverPortName; +struct SimulatorSynchronizer::RPCData +{ + struct { + std::string clientPortName; + std::string serverPortName; - unsigned numberOfSteps; - } configuration; + unsigned numberOfSteps; + } configuration; - yarp::os::Port clientPort; - gazebo::ClockServer clockServer; - }; + yarp::os::Port clientPort; + gazebo::ClockServer clockServer; +}; - const std::string SimulatorSynchronizer::ClassName = "SimulatorSynchronizer"; +const std::string SimulatorSynchronizer::ClassName = "SimulatorSynchronizer"; - const unsigned SimulatorSynchronizer::PARAM_PERIOD = 1; - const unsigned SimulatorSynchronizer::PARAM_GZCLK_PORT = 2; - const unsigned SimulatorSynchronizer::PARAM_RPC_PORT = 3; +const unsigned SimulatorSynchronizer::PARAM_PERIOD = 1; +const unsigned SimulatorSynchronizer::PARAM_GZCLK_PORT = 2; +const unsigned SimulatorSynchronizer::PARAM_RPC_PORT = 3; - SimulatorSynchronizer::SimulatorSynchronizer() - : m_period(0.01) - , m_firstRun(true) - , m_rpcData(0) - {} +SimulatorSynchronizer::SimulatorSynchronizer() +: m_period(0.01) +, m_firstRun(true) +, m_rpcData(0) +{} - unsigned SimulatorSynchronizer::numberOfParameters() { return 3; } +unsigned SimulatorSynchronizer::numberOfParameters() { return 3; } - std::vector SimulatorSynchronizer::additionalBlockOptions() - { - return std::vector(1, wbt::BlockOptionPrioritizeOrder); - } - - bool SimulatorSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) - { - // INPUTS - // ====== - // - // No inputs - // +std::vector SimulatorSynchronizer::additionalBlockOptions() +{ + return std::vector(1, wbt::BlockOptionPrioritizeOrder); +} - if (!blockInfo->setNumberOfInputPorts(0)) { - Log::getSingleton().error("Failed to set input port number to 0."); - return false; - } +bool SimulatorSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUTS + // ====== + // + // No inputs + // + + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; + } - // OUTPUT - // ====== - // - // No outputs - // + // OUTPUT + // ====== + // + // No outputs + // - if (!blockInfo->setNumberOfOutputPorts(0)) { - Log::getSingleton().error("Failed to set output port number."); - return false; - } - - return true; + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to set output port number."); + return false; } - bool SimulatorSynchronizer::initialize(BlockInformation* blockInfo) - { - std::string serverPortName; - std::string clientPortName; + return true; +} - bool ok = true; - ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period); - ok = ok & blockInfo->getStringParameterAtIndex(PARAM_GZCLK_PORT, serverPortName); - ok = ok & blockInfo->getStringParameterAtIndex(PARAM_RPC_PORT, clientPortName); +bool SimulatorSynchronizer::initialize(const BlockInformation* blockInfo) +{ + std::string serverPortName; + std::string clientPortName; - if (!ok) { - Log::getSingleton().error("Error reading RPC parameters."); - return false; - } + bool ok = true; + ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period); + ok = ok & blockInfo->getStringParameterAtIndex(PARAM_GZCLK_PORT, serverPortName); + ok = ok & blockInfo->getStringParameterAtIndex(PARAM_RPC_PORT, clientPortName); - yarp::os::Network::init(); - if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork()) { - Log::getSingleton().error("Error initializing Yarp network."); - return false; - } + if (!ok) { + Log::getSingleton().error("Error reading RPC parameters."); + return false; + } - m_rpcData = new struct RPCData(); - if (!m_rpcData) { - Log::getSingleton().error("Error creating RPC data structure."); - return false; - } + yarp::os::Network::init(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork()) { + Log::getSingleton().error("Error initializing Yarp network."); + return false; + } + + m_rpcData = new struct RPCData(); + if (!m_rpcData) { + Log::getSingleton().error("Error creating RPC data structure."); + return false; + } - m_rpcData->configuration.clientPortName = clientPortName; - m_rpcData->configuration.serverPortName = serverPortName; + m_rpcData->configuration.clientPortName = clientPortName; + m_rpcData->configuration.serverPortName = serverPortName; - m_firstRun = true; + m_firstRun = true; - return true; - } + return true; +} - bool SimulatorSynchronizer::terminate(BlockInformation* /*S*/) - { - if (m_rpcData) { - if (m_rpcData->clientPort.isOpen()) { - m_rpcData->clockServer.continueSimulation(); - if (!yarp::os::Network::disconnect(m_rpcData->configuration.clientPortName, - m_rpcData->configuration.serverPortName)) { - Log::getSingleton().error("Error disconnecting from simulator clock server."); - } - m_rpcData->clientPort.close(); +bool SimulatorSynchronizer::terminate(const BlockInformation* /*S*/) +{ + if (m_rpcData) { + if (m_rpcData->clientPort.isOpen()) { + m_rpcData->clockServer.continueSimulation(); + if (!yarp::os::Network::disconnect(m_rpcData->configuration.clientPortName, + m_rpcData->configuration.serverPortName)) { + Log::getSingleton().error("Error disconnecting from simulator clock server."); } - delete m_rpcData; - m_rpcData = nullptr; + m_rpcData->clientPort.close(); } - yarp::os::Network::fini(); - return true; + delete m_rpcData; + m_rpcData = nullptr; } + yarp::os::Network::fini(); + return true; +} - bool SimulatorSynchronizer::output(BlockInformation* /*S*/) - { - if (m_firstRun) { - m_firstRun = false; - - if (!m_rpcData->clientPort.open(m_rpcData->configuration.clientPortName) || - !yarp::os::Network::connect(m_rpcData->configuration.clientPortName, - m_rpcData->configuration.serverPortName)) { - Log::getSingleton().error("Error connecting to simulator clock server."); - return false; - } - - m_rpcData->clockServer.yarp().attachAsClient(m_rpcData->clientPort); +bool SimulatorSynchronizer::output(const BlockInformation* /*S*/) +{ + if (m_firstRun) { + m_firstRun = false; - double stepSize = m_rpcData->clockServer.getStepSize(); - m_rpcData->configuration.numberOfSteps = static_cast(m_period / stepSize); - m_rpcData->clockServer.pauseSimulation(); + if (!m_rpcData->clientPort.open(m_rpcData->configuration.clientPortName) || + !yarp::os::Network::connect(m_rpcData->configuration.clientPortName, + m_rpcData->configuration.serverPortName)) { + Log::getSingleton().error("Error connecting to simulator clock server."); + return false; } - m_rpcData->clockServer.stepSimulationAndWait(m_rpcData->configuration.numberOfSteps); - return true; + m_rpcData->clockServer.yarp().attachAsClient(m_rpcData->clientPort); + + double stepSize = m_rpcData->clockServer.getStepSize(); + m_rpcData->configuration.numberOfSteps = static_cast(m_period / stepSize); + m_rpcData->clockServer.pauseSimulation(); } + m_rpcData->clockServer.stepSimulationAndWait(m_rpcData->configuration.numberOfSteps); + + return true; } diff --git a/toolbox/src/YarpClock.cpp b/toolbox/src/YarpClock.cpp index b504287df..67592823e 100644 --- a/toolbox/src/YarpClock.cpp +++ b/toolbox/src/YarpClock.cpp @@ -7,64 +7,63 @@ #include #include -namespace wbt { +using namespace wbt; - const std::string YarpClock::ClassName = "YarpClock"; +const std::string YarpClock::ClassName = "YarpClock"; - unsigned YarpClock::numberOfParameters() { return 0; } +unsigned YarpClock::numberOfParameters() { return 0; } - bool YarpClock::configureSizeAndPorts(BlockInformation* blockInfo) - { - // INPUTS - // ====== - // - // No inputs - // +bool YarpClock::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUTS + // ====== + // + // No inputs + // - if (!blockInfo->setNumberOfInputPorts(0)) { - Log::getSingleton().error("Failed to set input port number to 0."); - return false; - } - - // OUTPUT - // ====== - // - // 1) The yarp time. In short, it streams yarp::os::Time::now(). - // - - if (!blockInfo->setNumberOfOutputPorts(1)) { - Log::getSingleton().error("Failed to set output port number."); - return false; - } + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; + } - blockInfo->setOutputPortVectorSize(0, 1); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + // OUTPUT + // ====== + // + // 1) The yarp time. In short, it streams yarp::os::Time::now(). + // - return true; + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to set output port number."); + return false; } - bool YarpClock::initialize(BlockInformation* blockInfo) - { - yarp::os::Network::init(); + blockInfo->setOutputPortVectorSize(0, 1); + blockInfo->setOutputPortType(0, PortDataTypeDouble); - if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { - Log::getSingleton().error("YARP server wasn't found active!!"); - return false; - } + return true; +} - return true; - } +bool YarpClock::initialize(const BlockInformation* blockInfo) +{ + yarp::os::Network::init(); - bool YarpClock::terminate(BlockInformation* /*S*/) - { - yarp::os::Network::fini(); - return true; + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; } - bool YarpClock::output(BlockInformation* blockInfo) - { - Signal signal = blockInfo->getOutputPortSignal(0); - signal.set(0, yarp::os::Time::now()); - return true; - } + return true; +} + +bool YarpClock::terminate(const BlockInformation* /*S*/) +{ + yarp::os::Network::fini(); + return true; +} + +bool YarpClock::output(const BlockInformation* blockInfo) +{ + Signal signal = blockInfo->getOutputPortSignal(0); + signal.set(0, yarp::os::Time::now()); + return true; } diff --git a/toolbox/src/YarpRead.cpp b/toolbox/src/YarpRead.cpp index 5542b9a42..683af090f 100644 --- a/toolbox/src/YarpRead.cpp +++ b/toolbox/src/YarpRead.cpp @@ -12,207 +12,206 @@ #include #include -namespace wbt { - - const std::string YarpRead::ClassName = "YarpRead"; - - const unsigned YarpRead::PARAM_IDX_PORTNAME = 1; // port name - const unsigned YarpRead::PARAM_IDX_PORTSIZE = 2; // Size of the port you're reading - const unsigned YarpRead::PARAM_IDX_WAITDATA = 3; // boolean for blocking reading - const unsigned YarpRead::PARAM_IDX_READ_TS = 4; // boolean to stream timestamp - const unsigned YarpRead::PARAM_IDX_AUTOCONNECT = 5; // Autoconnect boolean - const unsigned YarpRead::PARAM_IDX_ERR_NO_PORT = 6; // Error on missing port if autoconnect is on boolean - - YarpRead::YarpRead() - : m_autoconnect(false) - , m_blocking(false) - , m_shouldReadTimestamp(false) - , m_errorOnMissingPort(true) - , m_port(0) - {} - - unsigned YarpRead::numberOfParameters() { return 6; } - - bool YarpRead::configureSizeAndPorts(BlockInformation* blockInfo) - { - // INPUTS - // ====== - // - // No inputs - // - - if (!blockInfo->setNumberOfInputPorts(0)) { - Log::getSingleton().error("Failed to set input port number to 0."); - return false; - } +using namespace wbt; + +const std::string YarpRead::ClassName = "YarpRead"; + +const unsigned YarpRead::PARAM_IDX_PORTNAME = 1; // port name +const unsigned YarpRead::PARAM_IDX_PORTSIZE = 2; // Size of the port you're reading +const unsigned YarpRead::PARAM_IDX_WAITDATA = 3; // boolean for blocking reading +const unsigned YarpRead::PARAM_IDX_READ_TS = 4; // boolean to stream timestamp +const unsigned YarpRead::PARAM_IDX_AUTOCONNECT = 5; // Autoconnect boolean +const unsigned YarpRead::PARAM_IDX_ERR_NO_PORT = 6; // Error on missing port if autoconnect is on boolean + +YarpRead::YarpRead() +: m_autoconnect(false) +, m_blocking(false) +, m_shouldReadTimestamp(false) +, m_errorOnMissingPort(true) +, m_port(0) +{} + +unsigned YarpRead::numberOfParameters() { return 6; } + +bool YarpRead::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUTS + // ====== + // + // No inputs + // + + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; + } - // OUTPUTS - // ======= - // - // 1) Vector with the data read from the port (1 x signalSize) - // 2) Optional: Timestamp read from the port - // 3) Optional: If autoconnect is disabled, this output becomes 1 when data starts arriving - // (and hence it means that the user connected manually the port) - // + // OUTPUTS + // ======= + // + // 1) Vector with the data read from the port (1 x signalSize) + // 2) Optional: Timestamp read from the port + // 3) Optional: If autoconnect is disabled, this output becomes 1 when data starts arriving + // (and hence it means that the user connected manually the port) + // - bool shouldReadTimestamp; - bool autoconnect; - double signalSize; + bool shouldReadTimestamp; + bool autoconnect; + double signalSize; - bool ok = false; + bool ok = false; - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, shouldReadTimestamp); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, autoconnect); - ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_PORTSIZE, signalSize); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, shouldReadTimestamp); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, autoconnect); + ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_PORTSIZE, signalSize); - if (!ok) { - Log::getSingleton().error("Failed to read input parameters."); - return false; - } + if (!ok) { + Log::getSingleton().error("Failed to read input parameters."); + return false; + } - if (signalSize < 0) { - Log::getSingleton().error("Signal size must be non negative."); - return false; - } + if (signalSize < 0) { + Log::getSingleton().error("Signal size must be non negative."); + return false; + } - int numberOfOutputPorts = 1; - numberOfOutputPorts += static_cast(shouldReadTimestamp); //timestamp is the second port - numberOfOutputPorts += static_cast(!autoconnect); //!autoconnect => additional port with 1/0 depending on the connection status + int numberOfOutputPorts = 1; + numberOfOutputPorts += static_cast(shouldReadTimestamp); //timestamp is the second port + numberOfOutputPorts += static_cast(!autoconnect); //!autoconnect => additional port with 1/0 depending on the connection status - if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { - Log::getSingleton().error("Failed to set output port number."); - return false; - } + if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { + Log::getSingleton().error("Failed to set output port number."); + return false; + } - blockInfo->setOutputPortVectorSize(0, static_cast(signalSize)); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + blockInfo->setOutputPortVectorSize(0, static_cast(signalSize)); + blockInfo->setOutputPortType(0, PortDataTypeDouble); - int portIndex = 1; - if (shouldReadTimestamp) { - blockInfo->setOutputPortVectorSize(portIndex, 2); - blockInfo->setOutputPortType(portIndex, PortDataTypeDouble); - portIndex++; - } - if (!autoconnect) { - blockInfo->setOutputPortVectorSize(portIndex, 1); - blockInfo->setOutputPortType(portIndex, PortDataTypeInt8); //use double anyway. Simplifies simulink stuff - portIndex++; - } - return true; + int portIndex = 1; + if (shouldReadTimestamp) { + blockInfo->setOutputPortVectorSize(portIndex, 2); + blockInfo->setOutputPortType(portIndex, PortDataTypeDouble); + portIndex++; } + if (!autoconnect) { + blockInfo->setOutputPortVectorSize(portIndex, 1); + blockInfo->setOutputPortType(portIndex, PortDataTypeInt8); //use double anyway. Simplifies simulink stuff + portIndex++; + } + return true; +} - bool YarpRead::initialize(BlockInformation* blockInfo) - { - using namespace yarp::os; - using namespace yarp::sig; +bool YarpRead::initialize(const BlockInformation* blockInfo) +{ + using namespace yarp::os; + using namespace yarp::sig; - Network::init(); + Network::init(); - if (!Network::initialized() || !Network::checkNetwork(5.0)) { - Log::getSingleton().error("YARP server wasn't found active!!"); - return false; - } + if (!Network::initialized() || !Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; + } - bool ok = true; + bool ok = true; - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, m_shouldReadTimestamp); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_WAITDATA, m_blocking); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, m_errorOnMissingPort); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, m_shouldReadTimestamp); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_WAITDATA, m_blocking); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, m_errorOnMissingPort); - if (!ok) { - Log::getSingleton().error("Failed to read input parameters."); - return false; - } + if (!ok) { + Log::getSingleton().error("Failed to read input parameters."); + return false; + } - std::string portName; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_PORTNAME, portName)) { - Log::getSingleton().error("Cannot retrieve string from port name parameter."); - return false; - } + std::string portName; + if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_PORTNAME, portName)) { + Log::getSingleton().error("Cannot retrieve string from port name parameter."); + return false; + } - std::string sourcePortName; - std::string destinationPortName; + std::string sourcePortName; + std::string destinationPortName; - // Autoconnect: the block opens a temporary input port ..., and it connects to an existing - // port portName (which is streaming data). - if (m_autoconnect) { - sourcePortName = portName; - destinationPortName = "..."; - } - // Manual connection: the block opens an input port portName, and waits a manual connection to an - // output port. - else { - destinationPortName = portName; - } + // Autoconnect: the block opens a temporary input port ..., and it connects to an existing + // port portName (which is streaming data). + if (m_autoconnect) { + sourcePortName = portName; + destinationPortName = "..."; + } + // Manual connection: the block opens an input port portName, and waits a manual connection to an + // output port. + else { + destinationPortName = portName; + } - m_port = new BufferedPort(); + m_port = new BufferedPort(); - if (!m_port || !m_port->open(destinationPortName)) { - Log::getSingleton().error("Error while opening yarp port."); - return false; - } + if (!m_port || !m_port->open(destinationPortName)) { + Log::getSingleton().error("Error while opening yarp port."); + return false; + } - if (m_autoconnect) { - if (!Network::connect(sourcePortName, m_port->getName())) { - Log::getSingleton().warning("Failed to connect " + - sourcePortName + - " to " - + m_port->getName()); - if (m_errorOnMissingPort) { - Log::getSingleton().error("Failed connecting ports."); - return false; - } + if (m_autoconnect) { + if (!Network::connect(sourcePortName, m_port->getName())) { + Log::getSingleton().warning("Failed to connect " + + sourcePortName + + " to " + + m_port->getName()); + if (m_errorOnMissingPort) { + Log::getSingleton().error("Failed connecting ports."); + return false; } } - return true; } + return true; +} - bool YarpRead::terminate(BlockInformation* /*blockInfo*/) - { - if (m_port) { - m_port->close(); - delete m_port; - m_port = nullptr; - } - yarp::os::Network::fini(); - return true; +bool YarpRead::terminate(const BlockInformation* /*blockInfo*/) +{ + if (m_port) { + m_port->close(); + delete m_port; + m_port = nullptr; } + yarp::os::Network::fini(); + return true; +} - bool YarpRead::output(BlockInformation* blockInfo) - { - int timeStampPortIndex = 1; - int connectionStatusPortIndex = 1; +bool YarpRead::output(const BlockInformation* blockInfo) +{ + int timeStampPortIndex = 1; + int connectionStatusPortIndex = 1; - yarp::sig::Vector* v = m_port->read(m_blocking); // Read from the port. Waits until data arrives. + yarp::sig::Vector* v = m_port->read(m_blocking); // Read from the port. Waits until data arrives. - if (v) { - if (m_shouldReadTimestamp) { - connectionStatusPortIndex++; - yarp::os::Stamp timestamp; - m_port->getEnvelope(timestamp); + if (v) { + if (m_shouldReadTimestamp) { + connectionStatusPortIndex++; + yarp::os::Stamp timestamp; + m_port->getEnvelope(timestamp); - Signal pY1 = blockInfo->getOutputPortSignal(timeStampPortIndex); - pY1.set(0, timestamp.getCount()); - pY1.set(1, timestamp.getTime()); - } + Signal pY1 = blockInfo->getOutputPortSignal(timeStampPortIndex); + pY1.set(0, timestamp.getCount()); + pY1.set(1, timestamp.getTime()); + } - Signal signal = blockInfo->getOutputPortSignal(0); + Signal signal = blockInfo->getOutputPortSignal(0); - // Crop the buffer if it exceeds the OutputPortWidth. - signal.setBuffer(v->data(), std::min(blockInfo->getOutputPortWidth(0), (unsigned)v->size())); + // Crop the buffer if it exceeds the OutputPortWidth. + signal.setBuffer(v->data(), std::min(blockInfo->getOutputPortWidth(0), (unsigned)v->size())); - if (!m_autoconnect) { - Signal statusPort = blockInfo->getOutputPortSignal(connectionStatusPortIndex); - statusPort.set(0, 1); //somebody wrote in the port => the port is connected + if (!m_autoconnect) { + Signal statusPort = blockInfo->getOutputPortSignal(connectionStatusPortIndex); + statusPort.set(0, 1); //somebody wrote in the port => the port is connected - //TODO implement a sort of "timeout" paramter - //At the current state this is equal to timeout = inf - //Otherwise we can check the timeout and if nobody sent data in the last X secs - //we set the port to zero again - } + //TODO implement a sort of "timeout" paramter + //At the current state this is equal to timeout = inf + //Otherwise we can check the timeout and if nobody sent data in the last X secs + //we set the port to zero again } - - return true; } + + return true; } diff --git a/toolbox/src/YarpWrite.cpp b/toolbox/src/YarpWrite.cpp index f557eccbb..3deba6dcc 100644 --- a/toolbox/src/YarpWrite.cpp +++ b/toolbox/src/YarpWrite.cpp @@ -9,149 +9,148 @@ #include #include -namespace wbt { - - const std::string YarpWrite::ClassName = "YarpWrite"; - - const unsigned YarpWrite::PARAM_IDX_PORTNAME = 1; // Port name - const unsigned YarpWrite::PARAM_IDX_AUTOCONNECT = 2; // Autoconnect boolean - const unsigned YarpWrite::PARAM_IDX_ERR_NO_PORT = 3; // Error on missing port if autoconnect is true - - YarpWrite::YarpWrite() - : m_autoconnect(false) - , m_errorOnMissingPort(true) - , m_destinationPortName("") - , m_port(nullptr) - {} - - unsigned YarpWrite::numberOfParameters() { return 3; } - - bool YarpWrite::configureSizeAndPorts(BlockInformation* blockInfo) - { - // INPUT - // ===== - // - // 1) The signal to stream to the specified yarp port - // - - if (!blockInfo->setNumberOfInputPorts(1)) { - Log::getSingleton().error("Failed to set input port number to 0."); - return false; - } - blockInfo->setInputPortVectorSize(0, -1); - blockInfo->setInputPortType(0, PortDataTypeDouble); - - // OUTPUT - // ====== - // - // No outputs - // - - if (!blockInfo->setNumberOfOutputPorts(0)) { - Log::getSingleton().error("Failed to set output port number."); - return false; - } - - return true; +using namespace wbt; + +const std::string YarpWrite::ClassName = "YarpWrite"; + +const unsigned YarpWrite::PARAM_IDX_PORTNAME = 1; // Port name +const unsigned YarpWrite::PARAM_IDX_AUTOCONNECT = 2; // Autoconnect boolean +const unsigned YarpWrite::PARAM_IDX_ERR_NO_PORT = 3; // Error on missing port if autoconnect is true + +YarpWrite::YarpWrite() +: m_autoconnect(false) +, m_errorOnMissingPort(true) +, m_destinationPortName("") +, m_port(nullptr) +{} + +unsigned YarpWrite::numberOfParameters() { return 3; } + +bool YarpWrite::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUT + // ===== + // + // 1) The signal to stream to the specified yarp port + // + + if (!blockInfo->setNumberOfInputPorts(1)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; + } + blockInfo->setInputPortVectorSize(0, -1); + blockInfo->setInputPortType(0, PortDataTypeDouble); + + // OUTPUT + // ====== + // + // No outputs + // + + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to set output port number."); + return false; } - bool YarpWrite::initialize(BlockInformation* blockInfo) - { - using namespace yarp::os; - using namespace yarp::sig; + return true; +} - Network::init(); +bool YarpWrite::initialize(const BlockInformation* blockInfo) +{ + using namespace yarp::os; + using namespace yarp::sig; - if (!Network::initialized() || !Network::checkNetwork(5.0)){ - Log::getSingleton().error("YARP server wasn't found active!!"); - return false; - } + Network::init(); - bool ok = true; + if (!Network::initialized() || !Network::checkNetwork(5.0)){ + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; + } - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, - m_errorOnMissingPort); + bool ok = true; - if (!ok) { - Log::getSingleton().error("Failed to read input parameters."); - return false; - } + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); + ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, + m_errorOnMissingPort); - std::string portParameter; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_PORTNAME, portParameter)) { - Log::getSingleton().error("Error reading port name parameter."); - return false; - } - - std::string sourcePortName; + if (!ok) { + Log::getSingleton().error("Failed to read input parameters."); + return false; + } - // Autoconnect: the block opens a temporary output port ..., and it connects to an existing - // port portName (which will receive data). - if (m_autoconnect) { - sourcePortName = "..."; - m_destinationPortName = portParameter; - } - // Manual connection: the block opens an output port portName, and waits a manual connection to an - // input port. - else { - sourcePortName = portParameter; - } + std::string portParameter; + if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_PORTNAME, portParameter)) { + Log::getSingleton().error("Error reading port name parameter."); + return false; + } - m_port = new BufferedPort(); + std::string sourcePortName; - if (!m_port || !m_port->open(sourcePortName)) { - Log::getSingleton().error("Error while opening yarp port."); - return false; - } + // Autoconnect: the block opens a temporary output port ..., and it connects to an existing + // port portName (which will receive data). + if (m_autoconnect) { + sourcePortName = "..."; + m_destinationPortName = portParameter; + } + // Manual connection: the block opens an output port portName, and waits a manual connection to an + // input port. + else { + sourcePortName = portParameter; + } - if (m_autoconnect) { - if (!Network::connect(m_port->getName(), m_destinationPortName)) { - Log::getSingleton().warning("Failed to connect " + - m_port->getName() + - " to " + - m_destinationPortName); - if (m_errorOnMissingPort) { - Log::getSingleton().error("Failed connecting ports."); - return false; - } - } - } + m_port = new BufferedPort(); - // Initialize the size of the internal buffer handled by m_port - yarp::sig::Vector& outputVector = m_port->prepare(); - outputVector.resize(blockInfo->getInputPortWidth(0)); - return true; + if (!m_port || !m_port->open(sourcePortName)) { + Log::getSingleton().error("Error while opening yarp port."); + return false; } - bool YarpWrite::terminate(BlockInformation* /*S*/) - { - if (m_port) { - if (m_autoconnect) { - yarp::os::Network::disconnect(m_port->getName(), m_destinationPortName); + if (m_autoconnect) { + if (!Network::connect(m_port->getName(), m_destinationPortName)) { + Log::getSingleton().warning("Failed to connect " + + m_port->getName() + + " to " + + m_destinationPortName); + if (m_errorOnMissingPort) { + Log::getSingleton().error("Failed connecting ports."); + return false; } - m_port->close(); - delete m_port; - m_port = nullptr; } - yarp::os::Network::fini(); - return true; } - bool YarpWrite::output(BlockInformation* blockInfo) - { - if (!m_port) return false; - yarp::sig::Vector& outputVector = m_port->prepare(); - outputVector.resize(blockInfo->getInputPortWidth(0)); //this should be a no-op - - Signal signal = blockInfo->getInputPortSignal(0); + // Initialize the size of the internal buffer handled by m_port + yarp::sig::Vector& outputVector = m_port->prepare(); + outputVector.resize(blockInfo->getInputPortWidth(0)); + return true; +} - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - outputVector[i] = signal.get(i).doubleData(); +bool YarpWrite::terminate(const BlockInformation* /*S*/) +{ + if (m_port) { + if (m_autoconnect) { + yarp::os::Network::disconnect(m_port->getName(), m_destinationPortName); } + m_port->close(); + delete m_port; + m_port = nullptr; + } + yarp::os::Network::fini(); + return true; +} - m_port->write(); +bool YarpWrite::output(const BlockInformation* blockInfo) +{ + if (!m_port) return false; + yarp::sig::Vector& outputVector = m_port->prepare(); + outputVector.resize(blockInfo->getInputPortWidth(0)); //this should be a no-op - return true; + Signal signal = blockInfo->getInputPortSignal(0); + + for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { + outputVector[i] = signal.get(i).doubleData(); } + + m_port->write(); + + return true; } diff --git a/toolbox/src/base/Block.cpp b/toolbox/src/base/Block.cpp index b7d36396a..fd2e88731 100644 --- a/toolbox/src/base/Block.cpp +++ b/toolbox/src/base/Block.cpp @@ -1,15 +1,17 @@ #include "Block.h" #include "toolbox.h" -wbt::Block::~Block() {} -std::vector wbt::Block::additionalBlockOptions() { return std::vector(); } -void wbt::Block::parameterAtIndexIsTunable(unsigned /*index*/, bool &tunable) { tunable = false; } -bool wbt::Block::checkParameters(wbt::BlockInformation* /*blockInfo*/) { return true; } +using namespace wbt; -unsigned wbt::Block::numberOfDiscreteStates() { return 0; } -unsigned wbt::Block::numberOfContinuousStates() { return 0; } +Block::~Block() {} +std::vector Block::additionalBlockOptions() { return std::vector(); } +void Block::parameterAtIndexIsTunable(unsigned /*index*/, bool &tunable) { tunable = false; } +bool Block::checkParameters(const BlockInformation* /*blockInfo*/) { return true; } -bool wbt::Block::updateDiscreteState(wbt::BlockInformation* /*blockInfo*/) { return true; } -bool wbt::Block::stateDerivative(wbt::BlockInformation* /*blockInfo*/) { return true; } +unsigned Block::numberOfDiscreteStates() { return 0; } +unsigned Block::numberOfContinuousStates() { return 0; } -bool wbt::Block::initializeInitialConditions(wbt::BlockInformation* /*blockInfo*/) { return true; } +bool Block::updateDiscreteState(const BlockInformation* /*blockInfo*/) { return true; } +bool Block::stateDerivative(const BlockInformation* /*blockInfo*/) { return true; } + +bool Block::initializeInitialConditions(const BlockInformation* /*blockInfo*/) { return true; } diff --git a/toolbox/src/base/BlockInformation.cpp b/toolbox/src/base/BlockInformation.cpp index 5fa160eef..5e52eb899 100644 --- a/toolbox/src/base/BlockInformation.cpp +++ b/toolbox/src/base/BlockInformation.cpp @@ -2,5 +2,7 @@ const std::string wbt::BlockOptionPrioritizeOrder = "wbt.BlockOptionPrioritizeOrder"; -wbt::BlockInformation::~BlockInformation() {} -bool wbt::BlockInformation::optionFromKey(const std::string& key, double& option) const { return false; } +bool wbt::BlockInformation::optionFromKey(const std::string& key, double& option) const +{ + return false; +} diff --git a/toolbox/src/base/Signal.cpp b/toolbox/src/base/Signal.cpp index 9d5f6adbc..67e6d6b42 100644 --- a/toolbox/src/base/Signal.cpp +++ b/toolbox/src/base/Signal.cpp @@ -1,324 +1,320 @@ #include "Signal.h" - #include -namespace wbt { +using namespace wbt; - void Signal::initSignalType(wbt::PortDataType type, bool constPort) - { - this->portType = type; - this->isConstPort = constPort; - } +void Signal::initSignalType(wbt::PortDataType type, bool constPort) +{ + this->portType = type; + this->isConstPort = constPort; +} - void Signal::setContiguousBuffer(void* buffer) - { - contiguousData = buffer; - this->isContiguous = true; - } - void Signal::setContiguousBuffer(const void* buffer) - { - contiguousData = const_cast(buffer); - this->isContiguous = true; - } +void Signal::setContiguousBuffer(void* buffer) +{ + contiguousData = buffer; + this->isContiguous = true; +} +void Signal::setContiguousBuffer(const void* buffer) +{ + contiguousData = const_cast(buffer); + this->isContiguous = true; +} - void Signal::setNonContiguousBuffer(void** buffer) - { - nonContiguousData = buffer; - this->isContiguous = false; - } +void Signal::setNonContiguousBuffer(void** buffer) +{ + nonContiguousData = buffer; + this->isContiguous = false; +} - void Signal::setNonContiguousBuffer(const void* const* buffer) - { - nonContiguousData = const_cast(buffer); - this->isContiguous = false; - } +void Signal::setNonContiguousBuffer(const void* const* buffer) +{ + nonContiguousData = const_cast(buffer); + this->isContiguous = false; +} - const Data Signal::get(unsigned index) const - { - Data data; - switch (portType) { - case PortDataTypeDouble: - if (isContiguous) { - data.doubleData((static_cast(contiguousData))[index]); - } - else { - const double* buffer = static_cast(*nonContiguousData); - data.doubleData(static_cast(buffer[index])); - } - break; - case PortDataTypeSingle: - if (isContiguous) { - data.floatData((static_cast(contiguousData))[index]); - } - else { - const float* buffer = static_cast(*nonContiguousData); - data.floatData(static_cast(buffer[index])); - } - break; - case PortDataTypeInt8: - if (isContiguous) { - data.int8Data((static_cast(contiguousData))[index]); - } - else { - const int8_t* buffer = static_cast(*nonContiguousData); - data.int8Data(static_cast(buffer[index])); - } - break; - case PortDataTypeUInt8: - if (isContiguous) { - data.uint8Data((static_cast(contiguousData))[index]); - } - else { - const uint8_t* buffer = static_cast(*nonContiguousData); - data.uint8Data(static_cast(buffer[index])); - } - break; - case PortDataTypeInt16: - if (isContiguous) { - data.int16Data((static_cast(contiguousData))[index]); - } - else { - const int16_t* buffer = static_cast(*nonContiguousData); - data.int16Data(static_cast(buffer[index])); - } - break; - case PortDataTypeUInt16: - if (isContiguous) { - data.uint16Data((static_cast(contiguousData))[index]); - } - else { - const uint16_t* buffer = static_cast(*nonContiguousData); - data.uint16Data(static_cast(buffer[index])); - } - break; - case PortDataTypeInt32: - if (isContiguous) { - data.int32Data((static_cast(contiguousData))[index]); - } - else { - const int32_t* buffer = static_cast(*nonContiguousData); - data.int32Data(static_cast(buffer[index])); - } - break; - case PortDataTypeUInt32: - if (isContiguous) { - data.uint32Data((static_cast(contiguousData))[index]); - } - else { - const uint32_t* buffer = static_cast(*nonContiguousData); - data.uint32Data(static_cast(buffer[index])); - } - break; - case PortDataTypeBoolean: - if (isContiguous) { - data.booleanData((static_cast(contiguousData))[index]); - } - else { - const bool* buffer = static_cast(*nonContiguousData); - data.booleanData(static_cast(buffer[index])); - } - } - return data; +const Data Signal::get(unsigned index) const +{ + Data data; + switch (portType) { + case PortDataTypeDouble: + if (isContiguous) { + data.doubleData((static_cast(contiguousData))[index]); + } + else { + const double* buffer = static_cast(*nonContiguousData); + data.doubleData(static_cast(buffer[index])); + } + break; + case PortDataTypeSingle: + if (isContiguous) { + data.floatData((static_cast(contiguousData))[index]); + } + else { + const float* buffer = static_cast(*nonContiguousData); + data.floatData(static_cast(buffer[index])); + } + break; + case PortDataTypeInt8: + if (isContiguous) { + data.int8Data((static_cast(contiguousData))[index]); + } + else { + const int8_t* buffer = static_cast(*nonContiguousData); + data.int8Data(static_cast(buffer[index])); + } + break; + case PortDataTypeUInt8: + if (isContiguous) { + data.uint8Data((static_cast(contiguousData))[index]); + } + else { + const uint8_t* buffer = static_cast(*nonContiguousData); + data.uint8Data(static_cast(buffer[index])); + } + break; + case PortDataTypeInt16: + if (isContiguous) { + data.int16Data((static_cast(contiguousData))[index]); + } + else { + const int16_t* buffer = static_cast(*nonContiguousData); + data.int16Data(static_cast(buffer[index])); + } + break; + case PortDataTypeUInt16: + if (isContiguous) { + data.uint16Data((static_cast(contiguousData))[index]); + } + else { + const uint16_t* buffer = static_cast(*nonContiguousData); + data.uint16Data(static_cast(buffer[index])); + } + break; + case PortDataTypeInt32: + if (isContiguous) { + data.int32Data((static_cast(contiguousData))[index]); + } + else { + const int32_t* buffer = static_cast(*nonContiguousData); + data.int32Data(static_cast(buffer[index])); + } + break; + case PortDataTypeUInt32: + if (isContiguous) { + data.uint32Data((static_cast(contiguousData))[index]); + } + else { + const uint32_t* buffer = static_cast(*nonContiguousData); + data.uint32Data(static_cast(buffer[index])); + } + break; + case PortDataTypeBoolean: + if (isContiguous) { + data.booleanData((static_cast(contiguousData))[index]); + } + else { + const bool* buffer = static_cast(*nonContiguousData); + data.booleanData(static_cast(buffer[index])); + } } + return data; +} - void* Signal::getContiguousBuffer() - { - if (!isContiguous) return nullptr; - return this->contiguousData; - } +void* Signal::getContiguousBuffer() +{ + if (!isContiguous) return nullptr; + return this->contiguousData; +} - std::vector Signal::getStdVector(unsigned length) const - { - std::vector v(length); - for (unsigned i = 0; i < length; ++i) { - v[i] = get(i).doubleData(); - } - return v; +std::vector Signal::getStdVector(unsigned length) const +{ + std::vector v(length); + for (unsigned i = 0; i < length; ++i) { + v[i] = get(i).doubleData(); } + return v; +} - //the missing are cast - void Signal::set(unsigned index, double data) - { - if (isConstPort) return; +//the missing are cast +void Signal::set(unsigned index, double data) +{ + if (isConstPort) return; - switch (portType) { - case PortDataTypeDouble: - { - double* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeSingle: - { - float* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - default: - break; + switch (portType) { + case PortDataTypeDouble: + { + double* buffer = static_cast(contiguousData); + buffer[index] = data; + break; + } + case PortDataTypeSingle: + { + float* buffer = static_cast(contiguousData); + buffer[index] = data; + break; } + default: + break; } +} - void Signal::setBuffer(const double* data, const unsigned length, unsigned startIndex) - { - if (isConstPort) return; - unsigned dataSize = 0; - const void* address = data; +void Signal::setBuffer(const double* data, const unsigned length, unsigned startIndex) +{ + if (isConstPort) return; + unsigned dataSize = 0; + const void* address = data; - switch (portType) { - case PortDataTypeDouble: - { - dataSize = sizeof(double); - address = static_cast(address) + startIndex; - break; - } - case PortDataTypeSingle: - { - dataSize = sizeof(float); - address = static_cast(address) + startIndex; - break; - } - default: - break; + switch (portType) { + case PortDataTypeDouble: + { + dataSize = sizeof(double); + address = static_cast(address) + startIndex; + break; } + case PortDataTypeSingle: + { + dataSize = sizeof(float); + address = static_cast(address) + startIndex; + break; + } + default: + break; + } - memcpy(contiguousData, address, dataSize * length); + memcpy(contiguousData, address, dataSize * length); - } +} - void Signal::set(unsigned index, int32_t data) - { - //signed integer function - switch (portType) { - case PortDataTypeInt32: - { - int32_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeInt16: - { - int16_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeInt8: - { - int8_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - default: - break; +void Signal::set(unsigned index, int32_t data) +{ + //signed integer function + switch (portType) { + case PortDataTypeInt32: + { + int32_t* buffer = static_cast(contiguousData); + buffer[index] = data; + break; } + case PortDataTypeInt16: + { + int16_t* buffer = static_cast(contiguousData); + buffer[index] = data; + break; + } + case PortDataTypeInt8: + { + int8_t* buffer = static_cast(contiguousData); + buffer[index] = data; + break; + } + default: + break; } +} - void Signal::setBuffer(const int32_t* data, const unsigned length, unsigned startIndex) - { - if (isConstPort) return; - unsigned dataSize = 0; - const void* address = data; +void Signal::setBuffer(const int32_t* data, const unsigned length, unsigned startIndex) +{ + if (isConstPort) return; + unsigned dataSize = 0; + const void* address = data; - switch (portType) { - case PortDataTypeInt32: - { - dataSize = sizeof(int32_t); - address = static_cast(address) + startIndex; - break; - } - case PortDataTypeInt16: - { - dataSize = sizeof(int16_t); - address = static_cast(address) + startIndex; - break; - } - case PortDataTypeInt8: - { - dataSize = sizeof(int8_t); - address = static_cast(address) + startIndex; - break; - } - default: - break; + switch (portType) { + case PortDataTypeInt32: + { + dataSize = sizeof(int32_t); + address = static_cast(address) + startIndex; + break; } - - memcpy(contiguousData, address, dataSize* length); - } - - void Signal::set(unsigned index, uint32_t data) - { - //signed integer function - switch (portType) { - case PortDataTypeUInt32: - { - uint32_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeUInt16: - { - uint16_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeUInt8: - { - uint8_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - default: - break; + case PortDataTypeInt16: + { + dataSize = sizeof(int16_t); + address = static_cast(address) + startIndex; + break; } + case PortDataTypeInt8: + { + dataSize = sizeof(int8_t); + address = static_cast(address) + startIndex; + break; + } + default: + break; } - void Signal::setBuffer(const uint32_t* data, const unsigned length, unsigned startIndex) - { - if (isConstPort) return; - unsigned dataSize = 0; - const void* address = data; + memcpy(contiguousData, address, dataSize* length); +} - switch (portType) { - case PortDataTypeUInt32: - { - dataSize = sizeof(uint32_t); - address = data + startIndex; - break; - } - case PortDataTypeUInt16: - { - dataSize = sizeof(uint16_t); - address = data + startIndex; - break; - } - case PortDataTypeUInt8: - { - dataSize = sizeof(uint8_t); - address = data + startIndex; - break; - } - default: - break; +void Signal::set(unsigned index, uint32_t data) +{ + //signed integer function + switch (portType) { + case PortDataTypeUInt32: + { + uint32_t* buffer = static_cast(contiguousData); + buffer[index] = data; + break; } - - memcpy(contiguousData, address, dataSize* length); + case PortDataTypeUInt16: + { + uint16_t* buffer = static_cast(contiguousData); + buffer[index] = data; + break; + } + case PortDataTypeUInt8: + { + uint8_t* buffer = static_cast(contiguousData); + buffer[index] = data; + break; + } + default: + break; } +} + +void Signal::setBuffer(const uint32_t* data, const unsigned length, unsigned startIndex) +{ + if (isConstPort) return; + unsigned dataSize = 0; + const void* address = data; - void Signal::set(unsigned index, bool data) - { - bool* buffer = static_cast(contiguousData); - buffer[index] = data; + switch (portType) { + case PortDataTypeUInt32: + { + dataSize = sizeof(uint32_t); + address = data + startIndex; + break; + } + case PortDataTypeUInt16: + { + dataSize = sizeof(uint16_t); + address = data + startIndex; + break; + } + case PortDataTypeUInt8: + { + dataSize = sizeof(uint8_t); + address = data + startIndex; + break; + } + default: + break; } - void Signal::setBuffer(const bool* data, const unsigned length, unsigned startIndex) - { - if (isConstPort) return; - unsigned dataSize = sizeof(bool); - const void* address = static_cast(data) + startIndex; + memcpy(contiguousData, address, dataSize* length); +} - memcpy(contiguousData, address, dataSize* length); - } +void Signal::set(unsigned index, bool data) +{ + bool* buffer = static_cast(contiguousData); + buffer[index] = data; +} +void Signal::setBuffer(const bool* data, const unsigned length, unsigned startIndex) +{ + if (isConstPort) return; + unsigned dataSize = sizeof(bool); + const void* address = static_cast(data) + startIndex; + memcpy(contiguousData, address, dataSize* length); } diff --git a/toolbox/src/base/SimulinkBlockInformation.cpp b/toolbox/src/base/SimulinkBlockInformation.cpp index 48ccdde51..2b9688738 100644 --- a/toolbox/src/base/SimulinkBlockInformation.cpp +++ b/toolbox/src/base/SimulinkBlockInformation.cpp @@ -5,191 +5,191 @@ #include #include -namespace wbt { +using namespace wbt; - SimulinkBlockInformation::SimulinkBlockInformation(SimStruct* S) - : simstruct(S) - {} +SimulinkBlockInformation::SimulinkBlockInformation(SimStruct* S) +: simstruct(S) +{} - bool SimulinkBlockInformation::optionFromKey(const std::string& key, double& option) const - { - if (key == wbt::BlockOptionPrioritizeOrder) { - option = SS_OPTION_PLACE_ASAP; - return true; - } - - return false; +bool SimulinkBlockInformation::optionFromKey(const std::string& key, double& option) const +{ + if (key == wbt::BlockOptionPrioritizeOrder) { + option = SS_OPTION_PLACE_ASAP; + return true; } - //Parameters methods - bool SimulinkBlockInformation::getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) - { - const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); - return MxAnyType(blockParam).asString(stringParameter); - } + return false; +} - bool SimulinkBlockInformation::getStructAtIndex(unsigned parameterIndex, AnyStruct& map) - { - const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); - return MxAnyType(blockParam).asAnyStruct(map); - } +//Parameters methods +bool SimulinkBlockInformation::getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asString(stringParameter); +} +bool SimulinkBlockInformation::getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asAnyStruct(map); +} - bool SimulinkBlockInformation::getVectorAtIndex(unsigned parameterIndex, std::vector& vec) - { - const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); - return MxAnyType(blockParam).asVectorDouble(vec); - } - bool SimulinkBlockInformation::getScalarParameterAtIndex(unsigned parameterIndex, double& value) - { - const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); - return MxAnyType(blockParam).asDouble(value); - } +bool SimulinkBlockInformation::getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asVectorDouble(vec); +} - bool SimulinkBlockInformation::getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) - { - const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); - return MxAnyType(blockParam).asBool(value); - } - //Port information methods - bool SimulinkBlockInformation::setNumberOfInputPorts(unsigned numberOfPorts) - { - return ssSetNumInputPorts(simstruct, numberOfPorts); - } +bool SimulinkBlockInformation::getScalarParameterAtIndex(unsigned parameterIndex, double& value) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asDouble(value); +} - bool SimulinkBlockInformation::setNumberOfOutputPorts(unsigned numberOfPorts) - { - return ssSetNumOutputPorts(simstruct, numberOfPorts); - } +bool SimulinkBlockInformation::getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asBool(value); +} - bool SimulinkBlockInformation::setInputPortVectorSize(unsigned portNumber, int portSize) - { - if (portSize == -1) portSize = DYNAMICALLY_SIZED; - return ssSetInputPortVectorDimension(simstruct, portNumber, portSize); - } +//Port information methods +bool SimulinkBlockInformation::setNumberOfInputPorts(unsigned numberOfPorts) +{ + return ssSetNumInputPorts(simstruct, numberOfPorts); +} - bool SimulinkBlockInformation::setInputPortMatrixSize(unsigned portNumber, int rows, int columns) - { - if (rows == -1) rows = DYNAMICALLY_SIZED; - if (columns == -1) columns = DYNAMICALLY_SIZED; - return ssSetInputPortMatrixDimensions(simstruct, portNumber, rows, columns); - } +bool SimulinkBlockInformation::setNumberOfOutputPorts(unsigned numberOfPorts) +{ + return ssSetNumOutputPorts(simstruct, numberOfPorts); +} - bool SimulinkBlockInformation::setOutputPortVectorSize(unsigned portNumber, int portSize) - { - if (portSize == -1) portSize = DYNAMICALLY_SIZED; - return ssSetOutputPortVectorDimension(simstruct, portNumber, portSize); - } +bool SimulinkBlockInformation::setInputPortVectorSize(unsigned portNumber, int portSize) +{ + if (portSize == -1) portSize = DYNAMICALLY_SIZED; + return ssSetInputPortVectorDimension(simstruct, portNumber, portSize); +} - bool SimulinkBlockInformation::setOutputPortMatrixSize(unsigned portNumber, int rows, int columns) - { - if (rows == -1) rows = DYNAMICALLY_SIZED; - if (columns == -1) columns = DYNAMICALLY_SIZED; - return ssSetOutputPortMatrixDimensions(simstruct, portNumber, rows, columns); - } +bool SimulinkBlockInformation::setInputPortMatrixSize(unsigned portNumber, int rows, int columns) +{ + if (rows == -1) rows = DYNAMICALLY_SIZED; + if (columns == -1) columns = DYNAMICALLY_SIZED; + return ssSetInputPortMatrixDimensions(simstruct, portNumber, rows, columns); +} - bool SimulinkBlockInformation::setInputPortType(unsigned portNumber, PortDataType portType) - { - //for now force Direct feedthrough.. If needed create a separate method - ssSetInputPortDirectFeedThrough(simstruct, portNumber, 1); - int matlabDataType = -1; - switch (portType) { - case PortDataTypeDouble: - matlabDataType = SS_DOUBLE; - break; - case PortDataTypeSingle: - matlabDataType = SS_SINGLE; - break; - case PortDataTypeInt8: - matlabDataType = SS_INT8; - break; - case PortDataTypeUInt8: - matlabDataType = SS_UINT8; - break; - case PortDataTypeInt16: - matlabDataType = SS_INT16; - break; - case PortDataTypeUInt16: - matlabDataType = SS_UINT16; - break; - case PortDataTypeInt32: - matlabDataType = SS_INT32; - break; - case PortDataTypeUInt32: - matlabDataType = SS_UINT32; - break; - case PortDataTypeBoolean: - matlabDataType = SS_BOOLEAN; - break; - } - return ssSetInputPortDataType(simstruct, portNumber, matlabDataType); - } +bool SimulinkBlockInformation::setOutputPortVectorSize(unsigned portNumber, int portSize) +{ + if (portSize == -1) portSize = DYNAMICALLY_SIZED; + return ssSetOutputPortVectorDimension(simstruct, portNumber, portSize); +} - bool SimulinkBlockInformation::setOutputPortType(unsigned portNumber, PortDataType portType) - { - int matlabDataType = -1; - switch (portType) { - case PortDataTypeDouble: - matlabDataType = SS_DOUBLE; - break; +bool SimulinkBlockInformation::setOutputPortMatrixSize(unsigned portNumber, int rows, int columns) +{ + if (rows == -1) rows = DYNAMICALLY_SIZED; + if (columns == -1) columns = DYNAMICALLY_SIZED; + return ssSetOutputPortMatrixDimensions(simstruct, portNumber, rows, columns); +} + +bool SimulinkBlockInformation::setInputPortType(unsigned portNumber, PortDataType portType) +{ + //for now force Direct feedthrough.. If needed create a separate method + ssSetInputPortDirectFeedThrough(simstruct, portNumber, 1); + int matlabDataType = -1; + switch (portType) { + case PortDataTypeDouble: + matlabDataType = SS_DOUBLE; + break; case PortDataTypeSingle: - matlabDataType = SS_SINGLE; - break; - case PortDataTypeInt8: - matlabDataType = SS_INT8; - break; - case PortDataTypeUInt8: - matlabDataType = SS_UINT8; - break; - case PortDataTypeInt16: - matlabDataType = SS_INT16; - break; - case PortDataTypeUInt16: - matlabDataType = SS_UINT16; - break; - case PortDataTypeInt32: - matlabDataType = SS_INT32; - break; - case PortDataTypeUInt32: - matlabDataType = SS_UINT32; - break; - case PortDataTypeBoolean: - matlabDataType = SS_BOOLEAN; - break; - } - return ssSetOutputPortDataType(simstruct, portNumber, matlabDataType); - } + matlabDataType = SS_SINGLE; + break; + case PortDataTypeInt8: + matlabDataType = SS_INT8; + break; + case PortDataTypeUInt8: + matlabDataType = SS_UINT8; + break; + case PortDataTypeInt16: + matlabDataType = SS_INT16; + break; + case PortDataTypeUInt16: + matlabDataType = SS_UINT16; + break; + case PortDataTypeInt32: + matlabDataType = SS_INT32; + break; + case PortDataTypeUInt32: + matlabDataType = SS_UINT32; + break; + case PortDataTypeBoolean: + matlabDataType = SS_BOOLEAN; + break; + } + return ssSetInputPortDataType(simstruct, portNumber, matlabDataType); +} - //Port data - unsigned SimulinkBlockInformation::getInputPortWidth(unsigned portNumber) - { - return ssGetInputPortWidth(simstruct, portNumber); - } +bool SimulinkBlockInformation::setOutputPortType(unsigned portNumber, PortDataType portType) +{ + int matlabDataType = -1; + switch (portType) { + case PortDataTypeDouble: + matlabDataType = SS_DOUBLE; + break; + case PortDataTypeSingle: + matlabDataType = SS_SINGLE; + break; + case PortDataTypeInt8: + matlabDataType = SS_INT8; + break; + case PortDataTypeUInt8: + matlabDataType = SS_UINT8; + break; + case PortDataTypeInt16: + matlabDataType = SS_INT16; + break; + case PortDataTypeUInt16: + matlabDataType = SS_UINT16; + break; + case PortDataTypeInt32: + matlabDataType = SS_INT32; + break; + case PortDataTypeUInt32: + matlabDataType = SS_UINT32; + break; + case PortDataTypeBoolean: + matlabDataType = SS_BOOLEAN; + break; + } + return ssSetOutputPortDataType(simstruct, portNumber, matlabDataType); +} - unsigned SimulinkBlockInformation::getOutputPortWidth(unsigned portNumber) - { - return ssGetOutputPortWidth(simstruct, portNumber); - } +//Port data +unsigned SimulinkBlockInformation::getInputPortWidth(unsigned portNumber) const +{ + return ssGetInputPortWidth(simstruct, portNumber); +} - wbt::Signal SimulinkBlockInformation::getInputPortSignal(unsigned portNumber) - { - Signal signal; - InputPtrsType port = ssGetInputPortSignalPtrs(simstruct, portNumber); - bool isConstPort = true; - signal.initSignalType(PortDataTypeDouble, isConstPort); - signal.setNonContiguousBuffer(port); - return signal; - } +unsigned SimulinkBlockInformation::getOutputPortWidth(unsigned portNumber) const +{ + return ssGetOutputPortWidth(simstruct, portNumber); +} + +wbt::Signal SimulinkBlockInformation::getInputPortSignal(unsigned portNumber) const +{ + Signal signal; + InputPtrsType port = ssGetInputPortSignalPtrs(simstruct, portNumber); + bool isConstPort = true; + signal.initSignalType(PortDataTypeDouble, isConstPort); + signal.setNonContiguousBuffer(port); + return signal; +} - wbt::Signal SimulinkBlockInformation::getOutputPortSignal(unsigned portNumber) - { - Signal signal; - bool isConstPort = false; - signal.initSignalType(PortDataTypeDouble, isConstPort); - signal.setContiguousBuffer(ssGetOutputPortSignal(simstruct, portNumber)); - return signal; - } +wbt::Signal SimulinkBlockInformation::getOutputPortSignal(unsigned portNumber) const +{ + Signal signal; + bool isConstPort = false; + signal.initSignalType(PortDataTypeDouble, isConstPort); + signal.setContiguousBuffer(ssGetOutputPortSignal(simstruct, portNumber)); + return signal; } diff --git a/toolbox/src/base/ToolboxSingleton.cpp b/toolbox/src/base/ToolboxSingleton.cpp index a3d7981b5..19042b281 100644 --- a/toolbox/src/base/ToolboxSingleton.cpp +++ b/toolbox/src/base/ToolboxSingleton.cpp @@ -8,115 +8,103 @@ #include #include -// TODO: remove nesting -namespace wbt { +using namespace wbt; - // CONSTRUCTOR / DESTRUCTOR - // ======================== +// CONSTRUCTOR / DESTRUCTOR +// ======================== - ToolboxSingleton::ToolboxSingleton() {} - ToolboxSingleton::~ToolboxSingleton() {} +ToolboxSingleton::ToolboxSingleton() {} +ToolboxSingleton::~ToolboxSingleton() {} - // UTILITIES - // ========= +// UTILITIES +// ========= - int ToolboxSingleton::numberOfDoFs(const std::string& confKey) - { - if (!isKeyValid(confKey)) - return -1; - else - return m_interfaces[confKey]->getConfiguration().getNumberOfDoFs(); - } +int ToolboxSingleton::numberOfDoFs(const std::string& confKey) +{ + if (!isKeyValid(confKey)) + return -1; + else + return m_interfaces[confKey]->getConfiguration().getNumberOfDoFs(); +} - bool ToolboxSingleton::isKeyValid(const std::string& confKey) - { - if (m_interfaces.find(confKey) != m_interfaces.end()) { - if (m_interfaces[confKey]) - return true; - else - return false; - } - else { +bool ToolboxSingleton::isKeyValid(const std::string& confKey) +{ + if (m_interfaces.find(confKey) != m_interfaces.end()) { + if (m_interfaces[confKey]) + return true; + else return false; - } } + else { + return false; + } +} - // GET METHODS - // =========== +// GET METHODS +// =========== - ToolboxSingleton& ToolboxSingleton::sharedInstance() - { - static ToolboxSingleton instance; - return instance; - } +ToolboxSingleton& ToolboxSingleton::sharedInstance() +{ + static ToolboxSingleton instance; + return instance; +} + +const Configuration& ToolboxSingleton::getConfiguration(const std::string& confKey) +{ + return getRobotInterface(confKey)->getConfiguration(); +} - const Configuration& ToolboxSingleton::getConfiguration(const std::string& confKey) - { - return getRobotInterface(confKey)->getConfiguration(); +const std::shared_ptr ToolboxSingleton::getRobotInterface(const std::string& confKey) +{ + if (!isKeyValid(confKey)) { + return nullptr; } - const std::shared_ptr ToolboxSingleton::getRobotInterface(const std::string& confKey) - { - if (!isKeyValid(confKey)) { - return nullptr; - } + return m_interfaces[confKey]; +} - return m_interfaces[confKey]; +const std::shared_ptr ToolboxSingleton::getModel(const std::string& confKey) +{ + if (!isKeyValid(confKey)) { + return nullptr; } - const std::shared_ptr ToolboxSingleton::getModel(const std::string& confKey) - { - if (!isKeyValid(confKey)) { - return nullptr; - } + return m_interfaces[confKey]->getKinDynComputations(); +} - return m_interfaces[confKey]->getKinDynComputations(); - } +// TOOLBOXSINGLETON CONFIGURATION +// ============================== - // TOOLBOXSINGLETON CONFIGURATION - // ============================== +bool ToolboxSingleton::storeConfiguration(const std::string& confKey, const Configuration& config) +{ + if (!config.isValid()) { + return false; + } - bool ToolboxSingleton::storeConfiguration(const std::string& confKey, const Configuration& config) - { - if (!config.isValid()) { - return false; - } - - // Add the new Configuration object and override an existing key if it already exist. - // Note: Simulink doesn't flush memory unless Matlab is closed, and static objects stay in memory. - // This may cause problems if the config block's mask is changed after the first compilation. - if (m_interfaces.find(confKey) == m_interfaces.end()) { - m_interfaces[confKey] = std::make_shared(config); - return static_cast(m_interfaces[confKey]); - } - - if (!(m_interfaces[confKey]->getConfiguration() == config)) { - assert(m_interfaces[confKey]); - - // Check that at top there are 2 instances of the model: - // the private member of RobotInterface and the one returned - // by the model() called by the assert itself - assert(getModel(confKey).use_count() <= 2); - // TODO: and also m_robotDeviceCounter should be max 1 - // - // TODO: domani: quando finisce la simulazione (terminate) dovrei avere - // dentro tutti i Configuration al massimo 1 sia di model che di - // device (e le interfacce tutte chiuse?) - - // Delete the old configuration (calling the destructor for cleaning garbage) - m_interfaces[confKey].reset(); - m_interfaces.erase(confKey); - - // Allocate a new configuration - m_interfaces[confKey] = std::make_shared(config); - return static_cast(m_interfaces[confKey]); - } - - return true; + // Add the new Configuration object and override an existing key if it already exist. + // Note: Simulink doesn't flush memory unless Matlab is closed, and static objects stay in memory. + // This may cause problems if the config block's mask is changed after the first compilation. + if (m_interfaces.find(confKey) == m_interfaces.end()) { + m_interfaces[confKey] = std::make_shared(config); + return static_cast(m_interfaces[confKey]); } - void ToolboxSingleton::eraseConfiguration(const std::string& confKey) - { + if (!(m_interfaces[confKey]->getConfiguration() == config)) { + assert(m_interfaces[confKey]); + + // Delete the old configuration (calling the destructor for cleaning garbage) + m_interfaces[confKey].reset(); m_interfaces.erase(confKey); + + // Allocate a new configuration + m_interfaces[confKey] = std::make_shared(config); + return static_cast(m_interfaces[confKey]); } + + return true; +} + +void ToolboxSingleton::eraseConfiguration(const std::string& confKey) +{ + m_interfaces.erase(confKey); } diff --git a/toolbox/src/base/WBBlock.cpp b/toolbox/src/base/WBBlock.cpp index 27f9913f2..cf49a66de 100644 --- a/toolbox/src/base/WBBlock.cpp +++ b/toolbox/src/base/WBBlock.cpp @@ -30,7 +30,7 @@ iDynTreeRobotState::iDynTreeRobotState(const unsigned& dofs, const std::vector Date: Mon, 13 Nov 2017 08:52:47 +0000 Subject: [PATCH 40/89] Switched to std::array for storing the gravity vector --- toolbox/include/base/Configuration.h | 9 +++++---- toolbox/include/base/WBBlock.h | 3 ++- toolbox/src/base/Configuration.cpp | 6 +++--- toolbox/src/base/WBBlock.cpp | 8 ++++++-- 4 files changed, 16 insertions(+), 10 deletions(-) diff --git a/toolbox/include/base/Configuration.h b/toolbox/include/base/Configuration.h index 192656cdf..9b70142ac 100644 --- a/toolbox/include/base/Configuration.h +++ b/toolbox/include/base/Configuration.h @@ -2,6 +2,7 @@ #define WBT_TOOLBOXCONFIG_H #include +#include #include namespace wbt { @@ -25,7 +26,7 @@ class wbt::Configuration std::string m_localName; ///< Prefix appended to the opened ports std::vector m_controlledJoints; ///< Subset of controlled joints std::vector m_controlBoardsNames; ///< Names of the used ControlBoard names - std::vector m_gravityVector; ///< The gravity vector + std::array m_gravityVector; ///< The gravity vector size_t m_dofs; //< DoFs extracted my m_controlBoardsNames vector public: @@ -50,7 +51,7 @@ class wbt::Configuration std::vector controlledJoints, std::vector controlBoardsNames, std::string localName, - std::vector gravityVector); + std::array gravityVector); /** * Set the name of the robot @@ -88,7 +89,7 @@ class wbt::Configuration * Set the gravity vector * @param gravityVector The gravity vector */ - void setGravityVector(const std::vector& gravityVector); + void setGravityVector(const std::array& gravityVector); // GET METHODS // =========== @@ -133,7 +134,7 @@ class wbt::Configuration * * @return The gravity vector */ - const std::vector& getGravityVector() const; + const std::array& getGravityVector() const; /** * Get the configured number of DoFs diff --git a/toolbox/include/base/WBBlock.h b/toolbox/include/base/WBBlock.h index b3d5cf33c..227795a1c 100644 --- a/toolbox/include/base/WBBlock.h +++ b/toolbox/include/base/WBBlock.h @@ -3,6 +3,7 @@ #include "Block.h" #include +#include #include #include #include @@ -36,7 +37,7 @@ struct iDynTreeRobotState { iDynTreeRobotState() = default; ~iDynTreeRobotState() = default; - iDynTreeRobotState(const unsigned& dofs, const std::vector& gravity); + iDynTreeRobotState(const unsigned& dofs, const std::array& gravity); }; /** diff --git a/toolbox/src/base/Configuration.cpp b/toolbox/src/base/Configuration.cpp index d28857016..a963a3b41 100644 --- a/toolbox/src/base/Configuration.cpp +++ b/toolbox/src/base/Configuration.cpp @@ -14,7 +14,7 @@ void Configuration::setParameters(std::string robotName, std::vector controlledJoints, std::vector controlBoardsNames, std::string localName, - std::vector gravityVector) + std::array gravityVector) { setRobotName(robotName); setUrdfFile(urdfFile); @@ -50,7 +50,7 @@ void Configuration::setLocalName(const std::string& localName) m_localName = localName; } -void Configuration::setGravityVector(const std::vector& gravityVector) +void Configuration::setGravityVector(const std::array& gravityVector) { m_gravityVector = gravityVector; } @@ -83,7 +83,7 @@ const std::string& Configuration::getLocalName() const return m_localName; } -const std::vector& Configuration::getGravityVector() const +const std::array& Configuration::getGravityVector() const { return m_gravityVector; } diff --git a/toolbox/src/base/WBBlock.cpp b/toolbox/src/base/WBBlock.cpp index cf49a66de..0c344e33f 100644 --- a/toolbox/src/base/WBBlock.cpp +++ b/toolbox/src/base/WBBlock.cpp @@ -19,7 +19,7 @@ const unsigned WBBlock::ConfigurationParameterIndex = 1; // Struct from Simulink const unsigned WBBlock::ConfBlockNameParameterIndex = 2; // Absolute name of the block containing the // configuration -iDynTreeRobotState::iDynTreeRobotState(const unsigned& dofs, const std::vector& gravity) +iDynTreeRobotState::iDynTreeRobotState(const unsigned& dofs, const std::array& gravity) : m_gravity(gravity.data(), 3) , m_jointsVelocity(dofs) , m_jointsPosition(dofs) @@ -130,6 +130,10 @@ bool WBBlock::getWBToolboxParameters(Configuration& config, const BlockInformati Log::getSingleton().error("Cannot retrieve vector from GravityVector parameter."); return false; } + std::array gravityArray; + for (auto i : gravityVector) { + gravityArray[i] = i; + } // Create the ToolboxConfig object // =============================== @@ -140,7 +144,7 @@ bool WBBlock::getWBToolboxParameters(Configuration& config, const BlockInformati config.setControlledJoints(controlledJoints); config.setControlBoardsNames(controlBoardsNames); config.setLocalName(localName); - config.setGravityVector(gravityVector); + config.setGravityVector(gravityArray); return true; } From 8c61547526f4bcbfc782c4a18bb5bc0e9b2595cb Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 13 Nov 2017 08:55:21 +0000 Subject: [PATCH 41/89] Switched to std::unique_ptr for PolyDriver member The RemoteControlBoardRemapper is never shared, and it makes sense for RobotInterface being the only one maintaining its ownership --- toolbox/include/base/RobotInterface.h | 2 +- toolbox/src/base/RobotInterface.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/toolbox/include/base/RobotInterface.h b/toolbox/include/base/RobotInterface.h index 83da54cde..1c1fb4ab2 100644 --- a/toolbox/include/base/RobotInterface.h +++ b/toolbox/include/base/RobotInterface.h @@ -86,7 +86,7 @@ struct wbt::YarpDevices class wbt::RobotInterface { private: - std::shared_ptr m_robotDevice; + std::unique_ptr m_robotDevice; std::shared_ptr m_kinDynComp; wbt::YarpDevices m_yarpDevices; diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp index 59633f63b..9c2d859fa 100644 --- a/toolbox/src/base/RobotInterface.cpp +++ b/toolbox/src/base/RobotInterface.cpp @@ -378,7 +378,8 @@ bool RobotInterface::initializeRemoteControlBoardRemapper() remoteCBOpts.put("writeStrict","on"); // Allocate the device driver - m_robotDevice = std::make_shared(); + assert(!m_robotDevice); + m_robotDevice = std::unique_ptr(new yarp::dev::PolyDriver()); if (!m_robotDevice) { return false; From a3c61c55870d6a4aa42bd0887d82fd0759f88944 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 13 Nov 2017 08:56:53 +0000 Subject: [PATCH 42/89] Changed the name of the getDevice template to getInterfaceFromTemplate Before the name was misleading --- toolbox/include/base/RobotInterface.h | 3 ++- toolbox/src/base/RobotInterface.cpp | 22 +++++++++++----------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/toolbox/include/base/RobotInterface.h b/toolbox/include/base/RobotInterface.h index 1c1fb4ab2..6b85789d3 100644 --- a/toolbox/include/base/RobotInterface.h +++ b/toolbox/include/base/RobotInterface.h @@ -132,7 +132,8 @@ class wbt::RobotInterface * @return The dynamic(ally)_cast device * @tparam T The type of the retured device */ - template std::weak_ptr getDevice(std::shared_ptr device); + template + std::weak_ptr getInterfaceFromTemplate(std::shared_ptr device); /** * Creates the map between joints (specified as either names or idyntree indices) and diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp index 9c2d859fa..4e1fccbb0 100644 --- a/toolbox/src/base/RobotInterface.cpp +++ b/toolbox/src/base/RobotInterface.cpp @@ -195,70 +195,70 @@ const std::shared_ptr RobotInterface::getKinDynCom template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iControlMode2); + interface = getInterfaceFromTemplate(m_yarpDevices.iControlMode2); return !interface.expired(); } template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iPositionControl); + interface = getInterfaceFromTemplate(m_yarpDevices.iPositionControl); return !interface.expired(); } template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iPositionDirect); + interface = getInterfaceFromTemplate(m_yarpDevices.iPositionDirect); return !interface.expired(); } template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iVelocityControl); + interface = getInterfaceFromTemplate(m_yarpDevices.iVelocityControl); return !interface.expired(); } template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iTorqueControl); + interface = getInterfaceFromTemplate(m_yarpDevices.iTorqueControl); return !interface.expired(); } template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iPWMControl); + interface = getInterfaceFromTemplate(m_yarpDevices.iPWMControl); return !interface.expired(); } template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iCurrentControl); + interface = getInterfaceFromTemplate(m_yarpDevices.iCurrentControl); return !interface.expired(); } template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iEncoders); + interface = getInterfaceFromTemplate(m_yarpDevices.iEncoders); return !interface.expired(); } template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iControlLimits2); + interface = getInterfaceFromTemplate(m_yarpDevices.iControlLimits2); return !interface.expired(); } template <> bool RobotInterface::getInterface(std::weak_ptr& interface) { - interface = getDevice(m_yarpDevices.iPidControl); + interface = getInterfaceFromTemplate(m_yarpDevices.iPidControl); return !interface.expired(); } @@ -399,7 +399,7 @@ bool RobotInterface::initializeRemoteControlBoardRemapper() // ============= template -std::weak_ptr RobotInterface::getDevice(std::shared_ptr device) +std::weak_ptr RobotInterface::getInterfaceFromTemplate(std::shared_ptr device) { if (!device) { // Blocks which require the RemoteControlBoardRemapper need to retain / release it From 1dd9789559789d12aca826525b8b118ed99a236d Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 13 Nov 2017 09:00:35 +0000 Subject: [PATCH 43/89] Removed Eigen from AnyType and MxAnyType This interface exposes vector and matrix data as std::vector. The C++11 std::vector::data() method might be used from the user to gather the raw buffer for initializing Eigen objects if needed. --- MxAnyType/CMakeLists.txt | 4 ---- MxAnyType/include/AnyType.h | 6 ------ MxAnyType/include/MxAnyType.h | 1 - MxAnyType/src/MxAnyType.cpp | 16 +++++----------- 4 files changed, 5 insertions(+), 22 deletions(-) diff --git a/MxAnyType/CMakeLists.txt b/MxAnyType/CMakeLists.txt index 8d3250642..4faaebad7 100644 --- a/MxAnyType/CMakeLists.txt +++ b/MxAnyType/CMakeLists.txt @@ -32,10 +32,6 @@ if (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") set_target_properties(${VARS_PREFIX} PROPERTIES COMPILE_FLAGS "-fPIC") endif() -# Find Eigen -find_package(Eigen3 REQUIRED) -target_include_directories(${VARS_PREFIX} PUBLIC "${EIGEN3_INCLUDE_DIR}") - # Find Matlab resources find_package(Matlab REQUIRED MX_LIBRARY) # target_include_directories(${VARS_PREFIX} SYSTEM PUBLIC "${Matlab_INCLUDE_DIRS}") // TODO: when committing diff --git a/MxAnyType/include/AnyType.h b/MxAnyType/include/AnyType.h index 1637b38b8..a13d1d03a 100644 --- a/MxAnyType/include/AnyType.h +++ b/MxAnyType/include/AnyType.h @@ -1,7 +1,6 @@ #ifndef ANYTYPE_H #define ANYTYPE_H -#include #include #include #include @@ -15,13 +14,9 @@ typedef std::unordered_map AnyStruct; class AnyType { protected: - // void* m_AnyData; public: - // AnyType() : m_AnyData(nullptr) {}; AnyType() = default; - // virtual void* getAnyDataPtr() const = 0; - virtual ~AnyType() = default; // Integers @@ -60,7 +55,6 @@ class AnyType // virtual bool asMatrixDouble(Eigen::MatrixXd mat) = 0; // Vector - virtual bool asVectorDouble(Eigen::VectorXd& vec) = 0; virtual bool asVectorDouble(std::vector& vec) = 0; }; diff --git a/MxAnyType/include/MxAnyType.h b/MxAnyType/include/MxAnyType.h index 21fc7090f..b8990b741 100644 --- a/MxAnyType/include/MxAnyType.h +++ b/MxAnyType/include/MxAnyType.h @@ -92,7 +92,6 @@ class MxAnyType : public AnyType // VECTOR // ====== - bool asVectorDouble(Eigen::VectorXd& vec) override; bool asVectorDouble(std::vector& vec) override; }; diff --git a/MxAnyType/src/MxAnyType.cpp b/MxAnyType/src/MxAnyType.cpp index 77a2f10f4..55e7fe61c 100644 --- a/MxAnyType/src/MxAnyType.cpp +++ b/MxAnyType/src/MxAnyType.cpp @@ -184,8 +184,9 @@ bool MxAnyType::asAnyCell(AnyCell& cell) // VECTOR // ====== -// TODO tests with other types (uint8) https://it.mathworks.com/help/matlab/apiref/mxgetdata.html (Descriptio) -bool MxAnyType::asVectorDouble(Eigen::VectorXd& vec) +// TODO: +// Tests with other types (uint8) https://it.mathworks.com/help/matlab/apiref/mxgetdata.html (Description) +bool MxAnyType::asVectorDouble(std::vector& vec) { if (!mx) return false; if (!mxIsDouble(mx)) return false; @@ -200,14 +201,7 @@ bool MxAnyType::asVectorDouble(Eigen::VectorXd& vec) double* buffer = mxGetPr(mx); if (!buffer) return false; - vec = Eigen::Map(buffer, md.nElem); - return true; -} - -bool MxAnyType::asVectorDouble(std::vector& vec) -{ - Eigen::VectorXd vecEigen; - if (!asVectorDouble(vecEigen)) return false; - vec.assign(vecEigen.data(), vecEigen.data() + vecEigen.rows() * vecEigen.cols()); + vec.reserve(md.rows * md.cols); + vec.assign(buffer, buffer + md.rows * md.cols); return true; } From d4dd040fc443c44f691e98a119acef03337e9c6d Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 13 Nov 2017 09:32:24 +0000 Subject: [PATCH 44/89] Disabled InverseKinematics block The algorithm used by IK will be ported in another task from iKin to iDynTree --- toolbox/CMakeLists.txt | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 93f3a71b7..007f2d0e5 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -80,11 +80,11 @@ configure_block(BLOCK_NAME "Base" include/base/RobotInterface.h ) -configure_block(BLOCK_NAME "Inverse Kinematics" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/RemoteInverseKinematics.cpp - HEADERS include/RemoteInverseKinematics.h) +# configure_block(BLOCK_NAME "Inverse Kinematics" +# GROUP "Model" +# LIST_PREFIX WBT +# SOURCES src/RemoteInverseKinematics.cpp +# HEADERS include/RemoteInverseKinematics.h) option(WBT_USES_ICUB "Build models which need iCub library (e.g. Minimum Jerk Traj. Generator)" ON) if (WBT_USES_ICUB) @@ -97,17 +97,17 @@ if (WBT_USES_ICUB) SOURCES src/MinimumJerkTrajectoryGenerator.cpp HEADERS include/MinimumJerkTrajectoryGenerator.h) - if (${ICUB_USE_IPOPT}) - find_package(iDynTree REQUIRED) - add_definitions(-DWBT_USES_IPOPT) - configure_block(BLOCK_NAME "Inverse Kinematics" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/InverseKinematics.cpp - HEADERS include/InverseKinematics.h) - - include_directories(SYSTEM ${iKin_INCLUDE_DIRS}) - endif() + # if (${ICUB_USE_IPOPT}) + # find_package(iDynTree REQUIRED) + # add_definitions(-DWBT_USES_IPOPT) + # configure_block(BLOCK_NAME "Inverse Kinematics" + # GROUP "Model" + # LIST_PREFIX WBT + # SOURCES src/InverseKinematics.cpp + # HEADERS include/InverseKinematics.h) + # + # include_directories(SYSTEM ${iKin_INCLUDE_DIRS}) + # endif() include_directories(SYSTEM ${ctrlLib_INCLUDE_DIRS}) endif() From f2f31caaf43fd979448c56e1ee4b533cf5e8fd2c Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 13 Nov 2017 13:16:03 +0000 Subject: [PATCH 45/89] Switched to smart pointers for private members of all the blocks --- toolbox/include/CentroidalMomentum.h | 5 +++- toolbox/include/DotJNu.h | 5 +++- toolbox/include/ForwardKinematics.h | 3 ++ toolbox/include/GetLimits.h | 1 + toolbox/include/InverseDynamics.h | 9 ++++-- toolbox/include/Jacobian.h | 6 ++-- toolbox/include/MassMatrix.h | 5 ++-- .../include/MinimumJerkTrajectoryGenerator.h | 8 +++-- toolbox/include/ModelPartitioner.h | 1 + toolbox/include/SetReferences.h | 1 + toolbox/include/SimulatorSynchronizer.h | 3 +- toolbox/include/YarpClock.h | 3 ++ toolbox/include/YarpRead.h | 4 ++- toolbox/include/YarpWrite.h | 4 ++- toolbox/src/CentroidalMomentum.cpp | 13 ++------ toolbox/src/DotJNu.cpp | 13 ++------ toolbox/src/InverseDynamics.cpp | 30 +++++-------------- toolbox/src/Jacobian.cpp | 19 +++--------- toolbox/src/MassMatrix.cpp | 13 ++------ .../src/MinimumJerkTrajectoryGenerator.cpp | 24 ++++----------- toolbox/src/SimulatorSynchronizer.cpp | 5 +--- toolbox/src/YarpRead.cpp | 5 +--- toolbox/src/YarpWrite.cpp | 5 +--- 23 files changed, 72 insertions(+), 113 deletions(-) diff --git a/toolbox/include/CentroidalMomentum.h b/toolbox/include/CentroidalMomentum.h index 754e4249b..bfb620a60 100644 --- a/toolbox/include/CentroidalMomentum.h +++ b/toolbox/include/CentroidalMomentum.h @@ -2,6 +2,7 @@ #define WBT_CENTROIDALMOMENTUM_H #include "WBBlock.h" +#include namespace wbt { class CentroidalMomentum; @@ -14,7 +15,7 @@ namespace iDynTree { class wbt::CentroidalMomentum : public wbt::WBBlock { private: - iDynTree::SpatialMomentum* m_centroidalMomentum; + std::unique_ptr m_centroidalMomentum; static const unsigned INPUT_IDX_BASE_POSE; static const unsigned INPUT_IDX_JOINTCONF; @@ -24,7 +25,9 @@ class wbt::CentroidalMomentum : public wbt::WBBlock public: static const std::string ClassName; + CentroidalMomentum(); + ~CentroidalMomentum() override = default; bool configureSizeAndPorts(BlockInformation* blockInfo) override; diff --git a/toolbox/include/DotJNu.h b/toolbox/include/DotJNu.h index 9c70fe864..409a185af 100644 --- a/toolbox/include/DotJNu.h +++ b/toolbox/include/DotJNu.h @@ -2,6 +2,7 @@ #define WBT_DOTJDOTQ_H #include "WBBlock.h" +#include #include #include @@ -13,7 +14,7 @@ class wbt::DotJNu : public wbt::WBBlock { private: // Output - iDynTree::Vector6* m_dotJNu; + std::unique_ptr m_dotJNu; // Other variables bool m_frameIsCoM; @@ -27,7 +28,9 @@ class wbt::DotJNu : public wbt::WBBlock public: static const std::string ClassName; + DotJNu(); + ~DotJNu() override = default; unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; diff --git a/toolbox/include/ForwardKinematics.h b/toolbox/include/ForwardKinematics.h index 4a683e761..4cf69167a 100644 --- a/toolbox/include/ForwardKinematics.h +++ b/toolbox/include/ForwardKinematics.h @@ -2,6 +2,7 @@ #define WBT_FORWARDKINEMATICS_H #include "WBBlock.h" +#include #include namespace wbt { @@ -20,7 +21,9 @@ class wbt::ForwardKinematics : public wbt::WBBlock public: static const std::string ClassName; + ForwardKinematics(); + ~ForwardKinematics() override = default; unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; diff --git a/toolbox/include/GetLimits.h b/toolbox/include/GetLimits.h index fcabddeb0..3a0310fe9 100644 --- a/toolbox/include/GetLimits.h +++ b/toolbox/include/GetLimits.h @@ -27,6 +27,7 @@ class wbt::GetLimits : public wbt::WBBlock public: static const std::string ClassName; + GetLimits() = default; ~GetLimits() override = default; diff --git a/toolbox/include/InverseDynamics.h b/toolbox/include/InverseDynamics.h index 63c8e8a3e..afb1d2ca5 100644 --- a/toolbox/include/InverseDynamics.h +++ b/toolbox/include/InverseDynamics.h @@ -2,6 +2,7 @@ #define WBT_INVERSEDYNAMICS_H #include "WBBlock.h" +#include #include namespace wbt { @@ -16,11 +17,11 @@ namespace iDynTree { class wbt::InverseDynamics : public wbt::WBBlock { private: - iDynTree::Vector6* m_baseAcceleration; - iDynTree::VectorDynSize* m_jointsAcceleration; + std::unique_ptr m_baseAcceleration; + std::unique_ptr m_jointsAcceleration; // Output - iDynTree::FreeFloatingGeneralizedTorques* m_torques; + std::unique_ptr m_torques; static const unsigned INPUT_IDX_BASE_POSE; static const unsigned INPUT_IDX_JOINTCONF; @@ -32,7 +33,9 @@ class wbt::InverseDynamics : public wbt::WBBlock public: static const std::string ClassName; + InverseDynamics(); + ~InverseDynamics() override = default; unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; diff --git a/toolbox/include/Jacobian.h b/toolbox/include/Jacobian.h index 5db8f6d95..a38a968c6 100644 --- a/toolbox/include/Jacobian.h +++ b/toolbox/include/Jacobian.h @@ -2,6 +2,7 @@ #define WBT_JACOBIAN_H #include "WBBlock.h" +#include #include namespace wbt { @@ -16,10 +17,10 @@ class wbt::Jacobian : public wbt::WBBlock { private: // Support variables - iDynTree::MatrixDynSize* m_jacobianCOM; + std::unique_ptr m_jacobianCOM; // Output - iDynTree::MatrixDynSize* m_jacobian; + std::unique_ptr m_jacobian; // Other variables bool m_frameIsCoM; @@ -32,6 +33,7 @@ class wbt::Jacobian : public wbt::WBBlock public: static const std::string ClassName; Jacobian(); + ~Jacobian() override = default; unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; diff --git a/toolbox/include/MassMatrix.h b/toolbox/include/MassMatrix.h index 4ebea06b8..cd048c74d 100644 --- a/toolbox/include/MassMatrix.h +++ b/toolbox/include/MassMatrix.h @@ -2,6 +2,7 @@ #define WBT_MASSMATRIX_H #include "WBBlock.h" +#include namespace wbt { class MassMatrix; @@ -14,7 +15,7 @@ namespace iDynTree { class wbt::MassMatrix : public wbt::WBBlock { private: - iDynTree::MatrixDynSize* m_massMatrix; + std::unique_ptr m_massMatrix; static const unsigned INPUT_IDX_BASE_POSE; static const unsigned INPUT_IDX_JOINTCONF; @@ -23,7 +24,7 @@ class wbt::MassMatrix : public wbt::WBBlock public: static const std::string ClassName; MassMatrix(); - ~MassMatrix() = default; + ~MassMatrix() override = default; bool configureSizeAndPorts(BlockInformation* blockInfo) override; diff --git a/toolbox/include/MinimumJerkTrajectoryGenerator.h b/toolbox/include/MinimumJerkTrajectoryGenerator.h index 5d175e7ab..1e0250a25 100644 --- a/toolbox/include/MinimumJerkTrajectoryGenerator.h +++ b/toolbox/include/MinimumJerkTrajectoryGenerator.h @@ -1,4 +1,5 @@ #include "Block.h" +#include #ifndef WBT_MINJERKTRAJGENERATOR_H #define WBT_MINJERKTRAJGENERATOR_H @@ -25,6 +26,7 @@ class wbt::MinimumJerkTrajectoryGenerator : public wbt::Block { static const std::string ClassName; MinimumJerkTrajectoryGenerator(); + ~MinimumJerkTrajectoryGenerator() override = default; unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; @@ -34,7 +36,7 @@ class wbt::MinimumJerkTrajectoryGenerator : public wbt::Block { private: - iCub::ctrl::minJerkTrajGen* m_generator; + std::unique_ptr m_generator; int m_outputFirstDerivativeIndex; int m_outputSecondDerivativeIndex; @@ -45,8 +47,8 @@ class wbt::MinimumJerkTrajectoryGenerator : public wbt::Block { bool m_explicitInitialValue; bool m_externalSettlingTime; bool m_resetOnSettlingTimeChange; - yarp::sig::Vector* m_initialValues; - yarp::sig::Vector* m_reference; + std::unique_ptr m_initialValues; + std::unique_ptr m_reference; static const unsigned PARAM_IDX_SAMPLE_TIME; // Sample Time (double) static const unsigned PARAM_IDX_SETTLING_TIME; // Settling Time (double) diff --git a/toolbox/include/ModelPartitioner.h b/toolbox/include/ModelPartitioner.h index 2a8b5c435..32c57c098 100644 --- a/toolbox/include/ModelPartitioner.h +++ b/toolbox/include/ModelPartitioner.h @@ -19,6 +19,7 @@ class wbt::ModelPartitioner : public wbt::WBBlock public: static const std::string ClassName; + ModelPartitioner() = default; ~ModelPartitioner() override = default; diff --git a/toolbox/include/SetReferences.h b/toolbox/include/SetReferences.h index d599a1410..74b5c4b19 100644 --- a/toolbox/include/SetReferences.h +++ b/toolbox/include/SetReferences.h @@ -19,6 +19,7 @@ class wbt::SetReferences : public wbt::WBBlock static const std::string ClassName; SetReferences(); + ~SetReferences() override = default; unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; diff --git a/toolbox/include/SimulatorSynchronizer.h b/toolbox/include/SimulatorSynchronizer.h index f307c8fc7..7b7793870 100644 --- a/toolbox/include/SimulatorSynchronizer.h +++ b/toolbox/include/SimulatorSynchronizer.h @@ -2,6 +2,7 @@ #define WBT_SIMULATORSYNCHRONIZER_H #include "Block.h" +#include namespace wbt { class SimulatorSynchronizer; @@ -27,7 +28,7 @@ class wbt::SimulatorSynchronizer : public wbt::Block bool m_firstRun; struct RPCData; - RPCData* m_rpcData; + std::unique_ptr m_rpcData; static const unsigned PARAM_PERIOD; // Period static const unsigned PARAM_GZCLK_PORT; // Gazebo clock port diff --git a/toolbox/include/YarpClock.h b/toolbox/include/YarpClock.h index c4857e8e0..430c78838 100644 --- a/toolbox/include/YarpClock.h +++ b/toolbox/include/YarpClock.h @@ -12,6 +12,9 @@ class wbt::YarpClock : public wbt::Block public: static const std::string ClassName; + YarpClock() = default; + ~YarpClock() override = default; + unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; bool initialize(const BlockInformation* blockInfo) override; diff --git a/toolbox/include/YarpRead.h b/toolbox/include/YarpRead.h index 6fed985ad..33d9f6e64 100644 --- a/toolbox/include/YarpRead.h +++ b/toolbox/include/YarpRead.h @@ -2,6 +2,7 @@ #define WBT_YARPREAD_H #include "Block.h" +#include namespace wbt { class YarpRead; @@ -23,6 +24,7 @@ class wbt::YarpRead : public wbt::Block static const std::string ClassName; YarpRead(); + ~YarpRead() override = default; unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; @@ -36,7 +38,7 @@ class wbt::YarpRead : public wbt::Block bool m_shouldReadTimestamp; bool m_errorOnMissingPort; - yarp::os::BufferedPort* m_port; + std::unique_ptr> m_port; static const unsigned PARAM_IDX_PORTNAME; // port name static const unsigned PARAM_IDX_PORTSIZE; // Size of the port you're reading diff --git a/toolbox/include/YarpWrite.h b/toolbox/include/YarpWrite.h index 4d9e8238b..206567dff 100644 --- a/toolbox/include/YarpWrite.h +++ b/toolbox/include/YarpWrite.h @@ -2,6 +2,7 @@ #define WBT_YARPWRITE_H #include "Block.h" +#include namespace wbt { class YarpWrite; @@ -23,6 +24,7 @@ class wbt::YarpWrite : public wbt::Block static const std::string ClassName; YarpWrite(); + ~YarpWrite() override = default; unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; @@ -35,7 +37,7 @@ class wbt::YarpWrite : public wbt::Block bool m_errorOnMissingPort; std::string m_destinationPortName; - yarp::os::BufferedPort* m_port; + std::unique_ptr> m_port; static const unsigned PARAM_IDX_PORTNAME; // Port name static const unsigned PARAM_IDX_AUTOCONNECT; // Autoconnect boolean diff --git a/toolbox/src/CentroidalMomentum.cpp b/toolbox/src/CentroidalMomentum.cpp index 1294c7e19..58484cb9e 100644 --- a/toolbox/src/CentroidalMomentum.cpp +++ b/toolbox/src/CentroidalMomentum.cpp @@ -20,9 +20,7 @@ const unsigned CentroidalMomentum::INPUT_IDX_BASE_VEL = 2; const unsigned CentroidalMomentum::INPUT_IDX_JOINT_VEL = 3; const unsigned CentroidalMomentum::OUTPUT_IDX_CENTRMOM = 0; -CentroidalMomentum::CentroidalMomentum() -: m_centroidalMomentum(nullptr) -{} +CentroidalMomentum::CentroidalMomentum() {} bool CentroidalMomentum::configureSizeAndPorts(BlockInformation* blockInfo) { @@ -90,17 +88,12 @@ bool CentroidalMomentum::initialize(const BlockInformation* blockInfo) // OUTPUT // ====== - m_centroidalMomentum = new iDynTree::SpatialMomentum(); - return m_centroidalMomentum; + m_centroidalMomentum = std::unique_ptr(new iDynTree::SpatialMomentum()); + return static_cast(m_centroidalMomentum); } bool CentroidalMomentum::terminate(const BlockInformation* blockInfo) { - if (m_centroidalMomentum) { - delete m_centroidalMomentum; - m_centroidalMomentum = 0; - } - return WBBlock::terminate(blockInfo); } diff --git a/toolbox/src/DotJNu.cpp b/toolbox/src/DotJNu.cpp index 7c82c2c39..e984d7a96 100644 --- a/toolbox/src/DotJNu.cpp +++ b/toolbox/src/DotJNu.cpp @@ -20,8 +20,7 @@ const unsigned DotJNu::INPUT_IDX_JOINT_VEL = 3; const unsigned DotJNu::OUTPUT_IDX_DOTJ_NU = 0; DotJNu::DotJNu() -: m_dotJNu(nullptr) -, m_frameIsCoM(false) +: m_frameIsCoM(false) , m_frameIndex(iDynTree::FRAME_INVALID_INDEX) {} @@ -127,20 +126,14 @@ bool DotJNu::initialize(const BlockInformation* blockInfo) // OUTPUT // ====== - m_dotJNu = new iDynTree::Vector6(); + m_dotJNu = std::unique_ptr(new iDynTree::Vector6()); m_dotJNu->zero(); - return m_dotJNu; + return static_cast(m_dotJNu); } bool DotJNu::terminate(const BlockInformation* blockInfo) { - // Output - if (m_dotJNu) { - delete m_dotJNu; - m_dotJNu = 0; - } - return WBBlock::terminate(blockInfo); } diff --git a/toolbox/src/InverseDynamics.cpp b/toolbox/src/InverseDynamics.cpp index 5aa635a93..e0303b97e 100644 --- a/toolbox/src/InverseDynamics.cpp +++ b/toolbox/src/InverseDynamics.cpp @@ -22,11 +22,7 @@ const unsigned InverseDynamics::INPUT_IDX_BASE_ACC = 4; const unsigned InverseDynamics::INPUT_IDX_JOINT_ACC = 5; const unsigned InverseDynamics::OUTPUT_IDX_TORQUES = 0; -InverseDynamics::InverseDynamics() -: m_baseAcceleration(nullptr) -, m_jointsAcceleration(nullptr) -, m_torques(nullptr) -{} +InverseDynamics::InverseDynamics() {} unsigned InverseDynamics::numberOfParameters() { @@ -106,34 +102,24 @@ bool InverseDynamics::initialize(const BlockInformation* blockInfo) // OUTPUT / VARIABLES // ================== + using namespace iDynTree; const unsigned dofs = getConfiguration().getNumberOfDoFs(); - m_baseAcceleration = new iDynTree::Vector6(); + m_baseAcceleration = std::unique_ptr(new Vector6()); m_baseAcceleration->zero(); - m_jointsAcceleration = new iDynTree::VectorDynSize(dofs); + m_jointsAcceleration = std::unique_ptr(new VectorDynSize(dofs)); m_jointsAcceleration->zero(); const auto& model = getRobotInterface()->getKinDynComputations()->model(); - m_torques = new iDynTree::FreeFloatingGeneralizedTorques(model); + m_torques = std::unique_ptr(new FreeFloatingGeneralizedTorques(model)); - return m_baseAcceleration && m_jointsAcceleration && m_torques; + return static_cast(m_baseAcceleration) && + static_cast(m_jointsAcceleration) && + static_cast(m_torques); } bool InverseDynamics::terminate(const BlockInformation* blockInfo) { - if (m_baseAcceleration) { - delete m_baseAcceleration; - m_baseAcceleration = nullptr; - } - if (m_jointsAcceleration) { - delete m_jointsAcceleration; - m_jointsAcceleration = nullptr; - } - if (m_torques) { - delete m_torques; - m_torques = nullptr; - } - return WBBlock::terminate(blockInfo); } diff --git a/toolbox/src/Jacobian.cpp b/toolbox/src/Jacobian.cpp index 4901c7ebc..901a0a822 100644 --- a/toolbox/src/Jacobian.cpp +++ b/toolbox/src/Jacobian.cpp @@ -18,9 +18,7 @@ const unsigned Jacobian::INPUT_IDX_JOINTCONF = 1; const unsigned Jacobian::OUTPUT_IDX_FW_FRAME = 0; Jacobian::Jacobian() -: m_jacobianCOM(nullptr) -, m_jacobian(nullptr) -, m_frameIsCoM(false) +: m_frameIsCoM(false) , m_frameIndex(iDynTree::FRAME_INVALID_INDEX) {} @@ -124,27 +122,18 @@ bool Jacobian::initialize(const BlockInformation* blockInfo) const unsigned dofs = getConfiguration().getNumberOfDoFs(); - m_jacobianCOM = new iDynTree::MatrixDynSize(3, 6 + dofs); + m_jacobianCOM = std::unique_ptr(new iDynTree::MatrixDynSize(3, 6 + dofs)); m_jacobianCOM->zero(); // Output - m_jacobian = new iDynTree::MatrixDynSize(6, 6 + dofs); + m_jacobian = std::unique_ptr(new iDynTree::MatrixDynSize(6, 6 + dofs)); m_jacobian->zero(); - return m_jacobianCOM && m_jacobian; + return static_cast(m_jacobianCOM) && static_cast(m_jacobian); } bool Jacobian::terminate(const BlockInformation* blockInfo) { - if (m_jacobianCOM) { - delete m_jacobianCOM; - m_jacobianCOM = nullptr; - } - if (m_jacobian) { - delete m_jacobian; - m_jacobian = nullptr; - } - return WBBlock::terminate(blockInfo); } diff --git a/toolbox/src/MassMatrix.cpp b/toolbox/src/MassMatrix.cpp index 96f841253..a14a9f78c 100644 --- a/toolbox/src/MassMatrix.cpp +++ b/toolbox/src/MassMatrix.cpp @@ -17,9 +17,7 @@ const unsigned MassMatrix::INPUT_IDX_BASE_POSE = 0; const unsigned MassMatrix::INPUT_IDX_JOINTCONF = 1; const unsigned MassMatrix::OUTPUT_IDX_MASS_MAT = 0; -MassMatrix::MassMatrix() -: m_massMatrix(nullptr) -{} +MassMatrix::MassMatrix() {} bool MassMatrix::configureSizeAndPorts(BlockInformation* blockInfo) { @@ -83,19 +81,14 @@ bool MassMatrix::initialize(const BlockInformation* blockInfo) const unsigned dofs = getConfiguration().getNumberOfDoFs(); // Output - m_massMatrix = new iDynTree::MatrixDynSize(6 + dofs, 6 + dofs); + m_massMatrix = std::unique_ptr(new iDynTree::MatrixDynSize(6 + dofs, 6 + dofs)); m_massMatrix->zero(); - return m_massMatrix; + return static_cast(m_massMatrix); } bool MassMatrix::terminate(const BlockInformation* blockInfo) { - if (m_massMatrix) { - delete m_massMatrix; - m_massMatrix = nullptr; - } - return WBBlock::terminate(blockInfo); } diff --git a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp index 80505a592..d550be5fd 100644 --- a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp +++ b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp @@ -26,15 +26,12 @@ const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_EXT_SETTLINGTIME = 6; const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_RESET_CHANGEST = 7; MinimumJerkTrajectoryGenerator::MinimumJerkTrajectoryGenerator() -: m_generator(nullptr) -, m_outputFirstDerivativeIndex(-1) +: m_outputFirstDerivativeIndex(-1) , m_outputSecondDerivativeIndex(-1) , m_firstRun(true) , m_explicitInitialValue(false) , m_externalSettlingTime(false) , m_resetOnSettlingTimeChange(false) -, m_initialValues(nullptr) -, m_reference(nullptr) {} unsigned MinimumJerkTrajectoryGenerator::numberOfParameters() { return 7; } @@ -165,10 +162,11 @@ bool MinimumJerkTrajectoryGenerator::initialize(const BlockInformation* blockInf unsigned signalSize = blockInfo->getInputPortWidth(0); - m_generator = new iCub::ctrl::minJerkTrajGen(signalSize, sampleTime, settlingTime); + m_generator = std::unique_ptr + (new iCub::ctrl::minJerkTrajGen(signalSize, sampleTime, settlingTime)); m_previousSettlingTime = settlingTime; - m_initialValues = new yarp::sig::Vector(signalSize); - m_reference = new yarp::sig::Vector(signalSize); + m_initialValues = std::unique_ptr(new yarp::sig::Vector(signalSize)); + m_reference = std::unique_ptr(new yarp::sig::Vector(signalSize)); if (!m_generator || !m_initialValues || !m_reference) { Log::getSingleton().error("Could not allocate memory for trajectory generator."); @@ -181,18 +179,6 @@ bool MinimumJerkTrajectoryGenerator::initialize(const BlockInformation* blockInf bool MinimumJerkTrajectoryGenerator::terminate(const BlockInformation* blockInfo) { - if (m_generator) { - delete m_generator; - m_generator = nullptr; - } - if (m_initialValues) { - delete m_initialValues; - m_initialValues = nullptr; - } - if (m_reference) { - delete m_reference; - m_reference = nullptr; - } return true; } diff --git a/toolbox/src/SimulatorSynchronizer.cpp b/toolbox/src/SimulatorSynchronizer.cpp index 20f43456b..2f0c834fa 100644 --- a/toolbox/src/SimulatorSynchronizer.cpp +++ b/toolbox/src/SimulatorSynchronizer.cpp @@ -31,7 +31,6 @@ const unsigned SimulatorSynchronizer::PARAM_RPC_PORT = 3; SimulatorSynchronizer::SimulatorSynchronizer() : m_period(0.01) , m_firstRun(true) -, m_rpcData(0) {} unsigned SimulatorSynchronizer::numberOfParameters() { return 3; } @@ -89,7 +88,7 @@ bool SimulatorSynchronizer::initialize(const BlockInformation* blockInfo) return false; } - m_rpcData = new struct RPCData(); + m_rpcData = std::unique_ptr(new struct RPCData()); if (!m_rpcData) { Log::getSingleton().error("Error creating RPC data structure."); return false; @@ -114,8 +113,6 @@ bool SimulatorSynchronizer::terminate(const BlockInformation* /*S*/) } m_rpcData->clientPort.close(); } - delete m_rpcData; - m_rpcData = nullptr; } yarp::os::Network::fini(); return true; diff --git a/toolbox/src/YarpRead.cpp b/toolbox/src/YarpRead.cpp index 683af090f..023fb7701 100644 --- a/toolbox/src/YarpRead.cpp +++ b/toolbox/src/YarpRead.cpp @@ -28,7 +28,6 @@ YarpRead::YarpRead() , m_blocking(false) , m_shouldReadTimestamp(false) , m_errorOnMissingPort(true) -, m_port(0) {} unsigned YarpRead::numberOfParameters() { return 6; } @@ -146,7 +145,7 @@ bool YarpRead::initialize(const BlockInformation* blockInfo) destinationPortName = portName; } - m_port = new BufferedPort(); + m_port = std::unique_ptr>(new BufferedPort()); if (!m_port || !m_port->open(destinationPortName)) { Log::getSingleton().error("Error while opening yarp port."); @@ -172,8 +171,6 @@ bool YarpRead::terminate(const BlockInformation* /*blockInfo*/) { if (m_port) { m_port->close(); - delete m_port; - m_port = nullptr; } yarp::os::Network::fini(); return true; diff --git a/toolbox/src/YarpWrite.cpp b/toolbox/src/YarpWrite.cpp index 3deba6dcc..6b50029d9 100644 --- a/toolbox/src/YarpWrite.cpp +++ b/toolbox/src/YarpWrite.cpp @@ -21,7 +21,6 @@ YarpWrite::YarpWrite() : m_autoconnect(false) , m_errorOnMissingPort(true) , m_destinationPortName("") -, m_port(nullptr) {} unsigned YarpWrite::numberOfParameters() { return 3; } @@ -98,7 +97,7 @@ bool YarpWrite::initialize(const BlockInformation* blockInfo) sourcePortName = portParameter; } - m_port = new BufferedPort(); + m_port = std::unique_ptr>(new BufferedPort()); if (!m_port || !m_port->open(sourcePortName)) { Log::getSingleton().error("Error while opening yarp port."); @@ -131,8 +130,6 @@ bool YarpWrite::terminate(const BlockInformation* /*S*/) yarp::os::Network::disconnect(m_port->getName(), m_destinationPortName); } m_port->close(); - delete m_port; - m_port = nullptr; } yarp::os::Network::fini(); return true; From e0e7a9263926c64ae59aadb4d046418e94b48241 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 13 Nov 2017 13:48:53 +0000 Subject: [PATCH 46/89] Forgot class scope for two static methods --- toolbox/src/GetMeasurement.cpp | 2 +- toolbox/src/SetReferences.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp index 65cc1a49e..659c921d7 100644 --- a/toolbox/src/GetMeasurement.cpp +++ b/toolbox/src/GetMeasurement.cpp @@ -14,7 +14,7 @@ using namespace wbt; const std::string GetMeasurement::ClassName = "GetMeasurement"; -void deg2rad(std::vector& v) +void GetMeasurement::deg2rad(std::vector& v) { const double deg2rad = (2 * M_PI) / 180.0; for (auto& element : v) { diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index c739b0e44..6e20ed9db 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -17,7 +17,7 @@ SetReferences::SetReferences() : m_resetControlMode(true) {} -void rad2deg(std::vector& v) +void SetReferences::rad2deg(std::vector& v) { const double rad2deg = 180.0 / (2 * M_PI); for (auto& element : v) { From f64da6e3adfd1b5323a0bd3228f227cbda2bfe14 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 09:48:01 +0000 Subject: [PATCH 47/89] Follow links in Simulink model inspection for finding the Config Block When the Config block (which is a subsystem) is included into a library, for finding the blocks contained into its subsystem, `FollowLinks` must be enabled --- +WBToolbox/BlockInitialization.m | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/+WBToolbox/BlockInitialization.m b/+WBToolbox/BlockInitialization.m index 5783432e0..cc335147d 100644 --- a/+WBToolbox/BlockInitialization.m +++ b/+WBToolbox/BlockInitialization.m @@ -13,14 +13,14 @@ topLevel = blockNameSplit{1}; % Get all the blocks from the top level of the system - blocks_system = find_system(topLevel,'LookUnderMasks','on'); + blocks_system = find_system(topLevel,'LookUnderMasks','on','FollowLinks','on'); % Get the name of the block's subsystem %[blockScopeName,~] = fileparts(blockAbsoluteName); %blockScopeName = gcs; blockScopeName = currentSystem; catch - error('[%s::Initialization] Failed to process model''s tree',char(blockAbsoluteName)) + error('[%s::Initialization] Failed to process model''s tree', char(currentBlock)) end % Look a config block from the block's scope going up in the tree @@ -30,7 +30,7 @@ rule = strcat(strrep(analyzedScope,'/','\/'),'\/\w+\/ImConfig'); idx = cellfun(@(x) ~isequal(regexp(x,rule),[]), blocks_system); if (sum(idx) > 1) - error('[%s::Initialization] Found more than one configuration',char(blockAbsoluteName)); + error('[%s::Initialization] Found more than one configuration', char(currentBlock)); elseif (sum(idx) == 1) configBlock = blocks_system{idx}; configBlock = fileparts(configBlock); @@ -40,7 +40,7 @@ end if isempty(configBlock) - error('[%s::Initialization] No configuration found',char(blockAbsoluteName)); + error('[%s::Initialization] No configuration found', char(currentBlock)); end configSource = get_param(configBlock,'ConfigSource'); @@ -55,7 +55,7 @@ % Read the mask WBTConfig = WBToolbox.Mask2WBToolboxConfig(configBlock); else - error('[%s::Initialization] Read ConfigSource from Config block not recognized',char(blockAbsoluteName)); + error('[%s::Initialization] Read ConfigSource from Config block not recognized', char(currentBlock)); end end From eb0e1b751746f10fa8b0318aa122c345d9299212 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 10:06:34 +0000 Subject: [PATCH 48/89] Retaining the RemoteControlBoardRemapper needs a unique port prefix If there are two robot configurations in the same system which share some of the ControlBoards, the RemoteControlBoardRemapper devices cannot be allocated with the same port prefix. As a workaround, a new function to generate an unique identifier for every Configuration object has been included. Under the hood it uses the Simulink's block name (which is unique). Furthermore, the initialization of the Yarp Network was missing at this stage of the execution. --- toolbox/include/base/Configuration.h | 16 ++++- toolbox/src/base/Configuration.cpp | 26 ++++++- toolbox/src/base/RobotInterface.cpp | 100 ++++++++++++++++----------- toolbox/src/base/WBBlock.cpp | 5 +- 4 files changed, 101 insertions(+), 46 deletions(-) diff --git a/toolbox/include/base/Configuration.h b/toolbox/include/base/Configuration.h index 9b70142ac..b131ce81a 100644 --- a/toolbox/include/base/Configuration.h +++ b/toolbox/include/base/Configuration.h @@ -21,6 +21,7 @@ class wbt::Configuration { // TODO: check how localName is used private: + const std::string m_confKey; ///< Name of the block which this object refers to std::string m_robotName; ///< Name of the robot std::string m_urdfFile; ///< Name of the file containing the urdf model std::string m_localName; ///< Prefix appended to the opened ports @@ -30,7 +31,8 @@ class wbt::Configuration size_t m_dofs; //< DoFs extracted my m_controlBoardsNames vector public: - Configuration(); + Configuration() = delete; + Configuration(const std::string& confKey); ~Configuration() = default; // SET METHODS @@ -123,12 +125,22 @@ class wbt::Configuration const std::vector& getControlBoardsNames() const; /** - * Set the prefix appended to the opened ports + * Set the prefix appended to the opened ports. A leading "/" is always present. * * @return Prefix appended to the opened ports */ const std::string& getLocalName() const; + /** + * Get a string with a unique identifier, generated from the name of the config + * block from Simulink. It might be useful when yarp ports must have a unique prefix + * (e.g. two RemoteControlBoardRemappers which share a ControlBoard) + * + * @return the unique identifier + * @see setLocalName + */ + const std::string getUniqueId() const; + /** * Set the gravity vector * diff --git a/toolbox/src/base/Configuration.cpp b/toolbox/src/base/Configuration.cpp index a963a3b41..257a475f4 100644 --- a/toolbox/src/base/Configuration.cpp +++ b/toolbox/src/base/Configuration.cpp @@ -1,9 +1,11 @@ #include "Configuration.h" +#include using namespace wbt; -Configuration::Configuration() -: m_dofs(0) +Configuration::Configuration(const std::string& confKey) +: m_confKey(confKey) +, m_dofs(0) {} // SET METHODS @@ -48,6 +50,26 @@ void Configuration::setControlBoardsNames(const std::vector& contro void Configuration::setLocalName(const std::string& localName) { m_localName = localName; + + // Add the leading "/" if missing + if (m_localName.compare(0, 1, "/")) { + m_localName = "/" + m_localName; + } +} + +const std::string Configuration::getUniqueId() const +{ + std::string uniqueId(m_confKey); + + // Remove spaces + auto it = std::remove(uniqueId.begin(), uniqueId.end(), ' '); + uniqueId.erase(it , uniqueId.end()); + + // Remove '/' + it = std::remove(uniqueId.begin(), uniqueId.end(), '/'); + uniqueId.erase(it , uniqueId.end()); + + return uniqueId; } void Configuration::setGravityVector(const std::array& gravityVector) diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp index 4e1fccbb0..05cfcb9c0 100644 --- a/toolbox/src/base/RobotInterface.cpp +++ b/toolbox/src/base/RobotInterface.cpp @@ -11,7 +11,6 @@ #include namespace wbt { - static std::string vectorToString(std::vector v, std::string prefix=""); // The declaration of the following template specializations are required only by GCC using namespace yarp::dev; @@ -318,6 +317,9 @@ bool RobotInterface::releaseRemoteControlBoardRemapper() m_robotDevice.reset(); } + // Initialize the network + yarp::os::Network::init(); + m_robotDeviceCounter = 0; return true; } @@ -367,32 +369,75 @@ bool RobotInterface::initializeModel() bool RobotInterface::initializeRemoteControlBoardRemapper() { - // Setup the RemoteControlBoardRemapper + // Initialize the network + yarp::os::Network::init(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; + } + + // Object where the RemoteControlBoardRemapper options will be stored yarp::os::Property options; + + // Name of the device options.put("device", "remotecontrolboardremapper"); - options.put("axesNames", vectorToString(m_config.getControlledJoints())); - std::string prefix = "/" + m_config.getRobotName() + "/"; - options.put("remoteControlBoards", vectorToString(m_config.getControlBoardsNames(), prefix)); - options.put("localPortPrefix", m_config.getLocalName()); + + // Controlled joints (axes) + yarp::os::Bottle axesNames; + yarp::os::Bottle& axesList = axesNames.addList(); + for (auto axis : m_config.getControlledJoints()) { + axesList.addString(axis); + } + options.put("axesNames",axesNames.get(0)); + + // ControlBoard names + yarp::os::Bottle remoteControlBoards; + yarp::os::Bottle& remoteControlBoardsList = remoteControlBoards.addList(); + for (auto cb : m_config.getControlBoardsNames()) { + remoteControlBoardsList.addString("/" + m_config.getRobotName() + "/" + cb); + } + options.put("remoteControlBoards", remoteControlBoards.get(0)); + + // Prefix of the openened ports + // In this case appending the unique id is necessary, since multiple configuration can + // share some ControlBoard in their RemoteControlBoardRemappers. In this case, it is not + // possible using the same prefix for all the RemoteControlBoardRemapper devices. + options.put("localPortPrefix", m_config.getLocalName() + "/" + m_config.getUniqueId()); + + // Misc options yarp::os::Property& remoteCBOpts = options.addGroup("REMOTE_CONTROLBOARD_OPTIONS"); - remoteCBOpts.put("writeStrict","on"); + remoteCBOpts.put("writeStrict", "on"); - // Allocate the device driver - assert(!m_robotDevice); + // If resources have been properly cleaned, there should be no allocated device. + // However, if blocks fail and they don't terminate, the state of the singleton + // could be not clean. + if (m_robotDevice) { + // Force the release + m_robotDeviceCounter = 1; + Log::getSingleton().warning("The ToolboxSingleton state is dirty. Trying to clean the state before proceeding."); + if (!releaseRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to force the release of the RemoteControlBoardRemapper. "); + return false; + } + } + + // Allocate the interface driver m_robotDevice = std::unique_ptr(new yarp::dev::PolyDriver()); if (!m_robotDevice) { + Log::getSingleton().error("Failed to instantiante an empty PolyDriver class."); return false; } - // Open the device driver - if (m_robotDevice->open(options) && m_robotDevice->isValid()) { - return true; + // Open the interface driver + if (!m_robotDevice->open(options) && !m_robotDevice->isValid()) { + // Remove garbage if the opening fails + m_robotDevice.reset(); + Log::getSingleton().error("Failed to open the RemoteControlBoardRemapper with the options passed."); + return false; } - // Remove garbage if the opening fails - m_robotDevice.reset(); - return false; + return true; } // OTHER METHODS @@ -421,28 +466,3 @@ std::weak_ptr RobotInterface::getInterfaceFromTemplate(std::shared_ptr dev // Implicit conversion from shared_ptr to weak_ptr return device; } - -/** - * Converts a vector of strings into a string format \code (element1 element2 element3) \endcode. - * It optioanlly support appending a prefix to the elements. e.g. - * \code ({prefix}element1 {prefix}element2 {prefix}element3) - * - * @param v The vector of strings - * @param prefix The optional element prefix - * @return The serialized string - */ -std::string wbt::vectorToString(std::vector v, std::string prefix) -{ - // Add the prefix if specified - if (!prefix.empty()) { - for (auto& str : v) { - str = prefix + str; - } - } - - // Create a string in a format which YARP likes: (item1 item2 item3) - std::stringstream output; - std::ostream_iterator output_iterator(output, " "); - std::copy(v.begin(), v.end(), output_iterator); - return "(" + output.str() + ")"; -} diff --git a/toolbox/src/base/WBBlock.cpp b/toolbox/src/base/WBBlock.cpp index 0c344e33f..d4acfbb3f 100644 --- a/toolbox/src/base/WBBlock.cpp +++ b/toolbox/src/base/WBBlock.cpp @@ -181,8 +181,9 @@ bool WBBlock::configureSizeAndPorts(BlockInformation* blockInfo) return false; } - // Get the configuration struct from the block - Configuration config; + // Initialize the configuration block with the unique identifier of the conf block name + Configuration config(confKey); + // Populate the configuration object with data from the Simulink's struct parameter if (!getWBToolboxParameters(config, blockInfo)) { return false; } From 1f3f8a2eb5536ac914dbde49bc735642c0edff62 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 10:20:18 +0000 Subject: [PATCH 49/89] Switch from smart pointers to raw pointers for Yarp interfaces Using smart pointers on yarp interfaces is not possible because their allocation is handled internally by the device from which they are asked from. As minor edit, renamed YarpDevices variable to YarpInterfaces for the sake of clarity. --- toolbox/include/base/RobotInterface.h | 49 ++++++---- toolbox/src/base/RobotInterface.cpp | 127 +++++++++++++------------- 2 files changed, 94 insertions(+), 82 deletions(-) diff --git a/toolbox/include/base/RobotInterface.h b/toolbox/include/base/RobotInterface.h index 6b85789d3..489bf0f35 100644 --- a/toolbox/include/base/RobotInterface.h +++ b/toolbox/include/base/RobotInterface.h @@ -31,7 +31,7 @@ namespace iDynTree { namespace wbt { class RobotInterface; - struct YarpDevices; + struct YarpInterfaces; typedef int jointIdx_yarp; typedef int jointIdx_iDynTree; @@ -41,7 +41,7 @@ namespace wbt { } /** - * \struct wbt::YarpDevices RobotInterface.h + * \struct wbt::YarpInterfaces RobotInterface.h * * This struct contains shared_ptrs to the devices which are (lazy) asked from the blocks. * @@ -54,18 +54,31 @@ namespace wbt { * containing only the reduced set in a deeper-hierarchy Simulink's subsystem. * @see RobotInterface::getDevice */ -struct wbt::YarpDevices +struct wbt::YarpInterfaces { - std::shared_ptr iControlMode2; - std::shared_ptr iPositionControl; - std::shared_ptr iPositionDirect; - std::shared_ptr iVelocityControl; - std::shared_ptr iTorqueControl; - std::shared_ptr iPWMControl; - std::shared_ptr iCurrentControl; - std::shared_ptr iEncoders; - std::shared_ptr iControlLimits2; - std::shared_ptr iPidControl; + yarp::dev::IControlMode2* iControlMode2; + yarp::dev::IPositionControl* iPositionControl; + yarp::dev::IPositionDirect* iPositionDirect; + yarp::dev::IVelocityControl* iVelocityControl; + yarp::dev::ITorqueControl* iTorqueControl; + yarp::dev::IPWMControl* iPWMControl; + yarp::dev::ICurrentControl* iCurrentControl; + yarp::dev::IEncoders* iEncoders; + yarp::dev::IControlLimits2* iControlLimits2; + yarp::dev::IPidControl* iPidControl; + + YarpInterfaces() + : iControlMode2(nullptr) + , iPositionControl(nullptr) + , iPositionDirect(nullptr) + , iVelocityControl(nullptr) + , iTorqueControl(nullptr) + , iPWMControl(nullptr) + , iCurrentControl(nullptr) + , iEncoders(nullptr) + , iControlLimits2(nullptr) + , iPidControl(nullptr) + {} }; // TODO o pensare come evitare di avere due conf con es position e torque nei setref con lo stesso robot. @@ -80,7 +93,7 @@ struct wbt::YarpDevices * with the specified robot (real or model). * * @see wbt::Configuration - * @see wbt::YarpDevices + * @see wbt::YarpInterfaces * @see iDynTree::KinDynComputations */ class wbt::RobotInterface @@ -88,7 +101,7 @@ class wbt::RobotInterface private: std::unique_ptr m_robotDevice; std::shared_ptr m_kinDynComp; - wbt::YarpDevices m_yarpDevices; + wbt::YarpInterfaces m_yarpInterfaces; // Maps used to store infos about yarp's and idyntree's internal joint indexing std::shared_ptr m_jointsMapIndex; @@ -133,7 +146,7 @@ class wbt::RobotInterface * @tparam T The type of the retured device */ template - std::weak_ptr getInterfaceFromTemplate(std::shared_ptr device); + T* getInterfaceFromTemplate(T*& device); /** * Creates the map between joints (specified as either names or idyntree indices) and @@ -196,7 +209,7 @@ class wbt::RobotInterface * @tparam T The type of interface */ template - bool getInterface(std::weak_ptr& interface); + bool getInterface(T*& interface); // LAZY EVALUATION // =============== @@ -205,7 +218,7 @@ class wbt::RobotInterface * Handles the internal counter for using the RemoteControlBoardRemapper * * @attention All the blocks which need to use any of the interfaces provided by - * wbt::YarpDevices must call this function in their initialize() method. + * wbt::YarpInterfaces must call this function in their initialize() method. * @see releaseRemoteControlBoardRemapper * * @return True if success diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp index 05cfcb9c0..bfda124ca 100644 --- a/toolbox/src/base/RobotInterface.cpp +++ b/toolbox/src/base/RobotInterface.cpp @@ -14,16 +14,16 @@ namespace wbt { // The declaration of the following template specializations are required only by GCC using namespace yarp::dev; - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); - template <> bool RobotInterface::getInterface(std::weak_ptr& interface); + template <> bool RobotInterface::getInterface(IControlMode2*& interface); + template <> bool RobotInterface::getInterface(IPositionControl*& interface); + template <> bool RobotInterface::getInterface(IPositionDirect*& interface); + template <> bool RobotInterface::getInterface(IVelocityControl*& interface); + template <> bool RobotInterface::getInterface(ITorqueControl*& interface); + template <> bool RobotInterface::getInterface(IPWMControl*& interface); + template <> bool RobotInterface::getInterface(ICurrentControl*& interface); + template <> bool RobotInterface::getInterface(IEncoders*& interface); + template <> bool RobotInterface::getInterface(IControlLimits2*& interface); + template <> bool RobotInterface::getInterface(IPidControl*& interface); } using namespace wbt; @@ -81,7 +81,7 @@ bool RobotInterface::mapDoFs() return false; } - // Get an IEncoders object from the device + // Get an IEncoders object from the interface // This is used to get how many joints the control board contains std::unique_ptr iEnc; yarp::dev::IEncoders* iEncPtr = iEnc.get(); @@ -192,73 +192,73 @@ const std::shared_ptr RobotInterface::getKinDynCom } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::IControlMode2*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iControlMode2); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iControlMode2); + return interface; } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::IPositionControl*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iPositionControl); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iPositionControl); + return interface; } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::IPositionDirect*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iPositionDirect); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iPositionDirect); + return interface; } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::IVelocityControl*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iVelocityControl); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iVelocityControl); + return interface; } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::ITorqueControl*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iTorqueControl); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iTorqueControl); + return interface; } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::IPWMControl*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iPWMControl); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iPWMControl); + return interface; } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::ICurrentControl*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iCurrentControl); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iCurrentControl); + return interface; } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::IEncoders*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iEncoders); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iEncoders); + return interface; } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::IControlLimits2*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iControlLimits2); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iControlLimits2); + return interface; } template <> -bool RobotInterface::getInterface(std::weak_ptr& interface) +bool RobotInterface::getInterface(yarp::dev::IPidControl*& interface) { - interface = getInterfaceFromTemplate(m_yarpDevices.iPidControl); - return !interface.expired(); + interface = getInterfaceFromTemplate(m_yarpInterfaces.iPidControl); + return interface; } // LAZY EVALUATION @@ -300,18 +300,18 @@ bool RobotInterface::releaseRemoteControlBoardRemapper() // This should be executed by the last block which uses CB (m_robotDeviceCounter=1) assert(m_robotDevice); if (m_robotDevice) { - // Free all the drivers - m_yarpDevices.iControlMode2.reset(); - m_yarpDevices.iPositionControl.reset(); - m_yarpDevices.iPositionDirect.reset(); - m_yarpDevices.iVelocityControl.reset(); - m_yarpDevices.iTorqueControl.reset(); - m_yarpDevices.iPWMControl.reset(); - m_yarpDevices.iCurrentControl.reset(); - m_yarpDevices.iEncoders.reset(); - m_yarpDevices.iControlLimits2.reset(); - m_yarpDevices.iPidControl.reset(); - // Close the device + // Set to zero all the pointers to the interfaces + m_yarpInterfaces.iControlMode2 = nullptr; + m_yarpInterfaces.iPositionControl = nullptr; + m_yarpInterfaces.iPositionDirect = nullptr; + m_yarpInterfaces.iVelocityControl = nullptr; + m_yarpInterfaces.iTorqueControl = nullptr; + m_yarpInterfaces.iPWMControl = nullptr; + m_yarpInterfaces.iCurrentControl = nullptr; + m_yarpInterfaces.iEncoders = nullptr; + m_yarpInterfaces.iControlLimits2 = nullptr; + m_yarpInterfaces.iPidControl = nullptr; + // Close the device (which deletes the interfaces it allocated) m_robotDevice->close(); // Free the object m_robotDevice.reset(); @@ -444,25 +444,24 @@ bool RobotInterface::initializeRemoteControlBoardRemapper() // ============= template -std::weak_ptr RobotInterface::getInterfaceFromTemplate(std::shared_ptr device) +T* RobotInterface::getInterfaceFromTemplate(T*& interface) { - if (!device) { + if (!interface) { // Blocks which require the RemoteControlBoardRemapper need to retain / release it // in their initialization / terminate phase; - assert(m_robotDevice); + // assert(m_robotDevice); if (!m_robotDevice) { + Log::getSingleton().error("The RemoteControlBoardRemapper has not been initialized. "); + Log::getSingleton().errorAppend("You need to retain the CB device in the initialize() method."); // Return an empty weak pointer - return std::weak_ptr(); + return nullptr; } - T* ptr = nullptr; - if (!m_robotDevice->view(ptr)) { + // Ask the interface from the device + if (!m_robotDevice->view(interface)) { // Return an empty weak_ptr - return std::weak_ptr(); + return nullptr; } - // Store ptr into the smart pointer - device.reset(ptr); } - // Implicit conversion from shared_ptr to weak_ptr - return device; + return interface; } From 9b90b9e97529a074484db20e030d2945279114d4 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 10:30:34 +0000 Subject: [PATCH 50/89] Updated Blocks for using raw pointers to Yarp interfaces --- toolbox/src/GetLimits.cpp | 8 +++--- toolbox/src/GetMeasurement.cpp | 24 ++++++++-------- toolbox/src/SetLowLevelPID.cpp | 16 +++++------ toolbox/src/SetReferences.cpp | 52 ++++++++++++++++++---------------- 4 files changed, 51 insertions(+), 49 deletions(-) diff --git a/toolbox/src/GetLimits.cpp b/toolbox/src/GetLimits.cpp index 4923021e7..eec5fddc2 100644 --- a/toolbox/src/GetLimits.cpp +++ b/toolbox/src/GetLimits.cpp @@ -91,7 +91,7 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) // possible using i to point to the correct joint. // Get the RemoteControlBoardRemapper and IControlLimits2 interface if needed - std::weak_ptr iControlLimits2; + yarp::dev::IControlLimits2* iControlLimits2 = nullptr; if (limitType == "ControlBoardPosition" || limitType == "ControlBoardVelocity") { // Retain the control board remapper if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { @@ -99,7 +99,7 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) return false; } // Get the interface - if (!getRobotInterface()->getInterface(iControlLimits2)) { + if (!getRobotInterface()->getInterface(iControlLimits2) || !iControlLimits2) { Log::getSingleton().error("Failed to get IControlLimits2 interface."); return false; } @@ -107,7 +107,7 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) if (limitType == "ControlBoardPosition") { for (auto i = 0; i < dofs; ++i) { - if (!iControlLimits2.lock()->getLimits(i, &min, &max)) { + if (!iControlLimits2->getLimits(i, &min, &max)) { Log::getSingleton().error("Failed to get limits from the interface."); return false; } @@ -117,7 +117,7 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) } else if (limitType == "ControlBoardVelocity") { for (auto i = 0; i < dofs; ++i) { - if (!iControlLimits2.lock()->getVelLimits(i, &min, &max)) { + if (!iControlLimits2->getVelLimits(i, &min, &max)) { Log::getSingleton().error("Failed to get limits from the interface."); return false; } diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp index 659c921d7..97b8f971c 100644 --- a/toolbox/src/GetMeasurement.cpp +++ b/toolbox/src/GetMeasurement.cpp @@ -120,49 +120,49 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) switch (m_estimateType) { case ESTIMATE_JOINT_POS: { // Get the interface - std::weak_ptr iEncoders; - if (!getRobotInterface()->getInterface(iEncoders)) { + yarp::dev::IEncoders* iEncoders = nullptr; + if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { Log::getSingleton().error("Failed to get IPidControl interface."); return false; } // Get the measurement - ok = iEncoders.lock()->getEncoders(m_estimate.data()); deg2rad(m_estimate); + ok = iEncoders->getEncoders(m_measurement.data()); break; } case ESTIMATE_JOINT_VEL: { // Get the interface - std::weak_ptr iEncoders; - if (!getRobotInterface()->getInterface(iEncoders)) { + yarp::dev::IEncoders* iEncoders = nullptr; + if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { Log::getSingleton().error("Failed to get IEncoders interface."); return false; } // Get the measurement - ok = iEncoders.lock()->getEncoderSpeeds(m_estimate.data()); deg2rad(m_estimate); + ok = iEncoders->getEncoderSpeeds(m_measurement.data()); break; } case ESTIMATE_JOINT_ACC: { // Get the interface - std::weak_ptr iEncoders; - if (!getRobotInterface()->getInterface(iEncoders)) { + yarp::dev::IEncoders* iEncoders = nullptr; + if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { Log::getSingleton().error("Failed to get IEncoders interface."); return false; } // Get the measurement - ok = iEncoders.lock()->getEncoderAccelerations(m_estimate.data()); deg2rad(m_estimate); + ok = iEncoders->getEncoderAccelerations(m_measurement.data()); break; } case ESTIMATE_JOINT_TORQUE: { // Get the interface - std::weak_ptr iTorqueControl; - if (!getRobotInterface()->getInterface(iTorqueControl)) { + yarp::dev::ITorqueControl* iTorqueControl = nullptr; + if (!getRobotInterface()->getInterface(iTorqueControl) || !iTorqueControl) { Log::getSingleton().error("Failed to get ITorqueControl interface."); return false; } // Get the measurement - ok = iTorqueControl.lock()->getTorques(m_estimate.data()); + ok = iTorqueControl->getTorques(m_measurement.data()); break; } default: diff --git a/toolbox/src/SetLowLevelPID.cpp b/toolbox/src/SetLowLevelPID.cpp index b2d9772c4..94f3c6685 100644 --- a/toolbox/src/SetLowLevelPID.cpp +++ b/toolbox/src/SetLowLevelPID.cpp @@ -171,14 +171,14 @@ bool SetLowLevelPID::initialize(const BlockInformation* blockInfo) m_defaultPidValues.resize(dofs); // Get the interface - std::weak_ptr iPidControl; - if (!getRobotInterface()->getInterface(iPidControl)) { + yarp::dev::IPidControl* iPidControl = nullptr; + if (!getRobotInterface()->getInterface(iPidControl) || !iPidControl) { Log::getSingleton().error("Failed to get IPidControl interface."); return false; } // Store the default gains - if (!iPidControl.lock()->getPids(m_controlType, m_defaultPidValues.data())) { + if (!iPidControl->getPids(m_controlType, m_defaultPidValues.data())) { Log::getSingleton().error("Failed to get default data from IPidControl."); return false; } @@ -199,7 +199,7 @@ bool SetLowLevelPID::initialize(const BlockInformation* blockInfo) } // Apply the new pid gains - if (!iPidControl.lock()->setPids(m_controlType, m_appliedPidValues.data())) { + if (!iPidControl->setPids(m_controlType, m_appliedPidValues.data())) { Log::getSingleton().error("Failed to set PID values."); return false; } @@ -212,14 +212,14 @@ bool SetLowLevelPID::terminate(const BlockInformation* blockInfo) bool ok = true; // Get the IPidControl interface - std::weak_ptr iPidControl; - ok = ok & getRobotInterface()->getInterface(iPidControl); - if (!ok) { + yarp::dev::IPidControl* iPidControl = nullptr; + ok = ok && getRobotInterface()->getInterface(iPidControl); + if (!ok || !iPidControl) { Log::getSingleton().error("Failed to get IPidControl interface."); } // Reset default pid gains - ok = ok & iPidControl.lock()->setPids(m_controlType, m_defaultPidValues.data()); + ok = ok && iPidControl->setPids(m_controlType, m_defaultPidValues.data()); if (!ok) { Log::getSingleton().error("Failed to set default PID values."); } diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 6e20ed9db..121a72261 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -129,19 +129,21 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) bool ok = true; // Get the interface - std::weak_ptr icmd2; - ok = ok & getRobotInterface()->getInterface(icmd2); - if (!ok) { + IControlMode2* icmd2 = nullptr; + ok = ok && getRobotInterface()->getInterface(icmd2); + if (!ok || !icmd2) { Log::getSingleton().error("Failed to get the IControlMode2 interface."); + return false; } // Set all the controlledJoints VOCAB_CM_POSITION const unsigned dofs = getConfiguration().getNumberOfDoFs(); m_controlModes.assign(dofs, VOCAB_CM_POSITION); - ok = ok & icmd2.lock()->setControlModes(m_controlModes.data()); + ok = ok && icmd2->setControlModes(m_controlModes.data()); if (!ok) { Log::getSingleton().error("Failed to set control mode."); + return false; } // Release the RemoteControlBoardRemapper @@ -169,13 +171,13 @@ bool SetReferences::output(const BlockInformation* blockInfo) if (m_resetControlMode) { m_resetControlMode = false; // Get the interface - std::weak_ptr icmd2; - if (!getRobotInterface()->getInterface(icmd2)) { + IControlMode2* icmd2 = nullptr; + if (!getRobotInterface()->getInterface(icmd2) || !icmd2) { Log::getSingleton().error("Failed to get the IControlMode2 interface."); return false; } // Set the control mode to all the controlledJoints - if (!icmd2.lock()->setControlModes(m_controlModes.data())) { + if (!icmd2->setControlModes(m_controlModes.data())) { Log::getSingleton().error("Failed to set control mode."); return false; } @@ -195,71 +197,71 @@ bool SetReferences::output(const BlockInformation* blockInfo) break; case VOCAB_CM_POSITION: { // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { + IPositionControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get IPositionControl interface."); return false; } // Convert from rad to deg rad2deg(referencesVector); // Set the references - ok = interface.lock()->positionMove(referencesVector.data()); + ok = interface->positionMove(referencesVector.data()); break; } case VOCAB_CM_POSITION_DIRECT: { // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { + IPositionDirect* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get IPositionDirect interface."); return false; } // Convert from rad to deg rad2deg(referencesVector); // Set the references - ok = interface.lock()->setPositions(referencesVector.data()); + ok = interface->setPositions(referencesVector.data()); break; } case VOCAB_CM_VELOCITY: { // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { + IVelocityControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get IVelocityControl interface."); return false; } // Convert from rad to deg rad2deg(referencesVector); // Set the references - ok = interface.lock()->velocityMove(referencesVector.data()); + ok = interface->velocityMove(referencesVector.data()); break; } case VOCAB_CM_TORQUE: { // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { + ITorqueControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get ITorqueControl interface."); return false; } - ok = interface.lock()->setRefTorques(referencesVector.data()); + ok = interface->setRefTorques(referencesVector.data()); break; } case VOCAB_CM_PWM: { // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { + IPWMControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get IPWMControl interface."); return false; } - ok = interface.lock()->setRefDutyCycles(referencesVector.data()); + ok = interface->setRefDutyCycles(referencesVector.data()); break; } case VOCAB_CM_CURRENT: { // Get the interface - std::weak_ptr interface; - if (!getRobotInterface()->getInterface(interface)) { + ICurrentControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get ICurrentControl interface."); return false; } - ok = interface.lock()->setRefCurrents(referencesVector.data()); + ok = interface->setRefCurrents(referencesVector.data()); break; } } From a01cecfbed5b4098c7c4246021c0223311715c2b Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 10:36:03 +0000 Subject: [PATCH 51/89] Block which inherith from WBBlocks now terminate correctly The terminate() method of these blocks should not fail before the WBBlock::terminate() method is called, otherwise allocated resources are not properly cleaned. --- toolbox/src/GetLimits.cpp | 18 +++++++++++++++--- toolbox/src/GetMeasurement.cpp | 1 + toolbox/src/SetLowLevelPID.cpp | 3 +++ toolbox/src/SetReferences.cpp | 7 ++++--- 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/toolbox/src/GetLimits.cpp b/toolbox/src/GetLimits.cpp index eec5fddc2..60c665307 100644 --- a/toolbox/src/GetLimits.cpp +++ b/toolbox/src/GetLimits.cpp @@ -171,11 +171,23 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) bool GetLimits::terminate(const BlockInformation* blockInfo) { - // Release the RemoteControlBoardRemapper bool ok = true; - ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + + // Read the control type + std::string limitType; + ok = ok && blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, limitType); if (!ok) { - Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + Log::getSingleton().error("Could not read estimate type parameter."); + // Don't return false here. WBBlock::terminate must be called in any case + } + + // Release the RemoteControlBoardRemapper + if (limitType == "ControlBoardPosition" || limitType == "ControlBoardVelocity") { + ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + // Don't return false here. WBBlock::terminate must be called in any case + } } return ok && WBBlock::terminate(blockInfo); diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp index 97b8f971c..6869424dd 100644 --- a/toolbox/src/GetMeasurement.cpp +++ b/toolbox/src/GetMeasurement.cpp @@ -109,6 +109,7 @@ bool GetMeasurement::terminate(const BlockInformation* blockInfo) ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); if (!ok) { Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + // Don't return false here. WBBlock::terminate must be called in any case } return ok && WBBlock::terminate(blockInfo); diff --git a/toolbox/src/SetLowLevelPID.cpp b/toolbox/src/SetLowLevelPID.cpp index 94f3c6685..c593f32ca 100644 --- a/toolbox/src/SetLowLevelPID.cpp +++ b/toolbox/src/SetLowLevelPID.cpp @@ -216,18 +216,21 @@ bool SetLowLevelPID::terminate(const BlockInformation* blockInfo) ok = ok && getRobotInterface()->getInterface(iPidControl); if (!ok || !iPidControl) { Log::getSingleton().error("Failed to get IPidControl interface."); + // Don't return false here. WBBlock::terminate must be called in any case } // Reset default pid gains ok = ok && iPidControl->setPids(m_controlType, m_defaultPidValues.data()); if (!ok) { Log::getSingleton().error("Failed to set default PID values."); + // Don't return false here. WBBlock::terminate must be called in any case } // Release the RemoteControlBoardRemapper ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); if (!ok) { Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + // Don't return false here. WBBlock::terminate must be called in any case } return ok && WBBlock::terminate(blockInfo); diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 121a72261..4255344b8 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -133,7 +133,7 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) ok = ok && getRobotInterface()->getInterface(icmd2); if (!ok || !icmd2) { Log::getSingleton().error("Failed to get the IControlMode2 interface."); - return false; + // Don't return false here. WBBlock::terminate must be called in any case } // Set all the controlledJoints VOCAB_CM_POSITION @@ -143,13 +143,14 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) ok = ok && icmd2->setControlModes(m_controlModes.data()); if (!ok) { Log::getSingleton().error("Failed to set control mode."); - return false; + // Don't return false here. WBBlock::terminate must be called in any case } // Release the RemoteControlBoardRemapper - ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); if (!ok) { Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + // Don't return false here. WBBlock::terminate must be called in any case } return ok && WBBlock::terminate(blockInfo); From ed7dd9272b7c784cb1df20a37f55990e109ed40e Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 10:43:13 +0000 Subject: [PATCH 52/89] Switched ands from bitwise to logical Robustness matters --- toolbox/src/GetMeasurement.cpp | 2 +- .../src/MinimumJerkTrajectoryGenerator.cpp | 22 +++++++++---------- toolbox/src/SetLowLevelPID.cpp | 2 +- toolbox/src/SimulatorSynchronizer.cpp | 6 ++--- toolbox/src/YarpRead.cpp | 16 +++++++------- toolbox/src/YarpWrite.cpp | 5 ++--- 6 files changed, 26 insertions(+), 27 deletions(-) diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp index 6869424dd..7271a65fd 100644 --- a/toolbox/src/GetMeasurement.cpp +++ b/toolbox/src/GetMeasurement.cpp @@ -106,7 +106,7 @@ bool GetMeasurement::terminate(const BlockInformation* blockInfo) { // Release the RemoteControlBoardRemapper bool ok = true; - ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); if (!ok) { Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); // Don't return false here. WBBlock::terminate must be called in any case diff --git a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp index d550be5fd..6c7e82e36 100644 --- a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp +++ b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp @@ -56,10 +56,10 @@ bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation* blo bool externalSettlingTime; bool ok = true; - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, outputFirstDerivative); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, outputSecondDerivative); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, explicitInitialValue); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, externalSettlingTime); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, outputFirstDerivative); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, outputSecondDerivative); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, explicitInitialValue); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, externalSettlingTime); if (!ok) { Log::getSingleton().error("Failed to get input parameters."); @@ -132,13 +132,13 @@ bool MinimumJerkTrajectoryGenerator::initialize(const BlockInformation* blockInf bool outputSecondDerivative; bool ok = true; - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, outputFirstDerivative); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, outputSecondDerivative); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, m_explicitInitialValue); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, m_externalSettlingTime); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_RESET_CHANGEST, + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, m_explicitInitialValue); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, m_externalSettlingTime); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_RESET_CHANGEST, m_resetOnSettlingTimeChange); if (!ok) { @@ -157,8 +157,8 @@ bool MinimumJerkTrajectoryGenerator::initialize(const BlockInformation* blockInf double sampleTime; double settlingTime; - ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_SAMPLE_TIME, sampleTime); - ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_SETTLING_TIME, settlingTime); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_SAMPLE_TIME, sampleTime); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_SETTLING_TIME, settlingTime); unsigned signalSize = blockInfo->getInputPortWidth(0); diff --git a/toolbox/src/SetLowLevelPID.cpp b/toolbox/src/SetLowLevelPID.cpp index c593f32ca..6ce3a3aba 100644 --- a/toolbox/src/SetLowLevelPID.cpp +++ b/toolbox/src/SetLowLevelPID.cpp @@ -227,7 +227,7 @@ bool SetLowLevelPID::terminate(const BlockInformation* blockInfo) } // Release the RemoteControlBoardRemapper - ok = ok & getRobotInterface()->releaseRemoteControlBoardRemapper(); + ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); if (!ok) { Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); // Don't return false here. WBBlock::terminate must be called in any case diff --git a/toolbox/src/SimulatorSynchronizer.cpp b/toolbox/src/SimulatorSynchronizer.cpp index 2f0c834fa..c46c528e1 100644 --- a/toolbox/src/SimulatorSynchronizer.cpp +++ b/toolbox/src/SimulatorSynchronizer.cpp @@ -73,9 +73,9 @@ bool SimulatorSynchronizer::initialize(const BlockInformation* blockInfo) std::string clientPortName; bool ok = true; - ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period); - ok = ok & blockInfo->getStringParameterAtIndex(PARAM_GZCLK_PORT, serverPortName); - ok = ok & blockInfo->getStringParameterAtIndex(PARAM_RPC_PORT, clientPortName); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_GZCLK_PORT, serverPortName); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_RPC_PORT, clientPortName); if (!ok) { Log::getSingleton().error("Error reading RPC parameters."); diff --git a/toolbox/src/YarpRead.cpp b/toolbox/src/YarpRead.cpp index 023fb7701..9e5807cce 100644 --- a/toolbox/src/YarpRead.cpp +++ b/toolbox/src/YarpRead.cpp @@ -58,11 +58,11 @@ bool YarpRead::configureSizeAndPorts(BlockInformation* blockInfo) bool autoconnect; double signalSize; - bool ok = false; + bool ok = true; - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, shouldReadTimestamp); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, autoconnect); - ok = ok & blockInfo->getScalarParameterAtIndex(PARAM_IDX_PORTSIZE, signalSize); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, shouldReadTimestamp); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, autoconnect); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_PORTSIZE, signalSize); if (!ok) { Log::getSingleton().error("Failed to read input parameters."); @@ -114,10 +114,10 @@ bool YarpRead::initialize(const BlockInformation* blockInfo) bool ok = true; - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, m_shouldReadTimestamp); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_WAITDATA, m_blocking); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, m_errorOnMissingPort); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, m_shouldReadTimestamp); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_WAITDATA, m_blocking); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, m_errorOnMissingPort); if (!ok) { Log::getSingleton().error("Failed to read input parameters."); diff --git a/toolbox/src/YarpWrite.cpp b/toolbox/src/YarpWrite.cpp index 6b50029d9..5c59fd035 100644 --- a/toolbox/src/YarpWrite.cpp +++ b/toolbox/src/YarpWrite.cpp @@ -68,9 +68,8 @@ bool YarpWrite::initialize(const BlockInformation* blockInfo) bool ok = true; - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); - ok = ok & blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, - m_errorOnMissingPort); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, m_errorOnMissingPort); if (!ok) { Log::getSingleton().error("Failed to read input parameters."); From d4a64334d6f16c8252b94201ad343f29926d28ec Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 10:47:23 +0000 Subject: [PATCH 53/89] Check smart pointer status before using the KinDynComputations object --- toolbox/include/base/ToolboxSingleton.h | 2 +- toolbox/src/GetLimits.cpp | 15 ++++++++++++--- toolbox/src/InverseDynamics.cpp | 7 ++++++- toolbox/src/base/RobotInterface.cpp | 9 +++++---- toolbox/src/base/ToolboxSingleton.cpp | 2 +- 5 files changed, 25 insertions(+), 10 deletions(-) diff --git a/toolbox/include/base/ToolboxSingleton.h b/toolbox/include/base/ToolboxSingleton.h index 1140a2d76..78dcf47e2 100644 --- a/toolbox/include/base/ToolboxSingleton.h +++ b/toolbox/include/base/ToolboxSingleton.h @@ -110,7 +110,7 @@ class wbt::ToolboxSingleton { // TODO: wbt::ToolboxSingleton * @return A \c shared_ptr to the iDynTree::KinDynComputations of the requested * configuration */ - const std::shared_ptr getModel(const std::string& confKey); + const std::shared_ptr getKinDynComputations(const std::string& confKey); // TOOLBOXSINGLETON CONFIGURATION // ============================== diff --git a/toolbox/src/GetLimits.cpp b/toolbox/src/GetLimits.cpp index 60c665307..eada4afe8 100644 --- a/toolbox/src/GetLimits.cpp +++ b/toolbox/src/GetLimits.cpp @@ -4,8 +4,8 @@ #include "BlockInformation.h" #include "Signal.h" #include "RobotInterface.h" -#include "iDynTree/KinDynComputations.h" -#include "iDynTree/Model/Model.h" +#include +#include #include #include #include @@ -133,7 +133,16 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) else if (limitType == "ModelPosition") { iDynTree::IJointConstPtr p_joint; - const iDynTree::Model model = getRobotInterface()->getKinDynComputations()->model(); + + // Get the KinDynComputations pointer + const auto& kindyncomp = getRobotInterface()->getKinDynComputations(); + if (!kindyncomp) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } + + // Get the model + const iDynTree::Model model = kindyncomp->model(); for (auto i = 0; i < dofs; ++i) { // Get the joint name diff --git a/toolbox/src/InverseDynamics.cpp b/toolbox/src/InverseDynamics.cpp index e0303b97e..514c96d2b 100644 --- a/toolbox/src/InverseDynamics.cpp +++ b/toolbox/src/InverseDynamics.cpp @@ -110,7 +110,12 @@ bool InverseDynamics::initialize(const BlockInformation* blockInfo) m_jointsAcceleration = std::unique_ptr(new VectorDynSize(dofs)); m_jointsAcceleration->zero(); - const auto& model = getRobotInterface()->getKinDynComputations()->model(); + // Get the KinDynComputations pointer + const auto& kindyncomp = getRobotInterface()->getKinDynComputations(); + + // Get the model from the KinDynComputations object + const auto& model = kindyncomp->model(); + m_torques = std::unique_ptr(new FreeFloatingGeneralizedTorques(model)); return static_cast(m_baseAcceleration) && diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp index bfda124ca..bf96983f2 100644 --- a/toolbox/src/base/RobotInterface.cpp +++ b/toolbox/src/base/RobotInterface.cpp @@ -183,12 +183,13 @@ const std::shared_ptr RobotInterface::getKinDynCom } // Otherwise, initialize a new object - if (initializeModel()) { - return m_kinDynComp; + if (!initializeModel()) { + Log::getSingleton().error("Failed to initialize the KinDynComputations object."); + // Return an empty shared_ptr (implicitly initialized) + return nullptr; } - // Return an empty shared_ptr (implicitly initialized) - return nullptr; + return m_kinDynComp; } template <> diff --git a/toolbox/src/base/ToolboxSingleton.cpp b/toolbox/src/base/ToolboxSingleton.cpp index 19042b281..9eb76a4f3 100644 --- a/toolbox/src/base/ToolboxSingleton.cpp +++ b/toolbox/src/base/ToolboxSingleton.cpp @@ -63,7 +63,7 @@ const std::shared_ptr ToolboxSingleton::getRobotInterface(const return m_interfaces[confKey]; } -const std::shared_ptr ToolboxSingleton::getModel(const std::string& confKey) +const std::shared_ptr ToolboxSingleton::getKinDynComputations(const std::string& confKey) { if (!isKeyValid(confKey)) { return nullptr; From 9b3180aa0178be345ae1da76d1e5f2d6ced9db04 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 10:53:46 +0000 Subject: [PATCH 54/89] GetLimits: missing deg2rad conversion and typo for invalid joint check --- toolbox/include/GetLimits.h | 3 ++- toolbox/src/GetLimits.cpp | 30 ++++++++++++++++++++++-------- 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/toolbox/include/GetLimits.h b/toolbox/include/GetLimits.h index 3a0310fe9..e96a7ae59 100644 --- a/toolbox/include/GetLimits.h +++ b/toolbox/include/GetLimits.h @@ -24,10 +24,11 @@ class wbt::GetLimits : public wbt::WBBlock { private: std::unique_ptr m_limits; + static double deg2rad(const double& v); public: static const std::string ClassName; - + GetLimits() = default; ~GetLimits() override = default; diff --git a/toolbox/src/GetLimits.cpp b/toolbox/src/GetLimits.cpp index eada4afe8..44648995e 100644 --- a/toolbox/src/GetLimits.cpp +++ b/toolbox/src/GetLimits.cpp @@ -10,10 +10,18 @@ #include #include +#define _USE_MATH_DEFINES +#include + using namespace wbt; const std::string GetLimits::ClassName = "GetLimits"; +double GetLimits::deg2rad(const double& v) +{ + return v * M_PI / 180.0; +} + unsigned GetLimits::numberOfParameters() { return WBBlock::numberOfParameters() + 1; @@ -111,8 +119,8 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) Log::getSingleton().error("Failed to get limits from the interface."); return false; } - m_limits->min[i] = min; - m_limits->max[i] = max; + m_limits->min[i] = deg2rad(min); + m_limits->max[i] = deg2rad(max); } } else if (limitType == "ControlBoardVelocity") { @@ -121,8 +129,8 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) Log::getSingleton().error("Failed to get limits from the interface."); return false; } - m_limits->min[i] = min; - m_limits->max[i] = max; + m_limits->min[i] = deg2rad(min); + m_limits->max[i] = deg2rad(max); } } @@ -149,19 +157,25 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) std::string joint = getConfiguration().getControlledJoints()[i]; // Get its index - iDynTree::LinkIndex jointIndex = model.getLinkIndex(joint); + iDynTree::JointIndex jointIndex = model.getJointIndex(joint); + + if (jointIndex == iDynTree::JOINT_INVALID_INDEX) { + Log::getSingleton().error("Invalid iDynTree joint index."); + return false; + } // Get the joint from the model p_joint = model.getJoint(jointIndex); if (!p_joint->hasPosLimits()) { Log::getSingleton().warning("Joint " + joint + " has no model limits."); - // TODO: test how these values are interpreted by Simulink - min = -std::numeric_limits::infinity(); - max = std::numeric_limits::infinity(); + m_limits->min[i] = -std::numeric_limits::infinity(); + m_limits->max[i] = std::numeric_limits::infinity(); } else { p_joint->getPosLimits(0, min, max); + m_limits->min[i] = min; + m_limits->max[i] = max; } } } From 5c9912cd42de472d92442a57a79958f483377f53 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 10:55:27 +0000 Subject: [PATCH 55/89] GetMeasurement: changed some member names for clarity --- toolbox/include/GetMeasurement.h | 12 ++++++------ toolbox/src/GetMeasurement.cpp | 28 ++++++++++++++-------------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/toolbox/include/GetMeasurement.h b/toolbox/include/GetMeasurement.h index 101aed7af..5caec2f71 100644 --- a/toolbox/include/GetMeasurement.h +++ b/toolbox/include/GetMeasurement.h @@ -5,10 +5,10 @@ namespace wbt { class GetMeasurement; - enum EstimateType { - ESTIMATE_JOINT_POS, - ESTIMATE_JOINT_VEL, - ESTIMATE_JOINT_ACC, + enum MeasuredType { + MEASUREMENT_JOINT_POS, + MEASUREMENT_JOINT_VEL, + MEASUREMENT_JOINT_ACC, ESTIMATE_JOINT_TORQUE }; } @@ -17,8 +17,8 @@ namespace wbt { class wbt::GetMeasurement : public wbt::WBBlock { private: - std::vector m_estimate; - wbt::EstimateType m_estimateType; + std::vector m_measurement; + wbt::MeasuredType m_measuredType; static void deg2rad(std::vector& v); public: diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp index 7271a65fd..536ec00aa 100644 --- a/toolbox/src/GetMeasurement.cpp +++ b/toolbox/src/GetMeasurement.cpp @@ -16,7 +16,7 @@ const std::string GetMeasurement::ClassName = "GetMeasurement"; void GetMeasurement::deg2rad(std::vector& v) { - const double deg2rad = (2 * M_PI) / 180.0; + const double deg2rad = M_PI / 180.0; for (auto& element : v) { element *= deg2rad; } @@ -77,13 +77,13 @@ bool GetMeasurement::initialize(const BlockInformation* blockInfo) } if (informationType == "Joints Position") { - m_estimateType = wbt::ESTIMATE_JOINT_POS; + m_measuredType = wbt::MEASUREMENT_JOINT_POS; } else if (informationType == "Joints Velocity") { - m_estimateType = wbt::ESTIMATE_JOINT_VEL; + m_measuredType = wbt::MEASUREMENT_JOINT_VEL; } else if (informationType == "Joints Acceleration") { - m_estimateType = wbt::ESTIMATE_JOINT_ACC; + m_measuredType = wbt::MEASUREMENT_JOINT_ACC; } else if (informationType == "Joints Torque") { - m_estimateType = wbt::ESTIMATE_JOINT_TORQUE; + m_measuredType = wbt::ESTIMATE_JOINT_TORQUE; } else { Log::getSingleton().error("Estimate not supported."); return false; @@ -91,7 +91,7 @@ bool GetMeasurement::initialize(const BlockInformation* blockInfo) // Initialize the size of the output vector const unsigned dofs = getConfiguration().getNumberOfDoFs(); - m_estimate.reserve(dofs); + m_measurement.resize(dofs); // Retain the ControlBoardRemapper if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { @@ -118,8 +118,8 @@ bool GetMeasurement::terminate(const BlockInformation* blockInfo) bool GetMeasurement::output(const BlockInformation* blockInfo) { bool ok; - switch (m_estimateType) { - case ESTIMATE_JOINT_POS: { + switch (m_measuredType) { + case MEASUREMENT_JOINT_POS: { // Get the interface yarp::dev::IEncoders* iEncoders = nullptr; if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { @@ -127,11 +127,11 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) return false; } // Get the measurement - deg2rad(m_estimate); ok = iEncoders->getEncoders(m_measurement.data()); + deg2rad(m_measurement); break; } - case ESTIMATE_JOINT_VEL: { + case MEASUREMENT_JOINT_VEL: { // Get the interface yarp::dev::IEncoders* iEncoders = nullptr; if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { @@ -139,11 +139,11 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) return false; } // Get the measurement - deg2rad(m_estimate); ok = iEncoders->getEncoderSpeeds(m_measurement.data()); + deg2rad(m_measurement); break; } - case ESTIMATE_JOINT_ACC: { + case MEASUREMENT_JOINT_ACC: { // Get the interface yarp::dev::IEncoders* iEncoders = nullptr; if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { @@ -151,8 +151,8 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) return false; } // Get the measurement - deg2rad(m_estimate); ok = iEncoders->getEncoderAccelerations(m_measurement.data()); + deg2rad(m_measurement); break; } case ESTIMATE_JOINT_TORQUE: { @@ -177,7 +177,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) } Signal signal = blockInfo->getOutputPortSignal(0); - signal.setBuffer(m_estimate.data(), blockInfo->getOutputPortWidth(0)); + signal.setBuffer(m_measurement.data(), blockInfo->getOutputPortWidth(0)); return true; } From 3a0d018acbfc7d85a29fa9afa0149bb01f850885 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 11:00:34 +0000 Subject: [PATCH 56/89] SetReferences: Position mode now calls setRefSpeeds() The speed, used internally by the yarp interface, must be initialized in order to obtain the wanted bahavior. In the future this might become a mask parameter. --- toolbox/src/SetReferences.cpp | 33 ++++++++++++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 4255344b8..90edb1c6b 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -19,7 +19,7 @@ SetReferences::SetReferences() void SetReferences::rad2deg(std::vector& v) { - const double rad2deg = 180.0 / (2 * M_PI); + const double rad2deg = 180.0 / M_PI; for (auto& element : v) { element *= rad2deg; } @@ -113,6 +113,26 @@ bool SetReferences::initialize(const BlockInformation* blockInfo) return false; } + // The Position mode is used to set a discrete reference, and then the yarp interface + // is responsible to generate a trajectory to reach this setpoint. + // The generated trajectory takes an additional parameter: the speed. + // If not properly initialized, this contol mode does not work as expected. + if (controlType == "Position") { + // Get the interface + yarp::dev::IPositionControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { + Log::getSingleton().error("Failed to get IPositionControl interface."); + return false; + } + // TODO: Set this parameter from the mask + std::vector speedInitalization(dofs, 10.0); + // Set the references + if (!interface->setRefSpeeds(speedInitalization.data())) { + Log::getSingleton().error("Failed to initialize speed references."); + return false; + } + } + // Retain the ControlBoardRemapper if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); @@ -158,8 +178,15 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) bool SetReferences::initializeInitialConditions(const BlockInformation* /*blockInfo*/) { - // Simply reset the variable m_resetControlMode. - // It will be read at the first cycle of output. + // This function is called when a subsystem with Enable / Disable support is used. + // In this case all the blocks in this subsystem are configured and initialize, + // but if they are disabled, output() is not called. + // This initializeInitialConditions method is called when the block is enabled, + // and in this case the control mode should be set. + // + // It is worth noting that this toolbox disables parameters to be tunable for + // all the blocks + m_resetControlMode = true; return true; } From 9993d136d2abc76eab819582a661085ce6060f58 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 11:13:47 +0000 Subject: [PATCH 57/89] ModelPartitioner: fixed the forwarding of i/o data This block requires a lot of information for dynamically partitioning a vector containing controlledJoints data into multiple signals representing the control boards the joints belong to. Furthermore, this block is the only one which needs to retrieve the KinDynComputations model and RemoteControlBoardRemapper already from the configureSizeAndPorts() step. --- toolbox/include/ModelPartitioner.h | 4 +- toolbox/include/base/RobotInterface.h | 45 +++++++- toolbox/src/ModelPartitioner.cpp | 97 ++++++++++------- toolbox/src/base/RobotInterface.cpp | 145 +++++++++++++++++++------- 4 files changed, 216 insertions(+), 75 deletions(-) diff --git a/toolbox/include/ModelPartitioner.h b/toolbox/include/ModelPartitioner.h index 32c57c098..853c3f0c3 100644 --- a/toolbox/include/ModelPartitioner.h +++ b/toolbox/include/ModelPartitioner.h @@ -16,10 +16,12 @@ class wbt::ModelPartitioner : public wbt::WBBlock bool m_yarp2WBI; std::shared_ptr m_jointsMapString; + std::shared_ptr m_controlledJointsMapCB; + std::shared_ptr m_controlBoardIdxLimit; public: static const std::string ClassName; - + ModelPartitioner() = default; ~ModelPartitioner() override = default; diff --git a/toolbox/include/base/RobotInterface.h b/toolbox/include/base/RobotInterface.h index 489bf0f35..76d338de3 100644 --- a/toolbox/include/base/RobotInterface.h +++ b/toolbox/include/base/RobotInterface.h @@ -36,8 +36,12 @@ namespace wbt { typedef int jointIdx_yarp; typedef int jointIdx_iDynTree; typedef unsigned cb_idx; + typedef unsigned max_cb_idx; + typedef unsigned controlledJointIdxCB; typedef std::unordered_map> JointsMapIndex; typedef std::unordered_map> JointsMapString; + typedef std::unordered_map ControlledJointsMapCB; + typedef std::unordered_map ControlBoardIdxLimit; } /** @@ -106,7 +110,8 @@ class wbt::RobotInterface // Maps used to store infos about yarp's and idyntree's internal joint indexing std::shared_ptr m_jointsMapIndex; std::shared_ptr m_jointsMapString; - // std::unordered_map, joint_name> m_yarp2joint; + std::shared_ptr m_controlledJointsMapCB; + std::shared_ptr m_controlBoardIdxLimit; // Configuration from Simulink Block's parameters const wbt::Configuration m_config; @@ -159,6 +164,17 @@ class wbt::RobotInterface * @return True if the map has been created successfully */ bool mapDoFs(); + + /** + * Create a RemoteControlBoard object for a given remoteName + * + * @see mapDoFs + * + * @param remoteName [in] Name of the remote from which the remote control board is be initialized + * @param controlBoard [out] Smart pointer to the allocated remote control board + * @return True if success + */ + bool getSingleControlBoard(const std::string& remoteName, std::unique_ptr& controlBoard); public: // CONSTRUCTOR / DESTRUCTOR @@ -179,7 +195,7 @@ class wbt::RobotInterface const wbt::Configuration& getConfiguration() const; /** - * Get the map between model joint namesand the YARP representation (Control Board and + * Get the map between model joint names and the YARP representation (Control Board and * joint index) * * @return The joint map @@ -194,6 +210,31 @@ class wbt::RobotInterface */ const std::shared_ptr getJointsMapIndex(); + /** + * Get the map between model joint names and the index representing their relative ordering + * inside the controlledJoints vector relative to their ControlBoard. + * + * @note For example, if the joints are + * \verbatim j1_cb1 j2_cb1 j3_cb1 j1_cb2 j2_cb2 j1_cb3 \endverbatim, the map links them to + * \verbatim 0 1 2 0 1 0 \end + * + * @return The joint map + */ + const std::shared_ptr getControlledJointsMapCB(); + + /** + * Get the map between the ControlBoard index inside the RemoteControlBoardRemapper + * and the number of the controlled joints which belongs to it. + * + * @note For example, if the joints are + * \verbatim j1_cb1 j2_cb1 j3_cb1 j1_cb2 j2_cb2 j1_cb3 \endverbatim, the generated map is + * \verbatim {{0,3}{1,2}{2,1}} \endverbatim + * Note that the map key is 0-indexed. + * + * @return The control board limit map + */ + const std::shared_ptr getControlBoardIdxLimit(); + /** * Get the object to operate on the configured model * diff --git a/toolbox/src/ModelPartitioner.cpp b/toolbox/src/ModelPartitioner.cpp index 2c54458bd..e987604f8 100644 --- a/toolbox/src/ModelPartitioner.cpp +++ b/toolbox/src/ModelPartitioner.cpp @@ -5,6 +5,7 @@ #include "RobotInterface.h" #include "Signal.h" #include "Log.h" +#include using namespace wbt; @@ -12,7 +13,7 @@ const std::string ModelPartitioner::ClassName = "ModelPartitioner"; unsigned ModelPartitioner::numberOfParameters() { - return 1; + return WBBlock::numberOfParameters() + 1; } bool ModelPartitioner::configureSizeAndPorts(BlockInformation* blockInfo) @@ -25,8 +26,10 @@ bool ModelPartitioner::configureSizeAndPorts(BlockInformation* blockInfo) // 1) Boolean specifying if yarp2WBI (true) or WBI2yarp (false) // - bool yarp2WBI; - if (!blockInfo->getBooleanParameterAtIndex(1, yarp2WBI)) { + bool yarp2WBI = false; + unsigned yarp2WBIParameterIdx = WBBlock::numberOfParameters() + 1; + + if (!blockInfo->getBooleanParameterAtIndex(yarp2WBIParameterIdx, yarp2WBI)) { Log::getSingleton().error("Failed to get input parameters."); return false; } @@ -53,12 +56,14 @@ bool ModelPartitioner::configureSizeAndPorts(BlockInformation* blockInfo) numberOfInputs = 1; ok = blockInfo->setNumberOfInputPorts(numberOfInputs); blockInfo->setInputPortVectorSize(0, getConfiguration().getNumberOfDoFs()); + blockInfo->setInputPortType(0, PortDataTypeDouble); } else { numberOfInputs = controlBoardsNumber; ok = blockInfo->setNumberOfInputPorts(numberOfInputs); // Set the size as dynamic for (unsigned i = 0; i < numberOfInputs; ++i) { + blockInfo->setInputPortVectorSize(i, -1); blockInfo->setInputPortType(i, PortDataTypeDouble); } } @@ -100,6 +105,28 @@ bool ModelPartitioner::configureSizeAndPorts(BlockInformation* blockInfo) blockInfo->setOutputPortType(0, PortDataTypeDouble); } + // For some reason, the output ports widths in the yarp2WBI case are not detected + // properly by Simulink if set as DYNAMICALLY_SIZED (-1). + // Set them manually using the m_controlBoardIdxLimit map. + // + // Doing this now has the disadvantage of allocating the KinDynComputations and the + // RemoteControlBoardRemapper already at this early stage, but this happens only at the + // first execution of the model if the joint list doesn't change. + // + if (yarp2WBI) { + m_controlBoardIdxLimit = getRobotInterface()->getControlBoardIdxLimit(); + if (!m_controlBoardIdxLimit) { + Log::getSingleton().error("Failed to get the map CBIdx <--> CBMaxIdx."); + return false; + } + for (const auto& cb : *m_controlBoardIdxLimit) { + if (!blockInfo->setOutputPortVectorSize(cb.first, cb.second)) { + Log::getSingleton().error("Failed to set ouput port size reading them from cb map."); + return false; + } + } + } + if (!ok) { Log::getSingleton().error("Failed to set output port number."); return false; @@ -112,14 +139,22 @@ bool ModelPartitioner::initialize(const BlockInformation* blockInfo) { if (!WBBlock::initialize(blockInfo)) return false; - if (!blockInfo->getBooleanParameterAtIndex(1, m_yarp2WBI)) { + unsigned yarp2WBIParameterIdx = WBBlock::numberOfParameters() + 1; + if (!blockInfo->getBooleanParameterAtIndex(yarp2WBIParameterIdx, m_yarp2WBI)) { Log::getSingleton().error("Failed to get input parameters."); return false; } m_jointsMapString = getRobotInterface()->getJointsMapString(); + m_controlledJointsMapCB = getRobotInterface()->getControlledJointsMapCB(); if (!m_jointsMapString) { + Log::getSingleton().error("Failed to get the joint map iDynTree <--> Yarp."); + return false; + } + + if (!m_controlledJointsMapCB) { + Log::getSingleton().error("Failed to get the joint map iDynTree <--> controlledJointsIdx."); return false; } @@ -133,48 +168,38 @@ bool ModelPartitioner::terminate(const BlockInformation* blockInfo) bool ModelPartitioner::output(const BlockInformation* blockInfo) { - if (m_yarp2WBI) { - // Input - Signal jointListSignal = blockInfo->getInputPortSignal(0); + Signal dofsSignal; - // Outputs - Signal ithControlBoardSignal; + if (m_yarp2WBI) { + dofsSignal = blockInfo->getInputPortSignal(0); - for (unsigned ithJoint = 0; ithJoint < blockInfo->getInputPortWidth(0); ++ithJoint) { - // Get the ControlBoard number the ith joint belongs, and its index into the CB itself - std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; - const auto& mappedJointInfos = m_jointsMapString->at(ithJointName); - cb_idx controlBoardOfJoint = mappedJointInfos.first; - jointIdx_yarp yarpIndexOfJoint = mappedJointInfos.second; + for (unsigned ithJoint = 0; ithJoint < getConfiguration().getNumberOfDoFs(); ++ithJoint) { + const std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; + // Get the ControlBoard number the ith joint belongs + const cb_idx& controlBoardOfJoint = m_jointsMapString->at(ithJointName).first; + // Get the index of the ith joint inside the controlledJoints vector relative to + // its ControlBoard + const controlledJointIdxCB contrJointIdxCB = m_controlledJointsMapCB->at(ithJointName); // Get the data to forward - Data value = jointListSignal.get(ithJoint); - - // Forward the value to the correct output / output index - ithControlBoardSignal = blockInfo->getOutputPortSignal(controlBoardOfJoint); - ithControlBoardSignal.set(yarpIndexOfJoint, value.doubleData()); + Signal ithOutput = blockInfo->getOutputPortSignal(controlBoardOfJoint); + ithOutput.set(contrJointIdxCB, dofsSignal.get(ithJoint).doubleData()); } } else { - // Inputs - Signal ithControlBoardSignal; + dofsSignal = blockInfo->getOutputPortSignal(0); - // Output - Signal jointListSignal = blockInfo->getOutputPortSignal(0); - - for (unsigned ithJoint = 0; ithJoint < blockInfo->getOutputPortWidth(0); ++ithJoint) { - // Get the ControlBoard number the ith joint belongs, and its index into the CB itself - std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; - const auto& mappedJointInfos = m_jointsMapString->at(ithJointName); - cb_idx controlBoardOfJoint = mappedJointInfos.first; - jointIdx_yarp yarpIndexOfJoint = mappedJointInfos.second; + for (unsigned ithJoint = 0; ithJoint < getConfiguration().getNumberOfDoFs(); ++ithJoint) { + const std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; + // Get the ControlBoard number the ith joint belongs + const cb_idx& controlBoardOfJoint = m_jointsMapString->at(ithJointName).first; + // Get the index of the ith joint inside the controlledJoints vector relative to + // its ControlBoard + const controlledJointIdxCB contrJointIdxCB = m_controlledJointsMapCB->at(ithJointName); // Get the data to forward - ithControlBoardSignal = blockInfo->getInputPortSignal(controlBoardOfJoint); - Data value = ithControlBoardSignal.get(ithJoint); - - // Forward the value to the correct output index - jointListSignal.set(yarpIndexOfJoint, value.doubleData()); + const Signal ithInput = blockInfo->getInputPortSignal(controlBoardOfJoint); + dofsSignal.set(ithJoint, ithInput.get(contrJointIdxCB).doubleData()); } } return true; diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp index bf96983f2..e637d40e5 100644 --- a/toolbox/src/base/RobotInterface.cpp +++ b/toolbox/src/base/RobotInterface.cpp @@ -2,6 +2,7 @@ #include "Log.h" #include #include +#include #include #include #include @@ -9,9 +10,9 @@ #include #include #include +#include namespace wbt { - // The declaration of the following template specializations are required only by GCC using namespace yarp::dev; template <> bool RobotInterface::getInterface(IControlMode2*& interface); @@ -51,28 +52,54 @@ RobotInterface::~RobotInterface() assert(m_robotDeviceCounter == 0); } +bool RobotInterface::getSingleControlBoard(const std::string& remoteName, std::unique_ptr& controlBoard) { + // Configure the single control board + yarp::os::Property options; + options.clear(); + options.put("device", "remote_controlboard"); + options.put("remote", remoteName); + options.put("local", m_config.getLocalName() + "/CBtmp"); + options.put("writeStrict", "on"); + + // Initialize the device + controlBoard = std::unique_ptr(new yarp::dev::PolyDriver()); + if (!controlBoard) { + Log::getSingleton().error("Failed to retain the RemoteControlBoard "); + Log::getSingleton().errorAppend("used for mapping iDynTree - YARP DoFs."); + return false; + } + + // Try to open the control board + if (!controlBoard->open(options) || !controlBoard->isValid()) { + Log::getSingleton().error("Unable to open RemoteControlBoard " + remoteName); + return false; + } + + return true; +} + bool RobotInterface::mapDoFs() { - std::unique_ptr controlBoard; + // Initialize the network + yarp::os::Network::init(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; + } + std::vector> iAxisInfos; - yarp::os::Property options; for (unsigned cbNum = 0; cbNum < m_config.getControlBoardsNames().size(); ++cbNum) { - // Configure the single control board - options.clear(); - options.put("device","remotecontrolboard"); + + std::unique_ptr controlBoard; const std::string prefix = "/" + m_config.getRobotName() + "/"; const std::string remoteName = prefix + m_config.getControlBoardsNames().at(cbNum); - options.put("remote", remoteName); - options.put("localPortPrefix", "WBTtmp"); - // Try to open the control board - if (!controlBoard->open(options) || !controlBoard->isValid()) { - Log::getSingleton().error("Unable to open RemoteControlBoard " + remoteName); + if (!getSingleControlBoard(remoteName, controlBoard)) { return false; } - // Get an IAxisInfo object from the device + // Get an IAxisInfo object from the interface std::unique_ptr iAxisInfo; yarp::dev::IAxisInfo* iAxisInfoPtr = iAxisInfo.get(); controlBoard->view(iAxisInfoPtr); @@ -92,6 +119,7 @@ bool RobotInterface::mapDoFs() return false; } + int found = -1; // Iterate all the joints in the selected Control Board for (unsigned axis = 0; axis < numAxes; ++axis) { std::string axisName; @@ -100,13 +128,18 @@ bool RobotInterface::mapDoFs() return false; } // Look if axisName is a controlledJoint - bool found = false; for (const auto& controlledJoint : m_config.getControlledJoints()) { if (controlledJoint == axisName) { - // Get the iDynTree index - const auto& model = getKinDynComputations()->model(); - iDynTree::LinkIndex iDynLinkIdx = model.getLinkIndex(axisName); - if (iDynLinkIdx == iDynTree::LINK_INVALID_INDEX) { + found++; + // Get the iDynTree index from the model + const auto& kinDynComp = getKinDynComputations(); + if (!kinDynComp) { + Log::getSingleton().error("Failed to get KinDynComputations."); + return false; + } + const auto& model = kinDynComp->model(); + iDynTree::JointIndex iDynJointIdx = model.getJointIndex(axisName); + if (iDynJointIdx == iDynTree::JOINT_INVALID_INDEX) { Log::getSingleton().error("Joint " + axisName + " exists in the " + remoteName + "control board but not in the model."); return false; @@ -118,27 +151,42 @@ bool RobotInterface::mapDoFs() if (!m_jointsMapString) { m_jointsMapString = std::make_shared(); } + if (!m_controlledJointsMapCB) { + m_controlledJointsMapCB = std::make_shared(); + } + if (!m_controlBoardIdxLimit) { + m_controlBoardIdxLimit = std::make_shared(); + } // Create a new entry in the map objects - m_jointsMapString->at(controlledJoint) = {cbNum, axis}; - m_jointsMapIndex->at(static_cast(iDynLinkIdx)) = {cbNum, axis}; - found = true; + m_jointsMapString->insert(std::make_pair(controlledJoint, std::make_pair(cbNum, axis))); + m_jointsMapIndex->insert(std::make_pair(static_cast(iDynJointIdx), + std::make_pair(cbNum, axis))); + m_controlledJointsMapCB->insert(std::make_pair(controlledJoint, found)); + (*m_controlBoardIdxLimit)[cbNum] = found + 1; break; } } - // Notify that the control board just checked is not used by any joint - // of the controlledJoints list - if (!found) { - Log::getSingleton().warning("No controlled joints found in " + - m_config.getControlBoardsNames().at(cbNum) + - " control board. It might be unsed."); - } + } - } - if (!controlBoard->close()) { - Log::getSingleton().error("Unable to close the device of the Control Board."); - return false; + // Notify that the control board just checked is not used by any joint + // of the controlledJoints list + if (found < 0) { + Log::getSingleton().warning("No controlled joints found in " + + m_config.getControlBoardsNames().at(cbNum) + + " control board. It might be unused."); + } + + // Close the ControlBoard device + if (!controlBoard->close()) { + Log::getSingleton().error("Unable to close the interface of the Control Board."); + return false; + } } + + // Initialize the network + yarp::os::Network::fini(); + return true; } @@ -152,22 +200,21 @@ const wbt::Configuration& RobotInterface::getConfiguration() const const std::shared_ptr RobotInterface::getJointsMapString() { - if (m_jointsMapString->empty()) { + if (!m_jointsMapString || m_jointsMapString->empty()) { if (!mapDoFs()) { - Log::getSingleton().error("Failed to create the Yarp - iDynTree joint map."); + Log::getSingleton().error("Failed to create the joint maps."); return nullptr; } } - assert (m_jointsMapString); return m_jointsMapString; } const std::shared_ptr RobotInterface::getJointsMapIndex() { - if (m_jointsMapIndex->empty()) { + if (!m_jointsMapIndex || m_jointsMapIndex->empty()) { if (!mapDoFs()) { - Log::getSingleton().error("Failed to create the Yarp - iDynTree joint map."); + Log::getSingleton().error("Failed to create the joint maps."); return nullptr; } } @@ -176,6 +223,32 @@ const std::shared_ptr RobotInterface::getJointsMapIndex() return m_jointsMapIndex; } +const std::shared_ptr RobotInterface::getControlledJointsMapCB() +{ + if (!m_controlledJointsMapCB || m_controlledJointsMapCB->empty()) { + if (!mapDoFs()) { + Log::getSingleton().error("Failed to create joint maps."); + return nullptr; + } + } + + assert (m_controlledJointsMapCB); + return m_controlledJointsMapCB; +} + +const std::shared_ptr RobotInterface::getControlBoardIdxLimit() +{ + if (!m_controlBoardIdxLimit || m_controlBoardIdxLimit->empty()) { + if (!mapDoFs()) { + Log::getSingleton().error("Failed to create joint maps."); + return nullptr; + } + } + + assert (m_controlBoardIdxLimit); + return m_controlBoardIdxLimit; +} + const std::shared_ptr RobotInterface::getKinDynComputations() { if (m_kinDynComp) { From 212ffaf7837d0af52929eb5c8f7086513bffae85 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 11:17:05 +0000 Subject: [PATCH 58/89] YarpRead: fixed indexing of signals This block has optional outputs, and the indexing in the output() method was not correct --- toolbox/src/YarpRead.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/toolbox/src/YarpRead.cpp b/toolbox/src/YarpRead.cpp index 9e5807cce..939446e05 100644 --- a/toolbox/src/YarpRead.cpp +++ b/toolbox/src/YarpRead.cpp @@ -178,8 +178,15 @@ bool YarpRead::terminate(const BlockInformation* /*blockInfo*/) bool YarpRead::output(const BlockInformation* blockInfo) { - int timeStampPortIndex = 1; - int connectionStatusPortIndex = 1; + int timeStampPortIndex = 0; + int connectionStatusPortIndex = 0; + + if (m_shouldReadTimestamp) { + timeStampPortIndex = 1; + } + if (!m_autoconnect) { + connectionStatusPortIndex = timeStampPortIndex + 1; + } yarp::sig::Vector* v = m_port->read(m_blocking); // Read from the port. Waits until data arrives. @@ -197,7 +204,9 @@ bool YarpRead::output(const BlockInformation* blockInfo) Signal signal = blockInfo->getOutputPortSignal(0); // Crop the buffer if it exceeds the OutputPortWidth. - signal.setBuffer(v->data(), std::min(blockInfo->getOutputPortWidth(0), (unsigned)v->size())); + signal.setBuffer(v->data(), + std::min(blockInfo->getOutputPortWidth(0), + static_cast(v->size()))); if (!m_autoconnect) { Signal statusPort = blockInfo->getOutputPortSignal(connectionStatusPortIndex); From 9cb9aa8531d29739ce477f92e32e7cd10e6732b5 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 15:59:40 +0000 Subject: [PATCH 59/89] Improved logging support and log messages --- toolbox/include/base/Log.h | 11 +++++++---- toolbox/src/GetLimits.cpp | 8 ++++++-- toolbox/src/GetMeasurement.cpp | 2 +- toolbox/src/SetLowLevelPID.cpp | 15 ++++++++++----- toolbox/src/SetReferences.cpp | 2 +- toolbox/src/YarpRead.cpp | 6 +++++- toolbox/src/base/Log.cpp | 25 +++++++++++++++++++++---- toolbox/src/base/RobotInterface.cpp | 4 +++- 8 files changed, 54 insertions(+), 19 deletions(-) diff --git a/toolbox/include/base/Log.h b/toolbox/include/base/Log.h index 699c431b6..e3f9d6764 100644 --- a/toolbox/include/base/Log.h +++ b/toolbox/include/base/Log.h @@ -18,16 +18,19 @@ class wbt::Log std::vector warnings; std::string prefix; - static std::string serializeVectorString(std::vector v, std::string prefix=""); + static std::string serializeVectorString(std::vector v, const std::string& prefix=""); public: static wbt::Log& getSingleton(); - void error(std::string errorMessage); - void warning(std::string warningMessage); + void error(const std::string& errorMessage); + void warning(const std::string& warningMessage); + + void errorAppend(const std::string& errorMessage); + void warningAppend(const std::string& warningMessage); void resetPrefix(); - void setPrefix(std::string prefixMessage); + void setPrefix(const std::string& prefixMessage); std::string getErrors() const; std::string getWarnings() const; diff --git a/toolbox/src/GetLimits.cpp b/toolbox/src/GetLimits.cpp index 44648995e..ca668fad3 100644 --- a/toolbox/src/GetLimits.cpp +++ b/toolbox/src/GetLimits.cpp @@ -103,7 +103,7 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) if (limitType == "ControlBoardPosition" || limitType == "ControlBoardVelocity") { // Retain the control board remapper if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { - Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); + Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); return false; } // Get the interface @@ -173,7 +173,11 @@ bool GetLimits::initialize(const BlockInformation* blockInfo) m_limits->max[i] = std::numeric_limits::infinity(); } else { - p_joint->getPosLimits(0, min, max); + if (!p_joint->getPosLimits(0, min, max)) { + Log::getSingleton().error("Failed to get joint limits from the URDF model "); + Log::getSingleton().errorAppend("for the joint " + joint + "."); + return false; + } m_limits->min[i] = min; m_limits->max[i] = max; } diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp index 536ec00aa..a245105c6 100644 --- a/toolbox/src/GetMeasurement.cpp +++ b/toolbox/src/GetMeasurement.cpp @@ -95,7 +95,7 @@ bool GetMeasurement::initialize(const BlockInformation* blockInfo) // Retain the ControlBoardRemapper if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { - Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); + Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); return false; } diff --git a/toolbox/src/SetLowLevelPID.cpp b/toolbox/src/SetLowLevelPID.cpp index 6ce3a3aba..911b039d8 100644 --- a/toolbox/src/SetLowLevelPID.cpp +++ b/toolbox/src/SetLowLevelPID.cpp @@ -105,7 +105,12 @@ bool SetLowLevelPID::readWBTPidConfigObject(const BlockInformation* blockInfo) jointNamesFromParameters.push_back(joint); } - assert(Pvector.size() == Ivector.size() == Dvector.size() == jointNamesFromParameters.size()); + if (Pvector.size() != Ivector.size() || + Ivector.size() != Dvector.size() || + Dvector.size() != jointNamesFromParameters.size()) { + Log::getSingleton().error("Sizes of P, I, D, and jointList elements are not the same."); + return false; + } // Store this data into a private member map for (unsigned i = 0; i < jointNamesFromParameters.size(); ++i) { @@ -118,8 +123,8 @@ bool SetLowLevelPID::readWBTPidConfigObject(const BlockInformation* blockInfo) m_pidJointsFromParameters[jointNamesFromParameters[i]] = {Pvector[i], Ivector[i], Dvector[i]}; } else { - Log::getSingleton().warning("Attempted to set PID of joint " + jointNamesFromParameters[i] + - " non currently controlled. Skipping it."); + Log::getSingleton().warning("Attempted to set PID of joint " + jointNamesFromParameters[i]); + Log::getSingleton().warningAppend(" non currently controlled. Skipping it."); } } @@ -156,12 +161,12 @@ bool SetLowLevelPID::initialize(const BlockInformation* blockInfo) // Reading the WBTPIDConfig matlab class if (!readWBTPidConfigObject(blockInfo)) { Log::getSingleton().error("Failed to parse the WBTPIDConfig object."); - return 1; + return false; } // Retain the RemoteControlBoardRemapper if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { - Log::getSingleton().error("Failed to retain the control board remapper."); + Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); return false; } diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 90edb1c6b..05317332e 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -135,7 +135,7 @@ bool SetReferences::initialize(const BlockInformation* blockInfo) // Retain the ControlBoardRemapper if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { - Log::getSingleton().error("Failed to initialize the Robot Interface containing the Control Board Remapper."); + Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); return false; } diff --git a/toolbox/src/YarpRead.cpp b/toolbox/src/YarpRead.cpp index 939446e05..a25dc31db 100644 --- a/toolbox/src/YarpRead.cpp +++ b/toolbox/src/YarpRead.cpp @@ -194,7 +194,11 @@ bool YarpRead::output(const BlockInformation* blockInfo) if (m_shouldReadTimestamp) { connectionStatusPortIndex++; yarp::os::Stamp timestamp; - m_port->getEnvelope(timestamp); + if (!m_port->getEnvelope(timestamp)) { + Log::getSingleton().error("Failed to read port envelope (timestamp)."); + Log::getSingleton().errorAppend("Be sure that the input port actually writes this data."); + return false; + } Signal pY1 = blockInfo->getOutputPortSignal(timeStampPortIndex); pY1.set(0, timestamp.getCount()); diff --git a/toolbox/src/base/Log.cpp b/toolbox/src/base/Log.cpp index 9ee6abdaa..47769393c 100644 --- a/toolbox/src/base/Log.cpp +++ b/toolbox/src/base/Log.cpp @@ -10,17 +10,34 @@ Log& Log::getSingleton() return logInstance; } -void Log::error(std::string errorMessage) +void Log::error(const std::string& errorMessage) { errors.push_back(errorMessage); } -void Log::warning(std::string warningMessage) +void Log::warning(const std::string& warningMessage) { warnings.push_back(warningMessage); } -void Log::setPrefix(std::string prefixMessage) +void Log::errorAppend(const std::string& errorMessage) +{ + if (errors.empty()) { + error(errorMessage); + return; + } + errors.back() += errorMessage; +} + +void Log::warningAppend(const std::string& warningMessage) +{ + if (warnings.empty()) { + warning(warningMessage); + } + warnings.back() += warningMessage; +} + +void Log::setPrefix(const std::string& prefixMessage) { prefix = prefixMessage; } @@ -30,7 +47,7 @@ void Log::resetPrefix() prefix.clear(); } -std::string Log::serializeVectorString(std::vector v, std::string prefix) +std::string Log::serializeVectorString(std::vector v, const std::string& prefix) { std::stringstream output; std::ostream_iterator output_iterator(output, "\n"); diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp index e637d40e5..9755e4d7a 100644 --- a/toolbox/src/base/RobotInterface.cpp +++ b/toolbox/src/base/RobotInterface.cpp @@ -351,6 +351,7 @@ bool RobotInterface::retainRemoteControlBoardRemapper() } if (!initializeRemoteControlBoardRemapper()) { + Log::getSingleton().error("First initialization of the RemoteControlBoardRemapper failed."); return false; } @@ -433,7 +434,8 @@ bool RobotInterface::initializeModel() iDynTree::ModelLoader mdlLoader; if (!mdlLoader.loadReducedModelFromFile(urdf_file_path, controlledJoints)) { Log::getSingleton().error("ToolboxSingleton: impossible to load " + urdf_file + "."); - Log::getSingleton().error("Probably the joint list contains an entry not present in the urdf model."); + Log::getSingleton().errorAppend("\nPossible causes: file not found, or the joint "); + Log::getSingleton().errorAppend("list contains an entry not present in the urdf model."); return false; } From e210a17d10a3491b9d008762cca96f6243e0fb24 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 16:01:06 +0000 Subject: [PATCH 60/89] Initialize to nullptr the buffers of the Signal class --- toolbox/include/base/Signal.h | 2 +- toolbox/src/base/Signal.cpp | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/toolbox/include/base/Signal.h b/toolbox/include/base/Signal.h index 0fbe27f3c..21109b7c7 100644 --- a/toolbox/include/base/Signal.h +++ b/toolbox/include/base/Signal.h @@ -19,7 +19,7 @@ class wbt::Signal void* contiguousData; public: - Signal() = default; + Signal(); void initSignalType(wbt::PortDataType type, bool constPort); diff --git a/toolbox/src/base/Signal.cpp b/toolbox/src/base/Signal.cpp index 67e6d6b42..028daef88 100644 --- a/toolbox/src/base/Signal.cpp +++ b/toolbox/src/base/Signal.cpp @@ -3,6 +3,11 @@ using namespace wbt; +Signal::Signal() +: nonContiguousData(nullptr) +, contiguousData(nullptr) +{} + void Signal::initSignalType(wbt::PortDataType type, bool constPort) { this->portType = type; From 8ea9fe7d2ca6d5a25a16e048934636e2086eb21e Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 16:02:06 +0000 Subject: [PATCH 61/89] Use const arguments in Configuration::setParameters() --- toolbox/include/base/Configuration.h | 12 ++++++------ toolbox/src/base/Configuration.cpp | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/toolbox/include/base/Configuration.h b/toolbox/include/base/Configuration.h index b131ce81a..4f0884a67 100644 --- a/toolbox/include/base/Configuration.h +++ b/toolbox/include/base/Configuration.h @@ -48,12 +48,12 @@ class wbt::Configuration * @param localName Prefix appended to the opened ports * @param gravityVector The gravity vector */ - void setParameters(std::string robotName, - std::string urdfFile, - std::vector controlledJoints, - std::vector controlBoardsNames, - std::string localName, - std::array gravityVector); + void setParameters(const std::string& robotName, + const std::string& urdfFile, + const std::vector& controlledJoints, + const std::vector& controlBoardsNames, + const std::string& localName, + const std::array& gravityVector); /** * Set the name of the robot diff --git a/toolbox/src/base/Configuration.cpp b/toolbox/src/base/Configuration.cpp index 257a475f4..e4205ac9b 100644 --- a/toolbox/src/base/Configuration.cpp +++ b/toolbox/src/base/Configuration.cpp @@ -11,12 +11,12 @@ Configuration::Configuration(const std::string& confKey) // SET METHODS // =========== -void Configuration::setParameters(std::string robotName, - std::string urdfFile, - std::vector controlledJoints, - std::vector controlBoardsNames, - std::string localName, - std::array gravityVector) +void Configuration::setParameters(const std::string& robotName, + const std::string& urdfFile, + const std::vector& controlledJoints, + const std::vector& controlBoardsNames, + const std::string& localName, + const std::array& gravityVector) { setRobotName(robotName); setUrdfFile(urdfFile); From dc20f9a34dc479872f6f29956fb1bcb99cda0c6c Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 16:03:50 +0000 Subject: [PATCH 62/89] Fix passing booleans as mask parameters Sometimes the boolean are passed as double, and reading them using MxAnyType::asBool() fails --- toolbox/src/base/SimulinkBlockInformation.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/toolbox/src/base/SimulinkBlockInformation.cpp b/toolbox/src/base/SimulinkBlockInformation.cpp index 2b9688738..38c1060db 100644 --- a/toolbox/src/base/SimulinkBlockInformation.cpp +++ b/toolbox/src/base/SimulinkBlockInformation.cpp @@ -50,7 +50,18 @@ bool SimulinkBlockInformation::getScalarParameterAtIndex(unsigned parameterIndex bool SimulinkBlockInformation::getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const { + double tmpValue = 0; const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + + // The Simulink mask often doesn't store boolean data from the mask as bool but as double. + // Calling asBool() will fail in this case. If this happens, asDouble() is used as fallback. + if (MxAnyType(blockParam).asBool(value)) { + return true; + } + else if (MxAnyType(blockParam).asDouble(tmpValue)) { + value = static_cast(tmpValue); + return true; + } return MxAnyType(blockParam).asBool(value); } From 3a1941c13bf5ced45f9c0231527572dd02319bdf Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 16:06:06 +0000 Subject: [PATCH 63/89] Added method for extractring a struct from Matlab's WBTPIDConfig class The struct is then used as S-Function parameter --- +WBToolbox/WBTPIDConfig.m | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/+WBToolbox/WBTPIDConfig.m b/+WBToolbox/WBTPIDConfig.m index 9d23b523f..51999e301 100644 --- a/+WBToolbox/WBTPIDConfig.m +++ b/+WBToolbox/WBTPIDConfig.m @@ -57,6 +57,18 @@ function removePID(obj, jointName) obj.I(:,idx) = []; obj.D(:,idx) = []; end + + function value = getSimulinkParameters(obj) + if isempty(obj.P) || isempty(obj.I) || isempty(obj.D) || ... + isempty(obj.jointList) + error('Trying to get parameters from an empty object') + end + value = struct(); + value.P = obj.P; + value.I = obj.I; + value.D = obj.D; + value.jointList = obj.jointList; + end end end From f20f521f70625413948c536ea757624b2b287a49 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 16:06:39 +0000 Subject: [PATCH 64/89] Typo in Mask2WBToolboxConfig.m script --- +WBToolbox/Mask2WBToolboxConfig.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/+WBToolbox/Mask2WBToolboxConfig.m b/+WBToolbox/Mask2WBToolboxConfig.m index 0396a46a3..b9850d12c 100644 --- a/+WBToolbox/Mask2WBToolboxConfig.m +++ b/+WBToolbox/Mask2WBToolboxConfig.m @@ -22,13 +22,13 @@ WBTConfig.LocalName = stripApices(char(get_param(configBlock,'LocalName'))); ControlledJointsChar = stripApices(char(get_param(configBlock,'ControlledJoints'))); -WBTConfig.ControlledJoints = eval('base',ControlledJointsChar); +WBTConfig.ControlledJoints = evalin('base',ControlledJointsChar); ControlBoardsNamesChar = stripApices(char(get_param(configBlock,'ControlBoardsNames'))); -WBTConfig.ControlBoardsNames = eval('base',ControlBoardsNamesChar); +WBTConfig.ControlBoardsNames = evalin('base',ControlBoardsNamesChar); GravityVectorChar = stripApices(char(get_param(configBlock,'GravityVector'))); -WBTConfig.GravityVector = eval('base',GravityVectorChar); +WBTConfig.GravityVector = evalin('base',GravityVectorChar); end From ed2d5de1cf0099862bf7168575a956d4d1c7d81f Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 21 Nov 2017 16:08:34 +0000 Subject: [PATCH 65/89] Disable IK blocks --- toolbox/include/base/toolbox.h | 4 ++-- toolbox/src/base/factory.cpp | 14 ++++++-------- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/toolbox/include/base/toolbox.h b/toolbox/include/base/toolbox.h index cc0750e51..14f5a3778 100644 --- a/toolbox/include/base/toolbox.h +++ b/toolbox/include/base/toolbox.h @@ -21,6 +21,6 @@ #include "MinimumJerkTrajectoryGenerator.h" #endif #ifdef WBT_USES_IPOPT -#include "InverseKinematics.h" -#include "RemoteInverseKinematics.h" +// #include "InverseKinematics.h" #endif +// #include "RemoteInverseKinematics.h" diff --git a/toolbox/src/base/factory.cpp b/toolbox/src/base/factory.cpp index 2d1d5f828..ab7d9f9be 100644 --- a/toolbox/src/base/factory.cpp +++ b/toolbox/src/base/factory.cpp @@ -45,14 +45,12 @@ Block* Block::instantiateBlockWithClassName(std::string blockClassName) } #endif #ifdef WBT_USES_IPOPT - else if (blockClassName == wbt::InverseKinematics::ClassName) { - block = new wbt::InverseKinematics(); - } + // else if (blockClassName == InverseKinematics::ClassName) { + // block = new InverseKinematics(); + // } #endif - else if (blockClassName == wbt::RemoteInverseKinematics::ClassName) { - block = new wbt::RemoteInverseKinematics(); - } - - + // else if (blockClassName == RemoteInverseKinematics::ClassName) { + // block = new RemoteInverseKinematics(); + // } return block; } From 33532320a824dcb4603281baec14f97033657c90 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 28 Nov 2017 15:50:48 +0000 Subject: [PATCH 66/89] Use system Matlab headers in MxAnyType library --- MxAnyType/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/MxAnyType/CMakeLists.txt b/MxAnyType/CMakeLists.txt index 4faaebad7..608670096 100644 --- a/MxAnyType/CMakeLists.txt +++ b/MxAnyType/CMakeLists.txt @@ -34,8 +34,7 @@ endif() # Find Matlab resources find_package(Matlab REQUIRED MX_LIBRARY) -# target_include_directories(${VARS_PREFIX} SYSTEM PUBLIC "${Matlab_INCLUDE_DIRS}") // TODO: when committing -target_include_directories(${VARS_PREFIX} PUBLIC "${Matlab_INCLUDE_DIRS}") +target_include_directories(${VARS_PREFIX} SYSTEM PUBLIC "${Matlab_INCLUDE_DIRS}") # Setup the include directories # TODO why in the how to was INTERFACE? From b695ad4509fbaf79d3b501e54fd6f4225fc75e80 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 28 Nov 2017 15:51:38 +0000 Subject: [PATCH 67/89] Moved +WBToolbox folder into toolbox/ --- {+WBToolbox => toolbox/+WBToolbox}/BlockInitialization.m | 0 {+WBToolbox => toolbox/+WBToolbox}/Mask2WBToolboxConfig.m | 0 {+WBToolbox => toolbox/+WBToolbox}/PID.m | 0 {+WBToolbox => toolbox/+WBToolbox}/WBTPIDConfig.m | 0 {+WBToolbox => toolbox/+WBToolbox}/WBToolboxConfig.m | 0 {+WBToolbox => toolbox/+WBToolbox}/WBToolboxConfig2Mask.m | 0 6 files changed, 0 insertions(+), 0 deletions(-) rename {+WBToolbox => toolbox/+WBToolbox}/BlockInitialization.m (100%) rename {+WBToolbox => toolbox/+WBToolbox}/Mask2WBToolboxConfig.m (100%) rename {+WBToolbox => toolbox/+WBToolbox}/PID.m (100%) rename {+WBToolbox => toolbox/+WBToolbox}/WBTPIDConfig.m (100%) rename {+WBToolbox => toolbox/+WBToolbox}/WBToolboxConfig.m (100%) rename {+WBToolbox => toolbox/+WBToolbox}/WBToolboxConfig2Mask.m (100%) diff --git a/+WBToolbox/BlockInitialization.m b/toolbox/+WBToolbox/BlockInitialization.m similarity index 100% rename from +WBToolbox/BlockInitialization.m rename to toolbox/+WBToolbox/BlockInitialization.m diff --git a/+WBToolbox/Mask2WBToolboxConfig.m b/toolbox/+WBToolbox/Mask2WBToolboxConfig.m similarity index 100% rename from +WBToolbox/Mask2WBToolboxConfig.m rename to toolbox/+WBToolbox/Mask2WBToolboxConfig.m diff --git a/+WBToolbox/PID.m b/toolbox/+WBToolbox/PID.m similarity index 100% rename from +WBToolbox/PID.m rename to toolbox/+WBToolbox/PID.m diff --git a/+WBToolbox/WBTPIDConfig.m b/toolbox/+WBToolbox/WBTPIDConfig.m similarity index 100% rename from +WBToolbox/WBTPIDConfig.m rename to toolbox/+WBToolbox/WBTPIDConfig.m diff --git a/+WBToolbox/WBToolboxConfig.m b/toolbox/+WBToolbox/WBToolboxConfig.m similarity index 100% rename from +WBToolbox/WBToolboxConfig.m rename to toolbox/+WBToolbox/WBToolboxConfig.m diff --git a/+WBToolbox/WBToolboxConfig2Mask.m b/toolbox/+WBToolbox/WBToolboxConfig2Mask.m similarity index 100% rename from +WBToolbox/WBToolboxConfig2Mask.m rename to toolbox/+WBToolbox/WBToolboxConfig2Mask.m From 00911ae1e8f52b2397911e591e6ed34566047589 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 29 Nov 2017 07:20:06 +0000 Subject: [PATCH 68/89] Fixed DotJNu output variable --- toolbox/include/DotJNu.h | 2 +- toolbox/src/DotJNu.cpp | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/toolbox/include/DotJNu.h b/toolbox/include/DotJNu.h index 409a185af..0bc8153ee 100644 --- a/toolbox/include/DotJNu.h +++ b/toolbox/include/DotJNu.h @@ -28,7 +28,7 @@ class wbt::DotJNu : public wbt::WBBlock public: static const std::string ClassName; - + DotJNu(); ~DotJNu() override = default; diff --git a/toolbox/src/DotJNu.cpp b/toolbox/src/DotJNu.cpp index e984d7a96..c6fcd7c2a 100644 --- a/toolbox/src/DotJNu.cpp +++ b/toolbox/src/DotJNu.cpp @@ -189,15 +189,13 @@ bool DotJNu::output(const BlockInformation* blockInfo) // OUTPUT // ====== - // Compute the dot{J}*\nu - Vector6 biasAcc; if (!m_frameIsCoM) { - biasAcc = model->getFrameBiasAcc(m_frameIndex); + *m_dotJNu = model->getFrameBiasAcc(m_frameIndex); } else { - Vector3 comBiasAcc = model->getCenterOfMassBiasAcc(); - toEigen(biasAcc).segment<3>(0) = iDynTree::toEigen(comBiasAcc); - toEigen(biasAcc).segment<3>(3).setZero(); + iDynTree::Vector3 comBiasAcc = model->getCenterOfMassBiasAcc(); + toEigen(*m_dotJNu).segment<3>(0) = iDynTree::toEigen(comBiasAcc); + toEigen(*m_dotJNu).segment<3>(3).setZero(); } // Forward the output to Simulink From e24edee09268a068d8812fde88bb7785f52945f4 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 29 Nov 2017 08:02:52 +0000 Subject: [PATCH 69/89] Signal class rewritten - Added support to contiguous inputs (which are now the default setting) - For non-contiguous signals, an internal buffer is allocated with signal data, and data copy has been minimized as much as possible - When gathering a signal from Matlab, data type and signal are now read directly from the SimStruct - For the time being, only double signal are supported, which are the Simulink defaults. The structure for including the support of other data types is already present, and it will be included in the upcoming releases --- toolbox/include/base/BlockInformation.h | 48 +-- toolbox/include/base/Signal.h | 175 ++++++-- .../include/base/SimulinkBlockInformation.h | 22 +- toolbox/src/base/Signal.cpp | 394 ++++++------------ toolbox/src/base/SimulinkBlockInformation.cpp | 247 +++++++---- toolbox/src/base/toolbox.cpp | 9 + 6 files changed, 467 insertions(+), 428 deletions(-) diff --git a/toolbox/include/base/BlockInformation.h b/toolbox/include/base/BlockInformation.h index 06e2c37c1..43ef04306 100644 --- a/toolbox/include/base/BlockInformation.h +++ b/toolbox/include/base/BlockInformation.h @@ -3,7 +3,6 @@ #include "AnyType.h" #include -#include namespace wbt { class BlockInformation; @@ -21,37 +20,6 @@ namespace wbt { PortDataTypeBoolean, } PortDataType; - class Data - { - private: - double buffer; - public: - inline double doubleData() const { return buffer; } - inline void doubleData(const double& data) { buffer = data; } - inline float floatData() const { return static_cast(buffer); } - inline void floatData(const float& data) { buffer = static_cast(data); } - - inline int8_t int8Data() const { return static_cast(buffer); } - inline void int8Data(const int8_t& data) { buffer = static_cast(data); } - inline uint8_t uint8Data() const { return static_cast(buffer); } - inline void uint8Data(const uint8_t& data) { buffer = static_cast(data); } - - inline int16_t int16Data() const { return static_cast(buffer); } - inline void int16Data(const int16_t& data) { buffer = static_cast(data); } - inline uint16_t uint16Data() const { return static_cast(buffer); } - inline void uint16Data(const uint16_t& data) { buffer = static_cast(data); } - - inline int32_t int32Data() const { return static_cast(buffer); } - inline void int32Data(const int32_t& data) { buffer = static_cast(data); } - inline uint32_t uint32Data() const { return static_cast(buffer); } - inline void uint32Data(const uint32_t& data) { buffer = static_cast(data); } - - inline bool booleanData() const { return static_cast(buffer); } - inline void booleanData(const bool& data) { buffer = static_cast(data); } - - friend Signal; - }; - extern const std::string BlockOptionPrioritizeOrder; } @@ -61,7 +29,7 @@ class wbt::BlockInformation { BlockInformation() = default; virtual ~BlockInformation() = default; - // Block Options methods + // BLOCK OPTIONS METHODS // ===================== /** @@ -74,7 +42,7 @@ class wbt::BlockInformation { virtual bool optionFromKey(const std::string& key, double& option) const; - // Parameters methods + // PARAMETERS METHODS // ================== /** @@ -88,12 +56,10 @@ class wbt::BlockInformation { virtual bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const = 0; virtual bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) const = 0; virtual bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const = 0; - // virtual bool getAnyTypeAtIndex(unsigned parameterIndex, AnyType* data) = 0; - // virtual bool getCellAtIndex(unsigned parameterIndex, AnyCell& map) = 0; virtual bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const = 0; virtual bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const = 0; - // Port information methods + // PORT INFORMATION SETTERS // ======================== virtual bool setNumberOfInputPorts(unsigned numberOfPorts) = 0; @@ -114,13 +80,13 @@ class wbt::BlockInformation { virtual bool setInputPortType(unsigned portNumber, PortDataType portType) = 0; virtual bool setOutputPortType(unsigned portNumber, PortDataType portType) = 0; - // Port data - // ========= + // PORT INFORMATION GETTERS + // ======================== virtual unsigned getInputPortWidth(unsigned portNumber) const = 0; virtual unsigned getOutputPortWidth(unsigned portNumber) const = 0; - virtual wbt::Signal getInputPortSignal(unsigned portNumber) const = 0; - virtual wbt::Signal getOutputPortSignal(unsigned portNumber)const = 0; + virtual wbt::Signal getInputPortSignal(unsigned portNumber, int portWidth = -1) const = 0; + virtual wbt::Signal getOutputPortSignal(unsigned portNumber, int portWidth = -1) const = 0; }; #endif /* end of include guard: WBT_BLOCKINFORMATION_H */ diff --git a/toolbox/include/base/Signal.h b/toolbox/include/base/Signal.h index 21109b7c7..38f827a8b 100644 --- a/toolbox/include/base/Signal.h +++ b/toolbox/include/base/Signal.h @@ -2,50 +2,175 @@ #define WBT_SIGNAL_H #include "BlockInformation.h" -#include +#include +#include namespace wbt { class Signal; + enum SignalDataFormat { + NONCONTIGUOUS = 0, + CONTIGUOUS = 1, + CONTIGUOUS_ZEROCOPY = 2 + }; } class wbt::Signal { private: - PortDataType portType; - bool isContiguous; - bool isConstPort; + int m_width; + const bool m_isConst; + const PortDataType m_portDataType; + const SignalDataFormat m_dataFormat; - void** nonContiguousData; - void* contiguousData; + void* m_bufferPtr; + + void deleteBuffer(); + void allocateBuffer(const void* const bufferInput, void*& bufferOutput, unsigned length); + + template T* getCastBuffer() const; public: - Signal(); + // Ctor and Dtor + Signal(const SignalDataFormat& dataFormat = CONTIGUOUS_ZEROCOPY, + const PortDataType& dataType = PortDataTypeDouble, + const bool& isConst = true); + ~Signal(); + // Copy + Signal(const Signal& signal); + Signal& operator=(const Signal& signal) = delete; + // Move + Signal(Signal&& signal); + Signal& operator=(Signal&& signal) = delete; - void initSignalType(wbt::PortDataType type, bool constPort); + bool initializeBufferFromContiguous(const void* buffer); + bool initializeBufferFromContiguousZeroCopy(const void* buffer); + bool initializeBufferFromNonContiguous(const void* const* bufferPtrs); - void setContiguousBuffer(void* buffer); - void setContiguousBuffer(const void* buffer); - void setNonContiguousBuffer(void** buffer); - void setNonContiguousBuffer(const void* const* buffer); + bool isConst() const; + unsigned getWidth() const; + PortDataType getPortDataType() const; + SignalDataFormat getDataFormat() const; + template T* getBuffer() const; + template T get(const unsigned& i) const; - const Data get(unsigned index) const; - void* getContiguousBuffer(); - std::vector getStdVector(unsigned length) const; + void setWidth(const unsigned& width); + bool set(const unsigned& index, const double& data); + template bool setBuffer(const T* data, const unsigned& length); +}; - //the missing are cast - void set(unsigned index, double data); - void setBuffer(const double* data, const unsigned length, unsigned startIndex = 0); +template +T* wbt::Signal::getBuffer() const +{ + // Check the returned matches the same type of the portType. + // If this is not met, appliying pointer arithmetics on the returned + // pointer would show unknown behaviour. + switch(m_portDataType) { + case wbt::PortDataTypeDouble: + if (typeid(T).hash_code() != typeid(double).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeSingle: + if (typeid(T).hash_code() != typeid(float).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeInt8: + if (typeid(T).hash_code() != typeid(int8_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeUInt8: + if (typeid(T).hash_code() != typeid(uint8_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeInt16: + if (typeid(T).hash_code() != typeid(int16_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeUInt16: + if (typeid(T).hash_code() != typeid(uint16_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeInt32: + if (typeid(T).hash_code() != typeid(int32_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeUInt32: + if (typeid(T).hash_code() != typeid(uint32_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeBoolean: + if (typeid(T).hash_code() != typeid(bool).hash_code()) { + return nullptr; + } + break; + default: + return nullptr; + break; + } - void set(unsigned index, int32_t data); - void setBuffer(const int32_t* data, const unsigned length, unsigned startIndex = 0); + // Return the correct pointer + return static_cast(m_bufferPtr); +} - void set(unsigned index, uint32_t data); - void setBuffer(const uint32_t* data, const unsigned length, unsigned startIndex = 0); +template +bool wbt::Signal::setBuffer(const T* data, const unsigned& length) +{ + // Non contiguous signals follow the Simulink convention of being read-only + if (m_dataFormat == NONCONTIGUOUS || m_isConst) { + return false; + } - void set(unsigned index, bool data); - void setBuffer(const bool* data, const unsigned length, unsigned startIndex = 0); + if (m_dataFormat == CONTIGUOUS_ZEROCOPY && length > m_width) { + return false; + } -}; + if (typeid(getBuffer()).hash_code() != typeid(T*).hash_code()) { + return false; + } + switch (m_dataFormat) { + case CONTIGUOUS: + // Delete the current array + if (m_bufferPtr) { + delete getBuffer(); + m_bufferPtr = nullptr; + m_width = 0; + } + // Allocate a new empty array + m_bufferPtr = static_cast(new T[length]); + m_width = length; + // Fill it with new data + std::copy(data, data + length, getBuffer()); + break; + case CONTIGUOUS_ZEROCOPY: + // Reset current data + std::fill(getBuffer(), getBuffer() + m_width, 0); + // Copy new data + std::copy(data, data + length, getBuffer()); + // Update the width + m_width = length; + break; + case NONCONTIGUOUS: + return false; + } + + return true; +} + +template +T wbt::Signal::get(const unsigned& i) const +{ + T* buffer = getBuffer(); + assert(buffer); + + return buffer[i]; +} #endif /* end of include guard: WBT_SIGNAL_H */ diff --git a/toolbox/include/base/SimulinkBlockInformation.h b/toolbox/include/base/SimulinkBlockInformation.h index 0da7f51c5..dd79f9b31 100644 --- a/toolbox/include/base/SimulinkBlockInformation.h +++ b/toolbox/include/base/SimulinkBlockInformation.h @@ -15,20 +15,30 @@ class wbt::SimulinkBlockInformation : public wbt::BlockInformation private: SimStruct* simstruct; + PortDataType mapSimulinkToPortType(const DTypeId& typeId) const; + DTypeId mapPortTypeToSimulink(const PortDataType& dataType) const; + public: SimulinkBlockInformation(SimStruct* simstruct); ~SimulinkBlockInformation() override = default; + // BLOCK OPTIONS METHODS + // ===================== + bool optionFromKey(const std::string& key, double& option) const override; - //Parameters methods + // PARAMETERS METHODS + // ================== + bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const override; bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) const override; bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const override; bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const override; bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const override; - //Port information methods + // PORT INFORMATION SETTERS + // ======================== + bool setNumberOfInputPorts(unsigned numberOfPorts) override; bool setNumberOfOutputPorts(unsigned numberOfPorts) override; bool setInputPortVectorSize(unsigned portNumber, int portSize) override; @@ -38,11 +48,13 @@ class wbt::SimulinkBlockInformation : public wbt::BlockInformation bool setInputPortType(unsigned portNumber, PortDataType portType) override; bool setOutputPortType(unsigned portNumber, PortDataType portType) override; - //Port data + // PORT INFORMATION GETTERS + // ======================== + unsigned getInputPortWidth(unsigned portNumber) const override; unsigned getOutputPortWidth(unsigned portNumber) const override; - wbt::Signal getInputPortSignal(unsigned portNumber) const override; - wbt::Signal getOutputPortSignal(unsigned portNumber) const override; + wbt::Signal getInputPortSignal(unsigned portNumber, int portWidth = DYNAMICALLY_SIZED) const override; + wbt::Signal getOutputPortSignal(unsigned portNumber, int portWidth = DYNAMICALLY_SIZED) const override; }; #endif /* end of include guard: WBT_SIMULINKBLOCKINFORMATION_H */ diff --git a/toolbox/src/base/Signal.cpp b/toolbox/src/base/Signal.cpp index 028daef88..b55bb425f 100644 --- a/toolbox/src/base/Signal.cpp +++ b/toolbox/src/base/Signal.cpp @@ -1,325 +1,189 @@ #include "Signal.h" -#include using namespace wbt; -Signal::Signal() -: nonContiguousData(nullptr) -, contiguousData(nullptr) -{} - -void Signal::initSignalType(wbt::PortDataType type, bool constPort) +Signal::~Signal() { - this->portType = type; - this->isConstPort = constPort; + deleteBuffer(); } -void Signal::setContiguousBuffer(void* buffer) -{ - contiguousData = buffer; - this->isContiguous = true; -} -void Signal::setContiguousBuffer(const void* buffer) +Signal::Signal(const Signal& signal) +: m_width(signal.m_width) +, m_isConst(signal.m_isConst) +, m_portDataType(signal.m_portDataType) +, m_dataFormat(signal.m_dataFormat) +, m_bufferPtr(nullptr) { - contiguousData = const_cast(buffer); - this->isContiguous = true; + if (signal.m_bufferPtr) { + switch (signal.m_dataFormat) { + // Just copy the pointer to MATLAB's memory + case CONTIGUOUS_ZEROCOPY: + m_bufferPtr = signal.m_bufferPtr; + break; + // Copy the allocated data + case NONCONTIGUOUS: + case CONTIGUOUS: + allocateBuffer(signal.m_bufferPtr, m_bufferPtr, signal.m_width); + break; + } + } } -void Signal::setNonContiguousBuffer(void** buffer) +Signal::Signal(const SignalDataFormat& dataFormat, + const PortDataType& dataType, + const bool& isConst) +: m_isConst(isConst) +, m_portDataType(dataType) +, m_dataFormat(dataFormat) +, m_bufferPtr(nullptr) +{} + +Signal::Signal(Signal&& other) +: m_width(other.m_width) +, m_isConst(other.m_isConst) +, m_portDataType(other.m_portDataType) +, m_dataFormat(other.m_dataFormat) +, m_bufferPtr(other.m_bufferPtr) { - nonContiguousData = buffer; - this->isContiguous = false; + other.m_width = 0; + other.m_bufferPtr = nullptr; } -void Signal::setNonContiguousBuffer(const void* const* buffer) +void Signal::allocateBuffer(const void* const bufferInput, void*& bufferOutput, unsigned length) { - nonContiguousData = const_cast(buffer); - this->isContiguous = false; + // TODO: Implement other PortDataType + switch (m_portDataType) { + case PortDataTypeDouble: { + // Allocate the array + bufferOutput = static_cast(new double[m_width]); + // Cast to double + const double* const bufferInputDouble = static_cast(bufferInput); + double* bufferOutputDouble = static_cast(bufferOutput); + // Copy data + std::copy(bufferInputDouble, bufferInputDouble + length, bufferOutputDouble); + return; + } + default: + return; + } } - -const Data Signal::get(unsigned index) const +void Signal::deleteBuffer() { - Data data; - switch (portType) { + if (m_dataFormat == CONTIGUOUS_ZEROCOPY || !m_bufferPtr) { + return; + } + + // TODO: Implement other PortDataType + switch (m_portDataType) { case PortDataTypeDouble: - if (isContiguous) { - data.doubleData((static_cast(contiguousData))[index]); - } - else { - const double* buffer = static_cast(*nonContiguousData); - data.doubleData(static_cast(buffer[index])); - } - break; - case PortDataTypeSingle: - if (isContiguous) { - data.floatData((static_cast(contiguousData))[index]); - } - else { - const float* buffer = static_cast(*nonContiguousData); - data.floatData(static_cast(buffer[index])); - } - break; - case PortDataTypeInt8: - if (isContiguous) { - data.int8Data((static_cast(contiguousData))[index]); - } - else { - const int8_t* buffer = static_cast(*nonContiguousData); - data.int8Data(static_cast(buffer[index])); - } - break; - case PortDataTypeUInt8: - if (isContiguous) { - data.uint8Data((static_cast(contiguousData))[index]); - } - else { - const uint8_t* buffer = static_cast(*nonContiguousData); - data.uint8Data(static_cast(buffer[index])); - } - break; - case PortDataTypeInt16: - if (isContiguous) { - data.int16Data((static_cast(contiguousData))[index]); - } - else { - const int16_t* buffer = static_cast(*nonContiguousData); - data.int16Data(static_cast(buffer[index])); - } - break; - case PortDataTypeUInt16: - if (isContiguous) { - data.uint16Data((static_cast(contiguousData))[index]); - } - else { - const uint16_t* buffer = static_cast(*nonContiguousData); - data.uint16Data(static_cast(buffer[index])); - } - break; - case PortDataTypeInt32: - if (isContiguous) { - data.int32Data((static_cast(contiguousData))[index]); - } - else { - const int32_t* buffer = static_cast(*nonContiguousData); - data.int32Data(static_cast(buffer[index])); - } - break; - case PortDataTypeUInt32: - if (isContiguous) { - data.uint32Data((static_cast(contiguousData))[index]); - } - else { - const uint32_t* buffer = static_cast(*nonContiguousData); - data.uint32Data(static_cast(buffer[index])); - } - break; - case PortDataTypeBoolean: - if (isContiguous) { - data.booleanData((static_cast(contiguousData))[index]); - } - else { - const bool* buffer = static_cast(*nonContiguousData); - data.booleanData(static_cast(buffer[index])); - } + delete static_cast(m_bufferPtr); + m_bufferPtr = nullptr; + return; + default: + return; } - return data; } -void* Signal::getContiguousBuffer() +bool Signal::initializeBufferFromContiguousZeroCopy(const void* buffer) { - if (!isContiguous) return nullptr; - return this->contiguousData; + if (m_dataFormat != CONTIGUOUS_ZEROCOPY) { + return false; + } + + m_bufferPtr = const_cast(buffer); + return true; } -std::vector Signal::getStdVector(unsigned length) const +bool Signal::initializeBufferFromContiguous(const void* buffer) { - std::vector v(length); - for (unsigned i = 0; i < length; ++i) { - v[i] = get(i).doubleData(); + if (m_dataFormat != CONTIGUOUS || + m_width <= 0) { + return false; } - return v; -} -//the missing are cast -void Signal::set(unsigned index, double data) -{ - if (isConstPort) return; + if (m_portDataType == PortDataTypeDouble) { + // Allocate a new vector to store data from the non-contiguous signal + m_bufferPtr = static_cast(new double[m_width]); + const double* bufferInputDouble = static_cast(buffer); + double* bufferOutputDouble = static_cast(m_bufferPtr); - switch (portType) { - case PortDataTypeDouble: - { - double* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeSingle: - { - float* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - default: - break; + // Copy data from the input memory location + std::copy(bufferInputDouble, bufferInputDouble + m_width, bufferOutputDouble); } + return true; } -void Signal::setBuffer(const double* data, const unsigned length, unsigned startIndex) +bool Signal::initializeBufferFromNonContiguous(const void* const* bufferPtrs) { - if (isConstPort) return; - unsigned dataSize = 0; - const void* address = data; - - switch (portType) { - case PortDataTypeDouble: - { - dataSize = sizeof(double); - address = static_cast(address) + startIndex; - break; - } - case PortDataTypeSingle: - { - dataSize = sizeof(float); - address = static_cast(address) + startIndex; - break; - } - default: - break; + if (m_dataFormat != NONCONTIGUOUS || + m_width <= 0) { + return false; } - memcpy(contiguousData, address, dataSize * length); + if (m_portDataType == PortDataTypeDouble) { + // Allocate a new vector to store data from the non-contiguous signal + m_bufferPtr = static_cast(new double[m_width]); + double* bufferPtrDouble = static_cast(m_bufferPtr); + // Copy data from MATLAB's memory to the Signal object + for (auto i = 0; i < m_width; ++i) { + const double* valuePtr = static_cast(*bufferPtrs); + bufferPtrDouble[i] = valuePtr[i]; + } + } + return true; } -void Signal::set(unsigned index, int32_t data) +void Signal::setWidth(const unsigned& width) { - //signed integer function - switch (portType) { - case PortDataTypeInt32: - { - int32_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeInt16: - { - int16_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeInt8: - { - int8_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - default: - break; - } + m_width = width; } -void Signal::setBuffer(const int32_t* data, const unsigned length, unsigned startIndex) +unsigned Signal::getWidth() const { - if (isConstPort) return; - unsigned dataSize = 0; - const void* address = data; + return m_width; +} - switch (portType) { - case PortDataTypeInt32: - { - dataSize = sizeof(int32_t); - address = static_cast(address) + startIndex; - break; - } - case PortDataTypeInt16: - { - dataSize = sizeof(int16_t); - address = static_cast(address) + startIndex; - break; - } - case PortDataTypeInt8: - { - dataSize = sizeof(int8_t); - address = static_cast(address) + startIndex; - break; - } - default: - break; - } +PortDataType Signal::getPortDataType() const +{ + return m_portDataType; +} - memcpy(contiguousData, address, dataSize* length); +bool Signal::isConst() const +{ + return m_isConst; } -void Signal::set(unsigned index, uint32_t data) + +SignalDataFormat Signal::getDataFormat() const { - //signed integer function - switch (portType) { - case PortDataTypeUInt32: - { - uint32_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeUInt16: - { - uint16_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeUInt8: - { - uint8_t* buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - default: - break; - } + return m_dataFormat; } -void Signal::setBuffer(const uint32_t* data, const unsigned length, unsigned startIndex) +bool Signal::set(const unsigned& index, const double& data) { - if (isConstPort) return; - unsigned dataSize = 0; - const void* address = data; + if (m_isConst || m_width <= index) { + return false; + } - switch (portType) { - case PortDataTypeUInt32: - { - dataSize = sizeof(uint32_t); - address = data + startIndex; - break; - } - case PortDataTypeUInt16: + // TODO: Implement other PortDataType + switch (m_portDataType) { + case PortDataTypeDouble: { - dataSize = sizeof(uint16_t); - address = data + startIndex; + double* buffer = static_cast(m_bufferPtr); + buffer[index] = data; break; } - case PortDataTypeUInt8: + case PortDataTypeSingle: { - dataSize = sizeof(uint8_t); - address = data + startIndex; + float* buffer = static_cast(m_bufferPtr); + buffer[index] = data; break; } default: + return false; break; } - - memcpy(contiguousData, address, dataSize* length); -} - -void Signal::set(unsigned index, bool data) -{ - bool* buffer = static_cast(contiguousData); - buffer[index] = data; -} - -void Signal::setBuffer(const bool* data, const unsigned length, unsigned startIndex) -{ - if (isConstPort) return; - unsigned dataSize = sizeof(bool); - const void* address = static_cast(data) + startIndex; - - memcpy(contiguousData, address, dataSize* length); + return true; } diff --git a/toolbox/src/base/SimulinkBlockInformation.cpp b/toolbox/src/base/SimulinkBlockInformation.cpp index 38c1060db..f8825d0c8 100644 --- a/toolbox/src/base/SimulinkBlockInformation.cpp +++ b/toolbox/src/base/SimulinkBlockInformation.cpp @@ -11,6 +11,9 @@ SimulinkBlockInformation::SimulinkBlockInformation(SimStruct* S) : simstruct(S) {} +// BLOCK OPTIONS METHODS +// ===================== + bool SimulinkBlockInformation::optionFromKey(const std::string& key, double& option) const { if (key == wbt::BlockOptionPrioritizeOrder) { @@ -21,27 +24,15 @@ bool SimulinkBlockInformation::optionFromKey(const std::string& key, double& opt return false; } -//Parameters methods +// PARAMETERS METHODS +// ================== + bool SimulinkBlockInformation::getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const { const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); return MxAnyType(blockParam).asString(stringParameter); } -bool SimulinkBlockInformation::getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const -{ - const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); - return MxAnyType(blockParam).asAnyStruct(map); -} - - -bool SimulinkBlockInformation::getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const -{ - const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); - return MxAnyType(blockParam).asVectorDouble(vec); -} - - bool SimulinkBlockInformation::getScalarParameterAtIndex(unsigned parameterIndex, double& value) const { const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); @@ -65,7 +56,22 @@ bool SimulinkBlockInformation::getBooleanParameterAtIndex(unsigned parameterInde return MxAnyType(blockParam).asBool(value); } -//Port information methods +bool SimulinkBlockInformation::getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asAnyStruct(map); +} + + +bool SimulinkBlockInformation::getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asVectorDouble(vec); +} + +// PORT INFORMATION SETTERS +// ======================== + bool SimulinkBlockInformation::setNumberOfInputPorts(unsigned numberOfPorts) { return ssSetNumInputPorts(simstruct, numberOfPorts); @@ -104,77 +110,20 @@ bool SimulinkBlockInformation::setOutputPortMatrixSize(unsigned portNumber, int bool SimulinkBlockInformation::setInputPortType(unsigned portNumber, PortDataType portType) { - //for now force Direct feedthrough.. If needed create a separate method ssSetInputPortDirectFeedThrough(simstruct, portNumber, 1); - int matlabDataType = -1; - switch (portType) { - case PortDataTypeDouble: - matlabDataType = SS_DOUBLE; - break; - case PortDataTypeSingle: - matlabDataType = SS_SINGLE; - break; - case PortDataTypeInt8: - matlabDataType = SS_INT8; - break; - case PortDataTypeUInt8: - matlabDataType = SS_UINT8; - break; - case PortDataTypeInt16: - matlabDataType = SS_INT16; - break; - case PortDataTypeUInt16: - matlabDataType = SS_UINT16; - break; - case PortDataTypeInt32: - matlabDataType = SS_INT32; - break; - case PortDataTypeUInt32: - matlabDataType = SS_UINT32; - break; - case PortDataTypeBoolean: - matlabDataType = SS_BOOLEAN; - break; - } - return ssSetInputPortDataType(simstruct, portNumber, matlabDataType); + ssSetInputPortDataType(simstruct, portNumber, mapSimulinkToPortType(portType)); + return true; } bool SimulinkBlockInformation::setOutputPortType(unsigned portNumber, PortDataType portType) { - int matlabDataType = -1; - switch (portType) { - case PortDataTypeDouble: - matlabDataType = SS_DOUBLE; - break; - case PortDataTypeSingle: - matlabDataType = SS_SINGLE; - break; - case PortDataTypeInt8: - matlabDataType = SS_INT8; - break; - case PortDataTypeUInt8: - matlabDataType = SS_UINT8; - break; - case PortDataTypeInt16: - matlabDataType = SS_INT16; - break; - case PortDataTypeUInt16: - matlabDataType = SS_UINT16; - break; - case PortDataTypeInt32: - matlabDataType = SS_INT32; - break; - case PortDataTypeUInt32: - matlabDataType = SS_UINT32; - break; - case PortDataTypeBoolean: - matlabDataType = SS_BOOLEAN; - break; - } - return ssSetOutputPortDataType(simstruct, portNumber, matlabDataType); + ssSetOutputPortDataType(simstruct, portNumber, mapSimulinkToPortType(portType)); + return true; } -//Port data +// PORT INFORMATION GETTERS +// ======================== + unsigned SimulinkBlockInformation::getInputPortWidth(unsigned portNumber) const { return ssGetInputPortWidth(simstruct, portNumber); @@ -185,22 +134,136 @@ unsigned SimulinkBlockInformation::getOutputPortWidth(unsigned portNumber) const return ssGetOutputPortWidth(simstruct, portNumber); } -wbt::Signal SimulinkBlockInformation::getInputPortSignal(unsigned portNumber) const +wbt::Signal SimulinkBlockInformation::getInputPortSignal(unsigned portNumber, int portWidth) const { - Signal signal; - InputPtrsType port = ssGetInputPortSignalPtrs(simstruct, portNumber); - bool isConstPort = true; - signal.initSignalType(PortDataTypeDouble, isConstPort); - signal.setNonContiguousBuffer(port); - return signal; -} + // Read if the signal is contiguous or non-contiguous + boolean_T isContiguous = ssGetInputPortRequiredContiguous(simstruct, portNumber); + SignalDataFormat sigDataFormat = isContiguous ? CONTIGUOUS_ZEROCOPY : NONCONTIGUOUS; + + // Check if the signal is dynamically sized (which means that the dimension + // cannot be read) + bool isDynamicallySized = (ssGetInputPortWidth(simstruct, portNumber) == DYNAMICALLY_SIZED); + + // Note that if the signal is DYNAMICALLY_SIZED (-1), portWidth is necessary + if (isDynamicallySized && portWidth == -1) { + return Signal(); + } + + // Read the width of the signal if it is not provided as input and the signal is not + // dynamically sized + if (!isDynamicallySized && portWidth == -1) { + portWidth = ssGetInputPortWidth(simstruct, portNumber); + } + // Get the data type of the Signal if set (default: double) + DTypeId dataType = ssGetInputPortDataType(simstruct, portNumber); + + switch (sigDataFormat) { + case CONTIGUOUS_ZEROCOPY: { + // Initialize the signal + bool isConstPort = true; + Signal signal(CONTIGUOUS_ZEROCOPY, mapSimulinkToPortType(dataType), isConstPort); + signal.setWidth(portWidth); + // Initialize signal's data + if (!signal.initializeBufferFromContiguousZeroCopy(ssGetInputPortSignal(simstruct, portNumber))) { + return Signal(); + } + return signal; + } + case NONCONTIGUOUS: { + // Initialize the signal + bool isConstPort = true; + Signal signal(NONCONTIGUOUS, mapSimulinkToPortType(dataType), isConstPort); + signal.setWidth(portWidth); + // Initialize signal's data + InputPtrsType port = ssGetInputPortSignalPtrs(simstruct, portNumber); + if (!signal.initializeBufferFromNonContiguous(static_cast(port))) { + return Signal(); + } + return signal; + } + default: + return Signal(); + } +} -wbt::Signal SimulinkBlockInformation::getOutputPortSignal(unsigned portNumber) const +wbt::Signal SimulinkBlockInformation::getOutputPortSignal(unsigned portNumber, int portWidth) const { - Signal signal; + // Check if the signal is dynamically sized (which means that the dimension + // cannot be read) + bool isDynamicallySized = (ssGetOutputPortWidth(simstruct, portNumber) == DYNAMICALLY_SIZED); + + // Note that if the signal is DYNAMICALLY_SIZED (-1), portWidth is necessary + if (isDynamicallySized && portWidth == -1) { + return Signal(); + } + + // Read the width of the signal if it is not provided as input and the signal is not + // dynamically sized + if (!isDynamicallySized && portWidth == -1) { + portWidth = ssGetOutputPortWidth(simstruct, portNumber); + } + + // Get the data type of the Signal if set (default: double) + DTypeId dataType = ssGetOutputPortDataType(simstruct, portNumber); + bool isConstPort = false; - signal.initSignalType(PortDataTypeDouble, isConstPort); - signal.setContiguousBuffer(ssGetOutputPortSignal(simstruct, portNumber)); + Signal signal(CONTIGUOUS_ZEROCOPY, mapSimulinkToPortType(dataType), isConstPort); + signal.setWidth(portWidth); + + if (!signal.initializeBufferFromContiguousZeroCopy(ssGetOutputPortSignal(simstruct, portNumber))) { + return Signal(); + } + return signal; } + +PortDataType SimulinkBlockInformation::mapSimulinkToPortType(const DTypeId& typeId) const +{ + switch (typeId) { + case SS_DOUBLE: + return PortDataTypeDouble; + case SS_SINGLE: + return PortDataTypeSingle; + case SS_INT8: + return PortDataTypeInt8; + case SS_UINT8: + return PortDataTypeUInt8; + case SS_INT16: + return PortDataTypeInt16; + case SS_UINT16: + return PortDataTypeUInt16; + case SS_INT32: + return PortDataTypeInt32; + case SS_UINT32: + return PortDataTypeUInt32; + case SS_BOOLEAN: + return PortDataTypeBoolean; + default: + return PortDataTypeDouble; + } +} + +DTypeId SimulinkBlockInformation::mapPortTypeToSimulink(const PortDataType& dataType) const +{ + switch (dataType) { + case PortDataTypeDouble: + return SS_DOUBLE; + case PortDataTypeSingle: + return SS_SINGLE; + case PortDataTypeInt8: + return SS_INT8; + case PortDataTypeUInt8: + return SS_UINT8; + case PortDataTypeInt16: + return SS_INT16; + case PortDataTypeUInt16: + return SS_UINT16; + case PortDataTypeInt32: + return SS_INT32; + case PortDataTypeUInt32: + return SS_UINT32; + case PortDataTypeBoolean: + return SS_BOOLEAN; + } +} diff --git a/toolbox/src/base/toolbox.cpp b/toolbox/src/base/toolbox.cpp index ba9c86cb5..74468baa9 100644 --- a/toolbox/src/base/toolbox.cpp +++ b/toolbox/src/base/toolbox.cpp @@ -177,6 +177,15 @@ static void mdlInitializeSizes(SimStruct* S) bool ok = block->configureSizeAndPorts(&blockInfo); catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); + for (auto i = 0; i < ssGetNumInputPorts(S); ++i) { + // Set explicitly the inputs port to be SS_NOT_REUSABLE_AND_GLOBAL (which actually + // is already the default value). Since the toolbox supports contiguous input signals, + // this option should not be changed. + ssSetInputPortOptimOpts(S, i, SS_NOT_REUSABLE_AND_GLOBAL); + // Set input signals to be allocated in a contiguous memory storage + ssSetInputPortRequiredContiguous(S, i, true); + } + ssSetNumSampleTimes(S, 1); ssSetSimStateCompliance(S, USE_CUSTOM_SIM_STATE); //?? From 930b9e4e49eaa2c711fc885a7972242e941f35fa Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 29 Nov 2017 08:03:41 +0000 Subject: [PATCH 70/89] Fixed the parsing of the gravity vector --- toolbox/src/base/WBBlock.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/toolbox/src/base/WBBlock.cpp b/toolbox/src/base/WBBlock.cpp index d4acfbb3f..82cf92a05 100644 --- a/toolbox/src/base/WBBlock.cpp +++ b/toolbox/src/base/WBBlock.cpp @@ -131,8 +131,8 @@ bool WBBlock::getWBToolboxParameters(Configuration& config, const BlockInformati return false; } std::array gravityArray; - for (auto i : gravityVector) { - gravityArray[i] = i; + for (auto i = 0; i < 3; ++i) { + gravityArray[i] = gravityVector[i]; } // Create the ToolboxConfig object From c9239060b3d2fe220b5b391e0613531e09d2ef6f Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 29 Nov 2017 08:07:01 +0000 Subject: [PATCH 71/89] Moved the update of robot state into WBBlock This method avoids code duplication in all the child classes --- toolbox/include/base/WBBlock.h | 5 ++ toolbox/src/base/WBBlock.cpp | 102 +++++++++++++++++++++++++++++++++ 2 files changed, 107 insertions(+) diff --git a/toolbox/include/base/WBBlock.h b/toolbox/include/base/WBBlock.h index 227795a1c..bfd289f96 100644 --- a/toolbox/include/base/WBBlock.h +++ b/toolbox/include/base/WBBlock.h @@ -12,6 +12,7 @@ namespace wbt { class WBBlock; + class Signal; class Configuration; class BlockInformation; class RobotInterface; @@ -70,6 +71,10 @@ class wbt::WBBlock : public wbt::Block bool getWBToolboxParameters(Configuration& config, const BlockInformation* blockInfo); const std::shared_ptr getRobotInterface(); const Configuration& getConfiguration(); + bool setRobotState(const wbt::Signal* basePose, + const wbt::Signal* jointsPos, + const wbt::Signal* baseVelocity, + const wbt::Signal* jointsVelocity); public: WBBlock() = default; diff --git a/toolbox/src/base/WBBlock.cpp b/toolbox/src/base/WBBlock.cpp index 82cf92a05..c067c80e1 100644 --- a/toolbox/src/base/WBBlock.cpp +++ b/toolbox/src/base/WBBlock.cpp @@ -5,9 +5,13 @@ #include "ToolboxSingleton.h" #include "Configuration.h" #include "RobotInterface.h" +#include "Signal.h" #include "AnyType.h" +#include #include #include +#include "iDynTree/KinDynComputations.h" +#include #include #include #include @@ -28,6 +32,104 @@ iDynTreeRobotState::iDynTreeRobotState(const unsigned& dofs, const std::array Matrix4dSimulink; + + // Base pose + // --------- + + if (basePose) { + // Get the buffer + double* buffer = basePose->getBuffer(); + if (!buffer) { + Log::getSingleton().error("Failed to read the base pose from input port."); + return false; + } + // Fill the data + fromEigen(robotState.m_world_T_base, Matrix4dSimulink(buffer)); + } + + // Joints position + // --------------- + + if (jointsPos) { + // Get the buffer + double* buffer = jointsPos->getBuffer(); + if (!buffer) { + Log::getSingleton().error("Failed to read joints positions from input port."); + return false; + } + // Fill the data + for (auto i = 0; i < jointsPos->getWidth(); ++i) { + robotState.m_jointsPosition.setVal(i, buffer[i]); + } + + } + + // Base Velocity + // ------------- + + if (baseVelocity) { + // Get the buffer + double* buffer = baseVelocity->getBuffer(); + if (!buffer) { + Log::getSingleton().error("Failed to read the base velocity from input port."); + return false; + } + // Fill the data + robotState.m_baseVelocity = Twist(LinVelocity(buffer, 3), + AngVelocity(buffer+3, 3)); + } + + // Joints velocity + // --------------- + + if (jointsVelocity) { + // Get the buffer + double* buffer = jointsVelocity->getBuffer(); + if (!buffer) { + Log::getSingleton().error("Failed to read joints velocities from input port."); + return false; + } + // Fill the data + for (auto i = 0; i < jointsVelocity->getWidth(); ++i) { + robotState.m_jointsVelocity.setVal(i, buffer[i]); + } + } + + // UPDATE THE IDYNTREE ROBOT STATE WITH NEW DATA + // ============================================= + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } + + bool ok = model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + if (!ok) { + Log::getSingleton().error("Failed to set the iDynTree robot state."); + return false; + } + + return true; +} + unsigned WBBlock::numberOfParameters() { return 2; } bool WBBlock::getWBToolboxParameters(Configuration& config, const BlockInformation* blockInfo) From fc50b088f15d3c93187559c0b5c12a188a3c6ed7 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 29 Nov 2017 08:14:26 +0000 Subject: [PATCH 72/89] Updated all blocks to the new Signal class and WBBlock::setRobotState --- toolbox/include/SetReferences.h | 2 +- toolbox/src/CentroidalMomentum.cpp | 49 +++------- toolbox/src/DotJNu.cpp | 49 +++------- toolbox/src/ForwardKinematics.cpp | 41 ++++----- toolbox/src/GetMeasurement.cpp | 4 +- toolbox/src/InverseDynamics.cpp | 90 +++++++++---------- toolbox/src/Jacobian.cpp | 38 +++----- toolbox/src/MassMatrix.cpp | 37 +++----- .../src/MinimumJerkTrajectoryGenerator.cpp | 13 ++- toolbox/src/ModelPartitioner.cpp | 10 +-- toolbox/src/SetReferences.cpp | 43 +++++---- toolbox/src/YarpWrite.cpp | 2 +- 12 files changed, 152 insertions(+), 226 deletions(-) diff --git a/toolbox/include/SetReferences.h b/toolbox/include/SetReferences.h index 74b5c4b19..9ec5a385d 100644 --- a/toolbox/include/SetReferences.h +++ b/toolbox/include/SetReferences.h @@ -13,7 +13,7 @@ class wbt::SetReferences : public wbt::WBBlock private: std::vector m_controlModes; bool m_resetControlMode; - static void rad2deg(std::vector& v); + static const std::vector rad2deg(const double* buffer, const unsigned width); public: static const std::string ClassName; diff --git a/toolbox/src/CentroidalMomentum.cpp b/toolbox/src/CentroidalMomentum.cpp index 58484cb9e..126105c9c 100644 --- a/toolbox/src/CentroidalMomentum.cpp +++ b/toolbox/src/CentroidalMomentum.cpp @@ -8,7 +8,6 @@ #include #include #include -#include using namespace wbt; @@ -99,10 +98,6 @@ bool CentroidalMomentum::terminate(const BlockInformation* blockInfo) bool CentroidalMomentum::output(const BlockInformation* blockInfo) { - using namespace iDynTree; - using namespace Eigen; - typedef Matrix Matrix4dSimulink; - const auto& model = getRobotInterface()->getKinDynComputations(); if (!model) { @@ -110,41 +105,23 @@ bool CentroidalMomentum::output(const BlockInformation* blockInfo) return false; } - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== - - unsigned signalWidth; + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== - // Base pose Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - - // Base velocity + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); - double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); - robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), - AngVelocity(m_baseVelocityBuffer+3, 3)); - - // Joints velocity Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); - robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); - - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + &baseVelocitySignal, + &jointsVelocitySignal); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); + return false; + } // OUTPUT // ====== diff --git a/toolbox/src/DotJNu.cpp b/toolbox/src/DotJNu.cpp index c6fcd7c2a..8beab3e06 100644 --- a/toolbox/src/DotJNu.cpp +++ b/toolbox/src/DotJNu.cpp @@ -7,7 +7,6 @@ #include #include #include -#include using namespace wbt; @@ -139,10 +138,6 @@ bool DotJNu::terminate(const BlockInformation* blockInfo) bool DotJNu::output(const BlockInformation* blockInfo) { - using namespace iDynTree; - using namespace Eigen; - typedef Matrix Matrix4dSimulink; - const auto& model = getRobotInterface()->getKinDynComputations(); if (!model) { @@ -150,41 +145,23 @@ bool DotJNu::output(const BlockInformation* blockInfo) return false; } - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== - - unsigned signalWidth; + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== - // Base pose Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - - // Base velocity + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); - double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); - robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), - AngVelocity(m_baseVelocityBuffer+3, 3)); - - // Joints velocity Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); - robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); - - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + &baseVelocitySignal, + &jointsVelocitySignal); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); + return false; + } // OUTPUT // ====== diff --git a/toolbox/src/ForwardKinematics.cpp b/toolbox/src/ForwardKinematics.cpp index a34f5c72b..aca145cac 100644 --- a/toolbox/src/ForwardKinematics.cpp +++ b/toolbox/src/ForwardKinematics.cpp @@ -126,7 +126,6 @@ bool ForwardKinematics::terminate(const BlockInformation* blockInfo) bool ForwardKinematics::output(const BlockInformation* blockInfo) { - using namespace iDynTree; using namespace Eigen; typedef Matrix Matrix4dSimulink; typedef Matrix Matrix4diDynTree; @@ -138,31 +137,23 @@ bool ForwardKinematics::output(const BlockInformation* blockInfo) return false; } - // Get the signals and convert them to iDynTree objects - // ==================================================== + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== - unsigned signalWidth; - - // Base pose Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - - // Update the robot status - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); - - // Output + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + nullptr, + nullptr); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); + return false; + } + + // OUTPUT // ====== iDynTree::Transform world_H_frame; @@ -180,7 +171,7 @@ bool ForwardKinematics::output(const BlockInformation* blockInfo) // Allocate objects for row-major -> col-major conversion Map world_H_frame_RowMajor = toEigen(world_H_frame.asHomogeneousTransform()); - Map world_H_frame_ColMajor((double*)output.getContiguousBuffer(), + Map world_H_frame_ColMajor(output.getBuffer(), 4, 4); // Forward the buffer to Simulink transforming it to ColMajor diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp index a245105c6..e95e31dad 100644 --- a/toolbox/src/GetMeasurement.cpp +++ b/toolbox/src/GetMeasurement.cpp @@ -16,9 +16,9 @@ const std::string GetMeasurement::ClassName = "GetMeasurement"; void GetMeasurement::deg2rad(std::vector& v) { - const double deg2rad = M_PI / 180.0; + const double Deg2Rad = M_PI / 180.0; for (auto& element : v) { - element *= deg2rad; + element *= Deg2Rad; } } diff --git a/toolbox/src/InverseDynamics.cpp b/toolbox/src/InverseDynamics.cpp index 514c96d2b..49b3e8386 100644 --- a/toolbox/src/InverseDynamics.cpp +++ b/toolbox/src/InverseDynamics.cpp @@ -5,10 +5,8 @@ #include "Signal.h" #include "RobotInterface.h" #include -#include #include #include -#include using namespace wbt; @@ -130,70 +128,64 @@ bool InverseDynamics::terminate(const BlockInformation* blockInfo) bool InverseDynamics::output(const BlockInformation* blockInfo) { - using namespace iDynTree; - using namespace Eigen; - typedef Matrix Matrix4dSimulink; - - const auto& model = getRobotInterface()->getKinDynComputations(); - - if (!model) { - Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); - return false; - } - - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== - unsigned signalWidth; - - // Base pose Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - - // Base velocity + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_VEL); - double* m_baseVelocityBuffer = baseVelocitySignal.getStdVector(signalWidth).data(); - robotState.m_baseVelocity = Twist(LinVelocity(m_baseVelocityBuffer, 3), - AngVelocity(m_baseVelocityBuffer+3, 3)); - - // Joints velocity Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_VEL); - robotState.m_jointsVelocity.fillBuffer(jointsVelocitySignal.getStdVector(signalWidth).data()); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + &baseVelocitySignal, + &jointsVelocitySignal); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); + return false; + } // Base acceleration + // ----------------- + Signal baseAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_ACC); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_ACC); - m_baseAcceleration->fillBuffer(baseAccelerationSignal.getStdVector(signalWidth).data()); + double* bufBaseAcc = baseAccelerationSignal.getBuffer(); + if (!bufBaseAcc) { + Log::getSingleton().error("Failed to read data from input port."); + return false; + } + for (auto i = 0; i < baseAccelerationSignal.getWidth(); ++i) { + m_baseAcceleration->setVal(i, bufBaseAcc[i]); + } // Joints acceleration - Signal jointsAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_ACC); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINT_ACC); - m_jointsAcceleration->fillBuffer(jointsAccelerationSignal.getStdVector(signalWidth).data()); + // ------------------- - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); + Signal jointsAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_ACC); + double* bufJointsAcc = jointsAccelerationSignal.getBuffer(); + if (!bufJointsAcc) { + Log::getSingleton().error("Failed to read data from input port."); + return false; + } + for (auto i = 0; i < jointsAccelerationSignal.getWidth(); ++i) { + m_jointsAcceleration->setVal(i, bufJointsAcc[i]); + } // OUTPUT // ====== + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } + // Calculate the inverse dynamics (assuming zero external forces) model->inverseDynamics(*m_baseAcceleration, *m_jointsAcceleration, - LinkNetExternalWrenches(model->getNrOfLinks()), // TODO + iDynTree::LinkNetExternalWrenches(model->getNrOfLinks()), *m_torques); // Forward the output to Simulink diff --git a/toolbox/src/Jacobian.cpp b/toolbox/src/Jacobian.cpp index 901a0a822..c7574974c 100644 --- a/toolbox/src/Jacobian.cpp +++ b/toolbox/src/Jacobian.cpp @@ -139,9 +139,7 @@ bool Jacobian::terminate(const BlockInformation* blockInfo) bool Jacobian::output(const BlockInformation* blockInfo) { - using namespace iDynTree; using namespace Eigen; - typedef Matrix Matrix4dSimulink; typedef Matrix MatrixXdSimulink; typedef Matrix MatrixXdiDynTree; @@ -152,31 +150,21 @@ bool Jacobian::output(const BlockInformation* blockInfo) return false; } - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== - unsigned signalWidth; - - // Base pose Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - // TODO: what about the other inputs of setRobotState? + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + nullptr, + nullptr); - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); + return false; + } // OUTPUT // ====== @@ -184,7 +172,7 @@ bool Jacobian::output(const BlockInformation* blockInfo) iDynTree::Transform world_H_frame; // Compute the jacobian - bool ok = false; + ok = false; if (!m_frameIsCoM) { world_H_frame = model->getWorldTransform(m_frameIndex); ok = model->getFrameFreeFloatingJacobian(m_frameIndex, *m_jacobian); @@ -203,7 +191,7 @@ bool Jacobian::output(const BlockInformation* blockInfo) // Allocate objects for row-major -> col-major conversion Map jacobianRowMajor = toEigen(*m_jacobian); - Map jacobianColMajor((double*)output.getContiguousBuffer(), + Map jacobianColMajor(output.getBuffer(), 6, 6 + dofs); // Forward the buffer to Simulink transforming it to ColMajor diff --git a/toolbox/src/MassMatrix.cpp b/toolbox/src/MassMatrix.cpp index a14a9f78c..6a6aa50fc 100644 --- a/toolbox/src/MassMatrix.cpp +++ b/toolbox/src/MassMatrix.cpp @@ -96,7 +96,6 @@ bool MassMatrix::output(const BlockInformation* blockInfo) { using namespace Eigen; using namespace iDynTree; - typedef Matrix Matrix4dSimulink; typedef Matrix MatrixXdSimulink; typedef Matrix MatrixXdiDynTree; @@ -107,29 +106,21 @@ bool MassMatrix::output(const BlockInformation* blockInfo) return false; } - // GET THE SIGNALS AND CONVERT THEM TO IDYNTREE OBJECTS - // ==================================================== + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== - unsigned signalWidth; - - // Base pose Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_BASE_POSE); - fromEigen(robotState.m_world_T_base, - Matrix4dSimulink(basePoseSig.getStdVector(signalWidth).data())); - - // Joints position - Signal jointsPositionSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); - signalWidth = blockInfo->getInputPortWidth(INPUT_IDX_JOINTCONF); - robotState.m_jointsPosition.fillBuffer(jointsPositionSig.getStdVector(signalWidth).data()); - - // UPDATE THE ROBOT STATUS - // ======================= - model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + nullptr, + nullptr); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); + return false; + } // OUTPUT // ====== @@ -143,7 +134,7 @@ bool MassMatrix::output(const BlockInformation* blockInfo) // Allocate objects for row-major -> col-major conversion Map massMatrixRowMajor = toEigen(*m_massMatrix); - Map massMatrixColMajor((double*)output.getContiguousBuffer(), + Map massMatrixColMajor(output.getBuffer(), 6 + dofs, 6 + dofs); // Forward the buffer to Simulink transforming it to ColMajor diff --git a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp index 6c7e82e36..05aa56527 100644 --- a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp +++ b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp @@ -154,8 +154,8 @@ bool MinimumJerkTrajectoryGenerator::initialize(const BlockInformation* blockInf m_outputSecondDerivativeIndex = outputFirstDerivative ? 2 : 1; } - double sampleTime; - double settlingTime; + double sampleTime = 0; + double settlingTime = 0; ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_SAMPLE_TIME, sampleTime); ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_SETTLING_TIME, settlingTime); @@ -190,7 +190,7 @@ bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) unsigned portIndex = 1; if (m_explicitInitialValue) portIndex++; Signal externalTimePort = blockInfo->getInputPortSignal(portIndex); - double externalTime = externalTimePort.get(0).doubleData(); + double externalTime = externalTimePort.get(0); if (std::abs(m_previousSettlingTime - externalTime) > 1e-5) { m_previousSettlingTime = externalTime; @@ -204,14 +204,13 @@ bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) if (m_firstRun) { m_firstRun = false; - Signal initialValues; unsigned portIndex = 0; if (m_explicitInitialValue) { portIndex = 1; } - initialValues = blockInfo->getInputPortSignal(portIndex); + Signal initialValues = blockInfo->getInputPortSignal(portIndex); for (unsigned i = 0; i < blockInfo->getInputPortWidth(portIndex); ++i) { - (*m_initialValues)[i] = initialValues.get(i).doubleData(); + (*m_initialValues)[i] = initialValues.get(i); } m_generator->init(*m_initialValues); } @@ -219,7 +218,7 @@ bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) Signal references = blockInfo->getInputPortSignal(0); for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - (*m_reference)[i] = references.get(i).doubleData(); + (*m_reference)[i] = references.get(i); } m_generator->computeNextValues(*m_reference); diff --git a/toolbox/src/ModelPartitioner.cpp b/toolbox/src/ModelPartitioner.cpp index e987604f8..21f872c9c 100644 --- a/toolbox/src/ModelPartitioner.cpp +++ b/toolbox/src/ModelPartitioner.cpp @@ -168,10 +168,8 @@ bool ModelPartitioner::terminate(const BlockInformation* blockInfo) bool ModelPartitioner::output(const BlockInformation* blockInfo) { - Signal dofsSignal; - if (m_yarp2WBI) { - dofsSignal = blockInfo->getInputPortSignal(0); + Signal dofsSignal = blockInfo->getInputPortSignal(0); for (unsigned ithJoint = 0; ithJoint < getConfiguration().getNumberOfDoFs(); ++ithJoint) { const std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; @@ -183,11 +181,11 @@ bool ModelPartitioner::output(const BlockInformation* blockInfo) // Get the data to forward Signal ithOutput = blockInfo->getOutputPortSignal(controlBoardOfJoint); - ithOutput.set(contrJointIdxCB, dofsSignal.get(ithJoint).doubleData()); + ithOutput.set(contrJointIdxCB, dofsSignal.get(ithJoint)); } } else { - dofsSignal = blockInfo->getOutputPortSignal(0); + Signal dofsSignal = blockInfo->getOutputPortSignal(0); for (unsigned ithJoint = 0; ithJoint < getConfiguration().getNumberOfDoFs(); ++ithJoint) { const std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; @@ -199,7 +197,7 @@ bool ModelPartitioner::output(const BlockInformation* blockInfo) // Get the data to forward const Signal ithInput = blockInfo->getInputPortSignal(controlBoardOfJoint); - dofsSignal.set(ithJoint, ithInput.get(contrJointIdxCB).doubleData()); + dofsSignal.set(ithJoint, ithInput.get(contrJointIdxCB)); } } return true; diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 05317332e..5aca71d0a 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -17,12 +17,17 @@ SetReferences::SetReferences() : m_resetControlMode(true) {} -void SetReferences::rad2deg(std::vector& v) +const std::vector SetReferences::rad2deg(const double* buffer, const unsigned width) { - const double rad2deg = 180.0 / M_PI; - for (auto& element : v) { - element *= rad2deg; + const double Rad2Deg = 180.0 / M_PI; + + std::vector vectorDeg(width); + + for (auto i = 0; i < width; ++i) { + vectorDeg[i] = buffer[i] * Rad2Deg; } + + return vectorDeg; } unsigned SetReferences::numberOfParameters() @@ -125,7 +130,7 @@ bool SetReferences::initialize(const BlockInformation* blockInfo) return false; } // TODO: Set this parameter from the mask - std::vector speedInitalization(dofs, 10.0); + std::vector speedInitalization(dofs, 50.0); // Set the references if (!interface->setRefSpeeds(speedInitalization.data())) { Log::getSingleton().error("Failed to initialize speed references."); @@ -214,7 +219,12 @@ bool SetReferences::output(const BlockInformation* blockInfo) // Get the signal Signal references = blockInfo->getInputPortSignal(0); unsigned signalWidth = blockInfo->getInputPortWidth(0); - std::vector referencesVector = references.getStdVector(signalWidth); + + double* bufferReferences = references.getBuffer(); + if (!bufferReferences) { + Log::getSingleton().error("Failed to get the buffer containing references."); + return false; + } bool ok = false; // TODO: here only the first element is checked @@ -231,9 +241,9 @@ bool SetReferences::output(const BlockInformation* blockInfo) return false; } // Convert from rad to deg - rad2deg(referencesVector); + auto referencesDeg = rad2deg(bufferReferences, signalWidth); // Set the references - ok = interface->positionMove(referencesVector.data()); + ok = interface->positionMove(referencesDeg.data()); break; } case VOCAB_CM_POSITION_DIRECT: { @@ -244,9 +254,9 @@ bool SetReferences::output(const BlockInformation* blockInfo) return false; } // Convert from rad to deg - rad2deg(referencesVector); + auto referencesDeg = rad2deg(bufferReferences, signalWidth); // Set the references - ok = interface->setPositions(referencesVector.data()); + ok = interface->setPositions(referencesDeg.data()); break; } case VOCAB_CM_VELOCITY: { @@ -257,9 +267,9 @@ bool SetReferences::output(const BlockInformation* blockInfo) return false; } // Convert from rad to deg - rad2deg(referencesVector); + auto referencesDeg = rad2deg(bufferReferences, signalWidth); // Set the references - ok = interface->velocityMove(referencesVector.data()); + ok = interface->velocityMove(referencesDeg.data()); break; } case VOCAB_CM_TORQUE: { @@ -269,7 +279,8 @@ bool SetReferences::output(const BlockInformation* blockInfo) Log::getSingleton().error("Failed to get ITorqueControl interface."); return false; } - ok = interface->setRefTorques(referencesVector.data()); + // Set the references + ok = interface->setRefTorques(bufferReferences); break; } case VOCAB_CM_PWM: { @@ -279,7 +290,8 @@ bool SetReferences::output(const BlockInformation* blockInfo) Log::getSingleton().error("Failed to get IPWMControl interface."); return false; } - ok = interface->setRefDutyCycles(referencesVector.data()); + // Set the references + ok = interface->setRefDutyCycles(bufferReferences); break; } case VOCAB_CM_CURRENT: { @@ -289,7 +301,8 @@ bool SetReferences::output(const BlockInformation* blockInfo) Log::getSingleton().error("Failed to get ICurrentControl interface."); return false; } - ok = interface->setRefCurrents(referencesVector.data()); + // Set the references + ok = interface->setRefCurrents(bufferReferences); break; } } diff --git a/toolbox/src/YarpWrite.cpp b/toolbox/src/YarpWrite.cpp index 5c59fd035..bb445a405 100644 --- a/toolbox/src/YarpWrite.cpp +++ b/toolbox/src/YarpWrite.cpp @@ -143,7 +143,7 @@ bool YarpWrite::output(const BlockInformation* blockInfo) Signal signal = blockInfo->getInputPortSignal(0); for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - outputVector[i] = signal.get(i).doubleData(); + outputVector[i] = signal.get(i); } m_port->write(); From b922c549f47b9f7d5cf88dbbdb5f4a090a0c61f9 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 29 Nov 2017 15:32:00 +0000 Subject: [PATCH 73/89] New SetReferences mask param to set the reference speed (Position mode) The YARP iPositionControl interface needs to find the reference speed initialized in order to produce the wanted trajectory. Before this commit, this value was hardcoded, --- toolbox/include/SetReferences.h | 1 + toolbox/src/SetReferences.cpp | 13 ++++++++++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/toolbox/include/SetReferences.h b/toolbox/include/SetReferences.h index 9ec5a385d..16e2eba7a 100644 --- a/toolbox/include/SetReferences.h +++ b/toolbox/include/SetReferences.h @@ -13,6 +13,7 @@ class wbt::SetReferences : public wbt::WBBlock private: std::vector m_controlModes; bool m_resetControlMode; + double m_refSpeed; static const std::vector rad2deg(const double* buffer, const unsigned width); public: diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 5aca71d0a..e8681fc33 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -15,6 +15,7 @@ const std::string SetReferences::ClassName = "SetReferences"; SetReferences::SetReferences() : m_resetControlMode(true) +, m_refSpeed(0) {} const std::vector SetReferences::rad2deg(const double* buffer, const unsigned width) @@ -32,7 +33,7 @@ const std::vector SetReferences::rad2deg(const double* buffer, const uns unsigned SetReferences::numberOfParameters() { - return WBBlock::numberOfParameters() + 1; + return WBBlock::numberOfParameters() + 2; } bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) @@ -90,6 +91,13 @@ bool SetReferences::initialize(const BlockInformation* blockInfo) return false; } + // Reading the refSpeed + if (!blockInfo->getScalarParameterAtIndex(WBBlock::numberOfParameters() + 2, + m_refSpeed)) { + Log::getSingleton().error("Could not read reference speed parameter."); + return false; + } + // Initialize the std::vectors const unsigned dofs = getConfiguration().getNumberOfDoFs(); m_controlModes.assign(dofs, VOCAB_CM_UNKNOWN); @@ -129,8 +137,7 @@ bool SetReferences::initialize(const BlockInformation* blockInfo) Log::getSingleton().error("Failed to get IPositionControl interface."); return false; } - // TODO: Set this parameter from the mask - std::vector speedInitalization(dofs, 50.0); + std::vector speedInitalization(dofs, m_refSpeed); // Set the references if (!interface->setRefSpeeds(speedInitalization.data())) { Log::getSingleton().error("Failed to initialize speed references."); From 42ecb923bff4d588398fff1c13f7276f08350506 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 29 Nov 2017 12:21:35 +0000 Subject: [PATCH 74/89] Cleaned CMakeLists.txt and folder structure, updated library - The library is now exported only in R2014B_SLX format - Removed yarpWholeBodyInterface - Use default FindMatlab.cmake (cmake > 3.3) and Eigen3Config.cmake --- CMakeLists.txt | 10 +- cmake/FindEigen3.cmake | 90 - cmake/FindMatlab.cmake | 1467 ------ toolbox/CMakeLists.txt | 88 +- toolbox/WBToolboxLibrary.mdl | 4383 ----------------- toolbox/WBToolboxLibrary.slx | Bin 38548 -> 0 bytes .../+WBToolbox/BlockInitialization.m | 6 +- .../+WBToolbox/Configuration.m} | 6 +- .../+WBToolbox/ConfigurationToMask.m} | 12 +- .../+WBToolbox/MaskToConfiguration.m} | 8 +- toolbox/{ => matlab}/+WBToolbox/PID.m | 11 +- .../+WBToolbox/PIDConfiguration.m} | 31 +- toolbox/{ => matlab}/export_library.m | 28 +- .../library}/WBToolboxLibrary_repository.mdl | 2621 +++++----- .../library/exported/WBToolboxLibrary.slx | Bin 0 -> 534705 bytes .../setupForMatlabDebug.m.in} | 0 toolbox/matlab/slblocks.m | 8 + .../startup_wbitoolbox.m.in} | 0 toolbox/slblocks.m | 54 - 19 files changed, 1287 insertions(+), 7536 deletions(-) delete mode 100644 cmake/FindEigen3.cmake delete mode 100644 cmake/FindMatlab.cmake delete mode 100644 toolbox/WBToolboxLibrary.mdl delete mode 100644 toolbox/WBToolboxLibrary.slx rename toolbox/{ => matlab}/+WBToolbox/BlockInitialization.m (91%) rename toolbox/{+WBToolbox/WBToolboxConfig.m => matlab/+WBToolbox/Configuration.m} (97%) rename toolbox/{+WBToolbox/WBToolboxConfig2Mask.m => matlab/+WBToolbox/ConfigurationToMask.m} (85%) rename toolbox/{+WBToolbox/Mask2WBToolboxConfig.m => matlab/+WBToolbox/MaskToConfiguration.m} (85%) rename toolbox/{ => matlab}/+WBToolbox/PID.m (94%) rename toolbox/{+WBToolbox/WBTPIDConfig.m => matlab/+WBToolbox/PIDConfiguration.m} (85%) rename toolbox/{ => matlab}/export_library.m (56%) rename toolbox/{ => matlab/library}/WBToolboxLibrary_repository.mdl (74%) create mode 100644 toolbox/matlab/library/exported/WBToolboxLibrary.slx rename toolbox/{.setupForMatlabDebug.m.in => matlab/setupForMatlabDebug.m.in} (100%) create mode 100644 toolbox/matlab/slblocks.m rename toolbox/{.startup_wbitoolbox.m.in => matlab/startup_wbitoolbox.m.in} (100%) delete mode 100644 toolbox/slblocks.m diff --git a/CMakeLists.txt b/CMakeLists.txt index 034310442..7b99e7077 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,11 +1,13 @@ -# Copyright (C) 2013-2015 Instituto Italiano di Tecnologia -# Author: Jorhabib Eljaik, Francesco Romano +# Copyright (C) 2013-2017 iCub Facility - Istituto Italiano di Tecnologia +# Author: Jorhabib Eljaik, Francesco Romano, Diego Ferigo # CopyPolicy: Released under the terms of the GNU GPL v2.0. -cmake_minimum_required(VERSION 2.8.11) - +cmake_minimum_required(VERSION 3.3) project(WB-Toolbox) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + find_package(YCM REQUIRED) include(YCMDefaultDirs) diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake deleted file mode 100644 index 82846c3ad..000000000 --- a/cmake/FindEigen3.cmake +++ /dev/null @@ -1,90 +0,0 @@ - -# - Try to find Eigen3 lib -# -# This module supports requiring a minimum version, e.g. you can do -# find_package(Eigen3 3.1.2) -# to require version 3.1.2 or newer of Eigen3. -# -# Once done this will define -# -# EIGEN3_FOUND - system has eigen lib with correct version -# EIGEN3_INCLUDE_DIR - the eigen include directory -# EIGEN3_VERSION - eigen version -# -# This module reads hints about search locations from -# the following enviroment variables: -# -# EIGEN3_ROOT -# EIGEN3_ROOT_DIR - -# Copyright (c) 2006, 2007 Montel Laurent, -# Copyright (c) 2008, 2009 Gael Guennebaud, -# Copyright (c) 2009 Benoit Jacob -# Redistribution and use is allowed according to the terms of the 2-clause BSD license. - -if(NOT Eigen3_FIND_VERSION) - if(NOT Eigen3_FIND_VERSION_MAJOR) - set(Eigen3_FIND_VERSION_MAJOR 2) - endif(NOT Eigen3_FIND_VERSION_MAJOR) - if(NOT Eigen3_FIND_VERSION_MINOR) - set(Eigen3_FIND_VERSION_MINOR 91) - endif(NOT Eigen3_FIND_VERSION_MINOR) - if(NOT Eigen3_FIND_VERSION_PATCH) - set(Eigen3_FIND_VERSION_PATCH 0) - endif(NOT Eigen3_FIND_VERSION_PATCH) - - set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") -endif(NOT Eigen3_FIND_VERSION) - -macro(_eigen3_check_version) - file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) - - string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") - set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") - set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") - set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") - - set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) - if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK FALSE) - else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK TRUE) - endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - - if(NOT EIGEN3_VERSION_OK) - - message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " - "but at least version ${Eigen3_FIND_VERSION} is required") - endif(NOT EIGEN3_VERSION_OK) -endmacro(_eigen3_check_version) - -if (EIGEN3_INCLUDE_DIR) - - # in cache already - _eigen3_check_version() - set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) - -else (EIGEN3_INCLUDE_DIR) - - find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - HINTS - ENV EIGEN3_ROOT - ENV EIGEN3_ROOT_DIR - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) - - if(EIGEN3_INCLUDE_DIR) - _eigen3_check_version() - endif(EIGEN3_INCLUDE_DIR) - - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) - - mark_as_advanced(EIGEN3_INCLUDE_DIR) - -endif(EIGEN3_INCLUDE_DIR) diff --git a/cmake/FindMatlab.cmake b/cmake/FindMatlab.cmake deleted file mode 100644 index 373aa38a3..000000000 --- a/cmake/FindMatlab.cmake +++ /dev/null @@ -1,1467 +0,0 @@ -#.rst: -# FindMatlab -# ---------- -# -# Finds Matlab installations and provides Matlab tools and libraries to cmake. -# -# This package first intention is to find the libraries associated with Matlab -# in order to be able to build Matlab extensions (mex files). It can also be -# used: -# -# * run specific commands in Matlab -# * declare Matlab unit test -# * retrieve various information from Matlab (mex extensions, versions and -# release queries, ...) -# -# The module supports the following components: -# -# * ``MX_LIBRARY`` and ``ENG_LIBRARY`` respectively the MX and ENG libraries of -# Matlab -# * ``MAIN_PROGRAM`` the Matlab binary program. -# -# .. note:: -# -# The version given to the :command:`find_package` directive is the Matlab -# **version**, which should not be confused with the Matlab *release* name -# (eg. `R2014`). -# The :command:`matlab_get_version_from_release_name` and -# :command:`matlab_get_release_name_from_version` allow a mapping -# from the release name to the version. -# -# The variable :variable:`Matlab_ROOT_DIR` may be specified in order to give -# the path of the desired Matlab version. Otherwise, the behaviour is platform -# specific: -# -# * Windows: The installed versions of Matlab are retrieved from the -# Windows registry -# * OS X: The installed versions of Matlab are given by the MATLAB -# paths in ``/Application``. If no such application is found, it falls back -# to the one that might be accessible from the PATH. -# * Unix: The desired Matlab should be accessible from the PATH. -# -# Additional information is provided when :variable:`MATLAB_FIND_DEBUG` is set. -# When a Matlab binary is found automatically and the ``MATLAB_VERSION`` -# is not given, the version is queried from Matlab directly. -# On Windows, it can make a window running Matlab appear. -# -# The mapping of the release names and the version of Matlab is performed by -# defining pairs (name, version). The variable -# :variable:`MATLAB_ADDITIONAL_VERSIONS` may be provided before the call to -# the :command:`find_package` in order to handle additional versions. -# -# A Matlab scripts can be added to the set of tests using the -# :command:`matlab_add_unit_test`. By default, the Matlab unit test framework -# will be used (>= 2013a) to run this script, but regular ``.m`` files -# returning an exit code can be used as well (0 indicating a success). -# -# Module Input Variables -# ^^^^^^^^^^^^^^^^^^^^^^ -# -# Users or projects may set the following variables to configure the module -# behaviour: -# -# :variable:`Matlab_ROOT_DIR` -# the root of the Matlab installation. -# :variable:`MATLAB_FIND_DEBUG` -# outputs debug information -# :variable:`MATLAB_ADDITIONAL_VERSIONS` -# additional versions of Matlab for the automatic retrieval of the installed -# versions. -# -# Variables defined by the module -# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -# -# Result variables -# """""""""""""""" -# -# ``Matlab_FOUND`` -# ``TRUE`` if the Matlab installation is found, ``FALSE`` -# otherwise. All variable below are defined if Matlab is found. -# ``Matlab_ROOT_DIR`` -# the final root of the Matlab installation determined by the FindMatlab -# module. -# ``Matlab_MAIN_PROGRAM`` -# the Matlab binary program. Available only if the component ``MAIN_PROGRAM`` -# is given in the :command:`find_package` directive. -# ``Matlab_INCLUDE_DIRS`` -# the path of the Matlab libraries headers -# ``Matlab_MEX_LIBRARY`` -# library for mex, always available. -# ``Matlab_MX_LIBRARY`` -# mx library of Matlab (arrays). Available only if the component -# ``MX_LIBRARY`` has been requested. -# ``Matlab_ENG_LIBRARY`` -# Matlab engine library. Available only if the component ``ENG_LIBRARY`` -# is requested. -# ``Matlab_LIBRARIES`` -# the whole set of libraries of Matlab -# ``Matlab_MEX_COMPILER`` -# the mex compiler of Matlab. Currently not used. -# Available only if the component ``MEX_COMPILER`` is asked -# -# Cached variables -# """""""""""""""" -# -# ``Matlab_MEX_EXTENSION`` -# the extension of the mex files for the current platform (given by Matlab). -# ``Matlab_ROOT_DIR`` -# the location of the root of the Matlab installation found. If this value -# is changed by the user, the result variables are recomputed. -# -# Provided macros -# ^^^^^^^^^^^^^^^ -# -# :command:`matlab_get_version_from_release_name` -# returns the version from the release name -# :command:`matlab_get_release_name_from_version` -# returns the release name from the Matlab version -# -# Provided functions -# ^^^^^^^^^^^^^^^^^^ -# -# :command:`matlab_add_mex` -# adds a target compiling a MEX file. -# :command:`matlab_add_unit_test` -# adds a Matlab unit test file as a test to the project. -# :command:`matlab_extract_all_installed_versions_from_registry` -# parses the registry for all Matlab versions. Available on Windows only. -# The part of the registry parsed is dependent on the host processor -# :command:`matlab_get_all_valid_matlab_roots_from_registry` -# returns all the possible Matlab paths, according to a previously -# given list. Only the existing/accessible paths are kept. This is mainly -# useful for the searching all possible Matlab installation. -# :command:`matlab_get_mex_suffix` -# returns the suffix to be used for the mex files -# (platform/architecture dependant) -# :command:`matlab_get_version_from_matlab_run` -# returns the version of Matlab, given the full directory of the Matlab -# program. -# -# -# Known issues -# ^^^^^^^^^^^^ -# -# **Symbol clash in a MEX target** -# By default, every symbols inside a MEX -# file defined with the command :command:`matlab_add_mex` have hidden -# visibility, except for the entry point. This is the default behaviour of -# the MEX compiler, which lowers the risk of symbol collision between the -# libraries shipped with Matlab, and the libraries to which the MEX file is -# linking to. This is also the default on Windows platforms. -# -# However, this is not sufficient in certain case, where for instance your -# MEX file is linking against libraries that are already loaded by Matlab, -# even if those libraries have different SONAMES. -# A possible solution is to hide the symbols of the libraries to which the -# MEX target is linking to. This can be achieved in GNU GCC compilers with -# the linker option ``-Wl,--exclude-libs,ALL``. -# -# **Tests using GPU resources** -# in case your MEX file is using the GPU and -# in order to be able to run unit tests on this MEX file, the GPU resources -# should be properly released by Matlab. A possible solution is to make -# Matlab aware of the use of the GPU resources in the session, which can be -# performed by a command such as ``D = gpuDevice()`` at the beginning of -# the test script (or via a fixture). -# -# -# Reference -# ^^^^^^^^^ -# -# .. variable:: Matlab_ROOT_DIR -# -# The root folder of the Matlab installation. If set before the call to -# :command:`find_package`, the module will look for the components in that -# path. If not set, then an automatic search of Matlab -# will be performed. If set, it should point to a valid version of Matlab. -# -# .. variable:: MATLAB_FIND_DEBUG -# -# If set, the lookup of Matlab and the intermediate configuration steps are -# outputted to the console. -# -# .. variable:: MATLAB_ADDITIONAL_VERSIONS -# -# If set, specifies additional versions of Matlab that may be looked for. -# The variable should be a list of strings, organised by pairs of release -# name and versions, such as follows:: -# -# set(MATLAB_ADDITIONAL_VERSIONS -# "release_name1=corresponding_version1" -# "release_name2=corresponding_version2" -# ... -# ) -# -# Example:: -# -# set(MATLAB_ADDITIONAL_VERSIONS -# "R2013b=8.2" -# "R2013a=8.1" -# "R2012b=8.0") -# -# The order of entries in this list matters when several versions of -# Matlab are installed. The priority is set according to the ordering in -# this list. - -#============================================================================= -# Copyright 2014-2015 Raffi Enficiaud, Max Planck Society -# -# Distributed under the OSI-approved BSD License (the "License"); -# see accompanying file Copyright.txt for details. -# -# This software is distributed WITHOUT ANY WARRANTY; without even the -# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. -# See the License for more information. -#============================================================================= -# (To distribute this file outside of CMake, substitute the full -# License text for the above reference.) - -set(_FindMatlab_SELF_DIR "${CMAKE_CURRENT_LIST_DIR}") - -include(FindPackageHandleStandardArgs) -include(CheckCXXCompilerFlag) - - -# The currently supported versions. Other version can be added by the user by -# providing MATLAB_ADDITIONAL_VERSIONS -if(NOT MATLAB_ADDITIONAL_VERSIONS) - set(MATLAB_ADDITIONAL_VERSIONS) -endif() - -set(MATLAB_VERSIONS_MAPPING - "R2016b=9.1" - "R2016a=9.0" - "R2015b=8.6" - "R2015a=8.5" - "R2014b=8.4" - "R2014a=8.3" - "R2013b=8.2" - "R2013a=8.1" - "R2012b=8.0" - "R2012a=7.14" - - "R2011b=7.13" - "R2011a=7.12" - "R2010b=7.11" - - ${MATLAB_ADDITIONAL_VERSIONS} - ) - - -# temporary folder for all Matlab runs -set(_matlab_temporary_folder ${CMAKE_BINARY_DIR}/Matlab) - -if(NOT EXISTS "${_matlab_temporary_folder}") - file(MAKE_DIRECTORY "${_matlab_temporary_folder}") -endif() - -#.rst: -# .. command:: matlab_get_version_from_release_name -# -# Returns the version of Matlab (17.58) from a release name (R2017k) -macro (matlab_get_version_from_release_name release_name version_name) - - string(REGEX MATCHALL "${release_name}=([0-9]+\\.?[0-9]*)" _matched ${MATLAB_VERSIONS_MAPPING}) - - set(${version_name} "") - if(NOT _matched STREQUAL "") - set(${version_name} ${CMAKE_MATCH_1}) - else() - message(WARNING "The release name ${release_name} is not registered") - endif() - unset(_matched) - -endmacro() - - - - - -#.rst: -# .. command:: matlab_get_release_name_from_version -# -# Returns the release name (R2017k) from the version of Matlab (17.58) -macro (matlab_get_release_name_from_version version release_name) - - set(${release_name} "") - foreach(_var IN LISTS MATLAB_VERSIONS_MAPPING) - string(REGEX MATCHALL "(.+)=${version}" _matched ${_var}) - if(NOT _matched STREQUAL "") - set(${release_name} ${CMAKE_MATCH_1}) - break() - endif() - endforeach(_var) - - unset(_var) - unset(_matched) - if(${release_name} STREQUAL "") - message(WARNING "The version ${version} is not registered") - endif() - -endmacro() - - - - - -# extracts all the supported release names (R2017k...) of Matlab -# internal use -macro(matlab_get_supported_releases list_releases) - set(${list_releases}) - foreach(_var IN LISTS MATLAB_VERSIONS_MAPPING) - string(REGEX MATCHALL "(.+)=([0-9]+\\.?[0-9]*)" _matched ${_var}) - if(NOT _matched STREQUAL "") - list(APPEND ${list_releases} ${CMAKE_MATCH_1}) - endif() - unset(_matched) - unset(CMAKE_MATCH_1) - endforeach(_var) - unset(_var) -endmacro() - - - -# extracts all the supported versions of Matlab -# internal use -macro(matlab_get_supported_versions list_versions) - set(${list_versions}) - foreach(_var IN LISTS MATLAB_VERSIONS_MAPPING) - string(REGEX MATCHALL "(.+)=([0-9]+\\.?[0-9]*)" _matched ${_var}) - if(NOT _matched STREQUAL "") - list(APPEND ${list_versions} ${CMAKE_MATCH_2}) - endif() - unset(_matched) - unset(CMAKE_MATCH_1) - endforeach(_var) - unset(_var) -endmacro() - - -#.rst: -# .. command:: matlab_extract_all_installed_versions_from_registry -# -# This function parses the registry and founds the Matlab versions that are -# installed. The found versions are returned in `matlab_versions`. -# Set `win64` to `TRUE` if the 64 bit version of Matlab should be looked for -# The returned list contains all versions under -# ``HKLM\\SOFTWARE\\Mathworks\\MATLAB`` or an empty list in case an error -# occurred (or nothing found). -# -# .. note:: -# -# Only the versions are provided. No check is made over the existence of the -# installation referenced in the registry, -# -function(matlab_extract_all_installed_versions_from_registry win64 matlab_versions) - - if(NOT CMAKE_HOST_WIN32) - message(FATAL_ERROR "This macro can only be called by a windows host (call to reg.exe") - endif() - - - if(${win64} AND ${CMAKE_HOST_SYSTEM_PROCESSOR} MATCHES "64") - set(APPEND_REG "/reg:64") - else() - set(APPEND_REG "/reg:32") - endif() - - # /reg:64 should be added on 64 bits capable OSs in order to enable the - # redirection of 64 bits applications - execute_process( - COMMAND reg query HKEY_LOCAL_MACHINE\\SOFTWARE\\Mathworks\\MATLAB /f * /k ${APPEND_REG} - RESULT_VARIABLE resultMatlab - OUTPUT_VARIABLE varMatlab - ERROR_VARIABLE errMatlab - INPUT_FILE NUL - ) - - - set(matlabs_from_registry) - if(${resultMatlab} EQUAL 0) - - string( - REGEX MATCHALL "MATLAB\\\\([0-9]+(\\.[0-9]+)?)" - matlab_versions_regex ${varMatlab}) - - foreach(match IN LISTS matlab_versions_regex) - string( - REGEX MATCH "MATLAB\\\\(([0-9]+)(\\.([0-9]+))?)" - current_match ${match}) - - set(_matlab_current_version ${CMAKE_MATCH_1}) - set(current_matlab_version_major ${CMAKE_MATCH_2}) - set(current_matlab_version_minor ${CMAKE_MATCH_4}) - if(NOT current_matlab_version_minor) - set(current_matlab_version_minor "0") - endif() - - list(APPEND matlabs_from_registry ${_matlab_current_version}) - unset(_matlab_current_version) - endforeach(match) - - endif() - - if(matlabs_from_registry) - list(REMOVE_DUPLICATES matlabs_from_registry) - list(SORT matlabs_from_registry) - list(REVERSE matlabs_from_registry) - endif() - - set(${matlab_versions} ${matlabs_from_registry} PARENT_SCOPE) - -endfunction() - - - -# (internal) -macro(extract_matlab_versions_from_registry_brute_force matlab_versions) - # get the supported versions - set(matlab_supported_versions) - matlab_get_supported_versions(matlab_supported_versions) - - - # this is a manual population of the versions we want to look for - # this can be done as is, but preferably with the call to - # matlab_get_supported_versions and variable - - # populating the versions we want to look for - # set(matlab_supported_versions) - - # # Matlab 7 - # set(matlab_major 7) - # foreach(current_matlab_minor RANGE 4 20) - # list(APPEND matlab_supported_versions "${matlab_major}.${current_matlab_minor}") - # endforeach(current_matlab_minor) - - # # Matlab 8 - # set(matlab_major 8) - # foreach(current_matlab_minor RANGE 0 5) - # list(APPEND matlab_supported_versions "${matlab_major}.${current_matlab_minor}") - # endforeach(current_matlab_minor) - - # # taking into account the possible additional versions provided by the user - # if(DEFINED MATLAB_ADDITIONAL_VERSIONS) - # list(APPEND matlab_supported_versions MATLAB_ADDITIONAL_VERSIONS) - # endif() - - - # we order from more recent to older - if(matlab_supported_versions) - list(REMOVE_DUPLICATES matlab_supported_versions) - list(SORT matlab_supported_versions) - list(REVERSE matlab_supported_versions) - endif() - - - set(${matlab_versions} ${matlab_supported_versions}) - - -endmacro() - - - - -#.rst: -# .. command:: matlab_get_all_valid_matlab_roots_from_registry -# -# Populates the Matlab root with valid versions of Matlab. -# The returned matlab_roots is organized in pairs -# ``(version_number,matlab_root_path)``. -# -# :: -# -# matlab_get_all_valid_matlab_roots_from_registry( -# matlab_versions -# matlab_roots) -# -# ``matlab_versions`` -# the versions of each of the Matlab installations -# ``matlab_roots`` -# the location of each of the Matlab installations -function(matlab_get_all_valid_matlab_roots_from_registry matlab_versions matlab_roots) - - # The matlab_versions comes either from - # extract_matlab_versions_from_registry_brute_force or - # matlab_extract_all_installed_versions_from_registry. - - - set(_matlab_roots_list ) - foreach(_matlab_current_version ${matlab_versions}) - get_filename_component( - current_MATLAB_ROOT - "[HKEY_LOCAL_MACHINE\\SOFTWARE\\MathWorks\\MATLAB\\${_matlab_current_version};MATLABROOT]" - ABSOLUTE) - - if(EXISTS ${current_MATLAB_ROOT}) - list(APPEND _matlab_roots_list ${_matlab_current_version} ${current_MATLAB_ROOT}) - endif() - - endforeach(_matlab_current_version) - unset(_matlab_current_version) - set(${matlab_roots} ${_matlab_roots_list} PARENT_SCOPE) - unset(_matlab_roots_list) -endfunction() - -#.rst: -# .. command:: matlab_get_mex_suffix -# -# Returns the extension of the mex files (the suffixes). -# This function should not be called before the appropriate Matlab root has -# been found. -# -# :: -# -# matlab_get_mex_suffix( -# matlab_root -# mex_suffix) -# -# ``matlab_root`` -# the root of the Matlab installation -# ``mex_suffix`` -# the variable name in which the suffix will be returned. -function(matlab_get_mex_suffix matlab_root mex_suffix) - - # todo setup the extension properly. Currently I do not know if this is - # sufficient for all win32 distributions. - # there is also CMAKE_EXECUTABLE_SUFFIX that could be tweaked - set(mexext_suffix "") - if(WIN32) - list(APPEND mexext_suffix ".bat") - endif() - - # we first try without suffix, since cmake does not understand a list with - # one empty string element - find_program( - Matlab_MEXEXTENSIONS_PROG - NAMES mexext - PATHS ${matlab_root}/bin - DOC "Matlab MEX extension provider" - NO_DEFAULT_PATH - ) - - foreach(current_mexext_suffix IN LISTS mexext_suffix) - if(NOT DEFINED Matlab_MEXEXTENSIONS_PROG OR NOT Matlab_MEXEXTENSIONS_PROG) - # this call should populate the cache automatically - find_program( - Matlab_MEXEXTENSIONS_PROG - "mexext${current_mexext_suffix}" - PATHS ${matlab_root}/bin - DOC "Matlab MEX extension provider" - NO_DEFAULT_PATH - ) - endif() - endforeach(current_mexext_suffix) - - - # the program has been found? - if((NOT Matlab_MEXEXTENSIONS_PROG) OR (NOT EXISTS ${Matlab_MEXEXTENSIONS_PROG})) - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Cannot found mexext program. Matlab root is ${matlab_root}") - endif() - unset(Matlab_MEXEXTENSIONS_PROG CACHE) - return() - endif() - - set(_matlab_mex_extension) - - set(devnull) - if(UNIX) - set(devnull INPUT_FILE /dev/null) - elseif(WIN32) - set(devnull INPUT_FILE NUL) - endif() - - execute_process( - COMMAND ${Matlab_MEXEXTENSIONS_PROG} - OUTPUT_VARIABLE _matlab_mex_extension - ERROR_VARIABLE _matlab_mex_extension_error - ${devnull}) - string(STRIP ${_matlab_mex_extension} _matlab_mex_extension) - - unset(Matlab_MEXEXTENSIONS_PROG CACHE) - set(${mex_suffix} ${_matlab_mex_extension} PARENT_SCOPE) -endfunction() - - - - -#.rst: -# .. command:: matlab_get_version_from_matlab_run -# -# This function runs Matlab program specified on arguments and extracts its -# version. -# -# :: -# -# matlab_get_version_from_matlab_run( -# matlab_binary_path -# matlab_list_versions) -# -# ``matlab_binary_path`` -# the location of the `matlab` binary executable -# ``matlab_list_versions`` -# the version extracted from Matlab -function(matlab_get_version_from_matlab_run matlab_binary_program matlab_list_versions) - - set(${matlab_list_versions} "" PARENT_SCOPE) - - - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Determining the version of Matlab from ${matlab_binary_program}") - endif() - - if(EXISTS "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp") - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Removing previous ${_matlab_temporary_folder}/matlabVersionLog.cmaketmp file") - endif() - file(REMOVE "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp") - endif() - - - # the log file is needed since on windows the command executes in a new - # window and it is not possible to get back the answer of Matlab - # the -wait command is needed on windows, otherwise the call returns - # immediately after the program launches itself. - if(WIN32) - set(_matlab_additional_commands "-wait") - endif() - - set(devnull) - if(UNIX) - set(devnull INPUT_FILE /dev/null) - elseif(WIN32) - set(devnull INPUT_FILE NUL) - endif() - - # timeout set to 30 seconds, in case it does not start - # note as said before OUTPUT_VARIABLE cannot be used in a platform - # independent manner however, not setting it would flush the output of Matlab - # in the current console (unix variant) - execute_process( - COMMAND "${matlab_binary_program}" -nosplash -nojvm ${_matlab_additional_commands} -logfile "matlabVersionLog.cmaketmp" -nodesktop -nodisplay -r "version, exit" - OUTPUT_VARIABLE _matlab_version_from_cmd_dummy - RESULT_VARIABLE _matlab_result_version_call - ERROR_VARIABLE _matlab_result_version_call_error - TIMEOUT 30 - WORKING_DIRECTORY "${_matlab_temporary_folder}" - ${devnull} - ) - - - if(${_matlab_result_version_call}) - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Unable to determine the version of Matlab. Matlab call returned with error ${_matlab_result_version_call}.") - endif() - return() - elseif(NOT EXISTS "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp") - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Unable to determine the version of Matlab. The log file does not exist.") - endif() - return() - endif() - - # if successful, read back the log - file(READ "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp" _matlab_version_from_cmd) - file(REMOVE "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp") - - set(index -1) - string(FIND ${_matlab_version_from_cmd} "ans" index) - if(index EQUAL -1) - - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Cannot find the version of Matlab returned by the run.") - endif() - - else() - set(matlab_list_of_all_versions_tmp) - - string(SUBSTRING ${_matlab_version_from_cmd} ${index} -1 substring_ans) - string( - REGEX MATCHALL "ans[\r\n\t ]*=[\r\n\t ]*([0-9]+(\\.[0-9]+)?)" - matlab_versions_regex - ${substring_ans}) - foreach(match IN LISTS matlab_versions_regex) - string( - REGEX MATCH "ans[\r\n\t ]*=[\r\n\t ]*(([0-9]+)(\\.([0-9]+))?)" - current_match ${match}) - - list(APPEND matlab_list_of_all_versions_tmp ${CMAKE_MATCH_1}) - endforeach() - if(matlab_list_of_all_versions_tmp) - list(REMOVE_DUPLICATES matlab_list_of_all_versions_tmp) - endif() - set(${matlab_list_versions} ${matlab_list_of_all_versions_tmp} PARENT_SCOPE) - - endif() - -endfunction() - - -#.rst: -# .. command:: matlab_add_unit_test -# -# Adds a Matlab unit test to the test set of cmake/ctest. -# This command requires the component ``MAIN_PROGRAM``. -# The unit test uses the Matlab unittest framework (default, available -# starting Matlab 2013b+) except if the option ``NO_UNITTEST_FRAMEWORK`` -# is given. -# -# The function expects one Matlab test script file to be given. -# In the case ``NO_UNITTEST_FRAMEWORK`` is given, the unittest script file -# should contain the script to be run, plus an exit command with the exit -# value. This exit value will be passed to the ctest framework (0 success, -# non 0 failure). Additional arguments accepted by :command:`add_test` can be -# passed through ``TEST_ARGS`` (eg. ``CONFIGURATION ...``). -# -# :: -# -# matlab_add_unit_test( -# NAME -# UNITTEST_FILE matlab_file_containing_unittest.m -# [UNITTEST_PRECOMMAND matlab_command_to_run] -# [TIMEOUT timeout] -# [ADDITIONAL_PATH path1 [path2 ...]] -# [MATLAB_ADDITIONAL_STARTUP_OPTIONS option1 [option2 ...]] -# [TEST_ARGS arg1 [arg2 ...]] -# [NO_UNITTEST_FRAMEWORK] -# ) -# -# The function arguments are: -# -# ``NAME`` -# name of the unittest in ctest. -# ``UNITTEST_FILE`` -# the matlab unittest file. Its path will be automatically -# added to the Matlab path. -# ``UNITTEST_PRECOMMAND`` -# Matlab script command to be ran before the file -# containing the test (eg. GPU device initialisation based on CMake -# variables). -# ``TIMEOUT`` -# the test timeout in seconds. Defaults to 180 seconds as the -# Matlab unit test may hang. -# ``ADDITIONAL_PATH`` -# a list of paths to add to the Matlab path prior to -# running the unit test. -# ``MATLAB_ADDITIONAL_STARTUP_OPTIONS`` -# a list of additional option in order -# to run Matlab from the command line. -# ``TEST_ARGS`` -# Additional options provided to the add_test command. These -# options are added to the default options (eg. "CONFIGURATIONS Release") -# ``NO_UNITTEST_FRAMEWORK`` -# when set, indicates that the test should not -# use the unittest framework of Matlab (available for versions >= R2013a). -# -function(matlab_add_unit_test) - - if(NOT Matlab_MAIN_PROGRAM) - message(FATAL_ERROR "[MATLAB] This functionality needs the MAIN_PROGRAM component (not default)") - endif() - - set(options NO_UNITTEST_FRAMEWORK) - set(oneValueArgs NAME UNITTEST_PRECOMMAND UNITTEST_FILE TIMEOUT) - set(multiValueArgs ADDITIONAL_PATH MATLAB_ADDITIONAL_STARTUP_OPTIONS TEST_ARGS) - - set(prefix _matlab_unittest_prefix) - cmake_parse_arguments(${prefix} "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN} ) - - if(NOT ${prefix}_NAME) - message(FATAL_ERROR "[MATLAB] The Matlab test name cannot be empty") - endif() - - add_test(NAME ${${prefix}_NAME} - COMMAND ${CMAKE_COMMAND} - -Dtest_name=${${prefix}_NAME} - -Dadditional_paths=${${prefix}_ADDITIONAL_PATH} - -Dtest_timeout=${${prefix}_TIMEOUT} - -Doutput_directory=${_matlab_temporary_folder} - -DMatlab_PROGRAM=${Matlab_MAIN_PROGRAM} - -Dno_unittest_framework=${${prefix}_NO_UNITTEST_FRAMEWORK} - -DMatlab_ADDITIONNAL_STARTUP_OPTIONS=${${prefix}_MATLAB_ADDITIONAL_STARTUP_OPTIONS} - -Dunittest_file_to_run=${${prefix}_UNITTEST_FILE} - -Dcmd_to_run_before_test=${${prefix}_UNITTEST_PRECOMMAND} - -P ${_FindMatlab_SELF_DIR}/MatlabTestsRedirect.cmake - ${${prefix}_TEST_ARGS} - ${${prefix}_UNPARSED_ARGUMENTS} - ) -endfunction() - - -#.rst: -# .. command:: matlab_add_mex -# -# Adds a Matlab MEX target. -# This commands compiles the given sources with the current tool-chain in -# order to produce a MEX file. The final name of the produced output may be -# specified, as well as additional link libraries, and a documentation entry -# for the MEX file. Remaining arguments of the call are passed to the -# :command:`add_library` command. -# -# :: -# -# matlab_add_mex( -# NAME -# SRC src1 [src2 ...] -# [OUTPUT_NAME output_name] -# [DOCUMENTATION file.txt] -# [LINK_TO target1 target2 ...] -# [...] -# ) -# -# ``NAME`` -# name of the target. -# ``SRC`` -# list of tje source files. -# ``LINK_TO`` -# a list of additional link dependencies. The target links to ``libmex`` -# by default. If ``Matlab_MX_LIBRARY`` is defined, it also -# links to ``libmx``. -# ``OUTPUT_NAME`` -# if given, overrides the default name. The default name is -# the name of the target without any prefix and -# with ``Matlab_MEX_EXTENSION`` suffix. -# ``DOCUMENTATION`` -# if given, the file ``file.txt`` will be considered as -# being the documentation file for the MEX file. This file is copied into -# the same folder without any processing, with the same name as the final -# mex file, and with extension `.m`. In that case, typing ``help `` -# in Matlab prints the documentation contained in this file. -# -# The documentation file is not processed and should be in the following -# format: -# -# :: -# -# % This is the documentation -# function ret = mex_target_output_name(input1) -# -function(matlab_add_mex ) - - if(NOT WIN32) - # we do not need all this on Windows - # pthread options - check_cxx_compiler_flag(-pthread HAS_MINUS_PTHREAD) - # we should use try_compile instead, the link flags are discarded from - # this compiler_flag function. - #check_cxx_compiler_flag(-Wl,--exclude-libs,ALL HAS_SYMBOL_HIDING_CAPABILITY) - - endif() - - set(oneValueArgs NAME DOCUMENTATION OUTPUT_NAME) - set(multiValueArgs LINK_TO SRC) - - set(prefix _matlab_addmex_prefix) - cmake_parse_arguments(${prefix} "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN} ) - - if(NOT ${prefix}_NAME) - message(FATAL_ERROR "[MATLAB] The MEX target name cannot be empty") - endif() - - if(NOT ${prefix}_OUTPUT_NAME) - set(${prefix}_OUTPUT_NAME ${${prefix}_NAME}) - endif() - - add_library(${${prefix}_NAME} - SHARED - ${${prefix}_SRC} - ${${prefix}_DOCUMENTATION} - ${${prefix}_UNPARSED_ARGUMENTS}) - target_include_directories(${${prefix}_NAME} PRIVATE ${Matlab_INCLUDE_DIRS}) - - if(DEFINED Matlab_MX_LIBRARY) - target_link_libraries(${${prefix}_NAME} ${Matlab_MX_LIBRARY}) - endif() - - target_link_libraries(${${prefix}_NAME} ${Matlab_MEX_LIBRARY} ${${prefix}_LINK_TO}) - set_target_properties(${${prefix}_NAME} - PROPERTIES - PREFIX "" - OUTPUT_NAME ${${prefix}_OUTPUT_NAME} - SUFFIX ".${Matlab_MEX_EXTENSION}") - - - # documentation - if(NOT ${${prefix}_DOCUMENTATION} STREQUAL "") - get_target_property(output_name ${${prefix}_NAME} OUTPUT_NAME) - add_custom_command( - TARGET ${${prefix}_NAME} - PRE_BUILD - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${${prefix}_DOCUMENTATION} $/${output_name}.m - COMMENT "Copy ${${prefix}_NAME} documentation file into the output folder" - ) - endif() # documentation - - # entry point in the mex file + taking care of visibility and symbol clashes. - if(WIN32) - set_target_properties(${${prefix}_NAME} - PROPERTIES - DEFINE_SYMBOL "DLL_EXPORT_SYM=__declspec(dllexport)") - else() - - if(HAS_MINUS_PTHREAD AND NOT APPLE) - # Apparently, compiling with -pthread generated the proper link flags - # and some defines at compilation - target_compile_options(${${prefix}_NAME} PRIVATE "-pthread") - endif() - - - # if we do not do that, the symbols linked from eg. boost remain weak and - # then clash with the ones defined in the matlab process. So by default - # the symbols are hidden. - # This also means that for shared libraries (like MEX), the entry point - # should be explicitly declared with default visibility, otherwise Matlab - # cannot find the entry point. - # Note that this is particularly meaningful if the MEX wrapper itself - # contains symbols that are clashing with Matlab (that are compiled in the - # MEX file). In order to propagate the visibility options to the libraries - # to which the MEX file is linked against, the -Wl,--exclude-libs,ALL - # option should also be specified. - - set_target_properties(${${prefix}_NAME} - PROPERTIES - CXX_VISIBILITY_PRESET "hidden" - C_VISIBILITY_PRESET "hidden" - VISIBILITY_INLINES_HIDDEN ON - ) - - # get_target_property( - # _previous_link_flags - # ${${prefix}_NAME} - # LINK_FLAGS) - # if(NOT _previous_link_flags) - # set(_previous_link_flags) - # endif() - - # if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") - # set_target_properties(${${prefix}_NAME} - # PROPERTIES - # LINK_FLAGS "${_previous_link_flags} -Wl,--exclude-libs,ALL" - # # -Wl,--version-script=${_FindMatlab_SELF_DIR}/MatlabLinuxVisibility.map" - # ) - # elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") - # # in this case, all other symbols become hidden. - # set_target_properties(${${prefix}_NAME} - # PROPERTIES - # LINK_FLAGS "${_previous_link_flags} -Wl,-exported_symbol,_mexFunction" - # #-Wl,-exported_symbols_list,${_FindMatlab_SELF_DIR}/MatlabOSXVisilibity.map" - # ) - # endif() - - - - set_target_properties(${${prefix}_NAME} - PROPERTIES - DEFINE_SYMBOL "DLL_EXPORT_SYM=__attribute__ ((visibility (\"default\")))" - ) - - - endif() - -endfunction() - - -# (internal) -# Used to get the version of matlab, using caching. This basically transforms the -# output of the root list, with possible unknown version, to a version -# -function(_Matlab_get_version_from_root matlab_root matlab_known_version matlab_final_version) - - # if the version is not trivial, we query matlab for that - # we keep track of the location of matlab that induced this version - #if(NOT DEFINED Matlab_PROG_VERSION_STRING_AUTO_DETECT) - # set(Matlab_PROG_VERSION_STRING_AUTO_DETECT "" CACHE INTERNAL "internal matlab location for the discovered version") - #endif() - - if(NOT ${matlab_known_version} STREQUAL "NOTFOUND") - # the version is known, we just return it - set(${matlab_final_version} ${matlab_known_version} PARENT_SCOPE) - set(Matlab_VERSION_STRING_INTERNAL ${matlab_known_version} CACHE INTERNAL "Matlab version (automatically determined)" FORCE) - return() - endif() - - # - set(_matlab_current_program ${Matlab_MAIN_PROGRAM}) - - # do we already have a matlab program? - if(NOT _matlab_current_program) - - set(_find_matlab_options) - if(matlab_root AND EXISTS ${matlab_root}) - set(_find_matlab_options PATHS ${matlab_root} ${matlab_root}/bin NO_DEFAULT_PATH) - endif() - - find_program( - _matlab_current_program - matlab - ${_find_matlab_options} - DOC "Matlab main program" - ) - endif() - - if(NOT _matlab_current_program OR NOT EXISTS ${_matlab_current_program}) - # if not found, clear the dependent variables - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Cannot find the main matlab program under ${matlab_root}") - endif() - set(Matlab_PROG_VERSION_STRING_AUTO_DETECT "" CACHE INTERNAL "internal matlab location for the discovered version" FORCE) - set(Matlab_VERSION_STRING_INTERNAL "" CACHE INTERNAL "internal matlab location for the discovered version" FORCE) - unset(_matlab_current_program) - unset(_matlab_current_program CACHE) - return() - endif() - - # full real path for path comparison - get_filename_component(_matlab_main_real_path_tmp "${_matlab_current_program}" REALPATH) - unset(_matlab_current_program) - unset(_matlab_current_program CACHE) - - # is it the same as the previous one? - if(_matlab_main_real_path_tmp STREQUAL Matlab_PROG_VERSION_STRING_AUTO_DETECT) - set(${matlab_final_version} ${Matlab_VERSION_STRING_INTERNAL} PARENT_SCOPE) - return() - endif() - - # update the location of the program - set(Matlab_PROG_VERSION_STRING_AUTO_DETECT ${_matlab_main_real_path_tmp} CACHE INTERNAL "internal matlab location for the discovered version" FORCE) - - set(matlab_list_of_all_versions) - matlab_get_version_from_matlab_run("${Matlab_PROG_VERSION_STRING_AUTO_DETECT}" matlab_list_of_all_versions) - - list(GET matlab_list_of_all_versions 0 _matlab_version_tmp) - - # set the version into the cache - set(Matlab_VERSION_STRING_INTERNAL ${_matlab_version_tmp} CACHE INTERNAL "Matlab version (automatically determined)" FORCE) - - # warning, just in case several versions found (should not happen) - list(LENGTH matlab_list_of_all_versions list_of_all_versions_length) - if((${list_of_all_versions_length} GREATER 1) AND MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Found several versions, taking the first one (versions found ${matlab_list_of_all_versions})") - endif() - - # return the updated value - set(${matlab_final_version} ${Matlab_VERSION_STRING_INTERNAL} PARENT_SCOPE) - -endfunction() - - - - - - - -# ################################### -# Exploring the possible Matlab_ROOTS - -# this variable will get all Matlab installations found in the current system. -set(_matlab_possible_roots) - - - -if(Matlab_ROOT_DIR) - # if the user specifies a possible root, we keep this one - - if(NOT EXISTS ${Matlab_ROOT_DIR}) - # if Matlab_ROOT_DIR specified but erroneous - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] the specified path for Matlab_ROOT_DIR does not exist (${Matlab_ROOT_DIR})") - endif() - else() - # NOTFOUND indicates the code below to search for the version automatically - if("${Matlab_VERSION_STRING_INTERNAL}" STREQUAL "") - list(APPEND _matlab_possible_roots "NOTFOUND" ${Matlab_ROOT_DIR}) # empty version - else() - list(APPEND _matlab_possible_roots ${Matlab_VERSION_STRING_INTERNAL} ${Matlab_ROOT_DIR}) # cached version - endif() - endif() - - -else() - - # if the user does not specify the possible installation root, we look for - # one installation using the appropriate heuristics - - if(WIN32) - - # On WIN32, we look for Matlab installation in the registry - # if unsuccessful, we look for all known revision and filter the existing - # ones. - - # testing if we are able to extract the needed information from the registry - set(_matlab_versions_from_registry) - matlab_extract_all_installed_versions_from_registry(CMAKE_CL_64 _matlab_versions_from_registry) - - # the returned list is empty, doing the search on all known versions - if(NOT _matlab_versions_from_registry) - - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Search for Matlab from the registry unsuccessful, testing all supported versions") - endif() - - extract_matlab_versions_from_registry_brute_force(_matlab_versions_from_registry) - endif() - - # filtering the results with the registry keys - matlab_get_all_valid_matlab_roots_from_registry("${_matlab_versions_from_registry}" _matlab_possible_roots) - unset(_matlab_versions_from_registry) - - elseif(APPLE) - - # on mac, we look for the /Application paths - # this corresponds to the behaviour on Windows. On Linux, we do not have - # any other guess. - matlab_get_supported_releases(_matlab_releases) - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Matlab supported versions ${_matlab_releases}. If more version should be supported " - "the variable MATLAB_ADDITIONAL_VERSIONS can be set according to the documentation") - endif() - - foreach(_matlab_current_release IN LISTS _matlab_releases) - set(_matlab_full_string "/Applications/MATLAB_${_matlab_current_release}.app") - if(EXISTS ${_matlab_full_string}) - set(_matlab_current_version) - matlab_get_version_from_release_name("${_matlab_current_release}" _matlab_current_version) - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Found version ${_matlab_current_release} (${_matlab_current_version}) in ${_matlab_full_string}") - endif() - list(APPEND _matlab_possible_roots ${_matlab_current_version} ${_matlab_full_string}) - unset(_matlab_current_version) - endif() - - unset(_matlab_full_string) - endforeach(_matlab_current_release) - - unset(_matlab_current_release) - unset(_matlab_releases) - - endif() - -endif() - - - -list(LENGTH _matlab_possible_roots _numbers_of_matlab_roots) -if(_numbers_of_matlab_roots EQUAL 0) - # if we have not found anything, we fall back on the PATH - - - # At this point, we have no other choice than trying to find it from PATH. - # If set by the user, this wont change - find_program( - _matlab_main_tmp - NAMES matlab) - - - if(_matlab_main_tmp) - # we then populate the list of roots, with empty version - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] matlab found from PATH: ${_matlab_main_tmp}") - endif() - - # resolve symlinks - get_filename_component(_matlab_current_location "${_matlab_main_tmp}" REALPATH) - - # get the directory (the command below has to be run twice) - # this will be the matlab root - get_filename_component(_matlab_current_location "${_matlab_current_location}" DIRECTORY) - get_filename_component(_matlab_current_location "${_matlab_current_location}" DIRECTORY) # Matlab should be in bin - - list(APPEND _matlab_possible_roots "NOTFOUND" ${_matlab_current_location}) - - unset(_matlab_current_location) - - endif() - unset(_matlab_main_tmp CACHE) - -endif() - - - - - -if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Matlab root folders are ${_matlab_possible_roots}") -endif() - - - - - -# take the first possible Matlab root -list(LENGTH _matlab_possible_roots _numbers_of_matlab_roots) -set(Matlab_VERSION_STRING "NOTFOUND") -if(_numbers_of_matlab_roots GREATER 0) - list(GET _matlab_possible_roots 0 Matlab_VERSION_STRING) - list(GET _matlab_possible_roots 1 Matlab_ROOT_DIR) - - # adding a warning in case of ambiguity - if(_numbers_of_matlab_roots GREATER 2 AND MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Found several distributions of Matlab. Setting the current version to ${Matlab_VERSION_STRING} (located ${Matlab_ROOT_DIR})." - " If this is not the desired behaviour, provide the -DMatlab_ROOT_DIR=... on the command line") - endif() -endif() - - -# check if the root changed against the previous defined one, if so -# clear all the cached variables -if(DEFINED Matlab_ROOT_DIR_LAST_CACHED) - - if(NOT Matlab_ROOT_DIR_LAST_CACHED STREQUAL Matlab_ROOT_DIR) - set(_Matlab_cached_vars - Matlab_INCLUDE_DIRS - Matlab_MEX_LIBRARY - Matlab_MEX_COMPILER - Matlab_MAIN_PROGRAM - Matlab_MX_LIBRARY - Matlab_ENG_LIBRARY - Matlab_MEX_EXTENSION - - # internal - Matlab_MEXEXTENSIONS_PROG - Matlab_ROOT_DIR_LAST_CACHED - #Matlab_PROG_VERSION_STRING_AUTO_DETECT - Matlab_VERSION_STRING_INTERNAL - ) - foreach(_var IN LISTS _Matlab_cached_vars) - if(DEFINED ${_var}) - unset(${_var} CACHE) - endif() - endforeach() - endif() -endif() - -set(Matlab_ROOT_DIR_LAST_CACHED ${Matlab_ROOT_DIR} CACHE INTERNAL "last Matlab root dir location") -set(Matlab_ROOT_DIR ${Matlab_ROOT_DIR} CACHE PATH "Matlab installation root path" FORCE) - -# Fix the version, in case this one is NOTFOUND -_Matlab_get_version_from_root( - "${Matlab_ROOT_DIR}" - ${Matlab_VERSION_STRING} - Matlab_VERSION_STRING -) - - - - -if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Current version is ${Matlab_VERSION_STRING} located ${Matlab_ROOT_DIR}") -endif() - - - -if(Matlab_ROOT_DIR) - file(TO_CMAKE_PATH ${Matlab_ROOT_DIR} Matlab_ROOT_DIR) -endif() - -if(CMAKE_SIZEOF_VOID_P EQUAL 4) - set(_matlab_64Build FALSE) -else() - set(_matlab_64Build TRUE) -endif() - -if(APPLE) - set(_matlab_bin_prefix "mac") # i should be for intel - set(_matlab_bin_suffix_32bits "i") - set(_matlab_bin_suffix_64bits "i64") -elseif(UNIX) - set(_matlab_bin_prefix "gln") - set(_matlab_bin_suffix_32bits "x86") - set(_matlab_bin_suffix_64bits "xa64") -else() - set(_matlab_bin_prefix "win") - set(_matlab_bin_suffix_32bits "32") - set(_matlab_bin_suffix_64bits "64") -endif() - - - -set(MATLAB_INCLUDE_DIR_TO_LOOK ${Matlab_ROOT_DIR}/extern/include) -if(_matlab_64Build) - set(_matlab_current_suffix ${_matlab_bin_suffix_64bits}) -else() - set(_matlab_current_suffix ${_matlab_bin_suffix_32bits}) -endif() - -set(Matlab_BINARIES_DIR - ${Matlab_ROOT_DIR}/bin/${_matlab_bin_prefix}${_matlab_current_suffix}) -set(Matlab_EXTERN_LIBRARY_DIR - ${Matlab_ROOT_DIR}/extern/lib/${_matlab_bin_prefix}${_matlab_current_suffix}) - -if(WIN32) - set(_matlab_lib_dir_for_search ${Matlab_EXTERN_LIBRARY_DIR}/microsoft) - set(_matlab_lib_prefix_for_search "lib") -else() - set(_matlab_lib_dir_for_search ${Matlab_BINARIES_DIR}) - set(_matlab_lib_prefix_for_search "lib") -endif() - -unset(_matlab_64Build) - - -if(NOT DEFINED Matlab_MEX_EXTENSION) - set(_matlab_mex_extension "") - matlab_get_mex_suffix("${Matlab_ROOT_DIR}" _matlab_mex_extension) - - # This variable goes to the cache. - set(Matlab_MEX_EXTENSION ${_matlab_mex_extension} CACHE STRING "Extensions for the mex targets (automatically given by Matlab)") - unset(_matlab_mex_extension) -endif() - - -if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] [DEBUG]_matlab_lib_prefix_for_search = ${_matlab_lib_prefix_for_search} | _matlab_lib_dir_for_search = ${_matlab_lib_dir_for_search}") -endif() - - - -# internal -# This small stub around find_library is to prevent any pollution of CMAKE_FIND_LIBRARY_PREFIXES in the global scope. -# This is the function to be used below instead of the find_library directives. -function(_Matlab_find_library _matlab_library_prefix) - set(CMAKE_FIND_LIBRARY_PREFIXES ${CMAKE_FIND_LIBRARY_PREFIXES} ${_matlab_library_prefix}) - find_library(${ARGN}) -endfunction() - - -set(_matlab_required_variables) - - -# the MEX library/header are required -find_path( - Matlab_INCLUDE_DIRS - mex.h - PATHS ${MATLAB_INCLUDE_DIR_TO_LOOK} - NO_DEFAULT_PATH - ) -list(APPEND _matlab_required_variables Matlab_INCLUDE_DIRS) - -_Matlab_find_library( - ${_matlab_lib_prefix_for_search} - Matlab_MEX_LIBRARY - mex - PATHS ${_matlab_lib_dir_for_search} - NO_DEFAULT_PATH -) - - -list(APPEND _matlab_required_variables Matlab_MEX_LIBRARY) - -# the MEX extension is required -list(APPEND _matlab_required_variables Matlab_MEX_EXTENSION) - -# the matlab root is required -list(APPEND _matlab_required_variables Matlab_ROOT_DIR) - - -# component Mex Compiler -list(FIND Matlab_FIND_COMPONENTS MEX_COMPILER _matlab_find_mex_compiler) -if(_matlab_find_mex_compiler GREATER -1) - find_program( - Matlab_MEX_COMPILER - "mex" - PATHS ${Matlab_BINARIES_DIR} - DOC "Matlab MEX compiler" - NO_DEFAULT_PATH - ) - - if(Matlab_MEX_COMPILER) - set(Matlab_MEX_COMPILER_FOUND TRUE) - endif() -endif() -unset(_matlab_find_mex_compiler) - -# component Matlab program -list(FIND Matlab_FIND_COMPONENTS MAIN_PROGRAM _matlab_find_matlab_program) -if(_matlab_find_matlab_program GREATER -1) - - find_program( - Matlab_MAIN_PROGRAM - matlab - PATHS ${Matlab_ROOT_DIR} ${Matlab_ROOT_DIR}/bin - DOC "Matlab main program" - NO_DEFAULT_PATH - ) - - if(Matlab_MAIN_PROGRAM) - set(Matlab_MAIN_PROGRAM_FOUND TRUE) - endif() - -endif() -unset(_matlab_find_matlab_program) - -# Component MX library -list(FIND Matlab_FIND_COMPONENTS MX_LIBRARY _matlab_find_mx) -if(_matlab_find_mx GREATER -1) - _Matlab_find_library( - ${_matlab_lib_prefix_for_search} - Matlab_MX_LIBRARY - mx - PATHS ${_matlab_lib_dir_for_search} - NO_DEFAULT_PATH - ) - - if(Matlab_MX_LIBRARY) - set(Matlab_MX_LIBRARY_FOUND TRUE) - endif() -endif() -unset(_matlab_find_mx) - - -# Component ENG library -list(FIND Matlab_FIND_COMPONENTS ENG_LIBRARY _matlab_find_eng) -if(_matlab_find_eng GREATER -1) - _Matlab_find_library( - ${_matlab_lib_prefix_for_search} - Matlab_ENG_LIBRARY - eng - PATHS ${_matlab_lib_dir_for_search} - NO_DEFAULT_PATH - ) - if(Matlab_ENG_LIBRARY) - set(Matlab_ENG_LIBRARY_FOUND TRUE) - endif() -endif() -unset(_matlab_find_eng) - - - - - -unset(_matlab_lib_dir_for_search) - - -set(Matlab_LIBRARIES ${Matlab_MEX_LIBRARY} ${Matlab_MX_LIBRARY} ${Matlab_ENG_LIBRARY}) - - -find_package_handle_standard_args( - Matlab - FOUND_VAR Matlab_FOUND - REQUIRED_VARS ${_matlab_required_variables} - VERSION_VAR Matlab_VERSION_STRING - HANDLE_COMPONENTS) - -unset(_matlab_required_variables) -unset(_matlab_bin_prefix) -unset(_matlab_bin_suffix_32bits) -unset(_matlab_bin_suffix_64bits) -unset(_matlab_current_suffix) -unset(_matlab_lib_dir_for_search) -unset(_matlab_lib_prefix_for_search) - -if(Matlab_INCLUDE_DIRS AND Matlab_LIBRARIES) - mark_as_advanced( - #Matlab_LIBRARIES - Matlab_MEX_LIBRARY - Matlab_MX_LIBRARY - Matlab_ENG_LIBRARY - Matlab_INCLUDE_DIRS - Matlab_FOUND - #Matlab_ROOT_DIR - #Matlab_VERSION_STRING - Matlab_MAIN_PROGRAM - #Matlab_MEX_EXTENSION - Matlab_MEXEXTENSIONS_PROG - Matlab_MEX_EXTENSION - #Matlab_BINARIES_DIR - ) -endif() diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 007f2d0e5..62cf9590d 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -1,5 +1,5 @@ -# Copyright (C) 2013-2015 Instituto Italiano di Tecnologia -# Author: Jorhabib Eljaik, Francesco Romano +# Copyright (C) 2013-2017 iCub Facility - Istituto Italiano di Tecnologia +# Author: Jorhabib Eljaik, Francesco Romano, Diego Ferigo # CopyPolicy: Released under the terms of the GNU GPL v2.0. find_package(YARP REQUIRED) @@ -14,18 +14,17 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${YARP_MODULE_PATH}) include(YarpInstallationHelpers) yarp_configure_external_installation(codyco) -find_package(Matlab REQUIRED - MX_LIBRARY - ENG_LIBRARY - MAIN_PROGRAM) +find_package(Matlab REQUIRED MX_LIBRARY + ENG_LIBRARY + MAIN_PROGRAM) if(NOT Matlab_FOUND) message(FATAL_ERROR "Matlab not found") endif() -#### Settings for rpath +# Settings for rpath if(NOT MSVC) - #add the option to enable RPATH + # Add the option to enable RPATH option(WB-TOOLBOX_ENABLE_RPATH "Enable RPATH installation" TRUE) mark_as_advanced(WB-TOOLBOX_ENABLE_RPATH) endif(NOT MSVC) @@ -37,22 +36,8 @@ add_install_rpath_support(BIN_DIRS ${CMAKE_INSTALL_PREFIX}/bin USE_LINK_PATH) find_package(Eigen3 REQUIRED) -find_package(wholeBodyInterface 0.2.5 REQUIRED) -find_package(yarpWholeBodyInterface 0.3.2 REQUIRED) find_package(iDynTree 0.7.2 REQUIRED) -#On new versions of Clang, MATLAB requires C++11. -#I enable it on all Clangs -if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") - if(${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - if (${CMAKE_GENERATOR} MATCHES "Xcode") - #this should set explictly the option in xcode. Unfortunately it does not work. - set(XCODE_ATTRIBUTE_CLANG_CXX_LANGUAGE_STANDARD "C++11") - endif(${CMAKE_GENERATOR} MATCHES "Xcode") - endif(${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") -endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") - include(Utilities) configure_block(BLOCK_NAME "Base" @@ -64,7 +49,7 @@ configure_block(BLOCK_NAME "Base" src/base/Log.cpp src/base/ToolboxSingleton.cpp src/base/factory.cpp - src/base/SimulinkBlockInformation.cpp #this is temp + src/base/SimulinkBlockInformation.cpp src/base/Signal.cpp src/base/Configuration.cpp src/base/RobotInterface.cpp @@ -74,7 +59,7 @@ configure_block(BLOCK_NAME "Base" include/base/WBBlock.h include/base/Log.h include/base/ToolboxSingleton.h - include/base/SimulinkBlockInformation.h #this is temp + include/base/SimulinkBlockInformation.h include/base/Signal.h include/base/Configuration.h include/base/RobotInterface.h @@ -195,8 +180,8 @@ configure_block(BLOCK_NAME "Set Low-Level PIDs" LIST_PREFIX WBT SOURCES src/SetLowLevelPID.cpp HEADERS include/SetLowLevelPID.h) - -configure_block(BLOCK_NAME "Get Estimate" +# +configure_block(BLOCK_NAME "Get Measurement" GROUP "State" LIST_PREFIX WBT SOURCES src/GetMeasurement.cpp @@ -216,10 +201,11 @@ include_directories(include) include_directories(include/base) include_directories(SYSTEM ${Matlab_INCLUDE_DIRS} "${Matlab_ROOT_DIR}/simulink/include") -include_directories(SYSTEM ${YARP_INCLUDE_DIRS} ${yarpWholeBodyInterface_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) +include_directories(SYSTEM ${YARP_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) -list(APPEND LINKED_LIBRARIES ${YARP_LIBRARIES} ${yarpWholeBodyInterface_LIBRARIES}) +list(APPEND LINKED_LIBRARIES ${YARP_LIBRARIES}) list(APPEND LINKED_LIBRARIES ${iDynTree_LIBRARIES}) + if (WBT_USES_ICUB) list(APPEND LINKED_LIBRARIES ctrlLib) if (${ICUB_USE_IPOPT}) @@ -227,7 +213,7 @@ if (WBT_USES_ICUB) endif() endif() -#Adding files used for the generation of the dynamic library +# Adding files used for the generation of the dynamic library matlab_add_mex( NAME WBToolbox SRC ${ALL_HEADERS} ${ALL_SOURCES} @@ -236,46 +222,40 @@ matlab_add_mex( # Link with MxAnyType library # TODO: this will become an external project sooner or later -target_link_libraries(WBToolbox PUBLIC MxAnyType) +target_link_libraries(WBToolbox MxAnyType) # Link with ClockServer library add_subdirectory(autogenerated/) -target_link_libraries(WBToolbox PUBLIC ClockServer) - -# Remote Inverse Kinematics requires C++11 -if (CMAKE_VERSION VERSION_LESS "3.1") - if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR - CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - target_compile_options(WBToolbox PUBLIC "-std=c++11") - endif() -else() - set(CXX_STANDARD_REQUIRED ON) - set_property(TARGET WBToolbox PROPERTY CXX_STANDARD 11) -endif() +target_link_libraries(WBToolbox ClockServer) install(TARGETS WBToolbox DESTINATION ${CMAKE_INSTALL_PREFIX}/mex) -## if you export the mdl / slx library, remeber to call this command in matlab before saving it -## set_param(gcs, 'EnableLBRepository','on'); - # The following line is to properly configure the installation script of the toolbox -configure_file(${CMAKE_CURRENT_SOURCE_DIR}/.startup_wbitoolbox.m.in ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m) -configure_file(${CMAKE_CURRENT_SOURCE_DIR}/.setupForMatlabDebug.m.in ${CMAKE_BINARY_DIR}/setupForMatlabDebug.m) +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/matlab/startup_wbitoolbox.m.in + ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m) # Custom script to generate the library to be committed on the repository # This target is excluded from the normal build and must be called explicitly by the # developer who modifies the library -add_custom_target(export_libraries ${Matlab_MAIN_PROGRAM} -nosplash -r export_library WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) +add_custom_target(export_libraries ${Matlab_MAIN_PROGRAM} -nosplash -nodesktop -r export_library WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/matlab) set_target_properties(export_libraries PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD 1) # Install configuration files install(FILES ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m DESTINATION ${WB-TOOLBOX_SHARE_DIR}) -install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/slblocks.m DESTINATION ${WB-TOOLBOX_SHARE_DIR}) +install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/matlab/slblocks.m DESTINATION ${WB-TOOLBOX_SHARE_DIR}) + +# Install the package folder +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/matlab/+WBToolbox + DESTINATION ${WB-TOOLBOX_SHARE_DIR}) + #if MAJOR >= 2014 && MINOR >= b # Note: We had issues with Matlab 2014b and .mdl models. # But this issue seems to have been disappeared in 2015b. We have to check if we need to enable this if again -if (${Matlab_VERSION_STRING} GREATER "8.3") - install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolboxLibrary.slx DESTINATION ${WB-TOOLBOX_SHARE_DIR}) -else() - install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolboxLibrary.mdl DESTINATION ${WB-TOOLBOX_SHARE_DIR}) -endif() +# TODO: check if the mdl support is still required +# if (${Matlab_VERSION_STRING} GREATER "8.3") + # install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolboxLibrary.slx DESTINATION ${WB-TOOLBOX_SHARE_DIR}) +# else() + # install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolboxLibrary.mdl DESTINATION ${WB-TOOLBOX_SHARE_DIR}) +# endif() +install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/matlab/library/exported/WBToolboxLibrary.slx + DESTINATION ${WB-TOOLBOX_SHARE_DIR}) diff --git a/toolbox/WBToolboxLibrary.mdl b/toolbox/WBToolboxLibrary.mdl deleted file mode 100644 index a0e924d52..000000000 --- a/toolbox/WBToolboxLibrary.mdl +++ /dev/null @@ -1,4383 +0,0 @@ -Library { - Name "WBToolboxLibrary" - Version 7.9 - LibraryType "BlockLibrary" - SavedCharacterEncoding "US-ASCII" - ScopeRefreshTime 0.035000 - OverrideScopeRefreshTime on - DisableAllScopes off - FPTRunName "Run 1" - MaxMDLFileLineLength 120 - Created "Thu Feb 06 02:21:39 2014" - Creator "jorhabib" - UpdateHistory "UpdateHistoryNever" - ModifiedByFormat "%" - LastModifiedBy "makaveli" - ModifiedDateFormat "%" - LastModifiedDate "Wed Aug 23 12:36:18 2017" - RTWModifiedTimeStamp 425392578 - ModelVersionFormat "1.%" - ConfigurationManager "none" - SampleTimeColors off - SampleTimeAnnotations off - LibraryLinkDisplay "disabled" - WideLines off - ShowLineDimensions off - ShowPortDataTypes off - ShowDesignRanges off - ShowLoopsOnError on - IgnoreBidirectionalLines off - ShowStorageClass off - ShowTestPointIcons on - ShowSignalResolutionIcons on - ShowViewerIcons on - SortedOrder off - ExecutionContextIcon off - ShowLinearizationAnnotations on - BlockNameDataTip off - BlockParametersDataTip off - BlockDescriptionStringDataTip off - ToolBar on - StatusBar on - BrowserShowLibraryLinks off - BrowserLookUnderMasks off - SimulationMode "normal" - LinearizationMsg "none" - Profile off - ParamWorkspaceSource "MATLABWorkspace" - RecordCoverage off - CovSaveName "covdata" - CovMetricSettings "dw" - CovNameIncrementing off - CovHtmlReporting on - CovForceBlockReductionOff on - CovSaveCumulativeToWorkspaceVar on - CovSaveSingleToWorkspaceVar on - CovCumulativeReport off - CovReportOnPause on - CovModelRefEnable "Off" - CovExternalEMLEnable off - ExtModeBatchMode off - ExtModeEnableFloating on - ExtModeTrigType "manual" - ExtModeTrigMode "normal" - ExtModeTrigPort "1" - ExtModeTrigElement "any" - ExtModeTrigDuration 1000 - ExtModeTrigDurationFloating "auto" - ExtModeTrigHoldOff 0 - ExtModeTrigDelay 0 - ExtModeTrigDirection "rising" - ExtModeTrigLevel 0 - ExtModeArchiveMode "off" - ExtModeAutoIncOneShot off - ExtModeIncDirWhenArm off - ExtModeAddSuffixToVar off - ExtModeWriteAllDataToWs off - ExtModeArmWhenConnect on - ExtModeSkipDownloadWhenConnect off - ExtModeLogAll on - ExtModeAutoUpdateStatusClock on - ShowModelReferenceBlockVersion off - ShowModelReferenceBlockIO off - Array { - Type "Handle" - Dimension 1 - Simulink.ConfigSet { - $ObjectID 7 - Version "1.12.0" - Array { - Type "Handle" - Dimension 9 - Simulink.SolverCC { - $ObjectID 8 - Version "1.12.0" - StartTime "0.0" - StopTime "10.0" - AbsTol "auto" - FixedStep "auto" - InitialStep "auto" - MaxNumMinSteps "-1" - MaxOrder 5 - ZcThreshold "auto" - ConsecutiveZCsStepRelTol "10*128*eps" - MaxConsecutiveZCs "1000" - ExtrapolationOrder 4 - NumberNewtonIterations 1 - MaxStep "auto" - MinStep "auto" - MaxConsecutiveMinStep "1" - RelTol "1e-3" - SolverMode "Auto" - EnableConcurrentExecution off - ConcurrentTasks off - Solver "ode45" - SolverName "ode45" - SolverJacobianMethodControl "auto" - ShapePreserveControl "DisableAll" - ZeroCrossControl "UseLocalSettings" - ZeroCrossAlgorithm "Nonadaptive" - AlgebraicLoopSolver "TrustRegion" - SolverResetMethod "Fast" - PositivePriorityOrder off - AutoInsertRateTranBlk off - SampleTimeConstraint "Unconstrained" - InsertRTBMode "Whenever possible" - } - Simulink.DataIOCC { - $ObjectID 9 - Version "1.12.0" - Decimation "1" - ExternalInput "[t, u]" - FinalStateName "xFinal" - InitialState "xInitial" - LimitDataPoints on - MaxDataPoints "1000" - LoadExternalInput off - LoadInitialState off - SaveFinalState off - SaveCompleteFinalSimState off - SaveFormat "Array" - SignalLoggingSaveFormat "ModelDataLogs" - SaveOutput on - SaveState off - SignalLogging on - DSMLogging on - InspectSignalLogs off - SaveTime on - ReturnWorkspaceOutputs off - StateSaveName "xout" - TimeSaveName "tout" - OutputSaveName "yout" - SignalLoggingName "logsout" - DSMLoggingName "dsmout" - OutputOption "RefineOutputTimes" - OutputTimes "[]" - ReturnWorkspaceOutputsName "out" - Refine "1" - } - Simulink.OptimizationCC { - $ObjectID 10 - Version "1.12.0" - Array { - Type "Cell" - Dimension 8 - Cell "BooleansAsBitfields" - Cell "PassReuseOutputArgsAs" - Cell "PassReuseOutputArgsThreshold" - Cell "ZeroExternalMemoryAtStartup" - Cell "ZeroInternalMemoryAtStartup" - Cell "OptimizeModelRefInitCode" - Cell "NoFixptDivByZeroProtection" - Cell "UseSpecifiedMinMax" - PropName "DisabledProps" - } - BlockReduction on - BooleanDataType on - ConditionallyExecuteInputs on - InlineParams off - UseIntDivNetSlope off - UseFloatMulNetSlope off - UseSpecifiedMinMax off - InlineInvariantSignals off - OptimizeBlockIOStorage on - BufferReuse on - EnhancedBackFolding off - StrengthReduction off - ExpressionFolding on - BooleansAsBitfields off - BitfieldContainerType "uint_T" - EnableMemcpy on - MemcpyThreshold 64 - PassReuseOutputArgsAs "Structure reference" - ExpressionDepthLimit 2147483647 - LocalBlockOutputs on - RollThreshold 5 - StateBitsets off - DataBitsets off - ZeroExternalMemoryAtStartup on - ZeroInternalMemoryAtStartup on - InitFltsAndDblsToZero off - NoFixptDivByZeroProtection off - EfficientFloat2IntCast off - EfficientMapNaN2IntZero on - OptimizeModelRefInitCode off - LifeSpan "inf" - MaxStackSize "Inherit from target" - BufferReusableBoundary on - SimCompilerOptimization "Off" - AccelVerboseBuild off - } - Simulink.DebuggingCC { - $ObjectID 11 - Version "1.12.0" - RTPrefix "error" - ConsistencyChecking "none" - ArrayBoundsChecking "none" - SignalInfNanChecking "none" - SignalRangeChecking "none" - ReadBeforeWriteMsg "UseLocalSettings" - WriteAfterWriteMsg "UseLocalSettings" - WriteAfterReadMsg "UseLocalSettings" - AlgebraicLoopMsg "warning" - ArtificialAlgebraicLoopMsg "warning" - SaveWithDisabledLinksMsg "warning" - SaveWithParameterizedLinksMsg "warning" - CheckSSInitialOutputMsg on - UnderspecifiedInitializationDetection "Classic" - MergeDetectMultiDrivingBlocksExec "none" - CheckExecutionContextPreStartOutputMsg off - CheckExecutionContextRuntimeOutputMsg off - SignalResolutionControl "UseLocalSettings" - BlockPriorityViolationMsg "warning" - MinStepSizeMsg "warning" - TimeAdjustmentMsg "none" - MaxConsecutiveZCsMsg "error" - MaskedZcDiagnostic "warning" - IgnoredZcDiagnostic "warning" - SolverPrmCheckMsg "warning" - InheritedTsInSrcMsg "warning" - MultiTaskDSMMsg "error" - MultiTaskCondExecSysMsg "error" - MultiTaskRateTransMsg "error" - SingleTaskRateTransMsg "none" - TasksWithSamePriorityMsg "warning" - SigSpecEnsureSampleTimeMsg "warning" - CheckMatrixSingularityMsg "none" - IntegerOverflowMsg "warning" - Int32ToFloatConvMsg "warning" - ParameterDowncastMsg "error" - ParameterOverflowMsg "error" - ParameterUnderflowMsg "none" - ParameterPrecisionLossMsg "warning" - ParameterTunabilityLossMsg "warning" - FixptConstUnderflowMsg "none" - FixptConstOverflowMsg "none" - FixptConstPrecisionLossMsg "none" - UnderSpecifiedDataTypeMsg "none" - UnnecessaryDatatypeConvMsg "none" - VectorMatrixConversionMsg "none" - InvalidFcnCallConnMsg "error" - FcnCallInpInsideContextMsg "Enable All" - SignalLabelMismatchMsg "none" - UnconnectedInputMsg "warning" - UnconnectedOutputMsg "warning" - UnconnectedLineMsg "warning" - SFcnCompatibilityMsg "none" - FrameProcessingCompatibilityMsg "error" - UniqueDataStoreMsg "none" - BusObjectLabelMismatch "warning" - RootOutportRequireBusObject "warning" - AssertControl "UseLocalSettings" - ModelReferenceIOMsg "none" - ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" - ModelReferenceVersionMismatchMessage "none" - ModelReferenceIOMismatchMessage "none" - UnknownTsInhSupMsg "warning" - ModelReferenceDataLoggingMessage "warning" - ModelReferenceSymbolNameMessage "warning" - ModelReferenceExtraNoncontSigs "error" - StateNameClashWarn "warning" - SimStateInterfaceChecksumMismatchMsg "warning" - SimStateOlderReleaseMsg "error" - InitInArrayFormatMsg "warning" - StrictBusMsg "ErrorLevel1" - BusNameAdapt "WarnAndRepair" - NonBusSignalsTreatedAsBus "none" - BlockIODiagnostic "none" - SFUnusedDataAndEventsDiag "warning" - SFUnexpectedBacktrackingDiag "warning" - SFInvalidInputDataAccessInChartInitDiag "warning" - SFNoUnconditionalDefaultTransitionDiag "warning" - SFTransitionOutsideNaturalParentDiag "warning" - SFUnconditionalTransitionShadowingDiag "warning" - ModelReferenceCSMismatchMessage "none" - } - Simulink.HardwareCC { - $ObjectID 12 - Version "1.12.0" - ProdBitPerChar 8 - ProdBitPerShort 16 - ProdBitPerInt 32 - ProdBitPerLong 32 - ProdBitPerFloat 32 - ProdBitPerDouble 64 - ProdBitPerPointer 32 - ProdLargestAtomicInteger "Char" - ProdLargestAtomicFloat "None" - ProdIntDivRoundTo "Undefined" - ProdEndianess "Unspecified" - ProdWordSize 32 - ProdShiftRightIntArith on - ProdHWDeviceType "32-bit Generic" - TargetBitPerChar 8 - TargetBitPerShort 16 - TargetBitPerInt 32 - TargetBitPerLong 32 - TargetBitPerFloat 32 - TargetBitPerDouble 64 - TargetBitPerPointer 32 - TargetLargestAtomicInteger "Char" - TargetLargestAtomicFloat "None" - TargetShiftRightIntArith on - TargetIntDivRoundTo "Undefined" - TargetEndianess "Unspecified" - TargetWordSize 32 - TargetPreprocMaxBitsSint 32 - TargetPreprocMaxBitsUint 32 - TargetHWDeviceType "Specified" - TargetUnknown off - ProdEqTarget on - } - Simulink.ModelReferenceCC { - $ObjectID 13 - Version "1.12.0" - UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" - CheckModelReferenceTargetMessage "error" - EnableParallelModelReferenceBuilds off - ParallelModelReferenceErrorOnInvalidPool on - ParallelModelReferenceMATLABWorkerInit "None" - ModelReferenceNumInstancesAllowed "Multi" - PropagateVarSize "Infer from blocks in model" - ModelReferencePassRootInputsByReference on - ModelReferenceMinAlgLoopOccurrences off - PropagateSignalLabelsOutOfModel off - SupportModelReferenceSimTargetCustomCode off - } - Simulink.SFSimCC { - $ObjectID 14 - Version "1.12.0" - SFSimEcho on - SimCtrlC on - SimIntegrity on - SimUseLocalCustomCode off - SimParseCustomCode on - SimBuildMode "sf_incremental_build" - } - Simulink.RTWCC { - $BackupClass "Simulink.RTWCC" - $ObjectID 15 - Version "1.12.0" - Array { - Type "Cell" - Dimension 13 - Cell "IncludeHyperlinkInReport" - Cell "GenerateTraceInfo" - Cell "GenerateTraceReport" - Cell "GenerateTraceReportSl" - Cell "GenerateTraceReportSf" - Cell "GenerateTraceReportEml" - Cell "GenerateWebview" - Cell "GenerateCodeMetricsReport" - Cell "GenerateCodeReplacementReport" - Cell "PortableWordSizes" - Cell "GenerateMissedCodeReplacementReport" - Cell "GenerateErtSFunction" - Cell "CreateSILPILBlock" - PropName "DisabledProps" - } - SystemTargetFile "grt.tlc" - TLCOptions "" - GenCodeOnly off - MakeCommand "make_rtw" - GenerateMakefile on - TemplateMakefile "grt_default_tmf" - PostCodeGenCommand "" - GenerateReport off - SaveLog off - RTWVerbose on - RetainRTWFile off - ProfileTLC off - TLCDebug off - TLCCoverage off - TLCAssert off - RTWUseLocalCustomCode off - RTWUseSimCustomCode off - IncludeHyperlinkInReport off - LaunchReport off - TargetLang "C" - IncludeBusHierarchyInRTWFileBlockHierarchyMap off - GenerateTraceInfo off - GenerateTraceReport off - GenerateTraceReportSl off - GenerateTraceReportSf off - GenerateTraceReportEml off - GenerateWebview off - GenerateCodeMetricsReport off - GenerateCodeReplacementReport off - RTWCompilerOptimization "Off" - RTWCustomCompilerOptimizations "" - CheckMdlBeforeBuild "Off" - Array { - Type "Handle" - Dimension 2 - Simulink.CodeAppCC { - $ObjectID 16 - Version "1.12.0" - Array { - Type "Cell" - Dimension 25 - Cell "IgnoreCustomStorageClasses" - Cell "IgnoreTestpoints" - Cell "InsertBlockDesc" - Cell "InsertPolySpaceComments" - Cell "SFDataObjDesc" - Cell "MATLABFcnDesc" - Cell "SimulinkDataObjDesc" - Cell "DefineNamingRule" - Cell "SignalNamingRule" - Cell "ParamNamingRule" - Cell "InlinedPrmAccess" - Cell "CustomSymbolStr" - Cell "CustomSymbolStrGlobalVar" - Cell "CustomSymbolStrType" - Cell "CustomSymbolStrField" - Cell "CustomSymbolStrFcn" - Cell "CustomSymbolStrFcnArg" - Cell "CustomSymbolStrBlkIO" - Cell "CustomSymbolStrTmpVar" - Cell "CustomSymbolStrMacro" - Cell "ReqsInCode" - Cell "InternalIdentifier" - Cell "CustomSymbolStrModelFcn" - Cell "CustomSymbolStrUtil" - Cell "CustomUserTokenString" - PropName "DisabledProps" - } - ForceParamTrailComments off - GenerateComments on - IgnoreCustomStorageClasses on - IgnoreTestpoints off - IncHierarchyInIds off - MaxIdLength 31 - PreserveName off - PreserveNameWithParent off - ShowEliminatedStatement off - IncAutoGenComments off - SimulinkDataObjDesc off - SFDataObjDesc off - MATLABFcnDesc off - IncDataTypeInIds off - MangleLength 1 - CustomSymbolStrGlobalVar "$R$N$M" - CustomSymbolStrType "$N$R$M_T" - CustomSymbolStrField "$N$M" - CustomSymbolStrFcn "$R$N$M$F" - CustomSymbolStrFcnArg "rt$I$N$M" - CustomSymbolStrBlkIO "rtb_$N$M" - CustomSymbolStrTmpVar "$N$M" - CustomSymbolStrMacro "$R$N$M" - DefineNamingRule "None" - ParamNamingRule "None" - SignalNamingRule "None" - InsertBlockDesc off - InsertPolySpaceComments off - SimulinkBlockComments on - MATLABSourceComments off - EnableCustomComments off - InlinedPrmAccess "Literals" - ReqsInCode off - UseSimReservedNames off - } - Simulink.GRTTargetCC { - $BackupClass "Simulink.TargetCC" - $ObjectID 17 - Version "1.12.0" - Array { - Type "Cell" - Dimension 18 - Cell "GeneratePreprocessorConditionals" - Cell "IncludeMdlTerminateFcn" - Cell "SupportNonInlinedSFcns" - Cell "SuppressErrorStatus" - Cell "ERTCustomFileBanners" - Cell "GenerateSampleERTMain" - Cell "GenerateTestInterfaces" - Cell "ModelStepFunctionPrototypeControlCompliant" - Cell "CPPClassGenCompliant" - Cell "PortableWordSizes" - Cell "PurelyIntegerCode" - Cell "SupportComplex" - Cell "SupportAbsoluteTime" - Cell "SupportContinuousTime" - Cell "MultiInstanceERTCode" - Cell "RemoveResetFunc" - Cell "ExistingSharedCode" - Cell "RemoveDisableFunc" - PropName "DisabledProps" - } - TargetFcnLib "ansi_tfl_table_tmw.mat" - TargetLibSuffix "" - TargetPreCompLibLocation "" - CodeReplacementLibrary "None" - UtilityFuncGeneration "Auto" - ERTMultiwordTypeDef "System defined" - ERTMultiwordLength 256 - MultiwordLength 2048 - GenerateFullHeader on - GenerateSampleERTMain off - GenerateTestInterfaces off - ModelReferenceCompliant on - ParMdlRefBuildCompliant on - CompOptLevelCompliant on - ConcurrentExecutionCompliant on - IncludeMdlTerminateFcn on - GeneratePreprocessorConditionals "Disable all" - CombineOutputUpdateFcns off - CombineSignalStateStructs off - SuppressErrorStatus off - ERTFirstTimeCompliant off - IncludeFileDelimiter "Auto" - ERTCustomFileBanners off - SupportAbsoluteTime on - LogVarNameModifier "rt_" - MatFileLogging on - MultiInstanceERTCode off - SupportNonFinite on - SupportComplex on - PurelyIntegerCode off - SupportContinuousTime on - SupportNonInlinedSFcns on - SupportVariableSizeSignals off - ParenthesesLevel "Nominal" - ModelStepFunctionPrototypeControlCompliant off - CPPClassGenCompliant on - AutosarCompliant off - GRTInterface on - UseMalloc off - ExtMode off - ExtModeStaticAlloc off - ExtModeTesting off - ExtModeStaticAllocSize 1000000 - ExtModeTransport 0 - ExtModeMexFile "ext_comm" - ExtModeIntrfLevel "Level1" - RTWCAPISignals off - RTWCAPIParams off - RTWCAPIStates off - RTWCAPIRootIO off - GenerateASAP2 off - } - PropName "Components" - } - } - hdlcoderui.hdlcc { - $ObjectID 19 - Version "1.12.0" - Description "HDL Coder custom configuration component" - Name "HDL Coder" - Array { - Type "Cell" - Dimension 1 - Cell " " - PropName "HDLConfigFile" - } - HDLCActiveTab "0" - } - PropName "Components" - } - Name "Configuration" - CurrentDlgPage "Solver" - ConfigPrmDlgPosition [ 963, 135, 1843, 765 ] - } - PropName "ConfigurationSets" - } - ExplicitPartitioning off - BlockDefaults { - ForegroundColor "black" - BackgroundColor "white" - DropShadow off - NamePlacement "normal" - FontName "Helvetica" - FontSize 10 - FontWeight "normal" - FontAngle "normal" - ShowName on - BlockRotation 0 - BlockMirror off - } - AnnotationDefaults { - HorizontalAlignment "center" - VerticalAlignment "middle" - ForegroundColor "black" - BackgroundColor "white" - DropShadow off - FontName "Helvetica" - FontSize 10 - FontWeight "normal" - FontAngle "normal" - UseDisplayTextAsClickCallback off - } - LineDefaults { - FontName "Helvetica" - FontSize 9 - FontWeight "normal" - FontAngle "normal" - } - BlockParameterDefaults { - Block { - BlockType Clock - DisplayTime off - Decimation "10" - } - Block { - BlockType Constant - Value "1" - VectorParams1D on - SamplingMode "Sample based" - OutMin "[]" - OutMax "[]" - OutDataTypeStr "Inherit: Inherit from 'Constant value'" - LockScale off - SampleTime "inf" - FramePeriod "inf" - PreserveConstantTs off - } - Block { - BlockType Demux - Outputs "4" - DisplayOption "none" - BusSelectionMode off - } - Block { - BlockType Gain - Gain "1" - Multiplication "Element-wise(K.*u)" - ParamMin "[]" - ParamMax "[]" - ParamDataTypeStr "Inherit: Same as input" - OutMin "[]" - OutMax "[]" - OutDataTypeStr "Inherit: Same as input" - LockScale off - RndMeth "Floor" - SaturateOnIntegerOverflow on - SampleTime "-1" - } - Block { - BlockType Inport - Port "1" - OutputFunctionCall off - OutMin "[]" - OutMax "[]" - OutDataTypeStr "Inherit: auto" - LockScale off - BusOutputAsStruct off - PortDimensions "-1" - VarSizeSig "Inherit" - SampleTime "-1" - SignalType "auto" - SamplingMode "auto" - LatchByDelayingOutsideSignal off - LatchInputForFeedbackSignals off - Interpolate on - } - Block { - BlockType Outport - Port "1" - OutMin "[]" - OutMax "[]" - OutDataTypeStr "Inherit: auto" - LockScale off - BusOutputAsStruct off - PortDimensions "-1" - VarSizeSig "Inherit" - SampleTime "-1" - SignalType "auto" - SamplingMode "auto" - SourceOfInitialOutputValue "Dialog" - OutputWhenDisabled "held" - InitialOutput "[]" - } - Block { - BlockType Product - Inputs "2" - Multiplication "Element-wise(.*)" - CollapseMode "All dimensions" - CollapseDim "1" - InputSameDT on - OutMin "[]" - OutMax "[]" - OutDataTypeStr "Inherit: Same as first input" - LockScale off - RndMeth "Zero" - SaturateOnIntegerOverflow on - SampleTime "-1" - } - Block { - BlockType S-Function - FunctionName "system" - SFunctionModules "''" - PortCounts "[]" - } - Block { - BlockType SubSystem - ShowPortLabels "FromPortIcon" - Permissions "ReadWrite" - PermitHierarchicalResolution "All" - TreatAsAtomicUnit off - SystemSampleTime "-1" - RTWFcnNameOpts "Auto" - RTWFileNameOpts "Auto" - RTWMemSecFuncInitTerm "Inherit from model" - RTWMemSecFuncExecute "Inherit from model" - RTWMemSecDataConstants "Inherit from model" - RTWMemSecDataInternal "Inherit from model" - RTWMemSecDataParameters "Inherit from model" - SimViewingDevice off - DataTypeOverride "UseLocalSettings" - DataTypeOverrideAppliesTo "AllNumericTypes" - MinMaxOverflowLogging "UseLocalSettings" - SFBlockType "NONE" - GeneratePreprocessorConditionals off - PropagateVariantConditions off - TreatAsGroupedWhenPropagatingVariantConditions on - } - Block { - BlockType Sum - IconShape "rectangular" - Inputs "++" - CollapseMode "All dimensions" - CollapseDim "1" - InputSameDT on - AccumDataTypeStr "Inherit: Inherit via internal rule" - OutMin "[]" - OutMax "[]" - OutDataTypeStr "Inherit: Same as first input" - LockScale off - RndMeth "Floor" - SaturateOnIntegerOverflow on - SampleTime "-1" - } - Block { - BlockType Terminator - } - } - System { - Name "WBToolboxLibrary" - Location [747, 53, 2027, 774] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "black" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "273" - ReportName "simulink-default.rpt" - SIDHighWatermark "1773" - Block { - BlockType SubSystem - Name "Utilities" - SID "192" - Ports [] - Position [364, 17, 462, 114] - ZOrder -1 - ForegroundColor "white" - DropShadow on - RequestExecContextInheritance off - Variant off - MaskDisplay "image(imread('utilities.png'))" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - System { - Name "Utilities" - Location [551, 44, 1831, 765] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "166" - Block { - BlockType SubSystem - Name "DampPinv" - SID "104" - Ports [2, 1] - Position [435, 153, 505, 197] - ZOrder -1 - BackgroundColor "[0.848000, 0.128048, 0.320035]" - RequestExecContextInheritance off - Variant off - MaskPromptString "Tolerance" - MaskStyleString "edit" - MaskVariables "tol=@1;" - MaskTunableValueString "on" - MaskEnableString "on" - MaskVisibilityString "on" - MaskToolTipString "on" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "1e-4" - System { - Name "DampPinv" - Location [0, 29, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "100" - Block { - BlockType Inport - Name "mat" - SID "105" - Position [50, 53, 80, 67] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "sigma" - SID "106" - Position [50, 93, 80, 107] - ZOrder -2 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Damped Pseudo Inverse" - SID "107" - Ports [2, 1] - Position [105, 39, 200, 121] - ZOrder -4 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ParametersOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - Variant off - MaskType "Stateflow" - MaskDescription "Embedded MATLAB block" - MaskSelfModifiable on - MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p" - "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);" - MaskIconFrame on - MaskIconOpaque off - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "normalized" - System { - Name "Damped Pseudo Inverse" - Location [12, 45, 1279, 3773] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "100" - SIDHighWatermark "1773" - Block { - BlockType Inport - Name "mat" - SID "107::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "sigma" - SID "107::25" - Position [20, 136, 40, 154] - ZOrder 11 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "107::1618" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 98 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "107::1617" - Tag "Stateflow S-Function WBToolboxLibrary 6" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 97 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport off - Port { - PortNumber 2 - Name "DPinv" - RTWStorageClass "Auto" - DataLoggingNameMode "SignalName" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "107::1619" - Position [460, 241, 480, 259] - ZOrder 99 - } - Block { - BlockType Outport - Name "DPinv" - SID "107::5" - Position [460, 101, 480, 119] - ZOrder -6 - IconDisplay "Port number" - } - Line { - SrcBlock "mat" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - SrcBlock "sigma" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "DPinv" - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "DPinv" - DstPort 1 - } - Line { - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType Outport - Name "DPinv" - SID "108" - Position [225, 73, 255, 87] - ZOrder -5 - IconDisplay "Port number" - } - Line { - SrcBlock "sigma" - SrcPort 1 - DstBlock "Damped Pseudo Inverse" - DstPort 2 - } - Line { - SrcBlock "Damped Pseudo Inverse" - SrcPort 1 - DstBlock "DPinv" - DstPort 1 - } - Line { - SrcBlock "mat" - SrcPort 1 - DstBlock "Damped Pseudo Inverse" - DstPort 1 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType S-Function - Name "DoFs converter" - SID "1771" - Ports [1, 5] - Position [240, 241, 385, 309] - ZOrder 81 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend\n" - FunctionName "WBToolbox" - Parameters "'YARPWBIConverter',robotName,localName,wbiFile,wbiList,yarp2WBIOption" - SFunctionDeploymentMode off - EnableBusSupport off - MaskType "DoFs converter" - MaskDescription "This block converts from a DoFs serialization to a YARP serialization or viceversa depending on " - "its configuration.\nThe DoFs configuration is automatically currently taken from the WBI list option.\nThe robot na" - "me is taken from the environment variable YARP_ROBOT_NAME.\n\nParameters:\n\n- Conversion direction: specify if you" - " want to convert from YARP to the current DoFs serialization or viceversa.\n\nWBI parameters:\n\n- Robot Port Name:" - " Name of the ports opened by the robot. (e.g. icub).\n Set an empty string ('') to use the name s" - "pecified in the \n Whole Body Interface configuration file.\n- Model Name: Prefix name of the por" - "ts opened by the underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of" - " the Whole Body Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - MaskPromptString "Conversion Direction|Robot Port Name|Model Name|WBI filename|WBI list name" - MaskStyleString "popup(From YARP to WBI|From WBI to YARP),edit,edit,edit,edit" - MaskVariables "yarp2WBIOption=@1;robotName=@2;localName=@3;wbiFile=@4;wbiList=@5;" - MaskTunableValueString "on,on,on,on,on" - MaskCallbackString "||||" - MaskEnableString "on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on" - MaskToolTipString "on,on,on,on,on" - MaskDisplay "robotName = getenv('YARP_ROBOT_NAME');\n\ncurrentSelection = get_param(gcb,'yarp2WBIoption');\n\nif" - " strcmp(currentSelection,'From YARP to WBI')\n direction = 'YARP -> DoFs';\nelse\n direction = 'DoFs -> YARP'" - ";\nend\n\nfprintf('%s\\n\\n%s', robotName, direction);" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "From YARP to WBI|WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - MaskTabNameString "Block Parameters,WBI Parameters,WBI Parameters,WBI Parameters,WBI Parameters" - } - Block { - BlockType S-Function - Name "Minimum Jerk Trajectory Generator" - SID "1747" - Ports [1, 3] - Position [450, 73, 605, 127] - ZOrder 78 - FunctionName "WBToolbox" - Parameters "'MinimumJerkTrajectoryGenerator',sampleTime, settlingTime, firstDerivatives, secondDerivatives, expl" - "icitInitialValue, externalSettlingTime,resetOnSettlingTime" - SFunctionDeploymentMode off - EnableBusSupport off - MaskType "Minimum Jerk Trajectory Generator" - MaskDescription "This block wraps the minJerkTrajGen class from iCub::ctrl::minJerkTrajGen.\n\nThe Minimum Jerk T" - "rajectory Generator is approximated using a \n3rd order LTI dynamical system \n(for more details see [1]). \nPositi" - "on, velocity and acceleration trajectories are computed.\n The main advantage with respect to the standard polynomi" - "al \nform is that if the reference value yd changes \nthere is no need to recompute the filter coefficients.\n\n[1]" - " Pattacini, U.; Nori, F.; Natale, L.; Metta, G.; Sandini, G., \"An experimental evaluation of a novel minimum-jerk " - "cartesian controller for humanoid robots,\" in Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International C" - "onference on , vol., no., pp.1668-1674, 18-22 Oct. 2010\ndoi: 10.1109/IROS.2010.5650851\nURL: http://ieeexplore.iee" - "e.org/stamp/stamp.jsp?tp=&arnumber=5650851&isnumber=5648787" - MaskPromptString "External Settling Time|Settling Time|Sample Time|Reset on Settling Time Changes|Output First De" - "rivative|Output Second Derivative|Explicit Initial Value" - MaskStyleString "checkbox,edit,edit,checkbox,checkbox,checkbox,checkbox" - MaskVariables "externalSettlingTime=@1;settlingTime=@2;sampleTime=@3;resetOnSettlingTime=@4;firstDerivatives=@5;" - "secondDerivatives=@6;explicitInitialValue=@7;" - MaskTunableValueString "on,on,on,on,on,on,on" - MaskCallbackString "externalSettlingTime = get_param(gcb, 'externalSettlingTime');\nvisibility = get_param(gcb, '" - "MaskVisibilities');\nif(strcmp(externalSettlingTime, 'on'))\n visibility{2} = 'off';\n visibility{4} = 'on';\n" - "else\n visibility{2} = 'on';\n visibility{4} = 'off';\nend\nset_param(gcb, 'MaskVisibilities',visibility);\nc" - "lear externalSettlingTime||||||" - MaskEnableString "on,on,on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on,on,off" - MaskToolTipString "on,on,on,on,on,on,on" - MaskDisplay "firstDer = get_param(gcb, 'firstDerivatives');\nsecondDer = get_param(gcb, 'secondDerivatives');\ni" - "nitialValues = get_param(gcb, 'explicitInitialValue');\nexternalSettlingTimeParam = get_param(gcb, 'externalSettlin" - "gTime');\n\n%Inputs\nportIndex = 2;\nport_label('input', 1, 'Reference')\nif(strcmp(initialValues, 'on'))\n port" - "_label('input', portIndex, 'Initial Value')\n portIndex = portIndex + 1;\nend\n\nif(strcmp(externalSettlingTimeP" - "aram, 'on'))\n port_label('input', portIndex, 'Settling Time')\n portIndex = portIndex + 1;\nend\n\n%Outputs\n" - "port_label('output', 1, 'Signal')\nsecondDerPortIndex = 2;\nif(strcmp(firstDer, 'on'))\n port_label('output', 2," - " 'First Derivative')\n secondDerPortIndex = secondDerPortIndex + 1;\nend\nif(strcmp(secondDer, 'on'))\n port_" - "label('output', secondDerPortIndex, 'Second Derivative')\nend\n\n\n" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "off|3|0.01|off|on|on|off" - MaskTabNameString "Trajectory Parameters,Trajectory Parameters,Trajectory Parameters,Trajectory Parameters,Input/" - "Output,Input/Output,Input/Output" - } - Block { - BlockType S-Function - Name "Real Time Synchronizer" - SID "1657" - Ports [] - Position [155, 14, 280, 51] - ZOrder 23 - ForegroundColor "white" - BackgroundColor "gray" - ShowName off - FunctionName "WBToolbox" - Parameters "'RealTimeSynchronizer',period" - SFunctionDeploymentMode off - EnableBusSupport off - MaskType "Real Time Synchronizer" - MaskDescription "This block slows down the simulation trying to match the period specified \nas parameter (in sec" - "onds).\nThe bigger the period the more probable \nis that Simulink can remain synched with it.\n" - MaskPromptString "Controller Period (in seconds)" - MaskStyleString "edit" - MaskVariables "period=@1;" - MaskTunableValueString "on" - MaskEnableString "on" - MaskVisibilityString "on" - MaskToolTipString "on" - MaskDisplay "disp('Real Time Synchronizer')" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "0.01" - } - Block { - BlockType S-Function - Name "Simulator Synchronizer" - SID "1658" - Ports [] - Position [335, 14, 465, 51] - ZOrder 24 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - FunctionName "WBToolbox" - Parameters "'SimulatorSynchronizer',period, serverPortName, clientPortName" - SFunctionDeploymentMode off - EnableBusSupport off - MaskType "Simulator Synchronizer" - MaskDescription "This block synchronizes with the external simulation on a simulator \n(only Gazebo is supported " - "at the moment).\n\n" - MaskPromptString "Controller Period (in seconds)|Server Port Name|Client Port Name" - MaskStyleString "edit,edit,edit" - MaskVariables "period=@1;serverPortName=@2;clientPortName=@3;" - MaskTunableValueString "on,on,on" - MaskCallbackString "||" - MaskEnableString "on,on,on" - MaskVisibilityString "on,on,on" - MaskToolTipString "on,on,on" - MaskDisplay "disp('Simulator Synchronizer')" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "0.01|'/clock/rpc'|'/WBT_synchronizer/rpc:o'" - } - Block { - BlockType SubSystem - Name "TruncPinv" - SID "109" - Ports [2, 1] - Position [335, 153, 405, 197] - ZOrder -3 - BackgroundColor "[0.534601, 0.470279, 1.000000]" - RequestExecContextInheritance off - Variant off - MaskPromptString "Tolerance" - MaskStyleString "edit" - MaskVariables "tol=@1;" - MaskTunableValueString "on" - MaskEnableString "on" - MaskVisibilityString "on" - MaskToolTipString "on" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "1e-4" - System { - Name "TruncPinv" - Location [0, 29, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "100" - Block { - BlockType Inport - Name "S" - SID "110" - Position [50, 53, 80, 67] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "tol" - SID "111" - Position [50, 93, 80, 107] - ZOrder -2 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType SubSystem - Name "Truncated PseudoInverse" - SID "112" - Ports [2, 1] - Position [105, 39, 200, 121] - ZOrder -4 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ParametersOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - Variant off - MaskType "Stateflow" - MaskDescription "Embedded MATLAB block" - MaskSelfModifiable on - MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p" - "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);" - MaskIconFrame on - MaskIconOpaque off - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "normalized" - System { - Name "Truncated PseudoInverse" - Location [12, 45, 1279, 3773] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "100" - SIDHighWatermark "1773" - Block { - BlockType Inport - Name "mat" - SID "112::1" - Position [20, 101, 40, 119] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "tol" - SID "112::25" - Position [20, 136, 40, 154] - ZOrder 11 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "112::1609" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 98 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "112::1608" - Tag "Stateflow S-Function WBToolboxLibrary 7" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 97 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport off - Port { - PortNumber 2 - Name "TPinv" - RTWStorageClass "Auto" - DataLoggingNameMode "SignalName" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "112::1610" - Position [460, 241, 480, 259] - ZOrder 99 - } - Block { - BlockType Outport - Name "TPinv" - SID "112::5" - Position [460, 101, 480, 119] - ZOrder -6 - IconDisplay "Port number" - } - Line { - SrcBlock "mat" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - SrcBlock "tol" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "TPinv" - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "TPinv" - DstPort 1 - } - Line { - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType Outport - Name "Tpinv" - SID "113" - Position [225, 73, 255, 87] - ZOrder -5 - IconDisplay "Port number" - } - Line { - SrcBlock "S" - SrcPort 1 - DstBlock "Truncated PseudoInverse" - DstPort 1 - } - Line { - SrcBlock "Truncated PseudoInverse" - SrcPort 1 - DstBlock "Tpinv" - DstPort 1 - } - Line { - SrcBlock "tol" - SrcPort 1 - DstBlock "Truncated PseudoInverse" - DstPort 2 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType S-Function - Name "Yarp Clock" - SID "1773" - Ports [0, 1] - Position [335, 76, 405, 124] - ZOrder 85 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - FunctionName "WBToolbox" - Parameters "'YarpClock'" - SFunctionDeploymentMode off - EnableBusSupport off - MaskType "YARP Clock" - MaskDescription "This block outputs the current YARP Time\nIn a nutshell, this block outputs the equivalent of th" - "e C++ function call\nyarp::os::Time::now()" - MaskSelfModifiable on - MaskDisplay "disp('YARP Time')\n" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - } - Block { - BlockType S-Function - Name "Yarp Read" - SID "1632" - Ports [0, 2] - Position [105, 74, 165, 121] - ZOrder 22 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - FunctionName "WBToolbox" - Parameters "'YarpRead',portName,signalSize,blocking,timestamp,autoconnect,errorOnConnection" - SFunctionDeploymentMode off - EnableBusSupport off - MaskType "YARP Read" - MaskDescription "This block behaves as the command 'yarp read'. \nBy default it has the option 'Autoconnect' acti" - "ve, which means that the user \ncan only specify the name of the port ('Source Port Name') from which readings are " - "desired,\nalong with the size of the expected data (e.g. 3 when reading the torso state).\nWhen the user unchecks '" - "Autoconnect' 'Opened Port Name' field shows up \ncorresponding to the name of the port the block will create \nbut " - "won't connect to anything. \nAt this point the output will be always zero, until the user connects it\nto some othe" - "r port from command line by issuing 'yarp connect [from] [dest]'.\nIn this case an additional output, called 'signa" - "l' will be added which will output\n1 if the port has received at least one data, or 0 otherwise.\n" - MaskPromptString "Source Port Name|Port Size|Blocking Read|Read Timestamp|Autoconnect|Error on missing connection" - MaskStyleString "edit,edit,checkbox,checkbox,checkbox,checkbox" - MaskVariables "portName=@1;signalSize=@2;blocking=@3;timestamp=@4;autoconnect=@5;errorOnConnection=@6;" - MaskTunableValueString "on,on,on,on,on,on" - MaskCallbackString "||||autoconnect_val = get_param(gcb, 'Autoconnect');\nprompt_string = get_param(gcb, 'MaskPro" - "mpts');\nif(strcmp(autoconnect_val, 'on'))\n prompt_string{1} = 'Source Port Name';\n set_param(gcb, 'MaskVis" - "ibilities',{'on';'on';'on';'on';'on';'on'});\nelse\n prompt_string{1} = 'Opened Port Name';\n set_param(gcb, " - "'MaskVisibilities',{'on';'on';'on';'on';'on';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear au" - "toconnect_val prompt_string|" - MaskEnableString "on,on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on,on" - MaskToolTipString "on,on,on,on,on,on" - MaskSelfModifiable on - MaskDisplay "port_label('output', 1, 'signal');\nportNumber = 1;\nif (timestamp)\n portNumber = portNumber + " - "1;\n port_label('output', portNumber, 'timestamp');\nend\n\nif (~autoconnect)\n portNumber = portNumber + 1;\n" - " port_label('output', portNumber, 'status');\nend\n\n" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "'/portname'|1|off|on|on|on" - } - Block { - BlockType S-Function - Name "Yarp Write" - SID "1659" - Ports [1] - Position [230, 76, 290, 124] - ZOrder 27 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - FunctionName "WBToolbox" - Parameters "'YarpWrite',portName,autoconnect,errorOnConnection" - SFunctionDeploymentMode off - EnableBusSupport off - MaskType "YARP Write" - MaskDescription "This block behaves as the command 'yarp write'. \n\nBy default this block opens a port names as " - "the \"Opened Port Name\" parameter\nand stream the input signal to that port.\nIf the option \"Autoconnect\" is spe" - "cified, the first parameter become the\nname of the target port to which the data will be stramed, \ne.g. like \"ya" - "rp write ... /destinationPort\"\n" - MaskPromptString "Opened Port Name|Autoconnect|Error on missing connection" - MaskStyleString "edit,checkbox,checkbox" - MaskVariables "portName=@1;autoconnect=@2;errorOnConnection=@3;" - MaskTunableValueString "on,on,on" - MaskCallbackString "|autoconnect_val = get_param(gcb, 'Autoconnect');\nprompt_string = get_param(gcb, 'MaskPrompt" - "s');\nif(strcmp(autoconnect_val, 'on'))\n prompt_string{1} = 'Destination Port Name';\n set_param(gcb, 'MaskV" - "isibilities',{'on';'on';'on'});\nelse\n prompt_string{1} = 'Opened Port Name';\n set_param(gcb, 'MaskVisibili" - "ties',{'on';'on';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string|" - MaskEnableString "on,on,on" - MaskVisibilityString "on,on,off" - MaskToolTipString "on,on,on" - MaskSelfModifiable on - MaskDisplay "disp('Yarp Write')" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "'/portname'|off|on" - } - Block { - BlockType SubSystem - Name "errors" - SID "714" - Ports [2, 2] - Position [200, 153, 260, 197] - ZOrder -9 - BackgroundColor "[0.300000, 0.580000, 1.000000]" - RequestExecContextInheritance off - Variant off - MaskType "Errors" - MaskDescription "Computes two kinds of errors. The first is just the difference between x\nand y while the second" - " is the ratio (x-y)/y." - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - System { - Name "errors" - Location [0, 29, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "100" - Block { - BlockType Inport - Name "x" - SID "715" - Position [30, 28, 60, 42] - ZOrder -1 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "y" - SID "716" - Position [25, 103, 55, 117] - ZOrder -2 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Sum - Name "Add" - SID "717" - Ports [2, 1] - Position [95, 27, 125, 58] - ZOrder -3 - Inputs "+-" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType Product - Name "Divide" - SID "718" - Ports [2, 1] - Position [165, 37, 195, 68] - ZOrder -4 - Inputs "*/" - InputSameDT off - OutDataTypeStr "Inherit: Inherit via internal rule" - RndMeth "Floor" - SaturateOnIntegerOverflow off - } - Block { - BlockType Outport - Name "x-y" - SID "719" - Position [225, 13, 255, 27] - ZOrder -5 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "(x-y)./y" - SID "720" - Position [220, 48, 250, 62] - ZOrder -6 - Port "2" - IconDisplay "Port number" - } - Line { - SrcBlock "Add" - SrcPort 1 - Points [10, 0] - Branch { - DstBlock "Divide" - DstPort 1 - } - Branch { - Points [0, -25] - DstBlock "x-y" - DstPort 1 - } - } - Line { - SrcBlock "x" - SrcPort 1 - DstBlock "Add" - DstPort 1 - } - Line { - SrcBlock "y" - SrcPort 1 - Points [15, 0] - Branch { - Points [0, -60] - DstBlock "Add" - DstPort 2 - } - Branch { - Points [60, 0; 0, -50] - DstBlock "Divide" - DstPort 2 - } - } - Line { - SrcBlock "Divide" - SrcPort 1 - DstBlock "(x-y)./y" - DstPort 1 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "holder\n" - SID "1296" - Ports [1, 1] - Position [105, 152, 165, 198] - ZOrder 14 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - RequestExecContextInheritance off - Variant off - MaskType "Holder" - MaskDescription "This block holds the first input value during the simulation." - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - System { - Name "holder\n" - Location [12, 45, 1340, 980] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "100" - Block { - BlockType Inport - Name "s" - SID "1297" - Position [145, 43, 175, 57] - ZOrder 13 - IconDisplay "Port number" - } - Block { - BlockType Clock - Name "Clock" - SID "1298" - Position [45, 65, 65, 85] - ZOrder 11 - } - Block { - BlockType Reference - Name "Compare\nTo Constant" - SID "1299" - Ports [1, 1] - Position [90, 60, 120, 90] - ZOrder 10 - LibraryVersion "1.388" - SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Constant" - SourceType "Compare To Constant" - relop "==" - const "0" - OutDataTypeStr "boolean" - ZeroCross on - } - Block { - BlockType SubSystem - Name "MATLAB Function" - SID "1300" - Ports [2, 1] - Position [235, 37, 305, 88] - ZOrder 15 - LibraryVersion "1.32" - ErrorFcn "Stateflow.Translate.translate" - PermitHierarchicalResolution "ParametersOnly" - TreatAsAtomicUnit on - RequestExecContextInheritance off - SFBlockType "MATLAB Function" - Variant off - MaskType "Stateflow" - MaskDescription "Embedded MATLAB block" - MaskSelfModifiable on - MaskDisplay "bgColor = Simulink.Root.ColorString2Rgb(get_param(gcbh, 'BackgroundColor')); image(imread('p" - "rivate/eml_membrane_16.png','png','BackgroundColor',bgColor(1:3)),'center'); disp([10 10 'fcn']);" - MaskIconFrame on - MaskIconOpaque off - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "normalized" - System { - Name "MATLAB Function" - Location [12, 45, 1135, 3068] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "100" - SIDHighWatermark "1773" - Block { - BlockType Inport - Name "s" - SID "1300::24" - Position [20, 101, 40, 119] - ZOrder 10 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "unused" - SID "1300::26" - Position [20, 136, 40, 154] - ZOrder 12 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Demux - Name " Demux " - SID "1300::1609" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 86 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "1300::1608" - Tag "Stateflow S-Function WBToolboxLibrary 1" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 85 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport off - Port { - PortNumber 2 - Name "s0" - RTWStorageClass "Auto" - DataLoggingNameMode "SignalName" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "1300::1610" - Position [460, 241, 480, 259] - ZOrder 87 - } - Block { - BlockType Outport - Name "s0" - SID "1300::25" - Position [460, 101, 480, 119] - ZOrder 11 - IconDisplay "Port number" - } - Line { - SrcBlock "s" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - SrcBlock "unused" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "s0" - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "s0" - DstPort 1 - } - Line { - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - SrcBlock " SFunction " - SrcPort 1 - DstBlock " Demux " - DstPort 1 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType Outport - Name "s(0)" - SID "1301" - Position [330, 58, 360, 72] - ZOrder 14 - IconDisplay "Port number" - } - Line { - SrcBlock "Clock" - SrcPort 1 - DstBlock "Compare\nTo Constant" - DstPort 1 - } - Line { - SrcBlock "MATLAB Function" - SrcPort 1 - DstBlock "s(0)" - DstPort 1 - } - Line { - SrcBlock "s" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 1 - } - Line { - SrcBlock "Compare\nTo Constant" - SrcPort 1 - DstBlock "MATLAB Function" - DstPort 2 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "wholeBodyActuators" - SID "224" - Ports [] - Position [250, 16, 348, 113] - ZOrder -17 - ForegroundColor "white" - DropShadow on - RequestExecContextInheritance off - Variant off - MaskDisplay "image(imread('wholeBodyActuators.png'),'center')" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - System { - Name "wholeBodyActuators" - Location [425, 373, 1725, 1134] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "667" - Block { - BlockType SubSystem - Name "Set LowLevel PIDs" - SID "1752" - Ports [] - Position [745, 230, 820, 280] - ZOrder 41 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend" - RequestExecContextInheritance off - Variant off - MaskType "Set Low Level PIDs" - MaskDescription "This block sets the low level PID values for the robot actuators.\n\nIf only one gain file is sp" - "ecified, those gains are loaded at startup.\nOtherwise an additional port to this block is added which accept an in" - "teger\nrepresenting the index in the list of gains to be loaded (also at runtime)\n\nAssuming DoFs is the number of" - " degrees of freedom of the robot,\nParameters:\n- PID Parameter: String specifying either a file containing the PID" - "s for the\n low level control, or a list of files (in yarp Bottle format).\n\n- Robot Port Name: Na" - "me of the ports opened by the robot. (e.g. icub).\n Set an empty string ('') to use the name spec" - "ified in the \n Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports " - "opened by the underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of th" - "e Whole Body Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - MaskPromptString "PID parameter|PID Type|Robot Port Name|Model Name|WBI filename|WBI list name" - MaskStyleString "edit,popup(Position|Torque),edit,edit,edit,edit" - MaskVariables "pidParameters=@1;pidType=&2;robotName=@3;localName=@4;wbiFile=@5;wbiList=@6;" - MaskTunableValueString "off,on,off,off,off,off" - MaskCallbackString "|% Get the mask parameter values. This is a cell\n% array of strings.\nmaskStr = get_param(" - "gcb,'pidType');\nif strcmp(maskStr, 'Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0" - "]');\nelseif strcmp(maskStr, 'Position Direct')\n set_param(gcb,'BackgroundColor', '[0.3922, 0.7882, 0.3451, 1.0" - "]');\nelseif strcmp(maskStr, 'Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\ne" - "lseif strcmp(maskStr, 'Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\nend\nclear" - " maskStr||||" - MaskEnableString "on,on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on,on" - MaskToolTipString "on,on,on,on,on,on" - MaskDisplay "pidType = get_param(gcb,'pidType');\ndescription = sprintf('Set Low Level\\n%s PIDs', pidType);\ndi" - "sp(description);\n" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "''|Torque|WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - MaskTabNameString "Block parameters,Block parameters,WBI parameters,WBI parameters,WBI parameters,WBI parameters" - System { - Name "Set LowLevel PIDs" - Location [0, 29, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "1000" - Block { - BlockType S-Function - Name "S-Function" - SID "1753" - Ports [] - Position [125, 39, 185, 71] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'SetLowLevelPID',robotName,localName,wbiFile,wbiList,pidParameters,pidType" - SFunctionDeploymentMode off - EnableBusSupport off - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "Set References" - SID "1767" - Ports [1] - Position [645, 230, 720, 280] - ZOrder 43 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend" - RequestExecContextInheritance off - Variant off - MaskType "Set References" - MaskDescription "This block sets the references for the robot actuators.\nThe type of control mode is specified a" - "s a parameter in the block.\n\nAssuming DoFs is the number of degrees of freedom of the robot,\nInput:\n- Reference" - "s: Vector of size DoFs, representing the references to be sent \n to the robot controlled joints.\n\n" - "Parameters:\n- Control Mode: The control mode. Choose one of the supplied mode.\n\n- Robot Port Name: Name of the p" - "orts opened by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in th" - "e \n Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by t" - "he underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Bod" - "y Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - MaskPromptString "Control Mode|Control Type|Controlled Joint List|Robot Port Name|Model Name|WBI filename|WBI lis" - "t name" - MaskStyleString "popup(Position|Position Direct|Velocity|Torque|Open Loop),popup(Full|Sublist),edit,edit,edit,edi" - "t,edit" - MaskVariables "controlType=&1;fullOrSubControl=@2;controlledJoints=@3;robotName=@4;localName=@5;wbiFile=@6;wbiLi" - "st=@7;" - MaskTunableValueString "on,on,on,off,off,off,off" - MaskCallbackString "% Get the mask parameter values. This is a cell\n% array of strings.\nmaskStr = get_param(g" - "cb,'controlType');\nif strcmp(maskStr, 'Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, " - "1.0]');\nelseif strcmp(maskStr, 'Position Direct')\n set_param(gcb,'BackgroundColor', '[0.3922, 0.7882, 0.3451, " - "1.0]');\nelseif strcmp(maskStr, 'Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');" - "\nelseif strcmp(maskStr, 'Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\nelseif " - "strcmp(maskStr, 'Open Loop')\n set_param(gcb,'BackgroundColor', '[0.98, 0.98, 0.98, 1.0]');\nend\nclear maskStr" - "|subOrFull = get_param(gcb, 'fullOrSubControl');\nif(strcmp(subOrFull, 'Sublist'))\n set_param(gcb, 'MaskVisibil" - "ities',{'on';'on';'on';'on';'on';'on';'on'});\nelse\n set_param(gcb, 'MaskVisibilities',{'on';'on';'off';'on';'o" - "n';'on';'on'});\nend\nclear subOrFull |||||" - MaskEnableString "on,on,on,on,on,on,on" - MaskVisibilityString "on,on,off,on,on,on,on" - MaskToolTipString "on,on,on,on,on,on,on" - MaskDisplay "disp(get_param(gcb,'controlType'))\n%port_label('input',1,'References')" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "Position|Full|''|WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - MaskTabNameString "Block parameters,Block parameters,Block parameters,WBI parameters,WBI parameters,WBI parameter" - "s,WBI parameters" - System { - Name "Set References" - Location [535, 41, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "521" - Block { - BlockType Inport - Name "References" - SID "1768" - Position [55, 48, 85, 62] - ZOrder 24 - IconDisplay "Port number" - } - Block { - BlockType S-Function - Name "S-Function" - SID "1769" - Ports [1] - Position [125, 39, 185, 71] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'SetReferences',robotName,localName,wbiFile,wbiList,controlType,fullOrSubControl,controlledJo" - "ints" - SFunctionDeploymentMode off - EnableBusSupport off - } - Line { - SrcBlock "References" - SrcPort 1 - DstBlock "S-Function" - DstPort 1 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "wholeBodyModel" - SID "209" - Ports [] - Position [133, 16, 231, 113] - ZOrder -3 - ForegroundColor "white" - DropShadow on - RequestExecContextInheritance off - Variant off - MaskDisplay "image(imread('wholeBodyModel.png'),'center')" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - System { - Name "wholeBodyModel" - Location [425, 373, 1725, 1134] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "309" - Block { - BlockType SubSystem - Name "Dynamics" - SID "369" - Ports [] - Position [364, 21, 471, 128] - ZOrder -1 - DropShadow on - RequestExecContextInheritance off - Variant off - MaskDisplay "image(imread('Dynamics.png'))" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - System { - Name "Dynamics" - Location [425, 373, 1725, 1134] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "219" - Block { - BlockType SubSystem - Name "Centroidal Momentum" - SID "1694" - Ports [4, 1] - Position [495, 101, 680, 164] - ZOrder 72 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" - RequestExecContextInheritance off - Variant off - MaskType "Centroidal Momentum" - MaskDescription "This block computed the 6 element centroidal momentum, as defined in:\n\"Centroidal dyna" - "mics of a humanoid robot\" - DE Orin, A Goswami, SH Lee - \nAutonomous Robots 35 (2-3), 161-176\n\nAssuming DoF" - "s is the number of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenou" - "s transformation between\n the base frame and the world frame.\n- Joint configuration: Vector of si" - "ze DoFs, representing the configuration \n of the joints.\n- Base velocity: Vector of siz" - "e 6 representing the linear and angular velocity\n of the base frame.\n- Joints velocity: Vecto" - "r of size DoFs, representing the velocity \n of the joints.\n\nOutput:\n- Centroidal Momentum" - ": 6-element vector containg the centroidal momentum \n (3 value for linear momentum and 3 for an" - "gular momentum)\n\nParameters:\n- Robot Port Name: Name of the ports opened by the robot. (e.g. icub).\n " - " Set an empty string ('') to use the name specified in the \n Whole Body Interface" - " configuration file.\n- Model Name: Prefix name of the ports opened by the underlying Whole Body Interface.\n- " - "WBI filename: name of the file containing the configuration of the Whole Body Interface\n- WBI List Name: name " - "of the list of joints used to configure the Whole Body Interface\n" - MaskPromptString "Robot Port Name|Model Name|WBI filename|WBI list name" - MaskStyleString "edit,edit,edit,edit" - MaskVariables "robotName=@1;localName=@2;wbiFile=@3;wbiList=@4;" - MaskTunableValueString "off,off,off,off" - MaskCallbackString "|||" - MaskEnableString "on,on,on,on" - MaskVisibilityString "on,on,on,on" - MaskToolTipString "on,on,on,on" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - System { - Name "Centroidal Momentum" - Location [0, 23, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "248" - Block { - BlockType Inport - Name "Base Pose" - SID "1695" - Position [20, 18, 50, 32] - ZOrder 22 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joint configuration" - SID "1696" - Position [20, 53, 50, 67] - ZOrder 24 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Base velocity" - SID "1697" - Position [20, 88, 50, 102] - ZOrder 26 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joints velocity" - SID "1698" - Position [20, 123, 50, 137] - ZOrder 27 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType S-Function - Name "S-Function" - SID "1699" - Ports [4, 1] - Position [180, 11, 255, 144] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'CentroidalMomentum',robotName,localName,wbiFile,wbiList" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Centroidal Momentum" - SID "1700" - Position [315, 73, 345, 87] - ZOrder 25 - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Centroidal Momentum" - DstPort 1 - } - Line { - SrcBlock "Base Pose" - SrcPort 1 - DstBlock "S-Function" - DstPort 1 - } - Line { - SrcBlock "Joint configuration" - SrcPort 1 - DstBlock "S-Function" - DstPort 2 - } - Line { - SrcBlock "Base velocity" - SrcPort 1 - DstBlock "S-Function" - DstPort 3 - } - Line { - SrcBlock "Joints velocity" - SrcPort 1 - DstBlock "S-Function" - DstPort 4 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "Get Bias Forces" - SID "1724" - Ports [4, 1] - Position [400, 202, 540, 303] - ZOrder 73 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" - RequestExecContextInheritance off - Variant off - MaskType "Get Generalized Bias Forces" - MaskDescription "This block retrieves the generalied bias forces of the robot.\n\nParameter:\n- Explicit " - "Gravity. If True, an additional port (explicit Gravity) is added.\n Otherwise the vector [0;" - " 0; -9.81] is used.\nAssuming DoFs is the number of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 " - "matrix representing the homogenous transformation between\n the base frame and the world frame.\n- J" - "oint configuration: Vector of size DoFs, representing the configuration \n of the joints.\n-" - " Base velocity: Vector of size 6 representing the linear and angular velocity\n of the base fra" - "me.\n- Joints velocity: Vector of size DoFs, representing the velocity of the joints.\n\nOutput:\n- Bias Forces" - ": a Dofs + 6 vector representing the generalized bias forces\n of the robot\n\nParameters:\n- Robot " - "Port Name: Name of the ports opened by the robot. (e.g. icub).\n Set an empty string ('') to " - "use the name specified in the \n Whole Body Interface configuration file." - "\n- Model Name: Prefix name of the ports opened by the underlying Whole Body Interface.\n- WBI filename: name o" - "f the file containing the configuration of the Whole Body Interface\n- WBI List Name: name of the list of joint" - "s used to configure the Whole Body Interface\n" - MaskPromptString "Robot Port Name|Model Name|WBI filename|WBI list name|Explicit Gravity" - MaskStyleString "edit,edit,edit,edit,checkbox" - MaskVariables "robotName=@1;localName=@2;wbiFile=@3;wbiList=@4;explicitGravity=@5;" - MaskTunableValueString "off,off,off,off,on" - MaskCallbackString "||||" - MaskEnableString "on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on" - MaskToolTipString "on,on,on,on,on" - MaskInitialization "gravityInput = find_system(gcb, ...\n'LookUnderMasks', 'on', ...\n'FollowLinks', 'on'," - " ...\n'Name', 'Gravity');\n\nif explicitGravity\n if isempty(gravityInput)\n Simulink.Block.eval([gcb" - ",'/S-Function']);\n add_block('built-in/Inport',[gcb,'/Gravity']);\n add_line(gcb','Gravity/1','S" - "-Function/7', 'autorouting','on');\n end\nelse\n if ~isempty(gravityInput)\n delete_line(gcb','Gra" - "vity/1','S-Function/7');\n delete_block(gravityInput);\n Simulink.Block.eval([gcb,'/S-Function'])" - ";\n end\nend\n" - MaskSelfModifiable on - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList|off" - System { - Name "Get Bias Forces" - Location [354, 32, 1634, 747] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "255" - SIDHighWatermark "1748" - Block { - BlockType Inport - Name "Base Pose" - SID "1724:1725" - Position [35, 38, 65, 52] - ZOrder 22 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joint configuration" - SID "1724:1726" - Position [35, 68, 65, 82] - ZOrder 24 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Base velocity" - SID "1724:1727" - Position [35, 98, 65, 112] - ZOrder 26 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joints velocity" - SID "1724:1728" - Position [35, 128, 65, 142] - ZOrder 27 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "1724:1742" - Position [20, 157, 75, 173] - ZOrder 30 - ShowName off - Value "zeros(6,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "1724:1743" - Position [135, 180, 165, 210] - ZOrder 31 - ShowName off - Gain "0" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType S-Function - Name "S-Function" - SID "1724:1731" - Ports [6, 1] - Position [205, 24, 265, 216] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'InverseDynamics',robotName,localName,wbiFile,wbiList,explicitGravity" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Bias Forces" - SID "1724:1732" - Position [325, 113, 355, 127] - ZOrder 25 - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Bias Forces" - DstPort 1 - } - Line { - SrcBlock "Base Pose" - SrcPort 1 - DstBlock "S-Function" - DstPort 1 - } - Line { - SrcBlock "Joint configuration" - SrcPort 1 - DstBlock "S-Function" - DstPort 2 - } - Line { - SrcBlock "Base velocity" - SrcPort 1 - DstBlock "S-Function" - DstPort 3 - } - Line { - SrcBlock "Joints velocity" - SrcPort 1 - Points [44, 0] - Branch { - Points [0, 60] - DstBlock "Gain" - DstPort 1 - } - Branch { - DstBlock "S-Function" - DstPort 4 - } - } - Line { - SrcBlock "Constant" - SrcPort 1 - DstBlock "S-Function" - DstPort 5 - } - Line { - SrcBlock "Gain" - SrcPort 1 - DstBlock "S-Function" - DstPort 6 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "Get Gravity Forces" - SID "1733" - Ports [2, 1] - Position [605, 200, 745, 300] - ZOrder 74 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" - RequestExecContextInheritance off - Variant off - MaskType "Gravity bias" - MaskDescription "This block compute the generalized bias forces due to the gravity\n\nParameter:\n- Expli" - "cit Gravity. If True, an additional port (explicit Gravity) is added.\n Otherwise the vector" - " [0; 0; -9.81] is used.\n\nAssuming DoFs is the number of degrees of freedom of the robot,\nInput:\n- Base pose" - ": 4x4 matrix representing the homogenous transformation between\n the base frame and the world fram" - "e.\n- Joint configuration: Vector of size DoFs, representing the configuration \n of the " - "joints.\n\nOutput:\n- Gravity: a Dofs + 6 vector representing the torques due to the gravity.\n\nParameters:\n-" - " Robot Port Name: Name of the ports opened by the robot. (e.g. icub).\n Set an empty string (" - "'') to use the name specified in the \n Whole Body Interface configuration file.\n- Model Nam" - "e: Prefix name of the ports opened by the underlying Whole Body Interface.\n- WBI filename: name of the file co" - "ntaining the configuration of the Whole Body Interface\n- WBI List Name: name of the list of joints used to con" - "figure the Whole Body Interface\n" - MaskPromptString "Robot Port Name|Model Name|WBI filename|WBI list name|Explicit Gravity" - MaskStyleString "edit,edit,edit,edit,checkbox" - MaskVariables "robotName=@1;localName=@2;wbiFile=@3;wbiList=@4;explicitGravity=@5;" - MaskTunableValueString "off,off,off,off,on" - MaskCallbackString "||||" - MaskEnableString "on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on" - MaskToolTipString "on,on,on,on,on" - MaskInitialization "gravityInput = find_system(gcb, ...\n'LookUnderMasks', 'on', ...\n'FollowLinks', 'on'," - " ...\n'Name', 'Gravity');\n\nif explicitGravity\n if isempty(gravityInput)\n Simulink.Block.eval([gcb" - ",'/S-Function']);\n add_block('built-in/Inport',[gcb,'/Gravity']);\n add_line(gcb','Gravity/1','S" - "-Function/7', 'autorouting','on');\n end\nelse\n if ~isempty(gravityInput)\n delete_line(gcb','Gra" - "vity/1','S-Function/7');\n delete_block(gravityInput);\n Simulink.Block.eval([gcb,'/S-Function'])" - ";\n end\nend\n" - MaskSelfModifiable on - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList|off" - System { - Name "Get Gravity Forces" - Location [354, 32, 1634, 747] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "255" - SIDHighWatermark "1748" - Block { - BlockType Inport - Name "Base Pose" - SID "1733:1734" - Position [10, 38, 40, 52] - ZOrder 22 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joint configuration" - SID "1733:1735" - Position [10, 68, 40, 82] - ZOrder 24 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Constant - Name "Constant" - SID "1733:1744" - Position [0, 97, 55, 113] - ZOrder 32 - ShowName off - Value "zeros(6,1)" - } - Block { - BlockType Gain - Name "Gain" - SID "1733:1745" - Position [100, 120, 130, 150] - ZOrder 33 - ShowName off - Gain "0" - ParamDataTypeStr "Inherit: Inherit via internal rule" - OutDataTypeStr "Inherit: Inherit via internal rule" - SaturateOnIntegerOverflow off - } - Block { - BlockType S-Function - Name "S-Function" - SID "1733:1740" - Ports [6, 1] - Position [180, 24, 240, 216] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'InverseDynamics',robotName,localName,wbiFile,wbiList,explicitGravity" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Gravity Torques" - SID "1733:1741" - Position [300, 113, 330, 127] - ZOrder 25 - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Gravity Torques" - DstPort 1 - } - Line { - SrcBlock "Base Pose" - SrcPort 1 - DstBlock "S-Function" - DstPort 1 - } - Line { - SrcBlock "Joint configuration" - SrcPort 1 - Points [41, 0] - Branch { - Points [0, 60] - DstBlock "Gain" - DstPort 1 - } - Branch { - DstBlock "S-Function" - DstPort 2 - } - } - Line { - SrcBlock "Constant" - SrcPort 1 - Points [4, 0] - Branch { - Points [0, 60] - DstBlock "S-Function" - DstPort 5 - } - Branch { - DstBlock "S-Function" - DstPort 3 - } - } - Line { - SrcBlock "Gain" - SrcPort 1 - Points [13, 0] - Branch { - Points [0, 60] - DstBlock "S-Function" - DstPort 6 - } - Branch { - DstBlock "S-Function" - DstPort 4 - } - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType S-Function - Name "Inverse Dynamics" - SID "1748" - Ports [6, 1] - Position [190, 199, 355, 331] - ZOrder 75 - FunctionName "WBToolbox" - Parameters "'InverseDynamics',robotName,localName,wbiFile,wbiList,explicitGravity" - SFunctionDeploymentMode off - EnableBusSupport off - MaskType "Inverse Dynamics" - MaskDescription "This block compute the inverse dynamics of the robot.\n\nParameter:\n- Explicit Gravity." - " If True, an additional port (explicit Gravity) is added.\n Otherwise the vector [0; 0; -9.8" - "1] is used.\n\nAssuming DoFs is the number of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix" - " representing the homogenous transformation between\n the base frame and the world frame.\n- Joint " - "configuration: Vector of size DoFs, representing the configuration \n of the joints.\n- B" - "ase velocity: Vector of size 6 representing the linear and angular velocity\n of the base frame" - ".\n- Joints velocity: Vector of size DoFs, representing the velocity \n of the joints.\n- Bas" - "e acceleration: Vector of size 6 representing the linear and angular acceleration\n of the base" - " frame.\n- Joints acceleration: Vector of size DoFs, representing the acceleration \n of the " - "joints.\n- Gravity: Vector of size 3, denoting the acceleration vector w.r.t. the world frame.\n\n\nOutput:\n- " - "Torques: a Dofs + 6 vector representing the corresponding torques required\n to achive the desired ac" - "celerations given the robot state\n\nParameters:\n- Robot Port Name: Name of the ports opened by the robot. (e." - "g. icub).\n Set an empty string ('') to use the name specified in the \n Wh" - "ole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by the underlying Whole B" - "ody Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Interface\n- WB" - "I List Name: name of the list of joints used to configure the Whole Body Interface\n" - MaskPromptString "Robot Port Name|Model Name|WBI filename|WBI list name|Explicit Gravity" - MaskStyleString "edit,edit,edit,edit,checkbox" - MaskVariables "robotName=@1;localName=@2;wbiFile=@3;wbiList=@4;explicitGravity=@5;" - MaskTunableValueString "on,on,on,on,on" - MaskCallbackString "||||" - MaskEnableString "on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on" - MaskToolTipString "on,on,on,on,on" - MaskDisplay "port_label('output', 1, 'Torques')\n\nport_label('input', 1, 'Base pose')\nport_label('input" - "', 2, 'Joints configuration')\nport_label('input', 3, 'Base velocity')\nport_label('input', 4, 'Joints velocity" - "')\nport_label('input', 5, 'Base acceleration')\nport_label('input', 6, 'Joints acceleration')\n\n\ngravity = g" - "et_param(gcb, 'explicitGravity');\nif(strcmp(gravity, 'on'))\n port_label('input', 7, 'Gravity')\nend\n" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList|off" - } - Block { - BlockType SubSystem - Name "Mass Matrix" - SID "1633" - Ports [2, 1] - Position [250, 104, 390, 171] - ZOrder 32 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" - RequestExecContextInheritance off - Variant off - MaskType "Mass Matrix" - MaskDescription "This block retrieves the robot mass matrix.\n\nAssuming DoFs is the number of degrees of" - " freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between\n " - " the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the con" - "figuration \n of the joints.\n\nOutput:\n- Mass Matrix: a (Dofs + 6) x (Dofs + 6) matrix " - "representing the mass matrix \n of the robot\n\nParameters:\n- Robot Port Name: Name of the ports" - " opened by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in th" - "e \n Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened " - "by the underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the W" - "hole Body Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - MaskPromptString "Robot Port Name|Model Name|WBI filename|WBI list name" - MaskStyleString "edit,edit,edit,edit" - MaskVariables "robotName=@1;localName=@2;wbiFile=@3;wbiList=@4;" - MaskTunableValueString "off,off,off,off" - MaskCallbackString "|||" - MaskEnableString "on,on,on,on" - MaskVisibilityString "on,on,on,on" - MaskToolTipString "on,on,on,on" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - System { - Name "Mass Matrix" - Location [589, 68, 1869, 783] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "248" - Block { - BlockType Inport - Name "Base Pose" - SID "1634" - Position [20, 23, 50, 37] - ZOrder 22 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joint configuration" - SID "1635" - Position [20, 63, 50, 77] - ZOrder 24 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType S-Function - Name "S-Function" - SID "1636" - Ports [2, 1] - Position [125, 37, 185, 68] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'MassMatrix',robotName,localName,wbiFile,wbiList" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Mass Matrix" - SID "1637" - Position [245, 48, 275, 62] - ZOrder 25 - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Mass Matrix" - DstPort 1 - } - Line { - SrcBlock "Base Pose" - SrcPort 1 - Points [36, 0; 0, 15] - DstBlock "S-Function" - DstPort 1 - } - Line { - SrcBlock "Joint configuration" - SrcPort 1 - Points [33, 0; 0, -10] - DstBlock "S-Function" - DstPort 2 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "Jacobians" - SID "202" - Ports [] - Position [217, 20, 324, 127] - ZOrder -3 - DropShadow on - RequestExecContextInheritance off - Variant off - MaskDisplay "image(imread('jacobian.png'))" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - System { - Name "Jacobians" - Location [425, 373, 1725, 1134] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "322" - Block { - BlockType SubSystem - Name "DotJ Nu" - SID "1683" - Ports [4, 1] - Position [590, 170, 755, 265] - ZOrder 67 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" - RequestExecContextInheritance off - Variant off - MaskType "DotJ Nu" - MaskDescription "This block computes the product between the time derivative of the\n Jacobian of" - " the specified frame and the state (base and joints)\n velocity vector.\n\nAssuming DoFs is the number o" - "f degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation " - "between\n the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, repr" - "esenting the configuration \n of the joints.\n- Base velocity: Vector of size 6 represent" - "ing the linear and angular velocity\n of the base frame.\n- Joints velocity: Vector of size DoF" - "s, representing the velocity \n of the joints.\n\nOutput:\n- dotJ nu: a 6-d vector representi" - "ng the product between the time derivative of the\n Jacobian of the specified frame and the state veloc" - "ity vector\n\nParameters:\n- Frame name: the name of the frame. It should be specified in the URDF model.\n\n- " - "Robot Port Name: Name of the ports opened by the robot. (e.g. icub).\n Set an empty string ('" - "') to use the name specified in the \n Whole Body Interface configuration file.\n- Model Name" - ": Prefix name of the ports opened by the underlying Whole Body Interface.\n- WBI filename: name of the file con" - "taining the configuration of the Whole Body Interface\n- WBI List Name: name of the list of joints used to conf" - "igure the Whole Body Interface\n" - MaskPromptString "Frame name|Robot Port Name|Model Name|WBI filename|WBI list name" - MaskStyleString "edit,edit,edit,edit,edit" - MaskVariables "frameName=@1;robotName=@2;localName=@3;wbiFile=@4;wbiList=@5;" - MaskTunableValueString "on,off,off,off,off" - MaskCallbackString "||||" - MaskEnableString "on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on" - MaskToolTipString "on,on,on,on,on" - MaskDisplay "escapedFrameName = strrep(frameName, '_', '\\_');\nport_label('output', 1, strcat('dot J_{'," - "escapedFrameName,'} \\nu'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\np" - "ort_label('input', 2, 'Joint configuration')\nport_label('input', 3, 'Base velocity')\nport_label('input', 4, '" - "Joint velocity')\n\nclear escapedFrameName;" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "'frame'|WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - MaskTabNameString "Block parameters,WBI parameters,WBI parameters,WBI parameters,WBI parameters" - System { - Name "DotJ Nu" - Location [0, 29, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "391" - Block { - BlockType Inport - Name "Base Pose" - SID "1684" - Position [20, 13, 50, 27] - ZOrder 22 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joint configuration" - SID "1685" - Position [20, 43, 50, 57] - ZOrder 24 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Base velocity" - SID "1686" - Position [20, 73, 50, 87] - ZOrder 26 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joints velocity" - SID "1687" - Position [20, 103, 50, 117] - ZOrder 27 - Port "4" - IconDisplay "Port number" - } - Block { - BlockType S-Function - Name "S-Function" - SID "1688" - Ports [4, 1] - Position [125, 4, 190, 126] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'DotJNu',robotName,localName,wbiFile,wbiList,frameName" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "dotJ dotnu" - SID "1689" - Position [245, 43, 275, 57] - ZOrder 25 - IconDisplay "Port number" - } - Line { - SrcBlock "Joint configuration" - SrcPort 1 - DstBlock "S-Function" - DstPort 2 - } - Line { - SrcBlock "Base Pose" - SrcPort 1 - DstBlock "S-Function" - DstPort 1 - } - Line { - SrcBlock "S-Function" - SrcPort 1 - Points [20, 0; 0, -15] - DstBlock "dotJ dotnu" - DstPort 1 - } - Line { - SrcBlock "Base velocity" - SrcPort 1 - DstBlock "S-Function" - DstPort 3 - } - Line { - SrcBlock "Joints velocity" - SrcPort 1 - DstBlock "S-Function" - DstPort 4 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "Jacobian" - SID "1663" - Ports [2, 1] - Position [380, 190, 540, 245] - ZOrder 39 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" - RequestExecContextInheritance off - Variant off - MaskType "Jacobian" - MaskDescription "This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number" - " of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformatio" - "n between\n the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, re" - "presenting the configuration \n of the joints.\n\nOutput:\n- Jacobian: a 6x6+dofs matrix " - "representing the Jacobian of\n the specified frame written in the world frame.\n\nParameters:\n- Fr" - "ame name: the name of the frame. It should be specified in the URDF model.\n\n- Robot Port Name: Name of the po" - "rts opened by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in" - " the \n Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports open" - "ed by the underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of th" - "e Whole Body Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - MaskPromptString "Frame name|Robot Port Name|Model Name|WBI filename|WBI list name" - MaskStyleString "edit,edit,edit,edit,edit" - MaskVariables "frameName=@1;robotName=@2;localName=@3;wbiFile=@4;wbiList=@5;" - MaskTunableValueString "on,off,off,off,off" - MaskCallbackString "||||" - MaskEnableString "on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on" - MaskToolTipString "on,on,on,on,on" - MaskDisplay "escapedFrameName = strrep(frameName, '_', '\\_');\nport_label('output', 1, strcat('{}^{world" - "} J_{',escapedFrameName,'}'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\n" - "port_label('input', 2, 'Joint configuration')\n\nclear escapedFrameName;" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "'frame'|WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - MaskTabNameString "Block parameters,WBI parameters,WBI parameters,WBI parameters,WBI parameters" - System { - Name "Jacobian" - Location [208, 105, 1008, 627] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "237" - Block { - BlockType Inport - Name "Base Pose" - SID "1664" - Position [20, 23, 50, 37] - ZOrder 22 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joint configuration" - SID "1665" - Position [20, 63, 50, 77] - ZOrder 24 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType S-Function - Name "S-Function" - SID "1666" - Ports [2, 1] - Position [125, 37, 185, 68] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'Jacobian',robotName,localName,wbiFile,wbiList,frameName" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Forward Kinematics" - SID "1667" - Position [245, 48, 275, 62] - ZOrder 25 - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Forward Kinematics" - DstPort 1 - } - Line { - SrcBlock "Base Pose" - SrcPort 1 - Points [36, 0; 0, 15] - DstBlock "S-Function" - DstPort 1 - } - Line { - SrcBlock "Joint configuration" - SrcPort 1 - Points [33, 0; 0, -10] - DstBlock "S-Function" - DstPort 2 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "Kinematics" - SID "176" - Ports [] - Position [70, 20, 177, 127] - ZOrder -17 - DropShadow on - RequestExecContextInheritance off - Variant off - MaskDisplay "image(imread('forwardKinematics.png'))" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - System { - Name "Kinematics" - Location [425, 373, 1725, 1134] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "326" - Block { - BlockType SubSystem - Name "Forward Kinematics" - SID "1647" - Ports [2, 1] - Position [350, 103, 500, 167] - ZOrder 34 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" - RequestExecContextInheritance off - Variant off - MaskType "Forward Kinematics" - MaskDescription "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is " - "the number of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous tra" - "nsformation between\n the the base frame and the world frame.\n- Joint configuration: Vector of siz" - "e DoFs, representing the configuration \n of the joints.\n\nOutput:\n- Forward Kinematics" - ": a 4x4 matrix representing the homogenous transformation between\n the specified frame and the wor" - "ld frame.\n\nParameters:\n- Frame name: the name of the frame. It should be specified in the URDF model.\n\n- R" - "obot Port Name: Name of the ports opened by the robot. (e.g. icub).\n Set an empty string (''" - ") to use the name specified in the \n Whole Body Interface configuration file.\n- Model Name:" - " Prefix name of the ports opened by the underlying Whole Body Interface.\n- WBI filename: name of the file cont" - "aining the configuration of the Whole Body Interface\n- WBI List Name: name of the list of joints used to confi" - "gure the Whole Body Interface\n" - MaskPromptString "Frame name|Robot Port Name|Model Name|WBI filename|WBI list name" - MaskStyleString "edit,edit,edit,edit,edit" - MaskVariables "frameName=@1;robotName=@2;localName=@3;wbiFile=@4;wbiList=@5;" - MaskTunableValueString "on,off,off,off,off" - MaskCallbackString "||||" - MaskEnableString "on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on" - MaskToolTipString "on,on,on,on,on" - MaskDisplay "escapedFrameName = strrep(frameName, '_', '\\_');\nport_label('output', 1, strcat('{}^{world" - "} H_{',escapedFrameName,'}'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\n" - "port_label('input', 2, 'Joint configuration')\n\nclear escapedFrameName;" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "'frame'|WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - MaskTabNameString "Block parameters,WBI parameters,WBI parameters,WBI parameters,WBI parameters" - System { - Name "Forward Kinematics" - Location [0, 29, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "391" - Block { - BlockType Inport - Name "Base Pose" - SID "1648" - Position [20, 23, 50, 37] - ZOrder 22 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Joint configuration" - SID "1649" - Position [20, 63, 50, 77] - ZOrder 24 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType S-Function - Name "S-Function" - SID "1650" - Ports [2, 1] - Position [125, 37, 185, 68] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'ForwardKinematics',robotName,localName,wbiFile,wbiList,frameName" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Forward Kinematics" - SID "1651" - Position [245, 48, 275, 62] - ZOrder 25 - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Forward Kinematics" - DstPort 1 - } - Line { - SrcBlock "Base Pose" - SrcPort 1 - Points [36, 0; 0, 15] - DstBlock "S-Function" - DstPort 1 - } - Line { - SrcBlock "Joint configuration" - SrcPort 1 - Points [33, 0; 0, -10] - DstBlock "S-Function" - DstPort 2 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "Inverse Kinematics" - SID "1754" - Ports [3, 2] - Position [350, 198, 525, 262] - ZOrder 35 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" - RequestExecContextInheritance off - Variant off - MaskType "Forward Kinematics" - MaskDescription "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is " - "the number of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous tra" - "nsformation between\n the the base frame and the world frame.\n- Joint configuration: Vector of siz" - "e DoFs, representing the configuration \n of the joints.\n\nOutput:\n- Forward Kinematics" - ": a 4x4 matrix representing the homogenous transformation between\n the specified frame and the wor" - "ld frame.\n\nParameters:\n- Frame name: the name of the frame. It should be specified in the URDF model.\n\n- R" - "obot Port Name: Name of the ports opened by the robot. (e.g. icub).\n Set an empty string (''" - ") to use the name specified in the \n Whole Body Interface configuration file.\n- Model Name:" - " Prefix name of the ports opened by the underlying Whole Body Interface.\n- WBI filename: name of the file cont" - "aining the configuration of the Whole Body Interface\n- WBI List Name: name of the list of joints used to confi" - "gure the Whole Body Interface\n" - MaskPromptString "Base Frame|End Effector frame|Optimization Option|Robot Port Name|Model Name|WBI filena" - "me|WBI list name" - MaskStyleString "edit,edit,popup(Full Constraint (Position and Orientation)|Position only constraint),edi" - "t,edit,edit,edit" - MaskVariables "baseFrame=@1;endEffFrame=@2;optOption=@3;robotName=@4;localName=@5;wbiFile=@6;wbiList=@7;" - MaskTunableValueString "on,on,on,off,off,off,off" - MaskCallbackString "||||||" - MaskEnableString "on,on,on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on,on,on" - MaskToolTipString "on,on,on,on,on,on,on" - MaskDisplay "escapedBaseFrameName = strrep(baseFrame, '_', '\\_');\nescapedEndEffFrameName = strrep(endEf" - "fFrame, '_', '\\_');\n\n%port_label('output', 1, strcat('{}^{world} H_{',escapedFrameName,'}'), 'texmode','on')" - "\n\n%port_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\n%port_label('input', 2, 'Joint configuratio" - "n')\n\n%clear escapedFrameName;\n\n\n% if strcmp(robotPart, 'left')\n% prefix = 'l';\n% else\n% prefix " - "= 'r';\n% end\n% \nport_label('input', 1, strcat('{}^{',escapedBaseFrameName,'} H_{',escapedEndEffFrameName,'}^" - "d'), 'texmode','on');\n\nport_label('input', 2, '{}^{world} H_{base}', 'texmode','on')\nport_label('input', 3, " - "'q_j', 'texmode','on');\n% \n% \nport_label('output', 1, '{}^{world} H_{base}^d', 'texmode','on')\nport_label('" - "output', 2, 'q_j^d', 'texmode','on');\n\n" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "'root_link'|'l_sole'|Full Constraint (Position and Orientation)|WBT_robotName|WBT_modelN" - "ame|WBT_wbiFilename|WBT_wbiList" - MaskTabNameString "Block parameters,Block parameters,Block parameters,WBI parameters,WBI parameters,WBI p" - "arameters,WBI parameters" - System { - Name "Inverse Kinematics" - Location [0, 23, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "312" - Block { - BlockType Inport - Name "Desired frame pose" - SID "1759" - Position [10, 13, 40, 27] - ZOrder 26 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Current Base Pose" - SID "1755" - Position [10, 43, 40, 57] - ZOrder 22 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Current Joint configuration" - SID "1756" - Position [10, 73, 40, 87] - ZOrder 24 - Port "3" - IconDisplay "Port number" - } - Block { - BlockType S-Function - Name "S-Function" - SID "1757" - Ports [3, 2] - Position [145, 4, 225, 96] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'InverseKinematics',robotName,localName,wbiFile,wbiList,baseFrame, endEffFrame,optOption" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Desired Base Pose" - SID "1758" - Position [280, 23, 310, 37] - ZOrder 25 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "Desired Joint Configuration" - SID "1760" - Position [280, 68, 310, 82] - ZOrder 27 - Port "2" - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Desired Base Pose" - DstPort 1 - } - Line { - SrcBlock "Current Base Pose" - SrcPort 1 - DstBlock "S-Function" - DstPort 2 - } - Line { - SrcBlock "Current Joint configuration" - SrcPort 1 - DstBlock "S-Function" - DstPort 3 - } - Line { - SrcBlock "Desired frame pose" - SrcPort 1 - DstBlock "S-Function" - DstPort 1 - } - Line { - SrcBlock "S-Function" - SrcPort 2 - DstBlock "Desired Joint Configuration" - DstPort 1 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "Remote Inverse Kinematics" - SID "1761" - Ports [2, 1] - Position [560, 105, 720, 165] - ZOrder 66 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" - RequestExecContextInheritance off - Variant off - MaskType "Forward Kinematics" - MaskDescription "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is " - "the number of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous tra" - "nsformation between\n the the base frame and the world frame.\n- Joint configuration: Vector of siz" - "e DoFs, representing the configuration \n of the joints.\n\nOutput:\n- Forward Kinematics" - ": a 4x4 matrix representing the homogenous transformation between\n the specified frame and the wor" - "ld frame.\n\nParameters:\n- Frame name: the name of the frame. It should be specified in the URDF model.\n\n- R" - "obot Port Name: Name of the ports opened by the robot. (e.g. icub).\n Set an empty string (''" - ") to use the name specified in the \n Whole Body Interface configuration file.\n- Model Name:" - " Prefix name of the ports opened by the underlying Whole Body Interface.\n- WBI filename: name of the file cont" - "aining the configuration of the Whole Body Interface\n- WBI List Name: name of the list of joints used to confi" - "gure the Whole Body Interface\n" - MaskPromptString "Solver Name|#Dofs|Optimization Option" - MaskStyleString "edit,edit,popup(Full Constraint (Position and Orientation)|Position only constraint)" - MaskVariables "solverName=@1;dofs=@2;optOption=@3;" - MaskTunableValueString "on,off,on" - MaskCallbackString "||" - MaskEnableString "on,on,on" - MaskVisibilityString "on,on,on" - MaskToolTipString "on,on,on" - MaskDisplay "disp(solverName)\n\n\n\n% escapedBaseFrameName = strrep(baseFrame, '_', '\\_');\n% escapedEn" - "dEffFrameName = strrep(endEffFrame, '_', '\\_');\n% \n% %port_label('output', 1, strcat('{}^{world} H_{',escape" - "dFrameName,'}'), 'texmode','on')\n% \n% %port_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\n% %port" - "_label('input', 2, 'Joint configuration')\n% \n% %clear escapedFrameName;\n% \n% \n% % if strcmp(robotPart, 'le" - "ft')\n% % prefix = 'l';\n% % else\n% % prefix = 'r';\n% % end\n% % \n\nport_label('input', 1, 'H^d', 't" - "exmode','on');\n% port_label('input', 1, strcat('{}^{',escapedBaseFrameName,'} H_{',escapedEndEffFrameName,'}^d" - "'), 'texmode','on');\n% \n% port_label('input', 2, '{}^{world} H_{base}', 'texmode','on')\nport_label('input', " - "2, 'q_j(0)', 'texmode','on');\n% % \n% % \n% port_label('output', 1, '{}^{world} H_{base}^d', 'texmode','on')\n" - "port_label('output', 1, 'q_j^d', 'texmode','on');\n\n" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "'/cartesianSolver'|12|Full Constraint (Position and Orientation)" - MaskTabNameString "Block parameters,Block parameters,Block parameters" - System { - Name "Remote Inverse Kinematics" - Location [853, 51, 2214, 1013] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "375" - Block { - BlockType Inport - Name "Desired frame pose" - SID "1762" - Position [10, 23, 40, 37] - ZOrder 26 - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Current Joint configuration" - SID "1763" - Position [10, 68, 40, 82] - ZOrder 24 - Port "2" - IconDisplay "Port number" - } - Block { - BlockType S-Function - Name "S-Function" - SID "1764" - Ports [2, 1] - Position [145, 6, 225, 99] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'RemoteInverseKinematics',solverName, dofs, optOption" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Desired Joint Configuration" - SID "1765" - Position [285, 48, 315, 62] - ZOrder 27 - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Desired Joint Configuration" - DstPort 1 - } - Line { - SrcBlock "Desired frame pose" - SrcPort 1 - DstBlock "S-Function" - DstPort 1 - } - Line { - SrcBlock "Current Joint configuration" - SrcPort 1 - DstBlock "S-Function" - DstPort 2 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "wholeBodyStates" - SID "206" - Ports [] - Position [16, 17, 114, 114] - ZOrder -4 - ForegroundColor "white" - DropShadow on - RequestExecContextInheritance off - Variant off - MaskDisplay "image(imread('wholeBodyStates.png'),'center');" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - System { - Name "wholeBodyStates" - Location [425, 373, 1725, 1134] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "576" - Block { - BlockType SubSystem - Name "Get Estimate" - SID "1671" - Ports [0, 1] - Position [360, 212, 440, 248] - ZOrder 53 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend" - RequestExecContextInheritance off - Variant off - MaskType "Get Estimate" - MaskDescription "This block get the estimate of the specified type\n\nAssuming DoFs is the number of degrees of f" - "reedom of the robot,\nOutput:\n- References: Vector of size DoFs, representing the estimate requested.\n\nParameter" - "s:\n- Control Mode: The control mode. Choose one of the supplied mode.\n\n- Robot Port Name: Name of the ports open" - "ed by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in the \n " - " Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by the underl" - "ying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Interfa" - "ce\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - MaskPromptString "Estimate Type|Robot Port Name|Model Name|WBI filename|WBI list name" - MaskStyleString "popup(Joints Position|Joints Velocity|Joints Acceleration|Joints Torque),edit,edit,edit,edit" - MaskVariables "estimateType=&1;robotName=@2;localName=@3;wbiFile=@4;wbiList=@5;" - MaskTunableValueString "on,off,off,off,off" - MaskCallbackString "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param" - "(gcb,'estimateType');\nif strcmp(maskStr, 'Joints Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510" - ", 0.6706, 1.0]');\nelseif strcmp(maskStr, 'Joints Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745" - ", 1.0000, 1.0]');\nelseif strcmp(maskStr, 'Joints Acceleration')\n set_param(gcb,'BackgroundColor', '[0.9255, 0." - "8706, 0.1333, 1.0]');\nelseif strcmp(maskStr, 'Joints Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.57" - "65, 0.6039, 1.0]');\nend\nclear maskStr||||" - MaskEnableString "on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on" - MaskToolTipString "on,on,on,on,on" - MaskDisplay "%disp(get_param(gcb,'estimateType'))\nport_label('output',1,get_param(gcb,'estimateType'))" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "Joints Position|WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - MaskTabNameString "Block parameters,WBI parameters,WBI parameters,WBI parameters,WBI parameters" - System { - Name "Get Estimate" - Location [653, 318, 2086, 1271] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "1000" - Block { - BlockType S-Function - Name "S-Function" - SID "1672" - Ports [0, 1] - Position [125, 39, 185, 71] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'GetEstimate',robotName,localName,wbiFile,wbiList,estimateType" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Estimate" - SID "1673" - Position [210, 48, 240, 62] - ZOrder 25 - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Estimate" - DstPort 1 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Block { - BlockType SubSystem - Name "Get Limits" - SID "1690" - Ports [0, 2] - Position [475, 211, 570, 249] - ZOrder 68 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend" - RequestExecContextInheritance off - Variant off - MaskType "Get Estimate" - MaskDescription "This block get the estimate of the specified type\n\nAssuming DoFs is the number of degrees of f" - "reedom of the robot,\nOutput:\n- References: Vector of size DoFs, representing the estimate requested.\n\nParameter" - "s:\n- Control Mode: The control mode. Choose one of the supplied mode.\n\n- Robot Port Name: Name of the ports open" - "ed by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in the \n " - " Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by the underl" - "ying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Interfa" - "ce\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - MaskPromptString "Limits Type|Robot Port Name|Model Name|WBI filename|WBI list name" - MaskStyleString "popup(Position),edit,edit,edit,edit" - MaskVariables "limitsType=&1;robotName=@2;localName=@3;wbiFile=@4;wbiList=@5;" - MaskTunableValueString "on,off,off,off,off" - MaskCallbackString "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param" - "(gcb,'limitsType');\nif strcmp(maskStr, 'Joints Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, " - "0.6706, 1.0]');\nelseif strcmp(maskStr, 'Joints Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, " - "1.0000, 1.0]');\nelseif strcmp(maskStr, 'Joints Acceleration')\n set_param(gcb,'BackgroundColor', '[0.9255, 0.87" - "06, 0.1333, 1.0]');\nelseif strcmp(maskStr, 'Joints Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765" - ", 0.6039, 1.0]');\nend\nclear maskStr||||" - MaskEnableString "on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on" - MaskToolTipString "on,on,on,on,on" - MaskDisplay "disp(get_param(gcb,'limitsType'))\nport_label('output',1,'Min')\nport_label('output',2,'Max')" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - MaskValueString "Position|WBT_robotName|WBT_modelName|WBT_wbiFilename|WBT_wbiList" - MaskTabNameString "Block parameters,WBI parameters,WBI parameters,WBI parameters,WBI parameters" - System { - Name "Get Limits" - Location [0, 23, 1280, 744] - Open off - ModelBrowserVisibility off - ModelBrowserWidth 200 - ScreenColor "white" - PaperOrientation "landscape" - PaperPositionMode "auto" - PaperType "usletter" - PaperUnits "inches" - TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] - TiledPageScale 1 - ShowPageBoundaries off - ZoomFactor "411" - Block { - BlockType S-Function - Name "S-Function" - SID "1691" - Ports [0, 2] - Position [115, 33, 175, 102] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'GetLimits',robotName,localName,wbiFile,wbiList,limitsType" - SFunctionDeploymentMode off - EnableBusSupport off - } - Block { - BlockType Outport - Name "Min" - SID "1692" - Position [220, 43, 250, 57] - ZOrder 25 - IconDisplay "Port number" - } - Block { - BlockType Outport - Name "Max" - SID "1693" - Position [220, 78, 250, 92] - ZOrder 26 - Port "2" - IconDisplay "Port number" - } - Line { - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Min" - DstPort 1 - } - Line { - SrcBlock "S-Function" - SrcPort 2 - DstBlock "Max" - DstPort 1 - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - } - RTWSystemCode "Auto" - MinAlgLoopOccurrences off - PropExecContextOutsideSubsystem off - FunctionWithSeparateData off - Opaque off - MaskHideContents off - } - Annotation { - SID "1213" - Name "WHOLE BODY TOOLBOX" - Position [241, 157] - ForegroundColor "white" - BackgroundColor "black" - FontName "Sans Serif" - FontSize 12 - } - Annotation { - SID "1214" - Name "Copyright 2013-2016 iCub Facility Department, Istituto Italiano di Tecnologia\nThis software was s" - "upported by the FP7 EU Project CoDyCO (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b))\nhttp://www.c" - "odyco.eu" - Position [240, 192] - ForegroundColor "white" - BackgroundColor "black" - FontSize 5 - } - } -} -#Finite State Machines -# -# Stateflow 80000010 -# -# -Stateflow { - machine { - id 1 - name "WBToolboxLibrary" - created "06-Feb-2014 11:51:26" - isLibrary 1 - sfVersion 76014001.0004 - firstTarget 26 - } - chart { - id 2 - machine 1 - name "Utilities/holder\n/MATLAB Function" - windowPosition [1152 -205 -179 985] - viewLimits [0 156.75 0 153.75] - screen [1 1 3046 1050 1.25] - treeNode [0 3 0 0] - viewObj 2 - toolbarMode LIBRARY_TOOLBAR - ssIdHighWaterMark 9 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 1 - disableImplicitCasting 1 - eml { - name "fcn" - } - firstData 4 - firstTransition 8 - firstJunction 7 - } - state { - id 3 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 2 - treeNode [2 0 0 0] - superState SUBCHART - subviewer 2 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function s0 = fcn(s, ~)\npersistent state\n%#codegen\n\nif isempty(state)\n state = s;\nend\n\n" - "s0 = state;" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 4 - ssIdNumber 7 - name "s" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - } - dataType "Inherit: Same as Simulink" - linkNode [2 0 5] - } - data { - id 5 - ssIdNumber 8 - name "s0" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - } - dataType "Inherit: Same as Simulink" - linkNode [2 4 6] - } - data { - id 6 - ssIdNumber 9 - name "unused" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - } - dataType "Inherit: Same as Simulink" - linkNode [2 5 0] - } - junction { - id 7 - position [23.5747 49.5747 7] - chart 2 - subviewer 2 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [2 0 0] - } - transition { - id 8 - labelString "{eML_blk_kernel();}" - labelPosition [76.125 85.875 102.544 14.964] - fontSize 12 - src { - intersection [0 0 1 0 23.5747 14.625 0 0] - } - dst { - id 7 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 2 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 2 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [2 0 0] - } - instance { - id 9 - machine 1 - name "Utilities/holder\n/MATLAB Function" - chart 2 - } - chart { - id 10 - machine 1 - name "Utilities/DampPinv/Damped Pseudo Inverse" - windowPosition [699 -205 167 985] - viewLimits [0 156.75 0 153.75] - screen [1 1 3046 1050 1.25] - treeNode [0 11 0 0] - viewObj 10 - toolbarMode LIBRARY_TOOLBAR - ssIdHighWaterMark 8 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 6 - disableImplicitCasting 1 - eml { - name "fcn" - } - firstData 12 - firstTransition 16 - firstJunction 15 - } - state { - id 11 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 10 - treeNode [10 0 0 0] - superState SUBCHART - subviewer 10 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function DPinv = fcn(mat,sigma)\n% Economody size svd of mat\n[U,S,V] = svd(mat,'econ');\n% Damp" - "ed version of S with sigma\nS(S>sigma)=S(S>sigma)./((S(S>sigma)).^2+sigma^2);\n% Damped pseudoinverse\nDPinv = V" - "*pinv(S)*U';" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 12 - ssIdNumber 4 - name "mat" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - } - complexity SF_COMPLEX_INHERITED - } - dataType "Inherit: Same as Simulink" - linkNode [10 0 13] - } - data { - id 13 - ssIdNumber 5 - name "DPinv" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - } - dataType "Inherit: Same as Simulink" - linkNode [10 12 14] - } - data { - id 14 - ssIdNumber 8 - name "sigma" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - } - dataType "Inherit: Same as Simulink" - linkNode [10 13 0] - } - junction { - id 15 - position [23.5747 49.5747 7] - chart 10 - subviewer 10 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [10 0 0] - } - transition { - id 16 - labelString "{eML_blk_kernel();}" - labelPosition [80.125 91.875 102.544 14.964] - fontSize 12 - src { - intersection [0 0 1 0 23.5747 14.625 0 0] - } - dst { - id 15 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 10 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 10 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [10 0 0] - } - instance { - id 17 - machine 1 - name "Utilities/DampPinv/Damped Pseudo Inverse" - chart 10 - } - chart { - id 18 - machine 1 - name "Utilities/TruncPinv/Truncated PseudoInverse" - windowPosition [649 -205 167 985] - viewLimits [0 156.75 0 153.75] - screen [1 1 3046 1050 1.25] - treeNode [0 19 0 0] - viewObj 18 - toolbarMode LIBRARY_TOOLBAR - ssIdHighWaterMark 8 - decomposition CLUSTER_CHART - type EML_CHART - chartFileNumber 7 - disableImplicitCasting 1 - eml { - name "fcn" - } - firstData 20 - firstTransition 24 - firstJunction 23 - } - state { - id 19 - labelString "eML_blk_kernel()" - position [18 64.5 118 66] - fontSize 12 - chart 18 - treeNode [18 0 0 0] - superState SUBCHART - subviewer 18 - ssIdNumber 1 - type FUNC_STATE - decomposition CLUSTER_STATE - eml { - isEML 1 - script "function TPinv = fcn(mat,tol)\n%#codegen\n[U,S,V] = svd(mat,'econ');\n% Setting to zero value\n%" - " Setting to 1/S(i,i) singular values greater than tol\n S(S>tol)=1./S(S>tol);\n TPinv = V*pinv(S)*U';" - editorLayout "100 M4x1[10 5 700 500]" - } - } - data { - id 20 - ssIdNumber 4 - name "mat" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - } - complexity SF_COMPLEX_INHERITED - } - dataType "Inherit: Same as Simulink" - linkNode [18 0 21] - } - data { - id 21 - ssIdNumber 5 - name "TPinv" - scope OUTPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_NO - } - dataType "Inherit: Same as Simulink" - linkNode [18 20 22] - } - data { - id 22 - ssIdNumber 8 - name "tol" - scope INPUT_DATA - machine 1 - props { - array { - size "-1" - } - type { - method SF_INHERITED_TYPE - primitive SF_DOUBLE_TYPE - isSigned 1 - wordLength "16" - } - complexity SF_COMPLEX_INHERITED - frame SF_FRAME_INHERITED - } - dataType "Inherit: Same as Simulink" - linkNode [18 21 0] - } - junction { - id 23 - position [23.5747 49.5747 7] - chart 18 - subviewer 18 - ssIdNumber 3 - type CONNECTIVE_JUNCTION - linkNode [18 0 0] - } - transition { - id 24 - labelString "{eML_blk_kernel();}" - labelPosition [80.125 91.875 102.544 14.964] - fontSize 12 - src { - intersection [0 0 1 0 23.5747 14.625 0 0] - } - dst { - id 23 - intersection [1 0 -1 0 23.5747 42.5747 0 0] - } - midPoint [23.5747 24.9468] - chart 18 - dataLimits [21.175 25.975 14.625 42.575] - subviewer 18 - drawStyle SMART - slide { - sticky BOTH_STICK - } - executionOrder 1 - ssIdNumber 2 - linkNode [18 0 0] - } - instance { - id 25 - machine 1 - name "Utilities/TruncPinv/Truncated PseudoInverse" - chart 18 - } - target { - id 26 - machine 1 - name "sfun" - description "Default Simulink S-Function Target." - linkNode [1 0 0] - } -} diff --git a/toolbox/WBToolboxLibrary.slx b/toolbox/WBToolboxLibrary.slx deleted file mode 100644 index e19fa448e9e24bc18e7446db8c43a6dec97ec0eb..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 38548 zcmaHyLy#^^u%_F_Y1_7K+qP}H`?PJ_`r5W_+qTU!|J}`viI_!2R1_9(zO~4BGD}eg z6buyz2nY(u%@jk%tNC-*5*P?*00amK=f71uGgl*1BUd8^69;EAMP~;`GiO&TGZ%VK zJ6k{H1^Gcn)SuMSJDn|fq--W+HeMlP)Z&L4*K{SVt)B zepSrt!ETG^R7FTaz?MeNmC;n*dNV`NFga)3XQc~_V(^@((~YXCvY4<6tEz@QU>MEf zn@mb_3#(sIM%TmWs2)mH^K>0d>P~z5c7ZhX?%hDRD9ocUymK9A?JRIr14wvuE=-#0 z2Cdid&Vhws;F~oz+TKje~$6FF_OCVn&7I?-pOmkd6>hD*vwNSrWoxM??W z%18>C2LV;D=N=aR?5KJEsQrg_Ezb&&d#3d5cjM1aY{+--1nS-cUrJ-V`v5%f3euOr zft@6|QBF0b(F8nB>>P3)+$R|NYoktUjQ9lU1uUZC7=Z_q&uypv+cz{f-2L3DnSkQ| zGRUc@+jjInVSeqBK=EznPjjv{v` z>f|}Smkb__)!BUF;#;@NXZ(kQ_8R5Id8ejCuyMkx^ZD_v`QLj=5KTgG0+bIIkxT;% z_pTlFyNdFtGGC)5{-HiX%PB+YIs#^37nECjIkU5ciYnGM6ozW{ig$5ZH|r)jK=Osv zj5WdK9^G1|)c)8U|MdZzH&#y;Az0nTx^^8S{&?^i9%<$d7CAg%{!qS60C-jc4l3q! zJ;;7eo372ypT;5Ye+NJd3l zdbNjzh{1-T;PX+uv)O`t4!MyXVZDK31DCmUt-Y@A1HP&I}~9@ZIVv58Ha_ z-U!~0s2y40S-dBQSXfTYjYV{Y_AmP5W1Q%ZYjbX#6DQqpF-|NK-H`2vsfq2d5@Oy@ zQE8WkVcjD@m$;1qe1UTm5&wm#MqsKuQ8eyhrd8>d{+M6<3-rIW*-<7!PwTp#KzRh$imsk?tkRm^!RveaVBwm_X&WK`WL3(CKxu zNe102JMp7r!#|78)!)&b!~O35LFc#h?i~+(aCP$ax86A3b{^+)x()ieY4fIG8lKt` z7^j@C<@K1n#LXh08*?S_D|v4n^%vlQFm|w!Xg&Dy{aiIQ*O@&VJ#cE|wNYEHEm=ux zS5xO+F*Vk=_#L3E$A_x>T57%7)gl=R>kwm+sjK2Cszy;`+`&rl{JL@Yk?&jBUg@;^ z)^LE&df)Sh<-B@xRq23MYUO=4lpmg&1<>WG)k<&s8`t+~_l5>f$<=0JDA#iw`WUC_@fPUqwa>HezVYZ|)!1t+N<;iS zv9#>YcZi40#qBZdI=fmSZr<|=B}e1fgX`cyR$N?q0;+OyyR17-@+@b5bnA1P!K-IHyvB0;oGj&@e`V!E2dyER!bakR_$@Y13 zsxNWJ!9%D7`6I8eI)37Loh-;0Vs0|;;Nf$V#&3DY2I*~~amDs*U%s=l<)dfWTj%k- z2aCb|<8fL?vWBJOfe~i1jpQI11cZPt0Kh`H8(L-sd7OPTN}F@MCH?1%Sam=K@dwt9 zvvd@l2{B*cBC_r0HJspP|7C#IF`ye(A482R>@<}|Z_gL*>OhMXE@r~@%dULI?yjYi zDv9EYh+V*M$p2Y7@*brsSUtxsVnL%SgWn+DB@`sXQUN2&jO`>{J z!Mt-pne^rR>OM+=QZ50Y9@uo7) zVWa(lIO;05qt4etq7#fia0Al}l>KPf;s8s9iH(qSNd2Ghfxy&G#&6pTjyh5119-k^ z%7>FdV(Oql>#v$%<4JV*qsfd9RZYP?J*B|6rU|+S`mv$EvOmuUI38!1D*txj5iE|F z68b-3JD0P?$mafDbVGV)!A^ztSS>M3uQpCpG8C{u0xq3(TU`6|W06H&em0*a0wWcmvfkb*jlxbPwB5coK|2ab{;qmn>YU6629h>IIYBK)?^ z#$=(I79^~!crZM$bYeMb`6dVxMGSlx#9`kI)i=pOT*n@wv~{U=sK1^QO*tg3%k6rH z+AM_z-hSQ)?a>u%o%LRu-i;xjTwj9c3HziFMI(q}mL~8hR~3p@R%Ki1T`~)ZLC&sW(rns}Smx(h*@Y+pikPH>I=M65gI6G!CEr*jpo( zs(WCKO$I!W@HXG0r$t5Nu&;+aPRQMoW~(LK!AwiOop#x8)U@AoEnw{K3c#U%$`!%_ zE(IsT{5~$B0W>J0fXO|T;>|U4l_?s0uz}MDdEj1=Jp%8Xr@W>>BlwRU{K<&I z)*alKD5#7p!KanZzpgyib0-;TTjmP~ikdF(a-xQk2L8gPW@s{j<_T0*;&M}2pGtMn zBW%hzoAb?rZCpGvY}u-%g~Iu9Yz=0zp(Y^)Dy%$LEZ{U64{JUol$DLgB8xaQ&9EBv z_d!pPmX|^2cEC5YO~qu|DMz!tQbRzT{q2Gx0&<%#SU)0v&njO+0R9JO%suB?cEFlW zaNkh7^k|Abu}8=;puhDsrgEy;kiSq~3A}8hk0Kse*IkG-q-~tpn5q`IxOdT*tPER? z23Jf}_OVfKyfNixW5Gp{8KD-gYu55K@Ef|w$4k975k~Cd{HDxs2m*3b}SXE1jE+Y^^sG0xAx; zX)AXeq5}-_Vj~KY!$N-2vR)lSO1WVd*f_$MKgL>chQFRUqf4!0;t{a7hGL&VTA==> zHbS2Qo)8=mu&#>J0#EhqU|hv1L~8AKs|5RIa(R5B&Jd{OEz-r#W|^?Gk`;OA6Kz!W zxLnFu3I-eoYT2i>A5_rf=^znd3fQ-}6rN#Ahrlldy?!l}VqI*El}xBw=*;CDIt~es zl0v6Mxwr+Gm*@{LsuT0XsDk+tG8Cmpl)M_G`m3~(Vep)`c-6vTU{!EvEjv93*^}5R z%_B%d_FzV!)jM;*W6Vd8f#guERV3p8)f-0#p~h0{B`WLcN>?Y4V|8hW82VQXAj=y0 zrn5~9lpbQen^0b|wn9)rvvNee$Z@lvFbiNa+FeRQcNE>LU<vv$i;U0a#{BQH}DdsuQ+XosVZlZJBLtZ*=}N83`%@LoFvI=K$KTcCFNNn42u0d zn$*`dc!Uiqxj)KIU>wx8%@Rj}P%TwAuy98+%lo{WoiXQcV1OmKN%T5-0zov@N^j8I zn(3*_A-<=_{Jif=F^zDjxgu!P?Xl^h_Z%c}PwGq=nogm3e~0$LmDFH1XC9TreTOj0 zonHH@T(HU2!95lvxC|a+wsNSrU@&fwW67>zJ97UARqgjF+6m)@x}?s~R0*L)`8>3? zI<6|qk}BS5i7`HQ@mzU*2l#ImuM1|e59M3a%`SF>T$L|#wQ%SFJZL<~MUt-m z_Q=E%Y6o8j*$ zI-rVJ;2poBg3beVb-Jm0gJv)GYZMFigRL04E1%2yrF)=GnSlsrJG2~kvt~-p!%6U3 zw4BYrfc=cIw$^|nW1Z5cGy<~|A6wp5adamTYEK=;Ymy;`zA>>Cdbs(FT@yb3#lee- zxSL0xVm1gWyMABV@_NFSufTVcgdmo)JWut4SU!)U0hsMX;ZW8z^Y#`INQ+9tCUVbl zE%t{bYx4^$4@PVy)%BXI7O#XYtR6|!Oukx&TETHmM|5N`vDA$O zIsqW3XJ1zZ@%+kySs?1iWC^zj<#mm^VT4T9zOtn79zJ z+H~kN3kL)dqk|u=)X61PGD*#krKkL+0<~r`(9T(!(HbC*V?SQqkzhBMGON=11gaVQ zt4$l$)h=AbUeEY3f0b^x$7&>T?NsMIL)19Mp43LwcRbkFNKG&hzS;z_+2|}TuZSP8 zbD}G8mH@svAYM`~;VezYom-K4l?gf+S<#jRj=jp9%|eRbCi>6XsV)98%b5Wd#$|;X zb=3No=?PR+AfjLIL%&g;R=Y~|mgqMw;~y@b&S>t@XCRw0HsMgBdfgZ=u*PmkZ7ns| zJGA69&cA6aZxXXf63F80b5GAg$+&t5eU8MBusJ{ooK09B+bSb=%6`N zKv;JrZACRtH+>!_KjmS=S8-BGCnLz5_u#>tEy-oK?Ygejh=h{KmY?d`--4Y^r%HcF znWqA6hNR&LV5~spidIUcmSxVBDkU%66`EoxRrrwOh%EeBJ%f+&2T(enq3IN91b~1wz=zCgRE@z~EEnFSq_txJ&97>79haKn z4H&;cbVSH5CpjChw;RTVj)B3Mh;>)xR45Tz7EK%I5upoOO7lF> z@SCFqC7?gEl5y{N+LSGC&6QH-braI^a^^~XV-d)^)4i%2sx5Oadg{q?MBmryFH@Mbh% zZ=s8gMS@-vAj_j>b+yX&>}!rZKxyfTek*1P*Wb;Y(&>=#b@Qpu4GvrE8l?by;2!ji$?Ne)1q^{%D zbBKhixCPXNEXL1%lGe^GP@-DtN+lneUtE~=t;SB$iaHZ<2R9e8kVH&Fd=7nDh^C8l z7ttvL8M^7I>ghSB&uO~HAQP2ObXvWmOyhf7^id~mrCvO2g0fQZM*D&g_pxNNa!SPt zd*FM5ms8xcg@^#+VI0fZX4f|oTA`Y(aPGGu2X~o!(CgnWUUeHA%_x{m%nL*GSMv_+ zHSXY#FY(tvq^*-#3x8+c#Ps(`Gv=<&(LVMOV?QS1pI$9j|hG?dqArcS9pNpy9MCP{87ud;uHkST0!D$YhlBfp%T-!`uEc4&P+mtZcW2LjG*s^E2@9%oJ~QFYH!n#d7sD zFAa**kn12@j7qGfAu}`OF%miq9%?gXmU|Pr zw>8j5?Zughi<)sPin~5|D{Hpp5zv(0(_!H9;%}vYp=~MduSr6kpL2vQf`m8bBh1dQXU1+mdmzp(PBvyT@W@?}b|LLaaa7g2_onDmD&9w%V&Ww0%(fX%!@ zvXH^epRt;2>#_CvK*vfF2uw=;)^ro0Xx^BH3j!3#S^oILkdK^oU8&>{kU{5{PF@cu zsy$9w@*1uOE;-T!4d2QJwb_s$y>;q}9EKucwsdLCEUg z9SpjhvIiYmRe!k9isNMyz^&`UqRf+fK~reD*RUjV3!ByNRBtJP>9akmHtD%Xy#ke@tqnN=#!e1kbGP)IJ|l{*4K@9cZo z5pL!z(p`{Um~(xE=wlKoT5ct?+Q|cmtxNXNT1t%HS8t|=F$+zir+GYCk7j$a2tZ`4 z_o-}wYq@u0zcTL``ybg!SHVA3JPnsL%T@LNo|qo^V4B`CXjZW!I$ zPMJuRT8|@&rv5gT7c347Zpd41SU3yF16KdTiRNJwoSVe^ngCBsGMInL)&g3A0%AoE zENB4Z<9-wzQ~^jk8dMz)r5Ycu5i$3kp!F+e;=@Ph~Jks?hmo zYTLj%!6H(+QXEldAp5?bt(U#se>X+Oey2=fQELHRe|j+(0J4aD+FjOdf-8>w0X#MO z7;zmpZTk0ZP1t%%C1OE zHpa~ox<;BZ>(Qv>MaoR7mn=MXa(@j*o(7xbU*+@N$x=!rJqp!WjCd6 z(#@i6kzppjdg9vj2ve1xUO9MQj50qFUUx+e4JNKxpjN6gbYBsbk5%|sLqmipVWWe1 znpQ_=zae+X7I|!hKLBeKBDW1#am)K4?LC{8z)gc^^1@5OipKbOUl9gEh&sm)8Wvw? zk)aQxj9kFv_X^riHkD*cmZE$5tFc4Bx043-}{K>%=@OffYkpBH|zg|SsY3I7XK79R&i1T|lD9)h)ha2Qr@ItWGsKXw@Wr6|Y0l+EZylarpL z{cMi1m0Y)}hNALhayt<{8TKz4}k( z_O4K&^vHnE5?hi(gi{)TBb6kpC&a-qjIc;A=>5erfz}Q;uq`D5Oih7q3?_v#Wi=or zL{$9<(D>I%tb7oN*2&}szdgeOu_6G1L})Dju6l-T_#P9_eXjSm<<747eqA;@+8#3bA>y& zL%gdrDhAQ*NIKjB0=i&v4>DgDb3Hu^$~@B>kRk@`5Q_kC5A+rV&L}S_XY?{`qKZ>z zxLyD~$p#ittco7${HV$)sEbjk_Q9jh8m)E~h3gJKn#nj^f-`ivoRt;TZt! zQHK8%KW;>>!iPhw)waGgx-1dS4V<_~nNT6>@RRk#Q}I7nO5#7h6-?!lt%)J@M*xoyG2TB3JL5f%jYUn%%l) zb7$?{1^W+<`@UiC%DT^{()@9v9t8!eiOa$zxCcp>rKDqqQKE~j5oG-e7*AW?dC@*< z%V&tbiMLy%H<`vnx?5&0N+^?EcmF4m3C%)k400%qJd;!sD2JKCbZ9P&g`}y#^e_iqRI`UpHMw!|W zZRhbZwIqfUZJ9X_DJcb%JdHEU=yc~8hDom4o>^yrXM6%c0sDagV2AH4o8k_!3uo8D zljt|_Gad|k@n{r1Z82PZUa17fIB?$7cy9^oCgxL=(`+%BM^>0z8@B@>=_*Hana#s5 z(}jhLdRc3~Ss8gmXGGe=W>hI9k-bd3xC}!CY!g|BW5{`q{P?KQK_`_W(CFcdDN47; z1N9c-v((50a!rhWP4{PS8Eo(d#^YA~dPj>OnC9Bgs&q%G9A+W~kU72~@3`lq(qnxV z&q;s6@wA*L7xLLY8P+T}t}->zqD$cRYSN$})8>~^ympnDL!^#^;y&_V%bu1k-({SZ?7#g9XM^je zjQQT&Yfs4C zIU7zLQqGZu^QIzA|K$(r>XmQtlWB%_VhCu;aLW0k@Z{cJnYU^Iyr(*ZAU zyM6=1f+g&%8@K$?){*TDv)A!1iWJ8D9eyBa%i%7PZoyEw#v-jbWq@%VT@O;J>w4Qh zvWt!v6DJ@YH`;NgCALz*^VfcBb=opE+6IV0aw8W3Exz;!=VPG$`N=Ux|2;G}Ducut z9%1BQ>fRLPLGm4^EAVSMha5{t6II>cyqGNEqEAfw9?gw)u41YdTL;miZMu~Lo0UyQ zc!|~({hXkn@MzD|C>0J2iHaXX)bp#DDja4K;vgHZ8EeGKh;l5tYGHi}l0Q21JrF`N z`7y$HzdpqKPJW?t=IASYZ0CCQj5TSG$r2cC!prX|5zSn`0ylzG>)urq3qsYuue!tx zCAEmiMU3ApMyCDeb)aQh;a$OrE^9AD7Jgf`eI&Z|Y-S9W5DLpkFQpqweUemRN+KCW z2oA?YZ}_#*n&b08mU3Sy z0r3vL1XrAqL-V!(WCa-FGd`d?chDJVP|x`X(PKN<%o2TXJd2}=%wkca?6W#rh!Cu( z$V6N<{2x)%WaASfAd?VWBQxO2pMkm{pWyJu{r%UG{>V#o4{A|+N@dV^nE)e!QbGQuYJNLa~QL&(dLZ`SkOS-MiA z(zE`nS0D%vK`>!QV@`NNc6Kft4CUCuc*#a(Bv+S6Au}aRr`&h*8bbwz(Vc2$j>SjY ze^qHLN7L|KY*rr7?`3>J#ltEX1%fF)%BZ!OJZd;3C2QI>5pnFA|;2*gV)>JPQ~ zg`B9Y%r_|&Kvc}JQ+Yp%X+(<6jv1c0#a6|Fw{Nk}v3{N_pSOU@?gLP~2vbOvxAP`CRl9`Tr<08#+`KPZ*DBiCwre!O zqu4>YK7Rgl(#55Q(6XuMy>ynA#ge@pm z5pHT7OHB}^e2uKs`!F@{EbuZh7e`jAG>|PVauF0)#8deDifZm*m%E*I zEk!7CHRx{%^~6VNtw_rxwxx2{^x+Z)c^1zNw4?02AE9uxK5Ar6+ENDnmVGzx6>uWq z1Jny1@~*L5Hc5xzG!{9S)gn^wA45>1aoM#1k@DTk^Y)BY2CjydPFtH}BEzjaoeJNS zi>C42BNmobo7m^FB+wYbcWTovSqDbF>$Mn zkQNgrIawM=0wem$X==g5hxyFOo(dc`3-WWje!KWb;M%2}pUKEbuQ2&!<<#U!JBJv! zkeFfdNqDyH2BuH7&B@5@Lyi<924-!G`rw&67epi;_t(O`_HI?U`aX+It&`irtkRMf zDNJDIswYrIOGXk81)o~ggdFIet%R*QPNsaJM^5LlidMqt?+hO3*EG2mw1%XXftGzr zgewgMA-R9Ye;K9{-G}P0ToiN<+T$s9F9EZ>0*G1?PctH7yeAYp^b9yfaQ-T~?O-1J zA&IguC9xCBdIQ?g3|Ws>aMm@iyD;X9H7$f{7R(ha$@gB`b~56H?dTW1!eXu99<%(N z6`l7+1-83LDDH`AIM6H37->nc{5m`O1D&UNV7J zE-n~E7+jubI*hF2>W0@ou;Nyya6H{wey`ga8N8nBD!tcy$e|+y-c3_L!E16F*kR8u zaa;$Pb=IM`YE~SKxZF`&;a*2|_LvT=kU^ZVL}ZbY##|W3kLTT>-56Uw39IIiGi_+x z+yPFge1qol!OO|P$xM0n)YZSQgX5)zldtTnsV9X#-MpSYEOMWeI*U(atephd44S}h zW4-UWftQuBp{C-5#1Gsew%rk4WmhS8t{L|aR`*1Gx{9g1y>XrzSKu1Y2UT~l$;CGq zyg=}{&gWJJX96-4Wm8_qx3oFt@i4j%xPRuB&0hJQX+}?y{M!42Z|$;~cL&lDf}E~Y zm|0R~{nU;QEBhEU_e)F+*g70Xdrdq`_zy(5^Pwv7FH%xq2F827*u>7{6A(K`-DMXqk1Au;!Rr^@QFbjq`+k+B*|9y)gP=Efvw4?1q_bw zEcx@|RBFZ3dl^=keP}H*W@%x2MOT3pLjP!y0os6d5WVc>aZe$orEW87=Do8l^J&i2--0zA((DPXpjP?g z8FYJXqG2+x!yVvaPDGxDXW4odgV8k|ogCm|iSX(}ib*RTkK(PJ_i-VlNp>||ymwqo zQ`XE|Q=CIIg=Abrc+&VR%RNy@c)It;v95i#9i)W=<1bO=V0&D3P#TK(;+h9kO%6;K zI~TUp zU7lu;3V}ns0tP}5bT94O8)&td%IWH3H=r#IsUO6b%HSkBLK7GHR)P_tQsbXT32Kwr z$OFocCCEEC(j?s!8Dh4v)!_S7WyZ}q)<}#l$}5F>(uMi?j>P+8%1{`Ur7chx`N3{h z=sT*X<(cbLtzcPB;JxT0Nfy3(S!DEjPbzZPfAtVn6u~Ry*Z>vNQtTH%)FUS-xH{KV zRPi5~0v>XoF2Vd86{(GlWbP_xDmVYrXvOwlI|TN!kZH=4-f!blB;;aRLGA|})2L!4 zB*3vc@=AXoAzBN5t3UDEdR$MgkxqG+Sx-!BN$pi%2QHh6TvhVHPaq>Rtt-AYN@OTH zwHNkq8ogm2+10*%e)I;ZD;|oev$@SX8|;VFa7U@_*?|b?h8MbjJ5a{3a;X_`DSat z=^EtTprdjQg-#e39!Km&71VI9n{b?l$*cFrhv|@sH}sE7oI+9p2q?4pVGfUNQO7YY~5d!@^9Ov&y2hL z8d}B#d+St=%h{57I>%DU$4;S0L!swnt&6P(aS_mzXxjSuQQF`5`nph#rMsl1!Xtkf zl`YTv3PD1Steo*}&cxYN-(q3I$kgWP<64@wXqPMM6E|VvY_7>=TFz_7Uyuc1o)FK6 zT<~8C2w0eJ9n@>C;`@^ki^gjcW@0^RDQcTnD+N1vpb0j*kAzL?2Vr7qYsL8r4aGgf zNC5Z8%R|(sImibd(?05IT92N8j;~Zs?`y+}qQ7<%J;p{@PvlMf?<~$G9rO}%gJRfb zQzh^~F3-6atp8gD!-NxCIAe6L#Jizzyzt+|9X)3Jq1k>(C_#e^>T@L_!(udPvEwQCK_K@{0Q)_8CvgSwXa9{EF7MG`}=Xh@DVs5qY9es zinryAhN^hw~ac~TTy=I)tGr(Dx^QuE9?fG(WV=c1gf8IQ_5YNYi6hiED zJkAS>(0TLep+v~vH)l6qPVT{b_ZPA^Hzp<~s;&+gun{{3gjovGX-SD=;MG@KjF97C z|H+oa_pIS`FZ=j?O23*_mFmR>LD)G=xz_l}Ik3C-biBdvSnsdJ+ShI2y1vvPuAO~t z`u&l8Gw5lA5_$mDUFna9U+eM#kLJy648RF^U;2koCh!B5Z-9dzy#GCnQ)Hm+|7>P4 zPY@Azj@&0hN#e4La$7n+IX@nV9I31|lT#fF(1upJngT>0J1+-G@-7{T>}vN4U&DR| zhGwn{NBW#*97AVW^^^tpHzE;^<1w)Y37>~947C=M8Wx6+W8XQhDO;BreT>77_)3lR zh>F12xOiJi1&Q8-1jh^f6Itp|pu$g0@s7qaTCYwcY+Qs2*smjh4Dq-?9nP@cfWJ1RpQ}11(d<;I_dy z=0xT1o7PL>CL5&>6lU(ED4rv>V^&cs_=;}tOFQ`p;V3D^?2UugCi_vrOzZJwZZX{# zNm&t6$6Mo?{4pdO@hUV-WOUiy>(luc>3b*`F^`x>wJe3|?7ZtHWfLcC;uPPGv@Vs1>hH`|hPD&4wcf z?cD~`01#n6P3We6vp}0%Af2p8Enk=8G1kZtgV~pEu8y@78W$<#(o z>29kN?!O!lG`MUb-+AHPCHTcNcxoI!kyfeM1l2jd;f>i zHUHF6UA zy^5v3P>cH3F%0_Cbg-R(yys5fH{(jM(Ix*m%r|U2Kb96)CC6xi>60NvmFJ1jY4-lm zq-Hnf+#Ws?Fra_BtL@=&FH%G=R!1mmdfCzTOn0jeH4PP691AD$1O@K%&Z%`CA zWMa6MEO}MUuBM{hwxz4>;jO1;l`G*hRr=yEBW_VwdH2HD>%}r@zBg^wG{@S^-GPgw zO`Wl@b<4-7!PDAU6>AG79U4&Av9GUM0>LCP%g}x_>PwDiRUbURllSNNUV!8qN4%ls zi2Jqawdnz)!wu=^l*KtP0UT?3m_-?v^xbz*o%aF6sEd#{^gN8L9a;75a>X?eef*eA z@fcf`tK|YzZz1v zD*K89ml{m!$MXj4R9cc$S`jP~q;>hsPn%L(P@RgWxMGAe*h zwklPsa;ak3#fhuOfhc>WGP5d;9HR=6T9hz_B#4M=Wx} zCZFn@%o?&E^WSmm66r$P`fF9H!g2j_RVrDX$~0$Uu6VODX)1Ju4=DyZHEGrZ=}P5u za^3&73yah&T3$}MOQoggB-MIrQlHHhDwXZaq@3uUBO_5a?ugZ3?z5bY5qny4;7I?w zR^ajy*xF|ZK~lg#gwYc9Mj*s1%^8+z)1QZ*psAXihsU1EmiMu5hkzA1C~p*=KShddbqo6g59!}W z(b@wW8uBup&vG?T-$1Rv@Aw=aV0m=k8Jo${${2C-+k16)XEOBl75CLn0;VGhD}K#X zoxoNW$)u{U10WGBf(~n7eU$0sJjU4BSsP{~sn_7Kslu?cAMn$)E-p`u@z#rs^K>s>)^m=JjcR7x-|_NXg_?$&BCZr6xrhTU6o~KH+L`43Ur^dX?Vu# zUKHx)zlNQ`Jb;bXyLs5f-myV3=EU`sd8&FJUTXd0C39e8RF+K%E=$ZNlR;;GKHH5H zi9}@diisT_=N1$U(P}iXEqkbg&*aetx8-(gJfDS#i{2XVG$(%U#}eDWUunBJkLfhn zYT3~L;)`L@1AZ)Kaz*lsD`$KJ>%>)y2p6c>wsU|J~5N}Cr57w3YW&qs2+)}Q!Ll^1$R6qDlw z<`b`~vo!qS{oFG#nU9&czZ^Br5(RYWs=qyvz@5oPIeF%8;ZceS+xLJX+j_X>_}ic! z%4*sj@Dl=DW7x}+h#6a~E+&Q?Fb||@6c|){M16o+gmCESNZXs)`qT$K(NlfvWHI~g zxUxNvgd-jNe=0@6!=>_XpfEM7ee+&tvOnP{7hQ@3uQmryGCQXwWR&6Bz* z+`jzn3c%O5ii094v{#Q-4)1Q1jixGHB`yMa(ZDvMLXK^L3Ue*;z%C_ELme#ZTV#}u zd@4epKv=%gD9|WMBfT{+-p=3OrH$T5SNZcWUH0@o+(d#7_GN;CKJqd3OdqY0d40a; zjq9RNt=n|eE)CQhU@`bjcRTm5TM`T-Wmcrxyl#Ydup;cAmlE91FP-2$FJ_m-f~M}Uw_s>9LU?^L}wq^gzPDQ4EY;50zMkThpWNGGvKH)(&g=l;6V#(pfdYyuDwgGc3x)A4y`nesY_D86 zmom(?ix=kg3uA|k3-6s?tLLrhkfn5dNaI#dLd^oqDv{1Q?Jcl8V{5m_q+t8#==$6} z@9MN?1mZ%+quh?&RQLMGV|^8o10KIt#*IPpp*IIl;$B5lw=Gnu?9%X~LM;uc$|J;^bgXz3 z81fCez3nLj37hY(CT>)kzeLmIh&KQGT-Pt^+odh>SV7#^()*sgb!J&4oS!NgMz~u7 zO&kIdvD6oc(U!Q+$2b##J4nk3GNE&zccYJyZ+y_(k0}SLHkb;8uZ3NG)ArA^QqdlY zAl?CWH&=fa#CI}?6dMA6l&W;m)8L4p8FK1x7fbx@(F|#&<`KLt7|S1?m%;Npx2bOhGRF^_PBt6mf0)M zn+y!n_C0?e>#jk#BpR(JxK><*M#}*)4^^s0UBtwbqT$LzHC_J3Zrj%9ZTs%1vTfy9 zn1#VLo?KvgtRrDc0_DRqd7AWXSkdiZPmO`_vAewEI*VOg?}DOTj9u<1rdO$%Kfo7+ z&BuuBe>szW0dS(&OdK388Shi-ZO^3w1fByjC>#A}K5&Y}Dj&ued~cKTg?%|1uw=%; zh|Ln-~CEd4!x_6-+Ctx7`$&f2rSWiI~HCg z=kS{5Fmx9(PsY5KAU@&PIj!GV=8&mC1Y$Ct*dQ)E=#Ux+v#wIswt zZ67j>_IELX-I4cyh@>zw{*A^P6`a?U;jXN%9?WdK!r*(c(HrATUp{{n%8s3t(Qm~p zRV|sbQT(#{C{!SwQ832%qdXM&Ro4oxJ&S;tr16KRm^+>>w|knzS>wM7(QF5Hi&`bd z0!FM_QVWu$zi9uU*Hu$#lnt&!*YZ9ZCQ3lGGNRSa;WzADyl&| z`(JgkGf5A6{v>$vNj~hY|SKnnc`(jRn6Fu+wpcn0@zV{MD2D( zQ@+TFCofWXvY1t}4i9qjPBhSzW)*CEe}2+v&2rXDHF%10T-#sbe6;KQMLRNCo+2~J zLoiLX1tYSQPEdt&s{K^Cq#M`}XgCxjvYx>QCqqqjOm(?N2ty4BYjX&DMQX3fYjscB zoqKhY$Q5oKxGQk?B&(vSITAMiokmcfaV$~HPX?`bZsABtDs$R#t;Pi&UH6jv z1sRMf5V47C4_zg)2>aI|2n?3JiL%eFtEWQ-D$Gg5lxp)+s?J4Px`!mhnsvUQxr;=}$QcC}n~UDUuVSDXjfLxSTBwXt z+-ap`seJv?xP}JaN{}ST<3TVt0ou?~)pRs2zkZiCnxx`V`nCWh0}l;qy@OE( zuxBX;yLgEaSDNtH*?p{VxhC!9{paKTA>`N|9#L#bAuZe1ml#b3T!?(MV`S>xal#g2`ljuP?Z+k zVb@7wrG#(70a5div+N_-zs3(DnGO?vT+2^(?D;sOFM5Qjp#*1?M`N3-{x=I0+p~4P zm!UBl+4NCySxyJ&LIu;&`vF&qgC%R}%AJ~Zl(xQ?BxTl5a|4`4i8S7NLVr?pcvTBV z>L6r-e##-MeeVB5)i*|G5;fh%wrx8TXJXs7ohO-0Jh3scZJSSQ+jb_lo!q?dch_C_ zTdP<1sne(b)Q_rjc6IF?N*~vKJaEQoLaSnJ#Z8{h+HB1If_TO?An$&b>?SaQSl5m1aK9u0(6caOV14&L==A{@mIXERF!YUG z8(n^M?P5%*Wg~QGbB3K#qt{8?O+bFJ$GF;K=QIal^%3!cbFdj(oR{&L*p^-1UzE_N z2DwCmR?8l{)#sX7cq6uCHl_ZYJ`w1B78VN?laKM2-jWRhehqU*WBg|?!Py!3U(bF? zP^0*j3_#`?(rjsM?_a50u`J3fq4ik*t{&mD�*dev2vNW1)Q7De*T?FJw&>_72y- z2Rty-28tv67PI@=Pqeq+Y7pcBN1vMY%{ zU7*XZm07d$%^vq}_KJA&-WDBk{N*$(kqxWPNjjzv4%P zkOtKK<7Vzj6VYHyf(*w2L!m6)Y8apnDNZQjZc9336a`XCaTH~dci4QP8d$Z^u9A85 zovC|uKDsF@akRVeXziYkIq3t3hktbn73e-(!n0UPym*|h)MJ)ZayH!4%H#o_2Wbgg zbl_SDRg}EC3efz`2-pq? zS*%dSBaH2*z%=XDw=hgT_KxRr58=#JGAtdFZ3OJK7Jd^d)W)4i&C)F@|76S>fJpin zipD{r`DcRlYL<8PsCX5ZyYxYsOz}FsUQtUs`h2{-9IOjjsHOyv)HLlJlU;qD39U)B zRvtc#3|2Fpd<-)kSFKFt#im*y9Bf?Zg(TmeX1YON016b<9OiJry>R%wyczZ2?4Br| z-z|1Z4eHnQWb@DgolOB2^;zGrYPl(Wd&tC3&N_vB&SsM;@2xn@$XO2zy@xnP3)OLB zH*-^(bHg$RT(7$7Ta54z9lS)RL^0B6a*pPQD8`b<)Sq8ylrQoyCp6L#^nosB5rfF$ zG^dc(96fse<~1DTa(Ud^?f*Q&`2Kl%YPHs$iJ}FKBv&+;0`?<=?f?{UZmm132kTy3 zMbYP2S;r6CX=*;EWCOpLm~x?buk)7=Sz$_3IHV6-#94DCm_{7k4YXRcf5igFn#3?XLik{`XP)<2jBCu9+9XV+Tq-Ega z6w}!>`Ihc@aB8iP+(TUvw)8OCVL?a=d8%~A~^fG(D z#k36t+(Yim1tndd=4Z8EI`2LwLhLU&!BHOA5#}g$IB%1vP%>k3S|uUs6d_cput!Lt zO#F=P1a&3;YPak8yH$y_`CiZ6H=ky&i81e}4lKL*)hptOAliDL>%wGz6u;nt8vK{2 z$*fASz4N}diy>0kH2~HcxA;fI8qAraiR393k{Q2K4A#L99Rr>G)=(V#SNB$|+KY<* zP+Jk*y`Q_x_f#y?RK1au_Ts9b6%+7V269AXtS?;L;x`>BAqR7my#<^?g9l1~0@d_r zJ1J4KIF$w+Aj(V6&H~gI9uT#D85@b~8(KEzSNmo;itDGMC`2`%`DtR@iOqVcgGSz& z+>K4fE7Ly+NxYUTg+#bLd6ckefyFm|Qp$RBp21`<1bg&5fxlwSB($*@raHE9TzVV&gT=ld%|KDN^vCuGBO1Dz5m2jGU!Ri@feX6j#`7|c zYT~mbCulxgaU|=Lm5pjfPy3d`sagfs1GYyL-F>2yknMX4hr67>I{m6oZ8SSFhQ6%T=9cohE*bH(5Mk->+{b(kL zaN6E}y_rtK?@&OP_@{z_ni}FNn1O@z-lOzXS^KE+Q(%^fe|&WjsWC_Y3zhENrr&Li zhxB?Mi)!`PM)=P=+_vrIHGTV%hx!jNVCN^={W_#T2sgXez}%ae;TfMxV>qh&lIkbq z06)TUG&eU*`tMkAFmaeeMpz=@=VDF&uJxYeVRy>>6ZV@5$>j$5M?#YuTrbMtI!*7m zIH<#MIc~41-8XoFb!epJiHF7UC-993NAgU+-Hl1RX9A6Bx3Ts}Y9jf$6gRpwBP|1N z;6z8W7T%B7Vv26b$EL0E*b-u`?e`>bYV+*ryR6Gb4{&nKKmYd};qEZ3Oy)qBBxq6?NM-JS;rL|H!S)~2?+lA#`Y<}_5cFEGtw)a1^vajA>GC4Z}~AIUarkCy!^(H1k45`nsE>-WiN?t|ibD z@X+le~>jmEb|tFOW;@OZGDrbsG3)B7tEVZ@D!Tf30V;;J;|gz0o(3Z?#$$v zYB!#WX0L;)X$VQ7?I#AHiJ_7>Q$*_LnG^W5dEA_8SfowM!Q95jf6LV0Fq1`AI=z}I z8I>b;Q!BE!wd~So3hiFgTL2FG4Hc#i_s+BXcCjhRlaLK1at09$dBA_wu!vT9Jv;Sm z#n&SpZ6xC;?aln~x>$IKcW+$!p_h!)nY(D`|2Qi=*uARZqMc09xY@$cAI-@$9^B5b z{tBj_+_LFIg>-|k{u-G&S_MQ|E|LVBCPNN-`MPvhJT~fy9%?K|JYvQTLik{%R#aUN zcnf}Pu;lCED9Fo-YQlGra1}4ZbdxH6j-=_%pP?@p4981TjP%U$vF%#g@nwF(1OITd z1NnfD;&8LB4wSspb+jDQPYLg=tHGU_Sy}!BP1w;(u)Srft3f;OqD%!lh^-mLq06KxQX`B(Zg@#?bW4^m<-DSdSuae4 z@aG7~Z8Tr8Qrp+{Ovnp=)<=6CP9&4L>|mKp*wL7&IZ-(%nQ6z!ac-wdQx;oEYaX=h zUR2T*CHYXEL-m&xd=xaI4O0ZqZQks_J6`}pyjys%G*zZHMks88t8c*0#rgoNRt?0AwL8(9-YFtE3AKuDQ##FNDTqNY5W?W5G49*!!hDp`;ONNeb|?V zwJ1$P-5Z?|Q6{^NS9*PzG}UHzQi&(C`__?ul@1)-#fry#v#UdwWpi(Tv zYSRJDyf<70(#Buxrd%#gt?iDY+V*Y(fN&_VlIU{GRVv}vR=o04C!i9G9*_?guFKJ z{^A>X{>a*@W*YNQO0yBf*MLwl&QV>6*Dnr|lW6@*rWiY^(?&uYmuWy}@M&|OKkrdd zi?jgaXa@lmZ-&!j)ly>lKUmldL*r`Res2cm2CGSv5f($oWFG3yizpKzJINO6)vdp~ zX+{@@0m2V)S_`OysAh&Kr76kvicmwF-3FT>>CT5i<}99C2m#n_I!&0eyz)d}b0exc zW@6mph-3SCK5$-VAN0riA}MW;Y zs__-_!I{cEL6G~p2-_q5+Hfz$^X1*3g~_SGa$>^%((;A+zGU^0H#WPzdf;O=h$5N_ z3hj34A|Q|Hsl`pwhQh`h*vf|sYMU^iT39`RK#&Dam>#tcNHP%0Unys#ATI#Xpg9o| zUp^gaR0tV1OA0xOF;K`e_&43}sho|_j`7$QJ^@JY9 zHbyazKFu~p2a!%?OPrw%{4h{!X7e4TCbMnoL76eZACd6~9Sn18HPgcUlM?g#IyfR? z&B#Tw4b)UuYJm;yv7sDKO-<^u(pb`sY}O;#jjT36hdX3+kC1|m+8c3qjI*Uk9mFhGi$7{CA_D+C zaz6ejgC&PWgTIoZ^;2T&Fn9Q5s)VrAfT2 zo-G3TVp3klNCmIn+FygB=aI7!!Pkw8`{RXx`XXVjt{-07E(k+o`y$@_D0Qa;mDuYi zKZQNisgK6yDSbGEs!kg#Lnk(ks-7J%>ogv6MEj&qY_adV2+iSihJKD5f?JBexl zd-T@Cw|b)PSrvGH{LAycdz4^TCOo5v^qoG{%#3157e@PZpXq$hRd1UM${pKOlhp-c z9(bE6WK~#^$V+4z<07=u{Ms^+WYFdP;Q(U%Ia6^nkh_Ug@}WYD=Wes5-e05vRviHc4@akU{(tFtNXwfRC$`j`jzqH2 zL`d_VSk*|odM~96@D4QY%zb!F_dNd#LEf7i2-r;U9U5?`*3_o~*UO9!8GC!R(D$Ri zA;=gUDz55}Ud>8YL~`WLk#1c~`X#g?okAtI73D37Bbms~#fz|Jzj$7K`6R6JS^GSV z@}8Yxz;)!K4_1~lIHj9f)4+!QVUWlmn(%s6wwTNzP8YR;V%;xE`wiv#*T~q!@?UcCEr{%Up1VoCh0u_wujd(% zzenS4?_1fS8l~-;cOwz6c)s>_sb_ljbzAOXIa(aApLMbq;hY_?IfvLcAoZ&11bYC< zmi3Nz^9$#vz$UxhEPGghU^SbeXZxyAGCrwaL%q+Pyf~2@nke@}iK&UJEde05Fzv>c z1U{Jgu7XA6wx4IJ5XelS{^Xu3Ffpzx{%g!RpOd~*(SA91U$dVF6Yj{yDUJx_f|1}J zbOmEyA30x74s;e@1)XpA4jAHJ($keK8FV=J%<#g~>RWiLH02E#zMTR)XP0BMRj3)u zuI~c@ZPm-6Rmg;}Z!JQCYhyQ6_?Eu+6-GQ7L>;dC=gR~1b+;`|_?Cii9Nk!*%&4v3 zlh{AsAdf!l%vUZ6*M#|&r*b~1K3Z=Yj;4y!3JgO^QCa=LkYrl0{sZ$I(-syF5?_J6 zoY2;{nH?n)D9hS;?A~>fD*8K*j0GyB>c|WkOw6tzMIUR59{LwBVgM#@iH`BGU6j4A z=s~=LFc4Bqw~;N14=xKbUS_UM2pSe?jNN)M0!3U?;ODP0^qR)Wo(Z1EbJUo+)m1e=_?)6;}rK2rECrI*Q>} zMjQXYGYXX?rGbLPD*pr0YBj=R5*wuo4Uiy^eyBG8AY+COt`1?QYBb6m?N2nK)(OW= zf*VH*g;$|0r(_$oUrBDZ{~<^C)uHP%`Z5+fvtWw+#SQD8D%YM2p0`c_qc8953odl* z|D!OO+!Q*HcVxw;UlUpcnNC9SsNWD&Pbd=+k`tFn6 zs#%c!#5JbtZ|--Ws@?J9hBmb)QYc!qBoT1iU`;Wy@HHa#m)X@K4;kKaskP?k^9>Sf zE-9VwI7({$hUTllNMYYHjRjUDFX=+-j|#Z1f)Bq#Q%M68&hh+WVQq~33ldH_s>lI< z=LK((1B3s~J2 zBQi)yS_xt_$Z1*`;M?k9e1ns^U+iOii*KpGZ#e+lO$LW7eMkw_MY^xS2k_8TD@&Xm z^f5xm^I7A!m`7(zmnY?6cFVUEVUUt%dQQGcFDQ8XH+ z4II$nF%T~{AfNcjcNO%nX0PQgzItE-kXE?IT@+slq@u�HT492Eo;)mk401{p7SHY)0z9?p3I$e|?(~)1Kswr7narzz zm0VM*q5Y({t$?5i12>vl&)ysAiG6P7u{!+jfgVNvaJcjoYmfB&m9xazZOIOb!6^naK%Gx`fJjS0taol>ZhAUFgAajLMS!k@*Ui|^X3m-jdHR&SF&aWwfIk}_HrBhZMN+VjJ`{;v6@(BD zW#{MJJTaz@cs@LB_B!Vr&o^lwPo%SC)c`vk2f}!*8A0K_M`_P?-7UpnTu)V0cwTv? z6S@;Y;Za*{&n%>+vC=oe@WD$6x}>6geZb$@T-4(D=p*WykI_&zui3`MvjgPpw)xdY zD}mLjPOTlvPP=rDWD9?6d0+qAPqgl>N-;ff(P0Cl5)lFJL)rBtrt?l$vQN8ef! zUd);k;Tvv@;+E_$RZ4+Qvf|$*pan|y$^ED3XUxS)>#>(SvMjMtA^D2E%)}Qjf+6!;kyQ>N~gYQtXjNtvOZOL)Sc7!Z2%7k z>uX?se;5arOpz$XADkj=J$Db4$sQ=WjJWHrKBmL|t3^-T#%Jsw%Vv8}V)X7b^XSGHbRD@E;R}G8(Yw}r z>xt{^fAZ=8anHS<{fBuIhk1W1Klg~n8Yuw9lcH(=@4EWT`}_UB>T@+>8wA#$I&ork zhVvJ>g#{C$nVDsfv%4^fADIVyg|uf<2egRzwdXvDe{4Y^7l;=V`76SSmJ<1^V2PJ| zK-9<$x+|W+_MtD%JX=JUO$dpuC=5H#*$vewcr9sX^}1&0iM+iHZ$?N!$dFIRh%*O< z#q3j$VmVo{AJeJ^D^N!$_WC*eIa-gYsV_Vg6%KQ&Pog80@CThgYD@>$$RcDu;!akX zh?_U6pPEhXrX)`fwma^^rEV}uf$MNFIvSB103g@MMG^V8S;N`I=hVKb598FXl0ta} z^>^Lm`H?Jd#SkWp(IjOL=GKK=ml-UGABZ)Yp)7g{E2aA0X5=s2E!)`aaimOu+eP=! zlhx0b()~A2x)Z39EhNPO6b&t|0~ig@vB{1$;jFkPbe5OrAN$&PZ(KYhq)Icz@(Af3 zdqyNcMM#~tY^53MaSe5ofYfmSOooEF-y{&Dwq-lv#13=gLIsWdxCT4m^Kl9Lei#Mz z$FZT;&XtY3-lfepLw*fPj|ODI2Hv&Npna!PtqCN=bld)%XI8{azJXg*0A@d^s>^d0 z2OKZLkx8|bISFY#T^1Dbp`SP4xuz(Vx5t(j*F4D%a7nnRXFyc0l{w+JlbIt4LgB4o zJ_v5+%KRUpL&v{u(BGHroF81fVXL_C1FN^pay&mQYG`5nx`7l_N9I11nC26)d4$?V zcC!_K()n$+MCNvk7RN#^zVLM)9&fs|$yyCh^OqQkBPb202lZRO;*#2A{w}V{@;6jJ!nypKFqDbP8$&jrfHL0Y~fyd*HK-?Jy zQ=t-{m`~uc-IX~s_$yoBo{bm99+1h2D@Ld?QvR8P)K>gQ3B##EU5|34lrD{vTK?=3 zM2$Lm>obZj*yuPI3}!+H`>7pm(>U2wbJnU>xonv}k-978Jr*>W zHc}@%7;n;iJA-E>T(3G*EslvSdne^SqBuN5v`#%vrZHurW(fsFDS>H=k=L%*N}mYB zFkk97i?qf&uaO6LpN#A#V>C&*AE}=3KTS~eB3R3DYh73it*F$<$Z?({$^@=@95oHp z`wEOCO071sAoG`#{I+l8teiI8Mq6ys#npH>EXw`E_T^`)h&q1NjeS46(;r*#xfMV5CD=kz zSo1DWYli74#=l`5--F1t#oi+P*de}q1KiW^RWn@>QlhtG@9=)?o|Ln`oFq|nZ_N;T z59Y#noqNwBw(xVuqw&Xy^R6BwQCoXtFB2{`|Kuwv#R`~XT3@P!*`1i}isq~G7A5T$ z!0}-s4uouqmeo_ZDL;>HX|lt}k5`y=ozUj);H^nisQ$mvDS zgg@ptBh5r5>|!(hw@P(5Rv6&>iIK zneM{gzPO_Ww4--l+8JT#0Ezjle=&hvX@u3Xrfsy6yxWp)ZOvu&4}|rS!8DS0)60#3 z507Y;2rV3YV%k>tNSzhhxyD*;dkCQ19#j7o`;FnOi5+aGXz@aGDCCW%kfZ*c5(~w@%6u@D*j;j&$UB=fRM3+fMEUq zQWd|Qjh)Rc>>S+wt5c!-$6-whLG&dM(WCEsyawr*5ky=i0zvYD0h zmPz{f^`YMMbnm21VwJ@qKd1?S|>3xuF*qepS#5Q3zZVM10DFX4-C zR+M*pPUBUIq9(~cLSXE^SartMnQW+7TYU3hy=b1Y(S()QDfSMhnisU9d9fewMy)7v zm%)$jSoLb)dC=Hq!Pg9YHNO~>tF_=Mv0z`T@)p}Dz#j3>8nlD?aCDSdB)3B(T*;gy zWEUv}7>i6#A@OALAZz~gXC<<4HHe=IF|$Kx5}O`MehfS+!ETt5C%SPP)Vim zIRl`$X58`;MkSmjRu1!w=mWeI=1yRX1K)?uvg?k}z`gUvm7C1yI#NY*O3>?U?x$V( zYsLA^dIz5j<*DHLJZG;KphL9T@JAD2XXAUC4w}CC!L1?e-#pg6vW-f8m&+jNbHPf1 zY|k+737CfrKw~g{uokXh&F+~Ec0Vdn_>sm*KwD%V1z`9KFUmR#eQJ+@;hc9I=|XO0?h9Cq|;tgn-FAlf!W1 z#6_6gDps8asm{v3{X(rl@|f1T)su(GUXRJji_^@vErP4cxm$1BR(^b}iGo}1C3Po; z-)Z6tMB!{w#Y7(wY)nv-Q|9~S)|^w&hi_W|1o4$a0okErET^eEz*prWw;!T0xV+wD z5*{$JQ2!UY+n6R;%{tfoNM!v- zV%+kHz^{>{B~qXxcB;KW`jvst0>5<^dDj6H$0Qv@Qy^SlcQbw8e0PE@^%U-@2%LxX zqO;OM5t%)mns~xJf&IOc3T@8YwmI3O>p4BN%{{sTeIUDSF)2}J%8&d+sgKE zMh#bp6ni-|^*VWs3hKA_Zga$jf82Ho3h)Bx=c}`yj!UtdccspB&Wy4J8R_HLPv>-z zVe%ywD6Su=fgnOy^)vi^qYvUF2h3lEKSoj@P*x#>{=0a(Wd#aVr%S$(G14}#4+8}Ehzl6G*?eBt9cY@XL3T6LKz z#X%T+?*2Q%%DX$l((=85*w94hv{9!A(&|N~?;K1Yt zzgZ7;&L77x=Agys@WZ#@WC&F6#%#o~Wd|xEk|6ZwerYPolswKJbd?MW$^pl`hGw$Y zB1&Lv;;<@-M*QfZXfqSZR6!WzQmg?O3?sJFYlQDce;DGRGgssmfX?@2LVC)J`m96*I7HKSUz>)?S7|=O5YjWEQ>S0-E*Y z5iv66n|XnHAz|6ncd-O0DP`)Y_h33|ZUwWQbG_)EbXM1~2{&w0#@5n&7q5XXlu`Ki zvp|)O%GOSJ?uFAB#IRXB!~v0}+sPW7K>?Ap830Es8|!xM4zyp%iyhdTtG867p2dFh z(zEyp-k&|51-qB2;Y_(e?4l)7V^nlSbli=jYSLMJZdYX_SUeJ?z*>RhG2gnF?K?u< z9ATFaj@HlyKEWR2gEOdfVnTi3o?Aovv7np^%mKTp?D1kr(ili6kUpq(?&^@Ju{kLwI%w*+<6sx-CB)pb6dLlS-{N z0rzWuYKaRw_l?`b?MZ8NF9IZT z5Rfm5|JjlKKj}Vat8aY@fU&h5lOw?L!t+QKU17!EO4#$NlNKf{Oo-(^S|(vjlo^zi zgb+4ND3MA11ZIx(9x0EUQ22zAy(|x?RNei?ywS>b73y}x^Ol*Fwd#GCnf6gXO-;zi z@x#R8Td_>2IPY7U!jR}08QJga&}l!S(l>lWbaMb6ge30;ncL3+_b&(twUDT`@HVrl zAlppLrQ5tW^u1oHe?pi_R)Q=M&o#c`otsdde5O2Clo#>x@+71e*T`g9)ceF}yUpEAU~4@e<+5i(*vh4NH|>J4S(ybdJ$g6i z*`C;|nL~~J+O3)MbDQ_F$t$fNrXlEdu|1yDYM^<(Pj1f$uFMPSk?-gi8!Y1*(QN`1 zF`S?13tLm$*^_c={G7^4hps5VoHAtgFVop2lRN3L*+6%JX-VOI*}7g;Rl=q6nK5?w zue+Z|vsDl-?{xe8xD?&a=XjR4<5BCBpU)?P^?72>02!3UXMYK6qx_78bq&LaK-Y87 z@_NB&^6DSx^!z@YS(=0itmK1+4VsdYEY@-{@wX$A&`PBXS3Uo@M##j)|Bp(pCZ!KIJLn2wInkqCxUZ@w;gSG z%+%R;CoNnO7{2*5Qm5?^!_`el0uVkBhk|R~LCj042jvG{jM7JV5z=!_I4b=GSw9q} zcvgsCuuOk*#Mwxtl@pGe2Z*GQJ+11p>8|&b!^E}SFnfAie+)#pkvax8jn7d$-ch~i z1d7ryN@HZtL^D*08RcR<%h__I_j!boc|xVHI^n0Nxnfs!!W z)Os7`3c0)evWe(?)aj=`CaO*C5jLM<8yx(*cH7?36ljfrXLObeo(I_FSeW?txpW}b zt2Sz#7~(h|j3wKs{|;`skB@)gKXLti24%n3I{+~n!f(yS)@)nC_>+SQ>L>(w{10H* zYyat&DqxJzph%`(jAeMr0))e@ztlZ?ab-N7p5N#2+Naxp&c~YvC{5yT`+`rFZFkl z(09?MGacU9UY20~AgijNB^YaUqY&jYgCkcV-ztPUNoDKzajl^j+o#6?RKa+Ii{1vv z`P;P?Wz*-1$2|lcE<1(eWke&V^=iUo^&Dt=of={8Kf4QwXx)EKS5=5r~3iUANLia{)Y< zr+uV)5j@E~e=dK^Q3!mIAhBNrytL*0bnXefTRmH{w7fXX*n>B^1hnIx6qb-Rx{6gO z94xGcw}8N|voPqsf*xA;?3Pe}UTe~X0(rplB@SXAz3@oF?Y z$63F)8&moIsdX3&bTcjBT95N~n{FX$9ITKIg}R7DCH(a0=oq{Ei;0RzQ=p>*$Z3`! zhdy&R`7K)0CdV3Ogj@QztfLIYy5yVQ1ZLMFUUG(z)~(Af zYNUw#FY-WzKU&Ttr%@P@-*_qdKfJ`OwsVJO{uWLrK}ns7uSZuhw)vTRi^2Hy;5WP< z2fw3IE}5ZDUkGJPDtwD}hUVrI0Acv(X$VDLuS)60j&*iZO(+mYECRbaMDTl`VtAay zi}a6`55LvCISCb@1jTY^rhM^)a3r>jQf$Ahu0~uwJ#zVaHJkrTD-UnBmvp-&46Vu> zEg#pnz}_BeWeA@Yjo#ke`X!-{-c6F^z-Jfp4j5Neb5g&IBGI75WRpwbnYJL#8@Tsd z`*a(KEFS@$N?hw5@3qz=0m9q26G|w(Um9r zi-7(6FgzuLrD$e;SA+^X3~$IT5Db3Er)YwiujvZ6YH?$5v+VaCIX>88{nNHX`dT;_ zb0Lwn4j$47n9)j~B2J>dyQ}|qyy|F}r5@1X=T|Qgg0T;>%X~*;0{C>9E>C&HxM#-@ zyOEuz-Gj0CFb?ZLkcYc@qE#y0mI&Wf+9L~X`OP(_1~ zIaBa+_u4RR``kiXI)0@&^|i_SI9VP97QOqdV^cHYEG*nCG*Fos_fJQ@K>b;QjyV_W zwXr3Z`o6jFsNVtGng*touuo*;lRs8no6R5Z>)`;8Fm4{J(M z`Q&)LeH)J4ORPqyw-L0c_b)@#O=*kB`31&!oh@&H61T;?d-o+Sr?zkv5X)AD&}ZD} z>Auk#@KaW0&nsq_{#r7k9kdDYGx@i!mEGXs$01udxeU*spBh%t#fym#uPHf??n zkyT;?k0|FI@^#V#DV<{Jc| zNQiXqQcco&q!nQP`A3OGoqU72{d6~1A5IU+?*L`yo)X#)cQuxl9d2zG$7`FCt!MTv z7d5~-|F{0ibQKiX8nD&_t3n`AP?QJi9W$du6I#{outf#$!LUvc9d}X}?dY$g9RE(t z^1wst(MDkq(j%WhJ_EUZVQ+&=h;~!HXd-EQV)ov!lR#r>k$yT*6vI6hZG2&x zhqQBBq}I_cOKZ)HItB}{-*R(83Ped05P#h*DV`ME2a@0+`4Qk+@ed>vVhJ+nX)oXL zVrnVN?S3=3+ptm3t%6vJG_TN)#s@-G2=I2l-sf4H*SC+sT0oEj+!DlBh0u1{H=Ibj zQy>GGIXf|Nktc#j3EkjHSbxnZ_N@<@5OubWL9q=-(7rE?WeMVmaMby`y=*!0i|KN~ zvvO#<1<^!gRIj(2gp2~Mw_HEm>4VOR&F9_hD1o3V;y$Qq!7m*Lep zQZ1jUgryFI#zE?Z_lTJB>U(EM)+uC_=P_6O;h_sJ#64N6O$O*L1IAgOtyp>K?|cfF z_bdjzDL*bopeX9ZWN0Xgt!WpRZ^z$<7Qh3$H#09$AqT`E>O}HJpr?B-$C~t>N}>y_ z2$?)}!Hgujh}4(H|F)Pg&)f9BUfw~tKz9AHEfIn^{=EN3bY*d)OijMEGIcb`nXDnM zPZ6ji*38~Esf^v49Na)4wsl&Nd6s$?d(fc##@SUbO86KYc`JZ9<>dH$Uj&XS)OhYW zNAq6zcjDILN#Q!sl>w#abnRh_ot^_s+tXe4*L@vBzXz}h2^-GUB@4US9n0PB0>B|< zf-iP>+Ps_)-Y?U$r=D$4HZHRv5>Nf5j3ZLRkoKl6?Cz`A?Ef79F95f*Pf(#AG`gU= zj1zVkxK+a9ot{|s{>I^H?3{!=oQ9`DveYKd;Xdr^5!+>ix920v%VvHRSC(|vQvZ*> zi}y3n{!gdes<~jj**?SBD8uH5$9ht8HFMw%W2RA7xbrG`|11nN&Bf`Ltu2IN&AF#! z7#X_KnvAr@yn#ur-RN4^dSk@ngW5lTqWjWVh_2y|tL^cBT&^n7`llgPC(OwJHK|E< z0_V5QM;VSZyZr-Pjgjuc;_k}S1>mYBV%{euPf+T0wfOx~!OkkpSi_Q8lIVUNyfG^I3e=v9(AGT57}np+VFp zI!BMPyox2~9FMf*emqR{8Wkb(>|oyA*2&cZJf8SJQ^xPw$UBvDG5M4S*LJx1oN8PGe4jra2@C7gnn8RX~Bf6;pcZ`T9 zC?4Db%v)7`CaWG-6bD9`VT6n}Z zvbG~l{c;i*^HNURGN#kY=6^ZDwrBHkDg;^8(tQL&F_m_Fey@|Q0>SqW>OZKKqR19@ z6_M@Rj&$Usu#>t)RSN)w{?&c`+ z);ZWjDd>ub3+|QZ_CX~{xh8Av=vrE(nTUIWr^p~}yG!8QSBK}G65 zYBxNG3t5&#y>Qh4Ql++`Tt$kKjr~+|pAF{j>WKVTv;1S5q2hPfYfCOj;M}n;npzmr z=EJ0g+j4>nNbe~`rT>6SoPEco66TFK_2Ap=i-fg+F&_|3^<)~JS;wz zrM8dF-d)g=B(=+TQCy-j)92{BfT~-}AbMjBfQ3 z=c^vBnKWd|pQ+9HA%y=^$XQ24*{utFKpK>8L6B}q>68>4Mg*jDU;u}a7(z-K2`TAL zL6Ghc5b4gLTe^gy>x}n1>zwns&b@cP>s{~Kd#&fU_Vetu|NFg9I{FsI$gb%Rk!ZMG zm*1*2`xU#<{<=1z7eIF%tc)RwT%j&VeT`c?pZBo#fPRU1X+r1fCVhbIFzfKu%aXV6 zyi7Rthc@GRAoSy}zo7c+ihAH07f*bf{ngv>6^EFRu59m^vYz{H#~H&u+?6XkT+E2k z%{|Rxi_{jS7*aNUjW`bJK8$}LW`-0C-IHqTH;K+-pD7j!ue*9#foRjq{wTe0G#yKq z`f`)Df6akif!IjtQ2I^{Qn9-B+WNk<(|hX59x!X^pr9Gl4Ef%`G+{m`Xe=YTvHnAR zJuIHWnFrjh^NSoL5SR1dJjR4`wuxu0?_#c#_O66Qr}1{i0G`fjTXff4^wElopx{dK zKwZcijcMxR`VwQoIohg^S#unYLDixUEne6(9;d zz0`hKhwXU>Dn4jYYY>6foFo;)q!maMyC;-6Q`k#b!VR5?S=bgKwdb!3h16?t5=P+z zF8$;!NSz|~_PSbJ`Pyn?AzK|9bh|RbyCWs`8odF%++ecjGZ0)BWW!Lbp$&)}htaak zm9|?9lG4APLt&O?Ioz2wK3kYHrfcgo=lHoo4=tgziO>60JI!M(4TPd! zqZdq-*CI=WGA}e|vf4gH)VMm{ffm&#N4n2XCB=byR%rdY{AC{ZwMAIfN7h%bD=Z&W^Az7!(<&xcrV%6D(4CXeLkGKz zN!W;Q!jc4vQ=3t!8y}=55S|3adh_G8^sSR>BA?C4i0U1WgW1OUWOn31iFeE4R&iZ2 zhQC<6^v+}wa75xpn-cS@0`nQe5FA&WiaL6pnXOvlWM$a=o!uIKC-PvvyFG9>qTy-R z%1m97Cf9bAk^-$ao&>>Ero|N2u0L z$i9A$m+*uP(QG(!On{lEC&2QgMtLCM-V@wgg?Wy+^}GhHzUH;A^q*mv)*~1K3W8dJ z>{f1beAc8=_{YO^4sfwmVFM1N4Cs*{OhB1BZGE5$Q;GiwL9yA8xFuvp%;PtLf7@%< z+}AeFmG%y+H}g|ve-97w&#sHryp||S(xc17(sf`9Ap%XdxP2TO9SO&`mjfnaNK@+j zoi9HJiS;*4fonboT7W=3SK`kzp~bVHZPBa)3u^ldy~*^}tzj$$aeriI!?!A<_eRop zjosfD@ujE1D;GfAQq`Np1hNZL71sDw)Q|iLWt@W1As7C_HiW9lSNZgP`hQaatK*n3qVJx#)v>GBBQIo6d=o^=U*(_$SkbD-{ z;U(Z_pIRa2S|1hl%OSu@xi2ZacwAMHjl50KZ%)o9AU`5vqNyDuc{1Wp`xZTWGOv?` zml~HoBJsu54d_!s2kD$Plj!6<6_PG~vO_K?1b8OzWx#X^5}rBf0;Trhl-SUVs4f*j z5m5%cbM6KjEp+h|XA;H7H8Cq*(~(ts65fmfJ6kVrj63k2`jq9UF$Rxc^W#KO(_>iXo)B`PZFL9R=gsHY?w z|G}>=)1S(gKlHhk9>QB0r}R_lsYOyr0xOj_jp-}aKb7D&48#m2Jpg^lInqv}`6#vL zP>!E|VdEqR&NySEr(cttLNh3E+p&%|B7qRm+jGWfKCX5xYbDq~n$};AV6~(O9ZG1A z-D6)BVF+Z}*FdMnXStdsYO=7gGHkt!kMB`lHsoGf{$>^vWK|SPTzV+)JJUc#Pup?I zc<2ZZ03s|(yn zDq+KqMX+Lgg%0}`$Mww~m0iKmIe8P-?8Q&z)PI>*eddQy3iGvyRF;YsPG=>?w!GlS zdLEr@HFf=B*`)EaGq}9+$$3K2m|VWKvI8+Z_7vN**nK!#;X}nx3}KDsHFxqg0C!)&B2tDeHGf>(K>axEfMM%b>Uez zGx=?A`vR>vm77B{wdN+Ry1A+N=b5_eR-QqN)XC!W+-!huNSBoIkc*|n{NfE*NBXqt za>U(}pqtremtle}6S^_Dl^&#VJ<~y-dCH7lD*w{^ztB?nbEh^ z4KIq)A-~H3cz?kO5-}YnEkqH|gF{jOOn{!J&Eg zUx6{tHCm*ac52r2rXIvOi7r-2(F_=Jct3v0J10-t^;t3PvtTR2=3C3FfHn>19SG)S zC+l-bZYDCwbIw{cLR4b7f*=E%w+E>1_m$n8{e$);YXQ(r^My z4lx%)?v*|Zc>^l<3!@onX6GPlSKNT!LVz+VP^qI}75sJqL&oDU?_{9H zWuwVk&35}NpOr(|Z3I)JT$&#VoCfahy(&+Kv~Dfl-MSej%ub^Zop~G2&!vW+u+i#y zziZ~uiYX3perWMDxTPfptb3m&LRi)Ab(;@@VHEC3sx#~+WVRSmYz{AN`r+>A#E?G; zr1g((^tNxPEF#@yzVK6OQYp#~v*5V*OD#-H?Zej?CtQplxhnb+iIPWV*fUihwjW`T zGy=h0M*a^X#r&93RA&?lLSfc3Lp;WP>BCmYGS~auEo|CFQwNW6okx}#gqrjiUkx3R zfUT^82JScR&i89=v>qzwy})v?g7GzX7Dw_uI^e;SAvz0+!_2(MU+R(@0l8U8Tc`R| zkOz;`XM?hu1hJF1lXo*79<(_Bawxmu!e6+Eu+~5ZQwE0VWmJ3M65+!hCsMp?bL8`N z>4r@6r*bk6?cii9VlVhGd>Y<8-Y`&alprxMliKX#%v?IU^_Q5qQ&VmVP2P(G4vG+F zEE+d2EN6#~YIMgi@ub~HGr|<9Oa5fm6KrKYkiIBF$8nPAcBx&MJ{BvRyz(|@BBc1~ zCGYZ=w{#8-mhc*Y`F)AXbF z9{FZJw#YiRPjkW^A~>SzGdLH*mayg$cLi9W>)C4X*>dig1Y)dtiL3K3Bt!^?crx85 z9-N*h^~2ZlQ*G2#lhfqE@w=}HZnmhN(OBn^I?=jGBliiaU@mtT%}Fr(R=;0rR`A`l? zl^+@^eZx-ybStU7i1@MG^!x3BQyFF2(jG)@Dag>$#L+f;7?Nl43%BZr8e>lY9K^Ea6L2I_vgU3lZ-t z4fqFxZApxaSv=Z<+P=+|ADE7S`^&ct_)bIRL_e@F9?bJO&hfAHdd=VoG)m2b%+8t* zQVE6{YIw{=IQ!lmu!@pFMJB`T(8V4LJ(o~ro|P82rpyw5woc^ay24vnH@7k0B{0&uG1o-zJ|Lk0t|2icxx4`NwO8mB z(uvTUc^=pM-@6RgCG1UU?4!gL4JW5PAUShmSFvv6g&Xn9P{o3y;U+um3nLYv>9_Od42otiZ=|_>@htyU!GL93rBwgHtsg(G!z6^b4?Iu#)!^##_D}#$S{cq zC%4~V!S~$*$z_S_X}&y5z_)`{K`n|P&-|%_1w>+`VL6WVf*&QhDgl9=6w z`rpXL55=a9E(xI##2r;8KqKC=M$RrgTQjC;N-!t{tvJ`&W#Rs`A-`{aIAg9Hs4wGp zEw1}($3L7$$Xy%Lh|b5yjt_L~urmCt^Gr1~#1O|9y>Kw8}k^c7~B^fMfKO3y{-DL zM>{y!RbmY;E5k0!?K4G}^2_VbcFTIG?h5W5%H+f?-4V(G773qly1Rc!N>1M1+*-ab zRr4y$$e+8m<*7YBQASFNf~v!~&T@NiF`ArbVK%V>P+vt@SXfuUO`w`xmaS7>_I+%l zM+-DHobxGS{#2I#xE6{0P*@mw8QF1R2DqS1`f?`^j9j0sg|%8@1?mKyU|(Qjp57A> zFev{1{kz&CJx5uL+n_^eSXj}u)s2lZ4-=EWvxTZ=*W%Qa=0={q&+UnDSxE_RF=L>h z&MQ3KrC|BVonX?+$_y-Qlhdh?b)a}tR$2O256GH@4Di;wQR?ShvlrtP3Ix3zI3Qb+R< zQ;!DL23;(`N0OfLqHCmLeI1Y{-An(bAA4@@1+|c2xM{jzyPcR^nr>G&`^zMW*^r5iP1RZLvz^)kE)$o}!Y{ zQh&~0kz^0oX2Yw0+5_Q=Ori;MaArj^D(nIxJB9#(Gqm`Sb;d1WSkd8;6vt0_?$j1TOXGs($>eu;L;5+QeVS!j0Q)lvoxgSp*AOTgU*u!%&Uw5`Q=jP^! zZtOygX(iv07>shR-kJOOEaKvUFIpyVQ@?g(5x_WjSud?sK<;9*W*zOVVg3s~U82+@ znUz{=3LL-c9h;)hCmGK}KebBK^V{ZRq(+S~x3*+uW#<`Y;anX00RC>!+*|D2Z9+Ln zY$zWICD8z!Y@qNrHuiRWe@RK^Z(tU7mNsVAj%Lu`!q@*uO5jL2+aFH|6idykV4pbh zK9N|v1oB#Gl%v;^m`0z=nRih{T)9<<{Gb^K)Mwp~8H zK0QQNqV19A$2X`e_Y)f&!&bKS%8@1$rh9DLR;y!7JmLN#SAmzixqdCV;H0wTbvLw1 zK>FjV;G_rEZqm9}`EW-BAHPXkYGKLTes)%@hbPzd&@A*ec_-h$4_7$1l*&@#XP^b; zqx4hrYlZag3kCvFVBHn$7Ud72p#uP9s7!GHa7)VP?p2qA5LE7Ms8w3j>hFP?{&W1x zC$fM!f;AmsZ^4evHejdUmG~3nklpmA6BVY6il+N#_+Owb)aPFysO$fa(xY4!NJJq< zQ7`M?P}!(m{;w!SH{WCOGRZ`*^iYsxyKq$O*=djRUcF}+b1+P_Zy&-4B>%Rd$P z-=x2?a8Zvx>Oikzf6x8_BRKf# diff --git a/toolbox/+WBToolbox/BlockInitialization.m b/toolbox/matlab/+WBToolbox/BlockInitialization.m similarity index 91% rename from toolbox/+WBToolbox/BlockInitialization.m rename to toolbox/matlab/+WBToolbox/BlockInitialization.m index cc335147d..4fc94215a 100644 --- a/toolbox/+WBToolbox/BlockInitialization.m +++ b/toolbox/matlab/+WBToolbox/BlockInitialization.m @@ -47,13 +47,13 @@ if strcmp(configSource,'Mask') % Read the mask - WBTConfig = WBToolbox.Mask2WBToolboxConfig(configBlock); + WBTConfig = WBToolbox.MaskToConfiguration(configBlock); elseif strcmp(configSource,'Workspace') % Get the object name and populate the mask configObjectName = get_param(configBlock,'ConfigObject'); - WBToolbox.WBToolboxConfig2Mask(configBlock, configObjectName); + WBToolbox.ConfigurationToMask(configBlock, configObjectName); % Read the mask - WBTConfig = WBToolbox.Mask2WBToolboxConfig(configBlock); + WBTConfig = WBToolbox.MaskToConfiguration(configBlock); else error('[%s::Initialization] Read ConfigSource from Config block not recognized', char(currentBlock)); end diff --git a/toolbox/+WBToolbox/WBToolboxConfig.m b/toolbox/matlab/+WBToolbox/Configuration.m similarity index 97% rename from toolbox/+WBToolbox/WBToolboxConfig.m rename to toolbox/matlab/+WBToolbox/Configuration.m index 20374e498..6bb679ec4 100644 --- a/toolbox/+WBToolbox/WBToolboxConfig.m +++ b/toolbox/matlab/+WBToolbox/Configuration.m @@ -1,5 +1,5 @@ -classdef WBToolboxConfig < matlab.mixin.Copyable - %WBTOOLBOXCONFIG Summary of this class goes here +classdef Configuration < matlab.mixin.Copyable + %CONFIGURATION Summary of this class goes here % Detailed explanation goes here % PROPERTIES @@ -116,7 +116,7 @@ end cellArraySerialized = '{'; for i=1:length(cellArray) - cellArraySerialized = strcat(cellArraySerialized,"'",cellArray{i},"'"); + cellArraySerialized = strcat(cellArraySerialized,'''',cellArray{i},''''); if i ~= length(cellArray) cellArraySerialized = strcat(cellArraySerialized,','); end diff --git a/toolbox/+WBToolbox/WBToolboxConfig2Mask.m b/toolbox/matlab/+WBToolbox/ConfigurationToMask.m similarity index 85% rename from toolbox/+WBToolbox/WBToolboxConfig2Mask.m rename to toolbox/matlab/+WBToolbox/ConfigurationToMask.m index 1e7e3b227..433b38f04 100644 --- a/toolbox/+WBToolbox/WBToolboxConfig2Mask.m +++ b/toolbox/matlab/+WBToolbox/ConfigurationToMask.m @@ -1,5 +1,5 @@ -function WBToolboxConfig2Mask(configBlock, WBTConfigObjectName) -%WBTOOLBOXCONFIG2MASK Summary of this function goes here +function ConfigurationToMask(configBlock, WBTConfigObjectName) +%CONFIGURATIONTOMASK Summary of this function goes here % Detailed explanation goes here % Assign names to labels @@ -16,7 +16,7 @@ function WBToolboxConfig2Mask(configBlock, WBTConfigObjectName) try mask = Simulink.Mask.get(configBlock); catch - error('[WBToolboxConfig2Mask] Impossible to gather Config Block: %s', configBlock); + error('[ConfigurationToMask] Impossible to gather Config Block: %s', configBlock); end @@ -30,11 +30,11 @@ function WBToolboxConfig2Mask(configBlock, WBTConfigObjectName) WBTConfigHandle = evalin('base', WBTConfigObjectName); % Get the handle WBTConfigObject = copy(WBTConfigHandle); % Shallow copy the handle catch - error('[WBToolboxConfig2Mask] Failed to get %s object from the workspace', WBTConfigObjectName); + error('[ConfigurationToMask] Failed to get %s object from the workspace', WBTConfigObjectName); end try - if (isa(WBTConfigObject,'WBToolbox.WBToolboxConfig') && WBTConfigObject.ValidConfiguration) + if (isa(WBTConfigObject,'WBToolbox.Configuration') && WBTConfigObject.ValidConfiguration) SetParamFromReadOnly(... configBlock,'RobotName',... strcat('''',WBTConfigObject.RobotName,''''),... @@ -55,7 +55,7 @@ function WBToolboxConfig2Mask(configBlock, WBTConfigObjectName) configBlock,'LocalName',... strcat('''',WBTConfigObject.LocalName,''''),mask.Parameters(item_LocalName)); else - error('[WBToolboxConfig2Mask] Failed to get the %s object', WBTConfigObjectName); + error('[ConfigurationToMask] Failed to get the %s object', WBTConfigObjectName); end catch SetParamFromReadOnly(... diff --git a/toolbox/+WBToolbox/Mask2WBToolboxConfig.m b/toolbox/matlab/+WBToolbox/MaskToConfiguration.m similarity index 85% rename from toolbox/+WBToolbox/Mask2WBToolboxConfig.m rename to toolbox/matlab/+WBToolbox/MaskToConfiguration.m index b9850d12c..c3c51c959 100644 --- a/toolbox/+WBToolbox/Mask2WBToolboxConfig.m +++ b/toolbox/matlab/+WBToolbox/MaskToConfiguration.m @@ -1,5 +1,5 @@ -function [ WBTConfig ] = Mask2WBToolboxConfig(configBlock) -%MASK2WBTOOLBOXCONFIG Summary of this function goes here +function [ WBTConfig ] = MaskToConfiguration(configBlock) +%MASKTOCONFIGURATION Summary of this function goes here % Detailed explanation goes here defaultString = '''NonValid'''; @@ -12,10 +12,10 @@ strcmp(char(get_param(configBlock,'ControlledJoints')),defaultString) && ... strcmp(char(get_param(configBlock,'ControlBoardsNames')),defaultString) ... ) - error('[Mask2WBToolboxConfig] The provided config is NonValid.'); + error('[MaskToConfiguration] The provided config is NonValid.'); end -WBTConfig = WBToolbox.WBToolboxConfig; +WBTConfig = WBToolbox.Configuration; WBTConfig.RobotName = stripApices(char(get_param(configBlock,'RobotName'))); WBTConfig.UrdfFile = stripApices(char(get_param(configBlock,'UrdfFile'))); diff --git a/toolbox/+WBToolbox/PID.m b/toolbox/matlab/+WBToolbox/PID.m similarity index 94% rename from toolbox/+WBToolbox/PID.m rename to toolbox/matlab/+WBToolbox/PID.m index 33badfea0..8b1541ed0 100644 --- a/toolbox/+WBToolbox/PID.m +++ b/toolbox/matlab/+WBToolbox/PID.m @@ -12,20 +12,20 @@ % D - Derivative gain % joint - Reference joint % - % See also: WBTOOLBOX.WBTPIDCONFIG - + % See also: WBTOOLBOX.PIDCONFIGURATION + properties P (1,1) double = 0 I (1,1) double = 0 D (1,1) double = 0 joint (1,:) char end - + methods % function obj = PID(jointName) % obj.joint = jointName; % end - + function obj = PID(jointName, P, I, D) if (nargin == 4) obj.joint = jointName; @@ -44,7 +44,6 @@ ); end end - + end end - diff --git a/toolbox/+WBToolbox/WBTPIDConfig.m b/toolbox/matlab/+WBToolbox/PIDConfiguration.m similarity index 85% rename from toolbox/+WBToolbox/WBTPIDConfig.m rename to toolbox/matlab/+WBToolbox/PIDConfiguration.m index 51999e301..0fa0f707f 100644 --- a/toolbox/+WBToolbox/WBTPIDConfig.m +++ b/toolbox/matlab/+WBToolbox/PIDConfiguration.m @@ -1,20 +1,20 @@ -classdef WBTPIDConfig < handle - % WBTPIDCONFIG Class to store a set of PID config for many joints. - % WBTPIDCONFIG(WBToolbox.PID('jointName', P, I, D)) adds a new PID +classdef PIDConfiguration < handle + % PIDCONFIGURATION Class to store a set of PID config for many joints. + % PIDCONFIGURATION(WBToolbox.PID('jointName', P, I, D)) adds a new PID % for joint 'jointName'. % - % WBTPIDCONFIG Properties: + % PIDCONFIGURATION Properties: % P - Proportional gains % I - Integral gains % D - Derivative gains % jointList - List of joints % - % WBTPIDCONFIG Methods: + % PIDCONFIGURATION Methods: % addPID - Add a new joint PID % removePID - Remove a stored joint PID % % See also: WBTOOLBOX.PID - + % Read-Only properties properties (SetAccess = private) P (1,:) double @@ -22,42 +22,42 @@ D (1,:) double jointList (1,:) cell end - + methods function addPID(obj, pid) % Add if ~isa(pid,'WBToolbox.PID') error('The argument must be a WBToolbox.PID object.') end - + matches = find(strcmp(obj.jointList, pid.joint), 1); if ~isempty(matches) error('The PID''s joint has been already processed.') end - + obj.jointList{1, end+1} = pid.joint; obj.P(1, end+1) = pid.P; obj.I(1, end+1) = pid.I; obj.D(1, end+1) = pid.D; end - + function removePID(obj, jointName) if ~ischar(jointName) error('The argument must be a char containing the joint name.') end - + idx = find(strcmp(obj.jointList, jointName), 1); - + if isempty(idx) error('The specified joint has no configuration stored') end - + obj.jointList(:,idx) = []; obj.P(:,idx) = []; obj.I(:,idx) = []; obj.D(:,idx) = []; end - + function value = getSimulinkParameters(obj) if isempty(obj.P) || isempty(obj.I) || isempty(obj.D) || ... isempty(obj.jointList) @@ -70,6 +70,5 @@ function removePID(obj, jointName) value.jointList = obj.jointList; end end - -end +end diff --git a/toolbox/export_library.m b/toolbox/matlab/export_library.m similarity index 56% rename from toolbox/export_library.m rename to toolbox/matlab/export_library.m index 9044fda4d..c452b7fd1 100644 --- a/toolbox/export_library.m +++ b/toolbox/matlab/export_library.m @@ -1,4 +1,4 @@ -addpath(genpath('../images')); +addpath(genpath('../../images')); fprintf('\nWhole Body toolbox exporting library to multiple versions\n'); @@ -7,27 +7,35 @@ quit; end +addpath(genpath('library')); libraryName = 'WBToolboxLibrary_repository'; try + % Load the library open_system(libraryName,'loadonly'); fprintf('\nLibrary loaded\n'); + + % Enable library capabilities if (strcmp(get_param(libraryName, 'EnableLBRepository'), 'off')) set_param(libraryName, 'Lock', 'off'); set_param(libraryName, 'EnableLBRepository', 'on'); set_param(libraryName, 'Lock', 'on'); - end; + end + + % Export the library fprintf('\nExporting for 2014b\n'); - % This does not completely work: images are not saved. - % Instad if saved form the GUI it works.. save_system(libraryName, 'WBToolboxLibrary', 'ExportToVersion', 'R2014B_SLX'); - fprintf('\nExporting for 2012a\n'); - save_system(libraryName, 'WBToolboxLibrary', 'ExportToVersion', 'R2012A_MDL'); + movefile('WBToolboxLibrary.slx', 'library/exported/WBToolboxLibrary.slx'); + % TODO: Check if mdl support is still required + % fprintf('\nExporting for 2012a\n'); + % save_system(libraryName, 'WBToolboxLibrary', 'ExportToVersion', 'R2012A_MDL'); + % movefile('WBToolboxLibrary.mdl', 'library/exported/WBToolboxLibrary.mdl'); + + % Unload the library close_system(libraryName); -catch ex; +catch + error('Failed to export WBToolbox library') end - + fprintf('\nDone\n'); exit(0) - - diff --git a/toolbox/WBToolboxLibrary_repository.mdl b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl similarity index 74% rename from toolbox/WBToolboxLibrary_repository.mdl rename to toolbox/matlab/library/WBToolboxLibrary_repository.mdl index 357430b63..a122a1e37 100644 --- a/toolbox/WBToolboxLibrary_repository.mdl +++ b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl @@ -1,19 +1,21 @@ Library { Name "WBToolboxLibrary_repository" - Version 8.8 + Version 8.9 SavedCharacterEncoding "US-ASCII" LogicAnalyzerGraphicalSettings "" LogicAnalyzerPlugin "on" LogicAnalyzerSignalOrdering "" DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 3 0 0 0 0 0" + SuppressorTable "22 serialization::archive 11 0 6 0 0 0 11 0" + SLCCPlugin "on" LibraryType "BlockLibrary" + LockLinksToLibrary on ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off FPTRunName "Run 1" MaxMDLFileLineLength 120 - LastSavedArchitecture "maci64" + LastSavedArchitecture "glnxa64" Object { $PropName "BdWindowsInfo" $ObjectID 1 @@ -23,7 +25,7 @@ Library { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [747.0, 53.0, 1280.0, 721.0] + Location [257.0, 95.0, 1595.0, 866.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -45,11 +47,11 @@ Library { $ObjectID 5 $ClassName "Simulink.EditorInfo" IsActive [1] - ViewObjType "SimulinkTopLevel" - LoadSaveID "0" - Extents [1246.0, 559.0] - ZoomFactor [2.7309417040358746] - Offset [10.873563218390814, 6.1543513957307141] + ViewObjType "SimulinkSubsys" + LoadSaveID "192" + Extents [1557.0, 731.0] + ZoomFactor [2.13] + Offset [0.0, 0.0] } Object { $PropName "DockComponentsInfo" @@ -65,26 +67,27 @@ Library { Width [640] Height [480] } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAACivwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABvAP///wAAAAEAAAAAAAAAAPwCA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAANQAAAooAAABbAP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAKgD///8AAAUAAAACZgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAKwD///8AAAY7AAADFwAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACE/////wAAAAAAAAAA/////wEAAAD2/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAFL/////wAAAAAAAAAA/" + "////wEAAAB6/////wAAAAAAAAAA/////wEAAADe/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAEt/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } + HideAutomaticNames off Created "Thu Feb 06 02:21:39 2014" Creator "jorhabib" UpdateHistory "UpdateHistoryNever" ModifiedByFormat "%" - LastModifiedBy "makaveli" + LastModifiedBy "dferigo" ModifiedDateFormat "%" - LastModifiedDate "Wed Aug 23 12:33:30 2017" - RTWModifiedTimeStamp 425392371 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Dec 11 16:51:04 2017" + RTWModifiedTimeStamp 434911818 + ModelVersionFormat "1.%" ConfigurationManager "none" SampleTimeColors off SampleTimeAnnotations off @@ -92,6 +95,7 @@ Library { WideLines off ShowLineDimensions off ShowPortDataTypes off + PortDataTypeDisplayFormat "AliasTypeOnly" ShowEditTimeErrors on ShowEditTimeWarnings on ShowEditTimeAdvisorChecks off @@ -118,6 +122,7 @@ Library { FunctionConnectors off BrowserLookUnderMasks off SimulationMode "normal" + VisualizeLoggedSignalsWhenLoggingToFile off PauseTimes "5" NumberOfSteps 1 SnapshotBufferSize 10 @@ -149,12 +154,13 @@ Library { ExtModeAutoUpdateStatusClock on ShowModelReferenceBlockVersion off ShowModelReferenceBlockIO off + OrderedModelArguments on Array { Type "Handle" Dimension 1 Simulink.ConfigSet { $ObjectID 7 - Version "1.16.5" + Version "1.17.0" DisabledProps [] Description "" Array { @@ -162,7 +168,7 @@ Library { Dimension 10 Simulink.SolverCC { $ObjectID 8 - Version "1.16.5" + Version "1.17.0" DisabledProps [] Description "" StartTime "0.0" @@ -202,7 +208,7 @@ Library { } Simulink.DataIOCC { $ObjectID 9 - Version "1.16.5" + Version "1.17.0" DisabledProps [] Description "" Decimation "1" @@ -237,12 +243,13 @@ Library { ReturnWorkspaceOutputsName "out" Refine "1" LoggingToFile off + DatasetSignalFormat "timeseries" LoggingFileName "out.mat" LoggingIntervals "[-inf, inf]" } Simulink.OptimizationCC { $ObjectID 10 - Version "1.16.5" + Version "1.17.0" Array { Type "Cell" Dimension 8 @@ -297,10 +304,13 @@ Library { BufferReusableBoundary on SimCompilerOptimization "off" AccelVerboseBuild off + OptimizeBlockOrder "off" + OptimizeDataStoreBuffers on + BusAssignmentInplaceUpdate on } Simulink.DebuggingCC { $ObjectID 11 - Version "1.16.5" + Version "1.17.0" Array { Type "Cell" Dimension 1 @@ -387,6 +397,7 @@ Library { SymbolicDimMinMaxWarning "warning" LossOfSymbolicDimsSimulationWarning "warning" LossOfSymbolicDimsCodeGenerationWarning "error" + SymbolicDimsDataTypeCodeGenerationDiagnostic "error" BlockIODiagnostic "none" SFUnusedDataAndEventsDiag "warning" SFUnexpectedBacktrackingDiag "warning" @@ -405,10 +416,11 @@ Library { AllowedUnitSystems "all" UnitsInconsistencyMsg "warning" AllowAutomaticUnitConversions on + UnitDatabase "" } Simulink.HardwareCC { $ObjectID 12 - Version "1.16.5" + Version "1.17.0" DisabledProps [] Description "" ProdBitPerChar 8 @@ -456,7 +468,7 @@ Library { } Simulink.ModelReferenceCC { $ObjectID 13 - Version "1.16.5" + Version "1.17.0" DisabledProps [] Description "" UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" @@ -475,7 +487,7 @@ Library { } Simulink.SFSimCC { $ObjectID 14 - Version "1.16.5" + Version "1.17.0" DisabledProps [] Description "" SimCustomSourceCode "" @@ -499,11 +511,13 @@ Library { ModelFunctionsGlobalVisibility "on" CompileTimeRecursionLimit 50 EnableRuntimeRecursion on + MATLABDynamicMemAlloc on + MATLABDynamicMemAllocThreshold 65536 } Simulink.RTWCC { $BackupClass "Simulink.RTWCC" $ObjectID 15 - Version "1.16.5" + Version "1.17.0" Array { Type "Cell" Dimension 13 @@ -586,10 +600,10 @@ Library { Dimension 2 Simulink.CodeAppCC { $ObjectID 16 - Version "1.16.5" + Version "1.17.0" Array { Type "Cell" - Dimension 25 + Dimension 27 Cell "IgnoreCustomStorageClasses" Cell "IgnoreTestpoints" Cell "InsertBlockDesc" @@ -615,6 +629,8 @@ Library { Cell "CustomSymbolStrModelFcn" Cell "CustomSymbolStrUtil" Cell "CustomUserTokenString" + Cell "CustomSymbolStrEmxType" + Cell "CustomSymbolStrEmxFcn" PropName "DisabledProps" } Description "" @@ -646,6 +662,8 @@ Library { CustomSymbolStrTmpVar "$N$M" CustomSymbolStrMacro "$R$N$M" CustomSymbolStrUtil "$N$C" + CustomSymbolStrEmxType "emxArray_$M$N" + CustomSymbolStrEmxFcn "emx$M$N" CustomUserTokenString "" CustomCommentsFcn "" DefineNamingRule "None" @@ -669,7 +687,7 @@ Library { Simulink.GRTTargetCC { $BackupClass "Simulink.TargetCC" $ObjectID 17 - Version "1.16.5" + Version "1.17.0" Array { Type "Cell" Dimension 18 @@ -701,8 +719,7 @@ Library { TargetLangStandard "C89/C90 (ANSI)" CodeReplacementLibrary "None" UtilityFuncGeneration "Auto" - ERTMultiwordTypeDef "System defined" - ERTMultiwordLength 256 + MultiwordTypeDef "System defined" MultiwordLength 2048 GenerateFullHeader on InferredTypesCompatibility off @@ -746,6 +763,8 @@ Library { UseToolchainInfoCompliant on GenerateSharedConstants on CoderGroups [] + AccessMethods [] + LookupTableObjectStructAxisOrder "1,2,3,4,..." UseMalloc off ExtMode off ExtModeStaticAlloc off @@ -767,7 +786,7 @@ Library { } SlCovCC.ConfigComp { $ObjectID 18 - Version "1.16.5" + Version "1.17.0" DisabledProps [] Description "Simulink Coverage Configuration Component" Name "Simulink Coverage" @@ -803,10 +822,11 @@ Library { CovUseTimeInterval off CovStartTime 0 CovStopTime 0 + CovMcdcMode "Masking" } hdlcoderui.hdlcc { $ObjectID 19 - Version "1.16.5" + Version "1.17.0" DisabledProps [] Description "HDL Coder custom configuration component" Name "HDL Coder" @@ -823,7 +843,7 @@ Library { Name "Configuration" ExtraOptions "" CurrentDlgPage "Solver" - ConfigPrmDlgPosition [ 963, 135, 1843, 765 ] + ConfigPrmDlgPosition [ 963, 135, 1843, 765 ] } PropName "ConfigurationSets" } @@ -838,6 +858,7 @@ Library { FontWeight "normal" FontAngle "normal" ShowName on + HideAutomaticName on BlockRotation 0 BlockMirror off } @@ -863,7 +884,7 @@ Library { SelfModifiable "off" IconFrame "on" IconOpaque "opaque" - RunInitForIconRedraw "off" + RunInitForIconRedraw "analyze" IconRotate "none" PortRotate "default" IconUnits "autoscale" @@ -955,6 +976,7 @@ Library { SourceOfInitialOutputValue "Dialog" OutputWhenDisabled "held" InitialOutput "[]" + MustResolveToSignalObject off } Block { BlockType Product @@ -1031,8 +1053,9 @@ Library { } System { Name "WBToolboxLibrary_repository" - Location [747, 53, 2027, 774] - Open on + Location [257, 95, 1852, 961] + Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "black" @@ -1043,9 +1066,9 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "273" + ZoomFactor "343" ReportName "simulink-default.rpt" - SIDHighWatermark "1773" + SIDHighWatermark "1789" Block { BlockType SubSystem Name "Utilities" @@ -1065,8 +1088,9 @@ Library { } System { Name "Utilities" - Location [551, 44, 1831, 765] - Open off + Location [257, 95, 1852, 961] + Open on + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1077,7 +1101,248 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "166" + ZoomFactor "213" + Block { + BlockType SubSystem + Name "Configuration" + SID "1774" + Ports [] + Position [435, 260, 510, 290] + ZOrder 95 + InitFcn "source = get_param(gcb,'ConfigSource');\n\nif strcmp(source, 'Workspace')\n object = get_param(gcb,'" + "ConfigObject');\n WBToolbox.ConfigurationToMask(gcb, object);\nend\n\nWBToolbox.MaskToConfiguration(gcb);\n\ncle" + "ar object source" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 21 + $ClassName "Simulink.Mask" + SelfModifiable "on" + Display "disp('Config')" + RunInitForIconRedraw "off" + Array { + Type "Simulink.MaskParameter" + Dimension 8 + Object { + $ObjectID 22 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "Workspace" + Cell "Mask" + PropName "TypeOptions" + } + Name "ConfigSource" + Prompt "Configuration from" + Value "Mask" + Callback "% Get the Workspace/Mask menu\nh = Simulink.Mask.get(gcb);\ncurrentConfigSource = get_param(gcb,'ConfigSo" + "urce');\n\nif strcmp(currentConfigSource,'Workspace')\n % Switch the visibility of the GUI elements\n set_para" + "m(gcb,'MaskVisibilities',{'on';'on';'on';'on';'on';'on';'on';'on';});\n h.Parameters(3).ReadOnly = 'on';\n h.P" + "arameters(4).ReadOnly = 'on';\n h.Parameters(5).ReadOnly = 'on';\n h.Parameters(6).ReadOnly = 'on';\n h.Par" + "ameters(7).ReadOnly = 'on';\n h.Parameters(8).ReadOnly = 'on';\n \n % Parse the object inserted in the Conf" + "igObject field\n currentConfigObject = get_param(gcb,'ConfigObject');\n WBToolbox.ConfigurationToMask(gcb,curr" + "entConfigObject);\n clear currentConfigObject;\nelseif strcmp(currentConfigSource,'Mask')\n % Switch the visib" + "ility of the GUI elements\n set_param(gcb,'MaskVisibilities',{'on';'off';'on';'on';'on';'on';'on';'on';});\n h" + ".Parameters(3).ReadOnly = 'off';\n h.Parameters(4).ReadOnly = 'off';\n h.Parameters(5).ReadOnly = 'off';\n " + "h.Parameters(6).ReadOnly = 'off';\n h.Parameters(7).ReadOnly = 'off';\n h.Parameters(8).ReadOnly = 'off';\n " + " h.Parameters(3).Enabled = 'on';\n h.Parameters(4).Enabled = 'on';\n h.Parameters(5).Enabled = 'on';\n h.Pa" + "rameters(6).Enabled = 'on';\n h.Parameters(7).Enabled = 'on';\n h.Parameters(8).Enabled = 'on';\nend\n\nclear " + "h currentConfigSource" + } + Object { + $ObjectID 23 + Type "edit" + Name "ConfigObject" + Prompt "Name of the object" + Value "'WBTConfigRobot'" + Tunable "off" + Visible "off" + Callback "% This code get called whatsoever\nif strcmp(char(get_param(gcb,'ConfigSource')),'Mask')\n return\nend" + "\n\n% Parse the object inserted in the ConfigObject field\ncurrentConfigObject = get_param(gcb,'ConfigObject');\nWBT" + "oolbox.ConfigurationToMask(gcb,currentConfigObject);\n\nclear currentConfigObject;" + } + Object { + $ObjectID 24 + Type "edit" + Name "RobotName" + Prompt "Robot Name" + Value "'icub'" + } + Object { + $ObjectID 25 + Type "edit" + Name "UrdfFile" + Prompt "Urdf File" + Value "'model.urdf'" + } + Object { + $ObjectID 26 + Type "edit" + Name "ControlledJoints" + Prompt "Controlled Joints" + Value "{'l_elbow','l_shoulder_pitch','torso_roll'}" + } + Object { + $ObjectID 27 + Type "edit" + Name "ControlBoardsNames" + Prompt "Control Boards Names" + Value "{'left_arm','right_arm','torso'}" + } + Object { + $ObjectID 28 + Type "edit" + Name "LocalName" + Prompt "Local Name" + Value "'WBT'" + } + Object { + $ObjectID 29 + Type "edit" + Name "GravityVector" + Prompt "Gravity Vector" + Value "[0,0,-9.81]" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 30 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 31 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 32 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 33 + $ClassName "Simulink.dialog.TabContainer" + Array { + Type "Simulink.dialog.Tab" + Dimension 2 + Object { + $ObjectID 34 + Prompt "From" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 2 + Object { + $ObjectID 35 + $ClassName "Simulink.dialog.parameter.Popup" + Name "ConfigSource" + } + Object { + $ObjectID 36 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "ConfigObject" + } + PropName "DialogControls" + } + Name "TabFrom" + } + Object { + $ObjectID 37 + Prompt "Data" + Array { + Type "Simulink.dialog.parameter.Edit" + Dimension 6 + Object { + $ObjectID 38 + PromptLocation "left" + Name "RobotName" + } + Object { + $ObjectID 39 + PromptLocation "left" + Name "UrdfFile" + } + Object { + $ObjectID 40 + PromptLocation "left" + Name "ControlledJoints" + } + Object { + $ObjectID 41 + PromptLocation "left" + Name "ControlBoardsNames" + } + Object { + $ObjectID 42 + PromptLocation "left" + Name "LocalName" + } + Object { + $ObjectID 43 + PromptLocation "left" + Name "GravityVector" + } + PropName "DialogControls" + } + Name "TabData" + } + PropName "DialogControls" + } + Name "TabContainer" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + System { + Name "Configuration" + Location [550, 86, 1677, 725] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "white" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "600" + SIDHighWatermark "70" + Block { + BlockType Constant + Name "ImConfig" + SID "1774:67" + Position [20, 20, 50, 50] + ZOrder 81 + Value "0" + } + Block { + BlockType Terminator + Name "Terminator" + SID "1774:68" + Position [95, 25, 115, 45] + ZOrder 80 + } + Line { + ZOrder 1 + SrcBlock "ImConfig" + SrcPort 1 + DstBlock "Terminator" + DstPort 1 + } + } + } Block { BlockType SubSystem Name "DampPinv" @@ -1090,11 +1355,11 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 21 + $ObjectID 44 $ClassName "Simulink.Mask" Object { $PropName "Parameters" - $ObjectID 22 + $ObjectID 45 $ClassName "Simulink.MaskParameter" Type "edit" Name "tol" @@ -1106,6 +1371,7 @@ Library { Name "DampPinv" Location [0, 29, 1280, 744] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1152,6 +1418,7 @@ Library { Name "Damped Pseudo Inverse" Location [12, 45, 1279, 3773] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1163,7 +1430,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1619" + SIDHighWatermark "1622" Block { BlockType Inport Name "mat" @@ -1184,20 +1451,20 @@ Library { Block { BlockType Demux Name " Demux " - SID "107::1618" + SID "107::1621" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 98 + ZOrder 101 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "107::1617" + SID "107::1620" Tag "Stateflow S-Function WBToolboxLibrary_repository 6" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 97 + ZOrder 100 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -1206,16 +1473,14 @@ Library { Port { PortNumber 2 Name "DPinv" - RTWStorageClass "Auto" - DataLoggingNameMode "SignalName" } } Block { BlockType Terminator Name " Terminator " - SID "107::1619" + SID "107::1622" Position [460, 241, 480, 259] - ZOrder 99 + ZOrder 102 } Block { BlockType Outport @@ -1226,7 +1491,7 @@ Library { IconDisplay "Port number" } Line { - ZOrder 61 + ZOrder 66 SrcBlock "mat" SrcPort 1 Points [120, 0] @@ -1234,7 +1499,7 @@ Library { DstPort 1 } Line { - ZOrder 62 + ZOrder 67 SrcBlock "sigma" SrcPort 1 DstBlock " SFunction " @@ -1242,7 +1507,7 @@ Library { } Line { Name "DPinv" - ZOrder 63 + ZOrder 68 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1250,14 +1515,14 @@ Library { DstPort 1 } Line { - ZOrder 64 + ZOrder 69 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 65 + ZOrder 70 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1300,90 +1565,56 @@ Library { BlockType S-Function Name "DoFs converter" SID "1771" - Ports [1, 5] + Ports [1, 3] Position [240, 241, 385, 309] ZOrder 81 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend\n" FunctionName "WBToolbox" - Parameters "'YARPWBIConverter',robotName,localName,wbiFile,wbiList,yarp2WBIOption" + Parameters "'ModelPartitioner',WBTConfigParameters,configBlockAbsName,yarp2WBI" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 23 + $ObjectID 46 $ClassName "Simulink.Mask" Type "DoFs converter" Description "This block converts from a DoFs serialization to a YARP serialization or viceversa depending on" - " its configuration.\nThe DoFs configuration is automatically currently taken from the WBI list option.\nThe robot" - " name is taken from the environment variable YARP_ROBOT_NAME.\n\nParameters:\n\n- Conversion direction: specify i" - "f you want to convert from YARP to the current DoFs serialization or viceversa.\n\nWBI parameters:\n\n- Robot Por" - "t Name: Name of the ports opened by the robot. (e.g. icub).\n Set an empty string ('') to use t" - "he name specified in the \n Whole Body Interface configuration file.\n- Model Name: Prefix name" - " of the ports opened by the underlying Whole Body Interface.\n- WBI filename: name of the file containing the con" - "figuration of the Whole Body Interface\n- WBI List Name: name of the list of joints used to configure the Whole B" - "ody Interface\n" + " its configuration.\nThe robot name is taken from the environment variable YARP_ROBOT_NAME.\n\nParameters:\n\n- C" + "onversion direction: specify if you want to convert from YARP to the current DoFs serialization or viceversa.\n" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend\n\n% ModelPartitioner Initialization\n% ===============================\n\nswitch" + " get_param(gcb, 'yarp2WBIOption')\n case 'From YARP to WBI'\n yarp2WBI = true;\n case 'From WBI to Y" + "ARP'\n yarp2WBI = false;\n otherwise\nend" Display "robotName = getenv('YARP_ROBOT_NAME');\n\ncurrentSelection = get_param(gcb,'yarp2WBIoption');\n\nif" " strcmp(currentSelection,'From YARP to WBI')\n direction = 'YARP -> DoFs';\nelse\n direction = 'DoFs -> YAR" "P';\nend\n\nfprintf('%s\\n\\n%s', robotName, direction);" - Array { - Type "Simulink.MaskParameter" - Dimension 5 - Object { - $ObjectID 24 - Type "popup" - Array { - Type "Cell" - Dimension 2 - Cell "From YARP to WBI" - Cell "From WBI to YARP" - PropName "TypeOptions" - } - Name "yarp2WBIOption" - Prompt "Conversion Direction" - Value "From YARP to WBI" - } - Object { - $ObjectID 25 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - } - Object { - $ObjectID 26 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - } - Object { - $ObjectID 27 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - } - Object { - $ObjectID 28 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" + RunInitForIconRedraw "off" + Object { + $PropName "Parameters" + $ObjectID 47 + $ClassName "Simulink.MaskParameter" + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "From YARP to WBI" + Cell "From WBI to YARP" + PropName "TypeOptions" } - PropName "Parameters" + Name "yarp2WBIOption" + Prompt "Conversion Direction" + Value "From YARP to WBI" } Array { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 29 + $ObjectID 48 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 30 + $ObjectID 49 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -1391,55 +1622,33 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 31 + $ObjectID 50 Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 32 - $ClassName "Simulink.dialog.TabContainer" - Array { - Type "Simulink.dialog.Tab" - Dimension 2 - Object { - $ObjectID 33 - Prompt "Block Parameters" - Object { - $PropName "DialogControls" - $ObjectID 34 + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 51 $ClassName "Simulink.dialog.parameter.Popup" Name "yarp2WBIOption" - } - Name "Container4" - } - Object { - $ObjectID 35 - Prompt "WBI Parameters" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 4 - Object { - $ObjectID 36 - Name "robotName" - } - Object { - $ObjectID 37 - Name "localName" - } - Object { - $ObjectID 38 - Name "wbiFile" - } + } + Object { + $ObjectID 52 + $ClassName "Simulink.dialog.Group" Object { - $ObjectID 39 - Name "wbiList" - } - PropName "DialogControls" - } - Name "Container7" + $PropName "DialogControls" + $ObjectID 53 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" } - PropName "DialogControls" + Name "ToggleButtonContainer" } - Name "Container3" + PropName "DialogControls" } Name "ParameterGroupVar" } @@ -1462,7 +1671,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 40 + $ObjectID 54 $ClassName "Simulink.Mask" Type "Minimum Jerk Trajectory Generator" Description "This block wraps the minJerkTrajGen class from iCub::ctrl::minJerkTrajGen.\n\nThe Minimum Jerk " @@ -1485,7 +1694,7 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 41 + $ObjectID 55 Type "checkbox" Name "externalSettlingTime" Prompt "External Settling Time" @@ -1496,42 +1705,42 @@ Library { "SettlingTime" } Object { - $ObjectID 42 + $ObjectID 56 Type "edit" Name "settlingTime" Prompt "Settling Time" Value "3" } Object { - $ObjectID 43 + $ObjectID 57 Type "edit" Name "sampleTime" Prompt "Sample Time" Value "0.01" } Object { - $ObjectID 44 + $ObjectID 58 Type "checkbox" Name "resetOnSettlingTime" Prompt "Reset on Settling Time Changes" Value "off" } Object { - $ObjectID 45 + $ObjectID 59 Type "checkbox" Name "firstDerivatives" Prompt "Output First Derivative" Value "on" } Object { - $ObjectID 46 + $ObjectID 60 Type "checkbox" Name "secondDerivatives" Prompt "Output Second Derivative" Value "on" } Object { - $ObjectID 47 + $ObjectID 61 Type "checkbox" Name "explicitInitialValue" Prompt "Explicit Initial Value" @@ -1544,11 +1753,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 48 + $ObjectID 62 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 49 + $ObjectID 63 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -1556,38 +1765,38 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 50 + $ObjectID 64 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 51 + $ObjectID 65 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 52 + $ObjectID 66 Prompt "Trajectory Parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 4 Object { - $ObjectID 53 + $ObjectID 67 $ClassName "Simulink.dialog.parameter.CheckBox" Name "externalSettlingTime" } Object { - $ObjectID 54 + $ObjectID 68 $ClassName "Simulink.dialog.parameter.Edit" Name "settlingTime" } Object { - $ObjectID 55 + $ObjectID 69 $ClassName "Simulink.dialog.parameter.Edit" Name "sampleTime" } Object { - $ObjectID 56 + $ObjectID 70 $ClassName "Simulink.dialog.parameter.CheckBox" Name "resetOnSettlingTime" } @@ -1596,21 +1805,21 @@ Library { Name "Tab1" } Object { - $ObjectID 57 + $ObjectID 71 Prompt "Input/Output" Array { Type "Simulink.dialog.parameter.CheckBox" Dimension 3 Object { - $ObjectID 58 + $ObjectID 72 Name "firstDerivatives" } Object { - $ObjectID 59 + $ObjectID 73 Name "secondDerivatives" } Object { - $ObjectID 60 + $ObjectID 74 Name "explicitInitialValue" } PropName "DialogControls" @@ -1634,7 +1843,7 @@ Library { Ports [] Position [155, 14, 280, 51] ZOrder 23 - ForegroundColor "white" + ForegroundColor "[0.917647, 0.917647, 0.917647]" BackgroundColor "gray" ShowName off FunctionName "WBToolbox" @@ -1644,7 +1853,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 61 + $ObjectID 75 $ClassName "Simulink.Mask" Type "Real Time Synchronizer" Description "This block slows down the simulation trying to match the period specified \nas parameter (in se" @@ -1652,7 +1861,7 @@ Library { Display "disp('Real Time Synchronizer')" Object { $PropName "Parameters" - $ObjectID 62 + $ObjectID 76 $ClassName "Simulink.MaskParameter" Type "edit" Name "period" @@ -1677,7 +1886,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 63 + $ObjectID 77 $ClassName "Simulink.Mask" Type "Simulator Synchronizer" Description "This block synchronizes with the external simulation on a simulator \n(only Gazebo is supported" @@ -1687,21 +1896,21 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 64 + $ObjectID 78 Type "edit" Name "period" Prompt "Controller Period (in seconds)" Value "0.01" } Object { - $ObjectID 65 + $ObjectID 79 Type "edit" Name "serverPortName" Prompt "Server Port Name" Value "'/clock/rpc'" } Object { - $ObjectID 66 + $ObjectID 80 Type "edit" Name "clientPortName" Prompt "Client Port Name" @@ -1723,11 +1932,11 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 67 + $ObjectID 81 $ClassName "Simulink.Mask" Object { $PropName "Parameters" - $ObjectID 68 + $ObjectID 82 $ClassName "Simulink.MaskParameter" Type "edit" Name "tol" @@ -1739,6 +1948,7 @@ Library { Name "TruncPinv" Location [0, 29, 1280, 744] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1785,6 +1995,7 @@ Library { Name "Truncated PseudoInverse" Location [12, 45, 1279, 3773] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1796,7 +2007,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1610" + SIDHighWatermark "1613" Block { BlockType Inport Name "mat" @@ -1817,20 +2028,20 @@ Library { Block { BlockType Demux Name " Demux " - SID "112::1609" + SID "112::1612" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 98 + ZOrder 101 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "112::1608" + SID "112::1611" Tag "Stateflow S-Function WBToolboxLibrary_repository 7" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 97 + ZOrder 100 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -1839,16 +2050,14 @@ Library { Port { PortNumber 2 Name "TPinv" - RTWStorageClass "Auto" - DataLoggingNameMode "SignalName" } } Block { BlockType Terminator Name " Terminator " - SID "112::1610" + SID "112::1613" Position [460, 241, 480, 259] - ZOrder 99 + ZOrder 102 } Block { BlockType Outport @@ -1859,7 +2068,7 @@ Library { IconDisplay "Port number" } Line { - ZOrder 61 + ZOrder 66 SrcBlock "mat" SrcPort 1 Points [120, 0] @@ -1867,7 +2076,7 @@ Library { DstPort 1 } Line { - ZOrder 62 + ZOrder 67 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -1875,7 +2084,7 @@ Library { } Line { Name "TPinv" - ZOrder 63 + ZOrder 68 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1883,14 +2092,14 @@ Library { DstPort 1 } Line { - ZOrder 64 + ZOrder 69 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 65 + ZOrder 70 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1944,11 +2153,11 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 69 + $ObjectID 83 $ClassName "Simulink.Mask" Type "YARP Clock" - Description "This block outputs the current YARP Time\nIn a nutshell, this block outputs the equivalent of t" - "he C++ function call\nyarp::os::Time::now()" + Description "This block outputs the current YARP Time.\nIn a nutshell, this block outputs the equivalent of " + "the C++ function call yarp::os::Time::now()" SelfModifiable "on" Display "disp('YARP Time')\n" } @@ -1968,7 +2177,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 70 + $ObjectID 84 $ClassName "Simulink.Mask" Type "YARP Read" Description "This block behaves as the command 'yarp read'. \nBy default it has the option 'Autoconnect' act" @@ -1986,35 +2195,35 @@ Library { Type "Simulink.MaskParameter" Dimension 6 Object { - $ObjectID 71 + $ObjectID 85 Type "edit" Name "portName" Prompt "Source Port Name" Value "'/portname'" } Object { - $ObjectID 72 + $ObjectID 86 Type "edit" Name "signalSize" Prompt "Port Size" Value "1" } Object { - $ObjectID 73 + $ObjectID 87 Type "checkbox" Name "blocking" Prompt "Blocking Read" Value "off" } Object { - $ObjectID 74 + $ObjectID 88 Type "checkbox" Name "timestamp" Prompt "Read Timestamp" Value "on" } Object { - $ObjectID 75 + $ObjectID 89 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -2026,7 +2235,7 @@ Library { "t_string" } Object { - $ObjectID 76 + $ObjectID 90 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -2051,7 +2260,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 77 + $ObjectID 91 $ClassName "Simulink.Mask" Type "YARP Write" Description "This block behaves as the command 'yarp write'. \n\nBy default this block opens a port names as" @@ -2064,14 +2273,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 78 + $ObjectID 92 Type "edit" Name "portName" Prompt "Opened Port Name" Value "'/portname'" } Object { - $ObjectID 79 + $ObjectID 93 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -2082,7 +2291,7 @@ Library { "';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string" } Object { - $ObjectID 80 + $ObjectID 94 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -2105,7 +2314,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 81 + $ObjectID 95 $ClassName "Simulink.Mask" Type "Errors" Description "Computes two kinds of errors. The first is just the difference between x\nand y while the secon" @@ -2115,6 +2324,7 @@ Library { Name "errors" Location [0, 29, 1280, 744] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2248,7 +2458,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 82 + $ObjectID 96 $ClassName "Simulink.Mask" Type "Holder" Description "This block holds the first input value during the simulation." @@ -2257,6 +2467,7 @@ Library { Name "holder\n" Location [12, 45, 1340, 980] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2290,9 +2501,11 @@ Library { Ports [1, 1] Position [90, 60, 120, 90] ZOrder 10 - LibraryVersion "1.388" + LibraryVersion "1.391" SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Constant" SourceType "Compare To Constant" + SourceProductName "Simulink" + SourceProductBaseCode "SL" ContentPreviewEnabled off relop "==" const "0" @@ -2317,6 +2530,7 @@ Library { Name "MATLAB Function" Location [12, 45, 1135, 3068] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2328,7 +2542,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1610" + SIDHighWatermark "1619" Block { BlockType Inport Name "s" @@ -2349,20 +2563,20 @@ Library { Block { BlockType Demux Name " Demux " - SID "1300::1609" + SID "1300::1612" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 86 + ZOrder 89 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "1300::1608" + SID "1300::1611" Tag "Stateflow S-Function WBToolboxLibrary_repository 1" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 85 + ZOrder 88 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -2371,16 +2585,14 @@ Library { Port { PortNumber 2 Name "s0" - RTWStorageClass "Auto" - DataLoggingNameMode "SignalName" } } Block { BlockType Terminator Name " Terminator " - SID "1300::1610" + SID "1300::1613" Position [460, 241, 480, 259] - ZOrder 87 + ZOrder 90 } Block { BlockType Outport @@ -2391,7 +2603,7 @@ Library { IconDisplay "Port number" } Line { - ZOrder 61 + ZOrder 66 SrcBlock "s" SrcPort 1 Points [120, 0] @@ -2399,7 +2611,7 @@ Library { DstPort 1 } Line { - ZOrder 62 + ZOrder 67 SrcBlock "unused" SrcPort 1 DstBlock " SFunction " @@ -2407,7 +2619,7 @@ Library { } Line { Name "s0" - ZOrder 63 + ZOrder 68 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -2415,14 +2627,14 @@ Library { DstPort 1 } Line { - ZOrder 64 + ZOrder 69 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 65 + ZOrder 70 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -2483,14 +2695,15 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 83 + $ObjectID 97 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyActuators.png'),'center')" } System { Name "wholeBodyActuators" - Location [425, 373, 1725, 1134] + Location [257, 95, 1852, 961] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2501,50 +2714,50 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "667" + ZoomFactor "300" Block { BlockType SubSystem Name "Set LowLevel PIDs" SID "1752" Ports [] - Position [745, 230, 820, 280] + Position [760, 230, 835, 280] ZOrder 41 - BackgroundColor "[0.827500, 0.576500, 0.603900]" - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend" + BackgroundColor "[0.513700, 0.851000, 0.670600]" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 84 + $ObjectID 98 $ClassName "Simulink.Mask" Type "Set Low Level PIDs" - Description "This block sets the low level PID values for the robot actuators.\n\nIf only one gain file is s" - "pecified, those gains are loaded at startup.\nOtherwise an additional port to this block is added which accept an" - " integer\nrepresenting the index in the list of gains to be loaded (also at runtime)\n\nAssuming DoFs is the numb" - "er of degrees of freedom of the robot,\nParameters:\n- PID Parameter: String specifying either a file containing " - "the PIDs for the\n low level control, or a list of files (in yarp Bottle format).\n\n- Robot Port" - " Name: Name of the ports opened by the robot. (e.g. icub).\n Set an empty string ('') to use th" - "e name specified in the \n Whole Body Interface configuration file.\n- Model Name: Prefix name " - "of the ports opened by the underlying Whole Body Interface.\n- WBI filename: name of the file containing the conf" - "iguration of the Whole Body Interface\n- WBI List Name: name of the list of joints used to configure the Whole Bo" - "dy Interface\n" + Description "This block sets the low level PID values for the robot actuators.\n\nThe PIDs can be stored in " + "a WBToolbox.WBTPIDConfig object, each of them constructed from a WBToolbox.PID class.\nThe number of PIDs must ma" + "tch the DoFs of the Config block pointed by this SetLowLevelPID block.\n\nParameters:\n- WBTPIDConfig Object Name" + ": The name of the object which store the PIDs. It must be loaded in the workspace before the simulation runs\n- P" + "ID Type: the type of the target PIDs to set" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend\n\n% SetLowLevelPIDs Initialization\n% ==============================\n\ntry\n " + " PIDObjectName = WBTPIDConfigObjectName;\n \n % Remove the apices if present\n if (PIDObjectName(1) == \"" + "'\" && ...\n PIDObjectName(end) == \"'\")\n PIDObjectName = PIDObjectName(2:end-1);\n end\n " + "\n % Get the struct from the class\n PIDs = evalin('base', PIDObjectName);\n if (isa(PIDs,'WBToolbox.WBT" + "PIDConfig'))\n WBTPIDConfigStruct = PIDs.getSimulinkParameters;\n end\nend\n" Display "pidType = get_param(gcb,'pidType');\ndescription = sprintf('Set Low Level\\n%s PIDs', pidType);\ndi" "sp(description);\n" + RunInitForIconRedraw "off" Array { Type "Simulink.MaskParameter" - Dimension 6 + Dimension 2 Object { - $ObjectID 85 + $ObjectID 99 Type "edit" - Name "pidParameters" - Prompt "PID parameter" + Name "WBTPIDConfigObjectName" + Prompt "WBTPIDConfig Object Name" Value "''" Tunable "off" } Object { - $ObjectID 86 + $ObjectID 100 Type "popup" Array { Type "Cell" @@ -2555,57 +2768,26 @@ Library { } Name "pidType" Prompt "PID Type" - Value "Torque" + Value "Position" Evaluate "off" Callback "% Get the mask parameter values. This is a cell\n% array of strings.\nmaskStr = get_param(gcb,'pidType'" - ");\nif strcmp(maskStr, 'Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0]');\nelseif s" - "trcmp(maskStr, 'Position Direct')\n set_param(gcb,'BackgroundColor', '[0.3922, 0.7882, 0.3451, 1.0]');\nelseif st" - "rcmp(maskStr, 'Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\nelseif strcmp(mas" - "kStr, 'Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\nend\nclear maskStr" - } - Object { - $ObjectID 87 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 88 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 89 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 90 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" + ");\n\nswitch maskStr\n case 'Position'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0]')" + ";\n case 'Position Direct'\n set_param(gcb,'BackgroundColor', '[0.3922, 0.7882, 0.3451, 1.0]');\n case " + "'Velocity'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\n case 'Torque'\n s" + "et_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\n otherwise\n error('PID Type not recogn" + "ized');\nend\n\nclear maskStr" } PropName "Parameters" } Array { Type "Simulink.dialog.Group" - Dimension 2 + Dimension 3 Object { - $ObjectID 91 + $ObjectID 101 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 92 + $ObjectID 102 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2613,75 +2795,49 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 93 + $ObjectID 103 Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 94 - $ClassName "Simulink.dialog.TabContainer" - Array { - Type "Simulink.dialog.Tab" - Dimension 2 - Object { - $ObjectID 95 - Prompt "Block parameters" - Array { - Type "Simulink.dialog.parameter.Control" - Dimension 2 - Object { - $ObjectID 96 + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 2 + Object { + $ObjectID 104 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" - Name "pidParameters" - } - Object { - $ObjectID 97 + Name "WBTPIDConfigObjectName" + } + Object { + $ObjectID 105 $ClassName "Simulink.dialog.parameter.Popup" Name "pidType" - } - PropName "DialogControls" - } - Name "Container8" - } - Object { - $ObjectID 98 - Prompt "WBI parameters" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 4 - Object { - $ObjectID 99 - Name "robotName" - } - Object { - $ObjectID 100 - Name "localName" - } - Object { - $ObjectID 101 - Name "wbiFile" - } - Object { - $ObjectID 102 - Name "wbiList" - } - PropName "DialogControls" - } - Name "Container5" - } - PropName "DialogControls" } - Name "Container4" + PropName "DialogControls" } Name "ParameterGroupVar" } + Object { + $ObjectID 106 + Object { + $PropName "DialogControls" + $ObjectID 107 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" + "ock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system(confi" + "gBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block name: %s'" + ",gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } PropName "DialogControls" } } System { Name "Set LowLevel PIDs" - Location [0, 29, 1280, 744] + Location [114, 82, 1709, 948] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2692,7 +2848,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "1000" + ZoomFactor "600" Block { BlockType S-Function Name "S-Function" @@ -2701,7 +2857,7 @@ Library { Position [125, 39, 185, 71] ZOrder 19 FunctionName "WBToolbox" - Parameters "'SetLowLevelPID',robotName,localName,wbiFile,wbiList,pidParameters,pidType" + Parameters "'SetLowLevelPID',WBTConfigParameters,configBlockAbsName,WBTPIDConfigStruct,pidType" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -2716,115 +2872,70 @@ Library { Position [645, 230, 720, 280] ZOrder 43 BackgroundColor "[0.513700, 0.851000, 0.670600]" - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 103 + $ObjectID 108 $ClassName "Simulink.Mask" Type "Set References" Description "This block sets the references for the robot actuators.\nThe type of control mode is specified " - "as a parameter in the block.\n\nAssuming DoFs is the number of degrees of freedom of the robot,\nInput:\n- Refere" - "nces: Vector of size DoFs, representing the references to be sent \n to the robot controlled joint" - "s.\n\nParameters:\n- Control Mode: The control mode. Choose one of the supplied mode.\n\n- Robot Port Name: Name " - "of the ports opened by the robot. (e.g. icub).\n Set an empty string ('') to use the name speci" - "fied in the \n Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports" - " opened by the underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of" - " the Whole Body Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface" - "\n" + "as a parameter in the block.\n\nAssuming DoFs is the number of degrees of freedom configured in the Config block " + "associated:\n\nInput:\n- References: Vector of size DoFs, representing the references to be sent to the robot con" + "trolled joints.\n- Reference Speed (Position): Set the reference speed used by the trajectory generator of the IP" + "ositionControl interface\n\nParameters:\n- Control Mode: The control mode. Choose one of the supplied mode." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend" Display "disp(get_param(gcb,'controlType'))\n%port_label('input',1,'References')" + RunInitForIconRedraw "off" Array { Type "Simulink.MaskParameter" - Dimension 7 + Dimension 2 Object { - $ObjectID 104 + $ObjectID 109 Type "popup" Array { Type "Cell" - Dimension 5 + Dimension 7 Cell "Position" Cell "Position Direct" Cell "Velocity" Cell "Torque" Cell "Open Loop" + Cell "PWM" + Cell "Current" PropName "TypeOptions" } Name "controlType" Prompt "Control Mode" Value "Position" Evaluate "off" - Callback "% Get the mask parameter values. This is a cell\n% array of strings.\nmaskStr = get_param(gcb,'controlT" - "ype');\nif strcmp(maskStr, 'Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0]');\nelse" - "if strcmp(maskStr, 'Position Direct')\n set_param(gcb,'BackgroundColor', '[0.3922, 0.7882, 0.3451, 1.0]');\nelsei" - "f strcmp(maskStr, 'Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\nelseif strcmp" - "(maskStr, 'Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\nelseif strcmp(maskStr, " - "'Open Loop')\n set_param(gcb,'BackgroundColor', '[0.98, 0.98, 0.98, 1.0]');\nend\nclear maskStr" - } - Object { - $ObjectID 105 - Type "popup" - Array { - Type "Cell" - Dimension 2 - Cell "Full" - Cell "Sublist" - PropName "TypeOptions" - } - Name "fullOrSubControl" - Prompt "Control Type" - Value "Full" - Callback "subOrFull = get_param(gcb, 'fullOrSubControl');\nif(strcmp(subOrFull, 'Sublist'))\n set_param(gcb, 'Ma" - "skVisibilities',{'on';'on';'on';'on';'on';'on';'on'});\nelse\n set_param(gcb, 'MaskVisibilities',{'on';'on';'off'" - ";'on';'on';'on';'on'});\nend\nclear subOrFull " - } - Object { - $ObjectID 106 - Type "edit" - Name "controlledJoints" - Prompt "Controlled Joint List" - Value "''" - Visible "off" - } - Object { - $ObjectID 107 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 108 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 109 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" + Callback "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param(gcb,'contro" + "lType');\n\nswitch maskStr\n case 'Position'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, " + "1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'on'});\n case 'Position Direct'\n set_param(gcb," + "'BackgroundColor', '[0.3922, 0.7882, 0.3451, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off'});\n " + "case 'Velocity'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\n set_param(gcb,'" + "MaskVisibilities', {'on';'off'});\n case 'Torque'\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6" + "039, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off'});\n case 'Open Loop'\n set_param(gcb," + "'BackgroundColor', '[0.98, 0.98, 0.98, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off'});\n case '" + "PWM'\n set_param(gcb,'BackgroundColor', '[1, 1, 1, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'" + "off'});\n case 'Current'\n set_param(gcb,'BackgroundColor', '[0.96, 0.77, 0.46, 1.0]');\n set_param" + "(gcb,'MaskVisibilities', {'on';'off'});\n otherwise\n error('Control Type not recognized.')\nend\n\nclear " + " maskStr" } Object { $ObjectID 110 Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" + Name "refSpeed" + Prompt "Reference Speed" + Value "50" } PropName "Parameters" } Array { Type "Simulink.dialog.Group" - Dimension 2 + Dimension 3 Object { $ObjectID 111 Prompt "%" @@ -2840,77 +2951,47 @@ Library { Object { $ObjectID 113 Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 114 - $ClassName "Simulink.dialog.TabContainer" - Array { - Type "Simulink.dialog.Tab" - Dimension 2 - Object { - $ObjectID 115 - Prompt "Block parameters" - Array { - Type "Simulink.dialog.parameter.Control" - Dimension 3 - Object { - $ObjectID 116 + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 2 + Object { + $ObjectID 114 $ClassName "Simulink.dialog.parameter.Popup" Name "controlType" - } - Object { - $ObjectID 117 - $ClassName "Simulink.dialog.parameter.Popup" - Name "fullOrSubControl" - } - Object { - $ObjectID 118 + } + Object { + $ObjectID 115 $ClassName "Simulink.dialog.parameter.Edit" - Name "controlledJoints" - } - PropName "DialogControls" - } - Name "Container8" - } - Object { - $ObjectID 119 - Prompt "WBI parameters" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 4 - Object { - $ObjectID 120 - Name "robotName" - } - Object { - $ObjectID 121 - Name "localName" - } - Object { - $ObjectID 122 - Name "wbiFile" - } - Object { - $ObjectID 123 - Name "wbiList" - } - PropName "DialogControls" - } - Name "Container5" - } - PropName "DialogControls" + PromptLocation "left" + Name "refSpeed" } - Name "Container4" + PropName "DialogControls" } Name "ParameterGroupVar" } + Object { + $ObjectID 116 + Object { + $PropName "DialogControls" + $ObjectID 117 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" + "ock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system(confi" + "gBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block name: %s'" + ",gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } PropName "DialogControls" } } System { Name "Set References" - Location [535, 41, 1280, 744] + Location [2245, 90, 3840, 956] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2938,8 +3019,7 @@ Library { Position [125, 39, 185, 71] ZOrder 19 FunctionName "WBToolbox" - Parameters "'SetReferences',robotName,localName,wbiFile,wbiList,controlType,fullOrSubControl,controlledJo" - "ints" + Parameters "'SetReferences',WBTConfigParameters,configBlockAbsName,controlType,refSpeed" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -2968,14 +3048,15 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 124 + $ObjectID 118 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyModel.png'),'center')" } System { Name "wholeBodyModel" - Location [425, 373, 1725, 1134] + Location [257, 95, 1852, 961] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -2986,7 +3067,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "309" + ZoomFactor "381" Block { BlockType SubSystem Name "Dynamics" @@ -2999,14 +3080,15 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 125 + $ObjectID 119 $ClassName "Simulink.Mask" Display "image(imread('Dynamics.png'))" } System { Name "Dynamics" - Location [425, 373, 1725, 1134] + Location [257, 95, 1852, 961] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3017,7 +3099,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "219" + ZoomFactor "269" Block { BlockType SubSystem Name "Centroidal Momentum" @@ -3025,71 +3107,64 @@ Library { Ports [4, 1] Position [495, 101, 680, 164] ZOrder 72 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 126 + $ObjectID 120 $ClassName "Simulink.Mask" Type "Centroidal Momentum" Description "This block computed the 6 element centroidal momentum, as defined in:\n\"Centroidal dynamics of a huma" "noid robot\" - DE Orin, A Goswami, SH Lee - \nAutonomous Robots 35 (2-3), 161-176\n\nAssuming DoFs is the number of " - "degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between" - "\n the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the con" - "figuration \n of the joints.\n- Base velocity: Vector of size 6 representing the linear and an" - "gular velocity\n of the base frame.\n- Joints velocity: Vector of size DoFs, representing the veloci" - "ty \n of the joints.\n\nOutput:\n- Centroidal Momentum: 6-element vector containg the centroidal m" - "omentum \n (3 value for linear momentum and 3 for angular momentum)\n\nParameters:\n- Robot Port Name" - ": Name of the ports opened by the robot. (e.g. icub).\n Set an empty string ('') to use the name s" - "pecified in the \n Whole Body Interface configuration file.\n- Model Name: Prefix name of the port" - "s opened by the underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of t" - "he Whole Body Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - Array { - Type "Simulink.MaskParameter" - Dimension 4 - Object { - $ObjectID 127 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 128 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 129 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 130 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" + "degrees of freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation betwe" + "en the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration o" + "f the joints.\n- Base velocity: Vector of size 6 representing the linear and angular velocity of the base frame.\n- " + "Joints velocity: Vector of size DoFs, representing the velocity of the joints.\n\nOutput:\n- Centroidal Momentum: 6-" + "element vector containg the centroidal momentum (3 value for linear momentum and 3 for angular momentum)" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + RunInitForIconRedraw "on" + Object { + $PropName "DialogControls" + $ObjectID 121 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 122 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 123 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 124 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" } - PropName "Parameters" + Name "DescGroupVar" } } System { Name "Centroidal Momentum" - Location [0, 23, 1280, 744] + Location [300, 122, 1706, 880] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3100,7 +3175,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "248" + ZoomFactor "347" Block { BlockType Inport Name "Base Pose" @@ -3144,7 +3219,7 @@ Library { Position [180, 11, 255, 144] ZOrder 19 FunctionName "WBToolbox" - Parameters "'CentroidalMomentum',robotName,localName,wbiFile,wbiList" + Parameters "'CentroidalMomentum',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -3201,84 +3276,63 @@ Library { Ports [4, 1] Position [400, 202, 540, 303] ZOrder 73 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 131 + $ObjectID 125 $ClassName "Simulink.Mask" Type "Get Generalized Bias Forces" - Description "This block retrieves the generalied bias forces of the robot.\n\nParameter:\n- Explicit Gravity. If Tr" - "ue, an additional port (explicit Gravity) is added.\n Otherwise the vector [0; 0; -9.81] is used." - "\nAssuming DoFs is the number of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the " - "homogenous transformation between\n the base frame and the world frame.\n- Joint configuration: Vector of" - " size DoFs, representing the configuration \n of the joints.\n- Base velocity: Vector of size 6 r" - "epresenting the linear and angular velocity\n of the base frame.\n- Joints velocity: Vector of size " - "DoFs, representing the velocity of the joints.\n\nOutput:\n- Bias Forces: a Dofs + 6 vector representing the general" - "ized bias forces\n of the robot\n\nParameters:\n- Robot Port Name: Name of the ports opened by the robot." - " (e.g. icub).\n Set an empty string ('') to use the name specified in the \n " - " Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by the unde" - "rlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Interf" - "ace\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - Initialization "gravityInput = find_system(gcb, ...\n'LookUnderMasks', 'on', ...\n'FollowLinks', 'on', ...\n'Name'," - " 'Gravity');\n\nif explicitGravity\n if isempty(gravityInput)\n Simulink.Block.eval([gcb,'/S-Function']);\n" - " add_block('built-in/Inport',[gcb,'/Gravity']);\n add_line(gcb','Gravity/1','S-Function/7', 'autorouti" - "ng','on');\n end\nelse\n if ~isempty(gravityInput)\n delete_line(gcb','Gravity/1','S-Function/7');\n " - " delete_block(gravityInput);\n Simulink.Block.eval([gcb,'/S-Function']);\n end\nend\n" - SelfModifiable "on" - Array { - Type "Simulink.MaskParameter" - Dimension 5 - Object { - $ObjectID 132 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 133 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 134 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 135 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" - } - Object { - $ObjectID 136 - Type "checkbox" - Name "explicitGravity" - Prompt "Explicit Gravity" - Value "off" + Description "This block retrieves the generalied bias forces of the robot.\n\nAssuming DoFs is the number of degree" + "s of freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between the" + " base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the " + "joints.\n- Base velocity: Vector of size 6 representing the linear and angular velocity of the base frame.\n- Joints" + " velocity: Vector of size DoFs, representing the velocity of the joints.\n\nOutput:\n- Bias Forces: a Dofs + 6 vecto" + "r representing the generalized bias forces of the robot." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + RunInitForIconRedraw "on" + Object { + $PropName "DialogControls" + $ObjectID 126 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 127 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 128 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 129 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" } - PropName "Parameters" + Name "DescGroupVar" } } System { Name "Get Bias Forces" - Location [354, 32, 1634, 747] + Location [215, 73, 1810, 939] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3289,12 +3343,11 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "255" - SIDHighWatermark "1748" + ZoomFactor "318" Block { BlockType Inport Name "Base Pose" - SID "1724:1725" + SID "1776" Position [10, 38, 40, 52] ZOrder 22 IconDisplay "Port number" @@ -3302,7 +3355,7 @@ Library { Block { BlockType Inport Name "Joint configuration" - SID "1724:1726" + SID "1777" Position [10, 68, 40, 82] ZOrder 24 Port "2" @@ -3311,7 +3364,7 @@ Library { Block { BlockType Inport Name "Base velocity" - SID "1724:1727" + SID "1778" Position [10, 98, 40, 112] ZOrder 26 Port "3" @@ -3320,7 +3373,7 @@ Library { Block { BlockType Inport Name "Joints velocity" - SID "1724:1728" + SID "1779" Position [10, 128, 40, 142] ZOrder 27 Port "4" @@ -3329,7 +3382,7 @@ Library { Block { BlockType Constant Name "Constant" - SID "1724:1742" + SID "1780" Position [-5, 157, 50, 173] ZOrder 30 ShowName off @@ -3338,7 +3391,7 @@ Library { Block { BlockType Gain Name "Gain" - SID "1724:1743" + SID "1781" Position [110, 180, 140, 210] ZOrder 31 ShowName off @@ -3350,12 +3403,12 @@ Library { Block { BlockType S-Function Name "S-Function" - SID "1724:1731" + SID "1782" Ports [6, 1] Position [180, 24, 240, 216] ZOrder 19 FunctionName "WBToolbox" - Parameters "'InverseDynamics',robotName,localName,wbiFile,wbiList,explicitGravity" + Parameters "'InverseDynamics',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -3363,7 +3416,7 @@ Library { Block { BlockType Outport Name "Bias Forces" - SID "1724:1732" + SID "1783" Position [300, 113, 330, 127] ZOrder 25 IconDisplay "Port number" @@ -3436,82 +3489,61 @@ Library { Ports [2, 1] Position [605, 200, 745, 300] ZOrder 74 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 137 + $ObjectID 130 $ClassName "Simulink.Mask" Type "Gravity bias" - Description "This block compute the generalized bias forces due to the gravity\n\nParameter:\n- Explicit Gravity. I" - "f True, an additional port (explicit Gravity) is added.\n Otherwise the vector [0; 0; -9.81] is u" - "sed.\n\nAssuming DoFs is the number of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representin" - "g the homogenous transformation between\n the base frame and the world frame.\n- Joint configuration: Ve" - "ctor of size DoFs, representing the configuration \n of the joints.\n\nOutput:\n- Gravity: a D" - "ofs + 6 vector representing the torques due to the gravity.\n\nParameters:\n- Robot Port Name: Name of the ports ope" - "ned by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in the \n " - " Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by the underly" - "ing Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Interface" - "\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - Initialization "gravityInput = find_system(gcb, ...\n'LookUnderMasks', 'on', ...\n'FollowLinks', 'on', ...\n'Name'," - " 'Gravity');\n\nif explicitGravity\n if isempty(gravityInput)\n Simulink.Block.eval([gcb,'/S-Function']);\n" - " add_block('built-in/Inport',[gcb,'/Gravity']);\n add_line(gcb','Gravity/1','S-Function/7', 'autorouti" - "ng','on');\n end\nelse\n if ~isempty(gravityInput)\n delete_line(gcb','Gravity/1','S-Function/7');\n " - " delete_block(gravityInput);\n Simulink.Block.eval([gcb,'/S-Function']);\n end\nend\n" - SelfModifiable "on" - Array { - Type "Simulink.MaskParameter" - Dimension 5 - Object { - $ObjectID 138 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 139 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 140 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 141 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" - } - Object { - $ObjectID 142 - Type "checkbox" - Name "explicitGravity" - Prompt "Explicit Gravity" - Value "off" + Description "This block compute the generalized bias forces due to the gravity\n\nAssuming DoFs is the number of de" + "grees of freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between" + " the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of " + "the joints.\n\nOutput:\n- Gravity: a Dofs + 6 vector representing the torques due to the gravity." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + RunInitForIconRedraw "off" + Object { + $PropName "DialogControls" + $ObjectID 131 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 132 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 133 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 134 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" } - PropName "Parameters" + Name "DescGroupVar" } } System { Name "Get Gravity Forces" - Location [354, 32, 1634, 747] + Location [300, 122, 1706, 880] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3522,12 +3554,11 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "255" - SIDHighWatermark "1748" + ZoomFactor "285" Block { BlockType Inport Name "Base Pose" - SID "1733:1734" + SID "1784" Position [10, 38, 40, 52] ZOrder 22 IconDisplay "Port number" @@ -3535,7 +3566,7 @@ Library { Block { BlockType Inport Name "Joint configuration" - SID "1733:1735" + SID "1785" Position [10, 68, 40, 82] ZOrder 24 Port "2" @@ -3544,7 +3575,7 @@ Library { Block { BlockType Constant Name "Constant" - SID "1733:1744" + SID "1786" Position [0, 97, 55, 113] ZOrder 32 ShowName off @@ -3553,7 +3584,7 @@ Library { Block { BlockType Gain Name "Gain" - SID "1733:1745" + SID "1787" Position [100, 120, 130, 150] ZOrder 33 ShowName off @@ -3565,12 +3596,12 @@ Library { Block { BlockType S-Function Name "S-Function" - SID "1733:1740" + SID "1788" Ports [6, 1] Position [180, 24, 240, 216] ZOrder 19 FunctionName "WBToolbox" - Parameters "'InverseDynamics',robotName,localName,wbiFile,wbiList,explicitGravity" + Parameters "'InverseDynamics',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -3578,7 +3609,7 @@ Library { Block { BlockType Outport Name "Gravity Torques" - SID "1733:1741" + SID "1789" Position [300, 113, 330, 127] ZOrder 25 IconDisplay "Port number" @@ -3658,73 +3689,63 @@ Library { Position [190, 199, 355, 331] ZOrder 75 FunctionName "WBToolbox" - Parameters "'InverseDynamics',robotName,localName,wbiFile,wbiList,explicitGravity" + Parameters "'InverseDynamics',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 143 + $ObjectID 135 $ClassName "Simulink.Mask" Type "Inverse Dynamics" - Description "This block compute the inverse dynamics of the robot.\n\nParameter:\n- Explicit Gravity. If True, an a" - "dditional port (explicit Gravity) is added.\n Otherwise the vector [0; 0; -9.81] is used.\n\nAssu" - "ming DoFs is the number of degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homoge" - "nous transformation between\n the base frame and the world frame.\n- Joint configuration: Vector of size" - " DoFs, representing the configuration \n of the joints.\n- Base velocity: Vector of size 6 rep" - "resenting the linear and angular velocity\n of the base frame.\n- Joints velocity: Vector of size Do" - "Fs, representing the velocity \n of the joints.\n- Base acceleration: Vector of size 6 representin" - "g the linear and angular acceleration\n of the base frame.\n- Joints acceleration: Vector of size Do" - "Fs, representing the acceleration \n of the joints.\n- Gravity: Vector of size 3, denoting the acc" - "eleration vector w.r.t. the world frame.\n\n\nOutput:\n- Torques: a Dofs + 6 vector representing the corresponding t" - "orques required\n to achive the desired accelerations given the robot state\n\nParameters:\n- Robot Port N" - "ame: Name of the ports opened by the robot. (e.g. icub).\n Set an empty string ('') to use the nam" - "e specified in the \n Whole Body Interface configuration file.\n- Model Name: Prefix name of the p" - "orts opened by the underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration o" - "f the Whole Body Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" + Description "This block compute the inverse dynamics of the robot.\n\nAssuming DoFs is the number of degrees of fre" + "edom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between the base fr" + "ame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the joints.\n" + "- Base velocity: Vector of size 6 representing the linear and angular velocity of the base frame.\n- Joints velocity" + ": Vector of size DoFs, representing the velocity of the joints.\n- Base acceleration: Vector of size 6 representing " + "the linear and angular acceleration of the base frame.\n- Joints acceleration: Vector of size DoFs, representing the" + " acceleration of the joints.\n\nOutput:\n- Torques: a Dofs + 6 vector representing the corresponding torques require" + "d to achive the desired accelerations given the robot state" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" Display "port_label('output', 1, 'Torques')\n\nport_label('input', 1, 'Base pose')\nport_label('input', 2, 'Joints" " configuration')\nport_label('input', 3, 'Base velocity')\nport_label('input', 4, 'Joints velocity')\nport_label('in" - "put', 5, 'Base acceleration')\nport_label('input', 6, 'Joints acceleration')\n\n\ngravity = get_param(gcb, 'explicit" - "Gravity');\nif(strcmp(gravity, 'on'))\n port_label('input', 7, 'Gravity')\nend\n" - Array { - Type "Simulink.MaskParameter" - Dimension 5 - Object { - $ObjectID 144 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - } - Object { - $ObjectID 145 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - } - Object { - $ObjectID 146 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - } - Object { - $ObjectID 147 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - } - Object { - $ObjectID 148 - Type "checkbox" - Name "explicitGravity" - Prompt "Explicit Gravity" - Value "off" + "put', 5, 'Base acceleration')\nport_label('input', 6, 'Joints acceleration')\n" + RunInitForIconRedraw "on" + Object { + $PropName "DialogControls" + $ObjectID 136 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 137 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 138 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 139 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" } - PropName "Parameters" + Name "DescGroupVar" } } } @@ -3735,68 +3756,61 @@ Library { Ports [2, 1] Position [250, 104, 390, 171] ZOrder 32 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 149 + $ObjectID 140 $ClassName "Simulink.Mask" Type "Mass Matrix" Description "This block retrieves the robot mass matrix.\n\nAssuming DoFs is the number of degrees of freedom of th" - "e robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between\n the base " - "frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration \n " - " of the joints.\n\nOutput:\n- Mass Matrix: a (Dofs + 6) x (Dofs + 6) matrix representing the mass matrix " - "\n of the robot\n\nParameters:\n- Robot Port Name: Name of the ports opened by the robot. (e.g. icub)." - "\n Set an empty string ('') to use the name specified in the \n Whole Body Inter" - "face configuration file.\n- Model Name: Prefix name of the ports opened by the underlying Whole Body Interface.\n- W" - "BI filename: name of the file containing the configuration of the Whole Body Interface\n- WBI List Name: name of the" - " list of joints used to configure the Whole Body Interface\n" - Array { - Type "Simulink.MaskParameter" - Dimension 4 - Object { - $ObjectID 150 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 151 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 152 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 153 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" + "e robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between the base frame and the " + "world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the joints.\n\nOutput:\n" + "- Mass Matrix: a (Dofs + 6) x (Dofs + 6) matrix representing the mass matrix of the robot" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + RunInitForIconRedraw "on" + Object { + $PropName "DialogControls" + $ObjectID 141 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 142 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 143 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 144 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" } - PropName "Parameters" + Name "DescGroupVar" } } System { Name "Mass Matrix" - Location [589, 68, 1869, 783] + Location [300, 122, 1706, 880] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3807,7 +3821,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "248" + ZoomFactor "455" Block { BlockType Inport Name "Base Pose" @@ -3833,7 +3847,7 @@ Library { Position [125, 37, 185, 68] ZOrder 19 FunctionName "WBToolbox" - Parameters "'MassMatrix',robotName,localName,wbiFile,wbiList" + Parameters "'MassMatrix',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -3885,14 +3899,15 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 154 + $ObjectID 145 $ClassName "Simulink.Mask" Display "image(imread('jacobian.png'))" } System { Name "Jacobians" - Location [425, 373, 1725, 1134] + Location [325, 44, 1920, 910] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -3903,7 +3918,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "322" + ZoomFactor "397" Block { BlockType SubSystem Name "DotJ Nu" @@ -3911,87 +3926,47 @@ Library { Ports [4, 1] Position [590, 170, 755, 265] ZOrder 67 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 155 + $ObjectID 146 $ClassName "Simulink.Mask" Type "DotJ Nu" - Description "This block computes the product between the time derivative of the\n Jacobian of the specified" - " frame and the state (base and joints)\n velocity vector.\n\nAssuming DoFs is the number of degrees of freedo" - "m of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between\n th" - "e the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration \n" - " of the joints.\n- Base velocity: Vector of size 6 representing the linear and angular velocit" - "y\n of the base frame.\n- Joints velocity: Vector of size DoFs, representing the velocity \n " - " of the joints.\n\nOutput:\n- dotJ nu: a 6-d vector representing the product between the time derivative o" - "f the\n Jacobian of the specified frame and the state velocity vector\n\nParameters:\n- Frame name: the name" - " of the frame. It should be specified in the URDF model.\n\n- Robot Port Name: Name of the ports opened by the robot" - ". (e.g. icub).\n Set an empty string ('') to use the name specified in the \n Wh" - "ole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by the underlying Whole Body I" - "nterface.\n- WBI filename: name of the file containing the configuration of the Whole Body Interface\n- WBI List Nam" - "e: name of the list of joints used to configure the Whole Body Interface\n" + Description "This block computes the product between the time derivative of the Jacobian of the specified frame and" + " the state (base and joints) velocity vector.\n\nAssuming DoFs is the number of degrees of freedom of the robot:\n\n" + "Input:\n- Base pose: 4x4 matrix representing the homogenous transformation between the the base frame and the world " + "frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the joints.\n- Base velocity: " + "Vector of size 6 representing the linear and angular velocity of the base frame.\n- Joints velocity: Vector of size " + "DoFs, representing the velocity of the joints.\n\nOutput:\n- dotJ nu: a 6-d vector representing the product between " + "the time derivative of the\n Jacobian of the specified frame and the state velocity vector\n\nParameters:\n-" + " Frame name: the name of the frame. It should be specified in the URDF model." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" Display "escapedFrameName = strrep(frameName, '_', '\\_');\nport_label('output', 1, strcat('dot J_{',escapedFrameN" "ame,'} \\nu'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\nport_label('input'," " 2, 'Joint configuration')\nport_label('input', 3, 'Base velocity')\nport_label('input', 4, 'Joint velocity')\n\ncle" "ar escapedFrameName;" - Array { - Type "Simulink.MaskParameter" - Dimension 5 - Object { - $ObjectID 156 - Type "edit" - Name "frameName" - Prompt "Frame name" - Value "'frame'" - } - Object { - $ObjectID 157 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 158 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 159 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 160 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" - } - PropName "Parameters" + RunInitForIconRedraw "on" + Object { + $PropName "Parameters" + $ObjectID 147 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "frameName" + Prompt "Frame name" + Value "'frame'" } Array { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 161 + $ObjectID 148 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 162 + $ObjectID 149 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3999,56 +3974,34 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 163 + $ObjectID 150 Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 164 - $ClassName "Simulink.dialog.TabContainer" Array { - Type "Simulink.dialog.Tab" + Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 165 - Prompt "Block parameters" - Object { - $PropName "DialogControls" - $ObjectID 166 + $ObjectID 151 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } - Name "Container8" - } - Object { - $ObjectID 167 - Prompt "WBI parameters" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 4 - Object { - $ObjectID 168 - Name "robotName" - } - Object { - $ObjectID 169 - Name "localName" - } Object { - $ObjectID 170 - Name "wbiFile" - } + $ObjectID 152 + $ClassName "Simulink.dialog.Group" Object { - $ObjectID 171 - Name "wbiList" + $PropName "DialogControls" + $ObjectID 153 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" } - PropName "DialogControls" - } - Name "Container5" + Name "ToggleButtonContainer" } PropName "DialogControls" } - Name "Container4" - } Name "ParameterGroupVar" } PropName "DialogControls" @@ -4056,8 +4009,9 @@ Library { } System { Name "DotJ Nu" - Location [0, 29, 1280, 744] + Location [300, 122, 1706, 880] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4068,7 +4022,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "391" + ZoomFactor "428" Block { BlockType Inport Name "Base Pose" @@ -4112,7 +4066,7 @@ Library { Position [125, 4, 190, 126] ZOrder 19 FunctionName "WBToolbox" - Parameters "'DotJNu',robotName,localName,wbiFile,wbiList,frameName" + Parameters "'DotJNu',WBTConfigParameters,configBlockAbsName,frameName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -4121,7 +4075,7 @@ Library { BlockType Outport Name "dotJ dotnu" SID "1689" - Position [245, 43, 275, 57] + Position [245, 58, 275, 72] ZOrder 25 IconDisplay "Port number" } @@ -4143,7 +4097,6 @@ Library { ZOrder 3 SrcBlock "S-Function" SrcPort 1 - Points [20, 0; 0, -15] DstBlock "dotJ dotnu" DstPort 1 } @@ -4170,83 +4123,43 @@ Library { Ports [2, 1] Position [380, 190, 540, 245] ZOrder 39 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 172 + $ObjectID 154 $ClassName "Simulink.Mask" Type "Jacobian" Description "This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number of degrees of" - " freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between\n " - " the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configura" - "tion \n of the joints.\n\nOutput:\n- Jacobian: a 6x6+dofs matrix representing the Jacobian of\n" - " the specified frame written in the world frame.\n\nParameters:\n- Frame name: the name of the frame. It" - " should be specified in the URDF model.\n\n- Robot Port Name: Name of the ports opened by the robot. (e.g. icub).\n " - " Set an empty string ('') to use the name specified in the \n Whole Body Interfac" - "e configuration file.\n- Model Name: Prefix name of the ports opened by the underlying Whole Body Interface.\n- WBI " - "filename: name of the file containing the configuration of the Whole Body Interface\n- WBI List Name: name of the li" - "st of joints used to configure the Whole Body Interface\n" + " freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between the the" + " base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the " + "joints.\n\nOutput:\n- Jacobian: a 6x6+dofs matrix representing the Jacobian of the specified frame written in the wo" + "rld frame.\n\nParameters:\n- Frame name: the name of the frame. It should be specified in the URDF model.\n" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" Display "escapedFrameName = strrep(frameName, '_', '\\_');\nport_label('output', 1, strcat('{}^{world} J_{',escape" "dFrameName,'}'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\nport_label('input" "', 2, 'Joint configuration')\n\nclear escapedFrameName;" - Array { - Type "Simulink.MaskParameter" - Dimension 5 - Object { - $ObjectID 173 - Type "edit" - Name "frameName" - Prompt "Frame name" - Value "'frame'" - } - Object { - $ObjectID 174 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 175 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 176 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 177 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" - } - PropName "Parameters" + RunInitForIconRedraw "on" + Object { + $PropName "Parameters" + $ObjectID 155 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "frameName" + Prompt "Frame name" + Value "'frame'" } Array { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 178 + $ObjectID 156 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 179 + $ObjectID 157 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4254,56 +4167,34 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 180 + $ObjectID 158 Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 181 - $ClassName "Simulink.dialog.TabContainer" Array { - Type "Simulink.dialog.Tab" + Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 182 - Prompt "Block parameters" - Object { - $PropName "DialogControls" - $ObjectID 183 + $ObjectID 159 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } - Name "Container8" - } - Object { - $ObjectID 184 - Prompt "WBI parameters" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 4 - Object { - $ObjectID 185 - Name "robotName" - } Object { - $ObjectID 186 - Name "localName" - } - Object { - $ObjectID 187 - Name "wbiFile" - } + $ObjectID 160 + $ClassName "Simulink.dialog.Group" Object { - $ObjectID 188 - Name "wbiList" - } - PropName "DialogControls" + $PropName "DialogControls" + $ObjectID 161 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" } - Name "Container5" + Name "ToggleButtonContainer" } PropName "DialogControls" } - Name "Container4" - } Name "ParameterGroupVar" } PropName "DialogControls" @@ -4311,8 +4202,9 @@ Library { } System { Name "Jacobian" - Location [208, 105, 1008, 627] + Location [300, 122, 1706, 880] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4323,7 +4215,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "237" + ZoomFactor "430" Block { BlockType Inport Name "Base Pose" @@ -4349,7 +4241,7 @@ Library { Position [125, 37, 185, 68] ZOrder 19 FunctionName "WBToolbox" - Parameters "'Jacobian',robotName,localName,wbiFile,wbiList,frameName" + Parameters "'Jacobian',WBTConfigParameters,configBlockAbsName,frameName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -4401,14 +4293,15 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 189 + $ObjectID 162 $ClassName "Simulink.Mask" Display "image(imread('forwardKinematics.png'))" } System { Name "Kinematics" - Location [425, 373, 1725, 1134] + Location [257, 95, 1852, 961] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4419,91 +4312,51 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "326" + ZoomFactor "338" Block { BlockType SubSystem Name "Forward Kinematics" SID "1647" Ports [2, 1] - Position [350, 103, 500, 167] + Position [360, 103, 510, 167] ZOrder 34 - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" - ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" - "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" - "nd" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 190 + $ObjectID 163 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " - "degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between" - "\n the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the" - " configuration \n of the joints.\n\nOutput:\n- Forward Kinematics: a 4x4 matrix representing t" - "he homogenous transformation between\n the specified frame and the world frame.\n\nParameters:\n- Frame " - "name: the name of the frame. It should be specified in the URDF model.\n\n- Robot Port Name: Name of the ports opene" - "d by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in the \n " - " Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by the underlyin" - "g Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Interface\n" - "- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" + "degrees of freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation betwe" + "en the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configurati" + "on of the joints.\n\nOutput:\n- Forward Kinematics: a 4x4 matrix representing the homogenous transformation between " + "the specified frame and the world frame.\n\nParameters:\n- Frame name: the name of the frame. It should be specified" + " in the URDF model." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" Display "escapedFrameName = strrep(frameName, '_', '\\_');\nport_label('output', 1, strcat('{}^{world} H_{',escape" "dFrameName,'}'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\nport_label('input" "', 2, 'Joint configuration')\n\nclear escapedFrameName;" - Array { - Type "Simulink.MaskParameter" - Dimension 5 - Object { - $ObjectID 191 - Type "edit" - Name "frameName" - Prompt "Frame name" - Value "'frame'" - } - Object { - $ObjectID 192 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 193 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 194 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 195 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" - } - PropName "Parameters" + Object { + $PropName "Parameters" + $ObjectID 164 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "frameName" + Prompt "Frame name" + Value "'frame'" } Array { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 196 + $ObjectID 165 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 197 + $ObjectID 166 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4511,56 +4364,34 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 198 + $ObjectID 167 Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 199 - $ClassName "Simulink.dialog.TabContainer" Array { - Type "Simulink.dialog.Tab" + Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 200 - Prompt "Block parameters" - Object { - $PropName "DialogControls" - $ObjectID 201 + $ObjectID 168 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } - Name "Container8" - } Object { - $ObjectID 202 - Prompt "WBI parameters" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 4 - Object { - $ObjectID 203 - Name "robotName" - } - Object { - $ObjectID 204 - Name "localName" - } - Object { - $ObjectID 205 - Name "wbiFile" - } + $ObjectID 169 + $ClassName "Simulink.dialog.Group" Object { - $ObjectID 206 - Name "wbiList" - } - PropName "DialogControls" + $PropName "DialogControls" + $ObjectID 170 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" } - Name "Container5" + Name "ToggleButtonContainer" } PropName "DialogControls" } - Name "Container4" - } Name "ParameterGroupVar" } PropName "DialogControls" @@ -4568,8 +4399,9 @@ Library { } System { Name "Forward Kinematics" - Location [0, 29, 1280, 744] + Location [300, 122, 1706, 880] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4580,7 +4412,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "391" + ZoomFactor "430" Block { BlockType Inport Name "Base Pose" @@ -4606,7 +4438,7 @@ Library { Position [125, 37, 185, 68] ZOrder 19 FunctionName "WBToolbox" - Parameters "'ForwardKinematics',robotName,localName,wbiFile,wbiList,frameName" + Parameters "'ForwardKinematics',WBTConfigParameters,configBlockAbsName,frameName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -4651,6 +4483,7 @@ Library { Ports [3, 2] Position [350, 198, 525, 262] ZOrder 35 + Commented "on" InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" @@ -4659,7 +4492,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 207 + $ObjectID 171 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4683,21 +4516,21 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 208 + $ObjectID 172 Type "edit" Name "baseFrame" Prompt "Base Frame" Value "'root_link'" } Object { - $ObjectID 209 + $ObjectID 173 Type "edit" Name "endEffFrame" Prompt "End Effector frame" Value "'l_sole'" } Object { - $ObjectID 210 + $ObjectID 174 Type "popup" Array { Type "Cell" @@ -4711,7 +4544,7 @@ Library { Value "Full Constraint (Position and Orientation)" } Object { - $ObjectID 211 + $ObjectID 175 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -4719,7 +4552,7 @@ Library { Tunable "off" } Object { - $ObjectID 212 + $ObjectID 176 Type "edit" Name "localName" Prompt "Model Name" @@ -4727,7 +4560,7 @@ Library { Tunable "off" } Object { - $ObjectID 213 + $ObjectID 177 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -4735,7 +4568,7 @@ Library { Tunable "off" } Object { - $ObjectID 214 + $ObjectID 178 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -4748,11 +4581,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 215 + $ObjectID 179 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 216 + $ObjectID 180 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4760,34 +4593,34 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 217 + $ObjectID 181 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 218 + $ObjectID 182 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 219 + $ObjectID 183 Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 220 + $ObjectID 184 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "baseFrame" } Object { - $ObjectID 221 + $ObjectID 185 $ClassName "Simulink.dialog.parameter.Edit" Name "endEffFrame" } Object { - $ObjectID 222 + $ObjectID 186 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -4796,25 +4629,25 @@ Library { Name "Container8" } Object { - $ObjectID 223 + $ObjectID 187 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 224 + $ObjectID 188 Name "robotName" } Object { - $ObjectID 225 + $ObjectID 189 Name "localName" } Object { - $ObjectID 226 + $ObjectID 190 Name "wbiFile" } Object { - $ObjectID 227 + $ObjectID 191 Name "wbiList" } PropName "DialogControls" @@ -4834,6 +4667,7 @@ Library { Name "Inverse Kinematics" Location [0, 23, 1280, 744] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -4945,6 +4779,7 @@ Library { Ports [2, 1] Position [560, 105, 720, 165] ZOrder 66 + Commented "on" InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" @@ -4953,7 +4788,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 228 + $ObjectID 192 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4979,14 +4814,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 229 + $ObjectID 193 Type "edit" Name "solverName" Prompt "Solver Name" Value "'/cartesianSolver'" } Object { - $ObjectID 230 + $ObjectID 194 Type "edit" Name "dofs" Prompt "#Dofs" @@ -4994,7 +4829,7 @@ Library { Tunable "off" } Object { - $ObjectID 231 + $ObjectID 195 Type "popup" Array { Type "Cell" @@ -5013,11 +4848,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 232 + $ObjectID 196 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 233 + $ObjectID 197 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5025,33 +4860,33 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 234 + $ObjectID 198 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 235 + $ObjectID 199 $ClassName "Simulink.dialog.TabContainer" Object { $PropName "DialogControls" - $ObjectID 236 + $ObjectID 200 $ClassName "Simulink.dialog.Tab" Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 237 + $ObjectID 201 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "solverName" } Object { - $ObjectID 238 + $ObjectID 202 $ClassName "Simulink.dialog.parameter.Edit" Name "dofs" } Object { - $ObjectID 239 + $ObjectID 203 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -5070,6 +4905,7 @@ Library { Name "Remote Inverse Kinematics" Location [853, 51, 2214, 1013] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5159,14 +4995,15 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 240 + $ObjectID 204 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyStates.png'),'center');" } System { Name "wholeBodyStates" - Location [425, 373, 1725, 1134] + Location [257, 95, 1852, 961] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5177,102 +5014,93 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "576" + ZoomFactor "600" Block { BlockType SubSystem - Name "Get Estimate" - SID "1671" - Ports [0, 1] - Position [360, 212, 440, 248] - ZOrder 53 + Name "Get Limits" + SID "1690" + Ports [0, 2] + Position [395, 218, 515, 247] + ZOrder 68 BackgroundColor "[0.513700, 0.851000, 0.670600]" - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 241 + $ObjectID 205 $ClassName "Simulink.Mask" - Type "Get Estimate" - Description "This block get the estimate of the specified type\n\nAssuming DoFs is the number of degrees of " - "freedom of the robot,\nOutput:\n- References: Vector of size DoFs, representing the estimate requested.\n\nParame" - "ters:\n- Control Mode: The control mode. Choose one of the supplied mode.\n\n- Robot Port Name: Name of the ports" - " opened by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in the " - "\n Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by t" - "he underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole B" - "ody Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - Display "%disp(get_param(gcb,'estimateType'))\nport_label('output',1,get_param(gcb,'estimateType'))" + Type "Get Limits" + Description "This block provides the joint limits gathering data from either the Robot's Control Board or UR" + "DF model.\n\nOutput:\n\n* Max: 1xDoFs vector containing the maximum limit\n* Min: 1xDoFs vector containing the m" + "aximum limit\n\nMeasures are in radians." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend\n\n% GetLimits Initialization\n% =========================\n\nsourceLimit = get_p" + "aram(gcb,'limitsSource');\ntypeLimit = get_param(gcb,'limitsType');\nnotifyError = false;\n\nswitch sourceLimit\n" + " case 'ControlBoard'\n switch typeLimit\n case 'Position'\n limitsType = 'Con" + "trolBoardPosition';\n case 'Velocity'\n limitsType = 'ControlBoardVelocity';\n " + " otherwise\n notifyError = true;\n end\n case 'URDF'\n switch typeLimit\n " + " case 'Position'\n limitsType = 'ModelPosition';\n otherwise\n noti" + "fyError = true;\n end\nend\n\nif notifyError\n error('Limit Type not recognized');\nend\n\nclear source" + "Limit typeLimit notifyError" + SelfModifiable "on" + Display "fprintf('%s\\n(%s)',get_param(gcb,'limitsType'),get_param(gcb,'limitsSource'))\n\nport_label('outpu" + "t',1,'Min')\nport_label('output',2,'Max')" + RunInitForIconRedraw "off" Array { Type "Simulink.MaskParameter" - Dimension 5 + Dimension 2 Object { - $ObjectID 242 + $ObjectID 206 Type "popup" Array { Type "Cell" - Dimension 4 - Cell "Joints Position" - Cell "Joints Velocity" - Cell "Joints Acceleration" - Cell "Joints Torque" + Dimension 2 + Cell "ControlBoard" + Cell "URDF" PropName "TypeOptions" } - Name "estimateType" - Prompt "Estimate Type" - Value "Joints Position" - Evaluate "off" - Callback "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param(gcb,'estima" - "teType');\nif strcmp(maskStr, 'Joints Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0" - "]');\nelseif strcmp(maskStr, 'Joints Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]" - "');\nelseif strcmp(maskStr, 'Joints Acceleration')\n set_param(gcb,'BackgroundColor', '[0.9255, 0.8706, 0.1333, 1" - ".0]');\nelseif strcmp(maskStr, 'Joints Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]" - "');\nend\nclear maskStr" - } - Object { - $ObjectID 243 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 244 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 245 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" + Name "limitsSource" + Prompt "Limits Source" + Value "ControlBoard" + Callback "sourceLimit = get_param(gcb,'limitsSource');\nblockParameters = Simulink.Mask.get(gcb).Parameters;\nlimit" + "sTypeBlockParam = blockParameters(2);\n\nswitch sourceLimit\n case 'ControlBoard'\n limitsTypeBlockParam.T" + "ypeOptions = {'Position','Velocity'};\n %limitsTypeBlockParam.Value = limitsTypeBlockParam.TypeOptions{1};\n " + " case 'URDF'\n limitsTypeBlockParam.TypeOptions = {'Position'};\n %limitsTypeBlockParam.Value = limi" + "tsTypeBlockParam.TypeOptions{1};\n otherwise\n error('Limit Source not recognized');\nend\n\nclear sourceL" + "imit blockParameters limitsTypeBlockParam" } Object { - $ObjectID 246 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" + $ObjectID 207 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "Position" + Cell "Velocity" + PropName "TypeOptions" + } + Name "limitsType" + Prompt "Limits Type" + Value "Position" + Evaluate "off" + Callback "% Get the mask parameter values. This is a cell array of strings.\n% \nmaskStr = get_param(gcb,'limitsT" + "ype');\nif strcmp(maskStr, 'Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0]');\nelse" + "if strcmp(maskStr, 'Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\nelseif strcm" + "p(maskStr, 'Acceleration')\n set_param(gcb,'BackgroundColor', '[0.9255, 0.8706, 0.1333, 1.0]');\nelseif strcmp(ma" + "skStr, 'Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\nend\nclear maskStr" } PropName "Parameters" } Array { Type "Simulink.dialog.Group" - Dimension 2 + Dimension 3 Object { - $ObjectID 247 + $ObjectID 208 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 248 + $ObjectID 209 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5280,65 +5108,46 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 249 + $ObjectID 210 Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 250 - $ClassName "Simulink.dialog.TabContainer" - Array { - Type "Simulink.dialog.Tab" - Dimension 2 - Object { - $ObjectID 251 - Prompt "Block parameters" - Object { - $PropName "DialogControls" - $ObjectID 252 - $ClassName "Simulink.dialog.parameter.Popup" - Name "estimateType" - } - Name "Container8" - } - Object { - $ObjectID 253 - Prompt "WBI parameters" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 4 - Object { - $ObjectID 254 - Name "robotName" - } - Object { - $ObjectID 255 - Name "localName" - } - Object { - $ObjectID 256 - Name "wbiFile" - } - Object { - $ObjectID 257 - Name "wbiList" - } - PropName "DialogControls" - } - Name "Container5" - } - PropName "DialogControls" + Array { + Type "Simulink.dialog.parameter.Popup" + Dimension 2 + Object { + $ObjectID 211 + Name "limitsSource" + } + Object { + $ObjectID 212 + Name "limitsType" } - Name "Container4" + PropName "DialogControls" } Name "ParameterGroupVar" } + Object { + $ObjectID 213 + Object { + $PropName "DialogControls" + $ObjectID 214 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" + "ock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system(confi" + "gBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block name: %s'" + ",gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } PropName "DialogControls" } } System { - Name "Get Estimate" - Location [653, 318, 2086, 1271] + Name "Get Limits" + Location [300, 122, 1706, 880] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5349,129 +5158,111 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "1000" + ZoomFactor "692" + SIDHighWatermark "1773" Block { BlockType S-Function Name "S-Function" - SID "1672" - Ports [0, 1] - Position [125, 39, 185, 71] + SID "1690:1691" + Ports [0, 2] + Position [115, 33, 175, 102] ZOrder 19 FunctionName "WBToolbox" - Parameters "'GetEstimate',robotName,localName,wbiFile,wbiList,estimateType" + Parameters "'GetLimits',WBTConfigParameters,configBlockAbsName,limitsType" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off } Block { BlockType Outport - Name "Estimate" - SID "1673" - Position [210, 48, 240, 62] + Name "Min" + SID "1690:1692" + Position [220, 43, 250, 57] ZOrder 25 IconDisplay "Port number" } + Block { + BlockType Outport + Name "Max" + SID "1690:1693" + Position [220, 78, 250, 92] + ZOrder 26 + Port "2" + IconDisplay "Port number" + } Line { ZOrder 1 SrcBlock "S-Function" SrcPort 1 - DstBlock "Estimate" + DstBlock "Min" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "S-Function" + SrcPort 2 + DstBlock "Max" DstPort 1 } } } Block { BlockType SubSystem - Name "Get Limits" - SID "1690" - Ports [0, 2] - Position [475, 211, 570, 249] - ZOrder 68 + Name "Get Measurement" + SID "1671" + Ports [0, 1] + Position [395, 156, 515, 184] + ZOrder 53 BackgroundColor "[0.513700, 0.851000, 0.670600]" - InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" - "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" - "ace.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\nend" RequestExecContextInheritance off Variant off Object { $PropName "MaskObject" - $ObjectID 258 + $ObjectID 215 $ClassName "Simulink.Mask" - Type "Get Estimate" - Description "This block get the estimate of the specified type\n\nAssuming DoFs is the number of degrees of " - "freedom of the robot,\nOutput:\n- References: Vector of size DoFs, representing the estimate requested.\n\nParame" - "ters:\n- Control Mode: The control mode. Choose one of the supplied mode.\n\n- Robot Port Name: Name of the ports" - " opened by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in the " - "\n Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by t" - "he underlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole B" - "ody Interface\n- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" - Display "disp(get_param(gcb,'limitsType'))\nport_label('output',1,'Min')\nport_label('output',2,'Max')" - Array { - Type "Simulink.MaskParameter" - Dimension 5 - Object { - $ObjectID 259 - Type "popup" - Array { - Type "Cell" - Dimension 1 - Cell "Position" - PropName "TypeOptions" - } - Name "limitsType" - Prompt "Limits Type" - Value "Position" - Evaluate "off" - Callback "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param(gcb,'limits" - "Type');\nif strcmp(maskStr, 'Joints Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0]'" - ");\nelseif strcmp(maskStr, 'Joints Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]')" - ";\nelseif strcmp(maskStr, 'Joints Acceleration')\n set_param(gcb,'BackgroundColor', '[0.9255, 0.8706, 0.1333, 1.0" - "]');\nelseif strcmp(maskStr, 'Joints Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]')" - ";\nend\nclear maskStr" - } - Object { - $ObjectID 260 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 261 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 262 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 263 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" + Type "Get Measurement" + Description "This block gathers the measurement of the specified type.\n\nAssuming DoFs is the number of deg" + "rees of freedom of the robot,\n\nOutput:\n* References: Vector of size DoFs, representing the requested measureme" + "nt.\n\nParameters:\n* Control Mode: The control mode. Choose one of the supplied mode.\n" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend" + Display "port_label('output', 1, get_param(gcb,'measuredType'))" + RunInitForIconRedraw "off" + Object { + $PropName "Parameters" + $ObjectID 216 + $ClassName "Simulink.MaskParameter" + Type "popup" + Array { + Type "Cell" + Dimension 4 + Cell "Joints Position" + Cell "Joints Velocity" + Cell "Joints Acceleration" + Cell "Joints Torque" + PropName "TypeOptions" } - PropName "Parameters" + Name "measuredType" + Prompt "Estimate Type" + Value "Joints Position" + Evaluate "off" + Callback "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param(" + "gcb,'measuredType');\nif strcmp(maskStr, 'Joints Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8" + "510, 0.6706, 1.0]');\nelseif strcmp(maskStr, 'Joints Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137," + " 0.6745, 1.0000, 1.0]');\nelseif strcmp(maskStr, 'Joints Acceleration')\n set_param(gcb,'BackgroundColor', '" + "[0.9255, 0.8706, 0.1333, 1.0]');\nelseif strcmp(maskStr, 'Joints Torque')\n set_param(gcb,'BackgroundColor'," + " '[0.8275, 0.5765, 0.6039, 1.0]');\nend\nclear maskStr" } Array { Type "Simulink.dialog.Group" - Dimension 2 + Dimension 3 Object { - $ObjectID 264 + $ObjectID 217 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 265 + $ObjectID 218 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5479,65 +5270,39 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 266 + $ObjectID 219 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 267 - $ClassName "Simulink.dialog.TabContainer" - Array { - Type "Simulink.dialog.Tab" - Dimension 2 - Object { - $ObjectID 268 - Prompt "Block parameters" - Object { - $PropName "DialogControls" - $ObjectID 269 - $ClassName "Simulink.dialog.parameter.Popup" - Name "limitsType" - } - Name "Container8" - } - Object { - $ObjectID 270 - Prompt "WBI parameters" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 4 - Object { - $ObjectID 271 - Name "robotName" - } - Object { - $ObjectID 272 - Name "localName" - } - Object { - $ObjectID 273 - Name "wbiFile" - } - Object { - $ObjectID 274 - Name "wbiList" - } - PropName "DialogControls" - } - Name "Container5" - } - PropName "DialogControls" - } - Name "Container4" + $ObjectID 220 + $ClassName "Simulink.dialog.parameter.Popup" + Name "measuredType" } Name "ParameterGroupVar" } + Object { + $ObjectID 221 + Object { + $PropName "DialogControls" + $ObjectID 222 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" + "ock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system(confi" + "gBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block name: %s'" + ",gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "Container8" + } PropName "DialogControls" } } System { - Name "Get Limits" - Location [0, 23, 1280, 744] + Name "Get Measurement" + Location [109, 127, 1339, 823] Open off + PortBlocksUseCompactNotation off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -5548,49 +5313,33 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "411" + ZoomFactor "600" Block { BlockType S-Function Name "S-Function" - SID "1691" - Ports [0, 2] - Position [115, 33, 175, 102] + SID "1672" + Ports [0, 1] + Position [125, 39, 185, 71] ZOrder 19 FunctionName "WBToolbox" - Parameters "'GetLimits',robotName,localName,wbiFile,wbiList,limitsType" + Parameters "'GetMeasurement',WBTConfigParameters,configBlockAbsName,measuredType" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off } Block { BlockType Outport - Name "Min" - SID "1692" - Position [220, 43, 250, 57] + Name "Estimate" + SID "1673" + Position [210, 48, 240, 62] ZOrder 25 IconDisplay "Port number" } - Block { - BlockType Outport - Name "Max" - SID "1693" - Position [220, 78, 250, 92] - ZOrder 26 - Port "2" - IconDisplay "Port number" - } Line { ZOrder 1 SrcBlock "S-Function" SrcPort 1 - DstBlock "Min" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "S-Function" - SrcPort 2 - DstBlock "Max" + DstBlock "Estimate" DstPort 1 } } @@ -5600,7 +5349,7 @@ Library { Annotation { SID "1213" Name "WHOLE BODY TOOLBOX" - Position [172, 149, 311, 166] + Position [172, 149, 319, 165] InternalMargins [0, 0, 0, 0] FixedHeight off FixedWidth off @@ -5612,10 +5361,10 @@ Library { } Annotation { SID "1214" - Name "Copyright 2013-2016 iCub Facility Department, Istituto Italiano di Tecnologia\nThis software was s" + Name "Copyright 2013-2017 iCub Facility Department, Istituto Italiano di Tecnologia\nThis software was s" "upported by the FP7 EU Project CoDyCO (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b))\nhttp://www.c" "odyco.eu" - Position [102, 184, 379, 201] + Position [102, 184, 380, 207] InternalMargins [0, 0, 0, 0] FixedHeight off FixedWidth off @@ -5628,7 +5377,7 @@ Library { } #Finite State Machines # -# Stateflow 80000010 +# Stateflow 80000011 # # Stateflow { @@ -5637,7 +5386,7 @@ Stateflow { name "WBToolboxLibrary_repository" created "06-Feb-2014 11:51:26" isLibrary 1 - sfVersion 80000010 + sfVersion 80000011 firstTarget 26 } chart { diff --git a/toolbox/matlab/library/exported/WBToolboxLibrary.slx b/toolbox/matlab/library/exported/WBToolboxLibrary.slx new file mode 100644 index 0000000000000000000000000000000000000000..f02b5434bb5c2b45a8d92f1e16fd33edeb7522be GIT binary patch literal 534705 zcmaHxV~j3L(5A<>J!foN&)BwY+qP}%jBVTY8QZqC@AqqWv&nYSNp+{|$6Z%_+DSa zs*E)`A?L*?v55}e$AKqQ$xR3fS52;Wh{rU@S=1<@k*)gwn4^bx7wK4fELMWpf@77j zC51gu3wc}9L^bStQ)b?4h1yKyE|N7}tDd$SeMKJ!fbQz=q0mlcn6|UHR81hmr(x&Q zR@v)5L12$52tY65$nXaFx0KY&f~)Jt9A$0<(a)$l&dE#`BPUnZTG!=0ce3*j+-}ZD z?^e>7a8M(hu@agQsdq+l5U<0o)N0_9I?3EO{& z??G{pxVt`~?ngdn?_AWle^k8YloH)4p|60y!DnuCZky5nxQPG92><`&W9$C^5;BdSvIAj)6ZI#X>_ruzt)xbEyDF!a;_oNLK$SfZ zarBtpO$Lv~?rc7G_Nm+9GeY8^y+VC<+OGL4&^Y1Q`Sftx{QH&?M3WGl0Oid^B;CNm zy<7uuhJY2=1?Ada#_VLStb%<7g{hLg;#Hj1&ALHeDe=r| z%9`MOhhZgMVs~Ul@N$pC8>=gW5Ul2GRl9~6e>C_6k34e=ixTcXe<0V!4?HUl2NiR& z7GyW4Mb~EQM`NG&e+M9TKh-YdKjDl2P4fR=07N}pOzoU4?d|^OuIk23Ap|iZiTamh z_Rz@}hmA?C(l38&kP8&howEk{GmN6s+soZdP+g3~9Ej$5n@dNSf(2E($=YwC+_X*I z00n_=K!}@Hgm5rLbW%@O=ayBuqrT6hMz0umgwTha|3$lugKK2P>Rf*hEO8T)Er+y` zBvZ_(xc==1Yuts>*v1m`0r)AdD3UWLd1f7dHY;ei-t@&?fc|f7F8@6~w*Cq!BvT+D z2T@QU?EkbmTiUwXSlU@L7}?kxTbo!KnmZZV{*RpN9!vL4u|~!(-98X;_Z;K7an=Fd z#$xa0(Me-XFHtVPTa$-30g+@8I1o4>D@nN$4~#{5@pQZG4HJR3OD}+CocPBXIflAA41}NF9>hKiuil9cM}NPso8?C6>WFTV zS<(!<+sm3^GzL3AieiDTr!B5vE>?c6xbvO&c9uls7X$k`pr+3Vd;yP17)I_5UAv+!dXNcIvzK=kPRL2wjzhITl> zp4RV-&h0o#{=F3aXQ-$V*i2gr#?cw!h-FW$n>+q5!T8+(4X_);flh`LlHVr zBSbRO9v^sao8Rx(b@#4yIGv-VJuX*+pecSq0R@M4k}3?!bzqwczf>QX_lS> zbBxkM^!oph*rD9w^8OBntqvp7>+bEEleGHn<5{HFlMeL~_gXVJ;FJK3x9`!@+CWf} zorIdB$a7C6dove-f=EIf<;gX>NW;&4BrhswHU4$xrc zJ$M<@|9nQn25E%t`46v*6ONZt(Ntlz)|wDU$qRTCiS{c9LPx@|JZd&2N5;duU}cUF zZ>_?4Xs*@oQEC`$sVJH7qsZn?i`-Z1~cws_AVG{RDQ}C8Y8V>+Sxy7 z$~D5FA&;l^?jiXKx}+h#_H{iUmxS2`a|a41pHQcdhcgBsXA^HjvD1GIqz;o@@TiV} z-I3>cG&ln}Ey9g$5dZ?=T^qBd-OdIt+2ssE3V=BUg8PzNMDmZkq4)qvLHuFQ0M^d} zWiW&fRYAPogTX`gI;oB5DCkBLi315hg|tsE1|Qm{qq2yCion(DS|I--=8wR$fof`9 z5j;>_%Zg?k($e94xM9al%K zk+bnKYkP%1E&O|aKWHrf>Z{ur7&OX+1D9YZ9kA2cDmmw2c^d+gQZge%WaGUX%u&ih zW%-Ms2k++C2DxJa6RsJG&_3_CkJR3M44K~+md3apuY7>#(X?1A>kQ>ygBo>98#oik(8LEPHM{q;C)^R0VW(MNDIYE*s46_)ZjfTV#m9 zJLe%`!P*GFpBevkFm~M*W?ZyL>V^2-Uh}IgPkr9*MyeM4)Rvs3+q0BlF}9Y$sG&ud zl)qtvfRd=pT(+&kP~-`ZcFN{ri)0rU$BbV(a(<>{Ve)L4nYO=OklqYu-w6jg)xy(q z6B&76 zJK7Zo$dPo8-gA@mkS zE1Y&fz<5Ptt7cSztg%l^Y?73&X{%j{PvYa6dOX-qQdFNkFW;Dv}yh=7bWiY%E6~*ew`AimYNYL&b|V z>>SU#`bxr?Ah}>=7>=g{{n4M-?r<%)Pwjup9cDS5NC2}Q!$)-2+a^|S29xM@P4~4I zB~!i3M>X&dotD#}d}D_{6w?kFs!3@JB#SoZELfo?IiEDu5~ywfeQRH^=)UaPb&~aX#-?6YPFA zOH)Nt$~9VZMZ1>kuHEF2SuKt{cDhme44i1Oiju5e8-+xwf(wR$9hx(Pt%?z8bw=5- zyq;-gAm}d-aBE`7-z1_Uq+S{m3MiKbD6H8iE}%;C<+OwxRn4}sA?J}a2Z>XM>Y|+~ zhB$p%LW1sp`C#>Rz2iA8dh<6?V6bzMtgO|}q1kZvn3tnB+oD8}C2qOpt_q7;vcz`3 z3X6aiS-4?{>NJ6o&k+F`|5DbvXs$!uI~`!VX+ZJDrjtVL(RWKibfkO86vzmwi84xq zsw#UO2l_j&k>MzMFR@*|FOXy8;IMoV$2@YNyX^-!>$qzugMdXk`#nKttt{La{~cnD zferE|*po2}*_rg)cNDEp+!m-k2tFMoAs;feOE8MU6FCLTsy8RpS(Ob_-5RVa2>lA| zt~^Tb@Vy*Y&@PS3{Z6R)HZNzZ9U`HFQvC|>5|!#180k0>qqr4(7zOZFzR?~>9c8Fw zypR^I-Y7U1SE$&0}@;MdzF%g~?F@XHReT+5hhhf8*()k;T%^oJBT(x&2 zEP>Doc+hz6F|vwokBH5dLe$ieOjd{D8gB9dYpfSPj+D}WgZ)&Z!UutT8!~GhL<}ftnq3u9(JY3%wK4xGp z?jNQeaZbR};PFK5^@Dpc8w9;=zZY#;J<-Z1@TW07xcO{~hgv}_pL^*5f{a9=0FE;Y z??z!@Q%Zv-OZO3NwudBB(^D%CR&2TYTI^-=P-y6wSO~bOriS{8XF@i(uLN2JUu`Wz z;Dm+)CW=cG^%9Y`AJEm=zJ-)nK2`B7r^*p#`~lKPeI3Fu{RWHq*N#J_kZvJn2KPKV z)3h+N8=skRu^h4La_BTO2LyRNE${4KW)G=jiNagBImt~)aBq}?_buitt-Uvp9}DoM zyK9l2%t{b(Ic*2Os%L>$H_oFCve92EoBMNF*f_i_!hK_u21T!gYQ=jp#IMl603ckA zF#?I9*^r$gK-BiJiuh`RYjbe0gnj%`l9UOzB8NIGWCp6@E!m}ohP3T0?|nP@2WE7( zugYtRwq``o%A(-ifZ^_!xtu}{rBPlb#;nM=@YAR>D5ATL$YBm21yK_yY%Kz`{q`X> zgnFC4if7o&8KfULOXW;OO;jKHJWc@ALBo47N;)?O$Xw^xIfD%ePPfg5j>U*TS;O^C zg|fiBoo<&(Z*WbAYG^bk(?>vz0Hw-i3KT7yyes2mJV`rqj)*}&skqAX-RyH}T@2KR z7jQh+R9J2~h+L?h)oUgDITn2=iEI6r(UieCSh(hM1aY~r*j*f$B-VZqV$HGUK0=jpNNTDr z!HBcwkT+;45~V|-51+(*^c;-`JnT5_P837o>#^2{9M7KxKQgzvyuszrFREba^JnpO zQB;k@q6@+6jTLs5HZc=W>aQWrAx;})tTu=A2GCPyPM%{uy6!A<( zc;O#M7@aBt&R@qszBa9|$nubJo}wpVs+^K*Z~;eJ@7D4CQZt~00Hr+^8Mb`18R-?+ zGQynDIeOKMoe8>5StH0OnYUo!o6{7};tc|eNGnQemwtOGuhn_Um;{b|F03BHy)f0Y z%N}WlG|I*Xs@LI>ccKxg5^=|3cf|~4$!L*>MAUS-v5$j)8wX-1`JwCva543tiMoGkV6-{ zm?Z&Om61aide~t}^dyr6sPgi_HFC#3AXh0+fmS#xV4FM_PwuH8(pdYRCvx#J&Bc1N zek0JSCdm%@DuhYrZx((uzA1}Ea^-vt7=(0H`tY@(*PX_sC^$$qaaOx{vn#J1H8Dxp zV;WBW{I+N>yXo?E`XMr*z`zarXf_K=*s0DSM$=NGp!`oq4AJPI>$xlP1waII_? zX~nbm6)v0$0TlG3>&0DT`@&D67536NWNFR@^7A$xOhyCb)UzTBNhsVL`6vX}JzFX* z8O44SkL_({F$ZxN00-la{J@9x%W|=%+#-1aMMhk9PHb6eeDeDI{-$`ejRkL*SZEkq zVKOyrvqc35D4!${D%|iqB{zomLwU6f@&0=L?L7{%`9GYwGbgNU(9C8YF;bKj9-#M{ zJsHkzGZyi+wfut!WbSQM?Okxp9QU2{!~F|QzLv!5ODyIl3^ETGANLLf<_J^N3Ip1y zQbp4$?8F&(RrFU*BrIozJ)~bATyM#mxzIkO9BmjKRlt$rVI|vl*2RdQKnA!Nfv4q8 z{25$8k<2Nhl^%)V&YW#N`59BQq*vEq><=#cD$iYjBCVeN^{%NVcNKH>xofYtJ;;z< zlrq-2sbN^0K;175Z};IK>Hu8ymQQHm6IrMIHN=vw=dHJxyhWBFCMNx1^M?u88!2w{ zE?`^Fuq+f{vnTAowMa2HyMe|^>4e6A_q07_&FVKNfPq!>WlH~ip-o5Ky04b``Js{d zq*7O*nyXFN7du8zH%CsV40{g zDx5lWgmxi^yiAC?#Dt9Sk(Ml=D6c>a*!ED1h9u&wPg&f*KxfU~U40H>d zq`!NwxUTRKRUnJAK zYH;844MgFXPSL_BmHeSQwCj=jcs8?s7yUAlG6)+9#Ak8-(hsWwl!BkCP`cR96gi+3 znU;ZyT?1PS3_d6kKI62HsJ$GQiqlEPyR=|4MV~$_X}4es=MDlnCJ6fpda*^!zfO>l zJ0=|fgzGtTOle|gc6~45+1Nyil^co9c4Gg6RS7|~7BI#bR~{B7@v}AJDLI0bcIn%b z@ctxf4;b%&vN-o^KXP99JEtbj%xwmL&Vpni7l~!Orz!=f0!ekMLlETI#vxo`a_kZV z<8^8fl%8cJ#_nrIkNRWD8YQHMY)#YdE7$5~#)J>lgP+W;f|MX`j+?pZ`bBZ=qk%Ll zg3_wNzLXDnkh=`ZtZ`*vG-a&3wLgK}6dOPky3xa z$_H}QLM0-pE2Uw4kE#;D!-_@F4Wtkwj>va*e#ch$cYY7MiFsLcLHAbpXF~%oMf;y# zO6FC=!K=eif5t3bHQMdlUVHWk&Dp|A)#-$GeJX<)yr%wXvGnLz+oA4AxhN#E6N6`< zJC;NhqN#|ns)9~N4OuBorV`4$r};QM5KJ+QVE|Z-M2rw^Wu^ zeV<=&2kBRg3Sg4i74c2Vs%~{axeY&~P`Os}e^r#;Z#CCNj?rISG}60!1Q<_NfCqb0 z^Zyp2P9q6z8)^`oMDgY5=r%10uKW!Hji}rZjnKuAg2vBNW)Y|XD11Z*kyOfs^l~$9 zaI%UUVZf({cNtzG&d@}>`&Gg`rl&vIlmA({5L&mR3D)-*_S?&gVbk--+0rl*m5i!l z-V4UXk&r7ky+nv7qr|uwtXmM5AUQK2^p?b6tbML3}Qi8C&@kaX!(eZ*W+IQz%8}Azk_(R$yJXK?0=XOTj?f z%MA>uNU!jxV9mgk33j15Rz9PxudjMu3_XfCRTJg{rwrvV6I|mVy6WQGvNs-sUx%77 zkL%V`REa9JDu^ix$IZbJ+pTk;&Wlv@hQ|>W@b3_2`P>78mIQJ!SUaZdDx;wQJ{iqf z_=(P*82eB4IrKK4bW9pQdrDl=C?qnVfPYR~)CPp$TV%jVrp3O~{eUXci%#cQC0x51WLyh8)Q`1u4QRw=> zo`s^aX?>l57G8dXj!NSg8Y)nvW6A?O)a9CaPLPqqY}EJ^%*+mE)z-IMoMyw_>$aa#i^319_fX~y7E zSW>XeQZmFe+^XyS&BIEDLD&PmASP#vOn=YQVNsP!S0z(pl#v{K^k`3ec#=s;`Ik(q2Vu6C48q!>%yKI1DRG7QqWiSF0531+vuF363y( z_f>J>{^8`NQS$=OoM?1=v3EgI?*VT|Tm22o8jMxn$|ZEn9@Z`ju23l?Fk8V=VlY$4 z?AZrjuY<^Z2}PMzk<{Z{9p*csM}Rk0Cvz52&H^a=>A7;&FaUUj9yMgTx#}7)B$eDZ z4IUW7lN1u`#ZeJT1pyo)y|&eX;YF!XX5jrDs^ndw&H|Z_I#qaF3fkX{tQV-0W4daH zA8zVOapY90?HzybFS(&-kBx{%5!)1(_>jK#WcfsCWEN>rz6wr6LF1Ef$@4aWsZxyf zf)(eTQO)|8y4BIxv$aTe@p(%KR4T+JnSAIPKc_aEqn9rcL%ke}_i~ESnJj zkbPtby0OZp@<%*_VN}L7oO6xiU`*=vvE%A`AsQSzzuWhryKoQ7`q=KqnX=O`}rE zpdw5o*g{mKxhp3eiPFUfe%7ZDE`lGft4Kehf$`72K;>vZ$)vNb z`jcYS>$0x_YlTJQ5TVy&qpS4xIt?w6UYN5>&|_kJ{E04 zvX3Lc)sP%WAYkh_EKl$+c$j6D(rwK#h>%>iJvZ9~@wdf>TO)xXevufArBd4U8C1JyjJGpkkASEX{4^~lj8~}P~&>_bS!pIn9Cn<(E zNB`rR%O@G&cG@1iNY{-waS?6oxIS9M{GB}0L=T6HG=@#zEL%iwbfCHT6glWegt0j2u`>GlMU8(!HnXZS zQVjYt?2>!dpANS`0RZ*#f_N8p8Qr>VEWgn#q?SGnu+#F1Ih7zO-*{7Hm|n!~im*V; zlH=VarT2G##*^A#Zk?VmG?_K?1HE0~(RLHE?NClZ`(7u3=|&4F=?o|}^^{gAmE6Cm z-LtS(raaycMfqIlpm5a9s-%}-?`Y0?ap+e|t-GDevl(O1D`hORd4 z14CGb(NTV~RbpOeTjLv4J;be6HkOS;Bd7v!s*a_KgBBMbx;`v|Zvb8~yq{?s@xL6c zU}?uU=I(V7RusQ6y*+?xnWPyKduXoyS~;}IgMA{JkL1iWtW|xL*uS9>_@(Pea@m!n zc{b@0@CQhVvo{Wa1{tcLKUzKuvCsDciWs{GzhCUMTSRc}5_eM(xrIDupQw&a zPIGNSwMZvgB6YD))ld;l75TjGOcdl%l!hDu7>eR3ag$6yOQT!Z4ZYPH(SGen%pN93 zLluD;LosI%RiY|?xcfzZpX*vsH4p+ir1O6FBfDDQW)2`VA2C{w4@Dqh8drUcoQCST1*ckPpb#r@|=7-$MR#8nPz||-tP;0tgx^2jgWMQK|Y-Itv zbL6um`XJls8Q;jPi8CjowHi*&e<0q6*ND&wQ6#4l|42&Cf&U9 zV9OVvbfzu1LHe3%^_;~0TiF-4Zqo$9M0vC8#d4Km5cnqbtqhy@TP7!c3sJhcdN*-t zLR}{{P?S*~aDRXPm&W$*s~>mfWDffdq0PVmU_n@{?WxS&IAzgN?k`5E_zU%Xk94iW zapOx8x@wJSneNul6ZSI9)YfVQWaD^U+K&W80cece4`nfOPB$(;XpkL{il?_N@$zx~ zF9{-lZ$9VrO^SBrkCN|$&IicbVeV2Y%4_;}4D$r=OqHl1`{p|4qmGoH=)!6j;ps|R zqLmyzX`S5oFk#B*aC&|y?Lq&*C^=M&Es32LTLq98SasBMcy*Zvv7m};hb0ah8?YmW z;D->Tzje}FhEZ{dTGNC@`v@%-%r)mr6o4xiiAI#TIl#M_H?V=HB@?$oF#ef^YM0kX z0cXQ5C@9MXF?*B2-O(JdkTUnh^=!1f$?$tv3TKIBl&z58{0ySn*CM9iY!6&79*ceA~ak7iCv0bZt|*)GPV-EmZ?4T2t{hiLkDM znnSdIt&x{PN<IpOKg~RctF%)-xHPw0`1c^^OxZ`loDFj8^w;dl||H(-$q78A^ax9ZgS9g0oRP zDD;$3tJ+f^RNC-3Ly?QsLcZ0Wbh8ovwt+~14blDoPF zKh~1CsMdg(VEHf(>B9A)_VBGTw0D}JE>4PdlD*t4OY28Xm>$ynP;WccZll+V`j=$$ zt-;3_uJzSHyjwx#OQDb+{cN6@-1M$k!C4%I7%oj(TYx`-*Va8~>Z)4S(&i)1#1egm6Vr$xA!o9F zNT_-Ai*|{5XLMt272bebB!pH(IntrGUp*t8JL2(<@KAJq^y#o>wNWuy1=&z6yB*SG zJ>J0td-o2}K!FPzeg{&SQK2??Q<)R~lM-xJxc1T>U1~aorcs-N`N2N@f{xtf4oqX2_DX5O#?PmGmX$vZIoLH&s4#PBKaqU3P$hunD-K zQZjNL!PCCy){=1KA77W@+uWe%iN#f^e{UKpDlZ1{ALZ`V!$sT4b&W^9JzK;)Mv-R| zm=$j*dQJ^RF^u|l4abOcWa-*IWPo+2sU2_WTBT|Y{|D8|?zv_4&Z}HCr+75^0*AY# zKoUo{5&19)=DKIQ@+3Z*+dt`9+Jlm{o3=L=!8>uKuOHv|HMwXLE`@)BcqVtmXLg5rPoi9{#37UD)IPn)Ws z4^F#dLmItPqs9#vUXb5Q8kGpOQ4UBgcs7P2q;P+oK~V`eFH=YNhmgbgrWx3u#m+lt z+#L9KL>tuo{ca!(xrY2T(c$dyA^AJ-|9 zqZ;&obr>}`E z_tCee{G-Vt%Q5#h|h>(ym$4WbpP?3{KUTHuuOBED@G>!w@!s3+^r*lwSGYCQ#7+$pd zbK-7fG44K)^&>rvI9pFT&j4x-WOuW0n2q&?<5? zla}I8{>Ou;iZDS_DW&&sNprOYN8H4g8{s^EZ*{f6194exO$J5$$`mDNFu4-Go)hxz zxg#C-<@)N}4-=s9Shz1KRyQ)|(F&dJhf;cOk&=Fa2Y-lB8^2pHpBemSYOMU!c#EW8o+6#uo7=}^`6#SzM?m#2a7HL!= zwkUwQv;_vcOvZjhjb2;7B=oh4wtGcY*V7N*NlDKPUy7Hw%|g?%?Jwz!GWU07yCd{P zKR8Ji8dw&V>^8;P<#yDRLh0kOT^oTvnNYI8f>c(ETG6Uzq=f%8KwX6rtLB*Cf;1bmXZ26!f~ldNDp-I3^Cu2Qa9gbXFgFf>w)S-o7IHM>lR zcYzpzVfgHDQ`Jd6*7y}|C|99L>=8J`tnTVl=7}EFLNMgC40Y1>8rW(z3xrF21c);6 ztJqz3zy!r|!2=INEI|a_B4RA}s(7sh%P3rxvq*xsy^uXR7jh`O4M31LAzuAtQvRl* zz`i4q0~YVKo{2ivoD&JLs}Y!AE4s4QnTIk{Or(V%BNu!RddOl(g=)cy?`TIi zR}w1RtgSba5chF@ad3C=vGf`Im^}JVO$OFmTbqFi;x?_qk!|TgAzx3Bw$1o82Mm>34Jgs~?E(@Uw*LXi`ecU|TLXn382b3*L=hs>~a z-)5V1P-KFqD9$Y^VcY>fx0x~_o82ce5#b3=RB=0i5E_in3XWKeIUba7j?Y6cjteI5 z{nzp5kQde&*8;iL?Z8eyf|xc%cJF&7yZ@NWO)bk%Jb{9u%MqN*nY15zBOO?pZ#jzg z>wHLiXf5C1Wo&;|z3uZ9`R^I%pS7+rgp7+6FmQlozlym#Dq~A!K!sf{WqS!o=|!S# z3n$?KcXaTT3W-me3Drm!Nv3C&!^st|^r`l~hHLcD5(1`R!X!1rNbE3|6?4N7FZ$JS zMd9=MsD&BQ3akx!WG&eQSCY|r+)H2T(Y!<}IMWeVy0FnX**a6#)2@*hiBkaA7Sl=A z6!DidZ8#eFON1Ir2~xhsi<@A)@f4HtL?N-FC^$D0X*fhf=)}Mi!xP5-;#A$-7vFTL zKbm_ZrqL6}^>ROPp$GE)^77aEFroRI%El7$LO3~4^66 zqb4{g)1=9f?n#5*V9O=hk2kUVOO{uG^AxZgkF%lvZ4l(VW@xJ(al{=%PR)DMdnm+643sd*0 zJv=Si_2H|7-1M?|NV>quRa8{W8ZGUEfZ6HV`h(*@RCb7~xr*Fmr#1M)Q~CW^e^x8m=;FyA_;++4hk zq5*H_$%v=SB1RJQGa(a;AvZ?M2zds1*~DOg5K9Mt3IHW9$3Ce6zH$(1=Ap^35^!+d zGOsNgNM`zovL|wJ4CAALMsyCj8N8AGidCl*hvp;6-BeAGAtU(%r;jA9Mr` zl>lu|Wbhx0#lzMU@R{mQdf}B>SaRSgjn3R}Pn&Q7`9Z)1esngx@W!QQTE19dRL!@8 zT3Y_xcx&S>0HAG810S?y7SrfbiQod+D4JRNwt@c(#+zh8Xa*9s0FQdIn^)f!*wAzoOk|AbW4YbR5g^>fP<+$?l^~+ zyd}(0v-&hA&yF+Q!74t*5fVkoclAllYg?E!jT5e9YgNLN*&ME;M@K-Gr35u|Ur2_G z@{B46`1#h)nmr!&P*b|9F$*(n(*l{0V(}UEeSe5>KgZ;nZZdM&M?xOZEp21c^N^UX zyp%!Qgc*>tq>ImJOtwao6S^~OzVL8Utnyl#LPW8L@*~M$9O;~|Y$zf<87vR@^6v6# z>|RKT0Yv|ufGZbx!Bq6u*@kSw0Aigd0Y2aBSSitdquy z4#_yY|FQa}lTOaISbo<|VQ2Sgk@j*%|UfQdw^P#vwTB=x5Z)dhuRl)*j3W@ly zlQcX@CU82d4E#$bV~iopdv+M1u3H^ZX(GChs|%^V(1c9i>!!XnN1xU;a9Zk)+bXdVHCu+@wzNePEms;Z0V7!9jiiGuBRe{((7Du}(9) z)n3cR<}GUZA_Z5`K+ZL4){uJbm}G-d0*+p*d9aZ}h`rvl*RgAd{?pZ8Y<#i9WS%C38HfJXv)8-V zO6nd;le^$@b0Q4)Z}R%Deu7GE(q?1^ApM2y-u88NRpvDg{hX42!IR_M|K1)v zm=9k+4vEfm+9K@LE)$->dwoHS-Af_VzmDsW9xAkQxP1hzgZD6w=MmfeZ((sEK44&c z-(T#FW7_DlBYH){pZSNBgPyzZ9Zt93BsQSQe3;YW?l_~yV;_9lO*9Up$USPW=FUWP z6vpF$#NoJ%G4&uN$EUd{$u{^xtvAbqcE6k2 zgSSYx8Z6#n-$&|>0a3B@^q;^*6k+H(YuzfrmT<)ut>)ldB;v(+&!(c+tz78S4!FJG zf0kX>(vXbaAGxP&R_Ss*dJ&)dryWqq_ARCJ9>^q#R#~sx#ie3j{qvsieaBVNFHM)U zzQv;*V6BxmCDBf|b)~-J<(-t%B@e8$ZJy*UZOl&M@@)|26uTsEX|D~^*PpZ`?@OiN zATKBTp66WhSGmTEf8;IhCI2~DcnQtsYLEZ7!muqFs}ZuY50z=oUfxM%E8sd~le+fp z0aWyG@l9=EiIBF0RrJ&)e?aLh{&zg|KW7L6o)-6U&Q+=w4pf}V5(&B5LndYK^t*_y zB0{KowDVf!fs{jw$00w4W1SvRdppA$@fZSZ~eLCD=mvGl!@eBQ7ii% zN>gt;c|r=~Rwb+3!QzzM@+TlZ!T-!;;^A_u#wYaBmmk8RY-{l~V!6v)KCF&Yfu=0o zl3mytYPtbuj_@tPpS|iG4r6dR%n0r%uE-jj8eU3>B+Bwh0yNhDvS-pwm_H}lU^0p~jIL&qeXT)3sJMwlu3?mMOWa*r@ibUC^c96;aq6@(XJaT@ zqVqtPc0FcYQK+L?NImuR$(yLTJ{KDf)l(_lO_5jbf~(=5Kp+#F%De)r&t_#>jB9|> zX%q^my9arM;!euxzZ@Y_OHGbN;ViPVgm4rt9i=lPJLDbAonh2g%MsFfEW)?4 z&<;n;k{lf!L|~A@ak9M zsA2Ty_dD)q7K#fVUlkIkao>-XxXK)m=aNPngBXwmU9Y#XCcpphx8czayXH<_^PGl+ zf&CmCHX&&a$`muOn3FVKoZpzob{y=rV3`)YFk)^E=(e!&9j+@DrNzUMzm~r5{}C(( z*V}#v~{Nj@@-;!eZmz;D!hXrU-?R2dncEIr0Z+qcNOM02#kLV_ z>y1Vo`f{?-bDKH_1X!fqGB_|nAHbNu(13iEe#2;^e~nha+f#50W)<%C!+DmD zurxPh-pqG}ha6o{C<%>3Gs?P@m8uBx8y9GMWiZCiVp6S>kbt;IZG+evy=9mMDR`|7 zXd%sTF8Md8zf9V*ucT;9Jogz;O=mzTesPT=d>aI?e;6bw3xI%|>~g;oe7&?Xy6HMB%E^957L2BI~L zYC#F}E^x#uVaBHzo8A5Oj6-=9rAxstA5!mE2a!t3U|_nHzq3mtTbY86@GxI;^?3kA zoD&md2ecX}68BD@HHIWj1pX`@{`D~0&(a-JWj^{&ofvBCT28ZSd5YZiT^!aqzV%FXmhbssgtLHPK;RW8IsQ@W=OE;9 z$iB!1c&P~-Q1&Tt_=@T>yOIj(I$5_>R7%?&+csKP*%nz$Rc7}Zg)Z8a^anDBqKwR< zTBO_@=vr84OXeym0mFacavRP$`$&uMHLBcl{mi2Diw~@8b5b%28r2ggv)Z?e9(k9_ z1)2lgcGkFY%bGQ4)qM|~PAZPSIT3uqDW=A@A{{y3;7b?FJfGTV9Pdj>O+^|2zO1?JU+xo1SemTvE}v>nJ9(yO;o$*}yjp46;t50eqr3H~G*l zNHi*2wZ6y_DFe2`2(c-7%eh;9K)1*9yn<8Ed~egU;0qz$W(JB`j05Y6$m1-_(( z@Q;P>{rqaUMWIl3YD9QJpCqaV)XZL~KWJhd@*yAN3>5w#GaJaPHc_u@TQrfP2R)6D$IR8X7nCbUSUL$;hrI};$-YA>rb?sHTIp`iKbh3 zs4fZ|4{H#3J-*Yvfn*ExJ-uCIxTDfyab2f>|Juw5CGnZ@FtMi_14|KYYlC0dq(0!grd8TFj&l^f+P()DOPlOaUg(0?gkuQ~srtV}aRIoT`sKLBh8WEDv+8WU4Q$arusN#j)?^;9e z?y0Bz<$&K~3lY4z^peCtZl<;@)rby13u(1LCRbaHdBk5=|2ytx(N?#n+vsjOC9dXB z?!L=ywX^SDnuLLeS0TY2MaRi{06KhDte^DlOK(z&VHY^I2TcXj&LW@A#zpwwJ&J0h z>8(*Ig=|=cr~J4j7eEj<@#LSS(P6?aP$EbTd*2Yk{Nki=OCJe`34A1nX8?dh6|-TuM+;oH-Zve|VS^uJ}nm?Fq;Q|w*O=h<8h z5-p4Gd6?Ec8^!J7!@h&|4XHiJ692S>C{XLk{uFGUff8{$I;XK5I+@VZCUG10-k#sX z`2R)KHwAgt1YfpoyL;O1Y1_7KP209@+qSJ~+qP}He>>l9#Qt~hLp|K6dO0`a=83F4 zd4{LOI3ljUQHuZU9F`Pb8_euO=*R+H=zQjk^-O-a)>lL?NsLENF zb%ZY7;M4un7SePVks%gb&6kIU%-YmNb;P0hd1Z@cY?ZxZlONx93~S;BJKs9-WE(jS z(kI~A79cc}FYDSivA+EUBKMJ8u`Oie+qh-hwczC#v1IGZ>;ucA`G-A)Ht#ZbJw_TS zE@zX@DC;tLD+U&RxzVIV?IHbqJ2G9wb~-L~lTOdJ{lr_Py!04oBlE--P}ppzfSHR?C10g>~XCwCGHDUQV-Bf zr9A4ZM@*T=l?_#u6RB+ql7zdD&Jc;$y zix)Q*=k!HL^Y-oQV`sz?DL$9uT!o6&8LDTra9;_K>m0Aw*ew+8E*|b!7ME{Z3pRFE zW7j@RzGKL82%CthB#+#rQz-{hzW1QonVv_l?>~0aG3!C9%ef|9<-?op&4!y4 zJg~&nL09$qf7V}G8YUyZJ#S)g4GNik7h26(4YGmgZln*n1I#iqL$EPPM3q2&gT(xw z8(8OcBIEI6u!^V<35H3vVhP=0Gqa-qEK3C*%7}3|sQIKv@Igt5=Ng^~=uEw>@je%U z#afHBiev)8b@q7}0Jr-O-%;)XH6vG3wz^%}{A)&KKK+a~-!9sRjYS^qYc#!YSC54F zV3cbp$N{&`WDhqOG{G9EyVbdE;%7H+9^?p?RC#~Gh(%od3r+m%szE{fjSkgSnlS)- zB7DplYcc^LTY0echXw5D4o+=SL!xtih51^1-%ndR;8h^(!$pKqqyW*DGT@fz2J66g z(O2e*+$NbIU^q`!><1R(L(ETP?>@HPcdZOw!___b00S-;yj)aP-!3MeM!})KJUXlDY2JfDc+o5ahz6Cm*=Nk( zgajnhwZ91}zJnOE`bznK8*pvhv#`)=f&hfzfF~th=Qe`)^e(*3kU}ys7|tjL23*(n zO~unj21yJ2I)y5}r^<#Epmx(rWYuq~E>c)Bho_pj41!nNJgDDyaUQwB20XK1+i=sGn3J5n*CS+?DoD4BAl%WB$WXuv z0u$umAgmgLH3ZWc|01G60nRIMAsxAzzM22eF7qC61R9~t)gY2=d=l-HJQIk@;B>hK zL$;sS~O-uRgwSfS@#;w!y9?$m|5rJm^APn-{rE*)c7+Mc4 zel~tJIvlV0b48s)+inryCG%Rhe@?M--TXHW-c;Stos0zn50`=P7IbO~+oFYhMmc7p zG8qB2Au4C98{9wv_rB=n<7=5@pY0deS^Db4Q6X&0q$%F%I-;vU66@R{D|9iY{EbVd zuDm08qUD5l%<7#veTyE39NRq>UAhxv-Uy#P`5k5229!zLACFmQLQ`G!u2^Z5sqn=` z$K>1h{(Oj^yv4_@|#2#aw_s;~ly>(r5C z*%2n>M(aG9h=k!IQn4V?Pay>OG)mu;!-B?oB}cG}adBk$+PiQGB?9#WO_XCM@t1vK zCOjqzHVAgmg==9#We(KO%W(9eQ`NWisv!fZ9Tw$Ofc0=_9RV!(Pf&x^A)^ zqcRewy%<=El3^^_Pst%yfjLHBK^fXXl055ZAi-#=acS*6i=xu@sRG@0(;ajSmW&KJ9Up0LxeEz+ zvI3e!(arD>WkLcI0mz^uDIb?K#XEAQfYPJ%2hU{=KD?-Q&n_6ST^q@hhuVW%?a3uA zOT-geP{KoQ^0>TEkc6Zvdxj$EBCV^l^}XZW2awfvxmr*fhoSTq?lG0{;^IUJyzHgO zdj_((fg4!lPWOyW*_mtQEraH|y3!eJL+4+GrFBt71!YPwJ}m1G096Y3a?d-B?u_!Ci|iD|P{XycjiakdS(SOIES8uDOPlnHEGPMB*| z$lktm!#yfi@B3tphKMOrRD-f5Vzy9y8LfH<`HO+58U-qD?CME;O)-I~o*KH$TyS_p z#>kgueW(ZRO0aabJ~$96*kzEn6aq#UQ7Q zh8IC@IHs3*r{rslc_15>RX?1UzryTWj@+GK77`@M)0)nGS5rL$y=0>fqp`6_V`yJJ zup{Z8D`{H=VeZV%T6yzpyqz}0?y-^YLU=Z{b`TmJ7hzadukfcRZ*>%|5lrn_M{pPO zY_`5-X9R5Jr^B)*)uy7e>5I^0afQ98$*O9lfIP)!wO|C*VwzSQr~n0{<(-b3?h6?H zT=ifMBNrNV+hqy7c&*eJQj#x(8eFoI7@ki}Vbri{FG(iD8jh^HT`Lm~&H0JMhy-S7 zFHf(j!@Bc|X`Z;g!f@F-1H~>-9jUKJSXxNixDw35#l#5xlexPtYxFEZ>GG%Zj&mMj zyGi=eH74wx2#`yYpjlgs%VGtAr7AD?rKjcqObj32awL-9|cf^_JrmcB9ljSxYQ^7uCbdv=+g%-)nWlfiyDBdMU~_wC%XC zHzR@COaq4+_S$sb(MB6RFa9Sd zlkbbg)>?OLo9eICwAEoYZKL`Ce+pY&beBhZMb_B12X4tsyyAH!V1|6ZfE?plVeDF6 zV+U8^mTX_S1TNAwE9({y)Tmn)VGf#MB|NYl)krKhq%x}AF56=%lU&;kp$liSQq$5l z|LN1g-^Yx!rPEI(0y^WMb_P-{@&l0VRA}{wFMVL81y0I)l&PHT!V2L~E1%7UT5?Gn z{=`0glCA$qoi^6ajZsVK)}Dqr%E=58@SDecM0}eAnu#;u*N&NSfb#x)UtjtUk%o_P89B-ws z*ly+6b<|7##ZU?e%qr@q<$TDNBdSI1tX?mFzK9k#2Dv3iQV4us*q-6-y=P&^=K~|@ zH7U(eUuV?M2YhV7eN$L1|% zY?IO@noZ7#460L$gM$^R?Y_lFRA#naxi(*0ly`jrb~$NEqdA$ZWavtH6Gvehf@tZs z)lxfwx>|oZG5Mk~l{Ml;!CVo^xIi}xERnY|LAznQL;<%p@OgvlUgi{c`i;E^d-m#z z>BC`1bsSkcrB=tZ8AN+M5#vZmIpZtK(v)mtNwRv_sM=G*rnviP6LmSo+^v}PQVQ)w z#WvUsJA%+lb-`%y04fN-M%w9BT_R%pV*nj-K?7;*n31m%BkHUbo6j?GLBq&9{2D96 zVI^NB*xy?SnO`hp$w4=;5t!mvk7h(DT4EY=_V`7jFQ?HXxCScrL}We00FtjMgcrU26WB6+l?=__-?==@9~Y%{_Rt}VbsCm|@&JcO8MHDIOu0TvuTJle zg@b*XjNX9)xH5f{)HHeh&^&RPqv`|5@Q^Wsu*J>sGI!=M-q%ipKpgb zh#gS9&x^LM zEGQwXaJRq7ZAiX-(M9izm!6MXdrC-Vky#)R-} za-_^ZwX8|RHA zUI3)Im;ATaXG(}Y)KH($wWkt_Vt$Cos#*oSl3K~F<(g0tRVC_Fi92fZed@yKy5qeA z7;})<4aivGF@``*`<5-dF@VBtEr`j1Q>@k#I&1KvxnW`rk33bSnOLYfH2v!RqcYtHK;IFU(D zv=;SIoruG0D;N8U2N-Zx_be$k>&L3Sbf&HpDm_O_&rAGv{sqg zytONy1ga8`yH|&F9w?FLu99pNJzvqRO*k1OT^jgOA77|J2?uKgQIDl$j(H9m<{&KT zCDMs}21a(%C!J0ng0C~|5)-#M^Cjg)Id(L%F150%G;TI_ozQf>P({Nu+y^K-U&teE zjVX7qBH2R^TH*?=;EU}lCQ=Uq#!lH-A6`wV2vj1dzh3Buk59U&b0N&h#13`U;c2R)j%M?!?;?@8v{yI~jLsBJSn-$Bm4;Kk1|03O>S9v?Q_aD&3Q< z;oMVfe#;{%l-d5pb*_~2tCxf54shf!wia4Yw-QP`^kgM3{z=M=Plhw1QliXnabfyJ zg72K8~n1f%sHZr!Xrzy37h zsw9);FCo>{kF5PAScPk=j{T5LmY;%aN5?0?bbUqM`@R`zccRQDMq-2tO}$i_b5&@6 z5n?P^%5b4$Kz#`aC~vaOLsH0fTixm4F z6sRlQyG@tQW`|rc8ty`?=QATNo+Q8=pA%K6XujQA#S2Y`|a78&9j?hO3dh@-U5Vp(mcD6269h^4|0eXH#w$ zo|VfN%T+QVK%2Qi_kR0qol7M^>Gv&C;1xzj+--*FdCTj971mrmVRvA+?5o-Iu9(~V z5Bv$6ys1fOqSV1$PTNLE`&ObV(6roET^v$zR7GLMU{O)9|FMqV!kx4i;xAQW{(PI( zg||H3)b=ENa+-MU@BU;* zS2JQjNYX4;SjE3&SFYbPmwx*qf0{C~mpngLZo025*Wnov7jVHgTRiDyD#fw5G)9g0 z;C!2w+gQ=~mhJJ#Q$Z_Za7FFZQDDW2s#Q$4L30e()Eiy7!%ON3+Uk?-sRyj$}4zlZx~ zX8dVP04qO-v_dVv2~liuNFfl$!XkBsRCFJIK0X0IqU%$;8=LZe`x)QdJ&gy3l^hQ~ zn*6Z@GNQ}VDy6+joz+I` zmTIAtN`2?NqAW?u&`*~7_GzgJucVY=aCT9Z6guV)^5`Y65UF048sm_3y)>$(t4htg z%{^Uv=*m|usc&H_EB{tyN3??_6seQ?_&N{AD1u zv6zvHWu8LOP8N#c5z_DF^kP)DeaFeCHj5NJt3&&?zb+2Oct4fVgSr`77g5+@UAu;A zS!L`9tIyDHl`(_Ewvd;0csG)c#yqhO?dsOjyI5iDBl|H9?#6gCQ?E3P2yLbnD^<8* zURmLn$?p|y5$(0sjh8mWwfDSJs?xh1^9TgDZlPwD07!al72J{aL$4lLpw>p6gBv5bwhYOl6t?f!0l zpzjmVqa8UxjdM;O*AS}Y5j`#`pKtI7>zSPVec6JjvbLPfu;Gh zn9;&~#oXbs@R!hSJXk$J%+yIgXyz`G>}(S48~8;G2494FLRMwD{=_qjiI>*UD^nLQ zgl)u8kYm@hy32V2<5i||V|xhbYx=i;5;}Zvk1r2T5lKCrL;)e+a78{s`J6TVMdqta z#?7bBDw{7{AG4Hbvto<(8h3(QSF$Dsw*OxiFH~P0+stG99FCK5ciOYaA^l{x)@N%U z+Mf3cp2&(e6D%_ATjq!cH`rbwd^UB%#US`8a;VSdagOj4!HPdKa*C8_Yx~Q9XsY~> zm~Ii0c*$IgW3h)Vf>Jkn(G zgp4}q?gfG|k&Y9(W4zcrt0C~Q1RPaO+^?dNP6X%`5mjYuS^=ZxbI_=gqmxCeKSzW3 zy^dQG#3PXo+-|2)9_UfLFAjovtn=&K#I9tUJgu`lbr^5Z&d^G%I}WqLqTuLh40|zP zAecYj2h%jcwM!mLX_sq=?miESkzHB?LZ>sJp#>YHVSCKBDJx5#DDyh#OSRD$=v$F$ zUNa>43O31RI}$GQIZ6^0&W=6#C&9GP=-9;6c40z%vCFn$*RgP371!arE|XUx#eLdo zISTgOTGw*+!$#LW61~A`>k+|$PP#8bzjVhZUP1DHq_4qMQK}7?RVW%aVkp~H`l3Ot z^-pRPS~25JfVe4Nx;8ofsu2j(UxQ|Rcf$oqbN^K%4ms$gTOE--idGXrc}Bo^(i1Gj zG}iptQtoGE&JgqMV2YYtpo!u28}=PBtwO5PMIlw(-qZ-M9Gj&laZ7MOR>yQ!S4KvD zF51(i8jL61-(A3}tbx!NE{q(+JP_4A=<*s@t>|X1s;M}d*XAn;|22M5tdbE2)#nJz z#mrd&_M8kCl4zTmHMYzmI+;3I8CbIFO(_0NEBNE+AZ3$FjW~)v<)p4RDrb{>JX@(c zr9@vsIol{QQ^z*B+yL33UvS|qby~=R*nxZRXmq)HP(1rR|BO+Zd+Ww^c~G#sQi9+? z)_Zi5IY3X-6f_<-LEQO`U-7T`N@Ez+|UJ70F#4tpfY4 z>9*P)iT3(54bCMZv>`_dacwVX9HxKF4c(SqtS$Slyhj7;3$@I{=j&p}wTceK9 z;kmH}sT%w{)|vE3f;WVtkj2syG&WPPq#o&b!Bu{8iIBt>Ohc{Z4Vh?e6tP%=z5{I4 zd-?p8ux~5fv7e^h#7Y+60INq8@5kYD{5y5&i7?Ims1qCdnkL(b?^~~isBjr_Q70h- z7paD*w3NbAM?Jg^tzn5?`b(?|XJN>$61>~^be}C@CsGwpEfDi$t65CM%JwK zOj(&}^3oIBxC+Mo#nWD^ptXQ$J?r_ms1(r+5#1W{l~4$TYWeOM6RtQ^CuO#HsU`TV zdfJE35K{rr=eMjzsBB7zGFljl=)nN!@{=lWZAOj5j2D;Tx49mTFpfME|Eb55s7Q%Z zQZlVcOqjR%RrWpclTH$*Rz44FDR0@O7byG%`c%iHKVJ8iU_23TcQOXLwOm}LZFgDv+T`lFF^#L-f+Y3Iv)y4m?nCG_aS{n`z{?tK20-zA-%RK)Ix~vFh{L=oZyXt zO|zX{^%ZG8@Z0X4R>>gP^y!t0x7CW&#_f=nvVcjkrI4x85b59P1*H~YhtBzkM(}0FL0?;U(bp5QLjpK6NlugYXg>M zVm^6g`^sjl+NRcN4po?$Z-5z?*Ehk8x zyJ|Bd=>-JTqAOKaCDSgtVC_UGiDXD>`9qaYXMQy={<6W$SOz7!bR@D|f)d1IuOC6A z(7=H69|^rSzR>{81hSe(Sk#vkLK1@>FG)ru`Jf~KL&lOP6~>8R{tBD)4+9J*#o*~C z29*}*PeQzh0D)`t3-<^v%vsoD#$D&6luu*)q>WhY(MiBXm?UHn=U!3>|9ZZs8PNSU zRg7#K8Fzj9G@0fU%W)Q)lX)B)%i&nbBdv8$E5}Xa0cW(J=wvwH6aLp9*|4&PlCdc& ze}aN-!2>yhAN?-$pLiyh|52EMpkz^Pj@l(&v?WxzmYpjMoUitzY7quF+6#e53lN!Y zCt@j>prmJ=Q*&8RZ*&V5@Fq?vbXL`}{biFZx~;JTE#tNh-}-h#dj?=ctt!I0$vL zyN)3U_O*%F33mKB0Sm9z8vCUbgu%#`kuB0t-WGTbYKiw_LW9_cLp6Gy` z)*vJ{fViqEoF=Mg zQ0)*&V;a9|2vp_@!%vm-`o#enR@~`bTQex`BWva5*2V(5()x-Q zOR~phI5~;igWg48-T6Wjj9;PU5{2t~`4Ga>!Bj&vj1OJt--$IRFD_;ibGw@HrBHw| zhb^aZyactR?A3pgB$c`ZF3j52(=Xd7@D!8LD;Q6!bu|HmY~=fYqUps}%25}iV@Zxr zs*txMW#yRs(T3w825_)^M%T)@h+mv6dhsgH(g3M$6to4LktKFNZEQV>%0t!jp-(!3T)l4Gv3ULyF0rx4}M1Xg8C%r z3r#mZbxrfW6(m!LS`SUJ#9Y6c_ZC0UMP_*_w>;GkAy%a#ss#X4O{(_IWE+5d#_7~{ zA*&hzDULhY(YwnXDcjx!2+Q4Ens5H`H68qW zRy(R>MiC@ciA^We$Rzi~N{>vFZyX`jg@5O%4v)|l(xw!pua1kk&QY$Q$#p>zGM&37 zic@%Ml6$1vzwtf9L|VRS!!R7o0_v)lhDB~)+M^=)kW9d447;-WoO(quQGHVA$s=NP zHfDo8l76!%X>Qr^%_HN;tH+;GI&!_2xr&Yqkx)igkz+lq@eNp`C{LSQzMCCA1!5lN zx=y1E%*oH)mlWX3>aJ4YjP1-*?@Kj1Bs2$5HvAp=MdmC~Vf#cc5By7s{t=%I^}@8E zpVJK_3FGt#I$32^Lc0KV&7DkHT2?rUBT&$Q;XUJ+l)9~T%n)6MUTLlPr4xZ?kz-qBr=wq-fpL&Or(c}V8ji0@xg#pdlc%u1Y(lLq zQkkw?G@HT2bY-_pygsgZXr$7Tl1@&nz9G)~ibTK1r!o8~mNtwT=7%48 ztinZpstwF8CeYttkW5vY|H1p+(MVF7WMZQ0{rM|=ulHJvvXj< zGsSrEP_Q=j#z3L&^_bi9SPs8?xQMJ3>+_5BKk8InGz>2RO@bytz+%7?ib-_1K0mWJ z1qmCEHvljBEOW%BYyB!LFsic2vVJ5!Hf!#B2d3N^=M%Jla$Y}IpVJV8U8bJn?yX*hO3Lp5s zQ>E_a<`$D+#TUH=p-G8UhCmvjmRZ{?T@=tKe_)-GpwkwAkSGJexg2BWPQ>2hA$oW) zlU)wg|FZ)4e{59#^Dy#43s~ra00JUr0RqDKe{EEpoDH2#|5)3({cjHyZ7;hG2_(R7 zp3$)~%^8_5_@lXhBvPUM=s%XCtJ*TQ2j0ra_CPOrc7D^edxH}W&vsmEkt7zIT(384 z<@P5mL;vcb>oWt?wjeIyR82%xNd#y+!HmRB2^rJGTL*1dHKb9H7GyNmO&uMGa<+ON ztpkZDtC5m8tEh>0uHjETajdsQgiS-`Q5?hGordxfp2Bil=ZD&4+VKts=Kvkc6P+KG z+VXanyEi@-g<2F002+C@Y^=fO3?j`rP~!&-#dbA<9>k5zXg|^_G2R^aAR+D~7?wZG z*lHiiAb)*8@CN>OECa?(_%;}M8nw-n61HQ$g%fY4mT$hg>Y+2E^pIs95hOBL1J5%a z?nmHOZAQ%owni091r+1%uho|uPXe)k8bTG-RiJHke7vd<6iW2xxclrjsGXKMIAoa) zwEPQ?ihNKuj)EXC@=h8gM^*uF3MqO-711)D#8!D(x3>%~g%}E=+*3HZzUy^oOs(mr zs*UB(fc5Lv8B29&vAt5C2=YaKbINy{$v)(&5_c)w*zR@jCeCNI-QT$CK_8~q6EgLG zILrR9Y}ELOY!zXS1>_7{Lw(!Zi!GB_!{e`IPvf%)726sL&CVcjW^*E{a|bXI*mUT{ z&fS7>Do7;k= zm_r9+fq)uZ&!%Pm(o3a*&ej8dQ}M5*gVG_&PYL9A-Y5m!7Ncq@Eyra4>3JsdRS?} zHUoL`xnm0_x^}`RJ#vQOIP+)=s9xsqhc^V3)p<$5X$!t%4>{eWJChqrPCPMA!)>b% zD;A#(1-(VqbOgQJcCA}}do+yr6tcd0R1sUJueH1$At-IcI~)l^l@cB~I_)@@D~KD@ zHkiZxJCMb2N*7_8aZfs|>wxMup$<~F$-X!i)%cy9uzJqtFqX3NtIZxW-9|6-R?l~d z*P@rC_ZWh8ni{Mn2xg$Km1bbEFG-qW2Kzz?#zS)1Sz)P!*oIm~H0cS?=E+fsD(`dG zl=ww_96e3i3m8tE<{QS!_&h`>z+%R#x+{WC%@r)oMg~Q-K^Cow;`6J|6u#+* z!&*)bmJju6eg4OOC4T#{+?m>$PP!;7a}x9QiaIJ>wydloQA)S)Tq(kXd=1k!u2+00 zpr!R%+YLgqabVtj6ytVH(Dfms=zIPwG`Fp()@dkj>nny`fkBIg<<&LAdylbO}(b!ehv3uox*3 zL|mX4S{_Es7)81Kd^lmSSaPrHfFB6UL3WOJy%6c2fBzbDrl!ULnmH_wTK2~)GJ_V< zzin)HBmRj{gdgG~^Uva;Tw5F2=Pjb@_EPEx5Jgv1==D+*G$t90xO)f*RD0B5B*xvj z@rwfbWR7Ix88RlsG(HQy$Aj`(8ZX&p)MzHuzci%5b^0$IO3L;JVL}pI zQN<|V?J;Ua-69g0hY0@#T`ZI0@cb)&RS}2oe@A#9wAD0(*gP;fjEIt&dH~4}#lIF= zgBAA!_7>0)Lx?mx9-3>&fkgA0$WRd?9I{tn7ql(4$$#EMm5u2P$`rU16?XIilo*cu z%aEBczT#L(NF0df%0EL%k&M&XgSwhlPBHL|OW#ELPDlZSSrl3+*?<={3}tRAl{^@Y zM1nC8jU;^Sv6?Z^=h!Y1nMdsNPWN}<*>z*LCvvbpQ5r`5jrp^hgu`b|JUZ33tf{d$ zas6G(DoLS_&6!73V>g@eDmfiMmJR={cqrDQ1XPB5+$YH*?C|&Iip>F~wXP;Y&&?yl z|G0)Z{Y!V34$+5<|Lov0K!Jcz|D`)^VLRJ@)@RQ8DjxQxPP+f)x)$Z}|BT?iQz+G0 zP&b5CRpIbD@w5@1K&!EtQ4?e%)<2$I{Dr70Swt`9UpDb*ipEE98V1sv-BP-#!j$r> ze2R3n$6wc_9T-H3?cuR*>l3wQlw?MV*4YeDWLzr}@fF<_sF(4zBuWs^+?ihIK^(BP z(ew2LqApxHx|Lhpf@5JOa2$C&2RLcrBSwArrPY--qY-OU4m<|0eg5iH>0==U&a)zc zE>E!X8=-|knTj+G7R!aP82rrumRDv7L&S$*EOv4{8=XGqP;Ps1NSG+TUNCQ?v@xcC zILaj3eG^e+cV8Z9iPH!kTa>^-mS1L{}6`# zhp@h*skIXW{eNr8;;z)-Z#0NIb?6TLMGWKwmo7y^H{xgDnM7mUOh3|$&7B>Uh9+(H zk#0D`_nA0Qv)(NWQNDRd3t7=9+A=WsHlfVY!rSM`tAIqLekg?%1JPG3N=^C`yK| z{wm2!d$`f)%UFT}U>JT)QyJvPUSR;x`bEnLas?|>k5|%y`+US&qGvJ5#fDpruOUqw z-_TTn>y;0BlPycOFZxGfexsNZ*?{)vvu;@$o^7SPo>DlUWGs4jTOHM1ey2fy$vpiC zqrlQO^}~@#uJ*`>F;-iiGOD^co?GI~A4*%pZCw-i<({}01dDs{|3T9IACiLXfrjx> zkU%%7kpI7Zp_8?Yg^{D7qlb{Aotu-XBfX=&6VNY#oxcBb9tsKy$nD?xf8hV$=f7BY zGq^ z0a)*b`xO1Y`2o-Rt@__-O>{3Ow|=7Kej^f4_@jM4B~+W8>;KBArG`hdHTv4QSKi;> zb#wapeSeW2e!~7~PTis{K0o!deDr^Cp>j{(XDL2rAGLy@zy$X;-}`tUc54fAJ^;%> zzhxKq_Me9{ z-}*(r-afw1HhpgT@!XBL=`UJTow$5`>W@0>Z(jEO1%UMce(rztcl&>$AJPtP{aANq z{pNQmf4m+c9vQfvZsDKOPW^g1^)qfCKiV2AccT2Vpx){LAS-|m22o9$f2-e$5dgc` zNBsQ}_eRQE?{J#Loa26MN^#B6iD}Hd~&(~0LKw)3pmv+$iEiBaw zz*3?CfV^-)SO@Tu*axhFe8d%d8-ss~@8$b>0d~{lm;4Bj$$y}IA^i=n{uS)=wn=}) zrGEgJ{1<&c0KUI~gld3W%UG-rK;L|L?dJG{@D*@xVZM8UJ_axw+XsXwef#YO_4P>q z;6?-1!(3%9ANY&Y$p3v?Q@S?C@06+5?tPcVo*aOWCSZmAo1jiT$?(AD$NbFCYNhdC zJViOn12*|TK+`II04C>ZC~N#Jge#8n5=j>s9{Q!2EJ6x>>M4)C<4B`Jc9WNP2p-`>v*J;UJ(? z7N7V6+S^27!rD~#1!Wq@C4#8WWSU$*-=rLHME%3z|&g)TE-a_AhbVdPp__~%YzXwY2gb>3+5 z@!G2GUj5r^14XBKk2E@p;RfK=qPB)9e`1|V+_pP%;}LCQ@FMe{WpRn^qjZd!yA!3D z%%Ce<-@AaKb}het5ihphQ79P{Qy_0zXZ@0PYSiiBbM27^~K)YC(Y$F zEDT|gaQZF@NFma&Q1os`!rZqO48NJCemzk`-$F{jby~CuQJ?0oWK%~u z)eOk8)uTD-!m&X}k>(=m$4tW*Z$2BC?&D3Xe_GHl8{?a<0pF7%nZ`)^kZFUPlOgIELQ=6yPZtWkf5 zA)az@SXbpyTgsIwb#9@T))g`rhR3wyOn7%3Tg=DiDaLYl%p@tgA~k*Ck7dVlC+25T zaf^%Fqg4lhUAU*Ap07Byhk;r`o*MJ|+)Kth#yW8|jLCl3tXN;$(Lr-8OoaU#Z|Zfz z^I=wF{tb$uE!LLMRoYJB3Y=nh8v$y04>L6Qp#l4H4FP@`+`V zbW5m;lBQvl?&swq)WVNSaKHgRQ%}CATt>t@(*4~8^Ewn~(?H+b%)>KIL-U{~CxYu0 zW&(JJ&z;P>#<}ykhkaMp^!#-SxK{arWs6!5k~v8uojyTeraMzop zwk1xvugLTZzhDx{FLxH)J{s;fdgB>-Bd^t>xKU_h=wO$_Mjoo2{_?o>RZ(u2#Ys?N z*}e)h5d_EA6R)DO*UyW()@ACnzRQXezq3^AyRC)1pB-zx0=JXH8;yA_9uE&Fh>=5t zbOEu?py-y$UlGB?*-H-ZDCNRRLow1>Pizm`q|}io@_Z_*LB}L}4^gPjk2J9`SsKBf z6=!a-jlA`wzDqrYM=z&Za(hgB3lPq3KS(2@@O2_mEUrs;sAW4m!WkZIjC5X!8Q!tv zach&xm{x6yED})gqCv_< z>W)quA)goWSC+~aCG%qV%_6KpjTs~93q?%*rtlI6{4Gy)CWVPB@mSM2YfH!=`{rCPbeu{>XE@1_(`(Du`0UeBAQt)ftc4Ocp8i zbxx==umkgYSwic;m%nrR)eJSVkRnL=@>Pikq8BtpFhD0T@-D4AvKy^b7MNj#=92B= zTCR1lq6z~cs8$NLM&H-I;Hd~W(S?$0V5(h%Ta|%(_ir+FCoW&Kcwf2anI>{}gd+|e z!_TMpY?i@J#e5zJ2eP@vOi-G2)XC(;XFg55A+XydXHWudAr*+1P#2zw_jo@i*k#4B zbwC|G_hP=XfXiI&{z^1bX(Bd^i`pRM)s66~krt%GLDIvQQWSmc`J#LZ_6ZiG$H^U^ zbRZ&YbA#Tma+bTlzMjI#f;S4Jvqs2V`Ck|tbmChC*JOWL{+6J!F0G)s$5v_=2v_vic10xQT$Iu&(j$JV`&#_pP_-@x3X_#jDBC zzWxE#Nu7dB?9V&Dm(#hf{`q6VpqcZzMtZt7vGcn9#|t_Hh~{^7D6IR+x=P?jSQwj3 zN(#T%p{wwDgqv3v$t&YEs*RL#qNJcUVYQY5B|N^a#SqSjqEv1D4X-QnrtfO|<(*y3 z2$9R6PB)t@jJCY=6)Vg~N_1IE*ZBkBUsWcahA$k&8|gFj0|Y@aPFNrml2SK8biQ1H5_ zIKk8p#x9b~Ct6ONZvEs~{sivDd1xD$IvEE9gZAsVY}vtAuND6X7MM=yzFAh#GQv?W z-FW9d`s^{iDh&VY@HDA=3B~B&n;S(;+?qpq(ylaIV51GQ91#9Hp+e1DfX@Ag6U16s z=EmD>`e+`b_DI>1q;Nt-$8IckXyI#>dFO07yT)bq{&-FX-VutXA;B>JGOPo8W--J1 z38$$kD8tkvS^^7#hI z%*+2e+*vjQR7WSd2l)^wLMlT}T~#R6+q6b4Dn^5zHP45+?L0hTT@Xl7eZQ%na&AWJ zt?}h?-^QiBFj2l7fIVR25Wxb0fB>{HiTzaXzTptoJ0QL}k^*O1&Otl~Z##4gB`B;K zudD*)=Oe9~M&4z`2s+kVGf73r(RG z&ws3j*Jm?+5n%cCMxC81$iM^_XXtnPT%Km+YmB;qQhkj3awEDyBB+YQZ(P^QZU&jX zkB8Z3ahvAfzA`?~_(BX2{m%SIO?%ho@5ZHKZmxwE;xSI!0Vmv5Bx8FLs=oH}bSX0N z71*~uJ4sc&zqoE==nGs=w3oKAze)N7>#z7#@;!U?vTpskN5DiMP4WO708Wbqq69AK zL;*K^hlt1Ok8maTfs4@$ll56BY+1P?X5lVT8(HTH-QF=AAnn8IY~(rIJV?l$n2HU~ ze0aEJ(npVJfLO zso5RFAT;KKh)45~XBr9O>P~<*35j7dbL)Wwpua7LBrxZ8W=lR_2j#g5P|6v&z8XnH z%kNWfMU_#~cH|=drLi)GeizZfbnt#em0tnV-NqTzzJ1J$c`&C$TIoT@_l)A7z?3zz zditu-a6TShm1osHn$3aOX>9&EI~gu@|d6hZpOXSEHzMDZiQmup066i*L?qS&m* zIOAJ2(Cde{!swWw1y6D-K!4cmIsfPZ2Im_0>46LuSq<0eF9!R_nM%FCEH`Gq73j6h z7iPNmPrgtYaysQ_y%Ku+%L{}pU5m-DaYBmzHiCH7PF^*Vm8(mNzt%U7A$zT(LhX1bT3V{zobyer2s;udiURth7SEuvC8xZxOxl^rH8Au*qG+EL=re zw4xXcC0+p?ZV?gGE+93;>77-^bG01R4F|ndTW;&eVvy;2RZRiiy8&IZNfeJ(V_MJ- zI4;WS{3Sj3VU@bcRR(!kp!-pZta7jWVV4>9>owgwe};ZG3-C$0JqBX0ihFkbenjyW zp`BR@$PWs=s^{C(tRHe8i_Q&VY_OeQYir>?Qg!**gA<<%I+{CQL zCzeDZQSZ1)c;z~b#try<>9irda%UWN3q*E(Y9mT$ZuQbd9yjWZmjqNor!y-MS*XHr=j2%*& z+m9hlk!Hkpi|C}>h!9EKjfX`cIM@ixPuYp(oO`!^ zVhMcDWM-e9I8Td+%;N`3iJbe9Oopr*!@7-JWv`@;B^vyN-BV#?L$_r{Qv3GHwpG4R6*@Y~~5ZY>LRpd3? zdUdcHxU2)}UFWSzi{S%T6%1Gn{^TljIAyUzOS=@ui=jD)N6pC%c=yx4*2*2h zmPaU6oy7MHE1klcHWej9DGIEsTdL6W>SWQcK0iO4XA%OK11IkI;w?IBbroPNzCteE zv>kGWfMExhQQl!$sh|@{1CjFdO&R>#;@mzrr5yfLfmYR{ky6w&q_|6qg0)CR1MV_` zzZ@BS_T(+g#NEZ$p}+k2vJ?>0i1qpc^q2+E<|KmLm_{bm;8#tjpnZ4vPaWfM9AEw{ z0ck`9s%j``GbAD><|7&sUdj-L&5kIshyLg!LJE9EFZuoe9*766`m^*z8;Ps{`3Qf3 z216MdzH^t(OH)Hy&rbTzV*%ZNw)@DiJ{mJsNTSh|cnCd@m~$Z0;L;{B`fSiJb|3u7 ztfQ_PFwoG5Mfwofq^yZ$mN=Q#y7de1N$IacaTV_AT9To|Jet!>nDRaXS)Kn>IgFKP zqKC~D8`H4|knmQOUEuO6oEfT4B6@vt1T*moPI6gZ3(#H&Z0{XHsD?drADZrg3dAoF z2bEb|sYf;u5`V$A#X{j^Pyc{Hpm0i{NUIfYAvQ;Zpwi-cSEXL&G=CV3JAtlTYz%UO z=s)YZsUu7j`4?Fu#fj<^mWk07E*)x=NBAj%Q7@{wDw&@i!Ou(gr0jGYs~eB`6McXc zqn^N&>_`sR6S=SPWpn<^M^avm5Z^R-SW^J7_;yu=@Q5eD0r5&uHSuSssh+Lz38#5${dTRFvDA6g=cq*tTaqP>R|wC0bt z&JBgQ)GD%qlbA$VM z(SSS;$GS&O$Ru@h6=0SWjU56)AQ=sjyuH4OMi;zOKa0tLIQ{VjiN-Cxw_eTo$!!*D zl@;$0K+(8uce(q!v4xdKFWqZrKf;=?W%Uafhz9v#CtVu!97`ZR@6ob$1YaZ^j4b70 zj7dukg@RK0+?zbSa)l4Ndsh?VpELWqsIBcD&CMz0f-=JX zeOERh=#TIl9~lS-X&|WG@gpz`6d<0jjErDwCh(mlC)D(qv0Fi3g-qX{-y=_1I*Qj( zW`>d0HCwoA0{c;VAEOntd*tKinXEW2{=`i|{`wS)li-SZ9nIbtgAJRIV?Fe!TBem0 zZ`y1Si~rJv-L5N3i+;3%LcrDfY&bw^eP~gP%75|WS@mR1nS5~?L|p*!RuHORdH{Kn zL78)1Ir33Ln&-k{)Imt%i7TiDxK*Y0lTgVoR63v@(;aF{aB1O%e3hDkGzSwCKx1aX zbpkrJYd+Va69rn#fKaQG(mO2p#yx#5T|_?M$YAn8Q=j?8_D(FhS9B-S$yZ3mxfH5%;}d2IKbkG%?g+!#nO{yvC4QGG`fz)L@YSENZHhIe%^89xemX5XjYPFzEC zCglp#;>AKW3M|*h3P*6!`QVw6kdlk93g-bvzvsf*Ss1X~ae#y__68r(Dys-K7toNy+ zy2@;hRz|C(4KUVy?Q!d=BL-Y9vtgD0`6*V$wj@=hN!PWdk!&-%#5ncTRWvH|2VAQ z;Q=+zcpQxh@z7IV?Ol2cWB$YArf5Vw=G7P}r3LDeTG5cK+0F|mcm~$wp>P(`M|wqk z=iS4VV_aL5NcbaChu|bClrN%zkl$b3H~7peU1_d4)u6wlif#RRBin0o#a8KgV)(8| znc3!GSDbd$aQ?F?NW1Ng3S_*w8q4#cF#sE>$o&uSYH9FImn9=L`eNnN2 z=|H*46QSz&=p_^P=l8(Qc-;X06?FpFiw{QXpK*x+;>+vZT!Oas$ zcO!cMDooqwcsN26-x3d3-_#n?fXRpihKj)Q!XqdLb1Jl0>nF!#Y=+DM$032zmunFe zvY!9&3SbpO_uZClHmox6ZFWO$rYR~cr{ag>IxD*iV-7heKJfe&SpX{RlAERac=7X(zDb@``&o^)G1t<_|dcBTiXg>hpaH3?lG} z&0X+06!0Nvxd@S*u<4hr(760@zZSmmX?5X!eB3JICrt+k?%O}lE%Ze}Il{VhUzNjk zhA|9QY62M@J!IDA6fHEqOmUc_G~@%_s-Y3(au>**>iAnBS6uY| zIx~EB)o=8;rR(|I=d;PH6(-zliZ664k^hOi7L2~-X?O*sIxo65Lw>7gs(!&}&<1(4 zrCw)O#b1lNoA7hWzoyxdK#mqGewvtz%DCI72L(T1-f}?#7=kDMOd#*U*{Q5*13T<{ zzj`vNu|Ie!8e-gI%#0W+(`^vfKH`irltMLypO_z9mVdJ7o9;?FkWSyt-jCxSakyFc z6o2H0$6Ih>?={m$f;nhe^Fy>|odoT!jJ-Mn?IsnDa?20H>*%1AQ3-5yKBJmrGW1p! zESqL1C-l@Uo17fTm|Z3(euP3N;fV8vg#s)|KB+tIN@U>1>>-YJb${@yVVaBLFtHH0 z{iYUWB#mb+fa1AONOMa57r zC0u8F_3E3jsCe5b&f)sH8u83|mkfHp6;bP}^_Y{$ z-diCP+-r{GX+>@1bc3*X(4tY&@ez;l8m(+?W{F0P$?1qhM4nw{*ff5Yvf9D=kVg(sc!dBbe>U7^B#*ng zZ~Oy+fp0dHg$%ujmrQS)+m9T$@Ykcv)2SflQObrb>++L&g<#oZLI~tk8Tgl`n%cyT^u!0SvoBkBgI&Z|`R`?p*>Z?imzHfV?z& z&VlDiC@G_G(u9xjTkJcxUe~=+SaR^Jh|dqz2DhlCM)8j`H*sSF0idKj>9Mp1M}chU z($RCP_zWhrs?u|o9hc+$(%E1Kr@{95c3{zBdy_Ri&PsO56e;x(Qx-`P?E?$dM|;ZS zu8Z#l0`e5%><;mM4cEUM$RXA-OSZ%*vzcOx(GRHoOGrqF1 zz208V1XM1I9G;<%2}f$@VI;0peRF%MVElp~GBf(u?fqeW1Q7f&vC@4JL-&XghDrjq z2S?qI)^V?!uekNRR;!K1nC1})akGq70@KoOX&_L)4oo=YGG~6j8fcEsS~tQBuS0%< zOo8U@HF`P2a17%GF8eXjRYlG04Ix;v4t(&-b#jEq&?x}%IK0Q)Ss*(STEkw`6zv=7xjJ4Y)U5NL`d}KP5rMR$ z_rt_(1Q&|{MgWmbB6;}EX+`-Y^^q-@qUL&5TJlqe&ox~~Y^T6C^Y2(~d&2sCsi(R4 zZ3czI)U$_=_89(5cb9q6okJ-)Nr0Y0Dy2bJUYAVdcEUCqMMdP_oKYA#YqfOAaDsTq zxPFRC9#SCd=q?_~JqnVIr)*it-ZN>Pl9Jv{eZvT|I7oeLxA`2`h^10&8|aj|8t4N^ zva9Zo#?NY-=F)ZOYsEnUGhW>VPr zJ{c71<99OjX|*r5pN^mRAy{PF^DGi!6e;$`@pf(vaA4EoWSQbve)KLX#=S7-6)t&( zmiZ)WW8LQEv4=$VPe`#K$%CS-^AFwfl#vh^oM3D zXG!cTXtw|fTDg>Lz>2pi_)>|2u&}W<@Ji4pjwK2fxDFL$v}Tux^A_S^5r`1S{Q$_a zUYxWT(nsqG58Uafr?^j}<_rbuthdXGGIilevM#)9L>aaa%G?b6%mC3Gj|XWawLMi; z2HQ#*ihO!9)aF)q%f*uC6q1t%i&Tc!!=As?hlSYvp2Ts~zuuMj-iY3Qc?E%Gb!rdU z0D66#s{xOS%^kb;@l!?7unA!q-7eyIF{t3xgauF2Xei((kUYs}X@_5yC`k+5aGbKt* zSYq_**AIoNk@d|P-;@CHR)6MWHzw)E zsZ31kESZI-EPrxHOeX2_6PjHef{9j2zi}y)`?~@LurNo;7>E^(nxFkbkTW#`tN8#` zr=|g%b2}q#1K)}xenJWQ?yc#6ci+anty=?FwoK>+1FIi;biqy6N&^{#B3ATY<(=yq zz=SN^Z6BY8R`Pjv0y!fLFyH{F zTWWyLhEn$9wmmdwEl0&w8g)_c0@#Jv8>?cHd6S_&^I8omboPuAJ@G8iKD|6zpQ!J< zqHhV*;~|+#AX;QTLCJ|vp?QAa4KHzQYQTM-^_|NPKll3n*+$bbe8hjlxR&&w8c7*G zk={UrY8@7|i=HFjQsJ{#5o^?liF98LgH*(M#6OxO4&U=ah304SOVhIeU8z7Q3kXt- z8lU-f@`qEAs!OxZbO-w6*DX)cF<*bZ>4{{hz0*kU)?_~eQMlVPC!=G9&yj|YHVz7& z45@!50pc7I^-zwKt&0;|5fSri@+bgkdw=R>mMvdI2GPm@ho4=Dx$ZuSeb(*y=$}2} zUzexkCAaQhJW(m4=Uh^-;x+h2g2fh7-|Wsn&3#`){PGDM9FED&jmi8Q*5UFV{>mu5 z!cFbuT{=Se%16jXlpg>??zKv?m7fhkDL6kkjKFInz(vtFu<}aj49q-XPiPMCwEyJn z=I(1@yi8DS@XkdmqVV=K+_-^}(4LsB$K=a%FRztWzF*h61$%f%)+58xdQ~sF#q=X- z?rFIPGjS}`b~Q_!w5=4jD(lRY6FA+~@pTa4#*F7@Rrr=grVMzwatG-q0>HEkL%5rI zkd3QeStR5`62zHSPdqonR#ExPyO3dTtwG+}evlIT=g;cy?5nLx-$O$IeX*QZbv^I3 zNbt(|CEyc0SZXg$@1ns{zKev zz7w*VTP{Qb38KiYV2@#7p(g1kh;{gsbdgal_NbYsPYEftm9I|cwpD7a^heGNuMBvN zkCz_wRJa|O1bO-W^>+kIxQ90Eqme^LVvR)`;SMo0F#9byd{r#GdG+<8)}9}d1&2C z9=&9^7^JF+yJy0|rg_`Kd+`iq3KJWLei~SExULzH z6!l!JU}0XjNIP-3^&1d`kK_(iCu3N-2=^&nLb(e)WC0w9E{jtj+U(Hd?+E!eU>R>` zm*#6gUsHEZK<>~asX|f@2u{z6}*SEQ-;aed0xo5wAqu}J%m^J5D9@AP$ z$V;1Y>u-3ae0DT2t(8=n>fN_Ek?7eNWnFq=O7pHUZ6Ebld$HEzY9cht&^I%@83n>S zu!cIaXDaXVc+U2M#)|3Q`pP=CBeAe4ljDZ@qICWch6_GMtXlNvITKkG0#S^sk;!!V z!;-GSRz$jKv-qpjAqVi`mG^HWUd30^OT9Qznz-optM&SG?4@#vH8Qjj__J{%^GALG zy3Ow;rR?!nPj}|X(iZp=fpIsoL=!3o`16-)XlA){>DjfyCXixE+ zaMWp|JD0*)RSsAtIC$LdkU$>qn)aT?z7{ zHxWpT2td6b%`QXmeEvP%*;oV(2C^S=rF{#gTW1G#lrhL+e|%v~ZC}?+%dI^Z&9rhZ ziD)z)F7aizcf3)rhV``?R8Sw@RZci2HU?Ho^aRNb~5(G9}JR+W|Po(H5Zy?7w2G5uC^K{>NfLa6u&EiaahyFMRY?3yrKl-TMYxJ zH?w%qpP#2|F7?KA5sB&ba^2;-u&PdRxK&!|zIcLb0K)|6##$@3luTXEkyWV20OU|` zFBJCX^0+xf%01%5i9Tf0&GJM|UEe15R5<;Xx`5^9@2c71^zzg~h|u@-jVX3uD1crp zZ=h`Sjb?6u2r=ij$USOYc?4lh)_Sb$fbb3@Ej-V9@fpWwId5zjmw(l-^V{G}9+s`0 z`Ar$n)-wS6&O3aR?&_M~kd43o(^IGTZp{Xfon<#A;2+;|Eg@r@lal-?B*tApc>Ux* zKUOPK+1PavZ}s-imdy5!fBjQB-_S;sW{sR2nT93i7xAD!1o%_E04e>igDMFKv zMQ3qjVElAY01n0KC&w;9t9FOPg0%s2e}=P5#G?~riD zz6AGfeh>P1k)KN(^Zg{8i*Y~GAyi9=(WhIE#zPqqdZc4h07JmN=L5bPc@Uranu&tT zNE&%1bEAG~CX7W!m{D+4dVEsL_f#L>#HNo^fwJKBI;;>My&tGcIGrYUI@1sMLy=Yg zYiv86yQ_A4j{Ql!Pi7N#GI)BzhR7$aNhIug60ld4%6oTmGHgx-InAe(@1PEm1-St9 z<_72tD*P?S46YU$!GXer3h{bb0Y~c6*G67x>x?C>?fxsv9+LuB8F;%ehN~tSWJdTNE0-XEM=Np>rCLwCFA#v36P1AC4l?fB7FD2=2+{# zkhk}z&KTcuJ$hg%d#8R6@5k(p#NVdo5j=)t(NMm9g3IGB9pWeP$`AeX)vlPMi?)mCld2|v9*1x(;$ z&V^}+sz!!e1}FT2CrUIVmSZaF=Bc$r(~SJ4n8~CpU2pejR!J`%S^h7M&SJ|| zAc~?N!~nNpkl^mjaCZst^^bnjtZqoEZk@Y#0>yr;lNS**Y->%YJ>9m=gr?e8RYWc%R`dbXEMxHg6eW z-kcC3{>?}>r;WI*`VeU!o}1x2LFUl`L}Iybw<*_2{1I;cG16x1GJKEcrPCrf7x05a zfN=WKcey{xOM)p_b^-a}HW8ZO(l0n8Uf;y24^l31yhR^+)B?MTQAe}UBop{|f-Oj5 znK3X`xiaEmeN^j|MBg2*u&mq*e6$|+CxdG8e!-`mH;#q!3WCZ6B zx7w$o%^wgM`eA`EAgT z)mS&gu=BIrTfA!?c~+j&;*aO=-QxH!56`Kr3;49yq$b@hN3DaUCGrHuOecO%8O(Te zwsh0}9q~kR4C*N-mpjN=-> zO%I-+O^*fmL)I)~ehcNNO@-vjvcMY1AJurw{T#p-;W4|!J)gOybjWZdh@g|vcYN941@>TA_-UA?q zcNJG1{O+l$905M)KEl*dz2mx{(O6Z@cLAh>PM`w~l~oeW>Hd_3u@ceY;2_Ht`U47{ zo>?4w@aWx2EyCS{05i%D+7hQQ?o?z(Lhl5WqQk^G7On8cXCIMNhA9Jafn4u#XJ62- zo#)L_%<4BBTm#Xt$6D;p+AaS|RSkOd)O%K+p*1}rUf|Yh!f@p0yj1z_x^b9xfKQqu z+NU*n**&m+Wd>ja8MjRxqI23Nv3%a&(e#4S5ld z%X*E6F5Y}JTsl$5UsAY3PR47rl%rgvTbp#;)w6^wmA2bi?(CMV%d0ng_T{L>L$+;9 zX3>S?KSU})Zq5Yy`mPrPA+o3N`XXu@ky~ww?zj*!wQ#^F$!1+7?6#Apki{-|ZL@yd zeFSm$$u8X8m^o4TeH5gghERvK~_24`S|(LoAYw*jm;HFEP=z zbSP;A_8p?Ypw5rkN^4k9zFA>i@)Euc8_AL1zsreux2qf-q@pw@?G3K`M z1iy~%yzL>+j;k%Ni*_SiC9FdVvU48GnxwdXi{LfdPVM}#I46JZZpy0}ht>Phga)uh zy@p8H^MOrW=k4p@s5v4IL`tDyh16tffO}Kc+eBx8peORlugY~>kbJ1MS}9QFv4Yf* zY~;^4^CPS%(9Qjvym!HRI?g9R&0J;jP<-6@uik}6EXN>LIZ?@=aq-4}vVbMXoY#lw zt@uWPn5O$4tGJ|Yql3g_p;%3^mY>)6(|)$%SspDLiYlr0U^RD1`$Kwlyz|?$k)GPz z<(XKwWwVfSn{p_;g=b8mxfjR@nFtXT74|N}F{J6(P!7+wyFq@(iO76xZRyj^mYzcQ zjw&IW{{mmmdc`LCL_D~-+cTDWFa5D0&ys#zf4gn=7VExJSf9UrL3NfyR;tj^izf&? zxAKf)-4r1Mt}>oIGAZVd@!bMKeqqzxkqmvfZkxpO*VR(!hJYDib2=Q9_c*Y{yu-aBpvdFX(Xt-qk)4_PK0E; z&6-Lj)B`%=6EF`(>SspBRB5Vk8Ff@=UOwNR6E)+G5(iq%)LIyg5uP;tV4Re}Q z1Zq|OAR|R#QOk9;AU)q+R)}W0t;ThI$?OAP+&4=?m6|qgRXfRZdMz?G$F*eT`6zkKt^Uzd{H! zMh<6aVRyUyZt{eniYqpm@g7hg)|Y+3yV$ooAZnD(}Dg2!Z<7vK#GM5~g+WndOaO8UIxQR->t z|Lti|Ljo;EBXgvn$m?(#ki>&znU=V1#mX>(eIDbM*ibeIN^Ts^x6)V^&xhmJ?D2sM zi!wdM-?xN^pHOLzsb8&wu+%_vdpRFe7yZ^OiA%8qWxrCDsEX>gf!_pU?$p7)CTs%A zzc0gCc%9Tcl!cd}-Ou+?N94W;a)3ek0v-)hG~vSM-%*xJa4ay0z_)Isi0}L;>?55c zv2a?sIRy2TV3L22i;QcSNUxotskb0_;28FU%o~!m!V|4}wyLSpd2YS#9q~^c#Q44YVliZ9GjF^l( z2whT9?+V}Vg|BWD^hVt+^GHyj_w-K<$Y>ks>|=OhoV*d>LG9GdgBk&FM!!)R4@+ZA zHF5JF>O4`(Vp#Y`oFyueno{tLWl!Mk-z@~PuG*VGd%F2?r7fc;yx@Jdzz)S-Hy7_a zywgf?mg;#NjH^`Ocm9)FwCLHSJh6x0Ae__FGLN;fjqH!7^ z1DGG21_pdffa%xPO^Pt0!pou;7on&k<8REPKBL(4K3%8`e?{3=Ui*dJ=AF5{Yx+i6 z<&yb!bTt&Q^F$A8vAvi+ee=?rB(ywL)p0V6-qcq{;#`dfKk_8IF#F*x~dl9ZH z0HyL)l15_pFy%IUdF^u)#8}ns5-}cBkP_&g->vw?!Fd1~D7Hl-vx%X#B>%Bi`6oKh z*zeXiqGuK7_IQ1*C#yS0NbIJV%iTc&a^cW;PJ*$~KRsi}ih<@%jod=qz4%oFmb09W zWW7ts!oTyF`g%1gI^cz!JKAxt{u*ONYwE{v?s+0iNV*52PKF`M?pHI?E>P$+R*^!r zCvHp|l3CEgi@u*ShO~>XL`0Yi@d9zb{nDjE6*6;q5}ubD4KF_m;Nij;x$kb`n7a#M z2#ls*(l}fc+LK%}^gVh{ahxxN_*K|K0oon!sEqcRk_GJak&n=BJ$F!5+*zGm&ESPh zL|wnff_ei|^yKJZbw*N`*AuNpM4#B&dnt9_xGFqSTF{4Itw)Byl`8aQt6IcYb&Mf% z=*_Xj*NgyZ+b+zrMX?59szdcD>%t3*I7PiwtVjj*YUAc5X;Emz3-gHa^#?S?9+>sY z$;Yzl^J^MdlJCE|nv4NIObjed`vRghDYhk#-$eQ81*ov!(;)jN3~+*@N-Et7Lud)< z!)-AOu@(oe&s^lMlPD6L0bubX<^L=WfNaZ4 z);NZqipq;LE3v)JtBooKuoO3$d(U=ceF6Uk&$&~{H)&wIR821qXFfyU%-yk8gGRHv>)&z$6N-yId})5)Xb+br2hY(yJB zhIe`_*HuT(sV2I&bRgc^h;}El`2xB+wVTCJ$)vEDP#=RQ_ex?4fV&49m(PXccvWP@ zHj+1+LgW=Pqn(9V!p?U|1H^fk5!q5?mr%ODAN)k~6%Unu!l-pQb-#>38^nz0qE2=Q z(qt(zh-oU#vvS}1@p}Z9N0$-_w08@aaE=;^o2>P^j%EpoS=o-V9ydaotB0LF_r&a=wh4&Gz6zU(hc-jL?6CYCe7m>#&6z#s%Z+HCgd) zw|EWD>EHeoVOS_fBR?Oxf_1ReNWaLmo3Hh~0u*>0Rr@y0i8}QKo>leGOEFt3@P0+R zJniJGm&^-Scl|}R39HXlXEN&7rX&P2U&0xmS9A?3wNG4x2?m$K5RP8i^|d3*YUval1Jfiq)zTO zD1@GEg{G5}9bd|=ZUFR~JW)aP3fd8|VxI+{v*muCX<6~|Ic)I3Ah|0dOZCR=kt-js zLq$!9tt4o}?}^`Zx7*U*CVo>}b(gvtYz`TQ>*)xxH5!iSq&@O1wqLA*bZJTjH1oLM#C_Al{}u>9j2$02uzTmsFo0RZ69tb~6{Uanq*VCVVO!W%TCXeAMp z|GS4ufmHvbGDySy}q0qygneb+fJt8PF`-VbyF^Q&DY8q{xzYJ=HPOPl9 z$geqG63@Cka%3Uvv{IT2VbQgHO=H7y8C`+z9{`~-tyJ>Ui%p0|>}(=J%bdQr&%kWV z!f+x4xr2=&o1-I8y1E#6@hYktE=;%R$nLU}+uri;{9!Oc?(_K^p^F&DER6lktXACTgZafNYpxfp(d)13EN+C#nRq@&Ay+@!Cs)7x& zSiNzt&aCi&H&{wp>tt!P{1DTyes;ftZ7rwDs%Hu?=@@m+UN#X?Zu4Z;KY>geN4K zE^Wz1@O-6iHe!ByCL(+SzjNk?vzt}-%l@aMrJo2lVBs3QN4oV#a)`1#hb&fG_%V4E zW6APNfB9Y0EK2wUE8Jiyq2I3Knhng!=!+}9#J#1ORvqBFo}svcDm9S$ao#f!85vkF z@c7@;*LRH87yb0_B3F*tI`3`tzPp9ETQ0uC{5rcAknrkk z?AnYUDj9W%a{Qxr=BRs6I9yXKjdY#Sa(NM|PS<6KSZmGdA9XXgIk)t?iNo)P$iLLG z5ELAtGk>T3b%M?DhTM%mhc^LH_gh7+0`06PzxuX!^Oub~WSsL>iMrnR#PYrr6;S#P z_iY~usVJD*oUmnH3Uk-8u6yoeYFd-(#lNJv6<2X~-h3J4r>8q#y|I96Yp5cdrwSyd zyT{v(pdT<;n*C+U9*c00oIDF0I1f^qR;P#y;A*j7&fQ45vu$bWOEa{Q2i1o~((ES?zWN==IGK8b`Wz@1QRZozwLz z<|NGuGhm%;jmjwnq1Ri=FuBq`M9TNs-|eeQS+)|YeN6PylZjyXr5UsKC>*sgq< zWD>k<6{M;3bM{b|T0s5jK!Cbl-e)N~xg~)Ee#S7map^?ArwwN6Vl~yvfvo3qcWU?t z$=cz%W3=;_Y*vx^>#Op0bPylQ=3c@`qshX$=GA4x+VqW)#K{o?zVL1Lh|F<7W+gv& z4W*3Id=k`>cUmK@Bm*?4r(>p5*V&vA+d!j3*mj9E#S#5Qwa`6TwAC=~1uV)JB!xfJ z+1@*dKgc9-dsDopAO}(tQVfJf$h+ifZ7wYl9S5!Iap>-b^%_od-|a?Vabq3)(jsz z=iTVt8)N;3@b_jjG^@$RRQ%Xo_(-jaefQ7^Q^S%;-Pe!r+Ytq& zj_k96vp4cUaG}Utv~_{@BnbyO7K5E;PKS8AN#p6M_D19M1Sx>e|! zEP;|Qz(hRE`MyvfIstk~p|nEzILdVaU1xS;mV{p_j&VxOC62ECOiHN! z0Ld5QT^fG`ZC1n(ugs5&(2QmEBz!jNxsf`2{Y;lZzk>oB2%;?ZVaeXMr1IZ)hz-i) z0s=u2ASM&c26mpNLvo!s0rl#R_DoMadfpvT9~VC(7Hy)d3s3$zQmKx%*m-Fm&4~mN zGx)L+|NFS|D_sHsz1ZFrVOvXfRqZFQRh6)dx@fS@RNnc6nO2IBuN$p2WlFhYe}6tA z0HaZoIA+_|sy8WWP~`lZ2^Rp2qyh8IAX&@jWGQszz&1Rh8J|!##!MA_x8q+9j8%B6 zJKt~?oTGCUk!!?AMpFir0aFYi{i7@StZl=plTK}F)F6ExMRZr{QPNe~`%Au#^?rsI zNZOY+tqz^XbTK=Rwe=wE(cfD)FOV1l256VeDuS@3fI6*iG&-E@pCF#^M}Pq%*>*MM zM2Hn4(5OwW2U?4R(L1|GhL~td1$?Clf)l9ap$@zK-8xa`DL~f0Tg(=vOF*?&Ph)Y|AwMl$T z5WO!rCL1w3q$2%WXs;8F#sCd#`3Jf8Dlw7f!{T(kB2e5lj- z+^RrrUW9-t<^>oPeUb?Hwp^2By~4P3G!&~Erx@jO?PX5rsFoWWRu}qjLk+ub zXwE3s+`-uF?VqJKhogY?RDa{O2Yh@l9@%1sgbN!JjQIg4+z7}~8nWX>&w|k)*TqM& zSjx8$VgSw%rV5L^-AX>QS*swq!4LJDvUDTLI^oR~FdAEf;AQ}$*|EnR|JDVM!Kj!X z8!QHHgGR5BK8)*5t_gRWj4_m@e{A8C76e7|$qXXY*t7VGx1F?u1v6^K&-{`?5cyB2>z~I@W<$c?&Ybi{B@k|y zA2=vSA4?u8Oh4)=T?QsTkX>P~z{|Pl_tV>uhx9E>>3MGw;aZFv+g|Ne=bsF%LrW&~ zc3NXUm-GJ9s*W4N=orkN3>Rq_7)!eS%b6QC3G-iZtXGJ_2>&iNlIc(#@xDUXvla*2 zbTEmHb>wUW4vAq-r6(J_zjy(JMrp$pB~WV$3re3cqC|*!M^}xirwI^WB=YCZR+Yjs zZRlaMqEim`2J|8pQB8q^mvdu(C32!>6{e8lqLw1B_TVW&Q)-c}3@^Y6(U@Ef)H2g_ z*Pd@;i>CrnV*;9hVlKTJ!v;8z+Dk!sd;<`&Yg*5Vsoh{vj%m3iXzHG(amwt4v^B%yX@9Iw74z*_y<*mH79{;pgC;|y_1#?M87gr)an zG&hlI%+s^zy)8})!E#m0PxhRzV51pYRXM|V)_(g(jDTxD3)p(+L6!!sVk2Zy^m$y- z`)_%0gk(suPdBl~$J93gsg7K&9F`{D)?sC#+Jg)643gc1*BMf^(6-{=FNc|dBz4P& z?s#uuNXQ|G;ws)|Zy|1lk^KN^6i;e3WDl+nL$se;b(-a-MGHRj_obWj{n`HPEtL|Xf0w=G*GA{SV*ixs>=b+1&D8;OJN+L7~fFB8mDN!TJ`t{U#vb@;2EdYt-bZy>SNtm>gL|1G#;N)Xg_R zt@`DG6xAPUwRYbCaAy3`fO-e#-dpxMU%c|Al>vx+L!ive>I+_1JM<7vO@xdX`EB;9 zEuI_uwT9vBxYMpY(}tw=;~jnKT;Q5ID2XIER4hKe!6V~goA=N1Fk@JINSgJOn=&GN z%i2I&Y5}1~9Sm+U=+*g~qw)TNZ>xP-ZYjfK!~8E2j+J%Cv`*82Mv$f<+9U%+H+=Ip z2pqHnb13?$TtC{|C35ID62wn5t}WW$)uOQx)H8!ChaPu|Lx>gyvTo0LGDGAjnM*|O zK2bL+2H|3yoZB@>q@fvoo;6~Ico9r+uOP>-=;8ZoVFmK8v-I`tDKZsJus@2Io=`RB z$5sDr%_AeAy(zi||Lq^MA2&>&5>t>mDb1k8Xe1zZcf6fWH&CiX5MYctLCu1%IdwFU zij!4qWbZpZG>$*m376Y-?s`Me7D62;M)qBSNpfMN2Za|ie~8rqucp9Ov4smqVT6@kO|Xd(KEr*O^zX^_Ta}7fq3ex z#|>bjp6I^(&hl7cyq%xDC9AzJtxdP>7eAwu2T`l`20lK0Wj}y(2EJgP(*?3Bv>{TT z{qoQ%%#n=He5F7V^{YcWR4lZ7`xxAJ#rb?)Y5cBU2tmNmp$3Mj(KtV8OfN7nKCPVg&X>#?td@WrwC z(*N&GMN(){9Z5j4{>8g<__r-zg{ExF8e+7o6KTt1nkU;(RYldj2Z}Zc38|M^*5KM^ zLKMw}2VaD))$TK3*!-(OgBUG8C)9K}`PHMld5Fa+xglPosNy3T1Bbn16IEF6ILAUv zzQ0K!6JO{g;wJG92$us__Cw1pY~sA13ul_!L#+}Q`e8J;ZyMOCQ!VIuy%O$?Bf~g{ z9Qd1+U_^oEsCFj)tXb>6;MBcI^#;GvvZVX8!4~L-cHneYVMP&O=Q13ONQ2AQDj_1G zBHI8=?1vRqiGxP!Wb{DKt_!5X@#ft{(`Q*_C~A5&{j~_*H?)&&e%Jvish;N>L~D^g z&w1HS1j1?+fFNu3NnP-7Cy;y}pQ@D#_HPZr5W1y}0}2n91@Vo@4b48qPcRI!8sqvV z)F?95$%qzB+`MbL28;}UhG0XipB$?ikLhOC@bZGaZ1(ApOXEi`E0-(FK34;hB$0&z z3~bmxsprd{k}&fR`Qu2&A6u%JqWomdcu;Q{8dHq!)bUy){Y2U_3!^M9L9EpMjK3A) zY#^q`Q{~JFHK4Y91CrviH^w2@@L2{^` ze6w@v&+Zhhtgpq&Q06z<7*#|bCN1f_&A^d)%%6;-Q3=mrD29j3|DxSRwRwzerPKn^ zP2w?3j>Yi);DR?%YJ74C`C0z+i_kTS3q*| zS9R}9ARL5|4nFALHMEG7syWhRz6fvm93%Ju3&{K<3A4l|e*6;5?c6#W$b1Ng+(+Eu zA5!$RCX!(`H$|Q=EQa3^`a-UYKTjmas0-AyPUh?Qx96>aus%)-(Z+(W3Veo(j@u$w zD?lrsue||3&5u3H7D=Tq1!qR2&GC(I&`P(3+%lUOK~y2^)%BPs8p|? zgALD){_a8Drn(iy_`vS-A=8}VUHT%#CDw#IzM+myNR{k{w|+-t_4LKverE=fWs4q# zSRLco_q;zR*-Zo2jSMcZunGl6AKw&(9YANONew!Vxwz_ciTprFjJVHIGqjkYYYIL? z=^rLLpdiPsbU#NOH0Jdg3Md7pPq~?iS5}=nq+c2b79TeZl#StU`UP$otvd02fl;tM zFJVaksA)jhH4?LMuxcA+wd2rzzZhl$hE?Rom3-g?TFE(0wX#YqU^q`TOLh(=zJU*H z_NPS=0fv}_#GDJH%Cj^!dCl7ZMTQ;9d391=zLU)Gb>Gfcg;A`Z6s+I8yA|wxGj6-l z&sjgiiP$r^8_-wzZF9%XEn&y1glgQ~`yK|6>PVF{$%1`)!!@+zpI?$^lRSaa5B$HE zKWTvZSr5h)^-$K+kKOR#un+1kSNQ`rPJJpriKZ{?D@2(dC)hpfqt*?%jHI%^2_<-- zES9jNvN)EHNZdhh(Hk1bqs|$j%|v;j?^# z{AtmqE`HJJ1v4|_UcgZsTO$P1Gf654TP^9a<9~#7FQ-M=0^TUcn%791zEXbn*6IP-BxbxzG_y$KIk&I-?ufFWRI+^hsu5t&qq8* ziZad`^Cst{tX7^WbY^d0_Q4uTp7F)XjMYC7M#PnU zn4>}zZ7{nP_d0!N7VYOZg8?SpZ?XGhc&BLWItjB}*mB+UPWpyANy3WLx#~dk#rhy> zZ!dp(xtxVtfo$^Rq_oIRcfH%NShl4@gY8ytz@<%aK%gk^s57BsFZbpdRA-~OPdVWV zq)Y?m+O^ZmKgwf38=sT{Dw3v$??1|URp5|vg8th+5K*#R^9w-mBBzbl#ntQ_?qv z9#JX}y^>$f_|erji08M*eIf7|p3=kUOb#h+BF+2Dk6CB-Lj3Q>v>r ztSd+wU<`XI%&(Gk+X}aDQ!Vh9jSe|I$;y8|5{t%b*Cd3IR3VI=a3buUiz z;8W*}zYBqn2eX0xaBlG6AJ%4YGH)AuX6cAanVU#9B3nQ4LYb9UR zl(0MU3r^!NOiPXMJt}iEWC!#ee$C#fAJ8N!5RBOq$iEeein%XdRwy_z7W`fGsGcO9 zM;M9XP>eYT(Sm51_6Y)#TYBxk`wN6qDlNMD9a$_g$!HwK=dE-wJ>fBBza))o+mK1; z`%>PQV6=+kJa9Nn;==By!u4SZHRp2tB9R@#@UqQ4pH#|?@TERv%3hYyB%DV_q(l^q zD-Osg>jpy7Dx^qJvB*%V^7kgjw+)dPt{s%&=LMMoe;wKIQ`vN0<)0@a$7DljAIn_T zCzRQUA3UzN<*v@_(qR!R7`omR5`7*&<}tSv-r9z7I(c5{LLvk`VJynl;Qc5HaJoyg zXoe6SEmspNm1rVwoD!K;l%zPv1&VbH?H8c*<`m9r!i5^sJDy#N(jLM;oFn81^2EgK z6TTYr6Yv-PL2foK^K(xZZ`9SSg{0b|Bwtvs3`v06KC*1`tyhlZxAt-0*4NH&F(WYAfR~Ffj>lD|!ZGu(0 z(K9BW{%VVHN0dNXykCG2$=XsquWtG?RJ9ZKpUUXU~y6PR_CO9ucCvNsExcQXZ@&q z>klgKg5dzdSVWR+sqiE*ANn%xmn!WqL9z%A_V!}Gjg$DHz?&^9Y?|Y%gJpE2PyY8E zp<#ev*qQ(ZO?Pq}PNcJvjVTlO1=NQB?RKy~7XX4s!>rVyyJTv-77QTjO#wMPMirc4 z&zN0!g7Gkz$R&KRoQsm58mG43Ry<$itM;i0BSg=80T0*{IJMN+=9JTA?p|Yi%^*rl z+J&^hbipHSPLA5pse9@K@Z1_>h_bBDDch0SR&9nYj&)3JG9(E)0hW-u*M(V%-8)3u?$gOk~9C{z1+wPHYbMPU~OJDR1QC1 z0Q;O;plZ3ZV zFR4YuPwfN!nvBDH&2O3Duu|{b_^A*oO1N}sX{S~Pn~|y!N~1OgZJ%`Zk<(8EOG^1A z=v@ZKuS4fG_K&CY)^e5Eg75hB-{MK zlLnYKI{D4%wj!UoryKWlp)Z?#6=om&!%4BMAas!b>`nPb1%jp_2@K1zDsj3Xc$D5C z_qmYjV=JjLOU^v*9H3u#pu-+P5ZRN<$U8ZVu09p&3jvgp% zNC=X{dh3VjYk{8O6vQ91rymaGt3EkSbT8H7OC##e*;c7uPuz==PBhuAS#LXkB|wdi zUu5zbXM;Tg(#gS%IxJ9Om}ic#yMDy^ z%?atKeQQu2x5cz(4i1|OEFMbO#9aYD=S6)*iAQ}Ymcfm`&55Sx>#&Xu;6fD80uier zGI45Grd#1CyBHH!!p#Wp#8C_QA_0D;_fQ&|z zU?msxa}01V!Ui2vEZTa*S=14okbXcEpio-{O@OzmrVGNqBlUt7QU+W%b{^jpS%A5; zEOx(bsr1o2c)x!EpN`u8vA`G!wWuP>9;4@nYeZudI`DXY(wvlm796vf_=$CNmKLVD z>tT*ra+$e2SMrt>AX`Q9{hY7h%ZgJnm}}A8Ql1J^%i4MBFC4_h(>9 zDZOwWKzT?yZfd6HWpDd?TcZPRO?N@M!Utr067_Ur6}g=$yqo(3KKN~f;EI;{+hAT= zHgUGRS{rKq2$scE7Qd<;9oznjzHbYH@A#l#uK_E3MZ@I+?Z=Ic+Xd#2rcw2D`Jv)> zvMM(4LsR9B_QBI4j+^(u(*q^CL+jngow2ShMd2}i;ncYbFRo3~?Bsz)v*co8lpmGq zPcpL1U!h%Fxhe>yCU}w@v#Nu5D ziw;z^Ymf{I%o|262P1Nsb&gO`=pV5(?)V*|mvV$9HQvqE9QjcSax2pEdjGOpR%@|d zu@p@iC&&Ucvu%^)-;%YldKa&L55!lDZ?D&&j`#Q4T!;jFIuLt4W5K(2ZAik*!a5Ie z3_+1JWlyS?#42*|ID-m`K7lnp+?hUx3N2j}$_=w2_FjV=p2m#!efpTWvETV7kGtJP zSn=De#2+W}5uQb9g^pK?=|E?!;vZ8zrbT6?4=6zh^9_T^va(i8*^A8>*hJ@|k%@$# zZ%wCWOI(MfgpK1?-2+F$kSyMYs9Yq%&BF4*n5@ARu(6cReq_$h$|8nv@+wi#`loEuZoy*Ad@^`Msav2|*hs|GVx@h2RXIo7ZxYv9taefF3`<&NWhpT`6P$t2PK{(c#LXX;7&E+PS|!;HckV9EDhEiO5JvACi?h zHUEF{kF7BVW|W`bkrdlx2b| z#1{^o{|gFV-|| z=7&j(E;HgzSHdq6x_mpvVF25D$wJjH{$?(pir)0@Brk)O7Of$jSsJAPyM=e=ZQUg@ zJbN3OtNDgL6YjpkkwIk+Z}&Hs%`{H?4P>^LAd_=on~`b;)>ncXEyf?ddkSPSN`nYy z8H~%Ii(3`#?@AEZD%U zCm<5NUAQ_wFz0IlwLWcd5LUl#QBX80%@TviEPo6WGRw0d%U0x-X+>)Jt*#r24#k5S z5(%Q?F~aXoWbSC8WKmZL`RX)A8fHy=j=1?7XH{QNc|LQs_zb+aeefH+6|%T;dfIUy zw1LQ_4#pMd)l7c+Fo}Y*+GnrciN5)Tg2=-@6bTLm6*yTnSLhxKmUqP#`gZ zO2!3?6nkr)+YB#bmzU5bxk?qkLuR^UoCDuI9EBL;Zaec{YTtOhw1a}CV+fwvKlKOekFiV==0n- z<BEq?ti-6+-7dK&t-4n}_~5Ss{=w9lJsy1`XOE%uve zJ!`K{eM2}F&gv8n8ek>UE!z3CEDf+5+8$V6HHP$5M5vfMUQyjC@3D5A3x;u*2e35( zJ1oSCi$2E83RhX25%sQA*Da(CreX8(-?yd#X@LqjN}(C1CQp3yWGS8EX>iZyi`Va ztP~rs@d?$&IEdN+d{_qJ-satNFck6V1Z0Un@8cfq2wmSS0+DKr7wAngcbK}3FgBq7 zs`=OMwObCs-RGlkjYtfcgd^sYT$g!iZ}EgqFiq5uryCco^-W#NHgAp8k32Sh+O9?$p3K6##j8u~Hh z&FNQQcm2OfSd@dFW#@_j`LXk5Lu=E|jRaV1Gy?F2R%I^~8Y^me+yE))Q$Bb1sDF<~ zy<=*Ft4cp>*@R{OgDp%voq2(tuvo>Zdt6mY@gX%F5}#djgz_)r?o>w4LO6Fi^=V?; zRQTc$SaVoL4)v3sV!mtWXx!)G0x!31>Dxp5r9mXG#XuV^4fVZiZ>q>U9E4Yhc13y| zW+FosFV#CR&MZn15r!rSK^YGKq`Oq7!nAbD=#~atwVI+v;}logwMf&+j-zas8_Yf1 zP=m*dbsIi~@wb_6tG6GsyTRSbD%{$;^~`sz7Kq;*IW$pj4iPTHh-!WK{k8ELT$~yo zU&yFv$zYz794=Y>@Dx*}mX}f!S##SZ%O)3MYM4keKprJ($Q=O^T{{K*LL}1o9lIAi zR2#`xz?wo3*5LILK_nux(&~BsOKmr&$Sj6u=cJZ2DZ0rAFN0mHX7w`z^^VFt4jhB4 z9TGt$08de0x4Q;kBq2nBU(#}74E74992R}he_jI-ARw;4L)L>?(q$SrD+m@$l>H$9 zxcJNksCumu^aW~XshQbNU9$YbC=|nZZo{yn0^RCksCE2r&oMKLr#RU%AUNC4!T9#?gPYg~I^RTXlRCG7lI#}`+)*G^V? z7=VWebZ(VIm(&)PHS_BnpElbD5mY|N$Y8%Dl+3jA_d@nOS*DmJR=}J?W;xH3wtQd# z<2kFJ7LZs{( zog$~-leCjHpd@;1PCfO{1_^oceFXc(Oiu|-De-oWSE1v&?n85T`girSXIgt2-xKea zAW1V8nU`Hu4Ua0+^?s%;fmuIjdwa&cdY*E~HUcjJN7vt1f`Vv!N13XA>17lJO0rrC zV;!y*mogJg#x$=2j-J&;9JS5>Lb(gf5DX_Oma8h7CH!{mJkEO!it#uiO&R zK;TH{kuuH-0WF;iNlPoJWi}O;bnq#6N6!$dS&I@b(AQ_Y0OKZ^c(`#GEY5U)ZOIDv z!R+p;o96?|`;WA&o{s(LXwTo*mH5A;pbmrh!rz`zccW#x&G7qurMV>J()pOPxMXHu zRpFlU&r?=!&_jVau|m@MRSM7?^sw4eguG_7vP%oJvp9l`Zib}}f-R+y%x(8nR;d`x zdi9&MFpI6e214>vQX8Kkb$_-Y38O|8X7UA#YgEzbCvS3+@hYB>h^|ew74&?R{=gJO zanmYr`V3#ZPlC-xv5a?J#nV7OPI>C!pwI$zN(9UsG9oNif;%*8f1-_nf3^WwP!T#M z=EP_dNRtmjO}~T9v-p|e+Qs?R!paW?5UQ|Do&dPqEpiLr0>t%TsICu5k+q#4H3N{L zrJHr!LnnvwSc*wtFk!G>dEBUFafLAOr*r}OX1*d)|K9WTe&5J2bDoJ}#Al_?Xs+^y z1}X?$`hmUU#Hoha@!3H{lQ&1-)B4tr%xlE_i?fei7!DM}4S5B;oU?i1Onz7Y#*z+S zXHSzZgVu4iJl^!ym#aKsi%L*+Hk07K8;^8oKZRCJSr5iLq)Py3xAOJ?s?_7MM)Tcd**%UM=~&zzgJxA{YY-QP2m| z9jxX~lyvIMMpT4Xqq@{gGm{3!z z=zDSDCcmN(#gQ$2z8XB%f>%RZYDdV&Kijt#?jN6Oo`yp_vPk6$i&0`&8C%k~-}h|> zkLu7k45n>-qA(1TXssG*Q~Qekpa?RFA}+N2+GjzefImcP82Y{}>3DWK9BX{tfxEGK zun56k?*EsEWq8&~iqG#OPAQfs7d3ZJQI+23iu@vk%=Ri!K~iM| z74>t4$k04em7iWI%i*JapOyRX+uLm%`DsddLDWd|t z)0d{GEP;yjYt-K! z9>4PplWxV9&MPY_DQow@hO_irUvAatqGQ@x^$KGBz$fn@}-#ygD zzj;8xXZuzYW|-KOHx0c;l*^r`N$;zFy60ryB)iJ)*MS&X%Eqi2H ziwNWud=_?XX$J9cNQ9jWokiqJk&}`$`C($|#xpoebeV-~+t?-Q=Rk|U4Epc@PflZH zXP1%!wQ%g;NzODgymRvFfp5fi@;%MI@ky6|=SHI({CaIGSKF0JWv}Rqo3x|$o1JdG zdV!jQU*OS>EA3g+K?H64R=UHsi6ospc+gUBiVxut0@2f)#hm4zLl@XKAAFk7-1pZv zIv7wqAm4DU%dBCB4Z!MD!cFlX)+3Ef(ay74L`!pCnk6k)^2~UeR;2E* zP17duSVSpER&Uk%{Arb0TO#4h@r20kPQ?!cEue?JiKN{QVZJAks!p>g{!aW#IVao4 z7M4!Dp_tERd->S8QqJLNGf!!p_&VHac5a54qtWI1Hy_QsSP+XR1%M{|D|!49I}j>N z*MJQ-hHK#4!SLLDg$2MiNH-UZpo7L=Oj*zcz4d0y^y;cTkVzi4H`jic7svG%EQF_U z;V*)$?d>!6*6AM)qTaCC$PoC24cBxIH0K?U^G@OovU2_-c);Bs>NAg;7|uTQj2xZM z1v>a{M5hAV?%^{HlEjJU{8h?m*z+?8?duFz5(I8&7*nCDzJBQLc>~{{jL!I`a0}&y zZ_yPF5Ecto!&+z!t>_JV_){oexHyQ+Wz^>~s{y!$AIb#|)eglbS|3>T+&ZDdsKXr| zrayda_;BNs9VH8WIABp4g8HTk6(qcgVm3}BhwZ80brdvDV{Jx+4V3IfkGa5J2i7u^ zaP#$N{D4Yf?#TG7TJpjB6ZStygtq4Di6NOb&Nw2f{cdf)KNl71+jYv-tOfr5rp%AO zduD|1Wgi*k7QPo75q6mLDBaNEn0TpJb*<)Le<@AEho4wuiyrvMeN{J1BS4Fc!U$L5 z35~`MYCFz2{BJft33HswqOPG|C8wn52o{g&#D`5E^4d5S{)S&lV9@-2Few*9IT+}+iF}6 z^OYxmeZ9MFKV0&v^EDCrYV;ew@|R96`DM7-ri)=k1%wf_D(|nq*Lw<@hv>Gi{pcM) zH;adV?ai*BovA)Qo|Es3yc!*yMAj_k#4FccKxmm~maVY^g5Q;t#?gE-LlCYSC<`OoQ}LY_58zC9k~&@lk6B6LlRAW8U-efOFCOtv$yg~W(#8&tk%&}K5s=g zUN1^a0g0b}ux!$9;7`oz4>j@nBr`mK%mDO zi@9E*qE)LDn`MiUIn3f;-|w`Pi5oDkt5xKQ00Th$zo&q}2r@zZ?^=(dboK8SLvS_K zk{jb+Fe5aoy(j;TfYR7~2O_;y6mPN+v>`nx;pC;%0IM&}wzx?2NEn^K)(NC?VRBXtYgKgEK<-^;HQ-WDVpj!nV>cOBG7IOYUJEOWkc128|ux|dJn8S4}+m~ewl zZ8eVGjXD&PN;?26q=IUbB#>1?L(Y3J#Cmhoo#8)!hUeTIQ4hJ&{Zs#@L{sk6t=QIk zv15B9+avMzWEd^AjVC*N4~KRJ8QYPBs18~x@Hq!suGcP{5XYXDSF(4$*4H+fD;9wqW#nedb+*^gn}1EOO6mVf z!Ez=Yy4w6vBHp6QW=`^I4eg19Vj&PA3yuLM2a=-@)Qz;*RM0c`Is-} z{aKcIoJ77-NG7yv6Akk_&Xg}Zy`V`x4uy@%r_Ps|KcM0@q&TyBsS`w(3u)b7x^fR> z{_?%8Sz`j#zLXgsoU+5};=vu?yKm^AZ;WFh$;Pj7;EouSZ{1zK^Mzn8(5Ij1J1r=PO|B4-Q7z$tX8KFG7iyK*?A_Z z0_>JED?;R*az%qx)g|~O()}~CT|V{rEq4xDVJOrefe%EDyJp$scs#D>w+GLfx|ZZh zq((%KxX>wv;TXpC9ri2p1StmEV2l|?JN8CWrzci~g+5Z<<%5e?%_2%r8mL@nFVLnj zy;{mIl6MZ+?>x}L|pt2A+DROtC6v>X^JvATG$p=schBejGS&A(IG0OKeD$$ zA3^k@@7od|I&Gc>#rZ0PdRgcD;93{c{Iu;y~E+|ID5C7*COIZo$K$9|< zkOhAv^TQE`z|a%;GO=m9KpnRGjpaOqxk?i@KTq+WCHd<}KR-=*X)wou&`~bT^R#4X5ucpj6nFi7^Yk==!q`sB~5V;hc;&(VkP)zg-PnEDs6L>C*hV&R?EJi%)(~Z`s?^1xuy^=h*T7zGN zZ(yMP1BeSb2>J&mo-rjD(z5qSaCB#nT9p(dGjjrpB{$uu)wnW7?MROZ;iq$PW*mWR zsd+cSq|3_M!63-;6Wg%ht$i;EBrYN<(x=MiC)HBg~ZyBj147@anvNL!CGx z+kiQ89u&1KY4xu*vmG72xV7%;Z^Uvbbw16;bJ#FdP3v4o|Ngc`n6*3S{rIW0p)Q(mVSz9y_(kgEbAqPXa6#TrGAyK zsq*zK^epr;oHr6LUX3KD*nxI$W^Q;zjT(!)gkx{|A}Lp2D^Z#ruVr$ZYA9U#1>NvS zkL)hD`oBl5l_y&+cY_nVyRdY8PzN4PdN$7|&l_E#Iy?kx&NuEnJ(=_P)i`&HFZn!N_%I%9 z(yzWbWD=}KEI0o(A|io4#=Yh1^d^Ke*5*JqRbqQgufi`!VVms*S-bd^x`B(z#cBWp z#o zcl55*{3a9#dc5PSPklapX~@)Az)-a?@)NH9I@Uq{eikGFt)cwD#YD^CZ+;ozQ73aP zE=tNdGvrlZ4B%y8H;M!FhyI%z+|P_OK}}!H^hW=@TSbMhNJwpr)Wl~>>?r{OxTj|v z1V^~hYA47s=m@y)Qwi0F&bl!<;NUrCGqfQ)35kt?QM)S#sd%qmee=cIp965ruh+73 z!_TBO;HrHwz@-s>t6JVCHNoQ0r)=WG1sAv&{dL)*K3S2abg3pV$0%jjOOtw_Z|!g^DcP`;=^6QEMoq);2! z%Xxb4swml?t5q!Rt6vTG>Tv1qOi7e-#2{QF$NT*TPNWM$H1YhdjTy0OC>0 zFS`DRJc;7juD@x%o7YL3b024B-w4tR!}MsFdA>$*4;#+v*?4@Fzf|_f_4Ci zwwdRt2qg7jlGo!!hBd2)i-=+55Yy#a;f@GFwMZ-_OAi4MtNPn4klJEOMNd!emu7Yw z&LzkTr%6!b#OmI`OpcF+b7+5yLyqLwpS3hx%Vpp-#vnf*(Yo>sop@=_e-GZxze8(e zjO6rq)D2K83V(B3k3Y5=WZ=!BYFJk380+>)Jc)XEDPdnRq^Ux*D2Ow=1o%B;uG;d! z2|E%(mCsM84?1vTWQ% zJJ{rOTbc@quYOi+AnJ67P!Z8>+03Dwu|DiK?10(@en6e@p0Z1+pr-a$Sw*7 z?U#vA|F}W@Jljqge!lK1nIOIGSLX5puy6m%=L|``8EhG#JiYQTI|b@88csDs_&>Xs z-|&075l$sTCVej*p1f3>1-}OrM^D_aQdW~er2x70`T&z+U*1pe@u&wCbMCI7a z%ADZB&gf`c%+FpCRE==d*UjXUgoOpg+%v;156*}@*&xj4WRbaWL7G0Cxkvi>1qAz} zz{peWf`i^~`)B!r%G_4~yg)~`lObv%03aIA%>BjK0 zE5#Oeb@Wnp=#?XM`oHkN9P&jl;&C|9o^^ogD6Uk#=|RqzUBr!8-F8|3+LJ?0?5o9# zE8edq*!V+MtGdZ7aRll4-P5DSD7UXaGy26w zoww@=CZFbKCEDN32JK`Kfk-$pL}Ma(c2@HT-m1Cw(|sD7a9F=)aS(}!&rbMWU;qd> zlm~4k?z|3~O=s~*AARkJwpB{9dtbliA^mIL+C0*#_^ZQ7njnk(W zjJFsXcV<~U>4q4?a-+TSE)CeAawMN?8VbaXXKW^Q=8faYP(b^s_!T_(x z+)v;uw$xP<;FJt>&6Xw`wLzV-p2zUl6>BRg^@wVWrea^@ba^7MQU*{G9v}{#rfBnW0y6sRdtIt>5s`BUQ8=v?Z}3Nv!8Jln|LOW z36f~khVCm8pv8z(&U?K^0-W}4nXf;)rjZ-7^qBYYR#K>`m8<2y{a~!Rsyrtx`c5NM zIPDh}NSo?+ZgxY_$HXf{2}5Phe7Mo-G9t&s;%~L~F14V!!r!)m6#PST8)12N)DvJ) z;XOhm+UY~fYT=LlqaMbGop}53(iCvcIf2pE0r`&_V4Pdt%=FCnUe5d^`mxbu&66i9UnOs3v%OQxPBDto>oYkp`l1 z>6k{Q3+*bauFZ5i^Opx_{B}wz#QY(ZF{=+Lol1{Ii= zHZ>b0uG2aK9AG+rtjwG3z|2vf$4vUJ?=meFj>70Ux0Yz7Fc`TAe^qy`fO~!`)IrMi!*6;!T!T>uR+|~ zCbrunwd1B%her>b`#jPqDM3=X_l8>)MDlgXE(6do3PXI{{gsCBR_(#}mxcT^xuv2u z&M~Xrm8fyPMIlw_n2A}QKDoCNb%;idv=wiWuH}ymr_#4aWxZmQ(x?tTZZEHJ}YS^|9CZqMGcRH$IAi>E}#U ztgPbbb8F_+lUZz8*UWcB(G_Lixp;`$Uaa4U5^o zvW0xLf%oxCa+@%U3XJ7RCaYa6U^{oZ6?!}k!G+6@7FsO(o0d{OV5+=~QVwfa-TXi+ zr_WTB#Go6Lm>AHqO@4U<9Zc~<@b<~U+VN>-(V~ePr*4QuML%_VtLkm-J9f9heBnfM z9q3QT#nPM3kC^${#j+>AKeEr9H7#9NWPW|FdU|2k7`y0WjVB6oE##nO6X7S#d*m{o z{y=o`hXG==QK|%C9U}9@SR;c@PnA)3_J)s6o~)^#7Kz}av-FX+<~#V{@b~)w+*nj2 z%JiYOi|qp7U@A|Vt9vZ0Q2g~h0g+x%vVu2z%lvr!t^Ja9nslbvXLy( zj5eE*((OeSV40_3I9fN@!3u&~3izzvhGVeuD-5|vHOhK3%?}>gAV-xsijJlCO0^vs zctkiuc3#&kuJ6^$1j=LILz&(WaSsdz0F({i~<=ElK}vtK&R zK|}oFzN;BjE?(R#iO_G?=OdnpmA4j8HYk<3i`&wFOoxi8^OXBhp6|fDnu-X zvd&OvlcUi#ZXQ5~jf0P78`FKu#%bdBY)3*F&FBszEk8qr<{H}Zr(z(>6RQrHWiOVK|rWnHFcQ3sFKmV4`Op6o7TldO8LROpS@y6%wbxtw1%R)6bQ zNR?a^dtD>)Yi84SSd7i*=JP$-pIZ;w*bBWueLk)+$A(-aJzPb-bCU06(|{U8pt)0M z?zyqgkvlHwOVeJXwpx0wt+$Z^k1%fG2(wWVOKHDw5$QHXkCknKQlY*VVDK=8?X|Y4 z`Fa)Y2*xg`I%9IazA{FBg1ExhU!;!P-cVg8l??1iET5fE3bnrNYCT?npXRkafx-*q zY60?^wx>Uky@iada!UPbvo~m#aq;iIU-|m%sexi11)4nd=_^O*b?(pX1|idK^%m>1 zK>qGiJw%guKa`8!yqSwdpZ#H&}X0H0+OV%l65t^8!MNu$0gMe36@ zwTfv4vYnvh_8S!nnw$(EagD~N=MYSXane zgn$h|yQ+bZY+0Qxx5l!b_eqMml5)99vl{)ag9-s;t5+92Hd&=~IIQ*koU1L+agB^c zpRw6x2jb^qC<^|L>klJtY=%P?uw2ON zFgyx@yWV%-sSLJp4!=gN#{8*3`jIg_E7hL@z4?2hhHL-whF-`-^Ip^YIJ!kinB|l4vSxeMOd|AMk?ith05a!kt6I-gBXxQ56L;l zW=765Ieq=bPkJ-UQn%nxwQDV7IKxaSxru&x2?VV==)YU#{@u-zvwrZ}RziN*7pbL} zO~Pe9C3?>`BsNPI2`_k-^^rljPfE9krTbYD3pl|m?@?Lg4lnkqq<8oA&2Lb1m5*&W zhmO&!rxMNCKOfo*k*p&6;S<6$pQ6A~5;vscNhmI!VdAW?o1{cYm-3Y>4-zm5wu#CU z#YMXUBeWfUxAV|Ui(mpG&zD2l5#GpuY#L|kXEbuOz`eh}C1GIsa1pLt=ryr@v{$;x zo8rK0b3lsM>vN*$yt5<1af+kW*d+F%z!<|-0O(2HX5p3`gqyo(w=R7GFFSU)J$7PHpzBluCdb-GE66G4qdt_vMxqyAo-rU7 zrS9ZdUn5K@D(wMRmz2(5>#pc+fK#s0NCWeEsOg5><_Jn1Ta+Z~`|`9exaO(I4Xa4H z`e4>)X_a`QUvGZBJ-Ni3taBvC-35PlhoYD`iP>Dt8!aR z(K&Izk}=jIFf`GqyrVx$n8srdeX;F;xpzAEa!{>c1INP(4J|Xq;+2_Crb+4f;Sf)& z-#EGDWXKMeLge>rGq@xsPW*(jiJg36?bP|>iHM47-6dl!{JT@kiGZ%Oqr`9ca(4me z2;Tl3y8c4N%c-74xr24I@gx_q9z}_L{_KnalQWLrv(mY9$B(mRX2SjLsZ=-XZ8?stnF^gZl%@;asXdMHHxH=wN2)O*?Ine8a+&xq_X7)yFZ5O1H6#%6({N)(% z{nTnci{Sfp;mK>*VkdqxybumXy2cu)Y?kU`lF(PkBk5j5*$sU8J^tuG4O7 zS^7M7G&?o+jMkDq}a>jhsukP|I{Q#YcHeSG!13D+^l2^EWqSy zy7+s6KbhP7yCsPM3|#dEAtvtcHy^)u(=iiI>kK&s%+G@R#D5rs@e6WCYi0|3Pr?c# zVnfK}H20T6>Zb0C94|9Z)`veMlDJx1HZ}7_V!ahwEMIw@E8`q@T0ic+DEFt5&~_7L zn6pXZ2rQ?c2WjYK?(4)H-=_Q*wnb-#pK^a$zQ8d7-DW5a)}=Yg8{yxCmK-|XCo22I z3%bzq2JOLK7_>E6QeMvVIW0&*JYg!0l{M-`(vv7ZfbC?J<4@mrYUddAD-Cc0cN$lW z{K?yfN};}&{hDkAs9qoQnjr93C<9EZ3vKS zr&XQNt``t7=DXMp+td3oh12$O&&Bin{XxUNf8rt4TYKTJMvHwGx-HY$+`fkGsmK>O zRn6C!NlhMU!3d$*eWy`gKo5E7T--zD@HL+wl6Jsb8wX>{0gdjz?*o9{;6-BS zp5O=ln{9L2#WMXwnhv%Skz-Tt#Hq52^?w-8I{}453X$mR_7FrWLq;PZw zvXj4x^46jL?tb1miKU12VK*ZmYJs=+t(Av(jaZIPDSgyVCnq3hw~335f|FZWSR;$L zf*Iih7>0v8W{U~2NJ!$42SP%Ra@##C9(4y;L@RWCh8}PG&mVJf*ck_`nDMs&pBT>* z(-d`TjEyAd)3PT^mGp+pBhU;Z9Ix7wZGB`gNnP_eyP^JBip~iul zWBe_n$rZn_&e832M0U?=SpsQ9d>9gn?uKx)AA9%tS(oidyi{jHOz&F-nc@$#CfAbB*4I5bZ7yS1?oj3q;Qs0TOT8lq`T@TD~^ z`u`r^R?FWnW%hD9R$=u^@Cdq<@^4evvG6W_vL)%ovfdPyPka>3|mRw zvN4b1+aJvDy|z|{ioxB{Gh3}1n-{h|wUt;rGF3w_pNE~^UXvi&rp~c(Yqiv2XPn%) zYq$y>EkMNQNq|xKg>S@$BYj9*BaS!EM2sQ#Ir}4vb+{1QUI$0i8jjmahy@(p{n}}Y z5)0uuS*7X6hQG;oKFSZ^wUhJUKp6`v^?7dXwkRbBWQhckQ0Dt=RLbko`s4c5&Vt z`0jvE60djR|H$#6F?V0$@Y_<>@&mmu$5qoaSik-y3$aj#+1|f>CSmNbxi^37q1NF; zn1#-mx!J*9*@|XG`z^_)$m!h8%FPru&0TRZ9DgX(GS8RvHS`|g@LTB)dBAu3^t?BR z8>W4&ZXL7D1D~bDe{k23^HG{hcFS5~7M<#7gn=`TQZW(v3{4#|6GDtwAQRb9y0OHI zs9Z#j+O`Ndc~_cG}Y?EF*?j7aCWlaGEdC*_C>CQO>I%Hjcht_b0c zI>}Yt^$Vvg4BW%7$Y6dJz^TZkTE7OiJqb<^7q<@H+i#{@l$~pa^n~fzkev+EC)(c* zpGEa-00lGUPeP75gW&?9y}d2YIXikNg73yLX4IR8^V|FaFkh#X68)TmZ)J?c8^j?T|`dkOMrcrx3(otEuOaA^0fw5IECG(=yE{4lnp?l^H{p z2hFP8>GgI)(cmlHmkdl$B*gz1MXmf*dM0*HrYh;sm1G-9dR#oxX_tP~ld`O~iy{y? z#ZcLlxx5A@%3itwe25AD_PKw7e5=NdG#bt)($6o@U#6HkZXdm!^W2@sG&}{3YjhZE zT`ya2xOxc~mT&yFMe%%*TKMnQ|8TKI>A!?2vn`HkE%vx+8??X^G5)eQ&~WL}5GnHL z)77DSCi8t`o(}>VBl7*XC@F!{=igHdV#@iM43{)Oj&JM2c-lPQS7CqvQP#?>K;{GG zqtX!IZ}3UdgsLjTmcfdL@2anc(a**E6=uI{gAJ>??dI(gSMmBIv!s&D$6; zf75mhu-$LWVv)qyQnB)Fm-#6c`(xdLH~;fXaRGkmO+SXaFovC~F(t3xJ{a zP{wkxgglFMcIVy^a`C#5V#T;mC`m=)I9J^65)iw=>a$u+bOa6btpIcMm+k<1_d50m zTsbLwiYlZ^f-v?)Ef*yH&7qg<*UO_5ocFkDL`y}x(xKNvV=M_cg{6rN|Mt)sJJ4 z-0`MX$5a`iRHt^uPqYhqMsU(R1`oBf$%zx^2T>^{I3SFka3skKQq{%q*@$sXR*n(1 zF*2xjjMl9Z`EP1;L5olkxpFh-=gyCpB#q{NP}(upxS^_v&ta-}P_psd#%OrrE!kp= zi@8}vNO9KD&n88|plyae%WTehuj(&FW{x`ftJs-eR$(Hd7q^{65VUa6d(B=nHAWF5 zsV&ncUu(YK?;48&Z=9(Zey^{IX6{^jUVy?~{Js9(#rPgeY`t}FYoS_sXn7zb*BtFc z-8>Hs3@9;bZx|WS+P?y0`9zwwBTLr|M~FV&Rj-JE`kNFeR1`H**8Ss4M%Q+9YOHyS zrf4bK4n~p^p6rvQ0UAMe@>rM!KQ1E4I`H4S{;Y`%IgHXj=BE$Z9W`FH!uLMr7}?D| zC87}|4G#`a2y#N+g_tneEysZVg_(e2CGQscMG4s&QyTI5LL2eV$&Ba0BN0u1P(8F1 z#x!C|YH*TVI&FDA?fG}h6SXF!{lTxs9C)JJJOF1OKL2s8awuGGTD3ayID+g-)*z7= z1Mm6(cA(|65)Myx#^rLD5_l=IxEkxRZNWkKq> zjh)ahQUG>8=Wv0Kex_zt@xZuH{wxT+5t`Omk%mKs4rKdvSm8`7R6u}Qe#I&0$r;gS zVTxj7zAxa;3S^M-)j~+_aT#$(L)Y#zy;AX<2`b{hpUwB@qE0yb2gx<;FAic!#3uft zL|-jEslS`0alAI?i=vL|&El}Rbf3xgn{TUMpmn6Oe#2Rg>zx-e)#q0oLwVTp!X*2r zgXuDBmZ&Lw%4P0KDQTqK{puUIB;ZyQ#vFV^U3HQh(>MDxx8|&APY~o{MU!gXG1$*R zT_j_?zLU46j&$pIFIs6{?w=|x|IoBd$*9Z-Cq|T00QPF1I z=g3mt&rCh=jCdV2arZ;iKGfx!_NiWRhCM zWri2LZHbV3pp&w7)5fnljKg;$OxoB2(Ay&Diu0FTIEk|u5sMd}{<|-@eb_U{35iBK zJJ5EPwe<-$e=3|9V+VM85=Dce=!*64|7esqYm|qQ{beGN42UMv>{~L5s5n&go%=Oi z>$ipp5DAA4;-R;77UOKnXWOMW$yY1~_;BfQ ziw3L}*Y~LPxnO51Xz3~CNSDwdsQr__7Fk6WLGJMNt#A}Uh52Jdzl5$#4UxZu#!;r3 z@lH*Z&|!7t#d8y5NSx!E832fI=RKTF>Gp_7%gmw`2-e0)fj3Km8)}4BA?%yAh!tvH zTCbID!op{(E0qC&gnTn^9pdC&O1wNWlTq{Uw<%E>6*1-P$28|xqgT2}lW;tI&6_k3 zDT25IvAOI4r{>N4VYTZPwOs-Ti>{0gt^T^w_hacEVJtz$it(4>4r9%~vnv$*! zy*OdQDa#oICtvJ?q@eSs#2UP5`!&ozJ$sP626oHtKL%~{pYJZo1Yi{_?qiOR*NTG1 z!?+r(xL+W^Z3h}nUhYkn(#V;#=?c1Df3NVhzw4;0l-YbGP#RGp@8D~Y2_tzg?e{2| zw7-A0_iVE>z=eE}NpTfFLjgG{A__a(HV+OVM0|Y$onwsr_2#bSBmGIgo}7F}S<|C( z*Mf`WW_?)v!14Fl0YD{}4HY=X1^60e4e(=mp|5LEdoIY2g_y>$$&&i{1lzp-namL^^+1K>sw(=E`LHv{h{mII zfD#XtCQfMBFTMyg&2fmve7sWu!iE2Pd2(*U3G%nS-LH{f34*{Xh!ziF?1PBErkG)W z5Bc!(3gJ(FODo8nj=_aDO{leNSx2@|-Jz6%dX2PX5Oe>t=;9=hb}E4_x%k5S)^JBw zJTRU>k)6K}()G5iO5SXs!X&x-k;1%N6*?mQD(1Ar2+BimAC?{S?0M#u;);d2{ec^2 zpFpZpkAS=TPXMrH60sE&&W}h8kukYsjy9>+Pa`k3ymw@`1t4`|m8$`&TH+BPZM6KC zE5;Z}1lYXptR<3|>O=5jF4A`uPl(v!M$4czVF4-TlreqE1Ec|N>7Vf^>Ek z_Y4`SP&}~&S?GuihWeb{u&;lThlgFl>drZqV&Kw#5+c9j>8BD>!0?1jqi{j&Z!%lB zDFSPrpXLVvBk~S8|NO#+sRM1GtL)WuLPq0EhHc}BbC#_6no<%*eN#_k-bJ}`BK_hS z?XrE%(f5k~?4#1*Z9W2WPYa^O{t=pxO5I ze8BV8=`ggU4WqDjF5^46#{FoDct>dhtkYdgzdnrXkB~Zv_)8%+lT_~$dJtVugA9S& zetc9LYR=#F1&1yJj`uowgvTnVC4PyQqQxIBgEjeL;C<(i;A#S2T;ZV*eQEyAV8q#4 z{R|%${T}2wn9En^MHA-QjIx$0HX1C)8|JB4Mp<)1mRwx984AZ*tHxCLr1YidpU)*t6h{o+-so4Pi!$f& zjFzLeqfQhN;Q7If*koQL$OMXyhS)d92;2m%5~>{~Pp7D{idU|XHu`%d$gF(CIdMbW zixVEY^39a$L`tppQNY`94*JIPU$6n}k!|y@dkbvWdp>`u3l)DnJqR)GHBdR+)Ml<% z=gB-Gi^te}_>sgHc5dXwE-yCx@($dev5M0m$0Q#l7wV+W&xSWRNom?=1|w0>CPVD4|?mwTeyb>0j?@Myjg8^D!RAQd|nyQ+T>Dv;gF!f+BSyy9N5$n2VLmg(4Yr)qG z(3KC&8wzU((D%&b>jz($uP2PxZJ|W=u{m}z@LUkb^yjK8!rE`UqhSRF7+%94bm_iM z1??3WlmTSu6C-t>)92e!&RNXAmHJ*5c5+-_jk1pXIeQ)OG(6X#Rz75O;W~ZGJL+LB zbq?qrd_5JT^-$@wU*unTw6iGn`5iX$9{)v3U-2M5s8h79yN5n8e&g6AlWnLo+AgD( za&6#Rf4dDci22YV4EDbq#8Tx_SGscx_-j^YxUd_yQ|_gY@ry?XPyoR`6UI49dJZhu zIxAZf& z50~gg`u!f$k&7=+$?p6e_Z7dtmoKM2ZxX-{dA<;3JkTbepGym~MuS0=tl;ir*1iaD zphyMkFE1;Vm7Z>fTkuspZje+4-Ww-BPPH4JhyLV7zHS!(4&q zORr-#T@q4gpY9m9gy3$JH~reSDyIMD#>psk@InY+JM>Lv?{A>mo_8F!AWFQ4bOs}GRxJLRPf`Y1jCn2nNOz~t1hpJ>I*zH@{H8mZgt7wI2LA>0 zha##6#USzXLj@x8O^Ov)RK`hn9s8Y!C)$&Dh)LjVR~AV zi0MeMdD6g+PJrzQLz!+VzM(&ek-zmSvawypYVck-aV-3ldwDF~!afy$rQho2zDd7E zg{3+pUQWDU;>=191@`s})!|y>AFpQ6+%;s%d*J;hyqj}w-}ZgwWt(G+ToagxU%r=N zd_M~LT%g=y<@fV3E)#2uU(H1nQ_&1oqI6Xr`B!)`y?I$Vn}gw=cv2?O*!d;(ZCj6k zpmlpKpD&Q*Q{ok^O`5-mSmll65_TSc5%Il+FMGWm zpcBE%pnIR{MFW{-x?%uPHYmKwUZ#TdM^&*roKFdO2Q!jr4vkIM^gH`M@^1g>cADfF>ctORqs}C%PpIpS-#9*(+++ zkag!^v9$h`Yig6YPpTl7Oq`1W8bxvE_Ni_b(+mN%hmu+!6f^;3x^%AJlpXVAAb#B%Ebl5Q)g38*zFQ%#y>K|Z&-|d7X1M{&s)t!v-tDtt zBF*S1pRRKiRF9}b*#L{v%DBZph2Vx528Rrtd{>s|o#@23T3w{ESJ*ygXF=;TJclop zTt@ZhnH9aDdUmo(V(l5`jYHGx*dsHn4Q<_wuyVReUM@u^W?kdec1cFV(K=gDv zp5uS82!8==^l>~0S>denX>!a!+ZUAxpZn8pCrhOyz;Y>WoYZn1J*4^iPHsVscq4)^ zM~l2#XT`;2`ED7I(Qd&l8CO^PPKD^b^8ULk7=qe?A;YG^}(iBD8H0}P0SxhJFHSjOh~ zQ9$h?CR`M104GdROe`*4HjY))rJ3VTbkpLdiUD;%$$)xGem1F9{Dwh7kCwqAxC+H$k&FTWgOcBjV>%){87MK zf7)MnM?S-HK|zHZ^xxLN9S^%ZPm>ny#4oXjp82;EZlH}v^EnX+)NcN5zpJc>(f_Op zZF)~Fm$p3zFXxRaUuS3{aXJ8fTY1cjiURUJSBns(*OdB+FJ$k#_d7uzn!C&X~l{~NqrH1 zkNZN%mT$57a@sYo&J@U;hJZqi13%_VZ0e7%uuL+`4d;J#$|eH|XWfj!iJl5s`8lq6 z5o*#oxFV-*6j_S7+TeH5R8gu@EGV9}bfwXtxLn$2R^qgAs$Nby%rmzFT(g#sMPnI+ zS>F1BZ5Uz~)66FLb7OzJJ@3gjfFduG^7q0|Hv%6_zGg&vkfmW^ye_ zMK?f!ojl@~*f$G=g>P$8HxjV65_jeVrTQmxb+bVM^@=TDPz+3dmMVc_e&EbH5PomL zy)pA)EA591mR;c{;XJ3A@Fau;);BVP0Nzv_CenO27^A>fIXG5JXkBAiWykB;30AwTNCX?PCwloXBpLh8Rnsv^Mh0G9-FMfuErG z6#kNNQIkV&E=-ekQI75p`kI%;@O8W%mCtzKBjx@Kvi`7Mhjln>xS#SaO$C>nsd02< zeurpoIVwCuVdcHYMHn(7bm-hi!mVN=hUPMkzc|}H3%cD#KEhrI6(nkH0JlA4&N>h= znc7Fq(d$}06H~&|btgo9bEMPGaZ~-v~pePyyuOhAh|>q5c)`&T|_r42KxLbCu|KzjAQR4fAZ|oG-O?X@}>l zQ{DqQ7>}n{yVDN>_qP`B7*IPUb_k=jG+s%ZTD&uCtG2kZ363H!M=+JXD3yXLUJx*o)0~5pzP-~b=SzkG@{5|%>3>ckKjG>cJqk$ z67V_u$UHShQKY^4m|rI8806}OMcM!OmLTV){(1%iS8&kJ6HEH#LM~ap?7)*a+}P+ofnIGxH7%z8bFm zT@O~#9dAHtK1eh^6hfJjSEa8u=I6w%-JoJGE_3Y%_b)uP4r+W}BT?>Qovh6;n7|%9 z7R@~a7y#DZL20o9LSOv$o^)23U}t8nJLh0QU;$aggkzd!w)uNzM%$CP+0SCRL!(m= zC>+9S4Rf5$-ewRX)FBc89&d)_hEbr7y?C-7r4UX2Ah}f&IWF595^j!sC!~5>>~*_g0JH*5 zlJwH)^>E_GYpFnCrLfBVEVLq65}c^x8c=JD@y;gg)c16+GWSS7_`xWFy2A3L;nC%+ zUmwpR#`wf9sT_L8$)iXuGw;8i&}R!v`8!7V#v#EDdS<7Tee!moL1BBSnj?)xSY~=H zdGA+&s&?aHIV$n>9QIAUo#FZ^5%v8h)@u8OuPhbm2EGX0u#ZlNYOT4M7nR88Jxnt~ zEb;s-ut<#0#vmv1_As^6!X%axm0jqJ?Nr{AQulc*^H|+LePgaPfh~daP2UUu3$|wV z#iv`jQSP>Lim;Vu%L1bQe2K4WW{mgQM6P^)S6F>WLL97OF66!oW?4-PjQ|5c{J(<{ z`|V=bfZR$m9}UKr><+r&vmqbjJT-_Rx3{v{e!Yk87ALSq3$+oG)Q4q$KZMB3#nBO* zQ%ue1d>8O*;s7bM;1gjyGKCE!8gnc5sD0+@z}-8%+eMRE_-m)!8Zwpn|N#`PVUaaKoEU{VP|o_XFq!+XYoXTeAsC9X8}(YB5IVb8{d zGuOZyhYsZU0=R_z4%%b1x^0&>^~wREwX2MOlNPm}_2A%6L4LgavB!PIK9d_G0~ei# z`m7p}=_Bk0>?5n4cBoeqzt+c=bRM29E@l{hR;3fhAzl8!!-3^H$UV4*tSP?$_xT#3z?48r_vCOh36M9DP$wfuk%jJO=vg!ZaorP1u#JkskrIQ@>2QNJ*3CI zfmlNx5$ya5V(Pvm-p0N|l!7_rqFd5Ae=NasKYwso&+H6joPOSu?Y;}`t)kPgn3tM! z7?Y)FAL@+}9~`};gjYzvXVuvo=XR4Cq;>z^+iQ+f~J-JS_%#zYH`~@&BOVDuBi*u=1t0HmHD~sNy8tcdne{oZAV$RO&1IzEoRs50&L2M?|L-7 z-zlcEcFxlbiiq7nK}nhUigLzMP=9-IVOtCCyY;ISMq{=gmZ8hGrp3=p%*$&|3lV?$ zA+@LSd_Ev2PK&x8xk7VvOW~wg>Oh#7yvu2-1$gqs(b4_Ri5FPjxq#V2WWkVE@#neK zNdsuFZ1RFLnd&X6j#<&#hlUlVBesv^s*wVBrlrDIxJ;jm_dnLeOkSrLS>Wa8ba8 z7!ku~<#T`TM&Q2C2AJOwJiWAla#kjt**bdLY%W6sR$sxej_k_C*h-k{w{~RPyN`^| z>l3wMF!!KO8)8G*JHL&tQLpww)u9fe6(jt$X}2JOa-n%N{V^J zXPZ@GA3>P8zuSl+e1VLR_Nk$W*@c0qMPZ8qPCkCW>>!O!Atf=oGzJyX%ScmzbQB2BNabyH=qk+)QmH>OMI5J$l371wMH`0m88=VxQ~Ie190 z(^&Y8x*NcWK9$JgcWL&hu!N2F<`|7pH*Nvk6|PO&76j)Dl%G$NrHahc)jVDJ!!6w*gQ@3HnTxVi7c%N z08>Wk#@Q2C_kpATDdrZvJ%iZV5548FPd=n4Bjl&!zBsW-Jo#go=tY{a6#phj6S4g( zuFf2Jo)PP9KRYH+&k3$gBb*z;NdB^UinQ1nyDBQTj>@Y|0RnF5SW+_A~qapz{W;f$W4LQKR3T#;&gQ64*{_Ng0rT+H)90T{R zdGPu8$SRd18J8C$jnK56cH6e}IQs(dV-Z>37$@ z;2unpY?YB^l!ukR9KiJr>u^8SjkbCF9V|CeP4tx9lOSn)$A!b;-HH1Ppu@e$KM+i! z!4IFvl<|2g<{g7&NS9AW^H~_Q^SE4?^&OfnP<=2C4s}3)#SVAe6!sMO9V8L)OPwI; z0p#Go9I(Yu8#WQ;7|Nyf-7>6*n}>?2#eFWlk2oA0;|=nx*78@*w;Gf43>cRX9-Na7 zM-sjV;r&9S7;D5Z%9F{LGRuv!;oUO*WUW)mmS7hdjGZrP-SVZzGr`_9rY#(*ugNYG zMKVJx(U=*X#1W$Lt-%TuB;&Ukv_%C33f_YrV@iEZ$*^_L6OT!%QCRk*5^ZYAZN{m1 z4}d=hiBN|rTafYwgC>OogTDpUuMlYL!vL%wI6L_4&wF zQ#cayIUsc28&y87tX(-?MXB%`h}&XMJ@*tOYmeMyQ{FLLq^~zeRt`bw_MR{6psq&p zx>wVJTf!wtJbYcJFps~rWR4~3#NKu_^q2YQmqO0d$FhHj^yZmfb!dHZZ04RKXd!99 zYpKO)_J^viz)1aBHmiKyZ~!yyk0{MG>@z(};Hiq~$@LV*WGoaC>m*ziy{D@1t+5k) zc`WRoGP=1`(Xe7oQ@obcwOn~O@ft1|ZYH@}bMud*v)Gaph=S+`vA}JK7Vd@Za4X#T z`k`mteURk6h`0rr*+JHle6gWlxRFzA&3@=f3h@#YEDu9&=wMX0*c>{~K)x0GMc@nl zC37YOY3Mc2wt2bkRW9Lgo_Mo8964Gav<;(dl^K@#Wr4^I3%J$*_ORiyo0|Cb+Mu4( zH7`kUwWC^nYW_9E`a3Z748nd2CVcc0sC*k5WIG1T zG`$5djOK+^5WbTbkN>{Jk-6&b{X^3oBu)T_R$AEe=5}w?80k*ROOVBKECs*EI}+Nt zI%pp6H%#m*EQiQ5Bfi6Au=-)}-A~wkAiLjC6f0~xmxG=c3H`Uv?{4?sA?6;b28}`$ z7JwjL{|EAqMtJ5pDEHq>2#bb0Lxu8lZ*dnNA{CtW86KV()@2ersQ#BEwkQr@(5iVE zj&ICi3J#;n&!rN5g{z-Vo)E@Og<%-~9bS1+ZjWtbcai#r86t1~cGN zzlY=647AV2h9aB$k+f(u)@ttUc`VQs*V)rA7PlC!m^y|^vV0Co0e z+)@3+X!Q%x%OtT5NI6=T0nFzipLk{Bx1}b?nX3tO0+>En8vOg5m;QhY`XU&ENo4y? zL?;BCrOKP6;12zv0c%j8hg~!N2JDnR2|MQGkp<>+6VSc={MJ&BApAqTVWBT`Q9;cf z6>2k+x#CfNixdoHXf~nm9Zl=v0-?))v+R4}BkI`d5tHO)nBvQWeF{sFlk>O9|7IX; zDy*Bc`3xc@g%A$=c1Rdm!owgCS@~7c1ijiBeGs%_vP8$4!`NT@+?nx zk=4V90iz!|ZutpfhR&v(or`ZC@^e6Ss8C9pdi(G@VAx=m_YdabBgcXJkcQWi9$cjR+~9Fe(qgruo9wTUHENoOwY-Wd>`fBe2GWtA{JGfXjWO3 zpfj;Nt1*g&TVQ83Qo3Vj!wWGMH}MCgZ5qii`-n)0Y8I}s`!&hV?WBBH?qR7QAO-M} znu~Nd7&_^f;e1(>93b3CP!@nLwR1kKy2nIP<9Z(QfGoXsbdn7+A0lT3XDWCXIvo+S z*G;XGU=Y_6VLxFPp4DLHrXc1A2b!@+3`7a)Mz``OV!5zqkgX38+$Z}gnx2Uyo|m@w zOoTAUhM$9L{Q8hHXlU(!)E`IE$QO$s6@2@)!lL3Ho#}!e#rrnjrRqzO2f>S17v>T2 z`e<ua1e2_;uj7u$E@fX}{SkjS&eZf-gi-9xSuQ5~L@S zc>ZR~_x|1!=*#N_^eYeqfU0YPH&=>3IIQaS>)m;e%KzFqRM^UfeOfcW3g&jh(wp74 z&Qrt-?r5);1OTG;Ha!nch=-km{NPctW=g(`8V!v!oTo2@p8~3E@eMrg0@!r8y6k?9 z-&_wd5tUO^J*%5ytGgIu3RJZVmQ&v&hm>Dj_B#o-&f$yx>@7GDm2=6z6^94P5XV2a zNM~$ryI95QI9LZ$#q(qs{g%6in_wMr+$C+)Fw0;kz63Z0mQj0v;h&NTLLIWkXDoPN zfZW6j^dCmFrJB07{zK)z#jSQNSaQNUjGN30NWpazS6alQQ0ezZUuf-Us6i5Gw$}rI z6fB=k7~*K7AIUEg7*ZZE*)E_l^hhJ~fjI%JSXtX9Nz6*valN~Zt=tR!LLnUp{m>YZ zv`S(OVGWY~q7Ry$=s>Za>l4o)57aPFnNfPIH`NuJA7Vw#D?1ljbXQgEGR%-M0#k1Y4U_ zH2NljJehv(TO5|wJtHIRon(j|)pgxGxdXTV7_+8az?C!cW%@aQb#zv}y({*fo~${b zDi}+r9nbX(>N-w77l9BhG?B~la5g%BLyFGB2s9jg?3@DG4<~InM=>sIZbEWiPr(rz z+1howRIjV!RvDRApnLw)XaqXmjYTt z=&Dt;J9~DU^>o$iqr4p6yN6MCf13@e4V8>0f!N=^qI(UiOXZnF+Sp4{R*+-Ho^0a@F)vrn zoYiK(1sKX0kiflu4YhH1GLtqtr%sW8gXFe>Oo96 z%6afm5++mx?$h`b8>6zc+Drq z9fqCl}kQjdqY@gy2MV_M~zSzRz287kXoR7djp!z+c&R+ zvR|GnyO$p+(k|C=f&pu3@I2E@1n^qC`eGPBzolIc7vT<@h~tf;=g%*cmDRwn(#bxY z^}R)kJ54w&+wf&H@5bm1L>-)Gn>i_xXz{Ea#yL0_&Sfgr16@=C5e5n?sgg3U)#<2` ztW| z&5dn95CYxYkb0Ss&E1h4zby1+AegA+_wm#&wUxw|B?82&GhEifYBj`4D-o3H>Im3U z|8<0$8b}V5)$(bJX4`c1mlT8z8VU+dw}EOSurD7D*tb!&tTY5Y@fQsM>D7_4qww_6 z-+Esnxg%49j-|$w_|Vs@R?|)sL8vdiZLMS406xxj1uXW0OM5WL_a#KH`KWjVtHFf3 zrTdOuO?$>2<6PJFp$F|1s#WYK+%mM&)}A(~(5CajU|6zvbtaR6=}iQ?Jt%yrxcrod z8@t%FK3(&-%ZEu7xf%dVS~Mt@rlHZxTXZx?6gl~dJChDxY^&;m(ugrnyaNuTu-)BG zkxG*eAIKloWh1RpipsbLWPJQa3QCYt4*UWz2}5iE<%30WvZP|d5QC_I#DI|GVWHq~ zE78D`Ejg&u;-}+iNLzNSTaE$d=Vn(&IC7C;=jcIQpIFiygMLD9CnWq1EZX5T?gQSe!{OV5ktlesxj9HL`yB3g;y{MpCU^4yyS z%#Yk%bR1YW1L9A%dkue%lY#&A(#wW`gRt|!ZetE((B84nhaa82v3zFzoi(s#EdLdv z%6uyLxdNe|r5}N4ZD0Vw7tY-dOc(ejZ7DYZV4?teNlcJ4z93-$(Q7RaOi@0ytl{%h z+6SXWIm=8rCCqaLcQ1|y8I&aSOgLX4l42N`k})&k5+1^(cWlDr87j1XHIQOm^~RG> z9DOPbOEu8>dmq9Z-|~2Qet7)fr-UMyXZ`3z`lH~#X)c?k9*Vaq!M!)9eY>C7lNXnY zONZ>BF}MQqsl|;yDveDxbjPn&xEC@334{lJO1^=wYVQ6@SVDB+}gR&1Aa-!{bH#IGk<1bvYj2!b@ zyEf7Xq&p+xlx__4;KkN2QtX#z!1e483@9au66}$Jw7FqQ=Z&)c?K&lco&y62FjN@h z$@l|C*hkRqShEx^Ba#pK3R(TB89+$S6HYArj44i?bl@A&$d1{rY-n%Ry&I>;faXw} zlj(C8=e}WUB_ZC_jKhAj8B_z*-#PQSd!y#c$$jfyl-?VH~PGe6sM- zP8#&I2HF}pZr>Z87kX>qD&y3XGcx+TU!U_(K)0l5&!-AiG3JOdAVK^$wMI->E8rV? z>8>}8;z-sEyi$ZO>^*$RB6RK+`ZXeU@Z^(0Yjalm`5rN5B(l9hmr2*jYq*!^MaNJ< zr)1Wvc=Gi}qhuX2ix5xE8QL#In&z5_E!E;qY}<<*yFLt^=a2V(t}aKK_($ zh}&Xq^BQ<;SnEa==ePsE5Pj%SYKoc)HDgShaHt=p!0D_R*4H!U`G~5Xu5f*~dPDh^ zU!C~T5`p`lSfMIX9;nB+`lFlR=$Vl31>^em3RU&FWI|x)ara(2ihLJXu61I_=tUA# zEq>j|nxEvwf=?I96JvzSXW{F-G;R1fHxH3Ua(0V3)*O{b3@5*FDp9mde5y^*Ko6y% zT3y2&_+(6~uQ5vaQrRz{PF}0u_qgbw%k5!1^)3|l$+YS5d)>#eq({yge_5#yK8?Bd z7w&QVdflD2lW>QU4i~8Oa=*8qOiH|tB>qZepaC_5N~|5Oh6AN`0wDmcoT_^vxQf|c zQ!s|3CpMNNmNve57QC*DvL3;W7YUpZb~8jmD}B1~_6E22JS9_lGz~C=!h@_EzQs#4 zdq>f4{pLp(v@LO@9q4UO1DS|FexqA~*4K&7T{3Ez;$RIppLIU!zkURzXx!RNN%Rv6 zilA%#LgQ}b;KxrYt^{Z8K7a=!L^>%1EUEW1EtDYeCw)Bd&6^iU^OD3p+iJKhh?fb= zG0^L^Y&+&|CCJHutH$rbZENX&%Y`%hFZUdZHhyCAnRc6GETGJO8*qqW2&4cW%{g^k zR|tRrHu+1b5iG?T^o@Rii$d6Z>qp!tiRzP+2n)F7VwX!d@knX@dbdMi+?}>$)w1>@ zD8DDk1H>)!B8|*_prH^gcu{ZX(ZFbce>*1wa4oS2oYsCmKA3G^+`UMFBeHv8@3~+p zr!-doWb>HsPZN~LFGLaq1`1Y-7mS$BR)ghf(o*meRO(XOC@y@b%7LTN%t`QYHa<11 zan8ABMv-nQs;;-`HYYhVo&t?vV^(5Tk&BMGi68`-J!hb9%|Q?qr@Z|l{SX4STE~Gd zX$y&1B(H`(&0S?EB#0{I8LnRs#8tm;o#!%SU+TB~hxVp_msAWJ@ zH&x%+E;xhA$duf;b2rN>Q3drGX|8TTU)3>u=;Av zn}Lcpc7eH7+4_h3gpo%A*e(c>5oP=kXmE{|je!yC&g$%wb}uxUM;QY`YFq^WeHUfD zRYa663FP~GPd3;371?=a{1M0lye|>-Z}Hhmh=$JI9LLNyYo@`vrX%OVaTUmpJfh

ZBQqD_7}^V+>DNd3ef3m>(!O1m2HEUbBqk5T zmO&He;6&X1`sdoM*)fRN)@l07&mXA#xb?jC{-7?P(JA1EHH0x9gBScDtKU?J(8(459*7WwCb?BHFDXmT>9Z2cVx0G@3cxo`^3pYq-dSn%Vv0`dH3a5|j#B*h zVOYsEbJAXUSnmxs!RT~gVOo8d5(C?+!oPjo)k|G!(+4j>_vm4qf+MO^Dl`O2Oe8!s@lJJVHwx8O3agBgcQx>Ei)@)9Nd_oVL236@C^9~Jj_ zNVa6uWu&~Sq4D(Q+(MApt+x(H7~vhttScU=d@5=`^8uke9ZC5Fac}R9%MyLFfErJMa6*22e z-p4SyNfQsbQS-9+&@I~o)|aj9R{Xpm93*$gPc zB>8X?j^|E)!w*ynU5OmE;?Y`Wec-VA?S;wWATBX$03bj>fp1CbuxHMN@n#rz8N~Fl z=_XcYxZ8QY3;PNBzSmu_N*JMmASm7U$ke}jv4rh2b$!Gd6foJ(yt49nhX;l{6mS8W z1!#Hmv)4i5F_#4}ycVz4X5&m`BT*ig{rW1LL&kNKJLkzq(#3#@!*E#v?MBy+ zs9N%E`#a#P2KV59z+9N&gj^0P;KKQNGn`;}*uM|Xp`_Ql0mX$uNZ)L?UWAV@HJwS6_6;KqpY_$Bn;3x;uKL2NL{PVvV4ma#b2%~ zd_wJSviKY*Id^oau6D6+`23j=n{h>yzqjBW>`BBke>_(+$sYpa%4+%6vXttY)`(le zNEH?rgQv4WKsl|3qI6x68NKk}-8=Q@4DS64B?CKf4r%++YTw5FYBEMcZr6SFy}UD^ z+$qm)B~{>6rkPl%Y9&|+smJ&=>M@2^>MM>;^?5tJkRfWNSg?1PDY3hSSHatH@#0cliM`O4+xF3nQYCe=MqibQ_>KjH#$a+UKacCKupR zT23`>SRc_6q^)kjSv3Ib(B|ut^=IhbjdlF)?dM2%?aBDxoQzk0pM7|bRW}yIbDqmu z4Lh;DAkV!Aa$+;trax1_$yAh)SKhOlXxH#GR_eh0WRxIaAy!a@5e@|jNQy)Ds=;Vbdu46g@I-nMYE>6fh z`#pz2zqk8f$*%o2#{2#9lGu@ct*)FKiv4?*3(02E$F{Y%S@+8rJCar zL4RpR<>9ctg9*Uo+Gp?~+6*JE=ny3ZopjLay{^CIlN)-Y>ByRO;b!K&*kx29GZb%Y zw5mgQIce%#zy0b=h(VC_nxpMi#{+{4mj#0%HZVJ5F#@TanlkTiACSuZ@}xix*7Hft zN3~ezuGhPIEnD~8?wHvM#9nA1VCZjskol#)>Dv#!F_i66;nWkyc7Z-qXX+$?_H`#V z#~KhD)oTu-#wa~P8CS9)q__`1BWXLa%fb3jh=gC>3wLdFv&WIl zbGm!CbT{>qanyx)u`N$j_G1eUi}LctZ+at6SMgz&QULlH#wqhtoun#94cwhs(gONh zx@R&DDw-`RZ4JW#{ie%ls(a+c&^6#bcPFA*BO@mGZn$U&agxPi8 zAgX7!dTawRnpXT^n+uS%&)0%~X^I?ZX2LmM(gLxutP!^B8=~xx$SJ9fp}>2h1V9no zB5~putzk+`qXl1(X})@tv|9r4UnehӟS5h8;4TQwW+~JItdgEEbk*K5KQ3)o* z?tdQ@HBu?<_+RX4Z0GG1+iP>Ax5nn@%XIVS{c30W{a!9kEtW7Ze`|Hbvn(nK-d0)~ z!hb)@)ENb607c%{F{_}2@&4B08lU7$Xq=f-c9dkf>)X3+i3FQi6;^J-YgUauNWgdDnJSzhzi=>mqG5L2XOLewF3 zR>xSjySW)bL&Q(Tg)9fj<=RGQ4vC(e9$h34Ao!uS^T-vs!t$m(o}0@r4L=2`d7q0{ zWrtBz_7K&=hQI&VYgWIC7D?1{sc?xIya`zA>rMT1mYMSL;AJ%ui|1}aD)P6~HJ{Ip zd5&j6i88MI)Pc0oz657De?_3YbuYZ8ug~yksC|F*w$Y5Ng@OQtBuR zdRex3ryw^axDxtiAMcqMG3xleRtA>s9NRx1-1qdr=WIYAqf@dN0LQc1=dfp2{p#<# z>X!b{gRqRwWaYo@<{$g~u!I9dV(nUx%T>Ybo09ldB6!8`M;$4dZC9^kLsXm!@^8{Q za?5!;X>jvc~16B3-B3aE7^rLsRu;0Pl{H#@s%EiLb|vzyV~c<&u?)c z;zECb)oy!R3GJv+9`A19H?ZYh^eZ`e=x_6p597LP2aC43*aB$(Vg8{9N)0ZWTBN#Y zN3VH#+C!)CKo48N|4O{DZjJ;IzotN{exrLez-k?I@)5KR!|LLq*AUKY=jPTQqOHUA zWsXMvn1P5zoA)qXPx9n)+D?#G9Xfxs0z2rHr`p0J73kZGYYG z^A1c%GC4l!Q>DUwLB@|vM!7qJm!aEgql$u5M))t1YHbXZ#yf%_jrW*Jv8RZ6e%rp< zK>(xhD{BbZT=$_i=}Gw(ikpW_Yb{31E{cUYLv>G8YN}MM|ZIUohZpZPGR#k_gr27-HI*!bg{tHiLAMqXK`G_xeHmm z$bGz75eRWk-xTz^)tjP<4#CU~XzWIW2m}9(T-r(6{E-TKMLU(Q&Dd7&#FbjSfF=Rw zf~6vG+6JY!L_y11LB1uhT;j9hi(}`Js`io580g3Pb!FEfd-rZ7y*l8@Z)0G6(Bd8O zaR05oQ{%qnSVThWX2j$bg&cp6`OygCXLmrqq`>dY!;wH}nziN{%8VG10t>&U+n?uub>Lu*EG1 zt?);;-yX0p1OkGkn>pIU@hSgd;MMoa3q26>J^D5!F2-*+{$XpBVziAg{?9$oGZ}OB zmP{R8_{fDrCMK?8L?pS#6>i<8qZb5)ZD)vV3d*;f#@q+UM==q1e8?&ckNG+L96|up zGYPnX!N`M@2L@*hv{wR2p`S|!swPCD0F8eKs7C`JvF_|<@?S~D1j%DY1}RGfr*els z8Q(``vM9#JF9Rop6FaIzFWR!}=8z@8sWvWVCd*z_hC!Wb$?97z55l(;yPHPC!9~oY zKpt&gC>Kgq4UopSeV2|OV}Bc9L~Rb^WKROvGP-*q_aSGgn#c{iNkOo1R1+i@La%6ce!cB)Rg1x9 zUIKcBFB2R5Vnc7eQiRdyGXEa081V#OwTX9HCHROR>p5pt5Q#Wb2z(@{++GOklfwAR zIyaYfjcfO>R(M~FKMi-0f7zYi)%W$^P*U!xXJvW=lpy+AbMk}Lp}t`{%s@o0&=`+D z@&y4qJ0IxV{{TQH{Rct)iuaCc=nFAk9xTkhh<%l~(9)9J(Anz=xW{B9a{P3cN7||! zj+vzLvdyd~WUpect|g-gm)ONfS)DpDr};hRm<5T64u$7y+Jmi!Hp=AQF&xHTRv&AmkX3FM8B#u?~{~ z8s4#2_5zvU(i1>aD*Bekf-@@Q@7DwkR>F&V3DnDx{gU7?yDnmzvp;fl0rSHVg}_%X zoN?XmiH*Wa{4y1m-It!LVZtOG4W zBmcaA5GO+T@;b`l7d@|G7tG7t(z-ZWbfkm^bMxDR6X|==096tpKgL{^WiSgIsP|Q5 zEDtDnbas$Zz_jl++tkHXnRJBbEtnvhxFq?;=Wi-DQ{(O^JUDqbZ|fv(H|uo@_xJTH z1r7PgM4!3aO`878ij>Jc1gX9K7)UZqYm0YvvgbyKjFPxLT{~5}cQr}xwTn8~L(;)8 z7Qp);H{$(siDK}n5dQ!P$XZslhj-=GT*Z{I99ym`0xNe^17~X{P)M>qH(EuhJmrg* zxcD^k%DSt4)_oHo8uvx@__yLZZvC0e z@Es}xY!tp@xu3Em>`NHwJ)5YHRck-kkNb?>ei-?=j1SBf>v1`V1>k^#=aVqzmb$0+ z+k2nALp;vMh7`B153>8V|9ZG~_gLwZ`Oz*M5%f_a%OT79@s_`?N{$q8JMGt^UcOnj zjnHAZX=w2?MH>>-C~`I1*>SS~sGUjRwO#-&%mErp`^&)CgfbVS?c)bzro)ihuO-Qce9RAW zJ=OBrKC6vwo&-Osl0X!D>7-|r${0ImEn>*b8nTjXTwgG3N5;T)83qvgY()dP?s)z$ zJ{gatAC#bim%z=D(z2gU3uNQz6v{_vWt|6ApeK!yG=?U5lyOn1Vnt)SuY6y9F4wgr z)j21~V{q+V_M3K!Kc40>2}CH11XGBo7_(8p7HsJA2oA`-G$tdIMTTpd9Nm6YroOpH z<2M8_Yc8^6;e2e{>~o$&jtPu=lw&Vo;k!aS5z;Gm=`72+n+Ydj?rc(*$D1j%@wjhNWiIS+WXH$06Dn*0Y-a8F1BI4k1PmPMBpKRS+!!pqd$n+h&SAyz z5E5~5tFR_D3ebrRQ@Ud%IoyLeL7$;jL6J#UROX-94CJs@Yne2ESeDRvjLRWZg5%*@T3%o(Ra#-R$Kol20Ohv< zT?s)>=Qz?;lU#a^=;MGzNLaQmuU*$I>*s|p!`4pj*BGet@aUwKukbj2xrX?$Qs%KOVH~=4QWKP z14LKRtlkKvL`(D4y`@R7i!k;>8Zj`fSje09CIvufy`lCtZtJ=_R9yE)y=laNfg*q2 zu|x9si~n+j#Z*WXu|9HJTMmT*&hS{qQjuwm^qdp z)=R{=_!7$Belr0`*IdzTs_m(<9PZFsu_DO0e1~dijt@p?o^!}$E}7>w4=AIIz&IJ|d6k@&jV`nNa{9+ST;(<9|X`K?GMT(T=~9VNy|3Z&uQP{z6Y*5vdeA*@%X z2PMHoMCx@NB9@)4Jx_opB0DE|yMytJX#ZJTGFnLi-NTx*Ag%f+wA!Gm=Om7kkg1=0 z6q^*3^#wkL{XzuXXN4lkMy_wdvVQB?gLg8F9(77c2mS2~Eom_tk`H_;Nn~o1`6V;{ ziu$+DVjo9Nvrn&$iy!U8zmxshT%XD~$p?wd-+V5+bWt}_{ovrA-4+RXYWKF79NQ3i{`9A|@o+x8`mdlhK%bQ?H_ zXU|;;1A({1=n#)I-75TmGc81VUF|;7+wR7`ObGmC6!nz1rO?3M0Ng62zok6>1-)JI5&#;pHV!MIY zzby>GOzwwVwY=k+KDP29mNwa@b4wW6*5@p{**b(8Y&}x=elvdtU1JEaCeH{D7+Ryd zR#=LrFpa?4xC_*tNnL1xCM#w;0Y_x;(QG(w%FqBd5@?vIeGsA6KeI7U1Cir=k-D(n zq#nr7W6`^ITaTpy_1YKhVRv6fOt2UbI}<9z_C)f3Lja-Bm1bK3=&J~2;!_>Q zObTOsN8XZqyXGgpd5GN_yHj!PD?_kls*%evwr>D>?^sIyg29aQRfo<#>cKG6<7Nyz zG{1j4AsCju^RbEW&6nC`xinjjZBXJB&-8psDA7N?exSLDG~sUh<|c9O@puGzyY`U; z(xh|87G}tS6vhwPQm4<{k2s-xxPpuUmPh-P#}GL@z0cbITi`EI^(*QNymQ){#3sWa z7kA>AH*Ln_VzmW%C|h1XH9FDzjb2M+%@E@3GyWOQ+qUw#@bhQI`1e^S~8qe4C zFg22&mfQNfMHlH0KitecrPboIG7yN>fj_pW`NJ2{I%*o~zZ@wwYJ*Rd-dflmcqvW! zcDWa&tf5vqChJ|GqzTSBu`HFmgd5e9f1l$g8T+}}8S}4L>Cl(`A`c10Br9Ila55Cr zc+W@LyPrgvk@P}}7UebWCP?N|`+1ao9#)QJ`^&C%ey#G1+y(?+z7oDe6vB5or3A#I z%X`lz@g_>en&HJ@ic$D28x1ewbzoGpQI!0$)`}KWp$TWCLBe zg;(6UdVxX_AkNEsxJ6R9H-T24oQ1Oc_H*rg+HmP4jJ^~%eK_RK+U>B@?K%W{k^1|U z2v4&d$P$8l{sDr{0J&cPOUu}B5zCGKlb0wOusn8@rJM9OvkuRrO*x~h-(U2ef zQI34$4Bt$LBHDp&)ye_kK|4?BZU2cYSQtVzL1&)Jv`X-1ihZSD);q3y``EpnYtQ8Z z^fq)Kgl^y2wH!8Z zxDwP7dnv3u!Yx6dpW;h8g84QdY(i$1$I#YN<8xW?T=D`Qy{xU;P}E+KT-KJ{=r>jO zBo6-tMx0{9ZKl~TX-XxCT8BaxUt7v@^ar+V?KUOboK6-9(~rQFv&XPH7r(px$l5jsR3Lkx&6E_ zvd4G}$|$~#h#NcyiB!&Q5UEgT9wSfep!{d1r+Nas+8dN0l=vAVwu@)%May=2&hFe+?*0UGj zE1ib`#vYp^*6eGFOvTa(^|oOT=^>#ZZyhZ^#dc#);B7m$P}=0!BC20KtOY|tPun5< z4T~&9z7mqWiJSlxK+pByM#(;LAXa?D|fI^g=zwidV(2mBW1d2;QQ7rK(p%ya7Z zUJ0wO9*RE%*o*k1nm4T887QrZo@Q_y9j#lHtY#^S1X)L6*5Ycjtd*xeW+U5e9DjES zuLRe!dA{oh)y0hktd!_iCmm!o2hpjww!R#ZPL|x6mXmMFuMRo_Y_Jd(@g551S^J?y z`Kl!`Sro&(X+X|S{C*~O-G(-`?+5`y&^#8|bRb88^r_p17HJ}QRXBQ86DJ!U7-4*c z=t)O%z*2RK0;yRqbE>OUm1{TCw~6N(M3TGlM$5zwk3{$D&W^l}p`d#*Tdrf7EKr5F zHC8?O*0RefCU_5`3{&*xTzMJQ@WDkav4FHe7Y;}>d3Jxo9Z?U*=^%z~t8C-wsvd6bHN&IB4Ca?4#1rC;e6C zY)mk`GC9jDF`j79JzLuF1+`cK z73?xfJg-NDcqa`sPohnJbjdOPSkAc5*?rI+7M@Y_{ff7l*K!TG_La;|%;$Aqc72J! z)p_8`lis>$L!LJSV*IDh0AW;*XAwWd+0io?EkYx7De#z}=2&ffT##HNR%@EyG{7?b zT!VD|DVK5M7?ut27Tc&S&gyRVEB3_`8@IZH?`?RhT-S2s(s&^Ssa&_~YYKIoC$0pD z2@((0!nLB4U@)*>fDAw4gOMG8Rm9A-1Jjs50wL!}_WLE-AiLiy0^uXK>)XcYjTBtk z<__Ph(|ZdcBoQ$>dMUz3pNWhvkVoC61D^EOK63sDs9C>^{4qLf#hrLf-%a`6q47&5 zg55K!PN_I%R>^?6+&H&YTYW_ico8fx-8wFLHE?NI*&MtH$=YQP_8TNBpmd0BXK}d< zGh5K9T>iFF8k_Iw!o=E(h};8zMuP`wvty{V<_(k@sE*#iX^BR%2rvFjYI0BWvi1h8 zdkoq^ij~xj?X3kBQPV)@@92Y-A43$h=7JA?zRpre^JL5%#M&P2qLls1F+D%`y|NxL z$4T`cb;}iuR4Ys5Y8PBCytUnmM&T7xzx&9T&&j?4vIAu-gSVL)Obzd*vDiyL1{BLr z>_MDR<20oIq);4tULvSA#pV5uOnA`=cwN^=O{sCnR!XKVm0~6eG+Fw{4Zb9!wj@4S z7iVuShj1QJ*vQ4`AEPji+Ju&w4mVz+cQiEa)lDNl_lS(cek_V|!kg1}GecoUWJ~lbeB1(&>4-?l0l~zVl-i7=0gDgWtq< zjAa2T868)OtV0&(N(9)8VT_J6?s|}fM($spVl8dKT2&mhtmE`*u3gRdk;w5?knlHv zuP%Ixid!hbI=71SN5@8$gJx_@omBd3V6vM{J>j~=qOz>Y*GrT7aK2Tx>4@+>VDGy! zz*-Sa!N!%mfDxt?bu?SGi#iDxf^%R*R9;&T_ zwP0zY`kN$)j+&zDppMf zeOOSw@6p4Po=o7l_d^cY@HiSo56tuvw>S1>GLa#4$iidyWFgPo4X~ByB+L_)`E>`c z$?F;s%1pa(ogs@Tp2{we{$vrZVK{r2Bfs3I(2&ek6P&U(@*wfA^E3xHaGba}2gc{k zc{594bQyQn!m`bFYym{x@A#C?bTX&q97ZSiZcHy(lXvQhy5WS^zRS<-*BQTkPp#_= z>ArQu#gX=y8X@u9LSLHx#$}Y1i0mreXpEx8kPWEGbr9im!|sZRPbI}Wf9G-9tI64eD{k9TlI-?kKgmdWFz_h$0_gsNcj`StF4*B10->@18Kkyka_+ZT-hKdch?*O|k?y<_%c_?QxG@m-yED zsC3Bq^ziCi4#)g`6tI9m*9sGuIVwIcS-OZRdFY!9JPJNZ zq$_@6-8xJS6vb+4Sc+m&F^F|~VeS_J8sPnYuDlP<$NLmB#6%;3clN_<~_{dviL{TV0e^y^Ph7rhbA z`BxGD{QewV(eGb>_8-Y9P=6r*i~sQ{z7&f3?m{I~q~QC*y0KfjTGOaFgAKei(NjO`T6W7tk-kb=jZyzN6T~n$A6mXJabxw{HvshF9rUqgimAq*6zo+)TbinWN*7T{{C7M zFn8mRBu&rCU#kD*ncw@gX21UD{{T=+2ME)o4+16$0RS#x0RT`-0|XQR2nYxOc!rBh z0000000000000005&!@IcW7^HWkPRcc};I*Wo#~Uc4xHt*Ru0Kk|qegN?OiMx1qA2 zRc~dD0R#xGC2wZ_vA{_Lvi>X}|!TeBV>I|K*@!vh}fpCJF`|GfI{FGs$t^*H?f zukb%ZfBj27q+?bO<=_A6x!=E0fBirI^;C@NVI1pJi8lK)TFo3VZqlDr@3{t+_m>gjJ& z{Abet9``@5VVvT$-tK?I`}g(y=hgk~KL)({-IjP*i*f1y{?|Ul^G(l)q$u|Eu=C{)_r%@!t<`GXJ7N|9*h}^8Byw?#R<^T>h39#e4E^3jzQ2U%>KT z|0TDaHO@O)2(2Qd<1u@qbS#{Qp`)|5220uAu)&9Q1F`{~HUE`p*$z z6#XZd_=}35@c%f%=$pqsZ~Vsz@&8{6ssH-_yNCayUjOfak?h;{zkrLr-ty%?uK(jz zSHFiBBmbXShyNE~_5Xbv{syCep0GcAQTK6~{|Efiha(2 z>)$4)-xhmu`OVaO`~#%_ty=Qp?|;qWE&gx+m=K(X^8b^>YffPQRQs%9pO7MyBlA9V z$E$d+e9wwMPa^zbet)jV+kRJnPCg+k1)=tyq2QnK;3-1&qR8)g$?zR-`8GBf7o8_( z>-gc`p}|7#Xh%0mrach%XwqFC%+F`p_8V5~y4J3Y9oX;6I^O@<`hYNZrFk~v`Fz-p z%=j`^^QmvC;Z~;A&J%kcJ$^E?fM}6K=ivf!g(jHB|M)68a#Y3>#sqowMM&- zrX1YX>Y1;iQMx*y*LvjHNJ;g-{ms8nK-w^L!_wWe<06Ug^4R`>)zU{uTRn<>WYT$? zcOZu9s9|Rssa9gP4S!jrV4ox+JjP&*&pR1Y>yc^A)0BkDB~$NbHb<_SIKrnA#?iye z6$blAA9lU1mLe4OyyZzTS@s=jUk*P5%r2XZ)hOf&kC7ZSUVc9)0 z$lJHlnsjn0Y}NeguBDUFwL?(<3yr{!iyDpWi4S1jZ!A5oGavqmeRu!Jr2APig17L2 z;`Fdh_voEQs6L(9>=EIktq1zDna)YEsYmt`w}2=j_?ACRC#x#mdHl)IOvX)LL#t0O zhxj8Sh_<7MGv}sxwRxXc@!?>a2uy1iA|+}t`R>ulx$6pf*FeDTo*^7lOJ#vKb5AaT z$5Pt23)dd<-Nd313wS&BYP6$>H|ivPblMqP%HG@#)_dgR9Jrz39DnmzB1_a)+!!3B z7Xp6cTW@?4-0^13U{ibMOiK8nYPxH+@9bd$jupm|w_0HA0t}yjEO@k!7a1}lJ{ZdS zbSSoEFcbYZ_A}p`KSEdW@D|n^=EkM%0y&HlqwSUG#e92^?DbR-5 z=Dfu-H?dB~@ocl^n+7m>J}r3ya|_4iG^gh|ZeiJt!zKOPKITEjG@^*#O0cm^RWa1{ zRpm2pg%EWO)@N~?IwoW5j%*T>R#0kq*rQ_BT6GeBTAwNPu_}q0V?W_ls{g_~2WyB| z+izA%Cwy@eka|UXP-1Hy8wOxjwz;DMuKPJXA+kKxdwXWuW+v`+5X@Q=Jd&*`=Wjs_v?xQF3fQ? zXHYL5$SGcwWZX0rc5W6(<7npQ2q1^&{?zLARN4aUlkk;}hn6Sf9YY zU=ts`tv?Cz`S6-ULr#hJ059hz2;#5+S5XruV$GBad*Y4pDf1ms7ZPqE)GN-Kv5Mc1 zSM&5mPtgvx^?y>B>efwB~BNij8vvyl5SF(ty(CLFMeMc~OW?c-S-!|0etv z6`8TeG_m9?r}$Z}w0=cisdGf)`3cY228H`8?s}R;#`*+l$c+c{8#X(g8mXQRgK!f< z#Xq&N9b)+u`}2=^$UpNaN-ZL)asHV@@YZFu4W%~N6HG-rII7mxr)I1GTF1pvI&bOa z7j`|3?ARRrQx~`JtLR)_%EP9MbiM{#bD^IG?COr6X!ToFi$3$EH>ccVfdN9-1+5^S zi*ESrfcpMAhdZNlLI77L7mZf)$KBg~RpU^V2Q~$s69C|sJqzP-~@@z1neH9Rxq7aT-REs#^-`6H(8ejYM=#?0Pdivtpuy*d)!) zZetjQ-gfz($S#e$hsJLcYl9Gdy})e*U@v560gdvl|W98gqO z?j#0w2_?(L@j-GRb)HoW)L*y@TVyhKu#1H~d z4-E@ZbdS;Cjcx`8{3-?3EYeWpHMVp#Z8Dj35xxI*JQtGgsq?7{KVmpJdg{?A2JnD+ zIMX+L$`(2>(s;Gom(*Nvm^AXcR6yZur!Bqe1Ng1W)yWjVQy~60hrGWvO78kl9DV>A zTS6o1FwV<2z9#zFMFStcCN2R#!*f39<(7~6?QpuoJ@70JaD~ES?ngp0Rqi3A@K|f> zeyK+1g@?VW!4$U{?r+))4^lAp2dy-d9A)=7CqCQlh)9)K9ZZ90kPv&MXc?nxL{jpO zxrPNK=?6V_rc)jx4&=3R33%C6k28?wPHt8YmuvH#yK$)6U@i`jdnl$#A&RSVfIBaT z<`en$3EXcr+W?KV8&~~qYor;<)-Ypyd1CG5EQlm5G&Zh-Ptnnq9&C5Evyw(7ev zaMVMmgXz*StSkwF$c##}RXuR2qa-Dcdv`AlQ8i>vy>^O!0FdR@g z{|+{ENDNKlOWovv!Opp~&AQG6`#$`>1vu;@Ih<~pcORb=bpnh}SugU+ehr>)90n^R zw~!2Y{v+)JE5(lYc;_?RtravoNh&Dy?if(@=?|R!W8n@TkzDsaO$zi<6Q@)}Eq=@< z?8}RMlD=~xcrd&;hy|NH6RKQ^PNAlR)BR$2TziU)BA^~Vr-5e&@h7hy((lhWYZGc0 zTP;D`#IARhS|e~Fp~mIn9J2X}T@hgA3UD%s)IPW!g0}fOOe8+4mtZaJhe*Qlcqg1K z>Qk7*N})~xe*b6!F6TB8ez+^jVFKF#76B%da%ps=4(zp?JG0@ByH^d=9RShj&n3NF zOKqZ&c$bGm;A*{wU`<|CT!gmip*B?xn${k~PShaCz5y_cN@5Ai)mY z8W~slVis1IAYl$9mAuU2Qs+lu6?%StCeEY=AHc%!y(43Cq8LiAg&^P-aQ27FCcw*{ z3()+1a}6=vGqgiTYQJZ20a!<`C!yNu=UO*|_+ZmXldzrf_vTnKLn3oVc( zd%j462{vRyuCpIJmc`M&+_2jJcTfo7rR0g6n_gHp*r=JhxqD`+EIfls*?n z@}JM?aO~-r8=QY!)@*Ep0xPR1)?eRt*H7RW`oav&)2lPaaKDUAcz)_o2tUt+oskw$ zT0>@`K!R3}JVv^CaP}5BZ5BPof#8gD#S{8Ze46D;M8bBURcRMyw8!XSMf6CG@~|;3 zy@}hmMIo6ldrDUtRSrlW`OOFk+gH(v!w8q~O6v!#)i3iuAi1(wo+VT+9zQpHRmF>| zRMotzls;f#Qt$LcrDxZ@e9@5q@>y0xeQxUN25^J;=tss1B%w)oD7uv>k_}#i&a$WCHaT3b_6K1zis>c-5pYo+CfCBTB+R!c6rs`8^usvj?Tv|jN` zV!Nb(m@79238ggTlIzg|tW#mXh2;7`~U`J zf^NB+BCu$Dp5X$x^a2d`xmz6Zu2wXTau-gZ58AB8wuhRypu=xN4-)9_#jQ8+re+9h zLGepz-z6l=uLwS#(M8GD9&g1Pyxb9yz5@2a&V6_J@==ofkY96!yD43u-JG0#+GRR; zEmZ^N0o?fDTS9em<5YW)9+5`{qSGK`N04jHy@ZnFZtWPQUPOqCTL5GNF5|DM1URFB z@r~|38%P>MA@CIrPT6Mgx$!Zx<9!XrpSyX$fx&ZHcD^GgbC{kq#6Utp-i*b59L0{& z5$=enOd^o3%%Iiy8_d#<;^aof9q9~}MiVQyiuMQ{Y|-4ZXI`f-Z^){1%N$*L#_q{b zL}74#rl1Lf5kr^-%Ke^UuR4J7AA&%s2M6NP4AG?&H0+CXUm&}5$nJVX8FALsXkeMT zjUXjs*8Cd4wpKL+V~7LRH4%(nrbU2@EH@ll1A)O*twUX>fnawR{1U&Iw!nZ8rFw1| z2+C4noV4dzilvfq_BTl(FK%qrf2~uBOkzOmOx5x@roq&1+(W`eR4sBJvAY*LhGV}H zKz9qUDv2TbWCMf*%^3C&PZ+`QlAPxOY@dT9>0_DJTMPyHG4bZ-=X`^I1->x;fN_`s9T5 zMP8c|P@m&d`)2hC$*^g93SmY+sq98k7 zaqG~O??fJ*8Kf==PNZ;MW|bYjidl} zuc1h=A9|1#NwNuMHbqg)4$21jp>R;*njfNnSr7wQfdX1I#KIK}%y@aByT}W}H^lB{5E?@wgu9*7 zBx0t901nzF`-_tyhoZ-yz1DkzzLynlUTGn`>}1MCE+*I2uvwfo8c;Ag(gMEMD*dY! zL6!;YHH=N;i-+#KrgpoWOLdQ98=h%>`st@$EBe;aUXFoKcgLgU_Aa(NY2Xj8bC z`#nTs6`@38lEJIg!#VK=i*zFFZo%Uuuq;IzNF&M4KU*O|mqB-{D=zf|j1aQfleVY1 z*o{6hzIfD#?!-~P?wJmYvsuAh(nG?bfv4&3iiJrZFB-i|wR=Xs2Y4qFbaXH&+kkF!#P}q(`WeH^sHC2g-U1-C8nmf@z2K5iez#lDRl|cS|Mf(wHyrDwS|Wg3G+13 z$|Z+WUInKF308M4hBaY`i`L$z7+uf_G?|q_r34UTFy?1ufw!PiOeG-~hF`r@8aYKx zNAHHk4bX-Fg8Vl?acbyxGkN{VaOhovN@6Lc^iNIKpO>HKDnF#`^e4kT#ng$psgcQj z`vWCo`g7c`M^sA{4Xz!7!65fvwA%!adTI6;UyKVUaL_L`|X!m)^5Og9JNl^UNzMt!vn5?Xd zf(*@`3c|-!qlK%<3JqRD!YdTP#Q_`Wi(p#{A8pG$9N(iOh{A8lg9K6!Lm}nHVwiA& z@(c;W62npsm+JU?m44m=nv|Z#04L5}SfeSVdd)E{YCI#+vKxDrTeyrh#HQroSS(q3 zpKirn^UwXulXy<@THk;GeI)z2qiy`4^!0fgeJljdMO z==8crnH*`>2@&p zp)Gs+0K8!=`&da^8Y+%L!;Zt87vTG1%rD!~MA^3xrfs`8%(I1?d=?_u=+=3X4^5N2 z2bqU!4m*;*`MUJTz+bTs(rV?_o4uR zoJx7U;@OVlx93su8rclQ%VfooGbljdO}~j9)OytDQP|u|wtn@9SlMn`^G*e_dT(Z!<{M(q#14aWx3uS3X+v~f`Yyu$dgwwP|l32eN` z%U{5D1@X*LQ_cW6yNoQm8KoALcATN??dCzy5_IRHE!8T{gWo=fqE2owe`1lXh@RM zEtUO!((LQS{thpSJyXbKeEo?9d|WW1q^#G5=T5H;gZ+11*=Iinfx$@#L6sjYg(9wF zR0Cqq{K|WNS!E6EKpGxXy&8zDSr3>h zt?2+|4VrNI@H7Y{;j`oP{A0z#sO9k4asZy_%QjSEdtMbQh2{3fm+d@}pPuv`;G$)- zEbG(VD`$`gN48@WCai$|wF_Fr+mr$-VJJmj(QTYC4++O{h87iNA}{LQzo)Hz zia@3afZ^CF_z)y(K}3gJ37=j8(72zH&pj{whffXi6CQ_%X`~govPS9O&7mZ(1kTYN zZHE@%+}S6GPYgb|nau!(j)Gv~bq^u1-duIf9sFuyAI4b!2v;5q6UFc!eE3{A4BogF z!gz+93;@uc4w@~v>>>xkUjra06{hdAl8*gQj>xGjK_h-fhBpYLz)SF5Uo$Z1#@5OJ zh=Y4?WgzOx#)^Rxri2OQ@WjPr&fkva5-$ZqrZ9Wc!vID3R_^jWY#^~TqzDE-EeVvhz+~(|emdsQMQo>IN zxlr4)+_-OfTCZe00^OwCiJqtDEttB&avD(GUDG!MZOR7qFY;agv!TTqc0)$8Xog8g zkr-xugUPAn;3j=t2UH9MsM;^V<$QP1g|$lP8ow3&%cdFTa+8Zp3Eg9rltuV#n1C(z+Dlt829P4XaYSd;b`WS8&Z|@W!8(~%2PvOUcDw^4TQj9 z(Vtpz)0gMi`_~$GM8!tu^?aFEEquuz*9@#zXGIzNE}>y2flMte@lkcS8^hYs2Jf%= z5EH{Ng2orcGgCA^J6$%#pp!;n&WcoJ?O-#~S(mFy!%gMVR%eX2PB_H7p-}Qgc zLh76IeH2jk_L)Si8sPjYdXi`CF-6Vjodz&{$w+>E-`ax_Z}|u zs##9tRlNoy+VbbS6I{|%i-IM-XTqjnLGc;@6$pPK23YZ1lf^?(@(GH2oRD|$Bhk%(2XIG<%Qa9u=_{l_}UOW;8%^TDs?qhe%` z4DP2f)poPlG>jxVpmy2vE1IER>)7=2$NmoA%l#)_}*_CX`b z*3MlZWZTJj>E4pGIwJX)c&C!pHUf6uVI7difeCbUk@Q;(##sY(k)cv=7>(Q>8u?TY zeP*=j|2Q$g^v>7ym(Zj7Mp~YnjN&$pU6 zbNx-P;68)2D;|KrFaHe`$uI1%pONu|8mCQ_F&CPWaQE11_;=!FuDkg(Z}tcK#fOQn z1c+alG=RSF`Iu<^`gTZ6PEP@i~ak3oeAy zoVM96+m;DseK+b0cdNV4raT7LY4e1hs<1{KJtRW_dVx2brE7}saa=^gVfdVB*Cc0z zWtQ0n9^;tdmIa{c{JYKRIjdK69u@f#<*}Dv$V9}cfUK4Sd@zXRB$2xcULG#G^3{1!TZ zSqGOc`jExE(r`rVo>y<<4&WB%ArkJ07 zUbEinnEV40{Yh+?gPDLe=0K#Al-_{5m2*t;`STkEarJh0nG;dJG+Blc6@K{3?6Bk- zK{tV}AW*Jj(ra+R3a~K2tfXU-o2_Y8mEgB-)LZ;E>2J1kCu{|Ya}e! z4#vLi8N3ien)Wi zwPLIJj^oOYkpJMk#d(JLFpT5btgkF53{@^>$ZE5zwS!?ocS6ietXmdo;ZJD0D~i8O zn|wY!|G7TDOPR(xj}S@8X+rlgT*W_B-{#v1Tupl^RK~Tb9V+SltCXGymYP1Nu`3oU zvZgl^cC&cIATLglH=&NjG15C5CE69i`^9Aajy8oX(@hyiHzI0QUTnM%PGU3E7Q;Dp zMpaB3C(=rse@Ok+4t<;G1hm{=wqIV}3*C4H_e?bK+Qi)#6ZbuudosJEFjq|8a1E#! zkKNfbz)9s6^sNjsG(t9TF}@X)y%a+>?^LUIQ-V}ylT%@yf*3@f-z%m^{-v8f)|X=! zwGsd6_qFuwC&ATL@rM)>d}K{qDXdNB93<=>znpXHEhFX3kk^REou}vPX9c62}=G^%o18B2Ef7X40n4YpCsep0AadD19MCZWq zElWPiXQHZ1@*Ft=2CZGzy~K4_{cJO(!Q~uE?qR{ig*Po&h0c_6Qy0|vavgX=$pgf< zo}%HGMGaybo`g}PfA&}ly((9|bUYyLzK76MxLnkP5iWIGVy*l$* zmm(uRuTON=(GQ3Ld(^9Ds9%WV&t%UI>3G3ie&R#%!u?b5NmbJ#mtYsHzP`nJQhIS* zbxgoZfO+4qV!!P*f-?p1r^RlMC>YHQZLz@wzsq!?cqytDyn^M^2%oTM$A`6qZV}iJ zMny32!K*oU4N$Fi*G?G(dUodsF)l2UTE$7x{yl>Xt^KOeH825T^qVGudx)w=zL9Zt zkQr0Ot;``C(F_EHf;G)v_!u8zVMhqCuT=oP7VS7GSZRii5PIA+||S$1Nj?ta6cln^@rj*pJ00cQa{=yeuYLnY<$8@5wXGajOU~bv<$&A zI}P|}4-{N(04L`q z&2wvA%|3@fan;*zQ1wJV&yR7K5Ream5BJ-kmuBy;eRQi$ol~!wU*F@EwXJgWxBa>q z=^dx|1=lN333LKb~2p9$k%hnL>P#*O&+ zmY7&QOHyj;IYvDagB%Lj^poqDi@)N;v6R#ptn9lK`Zt&IBBwgkC_c+CJZTV2j0Okd zLIYUH_`q26+e^og(2l`3M(92njt!Sa^%F3cul5*l$67H{)omo{a4@&JLg=kc2j4%UUR?H$v)ni7J~oAh2};+I)LB zpJyTI99byA&z;l>vqc=agR*V3?8;1@OlRQ+TY80#w3~Mjx2+}r}q8SSQP)3v=5KcGH?cXe#e+#mXRRGbPb&1 zwes$*A|w6&{6sRm>**-sGIu+p`xX=DcrOpQFL_6n`__%`w#yH)myH0Z++n8O?1-d+ z-UkUN&;y+#tSS3KcPIcS#JY>pQj^R8Ozs5!Rj`v(eVLq=(PU^Z)FJd42gL98^g~)$ z@|YU4>OuiJg`>ZEgJAGp2+npP;-dzfsR=)pisY$|9f%wbDtPtLTcT_fN(7Ki8g+FP zPQ!2>xx!3MHr13mr*PdIsVRR7xP1CIOGsf6X=6O;GV4-qWp={-@XeX!Cg%N6`IVHe zL~PHJHz|Te%i2~x1gi81rzY_T(Q9a2VgNi`m(}g-5{MdWU2&U>WqJh3#!r#ou4P)W z>D)GogIdz1Ty#%#>=Sq~SODo^8Y_^;FVyF-hF)YS-gHE(PA4$ zUuibqIW@TyfZ{X@t!hkFW81atcm9S9NsPR7-|V3fP`0=L^g|Dgw<8TZpQqtMYgG0Pt!SUB;?XCxP|)2D~`p? zmpRo?dqO!SczJRbaGdm)zPGQ~1RGRa5r2O`b61v%iU(+1hhO(nouR+7fwS=Rj!>ji ziX;4~9h_Gse7mXn#4gW)MIl=$ot#_|;DB_=6qz@uRap9nn8xwJeN%ewOw_P8!rgv+ z+#ZVopF7{Bxqbj{BT*c#sJuquNhwe3Kmwgm-G4y$@q%d;Fm+6uJA2)gI#52>mbVIF zwZTw$+7nW-IlgDR!n^O0yGAH@d(|4hI~~JCe8fR&|4N|H&|Ph>DEqsSbW^Z$=IP24` zB|>&W(Ou|(8Jmc2^=}*v9!-9*=i#&-w%^KidNjB0oolkW@RBb+gW~+!7dZ*z2|7{B zpv4{ieBXOlN-?n~Ded#&b_KuVwVP}pTAze_?l3g{*h9d({O9!(#5VMvOSMTxewT0G zu_+J%UfXnhfPUgqkW!G#vmlB(%IyKG)``?ZbIz4yLS!Sc(QjxC+^~ukjZibu9 z!2N8mzKZzrDN=}^bn3Dz??kw}&!$sw=uE9C`8#JvT4jr6px?oE+{3ApE9tX>X==`; zhVMtb?u)!oOp-mkgM*DC-7SnU>FQ18X5UX8cOf> z5DI>C#8Dw;b*gG3PKs}ko(?iB{VMOt);cYVS`;5w5f{6AiN(jBq}t+3i%@ivkMSbt zaUR& zmvW17DAswH(np~dHGRt)7dQsZ)@4QeD{Zn7U*v*k17djjU;OnAm)c$R7F2zQtkqU< zZS)%6DG{sBcL`?SnT`Qx1R_DrH+OCmvh+aBCrayIp(ffgB=|g^ppF9Woj zoO99s@ZTnw$I~fFfDC5GCuJOQw@Is$a_+&1`7zeJM_p3$>m{KL{&Nm7ZW`gpH>>D~ z#y=A0GjqzE``i*+ zNWC@^-UhFY_^D_+!WC(aWZg$37~(UA(UQl|0b001bxX?33+;mF?wH>d@g1<<+j1MK z@fV1-cnGJHNVJ#ONu zd;k8b{F4*2O*fp6OVr%P5rBxjkZx3bqy6qpvEbu9u)uq#w9A&eE$#0Wxd(i5PgYT{ zc~JCZWzwgvVnJq5WK_QPYHm(3s9#`4kK#OW25W=wEDJAu6H+UO<7hFbpQCF;$YxE9 z0qzGF*=k1F@Hqhr8%1B0iB!)o6TUSZS`pa@fNVGmmSucp0;B4$Ofd zY8w1nfQl_A#T?Mobl&{)Eedh584{1gozur6ztW(f6dfNQebgxt-+#_mjK2boYv=ms z>se}QchP-C&6<@(d*GhowK1@K&ODKaZ}D~M&CpiTthj9Q1|T0SY|Tkn7f3kc zLwrL=oi^XJPN4K!>p9~{rSVok1k(w(k-af_Awg{-n6DyQJZjz27^y|$k#*27<ri(HiJ)e=i&EC?Ol7G zeYJ!)zq3%Ic^34^?7i6r`vOefq+cf3Rm(BgrTXsZ(_1)bf7(xa4p-i}R_#`_W|2}4 zvlYztdJ2B@Fuz9q37gOB_W|c^8W3G~XVGpO(U<0134RI{^BDSv1a^U)Vd`)qW~=Vm z@E06kK)RpWSI|Bsn!{`x*~8VaawgygOW++6Sl1O8IcgViLFp% zgSIQM-*TnlhU|c9hu`-i?C8+&zw%6OEYMIzyEO z$k1;QFtui)VcA2*))91~o^2{@g{`=-3@yfJj%89XrcyK-QVyzSMCJgdb51Ti5-uFA z4Qa&aWsY($+b{JwKR^j8kV1ba&41u4IpK+79~*lKr^drtBK*<;!%y?OR$KrT$t{ep zGYu-9Cz(wZyLf27-CG4_w@V3G4%as@W+V8?a7~+xV_6%(k}J#PQ@Wf-ZrojcKXnN_ z;00zxN?IN;(O6ysLDN!1bsw??Ej!iUi#}6v2-9SQD)$-7VndHbwL0nE&gp$M6XPL` zb4yBX&=DE2~%SN)a^5}bQIPF#(`yox%#Ep!M zf#>v66nSc9$1o{YFmT%j>N@tLi_NYd&TY)Qq06@;TjS+&dZs594o8ko%K!;L_P;3d zwQ%(=P(t7WlZ_LUuO&mb%?jRQ$K>>pqw@OPB(uyPCg5ueTAMzk4)OaQ#aH#BH?sDx zpvs8jW`R2aphqGYwk6D3=Q4B>87{p`z;|aw}1Q( zzKq}FE9+Za2d5l-0wWF~>%7+n#elvUvX%FHpIcg`+k@(!C)$ z0ogw;`1vJ@)^#x(UXxB5(vO?wlgBq9w@8F`O+7ZE2+zPVq|7|jABQ>D6|CJcgQ*Yt z_r-o-HTBam!Z4xGh|!sL)iLpb;wcJRqM)002EU#F%soDtYmcn51a5t4PyX`>Q1T;i zPc@Omxk4!TIVRVTx8i3AP&U-P3HB=fESoyylZNv|&^ra{L`!FUib4^~!NB3xz%cMm znU8ZkI3Uz@a)vHX!g8LmjcUS0v%4^@m>^brP1WE(Qe+1aIpB2f50*OmnUmUQcq65Z zA>b4Hx7%x_ZeuclW*8BQ8F4=yd(bLAq0@UBExF~#{58IVB$jgE%@PLJvET-F(42Fs~ zFp0BNlD{uDKnNgrv~({MbM=0JPvRp|Qk!D{Q!Z?Z1tY*5z$IjxpI3XF?ufS&%J5BQ z$F08`F`mW&+j^$%eaLAQQ-fF4H7p}j*&BIHtN^cHJ!EtuAsRsS1tJB>K<*tc;`98z z>!7x7Rq;6rK_gUK)Qh$?l+S&BvPwDthzGGw)j?KvAj&@vu+@`7tZ|y|Ar(Wm@spt_ ziv#oM*F!txXX1S^!2_(2B?#Z_!L-`HOa9<=ICcwgyJ9cbx-_OrV-ES;0Wt~ZC*dR%}ajoh< zCM#`mG!J#QOb7jg5^$87GskCi5GVM`L($ueCMf)$aOm&cw2*L2B|Z4Yy#=z_Ht$dj zDtArr-T}!WQ{^_yMPp<0kTJ!+uWvt>Rn%{KYL*+|d&vrua}$nYZJN(Dzt|`q$v@yj z3E~{$kq*np&y^j4Ef4dQ^XMmlHTgq+lzYEYf^f15UFS7s)5^TA*|JLn;D4ut&O8Wd zfZo+_n@9YtXA1C}_%2(j=I-;itt$0rc#cBvNX321S46RLVpe3?ZAtl|y*PhIkD!<1YXI@yZi#c%Cp*ocZbE;`T=rRKU~BE$9|M#X7HJpdgt zb5Zl$2|BwkTRHcbj`}`~Dix@uwoW{R?Zr&wpV3*$+3d?Yk8gKgonD`ToWh-ujB`pb zvdA7kE>vpK-p@rV8;rPpmBDCs{6l|VOLPQbILC1aBSEbl5a%EU>EOYuc_B4uLg|4s z65AzpB#`T-A+a#i-6B)%2qO43%l!RZm28ar)Xp>Iccg~Eg!zRb@I>p-lSkO&T7yai zt-~1y%1C(kc%#R^nz_sMU~I25Sb?ulPC|W?0BaOn-o$1w&W{GuYH_R8*A(6QgCmbG zU{DAdWY^gf?_@uUA_OBu5`@uVzFi`np?2LQbSkB(B!! z#W}r-FN^`L;hVgjSWnVHA?+DXiRgNUXSgVuUkz#JhS9O7q5<|<&yiqYxpgYC*KRv5 zCtfUE*VeI^0Tv-**b2C(;&wHVEpzGd{BLZmDYn_cOXi}C`NXdp>^mf|3P(Spg^tDa zbh0on25S1A1(`*8<=WmaE?OhJxZan$zVim;pXaxhv#kfgA^>g9CRc7R>Bv%?W%J8g zLi7z3p;>}SXC|UKPxc@;<>twX?mD}53ZKJ{1p<`BaQEBgqYAc;vlR)$1}plFGIaho zb$xaoFM8H=Ve#v%!?uu=K`+&K+V+PdAdoiF_44hWl0rGSq~OopnIPj9`ddgi&!eb& zi_{K}E>C$G>rEnHqRO7LcCLhQ%NvlOHLe;i3qmazoe-|#SELff_NAIbo*l))z2n%APBJeiH*Nck(W1cyN~W9kSMPyA;8~hj zuvur}ojrWtiaUf(cqBBDu$_`7Xo%_)C+ZI{O_!uoW{N_FomAQYkpP2!_Zu?hlx3LK zj*rRl;ao1559eW8{u_v1(kLkZG6s0JvUkb=l1X(=L*y6!@lRH$HoB~&0KzsP�%% z0F!!n%fK;G&2fNIuaKLS@XOpX$XOR{6Chd@^Ls44rWci?w%YyE@Cpx+0w96QZQ+|& zEVP>`=UQg4`{Hvjp4wpgr7`=D9yF1W-?~I-8uXLJs>aC;=?WF9e!q6R1UNj97I|C~ zCvY5v@laW2QZ$OP*D|pz+?5~D+rhnU+y=&2pZpta8bPZ zpfb>L^Lq+W$nP`ma>K8}v17i&4wbYH)`w*j}t7xyNDEK!taH{%+JrGB_0(zjiTu7yC#~rJ@O~ zJdpj*WKgrlqjkauro}$d{Jvk!^)d3SjiE`$;F+@3coLgQw6vAGRU0D?0;f%{_l&Le zs2OjT;d9BZkbN~i^ZPH|<5-xwv2mvGi#d7V?xuW9eH-LAOY;#TI4v?zbwH?sOb*!t zNN+)hCkzOH$yaNKPoG^|GwEgAg9+SL2Ut6S_DeswUBUKe<|e2t=A^B+5wc8%(>K*X;LBee|aH} zwnc@nR3Ft(2`s9w??+!Kcair%k-uFw&s*6eufwqS6g71^L&j$s8(+g(*T_68^&T>f zeuPf<1lw=L7P+SS+jJe*!q?>zn?S=#qG`)Ea24c@x`F<9=B;8S1vPA+%&}Na&AAN^ z>ZKI?ZR10SSyUj$NH_D!gUNQ?(Un&dof81zFPgf5!|K8e_gDKgV16{Y}aBfSKg9X4KF8^98D$ zMkIB#qD&vX3DXd<^&6lUL6#{%dpO5rm zWSjr~j;WcG_}*lSNt}Bf$AN9H1SEOy#+!{oQ)ok2R@fPX9eQ7t^t?tIZ{Znk|pJFUMPx&8)>1 zLv7bOThJnJvScO)zw8CxglI7{)$#T^30gIbl7{!OA5@bF|GLxkeK;fK)U=T1us0ii z-SD{61xo*xh;(Rz4a~=~XfaMV!q~{o(r)DWoCn=0wv2=XYFmL0WG~WP?7ZnUUEKI9 zf|2jZUe;+v+CWvmqE|I2Qe=cg|5^MK;l|*6J_`&#TG{kspWnp(trPE3lJAA_(M>FE z+q|qR=~et9n<>SptjnM&0!7hN1#&s4q(VqjEM$p)zuhnEl{`7Jyunk!`|^~pJwjXo zhAz~u_@cW5=kLs8V^Hu6dXIpira5*=J(iYx2J#a4mz2`=KuJH^ng7`V_+GPuGhV_4UTFWNB&7XF zYEZ)6Nq+Y@MaA8msa?HLD^iRmB1{QWKtMZSK%6uU2b|7MDAY(;AGqsi5Rt z-fEWtZ|Wq3^@L)50lwzgzEIT0VQ!B9ZtC}u^wlkqegH#2yuXhZSK!7*$i7i*4q)Bm z^v?=NYDb?h`Uxe^oFl^Ea|cbPOg{g!xc&v!^lH zdxmdAjlcFp<)X&+7Fjh6LD->)$y9Fv4*_#M(oD9kYIi^_FP!1eadk}Yo=J6k^|l6}TK3n)~EGqJ07PLb0YeZWW& z8nfzFDZWA~^S_@VuEMi}K0`j)Ck$Hlr3MeJ2&aw8Zy!N@Gk&MRxM!M@;Y;7lCAVzq z%aolSlu4R*QN=p>kIe-rjvzAGCzn$q^n}s3UIyWtM=yKK&V!qt!6sM6@G8Iq zpPxp-Tb7aEC;f7MfqmEBc|VH{lAMCtbQ-D^299 zV8qq@c1Lhw$dA=QgaD@1#zGsuJzqL}0P&O_`ke>%OCR&SR;^cW_HT^qev1O7XpL)W z?}_s-+%5zrN^_uacr#-ach{BSZV;|TpkjA+7HBITaEs{XmJ4uQAJblLala)^3iE8b zq)j+n4y^8yq=8>yoEc)7vS{t_&X!1_J4;vc(uGg#JK9#>Iu7%#+X{4D#9 zU6=^X9T4E`BPBc+7b9Jw))W3iTI*6jMSSg@KvRur(=d6VExRcmWH9=q2tyVA&bqKZ zfQk-A&>OPRLhb&RIgu>-2}{mMd=%Qu=K{nrpn4_#?1RHe8T1@?ZSfJJjM1^}figS= z&%TOZ>PO;&wie|ocJ?#pX`{@peTL}QeZYVac%;f0PkW(9qih_IY%k`FMybu{NO4i! z{K^-IyG8S6VMyZ{e%{mbo^X0!_?Tdx%7_tKYIlhA5X|57`iP|+(CPGXist+1x)^YT z-i%Gc+p5 zNMjN!digfn`s)9UxL656y5#% z^xj=dh>CJF8#-J`E`j`cfqR>3ImOdqCZXnxd;*f)^SWfd86Slw3%Ts2~3G8Zj!R10?)1-JcN zn3`gN_v?$)Gi8 zecuIHyUtLeqQ-}D-dU4MqNt=Pc4?A17=t>QY2ESk*~Hi10u*fdtO*oUufs4bu8DAo zaoPvdBrH#*yPTGxvb1srlC{+_Jh-BLhD>0$fSTQ?P;DCY5wDL{jc2sc#e*Uu`b$;8 z2wXhu5R-6O8Ax8(RSxl4eJ{;~+KQ%^;|+pmM#@!yy?O$M7%|t4+@& zqNY()F+Hotcn_m$QO(Gy_xG3k3fkw6l2NyJMgo9yBdC8_bw0s&Hms1lH@Uq-(okgD zXXvMA_*((s{Pc_gjFDQcFLP-T^!FoOjd=>U&R(<}wzO*}s7o*8M0A>FFv+xi?q?CY z!F^zDZ!Uwo3?0$${mzkw{d8_deQ$nWU-y1zeXRxti!_0dpvM;?=NzzY+F>XPqj?0& z`h7FmDL7r<0?!D~+Wd{Uk=zDe>m_3)8NO5)TGHfP8qKaPNS6qdX9=`ds}0GGMTj}4H9=_d1KoIw#-_Rb9ipmM&-L5%ijFN6X70mS)@@NHZIhdiv z4Y`8I)vp*OQ1gUE;Zw{5WH%WxuqOU>vA08FZ;lL&1+MCBCh7Y_D?RbX6e%k{^5X(7 z++9N6#_du9o_d*m-wj-@a-UDVii7eB7Bs9L`QtDUP9#EURr%szE8v_o+o(-M4`Ac1 zi51IOc$Pw(Zb{8(--cWV&U77st!J|a7+eBY;~~8;R_Bkv0p9#&3|gs5qFJyv1f4O< zOgwY1Wc1q?{hDZuK#hXW2GZ4pDcwOo8l&zLd>=&APnOc&N2Cp`RN4-^A2;BM&fMAg zx$*WVL>J%E8F8QS3cF!0ZhRkZr_Eh&lG}0&fg%Nf>zB6FAPL^~eNxeo)2sWk&UeuS zFMceCMBCYn1O(=r(9FP`5URkSFrt1`GlD--MxKZ`VIcdmKaV1ZjulTr<|xnLwiu4F z$$vnR7>^9LcD~&yPa-b2>)t_(=R1f+!#ZB6j1Wy=-?W-9{jy{VOj%-N`R;sq$qTll*Hk4 z3xzhd>YMRS-<~LhFR@II(yhB!`tHYLAof|sL|m|GaC%LG;7POS5B3Avj z-T08EQTB(5EGO;g!`P(GUXDSyy@LwHk(gaoe>+=HQOLo`K}>2KLny#(cS%dz$^w06 zWS}J{3%t1NG>pX4r}TRoVjZ>ou*UYVv%C*_{}zhvd*G27^^uzP>-8;x9i>Q`9 z#?K9pisHd6fG{JzAKPU$VI5va{SL&dkHfCuo%nuEyt7U4WN0eONYXV@X=&=3t5Fhj zO`Cv~C{eo8lbo3*XcAKQSBHyn4A7B~W7IX(Gk$Dgw4 zzuTS!N94>J44)zrTS1P1NA${3k!suthY6bUc5Qt05?n%6xUY?MM_C-{p>@*j7kOJ2 zV~wn?V0i7)Ef}o2FyiOk^AurFA21~Z=E0|?q02PIrGdT*@nXmtSoB9;;ntSI5e*iM z5Z^F`%i{z}MKQY~X44UHTfq?XGJwMksclN^(!H-0(j8v=qkh9=>9GLaCkwgP@z>`Q zt+>g~4Jj}>%~C1`Jz8Vz=_{S!M$YonMdP)aQ)wb%mJZU1ApmX6pG&k+r?U0?(W3m2 z*+=rtj0vY_>V@z?HDwZAq~iKsqs@E(iUts+{D~U(Bd=OSSjy7Q}tI_s@)BR`Ii-erA z!F1m~4H#zn0;233IQ$-S`D$ahTUe8jUsM#|1}PW7mK;v!L1a^WA_Nz#OKq$9Qt|zk$pEf%&{~pdet^o13~Nh{&Kgy-q>mca?Z$Kf1k=s3IbtMb zJrB3Lzl8|S`pC`{6ZLIi)A!{EbaZT-5`mfL9!6z{QB3Q|CBuR|J&V61%=!E8 z$TYZa^iB8ZnS!=&TJtvEI1JaGBfO#Gh2lA@?70oGoS6}CoibAIDS)mk#FxNwkNGRh>LK!Hf(q)K`Lju} zFJr-;wS-#MxdjiZN5@zVWOYBlAXRw{g2j<^^BO4aEvZ9{lPzr&$Hjkb{bqcw+_{@e zE^V${XhTiFi9d`j(7bHA-U;Q_SWJT#uRSnLul(G53~3E2D^{|ijw=zJekUK{H~eIb zqoRus=V~y1mL5;J+U;X~LE@rHha%<|rGDh@B&_qrq#uB)%bNe3=;wy3h>~UNCGM*T zgS!j*R~gr)!uK{hpgk6E4OBl=W?o$6ev8Nn&<*lyd?@w0+MxOOQ{x%pBPGw|r1B0n zm)kB|{$kGPxWIDf?O~pdz*Y{|p9DlO09}-VxF5PH5#i$W_&qQ&iM=&#|#xR5mTpC88ppOFr^}P06eZ}LU6(e zN#ATT6^R<@oZ#3ci_1^BieK+7x#g{$tp z@ps;Z_jOYZ$`m8UwwgOAtR%)TBoaFNR5hISV6rlhdlhC9ZG%l^d<5HNSn8Jm@hwJm z`2nXd3F2)-Us{PtGo@0XeEPqJm%la_LMev2;3xC$VV4NH5X`2gCOG=s7@2AY! zzAZxK`0ZSTqZ1A-hs1r{>(}%RFHuy!wXUb0#!)Yi8^QOz+fS>dKIA#s{vuUx5WY_s z_~uV5LQEKBZ!+8_zTQh{ zAN0$2{QMa7AhWaTGRtCxigEFx)J{V466&a?wO0|ARi9pNlG*+Y`Hj&;w)!;ZPVgA6 zjvgmvzA|`JzMUT&-EbKaebu-N+PTNFW$}VCb90eYNDwQ)ByVecBbq;k)$L@Kl1A2l2rjDj_t!S@L*9FG!1;+H3!Wj#}v}zV{^V<)Au3KP_|NdZW8@%=jNTLbqku0 zzkNCK%;xj6K*CQoB(Z1(E`?Nob( zrgVE9orlxn4u)g*k1Ij6q{Ds;3F>iN6u$qUJ@E!>{ib4BfK^ z%E=*c(4Bc(mYw`wqE%#uIu^WRMY0fFhyvG3ZJp~tfG8H%1i-SiYG*bt)GY^-HM<(+8X3TH|*N2=D`@q@D= zyv%$gb28m9O$#`cf8Tohg~BA1>r**ZIQ znQr9cWVl_9=(Z-FSJGJg(*~%>M#bjXBX|agJ6N&MRnZ&X^OguqnK1h7!T4P| zDr?2yRJ-xQVMqU=*>-c#!sc|JLE#R>?T;;pzB% zRN2f}`Y%1A8MekJzRNQIkfkj05je2 z4Wxc}qFs!kF^-4za7L^T$GoU1rOD><(wXO@;P6m2d_>5)n!)H(G$VL!T{5Fugv-0m-pQ)$#PXGR;s6(OS?p zD|D~z#U@3#=FWH%Fs8_#hL!96j#-|=!L)#P`r#CXi+4eEAX-I!fRxV7IvgF$j&cF^ ze0Wpvd?sZuS%k}D0`rBcP(&YsM~E1yo71yE&RHPKEgW>D^N`w%`2N-u`gm82?_Y_M z{Tltou$w!Py;QLk+^ale<2{4Z3hCa+5|8scta}-UzuyYW?M)G1ALC!fQ+b-Ee-Jp$ zv!!Vpv@`~tUg_Z6^L5ihk48Kce4J%nTCsai*}U81Q?Lx6ykO>q zBspgV+x^0{$v)orxXyGi#Phhge@T)5?F4SQ2wX2dA5x!5dnfQhy8a%p(#vdw=U$ac zT}tD_c5w>^7eT<$R>e^)REt~}|MuMXGy|2USMJYZ3zdSyx541mJ=VcAPhps={eG1- zv6ArjHCPg8OE;Zg_jXk?#F>e*zun*c1Tew0GP-e-;L?1Hqog{Z?m0SH^0>B#0pH4R z8$b;$8hE^sgn06?}CX3~5h`O{3-42!C zGur+2@y#LpL#zbRoq#me-0;FW#?V|ibY;nM}t=x|C;ErGI^xsVluzBkj z%`fs-ri4fIDpxq30^D)`F^hQzLlgv|*&MPjnA1XAvUD)4`g)15RLPXhc98ML$Thqn zYhJYRW;k_aV0QIEuo2o8tE25v!j+*NHHaQ|e748tDE- zRNG{LYFrRR6lIXW<3}gMxGd?A`>YqI>7>(UK91Ifd~%7feGT~h!|wdY)O~- z$qu|$6YwMKU8lZ{xQrr>pKt;_0pZ`qnR;3e)EUxe$;$ycToSmCZ>fy6~|I2$`}SrNb?IfC198p4t*%rBp(WR>Cx!T$71m68Mu@x z^;gS{iDs^Oo5`dMtQ*fo)j__6Pk?~#JXAClCY{DksRf_Ur~L38Z+W&tmh8tTsqggB zv`o{#cF<@RSd_X0fTE9s+`9vgA zsm7;@QREV4;G@t^QNAyNl;QU~@=vIy4n-tR7FvPT!=c9s6Q;5Pzzf5x2(B*3 zCF}p|RlCnt;FPy9WR{F+`ST|rC?J#5`nZSo0prvo=|?jAUJ^rY1)vV}vs-{Y1)JOt z{?!yV#-;A>WjoN>7og+LFLy+4p>uRxkqQNKajd_p;0GkPAwcL4h;qnUu5~i9hrp^Q?TIM%XegZYTqpG%*;;pNb75uD%5=M(obQr6NGV#X~U%8Twoxamk{C7kW;M0URRe3eb}y<2128bJ0A-xQfX8xsq^p#3re88g4$4@PYJWF-tToI$s^^h50F z_=V>tL&WEtuY8F=CWxw8)hX4WD6NUPO^`*P_{UthIqyK8`VWT zi5Kz{_XEpQ1TH{+?K5$-+Zq^M0{#tc-fF9MUF1e+mdI4>_6E$g}t1TaTaqUKiGp7s(J!i z-&X4$J$a2^(hu~XPop@Dg;%cUl#ir%C~ms}P+2uE88CqY1^~pb)QfakK?_wy962WJ zLnXy)XGNz2{3p+7J|`2c65fWz2>I<|_>U#Km*}8}P6Q$b$nUC{X;R|3iOoHrVjd(d zh_H`oD3K_X7Rl#xeUselWN8_D!kU4uZ2ew@Z=Bb%NUqFLKJ0Ygl1vYz#yWYt#3A3y zcs-PG-$F3QU3No^_p3u}s+|6|)H?)um7#&ir@;c!=}Er>V{!59hNd z=xI;>B1QfZuQ6 zi~bmDc@p}S%}@BT`Tz# z;(N8r+f=LekefQea7TJpnH9T7uXvYO02Gt#baYy0l7#hI$f$gyt^9ZbYKDwZ$CLnR zuo4WhDUnA0$AJ(|N^K#BI*vpAeasLn%EC_LaQ5QfA#CstOrxyDfI4zW%t)J%oG-2Y z{aR@ig0HdpTY8r*`wVrx;1i}mtdf0*gmB4zi1A~4w3jIfY&oUV(USSTF~Gmk)j^4pNvT#4!&IfpA{jZ}q&%p7_m9_0GhkreqhPWJZ@ zz8tykZzMDu`L@nR2~2?EFR(m7x0b~;sM+hvy4}wQel5+T`5>EiYo~$1&8lc`fK!&w z9TU(>AYZ%LPiH&qB+LgrrN^+|hbXbinLMD)-%>AXm5f5lEs%hH2!8$^Un=OUMW-O; zl6*TsM>=m9Q(;rm$u6bxReZu}tR5Mu%7QF0?%-T|r(dro=U@SD8#nKUn*YzOeOg(i z&g}KdRsjAcvEV~M`r)2t zp$XPA&Nw^7O!lOmg=JyJk>3xSJE4?!JfF=pHAmt&&uPtfrTuA$?B?Y7g zs$TcJyv0I6Tv}&^CY!|KG5(!@U9XB=wbJ$7yx!0#-9A`t@l-g9x40Al7c+NK?VQQF zGO4jKaFA9!u23Vs2Wzw74CrgCpLV{1IpJ-yvHJT|S&_t6@zT`uF zO(bPiuNyy&l(7;3Uw24C$67w&ZNcJ?BL#8-6wU8w#d}o6FOcV6Xm+WeghO3gc zf*v*;Q}~p>IQ;GvBh6}EB}$yKjND=Kcc#7DQwqCXsK8pbQb*emfqeaD2K1{^>_enR zcad{%N=?fL;v4Oz@b|1DULORaNn`H6dLrB zLVQCD0YwrGN93NJeW2((1TnaBEv;x~H@kq2)^L&iz{ifT-9%0X5J-ag*TsK}fOMrt{g+$r;0=>SQ5)maHYOXhRogGMY zP|nAfd^^^0_6mPCo*vD1+~JAOQDFf2>Jz#zi%0N)>w-f@M*D@5S3ZkG2Sd5JxWma@ zI)nFCsz|mHE_1DEv16DDdBH72!5{VLCi-v)IU^`EHBbWvNUqHGcq`)K_#9`kjU*nP zJ~p0-EA(bg60qX^;!c2M=TL*X{N;D8cfuult>g;~kKMn-f82)Fs=K4K(4NBmYxw~6Cu#ypno&hBdGPl_q{ z9zCRgj|e1<_*}lA`qWg4RH(-2{RmD*I+f_8xE~8AJf#|53Mhhx9zTiBC@oWu9e}Wx zb}@%p)OhCGi`wJT*fhD5qe_$#3bCifaRL$?pfJW)mPWBOdQ5?GlVx4vZoCn$jWxew zg0q4_=A(2g8T+o#_t(&1a}e~4bdWuA{dHA(D)h-$1n2I|wV3*$=Zi?dCs=T$8y zqW&aydsIPJzolnp%FEXK&W=`hl3(t71WL4pSLMF%N6yPR8M#E~qC`Ng$IcYZNtmb9 zVDl5>;|WVx;n9%A3YEl(q)?e=r_$ZB++fKU2L)QV?|V9Q_LL3p3nF0W2g(ieB)wenZnr)rMk?cMz1{t=M3dtNT82|*--NOUuVdLOvFcC z6ZsI+hghdsx%P&t=WJH^7-VPI zdf$<*CeM|O*|B@_7H;--7*hK7K(+0`swT(~GSC($MgH4jo}9TVzPDNV#RW`o8;&zR zmZIv4A&f8Z@+IDcA+1x)9kN6HHjUj9yBx z!5X?V2_Z2@0lL-aoteL?Fl@&{6LqV zerX*B%b-%%A5B(L-=(!6j_znRf(t<)?|PVRiEXsV>Ings5SS!BDfnYY=*qe;z5X3S zYaMlW)T_5W(ZPDbHzW2OD1Oq1%J4+KHzW%DuY)|eEaC;De2RQX_Z&nZZJiMYf5ch}hpCfBTuvAo49SOn6}lU-FaOxj4*`j(RyY+! zLa9Zc?+Rj}J}E{yMwM+!Yl5-<^B{ItBS8krutQ^3QR(d{Q4nmA15NZer>WBgyjs%4 z*lf=I)@UP}ssTiPogu)Z&LQ~YB~b)MCc=PC{~BmxvmpnuX$QgUg^1FFqLpD8#{ieE zvbbni5todx9b;gmPcF59jwuP=E161&K99?S4aM?Wc>2`U)edOXl_IQtyV}pPY?#rL z3wzsH5gp)=YCLtTHy9rZg&|vr*u_e_2pt`BA8yq6vW`h&tNfdWa<;LW$KvEy-6dGl zIfbM3-b|a$xFOwSGMaAnJy#P3mhg+nN3~~%57iJPIp3`Xk1rbod2lGod0&Ze-g;3F zuQT1IvjYgqB83l^&KVgA#LNkXAh-kSAsC9&TgT}=L*1tX>mK6$;TwL;1-;%0iR2`A zOg!17eebr%67m?5G()J51R;0i7V(oj^ajcMp4XIgpRDTA{p6Uw(%l{*gQ^R*@K>-Uh3aC;FPy^^A7d%? zMRc0Z-Y%1Hd~1)$k7(n~nBrup&+?m$5>x!%M7_WP|2wM>FMgAkzug3#JTv-IcG09M z;CwN@P*gq8DiWAE1Wssy2%J&n33%ib7drK^>slEt!`_b1cEOBokVkDMG?|=+b*fw4 zC{2@c*AfMO38?d!alX!*Rr>^K#lY`JBdTj}bt>Pr0a_rn?X)_Ke?#;#ijwpOLvwI2%V>hGbGgi{|dtn|aRKF0-2n8RN!-XM6qAl+C1 zVvVGxPv?>0`>19U$PJ?Q3dICCSWn3|kY`b)Q*pqA;y?(?ggl}I2sdXIqx6A<+Md<9 z0hG%A%sh)XNS7A)fMxf%ez+_Y+y_)jmuT3@#zz}PeHSUq7GrL*Gx^+U^8xtrk4eyV zwu4#U`*2(W>3VvGOaK++rPNr`#j8pk@6K_>>|Hreh18p|pyU$b+$kZilcRc!<1Hqs ztoXxD}W>9xLgi9yR_B z31DBtQ!kwT8rt`i~W#QL&^M$Wl+aWsHZqYtMo zg50;i_F+<+1~XULI4etKb^e9EAs;$|>;a`$D0_<2i%)rUf(0)GYVi;}np*{A2KRY0 zVGq3hw9n!$yOXBQY;MY&00lt$zreKAVfH3Xt(l`ylqJ#vBCh5XW;`%4zQ!)XCf-*o zH^;cwE|K!PGBTRo0rJr*zY@Nu^t(ZTAI>GZ1qy*i-%+NS5S2iRXZcqA}(j#5}!~3`Ov8=Nm9j>(zkf(iwFEuR0 zB3Z{d=lh9k$Ij?W$igJCeEjVt?4J?s~knjY0QmNdp& z_dkQaX4`JNHI$0ZT&0iT6FD{CUvq?=3GPU{d6-r{aY zbOEVQu$%t-CgKOvmy+e;ro2ECQ9Hc~zo1Qin7`*Wo6XO+dpAy2P+fbtC47%k)dP%o znI6f>O-N2c@aqi!g$-AWiTrXf32#q~M|5lnx#m9=RyDGk zdK0-c#7qTMDbd^SZrEsG-j}zGdn}x1a>7)fCQrA+oa6*p}pd(t_ zZU%aS12bypt94t?2o%c$Qduxfz@xQf;-ZCxaX*&ZB619=`NyL3#5SAK7wj%|Z?Q*j z<;+9|BFc1Xu+Z%7{)+7c@j?lV@VB-U8*wK6rcn;KiycQeqy4Qn@&cw_J6^k55_kj! zcxB@MTF95IUNyd! zvZ>1sH?};mNXWo!q=yN9v+M-_P@gJnz>(m|NOZ2{F=YtsG6rM(!76~dRUh1x0HU-&kqxKNiv+u0U-Mv z-~~R$JIcTW@_dg`7%#9l5*KQn!3vEFfd z^c61G9{SpBGVlg5SVUEg$?#DyH@U^UXGk14$R{WG!Qx02*;D9tV5}rid{KLX?QKOK z0{r#qhwfh`l@y>8?OQ8)D(s&hV?fiM2Sc9EfSk{Nzob*({rz4K|FtGVU!F}evUb{; z<+PueA!v(V965C~>V@ZEQeNqJ` zJhTLkY1P4@!qIHj0G+!)ij!cuooSGDo3X=cezXR}nb~wzpSRzf`}Mo; z_`CjvLos-itl+RTv68fOicC=)R&H3>i>+R*d~&b8C95uBkS(Mjg+b$>Y{4<=C10=N zTVKYaCPe*Qt9OZ>nT75ww}PVG&SM0MW_PrO=~HuWSC3X1&o?9#HWIx$deXjx*5@^@ z6Q%~unI&1HGk@Ii*a|KF2tPerv?LyklZ(Pocj2TA2WJUi7VA`6!K;*5iGe{JK|dM` z_33Zos(%L?AdXk1_HAh)yyqF=-kEBW4;cj5ug4>`d(?c29Z`B;P;PN>8{OMzYH15Y z?$JAOJ5M~&LF#kKrxkk_!3_8P(~rMlrhgT)DaKyZUUOv;9Lgyfe%&=E#n4MlQ7v&gdE1KrP}wpnu4-} zq}w)L4RnhGeHrlx6?{Kl*z*eRtg*)U&c(1Q=TD~1Rig4cf(4;&Ot(_qn3}#cN-L|) z^EZDzVbP?}w9p%c?M_4gP|$oO1(P2o{?s-eTxn|FzSO58J&R5UlK0J{81Ewt zM6NEV^cyhj#bYJSs8KSr0|%nwvMdfk`@oU8dw0694y6hc)Pig|Cv>fQ<{-z&FxdG=81%kj7gp)&ot`tx{eri<;RnKtZ<^8+2}b zvxh%wie9(05Vc|7}k*CVYW&RB$fdsfj2!2Qn*8Tg&psRma(dDiH!O@(C&*QMjpFX8oR#5yOz?Z zv?d)r#V@0oRhiFon})L^a!xkr5SSwNkM>E}Ur>FAPX06irBeD64& zMEjuOJfnZ#U!nazXKZ%}&Mp&O!Xyr71325nf*jE3a^`8f#?u+&JVd0G0ys!bAW0+a zq}910qp60L7hEx?ql~1|N=yk^*|0*7&YfeD;MTxFz)Z1Y6hdx_q(53NQ!_5j49dms zT(U6{Pv3Z)@}-liQ5e@ToN6IE%N*S96WgaG*rlB>a|XKT-rtM3ET#@ zjn6F8%mItZWd2&>yatJ;IR2IoQqe*YTo#kF>;bBPe(X$B{lx5B#^3TAS2+02Fn33Q zmIF()10uH0#PM*D+7myK#_?^s1@cZ!K$J$e1oWT7nX*FCWUnG#Qbz#XQ%s-MFm|H| zDHXuPu3+SH8|~UAHMQEO3lHh!Ya!{RLCm}nXG!RY`sv#e7}c_Ok2uSRe*{Y-TY%Zy zp<+L`22b*t({-`o8xH)Sx5pi?PsVphObxa7M_^D&Tg2M|KdVWrZB>ZaU_?_tV$$qLpXuw@ zN@ifk|I(li^}zuV2tkHekNataj>Zn zVHHvz5L3M;wL}aix|uX_P0At>&Rxughe^e>&Otv27D{Y)Ek6(2a!MI-MP&`)61D+e zYUPk7zU3Q=64vOuI)xL4{CvWn->Mc-Vo_lIBngYYaJRLe<`?NF0B-Kbu2#Q8-sUes zAExhP3>92#|90!8E?!=D6RCCZd)1C}#ku}cdMSlt8d+*O8P z87&f1?Y9SXwC%813TnmCRp+29IBJPI>5#h%w%C~B;kV&r%J%OcD+8a&;yGU$#Cma) z&23tY)eNFR%pZPS}8j`1TUeE&|SKEFwzXY`4R$I-Um$ZE*m z9LwZj=ZF}c(b}u5Cduy_EVbYA{O*a$ic2CU_2}#`k2^9!w@LHh0>N3L&m=H!ie~_m zAllDumCTYq?vMHeX%A+x#goor*+Or`z~Xm1d+rQkoHLvXqn%dFw7`iL6m<8Rq(8jG zB+4#y-I?AxZxp17^!Ks_De#06{P-ztlz5@w>H4;-L)k^If?A^htDKEz5xv$^9E zv@s3VYFp+|snfXO?OG-bf9G#{iSaw784j#M(3@XnOq$<<@e&yf<-^Yw*wZmNCs8sXg9&g?6vUrM%#H0ro-j{E0ojOO<1t4%&H7b-Y(R`bjllw?BP|q=UqUG> z+(A%Mej|70-79yt1&fs30T$1MY#CRnyxo)pg<`}0U~@b# z;P6c^$&WMtq-}VC-X39_XXAv0(Cy@FLw4}pGt%7XT!`0VzG89eI#dd0_2S{bFN1Lx z$oz6Cy^4rUvbyR8ENX3AiBg-bp z*^V{!K0IaqR{cjhJ>KYesK?6<)-qL5NF{<(GzcnLOYrhs-Ppw1ae7FfAUN&-aRFaE zbrNvGu4~J{>~G3EcrP8wYGt21oo%i@q9vUPmU!iJ-S>9su?xn*9#}2{bTW-n-|Mt( zJxdm#`QR{K01mZ4_B%?|OYNc=;^ANhf-bp-(1>L&Ws5sir%RxFv%(4#Ib4?Gd6&X( zwUqg>8+*1)x)&E5fHrv#tQ233F%ThxUR1kORCDE`8|+E~$V9?m+-;Gs36)@pRy64w zTcworO8`AU!oL-G*CJ_O;9AE6Y*VtK62gJM$&~3>H?gXFT`SLQAxt~l1KvU>C0uiQ zzjM1Ee~|{u~Oq_k&CIUJ2#3B{p(x>S3!oOqn1uxwe83h@O zq{g(SsMqKjn;Ge(4h1xa#bB2EMCXIljY(ohR0`dlX$!h8VpumzJ(mD!3aPK5P-dWQ zo=-+ex;zE7B|movt;jb6iSy04AE^Ik`y8vfhdYE@0*Zaj%OE-K{Hm4hUj;WA3|k2J zMRRIrxcDo?Wb|t)a?<`5-3EF#>HOdl?X7N{!a&@BmP3j6Z*T`hcH^q?%n@z#e45B% z8pu8f?lQM7h2#AITxt9-S0@6{O#3sL($TEZ(art)UB9|3etKHne?ki=p<#3^Z;&KlLtM!NwNEtU)41|sEH%SBuq^vS%y5o#}&`C!Nhy`2%wz2r>xQMJ6SRTC&WMqyh)5n zFuBKTDboU`L0T~#PljfYD8m71m`PI48RV+pvHH#p5Y*~si{^##0YT+;GkEeLTU7E} z;v6&`=o5H&<7vlA{PhG9k@)IYzj8j&0q*nSRMAs<3@zpkmt%Ph-RSR)XcDa59!el% zW=P>C65ehMrBS1m(}ir4lDp|cPHg1cfBPr>taMmHosm5hT=*5rbq?1%=u{UAUd^?n zylm?AXfitSvj9WW1gvJ!MDOGZ>(!Trd0N+gG;{aw_Y(ik)*#Y0W_jhJWt_i$ZKy!lAH-x$>}-=H{?W(yjz*MHeLmn@~A{;F}|5)K!?Ynn-qL0wJ4ofxF6}I|r@J!xay0-p zT8l%cH=fUgK5-@4(COQ+t(<$*rk`#^TN})EoNEW|6~m107d9pO3Uj_H1CwEN0=6^s*#%`9elnhbo~r1mI4ghkBiJl#-EV`}^Bk{ZvZ#4vhHUu;{Dv3sgn zOcqFNlPBzNZ*nX~Kfzu1p>hpC!xaZL}C>8uKq;IkQRF9dtvv27MCq#+4;ht-tX zo6}G*d>N(p(&g0bUD%NyjV9w8tV|e;P z0!9tb7(^@TB|GbP%*h`s8aLiof0<%Dk#q0uy~o3KtV!_e5lJYn7j1sJ5f%332wIB3 z=Q5sc&^8vhgN&)^@makgBH~cHrd86FWMrIFF8}ga%Z}+uahjNyfHqS{n3aU|6(B$4 zc`Y?8MY67?;D**Z(%-)l;=zw%ly}w0UvhcImrRI35(eVHdn~TlJQ>b#&kqXEC>h1! zkOvFE6Qm{+?39H9$NJ%vjv%2=_ItC~fZcvA1QGfw?OXhu=VFg+ z3*e8HY7SYD12SvIP`XXCJ906gqK80=+XLlPBD|4fC;CBMq2(PFCeWY&DU~nYEUv#! zf%Fo)3bGHJC>bYub#$DtbFWkOo!PHPP0lmrBeX?&)4fSTIr)36O8PSt^%*$lli2&J z^kg=D=k^9E0qU;i`}ri(aMz67*DBz7wt(W>>o`a{oJtG_Y~6NmD&Y8coC9)6L{&e= zxG+)9;PAEVwZE+!p1gAI;_&r65wJyCLv*RIiU*+_vVwg`xB1anjN_p_wcB?Z&SYaq zyO!MIkXA$3!Ds2q7;VYtH~4B5*=I%0)x88ShFhT*kMJ#_%%H{-pda3b$FWm}qfG!# zdN%uZ^spyrapyZf(ywc0Ah!ud2HF0d1hJhHB#pt_ZtE#?2D{Ys;)hLbuG~vT8C5y3 zNV5$t(DAvUR|*?&BJ7x*F&F-}XOu`?NwcI*(z&s5ji@~-dUO4mco2rnm8m&9C3e|5 z^t>7|KirE?C>UmwUgteqcOD?k$1fXWDLknK5 zaw-y!5`PVlke8RQYzSoI=gT-kbPEd=ym*v1wS$Ad4Id@vgRq^CLEf3Ea3~VY8)5m~8r+UMhRkpZr_~Al(Xf7xLh6(TW z_~VA5CWOK`=|UFoa}NR7L*VWBoqcfhWh>FCo==U)Dxq&oo$1yOMR`dkO)EgfPEM4^ zQ%pKm=WWP~8tlF`6awRl?M6h-X{(zGC;p0 z5uPd~B=Xood%T;!qnHyA;{gRA;tcNxDBgYeILi96aw@e{%WxU(tI?u)$GX;5C%QWC z?ChsInfgf^)QZ>IXNkeka5m@TYKm3JCYU;j?50Ps{|LcTKOFdAjAjDex=d@iYCS>z zttI|QdQo@V7YF#Ga}^wm;E2&045|`+$#nRUy++DSItGva#gIVir{e8D znu|dD>^+ErIP0Kc`ucFbzudrEfy9IJ4M1c>wd;#c+tYOqK8S$shD?8JzOYKfQg7Bx zN`Z)7cxy*~hb^-~;{F+mcQId8$XcefaE$}^mH%;aBymGDqAII=Ze6pA!=7i}jk=5a zb$`oZ#Xf#CEaF6@v#Vbu88g7WYWb)UP2!d9bz}M2R2Xw05M=(25VsNR_LbnTH#4tn zGrvI&;JH)yGp~u4+7XNu#KJ`a0hn-SYji5C-o!=&njC^XpC@Rb1(x&s%UjX*ofKm{lcF&lqX0|xUR-qIPX&4|6)298 z@I=j+Qr02JnnQ~eDjaQqln?j8k0Hp7k5Wuu5TZ63*Xs1U-J7ABcSXxAgv2`az0NKN zba=h8gPh}=dW-gTu){ zSc0TOr5{$REhH~B-5LD&sGX~7VvwPNs}v^9wzZjnM4TN`>F@KEA--3nwYb@wJ@5Ra zt0Hdc6U2kVpRsTA9}#@k+#$iH`8&z(&iGb#GyK^FP=$1kL%?_z4r@aVH8DTQp?T$j zbRa<$Zx&?gvJlei(L~zkCM36e+ZQK@B-)0?3tqIKpHQX?2Bn{FkE86I7eNeJM1vp z0{krPF^fvl$udVfMH)(ShUM;vvf^g>EUU&bq*Lbu(!WUwp0rr_E`cQt zm>wT*pVY_ye!3cb3xka_QK=(3Yv3`L*_A7GE1puj_+c6)Z-ZxV(RqYPIp0;@O}Fp0&4Rnv6G+_6@nHDz6zj36eYLWUzvE2{qVIFbUt; z{v6rw6+Q9O>1=Y6KkCz$v;Bpd2^}M1x|w(>Z&FOL?|GncZdW<*<6nB-!SYN}K$En7 z$6OIo<>?IqGQUe&^s?ZuAJB+3T}C=~Veb5$V|qD5B$g@5pk`i50aik|5uTBR4u8Ll z&$_uUx>WB+hLio6jhwVUD*ps1>D-L}m}DcM z>GlE~vP5cD5RjGmPn+f1j{zbI^he~AAn^=-+-zD1O^c8|;5W!C^aqahcX+v(z$0+` z{he84;w$HGzu)KXfMhK0NQ=H(Li2s6N%QeogJP0vhgj;yB55&*7r(ESf9%|yA57&x zV3D_g1O&*h6!gCLOox%Onq_C>Xx=)Lydo@=gV+8 z`W9IPST_FBCKac9F$b>GObLJ;zv^yaKkl%Q!9QY++C;oVg7rurnAvR`{Tj8amaE0Q zmLt+@B_MMTaI!na?HF6jEzV}amf}kwI;}U9I6PU@(~VRNWxe}fsnzXb8 zQB|zMcFb_*y$q40<3~nwNyO-*lT6T_j7Q8OE}a6!rle_ztV_goXZ|&QM)=fi8Q-si znqZ^Gm!s+GRndvel6BOhC!01F!z`}aSe>1Bt1aQ~9WL#Zau!|N2cpgEc|D{lkkW&E zdW9oMp%$1c+uoJCJ5+=h~(?FbO?D(H1ZCX9$;pJTr8|ryJC~`UcW#PdnI_{ ziBGgjSm=o!wNYLh^|Q??5ghz43ra49UK6thI0obvMO@wlpAlIwVK61K>mU@Nd)$lP znzqQYsk~#aL!2DCyZFY5urED1NG2jES0=qGKkP?!T@^b1AwtlSr+Z@?Asl_b=v;` z5M-0DKST#>I=W+Y>~9x6rt{z;I5Q@p(M$Z%65rygHi~iQ3{WMWUVV zVs)f+NGxcr(U2{FpATj=RTPMB9C65?>YbqrXR$61R-Hs4bWKD0{=KV zkL6Z@D2RR#3vyZ#By!H|$Rg+X^$)*|U9NIT!_2+ieWYP1_Q${h=uDJ)m1^27xe`&J zu=5Cou6rQr)Q)>}GkAc|Sv_2I;W@?wE)sb*cgw=Q-@ge246oDibqVV{ZRvkT{GoE2 zlkHLhrZpx!he-V*M#`k5f75CN&3fTh;Me%T^Lba zP=NX(Ab83MMn*Wr^LFO3K?v#{B8kE(K`klY1nnFQuG?dh8GFg^THMihRM!vao`5-o zF}6y|uX1HaMhjrlE>M^NE8S}%o%zbzNhKv99}Z;eWd3#+G@m6FatVDqp+!$it{{$k zE~2MFV-p^X zK=AVHVShYUrZha2ZcXrHU+n|hVk-Kld@^;m$G}R@`$89)+@7b$XOMxZIbbVCc#CpM zzH`4$`m?6A_9#^a$Uo;EPbZvSw-y|H0?}^^iPEci8LH}}N^1tNHB2{9cPdQ%H#v6TBP zaM(b{;UuMs{ypJYSa=_Y#G65qffMER(?yGzi_C}q!Y%LQs^z--{bgC|GgzqM2t!`> zjsoTQ2bZ7L-LX`TKdA|~P2Fhsbk}MG14lve&L@%(31IU=Wi07HvPRI^kKZGo2AVBo=(Xrdy;`H)2mjC z?JKnB2DBdz*ZnT^n6RyW+!JC>ZtveDgk`&yf5euXJ069_2}@$v^q|`3aqB1GkQ0Sz zKVrtDVIGZJB8QvgMz*wh8#zATg7+XAuKm3&YpE0g9?{m!Pt`A(NB;7MV)`p-rqM%Y za^qMOo}Q9_*`Lz|XOhr|Y^)OS2MJej876`ru-JTB-vx@wMDYzeoe)x)fS`$!m@ABo zZ0*HiNR@|bu5)pIxfT9uR=zjt)McXx_KLWu|vODAtR;QX*>m{ z+(17w5uZ?dbUXCrLK+HTs_}}y(-?75eW>((r+7v(6H5faX`9~~1*4AUmt zJFz|8zioFqZuFod=?lYS$)CRu*bhbzFv9svQM%lpwGO7)?8jU`h(U_>a*{12z&Ch0 z?g6^IBhp!*;$E(8xqaqQ0&)g?adY6UV<#7G77V6jX+ILK-8pW3j=Oo;F%3y5+#Kpe z2jkmKRH|CYFaAm$r@KSfY@?&Y@4EL$JSe&aus^TKkyA?{z&tO~=hcEWQGch0+eVEq8?mY)O{0X>8kr z3T0^9ciMD@92^!JcUY288+DBMx=(EIJ& zoOhuo4t%L{(Qd26Q->$j`iqX-F<^`_@^~Nr$h2U%LI=W{&q-p5QM5Iu;f8pAFv3=P z?jLo$7mKaULyg{0hR;j>4y0WIMYt=s%%+>8SX1H91-an$v3;!}^KbwMN-s4`m$O1h z?&OnVJdYbgY3Nxlac;+0e-F>vjbGb0XzzMBobX35d_REiWNXb^3?}3-Pqu9Md0s1a zBYt*ETM`NzXMG2jB}(>^PYG_LkuY7*5W`RK4QgzpRxgzq36ax5x|>pM@}SAQrjSJ# z4=6=}3N@pgrm(`&t|t4tOsO2zP+|1*b6z?;r-S53+{is)>(`SqxtFe?mgzG{F(8yE^{6ueN1K+T%+T$7 zy~Vzqk=)Uef(Id~CK2%4tux}$g8}LQDZJ3Z_=-t!N23e9%Y1~eAKeFXC?p@2>ft+d`+W-l&j0WSz&;cX$K*f8DfifH zZ%;9L1f@fPQ?i4~7QO|yL%A6Vc{UzS!84SiX@p zq9Q`qPs(J-hT)yky8G_M^?RbZxRooKa+UT+!-q&1p#X%j4E!~~?dAcgm%>c22j$Hs zkTY>6u$$&?R0T+`k|PaO^(wudo!nLg5xn&6JKSKudTMCjm+oFumxh7CxGKtFqPVXj z8lwAHd~cCU7ygZ~R%3gf1eaNu7MAandC&$8T{iIg z{o*K{gy02BKBp*i)Xw3sT+Z^GL8?cGBhPxe9NR|m{fC0x3!<1mZX}E|%k)z@R3)~k zMgFaT;7+2xeX}9rvn74HG^=x*+l(NY?!WuY?mOsQcR9qhKNa%(d82B@4$f}2SvJ*Me_e3LDDp}vrmARKb$Jhi=UVPYPxHaEjl6kahkXhi zA+Q|^;@`pSR}^3e7@$o(1Ak*MdlL5+K_kdI1YyD70rv0paUdAe7ocAcUaMk&X9bJX zg(f#wHxStQK@%hEi+TbOd)&f^U%j->dU2D7e4^e~wAV44zJ69$3?nhDY=6F9O`kEr z#A@h~`zb4N1!un|`>i(0X4ebsTcUGe=DRO`+UmFbO|Ie?O0T1vf6pt9y7DZD3PJtcP)tVrt-~e`BaiASp#A#Z2Z5)?B0cBE^|n zpb#E{1?7(gV2pO+s+6NM;$r-suNZ`S{B9leLTs*RZ#^c((gBZF`N7%nyxuN5sbKuR z##_v9Qg2b{F6FFy3U_u)^BSXzA$W~c%RagVs33P1o{*_dWz9>ZJ_R$C#3Eo(8OAV|p1* z*`>q#@>dzBUzy(e!!)vUTif(Oa$F8Evu@J@p*?mA&D^N#+r^V%Mjs?cr+?U zfLeRLLB=mr>{=P)61oLcZn&v|K+En8^PWl{O?gv7nfM*PJ7X5A-kdm$T*R#nd>T?= zdt{vg&<3fOGWKeybqa{hq0qCI!@zXqw-`;D?X&fO4)EgP)&pcz`#rP+QV7yKp7I4%s_e*c_7Y#wsE+4ZZA9 zqY^=ZPihX#04@%Fbmi($KzaeTm2BPoiSEq=6I_hz0y?%H6b4e{NlN@X8<_;d6gRLU ze~Dywi${TB)NA&TUAkble|g+wJv}(&mI3z_GX5F0w*7>l4Tv zW#tzxiLD$5v4YYmH^hE=(Fo8G7z>DT-F7z&r6oBz9O+AclAVhln%5n`T?s@3j z^e}uHKvAYKVF=BWB=so;L3E-oROj2M08`dpzDJJSjxV!S;i~$yEr$zDZ;#(9raK#% z8R>M0W=;>gfr;S()cV{c5=8>%k17JGC)*m}AgWg9J6!&H z=lRja8)GVwWsq;pi@58?b{0Q=D`st@esq;uY)y&3lp-UGT)!lhZK_jfsNS2GWhk{0 z9KpHvyI&bQsHgO9$|-#vO?dEO0m}k$WfZ)wraVX>0Fa$%4s24U zzjS+vCq?Szu*dm#;%!7G+6q=DwZ+MDcK3SxA$Zi%Cr9C&`$L6wHA|*Jfs232BCCIw zICSid=|ysAz1M}HPLXlhhVyzp_%?=FlG2j{;7rakJnlZaK53xJYHn$<^Y#jCP9*uu z)sN{fzeKd#(MF+CLh!*gz83yrr4$ySd71XmX-n;E;%J9^D`o*|hYOcZZ*Lmu0FW6k z3c1HP&rM7#hbBK-_io@3tu~f%F)uV6QQBAYwm>?AAVi~kwwt`ep z5O54pbI~Fk;NFGiRDhE3=Dw7;uI)m_zW!ad&!SOnb1-OcKDpT9u>V=LcO9(Cl7vr_ z+hRSts>@j~{MnyA#kt^y;}>)w_n|6&6-|c8100BJHDA2;0LFqBj2oq`%VeJo{Q=-Z z%^QN1@PVw(UZ&uw_&SEz+kOH9UdSeMxNx_8`yZ8(h;(OoMWio=@a07%umv_g2}oN5 zJeyL8+yvcM6zXDf{Cysp3rh#WN90FE;jsc8efSAMa0UCBeLn0v3x;c-KdZ2qFtTo+ z)xB}IVxXUqV{ejUKG9L={MuGPA4`;y0$9V~nh^P9lt3IRqIL|hO{zE?I$;hz5-kS4 zwu@5)`5-VzG4*mZgxWmlp;W!$#?I-R=CJa^f}KudLCA7#vmsGm`fl}j8_8Sgc=&Gb zD3bhWHIg&0J|N(VO|RFD0T9Qa>R?{e8+yotX7+j{2@4Dbas|D>CL{7t^t{kp#c4s0 z5aGk;jkuwDlq~4*_1zd@7Pk~`p8zQ^)9kP{Au{8E-3aD@7t_IE9AF5ZNznAo>qO6r}J=KjfD zH~pSzd65O|KIIF_Sl2hFzhKwB_Q}(CtlCeX#k1kpU*gNfpPGK82DMG7v%HsI* zBn2K0k3~qw#<%FY9GCFAK>#8W4)$O@b6&N#&vuCi4wmFk3{4XNcvDyfi9nmTz8uaH zfgPKE1WXuT1)omMBXKHKLRjzA zztMb7-8Nh|@Vj}2C^KZO#t`bN47EoXBj8$F`xQu<_3p&R@ne zVt7v<<{re%>33~a!V4Uq(sK5kf29%_m9sBdPufv-8Pl5#DE?x$$KOKUq`L~?Au*Ta zLEbLl>&WAnOlz_jg10Q#u&nx{QTP$iy!rk=JRtSo*XJ1yAp^nH22N13%PgIU>&C5v zgz6Cpu`l%tuMAave&6g+09fo`cKE=u1|*R1OBm{9S&^+V!kqfwFg6$XiAsIgTx06 zkj(0MvErKLwam026sEVx?q`56l~^rL~WdG++C>4JEEJ!B5$i}$b_K3Bb(Q7joAIaB~a2i#A- zU=0xMhK7CDY1Cw}DBl2xzyrXO#mJ!`1eN76$5Q%GMhYiyo@An@@F+)MhL?b~hC{gS52g`O-{*mAs6o`JVpi=m@2WzHkI-%7)Zn0CT%+esoLY%HTd zvjF)*_P=0Ilix;dU_3mB3^Yd%*H{hGm zB=ds5a&%|(E_9D5E!b-_K+#(kb@FhRVf>db;e3q0g5hAD(A`|M-KozhrhL43QD^< zG2)L(KDPy9WK29v|EtG0cKOZhul>g+*(tZ$vVb)3S(dg^kyGHC4&c6?MZtm`aDpIj zKbgnXw_ea!J#(`NeY2K7VvE#&uQMOi*vul%;Daf4jxjl)wdjG!I0I zeu5X6d(_9iH0&infC^|BqAOz_q+q!Z_PXSXBJF-a?rHj-S)$~w^W=G_E2}V;$(YQg ze|bTHaqO$fNdvPIL4{3PG?gvU9|2LqIL0aPTT2*6Q}@p%3bfYu1VDc#NC4uu-$U(v zogGDR?-#OX&^*Le4O-!4qAwKq1=5V`XSqK9KIkVZGuD#ees?F`4Z+=b^V$u(KfN~Vm;rl=<;lSk)ItH0X$z-*CBX2ldft||^@l!&ZK>t#;qWu8%bEZ8?3}1sI zqgV|4cxFr;hQ*23FUIl2!W8w1;qi0epPkMnTUrNq0)rmSS$JqZgg?eFr;Y$92LS#+ z4Y(xW_ljgTmH%rVjvJ(aW`*F)% ze#SEH%M%oh2g%q84=od((b0-6FSVa(7d9wkl3xfg0_e_oEyuhlbJkI>P{9q7X}!8E z0D3-2ltk+xgp5+xqfye0^97f+bf+%h{AjHaiTi z7oyOKR-cmZ6-jMHJYNOokb*n7*~A2o;@^GCKh`kP6#`|DU>;d)t4EUwIBp@#1LE%y zxsn?MC)m!z7xDfWim;uPq^9BUSARE+euX(Aw!Rck9N zWVRxD&8U?D-{#R3y#nn0V`swyq};q;NJGdHWKW#|>7eMI0+0Oy$5t#28cK!N8 z@Jqc#=MBIr;<3UzRnc|ysBoXaN|<~WB9p?)-!LKkJ*6iif~R6SanIZBrm?dEfUP5` zOjOD0!_d4zqIvKmh?Alk`WWoRHT-?RGTks{4CVolJU?4m-bYas9g=>R5C`1F;3cUv z+nis6TNMN*sr*Ru2QDm&jz9(!5@0@3;A4zs`O+|3#)W2cY-R2I+ou(&uB6A}8tF)& zk>jzzt8Kgzn`pjm6Q1zJEKXcS;VF1$CJt!jx{<*mR_X~ioBF#L_i470TmEjyk@a-_ zf?%GO4W+voIprN1@gl_K25>a0X1|W%% z)p| z8Ky0v z#vUMvT*X-+)cpo7fYU_PIrrw)KMC;t;eDYKjav=r8^gE>_!s~QXW2uf#v+`xd_ouK z!Z~50^lf6%#~H7F!LPXGzqPgFtnrotT^pyGy;E8@17@pS)MDV;ocG!M!C{6Ux9z3j zz@Ay<4g!!H6^lXqB*r&iE=F0GXwRd5s$jcmyrBWbEtbl0kvBbH`Q+k<%+uP_tkXOO z)x)^%IS7--V`eCA_2nq~65paR6N+nJbi&_P7C8GNLP0nDI^uiz_p}9kGJsyplJ;BV<}bin{;s27Y@k@8aO8X3d}b(a z@j#+y_%e~mPj}?{FYbW`1fVkhl-B_+oU}WpjF&FzZTj*?T)UiH3bzD?n@aZYz+@ht zilU)+F9XDhGYSmIBq2*!lp?WqD2}Gx-%*wfwzl;BJxOdlxo3lg1P)SX5jPq zM9{E#8lKkHa$5aKK7po>8b=|5lfZC5fN#u7QURjZj-0_VuPTwBbMb4I1qqm(e*wzj z$ovls7DfB;SQ-n!)x3ByGQ;Lm2Ca*0*^e*PSOxu8&p4d1(9Z^C?H7R(^D%GhM+(2X zg~69kjLK>B8pvNOxTsiMxstq7ifqD|);hoipKCR-fK_mfJ1R1(ASCuUSl(btT4|Ue<4kMUgkJP$huamttaEv%8M>gV#h!bz5VoUfBHm&!8Ct={X-oyHn_7eQ8;8^>?0}@c5##p}=pR z@HA()e%1&o4s7uJOSLbs{jh=o6w3CO5C7o66w3*b+1e(827A_+6gEjK?R zA%x#uwCj}1d~`q{jpsR4bqs zkY}d9;)2qoqhW*$#fvhE?V?K{O-a-o3g#gJqDMsPGqDJ?b6`XE(W7OA7i!o$&3QE4 zJrI-R@bc<(D9I#R@V7#MJEn+r!c!3y%2vRuiBSMdIp*D>8zH(s#&@S=;fLqPcKz89JXhhU;lz0U4 zr$ay2>1V#U!nAyp7mj4UpQM;Z;3h zNRYliISBX;Xc(fl2|!^K2&f$F$;xY%R4}tda{;Oe{shC?F3H*q_8hhpJyG+lRoneo zt$3zh@V?2~@+8d?^&-#s2+@}iu~okagDvTk1>wpDRlIUmpCo00TN2lqwoHxfpGC0O zX_LW<0f20_J|HCB37KEl%#rQOmJDcvOy4sR^A(152XM#k^eljfUmEw&!V5}UIDDfY zGwqPNcCA}BE4)+V4VV+`%M#aCVV?ewpB=7?Ojs@zY4>3R*gq*lDgQ-3D9rCO9r?HzFlXqAgA}k#6r(c zE8+$H87YpK{Jjh+9EUHYOEm*m=uU9$WS2+545}^d(p8-Up|G012xT=I)EIFBAP`$9zc+Zszgd(2lPt&PdNu zRS_O(vi&k@0fwEW#GSWHNeMI%8^pkISDfC4FWpFVkQu<9%0-V`y*nZ^*&Y}V?(kfu)`=RthB(xVlHW8&^Rkbg5LnW2+ zItuAvr|4t?M7!6B4!_Hl`s4h1$pv166sBY+BI*h+lI%WKsgTS^0A_i3)j@;Ma^7kS zRj^VxY+Ct}$>?qQU66_EB$W-iev+eI1gD|D>qB^=K&J*7EKhBuC?;=GPrL-szJaI?a1mDn zn*OGti~XI?(b^lgFHxOe!Y;xuAy4Zf|83+2l1~Nb7uQc2+83wI&(x59(J(o5&)y9J z?oSh8zqFbCtyOAw&@?_=CqA5(wmPTJP0AValT2QZjOtHB*-1emKz9|3 zLgliLF$9F+<8?Ds9;@_F6mQS!F}^rbYu^t~cO7|Aa zss^7)tuv=JnM{a^*u1L>QtE6IX#;2eEp+Y7%ST%C!GU zX1!O$M30~cZcSoR0zIrK+H7}dH|Goe)RzA6y+eoUaZ&(b3`Gx=1ZdG9OHKaYphE-Ea2LoUq11ZQ!=I&ZtFTF{ZqwNsx2) z9T?le9IG5rhF&ICY+g&_#L8kn`l~6kmN4Mkf*!-rE2$l~B#ERBmB;&*+}*Z@B@9pZ zxgL=RF{*iUS<-^vuw^uzEzdqY&`2_m-hZMg6D_lMh7T?KoQzm{ny6L@x2soxqu z=MRY(jr_Vxj38Fl3?ExQK#ta^c2(E_OF*>0>~jc7^|zPb3UamEL~$SFW(yKEeDS{* zLTfa~NPuP-QPC$*brQOVV7fc|x?26s{=V8kpHA{bh$hAl38`b=TTzK~Ru|qOt-Db8 z(8On0Z2m-oEDOX2@sSVfoCX7ew}`)<%bl7~eCkp(ey<**iZX$#~c$iW3Gk=X^zd#_o2wYwa+kG7JZpJz80^c;X1sfvm7w zL^7yFn0K*nuF-&|bQeCt1laQw0~RmsOGjH<{KThASH_U8Y>j8rs1VvnY^kpkdUN6BX|ECyPy?-ig~}tjL+JvFW~B;Ndw|< zI*}%1NR>Llts1~e9DnYo#A;A&Y04;((^%8$DIllo{ZE@rK3C!|C_)ZxWfQRu_IjMA zgnPT&A<}1gAtIajLQ3>zuA2CPNzz871#_BgD* zE)@qy^HuxH{PGX4hAQM=8qp5=+nBc&ho!ImJr|WIv1u7Y@No2e!gCZX^E$s%M85dN zqs!Btsfs_Igig{298id*MRZ}%pH+s2nZ&cc1^CdwWgbMzpWpsEh8|;9$ssAGq%~Jrm%!9UDT%&*I#8A!J*6+@MF~yEC7Nj8vEw zN(5GV+s2IX#zG`?iLDD&MAsk!6uMG7(sd@nHfSN5iOvuknQLx`bh z!)rzdAuDBamsDIwJ5VynN}stEx&pZR;==*IS_wBcJGGizB_0T;Ud9&6J~*ASK2!X%-8_AY?_}EMzp}1u*Ht9hzTdAn+WpcksxGqu zsZL4EgUx8n^u|0GGtXatbDyqmb?e#!Ct;LH1`)B=k_{-O!tC1kG-z6E1^=;*?T)#r zd48PjBtG=;UZd7{*T(wsRtLHFQ&^brxJSi(35v$!!{{E7bsq>s>zD~bYYjfBe5H@VM$^chV6gu+;vUbhtS9ekf*6jj#sqq=b4D zQs<1VauBEZSghMlv{~Z;WVjxZ9dHs>mV72Qa-Bd^{i-1zNuX1kKGF7Upj|1fqK(l> zryO5!n*lY-xr$!9ukN=YZo!c&7x!N2Wtw*h46V&F4*p z%}lCDG;W|Ah8?F<9G#F^#d#2ps7gzOA+aDoIYX^PL_8z&PJHqY;)zylyL*f#eO9eN zl#I5cH!N3KM=EiN7}V;AC)khS@=CAlG^OixmrMD;?nbD!X~_ncir-XuVBw$zT;m*D!pot+z{=b8+Ha}UeFQffPS#~% z^=@?xvR6=f>bFnB)QI)DY%Hfc1@}{^D(>eK=K73dyyL{-1U?1WLbwS_d)WV^3gGmW z1Vd;~l^T*JCFU;al+}-5=CT9C#mPz|hu8=u;&noxqEs^C4IXh2h^lku#F^q-L~_09 zLZVYH`Dhlhjh1RxvXBl<%qfj|iI`8@lggb!n5nm+=I?$XK?lHlB$Ig>YL2hKKm^g0#b~l4)q08TCwIx$V zQ5foUY9<%ZX7-;avATS!;HAT)8qUm`&Hv8s;5rP#^?L)A185iiMrcWb~&OP897zmB0V6#Wd*a za0{DFjdzgI)zs)f7yA_;P}D6HWww3WFGZs+J%P%0cs1KGBJ3`7<%>^*?`xSOET5e> zIm$BXa}%9lG*nB>Ku!>|M=#CFdnVV->F{1jP0Y|^TWc#-f{YTU`hl`gU4AEBBJ#-% z&8%a|Pbxbu4uC*YK(25jX?ZN)K87!CD<*+mXrgr;!G7eB*3OP6x%bitVbxZ%cNUeF z)2IwDw6*M^JZF-madmki1A1M{46|1pncVLxy4EJP5jM7!OL{%j2qyfMN{yUxZdVJ^ z2hQ#!%RbW=Cqd=qm-Z%K*`lkIg+DvRtjN2HtqqiU6d8=qA;@H)t~Dp(dSZOEM90CcPTtZf%6i33 zy&ecf!U`r}y)5l6s^mMrYJ7jfD(b-+YI{j*Xg;a=3vF@)w{2&UEY_wizpi-OrMek# zE10ffZR^-7V=wYp5Oz4+DjI}jEFZ}{0Zt=}fZ7S$VtnS0I>4%6^L&i@ex5_lXVe~d zbt6;HcjKuZz6f`7Y9~XBa}8 zow@2j?5dQyFITNU?)O<|xfj5)g1{7z;y4WB57dm@W8g8g9!mV~f~=O>;pR<3+HfeF zSve-nkwR2f2>_G2I-s#L_IXZ`r0b*1%dBZclO?m>PX%@M}eW}o_%809* zHE+j=P8<^B)&OL2B+bprEr*N+(tDIS^Jkyx#X`dsokApA6kE2@DV09^ijk{wWgn4h z{)$UMrO8KxX5>jRjDQmmUeLPZMT$ZpG>#B3^1{Q9!nwf=Y4Dz|y^xCoZyRHF7KPk! zW$JcHXjp+!m(Pf;lK)b)k;lT@T1n0wOX43}R$wQ|btgnwf*_CqGB`WVQ<_%IPcG-%myP zM5wxu>y7??$dUDi@i0OK7>VFb$qHJtim5A}hJ)jRuN3g6Ti^4j;TwDGC%mOiNi{oaX9q~6;TokPh0rxnjHU3lgowq{ZH`s6P)2jk zh{2`z*2ftGihFnDMjfs>(yx&o^HAf@0J!t0k}=Iiiuz_lCZ&mQ3W?(8X;s7WY8flL znZh%`T_iUjwVL4MW>COA>3N2VLX(3HC+Jz0>K)`dDEW0ZKUPcWj){+93e^SjJRETb zz%SGvp@8S$<=*FKR7ZE{F$coPNjVUsOyhqjAReqUTsBRAD^+&?B>u!k4IxC%eYfP~ zoSHR*s0U)*b&;Ug5b^Hr7)pG{_FB&ZgpLIEpmLZTpumEp(7hPF7_uF{t9V5LD5B5i z;a~9tM7e7E)~eO;y?P8c{{jL9N<G1k zlh1b`x+2s)NCfV;w=|bI$?Ks#gC-5@_PK5A@hw^lC^?!ptP^pH17yV;;=@@bAnl$d zk7@3u`EW?yv1gUXY@uS^KL#fFpPjo)2IPL+#LZ)}G((c9(VH_62|H5C6h25t-GL9P zaB+o8DK-Df1NzQ`IIFQC+!e(O_*3Sp7GSv2Io5c7_QX;U{k9O$(0VW!$q6o`t}Wt{ zf(ujDvWRWmGPw$4#I{q0RF2csr^DeOvx8E@^Q;uAMKGY%oK1aNXBCmyn+T@vF$$I? z-x>@}Ea6&o&!eFLx@bRCZasuebS4BRY@?`E3^xg)K2qO{R7lVk3S(viDvV-@JJM{5 z18u%0+OWW)R4tru58%06f4q}Rdb5HeI z{^Hc!)bpmzF6YDJOPhOEW>KRf5*>RZxyth}OIwxr%I;r0J4jW`!3U$PZk={jB3CvG zUYzE%Rm3f@?!q;kF>1^D_*J}b@ub?djrM4{`_P3 zwg7|vJNCciZ{MpQ4~F^gCPRNWf9>!e&0jlQzsC4i*P!{&vC1Fcrr*Ci|M|5}?ce|Q z&jJ7O^`ofU_UBjEclpn=~@;pbPfF3W#e zo4)RTetqgCpZ;zCwbj<(Gwo@-6TBEc^Pg<>}H-zvl(0TFYNk z|Le^BTIQ|%^MC#iP)i30{YPxIC))r3ha>?2P)h>@6aWYa2mpA7i%S3i0000000000 z000&M004JrZ){~kZ)ABvV{~<4bZ>HVE^~Hgw7O}tgGiPo_*cTBzjPTH$-dQOWE+bB z0b<|3v4hz6<<}1{a;dDI8XFt4YCV%2A;;Z0e($;GNbfKI>+ITJ_IzIIq5u6?_|MR< zzvO*7WOZNu{;TTpV31@4qH(oHqY>``h1F`%M1R_qTa# zpDy{H_d}Nbv(qzPp2p+vzb?}j&-s5k{~hWFkmR|f{bS0st;gTS_-D|+&HJxw7{)lQ z*Xxh@{;{5aUEOd0?C{{b&T+pK!`%J;YZtF`eg04QPtf!C$Lqg6ll{!{OZNM(pXdEA z>Vx7xng`5ZROlZK^vnHU-`$?4>oEV87sYe%_l1Ce{R>$B^)FdoEPwsW&%GEwlhn^F zKX1@~o%J{5Pro5Q6GH!NY5EuSIsNzMIeY&x;6KKB4*xdre}ZY7+yD7L{uuq={(;v2 zfc|*=ecAs3%AcwKHVlzJA^!o4P+FZsOZ%rZU@Tk%>C|G2Ne4^BTR_VDt9)YJYD>AzQVUi|)R7O(NY z|1lsq_T~ST#7jc}1bcf&~oC$%~+Fkj5OhL|@%68lnmZ=ZQLk-C#`i+~0> z%_OWDMoLH1yzlT<+$>Mz1KjF<*%c&2hcFzt4l;Se9B373oD7uVmHSUzd^PYp9sRzZJ>&}-8zib83018L~ zgO1*>_?$a^uir=pJe~Q|WmxhVJi|?67n@h1ne*ZX{-#!5Czv@-76%8sbxxdK3_WpW zAjwe+yx&QcG1_t0-`#6X4j8({>dtIuB#bEp!B&~I=F922%)1LAGr!dy5==aVs-!(w);WIG+uZxf77)7w}3!WrFVu zrR;a3a^Wv4J%YgD&N)S`P(xHS55aY`rr*;0` z4hC0Av#-HDlNtOh>Z^sbdooyPZRQ^CeWoy3(A}A_edqCIp z_VFnMj(0KYBsxdOcg_Sxg{RxfU(1ssQ=C4rz>Koe+$za5ZZYm!1gw&vV;&P5Suc6` zRiW;3!8-XBlPrR?s9|ybC2X~m^lIbEw__prU^EYoe))sT++ZUNOVgeTfGKwDu%0fiof^&L|sVFMAUs(A3#~|V`l{`GWma*=_VkN9}@_S2c_@s;d6%rPm|654Icy?L9w?0FNg>)!b0-z+S! zf%<0CJN@`O)|q(U3@G0ZAsp*h;O}#2_o3Nigx?O0p7WK?O*}q?Y{E$2SjRG(z>pEH z<^`#!iydF$i-gyiB~XRgS)GzXX$)R*zN@iE*5TP`+ZOm|e7U5G%S@NXeb+UNK`1y$ z&Ph-9jZ8JNKM3zGqaevcQS;<##)!Y{)os}Zf4jYnaXz>3w~le?i&l$9cE9A11%(u^ zNyr75Mh7e3SI#lsz%-3;L^0BB0)LECu~>!N6Y6bUU>Hf{ljVAFsl79S$?p@^yo~nY zmz!X_E{;49S5U06=-y?De8{NUx2rW_-zHe4@Y{b#MXaX+FUJL#q}LB0Ov_b=P8Q== z=H=XzaD|4I8r;`%xu-0pY_W|7!z3v&@`~4GlilgO-@rMu4k8)yy5HB$V5q00z$}wT ztOypE(l1f-j)$XVq+v8v)yI6vis_WT3|a~oeAejU0d5=|`a+O!jPCQ>gI8J|`Ff%c zSWnel0L_z0_-2}DU&1A5=Mh8?0$wpqwD34yNSWMIG^)(;v3JrBUd?X`;!};|4m=KI z$0N8jk_|NOrDdL$k|{ov^k(;x-2$5JRY{B2l>xdh_H0i&OC#T7wTu3+ma9V#HlXs( z@G9PoeqLi7dbg50A7V;xl)*HCL>p6x`|%(GY}h#$H-B9XmKdDitBD4x9!|ZmCT2{8 zVdRKXW8#8YY7$)UN>egC7*T}^?XkfahLT6$<{6hT?Gp1*e6)y-1A`QG2L9NIJ;OOX z-qx;CZ&+=$jL=4RokLD4cw~C^%>Fb%E#~u^DW>@Sq01g_vl)h-_>_Zg0`=o47#3{C zKjtxKv(M0#9iKUJ6fV_n_F_U%Z0`0q2A~w8k8&aoNLeY;wR}t2by~(CV$#m_YXHfq z8{Q*p)I#0a8fj#HTU8J=Ps#i5NS)Oqn9rJ|F^5~r;s4jq2zq+ZX{-Q@4Oz_49 zM6IvSYGt$`Zbzk#shX%$YO;QCtyKWp6N3c_s-88lAY}Fj7J)W=u6UV%cS*)N=kmM{ zO>oFllCEj6@XjhqfMM6n9yYMlFab2RqcQ^Di@gfnOjp&p*0cgl|6sRBabyMImFB$k z56IjOp*W0EKWK^eXP`GW!9<^EbH1=!Qm4vq*|@ed&UmC1ThXqjP?Y4cLLIr#A&w-04&$Lh(Ad}AFA*{`(kF<*AFaZF8Sw)P&)Z9@1iA~& zc-XARhvM_2hpuMEzP!KhG6uUi$lBFY<6bNZ(Rv3QZ0tTU?_@djwaU--QW?A+dm^>gugH|R?vCvYPjI%U#EVDr9EK;=6nxrAH75)!i|_HSwNrEfSYkk+4IOj7+i``Q^lK{N9Jvdm~=Iq zjPn|So^j@jcjr?{{&w#s1CX|d_3_T-mHhN)$WF`z_{>RQ;HmQ{X)MMrjN>YAWl_7f z5n_1m-uPON-QkMM_VN9dr(#}a_;42=LbBQ7E%8XYdQQz{D+U?|)VxWL;_p?r!+QpL z)G#PrED2>`X=Ld62IA%HB(sjiVRoMO%)^Y%*f^98>Z{AW3v^#=M@RFBkfV1xndb6` zBJE)(Iw^ybd|_G)Y2TU;aE7pEJt}$WwdTs0KU!yDceM(PL7@(glwb6H-?7QiUDFC= zM!qpgFXOM)BkOo5=glPt@bK2iy*gJYRQ)aGOwPrLid?YiG2d3nb)Rqj>-*7SK{Z=5 zWV+7XeZ!4>E*TRoraToK>3Dy6J^*1QyaDaN^mzBh5!lC9V7|?1#)+JK_@%G>Qi|Xq zjZl&Fu}4Z9DW}i*kxQ7Ki7xZVNj9`Gjo%e+7cjG>=Vk&$X7At)&1}6hU=JGR&sa0g zEQA(a-Vm~_QEziQpAj#{W$TxzE$U3@rIzJ}_<8L-56<)4H(>Z8P=rw4EPXvyVRV<0 z_L>kz9?J|Ax|~!z5FG*j#hy6uvDjv7dwk?r9tUq7cg8NpYrqy?MHxMYrPRD0ZXTYv zWw5WlN1?r{n<}gqpyHTAvAxs;DpFOCQb)3a%yAquV55cHgU4Gr<>T#9q)RZ(Qp_08 zTZHtd1GhXH1Y%5<;qiz}t7w7+x{HKyZee@1J~fP+UK{=z0U(Ar2lRM73OUx>es}KEgym{#9r0-tttdfsLabY<;{r z5JgU4Z)xdL{I0!$txQVGRDh^^jDEY&0PP2b>fF`<*F(O&D3>R==m+>a!*_o??ZG@! z__6u(rRBb~bz{UHNC)rkmbM*6g*r8L_Zr(2Xh(0>FwH>>g~{iEdK}~75k-jm7U>g? z^Cf3fbT`oh)zlt$IMWA;0twnD-t@8Z=bQD-95Pb2p~pkCo%`Twp%>MD-npdk6I36luW3=CkKqDPs`zZy zIPb}GKOruISt)G<49HOuBbUX13~Wv|{Z_1>w%@fEeB3ULunBAEWp8HL6sVlW`LShdZL#yagDT0Fb0%+1{Ba*qT~&>ShqQ28v%Ecqc8Gm@mkTD{iq5AGpzP z5aI_%#~=94U+o&A#&rca@+h&-9DJv7%34oMIACqh9e|XalkrE>@C`tAwUS+|$+^HGXxEDl- zDUR{bCiZ^u8@HEZ6ECQfLn3bosF2J?JrWt@J{{%G};gj&Sg;D53cOGAh#cU-{*_e2}3KJUsw2kndBF zPa*2X23OC=1)ik~_`r}id0PaJkJvnv*PJU9mKX5uSU~<`3O4Am z#rFO<05K?gE73jbM2U0z^Qy=)!#t`AN^g3k3xiMvj|DU0s61n(%!^W?iH*y|M{}6C z(NUWZ6_Wz`fq8f#SmVlKnF`Arm|3YJw+z>O+JBj$Xteg@@Qvzp4eh;<(b}x)T__=2 zmNF^Rd|&Zb)=)GP42m1sWUjrb?TL4Bf5AHAm_nY0bWdeOb*LvIa&-F4={E7_qf-E7 zj-_-L(V*ZF`RS^=@bctZv#Z)&;XTxY$m3*dYzaG-z=R$edm&vI9|oV8G2#>Ehh zL}M62D1~@1w((>&yqV~WC3`7O9S{2BfuO>i=#WChRDUd}%(T5+Zv`%v9ws&9y(Qm% zO+EP3-@f+*?^SVyZqpHMJ*k*95UdUl(b5K+FlfNeu55Dmy}(ZhpXXE284{Cy6@)%n zb-R+TfQ&r^*Li%v&viL%k85j9mLnM?;hV~VnB=<6R&_tbo+nZBt82cB4^?)!=ad~h zvLsOF4>xWNs9tTMQ$g2OfqCCk-zU4Yh8~~gT*vN{a0FKtB(#U^3Xb^F!56_}R}utc zQ@Cd&DtP-(c}LWZ31+FY(n>{WsPQ$U-hRCGEole2F#UKLv=Jg%_{#Q>aza_bY1x63 zS*6Cf@I~*wjosLuYC+s1_sTtj`XaRbC zEp3rfVK<-LrM}YD68rbUrpLZV0sZWm!O;N0pKm(&_r0;+ezbO;*|`2xz`= z;8MLcFQzD)i{xg1Zxs50mW8(BUf!#<7cZzP~yQuWT{xp(y54bZ|@c-(y{ z-@TU-D!?qhnR|vu?_khj;9~;)dh>ST{#H7`6NjcI*G?$*L=k;Ud;4Ue4RArPBq>z# zUSBs|XW!TuP{F_jjEpr{_}G#QA#e!VvNGx?(9_X^v9^=2qcQ7;HM8Zhqc0MIJwHL(ty&$f;&A#7_-j66J}eMsxh_BKg1+sJQ-S zF~vRt%G8DBv{#4ybb0oIgv?3i-6FQ!`j*lFv}W|)X1Us1Mh%He z4kmF^pJeJM-GS2tQ2|*=8w*r9B;4qgO(qS z`9!@}e^Ykv<&*=s*~%nH*mJ4@-R)N4zHeuVXlCvM;E(N<``1=v?Gq17j%n_}lD99L zx9YvW?XGp3znKy8(vv=SypZ5CeVHeIRA~*po7XnRfw>++AvV^NV$ibt*2q*{P^3*j z&3A0Kv*knp#9Dlhc>xpCdvo#e<*6|B;zS%LJHav2S$wU0=QiQ>1={+QlX6dq%6JHvN=Hh_OM+slp|XlW z&PU;U?Hs{RFb+eo_Ly6Of$8_e0ayA+5IHRI3Mz?ste?Gk%0m#oNfOf(QdyK#0eWVw z?)s2|Jom^)wL2m*wGdf9S4rUALS>BdvIl%x?30>k(M{gZ?^sWbtv8&Qd^BaMLUGzI zF$S);lNNWSTq%KiIk0X5QIePo9`bIu-IGxD(3tu&ogHajj% zGtb$?xi_a#k&_qO*Vdyyj6TVdlp?~8%OWz`i0sj8HdPsKZ&5UdIMrE%6dG}u<~TM& zZ8~8_JI$`!?lkjA_ymv6C9flxR{e9D8ay}FX;>FLR+zvf^+KB}I<{|KnqHG6Li;$r z$msbXN=0mR`xH|nplkD~$&wGj;U47Gd(IjaHos4Ik7Vy2r+`VWIe*woK8v}JA10u4 zM;ra{;hZcZ0C&2GiJ>p_0ACMmEE+sHo)eAbzJXU}c+j2|yge1;v)$l(e<$wG8gH8%?cJNrr7}57FWZ>>C^s_?l!J(jiiY9j=3r9q0G`c&+;ebz?nwbRm-sk&=*;oNl_fg7KN};HG@jL+Y#vAMVYMoGW6C0^zSM1H1_!TFnP~U-K#l+6*5~*-_$q39_>vKkJhgXkLz8?GjZ=y74DN@BV0d9kA$~Lw zwpOk7h+&+94|eSu(0D3iaEJs1_3fhtMv0e``t!KOz+>NB!}wtg;8dg&g>vFs&3)Dt zuEOZHoD_5blgr03NXbf48`28W?Q;`C&L)YPD@W_W_aWti>_u{gI;VPCK@it_!_ZLp zcwuCyLxy0`ulPtIWss;ldq7~a>sj@(^vCx{H<^U92S;ZhFdqASJKh-tKXKE8W zUjWiYQ?LrQbf2z~IHeM;OJPyg>8)9rM`Gkn6pr&%3<8EH=HAhPi~So+_c2Cn+iky0 zfWDx1T64>2Lc}bzL4$T-*j(prB!^2WR8AZ(Jj#SeQgo3yzs$4uU{CVfJyYQ<`0wUrckxWtY?7atgXFR2f#D z!2m{~J@8gAh90etE2JeVal=LtJ%o`4+k0d4;ccr`;-6T#{7Gx|Ph;!tFdfW^8!lVq zW5l!GyzetP;xU!6n5f(sED&^r&gX|&cvs>t117Ms@=tr8EdpIUVniTh%O8?-D~7n$ zJ!x0h0yWdSDG)uJ#vMxo6+7At8xB$S8q+Pa1=!3YzT_+&sJd8RB7#%$3g^O~`Q2j} ze7Tg5lq0l4o5o`^-O$Em0S+R}t{SW4ONwXQ_sxbrTYg?xW+N}&qdcpR zSKChMQ(|5LoE0zMJ7j>{Biu}Z6>z@VyOJ(;VeMK;^CcXio#59>V z0u68ca7Q(j&gcyxv$7%EZ>bTF^_orDRQTf-{FyuX;!#QJ4d+`jx@7g~+vu$ag7E>n zA?O&RVEVO(_?0s@eH(EuK$=+{5t6_KhTj#{HTr5b+FN1K_Zq9iYh~;-la9*YXiH!2 zp{t4x^*@SZM)PBl_f0Z_0%OVjkMFJoQzLtlgTC2|>joS2c*t;Lw;F9kKg=(7li-R?uRJKkbh6dw`Q20>%;DfdN@ z1Dx=rxliRtw77K<6qaFO>oy z2sxYm$ZlS$1}x2;^&*<2Ha-T%c^$?!nICV2hF~D>d3DAQ@@4x-Q)^{sn9<{cPIN z-Jyyy!MSu2_?X^2?4|T-6!wVP5Mzv;(|c@v>d=@HRyh`YD~*j@_4}kB)#1?2E98np z^QO&{KLYXia_IAECw{}xuEXia4|JBO=R!Jvzr?{CTDF!@nZbedWxhK)Egne|v*0o$ zBeHyg9TEtuuRR0|Y@yL_&gCam%pWn`O{@BbQ75FV>A#hyu6hqmJf7>vYtzJ51=4HV zTY_1!3z>Bs4+{)n^H;+s2KS+35|_t5^S=F~ziYh~vsaE&pAT=0r*PcZ8FtO6aee5( ztQ+LpB&gem$#TOX+Am-;Qy&IxN|JY4yq_|!1L%s|D^(gvYB0bYeJ!tj2-CIhsPQlo zB-UI9fw}mK@;x1Si+o}PD~?=Es$TjUvGF4hlFe@pc$44x$8pYZ^ptlHJ%(|{)owPL zh)};+7Db^}*L5)OQj?#wGyGrG5U zJ9y0=`76}u$JH$UvoHLFK*>?{E8=s$O^r?5dyCSs>=ByE)c~GX+*8^!_aMkR$r6u0ol1`l1gaS+KI8ht zB%cyOLc*GqjDbqMzLR7o*=VVdwq>5!@jg#7uDXwrxCV7o4DVoQJ+-T}7&Y>8E_@ch zvg52dgl7@qly_UnMu;gi2UJmaIiZ(*8X8t)OKkm}w@Q;i7zZE|dtdFji&Vx7&|QS* zKvbi2+cNz6FwRRf_$nFz+g>>-6_M*JBJUB(OLS&}%;|IyZNH;o^z0~oT?^q`W-`6G z{0{qBMzB9n2K@Hes*)!9{iK;Ru`pzO{fu~@cwZSk*bDt#pHf7TS{WfyrN51~@hX!A zE_yf%O!apuyV0N?vrKuUiGhK!WVOg8wWP$mK-0ON9hC7xo|H^x_vsKR=b zskHM_wSjhaLzN9&3Di8&`MW0?<+-~LJAuI-ij70^7}W!1_S3EtF!|ni{l;~82zyr+ zIRn~3H}S&|xGuW%VZ~ITeD3o+YmQsLl*h7~iLrN8R$Wkr8&sjhRkmJ4$pJY!V0*t3 zx<^wB=47C_$UFG8KQNqFq-VLWio-X-mD&Dq$QM$MP|%moP0#-NcsQu%0mfm(Mbnjd3{s(Z9IH_i0C1y}YsZO8QpDez+`~@$JQD zjE)fIjdJ~W6@ia3eo%?{IQrp6PesGQSQ&kWe2kG1KxZ_YY;Qr9zAoSOg?@F!Pu4Ry zNS8*jaRm{qo0D^Ak?9#KV!boL?o$4k#ab9OA1vOaRTqdQxC)s zOy0oKB5^Q&BxSsZ`?bT9%XUylvmYLL>mNm0C^ilm^4(g&t*pl6{K!%23(e%md=tH5 z^Glkg-Wh_Q+78sjnBIrrrA!J`O2dN3#OrKr!DTW@#0_mstnCb^?@a1#4jaIHYGEma zd}s%Sf}@{vL|zCzs6voJ=ua|`!!8v0Yh`2ZYq)ZspFJWu`eRSxw`6{j%nib1Sh#fs zrcy^xRpdz3oWo>!%xo8ZmQ*k=C}ME*D#HUPA*L6=>yl19!XC!zxclLyCnD_&z-HKx zVEI#ynQ@&!Pq;T$q{KMDkMV`k z8hhimwN4=$JKWBor&XW~_n8?>L=US*E@iGWl^YsDwFdk$KW zp=$#!Ytj^hY0GJ`;sK=5chG3yx*cqNno?>K$59V~+*q{Vn0?<^dPJCS88BZ3ssO07 z_yHX7t8`RvM`a0}lt8q-lvp;vVn9t^z3)Zer@}01NcsiyltJLbrl2wS?~S5zw3}}9 z;q<4(l3dBza5>(deW4|+6%vN`NkHli;gM*od%EhQTKdf!!2-;K2)o|MF$!6T-SM>} zCfV=fekZ!bVbwhrm2!_pj<%T9XMfIvG10R~OxfMXJT>`PaLA1w{*g<>U_R8QA|V5A z^4x$gL1}y8Q}A84D+#*cpxco5+CT~-(JTkX1&YnIKssemXo-9j3wFH^K)UgOTBt$# zu#no2FV(**^^l?n&=xIYoJ%M;=cB#&FX&jjF>{Or)q9MY6$UjVG03Z{;~$23*b)2A zvI+BYi?&lAIO}WU?W}g%Zc8#fy~pVI_Sz^M70jZoh&fWkGQvgP9tr8Kj5 zAwt3hPN=rZXxnZ3kBM*jcg(HF>+4gR3u~x5qJg3PJ5BHDijF7Az*{SqjZ)^se`I(Z zd-rQaFHoDvO%L!jl2WRdi2nZ4rV6MsgY3yUjo2_1W5+)oTpSf)Q${>F_9yYt&JvrC zobTf1_*8sobciy31!X3p%b4vF|2T+8117AAqP)(D02Q6CX25`MO`7H;G0Ny@v`(oC zpgBDpIjQ)7Av!riG6Ezq3X~{E17YG;e!b3Q0P715x^*>J853h5J{A45F)e0D#HtSP zC4JrvISSDGv&@RtZmFCps!^pvDvDf(#mA=z3;a>4t8Kw&jZ-nWAzewBDD;&BLRZid*|gCXFWC2pIm1$IE9{)6LbLk!*&qmWIjB+yp3@ z7U#JcPcYPNW&}BN_J%iMU-TB^Ywf;ismz6-)TQ7fU?9eqqSS9&%|4cUd1*9umvfUz zALqE#)|I@>iJ9smf@{+aGOcONfeVq&IJz}OSvqjO##>f^!M^1UBIQ$DY-CzKy4VUQm*OsJyu0XkY0F~JQ44t%h~OX z*`y3ca8d)K^MD5=pQW;%VFkBClYAUlqt7mVfgVjtME8kFhiffIdP(MLaZwVy$IW2g zd_GglVc`tGY3~ua6JNczdxvN@Cz!YhVi0SOx zsyGR1lhT6pOt`+oa4z`Qqtbkov2!3RT^ih2L{h6u@`B9LX%3cd_TI@i%wN3MWFZ{8ko;vR2R$*_gC=TQGHD@Oz((f&cYfy!LuZnA7>qLTb{R}95&wK=(0Qu0^Rt3Q$4MEiKGHhU>p$M$NmwNT zstF(zyp?9&YEtf#c-2dlF1OSl(O-n9i-!`Xc^=J7OA*5;M&hVPmUHgfn1b<0*)RG^ z9uE4+<|lYm14c{Zi!#AP<5`+|)Qe1zCA5es0I14f?Jpw6uub;;RMiGRhUK>pZvB&G z$W!!@*xAPAi}Djd&&L!QahkF@pCq+@rp$6+0#~8$qngJLSG+sm-E#(87oJSGeY(Nq zy$|ywm-F5jTh`(NG|uq%?1JeGHYgP-Ad1F}Apv=zY2kloRv3k%yxz>8!X6!Img`Z9b1`e?z} z7b8YYH4|<}dyjwJfVh8a_g8+?P*OU|8WV1UB{)4bT2`w2wjeWn$nanuGt7EN40DEs ziB(I@H(|nN1_K}P9`_dy05Mb3peYTe*TUH|3P2k%71Dj?I34SFmU#2?OXRzKs)%u# zZRWKYKu-HUCFDHCI?SW5o%cRFdOtHo+I*Ev!#KkGoPBlggscDou33C{KMA$b=6U7} zkflOC9UIUOd%L}IhCgjnt)<5KfT}enO1=dG92QPe*S4x*~nQQjMWZ34sG3h!9=H=e@yM|036XxbVdQE4Ixw7 zAp)E}$>b(BjwM4ot9@Nb@~|<(X#6pSZ5q<`sVe5zlugMAp>b9-nQ zmANDR{%GCo#Nm_W;0j7Q$c+s>&mcTUZQnBwQL!H~o>{Img&%DWaP7`rMuV<7b2IV< z__2nuG^z`FXxA`m$|wPb5T6sG>2e#GLmyE1SY-P#NY&etct>}BNJP={4=(4~6YEB< zH?_rKAZ9Fa2IbeZ?k5Dx4hICur&FLsje4hsSY_)5xhx8v4NGCbU?D3V5J{pH0ABQ{ zC(vzq>uP*Bv2hrRtxUBcOYcHP${|Y|mmtigP0gzPhNI;( zr7Q$Tsr!hqu3Ymaj&2gpcaYsspd{2u(a`69bP!k*H*7)h zY-E7)36aF5@gDk+p@3J1A*ho{`ZmnB``EqQU=vKe!Zh2V&`0;!4l+5}DY7iYt zB=sugmIe=T!-<$hw;VY#sRufi`g~o=u6viIvBg3_!N&b2TE;`$dbMuR1J2rPMMFr>f?HWb@IAhHt#RXQE-w~E6nvS9nm zs<)3+Z3_?$EQxG%igGcuDg+R+6{x=eO}Jmlg=b|Bf>hs8Kofm?^o<8eu#+`{fk}Rg zV3yX*9Lo1@HCguv+PN$X88Y!5X-l!+rD5M?i2pHeDav9Add1(p|` z6#jXC*BFH`>~rOe;g?)yWM2%fQCKIb7%q6t+1CVVjgDVsmoFH=_JxgPVO5g1E7MN5 zbu!H)El@w10O)w)EwVNp_!<2zw8r^dL|qFIe|YuS*ZJNM?oO%f^*H*Hb10mKf+aIb zyU@wxd*lO5uu@<5^*W?6dSymBv#J4MPHr@s@)Emu~Pc?1|{$Li3( zd$^!qK>WJ1?Csf5dVqwfZAL9($>fQa?Rw8XA1Vms6-)NCqUk3vdRXQ&{kSn%RB);J z4blIbr1MyER0*Q!2eH7rB_4#A@H@Qsp09uOyw&tXbf}@KGVeK}RAL__fNJn-J!-jC zxkFAtmj;l(lw!ygYP6%AZl%&9w zVhKTV9Ew_Bb+sF?C=-aDuM+?urNv*VAwSb94fhISh@96hzu>!n&Wv5&dqLH``ZK5~ z3}9vw@Bkwk*=p~4ry1bm<7n%+L{i-aKKGLN+gd95$;q%!6|XzcUM~1JS3kK%B|zfdq(@YT@;>v|k)J_p&#}q*scv8v*-vV> zNu(?eZ&UCJbcdYDH?H-nlRwC5<>U>R*)7mi#7Rg3nmCbug62kc8@b_`mv?V-QIMfyj#Y$kLOCIULSd7188BXBcn(~}xJ zu>tU|ebO`(AKH4a{Idle95Epcj21PGM!5}(^d{dEaBQ5S3i>Q(i-rd8zL)gpqxlC- znhsBmC`l0d?Xa69<}_ll0SA*w zk0eP>BkA@oW(tOcxmXy*hl~Z$f8~Ty2(Mb1iU>2>m!nkd5CHO_u;!pmGkV@}-@N3N4I)PNW2*}@CPAlZ6Ldbwk4u0bTt-XY2N^8PE4fx) zKKTA#zU+>`=E=o!dq@5(Dfv|u-vNBd`6|pMe8_eLrPfzMfO$~#WGrymptG76h79K` zkPkJRUKNbay|sD+*P*d;cdcKk;BJ$)4&r&Tuf3kB*{F!;#f#m^*C_yydt2n25m*=M z(IOaCgTx^pPjh$8@D0~WiX*I3$kqTXUMD3uil8yZl??Z?+37lA z1}&(hd9PQRsFq(p8@Wvu6dD&7+b}S5&$AzV&tq4i`dc-@1ZFm-wIzjD1A58OGe1Bl zs7q-9I3w9$ER2TXWW?sGgow=0E<18j1&4^eNE@DZ^wVUYHAlxfkbh==ye8LCTp_ux z-Xh;b-bh;P;zEHOMj>AdM7ah2{@7O|#svY$I{K_hV;ANDC}Lqzfpc@?QIBrPzEQvP zW#LUP%@dnxezGL1;2o6U@t1xPHP~W^_Z$0%?K#(hpAUI2KUj;~rS=Sovc8ARSo}k0 zU7I~%(5Qk2UPEyqYe)Q@BN8NXwTNeDkg5Kv=D@#BI+JsRC-=T3QAL?jMlurD;K9Jf zV*Rz4UNA|fiHe*SHhpeM<_E|o!4x)4ZtjzRr;k}$+nZm+$3e+W+xM=eoml8)D{u<# zJcehLc_5H(GWw-RzxgWSK^qiC(qb1WQkycC zRF7o$jxsi_+Yct-bM%0@Wmi(@ze4T`720eu9Ux;ciwAs#x8<^Zs>dxp84OQTh$oYy_zck>guUJ7<}=4-6R`+z2)_01f)Q>t~BHrsQ4Ow&@P z&!K>@)0|A-Z+SUvbs*I1JjVFhpxM@`QzhzP_LzCz6B%KTEF(q8wtpU(UJVfZO+Z#* zWO=oVuewJ>b=D4AD9Q|cbdKi7;tjDXa;+_#2Q5Biv|+^+bw6D~)~6i5Y980wR8aAA zw!*P_J#XAJ88+_6TfK*SZ#H|YgC`1Et8r}p5eWxl_)=LeiQ=os5W1xZSTK;ANM{bF0p*}#M11YLhD zxz5Jx1k_4TGH>4$^{b&@Zfc`IETjeG%)AR67Kr?n|MBjA(aclKh~S&_AnuDy4*GoXK9o z-$zLGml?dsqxzIT_Vk&naCPT7~kr1v-AV0s6u? ze!y^UK1nzV`%HRnUjWWN!Y%Mf@w;M=VQT@HDa;`;k@-aERjF@q3_%lLTtWf9@Sc&f;rZ#mXtLiTH|RH^RL|gHA4C5f_SD`W$`Re zSLN-{WN_F9Z1R)TLE^%#D+W-cSwi6sw`lK&&5l|O62x?Ocs>_cHlz}qDC)Szu2x_)?vSQ1B#ST>;)8B zf3jEr~Ge-GvxynQ10cmH(0PP`)Sk+#A4pKlgN6^_qBKFm6NR?tW4K<~Iw z-ijK_g1RbZ9e-k=FS&EYjUaF*1}H;En@)^AF+BOfg;A^S%bw4|#bbaL1IJ*%ex^qS z z=XqBq)O6ZprF7-q4~_n};hTLss{g5;Uk1Cn!$WrOiitzgY1*Ez5^);35a+s}qsmIL z(Jr6l8AukRR`Qm#*y5K{Tfoi+mW2@cv}WE!UjkDwb)lLSHc(THC&BO?j-A?bcciWa zMG@8shG1Ry>Z1)&y6_zVWu+Ny^YM^xubg&0&|s+Bs`k~&1(B2ED;7c@)XhW49pd^v zazNdx*kkID!3>si#zcfS-I> z2UzA43BcnTA9GSiomA_f&JGch_|w`#+D9W=yI+QiPK}hG4jp3-I6+|s2V@3aG3#AD z_(j-BM~0~|=e$p)N~$iwZb9?yexSMatm<(u91~VRFsxj`wP?`G)gBOG4|Yt;~jq zhIV6tVU&$u-;_>PH@PQ8$T_~Fox>Kg8 zNwaotPJesLz95}x1VOG?glheb9mnmOG{rY*Kg!U+hwCG$z4~(H>uB@}a&kUR|BlLa}yIp9& z9d1j&YJ;^PS+=ms*5X1=VxSecuA01>ab}y*3lB|)q1TF1WVyHtM`wDUCSoS=ua=-h z6iC1j(Uw`1|S^d6j z0*y(l;qn-iT7kr765hmQ4Oa+!d#T=y%V73v&&jbJbERfFlC} zO@JnN2N1?w_pqh}O5wsv-sSO_4^zzH-mn%b(H11A0QERztQB_zQ4<3agVJ%Y;AQf~ zy+53x?-dyp$20eU3o~JdpaIrLwU@jzdec9H=3@`b`-_el_>-WItFfEnsJv&5#18O} zzppe^^Km>jIhXvllxOZY=;`PoByWaRjOuhcw_a+sPFj`9d-iPZIVwp8NCQB)5%K~C zY1erHZQ-#Xx4L>c)X*vziR7TS9+8P8VhQ41TvWlyCTOsfuxiQNKhuwP#VG|`WZxgw zC)Ys5W1yLIzp0C7rnD^UdtSTIgBIDcjDTO9V3cih1CPTi!OcTUP9zPk@bFp@A6l7D_w{6x1rluS*K_xOe0moo(A8)xtuQJJ8$Sz_*`!BU>xP;4uoB>v_ zzyST3G}4W`#2ly=*+l5Do?CnPA^QG3W4%I3$4VAHb81d+Z$=w!THjg=*(nD8o@YZv zX7|glGN|NR&^dpQXNb;jF7m%<3#P1*bO6RS@|kv{$%}SXc|ty;7%$a_hg?aUT2tP2 zB|AA~HQHV$<*}1fn=)9I-9Fn-ktCQ3$^dgoj$@}B|2*U1p3p~RISJ+IBtP^8$N7@+ zpNZx%?qIfU0+2e4d0YWG7eJKpU!r>_?5`nER)2LiEqL!1^^~VH6#aPg1+rMr2`;hZ>j921`*|Y%525))H zq=$$8@E;<566<|oJ^K~Z^NYIY7Gi?9^IKxEazUH2Q`nPSNQhgvEc$&+Ts$sifj(+Lw+MK}g)d&Vbwu)AI? zU~*~N4kwSea)*V}PpX~^s)|e>yYSgTi6}H%o6i;QkBsp_WL}rQ%D`t!9&Jgp?fPXr z1~NAIRk7&TD2@nI1K4iXcM7u|iYb$}uug1XorxW`aKiZ8$;@!2lMBIP@g1#eCBzSc z5}7f+%e+%T1e+5w9!lohu{NYQQ(L{5U(f(CfCqE(wc_)c_^lo@+Zg{mQq$wQ*Kh3; zt-s!o#c{}Z3BH>p4b+jV75*kv9Z)0(QH+8ioCR8X!@nqmHPV++ zYHJG==w$Ph&*C}xQuFTuR4d@!-#qm*?sv~wYZ^X%Vrm>g$baKuH?9s`%CrdFG3X^ zZT<5TGgEL6yR!33P+AX38h{CkYGZS8>n6td{iWihUIH%omFQ*LA81r7oJWgiJBUE1CNz83Hz_=~!w{JFhwXP}=Y#yF+ z*uCrI_Y2cBwO^ohTxV{xx@)iqoDS~iLT^B%VcN8ly|AgbAn2wrr5%=}HMnQK>RU+? zYYJv5l4rlaP|Hc|`{$`9RA%30-Npw$WjA^GswBSdAt#x#Y5i5>M-1~5sDG5DJovA} z;buQyaJ%xS#?l?vOa+LYmA>uwa$Lr*d^vs@Z{HI^0|xQW^>++H=b9560;~y*+lgPS zfz9sii)DKiB0|aPPx)+`RcX&b;RPBa3}Th$#~Ukhr5a`}N0b4m>H z8|d$EwjUru*6imS0Oz)EX#gWA|M{|Lf#>+BtIRq+uPxL5!T@HiX?bkl0xgvQ`n`Gd zCh(e+u5PxMfrJcBdC+xq$QVOW`I%B`erW9IUg%k--<%B`ccuC#bxEJsYx^k)2U+R% z#?EQbX{0@)HLD1C=YvA(YlsE&5BZQ(JUo_ABzKVApQ9z+XZIJqy6UbVQ$xv_$Z$p8 zA$aTkf1%}hl2mxe9!2^Q>-#Qfe@hjz3b!7pfp!ivY|nEsELc&SewPmdY7&gT26adF zlly3^j+m4XxJ4{yrZ}3B;L@{oCV=vDJ+~?Q2q87CD;R2n%8j^ILhOi85iy0{MR5@d zG+Kt~@?XF3eROOBxi`Scz6Wc`-WVu$KoE0+8fd$8>Hr?Ug4UHGYj31p@cU6>0wAXE ztVXZLZeC<&IRI9wkM=O&{8lE>@Q+DGfPZI8PxWIQ3=0(9nw%on^!l1e9)zo1zztWqJVeY%Tet&2?ul522CRa>GP@G zHwUIXkUb+6$;I8L_NeouQ*)%u!L4{jIn5g|_!u;C38TT&Ga@1{V%raF?gXvlelzfW zWb1S1?|=J^hMt;pAa zL7P*124uqkiVK{g<}N#NII(52vwU{p#kOv~BX1GLp?jWIS939Ar9B-I{y~iGhv9#R zr%{L0kymoQqq(jxPcYKlrLWH~HJ}h(9A`DxnNly`pSlg6=SPek?FY<9A{A=I|2?C5 z`|sht2%^hYU5~Tl-rV#6v_nEXkc7;Zk^I9!GQXsC{6p6e7QKUvy>(i;rR;_`IUjm0 z=~Pz{QB_@T5qB%|FPek9`hf zWE=W4h=LEnv5}$oqqqsAf3iWR!d#{z8+io<&K*cC%@OV_pQRO@;HQG{csv+)z`938 z6jjJZjCx`(R{_rCx97sEj6)U`ITpp)9Sh2m@)0m>k$=)Df95Ziim>^?RRA`+s26^q z^_H=LqQ%l91{QlvenPmEC)u}+G37S(8q=?HkNUnOl>~+54AikEkxyL16q7601p2pM z*zh+qOOi%C1)u_^!s5jJg^tjaR_#ifIO%?eV}lEx`~1_UrFs6ei!QR*N`PnRpQvZ$ zB#%CcP5VfUpw5$0Njs`@f`y7NaiG_+ebXFRy*E9FiYm4Dv(fDeA?54`z;a71n--cGSf0vY(#l6A|qoJJebDm@^SF-ttL(Egb4eo+4EN*Sepd+ zF#4dG11JWvSiz)#`d*Lv3iF8=s8H!^I;7BuCst#> zjhY6+8~-Aue9@Cs)`T)H4b3A;%n{T(KgjGNR8i?oBK3APDD%CzCYv?2@^|b64TUy- zlX3YY4S8u&SIG%N!NIO&b8}{mj}KCAg@5CD0GFzj_#%~)82S5J6CznEvXUv#5}LO;g!8wQEBk01X4^P*@k)fF(U2N+g@JiNr^ z$Z;gN_aF=M&=1EG%=~;qT{k)#h`SVSi~L^MZ1Ez-5yA!9qhI`MUv6*3VOx3}W5gN@ zC8KgBfl2mg%J`oVNBrL>C|mYP;~0P9uu`I$txy517kHu`JaF-v zJq&*nD^=Q$;1VSmBnuYo?l!G_x2*0E91yTi()C5waW`?g$+O;%pC!WvBE|PAi@f%U zs4lKHPFALMch4x4SLo-x9a5WZ2kBE{8F{>)w&FVv?bcM)@SuYi&C8&!s+Wd*jIRdi zLG@06^(>N-=E#*YVrtU(5VrS5aae zke7!8k53L?j$+FM&zfh{eLukHuHjb(&jXd0U$Rm+-&JPR zH2>TNgMpMiqFZk~#640{YJJ(nuQn1{7){b{Zh$Q{nG$@PXY1X5fTZ=sY8Y4d>wf2+j8fdGK- zZp7=pr|as=K!AChB1XJ9M6K) zu`~*;$b1LJ>>3!i$}HNf7yM5W> zS&$!IUh>#=fha%&HUz~`h_dkM9yp(}RuL5wNzhBwY^Jpn*%r}~D|%ZJqZpy&R>jJt z=MRiW@y6&=!r>k?1Z9gsIoLDPx5U)+c3L7I@zrd=!Pf6Ho(ZMuN-wdu>!ey&va`h@ z{lqDQ8)Co4^NW!W)%z~W<5n{4#;>TH@fa)lGU+#jMltjq`~c;7VMU+5Pkd|lp)|!O zstGp3%cHcHA zW3|{E?BcY7HGNgQalw_Kma0K<_Ob zJvv>>3VqI4vI781Q+#>R<)UVd$Q<&^D2b>w4=AdkxRL^UZqiJ{=>sCxvMR|;qa=a_ zk3kDf^TXdN+NoizR00TDLB~CSEf7|N)YN*_e8P$^6U8Ehc=5sDOu+Dj zN4ZqImt2maQu28A5s4CeV%QZqo6)VrJ$H~jYsV84{ty%mxR(^OuIIo1OKNf$Tn`aIS{7CE;XxhRcoO zfM4QtAOJE2u&tP7wY_73Ct@1oW-V7DPdPv9#beLG<8)HGI$viAw6Ktqtvwk041f5%+cp@2NNJW({shZ#EL#Is7&(@Z*@fOM|?{ zE3cFc^qbP_h(5{`9Bp)X->v5%Mbg3@Db+>#zN+kdkte1`Meq>1=kE#Z&4^6xp4 zMv{s~Isx+=S>sTKEyCXvn;ocjI6G&U7l5hx^Q(x57W;dC7kibk^B7yhl+cTyO!Fr1 zICdhRI>G=ABoU7XE;AX*Y03BCQ$S(u+#8(>UrZ?m*59(DA3n6>191q*pEQp-oGiZb zXMlMBVeYgzNKiIyyNi4JIFYsh`>i#BD+Vhe3cpmj0PP^2C@x(hO~b!lY$3+WvCBn) zbYB5Dm&43{*v)`jDyto3jl=h9H zC$Vk{6_T#AWh_ztd&8$42PsGT6!M#^=g8r=_o9}V*SF<$%NwffbUVM}^-X*SB-Ns? zBLxSZ)ABbCn@y;F zxojg+Ndl+eF_6rHv^MyTIQRT8;+MG0&RV>CU|Zp}yBOF%`jFVv4_{OqkiN=HfJx~6 zmieT!iO`;sB>Lub1fagMn%Ga_5>K`}jgBz>BMiX>UIV-1?lFD$4H&K|$U65nxD@FO-n!9K~ z)j{YWO!~%Ry8QkwBmer8%K@xe^6P4#y=gtAmBR*qDvZW24FmifJ^%?o_Pe)u4kU66F!>})8*A^0n$f=(9V@Y|>@)h#i0CK8|r-C^Pb0`-{mUU}nYxJQZe zpo0-YefU>kG>`(~G5SBkWg zA)e(V!9uCL zh#?bbRvn{_67Gd11QqMIvB8C|{f{YFhC$H>P`@|`lS)CzrBmaI0N<@b5C}6X1BI1x zML=lLbL)2RHU;kX{9=u>%!IBWQUnx`I^$w+w%#Qt3=Zp?=hX-1=>|c=>tkqQZ?u zC(UelTAX`p*aSxkB%Dj&irEDT-fTJ}qTk7oa+Xfg1j3yo%zkf=-~(dMKD7M%z3V3X z^jZhjBo-7|YU3}PyVGj>m9y#P_t!Ub?=T9EG%sN9N=kUyYjEiC?9+9XG}*I3lH+|o zD~Exef9Ge^!Wp6B=y?tK7>4Cx+OojtcT~EP%LmM+L>~aX2+atHooqF#9 z1;w>KWC1QWEWo!D4al? zplgxa_KfNcrFe!iF%pwNP8L)Dh2BFG`NSuThEc$st{>-~5C(b~*JhkmaUo+~=@u_E zU1sbR^03Hv4|(YBE*5p)UIUlUDNjv1ZKlXzg!uizarWA!(-ryv$B0bpc{J#NAl#k0 zc{7Jwz}#CFf}EN_Hf0;UWmX8IQL!pkWJ!{Y zkm!p9w7q1U-InNv&WhjRCph2RL;$(fX*e}!CE{rp!w?D9C>0`QQe=%Mq-#!kcYP-4 zqg7vG8n9AL4k`nsH$dk|(!BGG!;@TzseZP6ufrYVqs*gos5DYjH&Y@L2PT*)sW(0` zaWDcvyawFsV=5vxna6?IyJm0>st_ePSmhM8O~i%H?Hj@T^DJAfaYK;tvNeo4R|XZ5 zwR5GzUmV7<##^I4RMs$;$Q!A$47(l?-4I7HK*rbRbI9?9sxOMR#=u${nuiYD>!=VW z&V-6py@Z*Gk3JVY4G_jl9+#B3B%2d&x=706Sqq#wq(y23RL!O`G;aO9qlLHzG-6L| z6?!3tzx4fIe&EJdo&uDEd`|5pV&dI}?RO`xy4X7dQ17^r0bU1Xy{fWhN_BT8f++ zr{9|f*%tLyKFBf6`)YUg)D2U{TyfL*6>KNAkO*S2Lwf?;fAcRp3Nd{GV}Z7hoZyUu z9Q5U7+HM}-RccJll%x#}LsuigHMbsZ!5!@eYruWF?$8f&M~*-s`ndSz3|O#QvmGwj%zn_Ym@TAdZ5tP3mRUpauT0j! zet>SrD8CgbBK;8O&|ws9L5OpbpBE$pKJaKhR=hQQ@nCo_@KTvq-EE#qTlTM@(C^O4qHooA#^$6HQ1ny zNWM`6T5V?72SFT>S@KsFb5S&AaLHC6d}Ds3|Gqizvy@d+Xt)~>1L@Jk^MHr}0}Ne+ReAE!liH}yi?>#a40<0TURr9QZCJCV8i2fRuE$A0(hxfqO^giNEutD~kI zdzRnrTY7K&Cd=U%kM(!7kK?^O(0s z$twES)xYzY?mHi@{qU}m?(bAh#?QAjZY zbjlcGsuIWmwWC2sOHw-2tKknf^8iby*Ysouuy@m%GT?)g&)=KO*4qT0s{!Fo%;KGX zTdZ)Q)Kep3R#tFNFCvJK%bdnOUvUyuKidj1DU;YqzC1=!J!%QW%f4=CC^O;%u%+kP+c^%hRrtuQk*BB zKGEiZjKH3^!in#9gKiNDuB@CG+uv?ntG}_Pk@9c$nE}xNY{Y&Dr;6-2>hu?h(55@( z53OUI-_&L&@l=T(#TjGd1c8eUpauKr7;-p7W5Oh{=m8G$M2DgjO&pwY)26hEqp=t2 z(k;E%_Fd(`F$khYXqfp>ThApMZhz4}l^^(14AeV{@aW)4p??~MVIFV&yc*y%-6dEj za&CJjJqK*$tM3>WROZf+Z!X}I5CYH?#{s~99G%B@t1uXZABY9LEluye@2IAO>G1TI z+*NW8iH)RRGv81oTAf=myYoSg1R`1~x>(AK)on~9J?T{hOoF7eq-x>uPJ!c4h5h_` zl?El%Scf~xD%R{468w(oIDDW^Y9&;%WWpCPfr5ePT(UGK3mll$Qz39V;WarSPIFmR6Xi}rK0krw|LZ;AVZ}ZC?KTo4irryvjiG2dJy~~M6#rw zenilA#G84b^8D3E)y<1KCH9M7>&P=f0&Vv?$kX^2-m{KPY*-dP_`1q?et6My`z+Tn zys8GdLjutJ!5>OLYz89PnRi#NLGe9oXV>}sVkqX1(L}gvaiGj1QjXp>H`n#$gPpQ1 zo=Glv_o1rfeRkl6`e-F*@*9p3&*CPn zP`GQGT81qt4f0Awr}1E4usdkUS$!zd%YFWnFH2j)#41@FS$6{k>KYnD@CY}f^yJmP zBhULADP?8#{mxWQ2BNf|JZ`EsIGJ~?&{f;PG{^{RQF0If0H{P2-UD*oEKGV2ZIivG z?oeW?Hlv12Am6Wu^F7Aw%_E_?0s4mO_Oj@D0X9iPPNM-!(`CuXALGSQ2|`V4`qi4ExEheurT@+=vCk%E-n_ z`Ic$>*h(=Xet@5RS20ZFn=oJL!1fo2?VGU}}R6$4+4)VYl+YGb5arv`4hpOY1F4r7=^ zYb2-WJ)c#kYodWi8I*9Bjz-A5pXyZKx^V9x9-r2s!0(B_V=Y0Pq0pm9JCwg%%gFnt z3YY=npdQO7a)^PZQDZxa2q>&fhVI~?b+k2D+FF(*5$V=lUzHGwB}WB7t#m{aG7#BQ z2~iCY#{7C=I2uU@hl2eT#=m-IOWyw5^3+|+cizj(ZlNag!0D+%s=nsY2mIv{2<1PM z_-)SCSXPqr)v-ZuQjr~usr#JO$#||X!VZhKboQ(vVh;MW@jPazo@G_~g6eL4A?|Y< zZwvaMG}R(Qx$WHI-U=Eg0Y?6>lQLDWK}* z4ZTSZh7b0prl@^hfQj6f+LB|BeuqCm1ssd=@Ej0%A5v$&odf8OU>46AM+9wr$IA5) zEf4LjLOVEHpeIaU+`-K72Oc(67^DXtNrDs8M}ffrQ$Vc0Z3?7Dw7%fmt@*DF0wuQp z){*6ImpybI*kpyuWs!aPF{L)in$<_{brK5WeR%-7kGT)X)9ETv;x(eD%{}m3Di

5{f-9!++y>3$SNsJ)ii&lj!wun6^J+Wb?RNKYw zc>vWYH)-ABC*zHf&dy5G$fK#R=<*L~8|}B*wq<5_nDgfn;MGZ5@_7K*M!;nQhRgz> zLRoAfO_*I#TX>cqjnS2iAmK>)ev(yL?R)H5zXs%P8zSKmIF1%Div8Sv+lL-MwzQj7 z@I!!*vgAxUEbg-5PyWsnEb6T^=%3rRNpu1*P$J;G@6)(pbIPbPY}Y{3sUvWo>_`Il zIBIX;sA_aQss;m!okQO^*U-1*3F)b0WWHMy?h)p`z~zY)+jR{{LVa->l z#kJat&4Z9})XnxjSxy!5kt_1~N!?0DGSwbt^&zrhtO_xaZnsKgS_l@cM>or>sr@)~ zu?}Xu`mH%%N@^ZDl!L3w-#wwc?b!_@WAu1lO26~FTAwz{iUZtU z4xe$2r+(r~f2!zf2Pp{i1#Xd5_>lOb!o^Ec&)|<52Lzie`|-}gbw3;w1Sj4ywngnW z%YMu&CNY-HsO4zCkqwgEbOtZZ-Mcr-FGxQx;9NI8OWDX<18{|QvG4nb2F%y`uJLZm zo;=V=32wO15>LCwgs#&a^{g_DLK0xJpa}-4?30X%Yib0FtJB6YFUpek5G!d2F#0)= z^Ry4_GPVkx`8WC7v$1TU@Pm;wCNGxwi|*O`8Cu9m+)>h<(7qxWcX)J_Df|hDcKB#T zx_IieQ9-gq7`kPCNNE;0lIYRSX1iM&NUxsp!#-?RGpsyMU!G;|?#p_P$HofF=g%xW zrt~0VL;(nrXESIe$MJgX&YRzalTE+$JWh^fXXUqn8u z=b=DS^uS4-M1%0K$0CBkk`4$FG~NLKK!n~TL?Tb?Ily>DM?BISg=>DBU^$x68pR;J zc~gthb`J52ZK9jbs!yhyf!noR#;6n7<%$`sy&0I>0z+eRo$|2fTaH-%8f6!v>GE>+R|$iGew1 z7K8q-5su#1(I4I$n^}&#WYEAlWOwNO^pVf|h3`x^W5GoFC(6uSqCpV$kD-XalhXB= zPZ8Sf9cyi+7_N!Uzj3g5{#HRTD5sF4zeGYa`&UP8>r?oXL^h7GM_9Q z`T{ZlS}?ky!;8JgpeHd?4k<&jz8d==n_O4jk)t6IP;?if4-1vu@_h12?beXHs<;8@ z&sC>ff}|pKVAJKbB)i*>QRDKre7K$gwZ`C5fiyyOcMKW@5d$mAXsOjxlv=wZp&kr0 zB)<}jzunAkGWIMwOL92M_U$j_B+tD2P?4$ZFNPh&5vdXe;amC6xM4&4ZsG8uF|1+# z?Z4v38rnTIr8FKaJ>DiX6(IBmNtE3F#`6Sgya`<1eBcu_S;dNAW^RZ8`-80yPFV6H z*n6n2<8Q)m)h(0y)X0|s)-~m2yXd$}qdpuMDUTr4rDbjUu_RzPnN%>V#M#~Q)d(B& zv&5{XKHxm5iJmGIn!{+FsioX2MMJvy%@wB9uUw zsab5w-?L2cUqF-qgQC|PjCM;U>LMrqXM@XD*T~?-TQRPj0FuohhpW^q7-uW6 z*lJ)%6y;{gSP5QUo4bN@#f0s!Yq93I z5Osr&90#CCfjVMI0*y|=^n(jC4tNJ4nsA63+-YKC-!w3)KakzNu|F__>Pu6`;2h7B zEhR39><=%}V}gkj5O7+|m*zZtiu_wX%$QNI;1EQr4lB0p!M(Yd3u^`uqG;3?W5A#= zTT3rJ)Br+HSf#m$yAjxs3-qF?o%PP`_w`Te%V^h{{g!w5$=1LP|E=c);sc6`NCZHu zk^#xTJz{wc0)PF_XOkOVR>23ApF3}zT=lJ0_bPs7$Jp7&V}Q6kj#N^Di>F|Aey@Pk zkCoFT-bWjuo2kL=`a<<3EyBkxThF+XBJUt|6FyFV+r}hKPqJdZy?A4-A3B-u+%`P3b4)y263$ypCvg+Y3x_R$ouuM7p*IUR~1SK zE8TiEh9YFwHPo9^p70k@kGQrMOFgN34N?%N1Ux@Lcn{KnNEXYh!ZF+Jw+1Ms3U3krgvOC&g2VV0Eb19P46=%*qVYMv7Y0wY}vF0{1)y{5XihErq|#hl+>ZP z_@YMZ^q$$s#7yX@qk7M{cI(kBJ%Y-TdvOz)6PVVk2(-~#c*;=7;gG5tpZ*GHO4Am2 zjy3@SrtVQ1v+{|eDqdP`7R8kZtmXPN%E|$pIh%+aP4Ke3qlZWv-FH~ari?3yBwpWL zsF#)Cz0qdOXpC%x8QpSx8cnV_NYB8ndCs5Y;Sg z-LKV&|6pp3t@%5P8*8Q&MO;mJ_^fh0aMw+O>MCZJcG7VXQ?3jBohAlbzle2b_`3? zE~Q_|WIMs@{Xd47Ph?_8?{x|FYUKf;a(0=ps#ylE`Vec8I0(!J*!Tw*jV`Sby< zsIX)uVI+Qu%(UnBjdXGOVX*A;mSG_VV;Zlg2rVI%m%eH-2&0k+ehy0Vb&FCbh_~m+ zJ5zo57*K2I+)%#&kfnWMPWH7O-*7TMipldz0H0`>Z;8=ABp|<9DDP-eJUsZi?kGoz z0v`$%!*xb}byP2Ti@yieZju8wj9)-T;|Cmf)90r*Biux3M*Pr}_Ry@r&3$&yP*VdY zD+b1M!hI0DX4oCW zwhdQytFsu=Z$j~ODCL={8&&@@LcgRi@#79 z7>nFrSI#7{ic_lXT2IHNSJR{7!b#ZTJ?UDk+Vk2|KRmcd1ABXwKU(_SE-^F8kM?tZ z-p-wVq#7gnNSx3u`EFZ}$Jbl*^Y3v|LA(KK&QhzyF>dIgm_pS^eArPSl5&v2Jcx{4 zE$FK`RxmDnS1f3#jA4YLd`4|Qv%R3p(=R=G9D5T=>_D+{pGV=ehSG_)$8kP_cdH}t z`fviQ06PShqJ0ocYW3vx)pM^S?RcX(&ArW{u{R*wXtCB?_SO)EITf_p)u%@S!StHN zvL#~svJ2YG^#BTkOoSP{0LnWiry(wA^v0XE7mzQMH6Z`975xy|ya0G1X|UxBISka~ zt`fbquL=781Lf=|2h`KqwEFFuSzYMMe<;eM8uQetJ&b$0_^vw3ffNpHJ%f(q(L3m* z>V;`7A8x_EG3}LR6|zUbrW@y5FU)($kr!)PTC*|Z`_j(zE2ZRk9Pi)CQZtJ3NyCE# z?yCR^?-`6aY8dg(twbw|1w$A464IndDvE4utl zL(WT<6Z!n&EPn4Dn^1ouT2oizYS6yN>uHT8YABP zZ2AR*qh!Q9YxgB1AsTJ#{7E6x+_II2VvG#e{zypDS+qIxD2h(j-%~exIj<`%fM&)W~<;WoJu(UPbzoZD?z{Py8L-9WV+p=8mJe z0TsiL!9QpNNW=w4LT;f#>%>ys{Mpfz&GZ0TC}N_(J5)5GU}37-bM!2h(`p?#%dY{9 zdW)7e&RqP=liQ4ua#sxHHDg{_TK%zC`n5=8gg3gDexH!E8a$zB#+diO-c2ti=j>LN zPcf&v4bSH?Hj=`>!64g(<*hlKU_KE{fkIEy(epj+jD}DBDON)_kvm<2&&40RGcd}6 z1q2OnWR(|RAd%}%?w@Vm+_n6grVooeUFHOi)ez4q@`4WJJu+LHA)23-hrPB(DG#(^ znPtx-oJl%M4cKW$B7)q@jp+;8dXSrhppubOq#ZZmG#{%+Z=43gD!T zvHz7m`ew9NTz*4Uk-2~09bJ|5F_4&{&?H5=buc9vxu-V6LRT4*1%r`see2;RES@)5 zP0jHbHPy6FZC0J7h`eHy{6N-O$e@;6o(mFrIAZ1#(?*rVeXE-1jaUiFZDRuTNncH0OLuBk&yJ3J~mOrjz&xQg99cAPIQx4Gf%sJK@ zapc%U;wB-MTZ4H}UVR5&LPUw~6EIa8zt7pvEM)Sb%}*hUWv_C;G5Sc^wtZ;TaBmD` zXL)S%(p^9II)-6Ge^5GGPR9j?h4HiRdU7I z=~A@>m|VA9K;I30b- z%8m;CTOA0wP*^2O?;Jui7^T6tpQmGx!8RCd6(<&!Jr--j?Xx~IIRGJ+N!&7KDiBB0 z)eqMC42s33X!3z+3UQzr7gUlSm(7tX`{SAC31-bu{VA{I} zbPKB@g%mOi=VJexxJ$SD(TuXa6NY=weLEWgA~MOLBr66hp^X$Ci@0b5ybJ;nkFF4d z`~Wzi2SqNzK;1=lGs~#b)Vj6d#eMo`43B?q*2ztT*m7c;7dGW#xGK2Z%6TqM3Q$XJ zE@Awb-{+U?zh#Dmwz?d96v)n4eBH zx1^TwsN4W@m!{?qWcf;TuP;(P(JwZ&gJo&mLzVilYzk-LDBe#el)Dk!zEDd@9*^7(d&P& zSo;g9S22Q1E7}$=5mLj4GR769=Q&wo1;aK~%mnVn-H{jk2MG2j8lSpxSM(=|#jc_# z2=VCnXuZWjj$(zwkv-20aV;E#UV&a)#W9Glgj>z{{W2(9iOJS?NHgrDiOO8(h$d+} zr8CBY?B}Oh5rkIKT=tF^t;IXXmp&WQrJi^$r%H*KG8+6SMNuZX?@o}DpRGhb^kV=w zh2aigR5HZ@Wl=s#Rxj8dSKS|nWG{!nwki`yZgpX`s}Lm|BBUEKIc=FshJr`O_MK2+ zROQF{w#zb`)w`<=gBRr1NOrVCfQ&s=RF3xOSo{Z#fmhNwJqVf)Jdx8xlu?$Z z7LmWJK29$B0Ia6W&862Z@gA0`QNNYccnYo+i>2n=VHplKw{i#D5lG`e5n}oA`r4KGp1SuA!R2j^dSp zODJ7ogW%d6>lNI)4#h>NrbO#wONNnbFPTrh>1bzEQd$_JoWEFb1E=QlJI!%Ke~=Tv zez5G8f`bH)EBN>RgAz*V)dWyqQLnZhHW5! zKJ4e?`VJFl=Ss_6p8*_8K6nh1S~HLEdOHi0r6O?l87D^vDh@ViO33nUrKK!&swjAO z?(q9kF#lMjJMm`IioOM41`&!K#sIFmqN6`?%L@<}ytY%ugxnM6wXI{|-efBMI{Wr} znlg$Mv}akqdwW7HRn9? zUV7z=EmJ*r^uADolHS;y%&q7xuF<9q8QxIEiTs^(8V^%792*>c5g@mh0NBpk256WY zw*uv@b)=gWafafNzIwUjtCfW&1iTH}PvC7^coj9^epwFMf7M~2;+3*}#zzylC8E*C zent)PEv>0e{`Rs0^f&LByJ|$dwj_R#{@&nj`VvSmWffVI_165|ee~I@%q?&g%TH=I z3vL!s4h>!6vf0KhExApAxi4*c5P)M+=|%^5WnlO6ZlH~N6q4@S!xJ+&hpvK-H8_)` z#eaFTL95EBzEoBq-#0Xeuo*#-a&uZVdt1?X;WpcwEMWI`V|o@8;#O`XWz8em1;b-k z5kFcDaSk_$ZHjjzjs@+~vK)<4_wzk!Ga|)nOYUFivo;KTl1J-L&W~%zhezL3i{&^8 zVqAp6->uFQ^z|0-0thVwt^WP0F1M4g1Og~Gp40w@Sbm4Ii%2GuQ0g^3P|SsuPMaby zE2YF@Ut~ILYfUZorCLzG9ui>LiX5t2J+7?$DG8l4*qy@ND8`=otV%}>uEZM8i>Qe7 zxEkH02%8geM^%r1I{zE{WdbneB*aY;6ofZy| z5D~C`mbKEGA9>8|S_qW7z8nb;z4o$Hu~A2=EZ&_>rBCQJ~!wWAX3hZuv zC3d2IshMbTt^ZC@6c+wd_-(!{nLAet*~=Hl?TzSpBI#|(W7}v)b50!oTE$PM z4!`R|eI${rzA_E~l0&bEsu?nPX3Is^IEU~1k#C1(mgO25kX;&q;Yq7;%lYsebinpa zB_;6e<*bec7S^0fNe*Os9>;zmpS$CNNBywD{$@Gxq2X$P4XbDZYkb(uB~W4pzN`fz zywbPOkr7~G^PY_Ajw+B4T227}y;@JvU)RN4%zCC81X&b(f zJCX#WDr$&GKIfCb2+SYl(|GzN4)1sDliuV8g!2}%CgwVQ zDacgelIqXko-Ui!3%tU>Wa*$08=1KkK#6Jqu%3E~x0Np^TXYRW>huiNF?JNr?4<8k3!u<|V+cvg3s>no6#Pxzf zu{#MXiP0_NkO2r!-eh4#Vt6rN6ICe1x;Rc=&#QCXGaE5E>?KRQu{VIl$gt30IqGZM z3hnL$&Qjw$euw1B0o6Xf;ZHz7XV! zC6c_Q{z+`7iNP1TxW@{bXWOvqTs{Is_HU}XXZX_Jq$cGpdq{g6Ewpp#i|Er9%Csaa zi_CcEpxlvbh|>}*?yr#X*OXdCd61L?M2G77!#m#^RX2ORiste_aj(gMpCf>Zy+|8_ zhHEU$ZxH=ZSTUaXx>;3t8VFck>JojoJQZA;Ozh~XArOSGxk(O_@0rBxKHhHfoX^DM zIq(kr9G@sDxJqcPMg`2}Iv~`2@S%#8+-E_VFvN`&Jt$mHji3qX0!YZTpQ{@ver1IB z!=uAO_Hxs5lpEU{jgX=YE;wMR2#FJsUx%2Vz$sjr4|_)kH)xhCtc`F4saHe{jQxPD zh4QyPmMP{vxD9o%N`&lK4c705ynpGmTrvEcs_OH&T%>+p4lPgPW%>0bNUA*}ANlnB zWZtgnD!--8Y?V|>qIC?@(_7;*wx9ddz8TKgIFAAMeLZf$atj12i}HWhX&(#y`;0dn zPf3_nz>Jxlia+hCNiE^Uk#g$qk8OR4o7jqo|y5kkL??Qgku}kjEpe z`%rV6%{XCu@Yg0niFP7m>^6)3W?yGRud}DCD>Yt?-vwwOKQ~J=&!MLD^v$C78hY0( zQ6^sxSp)|;W#!Q{A+;w}gR(k^jJ~;L`p83=@@qeE?lk_-Of~9-ReXnFJ4p^i$%d3_ zSd>I|-1w`BevKHnkZ{_dvsx081FJ?24dR-@*W zVC&rAjCVYyYkEd&D~$JG4NLDh4qI0SyexnLHrJO2+kh}B?iKzBhYBAh8w1}*>1i;B zPpSZSk2<(3z`c~J)gqX>e}c|Utn^P>VU4v-JH3xMivp5mrzQSK-v8dXTBFQ08=N_}Jp^b{@IV3%8iS-gFEDQiJMqTR7;J zG8*RPTUb*xrXV%=O@7_bd|OD!eyZs&x&z;7_&rB-Q~=XoAp1h0P$WMb>%r#`rPy@k zY`&_|4UaoFKPXr1sBA9_Ha>UJ6W`l-q?9N9j?(DRQvlwjZBr zfq!Tebwzt7Jl;t9-bP$*s>b2!0>SkHpy`j?%bKDS*{uto7!fs&#M^n1zRBnIs|3bR zi^kK#?L(la3$RvzauEO#D7WkKt`-M!=|e(45U>|T@Ln5s-(2z4qV%(t8YC8T_)&-l z`Q*~P(q@MF2oK;SCev8qhHc{iU>t~+vx*Ved)M1mZ$(m3qJNt7CF7G;raK0` zoA~-UVbN^qP(R7a^!0ZR&Wn)=Sshpcg1mT+oker9v`;FaS7{+ z&a;s663%0=FBt@K_%Y2=nkpsqY2T_Yb9#~@^3+Y7pN81GKxH2GHrQ9;@@kz-2nCc$L>JG}0ITAqn z6vo9gAs8yk1({2?%Ly2lhLbI?UY#zH67n!sMuYqfY@q&`+Vewt=JXNd-&T6T(X6AAW%J>3BVcVJ)+_4~p!i2Tj;L_L!Gg*4n{gR`vr%@EUw%EnCyWnkL zeWo&*TOVe}jK~|E+JF7iMHTSn=pHb3Z6O?@EnhlUt-JBf9U?UO>_h>1r&;tp5s%|* zAx_^!Hg*>K193}7%@`E_a_ToILO4nDwtTs}*(;>BBq`%Jt9?*mFm4i{4$P*t0MIQL z&a*qvV<@pCEd$DL!qa8oN~+Qm4nQ~>waX-S)JIIfu#AKW{9IPgC%>P@J9(OjB^3ba zWFuloYJ7$mY*`yDNI@SeanAWf2H|;WIf|kjG=|G31+|yhay)zg*!WmH^)Nz*+9oKG z)j-(EbFtRm<4~}A+#o^4IK}{^BbSLWSJ=CQ-jm`|R>nGd{pVnDP}2b($ai}u%)^Q^ z0=Ra`s;RQwwtA=>rJNjD6vBik{3bZnAH7EGXqlD4D_AP>s zwDGF2@jj*;WB5_WC(P)7nJ^lIS3S>G! zb4yZGf+o@q5f`Iv-(_XY;dkW&>yDK$D_`*3qjzF04Gtj}Cn1@Ua95chP!TNM^Cni! zXSw}7jvy!*b1e)%vWyX_<~91%0~#4x2hghPGNMVvX=r%@YpDdStwi#2Ho0*gnw8XO>Ywtg6)b$miAQC==679 z1;$Xb$H)`EImDCXD#C* zbTK;+RtmM>?Ih?>w)(CyHc#u6uLSJG_2rRrQYJ}Xwr2=+7W3yxOqq`Y3JSonC15I1XvRgns(<}us zA}*&qN35j$&Ar%na0H?8=6bq=z1&gW&a$cYJt#qgE`X{4f_;#Ce%3-34%I@ZeOi*N$rK*yB+(NeEa`8( z#wWLTh+HND`Tp|H1Pcx=RuYXQY;RNSA>K}8Yl@}*_bm_xpQjAFDq$^5s1fF8E2mYltIeM8~5CvGK6iglyy+qf7~@4TZ6s2`Rn!bUjQOZ#jZ z=XLmq+g#eOgl0!VZouKGz%u%oPL&s0eS9LaM!$df=?4|rgA6>bkHs6UVj^!2-O~aUiprRNs{$-`hT+rkD@t*q;r`HDT;y4!FA#;C4rYR9VYm#a7ra%~8eigB$8ph}o zqiG!3sFH9Um6DG7QBYx(+)qg18KjuqNu!2Z*?0JZw$^46agKh=+I#G`$_k#H-TC`f zp=Fo^!$s$zZ~lBtUEO^jurq%7W(8c6I7IFMsWw<%$QhQ%cYsL~A65SuU6CNs=PZ_n zL@+D!_(^liI<#&+>}7LQboCPg=~xI&(MrY6vJ_fKe_L@k#@C3d?xRZr#XwksVz_6u z_azCKYejZXnb7sxMj9F>l1I{ymlI+Sj^=Dmc=S&xW9rt?N@)TUlswwlm;4VdKGVlk zH3&Qlz=$FX<6z>+RXP4e5x45Hb3X{tgzZh2hb3kCUNUj|>y;Lf zDdb|&%_l_&OhY6?JVkZVMcWM9#Fe(+l*d=YwE!=?$;Mv3iTkAU=`63hRFiRl=QkmHv&d94wDqX8||KvF%8QhDD1lo8MShPj#dqrX!)PL)*YH1i)v=dI-^5Jb@rA`l)F z;=T7|cyERO`eioQV9uTyT6EQ`dxZpVyhgKl6n~eRL*7?Lp(jw9#`2P8)qs?BsaG*-MccurjO1H|V% zmXEv91gzqH5hUS>30R3)+}Ds##aH5Xx4Xvgh{YWH3uj<@yY_8_I(oEdS%lTr zs)P`^GJ?+y!7DQqrFbv!r^!!ptK#L9B8Tqeuswfy|B&>S_zCgsY*NwZSo5KX*50k= z)-Ft7`~CHQkP}Qem20TwIzX5ex|CV@WXGO_`oF;LedU{CPNr1kG8FITvX{qHy!>_; zB#@4rq3j$UA~bN!C=9s>{a|A~3r!!gv+z2-SGD1aWS$Q*5_Y?|Qx7363xdWz%#8zN z=gXV$BG>I3wh>fu-%^>aodj6QYn=A>*-D7$@M51KmR9x=Ix|;2-7mPYT;|nvD-R+$ zfO4e6(z9xj5wC92^7uVNG9RW*-WXJG7(-?-gjk+|VY6JNHs zb$2rrYJ?pUFRQw<2ZQDVdJk@`=z!Z3paJ-|46$6$?VmJnro~Ova9D=?jFCB$FWxX1 z{BSH-`d;3U!h*pZ00}_$zn(a_H#t@+YB6+T$F6s!%%ngUIVxKj4V1NUKgdxyYFS;@ zQxY|q#G`b4-Ka~+g`p(o(zfNQP%`piK+_oFud)1#UiJ$)Md%}~lYvaFx!&iOQ1SU8ZVnmJfZ4a;VwqCK zTI9>%@dC$irCJP^TbwME@#rb>Qj357ib9sz{Z`Rp&5O{HD;*P(U83X7&-Oi*OXPU1 z7t~W?r^Y*YwgC@!mMaTaP&x84d|BH0duYCDg}4#YL?@5q;H2F)r$vsW>WSJr%5*wZ zJ6vsL&+X@~q`@=%<;IcRS^ZP_C4Wy7{Q&^md<#?5J0?MMAV+NlwBs!4J?0qlL&+!XNkt(Yhi_*L{97L_&{D3>1wDT7_zLey9BSA{@)%G4`&7Tkb)%aaD{Wi*6Xa zCLR`~Xc3(Q6l96JSI_0JH0sgMt;XOXHbAG&lb!xByog8U*Z0ftv3iq{NM5lWwqr8j zxv(o=cm3D6O9j|9bnl)0D_r@?!YjBm=nAh7|04c!6y``fQYiC2gI7Om>fbd6FFa8N zt7bPiF4Va)W*)_?>&y%QOnz-Oip|+dYcyPEmB)mhA5U+~8=+X)Au# zSQ`!L&FdI5W~YeYIGb|(zGQM&gY`J<-B`?adoiLb9gn_v#M6cp0&E+!V^o`N&!fS+ zmTet0BR59XG2fN|I)PBWDTJ#xHt^5|0(sOOX7hzZnpD0pg>Up_+*muueAyB73$qBD z67PEQ0yOjMTC9MpZ3;M%nH6W~-Y$56$H7%J^UE#IV_3cA86&+Hp36$cFbz_ls9A7l7XH^&0!DkFb=F4Nt-if$V z+0$I)$(?T=t4RR{?wOW3JmCBx7A3H|n4~Z*_uaN>B$(zbX+;c9X?pdRz2&ngPrZXo zHFtr5vCBKZjpbNa8OrLpmqq&r_qo&5eW*X+Z}7fAk+&9wv|gb8Y3O`eBB%kY1nXbQc$Lj&Wcy4y$4IYpXg{~C{eBLg-8 z2K!7HZcCpx|0k8BqEx~{XZaa|{CDj?#IQBNUrH=6_V1f!S)oeRWG;vuwpHK;dv{Yp zU7JIz+v3{8j&Adt)9X>2lTFf4N`u`=gP&rW;T*rJ z-bRo2cohur74PP|*SOo;5hK}m0ZSnD4aT11ysSm)n!x3YP^2g5aw{J%PzFwDMHmMc zc30rbyA|d=vR^-+-#MgOk5If7k^qvzrETUkyWl#LJ!jh2*ML2W zOcBdAK}Quel@pCRhkd6OJsw;uTP`*^r`R)vzWessZ*XSVJNlKPs3z(sZ{Cx)FBDPs zzj^~tD91#T@<{;s+B9yNWew7XQq4Ar+BF69^Eh@{lVHdy;nmHT1E+v)sNHs>4TGWR zUl>=LJss;ZyS|m6W$ov0FZHKGt@--BBGL!O5$HXO)Q_8!PlMi>`Mt&)t@cWY5BKO; zdGi+0uXG;S6pRI35koWcY4_`G`BqI2|GU8y!Y4ilaKKAG>2U0;M_xTQ+n*(D{a*pU zA?7h23;JPkFGg>MikyujBwyqR1-)Z(bCC!gNW4_%v=YjJFip-_ke!fe%LgPz z7-BczRTEEhw6=dPbP6?0Eux^N}*+obtNA4hT-dWv(g-(D{Gf(SS~^xGvM z{~3q`a*yascg}$Dji4}JW~t`3b{-K`lf$DZN}_1l=6J8i6|`8=x}G!S!>zE`_@{g9 z)B5bikbsdc&68v`xi_m-iP~Le7DsW>C@5Kj2 zfp5jk9efkKn&u1k6Y*aYTLEo%O+Gx#ctamldjFn{+SeL}&7go`AC!(0nzxE zYk7U5lE&U`prltGLpE_G5Fkv6T8BmE&OFcCKYpiW%HZp2S7_*leVXy|0b3La)1S#f z2zSm~p0C3?9ROh#eQm_Af9avppWFI-Uksg|jC?1tH}t;M-!M2h( z3Hj?>gm%2AQT)a)^xteOsE5r)J(husfXSbkdPTS%gB?w>F#l0$f0)xwmmMCp7G9Zv**(&Mpk>h4s$kB$$=OeB` zg0cNduJ)sc*nr}-^d-pjz`(-7 z)z=r0vX~q*xtrKZ*Og9W46F?r4CU=!Mho}W7xd^GXb)|&>HLM;6~85-Vjfgy5zCE% zU_0@j{xIT4y0Ls9x@@RJUHTkSC&I>SI9n!}M}--eH#5X*t(%JsL_pQ1@D{_5+d`(1 zDdd}|Q)tSb4T#phQgtQ~Qk_Out0&LrQ>&ug`U(7tVu`Zla~(yBW4nSox*t`KwrPUz zh!f=iS-Oia^}lCrFJ;XGIR^kxvdP)P-uyX89AFPUgcpNtJojU>ZNs`hmkap{UxBR6 z+LvGv2RhLeuHL-$d&~7Bdq>~6F}9Jkx3L}S)Q!&DebW>6kRIlzp!P5LIuh~raD;kW zVeLAfd>273DFP+%F9I{AmRH)>24!=XDkq*?sw3Vouv`xFwYdW?v1WQ?@D1O8x{*LX zx}LRSvrcP{t?nF@xCafGW@1%~56}um$|YdY?bj>3cBz8?q|y5njnV|w31TQXL0+x z;}!m~zteOv@AECQpO0i;Nyh8h$_P4fPzU{)C$p8M=mAZXCvoPtWO_D)oKfmc2a+;2nPADI=&PyDm2{2TdQO?++ zBP)jmDZ7}RP;ya4@RgRza-JMQCDOpLdz#o*x|_Fs`k^_62G&*dW26GgWCz$$0e-J? zEwWUwEcnI1{-hoRz#;HEF6=uI4})Z?o>+_^wIWEXc2gOVcIQf&HtP;^kM*fdg=F8Z z%oFw#7W)dAvZ?RN*u(Jc?q!9t444(`&s2!7j%BCs?rCfqqFfVwr?qxK-4BeMa zb?AC*l*!#?;xZr1-?fOCWXEIMgV`LZ-`g`CB7%h{I6x>Jwf^m z$u}l?AUyN}Ds$Go0;l{)vsdLbK{5WfDCVn$0aX9enV_FIAuoQQz zm%L)xA#mE<<^0IZF#1l{Oh2Vr>cTaN-_4*kY7#AeXJvQKD(F#p^Zk0JCFh0iiF9G~ zDV>fs^mkKxlrr56X)cIN*zTxA`pj~(T|R7*WkRF*uDKN_%P$2ji$T(*onmo?GTt(` z1b7xPk`*^|CcA4Ezo8FgGt8NKyE0$Nd4Y6WeVv?r$%*N=wjj>_m3B0v74A|-D1dOH z-xQURdcRy?(a9tSBS=RT^dsKL3tfhAUK08)No1$;=tvzwk!v(_P59C%t0zPHNOah_ zu&)yZ$OZxyyxG>a_iXl`WxaK!I_nb9Q9O^J3=Y(s4}BwAz*G&nN3i#|A$f!H2IH%h z5%q06*bM1A42 z0?{6w8T{Q5>Tea@bj{&>o^v}jbJ0_rd>J^);j`y-!zG5OD>jh8hdCC~RQgJH9Cv%> zYm2PQ?|&yRmT)69%SZE@8u>Yw^p5+%%v_Sm(nnyjy zqDu88peb=PVx*KCxEd?Y4nxD9{qWtta#PP`5a4xEoCL0VIJfNF%pNmkIuDu%~G?1i4#lqfPc&Py5n`BOdqVOttfl2tevqU z-m6dxwoRaYba$>_>3%t10$?C%RuhdD-hXpRztQ~m4RFK^)^YFmuwp+mXM zvE z0?~k*$&ZWI%{O#m59C~q%Zf56GfbsuqWrO1P;saTHzdG!One74dcgjyt^ zLX==|izTyzTEaKyXh*A!oEJrTzl7Xix_8HjgFWx&;HAraLtCB)n$QBQ15*?TTAF7u zMZ_DDBx<01HgPMs{kC(KJumPoo>WRlo0~R1?>?EtSNvX`Q5E78)#ZoRi0^jJ&rGLt z_bd=|p-Fdsb6*NkUa!5s>AY+%J$Gdr0hYXYs3uvNr52={`qvQ6rnO|+12o#>SHf%w zwNBOv4a^F`L@A|IySig~Jm6iT`Xf9@M^zpS?1kjV?|1DL>UX~wAk-AVY-KI~tmBIk zk^ieDhN{wS2-Kt#Eo*A9<*M@)!YOt*xXxHCn?e|fkkqf--{``NLP8u8qVylRkmTEt zpnKaKbkzan8@r9*b7==`mdS?H?}nx#i-cMLofmOgZ4DzUi3V}_{uXG@bE;rQ9*MLL z01)G;w92je)r`Zwh>4|DaQJW?>S6StCRaUKze?1suN86}FV< zTvEmSUEn>GGC&^3KJ0RsUf`Rji=jE%H#B#59ow@0H3rSSGRt;0P&Wajh2^f;f48tZ za9OVO5s)i6k6f#xrAKYOEdB_o-YL^Z8XTD{dlR~CORd%*m@rec z!GcaikP{T|zrap@7D^rB^OHj=PhTbYjNSNZ{`mu5=HMs$@WFyOu3pDuy*W6ec=`&W z2S0_lv+i`(@uAl%?D}qOwtvYjd5YKbkP}ivsVIJuQDB3vNX?{I#B9wwl1`J`{QYg~ zZXFT1*0EU&trPO_EpjMv3Y-Qy2Hx-syGRBF?J@gKzr8hT`2L$e{PfOaut<-=RQDUo z$mgqsi@9cx+j&DS_0P4PLYKBs+}3j_lZ_qbv!GaD(p7NZmmBJ=T(vd_=m5_88pXDc zLFo=#TeImet<+TGx7>*5I#rp7>NJ>NO6-ugVKkUXRaSgigqpMpy45$0 z2|tB^An1)PQNUK9$#z;t^$2`$WH+Q_~SE4Yny0==i+1?My|PN1=09 zOA{oq>tWC}FDGCub6t3g1qbPS&QxZtP0FJzWq$4)2GLjAqA^O|bEoHItQ{Mfm&=J# z5=bs7IBX&ZgurzRjU04t-F+Re`x1kNHOo1BLqtw1pOKuDJCOUm!r#!}qi4p>2Ql*S z>HD^0n!`>mDB>B(j0oQy>I|j<+e$R4Z(utby>P8^%#NPQKxW`~uEaC3Szqe5V`#MPy zATnL9+jn`QJkdOM#h3a8^dENy`EFQd_8Y3}i`y3km0r;#=G)%N?7_|?AtC0IH@mFg zv-JHFg2T4zN^sDjVpQh})(uE3}*Ab9dl#EZaK(yLeuTo1WF zCf5!6cm4eu%VS6eGGAEPvy9k?mn&epL8^}bm}ki$PdT|2FNoZd)L(>)D%jRf8q@kj z&O=8cUB3-lPVuEe9IkvErycjwTw03L}W>dsE}MFvmuP2I7j z#;ZESm(eb0oim*f_X;jH3e^=tRo|McK9KUbk^n;EI$u4P3JOIlX?C)Im%)dy-c5~U z%R_+J<9V*j&E-Jgxv~Z^#&K6ez(Y| zgF6l)e+T&(S2FfQkhkc#mnm782jB!}+o3uoiz3E~t@~N6R@@!FhGmw;ML{3}Sd~uVSXwBfwU1I>LVzH=eGd<~FM>A1-r-G)EFKR??uZpjdlgk+OQE%&Syek%xYiS`{dOJOvTNAZB~-vO}yqVH#i@r*~)@_==Aa5LzT40a4z z8ANAvOFslBj|KRA3L+t_eWkar7k3pXRVds#GP0mwsmiVT(ndkOm{M#$!-{uUE@MLY zvNep?xO8!CZ(-hgqht8us(5|ZS6*(%nuAdqq2cd?f|~U94yYC`r#JC!HA0U&8ymPZ zxcXAA7sw3mSUn?@*>;QP{f?z_i!!$jst|pH$Kvc0r60Vfh#`UrvIS2;GuDIrL=Dxj z)f##udeHzuMz<%|$(+H?d+e~(Xu!Qg8K`sH|F}H#Dnrz$^9|kiz?Cx+g6In6-#0n0 zzsJ)7-irN@$q?n0D$@jklZKt>TY?$RSi`k|&8RiMv-SDL6D5A2lX6QyHR4gKU!G#5 ziGO2=f0241kA{zUJBH%mTxw}RjBoXH;yJl~t+>1Q$lk{poo?Us(B6F-_aaXS(JB6( zd#OHA6n7|6BW)N{y*GS{tp0XVpMy*6-Cs9ygTooPX1u#?`KY7axXDRHbjy-!`}ALU zN=mm&che~zMEA2I~e=oktH8xlXq>?x8KgL@DBij$X2iQts}H#;e-2ItG{pR8Gs+gM{HRlg=vKj!Lfd}6t#!12M}moGl(l}cl0@EsQ|#A=O>5l3 z6Ss-8$k_hEJI{`^F=rmDJndLLL#KkvgA(E+Fp&VB0LS#YEMqvO66g6q8gB7JLH=C4 zR|m#%CIvfv~W~YmLIZrp!LeVDIhuo;~LlIpMb-c%U1~bnuqK zXj%IAS?iAi4iDF3dM@62-Rrh-QU~1YO>NHL$jS{~#O}hN7VIO-*hb0Zi0}u0nC}PA z)h%WBvYIp|#EA4gFwkq}0KW6>g(c{gY7Mv3bLnc4P&Jw~c?4ZURDfpXN28{mNXO6bJDSMwE4`NE$BI()m?oI?x;8Jfm?Il7wykZn)fw zz(C#YJ_waNydeV^$q0|ESmJ)wxE0E>e5V0^E{dNUOI#&A)5)}n=?$B+xNicnJ7xx3 z2dB?*P!f2xxu#~r!1MnfTVSLUqB6qdXoT0J6y!@>6SO-wSj z28L8rFGgi};I!NW=|^e5w6ar<(%lII8?@2 z6Mm36B=>VGIJ zCtnFc?MWLXvCt5GGKDjko5*e(ILn47_73QK$o+=ggNF!uvD?qdkq4ns(C3@Z(E1}- zcI&i1kJw-`6JfVmp!ZP@|T)HU7GVe`md@g+l7c<#a*+>1J1j!M1eR% zp*>w-Ur}M)MMYreYqZ0z{(gO~34a{ble>lc`x;{Jya!d6W4BtLb#11WXfG0#1~`RG zt3D%*t-mjGiALh~GkgA>8=8%r`Q57ePNjlI%iv6HS#WQ1qL1>!+v!IE9@?pQ7Cn{k zh>r{nOW%6rLiFwWM3b}4LbbJsna4}VMy5uvPIX8!r_pVR2e;X$*qSA@{5GP{va+3v21-+}y;3_S6;qFVO`SGc+B8~Jd7ICh^R#aT)8 zl;kDbS<0-44Q36UncBTHq!*xNa(}<1QjSIY& zg2j4ZmwXdI-W0eoD{y*#rm+e^kr_egnVI>E&4YJ=5zsftP1y_=fNz;}3Y&c~AL)Qr z%NO@GZT?=>o@8~?3)w4d@!rZe?nq}=Zuf%ai)OvknhQ&b8sspc#;zKV{+f5=?>2Yl zwe99+xtJCTyb^~W43k`vj19FgT_@e|EYY&%Gy^!W` zs5jj;E(*~2Q-^XO(_NJ^Mw+X>CJ^@F&*9c<`?X(NH8R+sw;542AakQ1kpISZSZa*? zzMc1sGm`L~NB$V|_&P@M!AIdT#d)LrchFE>Q1~)V4$|n@z91*B2gaQ#Wuz^B1$GCX zB1|1#3D<`-7F(~oFnlD#*m+Q8mP-YBZP?2&~IH%s`r!PYpoO~kbe_@Vdq+7 zg4(IOWe;H-KMS?>kSm1Qfl0zq;*$PAL|VaSmamqfM$!N4K4j#5cabgx;skTu%obDX z_VPjnTE`mpMso~oc8Lb7q9NF=A&{vD9_zEn+E2k?3F8u}2m5y(SIurhE@%5s$Sb{i z`83vjG)k57L_z`|Q?87Zc}2g2WYin_Vla$&#h;P#xJ`4H9A&mMoUf}7&CQV=J}~AI zYxi!#pM7Ng_L;88J5`gz#mGWT>cKv7&70~@2xGgyfOL5SkZA%#PoBg*uge5Qce@^Y|d+=!{?AxFnJY)0T9RAj}6t8Ie|KM$qh& z&;=lwULfhv&*)JsmX@s0bun7AG&LNbvH*0V%1*XKVYJ0VYo7MIP)czSs2DyCF^IR^ zNjfWr>>0Klnfcu-wZA=G=<=a5 zQ#X66rT#+bB7Sf(0z`0?o!-*vLIM57;1Rp<5G42e4R~nm|BNg=byer z<-M}STwz@OOyF?iqcn`d>;u94nHx(-Qd$G!w8IU)q2Wq=Fv_f-p6vpzu9H_m)QeU> zSY{gG#bR;vsnNx#KnQytxL~LoKpfuy3qsqs3+u?2GCsCDR)c6_-pd9)qY!;wE zrnnBskD*`=@O*7741DPCamo~~f_G4oxv`{@@WPoV8ia(+MUsRnS*II}l%kqdGa$dx$K@^!J`x*YyBDzajHunXwlDkf}x?us?l&6ug62wj2Q7r%ODy(UkG zJ@dv~2Gnk!4l9hmqGcCq=)mAgO9dg`7;n;-^YPiQQz2CL%lo@+| zrC**K*y2K*4pAne2Aj+P`CE^jY0@@O)lru>>e^LLJEP9NWpx-cgP7c*-AGuf@`A73 zIe3(Caz`LpnO8hZbH-ycWSycJes@L@5xdy{FhAWR`g>2}M8gL@7-T@!t4)sF79-vEP8t2=HB>$78;VW{YnY?c#LL%w4sp z>F?|!ZiHt??kdyg-rW}}6${@SV4_g7H@}~0yMd2i45eZ~UsU|fYWTc5B?P~R+`P8g zK4W8r#!+yaB#?oWk~~=t%97t>F9YLT8+|o8%qX!qD(W{XI`m``QB{^SOrVGzlV2iB z$t>?Fd>n~~1~tKF=<)mUYpyi$KpKB2zN)R1Q`FZ!O`fpXrR9R&u-(Z4^7~+{@b+@$ z1aX{Bl!A(|AJw!2;xim>?#>}iWLfkHZ`}cCE8$BN)D8gH_WoqgC4}5LTy<$InIi;^MYu~&|JE=P%{kbc@Jc6YZo{rSar#rA*ta%0+uqg1eZ2d(MJcqJUB zV!XMuRT{WfCWfd55Q@JnAGQ3Vf&3ya^9yMBp#us%Uk}YfW%v8hvwfxK92yGD06Kvi z4;_KbUXfdQhW0xuB+9otbtY1doyFtFT{ckPWn!Pd?h+V>9_z@k=jWs>boA!p^*bza z*ZKI)>g$0_h)IRRuwS-$Ta|Nk%Ut!R8o|eqW`Si)ojn7gmvVN}c2DRpUN+07C%ZdE zC>|{mdXsVb!{!VkGWFJu<;u&|S3}FUBaVyI|6UVz=)17nMA3y){0;#Hxk+twZ=|VK z<4w04Xg8d6*~8l1k9D;B;*6Rf4cV*z;>gRRN>*>JNUW^mxbN;R-7dLWr1}a;{k>|X zr+Pzv2lFCkc%WadqCYe;1lr2US~DLfm)~ z99@+@XAuFHD!sx5cIh(YN9OE*a0Ccj={G<&)YE2RwJXEkX}{C?XNvDn7UYYfXzkxr z?C;RXwUhZNTqsH*+rn?WydJk&SgD}Dw&yFZZ{x&wf#MLr6Oza5e%Qs?!~D(trITI~ zT_DD$p1-&eRhqu4Bm&OH)ykBLRq2mG>3A34DcQoETM0+8k3@6TPQkViO>(cwxady#ZxTbjXJho-rG zdM;)g4$}!aB|d7vJ`PHk<+sGJmZ;{c#D69myjBH}bPDe02&oaT(QQ5bU3@X|g#zJp z6g~!qeD6qGxR)PEe<7P(LbSncuQ1pPx@8pO$v**MVcZ%z7b~riVFi=~B{R!=Ndl=O zSs{v8U5<}B7YZyHgWyv4_snIf%TaB}CI4lhg;2G&F%V@WCCL>fvG|DJ=3d&HJv#is z;1{)G+aYkrTU!DVUu{Odl^8 zpip+<9z1jLfFEa@ZbG*^7ULzGg@55WohlCg35QILc#(|o@G9J1IR-|*?h7XJZuwGx zR@SEKwt-PY?jQ%B0_62Wb_9u=iqDEsE7f)1*!yKHPE9iDB}?s<5B1DIl?y?ZV@ajT zakssDtNc2wJs^`a3(_+J41%w`u@}?eDgb_f*aDuk;FZ&8p zjmzrPet%a7U1o}Dyh^>IB2iXBcAOgKtCzM?W?=3IMlb%^6y8I`m{+B?nhw=V)!Dmp zWz1-?OgZBvb@C&m?9BlAWp%@SGop*YZ{VTLC*jbw!J35D2#?XFdtEk~=${Vge>V>Y z*jrnj_I5f$$N;5NpKJHofWQHM3uXMu3BaIV$iE0c@2Bwt+eLA^r`8tFnf6Ty zRH-DM7V%DFE$kCfCG@YX_u6?@Thm0(_f;&-KIs;elwB&&I4IYdxcR3m;md``~^M<!AQu#3?vZ) ztWKrLEbKTrRv>D0LJ#0sArbQ7Q%jg;>&j1eez_uv4=<^*7lPP$5bR}5sxy$=%Tvwk!ec6(-PaVTz*Vg;j zanxY{XPs(Et%fG82J-ttSMDF99dTixL=dw(JTs)SMku$eL2UC?v^|x*$@1`zv}RSo zDnZgzO~3c4pP%Dcq`5Wp2hh;bf~Kgf!G(w2{0kYM*?;Bl+FCo-PdP8 zu`G>~91C)#*akSAfli3CF&H$?y4BwyN+~+Sqp@EQ*l>-6F9p*}(7^tl3?Tpf{8tPx zckirIy<=_k@@#kpv3LAAJy$guaytIboCkYZD9bMhqsq0$iI}S0`{$m*BO6co z236Dr$mskOOX$633@n_jiSOhlJ!VdC*w6SpRGURF^GUS9(jtKrydFv~%(DsKKC{!; zxqa}_0o(F(y;>Fs#BY!l_q014^`#Vta?%o%PXhe16#%?OgiU)x?T+2osSO0{gjRjb zG#DV%>JP72+Wj-)a^+rmUX8_#heoLP%AXm>ou%f!XK?+I(Tj*4A%&7#Lc;`z5Kl@P z(a2o#^9z4%Tat3?2@~L+seSduL`ycju|H!-EQ-^5n5&eQyCMPW&P>F=*6Q%eVGW=A zQB17lL}>RP+@mU|55w9?M6Gk=QaP_WSmHgqd+pEsl=1F)GoK0AO!z2~(-M*Dx}?gT z*2NADQavZ{Uv2=~psFs~J>+oU$XH&$c%@$)UV=$ZeEA3PG0-HGme3i@AA)oiK2KNG zV%?MtW(#6COf;(KG^(e%B~eg}RZ1lFp=cF(t1&hM4|xJ#kZf}>mxANSA08?~T~nDc zijrEnmG->*$q|xC*(O4GCcm>40EXrOnj?;IbX@CQ_)D}7m2v@$<-Wq-Z1Th-CXp}D zj?vXMM+Mg5mf<2(;;G9?knikSX|GJm&BN-Rk!tqg;Vk{EX{ zWgL^}gWj_RL`FGPvi>{X%S>T%In5k6gq_LYl1r<(oXZC5 zobmwKFTLuL6eR_MCEo;i*ehX@K@tn*U^S%;#Ipioi9COXx}y>t6eyNWd zlxx7HjS~MRVQJgxwjT4-Xa1!T6VE9S+tw|uElXh5OWb?XIwwl|qohY>IaGzel0O^U z3=@ZPIKRa`EDZR_e=JUI&7*Ayd0-=Sg#!9iRplS$Ev{g`rbUw-H|kCi z5Z1am@w6%w{`(;Zr4z`tWQK`DcfsDch33d!B}%0cfuV;Ljh1pTb?(M30zQKsWWg{} zX2G{x*gvF^83 zfbDklM`%uZI5Fo*aTQpI+~nDf^4hOp&|WO;+S+ zL3>Wy0*cvx0jnlWcNSlO{u)yBuuD`DPqc?ujzoK^hJ+L*qjWO4Xg?236F(^JEpcLy z))gs-Q;vEcY!?aSNtn$*6A>he7P39M7VVJhl)fKC}c+gE-^zx0m# z)OcCIPEra+I7HQf2g`$OetY5-inn3VKjSh^xIFL3(e|jcU%?!^=QlW011k$>V4sy*?a=iekFx*ehnL%tp^{!>)n_jHwZBquf1Ju@v5;QP zpZ+bqf>Wgm^9V*OMvuBy_g4V? z*4(R9s%+d{9aUlCsfkl3vU`xWNqKVB`6X|6;)%2)y$Sm>K4W+2CLNkb|57U0}e0(1sWJvsEl>{KHn$3w-gg);0gb9 zudYZUT}-CR*mW>vpBr9;q;8(aX~-g9%w4tbhJD!wnoXJ=x%=yaooxw3=ivhGg%tMZ zy!u#w4Vlcz;EPq1SA3oD9*A016vS*=ztm?bvuz!u3&^HCq+50&;DuiWjy({^6JunhVIQQH0^txfnuBB(S;lNL;!f#lPla3e z-qsazH;*m?&8OCP;fsiBbw8DWO>AUx?{~|Ap{FU_?mzLPm7SACMJ*{UDp~hcAuLB$ zYLxEguN*G@)=`vC;!*g+=hJ11BuQYhlU5t^h_ z4pNho@T&h=H!FgohpQ1tKRPV6(A)>noeg+pO=ZC`~KMycWJgA(%Fr zXlI*fqnFRb(|?xg$a|xrUlH{BkaV=Yuci!wb&@EC6<^iIg_t=C>aB4t%XC_cIHsN@N(q zCeBva@BMoHXiP1>s=u@_rc?3tYu10m79EX|Q|!VsBOYcY*`T@)hL7T15#CF;xaO;< ze*rVhoXls@zn#{ysD%&h{;V`@WWlZOhog^)4sLqXNx>(KR7vrKdSJ7leDIinZ!Lzp z{)O~#xky6-;t@=etid&g9 zG=f3>jD1~J8u*GC1$_WZ>;VI?rPUp~@b}6N%3p~#;c%4*vn5p6kWa2H4xZYOltz}$ z-Wd9$aXB86JE-?$6zC0cT@G*jq!+Z#-m8%W8@ZccE~hKCjv)GV--BiEOM%599ceB9 z8Y%hxz+@@mQ^W6kMZxw%jXvVj`;|?AEHvu&L&mS#*!(SH;MayjjSwltg_ka6yWg+f z*lujJDl@vU-~k@EIcI2<@BB)u`->$GUN{^oDO6xy4phwL2}`6nhwXBd-s^d3fE8aY zg5aO{c)A2t(Ad416L4B z6uRV>>#4O}XK;ajUg>6+4sR!P3>G#y_Xo@Qlvwoi&wYReCLN!QrBoG_Rc&%^Si)IW zbuZ-ca*xwwn7=>ctLxUoSv7+$saKz;`U^ZY*IOHE2f$Z=EvPvdK_UHJKxy$ z!C~%pl)!-|aSU4xA8)bv&LMxTk%^gnniM|btiuhrw2GTGytF{-(%$~Y8;CDs0+i#- z$`I&hdB$N>tFQbOd2|63$&tC$jw`YU?3Kpsldn~D*`Be5*+N{!+$$qA!t{j3f5&1z zt7>t>4~Pz&oEwycx1SRfq){MoVf~6IgucpLFeg}I3Pmm$n@-x=2Tbscc%;PVD`oh%^CQ}x%h54zJ(&f4 zJl{ueWAA7V^hb--w77IO;UP;0EV=O*z|__~vbLL{$?mO1d<_&M~aPhk)XEA1t?i;X4*A0}I4i?~a>x0BI|L3H{MAL6L zURy&FoDvmH&WoHx07DG#t9n7eXh50(kmj7~X@WIQ+7up^Q5m%<-CmKu=mi8Db zkgy{W$%vBAotq`_yCuPgApy&)r;ZH!&sDxf&>v|nnM*;WRDF2~l>DUWfq%M(Rj_gF z^G(<(!2ok2*~Dv5!Y}Pxa~<4ZhP;51ok1!S-{4e`b;tK9xAi5Jmpr-0B@}`qA08Jc zDx!3uzUVeo)475MdzDnoun&&XB-yygD2GsxEyBRl>y40>>NP9l@g@dTA-ElE46PS5c>rgUonSY__;SS>caPoQUnpW$v7qCK=5r<@0&JY3`u zE@Xzd%jC@hBq8PaHNt8)u?kk5WyZ5Df3kOC&TJC?M9h$cIw1iZE3F;UKh6pGW^aMB zI!{S$!U=%H7Y4)o1_4KluHaHGVEcnl2PBD4TJ}{X2pUnUFi?<}su?-Y#%K2MX!Gl} z)!N4zY$_5;;WyJy?k1&_`Yjl2mEgwm{(-1|LsrUKC`2GK?Uw%sh{#B z@rAgd$pmSRIH9xy4V@_~MX1STcs*3P21LZPP{<9XS74Mp3h$-Yiby#ER5Y?{sfVAv zA+!A%FoZNm5`G{mSl4)81q-i=+!iIQj8*9#aw+8=L97abE7FkK^Tyf&2zrL7zyaq zr{8lN*#$Ip0{=$xW4Fj|ZXZGso5XKi=Rot2AO@o~EpM~dnFs3L6agkVyGIzxdu3F# zEY4=U*SrV4t_kGFc0lwu>6nEa%dtghGEI~fx2^!dkauQi;;BWDCt!rmL3LynML;@8 zSaF@9m|;M7(;6O?D@0SHtA~H8q)OUg4HjC#>a44CN-D$A-F+@jl}}U@gljrA{H?Q z8?3QY_ihNFCkA1|o6BLrhG#|UNj*I;oBinIp__|in*)r@t^Cd?I&4dD9Fypv07y9G z-2lPLBUNIQD zj!Fq$tp1T1jgd_v!PY=%N-h?ZuVP%w(xNwQb03KNGQni)<2LwkWqegHyh)xhE;b#N zuj`ZMAnS8*Bm`j5#g(VU?&U}OjjY*&-DF|?U|e#~Lz0)Or#zn@Pev$>J``3F;x;_O zfpVi>(RFcaN!kyg7_81R!gY3@rVLp%X?Bhwn2qo78f@1;4G1}77YA7)awr0(v|vPa zy0~m`iq5rO!j76U;7VI*by$R6$A>AJ6w`-CXAF8Ym=0>~WKBpnZ!#8P9q5ypDfd_j z19}!0Wx%n^M2GVQN*B;!?YN&$4=#BL8GfJ~@2O482{^Z|K0tjSE>wIeTTlq6*b8a%^7!LUrA^0)B*2}0_Wu)n} zr(E~d6t8j-LzF|C;b5-kq3Ai2@M9EGU=*Oq?h46q0#Ue@aZ z`3t!F{Sv|4u))?I*3VS4dgLt<_n7B?rG;H)Kp(r{4hyRatc{312oWLkl5q!z>Th{3 zv+n0-o6?8~hSw9?JoQEmiJTT1Ho}MQn3$Ck@fV)p-~!D+EG0wNpuNT79fY^}1Lx__ z{M-s`Ve!oaY6SStUAW;Le~CI;jqQ+Z>M8Jr26OxkQ>siu^n`OO@fJL(kmT-fGXt$i z`_1Wz` z=D9Ks{g!QJ_`U=>6A~PB&SOjBlOOjJ_7YC+KkF0L>6ikSQ+5t6A@@7@8M9O*<@3ew zpk&BKVr0V7**-7K^Ya#JxML2?))9fL0TCN($7*oTiSYz&$_>c*!%-YjX1!AacAC&0p2dc`QnS!jt>(leLTDa0T)~+H4UQ%ei=aiYC?qEh*?sUIk2OU~>FGx2tg)l* zNQ^W6A!`G5_ep{b0aKz<1X~o>_5q}=%7*YX1L_8Mqx~C>4GSnr%H;)w>__IZ0qXK= z_7KF1Vi)-6T=I$rM={_J^g;1g1E~$K+fhTw0>)SmPd1<2oSwFjMwG8X_E(_b0_ zOWfj>Nj?vD)%-fZjv7GXK_0&F+?!ck;!di$?<; zJ*o__zCUV?MZ-kgDoV$Ac;Z%EjMJN{D3_OVkQp`K!ZjL4+7@+*n6S%>2WOgo^nb4C zr)K}Z%d+)TakdL&OmqI6%t0-$6)2r^r)Weg+Sg-*D)5a#G|w)7apMta6~m zgM|FL$;n=@BZzaW{Q%5?&j>YF4zG+2+rKAIHwNwh69(r{=)7fWi(3?6fHf3P5 zM>WHMfw%dQ5&(Bh3UEGkE@I3cHqZxP1-O6hkUy5_IgEj2{)Ah5$QY+Ttj3T-1;&@O zCbux;&70|{!PLXh34wq$emB6|FB?c~HN5-Nk=MC)4CIi?)U1AZ-yhN>X&A!^VeXN~ z?v<*91S^~9e)IWwP2t5^2!GNA8fr#d?wTxW7o>ktFm%V0mp0(x=~HxPS2( z0$v*2&aM@Y&JUS1bGk`62xV4x(2uBmoCPDJ@|Gi5>ao5M_LI&AN_urh;_0#Ww)loB zO~zJr1oVWaFf0!d@mfGF87ZeX8OoXspUqeWf;S7~nwE|hD&(Ew+9Z5az69vJ^!cM$ z6dl#`jE!2YIitJI90;uZixA3M8X}>c8*o=nkuDY76Ow<|L5^jRBxo|kNC1LAgOQ_? zj|{*l;%AZ(sCu=fdW9sARd7&eCvz=+%P}~P9Jxhx=((fP`;$IO2D+u z*VeDJXcgMY8P$!n*I;^&e&h!C=OT|_%&N$m6-`O74~;U;Ld>y~QBg%$e~%9+JL0X; zAH=xG>;W97OatZhGmG_WZq4%Oa)%xIRlGsbET(yZS>lH%AZ~G8hx||JWuo^ex#8Bh zvCz_kWXg*GF|FyW)=nBd<7}ipuaZH?kY>Qkv`Ch!WB}PAP@Ayt_F+QcwT{4T(+0Fp z5KwhCeREE01&=B3O_D3f7#!_I-u=O_Ano}~%OErLbW>Bs(!kokt>eX9xXGTymP`{Bw!=2&+ zM-eFrSp6~Je2@-oj4||Pvb8IX!Tj}*vB6wB1F9eLs8l@%hLzcGGA=CuetjNKCKL9e z=i+~%V;5PG@dszKa3MEqkz%P=M}dmY&^|CZR>FdZs=^uE3Wu5`{C88tAFy5d@vw$U zoITFH0_j)M?})uBH9Fh)e2!sfdLgT>T_X1Te8qFI>zfdL#-TppcNC4(4mb2SqlKfiNZJy(jj8B05cS1&75IqM4YkT! z_{J%(PoI_@+1>Yx$XwT6rUe|Xj}d*A(aCg{pWwl)5WDU8ybR9I7^BO1G<+zcKeG6a zLq#(=)H7Bf`>**FGz5K#*gWBvsgThF_!^n(dvuaIf?KaKHcyAY**hM6kQ;tcqY)Pg z_kXU|Re(xmovOcIgm^x7TAvsbv;}@`_A>K?jybwV>UBwr!+juY$by9spB|CsJ*{>1 zO?nt8uZI|YLYnOn-we@K#A~%5k-*u4A9RCb4=LU9_K#CKc8ljL#yZ#M2J1Cv1=0y= z>riE9c0Yhk=LNghnnSJZk(MPX2IPrQ!nS!7&@W_{c08M%e=ZXfd~%Iie%q9XN-;={ zbB?a|2I6#C`~`N-4fY~4%PkLt97GYl7R=t{sw2Kw#{!uyBEmf7^oPnjYWQUzoGn&l z>sAq8*efaPU(nega}-K`fpC{NC^*K`04*(wVLAdxGRjUWYv8M-kz2}`L>38h)=$jm z;6Ib{!y%Onei0rX=S{@0evR;v1l7&kH(N62JXb;` z^>IeXVL(7xn*TjV#&b9Wl9F3M8U+#R8-+p=9%ThUnEPydgP1ciE`%>(o_!0^0EUeP zc%sL?F4s7vbNY4pJ2Ju{pD@Zw5%tYuZ&hqG(=l6MukS0OdHd2cP98sD$bZ6Dh77A? zX0XhP$Hjs$L8+)J08_t~>5|Qw+0c_1G8i*FxRTu=A>MEKgZ=M)cawf!Xn<+*{L1-B z_tKh2KlQuQnNG?D@eWkGipygp9!A(7#)mqkLO_%5I>zORVzdY<0F4W}_UWRzU2hAo ztB~M2cMo}KDM8L-k*iFr8_h|_S?9Wj=P&}Eq;gdQU3ilhO&as=C-6yp2s!f4jEO#Y zR+7N)dZ`N5+2F`KY}y243~nRq5GveANL;I>0#pz#IC5}%Ljm9yyzLKoYsSB}`bO2RA zs=ooJgRw@GcHn4Zxw?fCUenGLO5zeB%%ID1@Gh$EW=8#)Y-!<#1;Sajbj9pRVTAvZ z)~_h_I_@ZIi3XFmGrML7mJERsUu#{OwWBQN9S1Sm{>HMGKy9k_FgA}E9@$&^P=$|h zczslQg{^!}HObVkxm)O0RD$#k%3tC2@>Wj?Vaimk3<18XBidVf#xHp%mfG*(E^?~_ z!hjpt6cfQc`3H8;dMxHZW1OjSlKbX{V(!-=;Y@m%UdvM^jdC-?lvM}}8HJrJix+sm zW^q@TNlYj00)pc*-7-q!o;kR31{f6b$Nij*O~E9T#H3CFuWRGU8v!Qa9hW&mbk}ac z(PL5GW^K)$TTS7ARQ>uC=d1!)JxDfWdj_W&K0$Q>Efn* zx;Wjbs-C*!4E9xCIVb@wi)7?iFzuz<7RtFmvw*gQL(%5({>b{1#~F^u^>nbfUn2Y3 z2LOzq6eH+YZwiQCwd7VsCrf;CZXIr}Q)+}8=I<}nB?L)hOej8Hh?Wg{Z;W#7(YCc~ zp)b1az#o^>#Gf1XRIPrnG{tnCGhSx-Nn{uvioRw6O3IGr{{$r;KbEwLyx+;vWE23b zBY|b~>W|1Pb(|3V`q1s8sWVtA%2<~bQZJJRXGackA^)LC&}in}_uwnP(8}>XhPb|Y zo3zCSd_FB*aG)S9rV;OYrt`Lyr4rf_dfgYk3|Tb%AF$E|cn~W1i$Jgi+YF^Cx+}+e zmdAQ}J$6f3KzyX&@p<=D0pJFK!vm#%e?H4Qq*4rvwd{Pll$B7-pNH*wKTE;6P?rDo z*FWxm{RI<$f&cOEVEG^ap5?{*AOD_q^<>0b^^ZFH*I$3$!02E9@$b{0ls7^9C-VQ| z|MutUA1Y1%-@t+y~ zufP6L)Lr+lzlL$h|N6%`#c93$DeGVVShrj_5bRbzyH^x{Ez?V{{v7<2MDf0LF9(;007%S0RT`-0|XQR2nYxOc!rBh z0000000000000004FCWDRdi`=X>@62b1rjsXSDm*lI%d1BnbXWO3ur)ptBrs@ZQQA z0p}cWkT-D7Ilg{zv$LwJXM1I3MS9{&xWN(TM%2#HqZ)zxum9K8cYitZWv$2I?|+5< z1^nw@@*y3wdMN+?SI_1DM*Q{v`nP{wkMb{Poo&_M|N15V1;IhM@BjLjDzD3G`^PPa zguZ`n9d&-vC-Z_T#?|-efZTfGY&v~Bza>D;IF6HM7g5c*J z{GVO^+tt-W`@iZ4ktF%~$4~$IZ~t2UrKr2^?|&V}A^+=N)@>Qv{O^BFx;SnB{q5iW zx!QN~KYjnU?A_ZXKk{MBvj6Dxj+dwL^!LB6^Bynx|8&0@>lcvZ{YdwZDbuc={x-#b z2L12z{^vD}Q=Hb@{g3(neLeqqb$|Ph4llmj5)W%JF8$yC+Q-{cU;iicU%~g^KVJXK zJK3LE{*wLuuiE?mFY1fpzc(+Ke^J4IZ@|Ai|LffydAg0u-}0h(FaEiZ&%gcar~J2n z&GKUXw}1V!7xT{~^*hU-H}HR+^>5$*`nT_&3BmtpN%Al1d;6cwd-wUrfd4+ud->l7 z{-0plm+t@kKmIZLfBAQ`{txgUU;kY8{{ZD*i2rREe*f$Df5MO?{wwm|(L??dh{(S; zB>orm?efnC`QHaA^8YBL$p3(}_{Z1(9a8B3F+l$jlrL7`e*g#m^YwpYKob9XMi_gK z>zx6?zW<|z-sAko5B}o<|Njp{;=la=&f))v*Z(_VB>T4gFX*DLw|x1J`~P^?)o=51 zM^^Z2}6I6t9q~0&x@hZ+M z->brUUqq0h!nb8rXakf#9hiWY{6KrJ;Lo4_pHl?uMUmg@lHxkf`q{`}Tx14c-c>9> zO3FAj+7w3945m~P7x}%yNM$wyWQN1=cR1h}C%^X3*@MdZk>OnaKJ8S?eyyl*U~y$x zMs%Ki3itw$J|alN%(L}z@8W^c@3WpbPFxc~JhsT?XH#+M`)6loY?F?vL$)Cg3E)a!T1x^|)z6xoo>s|J=71%3NCr?aJ zN#wv&sifYhAJ1HLMcFzxdp=v+X36dT!LK*7mdn^>mPTznMvaQKQ+8Ln=(kP0(vb;v z?v)o$6X3ixYXguer+&1m%t|>^pBR`Ida=KM`_>==*#9!>7H$+8z9`mNuea}dPiF^Hr(%9c>9q9p@ht^DAIU_Q3R?w{#~qY;Pqj0R z-e-8s-a)msE&;Pjn{Blh+n_w}>>H|{Dh*4S!`5NDzPGkbc?Dro@=3ec6!D(r#tG7` zZ^-#eri40SPS2}~LixMZXW=SSJBo`1(`E>^b3|#a?$6oS6va6k0#i$f4Ir>McF&`# zyHN083z2xIEfcMWv)%9dQ-zZQhBreum%mqGD%)w;U@e@E$bi_LLs)o6Oh9q%9>qAa zq+|fqmkf@~shjD~o>XG@t0e(g~mYLs-$y8LHXr$$} zC4}ahHN52bvTmB@53z_h5Gz+w1G}N%_89_M`Mx+a=b!xKIC|_FmTJ=XTC5p3N6WPc zw##jPdk!Fy=75uz>RE>s6=<)I$qIC;=IYah1M2#-G=4?l^9v<0t7Zs%<4Z0_bpY

0*RJ&CZVs7^o=%w7LR-A z7qmkX9(_^YPYX^$t?2-ugYKsmybAl|2bXANhNNU(9*0PsaTvljNI#wLh_pFD{{2=djLD74L%!ugZqLgs*{~)Gqs{X1GY=prxb2Ny zQ83FhoKI4X4taLYj78+%v+=EPQ4#(tx?{{r&Ztbx__@SNfM!ZHt~dbtF4Bh-w9A&a z%9To)1tY%sePw4WY3D`{K*7H)`k|dtYaQd`K>`n8MjjI8fz}-^cc}V>(U5553W?-0T~6w zWgK}TX0z`-WGtL$*_OPV`fqQt;Op4GmznV;A|)Sz&;E2AOXSo<4kgv-EyEZj(+H(6 z0)&-h4(g4jJR?0lmlP_)hlrl0_;gbDcsvw@;popcru|8uT~+Hmy66ecJWmc$_aMlb z>_-(iawIp!ZB}s8uo!FqzF*r5FJ3E}>Ml?hOhmX+6jb-Zb)vNiM4yQ0IhhZDE(Vt% z0Izs`TQ7ZPl=CRK+|RMP>P!J(P#>#&`o*xcL0f?3R2EV>>A*i%3TnBjkrfX>WZGeMKdm+2HI}j|tDb{Zopyo;} zysd<#mxiS)y@gvsEZV8C1h=FxAAO*=MtamQwzNrnwUlM#+*2lx+er&>ogc#J9y#Vn zO6)mxobS;be;*GG$^Jsc;_SH{KVG#CxCPV|?)&Mi0h3gMd;;=i!*m=#0u-ba6rKmt zByB%TVmcw>P1wJan){Pv6=GpKQ&?{)1*vNzAT^tJ5&syNJg??iJ{X9ys!{6aM=Dt4 z3eXo(!!Ovu{voW;>|G%CV#8KZX5!~h7J5DtQ7%qy_^pOu+)lX9djdseANw>450l?C?Xvr{N8S_Tq=YGjnHUSN< zI@qWmE}RpPoBSHY2t_&*Pad8~e=XipDa;te`)yvao~zcd@;VmJw^mEYIvopA{u z>`Ls}8x#{+)rK;JjU;jgEO0R`1jHuq&<{J4e9}{dfgJ!=DiirC`Q!Kx7l(0I zHNGW%)X{C_o3p=x>2AX{@i74C7?jM>$VQg_^9KgUX9v4(Bt|Qf3V#c~JZE>{Gz}bG zc`Tn}m=P7}d5ub2vDl4Q%nc^5=zL}139i;16LGYBThW`B9bl7n>|ahq-y-Qd^p&OK zXPDV5;f&;M#7DPp<8w2h^KgrX)9#0M`4vx%!clyjk_Dfy!(n5AIs%66NCNYM0)fb| z15sx;)sYFMS5!X}v+n%ZR|zh7b8I&VUon%!9W2&i@~C9A3NIBb4#C2O&l=AuoW5TB zB=GfK8jh;q{#p;3Az z+s6UunB=@#+wi2M-f&8@ZAiy{9dd(yhC~w6q0Zx`V^57I*&h}szx@C}cw?puUjchQ zpsjyyAZz^PPN?&Zd+MFb_!=YuHJ3}61t!Cm=n3wxkJZnisaA)XlaqvGWS^iWLoY>3 zlU&bPky6t=aObOd*^k;P)wY+qe68O1YLQ#kE;It5>`kOu%v@~3qR9O-R|YwODI+v+ zQzYPx0)Ma`Ua9+JYcdCn`9Xo!`HH^;C_c>tk;c(+M!jWiAAt=3^Rk#a)u(G4j#8mj zhkJ$~9e2@s>+TLoqkrD|&PYr_-8PZvnx#Ps=k0s+w&9G>*Ac6_l5myM2T<0sw9g&1 z_a$@J*|B;eC*sYQV!_wEoxKg%g9IdM4W5O-~ApCdpD5W@uRM)AQQ` zCRmahkj3&R>B(xldZU_#?z7ihx_>XjJS@8<(*sgL!}@!!akTm%Q^G4I7p&OdX1Ty1 z<>W>aWpe;-_xO8;n+-}QNheb;%%8`n=1yA9Xdv*Mg0GCkVtv81Ol6G<1$h=Dw`5J&?Nuq3*~~634mv=7;l$dUcFt(W}X`rYYhpQx$$} z&+X_}z6rhaASIVZi8{`3qM5Hr`ow*gJ!2J+0LzXA@3g2lt@8-WBu8ls46&@rlrSqm zM?ZAiGRhPFP_C649toU6)E`i0HT(zAlGWKzek^y4x`{yNbarXd-k1`nG)iX%*7<=T z3Z~aDBo{tHOf`MqBlkTo?b18RrQZ z;?DJf@Y$3j19?+13-kT8ARbTm~5;U@#8V4g%QGAz0QfHq8(o1j1;5@4h zFtg^SbcW2v;NfPlEg6)3>HN8sk)1CcM+ZO2_htvd{==2hNBktCRe12nf$2tln!meg z-mWRKz}prW^+%O&kWiZJJ;hUQB0)>v&j5$f-$hG|e7F-na;Qk#Jx_7+f&8e=3u%-| z6&s&$tbu`RRZLdMwt2v5$twl4Be4-hw>f*ma6f0g1!gunUD zNqP1H>isbdq9qDWaw*b$e}44o_TFX?Crq>LPR=4Qg7S{3K~$>*Wz*FhktS-p6ugmO z_yd^LA_K~4)4Kq6S#(+fg1V8wpwVeOwRuEOd$m~-3mx(RkV=^Ri@|U(=$bLljMLrI+0YhX{-y5v1p zLLt&>Cs^(x)=3HH-Z%y&`fd#}Ahr8-lQyN~rD;WZZ|H_ozaZ)x-|R`Ct;4OC_;p!) z&>);gC3zmF0KoTC@6f)M&%a=|buYT7 zT*?m|x&ISOfgk$XO=p(} z!tBhn=?T@_ZZymjEyOSQccv^*?SOFe7cjq}vM%PTt zife0OY}h^1hgm14sqaFWZ>*puC;seUUiXFcMqAQw?Gr6_w9$8O;UUXIjBSn2qHk|JFeOAkdvrplX zSCVT^`tQ0OKp$9vnj?X6y1JWtwJ@b~@P;%W#z!Z9KvVirFjbs1iEsZU4Cq6MJ(-rD zjQF!!X!~Wh_leAECa4|qpN<#pz5AzO1VxqzIedwM5 z!M3~cscZufXgx*P>a*JpH7Tb?TfKd1ydw{kfD<2!C<@VxLpcO7v-(*lD8JJ#L@Kk- z*!ov7G?-rIstTvZ;?Y;AaRf)tHTZm0VL8AHQLUNS%QqiJ$nuEnXF|k2<~eFjL0VHV z_oKP(fnr%1f^thpngl`$SQfj)DEx!%$mNSF%e{fbp*Sv6+DB!7X9IzD_?n+BsouZ@ zV5GSbo&0_clhqk&qS^`uFEW4Vp32*!q{kmM`T}f$oX$R(V)@@lcGy_)D|P3cfL`Dt zfuUYQ4rv+$R}-0ZlVwpS3>VNj~R6(s;CLTCW93W@=%7B3F&S zb-UM29ZGqXxn11t1a)x&5XB!cl4-;8#m~@SzFZ*7JYb(#Ej<7Dbui(QhKq?6y6-tr z3qD9xJ4m#JCDDiqHw}j&#TRsToYvE=r{{z5weZsgR01II+ZMNol8K{;+Qv9+RPX_v zGWSoN??bY%00wq;Vm65O9#v9Ds*_+d!G}m-l=JwS#B#v+`7)W$08~pT_odME4b6IB z4i-iLQ49C@a9L&(u2UEioN!w?CJlQfCA<5%3?NII8>|u$bh5GV3&3<7h&OI54ew0H z_6y5qgo>l@9yxiS1xoVb5snGNGN#O4(nXsYfK~lI%tc%J$w5&Os7%H(xNz}GP06&Q z{No2uV>_|eV`*u{Ul?h}=2&la0q9%Y+^;`|1}@gW9lh);_M4IPB$>zo54t@n zkRBr4(E5Bn<%jQh%d?JShfmbij00VQr7EyQ-QcR;jqT4fNt1Nm>Gp6`N)A^n482JW zUx^Mr=dAQ>k)%IKMTRTqfW-gVBK)Zdn`GWFviDQZQ*3XITo9>gicwMbn8G>KUt=K; zijbGLA@F&;0s6LMiAL|#z;iKUsBI!x9xK!|tbo%Uk&VW3@ZR*^24MP|n(4FW5@t4V zMMmBRcZ(3tu&3%#j>K{G!$m-f51&i;hW!9jM376Tv>((udiik4@5~L9Typ!drY1MH zCDPz^!Uo`%d-|ao$`Xq^J~!-w!;~|~tBV)ztA$ZRA>217bmnfH zIVp*Pes;4hxK8hLfh<@vS=TTq?@C|yYJ1a7{fqP{ysme6F8{$Eh07Yfk{tJ8__@;Q&j5JJ=Sm8hIUa5`YD^vq`s~j__gEZZC~4x!@`E%1{;2^ z3!s15Jn+&0lKvtJha5ataq0JE?7}Gy)svN?m^lq!2Iy0y`%fR~LP*F11FJ&{Lh^9E z$9JFU^Fvb(B()Mj41~(4g57@=o8@WJbZMAPDQYFN5Ru}taGPBui3UZjmFvm;L$ZM$J-S07OOTm4AO z(i0c#)@tBn=t}JowAxcU8M{+(5lDRdnQiG&$UJLG2`hj9Wf_4;R@UPXy29Ny>cO6n z+EIt|YWs9m^3SpdGCK$$t(OAie)V2dxCQ2cqN7cFb8j5FYN8U+DqIJv2;BR+yBz*0 zpS*X4LxK;%-UU*c{j}#u{>^*xrg{l|n<2M+T3%#Dh)~yzobhH#_=5}YlR(X=tFP{) z^OKCBT(QR+ons1yjv~i#<4Pnu(r=V7lB}h+z;k^5JY&KG-r2y5fruq(uJBgzlf7HMQc$d3$(fia8pg5G2c2U|4EkFvJRt$nVfEIBXm zXf9z2xjD#vo&haphv!*eoGdn~f~DWh#6=lKra5dBzbCm7a8$XI1L}PTPBU4b(n)j$ zZrelqlf1VIDa&VbfQ`wL6a&vWa++ualLaLo2bIdlQo2_P%R+}feh7zsa(KrW zmH;1sFTi`~_5pH~B`~0p!Fj>+&SqXtt*pvj;22&Lttm6!tbm6-?pethppy*D=!PZs zfxPiphOqd}2JcT7KCZMB1G6;!7{%BVMfh6-n$B!42?r_USuWIEfSgplD`sL;VLJe` zIE(#iUcPM`kt;lQok5s**+=B(Z#C^86f*AVEZ?7jJtjUUPwVyy9pJ7ew~K@OaPL`Bzq`<0MAGo(s5ZAL1UEGY#o(BsEfo#-ZS^n7gZxvSgEFmH z9;6S_T>c_o{GrWe-Q8Z;Ulo*X6qVx7Fbh8d9@0XHHFKJs{gP$FP;Q|k!5>q&>&bjh zonw-t0x{HEnZtEx1L+202Tok@a2H2bWd1Ay`rCN3j!g8pF5QQF{$qxhq-c}74=~hy z8mqO@2{cGxQ!V-Ro=28SI+F=HYJJi9e5l-ox)F<|4^3a*?S=qE{Ae^gKJbtv$nT=v z7Ab;FTwn3|(hoSU`XW7BTD(1$;sOoVuIR%QdGD*MLC$*K^hnte2~xeKbk~IAubQA< z=SU#%&mfWMy7yw`9x1V+Q9k532DlgkjFrgxd7FL4eBic9^8RHgIw!I)Tz^8h8!Pkn zAVrosRHhPa6TTH}JD8453ap>cbtWLR9?R!SXRZ6&XZuy>-0UZ_(KR7cL4vf*u{0^n zsF_$EzuIqqngt>IU;d(RIGPh*m&dk_J-WWmo2lqAXjsTS?ADz-ics|D@x0ZT7qe#i0MR?weoaCq6YK?g4gxAy6$R|!<$D1B&~0%b zZTDLvI=Ajr_2-{`SK-|S25()zH~=bI2q2ejg-Wp6N!4Jna>Ku^pj#-;*IxKYbIS$- zcDLv}0RWKloOx`xSjwan5=`>R%NED%{heIhdD&7x%ug`eP%tb(z6uEqC-x4nX8^72 z)|`mS>M0h)P!~P>kV`!1pKt$ys7SCl-ocFnjI_XgL1!^nccqaR5hA$6~@cMb> z?DQ&9-|gIggROjli`-xNPoA;A-n@Y~5C?~+q3B#qxxe-GYr-N)I>XW~zsGdtx%g{* zD<%3QpDI5}mf^*F^!ZDW&e`~nrn6X*TZw|`2WcUurJw~ZX4r|i6C7fR;qtZkEd?ci=!x;iEW1ax53c;ZoAayg#0xwg z0}KSihm&nk#n;r6-#!oE2Sia11NX@T*9Yb+wHH(QgGyJ>mOiQBf64FZBRI@{Ltq1 zrUKmNd9d)L7E5}w>6pg30s2=gRaB1Dsl_-zB~}8F>j?V+G>b@C5@S;R;I;a)q+!m~ zy|xs>1Wc=Q;9_5V2PrfFP~-qTDkx4=?UwPo&BND0`MD}^QCYoAfbUb(u|!3O#zV7% zdIm89`c3~4Ln<%DEuJEfIzNZC_^JQNg`v# zlA?UsowB{c+K$-!#IdUs9Gw&%Lz##Vpv-jq5_xE*Ee}uRRVm&W%WxQ0yHmq50SBC8jIEAB%&w`4s^(}j%3tFYIeeN+lSt;QFVI!~|SAoMjuJA?IaH9hus^>|mje@==NX9<9*qns?oeQms zQIsX+bmYld(_*sbWrZ6Wz@wlCF)xTixs9>YEyBKNb<*XN252)5#F!(@0z-olr_D}Q z1LQ}@4}4;F&CvUau?K8`y&J$N>`~27!$-AiBhfPuM%UsE(L7S9pcb_f6rUk-w%nFH zXjYPve)an@(16+AJ~@0nEu4@_R|4vEFJ3%vxG;(Sw|}v>l($PA8`8!s;_o)yOXG@e zW(YM*8NK>7Kk1KeUO*Q+@$^k!Uuol~mIeL#c)1Z{#P2>`EHb_hMW6k2<1YHW?DG?- zT$28-IsfN5mlGy~vw#zZ-UHYP}47!)kDG=sOj_A(^z6AirJBvJkwRC;MWkFsMvM-!jW zPw7MwWV+CXgZ-fA9VR{lo~bv?Z{k49T0W~%+hTu0eA$3?me-_MLzzY^ajMg8nd^N6 z3T{tUshe4;y0cGd;gsPl z=|-YTBs>a8MJUdILLOUs=`aKYTu2!4N0B|{C9Ne)I3ZC8gb*aY@rYaK7q6DwQ zM@RI-HKGTx{b27eN_l4gB()BADnPt5X_wv>$e9VS4E$$$;O>bL{miI6P(WbRuSOZj znMpy$%8lsD<}%w@xw$aYIUKc;66ybJFI&R{`z#*L-F7O`ht|l9=H-jh=?)ORU9pi- zlws+sunPz}BTmkvheCjJ{H9l9%_tX8*8gdr{)KBVDM$U=J2D{UT>Ox#=9Ri$;wQKh$?O$A0AYG^R}VB&wJ?41CC}$EE&W z+yDVU{=dGLV%2dCFQxvM|g{`@Z6gC&WTeD!2}37KweAaRLWf1u zNtOoC;3i?^{M8nhR>5~@L_E#(iPb!kbf{}Bk~A0no_3p2NSdgM^)+VeCB_4hnX4}( zl^uYmGlNe!3NYQ(7T5BIKgPX4@L^vr`-&MecLJ*mFi-mD{eaxL6AJr#tyH(qo_Z}; zuz|zOs19)q6xY3V`VLaM%2Jdmdchk zmn)jQ-!%7CBFpA!EvswTm!#Lj_B}DhBe>^jIpG6&vk4T4p@(n0wX|Yh*EWK&gztnD z7|T(BpML0}K}4M6YT-_LF|w9!GB_)x#u@*|TW zH??cgIQp1-UeLIgnHy3BN!U;E!h4uW`g#l$W<4NQ?*Hj~bk@kG!Z_H=XZ!iyGeAC_Q zE}~n9=?_m`)Zlj&~=KMNMSh z5MQ-0RItCgt^`uyTWw-oNLy7DUcn}L(K>S~b&TGoAC=7&)&PifbY9KPbC_@dmuLG~ z?%&Q5#7>6a4?174c5B4mXOZtdd>{|uc5oB|AoxWNQ)xN`a8y)s{E%Z0k7qEj^tT+} zfpw~5#>8|*hHd#_I{K;3I53+R;j2g*$S+pBqO;yng2XD73{hkh0E(4+G)PHhr@1j5 zewdG~VHSw+V((Z=#&+(J0MUJEqgfFUS?We7r;4hfe&;l5P}#oMTLt$UG3UP>Tx@4i zm-Tk{fWGrtlCT%D9%hrl{Z8X~jOF0d>E9-JV*N4Z&?zl11<@sgpU86Ku}4bon12WX z3nVcVxxeapKpc>sq7e1BS6@iK*)rTTZ4fz@bPGn@(^|ITsiWq!G(U3!^G$L$<>Ec< zu_!VPkFatMGZsHMjYEIFi?|bioO}ukmXe8kw;aJq!%^@S!3HHjSd?&CE*VC=V=;iV zrSfk}_m^7)ueKZy4J}Q899t(C$5d~>a0*f#F*J#t81pN8+|bbj^P%z8U9~7;3TB!* z9es1KuP4@rDxD{~)McmM3JnoEGXO)VbNY$NM!~Fn0P4T}V2wZ^CaPJ0lI2lNn0XrI zu$+~suhz2V^h*N$t}FCg9X)e1y!={-uo#!C?yX<7o%8Hx9-5r;qfhi(y%2D=pr~#| z9xdHNE@kFkY_a{E>!^|PhKR}slle_m)6)64NloroQ1Z!zsZWZ~HTqL@0HIKg1B^#P z&@CN+?%M#{%5zp18C5p3+)mxEdW>6#jyGPTU+YunkEtCH%J3eOj>6ATf-j)Bc&$)1qg{?%$Y;6 z@37GB?M{7!VTatN80sQ+?w(XEZ!aTlIyo;sPLB2=y%^fyB53ZcInt6TZ8sb!71k5z9y}GD_^{FmkmY)U^Cdo7UmYUdp49@Ej&abMTwgPZ>f;DR4i9Pk5+^H!MT$*J) zD#z|W=i3k1jugyW#5~E@M~X_@HRTH5yH~z$=J(02*i+sy3ooYh*_V^wIsgXOy5S{r zOo}e#kA%VUo5U?sJ_&eb>m|~I70RIX95*#rDEodQQ@@@vG2gc8ORuAo}U4w z%E=P85N0IdWKr~EpCC^J^yq-HIU(cji1u|AK+0~Oiq!1+?BA>L!iRey+k>}A`yvLJ!1(WhjIR(Q*6AUNInf@ zZZs=yv&*Qix?(Y5eW9M+kcQu?)Z{7O1>fu!q7;My4fkYv`e`CC!G}+G0Sin!qNf=Jult!bE9wZ38F(%1q-hSz$CEhH@#Xe z%oDHrRUu09p3ep(4S=zxk|u~csXKfYqmJQxBshE|j89RZ1z+#vL;=KUjoOI#WWZjn#qn!&D9)e~B_`!CAf!k$J4u1^%n>`o#&INZA4=;6? z4~24H@S~lWVLgO*g&}Dgw(|iK(!kvz2+H#doH2LlLmk}CeceN z;tx@)Z-Xh?{3}OABzx-aax`bH=Z(TDq+WUF8mFoU0e^vkqrToEQ^m8W#SG(l--6EU zaqHdOFflCv_niS_g@y7tvz(CoO{}yZB`dRtcAEhXezOXOU*Ld18-Qp=i_6E$(N07YX<4fvLTE@D7#c`ajN z^oK1!>I`gk@cv`8z;RtO#{ztHlH$E}SZ?IEj0v5UR0C42QSQNL_>*LpR!%n&Heo6T zkK}Yf=WQX6i_QFg+Zy`DDW-#Ps8G6U?jJRI$wg|m_IAON%eeMq zNbI5O@FHLouh27VN96B${R%6FBxXp}kJ+rYPd*{b`E%zQhxs0e*ZSVvB8C330C&Ms zNJTC?&wcg9Ivd$vs=k0%^K!KnAyzK)i6WeaugAp3tn^tle3VpW?^~uw%_ElD&e;4L z^$2Nhjx`DevMAr&o~Xb}c6Uh*U%W~Og*z;9Ts)cTPgXfkD&g7_I7Ky{uk*D_UCt$( zB5oiC4`?ZX=lQZYHS{Tk$s3DAJYVLOp#nE(6IAt!)?dOqe84@((V!pI^OScaXl>-GtyW@=sQ@e%6`2a}11dTjwNa2>x8)OcNEt?@TYDT3S^N ziBbt0@V?!nq{+{v({NTLA5Z9+!gZ69hg?meKX4CmeqV5vuMT1*g}gS=W_WUUWwt=6 zj)cI+5~sSgkPl>11WgnZ4rC=CR{PAA#LTxTiK+z?^`g>b9&_KJo2(*=hJuTwx3AM| zi^d0pD})@-sTxeffWF5@8bO`J`aAOmY}o5R_UipM9v6wdk415#^6S)#wd zs^Bw`{IZZ!L4;pFHb?dh!B23h0wbp10$gTGsv0*S=%4%NaEa+t|Mn7xlE8hf@>tmH zQv_S~YqjIfN5!{R>~5rW_-)JJd%{EG)|dthe9$yD@Q@Joj4S;*zS+^(s(|oKD{Oq- zmF9l|xO)RUm3qFWs$%P@BEqrWR#CRfw!f6c-d`!LwJnj6H%Ro)m!IBwl23hM&+yKD zD>Op^sC6XXb0`hc>KpiBB@5@5TXoD@@_d)vF2h4Na@K8R9*9m;qS7f5U9UIGV0>)$;3=#ot4SgNPHmz zd4e4SU%0`DbpAv=h?=)Do^-kd@C8?WfXH59C;G<$%xSp&?6s=Ytcw0Y3*)t4mR@FO zLOlB|auKdD6MxO>9ZO*B5Q7aEx+T4+!m7AhESUkdjmB;Jb#88f^r1{$g*3AtpnY); z56Z7nsF;zuUr`+mC%0?*WB~}(bfCm*8ad-ifXqAWS#5AxP}2_Yx`@n`?iJ5mr(BRt zYUZGU&Jt98K`aa>PT512)*N~(GlUV7ls|*Y9arQ_;_jYm^M&Uz(BT(GGt603xiYxIBn&xUepVomw{4?yQ<)}grwxncI)rV zp8y)e-m`ghzbPb0JG#!-NL}X_bnJ;CqVa00DQ$5oiuvQ@rbzYDOk-Pbcuglr0LdG# zR*8mF6`(>zSrJ-KR#?xHc02S+HmdtAcm%*?xgiyG9RXbK%^w+yt;-wdGV6LG?7rC& zx%zkJg4*rH9cwfkRZ+oJdF$)X6IvRTAIAMr`B!d4N(QU zKXpVf3B@zRli|U_VhI{5oF@ub~i@i$hMEll$&EJmLV4YT+qSK+ZO( z8lI?EsywczR`KyNa8dZnX(fSAUb@G)-K2&3t$;pIS!}QG&8SRfk90FX>#6pV*H z{2~!;S3^S*OwacO6S9@`ZARUg7V;~+LRm$GPA*G`go0mqYDMqQ^{bH}F1-)3F?}^* zKqs}Osk~88B9tNdbiH*rE&dZ7==sz(r3yKjo~QoP0)oTFriE6WWixLUNrheb}36sESNG17n6rgyh`XvsHC3uF+ z(JF<@1*&GoZyZhrXj0-$>LlH`&SG0zgIh!4_mO^uA@&BhnvhhNQG-}peEbf0OYpst z+)C&SSD~`zg5}{D!Uir%E3W3>5>gq~+&MpOQoZq9h5lORb92KD@ZEL`7 zgXYc^#~2RNG&Iy1A|N7{ghiSj?s>!XYJlhv{=LWg^(Q36nhz#3=n96LP{PFb%RhI! z>YGbAk?3BqEmOY;eW_*`24iS|tC~e#mstuSWn_h!i>Z%Y))}}n0v3qsQY8rPH@uu# zbRNQR@$22-oGBZ6yx~@3N7=w@0NI2yRmwdqc*t@oP8XO=p>md@wXu`I3OD3ugx-A6 zE?0(X#fLoL1dq^TvQ{qA06BXzYx1Z-`PGM}Vbfl{m(Nx|+hu>fTtMiZ65)w%-tiG} zA0&H2>xG<1vD}|7i#v-7M3BvGU`t-vBhkP&$M%IO*W~zN7>;2a&t`p*Th9wNC+Y%E zQkJseHRL=zAJg-BVbd{uNW%EFVa$+Q`Z#0I5_LndJE9e+C}2Ig+R}o^abUe^3ioj* zVMqr;4=?-MD4NVlB)=RAfc+~7yVM;WL9CZ!zdqMguwsyj2=~|30q)T3UMrt zeNhpX_b;A|!xy>(a=`HsrJprT+@&(0O>K^#S?m({m|)g4Apg#%&3h9AoC7JY*DQb# zPR65>KTjx6FXQCVq)*kVaOI9Z07F2$zwDuo&ahPdQ^maQPHWyE_nUrjgNS@HclPtj zZ1!8m%@$3OJ|0K}(92jDjpED>%|Lc$9{^hCFbl075w;^uAt87;Oxh@n zfE`*t>;nLu!et0wS4r3o*%60fm)5EzxNIZ68jGE zN{qDohFoRhU#4RHJ|@qU6^-*3nWB%I>O@UHE;8E`c@7jJq$tS-Pbh%g&Mi0h$SP-} zl5BzpA}Z?jK1NC{Fg3(2>*e!QA4rB&y{nDi*J#>Zw*dQ%bH?V_6-F5WbsXCIDRatV z=^BGJaKNWJltD82tI5X~K6~Ko)67bt|B)#*UV-rWj!W;hX1o&q^F4=upHnsby%-C^ zpuTzLr|(AcK6L{*#UN_V$yx`Y09+B;I%ug%M> znj_--tEnn|dIL2Cx&!13Iv1$t3n%u+d9ihXwd-xBOMWaSkH&`|icwdGa)Uc>pd9LT zyYJ642E`D2A%~f{OcR3e`O9L+jjrUJf$#69FIB+qN080Z#)-viCmN}oNVoue%gQ%)O+EpfO9#cq zVT?CmqnTcyE3>Cvk%QmC6_0x&w*~oC=t#JZLqYE(vB72QLbuL^rtrc*(3TlZ-$aCv zH-CmU9OF)=ZbqX*3ZqmNGRVe47b5pt?$<5>+)8a*e*LpE@$E-P#0_W1Ub(I;gdkZ_ni#fADH`EJG`3;&N&?X{ggesT{t8hPn>UW9 zdd7o^?%oC$uq!~1(JK?0x$*VMLfb;X)bYfpD$sX~%^_Mf^F>-=3x#%B`igdHNDsJhczW=b@GZ@ zc-nl~nwTJhU#6rqy>qp*s+C3j`*4Yoy9n8?AOP2KzCXsL>V9ydQH1OWU9Yq+XAl6v zy!|Y@Di0wwT!fx=eogiIUw`$ND&MA*Utj_BN5K<8NpVUyN`O zDSyDfdyQskZlnn1JI9lbsctjUm7r+hxS$|-?8=Kww>J&cb19?wpve*LKUO|AF? zd>+PE%B+EMe;Ks${rMD8B&1tr3@l(G)@c6L>8r(5DzJ2+hTyMxx&YXH==lO$l3zqylWrI(qYXHheh(cq)@KKzQ zswg}Tks_!YBE}5KwCa3cyz`!H1#!xJzPX>ete5Gape6dZ(m_CO273D0C1^KD5r*&V z!1vGKvlD@kAS+k%9Vx7wC69j4lA9c(+g^!IKg$bdSE->_)*JABG016N2|MOiD2&fmnG*4sx-qj`|59hVNg?1HYdV!pQ{I+r%Gu7(IO3h2Ek zD{2f=NpjZKrCGkyunj9Zy2qqtxfeJ@78W!YNaH%ZmiQVBeKfQ&_do}I);{8AqdA}? z`@#w%V()aA~KF)QSI=7 zvY0+}BB|={`QUCJ@Y|FbV;}tJEtyv!{*nV#pP30d%eTtP)b#9&k!UtuH6hXK)!6bk z;Kr-}dJ1D2_kIhnU0;K|v;dzki9Npn2sa}9`O42H>EiZVeZ9%=9{lOm$R$JQc@?|? z2gBx01{toV9={-ZQHNAID;BkYE8UsL%#tht`bCn$ay&ok4bAX=bKFNDT0-R7md5sr&J-|7 z^w);U)bQQtg9Kg;(WTTl$H?HoQ1|X=I(<&^i#2Ldb_xxNmMhj)lca z=?o+`K)r3;8(n{?lH!wxfl%J&ok@D1U-i-in(LG)KM8x6OlW( z8o3DvxNwRr3+*7gNe6uj(R2W>PNor?6TgV(Hy3@%iOqb>kX=IL9UgzC)EC@oV_XG% z?;8A75FVo0v3xbF;Yc@tC({9Sukp&z3rgN7yC#?iBlcv7*QEoMb7_Nb9s@62ZALs8 zk2ff%yJ~ClKAQNa2@9M;oaak#y*sDcz+JI2JM(WBgg1x`fj{|;TmZ=fOL^fO1 z@sADhy=?OBertbyYt=H}mTi2Antf>cuUObO%r8y8!*Peh=->J&3)aUKxSTu4wE|k% z(T*ce>JP2f{rm|k0T_NVUP{ho?H6e3)2U{H9#f8%4?amhh7=LBVsCY^fqWV1^p*w~ z86Y|yEDt;6Z_HBZnjON1EW*nuW~0BsMz?&b^C6-XKY43$Ksy{mIn3OvS1GS#GmR57aTGpqRO4kC7SiS^pP)#O=DWbbSgNuhz_*$M6CU z!+pMcK?LQNl$AD66?O!NtJEhkgkRcAFO>KarNT80xjd;v26~fVcm?v4pKZU+Nr`gO zS4gnPH~8j}DQ$fCh1Ofar8l8|pNZ_-h)-DUd|)`BlnJwjY_f`rEk6dg7Gfc(LqACqqn+^4#T7kt}m# zg;r}!oG=1YmjE(ifW1N9wj_8lP45um)4h7Kz@*lZ$}b$i=47fVYIvR_?uY!E1IYe( zVBc^8TT`rXmv(vX$Zx2_tw75J3vzrAse`<2?37q3HmKG0I^uAe9F1PtFh5~jPp3kk zX?h*NB1eYrY$f^EBS0fbyZP9^*^g!=O9Q2JF;Dy@AEUBJPFf11ogWAjw)TT4{Aga2 z$S@1~daOSvj*QQMRZM*HeT$Wlceo%z$FFwYaLwqV2ue&Tv69wU)jojuX*B!EX)c;3 zQwm|l2>GAx4dStx1qOI|3oHv}kWYl1xc5SL2Mh*uBW7{y=~ZlB-!D0b%>7l18Q&}U z>@XgMG@07IRlO&7JokhQqGS03)NA`F8R!c%Mbaq)v53bq{uD?=$=O2K?w0G^_C<$g zI#r4-GTI9crOfe;?XthJM5_+$7e3)W@9vn&)TuNq4^9q`aRUsJ!d_AwF{weUntp|5B-tD zJ}JLwW1EF%d^D0gesQ)Zh}DV^DpySaZKdGJlUGDH968FPmAm~gnI&2=H_ zMszKe2qWhM-cvcSbw$+o5de$$>t{=F$YCjDJO`S&G2H9Y5&cDEHfaV*+VG>uWB5`) z8IP%-Mi6;ha-T$V4|B4qaB)jCPGUG@D?)FOp>@Jzu(15Md=|?aebE0qD^>cGaQH!t z8DF|hy%ah|qqBG|11qJoTc3J!o#m(my&sRlB9$8o`AGX`eOu3Kt{pyP~Kh8O?3v?eQfD=*9 zO?P7|s2*zDW^)%VAa5#vobUDy`!|NlA)-IRWMD5=_vA4>SydM+_@Eh;@imY`FR6=A ziiWj4z!c<*>Ny>fp^_-yTF_ zTx`wnb8}#K5v2hCo?-cBSF+!B9;(zXxA-F zZ^Jb|zJ$%{z5YC~m==wLKk9l}^fJlcl`eaX^hnv=_Thp`eoi7qTH4>R65RLo=RLmO z_KLGNjt<|{LH!)2%xE1J40gjS&v6(}gJ3*`E8Jh0h$EF;OV`EI!2xTB#Sza-Pz((= zAoRNu25UZkJrVl1dw0a=T@6P1$~@-^voewQJLoJ$vhb#O9ALp@Ve@JSenO32ixH-%mHK<@JEndX40Yx^B?w8c z)X}q@Y)>MVEon9B{ck|FANd$kqPEn>d`1_eS-r?67DGEaV7pTqWtom(=@Annn{>5L__;Qkx1B#luNyWe1KzDm53mf zS%e)LNvhL3nz2nuU#nRCdVBHNP``v~Y-DSdCc|S>c_SiPSIP5sR3ko2N3FImI8{>D zXtr@JYR)crkbDPrY<%i%n zmDJj`G5+ypQU}hB)-?Q2nbr!%y1t!EYtSB^P+m?&%k^{dC=?wR`D1|U8}CG)GTiTz zGTUxAn-Pi{*uZB#mB@HwnzGY{c?@bq;^%8KZ~`3OOQJe$O3b&f*%NuXYyM6258kJ& z=062e&>fsk+93E|7WJ+tHWQbHzdBT@ih5WUl zZ`LI+k5bE$&Oc1M`OTJc8z~TS#I-90%7=WTjN}I#Y4&sB>Hp6CFC&J!%TR$6fFRSy zn@=Lv{iN@?4j^%&kc{!xfv8Px%a}en=j$|A4@!b}Dx0P_o0dydAeTG5V-htBWa^(0 zCJ&NBrQ^9)DJ(l#X-a;7CsHR~*yx??W^fKqw3-T!N;k48%iA!W%g4vw5kJBTH}&55 zyUH)K(79PI2F1Inul5q7I=>wPD5l)qR-nR4G+JzhgUqWluD za$IHm{`tv}xaz$e zHQ?WMV9L`xW*B2>sXRXH6(?I@B7D3*L?~;O8Gcl#dw@D{I_CI$3*&`>PVAwje2pLv zh&%{lKco~}VTNL4^xfbTXyp8&q8M^SrNQ5o(?L3|D^qh-vHx%wpk=uV6xnKLb2saO z+kI5$4S}*m5m!UIvmCAksewiQO?gOxYRyifxcj)EB`(#u* z%!_FqIvhaSPxab4xnTJb3o0>jhz}I%pt3P0SyWyPjWY3N%#f3+so1-K3@!Rm$GyoQ z&~hGfbOi}w?{FrH-$CqekXdv-M~h&rU3C?O@$3%hqY*v#LG%q)i7Mw3)v+&(g-=zn z*&m3LlV$~Sl5HuyyO;3hwN#{CK4wd9A(mx{b2HjL>C$Ab)nruLyIOR?;{&dSy&xnP zznq#8RoPD#2(W5>9=l_zL}-U~c0KE}{s=XwY2j6*r7@d>dl>YcD3eVH(zpnWH;?*F zunf_?n7F#*R(>I|_X?uxK7*>%eouaY@ijhHh7m+0Bp)i)E&k97Gr4E*V4-JzYru3K z9M;O>^W9ZqLP^4Db6kzszgoNAjIsn{1n{i4oZgd;GTx9CBM`~(>KJsK;tb*I@~D*% zZ?sy7=Ft_pd#sC^jz92m8WhNWC5@RaDm38~i9s^~*)*6$*^ zl(&jo{vtSPod*r5#b=l=@hHf^Bsm>(LYpC9)f^4> zTugRby7m!xCoFE70GWCWgQvmNpfaZ-$CjZmIpG>=`-TT|%sIw>FN*u=p!r66kCCf{ zA9_+}`IKg-=4oXhXOB49Z2o9N+)n7n8&lZ0gE5143zCEK?+0>Gn>d~iA>jF^Q+|8^ z=aBsrgfbhIO6NNh7Qn?6wh2J&T&^6=(zi*&SW|_b?0shrSY*QUKC~mH5kS~DS(U3> z(!4Gaarq$Xb)aFCmTm`tir-Gj8Ud-VB=vqRo<;OOll&64IhJ9_29-JlmoWOyn^X9Y z2;MLu4{d=(Jb4rmt*&S(XV_U#T&P~i0vEX*GI1PnhZZ}uJ5J!9WS;{ zXx>5HO1qHVl8R-l`Gq+m)ZqikonCiFkxbWrF>QJ`!tb98i-S;s$a!lDqqO3`yGH8t zH-%*a9icJ7n2}v1v|PZ*5>?DaT9HxoTmKl&d}n|na~Fn58813i&0pmVSCyhljuuz` z+BCQlbk8dC?c*Y5Hhu~wl?CAM5KNN}cPS-}l^jzK3vB55Z%?~8`6kL)$A?Z2<@Z^J zkdT=##vtV%^H8uZ{s+hH?kXuz|s2k8{^#FX7mLT zY!doYKF=(9zaXFlaj*}|wz&{Ga;jt(cOJz8FHPt%pg}w_{^euMsX#Pr^<91oEL!Jo zgh6_tN^+dH+~kUNYCqHi>gXF#YZ3QsXN5#wQ`r)xT+J24$9^j!`mE^^VI^X8Z=bvb zL^@boCDxUFAe|qttfs8V_Z)MsH@rRJq9f%I2r3{DxF_D{ zLUuGS!XG6r`3!OcxOF@b-QZws807qRim^#-wK(3?t7WtR~k7S6(BlhFqr=x6w-^vU+L1VBshG6NzQpJEC~23@_M%iLre=P;k6qyq55(f zo->gMD+Q+NByJNs2=HZ}u+sAQyK;yE6R{9t^6w22r-^d-5FTUce2$gD*I5jFqu)lw z&)rL2!%}Jt4|i3bQ5STi=z7XD77|Sig9KGD6O6tmo-z#!V0L04{A~oKB?bwVWHON& zBPJM?tv|{~b*OkUr3~D29FYylSg6UmAB*CdF`tiho;ckq2KER}Du;O>2{E8gJ9h^u zh5NNqpG^~(bcoAuJnd;R$aB<}HCdT>X>j7&XMU{he5mck(RDeIn4*#c4fJ1K0cKO5 zAezjf%a)#R!9l!K zH@~FO*fWmexjYMtn|hi;{(hW^*W`l+0B>t2PjJarDo0#U(L^ILBacIzb#MQjLU?jq z<))7(9dgbY>DIdkF^+*Hb~?U8hUM{T^1|I8C0&mD7LNB(zF&ZgouJn%8G5YD%Q@&7 zCZmQ0IRd^(FKIOH(1{wnDIQ<+3SHFx`nW)w0wy!6!wtU;>e!*qJ|l)K7ZHyy6w^C;#IR_-?7X%=DPfd zXWEM!7-C29;9FT44Xm1P=8&`)0z8V+^AbLLCW8{e4?l>-%Jc&ATtjFo8?5dW`eYmb zwiTv4jg7f*3^U`yd2;SK&Mupl)|UR}JOt=o=-;<^?83JeT-{$EslMJjl}ZTFaU$=j z{L;ELEWcrG8#P}+xBj64e^366CsDAE-BF5`{0)WSD32_`(wqLY6{We%R}-2#(-VaW zSfX2q1_8|(Vp-7~|6b&q_7^lYP87Yx_ZIFJLK~ke`=gHK%@7nv~ z_(@E`hU6}Hy4=Fv_o<>rLh0FXLBoWUs3lP2Hu8j^K1ed}pODu#Mt8)1EcpxaBrG<8 zSTUm_*?<(nA-Y%4{wlj<{fKc5N{9WcBcv>lKxpP{F}Frr^ykxvOhHC5*B(HYi$U^t z@wP4nB4tMXv`P;y72?G;o&Xn00(M(SNq8bmbYr$s1(D&K(x}%$ewH?n`1Ox(f3c)!G32=<>dDV*_)OUrTB7mOMyZ-jprsRIGGGJ%awouE@rtY7Ty_0>o$e}PH zQjn&bjJEcICc`x%8MLD52YwB7BboQ-%doQj5xlG6ttMwHo9#lLcIsK#s1j0JfKLga z1juiGRi{GiHLn?ALj+R$6fv?H7eKiY7Kqe`a5rhk_!PY$=2q&BJkVUHQ4ofbcc%9@ z&+drQUJlJE$dBl>rJ{17Qss=hh6awqR$INbRUJW4-v}To8D!$>3!o9G6;s-VPf0nHF3YIOBvgG}h6<@Da2a4Ab{rQz#YN>P?D>aQ26cTg}>8 zX|#w{qN7U{&!2oe-jV{)W#Kwhi~yUZL-g;>Uc15AOR_&shOK!B(=`f^eJ9>vbDX;fd zb~ZEvVND&)kE-7Sm`^0yuurPtk~~+)c?J6sUc-LF8m^dmk~J6v0@oA5oy1v9T~_={ z96TY#oQfW5*pqRt4(AgIPx0DfCC$*4H7-_$mZzIo5%VTaBN4Q<#A0@wwp_Z^4SS-T zdJD(%_hKE6TCVUiq_64Rdr|P_J_suDX6v8dd%n@(>yBEexTnUe#hj=eC(4>SrY3Dv zy=^&=eL3oCIjT}J`^mVanspe%7={+#7Tgh*FVdYZ*pQ{ zcnA>{n0TMuQNQx&hOZ2u{hk+hif8lM9cIgw-&04^*6hAwL$nSEQIR0H{>-u}! z0~~BekHss%jzRGZLzs!v z86;p^Vk8hi@-8`|hx`0$Vy#EV26_yT_|9*YRo^_j`4AY?gs-i+E=~}xaSRZUh-RQC zTM^Lya8hx5N6C*m;HT5V&T-M*$URasJ`DhHIEa7LGu^XN?{R6lqv$!0aHKp4|R1x!*g03k%b1?B<(3evF(6B|!3hvb#xjdwJu z%F@1K9~bTzDMl|G`}WvN%n?8CqSR$!@#o5?3hh2h&SvP7WepPF+XRE7#>n`-lYU^S zfdR>+rxtZ-b#$})SAbP~uAj)5?cPtagB{MJeHY7C`sgRHQ{Y$semXIH%PGg-f>vY0 zZXuj`mWG$8hsDd`MVMN969Ci}#Ial1|0q0(ysLn_+^6B4bJSKK(sP1t;t&y(^%pWk z`Te+=z6Z#VJCedvS@G31vl0kZQ2`LlWI!4Y*Kx>pL(8&X3xcmxzFhqVT8D=ojwGDF z)C|a*+2EM|{BZq&5b0{AIgVbq8%-Ta$Az0}n)6m;u7sZ?6AsH2UQ-D)zb!G<6}QFy zKKW%x} z+vWbAo8;9A?h3Q-6Z3#X*8IvgX~b>}qs;#J0R&1^ zXmWa{`0jIgiPm?{uRHSMD5l~<$*rf;ewCTk023bs9x+w~LeE@JrI7a0`8pVloo0o6 z2DYNY%{p%MQvk)19*<(vc|t>K!jo2rl`IV)3+cCEbXqs2?r%jg{$>LQp)nhmc0|%N0;Vvv1w2Z zu$c6$-8}?+XNw}iV)lVdcCrry0h0HG)gzD=1nvXaUH-00D;l|Mupem@!v-8T?ta#x zRkt2ZRy2c#Ml43|7{|jG$`^!NW^d^x6++Ib!1ih1VjogMcEY7TF%~l(<1AEkG->_I zjnu+)=JV)Zdt?gp1nA@lDRllR-49BWJdC-01oGO;mk-QJ5kxR$!UaxNH8iydIQYDw zqgOo4_vvnS*2J0q(E5B#p*TLJJo&!7Csd`vVL;NACBe+Ccl z42~C-8uMPEHmn7o+K(lzsgZ$r!-Jy+Jsy!p(LNCt#0|%Y0awo1ZH!TVh)dlif0h7B z({R1jc84w$kc0|lSOpd{{mqRE_whcK%r#%u7c$F^Eh}?{pF;j7qKeGcNBNkZa=+zg z@~bN2m-@+7;5jaj7jL}-q&CK6V0{rpB8IxqV|Y=wWt z0u4^}ue&u|&TuM6U8)q%Wi>kDP4%`zRy$0UZt{33A!g!(u8Rik@WpnQdk0eo2pm!I zZ#=k%_`&Q0e=X8a@;M|9lyEfIfoK_|FV-+sxHV4ECR_u4iMV>LW!=WyUnpq=#$^x%Jd?Q=RfEMH- zJTd>MllRL2oHc5qbZZQ_5ykizx6g%pkqh)MJ4OHxS)uhH#($lnST=w}z1jiXy;aYG z#MH(7B|WE|)cbLc=15RGmudGsu6rmu{2O$G6^~0fS6#U>SNciY`cTF1fb!F=-q^Qm z#|yAX2HDITFxhZz^)yulsEYCho)#pzK~}{9IQ7DI-L?-dpV}A$m;>Xwgowk&ECr|? zeA__ae8fLv(s9ItG}eLuU+-W1ADvKWd-yu@76-=<39(~X@kiF$k))7@LjM3kvPI!Q z#9VkZ^FqHrJVr|^gUg?%>EA$3a&n;!)tH)2T|djO9wx1dO16pt9z=NPxhfPW!Vehc zcV1hMua|=ZX!rZu+rb+5i%In9_7f+9(XRv^DR<8>uss5Zw>P|-vBj1*|A<1ZXu*mR zf7g3*lUjJc{ZtxGKPS`6p}@(jF#N3G+!ayv&Ni^)FM0=h zb9+9T^Vv7)_5DmI&vc6D?1IG5=ZR*DW%s(KB$;l#!a{}gW63~r5u1-_rm;@`;QjR_ z#0AaFSzV#GK1^=L&@V8c$`Mqon1av=Ix zR`MbVer9V|I3nD_B*f?ql^(Gkpx=?kYEH(X>r-7?csRbn+DjX>b;|jLoYRS<(q!3~ zWkfoejwnv&rx#9B_6=wJv&pfy)34vMT9^oh_{#Td)yMTKF#; z;d7DKW+sa9P#Pe<2`MO>oS8+72aYO>fHO(gWoHf19tjaVWeKaE_hTd16TKTAegMhDycrWv^B)BKpA{^whDS zvqb*#awmWpe#zJEpTOAFZC3zyFh8J1r}vmNP%I>ghxMPq=F=61AriNVTq11tsx!Go zWZMmqn_U96%CU>CNx2N_9!9{e(aH7BS`-4z!D?|u8a-@TtO=5Ky>pFDo{~LZVZWSN znoFPdrQ8fJv!`dGjG93Z&l?GMwX`rW-fny`-8!0hG8Z%QFkjZ~8&>3a&^yk2FnW%+ z)(B)Rc=MKUny`IOr+54MUU*dqoAHg1qDp6pnc(3W1w)*`P*waM(?~0c!$A09Y<0`N z2Wm66UJ{^t=y$>26XX|gm|XWWi&WQTUpW@4P=LW{_X=i(Nx+hlRmKN*uXw= zJ^@l9D;oig4BOhCU&gfV(eM; zw`XX&1J2K3)-{Y|b+^kNu=RB)Rv_ccl=fodR36=N_O=VU*>o(Ri!Z}iEjY-;roWlZ z^E?p=P0=?2xl4$mDBHHU+$~959>Ge^Fa{INawlwgH+Vsw`|3ug&=BDl%9Y~LSO#<% z5k{kMq8KVud1o7Tdq4K+ghUCuZ*01h^+B5k`2}c!cs=766=LkPls8x5Ii+~{gCa0Z zL`(}W_H#sA=I9#(aGp|s4Ii)?c}Rk5LutTrZqpFY@HzTQtC!qX-da&`lZHzA@~j6~ z$0*Kp?rN@TIKf%)a<+mzqM{oPWcX2kr9ngFv_b<(B9&S{0v~Ell!e6I9Lg>^m9#BFxZzbs`;Q+%lm(qjq zIZ$j3hol`qydxxiDm2{}fS`-CnkBRpt+?;3UgF&_=WC|>(DH|GyejKk)-TSXrNJZA zw8ZfF4F?xXxf~kGA;Pj2GJJeN4zs*Z_Y2=>mL@nMOyasE)HD31tJzESPVEl~9?Wy_ zkgAP&$nulH$H%sN$O=aI@+#Ix62hE4PtZyVc2jyAO1vo-0esZAl6pm#3I~>)og!@T z_8{#1abJ*?LE~6wBef)PYZr+wTEqYZBrgq?T>QExA&`9wSZ{cl^zFc(%O69UK&V50 zqcZ85l~fN{N=ErchCsu4ZLy3bD1TgqB%IZk&a_-UjRkDKnkAVEHu9o5zKzsGKPYSi zMBr2WRLEtLs5c~D!~XXI`V#;!J>Tc&aHyCVBkXGskE-OwMZ@njR|XL&_KJE>%dI~p zf`QEGDX4tyxg8{2WvOqSYZmz~=VBgY2)(I@j08$(7RjCo7FZJ@ZY@z037oH$}1 zyrVY$Jt+_MEklRyu^s^JV!Zlr@!WHx0j)m^ptcbS>NHoy>3z0mjE%la+Sz;c@KavP zJ|nFQJJ8&Ov}Ed07s}i-pz`nf2_fE%N-jVv`@r*(185u_to(Qi^h7#JDFF)y)E#8W z*T-iL<8+eCDIoKkluU4p+xLU} z#ffe<`RIE>BlwntqxdU`Ilz6iWq(<7;Pk~e1H^3>KelK2+O1^^ z13v8ZT$kN40vGn(vEvb}&#qBJ2yJCOeD?Va5m4aZ#LwvIJ5A z3sFff13=VJfT71cCg@OI2UPkD{tTxMpG}*#-UBvBr82C}QgSDxOn_plrhm*5G|4Lc zZ0&GYZ>Kz@gYU!oYm9G(Nl_8ffqm}Ff-b_f$P|9*KMyr1(zy8pXPs*Q4KeFQXfii1hr3*Zr;YffP2509n}=R+~T2k##DIUsFcH* z9#czQ0ZryeZ+j>xHqSlOF_)j)iOazY0Ma=Z1(IDUJkA@~q@y%wQ!qxHaYvYaPHbkU z3&_%`UKlwox!;X*eht>1+I#iI^Kg-eL8fXwMR8JtG{NUy=&KCDz*^!M95uijPSxWH z2}PX{Ngbeh{m7&55fSWWGHEK2WH0#`U}aIfn1d{#ZV&~VHQu{TZ1r%BVkx#$n2#YP zNzNdlzY%L--aeYJ_b#K7k51}N@@5>S>Zg|XjL0`sM5eV~^_QvVY;ISSYYPsIg+*hsPzM$_8=(~?FdT+mSr5hFPX@YjvVa*SZ}dkTMp=U>3+U&} zGi=wFUvg~aw{HWVi0Hi^7{h%FU`AM}Azsps^FSW@9>H4e+X%Db;yOJBuJInid*qyLT-`wHK9lAUG6vLR=7+~MtZ~gub z4Fml7XO4Bw@DXrGbDrXSD~&?<5gl;Qwo_RSKP*HMJ->le)BwMDg8eLysXDV>lw8-rAF8 zv_*T=jNns0=m$QHlz|1AQl*j(L?HlvcoehNR1GIy!iIrF-}?nPyD?#80O_^oq)W z3B*=YWMB_pAk4RK?8oPZ8-gf)yCYoUt$NH%;(1uU(k%0%G!5efhVr9G^8EHTJELT_ zV{#ez+_RP-d7Oi*as3ulN!sN?N}xVE?@Yq&+rPvU)FegGAf7#pWKFuT7_*g_<{gb8 zV+#?le?IMZAIHa@{RLL@GoiX1CrS*bHzC0U_NlbpRDEDte2p!?_)U-q^qcH!^i+DM z4tK&%Le{T)`0!9^Tflf{UWhs1h#fwqC*kR8d-mf1>AaXQm2F~qI!|twjX>Kvb+*GP z$xCk0p*6n0YI_YsTtC>10jWY=N6PItCWq%iwM6|v-*j`!3)p?=_B63Vfn^!VAH=!3 zbu|YR>mct7;Fgudc77$Wy&<060*2T?hJQQXz#Gd+-`_3`_l*1T*d+5Lc3kUT_Y@@ZjlwUgihoel{7CV8#C&=wsrls}8FPnIU$bF*5@ zw}NUAOPqqX%C9;(#415^nY23U=1|#Lsrn0rn|Oem{SW{TE&!-~or2Zpga{NJCz1;( zKm&{A-*4-`ISz!CAT5mmZ)qmjH|yeJS#J}zUhw^3!0x%Rrt^P0(IcnngA7P9B^Eps zo6;YBd{_1UR(8Pwml)ljrZej&1#YBm65yeK%+_GifDn$L>WuXkGyrtF&`>Z7y`UnN z{Nm8g@4ou$v@EoDPmBTC{sCPxY@Mgomj#I!m4CuZ!Zle$g`6hkhqq6dG~tsh5x%_pPbA#!eqW=mK{)ksnGv0)IRFmEu^y9 zS(nQYZJF-ft;h3vTl7N+?vO8>;^-c*=Nhc4{3Gm>d931Ufh4LHa!rPq-Azc=RTl8Y z4hKdZXZFhPsN5yo8Q%=e(Nhd1+y%*-NN#>@@FdAzdD%vFs+Cf zALP4LY`X2o2G5@vE$Uy|4@i5)Wr6~YWiEbTNXokDtYsp#wsLIkO=?*1XS)fqJdo;p zF%VBIpnOhNJ~s=FBh$s`y8?ap#+as1utu(n&}S=aFIE(=iV8i5{c}|b-3WB@2^~1N zzGuYo_4chZy3bn(A$$+~@+GFey0J4&V^(>LDLRX3>5EEE^BpeZ!H$mN%an2~d_|yJ4aQ{YD<78sD!`1ZcPo^ zI6(8uuvCx~WboGL%eo(pnqUTjIzI=&yg1rqY%uwV?n1)0GKoU${*9Z^7{Y}f0t|%K zNJ(w0^{?9bPzvSWA?8^1i8Mlm`M+xi1u=p$VK_cBx9U({KA#|1024r*o^@65C7wD= zHxC%@`#?#SBR`dv+7FJp_{Ht6HIC_mFu*~!n7kFGFb`c({0X*k(B{&*l032wfzJ33 z%@YbmsUszzHi3|U_>GOUIXm+yOv$ci5(&i(_LtTD#Ya0~%U2DY{FWa)kgJRtroE!+ z=asw!IhAW9oREQ%qIwF%t-0;7E?6PI8n~8dppTzR;;V_{!Ala-yW)PHuZoZk@XcBN zj%-1K7FdDTjGjuYDCqKvc9Zl|BFni5ms^^9npZ^uHC#r z0=zkW4xWZADkKTfX!s{Y0|ofy{oPzdCC>`|+@%sXineiAw27(MuC}L4tUPcZMpHGEdiR`+WzYX!KHtF?2m)RUf7A4reJ=# zAZg!fcWSpH$Yv^KxX;0T|i6SH4vHrwZ zY85q<>g~5tX%nY+&?lh_c!f|N5c|fYMxyQ8ebbZV|tKp7?*S!SQNh zPb)ltPs$m_Uzh5v3^)UI<#pMLpgf>kaxQjC`EfPvyQ(NO9J0@p_&*oxJ|vXsGU9CU zjPctWN6n0^e%M`YQo2bC_OOITu(T3}-zW~MT-crd6i5TQ<#4ihELL%yK7wy=&<{W6YI{Kdr?{XJ(y>_pS@cOV4&`?_xm1B?aZz{ol z$$+X>;>&305@_coJgSmgThwIW*{D+x_0LAKlehczv}i-eI<0fC&c+iLpB*8;ef~0b zp{2qL^!@>O$RK?|hG8n^(|s)8QqA~!jqynnBlSMu7XzI@1gQ7Fr^54F9bH18enS+K zt9G^+0R*s9WecGA8#;Oxxjd>>S-QxuT_j0P|IVqI8x#lHoT5E4>&W3e08>D$zp0=0 zt0QMgLU`B!`^nW01l4-2#wF&T27V9-(*D@aAQ&Dz$M*E*bR8b|@dm69$aL~eynFMW z;$U%7OaO-uCMHJ{L z#%bmjod>E2CmN-P$=@fgi+Uq-4A5MEnsMYc;fT3e$&}1IBYns{dpn~GPnzsS{$acO zx_K9@VLM%hHd2+l-HMb`=7!&r^fq&F3#d^ar<9EN+RQ(g%_Y29E}8zL6v}dT9WAwd zleyGjAVwTqO?37VS?#q57S?e0x+p$xV${G01E+BE-2A{>cdbAqC!+b?e`G$7xrT|?PNA3Md3-z$Sp`YOpop9U2 zb-FzC?(khVYtB{$14^(C~* z>jBM^UR?g|>W8gT*l&tVv~Usd2Uw9krNWHfwFu}#s%mIgCeNDLMD0s;%WX=3@Mc(c zvWNpdxy9_KFabKGdt@MHvyX_Q!4WgH&fHy-DkCe|BUA@tga(#U(MfcjeB8HBaOW@A zSIyDdQ*UKg^vN>Ayt0Jo4JKDmTV(p->3hg7r(`3r4>*<%#t(#?8`=9`Y(+8tmx0O@ii9a4I&+08zR@e z>}H+YPPyw!{4Twvi932}M z%tng=0=Tsr#SUbTOkDmj5`M9n8%+g{cPZ-Si2kTVOhwtke-%tN3g{FJQ*wu=xv!HB zdu&7q2?}CY-+%D~r_S=^y&8DSxJ*4?Y=W#^=MdwY7`ij|YYVroU(JcO{A>TXFb?eHcP~>q8p|3GqOAm|(!IuJWB?FRGORQ{`$y(3P0#MC`(K z{raeIfxpa`lR5N%{8>`Xune=O-Q@YZ5W{ohsdU511DT(I5Y&MKqvv)cpi;+@vo1(D z@zdj^4@Rz5`JGODdHgD-wl_IUbXnhfmB3H~!cVOvE>}!Kocj;WA<`aIs`~-YDtQOfV9@$2 zSKV#MwTFK&A~zVKI{2o)%a*u=z;M2X9%=zaJVpgIKT^|HS#p_B+!c^)hg`QHA_xyW zP+&L>@1*LQKm*3)>=hVk5n-5EH^9S0Xr3!vpv;qhM^gq0g^lA=v4#l(9@FB)(RU<# zB^S$U{--J-@(RRKlx3^jG{{iYlc5UW|Y5`y4Wa z1HY0bJawxzhq7nmMKAHCij8yom4_=6Z6q*4!<}>fqB{S*%NO6=hU_MmrnTU0_rF>Afo-}+0KBO0XlwnnulWqRzw-J<}0Z3p4bY60iHdn|3aXybUWuM52Tc*01!(HRyvALhZ|nhyt%JRrj^m)_odO0Tt~O3 z2(2NMWi0pQ8qK!fG#j?Jy7TF5niNW^kW@R-1GIi_9MD3k7P zR3)PF)1_V85-4PZdlnW%>}WOWkH8U!9GnCoq-2iTP-g zlpA&J8^+;UX1oW+dD4!$9h?;Un;kE4G+a@inKa6#$>7ffBGz#m(gV|MSx(g}(*3&b z+oUQ~=p4#uQqC$N?$y8%;C$(8MLQnW^IR;g>9a@gtPEK!{*J}glyT#ZE748OIg0GVyB=;a8^LH{iL!~}T7&4o^6}wF}Yq8EnHj8t+ z<5KxV@Xzq=4vId0Q3ubw)-ODy_yYLH0;f0r!vop%(H%t$<((ojSVA^6DTMs(zmb|2 z%GT$K{7Mjh7j&P^?d+U)IfYPm$X>Ad*xNamm=CW&3gsIXBBW0j+=NVs1jf9;Udlt3 zsrj(ePrKj0S3-AdM+CHJ@4BakQ}-`?UEh|Z43Jh_{&o$%^PTa|A|;jZWV6gTjI~jK zXA?491aGCIwszXy$7T-9)+Ve|$lQ0aCO<8Nt8a>&{CMYmSAu-x4iw=_0HON{w*UAH zZBipjOm3#ET=gVKNPtlOiKS!%_$Lc+I{B}x_iA<(+7>h4`&XPgez6PRCZ{UjTSZ2Q zoZ*d}gUImfn|18#a(Ay)T9Vk>Vlo+XjPH|RA|wS8LBw4i0e+F?#vs(r>;Z*bgRk{% z_m;t1ZGT(d9&U^LR-WfiCN=xduSv#{X2|EX6BloHU$Q>}rh{yk6!uhqW(6NJZyx>}V2gxIoyi!mav$Ph%#jyhrQT~h{)GKrUsroXpaD|nGi+qzdUNVKz7uaP;q^)l@*2qAMb7R zn*e|}zc_EAzL4^~sd6;f4X)TQ^m$=QHccT0n87@}N(@O2jio((RhYnN1$%}wcTz}u zYRQE(v_J@4&nPF((`#(w`|M(lTx*sd1pG)Zwf_j`O=CZ4mCUJ6PU`^QnB5-gH*dX* zOKPkHZa^E$Di!6Y(2WiumUiZRb%cKH^VSZtPHul7)wyfaR8x-A30uBBc|dx{A0QiU zq51v44y2tG3i?dh8wkF#8odbweu> z^e?`bH`-Y9d)b&VNzbd<1p-^4IQG9Aaw#NzQTZ6fF+SRRGJiM|;-vEtxMD=wk7sYm z;5x!}`^$I5!mQ`Ww3!;*?cCL6g8Buh6(Y*a{(5Btu-l1`q#FKRw4v`a6Y7=N86sUJ z=Y(3YnliMv#P4=|$$XW6Vq=aaYx+7%9`$z^-sXC=SDbf2~hrNhtpv^kp~QuKRe61G_nAA{Rm zG|cA%M4rBV&iU}BNX0kxhrjb6ygH{6K%@Fcn`Y|h4XEUO499$NQ;W`Ug}YVuBk{QB z`00$y{LR^dTBv%s8evjjx2$I}H1|xBW^d`5>=(}abrWtWDL_9pF=**;H&TJ!jqP+> zTgfT8_IvT8$*BRF$M#E25htsHZuJDbkbAgs(^}B*%?3*`39sV9*3kIdE9AGM427f9 zKp4^v3bjZ2x(;3ra6-I+IY*q^K0!Hu(xz13FqO}s(vN2g`%>et=cx!^X+9=AJ)aJb zd7>u2{WIry{KuHOgpzU5>xR>Xz20f?y+Nvm6jU8#NdO_wv#Vy32C0^yj`O_A=-b6P zUZYt%$A0l<(oxrq1IlO%Wysh%otmx1$vme-X0)|C9ks}DQ#u_Y)LT={{dA~YP_Ad0 zv^}(53^|5JrUfMPO8$Ic;bnK3fNd!EoW*KkNWM$>i**8d#+$h8iujyK}$is#pxi_t*K!#`ms2 z(ccQ|(?*w1^;-t5GL-Bi+k{5&EV2|ofam9~HZTFfE;*L%#cRtL25UVC_WV~_+0ys< zn39G)E5VKl!lIR|5z=pK$_$9<*WuaRN3oixpR?cXmMD--K1kEJep)u-7w6f2v`k=6 za`#4|4-EFASFc|jK3XsDvBK4NB*Ftn5N=BwxD%2DYC-z&MUl3PM=(K^v33Um!a6iLz7jG!`9h+HcDFNoOt$&q$(iFU#e)8UL% zS*Kv8OreiOJlZd?13n3VM(-{7HW<&D5_ij2n=;l3ek{yiGhCRb{;ogQ^@H2od`U1$ zj{srK!Q~fi^V6~{Kk0(#?-rJ}S5BF%=A`c7X9oAdJj<{GaJtDzjERg}6=h>);4o$3 zd+~x2PnOlHfh_4)*k`@@GFT;B!n@%tFOKj}cLaP`_^~hn}s3&@+$5TH9lIZ3ACC6Mr z_A_z=7j?Xx(cW>}8X2Ox2$-1|ZG#y<;B!*(`)gjqA~v0i(PhXco{X)(nhReV=h_2D z)^W9F*(#h@KyTNt*3Vp0OJV#?0u?`t!4@SoaXL~8A0%yDxakJ7ojh)LvPP#*mOLtO z1wP-&c(i+Y4z28z!>E-A?1G92xr&6#W-)M@#(Ed6xJnZNt28yWGDlG1)uUj5llB1y z%>kqlSQ%y#E^idrSpgr`{j*JQyT*<;yfC?1;+^()%)4{6BX3cS{$_HO(+*^%aZ>j} z_eK~3n?9h*v#t(dmGmCZPaqs5t$bktd>ZK2fL&U#WfX2fjr4Bukv2LFL{`KV7J7Z< z^Fvo9i;QlxlZ~P{vBC+IIjUS-5MMN_3|bh62rE$AujkBGKGFx?r9B=rLnl)gv(z5N z6jLLG^sV524H@4~9?xWmqfN)!kl2M-}w}G_Ssg!uZ0l z{XmO^N(!0<#_0Z?6ws%}&&`B3v%#@44^!HLSxS#ry;YE4Oaj)gO^|Vrf7HjFpJzm= zgHwN}Y~~!RdE*!D(RV!jO(E1D`A1s2Y=(jH9Us+2^e<7r&BxJ?PU)$-D#YtU)~cbR zE+I0LtpNE4-588#ToPB+&%u;C5a*QAw zpX%*cTt15d%9|GC=@+ejbzQ;uroFcfYXazzzuYMAB7H1-j`jCYWs2}uJrK`*-^LggSPFn+$w~-i#9>tum zv6wR(RodbNusUk)HhQAew&G2@i03*?rkeBgy&*L~CgNB{z92WDhYi?89`!rZ%uqnD z;k>p-1!&j^lOD=G$o!o04hw4$cmEti=}mz5$+ox=w4(*|kuXlJAcTI3-#H&>$Olzb zjp&*OV!X9rvvaGuX|Wj>b3TOkC43Mk4~8;K7K{kHM5Z5IMtjfE&;0%e474Z&Wu~K0 z2rLX4tOVq_eUvdH4U+oB3lgRo0Yd%PzIA3No~-SysXi&cZna#p(d-}&_$+=2=Gxv{ z`wVP!W(>|yJWC^ye7EsO*7>OqE7;xXWAV%d5;Kz_`lg*DN)L>^YK1o9wYu{nnz9lI zKNxauXVDw+Th`w}v%Pd<)GL@7o;opHo+>o=t0f3QF10d<<%FYtfU7+td>Y z9V)!*}wN?dab@1&Q={AxUI;yT#J!LRJ>GT;-lIt+TC=6dyf?nc3q2M}={uE^75^4IDPM49 zICs$NwdkCrxZ5hj2dxe7I#RPYGTaHO5q{1+w0fPRpNUpbb-X_TA>Pq$6Q40g2$u>O zkVkRA9CUtCm=G$>Nd7TrP6T)x1CD?u6k44~6ZwRzaH6ti;Pd;Q`#7=F zJ_Na!%4u->n6T9VhcSxX75YmADJ#7FkEMNpNyF!H(JODYj+-?O>PLB;5AhmM0`wbR ze#Rc-l%o_^+iLQ}gj36R5ZHRQCVTcGw4+-C7IAtks`FWdVICgr9zEoG*S#}F#+H<$5|hoYIUWCkiAOKe0yVey6p%+XR}hE zy?O;ShGFr1mHW*84icpMH~B(zbzOXUFDmQW>vm0xf0%(|eF9W7I~7$hz8JB-^rma) z)LrE;xnULwT*HI2l<#(3+(kDAv;GcJV^|&%Z^|1_rt{Lg3?+j3QpXGx0S4T2(f-h@ z12#YyxE4CPeWKn19Diz#FJ7JR2(}!V+ccBnBbrBp2nPuvjPsrAUuCeM#N(e&D%tt% zukO5~Vs-?n+NdGov!CD{r&f~OV@nyd32}tFsmo>BN}*T;B}>-Jha-}{+oYHRI0iqF zuG8+~Sg5*T`?Gyc3fuHpXEn(F{=5+Z%KZmi!Ocy<8XS(W$O@6E#Y2oRSuewX>G6{#%D8i3w_ClF(%r8H4NAf6mW?o^v*Jza%^sY z(GE3_mo-kNp9aa^^n6ev$WTc4gu%N|JNv=N#3x%`U_|8RRN&@sLUVxRrCup*W4~Qn zmzvJ9e_w+``L{pE=`9s8gMRXpalxG5KHpsC*VbIUn@`BMybj}dUjS1!~2FMWa^t`6u6}o%jG<9DD%<^-I9e-tU+QQT0aB0fvfm0u2Ghl%pbNe9JWmj0a&v;lR(qmU;__Tuih$LE1@ z2I%^txV5$6++&8P8PLgn#ltVf>ynUqKvoL~fd;+5MJIhBgZBEc&++(FN03A5R?t3b z=j^or&0S~^{mGg$mr@@S1m@}~=w55g!;&|IEzLKG7`m_VD}_vIR+k!Rx4*CsrzAfVjp%MnY1Ze8 z+p1o5^oRToj=@SrtbS8}1H_x}VmPDteZ|vBV}3^DtD2kTLLlg;F2#N1ab^ z=bg|Oc)cRynUngCY2w=7SGa5#BY&vxkJqfF{A3{t2>epMO5Uzyq02gd{Iu)eXYs0h z4L933i2zv=(cC&&IrA~4H;KEIaWUG87*jb>Q&Xq69-&yumf{rKZL#VD0c@L2&32W6 z9G&v@TF>ClQsaUW7=|4;$Fo^qJHj)hYjxop6ypN%tWl6enR?77-u9IkwD3sR;c3oK zDl1qlDH!nBga#%>Kq00P`wb`d(*4~TFG(THj|a(Nj_&gn*u2KYjl~R;_kI~GELUX& zvJu{zVagYj;R?P`cwd zt1+e^b>F4I9VvKIEtR+>{E8uv$z1%r;~x8veE)&OmsuSBbhtzcNQopCLd3>3m~WlDdVM; zz8ZF>E+}4KFdi|ra?>t>C;7ul_$SxJZ7bNf$e#u&i%*4} zO92?8rRdpZRNS1lEfbgsH09tyvL$4cVA}geW4sTTeAmKseyLXo%0$Mv>bau#sOr-X zJ07g#F0rM`iHt3vQO$-kSm$aX(BEV61sRAIZ*=(F%1!Y4hOK7PpyqUC!TpX*-SiVG z47mdspDD+(C1A;=bc`mr$cmw%{*8yX_x4+=i2&f%UlBEWQW9 ze$`aA+95*i8786@##BtB4|UZ8-O4eBB84k-?0xM!*=C5(l)VB%fQe-cBulr1;BNHp z%km^}#kZ5J&<_t>1%?kmc{K3bZ)4+PeVR>T-S>-p6;yaClU3(PY+9urq zzS(;ufS<#zHT^BRJG$ZcYd57URT$fc=R>CHiXRK2$gRvDsQs)zsRBZk$ZUjee!Pq~UCKMGya(_g$jRYU0%Xg`$QzvgSd$eI-PI3p%V51( zTH*=@H%IQ$$X|$5v~>m^jBfwXl!M{StEI&GrNGd#J6%bZy#_w^|a9 zxQr{j_RJ=8A$rfBbvkMA!q$ooqtC7xVE4kgmVbEgtcfOb80R5g>u$CqGwd$rPD-q4 zu-Y}xnWOOz2aXTRM2yHSB?Zuw&yipT51n>DuJE+v)o2GoXnOhFeHj?=AR$AU)i7Ve z%wudnpa(5i(l~_U2LMQeuL06abiqoUI5JiB++Ps(%#&QdW>yU?*6lJU>1GM&09uycE0UJ7c3NW7 zc9#z;=E7G}2d)}d=Nj$5lJ?gZ)QyGfE%vENQ`vIdA_B#S3y!$I8v>~_3YNVYf%>HM zbggz46|)oam{5fQq9V4DTGWkmN{!Y@tKCQzC3mUcDU*jp>C7;`qzaEtf*S69G(fXF zWBcXMzYhDdtD&&8A`-dztw&DIDhxiKj{U8h^rI`yS^;+J(!+4BP1DC5 zTg=&=WB$eZo_kbj+mse^$k*FMF`5eOh!^+oGJ19X)#pfhy$}IdLMjXIW)nds&5eaP zoh7=qs(%k#(u@Vk6goNR(m6h!H8_B6O^jH04d>+Bb)+vMoaxSsu3wIN0gEOjA zNOF1r1VX1(EhOZ#i4uEhyx8SmEh@Jf&}jC4Jp^>tNP5LClRxQlY0my;7jZhVW{ghy z{HR%8CUYTLIfL~}Y`d`{9KNm$;v<6Cp+@DbT?E>fUW1v577;0Q^M(7!Z;46 zz(7AEH+0-^WrSQho5^zPXiAkI((l&Gdy0F4z6sw3 z_*>g1^Y$>nd${vsvf;iaW_*lOtrC|=(%JXT{~hpZOcX~PyNu}@IkiF@9lZJ++h#4x zCUDN(=fS5V_4>%@<8s8VQBZ>~u|gK5nemsy5~g}3Of7cMZ-UKJ-VlKLv{{`H$?EC1 z)mK8~b5Ow79fOyng!Q+j-%YgBlnbG&3$}Ap{Dyme^Rb!2{wKVeK#QJT9DTZI96ODF z9=rqNivklD?Bp@XMV)KG;!ws8nCmNaZ+}EiahT5u(|4uRk$4;{e)4$!I^U%V=ukJwv+z&3Hk&rv@fAFXZ@z9Bec z^UbZLk$f}=N`|}NewY5aie-Rt{%pNA>qvP%dzxZJ1tmp4>{8SYWE5wbm*2sS_K*)P z;zMl^-@$v;&dVh*ic0rQ#{~IISG^(<_12!_BM#lyw5q?4gbLrY#>~>vr12-@VhL=? z(m2gw>t<0qW0mZtcOT=r+@nw8;$&%;rGAK_GMv&H4O9c7KneoS$qj9=O*xA zTY41jH_b8RR>)wPQm08;@}+$T{ z^BTM>*SLnPNh4h&Wf|!|RmQ35(T5Yu1T3|{6&tgE->zcMl{j!SjTg*`}^CvfMY9VMEa4lpuy+W%AeRi)nHVb^2 zwtRTsW3QB({m?Nt$v$Nnq|>u$h(cpr((5OS3>iL^D1FME$yKNzRGfh|lRnOrAS;C+ zAr?bYM$XkUA>^rr)LL59y_8vZOo0bK+WJ-Z(Ml`m(!l(l@#t0zr11P>L=Z}Xgj+%LtBy3#F6G_DAMYc#3UW7Tdsfxp4*SUBsOTsotA-N!FmVIsPRT>wR>*1E$EpI>?B=oAf;V_Blcru zUn2#5&H2<3Gi$M^~5zcRjTFP|`s7)2gz_-RzCzwNeP6@=LS4{|lJ#{O7Z4fgCD@Pu8N@9EXSo_7c1YYhI^JSnI z-=NFie#~bK(LN5IH)!@N=@cd91>71QLU=d9D@g?*BJxWY4@pUZX_l5w9hC;5kcgsGi@sfNhEfm>Qv@E&7^^HHVs^sggWMo{LvhHY zD#?PGsd^&C5$!dZxK}O-vlDQo_!c^)*WL=JKv7TWlUrm~YORbKfg;iEqHj z9EIA_z-a_J=3J@YV0l5;bA7bj+{id&GIZ!^Z=JM#v$DJ~G`!dyF*L*p*Bgb_L60k= z0NT`LKDhDSDZ)%2Y9R;J0Yl>w4`872v0|rB_tpm7(0EOeZMH>CnvGlTn}1E7b6W5j(%WRD94S z z{h@~B=KcOGC*GjAJw4yjwmX>UQ;2@6->-Ix{BbE;7;erUG3Gm(9L_s^o@W1X8 zj-`1Gf4o$lAyJSG9^8_{+g{)F(rvb#YxZeOWfQ>l_+Sqzdx#P+zp=AhQn7S(z+F5^ z=21b28-YJ3uWE)7X9@9L6}WxxpAokg>9)-)0LCAx->>+MArcC64nNt4;vO6><*9$2 zmNNLgS;`w*U6ldx(P8NFdZHp($-y!iOaw*DnHbGHA#|&`E1r~I3yjb(2^E&thI=9j`x52QZpGY*M zW_E+DYzuJ=grUtf*v6bBWu!bUZla^k#^_&eZr;$rvT^pMUcM9hibe}|aWfWsj^;|i z1%+&N2f7awAZ~K$bHZ0lBCn!mQ1@hHNTi#vHIkDM@cHmY4|;tb6VAcEIU1FKg(|p8 zpm7KSZpTR!uh1-<$yi=nSrMmrVABOccK}-RW(Ht89p}+yF3EKI63$QDKGCdD^LC5< zwP*DX6#$dB%B8^(JrOfR5=^`+qz7=3%Q2rHq%`PL8DFPv4PmUldBG7k=G%9R%7Zj~ zIfQ~^*7+G}b(uz6v!g3JdFUcpvQx*!W7+CaB!!7^^7kFWDS6_w6H9*4D5!9nXAwaC zYKJuX_PQej<(Dg-xtuIOOumXDBCGuS z`z^Z>08IhHd`L>wkc$+f%GZ&Xk4RIYDg%`5+-kQ)CQW0bHmoW8Rr^5m(w9J_=wu=W ztgF9k7gP^gihGy&R}?a5y)*4re8$RSD6SM@h0DV$*#OrXPY0${l0VHtApmKts6xy6 zq!vC)&)47EZpg;hzFTXe{YqzFD1yb^nyQeI#S&B_QYR8T40I|3Lcj6gykPZ7))EiBo;}y~}g%J)!^tIp$>AyAQ#mk|jQMN0}LUT@9|XP&s* zS2Gfaxu!U;9pL^g%wc*Qmw#l2+m*;MI7jp=SYN?O-`i#{|hRVXQs_3i83u-c?$EK@d z=JZOv>5LAL#~M!U*|P~`L|}(K7XixGNv4EyaCS#3fG$#T2B) z@)N9-VGip+?R-NyFlnaVf_)C5M4Al*TJBLOzaO~R>QIf_Uf^1a1>pm1+PY_k^6CO= z@w(RX6xbhQbz3P|H(x7w0g`b?rgDU{F7kE4BCtKd%(HzF| zSKHJ2OJU=pbU(svXrc6&{w*jsJBQZNNWYQ@2oLpU?=`B|zctztvf)Va<$>w#l^Ov;6@rl&v*w@+1S5}Nj}g$EIF>|jr;mD^n^G5$>}HUYik4we;9 z1!dlR=)#Z3+QrQpkSZ)c7_m(R!e}}}nwAXNwKyT{O2uz}&?630J(T7W^y4SOlLg@4TTiwu|)FFBOcv?5z) z|C`k%59wdYg_7;V4cu~8ja!Q9HE@$u4 zEbxJ-1O>mLqzXZl@4?hA{*^hObU9&&SZN^phrTI`V7;~?=?nR9X&O^+z! zb>X(|m&?Nn=KbFsGxVIzkmivE`HK`YBNFl(;l$83p~k@mtC9E& zh0Q)zPprIV_i>8JBj1{>(L&m%fVgf>ZnFJa=MBfcf(Ok==GrbUSJ4kvU8r59s2_)q z{NZ?*mlGwxSyB$qrENGr$Vq6E(t34y&HqV4G7q-CuMzkG1x>rocnXRsdm6`pc&*=a zC@t?<<2=fLLsqj^W&XZIGHaIc%|MQ(*}5N;zj6I=Ocx|X5OsUb`*}C=NjwDphjcHf z!_IaMvThuN;B%W0&ugBRRe2-G=iD+lDkhSnWCrc2K$9mcOe+3T^{Q#Ew1p*cg*}!= zo!rbzsp&~!0etkhsqr8UizbWU()BtTwub{LKN@e?fBPti5@DtLxSI1VDX9ZiC zpx|uN!)S;#{i{#do^$wCJ)Cr${y3Twwzb=)H_ZB!?eUmus=xCjCHE6`StJ;FWe>C# zlVk8=!VAlI+mDUNv$gl`!}23AJ3JVHA*HPFSn>Rzs@ZW8&%4SC<4o=6_OEZZaiZf- zg)$Gc5?18={uST6s7V3xO}{z1)3FoqY?VQ~jIPRbSxc~j12=q_zKmG?;{m1{vi*o&s|B?00Y$jX%k0$^3fBrpz(SQEO z|Cs-lybH8{L;i34zyJODhYCZ~|L5_){{BP#*K7XI$NzfG^RM;%=i9#z|L6LufBY@_ zx4#wq`#<-k|F8e$f3Ebu{qrAX)A#@W&v6=yfBs{dldRd^_QL=C$GR<3SN!`w{|of5 zpZ{F&Z+$ZsSu+3oKhu5N{$ESmsTu$MpF^`1%l}^fuV;Pp-o2ghznA|1*Yj^@lmE5t zU9c_5xR%p0yziB4OLP5K_`gH%FZ|#Cw(`CGxBs)N-hcV)81g^w-rGM^=s)kEw?qE5 zh5xqf{kt*6EA&&I|MMS5k!{oRZ^cXE+4jFS|9|bFf8Exj`j7wX{{c`-2M8#0oBfSp z0RSe}0RT`-0|XQR2nYxOc!rBh0000000000000008vp|N8si z)}#96Z1S!C{Z|zFD@`F--~akcT{Km_{ij<;Q=II0{r)TT3-k*N!#~eoe|h>C*57}v zw{7}Y5IoQGuP6G~aj8HAf*|l|{NJnm>(w>G=fA25(=-kK@$RpG|6BD-*>v6SzYgP2 z{QAqfE#s&7{nxZhvd^C{fBW~*KAr#6{cYL1r%G`Y!|+xL(D{v6LgkM6gBRCw^+mSkAVap`~m zwNJLCx&9~o7wGx<`t$l1?LqL*;s;O-`m=z3x&Q0g9YwZ{%Wp+l zJ{SK!5b*DR1FOIPEicOT?|=K5i}lk<^Yrrb0{!P+e?$KA8}f7r(f=r)=P%lG|IhNd zfB(_spDjI?|JL+>0%~8n|M@@uX#ZdS#Or@Re?0zu;Qs;3U#b5#6p_Cm{|QAHBL9N@ z8-Un9;fVdCfXJU&lK*jm{r4e@{U1ga`wwKxKOX-(vhe>gT>ou%K5#+*1RwNg@!z;R zsekSjC9uCBKkG)q|0rRE`bGPB;~y8#JO966r2ggq`#}F=>;CWTk?z~}UvNj?Y{l}A z`+vOZny2{vSLFYRJp5lU*Z=!c{Du@T%vkw4Yxx#4*uR@*2DW>Q4nOWM4t3KUXODQy)BKs999yxvm(Y6KwyD4d$0F z(z6Tv@BpIqM#JG{B5pC5#k%P8IyTW5H?=x4POauZtNCk&)_Njpz8@TN3(+wUaXU2D zbg6z{sx`67X^UO`d5zaJ%QrjUbF1EZv)<%=SuxfE7=B_gRj6B;7J^fnnFLBCsN*!0 zu84eWVr~Is^J!$O1ekZB2#v3HT>Ty^WOLOucNBa4c)01x;u?!4>H-Vcyj6I<)JlOC zFP8;osY*4oC3;v+4C2bu#htPDW@c#Ww++~@@10%&5nyG)BcUYVUZMz@Q?bRY2zmCA zP+nr8u_xmUP?!W>7#cXsxxECM8JyaYg{LW}w>yEX+RE@01e=b!ezf#83Z1PqtF2Zu z$YHK>3iRva)V{#cf}=3wsGyx^zegY5A+BgfbHU~ola*ce>jB7RR8Y`+#BvhxZ_&n0u(F#tG>2NuqDPp%{^C18%X&=JC?Vpk^G z&?fC7?U;nd8pA0c3Y;7aqvYuQ#Q^&|0q)*r0H_Ey)@&55GDJqJ{M0mOMrl25Wl+Y~ zLZ&Mn2@|dDIMzB6Ddwn!4$qvCE0g*)s?@7J`3>#T05d?$zl9yP6!Je!vSzuef3BTG zU*R@ZXcER0V{LLb+wR!7iDJHLxlS2dUKSCJZuu7HPF1XEWIlJEe$p39J)amd0fCAi zJhqoC+Wtqzu4kv8;SqoEC_xi(6iJ(KMi|yoL*d69H&^yBCzm;LL4tJ!VvS7~h4VUx zb2ZQ^%QzVIkIU@9xy+Fz=P!6Pa(MSLwq!K)t!y%vGj3}0J_iaT^P)6#2$MCQa`f|# zH`IC1zL!C(+jx8isdH(tQwsX@BXd6E(oWyhLo?b;!6vU7qB4o--g@4CeOlQz@Exb8 zp$X=?#WkbpU&W%*q#HlUCJ6{OI15Na0 z7R%=9c@!Q-GPB%WyCKKMZ}fCI7-tXe9RQTHGkO1|H{|P-ITd}9@qE@jP@U0V=~&vR z{23(8pQjE3GbrBrt!+^-NLPleY7OR}OlDDO>DQ^wDnsXSwBke7!OiaAh2Fi*kgKaw zn_oowak?qi0&%)lAA5=Gm1cp}->h^%dtdW%m2``fMwsH#gG_&A&e z{ajhn^xC7)!Ru1(#SP8;1pRmbdB#x>jVzc#6K^Hmb6}3n0?{euIt%vjtsfD(Toz8_ zP$q>ooE?~pjtK?)h`}Cu{t50*veI5?eReCe6ft+kcKR?&*QeUaQQqb%`GuqRvL75^ z?GX8)N(XA}2*f|o1EOdaS<_}oRGsp;dquqVSh#w~rGlc;L<(!4%3+wE{ z`L2BlTvhjs`vo6r#RHUq9m@jT%-zGq;XXbGM7@+1tzku5+WJs@m3# z+vyPin3|SYvBJ&rI-VNl)!57w9CNsG0Hj^}L81jiM7yc8U76bv*2;vvKEMm}Lc6mC z%JY)+%3SoNL02dZlf8qW>@?gY(aJX%QorI5_Io9P z`HqdHwK!58cxF3D(0oA1^+}!G5~D}xD0uwvJ81-Bmj)q!12;bxre>ObI$$pqci$Ar zqBRtg2H;->z6FWvPPJX{cwmfLmY-y9*chHJGGlRQJgeV65HLUd(X7X?%-9sCz2~8I zMg!^F2cNC!gL+02-wLiFovGN}!b3jQaug%p;Bpnj_2S=_rS+ypfAMNEtR>br&lU zPGyuNQ`?I4cwzaAF3zjmLp_bT(-$g~Y2%p-14?!%!y6Mx?aOxW zVo#9!`64!y(M#H9sAGDz&AwGAhOJwLzXLvL@{@aU&wQB#1JIX{wrJKQz*kz%5|S2n z+JPFz_)yd#WL%LR?#ZPUNM@Kn)T8_Q`)!x_H_6n(pylr1CXGOvM{SMo$Rp(1Hb9n< zkP*JWR&l{!U1m^dl8xE5kzyBpx?gd`_Gj#K6fTkOclrqevvqQ(xz6_MMm z-dJK1BC;wTZy=GFnHmXP(gooxk}`epl^##^Wpm2gxx9LGk5W7iBvr3u>4F7y<--*q z4sjZQPjv9%CjmsQ*A;rDa$=QgnM`6U=AD{H^tLi@>P=2!LIx6vU8_h}?%ZLkGfFg37gym^FAywzA2b(IMA<2+3@eQ2cNn7` z=i@1`pu-uU4B~$=`Y8I$4A{Gg&8}Lf?fizz_(;@Iey#CZu_#h`hOzjv4o)5x<{SDV zB2El|}m0%i8^AT_}(jdh76!kSK6DnMN@cl%eS0Fh{UULJxU~F9cuSe zJR;SSIy_7I6n+rQ$R`81>|MyB>&o*UO`CTL~+9ZyQ&t!>WH5yi_Q+(WqK(VZY= z^tI$|Y|7(rQP^qfC<`^?KH*5c7Y(FwIG{j{)-n3q@nw>1-((47_qyNI{^KvbtW?CA z|E5R+G*W3k$yi6v1}q$8>nyG_f#R**9L%2!uD&>wYM>QWhK)&E7l64rl$)6auHqw) zAOvoiPm9VX_8udnuERAMGs9m&XLte~D!HKJQ{2Gv#FrmPP7j@7kHyZA=2J0VU*|nI346AxoJ02b3^jHkUK? zwKLZen% z=)N`)Gn{y{>!}B%A*5+CQa{wN6WR#9AlBrUYB@ZckQ3;XPAK3)s?Jy7yC7(zY*(@& z0s?zVkRLWQ(^OU)Qqw@D&JC2oxNLfV4~VSK-u*h_Oz+20E6I1QTg1#f!-uyAmv`1v z6?)!u9Gcg$6a|~o-&4&$Iu|~P;(8VWb}}x-DIEj8^_h@s@a2FMD0s}j7Szs3)fANk zMOw9ziMlr2-K_e<6+|ROEL@iFfOu9Vwit^cwjaI*DWQvkt&IX?(J5eyT=XG_&)W-_ zvbB!&a=%5QQ~CS4740AU27dg^NrmX5gV*Bnp2d2g5Mq!&yy& z!SoYQ&to;D2OX;-qPz4at^aH(@$MLwwy|om1Qq#Ka<$Y3ewb12J;we?pMU_y+@oavsPb^6yA+2DI%m~@gE$U@?WkfAtE-LXuYFd2-m!&e69v*#`(l>tc zWa?~$j_#WAoP7|3LyeJ85QtZ5RU>6@+3Q&pj&3j3kR}mnw3-ObkR*+;pk{Oa>u0pX7z^=^m$A`RRE5$ z{TLwVBl3@Ek~`oRga`sF3WFTbelh5~i$crd#KCLrVTa7pJ1giT*;m!=0Lcy3WK(;< ziDj&Zdz01r98TK66b{XdMSlkc62^Ob6rXcgzb)>8sE$lB^8yDJ|CGp>&PF{?$cH1z zKk!IBTT{6T&X9nNlQnZB5#Yj>ERg6R03g~R)zyjQ%Ae3VCO1g<{yv4q5l8F612B-O zOh_$tn5zCwZ$3Qj{>Fxx7BGHt8z(??GqUyUux}#YCbZnU?yw{Y2Y78u+JlEckpdYT zcfa5rb{@+S@{e3xPp&Td&miB=iAo-);wv}T5}78L)g%I4g6KSuen-~KC$1ctqv=bl z=mA35jO8qLX(!^Bg}&i~LDPMId2vXT44uCgt127lA(|4WXlYBR~40nK$zzD9%5 zz^h6>(s6y(kJk_P{6&x|*rxu8!SCo)-{dRAZ#_aoqYePwtXNF@fQpFFxlYI?`$)Xm zQOGlo71<4*B4pC`m&{+R+KT*bNe=QIUZ4FTl&tgvwGs)3ar{8=+HzP62-*D}y$zPudQ#4{!!mO`<6%SsqWp|970mQto(!2g z@Ga~B@kZ+sOMra$*!cz<-Camyse<(bK7$^|#y8dP0*cP*lcx`XY8*pPgZ+FIa?w1+ z0k;^xDwu+kj^cJ-P-hV6fW%)Ge+4!Oe5ssy$uAk>=pcIX@Z?TUl&8m(j#{h4xwFO2NR^To|{Lhjzjo(0FG&ZD3SKl6RrZs zbZHousb#cd;Ow|sc+|NuCm@Fw*|*>^^a-X}H3Qn-d3W!7n?4P_8X;FjNxW~3zY5&8 z&nE~P2fiz?<-bZZ^uK%n=68jNYytvl@NKF<(~K8!mD?d3YW>azZLM7XrEfZ}gp*t^ z4Y@qYWxBRudc{1m>|dGkYh`54JF&?VKr%AQNSloTeI|47$#AJQj5kvf$$Cz=RzdE& zTRQYeT=xpkI-rK=A<0PzV4;bJT(guBW1yP(_RbuB6F6xl*TSVbgbwRG`dr2$#z4cn zfxksqFHt$QOUCZ_6yhle1M*0JfM|_ZjilxrpY@V4RBT7QkGDNO%79(nedG9{eEB^c zbHKbp0vr;bP$VM%X!BVa$+aQx`J?Hw)Z>Q+BezGSGSS zQwUoAO*RTmz4a<$(?;IadypCfKH`S3J~NnFQL-*?1k-UITCAZj4lLT?4zmgB zHiCA2uq@It+@0`^lH>;A=bML^a|r4Z4a+CDcmgrP4s$rtmwDehU-)Zyj}gUPtWLY8t;P7isL}RJU;5`gqd8-ykU)WqKywWpau!YsS8X z2GYCosywuNCzP=!|$n-Bpi;ncS>aoZ7!)vTKGywxukCA}vq z3wz`1Mu%J{f#YC$3DAA}w!%>^dy` zLoAqGrV4fI&#>o|xhbe~nK!Xrien9~Zxt-krE4IVIa1KV7< zrS|Qs@Hui4$Yk!~a(dF%V_NcbHY=|a9eCCJW4?}h7pzxuvW2R3X&B0HBr_0L9WQTH z+A7_)*$3!1*S*q}J5r{BbK{Nic}Fj@H)pyE!@(ySqhLaJ75rk1nAQ$hr3CE0SRH%> zjXYf*1~^BZ7VKIh8IuzqBELU zcfKwy-fD5&ei0^W!ZdgCrdDfc`Kp5*o^$ehC>a=I$fYb{IGc3wea|qJc^}a=wuGkz zkwh184{gXbGVz9(eA$4V2c7J|22P!NUl2b;FP5Bib~jbm>HCY}U4g82BkQK-4HMFX z_9}|2Umv!!#0C|<(=rPZx};t^VmyZf3yz~$BuEX64Q4@739|{w;#V9~$1_jaT$q9b zhq<4G?lvUQoNmU&+qU#`kims+^FYwc@I$X&w9DCE^R?gi$u2$lq!MU^pA23)x%pTz zg3rbGB^#K8?DkY*DeJSZdf6Ii6)%}`x!zdXy_eS~CQ@&Tz+NS`UsynLOF6LBM3Pgy z>MlsyH|E01<20vj4rxy}F`V|dQN^`uCfHL7HxaN1du&4Jx*&OAuHT%eo=p^ z`C)m_-Z;S*@-FLH$8)l;CSr2%*cl_ zY2~xOnHy7&CE0G~S;miND^NmvT84P^cc^jS{|-(y@sE1g2#hSz^c-dKE$CB6fC(S& zL@|zL#tzipW48I~q#S0ZC*MMaqC!MJ(wxo#C&W{|^jWfidjG4SHtL zCilo%BTJw{HAZmojZ{>ESE!_oPlCn@YmTL|5sy& zdx;cH1g!wdl)995;v(-q8MFfk^sORLYvYM@k(_MSA=hC|6_T?)bwK9b zsdj1F7`_SsnqqGbm3|hkfXpN|mrdm2-s>?v-1uJ2Z0f{aU(t^NL#*K@&Y1+$%$kSL z`bqdkkT72mC{W)F@pk#`755h*IMq5qQ^D*C>L7_wscX3r-t1=5lYIt#%PFsik@-lc znBup`#8Sz(dh;zoDc{~m2!!#hn%zPgBiwXRvwc6CB>ABJvtI+(&g`{5sH+P*oHN2jv`-ik2d&P-dhtKMENW(xoAon2Y2$ zQP4~$(uOb?vyiLO_rX!|cl_s%Ca@|@2_c5>p#PHReMi#P(}!)y^xz3}bG?ZtC%|mq z8u{qV02n@rBMi5c+e*HnH{C{QjzjOoAtKQ7Lrv_KwI6~fgF{wcN97VVZL7E6C z)Wp0Mgztwg2$zWSZ97UzD&Fs>umbS%#9#q!lVw9Ex@A^jzMvO!L#8P$67h+uFyeNHiy67v1FN&w?UIm zpXmwU*3Nuc5@qTSk|&lU;zidt&Jgl{z5lVQscs6baV zsNl^>dawFIJbKgs$_BW4J|{ypm6@-gfx2mq#=^gYk0i5wC{G=WJeNFM0&%ZVWy;>D<5u%bbR** zp}D=!EkhgAdV54suM7chbWKQbU88mILDSjZGCStP+_rLoBc4hOFZL*+O(!<0Z{-1e zOip}|P}D!CLaltUvL{GF$$&!xB$NX# z^EdUC1(xCAC+99@v^hr!q(%zmba%b zlTz|h7%twPQ2M5O+LE1EEQb#+jYGgx;pZ*)vmUzVbSDoRrzd_#o~dMON~I||b=<2< zCK4hGnn?@Qn1cperNV=Th8K7N^Ws&X|`vNp3=nW?i> z6zjW%h;zi`zDc%DE5W+~S65snE!JAb$cxPmHg+jKYcA}$>3ALW!DyzWZ)hfdzD2?w z)4iK2L-m-r9o|Dhi2Ww63a6Fn+KS~%V-1U_DcsJtjGUomg}(9lAHEKv?;MSuSyIqP z$Q2DYf{eu1u!H;JoOY+?&iE<Q&lXDfocewBnI5gq?x!;{ENzfyg4%CU zPFTf!qQ5;GNG|e2yYxFvvc=47-AqB*WWFz>;zIUe*o@IwfdVG<3FZ#Zqu>Fu3xG=7 zE`Lg*=o0N)G0euzFk!@U~wxPmk$R6T8dJAI3D8@l?5U zO0@=)K-g`_Pfdl|oV@p4+=6|3O5V?lxs15di}|h@5#NTv8kRi5L4Y>{V}kpwK62F* zSyvh#!)BtXldK&Hd%HI?#Xf}Hpb z0ceS0<v|q_l|-CQpVj(=IdlDV)$)@%$rGx(5I)NZNac*C07tjn0-#O zc+WhC$G0cd_O@7tDhP^XQ+Hv0_~Z9lq6C{NNoCEvGzhK4Jp{~}uAnoPQ=}Pq@Y$zQ z?2PGVt5%@fkwzn9Q#wkQAq=ropYD66yzPj=&;zMc{H>w39>Xbe?3%;8wq;?o4Q`Vb z&*V+{qACo>9nH1LMjoTdzC;wMR)rhhIN9GKtjst@c3zoVk1VBuQhE50e5O`iHSx67 zB>bFaHF{ES)jBd)XZ&3)^o3N{5S|@Lqk=Z9_(DVh0WJKARv1eX+51Y>SNx1oO@TD; zMn;$EBfZE^1zLx0bKZ4v@|lg=x;)Za?r$STagLE%N=KEU6`doh!;>q#dO8<3AVN^n zO%pTM>dV4i{Sc&3krTw{Jhf}CU|g10rZH?KN$TJmbQ8Awo#BNWRmKgqH!E@bPH`R` zP7eip;>~)gf8FEN15}n4&6Divd%M2ShWH|GB=hI>?33j}r89$9G~dC|&X&0>Afk58elsLq z0?U=qMQtD{zjRRtWy~-r?63MBzOQ}X9WuU*4*eeFD|7S%pXUT_L=X;1|NVsb$pS`z zdr}3s%#MrQ7M!*YIM;wW2pm9Ar2Lr*tu8`Rr8T(gQY8c;LI-<@i}<~g3SwS3xc(J>?iR8*7Hsv0CRsTXuwvlNtTnKPAsyjcnulblEmSL#`$!CdFspVq{ zL?5kcBXQz;oj%^re9uCp%e+Aw@K~2qI<;Ewryi0JKE&B4hc*<&AXN=>A6~Hz30;zh z6Im!GR%Z`L4T=F{`J7@7QIM0oU)f3!q8XRLRIjfyZR$wJn8Abt6{JDJ1B(^_izj9X zSnPjg=}gJYDdZH5c2W;zx9eeb+ACupZX|XHbJ9m*y{pH)^UPBfXs5P1nkcdP$QZ0D z#WMCdw1YtTFmJBKJ1}V4KHoFlL-xC~&hd#)7~ujN;wb{*^9rBU?d-y<4vs3D7{e9; z?QN+F^JGxxIU^GRk8++)hw@frb0|Jot&X|Kl*Wu9Le9cru|bx-=w7s=)x-CU0g&Z| z9eW7@ilTL<8C*C>6q+>=&v=hJQ4a$<-lIo;*1_ds-(e8K*QV=&@;QCOvB>&JmOP># z=W^h^Vp3ss`Y;T?dZ8Q!hnTrjpAyCaDFaly# zxL(b?-bV&ZHrj(!+G8yHNZdFPv{a^YXE=YcyS&=34A5>vS9%DSg$ObO37Gpcto}n zM@N#{n-v^^3_AoYRPS~R%Z@Ut@+=TucA!~1GPgU3jBe=hD##7Y8-PrA!9cq~cyVY| zqYTQ@kYYL4llcPiSe|o#N)@Y#5fIq;GaDyQXJzfbN$(F7L=PJQOi=+Rs>r>#Tbhwv zG78qb)EmPkl)AQ{8oLbv#zC3I_gYAIBt#c=@hVQg))IT7i=Lmfb2yJsDLafH z#tj>3wN6hUim45dZ}mJZ7mXXM)F?0 zbt!NGKol#j^BhyDZ%<9HuJE3Cd!^P8W7+wlKWKc?Cd1G>xL*MSmKi(^9Aojcw=vfb ze^?g8DuAk0w-ZKBD{vnw^KgyHY?kU^fL zi$54mW?#<1Jf76@hew|HdNuHQ1o0%e#XGe) zBt^(#5jQ{IUOCDw5sT>-`LuyXD{cZ02Ie7gIFF{uv3Ti1Za9A{6NxW%uyPs9s9{q8 zd?~t2jd>)COh;&Y8jD7UbqD*L2jf*0%1k$KGQ(~G`kd^z#(U(5qP`~LI@q-M64Rho za&t`2w3*cBs?YhVDD8_2IwjlnWI|(5Km{33DFG|g$ztqL4piJjRw&mfzpmDD^+EEO ze)=ecePApdD_@so`B02nvueemHFrt=VJ%E)x{Gohn?HT*$NN;?Q7tw6c)$%IDuB)8 zz^wot)5_2q7k?*T+OtVf?4mt<=FJUJVs`M~*xOpU`3Ybowv<&IQT{zt>IeMnhKNZ} zl?rAh#0*p#WFeUEQgTPFEftHJP*P??U3PDTtXgaEN&dZs<{Drke#D&40_yx8Ghzls zZ5fZL_dTy^NTMLEkz7Ya&c9CHlZXAqL-oXV*CAVIwI9~KkjTmryn**8KKS0FCslgF znCw5thDQ4)lk6R>6tl^i0bQL}F{Bi~?Tiwp;T^8M-T0&Vb%0fDJd#fzBsi) z!-O%Cx`?L;Z2=<;*UGrm>RO>jmj<_xba4sY8MOr|)wXoY znC0JINU;cOM{Ay-*1s`ZA{*$?hT`D*j>_CNohvZgYUeXSkC6#qbhSIhQZ8m^AHo4~ z@$^_$oSw%o^+w;Myv&yL(MOALUbs72xwGmSvYIt)zwk0*0URvc??=UqkKAS^WJYD) zG6THik-!gHj|tj&mC|#o)4FQWAItg%2o3{qD1uN{3k^Ioy9oWR?`kMEVO=Dm4U(W` zyU#@3`36_zX1QV*?84&^Y+D-MXJ+22o?w9SKAJgj`y;!uz;o(z@k1Hwivvo89pr$+ zjH-v*;20nti}pcw%yMwn5f6!`AOsE>hhULIZgtD?+5MV&Mt;&8#)t(P-2Eppt_S|4 z#wN-lrY!MQW^I8CT-(aqFM_D2T~m4Fq@8s1_7l2?H}&M5w+!Tk4oCI?o(ebJdx^ky z#O{yqR@$eA1OG9_-SuVsS?2SBYXqd#k22Q;2e#qcu$Q4cCo)Ift4NEt%oG?fPAqB> zQt37j3?$b=aD@be(qs=ghIVyI*WZ#W{Bs4%vi1QyrHcw6_QiThW(O5#XvC^kVI?Xm zC`-uvPNkQ!6~)D;?`2t}#RLILymplwLTJIAycp975GaE^Comn?WzK=m#Lig?UshY= z87K+hRYlp2^V^$%;CGoQ-4J}EF>;9!Lcgs&A*z`sL`GTg@S6Am(hlX^U^RvzRIQFr z1#XKCF}Xamk~U9e@u@<&AwGys%$^E7(h~IX#y*4>aHvtj?frODHY|q(#PfPfa95iK zK25-~LgxMdrs*uUo@Le`dO-|u8{!1FU^ATHZdd>FerKI=5<<&URXgc!q*YUrx!*sS zh099Ye|(KqsWrEz3!x~aFtIzdP>eiK8FG01&Q9au_L?sTy%4G5OQ1w&QN%$vH58v2 zbfDv^VfTS`1VuIBT1Z~fj?m+ZlZbTXtOOMU4+*bpmCn$)JA z5ga_Bpys~0lrzWAFsWir`k#6%O&-_5oYiJcwTZ|lEVrk;(g^F*dvbtEN@T22AHq@g zK*3fVp7U@)E`mS{1C7$cHm#+Fa*g*WSVIhF77oQd7SOs(T0jKekF)9zIn#6W0pw;t zSN*`X3liLN5Zq*@90E*$Y8dLOU-o0KDtCub}(oAJIH8v%SQ z3aJ!>xrGV~?3;7R%RB4?+}1W-d5a+PU`#*E4*{%Xe$-s61eW?2CQ>GH2FD6pMuR-f zsb<>-UENe`7AJpqErVqrA)Sbzl?e+7paaCca{;DN(%lM?Z@uz^^Sq9PZVPLt$Yk^; zg6}&kEs6I_&kB9X9A2U?(IC1+4NAcCBsv7k(RE zyg3Xnv=I~DU?--o$ch8jQkm|~-@7dAxpDwDG2 z^aA+|;FjHddfD%Xa_Ml2qD>TAC%%Hi(j1gm&Stt|vf@`zoyi|^G1>5bZBU{y-tkUR zi4@8D$*$+w5H)!Q_j$2~v9Z1?LJxscb@!{60Yc#eJh<(-2QYR8DvM_#GuuKGvVZ|r z=jR&kXPQa{YpNutrKobXX+?G(OK%`$jc7O8uM3-;TNevQ$8xLhaOMPa^YEeH7l*8X z{QwZKfu;I^2n%tK3f5<4=4Jjjb@ezLU3wa*zsu50nZ6_^8~lc$H*CSL@VhRTdu<)Y zg-n5KYFqsbaa;w>nUD@w6Pp7~U$E zDUdB2y4%A<+bvW6Qmw&}myqGu@*-frC-JDB@OBP-1A;1BadK!3p@$ZscFlO_onazXCe@82W4{p0}5-ltg@g{@w`I zWm-NKz1YzoOxtgu5YsqZe631kB9R;R2bcZMxd1)G_q`pi{Bf}Hdgz53Z5S0GYu0)m z3@r0XGZ$g0SNO;vf?N$`0yF%ggEAX@sqQ&d&?Ujshp41DphlZLP1^b5D~?Qp#y6^= z3KG@IA0JwEdQKVCfCHixK>a$<&t`$<{v=9VP}=E~meed%dUh=-$|-&LAo{^Uew_0z zqK4xkPM}^f0|+|b|Jy7Joavb$04Wa;O)JN2MDX86;+zCqlN`wNv!^}$2;}h!_f}L7 zb!=ot|DN>%$Q<8R0yH;<47u~~sO7H`FF=DqEdxTW%x03ojgK*C7JPqftSfEo+Ke6`H^1Uo)e zDK9JqR?|h41Oao@6e50l3gw+vH;N13&^Aw;bMEc}remlt7HyJD4}0ibD;L_)fJdDD>vCNo+#ItUgj)#fT= zsv(Y>JP$qwmp;qk0Usp;!y_1vy2}36j01-RV8w3(u%IY>pa&MWD^*h@Y4~A$5{JbO z#NmF(O7uekLMTi8n+-q6pMRS@L8i?n)#S)6U<%=fiTfVTETOEgt{#P~mk`7!+0EPi6`jh{?7k^C*EQ6-+}&Dh=xmWZqYJEUYN zCCJE}m~ zj)4_go~CkRFlGTH2;!_v;o(*^vXfXgk%cGLPjGQjL~KPoT!t8gbr9N=;H&M>6v*=L z?O%H3Y4XOPY>6nR8^W6629Um^@_OevWJ+lt9C{8UyVdLrcz~Fivg`JoR%C9sah?T& z$p<>o;t_lA|S+%#+wZde;QHwZTt2R?Qu3a`4n5Vf5K8Qz(M zJbxWdw#u%~2V2R&TjbvNK(Mx6=u%?Q$b|NhfC;sw;ywro{vOSqOW}QVsq!Q4(=1n4 zTc%s$&;=QT)J=rb@gewhTZvd4K5JM&FFC4`>2t4wO;Q0GZ~p`!n1cC_1YhqpKQ7)u zTnz%_RW@&zJ=k{&_HoopV=W3~3@(_Y8$cnEJLtYAQRMXZ)6`^A4USrlcGp5fO?otJ z1O$;*r!^=KQ$I8;Ody6+<@_qozVIWdFHJXv>tz_fj}JD|_jYw&4(t0PY=3h4!p`@^ z90dWLH@=f9CLjkI0l+Yxho#g-)hw|%w;Ecdvu6gC;`3pX6drAUu=2WW9E=8X#!)4I zAeA423=LXWwZo!E1RP;0{hh&;fwi~`VW{bY>w5k<0~aDT@r08?P~D9GOl0F;zc z`5cnO)y4tge_`+)@~u3P;_Xg?)4e6D{jFU6ES-pq{E;P%%W_763}zjVB9P56e)pQ{ zfBrpLHQ!eQ>G=}fz6g4sm2yIS{KHmxVJun-uj#$i0^QNXaSh=HX9lw&hEUM_Hf9E6 zTe;cel4}C>(F2wNZ8@@kZ4?N83HBWJM(6R(W(k&Wg77ho8mExm?G8Frj&E-(^phx$ zB`h)SDEW=(Kr2ev0-zKz#K5IFuh6Sz3V^(Z0V7(+%J;oq41sNCrK`-AwjN1UZPx1= zx&xE3I0zz}nXEaA_yKM4%X_VgBh2lnIo}1;H$5INhvwC{oX)jxqxpxAEKR`7fF8|U z#y&2)5$3+_-0GghL{w}Hhfw4`RtAREzfx|mrtt_+3_rYN+20Z^owqk`TB{LrvV z1B$CJ0nmPP-)rXdCXFeVi2t&)Nz*X+jimjtWudS$?QA(lG&cFRe;CT7l zzMroNF1p`7e`gq#N6GA>J#0|v`7a5-+R^V;njD_Hn`*U^Ca+@THih^(Q3|<2VnIz; zX6zgA2sfMG3G(*) z!-gcq6JE9rPcn0k%fB3>;RL)4vUC}l8f+PiqNZtt5+!B$fbLkM0-2uU)=T3;P?Q0s z#rPZ5O%gB<@6CaESli{}KJcyltpPMN{48Pyyd*f+lJM zqeX{WupxJa5$8J+{9xIM=oB4YuSC^ZxRWUw71U0DzqnYl2%qmpzN&l~IGXaBCH>`a zYL#Gh>jQPk*2;Gc35~PELF#m>+6i22zC({5{c+)EXP-*U5^nGeS+wfUk-<4RW}!J{ zKZEtbVADPB0KOp|$@fz)L-riF)S!4;TVy*O@tklm*-Q0wSM==PSfix zOAHPpu;`&V`m%s7vxjKZOZIAbUOk~)Z^76)(-Kg^ESn4--i`3LmR%kx>-&T|%)B9_ z63k2vm39K&imuz0k(f%_Gc#8;c8?(N#7;x$lo1l1Ps$ILdB_28`YOgfAWRyAgG0Pq zgl|Yr!0iI5LAkG`XLtrbB2D(TFK|j4oDp{U3+AsGy=pn<=5tQGk)O2PU%pTH)TEEU z4MTd0DsUVSuSbWL_I4K!k5Bpu;4vSK0tW4ZI#O+SYFSU8QYzga@O8sC6Ml~($9tVL z<043W1)oa8ptj(P-&|8w4D(@F#Sz4>)3^GsL77?iYYLS=`3eD>z-(+Y(xw{mpAK!e zDz{4tQ271Y%e)-mk5;B$XIiRRk$nb;8H9Ulxx_xPn6D&TDLBf`O4LJ z{M7)P8J}I#$7A!f$|+=5)pwwv?&niDH5>XllJVJziPY@C2%gS*NeU-dmXUU ziJ|H5eFB5(N<1EZMW>#AT}~=Bm5s)F*z4KnP}MbCy+xw8dd>%R06##$zjf1pJ72hf zBH0NpyRu{CGq*2YsXHdTmzj8eb;lc7eUWc~rctg*@;t_O&G*n1RlSBZtsnCe2<8@E2K#!muu>_bM7|eI)v}{ zOwynC_wQaV>z9ExT;!s?3!-kx!02kI=ioY9-1Ug!}D(pFEvzRkJw=5d!SS;*X`HkT*cQ>3Rm2c< zf$#XZ#xF%QhR2Tcv;?$Evvyeom;J@Mn;ZR7+5zK31RyGuk>DpIwpdcUEuwuGg2E4= zvu13VEu9YRRQN065k=fvR%QuguI!V|D|$Iu(hUzFta-oP`x3`=!?+O;*LsKB z8H4SYL7%JFipfv4>Q?&IoBTW&_{^nW(J!u}thG{jR_e&{Cr%zI9JiGCLejp+LZF2v zpV5~DA45}L;h{0v^&_9~+?$UyN*UpF0;2aG7`DB>s(et9Lo^;#d-sC>LWz) zD?eNsr^p{;Yz%C~rM-k!%0ORhVX(q($kpMW*&-oF;lWwpky7-iUM76md>nN15F?euGW-oli^QSD8K_8?kk3r?yxQ&9I{w~UT(lSf z*H43Aq2HT$X0b?{A9~%SsmO=EzF&O!9Pw=8t=!DMX091b4I^vqV-k6Jd$@67W!#fa zN?61yzi5rUTPt6}jQt(`V`vA|TPu4eY)7SkZfUZ6uN#5%YNbrY!D9m&!Btl;lTQB2 z&uiS<4iFB2ff)4#On7=p$pC>SDo(gf<_8CgbL$cTgfHcQV&&-#T!{eD32^BcN`vT@ z*IXzVhIPhVFoV=SnyONM*WXL=b#qP4mJiUHTIE>{?{3oJVI6P{J(bUfXON#Twh7rv`2&M$=}O*35Yj1he`OU!ZSht}Y@n}r z2}PEhZEe}R5M<56THo?>e}poXfx78dW4#7&tq|sWbK(4=Nd@>V zJ`X74OcL4UR2@~URByoFa|&HEyZyp)%#SV^8_6RxWiW;XI9ubCljQQuGnqWviONm} z4<f+e-j|J=0dK0DwH|-tV8e3e@*Zr+n z!Bh43yJ{-ZSaYq#j{1Iz*n^sw5#mEVRpi#V-nHhk7h9J2;QeiyHhft^G*4Vb8T8FY z)CrlbIkUMrzKj*J4w0dpquQ;|E>q;{2_LvFU!<7-vI1hVPdt*Xz6Pf0(6r1Afx{8^VP%ynZ zVlhIEwQxX?IWUl;ureIqY_CjhDc}&fs88QRrOIRcHdm|5^n;^gm?v>RjHHv(CK>T9i_Prs(L|j2*92N>6V0DIl zsr}P34w5P5+Kab`&2N+>gfXlK`?tr&X6oXVYyMM5Sdvcqp6PFdh`P5)*M2}Kz#NWL zyaPc}3RqfntVpx2%ta_9WOH63kh%4kq9Q(oE!3umIoV3V{ac4y${%0W2!hb4J%42p z(uNc!$h(cOdf78iI9igP%?_e^0!MAK>yxdHfykHHSC%m(dnyewzU9@lp*y6wUuQ|yOdsoy%o&UYCV>Rtg7 z4FID$FuI>!r@J?P=Bi4YpNgDj^p_NZgAm({9UWkro{E3ZqNUg|;dZiI`m;xOB%AMt zprt0s(_34X;rj~L@_9$bf%}Oh#?_s6+~3z5E)0hs>CcDR6RDMhS$>jZo8EM;ECHK+IQU#n))3W_LJNX3MJ&A$U~z?L z^9>+O*ICxyY}#x5o7n=_1A-E~j!A7!+?H8D1u5PGgilc(!5;1bIp$puaNJ4^YgB^E zzF|0u)Y*kIhnriszs&Zw_8ZMajFml#Pbu50Z8p60Uv%9<7JgNTSqCF!^L?y0Ch5sp zuP#dKYY8_!t`_Huiiek7C$!)+_Y5f{2Qw9TjlQX1J@L&(*4Gk?-uc-!KXs;I8u<Gaaj$o@_ z0{lCL5%HS|*jonWWSKv^izXSve4et=V?tA0-pb`78d5&#Vdx-7C)rXTqCU}?vy-mI zAP666Tr8G7wgJKdBwozIK}PPJ=DM*Od#^0GM7*gGOh9><$~zv%=HLfonM9H%_NHjVu=(~#inYr4-^Or}#~muMf^7*m3X zQsdnC3V0eq67X6fDK~@OoZt@v#U!}t-{JE31ntWXWN_jd=xLjwGUb@bw@}&r#)M8q zs-?mXkJux#+;!kEYSt!v=H*5NL36u^(rr58>n+lWKYI^P7Mvjd5@_u<5Qf67YlE zj?o%s$Jz3Mh{v=DdSLxQY(|C)lR%IF$=_x9yc4<$f!AkynO@&eqAW-=RGh})t517~ z;E^J{g{!I~#C}JGxckm|dyFg_&jWGa7ZI@jGD`=8?eDG>@Txk{j%^hTb}KXb=Mo(_ zB}u?9l$=o|@CLhVAJEBuXxu^@&1br-eOOC-`c0&1{j!7d*~VSFH^WixSn-pIzTVq5 zGhR!H@nf~cxC4z_xJ2)ROT`Q!b3|)7ILVr$0Q-G4E9pxri@eiTa3~~OuFXmNV(C); z@ylj#3myy1QTEB8sDF!Jqq37lL}HDu(!*WNa(kg#cILx=dGd|{%6ym*F$%aJz{5-7 zEwHJEv2DPYQP{mwjDLpy%#2myfr9l+A1h_WFt4$GBYYqs1{9I4Pr$Y6+~n^(j{5aP6;1x4o{j zFMnA@fXhPT+mM?|OerwK`WX^wx(Xl0=!9+LTCr-W{YCGJJlyQXuOUasv|X7ntd>|4c9(yB;!+5btPEF zQxgEM1f|_6X>j4->==7`@ zhKScF8)A43yqJ3t&ubP?<8t}U2XS!&#(rsG0z+Zh&6L<@vRVp4M8hzzCEF}!;O`=U%As|VVN%wL^*x@@n#}C zzKeGeQKw(yB^d#KgNQ#`)`(h4Zd%K?!H6D^YI$hbhLz`jSi5c5oL^mbUoS~Bgh;1O zINo?ZIZsFmW$PFlf|x@ad_4()G{hUx$Z@^GCTDdARdD!vVJ_0add>0Xjp?pOxBl$(5!4n@n^!((4e~aoIve?IAkD6`CY!QfP66E1yx(50%L2V=~23-*|i)V zB6|X?_luRclHqF<>9p!^Uo1@?7!$s2JA2Ed;~lB-GSpp%t*$p{d&3ENI`hD%CZUXW zOJ{G%r*r+wvDqz|RE`Grfe&DtPk6Gx?wwqXvedEZiSle~?$`){Y zUV{@tKD*IvD8z|(o>1OkIRZmtK>amF4qa^iiyb?7=xH3SpJt<991Pr1Bfx%5x$i;+ zjWsCt2C5q8=nK3|U7~YS0GC>lA-HgtpFW$+zD@5M9NXdCN?DfDP6LfC=wfz>&#b%q zwA$ylcqFfh8;DibiP@y}Iu%envkcg%A`ium#9<*Q&4u7l6EpJ&Mk6CnlrXSzi6wRE z?}bz&ynGEo==Nde?OFWNrDfEEg z?0?CI7D39GkyU&=tVK%)t4Yp6%nL7*vQ zra-MdcCW9NTs-fktD#-~v6t>~NV6b?ji?H)?z{N~W$TBp+-NK?T)}|v6gXJ$NMED_ z^S69W`IPp80mv;`k;{UrnX64gk?iIcN)O6&@cgKq3yAVNT`Ex66bo9a)2QsJ4LC}O zwq>&^Kcm$(=qLV@mOW8-K15MU2|@Brvnc^B&22MjZS%{n$Ak5Y%!{>*5`eDsJRR$6x zzF+ZV(1BLLZn!E7#q4VFXOD#3AhBMXzzYqz@5CT0!oRo4TfIVwfz<*&jvXIUV^-t} zECo+CMWG@#U}*}*`5mbC!yKi(MhJ=u4(**kJ9_YDjLPV*RMUP=hu_fjC_e*$iHON` zLC7kzXxh%$hbqZ6y-pay4)smPsmF>4+@0G=fK)v^_j>a56bCN)gO#=$`1yIGb770L zwN)v$y5x#%gfRp<{r8`}Z&)+<7{ISwSxbfyp)AV5d`;2D)n(k{-3#A-L+jbI9=Jf_ zoSAv%g8W;~{mKBuVaL`xohhu+u%!ft0ByjKCOXR9ob~yl2R1vSkMoOG%^^8*KN>(@ z<7Lsn<(ZR>0CTF(077jPCekiY9FPue9c9&;>+K0*lau*`3SLNkS5773f?{6R*PCP1 z`RQCdi1{CwT4`#uL=;^UT586R;Ya?AV!fVs@hDO7br&^*25}tM!JEPQmm3S976fUxWT#G6Epg}ganq$O#{Uxm&afa5}dSd97t0o6lJh-jd!D% zQDFwIvCAv*8hrthf zs>6J4rVa!?*HgoPh&YH#j!tj>|^lKB(4vU#xkW7lP(1c`+0L|yKL^|d#fbGe1R zcZ-g5VQ1W%`Q*BkLfgid0bYyA*KLLABD{VS;0B9?sjGTk^!DGLU%PW?-;MnJJ9ot0 zlM4b6#VPE1`{G5hKSY~BT9bgMGJ7+VR&;&XQ2JBckOZ4rKS&ECvuio0JopX2zf}!c zS@OS=or~x+<6vhF;$y+^KL zDaNV0FAKO2@QT9;p*4^)pRs1ta3>&0M^FjWmrau3@?$5jjM)TFR4(yOVPa_Da65Ja z0nxPUat$zs@)+4)OVhmGNcnX^V6O|gMbz+kQbsJx%Z-&6&=n@O#bF~Ux`Ko@J*|E>0v%kmsc_aV3dv5MKDVMA+^yy5wpGUtiDDR;eP@qv4KWmY`iu8i zRt^~x9*6Y3HdfM6TdUFFHL3Sc|i zBKrumC81yh(WB=6a8~h{8Qi-SvB$hsu9x=| zAenoK@P>k$1Za~squ1bJ^lO7yt3@D4V7JaxZ?lHgY8$aU3F+m?WsfG5Chsu3Hk`(H zL&^KO$^8OH7AUVVhcxqn{e>Tg^X*v^it)*iRadz0IgOPPr)vWwQY!#-ejN9=lwx{5 z=E_X~v6Hy6b_XLM#oV$9qD-dMc%rYCy-<_^_ci9sLVgkyP9kaft>4gjaC_(>=P%G-@~9341-9G_YunnJCj)nnF3i4CufxK=jt(g?+EZXh>-R zu^P58+J1EC2kD*@8uZ`sDhw?fztDdnjt;sRC>mFlgv$=$J{^NAJ~#M>{(c&K;>%}W zx(y2O*qeqU+rBW!b-nLws0`Gatfcm+`HBIa%h#ibt%?BrhOZMMPOCkj9{>W3Y8|Z@ zWjD&_UOHrn;e!PVF&4h_(@9w-DnmXRpD8-TjX?D?J2Bk@IrYvn8J35m6p~LvI1D0? zezJm=w)u%p1nvWEd>wD@bYo1+d zG=y=^k|5z=NDMnf_?j~H_asedwttmIS1UkwV%9n%+!}PCX&v^e82!`nOjT=$dTz6cHegohs$pe+-iKwl1SYGU@qbFg_}>4ljVlShiH{#q@^$OWpHC{NU4$3 zQEDr_KZx%k{G6S1{bBzOQ;>MgW-xQ3Uwsk8sH0RNji;AZrs7fuZwn+Xf5$sSD3bx9 zTrwMNOtWDkdR>y1f&brrjrthNbicE%O>P`;zB&tIY=TPQoadRU{bU9_?z@?Y;#nvv z*)3j{Dr|pb?r;4CXtf+F0lzWZV8zF-wv2bZj1qS7OLAOsM-L!H$?-`b{(fG%WPPwQ zt#|U5m9ec6jrAdN+X##AP&F|7iG)II=J|Zv%VJDf`t4yImb`;}A3lKm#u*TnbULI| z{70-!UY29Ae8)+s6ck3e#ybI}rn5 z(V8R-ufiKpR^sw$jt45vEEmW`i-F)P5yk~i(izqbNL>4xdBGt&%46cTrm3ri&wwplPf)0(FXYvScK5>f7nALSNM9I& zNGAYZTtHX*Zm-{oCLlUEn*5M}^BfmM((o}O_41ob4UnnJ53+--)QBPC?PEEZ{qL-Y z-vh=wZRKx7scRLQ+Mf1T-|N>vwl9+eB&2NNK^M4eX`#QpKze$25(v{tOYv?|5P-2+ zFVw?%4r#fg!aHyc`(`X|T7^iSJdKa+x@1XE(AL-cmX#{l#!N-qhr_SWhY27&E3r%W z$cnM6Gm*8Fw2Gr~LvQGh4_7hyJNLzRg$Yo>Z6HmlI(`-6CBOs+#4+LiY7SU%QNv89 zCiPi78_#1%=P-ShGy2D7sqC!bMV;BhmjQbriN6jwZOJgK~HRK#juCTE*&8RfeQ?vDGz>TzgVo>+`@=&WIuN?`| zbp)sIK?`J`N&ez08B$gL6jg*oV}P#cg4+MK!jv0dSkPg8%)fJfkRP=5BB(}XaGLxB zA@8&=cq?5=T>^%`=RdADyF3mn+{x>$URQhq?gvU!99@>?nx5Co>EF{bond#LB)pyi z$oWFl#v%0w!w+HTyhWa>EQa6vyWtJpDkucJOAG1kJiLz{K8wl=PeS5j4zR>3isPHinAyALP-^oCkNlWtIzT*}e;|Bil!R~?j zLBO_VzIEmP)z$As<*s{l0;Mng^DAy>?-GW<6djxI;fBTLyp|OYYm?i4XiWb~^l%F@ z{nhEHm`d?PqJKCjT*P>Wb5faX@iwU)yga0ZJA%?P_}!^04M~tB*GZ$}-lB@gAOyvU zL)Ly!>-3$L00VR7`&pq6RTBiVY<{rqDQ*|~0;JVx*c9-PvZHAvynYP~x*^_~DYT#C z9tL?DxYKKQf&2=gVVZa=!)a<$WMfU=ZvmNwe(n~4u5}!dpU2N)57R^^x4#n+fhh6c z1R$=(op|Ra@)0Gx%}P6W_fQq(5LyRD^Tf}`4^v9f2J#N94{*3y5TrCrV?X=}-k2cy zvm2iIFTlJs&1IRx>%0$!SM$0=&b(Ky8Jl21YE6j}9`Q3jpcJ%j}>VU~8Sgv?6*YY2pw z(i(Tbta;}lTodFb?5FX7o$5`u|JnY_1r@p%pg9N(2$hk4nPB{>lcY1hkQ+B^eI4np zjsUT%55CaNNWnoiP~LHrb}I0$%$w7U@MGEZ4wYFi$)4gsBEa?=osEeRwosMu1QMV) zUpBgaB%kr4n%P&D2bQ$710do(B#AW0k4Je2{PDU?gQ=MPcTE&t9k_nMV?W0^WP&DC zxxu^pEC+ogls1Yt2JdASud=@@$GGhM4SV?;b3rt;E99~A9&Oi~Em?sqq%RAvU!@+7 z1}J6k%kLS_x?EZ#YUvZbRe2aARm-G*F~^bbRr0WpJ zXrvF%33@VMqQq5EL1ymcxTc-Uh^e3c`+nhU=j#P2`BoY$wW?i~8Q;svFHjqY7aLQb z5||?#+g7$too*Pa5h3pyqAhLS-dKstdi!0UIB$99QV5)_N|%}6VPxi%#u z0+)9#AMXYjoBBaQSo-eXX)7DZgB>#qT99LOjEq4{FPlBFmc}42nW$F3_6rc+n1Mh$ z4BR7q<4woByjUykZ!7L^BK@u4-qi$b(e$}>Ymi#gjj=u0tK~dGNeB3s5+ymNLB9++ zw83d(M?A~LkVdsIE_LBoxEV=5!Is?*BpO)rn4>wj`$g6W7J>{@o%j5LqDDvMzw;(^ z3zQP>hZ1qH&sZaucI23=m6HXDi1Hvfllx{f@?oKt9Gm!l^^$=hp5Pbh0nsY~X+CSHhl2m@424AkSpEeUHh_;nftW+3%V`R zg5OZ?gP2lhhj5-*3t1jpr$1T{}t|aWh z=sA%U7u65fmozJk7{SF~rcY^^Q{eG#>VKDTTR?v-$m>Cja2j1><|N6bOo)c|pNWD< zc$nH}cc|`xp1}MTFqr!}>Z9EYjyZ{FO3HFpg33&$P^zN7_g3*VC z_Osrv@f&n))z&J=$PEOledxR1C>N@DGW${;4-rRw ze7N=v?lGX!iXcIEFu!sYhKbKS&aiMtQ=p-sjhXCcgwr(ukrhOvqy_HddNu&77D;K{ z;>|@Lyxu9=z-);G_A0Kv=_#rJ%RSLH!DF~rM|X6DCG%jJ6>(=tIzzIjuK^bD(pRyl_6r`5m(1 z>;@z<@1RaU8Dsf0GVY#%N)24k6!i<*-Sp(LLQuzG7xINtQCp=z= z0k#0t+Q(bO`4_8LH?w^9LwEw2296 zJrDrv0yHRaKfAIFUwHU}+9eZxCYwyV{jMzkbqA-1n7#6?PjXqJ451b4{dLKG0 zyZ%n+O51^P3gN$Ybg?2e`gnOg!>3vpP%E^>KT?!|^p?Br1JVFTlK}lVBK%7{@9*3& zu(clK;uGkDP!!CE_%P%;Yv`A) zWG@VVsDKwFyu~ zOKoHcj}rTZ*J_o4*D~Ud z7;@ei@#>V>_oP_b{ibzBzhxt|`!itwigMC!c*Dg0#&X|3PQ}CImyvukFl^;sl^)0@ zDiOLukp~sm-)zwW4fyRZ;M7= z&JdRu)Qzxt<%b(5$=Rgrwr?c*zO3-o-;|m6wjM#wlmE7J;QE|?=-v_`{cRX?j9JC5 zGs^@p!z4Rjl}S{`z=fgossWEzLFVg&O;dbl;a6;p!{M0|ws(|wp29TCQ}i2w;DP`E6F}_0f-}C^!SDN0 z_E)~+&M(d57p{+CgEc2C9Q9dEf85o3lM#`62eV_xAv(~97s1ciKb$fzmMDI3r3^dr z6}TjOjKyh_Nt8<=DLrS;di?2Gxn3i{pPIl**-i}WyDvgg2zw{o1R8lbrm*Pm?1n)E z6c*GFsNg9cAhz0!e{vp{pod2s6bz|wp-P-0M^wOk0M! z#mRh8TU*1HK8`;RewFrcNhEa0vodFaVM?(qd3Vr;Mm0dZLQ!|!a$c|xQ2VWn4?PU8 zq@v|vB?`5tHZBt+-hPjgD>j?1rS5fG4~da(Nlr3rIS4l41S^EP5eRXlM#Z(uYz!$~ zKZuN%%25ov;jHCXTB-6;KJN2C4cFjmL$G|~8UOf1N6PC+#v03%mqIjYUul3?fT zLxqI3Z>Lz;*SUqOqoJnS99@ckkKAcW^%`YCA;k9s;N=yZc*u*NyC`E_4Bq73#;ZYL z7COObg3wKi6M#xnsX22v6=9G08a7cQ0Inqx0jSAc3D>(A!6MOh=_a~z@ZZk#*qcDR z!Eha{3R=j_SG6Xh(@`Bvc~jL4-9GF^?!xzY)C@|U$%YaqMKz63oln4#yIYL`scn2= zL~xn#oO8mt+g41%{=OvLRoXDuWP=F(ytJabol&kg?D88%PUgz33kUj@@&XgY|@lH5}z+Vm5GD6Z9fKy1^xG3Ko0J^yZAY7#I|PUDcEu{Sc*oTwiXqDJTwtg+F&8W6dvYmI#nRg2lp~WUdt+M{ zN~FQ;Ilj(Tj*po&dQGngmBiT_*F#)EJLJ)hNilh=5kIzk_N z6HvuiO-L3sTk--cewvB>M0=^)Q6P#zSdMJncSfCkv>kLVl`JBmc?+nE{3XOtznBBA zo8&wU9_L{HJ*PIcFG^lap=TTN)W?f({9op)4vn}wMxYZD9%#e?XzssG>(^qIq53Yu z&wF+&q*Snqy4bIg5axtv9>W_1o zJzaPf3Qr<1lhml?ARl8N1J`w=^oblQ^j25-gZYf50I#^k``hf_yV4TzyIm*_;M72R zyT%7krR(GVz!SvT$n0Qa%I;V(Lfv{QyvxNQ+2!l3}!_Lk92JH2@JF~qL%u?DLXj5uK=Ss*|ljlS0!0lcb>N4*!X z?Y0EUlP+|M+-n;ans`pmA2{_iUs`?v(kz=A`;=o2`o6ltR1!WC>-5R`+-<6jKx?qb z#hQ)#gOV7j)?Wq7Zvot_&g@lj(ZXiQ)p4VhO$6H=g7Xv0eo@lVL>D3iuFa{$mHf7vx2_twx|HM#R0KXe(^;v{Zyt+bO?y0R=7$v0jo z0JV3OyKl~u{V?n_*~-M4s6ZywdOv^yO)Q1$fw#{3)$0jyvUk!kVrdt36w<6fF~2PG z8*Dgxw;$$nCjjjRsHU@1KCbHNJQ2VWXG4R9ai$6Po9&4GS-Mb|ywfxSY_Ot!wI9Nq z4mqiwt6(2HV7^G|XdW+!v-+8Z@6o53LO{PBue+(0($do1uMQ?mqSP|l_kQ@ns^Mdu zpHFpuW*!e)Z5=>!ZOC+b!T+1f=7owYbqva-OCJA=vgLMZ&#n+}_IqoTn7K!54u2=$ z8cEPEZm}HQHN(u-D{UhAN=#$8^-u@+g&8}jiOj=GbLyTs7drOA^dtpW3P>+;acXz{AV-*ku^Oeuz%T51kKP$O%Va$F z=;sX{2pMDBxI25Z&r?PwDE#H9!}5Im8~o<)##%leB#Br%^-qS*#dw>vK~s*crsPYp z3<~!_z0976Ns{|TM*B^K{!9USo*!zHu&AOj8{>LVye)Qs(HtPOZr~G6qkM{_negBj z(Q#}Oc;cHCkjwPth3seW1{@iBpKtr%NJ{6Gakt#wH;cDw=a%`vT6!{q19?7~3= zb8HDm`R1Zz{(U<&1%s7>!nVSD;V1Sy?e)L1>QNF&m*?s{11m*njc|Hn9+dX3Dv`77 z>`-xh&NolWA&as*X$5R-N%{-2zvvs;52yQnW9x6GE&BDW_A=`T1x5$q>pn62L0XWVLMQn= zjbeLxwgyl&p8J6lyuvA<#!Fv!&N>S-Mx7y)^YvQi2Q3sEH%AKlzP?4Q&nZX{MWoHL zHIgxP;23pxysd2f2>0s3DQ)+J=#DExz_Q8enOT8y{QT_}nY1q&Ov{7-{^-fPjvwlDN=KkFubUsa|Q_>fnyBZ_g+n-;%_`7i;YHoJc z5<}GU^7TI70*nZ%qS;zQ`(G4lZ%JNy6i!3)rc?s9kz?vigv=%MjyaTgeFo^zcV3q^ z_JEM}sFEYDv$;&8q8j_NhdQ(Cg{uBCqwL}%NTDNF&AP<(zCppG>A4txXE)`&Ha$4e za+gH=@Q)r*JgM>jYZ;m4-YPvkP-M!ThrJ&+1C(!eRRj?Cc3Di&3k&AwvF`yiN=px#0D+fI#{7-- zXWS5d(3_-|o`!hs7ul9?%g`(C{^E*`{`O7$qFlR?Og@(Z1Ls~;ys2MvceK%yz?f`K zE-Kr&!LJ0%QdgCh`+<&V#B0_Kef&imTQ%i4{oxmf@WcLKIQ!jc?{1D6%kBM(Irtxz=w?^O9;DVF_w6dp6EoE|) zvVs;W1veqY7%LxL%sNIHGQ}EE376g|X1o%{Beo#9{SXXZ45p#W(fiaGic)xW6d-Wd z$-Y1I zWsxo@nB=DFk#d7*qWyc)E)wOT`>?(tUE1pOIaOPlXDl9NhGqA1feQ287qj2FVHhTu+Y^SA%1 zeYSk9v2J%g{kx}7S6c{ExWu2)mP=v4#EGy*P^Gpky4i-x^A>(c;rnH+sN;}5XZ+E zrNK1Ri!~=vX&Kmz0Hkl{&q>T%?&}VWn?44L2g9NJU+nrmDR8*=$hP?I&&^N1b{!oFsW-qdN$1=gm-vGw z54X8|rXp7)oS$_0)qHigjlz5r(G3fS@$Q^ofK17pi^Hi-k(gvxON(Aih%V>KCefD8 z+Y8Jpg^#F1RzPxqFtEB5B%h2oNg-LDwg|%JIq?RW=lztG!{dHR5S;6ew+=tA*tPP! z&^aYTdLOpf8`P#5f`e2Bi{qk?azpI*WvjC!e~Y$BnzY;43BFCz34xLb$Zz_>dLBd) z1~B!C-0Sv*6bet_)tp}Trs@0QPw?+~9(if&QLy3wPx-{DKa@6;h4>MKBI?D2Mn13@ ze(n=67cDx?Ry2uyx_c#@c~*bZ`cx#i59;h9(K)WX_Q`H)(cQQl`1i-qM#_(xw#q^$ zr0$Pbu|IoVXFq!XPN>fhC$vWqd8aOD0RNqV(N>@P*6`l(7iL#Y{8Ns6n(yIR(Q2F- zSj_>w&cc#{b|jS$-tlAxnzcez6z0?II7WlukJzlz=z9(8I zNB+VOI5&rU()%@C;sjOMOA_$17-WWG2SMhu2*qHnJ@9Q2p+|Oxa*CPt(VyzB ziSTXY7w0_W=R2S60Oezo1gHz}qHLWj@Es~d7<-$iHg;u7)>RhVA$N-Pnr+QY1BdJ7 ziwWUTGy7(FJ33HfKEFc>zjX|wn%LH7dZbhmNd_w2DC>y!s6BtrXk<&y4#~d$))pJ0YQ^lo`H`j*?2pM2Ag7hiO1mV9UJ9+om-~?Kym(& zCYy-=Eb=HQa4QV=Q%?!%4*7@WIx{Oo4inic0Wrk9xLs5OW@afQN;UageRe#_479 zjdR^#3F!5hS-Jk+KU{M~xHct432hi=)t0F4yJI8Qu};Ww8pnlG3GxGLiTX?1c=$Ef zW*9nkREjcj_5zHzq`t`|+p{ew49#*iB@6=5KsM3*ppz=g@K^}O0=-S=&Tad34E_fn zT0_{Y$%i4?Xe|g{U%XB!5m>ib+NRRovmmglI{swRH)ZEzCH13J3LlbFG`yX!>ASg;ysp-bu${Ooa$@4 z`XBBsS({RLlIt0rWlOx;5iNNjwe3B0iGZ*b)n`g|xp$)7gVgX9;UUqX`9sd3SYSh~ z5#>kAb^$EtSMWD1&p^Qb0-yoD`n^aJz3VFY;L%+j94S57#Sk}~h!Nrmf0_A&i(;3V zu|!+mi%b3S%9HwaHP4W042tMRrxgo>^xvYgm!*uB!d*y5jU3{fC@zk+_zFEjSBkCj zcQ<(>*g^*))S2Zqe;$DCT3Ob{JX4E}2yYI(FsfdcFVvmI_K+BtUKB8;kkn>vGOROI zaEkmgyrWU)b0@#~`hps9152g8!j;vZFPnymv-0EcoSlz!dxt%h!4UIwO5;}Pz;C$$ zaX*>QfVra5C|1qc>FMs37R{CdD_%K9h1(hyd99s1W_Z}CCC~UwoilL^0Rm@rAWe7A z)Elr@c1~zO=(OlSS3;|+0^FRiFKS3^ms>-=O`$EXB+g3(pH-g>@cpnam^`H$7Qp}* zQ~U7x_Y8wbA^tA)Q79Io;Q3KMn=*lxQG}B$lp~2UQ}xv(1m#zc-w0sHP7Tkx`;QN$ zc%7l#weDbu@#6Z7;;~zRR2{9dk&x)3BrecWU~N(z9xKCe4C8n<>x+1I-fLQkt7;gw zVVcEI56gcIZ|#a~?k9*OY|`lk`89PNHxNk@lh6V&_PQiF@!h{?XE`8R!j+0c{( z+=4T&&E?|>pgP+=Z^%a+9aK==%&++E3-|L>TCj&4#AQ)SgRUR#WeH~N#cD{{&Kvsk za=r@B4W|J{06Rd$zXjGu``P`DG)21N7w zt^p8c=#+ebw*c`^^t$?&D)yyIsS_!SDhDaN+?Ai_I5_kz`88bXq1d{veS6jC2z?3a zFS_MUwWyhBhRglEoDgBvp)B_@H`M0%TL+u(Do~ubBZTBlVj?Y-ZR4ZFhloec)R+Oq zS^?YJUGm=cJ(2ml9(4+#Kb{t(36)-O1>S@{B*VPh?m)XRx!|*oJ!71?RNCm|fyIBk z{R@|b6Z)UfJh?o5ebm*C=X;MfJMEd2Mv*gjFMhUk89;AFwbECr&LOqGjL%z5+0Lrf z$zG+5p*L|iy6A-H{+I~$O49*&o)6>dZKo*X_iz2T2zQCV{WQE(dk=5KGMNwkzA)qx zo!nQqHon%?j^`YSp=zu>pxvNhB!DAkC)b41XEMK)DtJeE(1tZksEJ430X-pu6i4>) zGLkY%kjxMa_nT1>K!(h2_Cpa*Pax>u^{x8EUnolm{`1Zgu~%7HZUra~@Y_=~P2duh zA4KyO`J5X!DO)^~UQ6-j>b;xfmzmBk1u@PdwkK&8$=ynZK@3TV)33*$FW0rd!2kZs zcD7fjqMuqt$t%iDg%wvu`h%!Q%M_xevVfR2YyLFK%GsaTjqdhrAi`CH)t4LRhaY}H zh%ZeQ>sVohR_qYGqKwq(XM+6JQV!kHDw0uk6j)Gpg_32Lv_V zkbanmPiaE7Xb%$PrrzJD(aKAAA!b^tiI{I*rs$Ht1Y+W_bx=QX_*u;eG3fTUlE~E& zhTFaLW4L>+|8~QiaGeN#>p~@oL{r~u_OIgFq+j_t$7_+q>D!Z01TP{4Xg3(uYU1j? z+D~J;h~i?7y|t+|W?HUW_?|!TZePlR@uuli9ccUugBpD1mqDEDaH-NW5+O<5PjS5Q z(4-3^81Usnm#jRMwM08%9cST7eC1nSUwdw#=5amphvcDG>x9lUAz?KN)s(wOTdlSG zAXds&yP;rtYe}2DwR{-`s4vngZW8xUxdmindC)4LJ7rnHLZeGwAnTIUHew{EWPCv! zJC&XtI6v`BS9B9Kc#r4r&n}v7aD_QEd!2duRFT!AM}0V>rR63Ypm~S=bvtLTAuyLZ z7$X*i9rtyW#}t>~rk~!r+VS!oJ^Qrw>B=B*P&gfi_;jjJAZm`Mhsjtcr-eF^?HdS?u$-(@Y775}+R@c=9EneI-c8gNJ1Gh%=b36ylsk zX~DCa%Yo}SZ-B)LQ8etBJ5}3{yF{7WPd|SRGO;cVGNiv9`jroLsO4Tp#gDV#;SP@` zm2unOhWkQCVd@kcppiJq7LR)_Gzb^N{f4<#7wI{^!mgVZ$$gv-<+hGDDx$3VqA~ek5V=OGQD=dQKXC8hA(G*-WrEi`rIxVUdReE!`=N<%Cl zG$Sb(qCcL~?oRYDKbV~YS|y=`T?N;N`N)Tqm8ws3I9V>P1A6Uf8z7^I@^yOycPvk9 z97`6t5WikkCNyv_xI_P1ui^*E!$KPDCL+43nsRgyJ6LQPpHh)%q^L17OG(|C_|j5* zv)c?E%b5Vh+3+Gb8X})Z#YEeNld+ueztD`eJK77HtIa5j#_Zdi0@`fe3#j zpcxKuqfZ|?EfAUjMG{KI9&%ecUd<2AUpuW>rB965DzI6n5=)}4_Y>q3zo#9fH=QyBg2mE@H#4+>r25ht_ zko?igaB|}_TCMOaG(R_GFU?5i^LM>g8RE4cml6l{d$&aa12p_X4e(M|7`J58n=yPn z#g7}`4E-JsibQphq>)9BP5QaxZ)DW1K^_rg9=yvF!js?z@J0a7p;miTU*iHVc&82< zb<0;Fb1{c~T^J?c9@_!dw2ta&q!e!mXw4K-K{VCwulC^(3AOgHn)A#Son2BrHHf|I zn;iAsyn0cdZZ6NiZczHz`B3Uw7V*}YoyVN!n-Bat`}}Bggs4?b=qA;#iuqAI5`cu|bcTG=F+GXM(ax+pZqzt_-*}7BPpy{MR zYO+VZts_K&Z*P(uz0yOwtO#H5H>&|T*_FtZl*ZY)f^HpUFVZ{+j*}EGVt3?vKIO4c z`|<`fuB*h5&F7-&8jE$i zbfMN7MQDMr#3t-}$ZIY7_CG%a>0ska8Bl5&SV}7pq#t#TUVp0aQ0U^yC*jkHwnIl`-PqLTL-5$iJrJyLE}~tE(+8mCMMF$#Q4nR z=*%sCw?%Q00%>Ard4!y;!qn$O=SQnG0yVYQZTU^spEb?hx8@?`-#Yr(_3IkxaKuE@ zoOoKGy%tkBJdv6?y1D)q2S;itaJyGjt9tC~_DOtN!!>f3N%T2%lk8NIPg zV`)?NsHwzVz5KnRM0Bp{lw8!ae)eVgj!%$&1Zw=AfZ7A8{n^@@pa>v;yAj+1(4kW8 za=ucr$SUy(U+psP?6VS=v>;*R7AjccvG3ESqyIo$$Mji`+=oV)ATu^Yw?&06i(6>+ zBbU#BenUtENN4bJ3>X@Fe)R~Nem}iyM~ct$%EUS1cKt(=XHNPSN58p{!V?0u@|KrHvhhpAub}m1eBGy zT-ppmku{N{-$f_1GHFJrP4TnYW>*!D9-Kc!%Ej%6(5K>R+z}`IpAfT%>RcbKh49va zS6ax7DU;#E(y=*hSGN67+oU3}GSkRx@Hd_Hu=Tg9&=?yS?U35zXK?E+j6$r_+@r*EK!a7G@?_wDu+*&7nG)$!{+D5k^-?$|zyI?Dck7 ztNPGZom}0)U9SePG3IVHP0jISykWFa0(QECSOBV9 z@KuDXsrkSI&Nk_nm5J&D$1p+xRzo)@z$?ESYzVnGv1K#EtU6t)MgXmMu3YVPzLgL5H|8&tHdlaB>` zdY~KxkcEZ}ykvu%2-rzT*E=^HHG7{Ga9ff7-ZT{1Cb_1e1O$@;-yaiM8He_FPawPq z*_8e~)AM>DYG(B5`^7uqWI*;ahl`~fZy?wkSPue>&gFr;np(1Bcunkqr zwOOk|uYp>4`U)++F;0LDKIMQ!0f4XHaq7ZV0A)|`Tb9hsau)ZKvoxO@ZuDz>GrKKs zPwp|5x+L7s@$ad>|Lz|Jyo*xLD?Z&x*qql}#S@FIpp1h6|x~huek9iTjO-X^vQMeUvoNg0=sU$b!{kgCESoW{|!) z)fV{MBju8hZ_eIddaLimm5#%a;9?paVc{fo3;-me0wWg$eM0Yot|#F{Tpx5>uYN0( ztk|!#*M%qt()K!3L~B8#`sgBZr4dLKq zSlhRlN`@$Yr(KWa4~pbY?S*mj5Rt{{6DU5ZhQT;5yJs~?{0e;-){RgaijFWVb-skF zjzR;uK7Mm9;#)R3g4GF<@MNY(>qP*xP5xAWHD0F6jGmup5s2)0?4Tj}r ziGhI7;zV?o>30p zk9Z|CTc-i2jt>#?qd+!D?}{+%u$x2aVL+*e`h!xDL!U0bTUj{FfurG*H* z-us_A*LOrO3Un5Q&_DM?oTqA@9iJ0moIJHh+uM23OgaZ3>ox=$Tl~+DJyhU%pThycu*0{sd!~1K5{t@;PqUTB$d7{ z4>FY4+&B1GM;}v|1C|wHNZX@q1(@{d^1h-3nC>luWdN5?90{m|N>*rF7!X0paCBjT zG2PF=tAn7|XGe9y*tR&bG2+T@>gZ~P$kn6ho*l~b7kG~i`r&v{y09;Bk<4cRuVPdxr0x^D3RGVADE@waRPM-%*L-5F#D_V9~#1yR3DB(Nf( zCTv*jIPR@Y@T9-zN1A2c%bW@`FT*{mujOw|r)%c{~QTLtH)H7lP*p3sZdagFA6f?}9i2RoIjcNVj_iIp!&tv1s zSA04Vb?#ck5tv&yt0r4L<;ACRw!yb9ZGt z_yF!>QTki=Wtwdhy}^iVNX@Tl=F(0-$@(yL&oK4A6rR-tS}5@zN8g$Or(2sKQ5GBl$H}mpPaIBKH?UG%%&)L2jwIg)$rBN=b+yl&g@Qge4 zyk5rA?`L_ko3GKRbA*Qx>uu@sC3~Pi_~jSjP35vdk=5Im35ID(4`!vs(MG@#k@*dg zr&uuT*e~q@)q~(c*~)WoF_nu39P>8FK=L-}VzR`8&FGba+zIH=gwb5m0QvVFX8uY% zS?#g>OoAnXD{uUo1=@f003)3YIw-67kgOM@jm zn^FJKac~~Sv5eoQRij+wsxx%v&NYxN+j66@-oy&XxGLTC-LaWfKPUPe0r&a_*{_bs`-iA5#4 zu=bnMIgT-uz0vBmx5$!K$z?!V8Y4rZ9vh=4uqyKX$mlR#Hl^}90n|J_mcto)HN3)@ zjKluz^ESp<;NPNIBSSLLY)^G-MHyABoBT4!Qbww-UsPO#iaP$1X;{7>U^2owxg>4t z3|plr^-~;MmpKG(XXu@C>VVEq5+FF)Hi7i>?zLvwug%Ug;F9Q9Qas}C%g{H6B`b3p z)p2#W^N?~qkPn3f`nD}45)KfF;EqVlw7K`EdyFj`OJqISSKvfYV$*pyOYX40Xr`$r zIC+W;79k}-6jDY)&d$+zLh+jHW^sVu9yFUCTUxa20>kshuFs3K5@Dkbxy|GOYoim~ zMj`k+iywvMNpCnZ#!i|wcA>L57cMo2>3fDNgxfRCdMnTi7&ta-h5rmz+PsPZ(bqIfZ z0zM82C<=*_SJBM!p<2VRIOZF@cLz9++Mm7MZ%dBobg88`9r$< zu04_Q=4jRbo}Q?4DSn$*B@06KqgAexkqtMJn3X6sCAhj-!g-eVX>HICu+=e#M6(G%6o)XMJlv^W8%`F?#DDRV+@+)j_>= z?%&T1#7LZufaHVgf~K)17q+YpGOtHBoyH|4GOka4eZUUTOPqG?c?2#S;1?m>t0%WA zzKD6P=Zz`Qo}ao>7PJtTKjGtP7e>Q2eP}dLwgAD>V9kNA{pS?H&W$fHW%QLF{#USK zk4Gh_NR}deJc6dkH8e99@ueDEq*$C&8f`;i86GH>p#4B^SO|v=%kOZ1Z8lpOm4rRh zqVV^lJQJEZ$uX%$994@`Zb8^dHc-^OSByR}_R57U{9Pt^n#Z{T)xYw=Wi1DN;)#~? z8A{}hEob#=6&OblgJ}yReVXIp{;YGff85(pCle3&v`DuOf>R{FlcDTUzFW{mRB@3I%LOs3@#PK`UPJivqY+|&H=+{{kH&W(2|{rki||2AI4N`KdF|KG2?4lA4&2a#rfE;;?5A+Db8@N=~N|b7lGk zpJhTCucRpPkV}j}RZ*tn^VWPNBI37?({U*&xO*QWg1sMnk{spx#qX=}UDkJ6YJhtJ zSZ80@i&1RCOcfToR%GIgoB}`f0Y^OV{;leXHUuxxN*vyYjzVs(-FhP#z1q^L*pDEJ zDRxsDMty_+Ctbdu2?lk=Z-GIIh2Op(!%5*p)&if61gRUZD;~poVc|eI1ilDjQeO{` zBV4|+=!eT&6uf-lUNVgE+GK@$hCPz~{kPV(Hf@*vhGz*C55CNlp?l7JDsRC(IbAP1 za^&zHZ3hWC!tPY0BeKCuSZ_iN`-=35|7^xhy~V=?WAy!Pv~(lw=U~2%wMij2J%W#l zT(3BaYw6UDe5(Q=lpU)i=w_e4vn@bdp`rfgTg}9sEn;=;hwgAf^?TX7Fl^!2;tPoF zFFv0x>FebCD^MT)rFJ)bdblOT=NN#hd}k!1{8DI-WO(V1;53k|)^i@%sxSwC?-M#nEbsq<1nsqcoo>_bHzW!hgAW7pvNI zPFtDJW3U_kH2jgR&Z{h~U8=)}g0MW#k-A>E=zC{ASp5!8-~GZM?%RGKIK9>C8T3}C zGhRV0{^A}Q{u$R@fRg7Xn0R_gxdVxa((Br4xIT`{Eu!ECjl7~0ad)JuNkNK<{NgH=5snRYzWI#dHBt3ZyT1KSBD;o%JN zG>{!2_P(|Aad*$87S7dwSV>a)G7y#K$eVtpHnaUR z$q;j~k>EMd5GdF_=O&f2TywcWCQFM`rYkAWfXGHL!v8#TGijLzw6&{eim1s242mQ$UuGzXB(ZGZY#F zyTKi7wjKv^x0l;kQ;Ji@m0yLzTL02 zsFc(`m%5rv4g)`-2vx`ia9?zYc^%S!>@gBg2N8;1FRzjkva4o+ZbD%K)DPuc)OWe8 za4u^KYr@OqK(U>Oi|LLphTU<&^>2^bC~+f);tiu6%f*OAHM=psj2|NMoK`R5@9^&= zu!3ZR08}h9QQ?au(1+H7S;$?`zVNFl9iLoF<7b+$)TO|0a|ac-sINMgkrtg*qyiOl z%u``aMBcLI;jBe~#<5Ii$=^jEudaM^HERv9oPK|2wvHg6;FT3*{Tbi_DL9?{9akg1 ze-*7@%*UG&X~NdO8=X}9%XVSK)3rKZ9h{c3gUNBgQ<#*3bZfMq`Q2ShdFQ{>CexIS=i7 z!oinob>GcABv%IWP3M`eb9G~zIqhAZ{3BW$L5>+3nn^B;r9=J@+O^dSfJJfUeoLkK zWWY>j!{7Cr_yw(@ds%f78WrkwCUjpj?mKkP)c`kjn|(K?P*mH?V;E-91XbC%Y&i!Hf2|r!4kF&EN4<3Kd|SU%o33cx@{m3uZqP&zs%7S<;93ddi#ZqLEnJyi50&BGP-h$0DnD1j zQo0P{sdeE=)Z{?w31=8$geqV3BGO<6UMG>fG>Zdb=A4XpvB|>U84Q+iTO>(-X)_-E z9j--GwBV9w8B*!3NsJes-^WABO`Nshw54`9-lM*sqQGR>VvV#P(aaP;xwGgDagy3m zr7h-8rAFY{RU$Dwc&!vof3et$>-Mg`=LtiL^=Rp;i_0c@2FPYut>Za`q_&@vNURnL zY#Q2xtJOB$lS4hYsf7houNAmEo+<5<`RU&*Rl0&%Dqy7}$Xp4H4}**E6T$0Cfur|q z#FT_0QQ09!KEQx*FLuj6G~atJ{Nw8_wwPs687T4SfY`wVey{BWj`P=FSi;0LKhN>l z*MzDRFoPbI=*5z({R+xF$L4nfPr$YA z%DLzzP_aUURP4=hVvmD06l#QG9u3A>jU^Xkd&45J?(gpw(jq^NmeA&K48?Mu6NM1q zc4%ib0IF|f9mUx`w55?!T1U2Av`rI!0XE2l2%1yHCPwiN(x*ACHaWKf0-Cn_+W>4x zUn`=bU&dW;oez&SzVU~+pkw);_F~%m>jcJc_L%+-e4yCIf@-JiwmLx0#>Cj! zhh>}+PCm+b^3?^ihe{bYWERrczqi#E(HLZtgY;+b{yX-l*SlcV`Wv@?UfsDJ76S6% znD#iNR4x9HIzv7#XoBJ<1-s7`3Tu|Q1QxdXa`UEYp`LCfHJfBq>19qzfMw<$Hhz)! z-5rrw5{Zs%?%VW51^(c7bmKcRPo>ozAvRodH}v$VQdCwD$uc*XQ`XFSj3mB?dOmdv zg$&-oIcqYkj;iQ`6WqbmB8v-r+7EhQverX6R>tkEPzFR?E~CxZ+(+`guqNJiA4Eip za@WVNHR`~LBZ2g6Rh|^&nO{(V*7%kydUTU%Trth(f(L&des|;3r#`DZ_f`hG4{E4B zvG{k^9zy@F-(<*bF5}$@7wwNsizpdetX?eJ>;P%>PI6Fw-w9r-RhaES-rj6@`>ehvFPW-FF-x$1+ z(F`i>w!51(kpkI?@0o0p@{0Lwj*oTItzADqHmZ~d0@*UNAKD2oL}Q5^(`Q)rQ=O|~^!|xZkdWc*Xai@0%4R6jglFinu5-ml zE_AzDa8r8h`mRqkoHv8%Er>shY?FDjgD{;Jct^24#h&Hy<8BtK}DPg`Hit`GkXS#bn8s%wKV^jq6MhrspJGo{QMN#4pr z!IxDiO4kq1T_QcioZ74xBLOxodapF9-W3{hc{;(pHJX+s?fAs9nJ~Z8zWRwyL}BAK zkKLK>7^pW~QNA%UebiuarW9TSa@lE{7WbFoJbB`OPY=#8w0Dl_evqoF+Z~)n*#2@a zbU0R=kqr!B`7g%nJho3Sr`4>R;C=7`>|p71aE+_87&AQI&s+oDEM4(FX(h%2AWhVt>Jl^2DGyFm@Fei} znU6YU_PDmE^GKI?T)SDW4QP8=A$-p4yb}4=0V-mscs9eYw?Hws!rvYah`$@mJw6Ll z!NddDWc#+0`4MVrNGBKf(l`IaatT`MmEb?S%P;NXRRwIes_6pnKiYcmT`D$iAG$qY zT^E|ROV+z81UbWLULkY+ot<+rnfnGCkrUaTx8_<5va{V_f-3|i=p(-4(DP0-s{4C3 zv0!LgVd=6h*dS3P-YXbUD;UfTZb3w3HI4BrC#+iB$@xk5PATL)K{G=9^1w_JC$5?v z-k6^S-^rY_99S)WKAsY2Zq^@;Jjqqp5m!||Dqc!dMp>F50st`{9Tgn=v#zOTc zxA80yVi6I3aF%&5zjD690`Ml!$b>-9To>2bT3-c&{dO{p-q%3oZ(FkF;UpBLtTmv% zarz?tgCISM;e26X+*VawMMTFGXaY$}(Q_LBC{5w+HF`w73as4>O#oDW-QF43wx4v> zrs3a@-RAylDd*NHQb!{#T183(55g3PIlPR{S}zQk|2a7KJ#@b5vFpE@PuUz~{+oHNQPIW5#ft5jh*A)|W z*;nzen1%KjItV$Kb}1$^?_|QSB8~|5_P=sHy`G9sn5M|ZZc$DB$l#vkMZQT{+X76M zmz5Ec#^?;}&t)-II1>RsOmsXj_W-G)e_AM73;d$G6O~&0a#*hJ?S|qj^h?hA<6L!Cv*GxxeB@F)YcOJ0_BcI6wq?P>f3V z_LX8C+gnCxFNj1_$PoOKe0Jw!k;4d)bpeuJ25+(e9zvgTh^x~_ZZ_%OwHxhrSD7fp zkBMws(cniss1#=Dw^uukkD$*sc9F1o%~%42^I<=Qj3*brj0qX&-D=+@0#|sy*x%G| z*{vGpD=9NgZl}g~jibKTav)jB^A8zu&wTM9ETPOB4J!4E5jA=TLYW4;tQ#?nywF=O z=jN_&Vd6=_d|aMu?4@}7MVc#L-Z=}8auha!IheoPALw7KVKhW)Y}~~W(8lKz=1d9^ zSYb=7;73A1hr#n2a?Ki*posh7W;dSA`?;)c>Hhy-KoB6z{Js3}jWa@Bo*D~vwQGMH zKZ3HvH^8=*R>m7i=+v(M&9TM5oRM#;Mt=&bs*2Tyq?ZqH4EeJ7B`<(iFf*IVUp!_M zS8u#&iX|_zi%r7yZyQa?+p%uK{p~?g2FfJGo(b`}XH+>rRL5JdfCcCyIZ?AjyhtLm zR2B5&RX^opDEjM5J6-7jN9&0kr3?ClG3U+=EKg3=3BJi1xwB?%%Dp;GUOlLVQs#Am zZxKtnaSg*%S2pxeduOt!uwI?<>KTHi#rism&5)ovW#df8eSVF38c?{7IdQ~sk;App z>#C_`t(bXFcl&Jbyp7USJXf`XC(F=ONFBGh zNQbLQXEWGMYGaAXD*)GUlv{|`4Sk@1+AUt=*{G|57XkvXY#HQ#u6WqT%P+wSApR7+ z$?EL!KoeUzJ{a8;-s?7B)+aiI7_LE2u_)Xtr8AYkJiG!{P+@DgCLxxh23p2#a!L;ndB)}?h@Fq@S`1o9vt$bq@*Yx@^9?>AjtQJZ@NlLKa=+BHai1- z%X`r-8Jyi1X#GVOwA8Ocjy^WoKeIjD@!zvGI@9UE>) z5}c{aLl;{UXp%I%=N;=w(%YVRZ85z{V{fLkC&=Jk?hOS~AW}9X3J&)Bh$Xt6 z9!7N4^>w_)&NNU9PGo_(*NxLneNd*3B42V+$TJvhy$1$x1V;TFe0fOdCA?Gajpn%z_4eb8;WG|8G(LD1(aZ zy9l^}5!9y^q2m(DdO$>(s6NzhD}*6_e*5Nvaiiht_?)?=y`|-nIr%^@>AIBqoYr6l za?ZgF$tGL2E$`fBgg;!D0Dt91AttzI;-7{oQx_<76u-+_(r#E#>rI=WRkS-8)`&B> zj%;{Y(G+gL@JEv=vub{;7nM3L+#Om5naYpyN6y_l$EfXhxG^(8mM>Zi5)K?>LuL1&ha5|TN*unFPUygX@QrXLR4w}ryA#sJdeitzO z)@VM0B{3bHS@!dG;aXde#ZV;bB@?Q{$TPsQ`jJs>U=B*L7x01DpR4jrRLRo|QYY*r z39G|FaW??8$6=74))a3}^Q?WMd6vqO1=RaxzOT%2xMs3Dr3x*NW?yza^DODLF^_P) zT>5Dtgo=xZ0j*nzo+TWO%aO)YS;`4nQ&on9gZB?LJy_@OIR)-in_zRn^X`AKPDg0r z__@3Fn;Qz3((HuX?vN{vfk<7*-75>ru%-^_%(O_+6~6sdA1BQ+*Q1}ZDbg1c|G&m~ zU4)Q2<_Uo6QNgN?;;WFb8PwNATIqMiVv$PZ@Ruwt}j>Nxd=xM|l?Uv78PrAX& zpoc>A%2g8uQK==X`J7XO{)9tPK~r|@n)26B-kqb=ay!vM8+Udk?E8^?_*U1k5onTvcYj)A&A? z#@%2ABX_EQ6$;M4{NC;(p6~I-@^e@_KQaM)_SC-J!~5aQtF+y7uOQkSUs|>u%$}J-(pXV*WVS9_ zZ^)|GRUiV7H476&y9fDAS@j|6BCXoZC`t+=)joWE4q*01?QI}?t&$Ox@Cil*A6~+}b;@C^1U(Kb#}Q4f3FjKon*Bag z)yO;C#;;~fYWGAL`{QeKCnceon9YAe9S;G_qBmE#bwLO8r^^Sx%vTlBy${8{mUtd@ zdfpjRB@|%*zx-QP()kSsFj;0FIik9zv8+<3f|=ibyYO8hH`gQQxiY_oX>RA9fz9% z=XH^ocPFWC< zYYw-_tV1#hU%qt!puo30dV@pg^{2VM@hB?N)U4RzO`k1O0ykXSxL9LJ!2?L`S$w$` zF&UON11hsEbc(r@t7bpc2`^k&o>0^JmeV_ZwzwSqzMd^N=nBi93l^k%K7|6+=uv%Y zaOg~vyp6=qVE0oUUx~_%PMel+=(68P=KZzrVTZx2Y<2c50YSJQ79iAVJFC=|eT|G^ z=u!J(*oSmviTQZDja_SD8m$i1!f$<1vVD~ZNL0GGj5!%)vVBY3#FGPfo4_wPhHxv& zT%>LA*$8HbCP;UH&0pqRpm7c+$2dTf>(=*cXRs?J*s0b`_jzF<%KEG{k1od7g6b?g z^%b8ol~Dbmt&qaNfhw(7J|HBO7f6=*X=Q7(YKZ*Aq%!-y&UGN^nzmi*M~9kY!#W%H zL-l-2uLF(2K27`Ack^$Pg>QfISJMhjcpS%WTzJ1MWnp?OV%-;l}myUM}BwNQ7E@SOM>P>&k{z-zu6D zMXRt_y^V_QC!3?`YojU9MDwf%jeJ!Wni(LMR3!2tA2cJWz|ln2!^&H*!)u#i(}!n7 z56TEj<3hwq_Lbzcj``2JJa-%A{D1~2!1Ndn4*)^=f<7`IFgV_$dAN#Y31QhO^H7cpPC|G(al*Tj%R+k_(GN}I|W03;7Z1M{iJj1}Jd8=5B=rf}w%$I;m zTklsgX*s42TeS)pXiA4*-f|qiYwa|c-h?-3-}2ShCnU#-4^*j;XhUk$xvaH+>gKa0 z9n|J^CP-e*=8gS2(Wh5>V*7k!H@(>Xe9}2u%#T4G@%Hz9AbEdlfU-_|9H$q4fJ*Z7 zwcjA_gDtW2>RX&J93f$j>2Kv)sz^tmP}?9dX)dhyDACar$o?B9#Xe09VV3anM=30^ zN2$U+uT3dK2=2$RqLZ);#Py{=lq`Z&1>3~k+e@>)z!OwqiFcH41An$>w@|6Q34dee zr}Nqu+an8}#vq&h-R2t;7Z^cE_$WR@V5HWT#~aN43g-`*AV+*X68se-!Zo z^2^G09@u{Btd({k3NBU*rbP-XuESuhy=5{ z3t;hka!+l94au(Ye5YOc>C=+6ybMSA`cBN@8cQjMm)^MC-Q6PD#aA^ z2R47j^%+{{ba(&ouhsMAPWl0^^9+ldQ-6EY$rjyaR z5DmQe%D#{MTA~iL&Weu0Mrp1%iJA3|BapLvF{@U{B^&dQO2b{UJxhL&&}+3+`6+t1 z4^+RX1=}ldWT^aus%B$Qh@_%WX*{g{N&_Yy6}d*yv6>qM<=H`( z>aHF2-~1}#b@<$6-;?Hi`#2rS$p<%499QDhS^W^&aLw!-x*)TYD&2C{3pLwxLJ+Cp zM;eE@|7ia(r_K};LhY$&@TXN(xxH99R2z$rYb7dLIM5)^?a3Ny*PZzGwgqDGu%c9W zJe6E`uO=77X}ml;@W3?~WP=P0no6CqL%n5CRFJG59_rqIi%W268g$VrDGj5_SNU=W zyuC1h&>N(hcW}qqC8NNcEB2fI7Q{Euj3Z%33PByQp1HzVfkyeRVXtb88h9anr_filmkLl z5%8m8ih;r5NqOfzJPpb=kB3~H2F?e2jb;biwBH;88ME1Sd!T$ zHIg*-N9gt%qa{H)z$sv+c1)bCJW{gX>kxnEZoYE*r7?Wy6&tgNjRWY{S*r;nCf08M zK6~0U(}(7t+Kwg+*r_kR$b~s~+StTC+C}$}EVrN#oXp}1OS)L*sFTa$9w$Tl-tk#v zrcNIQjgkq>DP;-L(5y0gcXD|9n=i(9;>ORLbSY{=!a2AO;QeqdI;YodAu zRt1|0MZbL)zsgOZy^y9Ex&kqIK%x5 z@NM@r*=&Zw#Un;H@^PP$Z+>&3n0C1pM%J0Pc0}O9tB`+d z5c^)|^em9vqjz|I+buq@0&}#6S?f9@8B0}9#>n*7CBB?4UX`NGRnCuH`g{k~oP4li zokyxW0)xv|k3BB-<*0caoCpy{vZN@PLl5_PfQM2{<*4Z8_|h$KU9uAi@2uT>?YF z1iPsFkWr9$&AFXExjlI!YW8<5j=F3C577XUasH02n{_>@(0~I|lJ2QHK`!zEv_`%Ok^3t)SUAk#&iQyE_>Y02q$6IrEiy3HGUT-rN z?e#D9)XUP_$J|i>!(k1eZXRmsOa^I`36h<5sh!q>j?`@feP$^>PL|4U1}8U>exCD) zIao49rYatv=SuG{AH+Mtadyn@=J*ws3tP8BQqR>|gD?C3JucQ-plNMPWBL`PacJMEr7&06%t}?@zz2I6j%R`=*L~ zw?prflkXLg@*9xQF8xB5{ON8EVfLlRlI*hK705HPui$AKe_Nb?0!HX@P}!oPbGvcR zvYF@(#hs7$CAL;uii&;!1nrUJ{j`J=M)MkH$*Bvw50VN)w`fmXHE%^u zIUWLembVE`!o}#&xPP!_&|XmZ%ZvYAH#9w1o#n~-Kd>FgX zV`-!2`GrF&(&r;OSL{qc-m!W)I-$Lr=q&)#VI<*f8k)V2BbiVz`+IfVC$j+KDFBo5Hksh|m@|Z3vZgm!k_4AsqC28Tn}V5i4HBE!CI!mieGi z`6R|S#i=H$a zhK5rv*-R2Yvoi3Q894)6GG-_l@!RTn+l(BvTAoX9<0>S1>fXik)53{NbuCkgGK;0h zDcGOQq+)Y8Z$~Z>^38nbeMvYf97W#@yt1OLqNw3AXUQj*0BbWBi+G(kyMd8S0g=P} zjqBd(z5|gELTMVq193Utpad>%Ezy&~1>eafF`~+vQm}u%1p?oE&hbpL-7kwqZtJR_ zr@PC86vHjQ5;;r;PH}5)stSeHF5%Vip98ty{yr;-F3XN;U2o*)Jnc@bw6P#TAZnG|uDPH>d zB<<08a?B0i`PSsM)?aH>LeUrE)b4tPXQP_ewOZ!Y`t$c&Ny6XJBJ>R;XiqX2_26;& z#PIZ9SD#~qTOA>w4p8O|VxaylP|`bg=Pa4Lw)UfGQ^(yg{CD7DdL9DT#`d}-nvj?IVq51 zZj%%)^9ueu8#Ur}Mi+H$x;3)&by$YHlkhaQP~^XL(qh=!xU$220_SfLga*tCWwoQ^uFD1y&B(RQ)av(hcats1Vpc+r^l~2-4rTH(_fBcsc+B z9RPejpYMn7c{^~{M>TcK~H`*r$G*+APVe^;4!H~ zsCYzE`}uh=tg?UoT6uHsM!L#~_H-+z^n{vZA$#hO?UuR6Xm~v0W4Yj?6(a-j8^Ai83o)CV;E&<3YDw*g9qQK&Ohpo-}Ja>Kf zq{1bB>ifq+LD#KKfU{3l+2&(%=P{&0!P)gHI>DWso^rECj+bLD{vC?7sL&Q7*M6Zj zt=9yyjJ-0tZle(z0whB>rT3JsTTUvKb)|Ec4gt00QkvbJ@NMJ{ z(94NYv5UAlEbV*3;MU)@p%^xM$0IuErYR*sFSc=%ln$WMyjp%Vd^OgMgH&FBlZ3m5 zdiUK&+}9-+>qAFgvG_*Wwx#=vUWE0m43%D$-qR?0ywc?7MhK3}2Y8uF&5)HVr^Ec- z(3_O!(fagrk^&{Il6kColJ1eB;yfu}pSd!JUjFstX0jQ7s?UB~Dj29$C z6%ir2qs@G_3=03AEv9C{c(`)fmKKEvk`bs0KTc_I8HY*W1S9$w;k1aWKMD$x_-kIv z!xG!sHHh^X1~c!$&z!g78@Yxti4L3#5IEn^B5q|lMWN}gORzjYspJxZffJ-}#LDPn zlgb%T*&M`!rJwr_1ra_p`*RQT;3k;L>GOC8AKF-PJ*Hp+SN7BZGz+mCvB)*OzBfvN z=~Vdg{m2z%n?uBhMf9b^Fp$OXCMU#gezLNH+!Ss8;y4CmPwX;6z`9}j z;*_xjpxW&I_6} zqA7*_-=1ZyST6Q(O|r*Bk)o?mQlz(TDX(R-ix|hk{8VW19r^A1`>kH-O@(;;q@-b9 z3zZ-3;APZAq6Nzp$uHR_-DwK1<6#y$j96EwF3jB;rjOC?nrFmGTHUn$gZ8q=0@5Rb zvGa_(g}V42xo?kCc$G-w5`c665&X{~17BWPruA3JCDrj;+n3ykEnpR)8_9VKYWaZ@ zeYZ!qt?|`~jF$0BP+i2V1NUM>E-nBn5%s0L z+^H@ZGH(f)vJ6A+7P~tkimb`)%hHn1rAhK=BwP(_A<4P=(PhEJQHO zQct_I<(nC0rYenJ)`Ueiq}anR-8wgMhP~qRR4(L_+ih|E>AR7(lZz+#tfb}a5dt{QZ1F3nER8_I!kspqv3kpAvHFu!cP+0ThWV4Xo- zzQSVT?b&4AKq+P-r6rn7AZtQr5vHy&Q@UykIVW-Tv zhpyUK+S%SW3^BDh2l|y{@3f?4b?omj+jAf(+pnOZ;aMQiA0s-kVIETy(zf@`#T|UW|C^yYqjW z*iBbZ?@IsvL|A|L;F-of?LMT8W-HdJO1KyGd;@)9bI1`Kp#AU(`=A8;O!@c)8wRa; zyu!&lc*d!AvI*C&j9&la=q%P;g@PdbKrC=uBEc=e+2QWa(_iM!GT+P)`gB+Ql>q1C zBB{4bV@XwXk-yR4i(*{^#_AbliRFwueRfNw=BtN^cd7U}enMqjX~)7Fw}4LQ6~*7? zDE7_Zn#EV{c<5-}u?W2+!a#&)&`BO@gHvA>pnM5l9pU+94;X6ljQCH&Fl(O_RU2d(h(V8B(jY6feK!xX$bB z2X8q}{xV8{0girmVGnM9yshLM8ojQhtkKF2kA~Ahv0MI3%`YX*PQ(L0ogqc3L+)#&>p0w!3N`C znqm(0oCLpg35C;&Z*U3V#u&MsYMh-0x}2wt|%@lLHGeSj)xsgY3%Cl{0N&k zEgdT#UGAx7lYiU4HNs33M=ht~vzo}i<>H2ZT_%vET;CN8wG{Vh>1#P3;G=G0#AO?S zuzT#$KOj60xM~q~ zU>Z#mNW(PNpv&!Xwn^U@9S2qwXe7M3w{3MZJB|WSePe$KMtkT@B&x3=v3H?-WGsO5 zsxa9?r**7ESNz7?U6Q3eQ zGvl=6uqS!OYzWg2tX@Q#9#x^F6{Qnwx1X-^ERk=;%q^RnadNkYQ~Fd>W%upwLt;ST zRIW5oNwkG2+89|-p9XlaW|$@7eubNSO7empHQ4O1v6e1gW@ev@dqNXlWEr~!NA>GO zOEPbPJli{;M+m(OX-JvVXBX-6GT@fqTVR3lQiJ}sEc~4OYrXtQl+>HC*6?z0!bxcIFg)79y!bd{$Q=V^JI6YXeR!|0gXxs zWDpgd?_8C@+=KZL$cGa_r>~T9$D0$FBU$&8cc5QQytCUG<5m8i=jD$QqAZeCqr13y ztvPI3lR=Vv8?`*=JIqZadzlbz&voohM6|wM75_F>fM(cr$LDSXoO(rPu8Lv5JhS2~ zCspF9PNz)Xy=FDrZm=Th58o%VZEsD4`1aL~Jo^hyicuIEye`Fp<|5U!N- zGau@7-MVshS_<5Rz^DdK9q=?6GwvhctC+(I*Q8$!VAzXy!wz}jQVJIC!4UCZHGoax z$kOZub63!R*a! z9JG7rwpKfoO3U#5tnVCfS$C3LR|>q|ppFfsDGSV9i_npA-uB;<{Pk4i_8H@mCwfKn zC`@cayts2u+f8!dlXWb~J}ulgcj?|;`5 zhd&4kiut#HIpFpZbEz&*7CyA0-Dac`Ca%lcn>q#l1rOW%T#tU|=Iy4^v_5SrQ9%7p zmfydl@1}P zoufxWS}|p69?q+SU2`HPkY3(Ks3`*ULCOf|Twg#yhj>+o6$~yWXR2Yz`W-Ht32Xke zp^=qXT!D?lU*!=VS;@Ty#%G*UqgaSq{EIDwOo0@na>|xvSRyaVBUk{ifaD(?da;Of2m@$YNl`>T>#Qa5A%wFE;)I%Qb+966S4ngoHP<`4d% z;yxh&CgrT930#Nc@tJJ1aoM7??nIn%QOana{ecqX7aScg_M5buO&ub|&hA%6?zpT- z`R3DrYFIf$f~F!yvB|AK`k0c}Gaiz0g@Y(YFDg4w>NYoEkBNW&Je~QxfV$d0-l&m- zjCi6V*b;vf`ghQcg0(lzmn z@^09+`3q{J%mnMgHbvf8qrlAf42R~hhB*&jKSGwVdRg)>zJchSI4D+tAjuUU(Xj<@ z^coh~K6|DnO=DDIzVDFqJJzY}peeUL6Zc$v$b!4HY5MbowfB=GYquP|H$1GK_meb3 zM4l^51cbkP=hlx#ytD<)as0aM3zDojR|U=)Bprng6OOJ_4s z9GsehAN(Vs-=nlYMQ~#(!Tpvwh~Vq65>u0j?3q8Akm>Nkq6@Zu5v1D&By?)^aci`x_#AcT_-5|T{_lddTY?Q9pE3cr=<;h#$}`^AaAU5 zk%x22n}I(Rt56u=!43fY;!sAu7Pr|dM0R7ZW-jVlkuF^$bEwUcIeKmA&Qhy{<{%vr zvIU4q1Htr^2T|sL6VgV5K;0muvVG?GCe|e?z)9(JYtt1GU!mBfaHERYcWYx@ z(7D{xt_q_gI4UelNuLVO=02~QRdKoy+()zp?fkHuteg(3M3N!=gm}W2@aad3uW~;| zpwWvn7^%_IfmKnyf~CmD|Hj{B__{zLrfj=LWcy*!c>sozq%M?xdVr>o&KE;pJ4U;sNUD%htN9GN ze0B!-{N1YywL%M=y#BEM8POEfOmsdQe|Of$WaPf(QEH9o>AF}%T_UNwPV?f;p{=38 znU4JJmL)u~my&@^5eXD#gyYzeKeaivw?DvOoSeZwTe%q zyukrD)Yg=e1JwY&Twke3Ds*-bu{<#S&1LQ}qrZryKSn|p8`<9Fj;%0|PJ*ARHTXR@ zn0=E&QFrVO@&tLjJXcR$(rbe}`wB&5fy)NosnTfiJ8&=+Te09vXOOf>-!SlLKfOwL z6NKSZEgeVebh#_w%Sn-@7$s~gHAsH-yndQn&Hj`diocG|7Op4AKCP*LVzdZMM+d&pKpqRTi~G0^)G)N*7En~H?se35>gb?gJfvVc z#xd<`OHYt3*S=ZdDSYo2U_;ULoB2B|^G6fQ42lk1?c{NwCkT(k_AoiN8xj>(p93_% z`fz}X$wu383zoX)ehnJ-uGx#a>Sr39REdMzKnc{;I-ly4db&tNLl*dp)UfNK>C#GH zzlw94t0Pt{w2Ok_`CIkLv1tg^6K`Kdr&Sj1-!<7ESd6pbuEV{wo!e+Y@gpFMwr<7c@fg(J-gU{OV7k1*@gy!kNW z*nM%XM;r!CJV^*k7epMyN*;3e zFSd;a;A-p;Dh78D?ZbE`r zHQfYM^-vzWSgjYFPO|?=5nP9TAcFUP28tjh|8QpS`BRNm|NICu7n6+!qD?&uNCbAV@xR4i-t zbqNSXf?f!`&=6ssmPOYhxXxlm!%hs zc^`4T3|Z>PR!#5v;htp%J=x<31Qs)KZL$WNZ>Xn}rMSC($9!cGl4*Pyq%({Cidlh$ zvGW{zGje8%^-0?$Mzj_{v zxw&D*CL@E5_@3n>{Q+tyqxQRe>g-oao=*QFv>^tb&AuR@f+kjjz?xl~vWaioV8q|^ z1oisW8goH9Z`+sA-lZj2<(>K!t(0XJ3)eJ=H)@Z=?ff#HRBa_}zIC)BkTfH^%S zU(hdfZAR##uvyoQ04#f>AoKCi9-Q}GaF4WdNp(ba@1Bgs3LZDmS0Z?@2zsd~3xzvT zo?w|5=$B$Zg6}AZL9MH#1VdDSIqWxB{%+vq+*u5IDl_zJisfb{<~dYWA`K{Qf9)yk zXW00aYxOWv32QBP94}fJI8l%vg!2S<*m=^GLp-o^P=7g9o)0>}@ODD_>swCH*SZ87 zjlV9A!rF91#HZ2r9~Wlx{aR=UJRf*?nSwe#PucCOw2IjJ3qTIi*VEA--A|0(OsHRm zowus-PvWA^C8{qOEv=B_JEIbI2f}b@kHcmQ#RoPb(11YF3oxGB5{NW#%NlA^BJXDm zVhGqdv!oUA@{-~pgmHYm?TTM#W2mm2Z=_&yq~YG;7qJ{0b$AqjW|7lv_o?+c%&IG{ zS{?}9Jec_`8tQ3Q-||X4{qV1w9hBM{nr35kt^k3^nw9ouzQ-@WGzRM4i~t9BK6>9%Y8=pu}j3>tv|*0)q;Dh!@CezkG?P ztx%ZY|LvU=c>D@QLWHBk*AFx}VQ)y-*xKFsPOlXm%1Ubv?k*y!Y;^)Z* z(z@ka#^wuD$AZlh6Ge`LvKFpF`x`>{yd-Rx^XTd;eQm7}u@|af5lC)3~H4B*EGB%gFmAze%HT>Y-3u2=*$A zzJ%zNU*Y+IBztBj@zk(K8Q=Ghn(Fzwu1!9<^AyXI_E4}U22p5A)@6Q!a!V8PCXmXV zf?K2!tM~pG<{sj7(R*JU;=1X?6q6a;YrTb?!65)*V3Df-5I|3rs}5$ z0%gfLU0vBvmB$=+d$=OwS}e0i-_fc?JBlE>s8N7n=)YrP7#&b(pIWrIK}h!?ehq^W zR7!o(bt8f)TVQ9(J98qdR045L+JL zSbf%hMllugoW`Q+CS}lqw^!W?HoaEAUxDn&ttU!v08{CVvaO zlv&0^a98iJ5`-0S3OnU1DG_-=>9}rK6jxV2s>;53>lYTl z@BNt)5~_41qmu@s`0G3{VY8lGT3Oylp1~u^t~#78_Y%=ccH+p~X=R6^KF|-$gT$*z zmQ+6XR4~Hs(cw6Mz-Y}KOAVvw_rz4T)9CU6jJH2HbNJc2dbDJcXwheShWh09r4%gu z?j?iTOd+qOW~GU@8CxnaBFiFIF*=EuKyemuhR&=BJQdR@UVUTPzUNNT9A1aoW~(um z=uyNS@gpQxNf-WVFZ-j9-MwD^t7NuAOzrsYhZNIv(f+8VhT&P6yj4>%IXb9Lc)o8-WiY7SBvSWjB*?sg8 zGyuqEPoKFBnf}Eq&P{k%^@52YWV@=*i@9u(w?C3-mM9j zXwetg1y+h>`I7Jtpo=PqY{cQNR(uuuqk;m}T8KVC&a|uXckxHsn4*viz6LfBYS!cY zAzFnxEeWaC!p}Q;zMP!2r_3d3Dg27DVnk{3s(Ns7xUVeJyGkq`41B&Oi;;mc%P?=o zYj_TN9W;(%Nx0Fp0m$ixp-n<=?j0mX>?<88+ibms5 zeyfpo8bv>i&1NU%QzkqJ&F6A&?H)NdF~CBrt;i-nTj5B^1V_QCfy!?(;>&hmpnU|z z(aik2dSt-$pmu{HY9Uy%73Wm zlA4Z438Bm)l#6ctsC-Tn_{*XiJCmVR*O{md4K!e>MIsD+;yQ4TN82F}1-bvfOF8bG z9d%}eSqLub=&YE~ca7dV$uz23Nd ziElTf+C`TcDN4;!(gK!P5w@F0KW_RN5rLDlSR||Mv#V1$-=e)GJ#xI`M9XZj z&NhyZe|s`N`M60KLlVXHVGJ-H79|)!bH2?A12>R@hBTEt(utK-xFEbwKhE=JNx&WQ zLyj3nr0#S6;ohnb5wwKq&GCKF*%Z&BkJt=Ty7)cspC|eHSVzs7L@77TvzuCWGb|?n9X~ANr-e+)ss67&~&tZql+tKO*O^2;c^W zSK!|{Ze-$tU+0)0>!TM3PXYO8CMAxu+NB)5#5xT55WNKFqDoXeB&{z4h98K*-*c9f z+SxkudG&2Teh(sX=T{lEC7|XHJ*N07%KhaHwG%E5saWz?1JLq?M_jEM6#^vfS50@K zj=)x%egv=!Yc13x0p|N+oUw4o2@R&YD1LQac6`S6vY8Up?$G%) zCH*Mbn@&&Tt0w>@cwN(kE-40hIfp3-Xl5AXg%JxPzfUy|`NVurXQey(zInVSYBEy#9kW- zY-mIXfPPOgBcD<|JV^P~0JJCtOX3ssy58cUsk$!;%~$RRQ6#$y@dkTlFDsCgR^ZFB z=E)}6ExkcRnkFYG$KZw9KEg(<@G<2)4dw5e9FIFSyjDgP3$??e2cLV*^raxtgC{9& z&8r<|5s0DR4Qn?@tbOdN{Dh4%K74gUIgF@pzfjQciBj_OiM>2zKj+p<@kS)YPjR&) z`TRY|4!@YAf#H*HSB7uq-~PL33gQv1?kzi!w3LQ2@J!e7_>y6u0u=foB3%i5R?Dfm zHkBlpEY|LfT}i*B_w2M(Aecrs;3I1!5|LR=5HFLX-7Aw5@CSGSPJW&fTgOgjNfQ!k z1ISn!6yIE~-gHjUxz(`Aj-rhnu7IQO$jK}K^qyTG)NrHw4RWYxkq*XJr8j2N@Z^jK z*lHI;(EAeyzRor~)}kEcD$eXAE`Q|xfca4l*X=uI6K`7)MiaVbw%TLsD{qRJT5n#r z#(K@RC!56$%mW;ucHkDuQCzYUy^RHb6b}tjBQU?kL*Qss-dLFbU7J?n1AdFT>jy2>-2_v;dyjuv~!(j z%i)18$)!w&>7GgX1KIGKQBnvj?&b0rkCDu+H=LCY;k!!I9lm3tbg=w;wNb$f{gt>8 z#C%oDNr8ESe326iF|JCgx9RFc;KRkvfu8` zXzhd9N$;ic^`Vi6gBWg!UjH$DCR8?@%FYoij`nXer%5w#A|Yo4ar0JIf75A3aO~am zdS+Sd;nLjY$DkBG_=Wk`)1_%~72a}0Sq^da27grJM3Jbo&j(eX@@Gpa1DlA7k*u8Q z0oo07x*(r7@&<=su;0mq*SKqnS6J`vrn^sOYj-y+^IavEs5E99 zorzg@8J2~^Th;7zqAmLcjQ7E?$KFtyZz95;jIgFeM~AM@V(sXG+Eh2va`Trm$_0o< zbA;58t(ZlIq_NAf3LzAF9H)fd5Qop>EvXS|b67y2X^^@s{EYz%aPf%cD?J=liTsu1 zY6$o6^LD~R95$U+?@|g4J0Z98`N9NSD#US<1&k5NB9bpNX$GtPz6NMD9S`z`^v*N( z62$2%KFvP3%}uJ({L*8F5|fM|WaOSsrONg(%h+WHyL{JUa4t-@Cg3>Pv2EM7cWm3X zZQFKsY}>Yz9ox3^%=?}*b7rch>g%em>eWBH>%M9&*+{BQjlajjA-v64V3D|R1+=?I z(rKvo)(5+bw_BqW({(uKq4y2#HT>VHFHenpRnFjtO1j-C@+jJJski!0S)^-_( z1hQZ@18z5S5I311Jo-OVd&~!LtOY-95woE@BnQw%YXRa>xNXOH z$AXQj$zN(E*yFoO7=njd-B9MY26Sr9Z`*Ou4ZyU0SEdJz+`C?nMW3-f_+XhK6Mlg{~mfK{(#0GNl8*GcgVRLLKh);RQ;x}@?mdbCzI!1F? z4V&{TGYiAzL6M_;cB;ord}%{LtuvXNxyWv;Z&A8P0HVL-)bH(;NsF~2A#@~Qh7oX& zCffE_7^OdGg&7Gm?3mm0l}A2t=l4-;%261kf=bUe)X_MFl*Q{>r0s9z)pX`Wk{|Wk zQIEiv4lIu!cf_@Kgwv%PcZPZaKoMuWs-1!hXKmrV+%@4CgF(m*{`I>VJS`3H?ps72 zE*Z?yxkx9nokU#9HoXUij4-D>yO zghAkrXqPnLs8OQ0&q+%rS$AT3q8t)}u+IKUet<1uBYZ<+tOMOQe zpwnhwQ9qWb-^|Z%>b)1^Nq(2eeH9%;FII)%znf`}_U|$iNTciiG=ZX0mlP0!f8Wg1 z%2H+`qFOe^@w|;j7?~0ez0N*aI}jjl!}34Am*&*CwB2Q4b~6(-jBs z`usgPfM5HU8Nt4K;51OUo3*Sh<0SY6eE41m_#)z~cq5klM=wIMc>k$?2x(cukPo?w zN`pG-jvfsBV_{m-FTQC9-oa0-CH4uO+WUY~Xus;KKwWIc^7~6CoKdmQEkyQp%A(CY zak7Q-q4zO88p4K^qBij!A=2!CJgJenmeB_x-MFdil3M@@L203M@;>?TR~BD`*jF!x z3Nxb*-5{gC3;m-+e>U~;m|u{dEXib4a#8}28c8I{9)jdlz!iu2Jpy>#)iDxGZa4@$ z9Jg9IgYPabQ?4rtJ?Qa*J*<{5E_!%VwV}=ScWX~l0(`GX*69aq)_$S_c>{MV*O_|c z*LLYSdstO3mT4EiPKM^-sHa1pWr8oXO#uu7>}DT0!;s$fl$b$@2{V$P=a4I94c?>W zD8?^XJ1Ap#Hog~+?`+BqhAZv3%n;EqXjMJ=+Vw2b3b?sSw*-Ow*XcDD&-i?_Ajm`) zXv(cs4gzIoL%B|Xm`dD`lg)XBTgsCuGsL%vndn0)c&ah>9ud9)`83;slh390P`jyq zs6#l|Q}+|%F_amzJcP!YkkO>LNExsC+*71^yga?p*lJnm;X89j5ib1>TBJbV-MDQiR+_lUe*^kr}b`8=KNXXQNtKz zYQ*|E2#Pr|852n|$!X@#c49ikF%rgyi#-D6SW5t_-L&GGrx814GF$u zp6p#N>nDaRn?3g{nQa1$&>(OfP+Uy{V9$NU2^3j$p<{-?b;=G*P5AhJV33}aEUmU+ zWUTo%P4f1$tF{*ubwV&iek|QHV{&TWx6{vjcg&hZvIlB$_fJySzB6>?d-+oL^^#Uq zcwH87*!3!j7@{zO7F>OEKbP`}_ori$T99n!U0*5qroEuNIFOb4EK{ned~0r|xRk^+ zc*so7e#io^h}DgZM&TK(!m$n$HVcCR2tmZ>p+y!UOQ6XpV^{c1`)9_xidW4}JXE)h}pW(UthKpAcsMzk$i8TB1bP$wANue4EGh`*j3*S2Q6fE1{XXcW!K~>)5O`~q8e$7QIZ6N4=)nb% z7;|@_IGw%YrSp2s!xiwCn8Ds=n-$;hX*6Yi|NN2s(=Cnkb%`33f&LO>!NwzqkJZ8K z4C-61#(}^G=RW(-dIE;n&BgCSFPKA>SajH5DPlbKp$H|Qu9Ti@o-i=1jHvBOwdn3916R*^sTlNmTp<*^9Q#0r)!hx zsdaOas(fopx0UG-v85g0QQG}goX&WmBDY}#B2o2A*hNAiXkt&WY-fxEDfX#|OsaK# zJ!%5F>_f&NxTW=SSl&U2@K{}P{@wxn-P zN$W+!6b#JkiS5CmTZa2y5ecN5iMuj|YY+3obswSKWSg))B}RjZ{gryNIw!3nW2k6x zPQ0O7oK4C0&cO7)9@2lcn{gnP{7!9-ptoT_pJquwNWTN=Yi2?mN_{mO1im~iu&ujb=DfFfTK{J}jjq{1lw3~WX3 zMX`=YuBg;@A3oJX(3M%r>t^JsB**9G_e@l|X{8m3J_aE%J!CkjZjPZOI*M#bOsM-5 z?ovRN&qb!RV-v<#!FgD`HLx<7Q6ITKk12pjBjibAZwM}-dEevoA9?de@YmTd4(YjH zO&E?720e+MTFFi94S@yKZUwgQ2Y){%?CXQ30OAJ|YQJM*mXSNfe8x*_lJ-$_cKmL; z(jSjw*$$2su4Gkf{Oo<&n+sBWFV=W{{rE&LSf>~y0DHm}bXWoB8p>g_E+9`M%eem+CJY9pF0B`guPHBhvmY#?29eSz<{VLq zsQx>n=e8g66M1BpyXKQY4pQo+%Mk)7QVca_)uHvHz$Ca`#-4Hh)GHW%jIBH!nrk`_ zr{|Psm;eoA^%8ag5x}Q5>!+5EfmMW=nmVSReR)qkB6cyltkUOKD#DemoVfc(X6%H^ zoH0n`2&iI}rDi}bs+4nFiSitw^{_GHl}^m^P3f2%W%P}dK3Num*xtr`fKvg7BTgE1 zZa6NYZ137S&j1m%dabV*TZ1hyEZ`a7?L|)AK&J_0P{y^-A6xa8?CgB$AZU+-Tpu4< zP4aIF4>)AduWnn{cw`!o;Y6#kJLnes&8t&KP!ZBQNS5GBnBU3f7;P#E3E@43fo+1A z;DUD6iPfvuK+|2vjDaDy&nbL$=055sl1j&KVQ>Iwbse5seOO>E0`YfbUw?s(Cnb_X zI;Ow}{Ozxop@Aj*Se+I*AfFA0u>CT*(PUFd55@`kb6l>a$QNe$trrO97opd|i!b_@ z%s(e(KwEUSX3BL|(Q)v9&)Q@T5p4^cLr09DI^VerQinKxl zlU=E$E@DG1goa{mee_H@ZM@P&Lq&j*=29oiKHapX#uu-b023C`Jt0hp`8Iq9m7qQh z%pdsYCt8t$5B-uKkmK$B5%*DOM<{JVQTCq7c1rk6OiYV1bH)DCV+zhZ4HV;jw^2mo zxmNI-<1O2CSq>YQ#iEZTnw!vXRm6)$NBiRnqfnas&d(h##yxdStYh)UrN1PRy*C;hpivgpipYDH8E=>@Os=vQX^&BicD^OJT>v6nwdrU8@2 z#}O}u#^6LQ?Qw6Gs=VNz9jU^{>1}Ct%CKx%0R((pJ^xn;FPh8#K9OtcXLWaeiLQ+= z2evDaq#RD4O@ViToj{1h_Jax40Vj)$DIq2_#_+FSghd*Zh{S+7U19&5zveiT#rZ;Id z>hzRwU=#V&H1n&I+>0kUg6zEv9-5mJ^Rkxf_P_LO`p*XF)uz6$@wKjXVyDWMS9s1{ z8@rIar^>KwP5jnA>Z)HaT+_7ISm!>~eLgP!NL{TQkzTiuGH1k0n48GVp09D-R&JDB z*vT8-F(xPGIp`1K%RLkWM#Qj8#D0NrfwKpO<17r7K_*e)R1;b3NFR!1wjo{qfZE!p zoo5bC^-*9Zgb3YbcC<5#!q9x*VWSos+n82WXO$XUmgW(zLl?o4vVk# zTY3aRcd91H6XX+f>_ke)W6JB=nYA+1;F=KLILNPI4LzHCXH{;&!@<1-js&ftwOSoY zVFc123K)z7alkKI)ZUg~3|l{~@orLJ(=6YEYixjy7{W-JKsS@wB?UDDJlVLt2b{j5 zbXnWK6H0(Ayu#3&2vMX7?wOxzBq?nZD=OOl>y-X49takkOf)XB?eV3I}L_Hh12zn_MY`}wO zv1c7HWW)XsKMf7vyF?^JZ;sBe!WDvD2RBnMc#_U5B&V3uhaK)nNFTK#gLj8GJDpRM zhM$M#PQL6csi7TEB%T1ttL%PU(xF?kfUjxLlb;(LO5G1juw8nciCW0`s8HA2;u5~{ zIhQu2e)-t_Q$8}RY0Sq&$GmA;)+#FeO_^-EJAN;9_0`@jf6g<@02Wm8?5a;kNTS_f z6trl?XQPVhHorDP+1AA}N7?~_%PjSV_SjA3QB=BLbRIW&>J+%1OCQbZVqE&8{ZEPW zsA2+JF0nv6?AK@2-MTldhwDMzOQ4uQxG!x^D~xp!_^h-t<2PFd$KJQS(k91{z~_BC z;}}~Rk4Yd&7Ec1axD&T(RBH+i7*ZhX$evhLY-A7|F$Zwc~Weg+ty2z|LFb*DlN zlAeBv>q`ag@&iq@uThs^!n1e8SaPlfmrmoiXZVq#x5MyV-2=luoTLG6UlSAX=1}}h z)45nCW5Qem8{xjet#DQ2#9{aQw6z*FD3LGrM!c9_KU^tt)!{W}J`)i`c+XG=)V;Do1{z2dHn%|wSKanVoqV-!K4Egt&97PmVDrK!Z;F@rOMoX= zTCmbAV8T_J+{D#A47%#~VP%&DOR|DTQ9rC2H%}AXm5iGy1xGl<)qH}3-yuDZ-b+84 zmFJ*tr@&f^a;1J1ZN>-e(Ug{1Tkw2)aN?tjHLJ}A>F_OBhDidDfQU9Psb$qwIDekV zR$YsvamesBfA|#*eGQ7*BD*#flxAY`gr66+JZZMIra5WXe0(=OeU?0!D%TN;IN|!c zIq9cl@nVq?pppnGOm=>k_;)`#xC<(Jftg;{YT0F8Kh6?SOd`HO-U|P48=+SWmP-2J zO8HqVMom9vW7xnd#uoJPQ_!$OKS0jD3;w=b$V2P%7+%(I?pByN$CC%swmy-k3@|r3n@660mhwJ z1)?r)rx+0~9P1-8YlYx1P%VEMzAsi|#TF0Ch4~16_)wD3;0rW$r!I#-`i|e0@!Y3} zLc}PZ*t?x~+Zyh!gG;*;Ztu5ra6aEEU`2{d{^4@YwWRh?39o!hyBDn8JfE~Jk*so+ zPrfMW4V~(MbGu`5mHYtKRR(TlBq!{;<(SSL1%zMqXij`P>l;eb?@8fRV!zG$#W>#H zg$-7_A-&l-5xu`olrQ(m3@Au0sCc;yWTql%hV4C!9VE4SyorXym-)^9*n;xU>bufoxFJu)f z!N>icxz2_CoA!&}fC<`at?u*^#q>90*hV@lYn-K3eW+G(+0;0^wuzVFt1Xy{TDyUC zf6aLx0cW6CDFuxMf|oJNNLXO06-66)&Q56&Jszz>Yjf}ZFDo+EL(V*Gy zr|CaIB(86*SB4E8^2j|yK22RY_-iXJ%GA%&X}|>uN$PG+n7Q#aJh|CH=7pENK>@ER z0!-}kNXpD0#b3xE5U9U+b{vdy*X8$NN|CV0`VLBaJhDZ-xpyPs&*O@7`h8Ntv3z`{ zh}jTp^_pgHrv{&Q@2om=3reOQ{UJVAdcnhz#ECByID8&P&5Rr{p zFj00h($V4|$0NfM7FKM8=^%mFfIOR-OaRQZnh<{u#}`zxERKAEsLWpOtenY5Hp-c~ zoJZY2jE7PE(D~ado|+O~58qMm!6COp2Ty-bL%)LeHAEB=1kDmvlxKzmK+<5CCzzSw zQJbAyK&M-aMx8-gL)gqGOJ4puu}1DHg>7Iq4Tz3M@oJ-?|4|E-ShUe*;TN-uif+-0 z>MfND^%{XKJ_(U^$=_1k+)r;2iem1m=4};tZJpD~2gVg-cQrPC3SqM~mr31^GG{r= zdGX8-kOpzn>J4s~&9j+KtQz9}{&Y&1eB172I*l z^gHm)H&-Qoo&XEjjpAhC?9yDfPp;PoS`E`#N-NIyNsgH^(VG#SRtN&b_>|tD+a+L| zMm}O9Hiy;B{CQoUl~unMhn|&CXIkWG@Jg_L(7aLwx^53g6lEb%e{8v>jRTIJKvv&0 z{4(^FTJJl1sBRT`%0!TZ4>gx0t?a=?y+da?2Voo4?yIIQ(#6FV6a_Bck<&La%XIrY zjJ*D&a#=r=icPEE(5U@Mv16stP&;0<#|9Cz;8;*qZ?!u`qS668S;*wke+W`P8pA~l z`bOa(rVsP3Rmszji!h}1yNwYp#`f<1x#7>phPL+AGLhNRqLrbn?B7|FaMGd5W=v1bucv!w(|2)hU3CN;lNWKQLyC8^r}(Rkt$1-tg`lNF z1K}xR=6pv(`)UsCb^;lqM)3Ljv0wRyZ7m?@%qR@R_V)sgIj@m82`xOQv+A6bpD5+t z9{l;}Ft{s~{nPw7ri;ky8sF8ZL$uxXpeG7uZaiqQX|UQ7PW6vuIC}X|%$LjYZ!kCv zoU)C{`7TBYH;{ty?{LLtszcbc^GGUWgnZO!P;d4hg8{w^gQIa~I=b>0sURl0TdC;Z zhETH?D7bFv)i1U6viC@ZVA1B;?H4egk<8-Ly)KR(=-#}KmuXR(uc&D86hz_(D? z=|R$|ZUWH{?a(T~ci5TmwI@H{G`pq{&VaSM&&xBC4S1+DGYyifG!k{(astKxM~u;e_2(ny8U{R z#*3Q@%axoJ>Qez0(AehI`FPIjf+=4#l=i!!Q@vhL+Pu=zu$US!`mi>^aR z5!~;EKQ=a53!TDn6& z(5EYUk#lTEqhV(;jd|Hy)u*g$+kaUsBxly0(DW6f+~Ifk1F2L$%eLt`?gIqt#aMWU ziH#pD|9lE(bD1boO|Z~jEnvKm+Z_oxK{~V9zaKW6%>~n282*AGZYc$D zWa&eBegdWL&fPJSzP~-1 z#tP&YPi-#F;eX(cApiytEzesQiBCd(gawd&!YDpS{ohx#x7~x3-Rc=sz^8JaKxG%= zU}w=Ku#BR65*ZnMh9$gnnrPsaQ?h}l)ICyavS+RDqbS?LXj8=b*#xr}g1gM~&=C4* zDcMvBsiZ4Td5_?H-<}6zKUX9_lVf_&+yvUzZvRj!i30ln`s8t#@egJ@y}cu=~KT5a&S8NYa8 zq)FYdhqjw3AkmjQ_u-jY$7Odp`&jJZ&3C^8jxh!M{hXKu=<=`peF(o{uBPKv!kZQ0eH%L8hFL*k>!~5kB-%gS%HNiGZar?G9h}JX!+bU#L5rB4FQlvg-4r2C|$e zMemk64)+&~{>cuvgD7u&W065UT>FII7hO^&IvP7A@=q+wAuLGc>wHQN_LpSAa10(@ z>iLV@;mB{nu*z*c9V2}DaMgAxBMQ?lQ#=jTeNQt6nIhDpgxyM}&KYIV``^#&x*~5) zqK={D$}vYcoE&}46Nnb1%mHVcXTwtolEeHD7jE;wZ>wfH%QWz_CYs38exYE3AG%#x zcTo@B##`UANx!u=>$OQ;9Y07+>7}5R83Vs3w>8h-Xl+{UR9#9ZG#cZma4D!l^0C3r!!QEm14^k_x?UV zd#J@<2Tp*Ww%Sp2HD=~gTM9)(C$kyGw*K57Ixj!pISv%?fQGcbF^%M%@EOLP?*~~x z&RbH8^|2e|Ndyf8Mf75CII6I2B&@yL`JW@QFIERKnSNw=%LDJW(;kWoO9r2 zNkAhK<@?H}%S20~Qgi-!P1Uh-D6jwR^vUJg2i_P6_{isQ@1F!47TI*M8SL6 zy@)VJ27r|4-ksD?WThI8)3bR_M(YM1%IDkmXfg5J=7ax2Yl?FQxT-V`4H74@O6qSA3iz z7_aDZQme!{=`;}`WEH3p`K(h0ht9R*Dwx+WtX9j&{3<|zf{=`7QSLy#5OC5e->d;f z=swY8mfvxwyH-}MZ&u9vC{cy#3U{sg5rbP00L{e~;@Ia`Vzv}Y`{tET=I-Z;wGxuQ zI%XAZMM)Z7&CYn$WO;m8_7!a^is{hot-N86)QE_0xIwaRK61xsPpd2Qb$&#&EO=+K zS)M!Z9-=_q{6JpN&M&~X7u(_GYRan~_kzd@0|M3O7h^qrgut`l)y3#~%TZQ{yMaX} zl+E7Y-p%hOl5OXMXkj2{ONodiUn}-lHO|9ABJMR31snf2p5FS3!R;c%T48u*-2_H` zK~jNCmDE5Y&Na94+0=kdKkdkumc)C>PqZ0^3sj7l^Ng&Vfhdl4&vfa^X$DK;B)d0L zC^1Y{74)<;X0k*OuEZ#u0kk5sF})?it*AGHwCE~f6fQ~TZ(VP^gF75czq@{7&8=CC z*alC~)f;sln+zP@m^DKm`-w@KnE6%v;k2Vn<1Q(qXkuB0JNCC4hyvExFq~DpJvNls z?61Z)t#z!djK8?COat9$3e6mwS!Kb^CAxuxD{l%#zhn4Hq48Br>Fh~+eU0tginewSjW z*NXby#52vip+U?VYHXc~S==VPvwmK*wBD55mHhp_418!fVuqNw^ut<;z9t<=bj%i@ zMe4jrhjGr?Ve;XQ=JT)EQhb|ad7y&Nx4?|TGqVW*9h7HfSac9dhDF{Njo6LFpHS8y z#E-M8vjRfYJTPBERo_e@`AWD45)&W{BSpY#)@bp*Xe2-a#SYTjE zFjRvDLuV8D}O4)yo~w5Jxw~Rkw<%qgVB341BWz#0Gs3lmfF_+Jt5dq zW~XV5P!>>u7nF7q^%B_r_)ZbZ4jGB{JVMSt(YQ|i?cUZ)B4|zDPr<3g|$HpLo_$kP+z3nvX(Q6(iI#%Xe#u}dx{rHCC zpN45T6`<9t^0d#$aA8H5=>l)0D~VUot!&-k@ZQ4p5nah*+=Vw^_)Jed0)kqc7D+zB zb3LMJ44HCE!gM3bB~Y?L&*-XQDC*whGeh>bSFZ+zh#z{za^{Z(bC>GXe4wC63Jll1 zD-g8JlEjza?TO~NRguO#%5=PT74-Y&AYl_|w#79%KrLt?Y9KX#o(iQO8m@S5r5YF| zb4rkp6#_&0z}*s}gh%7hy*7+`> zHA5A3#%{_zg_fe&_h%EbJ4;B$hlw7V8_Wu`_!8Ka-S>l^ogLWcd9?Qgb9V~<{!N@w z{TK82IZ5|QrNswGRdh_1OLIn88D<4Olt!u!8Uu$6xb8BkRD$OqE(~UGOi52F^oMS3 zAK~#gn1m|fzc5CAJ=ls*M2SUgsJMK0 z_%+Tjdf|aW^l$+>uN&1xy#ii%l#-?W;-oXSH}CEjs1vC>MWJFU_)Z+CVa0opQ+(WV4kC3&Xd#T!mthK8{nv`TY`A$(Uwgct(cwa%x;Qgllgu#zx(M;8 zU`e}P)Agnc-cM^kQ>ZnWD9j2X^u^A4{>2G%s`|M!{oU`1l48jmx19NQPGd}kw^w_p zP;mF6UsEMEgG@dX%XW%PMMuMFyMON`BIx|;SjkD6d-|RgGS0UjfMi;-FsO~Dr$hs! zO#|Oj{ee5c*78j+VR(OAQ*RaCAgt=Betv(CeD1`I%^tD_?zRq|KwXmg)}wNFYJZUT?Ce}mA-xY%qi~Q!??`$6Y9Pwj5s*Gx zybPKG4-VW%ibj!_!KcQ>qupooc10OeN?&~2$?tAxIYdrU?U1Kt^>0V4;H{WHNk`MC z2kv1tV{ddi{PT-ksjV^}8AuR?<0rj{?;Om2EI)b0a=@zbw~JI5<}do{G9?@at`p(3 zC6}58lIl4kPSxhg(3ckF(hM(Ns(Q2=YN^;ZbyD-bo8wQRcdh{$Di#`w=m;SxiPuHb zPw=j~kcRhGivi4lJ$z+Rr^hoE4m35rHDf_~Jrd~t-80bA#w5PLE$a;DHZm+^>9 z&eYeSttW~)&&h7PAoo#=2>%JlZ9=m%-gSb#3?B#`Q?TgG8Vrk(ad~u)ceK`Na{;W^ zIv1<9}5>+?KD%;gbv=%rkyElm6B_O(2 z6Jj`HKag_7Mx%+8FI>SkN_WOxL>0w6F>< zWM3_Om=iSY7Q-urD43ys(PM4#`ZMTEt7OP(>wWnL!BYsK8YtKe*Y5l)ca*?$##vK8 zU#O*o$Scb3cFeN&b9Hh^XzvPWL%Q{%nH0`~lOEx#1os~NH!3+~%somCe@VNa_?90U z>a^)QUl9;qYJO7oxwRmTfyX1D}!UmH3pvJoRMGJj01rS~iJ=*J=^D8TV-sk!Noq%@^ z5CGiIrNF?2J|Gk0_@fRm-MMQ*rv1If065+C*Xuvq1$@k%%TH(*06^dS?g4L)Pw5aM z%Yf6j-8p|t0RJg~_ve*suJ7`_ZmKUF(A0ZTCm;rRQyvfi?4;lH0onkX=YY4*x`#Da zz?U~>_`z-8DAk1D=BD1;ZQrUb*5*OJKY-(GHy%L!_WKnliZ=7--M!O(6HwKRDz*Yjn&6J^7dU`Q^cKux<*M?LuO&G_{)>w-$ku2Oi7M4HE z-%gZH&4-QXs+k5Ch%p`=)oMjL6mR)`{!JubrtCHD*3B3+&j|)|?t}C3Q6R;T^s5Na zerQ~dq8aShV7mYJ$GgijIr z$+0zvMb0>iQjED$>y$`dc?Zo@++wYXi$n^K#ImWeTFbUsC7aO=r|cob-b2$Ct><6^ z{$3f5U$ubk(jKkyKMJ4 z7&U8dN-Q^AA+}ZA2E8n1xEDFAPfpZbRd#gk5_$r;z`^m#sZroa_Y&?@GP^moCxX;N;Qf4%^%Tv@176#Bl zEO@}B=8o$skT@L*MY*+$G~HODFeuEOZ%xy5RiJ}f7bp~{iXlsnQJ7!;dEpdX7gZ|ij`Hh4{d`t&c{vrNaWuB z$I`*Kg8+#FzKGzY_wvo-1#<7td?PhW!w$P#E(-k6Yv6n!hWp7G_a5ZjQ~z2gmQzs{ z(&543s2=#kSYWl>fg{?64UzPk;geDPsWr2Pw$@e<`F4jqQyr4egx& z=k$MU*yKd?-K`_cNJpDJqKK+z5V-vfv|fs8>vFI3RPz1-UlCnHDnU1F zX^gV~%WwiamxHC0>C27Ewed^SToudEhzB#$NwublIrgp(;L`x|WyxRVPPJif?TTT@ zQQvWj0WR)NEPadmHQuDlK-g)W9{GKGfoKTH74b%YKqPKZSWV(nwtV8oy?y)olDD2J zQ?~4wLq)qS*THwB=?T~pMGo6vjyZUz6_ZRmMWU}#GEkw>+>VA~c=T}HWvf|L51P@D zwsX!Pb}Oo0z4Sw_rck>lAAAOEYD5VE{8b?wX!j{Pm^vn#Vp}4gi(mE;+d8+b(&Zx1{U6m$?2gRyQ+};Gq;eBQvNidhx=D7#|!GR8FSndvyF6ZUEApG>T z6@^AsQkwJ^Wev{KyG_=txpm_5W9H6;{)Q+7yGwBHMsvieJgxXb5ytXwn5AtRuf2il z4LS&`rVpTsnrVS8OcVlt_`k8n{PM*lRkj^|JdEm{=hdO@KJC;uy5Gi+7qroUWTzpJcWFB9FgzMarp;Mg;sFS zcbQh#&p|kEoP7>0Guk=@dY&5DR6fgD{@*8V)KMaz2>nfABHIfG%_}fS)X1A0|*vJpbqLNwRVrl0>0w*iSZ^bO87cKniqSczz zjLLh}8YBy&|3=a3({9Ky=4Jf8oTmiriiVyo1OEB`F<8H(N~%-K#@5wD+~&Mj2e^0c zEQ%F%wtT(0{fp{W&;o5VMYU1S2IyE-6|{*TJSHZel}A1qa&w{R`8S+N3ur>C0j{ir zlgNdkNAJxj&s2=G2xEmT(-`u@1t(^`i1wVAV>$jQD&!0i^uPN#==T+t9jLOtbhrev zs;*VzVUTQ<_^``fPw-0F#J=vur5*t5e!Kb)-WIt;`^FKLQBAT`$_+17DHO#1v9kVT zW2^aeS|X0~MG|8T`mD}H^uI21|B%5MY7c?Euht-88>J=qOt{rUGq7D52ubw%cVJt! zokrbZE}ZA~V3u{O#>kWHro{l%@$?@syy3QfU4t;}t9rCWk;(2^@58~ehUgJJ;QPWI zZPP!9g6>eODUj&HFr#|DLMKwx1^LW#82}qn&pB^M(Fe!Xg|eZ(6@dSctL2;RhqvcY ziQLZ|iYS(n=T(8L-#NZcV?mc}qlS*7h6E&P(BS=U#KhJTYlQ_#%0uNb%dX_+37m+Q zgx)Wm`ol-vP{#`X^pU4PINLNCRjisM?3pz!ilmzGSj@oax1>dmP%jqb+VGJ@orhXQ zX?x)&J9*1%RIr~^l@VoA!5jY<9~@zWMp($rUwcrg;Ib33gtB#Bx5lb*oaV`rH>uAz z6XdrhwR3+zeirkh)M&|ZVWFzY6eDy_N_j6ZIIdCWz&jOUP9X4KxBB1S3WW4Usb?a< zWzxWF6+QV0YOAvda5c=l(46Yx!0|dZsO6wQExvQAGH+avz5bUX0dmmBLU#y94I^B%c?w*PC9~r&Qimm0{m@JH)L-?tJ>MW@c$knk{Gj6mNjV_$x2k0p*Jb1X0AZyaVqGY(-j1QtIB-O=zp{l5AR-d%+HnV!A$-Z>l2 zSpi@d? zOGRy`ev04RuYYPgamy>m7_D^HeT8n2+26JG_bU3Fx^lkmLI3p&>cdOI8Vm?X6aPQ? zMg9N#CF*HsX#4LtBx~sG{4Za!hAvK)9{)v{`h?vkBVx}h4b({z(5mn?G%e(KVfY|0 zNb|pqjtRS#PLy6!-ED%F&AJ^ni5((Ogc&EUW7utM_|u(Tvblwr=g1`SpeP4Z(MWSn z199x#9D#ld&~9wl3bmLuJIW&zPZ~Z%T>2^I43+dL$PV;#z}sZAk&Cas9kC5D3euVo z{{WOQs%proFs&V&agW}mwKjvNl}4#0cD>s_`b}uuyG$pB7+RET@?w`#8fv2N7tvCR zr|uo2#NLrcm&so%SSN+x0cS6&+xtX91yXgyqgNQJh)a~PZyyzJOP`R}ztxkrgxQCk6@Bs;v)gSpp0 z;D>A(Be<*Y*Y-l+ygML#woAL9ZRHk8LU0SoRlnYWI8qEi?#Wf_H30|Eo!QQ+c@ZJb z|EkXm;c4LBt3!UaMBW`amf7AQ(D>civh-^%{cdHB#m#!r8Y>I#$P;n?r7a4E0!)Tg zI7=S%URT$+5%u6|L|V^w)l9i7$WC<1FM9|0Q&JO(F48$|z+;*`Wento|BQ!rcUtWV z+zqpp~+7==F`3+z#b?` zaH&y3qqCaDOPUVsJ3E^e1V^Jy+i(h=aOR(vm#Yff8>}2~%*x&@qk*KD&uztRzTUf4 zy+>3>^HTWq7Gcx)_U^75`ek=s%Nx_pw+-{BR>w`{*ip$*X#eRUbP)bi2ATgu21QKmT%7DJO$=@RD~a~D|J1Iw|2=ou+81*o z{`B?@I`v&i1sVkZQXxw9TJ+^KEs^qm1+_3Luj@YQ_gw$^z{8Od$=Zgqs3&|a9=#4U8LM0=fBGyR(A1hB&eIFJ$%E0#dTNQ*C)t`22Mxvg*Ivd|C?hpzo28ukLT?CZJphRXln96 z5=1V0)Po(btm!<7;@PHB&Z$foDxwhL#&3FJoW#N|F@c{4{dvL12v$bm9B)Q^kDcdL zK}&u8l&0r9eK*+k_-t-!QsuZ)1D{HUp0uvhlW!6%18OO~AX!9f-dMFhi#b5i{0c27 zmUcCO-<-sae(Rh6X4-!L1~(cp*4x;?3-RU${9yKrqo42UP!4V@*Vq&8CbkgN5rF~G z{R6!nrIVmoXu~$8gsB!OXI2pdB2fM;w7{NKpI}WO!8ep&`HaV0H2>LB9(FX>xSI{f!YglS_5%^MR2#HuC;;K#9DiG)?2aAF;6YQ4B*K#! zjK0<0C~UE~Q#RM(-i$-5@@{CJ0K3#OG4enOeNZp7>q~pOoh0d7g)JegKjgBOaGdv0 z`YNF>N*(B!Fy=*j>y031`98t5+Sjcf{F1}WO8m;R^iuK6X>s0cU3twzVl=2AJ?WNE z-N>^Z%Ab*Ig3|i9#C`|}t%_Rf-sm@y0N(gtl$>L9W^0(HW81dvq+%Nt+qP||V%tu| zww+W|u~o6{WRi2b*UahZb$aISx7Pl%*LB_RgLi)ut1bJ7RTWQ873c5HXY2hdiq*7T z_G{l$I$13|)Sh(b^SY#-G7j!SsXu~N^0>YQ1Ns7rBhii#As)4j9-`2>g=*PFAz*~E zAx&ek@3ILHi(N{sgsyLYyo5~evm0AJB?tf1rC{uvGWX#W-s^#2kK5;iWz4vxlOx$vLA{gn%; z*0vd(D8rkZa{8^dY?KIAhIl_?B|K}<70I-Z&V2wRGZT~e!T#vK-EuWCLYmKml1e2g z?cv}@dcWRu|7j_ZTNMde$glWK6vI%98!4QPGRu-9_V{t3TLIvU74!2B$NIPV8-_l6 zgHVh|1gK~XQ5N3H78A9ti)bVvB5p*H3*m z;tk+;uO0HLKEMoO6<3yS0tv#hb$9E)z9|#{n1n*A2;0(o1=`h7&ovOsRaBfRstvCD zu6mRO7_e&0hJGVeJ-n+$c#N;8%R%YX_#0(}GOxF>Cz?6!7+aNwIuu#oEvbhC1>Ez3 z&fz?sRp{VM(D=F?*sIoQ=Nsj#zb!;_^x&O}+3v%i^K}SUsX%bz#ii{@RM)%Vo~dD; zQ-=dE*Aum_{VLdtQJxNg^H-$L?jisl_F|zn6|7oj37I>OU7WWA!hM5u3g&X5R!~L< zdm)9pTj2VX=uaS>yCBR}mMP+d4mRcmIA4pvV~|z=FDdw~<|EfVn2%65xtF93;=9OF zd)99x1V#1n-wR0lePoZ7!ESuFHWBYO#k?}dFGiC}IWNG^lHTX`h7bj9yd|bpQ|8mO z(>&^a{e*s)#MM-5oY$g!vE-fncq-1`E|3ZA&`$JLP`4UZmc)Pwo6wNI!aJHwu81ab zc_i?KF&FLjMLv2TC)TOFLr9T^Sx$7*eoM55)sr=uM|Ye@`}dO8r-pgaA)Bu42aPA# z232Ys#;vQyB5~Kzh>(u4a)T{1V0WccKVK zi}EqxDD(0LXXKbTIel)}dS*N2Ro1Y3CkU9bC!MY<9YAE#o6Lhrjq|q`?0kue+x8K2 zIoKEOsZs&~Y!;%2h(_vF-BMDepuAS+)IF+*o&o+g2h8rl@Y$YCV_=?_geLsVbe#e2 zR^YkpfrHo^?nYT4qBiWUcPTO@c{Dx)=ga$|5v z4C+D=JT2bKA#It<$h0O0MgFGypk0WdQ-<<7ZNfq@+>N|Lf|$nf4&CQ1&wdUy+~i;d zC8o%JPm7c0>F7bQsPPSK?moH_C&N`pSHdXX%+B-iSUDNjU0AMZEwkXHwzEh{wBDwFfx zk!c5_gwV-Sk>|l$YvmZ8aVH5g;u&lTR^2G=7|6m1doc2Si~&5?R6`Gvl{TUMXD%l; z-pv%~LNN$TL0eJdXLJcq{gh%@A~Vz6g9%qaMNH!cf2EIt@fCrAR|;u8c)L@&6(w7j zZ0lr?h?TI;uY*eD0pxlk)ZOC;PD+vC_wrbP5!?<^X3a^aRtJ?Ia2gk$sY11qCJJwi zFUwwV^^O|gJ*r6(Iz=L_kylLeMQh%Lz(9Qa7vxLQ7XkZ>?{JnZpeU693^R#p0@J%k zv(^LBs71uTQ4-1GwvD*8q=y{iR1#`=$KgAF1Zm&)@vS+t`gNlo4`sFH{ygE7PVkL>$qaBGo>xJAXW>97(kbcEivWJp^k(7ye4$U+ zui)aunI102q8gR&f+v*0I&P9zoLT)>d47IqcVeETFo3+9P}b+51rPZf_RT8CJ0QhV zKI-u%vhoHcR&UlGa>qGntq#A9D4Esd_Od9DK@ItyA-6M1lDvYDMO}*CjY5d`m`1Ty zn=qf3@c^NdWeupVgImcStMFc7lk}RpD+%E3w370X_P{a;K~p)qNs{>a!X+a^pVy~S zHu>nf@!9s8CATu#*ab6Ql0HNNk24axZFDcHUWr>sp9>r@E@Al<%vAoAX;qv=PHqEn z?F;H@`YXS-fwO!*VDGPuiyrW>48>tb2+{A@4ye@voptbyW^0%R!dk2bs!~&cxSO-Yo8>E?I`62jQ>Scb!EjELH?LOYy zd1;R*UA>aseT31+!HlLWV!ydvp`roW(7|_h-naKbk?IJ% z=R~`a5BFI1?s*f&r%Uv&P-xx>lW6)ZNzDIIlDPesP+<9=LqW_z-^JX?{lDabs$?GaB z$J2qaGvkg%NU92o@a8KN25#bTBD|1xbsu!I=(>&`!uLQb2h`*YaTwT-!LpWN3MkPN z%Ay$~wEEf>8c@8PsTUc^!U$I25J&+<5s2vxxaU25&fLrA$F!fSl#pW(1-aEa99nfu ziF`T!h!;>?pVFK^kWrwjqwQ+#;}i%|KlK=L|B9ax;&J+EMcLF9tzop)m%^R0YrhL} zawX3cV}I2mjU7ja<6&5+Tr3WaG?%a89 z6mDYQG#ozs(m9t#0F?jLvl)%m)Q13zgJnet2V9A$=hH;J0PU|))e$%=XqK5I z?<{b7U~HYJ^=B8jC=GH#*|51l+7E!YD=6z6IS}_}pduPj(DJS<bPQ*U-A+>2StO+~Rs0f+wRlR^#g zH~cKf^VLjK{9gAl7jfHkD5qvUVoFA%P4NTqA;krL-?$aIq-QoJ^5Z4HW&dbgmH;FL z7b}OIXc=#hb}(Hq=84}D)uT(*WT2wYje0$^TVL+3LtpmbPp%iCs84ht4SB0x zx0jRnUNiULw+Bu`ZS{Lx0VE3Iekn4lCt`L6L$9wZ1`!HiKS$|8J2vi%G9nMx=6RY} zK?EG5Tpb-w>)T*B6G?%aGCy)rca<4I{>b+%!$a?I113)GehGKpmYLL=MGu(FmW5;m zKB=$N%qgJg+i)KHa0^fU8Xw%8tiEsI+kE^zKFGSpFsA<$(h~mPgrxXCg%mJ!a@KdU zb#SCt{M$tZV-sTsW1Ih!P+8YDg8{*Jv!rG%MFmRlor47RyE(k)SfS(zF92kS8!HF1 ztoX~3tBW6*)LfayeNFD;j7rSv1uPZ$lmS52VqDX8(^fe~0gwqIVd6*vq1j@$%DerI zo3<*)_UWXb*Fb_{;CY@YFP&QxH>6^|yg8otw3G2rNm^kj>~;DWjT)n-$vZ_Os#E=Q zN0xWBcg}Mve$cD6qG*GMAXiINn}scJtrMtRwI8uKTWWns`ifN`n;aQyAUuC~sswDV zQ5WgiHos5VJ5L@099kr?TT?1$?QkxJU)a7u`VU9D#{M!A5DD+@!=W{i=lcyoE6#^> z>)~$7)%088TLq24QN(54KzUuEo^b}L34RP2U?_|U5}s)!xj-bE>?;{=K_t+SDz+^I z@G;>zq@X6Ljjh#XJ0GyK(CI}7wWLkPj-aANRT|cr3mFGQ?*6x}_lrabI_I8m4FI3+ zNgy8%A&8AF|iiq#lfS9U4kk4XE*-WY#jC ziAmBJjwWHABp_c31|l?2WJdjlFvKsc^@R}3d4;u4(X6bV)cok=6_%INoVfQed;h+n zej~MPR`Sb|POocAs@MUxneQ2F`=y!IHyHQXDD@a!A2a`0>$a(?`?Y`_0<1wA6?#Fb z_&m1F&SEQY>#-JF(Q|&Y`8%weEsAB@N-q=z2}t*;eAC)oyc^lj3rlW^=}2$_R?GC2 zo5xRu&C-Q!@kTg&V$ip4aeWuajMq@`x%cgtEbnK~FQ_pGJcE6K006`w{tarne}!7w z)>Ycr#rUt8*mnuxztL9Nw)sT+z55ewzDksyCDI_MQWlv6GGm288+iTzi4LI2NaWwV zZyqor2T)yudaU199Sk|q_K~~H-&mhDbbVigb#v$E4Plqy>*4Zwr zq{kv5xQ84#*S6p*P0eE& zoSc-&z|wh*YpN%El7|hW4|4XeKgi@g#=T{r01V?K&#~?0qgmQ&r{>DJ!&}Cl7!ckD zPO#58s%1610ys5NtyIPT9JNz4BXvpH4(qWVrWQA;%Vp}%5fGc@us3vyDjtXe;mO`y zPNr{A!mFlAZ6ONahD~dof43JL<;}X~#Heqq@Jo9p)U6ZA#YuQd551L<2#xD{Y-vzzi7^nXP%oj=h#=BC zcIcf5WViev%|+nUS@Psb^n|(4CTQQnwpoLgzo*0-%EN6x;rWfS#z^?=n_1Uv{|x+I z7LGTm=9#hxvBh+W!K89t9&{cT#Knfo4x$T2yudts<6fvm_{y@!Vo;T@AVaS4&(v!d z^J6?kUz||LhIANDnp@1Qpr(s8r|(EK>eL(djlss>VQ+{tH|?W}>%w}>J38roP~zqf$c?1(&iDr))gBx&BxKcqPVjmkz6Zc<(^t;N zh?@9?u<&M!=A~(b29`OyA#w@cHywU5KE-#sdhd@P`MUnsU9@s~EXdGL#1H=!@vqg3 zvXi-$xs$oEBfWz0-y>cnbL+oGI*RT#hGq`7Hs&724qtdTDzAUyjq=%6sh!JEgVy-R z;sYkR)Rs%9FPsBHFb+i2GVca8$aNcn=NC?}DW4f_-}GO7%i)Wb@$fYRSz%sxAZ3>J z4nRyO(y&)IeH`9Jw++9zX;@z@-KDOdv`c&pB9WNYYcT21!B!J_ z26WS7wU3NvfoC|kNn~^)p`3l|o7h1Xke_w|^s$n`k~+Ca^`w7swV zfOKD`M;iQ=ZPc9y)m z9t1pY`?533z!h@JmnZMsA7AqQi2aLN@gOxBe_@2|FYYz#ck5(ih& z>m7TBPCROq=GyEHNCwxil} zjKGzBP!pUCPMH3(B*ko3Hq zBEh-y^N{fpo0kmE4;XjjwWKSZFZSa(-V3}2&t4+=k|R#gIu2g}KV{@xxiV*XIQgp( zOVBzJo4?ua*!jF&_+ROebFxjr%gOpyxj#Jn>d zH`}&JUqvJ6O{_Gbo%KuwGb8d1srTGC6^46>InObwosk5>$^v0K)AjTVIS3sI)eh;s z$9>l^)DHmusLGQfCmszMw~K@U7g?ew|$^PG+XgIT=* zkSF6-UK`gTnZ7;ChbNKA#L}6uIYH8dv&bkMj3n-c%1lhBUuHMS1mMmNg|S35<;o;& zU2O7VGQY+Sy|oDf(+(hM5zWF9RaY@;AZgHorh3iZ0jZ`hB*NgY$^=*&6)*_;2O=7o zJ%i8(AD+`%Vt_eb{z_I&wp?uj%@a#`fi0NWr4%C%Cz4tpG}p?DXG-2GwuTVx+&Uwc zxXj#(5Ua#$Q>pNR?K0RBTqrb;FgO6Y8X>wsTKmx2%fwmoZ9v&d?I)Hl(qdS_)TRrx zxjA2)c6ka2*QLs-GVSIxJODU^%4YXozUJ(8rsG&&<3z@Ox9A8oVuGeZdc|}~44&Zt zgBv@wZX*=5+Tct6`pM&Mbc$zq z-m!*y{tZblD*7P+jF?}Yz#QPUI@r@Ks6EK-XRz?*_r}PUz$c=${ttLp+k1Gs7&tp{ zJ#Dx4VvR?{o-+%;p$LW;m(6;PY!4A?gs*O|-`lCyZAi3VvCR@lPFQUTXC~ zMsx{2UfIay=&?puDqlsEb@rTOMm6bk>%AT9Sr{3iVwzLk83Y&eWQ``U^$yi5)W6_K z28eOWdh{(`VkqsV0m^!6uzri6#oXsUct<>l*O7xB?f7~{DfZ{xmXpmgM{oA$z!N?g z*573OmFn2QkJG!N8KdtDx__U%3lt|O3 ziOvdrNuL}ZqXxD=T9$@OV{h1qRQBaWjM8PlLLfX}Js=iE=H@ESU?)_Yb~<@9yZKX! z2!nxiWzEZC$;$`ZoDp0f@m(LyJb!cxShBc1usGGfyF@6ZE>w;#dFyZ3yiM&Oio)_3 zc|C&=L@eF`i;~U{O*}AxtZpjQ)T>2_D_b5ct(%Y=+}TV{`4n{%ikU;GRILb*HY*|e zbJWYcbs+R2T^^ayMq{YhnndhC(o4y#*77)!J@}e?+5s<{8CrG3Z`H}bI8tsRsj<|( zT)+2Lj(E?xLz~7`wRqCp6nSq>M|uIKm6>dx>yn%UPP8Pt+Fj64xSA>Uz4 zKa~&%MmKgUMB&M)vKm+UnG}>n3So%kg0-rQgJxtHE_HJ9)1^>5?~O z%gu#&O9?0cJuN`7CAWoO4M8v7HH{ibiXcyBC;^u4VJ%lJK!Cr4QLY#A;WprH`EReG z!h=*TGXua%WMnS+4_V;m67)>9P(1$wflV!ZW9BN%H2X?lQ+C;p%m$TC^nvFsPo|x9^ViP98TQ%1cq6H6RpTLeXZN~ zO$Tx#Cp1h;s;p*wjjDr}nn`47ttkhyKN;&jn?jP8lH}@Bf6#OSt+=Usj;6F+4nu>a za8_65pC-YF7l#7Ik2LIfgbvhhiD|I{)68Ht*uZ-|_w`!kk9S^9POT29@t^A7|5}!YS%dROd@{}XuLbpg&$N)0t)b=LOmBY8K72NH^;_vHm3m(7%&2l#@a_{O z<`;NQ<3NNuure{p3*Sy$T?mP-6H6_U!?<2JHnX~4tm50PX{STVlE=vgyQCNlCIrzs z$`oF2w#yv@zX}qi(%oR4Vh#u`LC8wW$mB>>DcEwNlxKM}Y3#{1dWQ^y&;GC^3=Bgc zq*&%k;bgw@z*+miC68Kp4>Sk?UhsjgS+h4|SSb44iSzDMGn4d-YDycUeg=!i_!yc& z$2fYt(6ZUZW5bh^(nbLECjBUy@GRs3!w9dWo%<4#|lEbIVNj9szA_q0~zFhjgair?8_<$tqfvM&WKEe!oBG565QysP|g{ zoP=X)V@&NFVRl#`_uL9bK?t9=DvWXvU0K=$&6DHpTSZ16GY@|rW`N(#&QG_#n?B4- zWRIdVF39>K`YCcMZH#s8?hriaVzcQL(`7e7|7T8+ao|tKnbo%UpE2*&uKYtvjD|AC z$Dcj21cdrF^cKh*Y{BKw#(6VsB8Zeqc%!tn9;Om<8;WT+uWz>)e22*oQzBej?sY!a zs-p_%66Wkv_<}M_jnJ8C&W;$cTVj^f>PFs|Gv`Dsla$NXVPbRqM-$VWcc3qz=>$d1 z!#;uL`d6Si{sFX%xy@&_ZT-cxq_KnLU(KR}zJ;;jXZiittX<65#@OMj82`V=?PZX) zKadp=sbm!S7|k`B@ez6;b%gqGkj;GGCMTux2o_2!s0q9d#s^(}&VM+<*s@2UBx#`$ zO$ka{ffHxT+@y`fBYeEto5Eh6nW-g0YDA=^hc4Y&?;yL@_LxnIFsa(5m?jA1(iUx< zOg)hez&RjX0S$ZtA5;NxrB${kQ{AjnedfG5Y5cn&diPfEXl2hNA^dbZmKZW1*R*P9 zhC!fKd=|mfeTb1N@fW^Z7`Zu%w^^}D7Ip8?VthG@mfL8(7n^049uIF+G9jH$HPNW+ zX}1HQo%B}nTu^4QfjKR4rx~g%SEv`JOt1&bRk~MGeZ2-azv?Ij^^O*uU`JRYN`{of zSk-1o7i>(*01yq@;JMr-U_2_|JQ8d!^HI=*42!7&Q8`erH(Ve-(%B%I!$ryk zl>%G_*13?^DxusEJYu+7m;>A05tfeZ;O{X%kPd-^4}-kz$d+s!v%A0#AlJdW`!*tN z0^Q2haO22tVe)*@|185@r=LA{3TlpW`>ol6sI_lTYOK*4+>{}2hU^L3O1cTdcTi$l zf;hrDod4KNG*6XvGe*{|N1)M zqalU4J6~Ef=l!n}>D^SNL=zYQ;1ucK1V;adz-G2qM#c{O0{nmX9slPfo=OjiATK~^ zr9Yd={C5*Pe*mEl?ANr?=Q#^kS_s)VB3Z&R|M7IUg|Bk+4 zgI;9vm}zjc+uNb|(LR}F6{(R%wHzfT3_DmrOBth*f?5^o9IM?P!+aun=DuL5=(9g2 z1BB|u1u4xysZP}3bJJOEvf*+Grs+f^8~~om8tsp+e?Cgg!ym5wbW>d+KfpeFM`I49 zKvd#NSg~Vk`Fa#Di%uCITT{%2BXyd#QY8C9bSs9g-Z?xYCqw$T|9hyO-w=?^Hrb!} z`o{XnhFR~mew+bLO{P1moxe8Y9>DxUqZKw#gykrzY4KLBC!5HR>347eM-&utSEd{V zPb7y62p~r)0Xnj(O+`|qg+dWPY}!a0G*@Hl@w549q;1jkoB)9k&kVeghr$V-rYM5k z)VDtyRV?lz?9F+4T`oKTev4kOV()&OKd7xE+?guJG&RF7HHVdcarsq-|x;6Mb&5xzmoZOJ- z0NoS|Xtzv~-F3u$(Cg$a}xPmCq%g`~CJ`OX8c|5yFPg zldI}q+Y6L`fN$*JVC(Q7L;6*RPwKrdRLv4ojE1y8fTV|7n7t1ZxMtIu@cp6)ELvI! zbmOZph(OqSW%>tXcHQ!ml117%55&#x?Q@FE#>c|Z-Ti*p7z|u1BVKVsUdCMHG*Z5E z~|5Q zwuv=l(>&|iq?7nVKdsojXO8NROV0J{yAr{b_hzizl~;0ZYKghu4Us8^az4$ux?hTU zOPg(I`p+ai%<`lR6v3tBJrNXV0S0`{;1dAVZZ`cF0W zaI|6znOipxW6dTck+NEjkZ7vD)Tu}#-Uv0V3DY~yt_4UAqf+pDdr?~h#fIs%z5T`M6i zP=Ymt05RNQd6IA-PnZ+vav&dQm9`I4l1#b;IvR!vb~mo5ZLLECb|rOj^{gkhaIv26 zKiNHixA4tB@V^zc)+og1$R%4^~4gbygXc*d9->cQ?cDVW34H<*BqDqv?~5Q91J@EXcN7tjt$e2 zRMxo*;5Bbwy9QzJvK)UPPEf2+spmPvw3*DV$4lpsH>f|Ph(Z4tpOoh(MfMCfTV8_d z8-)6NCeB@`fNP9aJl3qJD(%6a3tCQR(0g4-mvLNUe6Jv6m()?23lpZ9Qem)%W{?Ne za(HSg`u4j%Q*&Q&(t5D9u8>eOZG|h`alC_L@=R*~b(=IZ0q@`ptXY%z!L@yTe%8J) zVVr2a=)$=GIsZjblJsgt|MFM%qD~=zWiCLwJilR>L098$^ILfp#fJ#sh105Lwk&Q2 z3ls;;dxf86Y)_O;hd7wdM_XZk<*(n{I{R&Jbk4_BFdWBYe)e^jbW&({RQhormsDgQ z<Sy8Orq8|0=|qD|cH%`pvX;p(H5qlYm> z)gj9v{wBCle7;a-;|=Sl2a`og27(kmsp>dXSzPMeSWH0+-;X$8jGkdJG zvA`^dZ!ja4$Xo-9An{@Yod<7M9cspiq#Dh5u3@-c2(T#|j-x@?ejqu(N8okKJ`|bY!C{Rgm*oxv8J5oV&S=glWEHchW~U;prw%)!w$Qk zqXh7=YQmr+@%prmv^tJot3YFvt*2Kp?C~#i>Lq@quNiKf-YBBQX*Mj;%F?>(J^H0M6dzp|$2nKV>om%*L8O8 z(NP7@EQGw!UcCLtg;XP@xu(w^cn0^C4$bia{1Qs4I|vQtQz(Ley<36y521wht?mA| zPiHotKE3_DZWiXv#E4f4lALYOmHydq1_;F1Yss5x$h%Uq*|-ZpSwT&;3I(D2vOo0j zoJqV2bHOGOts*_wL1idyg-)y@Tb`Jr9@*{tVio*o!&0G&sd1w;Y53(6a5V)(I&=2l zKA{Bti8je{7By zQP#1m2JJ_B!)Uv%P@{OU7Voi_HP6eu`s(LF7G79knnYosMWS#y!FtqsIe!LAqNx5I zYnPrubT>9VImGNVeL+NSShdMOQ5`a3!ep9JgrguYAu5D2W4<6yW4KeYhcFQ9@dYz5 zTW)3v%4LCQdKv4I_vmTGJa_fis(=~g^$9i4(?pJco5S5fEUG;1_$>q_Xo|Gen@6G} z1Hpt)�q>+b~8A7F16utBd&ILxm6UQ_JQ_4)k5G4^&kB;?^3*RPLt69xvo$Z!o~7 zq;BsfJD$|ubfuc|@r|G7|W8Cy?_fWF+i9YZhfM$W3|Ls%pghAb3 zKGFZvCv{~>%VZSp(5qCrRB;k5=V{f{4erV(GHN zY|$Sd_S1j)|GGSEX#4dkgVWzz!}%2Tm=y+u;msFHnrbC$ z()guGk)|MJcx&C#hGWvZGN>&OS0O=UN1B{2!vhLc*9b{fP~jmwuI#I+kEJ}<&h%ol z-Ux+1>S1LP3_Hsd_z^UaR|j(JqUgF0o_jK<3qIQ)^*~mGZbD;o& zS{v$vCuF!tdI-ljbF@5V#A|e@TdHV5d$Gmbs6xdH)*yE*Q^8Ylg^^6XyhFLcD3&D#Bpl|^+i09%X%bBLC#9?3B~20 z$8BcRQQa=DBIl+*x*yu6c7h9=v#H#raH(h<^TiqLUf~FFq?D`DdESx)!7U!7ev)Jx z-z;oyeznh*AdW633V!B?aYvMJdlSaje{5?+u}mc-l~LX((I6gVk+j^b^LcUdAv7~9 zqXFZVQrhrYylq$N3=UbU0iJ~mE7@ePmK2AyNki@UGYDi8Xk~?r2Y+S4>xEwV#PHGA z)yg9U`;be7v8))RNS)tQp(1FQ^g$uQ;NTK&_pT`77QJk&tw9}-26weY9RB!x+)vAt zO@2-4k+GTROY|eldLd{%Gp%aH;7E2ET1nk?nc^64SYm*O`9}|pe*A|M^djG5=(6uU z@|S1M*yan^pPn7V{lAaGg=}q1%uSsg^qtIY|NhSs<%#G|$GRRUX&lu_;+LMxAtdV* zsPBY}Bv*O)0mxf8?2j?}u5WQYk)Z;bG|WBCzVFlTrd=Ie#q#1=`ol>(Yq4P#)Afe2 zTDES>ByHR3@_loBO)Xi^6Xe-Ev<~s!Bt`sjj8#CpJFjBOVc#G0-9j3C@)`a*_3x`v zL0_Zr@k^Bxmu+|N&35lCNo99l!hyi)!0{alxBB)2@dT2qQ`O9puSq!O*ChP7A)YIF zJBebUUEVoF)QS2<$dgv){4n*TmWqvs4}XO%BC%J4Lif&oWz&)PEe!LXbq#6rqeS_vS3(2 zW)fhM(?{=R;ES%u!ELIeVB&$?S+TeG_Fy%jcN&-cm%Mi6`>UQ(WB%Zk0|Ef} z{jaJ;a9-awo!OO|Q`|wnE)b91{oo7b1x?Vs5vcjkKlV5JfM0ggR z`g^p^D8r>vnfC6z+VP2mrZ zp`nkZZ$7P|Z)AAvv^zP^?q{n`p=k6m*BdLI$4u2rFG6$Xj~(4xP-Xr;D|2Y;yzQd+ z5WUdq8VxVjPiv zNT0H@)t9pwQ#5?&$`;@Zf@yl0AcLlqilq?8j#`W(sn6)ki`po;mvsP&{2kDxh}TW} zI7F5K69Roz%>V@W_Hq}2x2HomQH%AvPou&U*5ho?h3b34dU&3lKvB@5=?AT}E+VMh zyC%XFRq6N~DxO{78zSsgxBsQr7FjP9!(CK6g8*+N@R1JHuxru;Z~y7TGEW0@e>`O3 z+|r&Jr5wXvm|J46lf^DUnILJpV0w8GCQT99U4XL0&x!POMPa5g3DB`QwIbH+FczX@O9Ykj6{TX@Ab-p+i3f?^F%)JNtBZKJ#B& zE}}_-tz4$@-KAX#A9K!Xgd77fW0=ne@r_C_%)sbDAKfl<%1~`TdqlPeNQAbo#jjmi zv?~7UB`HV!)8HM%VDdr|!;g8*oCYAe0)S$&gP zf_1af@VA8kWgGcKvVXROCGAuY#%jIc@9_s#W~pc0--yWPQuQ!23pZEvB`v?&7E)d| zAOOR|k^uaYEVJuRJHb%cun!aR6<4%0Kk7Tq1s)uO^H$GMC4@u?cqtwbX{X>piHWhP zu}ox4H9X=MW>LGS2WePx`eS#5r0thIaq|OP8egEm?p6r^`1f< zP+EkGDh@I&P@;ak!qIQjW!CJ?A2D|Y0-C50`36z);ko6dQ^an_$na~kFD5+hYTM}UoBddCudmB^`sQ8R<}2oixz@h?5nW-r9D7;3AstVX7P- zwj#^*Jeum){+Sb-0`I=9m%pr$zpPzs8kjSrZIH-S&=WFsWfVGxzWc-dA^e46i%W_H zyofeYlAIuxw>BSklvHwW(1vj4WK>slMaQ?L&aXPsjTlM!*X^rgm7KqJwj+h8-jODaj#l?C|B*o?8HuN z*t{vUkcP4Ko2iAF68!#E!*zhRdkMuOhpUWYj zCRa%PcNDf}Y8yC;#kbPD7$_kzs5Po^Y0TzNnDkGAkR1g)k$zV(odZh4^rDN9m+5{J zXV43_^oQzM=18>z@0#|o>ycfW_q?A^l1=I~#7}yFa4tVR&p8!^c&xjoyG#Oh15^_r zKgP3kUXs5lbk;H!UF2Z&SXafCeuW`ngC=n(Y=$QkA@Mts4z*)T4vxodT#8%;%;FO1 zkLVt-Z#v8{YnV|T<^nrFZEGQp)tTwv=xLKl6N9~+Zfl5|)lPC(LaIRDB^ZGVy{6)C zU*x5QTv>!G&JTF3zJI6>)1I@kZXt7yC2S{qKtHCCHN zQX7Alq?>Jjv+3IZR+egOb@~Mm(c(DB}1~_2v)neD2mo>NN zWP};xrWDmD0tQPEgZ5k(&EhSzVW7R=a;e(tb2wVY$e~pWQF4WKmacIwfJLuCtwP1) zH+wgF3Rin(nS>K{u(iuY4NdisqW|n&8YQ$tgKnsvuLrvpVkC>7 z-r@nD<<0VhsJuA6dBQL{dUlV0PwmRq+U1%a7~Jb;dg!>;Ozb$>gXH5TiJ+W@+a{9! zbM>lpj(EGTIEEvxqbZDCH-Z7M+rP{r$rpwMQQ97rc?r>mJ_?bdgcJ=B0+~$|RHyQr z$=bn}Is3D4`Q_gewTQ5Irxaw^6r5z2x0}7Rj)Gw#wkcS~b{1oJOSUElg6DL}@Vy-+Z zZ?U(`+%uFyQuBEzf;HSY;3AOHPP~^-(Kw%Do&TZjHeR@Ho$T{Uyl=ia<>MsjUf3M4 zdWu)Ce6b(>%_Rv~CsA{Vv%*GwLD0|G4?~lOQ$6x}y_FY6BKK0MML`|XwgKq;un&_5 z=424%0ft8emG{g@@1}-l>Xca>Cq?c1Tu9Qh&bzIdg5N7yLFkBu< z-(Q8@%h0%2@r$Zc%bXZ$$@_ShQ$77iAs6c`SB06*D!3Vc!g;SE)k3XyKO`n~&$FUs zx8vN#l72HmO^&x=)wda|N2#;iHK^r_N05I900$w=6M$MLr9ZECD8%uclY(z40sd#VML~7wVNc6 zPgtuHgEw5uNGJyXNLpz~*R`-6`GB((AmcYP03a?(|0aT)qE#f0#?!+~Sx`ETsm)9F z+j009=f_+LVq3lR+YIJybFaoeYtVfv74elwh_Z0+!PNOQ}s^< zi0@H@seULg9a}@I4hK^ za3_+>s;wDq)C+6*yhONoLbGuMuC@d8p?4`dr^YhOa3)VK?tIwnpMPZNT!RjPAFwx4QUFw!JNdWeQq(Er>Ea9D_CK z51|v=fQ1yFL#ffr*!YCUl+LvINp(I@!;&OwZqcLclgww)K+%^Yj&O`-cJ;QURcnB! zz;RN8Al*tAG&EVl{g#L&rknJU65){moMZo2XXgUW^!~^3-!?W@u8|bSHN&Y%7nd!T z(H!?nlTPU}Qjs-6MEHj#h5EN7<&sN8!a)ymkhA>}_IkT-QPMSa)$w@$T+FSu#^3c1WhLnm!rQF7^7fb= zL!UaAe#!e4cgIs}&xu~XX*jcQ!VY^ho|Ce*i8_1^TaZY;mWWuktu)eVF=}}kDb3WG zw#x6;CD%qPOZ$<_0mQL)y}bsUQjLpYuG$sOi@n*e+;?%#oS%-cHv2)bH?TyDQ((c+ z;LKJ0aMvaBd1A|val4|rv@c6JjN}?yoNCz1QI@}2hv4;SzgJKwl~QE4L{D${3_kP3 zNQEsXUeiH6ud4NCj*dZ$1&1+J%p1u+$ef-e?Zwi?GaYwMXyZb=F^-yHVQFsLKeptG zH@<#1L$F2M-xl)6q$l$D>J06+yrh-K@bpqsaw#d1lT@!yiSujU{8YKOYGrxon=J(8 zI&Ed1LZRCM>x|0i>J4v_TCK^%$$n~_wBh8mx$`|^R^G!5qOaz04mR&@>49*)k7`Q! z%qjf+7oP7f61Do-(b8J!RfJQfnyHfO&sRDXmoJmPs>f{cKb}>_c=3rX$@1{# zJe#SnyR)Zo2o;Hp@v-y>LS>Q`sqwgHM#qx8tklO+^q0}&vK8E4O9Q3&vuu-e^d0-2 z#wn|ko(CZwmaJRmVx;a}_i_37^nqN4)zNm;#tYR)j@==fk}DRCnDwlE@kBmssDFz8 zo)?h3VFe{-CnsvQE;9U6{Bm~JVU$zoE_p}QQt|f%etUQ>qh5p?%Dg`Y;-3uPy#C0< z%*UwHom(Pz`_$FFnaYWmXzN=BJ6xnrA51@#AJAo}&w2IoV`s;wd_mYwEaJnSm{skg zUJmRW!m8oDg;7uI!nwNz1J7sw)jHcc%XFPlp z-)QBLkJ}T1H$DSEyMBR1DD5kIP-pcqAoDjq*vGWlXQO2{Nf6 z4ifjJYh!0`@Qx;K+S*OoA$ywFka!rq)Q+Xvwr{+kfXdfpDwHfw-=|QLk$gKX(|f!s zgme4+*@=X^zt7|*NIqNLq}OcQF-?8!CVxbk#QAV-RMGyl^n+lh4Qtc0s*es4GpJht zT%25jwPd!F0=>oXVR=J?FQGAh?a3zl@|=9uWWen;g0kGbdHJ+;YB<5;5-ou0a0{N?92vf+0Nxf+X(#Y8!21pjE!r`1n2 z?&`DRHmcqE>{3;s_V!Wkbrpd&5Er?IhzJ)<^WWclHqGJGPJc0H+2#dWHjos>E+|HC zj$mjKx}&cI79;sh!IcM1w(@XNx{G(`I25JT<1p-D8_Eosel3Vy!KWomcqO5|ZfvP( z_OGlPM3gK$`|D8M73mAeH_f?2=09ro@oQ6K-=6DIGGgUW81BZc4|T+rmQH=?jA9J- zs4XGRlIACkywLgkKga< z1^qYM3gVwnHLzW;3litfw9ZompaMZrip4}4c7+{s@PhzWKuD^xXi?@0VVeZ@gs+2uHX305 z$x{U21;B>km5>7a)_qq%xO^Or1WP}05M-k90!XN+_`7ohvKZ<^pbi_qgDzkXg>%6Q zw6zfK6Ow4IXd@z=50)GmK=?zJqWL1UYH&DMmSPBj`~D;vF4CF=zly=?hxHJAl>J}g zh5A?E6~H-sT}TC4XUyLgO2Db$=rbNd?R8yx&WEpb95|+k~K-vEmR$RIeQz z0`84~-bwfcU_|s6!5QFnUo{A$HVE@qJr!^mc(D$8Eog^g7FsxlgTOa%&;wi>h7r2b z=WHjAp8tmW+^bI#0LBt9bD#bMP)h>@6aWAK2mo}3i%T)Uib+Q%004z3000&M003=e zbYWy+bYU-aXmxF3ZeeL`E^uyV<$7gMn_u{4aFkUbIkL0~9Dw+>7nYe`ogde%zU5c=JMXbLZaYoadZ#CyDx~B8P)XjtK&Ra1`XF zKLPiVmkS*gc&yT{&I4{3PVzdg!0XR17s6$9eG~{p15%KdQ1{F^%JumIp3B|qZZ6+A z&{UkiH_>SdH6J^d8tRK5L>}2X^~dIdzvkY2r7DlBGa0&sjE?a`g`4(=8G3_Gy@Go> zHU_N}n1Ga&0N2cv*zN&j23@MBv>QvdS6HlHe=I+4yC1G9Y16QQnf*xI*vqNc(`Y{F z)Nt#*xoMxW012Q6k3_y(Aft)AAR|DR5D-VCMMsmTLEZ#mgwlX%g78C;81RFg0+A*0 zaTCSS`w;$r8|3uffQ^MEqsrGzMwm+O9r=XkQ7;;`v;a772dq3^2OU05$$wzjhD~Ji z7j|{|r@6l4>hJHry>9N#br;1)@7X>Zmx$*sEPopw5n;)zEEkmX_g2djTJxI}h7A$F zm+N^C!WEx*Gh}3E5F&Ka!LtoBhD37OdotXw8VLONnd+0HP|U{}yYq|T{G5+8TKdi? zeDeW(5wiw?zY%Hrul27iM^ivr#bDb2IIY=Fv^@WR*81(-3pY~(y%~@1rXC;h>Jq+{ zUcY|7%ghf(p&MML!4pk2SI0JX2y6w(FFJXM8LqwJsWzP7>dSX$?%k+DZNa2LDUH4r zQ#7qBuIv}2pZX}AspjhQoN3|^=+!6{(1jhyfSC`QnMUa&3LJJNh)I=a;10XN3;cmk zRER7^Gt>(c_cqkvnbM&{tEitE!`@^`{F?|;@bPj&O2(?(pR)e;bW!4EJRxPwsuI}D ziRy`I9K92!a$;@~fh|upGSLY)sQRed9Mgb1KVZZ4J(;QWQI7IL$aTvJAsH|6K@yxy z^6!F3S?;GgRb+-2p3ylu zN&;_JE^%yg7>={HSi_JAoRVGlXcrZqgwrT)!YOXJ5pj3n{4H{U9=-yi|FZ*8b=M>4 z6IxB62%Hc2aJ(ZdFay+nU=l-FM!N1ru72(kipif*-2TI;wsO*o-n( z8XVW>Nna&RV+x%4-LDKDctgd@H9jPOA+62)(A)Uat z1i`sOcZ{>lG}L}@@p^cSG2x2T7&zj-Ch*m!)7AB_gE=RDXN8)FMl2OEC(RvK zO;1Qe$RvM*pJO;`%Wslm4Qla^t=0=JM{C9$4Sr_tTsq-xA9E7VrM-Pk`9l>&Aw*j; z2O{z|n8xp8rQ{7KOn1n(Mi{V}wJ$(@y(cE8SBPI9#y|wchyCWYGODV+oc3W$#ip1H zAwm;ri$Wr|N!iBn3xC$^x4i1wEP13)P;Gt3sD{@um0+x%#XmmLBo88cH_En7;iF~X zJ^F~^Xx;}D|94A&Q!{7d#Wb3U4AMR7ZA4^dELqnxi_|$0cfuMou?_(hGr9R1rq6f5 zb%o3j8K?z77TO7f(iAii$2Nt2ztQMUlyty?Qib43FHpykkNBXLbWuO4d*>R~`dxFM z>33OaDFaH>t*(d3Q8@I9gUO`N~Z28GU}>AM;;6%a_%LYjmY>IsT~iinfi6 zX-+>L*DP1I#J|fl;VhS^tuTuwZ9=R{FZ2#}>~+4gkqe}o%7$T+*WZ@hgo}bi4tvNi z5%_9$92G@l{vst%aI}gOY8S&o*cOLA#Ewdgv0k=J7}i(*Toc&I@!(|1Q`sc1` zUUsx!rh;DmD z{$-1mP)nusDxXoN1+rw^g5V%!tWOf9_Lax=jxmndMBw|dFi0`zf{;`ZgEb=5Q)M-^ zbE8tMd$E`7RRr!HDCGek)+EZ8JW%%k0=@Pt@Ph&TgYAOi?R-z|h}zqQ2p zOutxT40Pg!?48cZdZ)Atp|7DZm|=VV#N$(e*0Vx|Sl;#KSgO9oUp6c4<^UZjBh*&( z0L_WXRW#m?&^}V8d))I;tl2GO?jeS(>(G$2I(4czoG>Vt3`+f}I0{lC{6<4p&M5bEpW@#}WtacN!tixrXl`6AkIQy*ogqLN;{-{Js{b!tAsd_L)ba;nXRTKR*5 z8o4okZN^-)WSfM1&$gUP(H%gXN?&5hRm7ez-92QwA@az*ZzaSfnRxx4 zjT>hWSXExc;5+<*LcWoK~3Qdhc zi)}O+OES_c-&@I95Q+CY*uHV4Qc>4h+oQ?p(X&2@=)f#+ zTGgpZT*Ify>X%ApDyjxUttA6L@bR1)&FuEx?uU-!N{L|?Cu=X9L|jNoT_+8lEIRJa zaf<@G1dK*nsbN+LPK>{1{kEqs+QcV5H<^*DKzyVTHrZx+SdN3?Y@?!wPp1v}5}06B zYp!}XTFn!i68dCSPM_pQ-!1eIn*#8Za7Wd*=_!xF!5h&g;CG=>8^S|w6p6sEWjPO~ zF7B|_b@-X`klb$}BxK2hzH-zaB-sd^Q)DVTrjpW2&(Ce&Sbum~bzW!8lQc(bMDHb)7E`NCFDn1i4P)bmN`qg&r;&9Gl z<3@Y3nDn9NyGh0a1};%)k#Wca^<3OYC7SL$LR(qRE5^pjS2bPlc25NY@u!VLpcMd= zx)1iq?c97RrI=8)T0g`vz*#S3a|uoLzr`J|P$4|!bErU)nGwxWyy@bmReC(cYejSL zRiU3wgr0n7#*ik42gLxIy4f*KD)GqisVA#iwv8-aqJq2wE#cABy8NgaT31gbF)Y1% zhfwvqCfv)vF=B$h&;r$*kt1KGBYYSqN^T`*5L?v(HAD)RAlsQ!7rqHcMOZFX+da+3 z9Th1vCZ}6?ZB>+vt5~%BKG7zZBuFfyQ|zJ$&rXU;0=rFtpta1O$w5ut>$KN6`2Dy5 zn9z0c*niH%Ut2=N;mXk8oBw5vaUg&vk-dTdk6Eu(zc(E&yfV`N^o1!)JiK2j*wa>krJu6djPE+#ONLfii>jo-@_$wk=mY$wOl-BC~K zktI}T&9*)TOWJUXmIZw*7GWm8ht*u^n{#Bz8|^xY!FXw?JL@5;#W=4-;=5uu7{HGB zDcOx4&AnJ+-YTY@H(MQkLH%*|#tKJFJu%I3UT*Imchc^0w8# z3@z;GIo)u3e$i)cvgzhH>EIDKZJ(<0Vp0@uQ*<1C(q17v*+BlX8>L=LVHNp6pOoo+ zc^Mw8Xx(be^XcrT^=Z>1_7UZwLQXrQ5{dM;*VSz0bF#T#*y;{A!myiod^1^_CXGXP zD|=jYs@D+gHwWm&n!t1>1ZJwnC2Db%AD_}QX69e@R1@Ta zGxuKgPLY8(dbQ9ow(+GrhSy3_X#dv(oB$YEVA)_JC~6oLM*ymerg z(dv!Fh>G67W~4mt$71tUY>CZLs^kVX?)K5mqcZI)3N#*BadQUn+gH75stgWJFLv)^ z)-IE8NbZfcc5C(S!8MFr&p`0Lo{=)wkE)U0k}YFt7s|(Bf;F2!`l@+H)L^2%9B(v9 z_wW6|LW|{*FY)`r6>L$o=Vs;!t0Tn%h-pSK|i5 z^7SFMDJWJrJn;V2(BO)O{v8$F1N#`ohzQcn5LQ8BRsVVR+YRQ^iftrff-o*wVkQ-Klv>jGUECm^U7kBH(@xw@$o2k zggCWi%7EY%Nj&DhaQcZNb-LZ!RWC>T{YN#r!VF^8V18R)JPJqChlmy-!|hcbD#hAK zn({Mb{hFOv7SPHyCmSyco_*nqz_=(ul!;#X$%+Q#iF;|BF5bSMK7m0;>3mBEL%fXa zg{|f&nUvp#w@@gn;04R*clL(oFjL)6hqhv>9nm3ZhjoQ`Hz2}OyN-nJ6NZY_rUeq zG{?vOjmW`^JQDy9+1GE07|xDDk1$8%;kVeH_(tAG4ODuopr)s$S8-BM>#kUQ^{8sh zhgM?UAy&y=-Z3eq@0C-fL>U;o zPZv$c+m)<)@0B2*(C(&Pw{MXQpOTS;m0WI|i35?@ad3bD(`A~H?q0upp@QA2q!dsb zx2&xpO3_Pg?dMt6@G9p`m+KazQtIgw^1p{y+bdOR#f=jUU>=<;&vcZ!L7 z<1c1zD5pz+G}sL6vW*BSJGki@4c!HLuz~^FH)z#c$|!JIiJ_o@!zY8rMuHK&d;fR^ z|EaTHE61-^mtrK(2LkA<$Ik&g-&;eT@834qqY~Wow;r_8Vx16Oafr6N$jq~P=^VJ@ zI1qkN-P7gXQdf4veyz*1G%SB;7ft^{9}V>^Rgz}QXiV=i!Jj%;Z>;|HD!=@FofyZC z@$F0C~prRh%h5^emOV8W0=&^mEP6M(|aoJ{iFeUZk@9 zvVH5k$5B_6-KF|d(!;^#{EIH#iAGod53NqJhT}Qi0`v5MR|)sulU<{;JiivTXYFdR z6;+FfN2K-Zv8D=v7Fx%lt^o&6wXk=lMH9&6AU}kxd0zkZsQ}*HUvZY#o*zC-x>GMj z25g%gj~oM0SS$Ods@9pgs1k?`AaDZwSQqt2TMLELjzPLF! zIMhr`c&wn#ySCvLTA5&Y*3Y7Fb4tocNSJwF$pj06^GdAOzb}=OMQlNiO(wytOFL9- zqndpF1`c3P=QVE+Q&V5^V|QCi8V&@eeRMQrWJ8e0R~A;**Oq32mG-k3ZeHHIgF~BY zx(&9BfWA9?7Z*b!Zw?q{m*^2a!nQ!hxd96~T4cdTlNwi^&^T5h#xGC5A6DJ+SH;=} zqVP(!y`=iN*l^?ac9+f^?da+0e;Js+OEK!H{{6dYm;%eBhxKr84|Z=a?FO_)C4YTr znplL3`z$VJh}j4`$8@FbW5Z!5y0qvlrc(Lv)ug(-OaHK&RQXDywzM9-{VkLM4BDro!4BK_|qNKiiahtodlV3YUBO366l9i>E zo}J0*{p2aFpunIK!s_TWHEG=yjUsS^t_Hyjbs<4Kbff|nUOhQy$i=XHk)g&RxP<6XfYA~Qks z>?zf|pU|WM_#r;NG!l=>S+u-W?H^6U2Mn+VMxZ^L746M@1TrKz{FASz_Vx}<8OT6P zSg~r2?5RyoG+|}C(9r#xoqmrTR*~~cvO3zhQ@Y6~n;RpDxrvFcHK~hLPd|};mc^dQ z00=xCw4l7t7g@azKEc`9Bzaj>nwt0Wyb?YNNL=jwr-|Jl0CBjnpwz>e;ywWHa5%DL zGyAZVLQK1lSA}wUFuXc(e8fKu6XL1KS-u-7Cx~@!r^=Hw!vYd^*I+T530h4Ia@vy;I57ATRD`h|XuLOUQCMI_O)e?m(>GXewLxHu5!z zb*rE|>P!WwA*-n@%@7?Ci}Q}}YvVJy$Modnbl159XnD-&5o}d9V$>RE)7HaE*wP8G z+H)qj{Xo-!A=KFvqb?0J^6cJ#o-!-bXp~kT*PRTubykTKXBoKGmMeE*w6veCiAbgS z3XwMO@#o8YwkpcKL>;+weF^>OUYsJBTOpsqF|GryymR@EZH-QL12M(z?CidRek$so z1x-v$K=B1i5Dob=GE0*w9_%)et47A)s#@8A>^S^UqWH{Zi zkP3>fA1iIWA&_7LYShCGq3a3RMq8d_nV|ceiULV&GAN(#b6r3L_Zt5F6p7LKR3a|F z&+C&Z6d`M}I{O6V{)pXm8V3XWy|grwkwVWjZp1C3_gWN_G1QhJLT;bk?yYmLN2-H+ z)wDdk)=aG^u`JKd!|H}(M@WT4xmD?iKh0%dfB)GyX^UdEw3WszBTpeqbx;r#GSOQ1 zFhZ;3nn==y8FTNz=Dc%*c-tJk(e~}->JxlaCwB1n?~b6Rf&#Pb^K4`uQc_{H_|Ko& z9UhAs8u&aqXm_$_w5BH5urk=sMzZcG0!aW!uHNpjjgPcZ6BerYPk9X^X-a|u8+=jO+SC@7{ieltgY4Th49~$Z_ zqU}ZC)QZTLAoX0^#HOslk0(@Lg{xD8h!PZH0m0#L)3*&eb8f?t2!Vk2Fxv~Rs}b&v z#{xIOA7$Yohr>9`Bw~Kv`b|4ND~)@lwls|{9IDiCR&8q2s|tfF-tx}t9>Y+FEaY+) zkKIgPX_aG%E*%_#m-+Z;KXzg$$X{lWpq;9JkS=_F7c{N^u@m`%G?G(gk^XDDgoeMMi;xdFK^Lh4ikgxldLW?iitZygRVg3H$dCRvrNX zq@Vj}aEs5rPI08sh6wSPbju41DS;;}xAz0m#5j7HHX@|cC;;iK)uoDx3UWN%$l_x- z+x?S878dVFKlV$eeY+o)kp&9llq&lP^YS>RE)y}h zS*Tf@W#rntBy`bWwe*62+`9#fn#S|VvBy+;mh~i_YzXe2#e`NeYXT1N>+JdIs=QoO z6(~G++O9~tz4f7dKeO$`G9)YJ!DK`xVLQ!5B}YcewQZvGz4}`g0-S}IsYH#t;|~+3 z-{U+4Ud{#Thzdb!ZI^-pR!9(01gW9^PmBS+{{Cn|V$e_m*jU(I*R`goh~#0?($d}G zuvty<4j>TL-g^=M2OJcF-tImOWPU!j@Q zlnn)I=MDyOeyD$5*5#-hv5GsN4ncqmBmSP>5q0{rW=0*#hKZ0exOAtLZC+N9`r!DU zscTf<+OA^rtfzZ=y2rG-;&4JwSa4{%J8C1}csybkN(T0<^62F`F!X@M*w?Uc4BHF~ zYw0ZJsh&u;tDnGFI`Hv2aEfMx^?NuT&NTa1x@*Q)@|;g`1n;100hQ3=5fmjASw@Li zn#3wq2)M?CnJUh1T(m#n5%SQ1k;~C;$f-F`>L(30MnCBVp#w?cyg%W{iEld;blF9HG+#f zwHxm4JPK{15_xXPJ}+-Lsk}0*@RA^@>4k+G#~yKlG^Mt_q?4_#fT;U>L@_laG?`GI z=13vvG}JU#OWg}is8uX&qj9$r#)ppvgup~0<>Lsal$7Nfqi#-Ct#p=nilV$j`1xU5 z!Pu?&Q6ypLI0L*$S~48m5^L)O>~P85_rXD1A8EdY!NNvrA==EqZrg==*gHs_o}P9a zhcno<76e%ZqVQ+G>Hx;KG}G)nJn%&1%(&tQ?2!?@ey6Z#!L+6Y0d~gzU={wr8wLbw zQBY8bQ7R|O!dVb6Uz>%{r7~ z0T+VGIm$@c-G%z)Fvxo0#FF|2Ap^Ez9ClF(N_Jug060n6NZVT8^hXU+CnqP!gkSbn zhaiT5i#Tf!`bFrkux&*$Ch~4=9Wn(D0hc}i4+XVm)(oJq(%+;2mTg4VVH1i)#mDO zaI~QbdOSTjO+w<^*T(E}h-iW7Q=Hc9`K#ztWx*?F=?C{*sL!awNY2#>7r%X`O#H|p z@*$;lbzn>@Mb~G~qwTeg^#kU~G?_x3tK|uA;h`San9n(Pn6OOed$Aj9m|`NlLm@(% zy&7)y1u9#deL>V3WypQei98D%&`|9UqrqkRMMTBw`DNpym{~#GAwry-eH{~wweWBm zfe~5yX2c7tBAD?5yaJbEWQz+fey`V7Uq$eS@p7`meclJ2$;ir{pBK&OS#%<6FeK(F z5WsN>;{1>q%YYHvdd<$P+k?H`yfQXR)3LToRZmP(L<;PSXu`}}7F&a|LnW5;(ijNl z6v5}D^+DR9MUf1^ z7PGDa@x`G}%P-|k*sxN%rBF$_#YB$aaCB@Z36C9X?`di<+Sj|;)a41I;bBz1^#!R} zSgXsWH@6PYOJjlP{b-Co&MiBT?LvE$ihoN15@AsM$gl^)R@;N;+Oh7;aJ#R% zXjiBQ?UE)#meSrlRV{P4B@h-W`wOtJHu9VUDbi_mo+7PK zD4K?V7jk3aMZYeqrZ!JSf_2aPw>9dkXL0F1E8782ncC+c%LUaE*&7cvsgXGmOE)~O zlt{rXR2@XpDRMZ-ev>;L?6cQ6YHoz|sY&MBfh|leoK!q89v0urj~6e_;g~XyYZ}O3 zMmjitKXuz;nW=HR%B}=2(y9&Oab{e-y}~L!_Wp{j?sBcUE$)^SjETS5{zY=8Vo`BI z8fW}tXLNe3h_Yz)IWy4sg>`_UVOKRd6FjQ>J5c$YGMmoo@^wvBO%Vzj0QgI!=l&+r z1HY4zuzrhD_ggQ~TcrLhZ_WRd|4R1Eom?%|T%DXPUEORg!GFf=-yoNQcF^FPne3Z2 z$$!lM0_D9`{{=aC{=ZQ%KT{mj-Vn2I=bL`;f68B|{5LQEuPA9RH%mvbt&`)w-TPCF z$lYD0fME#$I7`C--v5K?2lKc5_;+Ufdz8L7L9=ae001S|Uz)~0Nq@Q4Z}T?{*v-_< z(hB6{`EOUiZ?2rUMDMV_8Ku8n82cZt)HwkFhLTQ>Z{u+LsO9Z!2?qWf@aI^nk_56T z-T?7$0OWrFa9sd^zf$~vQu^=Sd%YQo^9Gy7e}M;_rNaMrj{dU$==!(7fcZ^pk@>7R_hf>}h3@c*@D)f8dh UCIbLKdOIfHa$|x9;jQ(*05So(2LJ#7 literal 0 HcmV?d00001 diff --git a/toolbox/.setupForMatlabDebug.m.in b/toolbox/matlab/setupForMatlabDebug.m.in similarity index 100% rename from toolbox/.setupForMatlabDebug.m.in rename to toolbox/matlab/setupForMatlabDebug.m.in diff --git a/toolbox/matlab/slblocks.m b/toolbox/matlab/slblocks.m new file mode 100644 index 000000000..7c348193b --- /dev/null +++ b/toolbox/matlab/slblocks.m @@ -0,0 +1,8 @@ +function blkStruct = slblocks + +% Name of the .mdl file +Browser.Library = 'WBToolboxLibrary'; +Browser.Name = 'Whole Body Toolbox'; +Browser.IsFlat = 0; + +blkStruct.Browser = Browser; diff --git a/toolbox/.startup_wbitoolbox.m.in b/toolbox/matlab/startup_wbitoolbox.m.in similarity index 100% rename from toolbox/.startup_wbitoolbox.m.in rename to toolbox/matlab/startup_wbitoolbox.m.in diff --git a/toolbox/slblocks.m b/toolbox/slblocks.m deleted file mode 100644 index 89a28e382..000000000 --- a/toolbox/slblocks.m +++ /dev/null @@ -1,54 +0,0 @@ -function blkStruct = slblocks - -% Copyright (C) 2013 Robotics, Brain and Cognitive Sciences - Istituto Italiano di Tecnologia -% Author: Jorhabib Eljaik Gomez -% email: jorhabib.eljaik@iit.it -% The development of this software was supported by the FP7 EU project -% CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b)) -% http://www.codyco.eu -% Permission is granted to copy, distribute, and/or modify this program -% under the terms of the GNU General Public License, version 2 or any -% later version published by the Free Software Foundation. -% -% This program is distributed in the hope that it will be useful, but -% WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General -% Public License for more details -% Copyright (C) 2013 Robotics, Brain and Cognitive Sciences - Istituto Italiano di Tecnologia -% Author: Jorhabib Eljaik Gomez -% email: jorhabib.eljaik@iit.it -% -% The development of this software was supported by the FP7 EU project -% CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b)) -% http://www.codyco.eu -% -% Permission is granted to copy, distribute, and/or modify this program -% under the terms of the GNU General Public License, version 2 or any -% later version published by the Free Software Foundation. -% This program is distributed in the hope that it will be useful, but -% WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General -% Public License for more details - -Browser.Library = 'WBToolboxLibrary'; %Name of the .mdl file -Browser.Name = 'Whole Body Toolbox'; -Browser.IsFlat = 0; - -if (~verLessThan('matlab', '8.4')) % R2014b - % Add repository information if not yet done - % ???: is this part really needed? - try - load_system(Browser.Library); - if (strcmp(get_param(Browser.Library, 'EnableLBRepository'), 'off')) - set_param(Browser.Library, 'Lock', 'off'); - set_param(Browser.Library, 'EnableLBRepository', 'on'); - set_param(Browser.Library, 'Lock', 'on'); - save_system(Browser.Library); - end; - close_system(Browser.Library); - catch ex; - end -end; - - -blkStruct.Browser = Browser; From 9daf9de46ccddf9f5eccba0b42df85b470bc3481 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 1 Dec 2017 07:57:05 +0000 Subject: [PATCH 75/89] Explicitly set the velocity representation in iDynTree --- toolbox/src/base/RobotInterface.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp index 9755e4d7a..30662d2e4 100644 --- a/toolbox/src/base/RobotInterface.cpp +++ b/toolbox/src/base/RobotInterface.cpp @@ -410,6 +410,9 @@ bool RobotInterface::initializeModel() m_kinDynComp = std::make_shared(); if (!m_kinDynComp) return false; + // Explicitly set the velocity representation + m_kinDynComp->setFrameVelocityRepresentation(iDynTree::MIXED_REPRESENTATION); + // Use RF to load the urdf file // ---------------------------- From e6b13476cdacd487d888277aead6bc3813bc32b9 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 6 Dec 2017 12:05:34 +0000 Subject: [PATCH 76/89] Fixed the generalized torques conversion of InverseDynamics block --- toolbox/src/InverseDynamics.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/toolbox/src/InverseDynamics.cpp b/toolbox/src/InverseDynamics.cpp index 49b3e8386..d5d0ab1fa 100644 --- a/toolbox/src/InverseDynamics.cpp +++ b/toolbox/src/InverseDynamics.cpp @@ -7,6 +7,8 @@ #include #include #include +#include +#include using namespace wbt; @@ -188,9 +190,20 @@ bool InverseDynamics::output(const BlockInformation* blockInfo) iDynTree::LinkNetExternalWrenches(model->getNrOfLinks()), *m_torques); - // Forward the output to Simulink + // Get the output signal Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_TORQUES); - output.setBuffer(m_torques->jointTorques().data(), - blockInfo->getOutputPortWidth(OUTPUT_IDX_TORQUES)); + double* outputBuffer = output.getBuffer(); + if (!outputBuffer) { + Log::getSingleton().error("Failed to get output buffer."); + return false; + } + + // Convert generalized torques and forward the directly to Simulink + // mapping the memory through Eigen::Map + const auto& torquesSize = m_torques->jointTorques().size(); + Eigen::Map generalizedOutputTrqs(outputBuffer, torquesSize + 6); + generalizedOutputTrqs.segment(0, 6) = toEigen(m_torques->baseWrench()); + generalizedOutputTrqs.segment(6, torquesSize) = toEigen(m_torques->jointTorques()); + return true; } From 41b2ec64fd043c9b6eae2b48a7b3176dbea031bc Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 6 Dec 2017 15:46:13 +0000 Subject: [PATCH 77/89] Updated README.md --- README.md | 132 +++++++++++++++++++++++++++++++----------------------- 1 file changed, 76 insertions(+), 56 deletions(-) diff --git a/README.md b/README.md index 34b2187de..4478dee41 100644 --- a/README.md +++ b/README.md @@ -1,67 +1,76 @@ ![](http://drive.google.com/uc?export=view&id=0B6zDGh11iY6oc0gtM0lMdDNweWM) -Whole Body Toolbox - A Simulink Toolbox for Whole Body Control -------------------------------------------------------------- +# Whole Body Toolbox +

A Simulink Toolbox for Whole Body Control

[![GitPitch](https://gitpitch.com/assets/badge.svg)](https://gitpitch.com/robotology/WB-Toolbox?grs=github) -### This toolbox is intended as a replacement (a 2.0 version) of the [WBI-Toolbox](https://github.com/robotology-playground/WBI-Toolbox). If you are interested, see [the migration guide from WBI-Toolbox 1.0](doc/Migration.md). - ### Main Goal -> The library should allow non-programming experts or those researchers just getting acquainted with Whole Body Control to more easily deploy controllers either on simulation or a real YARP-based robotic platform, as well as to analyze their performance and take advantage of the innumerable MATLAB and Simulink toolboxes. We like to call it "rapid controller prototyping" after which a proper YARP module should be made for hard real time performance and final deployment. + +> The library should allow non-programming experts or those researchers just getting acquainted with Whole Body Control to more easily deploy controllers either on simulation or real YARP-based robotic platforms, and to analyze their performance taking advantage of the innumerable MATLAB and Simulink toolboxes. We like to call it "rapid controller prototyping" after which a proper YARP module should be made for hard real time performance and final deployment. The following video shows CoDyCo's latest results on iCub in which the top level controller has been implemented with the [WB-Toolbox](https://github.com/robotology/WB-Toolbox) running at a 10ms rate!

iCub balancing on one foot via external force control and interacting with humans

- - [Installation](https://github.com/robotology/WB-Toolbox#Installation) - [Troubleshooting](https://github.com/robotology/WB-Toolbox#Troubleshooting) -- [Using the Toolbox](https://github.com/robotology/WB-Toolbox#Using the Toolbox) +- [Using the Toolbox](https://github.com/robotology/WB-Toolbox#using-the-toolbox) +- [Migrating from WBI-Toolbox](https://github.com/robotology/WB-Toolbox#migrating-from-wbi-toolbox) +- [Migrating from WB-Toolbox 2.0](https://github.com/robotology/WB-Toolbox#migrating-from-wbi-toolbox-2.0) - [Modifying the Toolbox](https://github.com/robotology/WB-Toolbox#modifyng-the-toolbox) - [Citing this work](https://github.com/robotology/WB-Toolbox#citing-this-work) -### Installation +## Installation -#### Requirements -* Matlab V. 7.1+ and Simulink (Tested with Matlab R2015b, R2014a/b, R2013a/b, R2012a/b) -* YARP (https://github.com/robotology/yarp) **-IMPORTANT-** Please compile as shared library. Currently a default yarp configuration option. -* (Optional. Needed for some blocks) iCub (https://github.com/robotology/icub-main) -* yarpWholeBodyInterface (https://github.com/robotology/yarp-wholebodyinterface) +### Requirements +* Matlab V. 7.1+ and Simulink (Tested with Matlab `R2015b`, `R2014a/b`, `R2013a/b`, `R2012a/b`) +* [`YARP`](https://github.com/robotology/yarp) (Compiled as **shared library**. Currently a default yarp configuration option) +* [`iDynTree`](https://github.com/robotology/idyntree) +* Supported Operating Systems: **Linux**, **macOS**, **Windows** **Note:** You can install the dependencies either manually or by using the [codyco-superbuild](https://github.com/robotology/codyco-superbuild). -###### Optional +### Optional requirements +* [`iCub`](https://github.com/robotology/icub-main) (needed for some blocks) * Gazebo Simulator (http://gazebosim.org/) * gazebo_yarp_plugins (https://github.com/robotology/gazebo_yarp_plugins). -**Operating Systems supported: macOS, Linux, Windows.** - **Note: The following instructions are for *NIX systems, but it works similarly on the other operating systems.** -#### Compiling the C++ Code (Mex File) +### Compiling the C++ Code (Mex File) + +**Prerequisite: Check the [Matlab configuration](https://github.com/robotology/WB-Toolbox#matlab-configuration) section.** -**Prerequisite: Check the Matlab configuration.** Before going ahead with the compilation of the library, make sure that you have MATLAB and Simulink properly installed and running. Then, check that the MEX compiler for MATLAB is setup and working. For this you can try compiling some of the MATLAB C code examples as described in [http://www.mathworks.com/help/matlab/ref/mex.html#btz1tb5-12]. **If you installed Matlab in a location different from the default one, please set an environmental variable called either `MATLABDIR` or `MATLAB_DIR` with the root of your Matlab installation**, e.g. add a line to your `~/.bashrc` such as: `export MATLAB_DIR=/usr/local/bin/matlab` +Before going ahead with the compilation of the library, make sure that you have MATLAB and Simulink properly installed and running. Then, check that the MEX compiler for MATLAB is setup and working. For this you can try compiling some of the MATLAB C code examples as described in the [mex official documentation](https://www.mathworks.com/help/matlab/ref/mex.html). +**If you installed Matlab in a location different from the default one, please set an environmental variable called either `MATLABDIR` or `MATLAB_DIR` with the root of your Matlab installation**, e.g. add a line to your `~/.bashrc` such as: `export MATLAB_DIR=/usr/local/bin/matlab`. If you used the [codyco-superbuild](https://github.com/robotology/codyco-superbuild) you can skip this step and go directly to the Matlab configuration step. -Otherwise follow the following instructions. -- Clone this repository: `git clone https://github.com/robotology/WB-Toolbox.git` -- Create the build directory: `mkdir -p WB-Toolbox/build` -- Move inside the build directory: `cd WB-Toolbox/build` -- Configure the project: `cmake ..` +Execute: + +```sh +git clone https://github.com/robotology/WB-Toolbox.git +mkdir -p WB-Toolbox/build +cd WB-Toolbox/build +cmake .. +``` + +**Note:** We suggest to install the project instead of using it from the build directory. You should thus change the default installation directory by configuring the `CMAKE_INSTALL_PREFIX` variable. You can do it before running the first `cmake` command by calling `cmake .. -DCMAKE_INSTALL_PREFIX=/your/new/path`, or by configuring the project with `ccmake .` -**Note** We suggest to install the project instead of using it from the build directory. You should thus change the default installation directory by configuring the `CMAKE_INSTALL_PREFIX` variable. You can do it before running the first `cmake` command by calling `cmake .. -DCMAKE_INSTALL_PREFIX=/your/new/path`, or by configuring the project with `ccmake .` +Compile and install the toolbox as follows: -- Compile the project: `make` -- *[Optional/Suggested]* Install the project: `make install` +```sh +make +make install +``` +### Matlab configuration -#### Matlab configuration In the following section we assume the `install` folder is either the one you specified in the previous step by using `CMAKE_INSTALL_PREFIX` or if you used the `codyco-superbuild`, is `/path/to/superbuild/build_folder/install`. In order to use the WB-Toolbox in Matlab you have to add the `mex` and `share/WB-Toolbox` to the Matlab path. @@ -72,10 +81,10 @@ In order to use the WB-Toolbox in Matlab you have to add the `mex` and `share/WB ``` You can also use (only once after the installation) the script `startup_wbitoolbox.m` that you can find in the `share/WB-Toolbox` subdirectory of the install folder to properly configure Matlab. -**Note** This script configures Matlab to alwasy add the WB-Toolbox to the path. This assumes Matlab is always launched from the `userpath` folder. +**Note:** This script configures Matlab to always add the WB-Toolbox to the path. This assumes Matlab is always launched from the `userpath` folder. - **Launching Matlab** By default, Matlab has different startup behaviours depending on the Operating Systems and how it is launched. For Windows and macOS (if launched in the Finder) Matlab starts in the `userpath` folder. If this is your common workflow you can skip the rest of this note. Instead, if you launch Matlab from the command line (Linux and macOS users), Matlab starts in the folder where the command is typed, and thus the `path.m` file generated in the previous phase is no longer loaded. You thus have two options: - - create a Bash alias, such that Matlab is always launched in the `userpath`, e.g. + - create a Bash alias, such that Matlab is always launched in the `userpath`, e.g. ``` alias matlab='cd ~/Documents/MATLAB && /path/to/matlab ``` @@ -90,75 +99,86 @@ You can also use (only once after the installation) the script `startup_wbitoolb - **Robots' configuration files** Each robot that can be used through the Toolbox has its own configuration file. WB-Toolbox uses the Yarp [`ResourceFinder`](http://www.yarp.it/yarp_resource_finder_tutorials.html). You should thus follow the related instruction to properly configure your installation (e.g. set the `YARP_DATA_DIRS` variable) -### Troubleshooting -- **Problems finding libraries and libstdc++.** In case Matlab has trouble finding a specific library, a workaround is to launch it preloading the variable `LD_PRELOAD` (or `DYLD_INSERT_LIBRARIES` on macOS) with the full path of the missing library. On Linux you might also have trouble with libstdc++.so since Matlab comes with its own. To use your system's libstdc++ you would need to launch Matlab something like (replace with your system's libstdc++ library): +## Troubleshooting +- **Problems finding libraries and libstdc++.** In case Matlab has trouble finding a specific library, a workaround is to launch it preloading the variable `LD_PRELOAD` (or `DYLD_INSERT_LIBRARIES` on macOS) with the full path of the missing library. On Linux you might also have trouble with `libstdc++.so` since Matlab comes with its own. To use your system's `libstdc++` you would need to launch Matlab something like (replace with your system's `libstdc++` library): -`LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.19 matlab` +`LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 matlab` You could additionally create an alias to launch Matlab this way: -`alias matlab_codyco="cd ~/Documents/MATLAB && LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.19 matlab"` +`alias matlab_codyco="cd ~/Documents/MATLAB && LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 matlab"` -A more permanent and scalable solution can be found in the answer of [this issue](https://github.com/robotology/codyco-superbuild/issues/141#issuecomment-257892256) +A more permanent and scalable solution can be found in https://github.com/robotology/codyco-superbuild/issues/141#issuecomment-257892256. + +- In case you compiled `YARP` in a directory different from the system default one and you are not using RPATH, you need to tell to MATLAB the location in which to find the shared libraries for `YARP`. If you launch MATLAB from command line, this task is already done for you by `bash` (if you edited `.bashrc`). If you launch MATLAB from the UI (e.g. on macOS by double clicking the application icon) you need to further add the variables in `${MATLAB_ROOT}/bin/.matlab7rc.sh` by first doing -- In case you have compiled YARP in a directory different from the system default one and you are not using RPATH, you need to tell to MATLAB the location in which to find the shared libraries for YARP. If you launch MATLAB from command line, this task is already done for you by `bash` (if you edited `.bashrc`). If you launch MATLAB from the UI (e.g. on macOS by double clicking the application icon) you need to further add the variables in `${MATLAB_ROOT}/bin/.matlab7rc.sh` by first doing ```bash chmod +w .matlab7rc.sh ``` + Then looking for the variable `LDPATH_SUFFIX` and assign to every instance the contents of your `DYLD_LIBRARY_PATH`. Finally do: + ```bash chmod -w .matlab7rc.sh ``` The error message you get in this case might look something like: + ```bash Library not loaded: libyarpwholeBodyinterface.0.0.1.dylib Referenced from: ${CODYCO_SUPERBUILD_DIR}/install/mex/robotState.mexmaci64 ``` -### Using the Toolbox +## Using the Toolbox + The Toolbox assumes the following information / variables to be defined: -##### Environment variables + +#### Environment variables + If you launch Matlab from command line, it inherits the configuration from the `.bashrc` or `.bash_profile` file. If you launch Matlab directly from the GUI you should define this variables with the Matlab function `setenv`. - `YARP_ROBOT_NAME` - `YARP_DATA_DIRS` -##### Matlab variables - -- `WBT_robotName`: if not defined the robot name defined in the `yarpWholeBodyInterface.ini` file is taken. -- `WBT_modelName`: if not defined by default is `WBT_simulink`. -- `WBT_wbiFilename`: the name of the Yarp WholeBodyInterface file, by default `yarpWholeBodyInterface.ini`. -- `WBT_wbiList`: the name of the robot list to be used in the Toolbox. By default `ROBOT_TORQUE_CONTROL_JOINTS`. +#### Creating a model -**Creating a model** Before using or creating a new model keep in mind that WB-Toolbox is discrete in principle and your simulation should be discrete as well. By going to `Simulation > Configuration Parameters > Solver` you should change the solver options to `Fixed Step` and use a `discrete (no continuous states)` solver. +r +In order to start dragging and dropping blocks from the Toolbox, open the Simulink Library Browser and search for `Whole Body Toolbox` in the tree view. + +## Migrating -To start dragging and dropping blocks from the Toolbox open Simulink and search for `Whole Body Toolbox` in the libraries tree. +#### From WBI-Toolbox (1.0) -### Migrating from WBI-Toolbox -Please see [here](doc/Migration.md). +Please see [here](doc/Migration_from_WBI-Toolbox_1.0.md). + +#### Migrating from WB-Toolbox 2.0 + +Please see [here](doc/Migration_from_WB-Toolbox_2.0.md). + +## Modifying the Toolbox -### Modifying the Toolbox If you want to modify the toolbox, please check developers documentation: - [Add a new C++ block](doc/dev/create_new_block.md) - [Tip and tricks on creating simulink blocks](doc/dev/sim_tricks.md) -### Citing this work +## Citing this work + Please cite the following publication if you are using WB-Toolbox for your own research and/or robot controllers > Romano, F., Traversaro, S., Pucci, D., Del Prete, A., Eljaik, J., Nori, F. "A Whole-Body Software Abstraction layer for Control Design of free-floating Mechanical Systems", 1st IEEE International Conference on Robotic Computing, 2017. Bibtex citation: ``` -@INPROCEEDINGS{RomanoWBI17, -author={F. Romano and S. Traversaro and D. Pucci and A. Del Prete and J. Eljaik and F. Nori}, -booktitle={2017 IEEE 1st International Conference on Robotic Computing}, -title={A Whole-Body Software Abstraction layer for Control Design of free-floating Mechanical Systems}, -year={2017}, +@INPROCEEDINGS{RomanoWBI17, +author={F. Romano and S. Traversaro and D. Pucci and A. Del Prete and J. Eljaik and F. Nori}, +booktitle={2017 IEEE 1st International Conference on Robotic Computing}, +title={A Whole-Body Software Abstraction layer for Control Design of free-floating Mechanical Systems}, +year={2017}, } ``` #### Tested OS -macOS, Linux, Windows + +Linux, macOS, Windows. From 022f464dd36fe1099624251e0c7f416876a31c15 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Wed, 6 Dec 2017 15:47:13 +0000 Subject: [PATCH 78/89] Added Migration How-To from 2.0 to 3.0 Also renamed Migration.md --- doc/Migration_from_WB-Toolbox_2.0.md | 82 +++++++++++++++++++ ...n.md => Migration_from_WBI-Toolbox_1.0.md} | 6 +- 2 files changed, 85 insertions(+), 3 deletions(-) create mode 100644 doc/Migration_from_WB-Toolbox_2.0.md rename doc/{Migration.md => Migration_from_WBI-Toolbox_1.0.md} (96%) diff --git a/doc/Migration_from_WB-Toolbox_2.0.md b/doc/Migration_from_WB-Toolbox_2.0.md new file mode 100644 index 000000000..b194665b9 --- /dev/null +++ b/doc/Migration_from_WB-Toolbox_2.0.md @@ -0,0 +1,82 @@ +# Migration from WB-Toolbox 2.0 to WB-Toolbox 3.0 + +Most of the major changes delivered with the `3.0` version of the `WB-Toolbox` don't affect directly the end-user. Under the hood the toolbox had an important polishing, and the small manual intervention required by this new release match the new features which have been developed. + +You can read [Release Notes](https://github.com/robotology/WB-Toolbox/issues/65) for a detailed overview. Below are described only the steps required to port Simulink models to this new release. + +## New toolbox configuration + +The `WB-Toolbox 2.0` was based on top of [`yarpWholeBodyInterface`](https://github.com/robotology/yarp-wholebodyinterface), which configuration was stored in a `yarpWholeBodyInterface.ini` file. This file was retrieved by `ResourceFinder` and its information was then loaded into the toolbox. + +### Store the configuration in the Simulink model + +`WB-Toolbox 3.0` deprecated the support of `yarpWholeBodyInterface`, and for reducing the complexity and sparsity of the information storage it allows configuring a Simulink model from the model itself. + +The new _Configuration_ block allows inserting information such as **Robot Name**, **URDF Name**, **Controlled Joints**, ... directly from the block's mask. + +### Load the configuration from the Workspace + +Sometimes it might be useful loading the model's configuration directly from the Workspace. For this purpose, a new `WBToolbox.WBToolboxConfig` class has been developed. The _Configuration_ block needs to know only the name of the variable which refers to the object. Its data is then read before the simulation runs. + +This snippet of code shows an example of how to initialize a configuration object: + +```matlab +# Initialize a config object +WBTConfigRobot = WBToolbox.WBToolboxConfig; + +# Insert robot data +WBTConfigRobot.RobotName = 'icubSim'; +WBTConfigRobot.UrdfFile = 'model.urdf'; +WBTConfigRobot.ControlledJoints = {... + 'torso_pitch','torso_roll','torso_yaw',... + 'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow',... + 'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'}; +WBTConfigRobot.ControlBoardsNames = {'torso','left_arm','right_arm'}; +WBTConfigRobot.LocalName = 'WBT'; +``` + +To check if the data has been read correctly, it is displayed as read-only in the block's mask. + +Furthermore, a good sign for a valid configuration is the `WBTConfigRobot.ValidConfiguration` property. + +### Multi-robot support + +The scope of the introduction of the _Configuration_ block goes beyond the need of a simpler toolbox configuration. One of the biggest limitation of the `2.0` version is the support of [controlling only one robot per model](https://github.com/robotology/WB-Toolbox/issues/37). + +`WB-Toolbox 3.0` is now capable of reading / sending data from / to multiple robots. Multiple _Configuration_ blocks can be present in the same model attaining to the following rules: + +* In the same hierarchical level of a Simulink model, only one _Configuration_ + block should be present. In other words, you should never see in the display more than one _Configuration_ block. + * _Configuration_ blocks put deeper in the hierarchy (e.g. in a Subsystem) override the previous ones. + + There are a few pitfalls which are worth to be highlighted: + + * It is legit having two Subsystems with different _Configuration_ blocks which point to the same robot. They can have for instance a different joint list and use different control boards. Although, despite reading information never creates problems, sending data to the robot in such scenario can be disastrous. In fact, consider the case these two subsystems share one link, and configure it in two different control modes (e.g. Position and Torque). Sending references to this link causes unpredictable effects. + * In line of theory it would be possible to have two subsystems in which the first one refers to a Gazebo model and the second one to a real robot. However, this case causes unpredictable behaviour for what concerns the synchronization. In fact, two different blocks for such aim are present in the toolbox: _Simulator Synchronizer_ and _Real Time Syncronizer_. They should be always used exclusively. + +## Other manual edits + +* All the _Get Estimate_ blocks need to be replaced by the new _Get Measurement_ block. +* All the hardcoded digital filters (e.g. for the joints velocities) have been removed. A new `Discrete Filter` block has been developed, and it should be manually added if the read raw signal (e.g. from the _Get Measurement_ block) requires filtering. +* The `C++` class used by the _DoFs Converter_ changed. All the blocks in the `YARP To WBI` configuration need to be connected again. +* The gravity vector is stored is the `WBToolboxConfig` class. If an alternative value is needed, set it globally directly in the configuration object or scope the block which needs it in a Subsystem with its own _Configuration_ block. +* In order to set the low level PIDs, loading in the Workspace a `WBToolbox.WBTPIDConfig` object should be configured as follows: + +```matlab +# Initialize an empty object +pids = WBToolbox.WBTPIDConfig; + +# Insert data +pids.addPID(WBToolbox.PID('l_elbow', WBToolbox.PID(1, 1, 0))); +pids.addPID(WBToolbox.PID('l_wrist_pitch', WBToolbox.PID(1.5, 0, 0.1))); +pids.addPID(WBToolbox.PID('r_shoulder_pitch', WBToolbox.PID(0.2, 0, 0))); +pids.addPID(WBToolbox.PID('torso_roll', WBToolbox.PID(0.1, 0.1, 0))); +``` + +If some of the controlled joints are not specified, the PIDs are kept in their default values. + + +## Deprecations + +* _Inverse Kinematics_ and _Remote Inverse Kinematics_ have been temporary deprecated. They will see a major release in the coming months. If you need them please do not upgrade to the `3.0` version. +* _Set Low Level PID_ block lost the capability of switching between multiple configurations. Since they were stored in an external file, this change is aligned to the simplification process chosen for for the configuration. \ No newline at end of file diff --git a/doc/Migration.md b/doc/Migration_from_WBI-Toolbox_1.0.md similarity index 96% rename from doc/Migration.md rename to doc/Migration_from_WBI-Toolbox_1.0.md index b73a4590b..a89c8c6b7 100644 --- a/doc/Migration.md +++ b/doc/Migration_from_WBI-Toolbox_1.0.md @@ -1,4 +1,4 @@ -## Migration from WB**I**-Toolbox to WB-Toolbox 2.0 +## Migration from WBI-Toolbox to WB-Toolbox 2.0 Given a simulink model with some WBI-Toolbox blocks inside, the general procedure is to **substitute each block with the corresponding one from WB-Toolbox 2.0**. However, there are some things the user should take care while doing this operation. This guide points out the main differences between the two toolboxes. For more information about the WBI-Toolbox, please have a look at the [WBI-Toolbox README](https://github.com/robotology-playground/WBI-Toolbox/blob/master/README.md). This guide follows the WBI and WB Toolbox blocks partitioning in Simulink library. It is divided in the following sections: @@ -24,7 +24,7 @@ They have already meaningful default values. Nevertheless you should take a look The world-to-base homogeneous transformation matrix is a 4x4 matrix that maps position and orientation of a rigid body from an initial frame of reference to another. -For back-compatibility, the transformation happending under the hood in the WBI-Toolbox can be obtained using forward kinematics blocks as in the following example: +For back-compatibility, the transformation happending under the hood in the WBI-Toolbox can be obtained using forward kinematics blocks as in the following example: ![](https://cloud.githubusercontent.com/assets/12396934/12293044/3337da3c-b9f1-11e5-959f-b40418b5469d.png) @@ -43,7 +43,7 @@ It is divided into three subsections. The `Joint Limits` block is now moved into #### Dynamics - the `dJdq` blocks have been moved into **jacobians** subsection; -- for mass matrix, generalized bias forces and centroidal momentum computation is now required to calculate explicitly the world-to-base homogeneous transformation matrix and the base velocity. Furthermore, the base frame pose and velocity and the joint configuration are now separate inputs. +- for mass matrix, generalized bias forces and centroidal momentum computation is now required to calculate explicitly the world-to-base homogeneous transformation matrix and the base velocity. Furthermore, the base frame pose and velocity and the joint configuration are now separate inputs. #### Jacobians There is now only one generic block for jacobians and one for `dJdq` calculation. The link with respect to which the Jacobian is computed is determined by its frame name as specified in the **URDF model**. As for the dynamics, the base pose and velocity and the joint position and velocity are required as input. From 3fea0700a3a4b09aa9ce7ecb9ef61fb8313fb6dc Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 11 Dec 2017 12:01:48 +0000 Subject: [PATCH 79/89] Clear the errors from logs once they are forwarded to Simulink --- toolbox/src/base/toolbox.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/toolbox/src/base/toolbox.cpp b/toolbox/src/base/toolbox.cpp index 74468baa9..2af82a38b 100644 --- a/toolbox/src/base/toolbox.cpp +++ b/toolbox/src/base/toolbox.cpp @@ -90,6 +90,7 @@ static void catchLogMessages(bool status, SimStruct* S, std::string prefix) // Forward to Simulink sprintf(errorBuffer, "%s", errorMsg.c_str()); ssSetErrorStatus(S, errorBuffer); + log.clearErrors(); return; } } From b0cdd644e5cdbdb8502807519fc586735465a4a0 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Mon, 11 Dec 2017 13:54:38 +0000 Subject: [PATCH 80/89] Fixed tuple initialization for C++11 --- toolbox/src/SetLowLevelPID.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/toolbox/src/SetLowLevelPID.cpp b/toolbox/src/SetLowLevelPID.cpp index 911b039d8..ad5998e53 100644 --- a/toolbox/src/SetLowLevelPID.cpp +++ b/toolbox/src/SetLowLevelPID.cpp @@ -120,7 +120,8 @@ bool SetLowLevelPID::readWBTPidConfigObject(const BlockInformation* blockInfo) std::end(controlledJoints), jointNamesFromParameters[i]); if (findElement != std::end(controlledJoints)) { - m_pidJointsFromParameters[jointNamesFromParameters[i]] = {Pvector[i], Ivector[i], Dvector[i]}; + m_pidJointsFromParameters[jointNamesFromParameters[i]] = + std::tuple(Pvector[i], Ivector[i], Dvector[i]); } else { Log::getSingleton().warning("Attempted to set PID of joint " + jointNamesFromParameters[i]); From f658be29031d9689b22ba1f92d42a363f708f28e Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 12 Dec 2017 08:50:06 +0000 Subject: [PATCH 81/89] Initial support of DiscreteFilter Block --- toolbox/CMakeLists.txt | 6 ++++++ toolbox/include/base/toolbox.h | 1 + toolbox/src/base/factory.cpp | 2 ++ 3 files changed, 9 insertions(+) diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 62cf9590d..1059ffe59 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -82,6 +82,12 @@ if (WBT_USES_ICUB) SOURCES src/MinimumJerkTrajectoryGenerator.cpp HEADERS include/MinimumJerkTrajectoryGenerator.h) + configure_block(BLOCK_NAME "Discrete Filter" + GROUP "Utilities" + LIST_PREFIX WBT + SOURCES src/DiscreteFilter.cpp + HEADERS include/DiscreteFilter.h) + # if (${ICUB_USE_IPOPT}) # find_package(iDynTree REQUIRED) # add_definitions(-DWBT_USES_IPOPT) diff --git a/toolbox/include/base/toolbox.h b/toolbox/include/base/toolbox.h index 14f5a3778..979eb6b93 100644 --- a/toolbox/include/base/toolbox.h +++ b/toolbox/include/base/toolbox.h @@ -19,6 +19,7 @@ #include "SetLowLevelPID.h" #ifdef WBT_USES_ICUB #include "MinimumJerkTrajectoryGenerator.h" +#include "DiscreteFilter.h" #endif #ifdef WBT_USES_IPOPT // #include "InverseKinematics.h" diff --git a/toolbox/src/base/factory.cpp b/toolbox/src/base/factory.cpp index ab7d9f9be..e6caf80b0 100644 --- a/toolbox/src/base/factory.cpp +++ b/toolbox/src/base/factory.cpp @@ -42,6 +42,8 @@ Block* Block::instantiateBlockWithClassName(std::string blockClassName) #ifdef WBT_USES_ICUB else if (blockClassName == MinimumJerkTrajectoryGenerator::ClassName) { block = new MinimumJerkTrajectoryGenerator(); + } else if (blockClassName == DiscreteFilter::ClassName) { + block = new wbt::DiscreteFilter(); } #endif #ifdef WBT_USES_IPOPT From 2a8397e5eff7bd538c63bf1d31e952e3e91dcfb8 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 10 Aug 2017 09:16:25 +0000 Subject: [PATCH 82/89] First working version of DiscreteFilter block with 1D I/O signal --- toolbox/include/DiscreteFilter.h | 44 ++++++ toolbox/src/DiscreteFilter.cpp | 230 +++++++++++++++++++++++++++++++ 2 files changed, 274 insertions(+) create mode 100644 toolbox/include/DiscreteFilter.h create mode 100644 toolbox/src/DiscreteFilter.cpp diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h new file mode 100644 index 000000000..be47c7944 --- /dev/null +++ b/toolbox/include/DiscreteFilter.h @@ -0,0 +1,44 @@ +#include "Block.h" +#include + +#ifndef WBT_FILTER_H +#define WBT_FILTER_H + +namespace wbt { + class DiscreteFilter; +} // namespace wbt + +namespace iCub { + namespace ctrl { + class IFilter; + } +} // namespace iCub + +namespace yarp { + namespace sig { + class Vector; + } +} // namespace yarp + +class wbt::DiscreteFilter : public wbt::Block { +private: + iCub::ctrl::IFilter* filter; + yarp::sig::Vector* num; + yarp::sig::Vector* den; + + static void stringToYarpVector(const std::string s, yarp::sig::Vector* v); + +public: + static std::string ClassName; + + DiscreteFilter(); + ~DiscreteFilter() = default; + + virtual unsigned numberOfParameters(); + virtual bool configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error); + virtual bool initialize(BlockInformation* blockInfo, wbt::Error* error); + virtual bool terminate(BlockInformation* blockInfo, wbt::Error* error); + virtual bool output(BlockInformation* blockInfo, wbt::Error* error); +}; + +#endif // WBT_FILTER_H diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp new file mode 100644 index 000000000..9045446e5 --- /dev/null +++ b/toolbox/src/DiscreteFilter.cpp @@ -0,0 +1,230 @@ +#include "DiscreteFilter.h" +#include "BlockInformation.h" +#include "Error.h" +#include "ForwardKinematics.h" +#include "Signal.h" +#include +#include +#include +#include +#include +#include + +// Parameters +#define PARAM_IDX_FLT_TYPE 1 // ::Filter type +#define PARAM_IDX_NUMCOEFF 2 // ::Filter numerator coefficients +#define PARAM_IDX_DENCOEFF 3 // ::Filter denominator coefficients +#define PARAM_IDX_1LOWP_FC 4 // ::FirstOrderLowPassFilter cut frequency +#define PARAM_IDX_1LOWP_TS 5 // ::FirstOrderLowPassFilter sampling time +#define PARAM_IDX_MD_ORDER 6 // ::MedianFilter order +// Inputs +#define INPUT_IDX_SIGNAL 0 +// Outputs +#define OUTPUT_IDX_SIGNAL 0 +// Other defines +#define SIGNAL_DYNAMIC_SIZE -1 + +using namespace wbt; + +std::string DiscreteFilter::ClassName = "DiscreteFilter"; + +DiscreteFilter::DiscreteFilter() : filter(nullptr), num(nullptr), den(nullptr) +{ + num = new yarp::sig::Vector(); + den = new yarp::sig::Vector(); +} + +unsigned DiscreteFilter::numberOfParameters() +{ + return 6; +} + +bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error) +{ + // Memory allocation / Saving data not allowed here + + // Specify I/O + // =========== + + // INPUTS + // ------ + + // Number of input ports + int numberOfInputPorts = 1; + if (!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { + if (error) { + error->message = ClassName + " Failed to set input port number."; + } + return false; + } + + // Input port sizes + blockInfo->setInputPortVectorSize(INPUT_IDX_SIGNAL, SIGNAL_DYNAMIC_SIZE); + blockInfo->setInputPortType(INPUT_IDX_SIGNAL, PortDataTypeDouble); + + // OUTPUTS + // ------- + + // Number of output ports + int numberOfOutputPorts = 1; + if (!blockInfo->setNumberOfOuputPorts(numberOfOutputPorts)) { + if (error) { + error->message = ClassName + " Failed to set output port number."; + } + return false; + } + + // Output port sizes + blockInfo->setOutputPortVectorSize(OUTPUT_IDX_SIGNAL, SIGNAL_DYNAMIC_SIZE); + blockInfo->setOutputPortType(OUTPUT_IDX_SIGNAL, PortDataTypeDouble); + + return true; +} + +bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) +{ + // Handle the parameters + // ===================== + + // Variables for the filter parameters + std::string filter_type; + std::string num_coeff_str; + std::string den_coeff_str; + wbt::Data firstOrderLowPassFilter_fc; + wbt::Data firstOrderLowPassFilter_ts; + wbt::Data medianFilter_order; + + // Get the scalar parameters + firstOrderLowPassFilter_fc = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_FC); + firstOrderLowPassFilter_ts = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_TS); + medianFilter_order = blockInfo->getScalarParameterAtIndex(PARAM_IDX_MD_ORDER); + + // Get the string parameter + if (!(blockInfo->getStringParameterAtIndex(PARAM_IDX_FLT_TYPE, filter_type) + && blockInfo->getStringParameterAtIndex(PARAM_IDX_NUMCOEFF, num_coeff_str) + && blockInfo->getStringParameterAtIndex(PARAM_IDX_DENCOEFF, den_coeff_str))) { + if (error) { + error->message = ClassName + " Failed to parse string parameters."; + } + return false; + } + + // Convert the strings from the Matlab syntax ("[1.0 2 3.33]") to yarp::sig::Vector + stringToYarpVector(num_coeff_str, num); + stringToYarpVector(den_coeff_str, den); + + // Create the filter object + // ======================== + + // Default + // ------- + if (filter_type == "Default") { + if (num->length() == 0 || den->length() == 0) { + if (error) { + error->message = ClassName + " (Default) Wrong coefficients size"; + } + return 1; + } + filter = new iCub::ctrl::Filter(*num, *den); + } + // FirstOrderLowPassFilter + // ----------------------- + else if (filter_type == "FirstOrderLowPassFilter") { + if (firstOrderLowPassFilter_fc.floatData() == 0 + || firstOrderLowPassFilter_ts.floatData() == 0) { + if (error) { + error->message = ClassName + + " (FirstOrderLowPassFilter) You need to " + "specify Fc and Ts"; + } + return false; + } + filter = new iCub::ctrl::FirstOrderLowPassFilter(firstOrderLowPassFilter_fc.floatData(), + firstOrderLowPassFilter_ts.floatData()); + } + // MedianFilter + // ------------ + else if (filter_type == "MedianFilter") { + if (medianFilter_order.int32Data() == 0) { + if (error) { + error->message = ClassName + + " (MedianFilter) You need to specify the " + "filter order."; + } + return false; + } + filter = new iCub::ctrl::MedianFilter(medianFilter_order.int32Data()); + } + else { + if (error) error->message = ClassName + " Filter type not recognized."; + return false; + } + + return true; +} + +bool DiscreteFilter::terminate(BlockInformation* blockInfo, wbt::Error* error) +{ + if (filter) { + delete filter; + filter = nullptr; + } + + if (num) { + delete num; + num = nullptr; + } + + if (den) { + delete den; + den = nullptr; + } + + return true; +} + +bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error) +{ + if (filter == nullptr) return false; + + // Get the input signal + Signal inputSignal = blockInfo->getInputPortSignal(INPUT_IDX_SIGNAL); + + unsigned index_element = 0; + yarp::sig::Vector inputSignalVector(1, inputSignal.get(index_element).doubleData()); + + // Filter the input signal + const yarp::sig::Vector& outputVector = filter->filt(inputSignalVector); + + // Forward the filtered signal to the output port + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_SIGNAL); + output.setBuffer(outputVector.data(), outputVector.length()); + + return true; +} + +void DiscreteFilter::stringToYarpVector(const std::string str, yarp::sig::Vector* v) +{ + assert(v != nullptr); + + std::string s = str; + + // Lambda expression used to remove "[]," carachters + // TODO: what about removing everything but digits and "."? + auto lambda_remove_chars = [](const char& c) { + if ((c == '[') || (c == ']') || c == ',') + return true; + else + return false; + }; + + // Apply the lambda expression the input parameters + s.erase(remove_if(s.begin(), s.end(), lambda_remove_chars), s.end()); + + // Convert the cleaned string to a yarp vector of floats + std::istringstream sstrm(s); + float f; + + while (sstrm >> f) + v->push_back(f); +} From 4d6317bfdcdf3c25d3beb529b3bd9a4cadc3a810 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 10 Aug 2017 12:57:46 +0000 Subject: [PATCH 83/89] DiscreteFilter: Multidimesional signal support --- toolbox/include/DiscreteFilter.h | 3 ++ toolbox/src/DiscreteFilter.cpp | 66 ++++++++++++++++++++++---------- 2 files changed, 49 insertions(+), 20 deletions(-) diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h index be47c7944..234af3da9 100644 --- a/toolbox/include/DiscreteFilter.h +++ b/toolbox/include/DiscreteFilter.h @@ -1,5 +1,6 @@ #include "Block.h" #include +#include #ifndef WBT_FILTER_H #define WBT_FILTER_H @@ -22,9 +23,11 @@ namespace yarp { class wbt::DiscreteFilter : public wbt::Block { private: + bool firstRun; iCub::ctrl::IFilter* filter; yarp::sig::Vector* num; yarp::sig::Vector* den; + yarp::sig::Vector* inputSignalVector; static void stringToYarpVector(const std::string s, yarp::sig::Vector* v); diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp index 9045446e5..fb1b1874a 100644 --- a/toolbox/src/DiscreteFilter.cpp +++ b/toolbox/src/DiscreteFilter.cpp @@ -25,13 +25,15 @@ #define SIGNAL_DYNAMIC_SIZE -1 using namespace wbt; +using namespace iCub::ctrl; +using namespace yarp::sig; std::string DiscreteFilter::ClassName = "DiscreteFilter"; -DiscreteFilter::DiscreteFilter() : filter(nullptr), num(nullptr), den(nullptr) +DiscreteFilter::DiscreteFilter() : firstRun(true), filter(nullptr), inputSignalVector(nullptr) { - num = new yarp::sig::Vector(); - den = new yarp::sig::Vector(); + num = new Vector(0); + den = new Vector(0); } unsigned DiscreteFilter::numberOfParameters() @@ -116,16 +118,16 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) // Create the filter object // ======================== - // Default + // Generic // ------- - if (filter_type == "Default") { + if (filter_type == "Generic") { if (num->length() == 0 || den->length() == 0) { if (error) { - error->message = ClassName + " (Default) Wrong coefficients size"; + error->message = ClassName + " (Generic) Wrong coefficients size"; } return 1; } - filter = new iCub::ctrl::Filter(*num, *den); + filter = new Filter(*num, *den); } // FirstOrderLowPassFilter // ----------------------- @@ -139,8 +141,8 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) } return false; } - filter = new iCub::ctrl::FirstOrderLowPassFilter(firstOrderLowPassFilter_fc.floatData(), - firstOrderLowPassFilter_ts.floatData()); + filter = new FirstOrderLowPassFilter(firstOrderLowPassFilter_fc.floatData(), + firstOrderLowPassFilter_ts.floatData()); } // MedianFilter // ------------ @@ -153,7 +155,7 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) } return false; } - filter = new iCub::ctrl::MedianFilter(medianFilter_order.int32Data()); + filter = new MedianFilter(medianFilter_order.int32Data()); } else { if (error) error->message = ClassName + " Filter type not recognized."; @@ -165,6 +167,9 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) bool DiscreteFilter::terminate(BlockInformation* blockInfo, wbt::Error* error) { + // Deallocate all the memory + // ------------------------- + if (filter) { delete filter; filter = nullptr; @@ -180,6 +185,11 @@ bool DiscreteFilter::terminate(BlockInformation* blockInfo, wbt::Error* error) den = nullptr; } + if (inputSignalVector) { + delete inputSignalVector; + inputSignalVector = nullptr; + } + return true; } @@ -187,23 +197,39 @@ bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error) { if (filter == nullptr) return false; - // Get the input signal - Signal inputSignal = blockInfo->getInputPortSignal(INPUT_IDX_SIGNAL); + // Get the input and output signals + Signal inputSignal = blockInfo->getInputPortSignal(INPUT_IDX_SIGNAL); + Signal outputSignal = blockInfo->getOutputPortSignal(OUTPUT_IDX_SIGNAL); + + unsigned inputSignalWidth = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL); + + // Allocate the memory for the input data + if (firstRun) { + // Allocate the input signal + inputSignalVector = new Vector(inputSignalWidth, 0.0); - unsigned index_element = 0; - yarp::sig::Vector inputSignalVector(1, inputSignal.get(index_element).doubleData()); + // Initialize the filter. This is required because if the signal is not 1D, + // the default filter constructor initialize a wrongly sized y0 + filter->init(Vector(inputSignalWidth)); + + firstRun = false; + } + + // Copy the Signal to the data structure that the filter wants + for (unsigned i = 0; i < inputSignalWidth; ++i) { + (*inputSignalVector)[i] = inputSignal.get(i).doubleData(); + } - // Filter the input signal - const yarp::sig::Vector& outputVector = filter->filt(inputSignalVector); + // Filter the current component of the input signal + const Vector& outputVector = filter->filt(*inputSignalVector); - // Forward the filtered signal to the output port - Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_SIGNAL); - output.setBuffer(outputVector.data(), outputVector.length()); + // Forward the filtered signals to the output port + outputSignal.setBuffer(outputVector.data(), outputVector.length()); return true; } -void DiscreteFilter::stringToYarpVector(const std::string str, yarp::sig::Vector* v) +void DiscreteFilter::stringToYarpVector(const std::string str, Vector* v) { assert(v != nullptr); From 1ff1fb0e78909b12f7dfc98076c71926bdeff65e Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 11 Aug 2017 11:54:49 +0000 Subject: [PATCH 84/89] DiscreteFilter: Included parameters for initializing input / output Kept as private variables only what is strictly required --- toolbox/include/DiscreteFilter.h | 9 +-- toolbox/src/DiscreteFilter.cpp | 114 ++++++++++++++++++++++--------- 2 files changed, 85 insertions(+), 38 deletions(-) diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h index 234af3da9..098ca3966 100644 --- a/toolbox/include/DiscreteFilter.h +++ b/toolbox/include/DiscreteFilter.h @@ -23,13 +23,13 @@ namespace yarp { class wbt::DiscreteFilter : public wbt::Block { private: - bool firstRun; + unsigned inputSignalWidth; iCub::ctrl::IFilter* filter; - yarp::sig::Vector* num; - yarp::sig::Vector* den; + yarp::sig::Vector* y0; + yarp::sig::Vector* u0; yarp::sig::Vector* inputSignalVector; - static void stringToYarpVector(const std::string s, yarp::sig::Vector* v); + static void stringToYarpVector(const std::string s, yarp::sig::Vector& v); public: static std::string ClassName; @@ -40,6 +40,7 @@ class wbt::DiscreteFilter : public wbt::Block { virtual unsigned numberOfParameters(); virtual bool configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error); virtual bool initialize(BlockInformation* blockInfo, wbt::Error* error); + virtual bool initializeInitialConditions(BlockInformation *blockInfo, wbt::Error *error); virtual bool terminate(BlockInformation* blockInfo, wbt::Error* error); virtual bool output(BlockInformation* blockInfo, wbt::Error* error); }; diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp index fb1b1874a..f48a8553a 100644 --- a/toolbox/src/DiscreteFilter.cpp +++ b/toolbox/src/DiscreteFilter.cpp @@ -17,6 +17,9 @@ #define PARAM_IDX_1LOWP_FC 4 // ::FirstOrderLowPassFilter cut frequency #define PARAM_IDX_1LOWP_TS 5 // ::FirstOrderLowPassFilter sampling time #define PARAM_IDX_MD_ORDER 6 // ::MedianFilter order +#define PARAM_IDX_INIT_Y0 7 // Output initialization +#define PARAM_IDX_INIT_U0 8 // ::Filter input initialization + // Inputs #define INPUT_IDX_SIGNAL 0 // Outputs @@ -30,15 +33,15 @@ using namespace yarp::sig; std::string DiscreteFilter::ClassName = "DiscreteFilter"; -DiscreteFilter::DiscreteFilter() : firstRun(true), filter(nullptr), inputSignalVector(nullptr) +DiscreteFilter::DiscreteFilter() : filter(nullptr), inputSignalVector(nullptr) { - num = new Vector(0); - den = new Vector(0); + y0 = new Vector(0); + u0 = new Vector(0); } unsigned DiscreteFilter::numberOfParameters() { - return 6; + return 8; } bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error) @@ -92,6 +95,8 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) std::string filter_type; std::string num_coeff_str; std::string den_coeff_str; + std::string y0_str; + std::string u0_str; wbt::Data firstOrderLowPassFilter_fc; wbt::Data firstOrderLowPassFilter_ts; wbt::Data medianFilter_order; @@ -104,7 +109,9 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) // Get the string parameter if (!(blockInfo->getStringParameterAtIndex(PARAM_IDX_FLT_TYPE, filter_type) && blockInfo->getStringParameterAtIndex(PARAM_IDX_NUMCOEFF, num_coeff_str) - && blockInfo->getStringParameterAtIndex(PARAM_IDX_DENCOEFF, den_coeff_str))) { + && blockInfo->getStringParameterAtIndex(PARAM_IDX_DENCOEFF, den_coeff_str) + && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_Y0, y0_str) + && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_U0, u0_str))) { if (error) { error->message = ClassName + " Failed to parse string parameters."; } @@ -112,8 +119,12 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) } // Convert the strings from the Matlab syntax ("[1.0 2 3.33]") to yarp::sig::Vector + yarp::sig::Vector num(0); + yarp::sig::Vector den(0); stringToYarpVector(num_coeff_str, num); stringToYarpVector(den_coeff_str, den); + stringToYarpVector(y0_str, *y0); + stringToYarpVector(u0_str, *u0); // Create the filter object // ======================== @@ -121,13 +132,13 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) // Generic // ------- if (filter_type == "Generic") { - if (num->length() == 0 || den->length() == 0) { + if (num.length() == 0 || den.length() == 0) { if (error) { error->message = ClassName + " (Generic) Wrong coefficients size"; } return 1; } - filter = new Filter(*num, *den); + filter = new Filter(num, den); } // FirstOrderLowPassFilter // ----------------------- @@ -162,6 +173,31 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) return false; } + // Get the width of the input vector + inputSignalWidth = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL); + + // Check the initial conditions are properly sized + unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); + unsigned y0Width = y0->length(); + unsigned u0Width = u0->length(); + + if (y0Width != outputSignalWidth) { + if (error) { + error->message = ClassName + " y0 and output signal sizes don't match"; + } + return false; + } + + if ((filter_type == "Generic") && (u0Width != inputSignalWidth)) { + if (error) { + error->message = ClassName + " (Generic) u0 and input signal sizes don't match"; + } + return false; + } + + // Allocate the input signal + inputSignalVector = new Vector(inputSignalWidth, 0.0); + return true; } @@ -175,19 +211,45 @@ bool DiscreteFilter::terminate(BlockInformation* blockInfo, wbt::Error* error) filter = nullptr; } - if (num) { - delete num; - num = nullptr; + if (inputSignalVector) { + delete inputSignalVector; + inputSignalVector = nullptr; } - if (den) { - delete den; - den = nullptr; + if (y0) { + delete y0; + y0 = nullptr; } - if (inputSignalVector) { - delete inputSignalVector; - inputSignalVector = nullptr; + if (u0) { + delete u0; + u0 = nullptr; + } + + return true; +} + +bool DiscreteFilter::initializeInitialConditions(BlockInformation* blockInfo, wbt::Error* error) +{ + // Reminder: this function is called when, during runtime, a block is disabled + // and enabled again. The method ::initialize instead is called just one time. + + // If the initial conditions for the output are not set, allocate a properly + // sized vector + if (*y0 == Vector(1, 0.0)) { + y0 = new Vector(inputSignalWidth, 0.0); + } + + // Initialize the filter. This is required because if the signal is not 1D, + // the default filter constructor initialize a wrongly sized y0 + // Moreover, the Filter class has a different constructor that handles the + // zero-gain case + Filter* filter_c = dynamic_cast(filter); + if (filter_c != nullptr) { + filter_c->init(*y0, *u0); + } + else { + filter->init(*y0); } return true; @@ -201,20 +263,6 @@ bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error) Signal inputSignal = blockInfo->getInputPortSignal(INPUT_IDX_SIGNAL); Signal outputSignal = blockInfo->getOutputPortSignal(OUTPUT_IDX_SIGNAL); - unsigned inputSignalWidth = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL); - - // Allocate the memory for the input data - if (firstRun) { - // Allocate the input signal - inputSignalVector = new Vector(inputSignalWidth, 0.0); - - // Initialize the filter. This is required because if the signal is not 1D, - // the default filter constructor initialize a wrongly sized y0 - filter->init(Vector(inputSignalWidth)); - - firstRun = false; - } - // Copy the Signal to the data structure that the filter wants for (unsigned i = 0; i < inputSignalWidth; ++i) { (*inputSignalVector)[i] = inputSignal.get(i).doubleData(); @@ -229,10 +277,8 @@ bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error) return true; } -void DiscreteFilter::stringToYarpVector(const std::string str, Vector* v) +void DiscreteFilter::stringToYarpVector(const std::string str, Vector& v) { - assert(v != nullptr); - std::string s = str; // Lambda expression used to remove "[]," carachters @@ -252,5 +298,5 @@ void DiscreteFilter::stringToYarpVector(const std::string str, Vector* v) float f; while (sstrm >> f) - v->push_back(f); + v.push_back(f); } From 2c54e8ebdf5cead2f09fea5f6119b849b83fe7b6 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 11 Aug 2017 12:54:42 +0000 Subject: [PATCH 85/89] DiscreteFilter: improved behavior of optional parameters Now it works as expected when the initial state vector is set but is not enabled --- toolbox/src/DiscreteFilter.cpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp index f48a8553a..589130787 100644 --- a/toolbox/src/DiscreteFilter.cpp +++ b/toolbox/src/DiscreteFilter.cpp @@ -123,8 +123,15 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) yarp::sig::Vector den(0); stringToYarpVector(num_coeff_str, num); stringToYarpVector(den_coeff_str, den); - stringToYarpVector(y0_str, *y0); - stringToYarpVector(u0_str, *u0); + + // y0 and u0 are none if they are not defined + if (y0_str != "none") { + stringToYarpVector(y0_str, *y0); + } + + if (u0_str != "nome") { + stringToYarpVector(u0_str, *u0); + } // Create the filter object // ======================== @@ -173,6 +180,9 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) return false; } + // Initialize the other data + // ========================= + // Get the width of the input vector inputSignalWidth = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL); @@ -181,14 +191,14 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) unsigned y0Width = y0->length(); unsigned u0Width = u0->length(); - if (y0Width != outputSignalWidth) { + if ((y0_str != "none") && (y0Width != outputSignalWidth)) { if (error) { error->message = ClassName + " y0 and output signal sizes don't match"; } return false; } - if ((filter_type == "Generic") && (u0Width != inputSignalWidth)) { + if ((u0_str != "none") && (filter_type == "Generic") && (u0Width != inputSignalWidth)) { if (error) { error->message = ClassName + " (Generic) u0 and input signal sizes don't match"; } @@ -236,7 +246,7 @@ bool DiscreteFilter::initializeInitialConditions(BlockInformation* blockInfo, wb // If the initial conditions for the output are not set, allocate a properly // sized vector - if (*y0 == Vector(1, 0.0)) { + if (*y0 == Vector(0)) { y0 = new Vector(inputSignalWidth, 0.0); } From aae5cff8e3b130799f3c36c7b250b166274eea55 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 11 Aug 2017 13:49:33 +0000 Subject: [PATCH 86/89] Reduced memory allocations --- toolbox/src/DiscreteFilter.cpp | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp index 589130787..ee623f3ff 100644 --- a/toolbox/src/DiscreteFilter.cpp +++ b/toolbox/src/DiscreteFilter.cpp @@ -33,10 +33,9 @@ using namespace yarp::sig; std::string DiscreteFilter::ClassName = "DiscreteFilter"; -DiscreteFilter::DiscreteFilter() : filter(nullptr), inputSignalVector(nullptr) +DiscreteFilter::DiscreteFilter() + : filter(nullptr), y0(nullptr), u0(nullptr), inputSignalVector(nullptr) { - y0 = new Vector(0); - u0 = new Vector(0); } unsigned DiscreteFilter::numberOfParameters() @@ -125,12 +124,18 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) stringToYarpVector(den_coeff_str, den); // y0 and u0 are none if they are not defined + unsigned y0Width, u0Width; + if (y0_str != "none") { + y0 = new Vector(0); stringToYarpVector(y0_str, *y0); + y0Width = y0->length(); } - if (u0_str != "nome") { + if (u0_str != "none") { + u0 = new Vector(0); stringToYarpVector(u0_str, *u0); + u0Width = u0->length(); } // Create the filter object @@ -188,17 +193,15 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) // Check the initial conditions are properly sized unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); - unsigned y0Width = y0->length(); - unsigned u0Width = u0->length(); - if ((y0_str != "none") && (y0Width != outputSignalWidth)) { + if ((y0 != nullptr) && (y0Width != outputSignalWidth)) { if (error) { error->message = ClassName + " y0 and output signal sizes don't match"; } return false; } - if ((u0_str != "none") && (filter_type == "Generic") && (u0Width != inputSignalWidth)) { + if ((u0 != nullptr) && (filter_type == "Generic") && (u0Width != inputSignalWidth)) { if (error) { error->message = ClassName + " (Generic) u0 and input signal sizes don't match"; } @@ -246,8 +249,12 @@ bool DiscreteFilter::initializeInitialConditions(BlockInformation* blockInfo, wb // If the initial conditions for the output are not set, allocate a properly // sized vector - if (*y0 == Vector(0)) { - y0 = new Vector(inputSignalWidth, 0.0); + if (y0 == nullptr) { + unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); + y0 = new Vector(outputSignalWidth, 0.0); + } + if (u0 == nullptr) { + u0 = new Vector(inputSignalWidth, 0.0); } // Initialize the filter. This is required because if the signal is not 1D, From bc51e1d22883cf7536fc8c0370b11aad81557186 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Tue, 12 Dec 2017 08:55:31 +0000 Subject: [PATCH 87/89] Rebased DiscreteFilter block on WB3.0 --- toolbox/include/DiscreteFilter.h | 43 +- .../library/WBToolboxLibrary_repository.mdl | 583 ++++++++++++------ .../library/exported/WBToolboxLibrary.slx | Bin 534705 -> 537056 bytes toolbox/src/DiscreteFilter.cpp | 177 +++--- 4 files changed, 497 insertions(+), 306 deletions(-) diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h index 098ca3966..65915f3a1 100644 --- a/toolbox/include/DiscreteFilter.h +++ b/toolbox/include/DiscreteFilter.h @@ -1,6 +1,7 @@ #include "Block.h" #include #include +#include #ifndef WBT_FILTER_H #define WBT_FILTER_H @@ -24,25 +25,41 @@ namespace yarp { class wbt::DiscreteFilter : public wbt::Block { private: unsigned inputSignalWidth; - iCub::ctrl::IFilter* filter; - yarp::sig::Vector* y0; - yarp::sig::Vector* u0; - yarp::sig::Vector* inputSignalVector; + std::unique_ptr filter; + std::unique_ptr y0; + std::unique_ptr u0; + std::unique_ptr inputSignalVector; static void stringToYarpVector(const std::string s, yarp::sig::Vector& v); + // Parameters + static const unsigned PARAM_IDX_FLT_TYPE; + static const unsigned PARAM_IDX_NUMCOEFF; + static const unsigned PARAM_IDX_DENCOEFF; + static const unsigned PARAM_IDX_1LOWP_FC; + static const unsigned PARAM_IDX_1LOWP_TS; + static const unsigned PARAM_IDX_MD_ORDER; + static const unsigned PARAM_IDX_INIT_Y0; + static const unsigned PARAM_IDX_INIT_U0; + // Inputs + static const unsigned INPUT_IDX_SIGNAL; + // Outputs + static const unsigned OUTPUT_IDX_SIGNAL; + // Other defines + static const int SIGNAL_DYNAMIC_SIZE; + public: - static std::string ClassName; + static const std::string ClassName; DiscreteFilter(); - ~DiscreteFilter() = default; - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error); - virtual bool initialize(BlockInformation* blockInfo, wbt::Error* error); - virtual bool initializeInitialConditions(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation* blockInfo, wbt::Error* error); - virtual bool output(BlockInformation* blockInfo, wbt::Error* error); + ~DiscreteFilter() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool initializeInitialConditions(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif // WBT_FILTER_H diff --git a/toolbox/matlab/library/WBToolboxLibrary_repository.mdl b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl index a122a1e37..26ffaba9b 100644 --- a/toolbox/matlab/library/WBToolboxLibrary_repository.mdl +++ b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl @@ -49,9 +49,9 @@ Library { IsActive [1] ViewObjType "SimulinkSubsys" LoadSaveID "192" - Extents [1557.0, 731.0] - ZoomFactor [2.13] - Offset [0.0, 0.0] + Extents [1557.0, 698.0] + ZoomFactor [2.1474358974358974] + Offset [-7.52537313432839, 7.4805970149253653] } Object { $PropName "DockComponentsInfo" @@ -71,7 +71,7 @@ Library { "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAANQAAAooAAABbAP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAKwD///8AAAY7AAADFwAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAKwD///8AAAY7AAAC9gAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAB6/////wAAAAAAAAAA/////wEAAADe/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAEt/////wAAAAAAAAAA/" @@ -85,9 +85,9 @@ Library { ModifiedByFormat "%" LastModifiedBy "dferigo" ModifiedDateFormat "%" - LastModifiedDate "Mon Dec 11 16:51:04 2017" - RTWModifiedTimeStamp 434911818 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Dec 11 16:56:10 2017" + RTWModifiedTimeStamp 434912138 + ModelVersionFormat "1.%" ConfigurationManager "none" SampleTimeColors off SampleTimeAnnotations off @@ -1068,7 +1068,7 @@ Library { ShowPageBoundaries off ZoomFactor "343" ReportName "simulink-default.rpt" - SIDHighWatermark "1789" + SIDHighWatermark "1790" Block { BlockType SubSystem Name "Utilities" @@ -1101,7 +1101,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "213" + ZoomFactor "215" Block { BlockType SubSystem Name "Configuration" @@ -1561,6 +1561,217 @@ Library { } } } + Block { + BlockType S-Function + Name "DiscreteFilter" + SID "1790" + Ports [1, 1] + Position [105, 260, 165, 290] + ZOrder 103 + BackgroundColor "yellow" + FunctionName "WBToolbox" + Parameters "'DiscreteFilter',filterType,numCoeffs,denCoeffs,Fc,Ts,orderMedianFilter,y0,u0" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 46 + $ClassName "Simulink.Mask" + Type "Discrete Filter" + Description "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl." + Initialization "filterType = get_param(gcb, 'filterType');\nnumCoeffs = mat2str(numCoeffs);\ndenCoeffs = ma" + "t2str(denCoeffs);\n\ninitStatus = get_param(gcb, 'initStatus');\n\nif (strcmp(initStatus,'on'))\n y0 = mat2str" + "(y0);\n u0 = mat2str(u0);\nelse\n y0 = 'none';\n u0 = 'none';\nend\n\nclear initStatus" + Display "disp('Filter')" + Array { + Type "Simulink.MaskParameter" + Dimension 9 + Object { + $ObjectID 47 + Type "popup" + Array { + Type "Cell" + Dimension 3 + Cell "Generic" + Cell "FirstOrderLowPassFilter" + Cell "MedianFilter" + PropName "TypeOptions" + } + Name "filterType" + Prompt "Type of the filter" + Value "Generic" + Callback "% From: https://it.mathworks.com/help/simulink/slref/simulink.mask-class.html\nfilterType = get_param(gcb" + ", 'filterType');\ninitStatus = get_param(gcb, 'initStatus');\np = Simulink.Mask.get(gcbh);\nhowToCoeffs = p.getDialo" + "gControl('howToCoeffs');\n\n%set_param(gcb, 'initStatus','off');\nif (strcmp(initStatus,'on'))\n vis_init = 'on';" + "\nelse\n vis_init = 'off';\nend\n\nif(strcmp(filterType, 'Generic'))\n set_param(gcb, 'MaskVisibilities',{'on'" + ",'on','on','off','off','off','on',vis_init,vis_init});\n howToCoeffs.Visible = 'on';\nelseif(strcmp(filterType, '" + "FirstOrderLowPassFilter'))\n set_param(gcb, 'MaskVisibilities',{'on','off','off','on','on','off','on',vis_init,'o" + "ff'});\n howToCoeffs.Visible = 'off';\nelseif(strcmp(filterType, 'MedianFilter'))\n set_param(gcb, 'MaskVisibi" + "lities',{'on','off','off','off','off','on','on',vis_init,'off'});\n howToCoeffs.Visible = 'off';\nend\n\nclear fi" + "lterType initStatus p howToCoeffs vis_init;" + } + Object { + $ObjectID 48 + Type "edit" + Name "numCoeffs" + Prompt "Numerator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 49 + Type "edit" + Name "denCoeffs" + Prompt "Denominator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 50 + Type "edit" + Name "Fc" + Prompt "Cut Frequency (Hz)" + Value "0" + Visible "off" + } + Object { + $ObjectID 51 + Type "edit" + Name "Ts" + Prompt "Sampling time (s)" + Value "0" + Visible "off" + } + Object { + $ObjectID 52 + Type "edit" + Name "orderMedianFilter" + Prompt "Order" + Value "0" + Visible "off" + } + Object { + $ObjectID 53 + Type "checkbox" + Name "initStatus" + Prompt "Define initial conditions" + Value "off" + Callback "initStatus = get_param(gcb, 'initStatus');\nvisibilities = get_param(gcb, 'MaskVisibilities');\nfilterT" + "ype = get_param(gcb, 'filterType');\n\nif (strcmp(initStatus,'off'))\n visibilities{8} = 'off';\n visibiliti" + "es{9} = 'off';\nelseif (strcmp(initStatus,'on'))\n visibilities{8} = 'on';\n if (strcmp(filterType,'Generic'))" + "\n visibilities{9} = 'on';\n end\nend\n\nset_param(gcb, 'MaskVisibilities', visibilities);\n\nclear initSt" + "atus visibilities filterType;" + } + Object { + $ObjectID 54 + Type "edit" + Name "y0" + Prompt "Output y0" + Value "[0]" + Visible "off" + } + Object { + $ObjectID 55 + Type "edit" + Name "u0" + Prompt "Input u0" + Value "[0]" + Visible "off" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 56 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 57 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 58 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 8 + Object { + $ObjectID 59 + $ClassName "Simulink.dialog.parameter.Popup" + Name "filterType" + } + Object { + $ObjectID 60 + $ClassName "Simulink.dialog.parameter.Edit" + Name "numCoeffs" + } + Object { + $ObjectID 61 + $ClassName "Simulink.dialog.parameter.Edit" + Name "denCoeffs" + } + Object { + $ObjectID 62 + $ClassName "Simulink.dialog.Text" + Prompt "* The coefficients are ordered in increasing power of z^-1" + Name "howToCoeffs" + } + Object { + $ObjectID 63 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Fc" + } + Object { + $ObjectID 64 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Ts" + } + Object { + $ObjectID 65 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "orderMedianFilter" + } + Object { + $ObjectID 66 + $ClassName "Simulink.dialog.Group" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 3 + Object { + $ObjectID 67 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "initStatus" + } + Object { + $ObjectID 68 + $ClassName "Simulink.dialog.parameter.Edit" + Name "y0" + } + Object { + $ObjectID 69 + $ClassName "Simulink.dialog.parameter.Edit" + Name "u0" + } + PropName "DialogControls" + } + Name "Container3" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + } Block { BlockType S-Function Name "DoFs converter" @@ -1575,7 +1786,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 46 + $ObjectID 70 $ClassName "Simulink.Mask" Type "DoFs converter" Description "This block converts from a DoFs serialization to a YARP serialization or viceversa depending on" @@ -1592,7 +1803,7 @@ Library { RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 47 + $ObjectID 71 $ClassName "Simulink.MaskParameter" Type "popup" Array { @@ -1610,11 +1821,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 48 + $ObjectID 72 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 49 + $ObjectID 73 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -1622,22 +1833,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 50 + $ObjectID 74 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 51 + $ObjectID 75 $ClassName "Simulink.dialog.parameter.Popup" Name "yarp2WBIOption" } Object { - $ObjectID 52 + $ObjectID 76 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 53 + $ObjectID 77 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -1671,7 +1882,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 54 + $ObjectID 78 $ClassName "Simulink.Mask" Type "Minimum Jerk Trajectory Generator" Description "This block wraps the minJerkTrajGen class from iCub::ctrl::minJerkTrajGen.\n\nThe Minimum Jerk " @@ -1694,7 +1905,7 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 55 + $ObjectID 79 Type "checkbox" Name "externalSettlingTime" Prompt "External Settling Time" @@ -1705,42 +1916,42 @@ Library { "SettlingTime" } Object { - $ObjectID 56 + $ObjectID 80 Type "edit" Name "settlingTime" Prompt "Settling Time" Value "3" } Object { - $ObjectID 57 + $ObjectID 81 Type "edit" Name "sampleTime" Prompt "Sample Time" Value "0.01" } Object { - $ObjectID 58 + $ObjectID 82 Type "checkbox" Name "resetOnSettlingTime" Prompt "Reset on Settling Time Changes" Value "off" } Object { - $ObjectID 59 + $ObjectID 83 Type "checkbox" Name "firstDerivatives" Prompt "Output First Derivative" Value "on" } Object { - $ObjectID 60 + $ObjectID 84 Type "checkbox" Name "secondDerivatives" Prompt "Output Second Derivative" Value "on" } Object { - $ObjectID 61 + $ObjectID 85 Type "checkbox" Name "explicitInitialValue" Prompt "Explicit Initial Value" @@ -1753,11 +1964,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 62 + $ObjectID 86 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 63 + $ObjectID 87 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -1765,38 +1976,38 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 64 + $ObjectID 88 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 65 + $ObjectID 89 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 66 + $ObjectID 90 Prompt "Trajectory Parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 4 Object { - $ObjectID 67 + $ObjectID 91 $ClassName "Simulink.dialog.parameter.CheckBox" Name "externalSettlingTime" } Object { - $ObjectID 68 + $ObjectID 92 $ClassName "Simulink.dialog.parameter.Edit" Name "settlingTime" } Object { - $ObjectID 69 + $ObjectID 93 $ClassName "Simulink.dialog.parameter.Edit" Name "sampleTime" } Object { - $ObjectID 70 + $ObjectID 94 $ClassName "Simulink.dialog.parameter.CheckBox" Name "resetOnSettlingTime" } @@ -1805,21 +2016,21 @@ Library { Name "Tab1" } Object { - $ObjectID 71 + $ObjectID 95 Prompt "Input/Output" Array { Type "Simulink.dialog.parameter.CheckBox" Dimension 3 Object { - $ObjectID 72 + $ObjectID 96 Name "firstDerivatives" } Object { - $ObjectID 73 + $ObjectID 97 Name "secondDerivatives" } Object { - $ObjectID 74 + $ObjectID 98 Name "explicitInitialValue" } PropName "DialogControls" @@ -1853,7 +2064,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 75 + $ObjectID 99 $ClassName "Simulink.Mask" Type "Real Time Synchronizer" Description "This block slows down the simulation trying to match the period specified \nas parameter (in se" @@ -1861,7 +2072,7 @@ Library { Display "disp('Real Time Synchronizer')" Object { $PropName "Parameters" - $ObjectID 76 + $ObjectID 100 $ClassName "Simulink.MaskParameter" Type "edit" Name "period" @@ -1886,7 +2097,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 77 + $ObjectID 101 $ClassName "Simulink.Mask" Type "Simulator Synchronizer" Description "This block synchronizes with the external simulation on a simulator \n(only Gazebo is supported" @@ -1896,21 +2107,21 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 78 + $ObjectID 102 Type "edit" Name "period" Prompt "Controller Period (in seconds)" Value "0.01" } Object { - $ObjectID 79 + $ObjectID 103 Type "edit" Name "serverPortName" Prompt "Server Port Name" Value "'/clock/rpc'" } Object { - $ObjectID 80 + $ObjectID 104 Type "edit" Name "clientPortName" Prompt "Client Port Name" @@ -1932,11 +2143,11 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 81 + $ObjectID 105 $ClassName "Simulink.Mask" Object { $PropName "Parameters" - $ObjectID 82 + $ObjectID 106 $ClassName "Simulink.MaskParameter" Type "edit" Name "tol" @@ -2153,7 +2364,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 83 + $ObjectID 107 $ClassName "Simulink.Mask" Type "YARP Clock" Description "This block outputs the current YARP Time.\nIn a nutshell, this block outputs the equivalent of " @@ -2177,7 +2388,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 84 + $ObjectID 108 $ClassName "Simulink.Mask" Type "YARP Read" Description "This block behaves as the command 'yarp read'. \nBy default it has the option 'Autoconnect' act" @@ -2195,35 +2406,35 @@ Library { Type "Simulink.MaskParameter" Dimension 6 Object { - $ObjectID 85 + $ObjectID 109 Type "edit" Name "portName" Prompt "Source Port Name" Value "'/portname'" } Object { - $ObjectID 86 + $ObjectID 110 Type "edit" Name "signalSize" Prompt "Port Size" Value "1" } Object { - $ObjectID 87 + $ObjectID 111 Type "checkbox" Name "blocking" Prompt "Blocking Read" Value "off" } Object { - $ObjectID 88 + $ObjectID 112 Type "checkbox" Name "timestamp" Prompt "Read Timestamp" Value "on" } Object { - $ObjectID 89 + $ObjectID 113 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -2235,7 +2446,7 @@ Library { "t_string" } Object { - $ObjectID 90 + $ObjectID 114 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -2260,7 +2471,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 91 + $ObjectID 115 $ClassName "Simulink.Mask" Type "YARP Write" Description "This block behaves as the command 'yarp write'. \n\nBy default this block opens a port names as" @@ -2273,14 +2484,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 92 + $ObjectID 116 Type "edit" Name "portName" Prompt "Opened Port Name" Value "'/portname'" } Object { - $ObjectID 93 + $ObjectID 117 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -2291,7 +2502,7 @@ Library { "';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string" } Object { - $ObjectID 94 + $ObjectID 118 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -2314,7 +2525,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 95 + $ObjectID 119 $ClassName "Simulink.Mask" Type "Errors" Description "Computes two kinds of errors. The first is just the difference between x\nand y while the secon" @@ -2458,7 +2669,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 96 + $ObjectID 120 $ClassName "Simulink.Mask" Type "Holder" Description "This block holds the first input value during the simulation." @@ -2695,7 +2906,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 97 + $ObjectID 121 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyActuators.png'),'center')" } @@ -2727,7 +2938,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 98 + $ObjectID 122 $ClassName "Simulink.Mask" Type "Set Low Level PIDs" Description "This block sets the low level PID values for the robot actuators.\n\nThe PIDs can be stored in " @@ -2749,7 +2960,7 @@ Library { Type "Simulink.MaskParameter" Dimension 2 Object { - $ObjectID 99 + $ObjectID 123 Type "edit" Name "WBTPIDConfigObjectName" Prompt "WBTPIDConfig Object Name" @@ -2757,7 +2968,7 @@ Library { Tunable "off" } Object { - $ObjectID 100 + $ObjectID 124 Type "popup" Array { Type "Cell" @@ -2783,11 +2994,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 101 + $ObjectID 125 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 102 + $ObjectID 126 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2795,19 +3006,19 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 103 + $ObjectID 127 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.parameter.Control" Dimension 2 Object { - $ObjectID 104 + $ObjectID 128 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "WBTPIDConfigObjectName" } Object { - $ObjectID 105 + $ObjectID 129 $ClassName "Simulink.dialog.parameter.Popup" Name "pidType" } @@ -2816,10 +3027,10 @@ Library { Name "ParameterGroupVar" } Object { - $ObjectID 106 + $ObjectID 130 Object { $PropName "DialogControls" - $ObjectID 107 + $ObjectID 131 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" @@ -2876,7 +3087,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 108 + $ObjectID 132 $ClassName "Simulink.Mask" Type "Set References" Description "This block sets the references for the robot actuators.\nThe type of control mode is specified " @@ -2893,7 +3104,7 @@ Library { Type "Simulink.MaskParameter" Dimension 2 Object { - $ObjectID 109 + $ObjectID 133 Type "popup" Array { Type "Cell" @@ -2925,7 +3136,7 @@ Library { " maskStr" } Object { - $ObjectID 110 + $ObjectID 134 Type "edit" Name "refSpeed" Prompt "Reference Speed" @@ -2937,11 +3148,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 111 + $ObjectID 135 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 112 + $ObjectID 136 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2949,18 +3160,18 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 113 + $ObjectID 137 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.parameter.Control" Dimension 2 Object { - $ObjectID 114 + $ObjectID 138 $ClassName "Simulink.dialog.parameter.Popup" Name "controlType" } Object { - $ObjectID 115 + $ObjectID 139 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "refSpeed" @@ -2970,10 +3181,10 @@ Library { Name "ParameterGroupVar" } Object { - $ObjectID 116 + $ObjectID 140 Object { $PropName "DialogControls" - $ObjectID 117 + $ObjectID 141 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" @@ -3048,7 +3259,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 118 + $ObjectID 142 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyModel.png'),'center')" } @@ -3080,7 +3291,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 119 + $ObjectID 143 $ClassName "Simulink.Mask" Display "image(imread('Dynamics.png'))" } @@ -3111,7 +3322,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 120 + $ObjectID 144 $ClassName "Simulink.Mask" Type "Centroidal Momentum" Description "This block computed the 6 element centroidal momentum, as defined in:\n\"Centroidal dynamics of a huma" @@ -3127,24 +3338,24 @@ Library { RunInitForIconRedraw "on" Object { $PropName "DialogControls" - $ObjectID 121 + $ObjectID 145 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 122 + $ObjectID 146 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 123 + $ObjectID 147 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 124 + $ObjectID 148 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3280,7 +3491,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 125 + $ObjectID 149 $ClassName "Simulink.Mask" Type "Get Generalized Bias Forces" Description "This block retrieves the generalied bias forces of the robot.\n\nAssuming DoFs is the number of degree" @@ -3295,24 +3506,24 @@ Library { RunInitForIconRedraw "on" Object { $PropName "DialogControls" - $ObjectID 126 + $ObjectID 150 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 127 + $ObjectID 151 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 128 + $ObjectID 152 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 129 + $ObjectID 153 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3493,7 +3704,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 130 + $ObjectID 154 $ClassName "Simulink.Mask" Type "Gravity bias" Description "This block compute the generalized bias forces due to the gravity\n\nAssuming DoFs is the number of de" @@ -3506,24 +3717,24 @@ Library { RunInitForIconRedraw "off" Object { $PropName "DialogControls" - $ObjectID 131 + $ObjectID 155 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 132 + $ObjectID 156 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 133 + $ObjectID 157 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 134 + $ObjectID 158 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3695,7 +3906,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 135 + $ObjectID 159 $ClassName "Simulink.Mask" Type "Inverse Dynamics" Description "This block compute the inverse dynamics of the robot.\n\nAssuming DoFs is the number of degrees of fre" @@ -3715,24 +3926,24 @@ Library { RunInitForIconRedraw "on" Object { $PropName "DialogControls" - $ObjectID 136 + $ObjectID 160 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 137 + $ObjectID 161 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 138 + $ObjectID 162 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 139 + $ObjectID 163 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3760,7 +3971,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 140 + $ObjectID 164 $ClassName "Simulink.Mask" Type "Mass Matrix" Description "This block retrieves the robot mass matrix.\n\nAssuming DoFs is the number of degrees of freedom of th" @@ -3773,24 +3984,24 @@ Library { RunInitForIconRedraw "on" Object { $PropName "DialogControls" - $ObjectID 141 + $ObjectID 165 $ClassName "Simulink.dialog.Group" Prompt "%" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 142 + $ObjectID 166 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } Object { - $ObjectID 143 + $ObjectID 167 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 144 + $ObjectID 168 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -3899,7 +4110,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 145 + $ObjectID 169 $ClassName "Simulink.Mask" Display "image(imread('jacobian.png'))" } @@ -3930,7 +4141,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 146 + $ObjectID 170 $ClassName "Simulink.Mask" Type "DotJ Nu" Description "This block computes the product between the time derivative of the Jacobian of the specified frame and" @@ -3951,7 +4162,7 @@ Library { RunInitForIconRedraw "on" Object { $PropName "Parameters" - $ObjectID 147 + $ObjectID 171 $ClassName "Simulink.MaskParameter" Type "edit" Name "frameName" @@ -3962,11 +4173,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 148 + $ObjectID 172 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 149 + $ObjectID 173 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3974,22 +4185,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 150 + $ObjectID 174 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 151 + $ObjectID 175 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Object { - $ObjectID 152 + $ObjectID 176 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 153 + $ObjectID 177 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -4127,7 +4338,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 154 + $ObjectID 178 $ClassName "Simulink.Mask" Type "Jacobian" Description "This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number of degrees of" @@ -4144,7 +4355,7 @@ Library { RunInitForIconRedraw "on" Object { $PropName "Parameters" - $ObjectID 155 + $ObjectID 179 $ClassName "Simulink.MaskParameter" Type "edit" Name "frameName" @@ -4155,11 +4366,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 156 + $ObjectID 180 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 157 + $ObjectID 181 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4167,22 +4378,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 158 + $ObjectID 182 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 159 + $ObjectID 183 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Object { - $ObjectID 160 + $ObjectID 184 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 161 + $ObjectID 185 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -4293,7 +4504,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 162 + $ObjectID 186 $ClassName "Simulink.Mask" Display "image(imread('forwardKinematics.png'))" } @@ -4324,7 +4535,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 163 + $ObjectID 187 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4341,7 +4552,7 @@ Library { "', 2, 'Joint configuration')\n\nclear escapedFrameName;" Object { $PropName "Parameters" - $ObjectID 164 + $ObjectID 188 $ClassName "Simulink.MaskParameter" Type "edit" Name "frameName" @@ -4352,11 +4563,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 165 + $ObjectID 189 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 166 + $ObjectID 190 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4364,22 +4575,22 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 167 + $ObjectID 191 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 168 + $ObjectID 192 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Object { - $ObjectID 169 + $ObjectID 193 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 170 + $ObjectID 194 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -4492,7 +4703,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 171 + $ObjectID 195 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4516,21 +4727,21 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 172 + $ObjectID 196 Type "edit" Name "baseFrame" Prompt "Base Frame" Value "'root_link'" } Object { - $ObjectID 173 + $ObjectID 197 Type "edit" Name "endEffFrame" Prompt "End Effector frame" Value "'l_sole'" } Object { - $ObjectID 174 + $ObjectID 198 Type "popup" Array { Type "Cell" @@ -4544,7 +4755,7 @@ Library { Value "Full Constraint (Position and Orientation)" } Object { - $ObjectID 175 + $ObjectID 199 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -4552,7 +4763,7 @@ Library { Tunable "off" } Object { - $ObjectID 176 + $ObjectID 200 Type "edit" Name "localName" Prompt "Model Name" @@ -4560,7 +4771,7 @@ Library { Tunable "off" } Object { - $ObjectID 177 + $ObjectID 201 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -4568,7 +4779,7 @@ Library { Tunable "off" } Object { - $ObjectID 178 + $ObjectID 202 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -4581,11 +4792,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 179 + $ObjectID 203 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 180 + $ObjectID 204 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4593,34 +4804,34 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 181 + $ObjectID 205 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 182 + $ObjectID 206 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 183 + $ObjectID 207 Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 184 + $ObjectID 208 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "baseFrame" } Object { - $ObjectID 185 + $ObjectID 209 $ClassName "Simulink.dialog.parameter.Edit" Name "endEffFrame" } Object { - $ObjectID 186 + $ObjectID 210 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -4629,25 +4840,25 @@ Library { Name "Container8" } Object { - $ObjectID 187 + $ObjectID 211 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 188 + $ObjectID 212 Name "robotName" } Object { - $ObjectID 189 + $ObjectID 213 Name "localName" } Object { - $ObjectID 190 + $ObjectID 214 Name "wbiFile" } Object { - $ObjectID 191 + $ObjectID 215 Name "wbiList" } PropName "DialogControls" @@ -4788,7 +4999,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 192 + $ObjectID 216 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4814,14 +5025,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 193 + $ObjectID 217 Type "edit" Name "solverName" Prompt "Solver Name" Value "'/cartesianSolver'" } Object { - $ObjectID 194 + $ObjectID 218 Type "edit" Name "dofs" Prompt "#Dofs" @@ -4829,7 +5040,7 @@ Library { Tunable "off" } Object { - $ObjectID 195 + $ObjectID 219 Type "popup" Array { Type "Cell" @@ -4848,11 +5059,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 196 + $ObjectID 220 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 197 + $ObjectID 221 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4860,33 +5071,33 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 198 + $ObjectID 222 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 199 + $ObjectID 223 $ClassName "Simulink.dialog.TabContainer" Object { $PropName "DialogControls" - $ObjectID 200 + $ObjectID 224 $ClassName "Simulink.dialog.Tab" Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 201 + $ObjectID 225 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "solverName" } Object { - $ObjectID 202 + $ObjectID 226 $ClassName "Simulink.dialog.parameter.Edit" Name "dofs" } Object { - $ObjectID 203 + $ObjectID 227 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -4995,7 +5206,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 204 + $ObjectID 228 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyStates.png'),'center');" } @@ -5027,7 +5238,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 205 + $ObjectID 229 $ClassName "Simulink.Mask" Type "Get Limits" Description "This block provides the joint limits gathering data from either the Robot's Control Board or UR" @@ -5051,7 +5262,7 @@ Library { Type "Simulink.MaskParameter" Dimension 2 Object { - $ObjectID 206 + $ObjectID 230 Type "popup" Array { Type "Cell" @@ -5071,7 +5282,7 @@ Library { "imit blockParameters limitsTypeBlockParam" } Object { - $ObjectID 207 + $ObjectID 231 Type "popup" Array { Type "Cell" @@ -5096,11 +5307,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 208 + $ObjectID 232 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 209 + $ObjectID 233 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5108,17 +5319,17 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 210 + $ObjectID 234 Prompt "Simulink:studio:ToolBarParametersMenu" Array { Type "Simulink.dialog.parameter.Popup" Dimension 2 Object { - $ObjectID 211 + $ObjectID 235 Name "limitsSource" } Object { - $ObjectID 212 + $ObjectID 236 Name "limitsType" } PropName "DialogControls" @@ -5126,10 +5337,10 @@ Library { Name "ParameterGroupVar" } Object { - $ObjectID 213 + $ObjectID 237 Object { $PropName "DialogControls" - $ObjectID 214 + $ObjectID 238 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" @@ -5218,7 +5429,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 215 + $ObjectID 239 $ClassName "Simulink.Mask" Type "Get Measurement" Description "This block gathers the measurement of the specified type.\n\nAssuming DoFs is the number of deg" @@ -5231,7 +5442,7 @@ Library { RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 216 + $ObjectID 240 $ClassName "Simulink.MaskParameter" Type "popup" Array { @@ -5258,11 +5469,11 @@ Library { Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 217 + $ObjectID 241 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 218 + $ObjectID 242 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5270,21 +5481,21 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 219 + $ObjectID 243 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 220 + $ObjectID 244 $ClassName "Simulink.dialog.parameter.Popup" Name "measuredType" } Name "ParameterGroupVar" } Object { - $ObjectID 221 + $ObjectID 245 Object { $PropName "DialogControls" - $ObjectID 222 + $ObjectID 246 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" diff --git a/toolbox/matlab/library/exported/WBToolboxLibrary.slx b/toolbox/matlab/library/exported/WBToolboxLibrary.slx index f02b5434bb5c2b45a8d92f1e16fd33edeb7522be..91441fdebcb348fee849dc01b487db5d5063b83c 100644 GIT binary patch delta 80824 zcmV)9K*hhY(IDXCAQMna0|XQR000O87Ke*V2$g1zXps{$e-mfJSfXZ6TVfQW@vSR& ztO_k@>llA8lnv*y_|~3tdhWTmH(4C2(%XYJLdiKIL5Mt%T;)O*bM%@m{TcF%Win@_ zk}yXt800O|gx8cS4NqFtpsfJobw<)qUeD3a+L~f)_zo&&f=(eja;tR3Yh(QV_bn{e>t-Z_lABwI-;Xx&PU7oTbIrt=NL+;Kw5)IK=7}&61J`3>WdS3rnaCf zb5-n^QF$<8R5eW#G!v)m@)P{NUcI=HeHF0X1Y!AIhY)aQ?8-I za<3~vhv+BfZSwG#Em0bUWa@{JAI4c2(TLE=-A95iY2NpS!&&L{Q|TSsh)u5=!9G*T zj9quXyA_b%w`slwEehq5T~K4b>WKXx@YW}hPe?{$dK1&>>@48Pzy;8-7yYiKB$&=Z zS9pYa{pAWbH`_+M(XN6+_1hbJd5Uz`^pct4kS3$p#{!1c$U z_$Pv<^h@tkcspdhtO&|Eq^yVKEJ$JdieYHEBM7~m7AQ}pHGk89y>096_K~ZyN+htuUhTQ z6wj{yMaGeJ9{39eb1?bfcoY9N#GTo;*!L$#uqoTtp=*aBU%oS6x`3v}!BMt7>s8dR zq}R5%SJpb$WoBI`;r=*sZU~3{Y1{R$ow4o0+LDzOHTIpt!1zcvU5kwExUxe{n(wCs$frGdH6w#Q@Eo z`=OIt;usFd_N%vK8p*&uI1`|UxV_^=o%Z&to^+Hq#JQeD`t5=4;CL3>@8;3^f+0&ull{t zZ_0hn|6Ta&(D$a!^{MUI*CZH`D1!SZOd#Xgybeu4(^WW#kl?I$zR>*$NgpG|MfchQ3Y}Y-D?9~Tz-h*@G2Tb_I$DZs@v)Aw%YCH zP%;j2`F|xHkJD--Bb|8TfXsm=_5jnlVPC#bAnYYO4^k|SU}7%8IUf3Osteblu;ISv z`4MIK^49!v$EzML+=Xky4o%q2I}x=9W=U^R#rQM-7F!&_47^Y|I5s`^g9vua#zU)a zc|=0z+Pkp5>#EUxe-WN}uY&+iRaUmha+hllSbyVzGjRf-XPBk!RxfJ=8w5K?kD0RJ zB?+T*A6Wg#n5|zp2ihep$#ySD=(|gtMkN69IJh>I%>bojau!TTAf4sw&tyyo1d@!% zXFAc!EwuyZcS=ur%976Y)DAu^7qS*q0L5SF_B#vd@U-DM$hmU@H)*KV9EfEcI18NO zD1QpzL{&ju`o4Q$OX&p|L$nMP4Y)kRP?el1IhpYGK&zkLd$3cdc35ov@`Z>eyrg#- zMZTO_Bu$W6ZNKu6O}V0JoZHKgG7A{HCu;C+Ilm&o*>n_Ga>&>ZCWk)Ue85bklYs^( zi?El?*uQ~W)0VV5B|zficn-k8ZzT=!m*jq;k45?5Z=yNyr3LU3)=)_G&@FG zN6{x06yTl97QFrHOxhy~kpbKTuTS5`jTKaJJoc(v9r#Oc0$gbS3ZC#TeU`(2;?tIC zenH&0d8?Uy#pj#9XG|kK#)|1ic7G6Y)qu2#zbLkN`J#M?UdDJr%nGiw2tPltqw$Qf zJ?Y&?8!{XoyS}ZWaNhVbaITr&WNv#)F)z(Dtzx^GrstH}gnTvA>a|PH?Di|$yQ^q= zv<%oQ|5Zz}fr7><8f=hZD}dhk?u4H4%H9bTC@Y)ALfH1J0G^}Xbw&HP@PBG>)!ps~ z;~5-g9XjAb`3dwEU!T+Pi?*ciO8#% z({8R8(Y%J5^BVas2sej^g%0!LbaE*)imJ0h%lK|+RkVz6x&i|$wSPNyK9dQGv(-&c zJSTE&SFiD@{Tam8Q^(_?^7Epes@R&p(0Zz7{}^A+P)`mVw|WLZ4}zdSW~m97aux|P$$#^Xl4L3Qv#V#nVjf%=_Py~k0R9st@A4vF6Kyt!xhy{&rL%%& z6Vg?Z9Batl9h!~*Zjb#d$M)bMGV>>>QV4WT;%H_s$T^TV2$3X#N)12CgFyoSFz~}L zfdiQo4IFOF!L^|G-D@AN-5Ffv?|jdm*bAfwC7W@t3B2!}F@LI=Vx(RM%P_hi*G}%9 zlwj)u7925DZNEB(7p@9~88mJHO%6`!?Pm;mpy z&ODZmEBG1}SAR~t#S(DvYqVo6i%R?)z_H`uMFH4mC+9Q$KdO?W%Dk=%=hNOtnYL3E-OCk>D%nb(Ox_K%NDH~HA zEJ(W7z&5gwR5C;q!goq`fh~Z4l0Z=uew}kAhs(-pY8l7skghRT3W0mw?eR2Ha zgz(^XekXu^{Y91x`+{~CBw7YuqM>1(Yusdrzf?E&*?f$zJ-_QL(}z=@^~ zaVLO&Y^DW$4!>WJWyq1cA6!F6tHCEv1cPo+0!rTGWbOxd`w@MWEfz3}}c z@k<=j+jo#pnnsPAV?_U6#t#L%jaH04~MoDPD}uxyZ8F%0c3m_$b8pO2Fu52@>$f%j$PHu(0vs zu2Qb1tulI^3ooGSoEuT8 zFU4(Fg$uwe><}-ajJFNbKD5K?cYj<0 zc4~vf^bX!!46y#r9hf; zGd$X{fwPjaIdi<^9G3)HF|+cv+aHgqSJ;&wl7pp#$3}z&ZCSXbtMedNOZt#dsXZ<( z&w8#8dN!5tll(qrmx$(_gxFGI-UrxzLI+ zQB&8y)u(`{)4uendfx;) zYF`-%^yLyBbB+r;VFFHtZff^3n{*17!;wB%d9hWnbb_OzdPEb{BV%|+f+UP^(H-h^ zI2l#J{z5QLg^@b~G4WKB+Hc?C}I#CNR*Ic4B7m$VrmdNylwGDH@^7^zqJ2-r7TA zyg>vfk1=8#hkyFyDqSj|IsUSbJ~rF05KsHw$E@41nZX`OAR7N+0Lu7F-G2e z;$fhFgd1ki*E^qHCb7AQ8&{}kO+PZ|Dg^@Wi#PE*JAYaRwhKZRV8{qTnHi`@_5`I`=U z5#QLs1ZGQAT11c&CI?P*PJ%;fE*+}Yl{OyDScF4gulWi3oOO!v){@3=eN_LMv}O<$ z7MlcuzJIgy2RP!9*Sg(P*DQ4Bs~+Gj>bt{eKl10!m{Z&KE8GYpNWP$VRWC7sJHc~( zbAejjOCQ|;C)C43=>mvwui@<29>EaV_g%+*R%CxubbK7o-HmCC%cn z@_+5wOktBxrXanYO)*2pSr9uQ*e;v}@4DKQA?h>{Qz{=DOkIOYI@cv`AuS_(&xwz5_J8Q^ad4M>QR&-~M~UO@yVqzoJR7rM6CG!w zWJwvFTSy6y_FiqOS}st(Q+V7vb4GE8%MiG++|NM!>~(dc*^bdCKo!h}4PK9Dl@1fw zPZYR^8tv#kg~kMpwy_ec3Nv{qI0PZdfmDq{Pbs^X!s$mR&Ld#EAFuG63TP0VPJiYL znwkU|GdxBml%(E3d{!?P+-)=2ql?QAutVA1?P=~VgDg$Ft_v}6^kQF@$kp`38@tO1 zc?10fIFA#LWfd^+R;7|CGnI`A=FzGPyV}SMdgQJMKV1#}^}NmmK9H*$huqeH#3RGf zZiKbbhkXFB3yZ*WWZVyC4nU%erGGIKFY$*MDndwUHVdA#&Y{jLwGR&R9j z_Wa~6)!Ef^yB^(z5#hJQG0pRG9Yl@DRX^6=9rH2 zAdG4>zCEP=cRCGa34cUwnUzWh12yh`28FVSJ`85Zg0t#)ETS1hD90FoV7Zu5lvuV0 zLMq6pWmZN_e3||lqOAAB_-&FOKj7tL0?kmYH2fG?n6NyS1fk7dAB z;=!t^?s#is(?bpVI9P^n95{5rcy@OZpMSb9$;YR5DJ5K~x00Y`27jSRZ9<`{7Syy3 z6bMk-U|ej{ZeJ3Xj37&bi4KRW<-?6|oF|r{E93*FA(^<$fW$q(E5;jv^n;f-)fz-0 z58lZHUbLERp-!}BhL^VJZBl+1_qEf{fYh;A2%b;EN$NhK+5=&K;%5|fzhs-1&CV&8 zg_Rv)-p&OLmp28cseccW%#*dC&wcl9gi*IBR1xzzjE>P>4EtDP22A&LJoak9#`I8s z^dqXT1}XvSzgW7`xKHT>P|Z{uocaychGx-&K-6<)sW$}Yq)RhW<#a^)+ zJejNYaEvi{D|RuKz96+b*blB(>*K)vbaJ*LGUtnxH{{eF2Y*uSj--nV@^ADSQAANo z0OMGnw;e@yt~{w#4nlr2>x@^tjSG;z z(Ob6Gn(5_QbDQv1qJxUG5jnu3h$~eL<)8|psUA>K9M*#>ykImBmhjzT#4zZHCRY(V z|13GX(TH*0asI&92P{iO@a8~AEh2WZDFEl+2ivdSI+*s-6)8A#kf{WxHA|+`yqJiWzTDI=+Sb}2dFT)yr#596_R%7!6cR}Vp zR(c3Yg!Ey0PCh#!MvKMjTjLF2!*lMYo`B9{kXyXR-dpDi3j~DD$7t$)q%`(1n%_3& zwtpy5il7hZfaR+f>beC0=K)O#25?Dj!AeNjJC+yBj%GZ*6K<+^XP4H=JL~=EwdPbo zJ_jy`IbA^3Jsj@7INWVoPxs%APM%f2W^-1Bq`;+)oP;8u6W!siaU!IP)_D>mJ4AQD zTR8XJ1qx0Bby~3GF1xud68QLG(r$J|&VS`agww-vymZ|+#Af+}E9ER9D>qUwsbU{wZXXafg(Mx6j{KSMEa1eFsU*RBlp&LQasFO1zQr#czpSShI!`w}UG6Un` zuO-1paz&T$U8Nm5Qc;j%HgU%=uVCpk@Yk`b9z{~7bo2_px?8qifd=4A6JJi z)*5>$cw|UxrD4f;XY539(jz*t65pi5sQY`#-lRNTM0t>NdqIM;z<)`dJY9H)hhuvo zuDXiNV??nlLsnyuHsvXsLl-6`E)Gtcy$2~V#0;|IS+4v+swZ$zb&O3a)Q;)Fcul2-E%&V;@F#Wi-v z0YP!b#p>5ru8rXo$$#dHw)Q2r`bf4LcnmhwZo zVElO=t7ufUG><{><7|(>xkb-(Su<5>V*LIC!GP_m!S>NItZIZ*z-FYVe0|}^r$;y) z+UC^ZHOzAdSg>6$Q!~GGAK&86clTY89T-~2#FHE{xPRcNlYgf1+?h;V;eLK0 z8ri(9r)3kug=^nkf*`mb9s)c5geuM}d;F=mk+MyDiHYA7=h1M{yZ5o?moLhbe2Ga^ zrSl#Ucgi#w%G$+PiJqpVlbs%HaJI1jy_B}NSb8+|x*uTM3o;38IStV!J}b#2EgX*N z=6D+(Bqi&LUw=z@!-aTJWDr)y4n6iX~iz zpH@fY&XtkVGj-!~JcMy$#lZXpE*kq6*w#|TqznVbbbp)d&DJ_9J>b)Gg$nvaXT#L> z)|DL=tO-f`vt@LOn*2f~sNDBhHbwD$>`pa`f+Q{fo|Vhb|5tnAhd;)vw~&Gf`%K*H zTfi8Xtfae;Cg?3!4oF!RO=$@e!Rm_AR+*}kbb^;9{$ zwY*9RzJJoYav1{CWJ$|~m8`3#_%k)xlsjghOX!Y1vhf+g>7ADze}3zPZTLV(P;5&X$eIFcyVEWcN?m8e{9w?}azPvY*?pj$OZ?ugK6i%^*iG zp-C=7bOi(8$0ApBxRO3jJf`FlY(%LleTl$h4Sy_3H;P5y+oqJN%Np$krj2FwctU2d zQnW>6B&v{dW#Gr!qhP#lPg+{MWT7k%M-LJbm_{kB1xXyw+>A{lGzt0Gv=0gHqF)T;9|}ZeBfWJ zIDZOO432&NwqMPNE0JJwy%)B$lE5%|7u^R@zwF*_i-%QSjEx^8*CIS~U3(D{UJu-L ztx2-SY6zeODY?_B;iDZd!*{XU$c4SsF6BG#PRfJ<@oJiggPG!lQuUf>~Br%7fI zH-(*Fx~K_c@~=t?KBs2pB{kTYk@aYK#eZ~rqDfP=By`PEae@2+5<32k4_3)U&>tn_ zj0j(Uw1knOZ4`&zpw1{QGeap}3`TP~?}se6;(MBKO-du0iO4eIy}v;1Hm?^DUw6x^ zkmE@H#-I&!IOoOX2iBO`ph_+z*@nksN-!YuhgB^dm*_N&i7T-vW1hq9S2w;h`G1H% z)KIiu3R&2AsPKrZj_2ivQ$PqABP?=WbX6)8V=HH;6NB?|r8S^&k{M%mE%;Q+Q42iL zHW-XQa^}A{gyyxTDJfMd2F_`bwFM4Rl(IO16mEY3R}={`6DS^;<$?>)C>o+?|py|Ka=dV|z>|+39w~`fBQT$7D3_diI=a?i;~E zsJ$PZ9KCU_XCDC9!Q2i$0Vxl5bGlxI(8c>;-r;EWwMNWSq%C{N7A3sg*NIm_r{4u! z0nWO8Kv%2Pm4N)?ETDefGJj#EWS}dW-Lf}I*6=1FF)Y4A!YCtS@e^Wu%3F>zOhF6o zWs~});MKt^`(%7?+D%D@WN4jpP${ZgxASf)&fLBxPn|h%_sLTxk}u=UYb?BL_R+J# zqpswGQXu(yWM4`8c{Ip_1UagrVkx&=?53V*tm(RLSoW!nAb zj@4_ykM?d;0T&baZn+qBPA@?HBtxo(uzs?xN%Rq=-1*b%@zqXZlYm7rO$FcOAGfxg zsTD@Sc)obbfOohB1M@+L7~RU7(vf{etE&DPp(Ony)Q1DKb6xqL8#%mQ`bfL91Ku&T zC*EXhD+Rz7mwpZm0e^7fW0%-LJR>%DIfe|6Z4Op?bZINyC^0X@r@GoRP>k2yaTwiTy7P~*9G@EQH*NYwXiM9(Up}(Q991AW#KQD3&F&N zlcz5Qp5RZBf=fT1HaQ=r0h)lS)xzHB?Oq7Z%p7eQMlz3q`F|o3b`X19;_I%e8>ZY8 ztxXM=_=My^pIASW$ckpfN)Su!;1=x8a%%=GCr4}zU_hJ^aG$;`3TiSyK~FmhX86P! z-8#{DMrVDKc(Pgklz#H_`xA>WO=(Emhk_~LS`sHXxPjTHJAbf$eUJV7rd|26>Z#~T-PxqPMKn~3bK_*zV06)&& zhVW=2?u0_-Xzc8su2aDv9F|?0g74wxbwi?a7I2g=dkm)24LS$OWHb+gqP_4WDB25; zf}*|fEGXIw4}+pT;D@VsG}(kf>z+~TzESIeQR|_qHGdUf*&&-*f-@w%9jlX)WaK1K zodva=L?%vU7`#-bU?bO(De$Y93;0^yh-3}k(MK^Wg8;-+P2uI4gd+zeS_U4Y#dj# zH3YVFpyf!T)~v=7I$9U%`VYveF$5Tx%HMes=Xy^--if8xiwKomVsk$dIR3X>jEg@D zXZ{jvHGf>7yqNnk@`KR-h{LnL|6CpW;%wUYhkyM8-wq~Jy|WVjE!K~v)sboo$Ta$B z2lE`71B`npeM*tPHoS{-lHCK|shPGmpQX?%@>#g&*2OD!9|~(f2lfp-&zO^aEzI#A zEWIv7*!#7))7*K!+ZdLt0cy<2Vrk3y^U&jtAE>gnazejuP|J3$N&%gwdbfmVd-7K| zqJN?q&gQSz5|KuvQ7$egGZBeanXeE-v1A%ey}4%Mv=viUQylhzGx?Gg;2KRkwCWL^$rOJ zrR(X)g*X)p-8z^(mg}HbTF@fing(yFy?;E(5_`&>NryW}bA__=m390{H;VJ5SV>=V zC9fxrjIERw!uB??21!oxye}%Mdax&b)H_yU(eA-lS@FJJZINBEMD*Z`)O-oAxPMqt zMwl6UUSD9_IIpSe=yjw2JN2cSzPHTmupn%ESGDYb(rf5v&AXA zMF+z6@`M1f)7qR#ZyEQxy}n4|5r4VRwb5@59q8!)C0c}{3*AthVy>X75N5tBI1tNM znQKm*A8QsVlQ1<8^iLJ^DA-g zO5N)xztqsVp4-V{MbC5DVt4ssTTSiNwkyb&Sv6om;&`J#L#NNt&=cOu5;U|l(4gYo4~8$6 zc8m{X(Z8%5+4S2Ql*9OUOUl2c7#^VH(}5oy`t}@19-U$wH#Lx)WKWYOLN~!yCYeW! zvLr7@5p6QvZOW(Z>&%T|rG2Nq==?6S`g+vFFVVKyCbXDH5`XVRt8camGOm_sT*z`V z{TNP{vibpqB_-)g+FHv>cH@XFT>lRJI+3w;Eg2dJCnOq^Qru51)SzMg@ zE0#}lItf^k$$w2i*TZhaq4|}laeCLt;>CI=;mrCPGh+%UN&^c0Qvq?T$TkRLs7BF1 z71LV9j8mq!Vk?)e&uHNTlZCf?x+^cQLr%)8tV4@Olol;3?Ut3Kc}k~WH8(Cp9k^gb zlZHT(qN(=)d%bJ83~f5k?RNYAS0XeCs8-UfwUSnym4Arz$Lw=Wg@Te@>#FtJsM8;G z@Wz2(hT2+|au$GGyEQ0K)@coB_Vq0R&4G*yiFZeOfsjaI*>n?s}3EaO;iV7VhMymsUO z`L{yy5`R)mKa)s}`Hklh#hvn}^bf@C4jkS?pG>FW&IHvt^ta>j&SkjcKgTly^m``8Kc@Aq|G( z$h2scO-eRzv9e7V#$VXp#LD8GrNVO#msfjx<9{e{8>-3h1g>#)!2EpDeJ{n@@i^18 zMG|V4P>2OUb31B>QSdbR5E>+elWmZENE0Byq*w`3*|=$oT-t=EJZ3TddM6(b`;5gS z-Ze8r-!*w?^)mbE5um7xa8ECrdY`w&$XvoNzY)Fj$CZ$#pBI`qNg(EmFq@i^G&d; z29?GmqpR|sHH)G}xcB0P6Je$F*)1kHX^j2(iy3hjFXHrZFG4pU)8tQR8h&~{#{L?! zXzp&+Vq3L%&O(+2W(Hr*{98;EMLFG1)_(%O%DFW2W1VT1^0FtP&J1Ga7H2@fW#}`( z8z=mTZA3`}V!l^ylV)3>9L|}>!)1g7-j>ruZcyR6^pb7tX#E+_PxtQ_JYH$?OOkLS z-Jb@J!uM zPsyoVJ<7BDV6m}@1X$~mqREq~W}SB8jIrcp_(PqW>Q|X_rU}{}EI2lii)*3nh{N?Al4^S+Eg7D1N|Z+vGxee=6W(yl~RrMH4vLKhQMud`qiA5~{&Il{gF$Hfe^(!TL%_Ps1WZ*xB zf6DT{<(lmZoAG>t2Ja*Bz`0CpnGQ1&t^5I`oNKOU6bh+eXd?dR`c4yAWq*hxx8doO zSC=Jw50=mBu97v0IU)6DvZ|p?q@Rc3hYyzXN}#aUK`pH*VO0ZvQOs^ivX)*&8mQyKBiVvXVkuz2%}=_a(I|%rc3rqTi@7 zXLTy2HO#q6xlm)y?YbPL>VK&GU|r4#2-m3+!?FWn(Kiyy9O`)EfY>2EEf@YR3DBtV z`~NDO>ea}}DBRYE9(B?r(=A;k?!T7qq{fORp_Lo9VyP~p#)@^*FMZh+>nay&AV8h9 ziOEgUDc?^HEmbNF!3xxJQD!hyIvn_)6}_*hdeGSd$s&g9N}g5gDSx)Y#C0W%l_oBI ztLROJ_i%Hp8w}}1BQcKp z@#5^@?DFHg{nOWtt*w+6_O`a3V~+eA7G`Zt9IP}7zwKG!f`5#i=^bh%@BF28YeS!~ z3cgl0CS4GGh-34__M*jS7undV?L+v)`f$J)K)%`56YD48e_LBoa7TywRY3DrayEXm ze##V{p^fq*Fm37eIDD3=lOs4~|qzbeiVY^_W+H($ib(!8*+Fb z$SAmVLV}1e_iqg1Ooz;T0B3=_MHLP}YCGU0J0dP4dPy0Gc(#5PVWQ$%Nk=;>Br;ql zCoNIg(}vGcxkb-V$$_d2L5~`PNRiB4K&Fep@uDe5!GDwRpWZ*cCy8o5!PQRM?K6>5 zO*ci&Kv4p)Zi*VYljmA1;E7x{3md0!w3Gm6XkP{_mA;fha_;b|10`gc_+w^&6z{yc z8Dj37Z6hD82*XdU^W!pi<&I`Tn9p6f>)xxS9PRIXMBQ5&Npmj-c{*SVey+$*va z_3lV!zJHSL#^hM3RV)X~D8g5p{C%$WY4@+M-MEevdwE)z9md6KBOt1)pZ*n&7Fn@$ z$NG}w%dbzuz0u6SJ%3C)*^4pyn{{kESl0(GqDA1t>f&b; zf1Ke{Y0pYTA6lR*?dMs}V>a`J-o6TsQ$z_ylSC?YLn=ix9f*;4RYuwGP!DwKc)$tf z*566+$+`^ef6=7FcNX=e$z?1p#UHJp8tP(#IShU7H(B~C4b9M?oDpEHnjwY=#6yaP z1b++sScK?GNiH3H5ria)(5;gFHN|j)BY4l>01LVyA%2gJX}OQc=Y{Kxortq5Dl+1y zFb~gmN7)Vo0{x$P+0Qmy>m<>B8*7~^DRPaWo^!{GHypaRXh>5rnsaY2->R`iHB6P$ zM8%W60A~ByncG-vaY-$uwzbs>Cd`a#y??ztu_kxGdohKc#V!GB>**Ar=YBw}2?2iP zqN0dczqWpR*08qXQ?z5<5LgXXVZwM+dpstXKa{D_;_0Br85(WJaAhxH4vj5p)SUwp zwCu@^4eaomShr3zvw+|hxX)Zqg?XAMHcWKkyLaf*4)fcBdCjRlZDvP!p<=^;On(Wy z>c_;oL2|b4CKe#vyM~dsP*Xt_aGq~@gxx;ym|~O3#J}aN%z$55s&#Hhkv#?^?^y2} zKU(i#F+0{V{$@wE3&-Xy{C*0@!``uehF?cE>I5D9W5@dIzn6aWL;Rn|*V3X$y_OtP?aPi;K;-|mD??2iBOBwMK?}&cr zgvp29;b16AN>ibFig~2maW&6F6S*g0r83A8uT8`AGOSkWZY$!;Ne?3SOs@}lZ)Jc~ z9rlEJ?uA>(aZkJn`3&8(e}ANZesnRR&C}R(7@Eb3s8-zfMSR*~C0QEYm0Rgf!NHZw ztLjmbDaFtnA66D2Nk8%A)8GGQwMs#XQtYi~hHl@agLFG`pnIoGg^ zc!-4H&hy)tjkaUNth>(qu!EXc;rdStR zHP#eGFU;Coes7xxlC2QvAfoiM^=vDA2!}Ww3u2vFmZ^#Gmd{yGZqOkr{n$6 zt`29o`=cug5XeSoi+b}kI?KnOnnW3-9Rd>AD_onlOcaL02MZSrS3MFdp9D#=B@=8t zSm<6Xt9JyJC?1{5Z25f}O2MyON z%H^xX0u|+%S{Rg=;!F@|9y+xG0!z=6RWMkRu)Y)u%|oPClBCK{w3?C>>&VJIZth8v z`nJNThMN*bHGB-CTAPV@_~2nwCL68xOy1O**QG3%`k2jVl7CZds;zAI;!jT5%z3iG$KuNYy)K#bxLQH?4ul^j{GAYg)Ay zt1`6CxVZSPtg1k6Ci3v}7fd3(XYLg5q<(ynaINaf$vK1a3Gyj%3 zvnX9S*5LPo`-0n&-4(8MM$@@p4l&p$x07AT3&Gg!M1M@5EBl&+u74X^6aUs@ZWTDc z*h%A#CS~(61NL}Ezh)TcK}8OkSX*{zC3m6q)bY6KEqun}oUWYfYxp7qOWj(~cVrRx zS2VI|EB4rkkz{h&R1-o67tA*(CRemkBn&uY2n&wdjSH1_n{1DdCR@2}WsSkNmka ze(#|HuqxX@joT{+KRQl`WBb+V{^i^K11o-$R&id%-A7xBd1!viQa;)OBI>drL?DgM zU?2!#EI_NcX?~SD95n|~%~kUQoPQi)2<-K z@zH80701mBj%!r>U1Py<16JL}V%nxi==3=fdcqr7f`pb

jFzelTnuk@@m-j4X@( zW#x#bFXY^%g~%(tDlt4jsiy-!I*4re(*(zoS9z{6j|!62Ha5goS$k?Fnt!(<+A5FR zrG1^bFRZlhG#LWFi7btR@tG^MZMF$5CX&QE(dwIRf<{^Jyz(@hehjBeMaGTnk`Ccz z#@3~^EF(Q=30hAdnu0+S@mGaKyraieb=He7pI4XofR+#aLN>M$IbWj0;jyFuoSf5- ziu5ck6e=L8Za>Zx*+t@N_kT<-kXBQMC!@s`O1fWWH4?Zalbe99huw!m^9!;6pe$Zo znOr!tzQ)X0iE>HL)vh^7)T!W#e3va9HvsbJ` zPRgsSLyJe07G2J_QEK}ZBSb@{jnLV+40Ygw5ltEbO^T-81MKy#;eRsPwRXF6|0@}2 zT(yD?F~(rCaO9qLX3*mFPf<6T-0>b_#Zo1 zScljm&F)=17^UYx(|^peGU&&KmA2ryTSD%zd~nw9p|J+n3q&2ooWxqTVSW^nXeeVp z3Yw|(P?FZo%y)vNiwJWmuW9$_9M;%6L5B$s8qA36?m#OcAo=&wxv^b@^;xo^!@vE_ zn#RxGvF*AvNAljDAMWj8&wG2GfBRJE`jBiHai_3H&XlJ8)_)0$)c40oRv0I}C~xZe z9I@#6z99A_@xAI4RWG9BEqE47mE|<$P`@WpmY11)nageL@kYeO&Q!7cR7g|2jGW)e z4kbp%yWYW?ximr2j=hY0xY#||y&al%@eK0`dXn708^$8mm7I-IIO1+)4sk_h_6?>d z<4nSz&oOOQntw~&3X&~qSX&2o)`U#$r5jmJWX(7f%Qen{_R|ewA6WJnVtqp2j|NtUQarGK&}@PBw*2g^sYBd}70%F4dAR_L_yOjE^I3@no(`^r5&VT` z({q3gV8M00FjbSq+Fan(|-mo`1det4%jIR(Ac*SS~ZB!?b?F)Ki#x zH{qjceyOBM<`0^xDsG?{It7X6kPMI}Zey9qtf&9UuV_kzJx`ly9NiaZ@?q3hBb5M`bVX@ztk zv44D43snakSSH9+qH7%~d`z8Fa9~}utvj}Dt7CVZ?%1|%n>*}uY}>YN+fF*R?bH7` zRkv={tlG2Y+Ry7@ePfI{z`*eIyC#TlsSlepjNiJUt))1K)v$A`(}2#&Vxe!fi@yV; zk3~L?%_r3Z2wJYE8`PDM90*UZ+B3q2aOXyEeOU<*v3P;aAD@#lg?i0TEE++RS~-{H z7)^c>Bca`X{d~XEpv1~?CNR|SB3nWMs7U?|L~#2yD6jUH(~L>z6t$`raiiV?4^PT` zm(K9LPd7uDe>%Cl{r!n+_0$YG<+1Isp?q;qVow@3^sd5vZgrYaDg<+yu--1Clj+!Q zoxZ7Fh$fb-bD!?XbDlWhXi4qEMV-cjqqGD%afe)&4_FrhRVVxj$vD3=x8 ziavjRO)Fg~cXw)M^1vQT&?7Rn$icrX{ufJm551gLzw8?rpPd3m!kKrNTH5Y}B^&M% zoO0q1Iyb(mEOt-zL1ERStm}LrGStgjWq))%M$IiYfb(RLek%AbQu4Kt)e%_C`&1Yc zB~+K)5=QwviyNbL6!q;^^uRp*bnLETd`{Zs*pGY9xnG-e#=;uAWgCIbe^^_X)KoQw;MMdf2H178K=cKQ6p`gzbrXPDKB4Ht_W05AF-+Eo58cqj-sF_#-%x72> zq`Sad@ASrFR!;h}1;zYI5jaCBsQY-Q@7s0t57otwj0hh%uDn_i@Wn{kr&fB7D&lovj^?updxH`$i`<`}wl z4rwu#Udr*22o;JT%y6on{UTgxk7{|Ns1uGScwl{es*k@!*?H{ZL?=TTjqjb@FvLLY=Pk6qtd+qMvIg8ZgfoXT_clPGpS{zM-) zW-~Vi!n)}fsWg(};5Zu}Maw>ecuOTn7pDt_wnSB$SE9X#I6h~E@!sBA-z#L^!LH@o z`fMSV}CVe7I_TXl6 zD`>H^E4Y7upMYk5p(?SoAQzDb#b<`I?Qyfz?K;%(`s$Bn)4Q5CCdTO9>(H3k8!!7s z$AzV%67!5D4) z{67b;ylGN(=2d0P9&OQ_Z2j&}4Ka(kOZ!3zsH@A|s<{Fc5>A0bk2lKOjz(%1Hj_5C z$#JgkF~+zfnWU0mZ=NCj&CSb67@lZ&50*zB@!FuRO-RW{HYfB#ETd$s9wz+9b1DD-+wGNJ&RBR7XceM;?2q13qS+QY(j)g`RuA0yU;8HY6`uSJT5J z5|+H}Qn%&~aQ_`pImE|xQh>^Jer%Nh&UP<4%(~;EwxmmY3RiRCY$|SvL$iuT^CNW3LMqu$VYux?zSkHKLcdFZrczZ5 zH11UfaR5PbHrQlRF6^{NFT(UO=#|W4m*aO^lch{*7lB+es$LtRfqD{abM>f_SyX7a zySu6f5gy!Cn%JN78ekaQr%!lC%fu=}@QiBivC%7#08ZynQ;2Qz@=qWuAr=wUMiR z5r?Vy-Jp7q;QlSp6quh}y>P)OFkUw=d~2z*;34OwAuS8sXwfrWN31NJw)_V5nMh=? z#_GG!WFD)Z@vZVjs-M^2N`?*!jft|X0P-C&^4o>d(b_GIs4Jaev>7k(8(qoL!vLMm zqKGBUh!!r26=!ciL&1>potT=yBWf1RB65%H>xc9$R~^bKss%UG@#ksO;0`F}UcDdK zjA%{X!ftKz!^YTTh8Wohk}}$+)K;xa!7MP6tzW{qd7=@j`29b z{Ecb}BK3{aI{qS<^>_Y(bft*kua-P>vn=e=P6=XdRm>C=^&BIn*Q-9283|2zOdM!H&MUDAB5eF z)#pr8N-;!{(E;j!{VzOblv^q732DdNoDB0gMlowyfm+rQVk~X+PrXr*L7ejp7{YNy zKiYJxFOE=~%z_MT;y#gY@D{j7z7x0ifT!Fq?lW@u_=g}LeRI#Ee6!t|XLgZ-@N>oh zzsbiyjCc&9+Onu1lysIQ-NjU1Ssj$DERmr(v_cwnu_;C%v{t1N2@a@DFx~jBh|-s> zP$xBV9Me(-IXiP%W5ZmIACIrs)pOO*%kc3nh>I(RsjSft4#eR`Ni{re7q1QRp+H@} zzf06%B5DciZ}#mK7n8qq84~4_K?)_bB;p|r4p`ZaWdmLM9Jn|U-5Q=TOAv%^`6zgE zpB9t}JK`X4X(b&rra91}4^UXaLhxZ_K{8hxf8Cn>dKdqb4ZU#p&}_^S*Xy4ZNS@v+ z>2!SFw7ziV`lH~>wXHMB6R{gPCOYHTHWK<6sl-f@48|Y{aSCw>_cvh^)WUWP=kYx7 zHaS(Le@uR7&4P`9eCTHAT||eZOHf1d$*!7~wOrqrRkIlKO3DAR@6R~2TjZPf`6pL$B~U9!MWHG`px%bdKNz@ql0WCG2~_su zq1XZxpZb>aH-fG>-K93+bZU$xD4*xV|5F~tNn%T)+j&U08h=M4DSuoQk5G<}$3}>Q zho@l6$bVLegfcQH+=X6h)u80!uoQU^aCqu)85pe(q$^XI~ zl`}Ju^#eMRxb?+kGfQie08V6mDF+60%U139vEty?`tXvDE$p6sFLrDyVO7pBUR)%V zGgYHthVH?|`tJ3*l4PlCmL7@mra#I3$Cy*dVX?{>QJPxVEi0LP?343#pbYn#B7bvZ!-c9{z^#7nm+xJUSbpviDTF7392l<33YD@X-fCr{zGXV zxPv)S2L+Ark2%3?X)F))6Z3JXyu)=@Mg9axvogDzOh%5qQlSDXA-4dhzN^^&6?ptMd&_Xr9SKk zGz3mUV|>)tgO=eg->>1qRE3V!McPcu{gI(5RL?4}7^T&ocNUO$V-{<*+f~S%1T8f< z>LRXr8f=wQILFS-z+~|2E-7e&>0M-Z1UN_R>xvRh=E<;<9)at}lsCP3p>&VSf7f}s zZhxL@NK3-~5tcgQ1|cnDfe0muLRh^49WMv^s&QXz9bVp?icB2VP7aGLUC}j5L z*pfu8pZVpE!u&j>uKN9pij*le@6PET1U?DSIGN@HSB{~ib&rOFs#*Dmk5xyWwx*3& z(TUS$34}c>F<|-m)x>571OCyf_OI)*e(wdpracjTOEl(27|P{vz`E_85jyLY%Wt+q zBbrjka|?YI%>;&f{iw0y%7N==hSw9w`&0ps?p#kT6?q#HaIEv&`oHr2C; z^VhFqkE3L?MSlbbU^!9~H zf-)$jmBa@sBO1{K7PK~XmkSOAvy>O^{Vc*d9Q4%;%_@pch__jTO-)RTv5+oAs2#zX zp^=cH9e_e@5C-FWC#|AuQc0&(LPxWd!f&p{SGHnJy*RCU;n%>hAlCAtx=LB`a+xh1 zu-mE0pE-P(=W>Z0YU47N6JubqB|c3^S8@TFT6@kdTtv1|AK8@PSrrR<7Ixu-jAoP^ zJ5Y!hO-<}tQ#MFl`V6AH>aVRX_iNm6_;$l<&4BM?zt*>vT2Z@-=AfQrE%pATyQ5E* z1_orh25!DHi#LxWoH_9`F6dt_yp=6x$e9vLsi)s$D0*=IdaQqJ44y;y-?g6qWDmsA zK|?YVsppdW(4V>m9XLQKCBt^*5K%}zq&%{K*noM1=PUb+>QV_^$GZ5r97+p?UQP9| zmHwB3V2%<#;;4!98DY4k+|?xq0r>-5YPzNbT}Bm_40Q%LQi!=lVzXKEejg=s#9N%hvo0~93^*`E@3bd0ODAu$CA}c6x8AOu>iSglm_(ns>l9M+aVZbjhyjxJy}q992TJrr9#_+u^}-QtTOf{d?m@72u^|3qSA;IjYU z^!bt(SBx-{EP4p#Znn#DPn>5dx#4#Th(R+MeeBEMrr4y4^W0h+VyG@qMO;HXKdV^J-oU|ct}c3U(bZ!4mgij%N#7&=H@R>>VQ?FQG-k5%wyw< zC}OQF+9{VIGu2DFk+pf=g>Td$!o5+^Qh~p4GTDH-STR10IP%0$t_hvK2DI*nEU19@ z+O~xR-@P>QBdhjwXkNr8Az4|SxK~mGwT(hOGOAj;i6T{Jem=k{grPG^8%dvov_{5L zhk}<6ZQ8$R>4o_<2(&p^Ck0Q}>(HvV;fm(gn>0M~i@63Z!*d0?ISE%u*t-e^luG5} zEE%!Rg>Bw>N1$h)g4N>$?78Qw=^`VwUre>y8 zoX*A6MB}-)eVv*JwNHVc4DE-aE~vDT1+U$9er>cg+m7c#0SfLl=eLoaNW~`_i@He; zg^_h!iUHYRJ+`Xe#T-;RM9_7-=>pH+b6knh^tc4s7Gbjx&q+yVhZlM!l-3=%tqo*k z5D=9#MNNdiVrhuLr4?03b-I4wP*j<{;?9ZkeyCQSjBk}9S#f-E)!5CnskR&hTlh(V z>aUMDZrK?!03Hd@=8vOM)SX_49JLv01#K(%;~sXI{?Ru}IiZg9MUI3wB@OHaUH}lW=@inEkuCO_h}mN8N_cw1RWzFg5o;ADHOu17ZF#A;Mtv z?_DvWn<25;#*n$@gxW%P>M108PIaHXq}^Nz<(+0bfToILJJlUGClNc+jqhg@8$}m4 zH}^~=$5tvAvHX${*x|L<^oV?^|u6xzl(=D(@#3B0)1 z5jIRn$nwQ@J$SA%#R41TU|aG$8O-b?jZ0R);`TiGC<}T6(_$Ln4Y?HMXVxfFV;T?^ zX>u!)0ojHKC)$}&d6QdB&dd=ztJMi~v)8TB0+Sy320qZZ%D!>C%plV3esq-9$j8(YHlp7h{+0|z^8^(?r${uAv@32 z1mrA>P;uVNRK40_ST{3u;5ajKIwlo7{Cx|a72#91xeW}yc(i;Wyj?>3@8S zHh7EP!v0s=C7hx@ROb2#xh|61>=KrfZ`Z!I^=#ETbS7d(GA=}~s7eozo&rzHCm$KB z$&DS^cOHE!_4-jpHJle?)rXmOm$MmwN0$i)Shl^~9_I$@nbRZEswAuX)Xw>hNYifk z`A*Qo)v8O|*D-gby1U@=$!GrAeSXa+bp?>$*Y<$WD(mKD<&=5pCaqmOo6gD*sILmK zW99JQ0Iu$yiyv~^7zDMl44DAo>7?wa9Dc9@`OOc@@Ct71K{EEe=QI0R7ciJkyqbGZ zDw${cE)xi{wn<{20&>})K;E}}bq^CV!i3^0b~5-hBSOr*CYv3at^j4bco>a|fH<>q z?!Nkg;D{M~*5|#N>ApJMh?;h=WRpfYG@>H0pgton)cfM*b*{(Ng!RVUm_9gccVan1 zyS-(Lz?2QPByc9F-7nSt0(5j_(l~af-Cp>h+2z9CwdQ!gbw5XK+Pyp?ZXKrB7dM8d zx6tx`)4Xj+rK3;C=V{LaPr%*x*%t$vx9H=&WK*&$gm+BY1w5sVqT?vIPa5A)2{lNc zx_*=&LY`hip;~#_+S$ZZ=PAk^idiVrdPVS{G(WR!B z!w87(shz}VZAvSMz#)n#NX*>4#k<0J<^%KNvfxP{5fem{J1&#`uw>%k-d~24g5|0VC`mkV4{rj z)?13M2A_Bs6NwnF;CM2w9zW)DRBIh*hOfQ_FOb9`Zdn`!24Xd=7bW-iznOPT2rs22 zys5bLtWo0m2wH=#omz_-)jA*X@V`*1p zk%v;tZ%rl#7!$PhL|!~}M?ibw!y6@<+~2NB^{)Kwn7P-aYt#xkt%ftP7%!1>A^8;G z47wex(An8Ers9+DZA>SE%1WAE$o*m&aVq*Uf=r|n6}`o2p}bt8y~rsk@tYdYJ^>+8 z{!s>_>J*QHI{&C@w06l7C-=gNM!WE!D>u7a%Cg1dmaN)oNY z1ebO?ciPlG!+Wqh7d*eM-T0Y?b#c2>4iG9kV!S# z+Tvddh-x2A$bAg|u`-I+`C{J1-R{?JywAkD__iihF136y+0{}hGLee1%BMIrzaIw^Q zk;DmK*-_3Yj~VtZ|F|XQbk%Hr>2qfeTrRRaooZT!z!GJNNnI?PlqSAh_%cgfRJA>i zCJibz_9GgM+KTN)y#BB<&_bjrZs(CT^*@XrPWhTqlPNfi^_? zd2R|-K{PWE1)nHgpg_Xq^Lm4A3Km`Q6*Dc1ZDr?w02DMOxP{6_c3at=6EE2rs%IZd;Cb7=!)aVd57ib()CH3TQG1N{EI(Rpm@c{ZHWDMU;nQ+{ps&;3hq zCS2(YW{V$L`OMc^&%#FX)=BX04zBjDFE_X6yI)}V@?T5y!;L2G@AR0I5dI0|Gj#38f zsb7MrlfrU`*%;uwo!I7jWAvP<@I5cGY;$)jv|~MSqHg3Hc+qP4^Pc5A-PLg;rw3mq zY#Zqb)?%g&-zvV8oyYel!_!J^(eb4>VZ-yyfNJ@WNtKm*eX#n_U49*w$T$!AGL0G~ zbT|lG?*{<`SaXB?e{m+paa5{}u^kN|FPY;CCUHz14ZQ$FS~%Y4)qrt6P*Hc%Of*oG zT>$a*Gz{R}hlyhub~sxXSpAt*al&}k+(YO8O9k#k$uZS z&PW6TBYFC7BseJ6=g0X0ToA`8@5%Vn22Th73-xK0YR$|`_#cB3j!}~546Q=?k1k<0 zvw*e6*}UIcv!2r)d0=Icw|#h}3~f%ZG?}st$qPV+JUK}sNuNE>noOiO{@EttGST{_ zIxRv=gY;f%@#s{Xl4h;w5sOUUcv@Ndj3WEPbZ4IuBT)p4) zSk|}`FSkXx`wy6PJB{RCNj=un!MAnWvX4tb0{uuRWSe%nOhF7tHCI1&QV5sLB&oOW z4x)fktZ-)_L?_IrwikDcDdqeCqnfFMhDTxPQT58S`V^_MRWi6liWeT7LgMo( z#y$g!ii68fH+BpZ2?5-!L>71n)X&O@+%-!-cDk}cL_KjVm#C{t47I|$DK;&o!Te@V zNf@EHMG5BAXy|T7kTxc+Qt*cu_~1C(taM<6wxl$v@~|_waM_!P>Z3yNbFstmA!LvH zP@@yBoWFdDS*kT5k8d27Yuh^M%m~S```6}+@|1`bp(E$+;=oe%phWgte)C$2r|-;# zZCHqJF@?)W`L#?4R3IAp7G`)v`Dc#VZrau^MswQ`Ml;(O1z$$_=MD?>A1sZ+o?zhT zGrh#T zMj`cO9gCvPz1imG-|vbW^UdhTG0+w~q1X-m5RnAFiC4xOj&-)|i*o+ebk8KFd!St= z==SXUoZ7`E_>K=XwMcEy2KXoPBeB3Mu7j}0!hHxnyHI`&^2MAh()hv^nJ>6{lEynU z^6>|hqIt4*fYp}sYv3CrXOYe|EW@hieD`*M8ty)koE}dE1?R^b1!1eL=&v{2;uN`V zJ=#C!AH8f?#+PCPiUSMxB5CA-?oudH7qtU+4P_)U??qZY<&3_pyh98B3Wz}nE465r zuG2a^yvA0#`+pTVydeE|zBuzwQNVEG4~UilM;K3 z>;gi14c+bUU=u;GkLUC<=nQhm;_t9jQT_ff$A{(KM%WcM1Fvm{-p9JuzcS=F1P?9l zQc32xBtn;NgFyjb&2+qLGFGwhh)N{F^f+C#M!sKS4dIC!1*^Ip*kE#`B)n+q$yQOa z7R{$3#T9-!6`b05)n6_JF!X&T3(l!}7av|fnoolbcz=US5P%X21)wT>i^y(W$drq% z+X^h@cLG#bO`?9o$!@o{<@K+p+#Yxc8YqVPdMAYfx*@>wBi8LIW^8yHTV<}mt>#Cg zS`w)Tr>Cqh=t2n?%4yz^wxyN}L0lO-duRRfUV7yLjLwC7D`d4bLh-m*w&bWcD}Q1? zVdZ;Jlz1h>8kkmQq(eFW2tJgmefhbR2y-ZrcKTayYU`eLfmY#|Kn>#}zxZ{))DIma zy$m}Vv|B*1_|oA*dgPdBCHe!*jdNKbhKq5e1AHD1816fB9DX&;{7UGu6~)L6;e~K4 zHpu|h=wCIP*IBtA?;5h@?L?6DhsTOIxo^iy7{k8w*PQ>l6)_HvghgQ6sIfItOII)! zBX{lU>p7gD7MaQt$oM6@HJpcf4Sd&YY(xE1&;VGgy?@ja#aP8qbcQNtNj8h>`ZrZ& z%Qga0O?t9g3UTzql(ZAe0b%vQ^b-r|2ZtHV+(m=F@*ea`8U>ht=15W6+AggN_bV8g z;V2G#>(xnx12+K5e}FlaM`T~mk8Et`c1e6Y;MAKt z1bPU8F#^ATo@?E5)}Ar?$)TEM%ew8?K+3;D%qv*c?HjgId83Oi$q-A?&PWjN%M)le zD9@ZzK|Fa1Na*8*d|Jz{LB}^$m7xTy*Cn?{m=4#hYe}z|wP+Zc+Px+O&&O&9P#$6h zD|eP>P-hFD9cGREN=J=&hvzExWvT1g00^TK2V9V!)esBFDmR5|3%tIBtKiH!bq!UG zM{bFulM49ulL^AH{xV|(su{2qwz;C6aDUybH6r1FfMKalf?3tD%wU0EUQa z-=e>ysM3G58ADA9eNp4>V}YXo6?S`o92Gk1_2F%5mM!AYdecwswVPFzjrWGhU|fWqwn&;3l^@S`B0!l$UrXGPN;vNQW(u zrN#cx7qz8DW%}VTl{aZ(f8&eG(ww4w723NqrfOiveNB3pWGIC2_6q5DA-`rd@Nqj~kTeF#55G=J-RmtD?e4}W4`%ifZJ zxE$53Up`RFQZuAQszQygo=hLiA$@j|mrogrO9_LTLi)Vgb+4|{15==9;!3iXH#FD| z;xKUP`9?V-r{8zS41c!}h6s|mRmcWIrEK8XPe%EbbAf!%WKebZWNo(|gxr5~de&O? zk9@&hbhW)OFQ}`0GB3GZLXi>8x0K9;>nf6(=6(;ID3QY#HXK4+NHx;Hc-4gWPpmz9 zbT%ES+4`}}iuD=21GFqg{3IFOpS?{13?%GD3joFuS(Rq>^y4>BcBONPUfUDEs=LUPhXJUKp&6 z>PF0NR~W53XgHseUK_ZOxY6zoflm*uEK${BK^rmwtf}vKRCglurbG(AW{D%)c(WBq z$qokW>|MBiZK&^dQPuIdmV8?g-6s7Wf)LX8t54I`%ap;(kZiwd1cvzHc^~@e7>t)Y zpq0S;*S5d^2xG3P#_Q%G;rq;3JSm4EW?Ips7n`2q3+CXYs#5cP%>Yt)5>is%*Z11R z>-r)Mz^`i;v-~R1rqSAIcR8}eVgZ&#LJ`eq45S zOUd921z#+z}vgvrUr*!@AHO4GDW8mF`X@Xkj4*$g=m%pRL#>gIhndLB&D!jcuN8RZ}&Y> z(I;cmotb}EK~*G~FL@DpPUTgxNR9kQg(numwTP zu;gx-OoTjtVs6gWsq7E)DZ~+}P)5|7jU+0ej%zj+Eix|uJ z9qJ;Ospx5_rL!mMTt$h88G$w)-ukSrJ3LHn@j%sWZmy1j(T0DTRJg8g(a_7^>JceI zX_~P^E`~4x#cIUBWS)8L)9&EKg^kg<+~tsOARYUkdI<7R)ihnJWKl36dtkMSv^6Vk zw@w<8Yf(iXikP$0`{y3!u53pEbZ>d(7t;T~Ijnxh`~ngbJ*2+dP9FpmBcz_hK^z1T z=id&ozrHPs8BtK*4ASD=99clILjubko68+RJYfFy&6M6C&g=g*8ET*#*Z(!1;$TSS zKnw$Xh5mYz&HvR?=YP#slTErF-raP?mZ@%8-3UV>H&y8ZswQ29B_2;=&H4w07&bJw zc=sw`leo7O;-YGXlBQ2 zo}AgviQ7zGN#)5@Mp|Pfnqgq9evnV=OoKC^cXsi9>ZBwN9!*l8h=D>EtKQ#MR?cmX zADfCrwj!ae+Qnbq}<1X!G_UzipTXE8AA1+9EPrLNR!<7 z5YRP7y;y36D?4TzM>A1w5Rsz~!c>E$Q!kf!tr+BL$#UM`O+ff%L|ZhwVB|CFB(yl6 zM?V%pLhVHO?uj#Nea!$e>7Rd)H=}IxQ|#OFY6^*cE4Fq{8dJuTyo?;Ap!k47AxC)p;?lad|xRNMJrG`oIB2ZfnhUD|isST4TS?L!m zEw-jljLerl6qS+5XZfETQXGpX?hbd!=x=x=l;d%cwxGWZ#y$YVdXZ3H4U$N{ZlQM*(z&7anre2RGiLcH-Lqao!;3=v@ox+8vAldelK^>PT7Iw#nD z1YFT4BI)H5vgZ^GEIxduuOUU~pxgNyeAL$5V0*2V6%BAKBF?J+J$(#mCI|hOXgj}w zkr_z;`MTEROub>kI{`dDiqW}%rT<^84% zcjDx{9zOogZ%9~Uirn@{gspLimG=wiW;DL2iA&WFbF0w<{E=_czU10u(!$b6#AvO{ z@yaL*l|42LOBzMoG-V2!TXZ2{rcQU_oWZ6rfY~n=VH{ZF`MBX*MiL?Jf4eqhAJ-6D z&7t4YqdkSk(@#nJpN&2J7c)-$)TpzdJq$9>0}r~Hlqf+GePUCIL*r?jii2+)b0nTP z{WB#s$iU#Q%97mbYMLBok*HgFF%y-+h2*kH(XW5 zeFJAiF_b(Q6<|=jg!p#mts8Y{>5AE6GZ#P*vC4%+l;Edf0TyjDsVn_scK2FY(Y2s9V1i>{1{oBI*MIxDscbzT|103jM=pr8KDwP%QeZCttDi8 z7(yBUFZFQd<^h>NT0J1kf~Fba-$V7Y8MQ}a+tG=r1Jz!UwG4Sz-FL%*V*{lNybK`z zksoF$F-OG3&Lcsf&g&@z`yXtuy*y|Bm)-3dl$H|(HJ!dB>4K;dF-wpz{kJjgW-Cj+ zw(2{M|GHpCDxa!QqSXYk&&ny`=-o- zAK|A*r0L9ZQdkI7lniF|44AnmL*#wC7}+l#9|)ql4y_HiIM^VVm*_QnCx%4V@7P#A zJLCeGO(Fv*m2!YO>DeMY7PP;=n{m0}(_e{*Ly#WX)+}Dz?pAVVv09oEeD3nq`BT<) zCG)=@hH~G$uZX^-{S?S$KcY`SOGi^GMXy;ZBh|ERVtwiD{PP0>6o^7P(uB`jTcgYn zDj^Y8Y^uewm66uRBHb|d*JGv4&2^tfk6lBYuyDXkB07NRHE(^cpoZ$Jn?m7 z1rWu6h(s+ZNY}bx~ISV`V z^8O<%uvQDmYzz^f3l`G_gza@RDRl4c7pW>+@$^ubk0Cwl%6HP=B1Bo8l8O!l;Vl_0 z%Y z1Kw}OuIN^zjV`&$g4jjLpwoe}}+< z6t4p`+@!Are=f_iTG*Or1lJe*W5vZ%s(1T@;oL*|qT=lD9zH^*?;@3yK@{2z)1TZrrX}ccyS|Hn8VD<6Y<&&qr2f7hf6lD^`2P z#uicY^%{EWkGjhz(-U7qP(nSCGL^Me;B=eEe>NX;^Ic{64Q)l!X?u`uGWY;ja4F@n~FsT@{u|H>T0@}0=rI|c%Xo?LrO!>X^F3pCco^h2`j*phG+NOFV8$};%V9?om_5K`g&o4kq)!AqU+KdBblf>ZHN8C2s z&OEHyc3#qWKVXY9o?`v9WVE&UJnc6$*DhbEO}~G*zCX6z_rl<=A(SU8K<%vdC0O^b zWYh~C>w=(z<#xdor}+-)ERf1nQTzf5QNRu#$X8@yqF;1D&DBNYcT%MI%)~(IvkmRWGZJ*FZ7z_)HL2 zs| z`TY-8NKSo6rZsTrMv7$3^GBZ->5e#B^*1lE^cjmLP1x*uZA^=!e!;mzN5B<=MX69Ir z9dXW55`8(n$$9oQWcbgf>V?^%nmGiGo+zi-+hqi2hmTrsA$cGxa$yBpTvU;8?%?w( z5l)z@af9wfH03oovRI7Q;c7Klie-sv#_ZE4y&6H{B?!ol%g~#8ReV3qu9jd~X@g$g>c(W)`)7Mz;H9S zS-OsOme&h*x#}gKkZQ1V;t?Df0rr~4Z_QphO()n;aQMp_>mZ|WMJP9oaZ3N{n?(%l z)&&)bI*%{stzbur_gIxvVDseygd8syMR2k|C^!MIH8lz;=GpK>ful3$ za~BTp`MIQhZ14A8b0!%XRGiyU_mQh}L04jTdClIsp?nkqNjNmm+hLm^if3u~5A+0v z;eGb(tPT%w>Wkk^j5~lWB}9Kaj)goh$wqm4%rZ6%z%UPmtdZhtGk-gao^LMq1s!60&Py zPAlC^i|eed1uf^RAE5Sz{T2_(W z8BuF!C4qW3+}ZPumt_n)wr%}tcTAMS$5?6fSf9tJKUv6jUeuD@y6=;GoNPG;h7-+n zE#=Dt==x-9DxfqsT(1=*_R#Sue>{1m!h+Jkd_T6jxs>IDz77$nQvj>Y`4fw#oYn== z;M%R>Q-4Ke*%c@}!FNFJ-SiCZ+cQbsQ5V+!hnG{D`i~QISi7Qs-EGyL;taJi1{>#r z<2zH&Gq!KOM#pd|%gh=7 z!+5Yl?4taDQI7i zyJn3d{+WAp?ylulr34Ziv5S;_09qe$XZ{BtCQ$aDu~`cIbZ|#7y2A!arLsr^SFMfq zeei!sI;ZGBx~^Ntwr$(CZQHifLB;CWPRF**j?qcFW7{^*`=9S}j$LDox~Pj9bFXL4 zW5j|C&Y@0FnP!2Cwf>BBC5kqedlD{U8cGM zg7#Ku>yjf_fY%s>Y>WO4FVL>uOkb;#a(edk9C!fRM4Dp{pR#V@Y?voENk*8pq_p?I z6AnF;FhQ_uzxZz%pb@?r`I0F82iUliQLwuE@Mk4cw(N0?o;hT5D zo!Gu5=c~o&x=Bl7o}a*wMXKJOErptgQ={Q>nFkQZ^?(15B9w|!Tp9MWRpEIDtG zMa&i-MHQ4QOtS4mgT#nvMevav!qu|>vIOfH)oe!MsooFgIJEn7>kbJy=%4QX*lXRT zCjf-MavN?c(--&>%9=h?My5h!q-HS5p}a+*hf64S_kdJZ3-)Y(;Eo+0399meF-<@b z3iR2gLp9=iaXlj$0N@YWDT$w=iX!&xX)5ggW&wd8l(a&=18*zR8mRta_s7a~pbUzb zt+;{DtAEJ~i zlp(vCNQw}GH`#!f8WLhXgo8oeJ*y-n4x<%_2nId5&y;Q^ zrsL7#8IcX%gNAnKJOek7niT!S6*pUho7(bT&NyPnNxj)Zdr;_=r0CT|*bu?}KFi!D z?x?c<4Ewx9)bstW3G8)T1nljA3D|Gi^pz+M8!Ux=O3UEFMIH^eC={||q;I2I-{8;* zeL_PhU%jNbRozl(CR{*j9rKw|*&#zqeo4lq$pVMD_ZVPHRCK=<^0^Ly$wzP89lpj0M?I`|c7tdnYaVwGYHNsrNbBQP~`H_OkbP zc1$$KwU)JkCdc*+Y4BAH0{i*?qcV!_dY9rqb8dG+B(J%4+kSL+*_~zBGSTg<^9laK zWWZiyWa7owt3#8+H72A#b72fUQNtm3HG&of?Vb9|&!kf=MV}i#)CnJBaKBSe`E}8pE|#2ZyPTzI%M&$Ua6ycg z`zIEsCK5urq5|*C_6t298zH1NZjYd`+Gt}W@t{g{eV{2_V9F6A)we$lME;MK+-t%) zf6|ogdP@Z~GF9D!1aQAsjQpz@=())n)R0eGXRgmDrppU!`YaXewrgWTgmq81wm6?S znbm5N!fmgc|2Hxa`N;>5d%H(CT#~`$&}?6*e)T-^rMPo$a)-!&9yNDR)N4TL_tYD} zU^miD6jH7BOCljk&L=#x&zP1HYS-r4T@*|^1$0krkrpq029Wy{B3LA6P-wa0cO2AX z5GZ@o#b{eckSn0*FJ zsn8VLU#CJ$ytADBpNPDyQczaf)QO)ya2>q=;VD}!Mw>1#z^dS|?XMnaj1U9TD?cUQ1Wc}I@MsSbLFUW{>U?Hj zlN>!xMZbKj%^b0e9;eAxo6ykExPV33ZzfpS7RVUK@9rYFYua;?Q-KwhNPo*+NN;Q2 zXf|3M4CONouAhE5u5)7J#%;%Xp@9Y1&#=a@RLJ8+GZi4uD#yI)*hdOy1WNL4Hy({3 zYv}MZoUtTB;87TuG0K>4Byj)oPDh%+brhh8W2rX|Y%ky*Bo3yNu`6b1nMHF#;Bu); z0Ulz8?X;Q#D|tBq9JRhP-q(b(T4;wWM1;+8gKhr2k?f=$k~JEmU@(VzdZY7DgIU=p zlbURcV$3{9^bQ<3ISfrsK(=K?5xL*-ALr*i)-7hFCb4_MoE0)_QS+z)wFItl>~Gk3 zJ1Ad1;cMHm)y%)IGILNj1SIr)-PXjA0MdDchdZOTYaRBXdnvzSby}=TOY4+zx2pAa zP3+}CaUXx+BL32@1qdci5QGye3erY8bS-}=80Km#F zV#U=)_AY0_GEvxMI?N0cPPPe{iStdc%3iyRtZ*M+x(>b?i3I$J~exubP0W}J{hiOc{>Z}q@A#=QQPyJw1lW< z4mSc$xN5NweHhv2s1yG)^w;uyYv~*%27t4-Xalhx9Y*LT+n!!|a6El##>WPvrV24x1CMsk%b(cCqqRF??ffx;m*X*z);}+(~=|bnjZCYu7z24Z-eRYm{ zK%7G|V|rTxL>X#6zraP|x_|P}-9e+s`7do}+SthLJJk1>**mK&@>3b!bif07k3EXL z>Z;_H=Q6{$=ynh)WZ#QtZ010iVGqWCxmFB*4RE^a<4|<=kJ^p2<;rXK>m?4e89cAD>%A0Db$hUaDuIKIOcH|0FB;za68RtcP;pu(>(gE0F27Z!q}4n_)yiP{HBDnc%HYUH_}s zU(7#4IAu%yxzxNSh5Jx&=ZY2lovQlnm)%Oe@((X}L5UVAuv?9ZNTs?66R z1k_hWuvA!-9K926a^CQdEiC-1|EL3mdJY46{l~aKKhJzvzTyONpX*F4wH^mKtG~aX z8WGEF8cY}&sx9yVx$N`c*qywCWRH*5Xdf7taLC%@nd)~wuSOASxCy$e)1-aMXneB4 zt9&bz_3JWu$q=4+>_zD91a}>G5)qeuaZPOJ=H(*8Va5eQ(G-uq^eg`qhm{L*&|sAmFXnQb`?rb$FkA_%c(1f-9}Y z-=$IO`JU!PJtGd`*6OEIAIW2Y<+rPv0%F4nj`Jqo@O<9Q^l9>&W)6DBeF8mCa(V!g zswJ2x$XybkULbDQ|0DS{xSSbP6^0wD%!>nE>p1+F6JEz5IL`0%WoJJJ!SnHp)gJ1< zpT$^k3PtAyF>gNYe`m;ix6zQYIL(StY)f&|nQxZbk4R=b|+uJRvx{{p&SxWWQB?RB^lLU;tI72cc!&S!H*-OWM9u}j$v0W)x zVq6`MvaYmoH=fqy`77kwMIZ@E2OnR6HIUmLpc-?X9vjrtw3K(7R$@sZZD~(mJ=^eIn=_>?j8jfLN#wO4 z5UTCYvESG+WQo&CIH!tkEaV9oLA=Wfu-4q`)_+K-`Lk!tn)(FM{#tZ$dt@Aupy^f- z%=Oh!vk#aVT|h;lMm3(s^10sB5u5ZA$yauUBy@E9H9^9ZG#-3c^z9o0B0d0l9PiAH zVYrWYq{e!7;LiM)m#0gE+T`yEe$T%bfVumggGg+=lOL8gO1|3j5nt>(l=^~FRsYg> zWD^~w$h!L`IjybRnL8^obh?(54yH`m)ny}qZj0JxBF_RKu`)M7UvE={+I5iS8kh+2 zk7U9bAtYCEWBG$R0vb>vq|lWRxX%c@V-1AL1idiQ48tP{neiD=u&X?=9y}-jdQTjM zMFC|jW^D7@n*R`vk2tK{e>(m}9USOybM$Wf2|PvLDha{lv5pR2uC%sdhA>cWHF0hO zq6?Ty`t0japA6&N?)=_q-fLNf^ESIndUBG*n9T_4dtgv6Q19Zbm4_3$bLnPa_ePRF z{~+h6O7@h1-i?`287GFYUiw6!%UmDEIONAMItFSr(IJ%!Gb^qX(^VR})7D%H=b5(!8+fS zsr9-W>}9d#ykkax2^!@}i>52p*wwO5&w<9LG^||ZuNTwF? zuY_y!my2r@M`=pwgEAt#BVz1y%?ne9Puc7YYn=a#)ot^B(y-`U-Y@FLb3_~!ll)Wv z?}U;QWVbNQ&T#F$+uDK}y0vgK*&=RbYU8dd?eUR$MK#H#^L{QMJYmp_AYn5l*V3!I zxC1>#9v<^v0B?3$1}NyPs~bVOZiF+VO#_23sRv2>S(_E6+Oq)BT+8|`jo=$pyu3M9 zqM)dn0geknkHMgDn=kE%wgv3S11f7{no98-8Mda zzh>|^Q9jM^!-XIcr|oE%!ByESqQQau-S$T_JtwamS&3hp0an}X<>@%-QibJif&m9i zn+<-97V3d~OI@2tT{x}p9;&XVO5>be<@Xl5*gyD;g9av` zKL`0E9K)|ef1b6R-ci6pVUxzP*}bZTV+)>4mw^SHK+zzf8rcS1olRkwy7|CtC~6)9 zp4r4f4Y)bAj9{@PraMDO8&m^2Y1#{?d!#b?2lx&vGR$8GG zrK{PftlqRJ+oCkYOtUPSjyI^&Z{f|mlTVt)$7>-C=)BpIjX56Rf`r}af8NdRedLs= zC<)We5WxTMBjJ5rRG!Qo+hk_v>;FMME@CUWnx z*$j(Hm`0(w%kXf?9iv-6yLo{oKr`7ad9yv;Z8Fo6@{e79lKf_ClTL9rgaMRxys=G_ z%;Z+`FQO@Ut3T&qBQ6XGo}hKMf~?M=-Y;}Up5VkwH6S)IBo4eu+v#TM zuni(X75E%10Q-2vJ+KnB_KC|E2KkU2AWlMrLE9d=F4>XQyW^r#l8Kk*$L0@*iQ{dD zBe}0+dYSMd#J)|6QgcS658bShJ+jVesqoo2v|8}B2OJBK2-WGbLQ@@3I)vi??5b@< zB4Kvy7UC%kL2JPUM_tXUVVH^S+zc?rZmgDf0B460G*!BiU@#4aaKnD|h*ei;K-`l% z3h^EI0Sk_;glPg;_I0Efr2bTrKDlz5%7}#nS5SG7EHyD20x|ip8xC!}O)+m1{!F|@ zq3z$n99FzL(~N7bQkCn{m1Nq;VYVLJ`g2sbD&pe}W1e8u(6Qzne`Ui{7f86Ybh^no z0P5^ft9?dU1ou}h{2l9$)vJLqDi8m< z6Eo?7bS=s*pKyYAa?{_fYshDNY(V@bZMKP2e>;v5_DKBxzF!&7NSHF!P+WA&&s{1w z%3^22TP3^vI7nzHNw9GI)64M##9F93CYe zWEsdr5G-1F?G)e|+Un7!r+A~4HP*EG~F89Wy>u-yaR zbYG|7+J9D8DzIXFE9d)`27x&ykwM?Yn`{`MI~Y!`)$v9KU~eS_u^DMZ5H!GxMf}35 z$dB)G#e{x@&*MURr6nEe-yI1A^`{T|T;FdA$m6uy6O?if8zg3y7`d9XuX%CgkL3&P z5b;8ZaDBVO>|lMTLTWI^JLz`nl$ktZJqbb{VcTdN?W2ofi=o#Ke}$BWKhoi<) z8#u+#dcajoUL^NxhDrFlgs-BfZevXUSHyT7#$HIdcP0M(e6m+ugxnAOxVJZVmz**d zc`iB*&dFL$W{JeKe1c=Ah{PZ9camIleO*k?9I7RZ#Y7m+*~{%Yju?O((fBXG;X(Pu zI_Vp4h+n4UUE6>T$FvJkZ8kPY1TW~xX4!{xoy?SlKtWPcy7Pgfzpq#O1uU1&ABDnx zI=Hb(O?p*;4_r=TDJB58v(HfmyM-Rs#gC@q07X5GWbV<~Bz`*yU@=U`f1jq{RXuQ_ z8#+G_U#WymHX=9zzuos!tY^L;_oJ*TQMK{67|^+MWz({nd}3fJWX;z&z^YkS^V#33 zeB?H=!B-VS>M#g*M#m{Gu@gKtY%}q*b`D^fy4dx6CL>Q9hKNAl>LfK1phi8CsiCta zKM)*ZtK`j?CV`*;faN02%pGU}jOagx+9pgX7?wT3!W5OZJG9uWB_Z{#e?qU7k^hUv zBgQl`a!^EOytT(!)=1PEH#6lAORN#PF}V;{l;cf)Kc|PDT#+2@t(?eZI*M$CoUatR zTd7~WNyCelHT@d@9hzCGvl&I|a=3+}P~V7`9x6NYug8`h_}iko+oh=oi~10*zKdgZ zQZ!heTZf;370Upb7hZOGSozu@F6n?H#E)7|9|N~`3(kw=X;cHp@KBh(;Fri#=YU0c z34;v*MZlicaf+9*m4w=Oq?G4}MrMhM|K@MOiNfqoLpb*nMa}&>Tt6-b*0+g-Z=))N z5BmvnI=)64m?Yn8N^hL{s^;NbiZZZs#2amT@T0&zCqpw`(7iud!Vdsb{(f&F>7B|& zb+D_jLU=M-fbo<2W>HbX_lkm1cV^g@sDg2Ou(B$*hgcQLA68&XXB*u^l!=P9P&fXgeG`_NKK-l4lCyi(^l0t#ejzcU+ zJaI^$AYBjm00QPhmR*9NHHFA9u%*+8&tctW!Q9bHMydP=&s&NR3mE_Inv@>F-rJ1C zJz%X?N#67@X)_I0VSnq2&!A+S9OAZ^P&xhfVbfJsHInB>%u54QR$Q+_Wn^?NA) z;MBeeIF2)b@Fl0hxKj+7<>j!{hLA^|CX$&NPT(DsBPww8_A=zyMkgOz7$jdyGp45) zr9u?ZFTB9(%TsyrH4C=NfVhmuJKH%O1u;WvwP`CnO#OBx)84j_C~%f5<5XkK;%+{& z#I{;5fjj*{?OWd(9&IdK5|X6LdHTdprjMin^p}!lz@TA=#Q^=mRi}|k}iuTfP;u5Pp)4H$0@i92tr(vsJN6y_fjw_K(GwiHL z*H}SOaK!SjRYpMV1P6;_+{ajY_`aSKzREDG@B8+q<@UUEUOBWlD>gnb;XTrsQ8Ni& zO-)3n@f&>fpegmQ0Q0Jx*+N$FAJ4zIXapaM@p#sC7< z2v0#;3)&4HJ4j_6MlG7-T>4c)Cmk^CQ#|69;99&5>9BZZ80Ev#O4I#NNZFkk#m~g+ zH6e#W$_WMIN^}0aWasK-B|U$@C%vNjph3{TBeOAY1b5aw6 zmFHMcI>~cj*UPZ3JRx+jY;w7d4>1Dx8P^mONq0f1*PL-pDZC>=cyZ;C zG30!C6%01Y7%f;Yf;zlF(c$g_s79AIsZ`3h8Q%{IO4?~v?0Y2!xqejqcVT;t`ESZ1 z;&}Y~?7kf7hd;-$o>1bh{MJCzczyv8}PC{cSII*K{7xD%&KC_R@KKg4vqQ zVnt>}H=+Y_X&0qS{{rUEzq}JgMtB)c!ldLj{5km4%IWkAt#1{#hpf2W$|)Cjd>-f5 z3$G({sT8&LJDTAW#)RP_Q_Hx4!^PRZeRU?eM9E%@ZBVX{l=OZ@fUkZ?^Tj(x#ll#X z#6e*}d_Jya@);ReE0vHb>r1&E&9ls9AMRBjqJAME1l5o`eEDNCc$XY~)19&1`G)3< zm1<}F!$+$;{546^NkG0A58%%5PT-6bmaad3IUjzE?;V7w3*`|SG+6zfYxRT_+E`ODL z7qRK*gm<4-^*0vGn_uajR#bzNcYU4W9n%VT7cc0;Qg8xQ%W2N&c=k5w!c!c z9q?NbBQ+b880&IC2#QOfrLl0Gy+J3Wu_iqLSCZk27f9VB`KfE$(q2aNfM%hk__0T8 zvtD$K%Kl8rGXlAb+Zht(jghWDoTlRGGqS&|)pZ8oH`kLps0k!|brk46YsOZI`}U7r zn<-L5?;!Cij^2XZ#EodU7h^k5xGJN}7pk0Rg%dSrWC=leDB54KoXLhF@s4wubb)x>@-k4hJI-A`XD2`hZap{oyZdqYrAJN2dk?JxR0*Am*2lY{8pE{w=?`73T}T40+!ms#20`B zaot{?Wi~?}F^GMb-_=S;am1%^tMOMs>@e{9R%5fqunn@7rz!I5NsIhxZVay6abNrW z+1@kHduvYmMVo)8{mY@whwaZC_7xqh)12h>zJh(z&&wL%g_|bfnO?|PoaF175jgHV zKE*xi%{Tg}R(bJOmzcksGNlIW(unilenXs}+Qo5cHE;MxCS2Y zuAWb^Wn{6?=Qx_x4y3aM#g=8+ipxnh~)-l^L@f-)LDR#4nObmadhD8JLO_2GkZvNx#CNjK;|B2M$ z?q!lY=SsN9-pGyc;<-~+p8EtW4cfzkAN-SR*3HnMysWX~eCs-*YIN?7khfIT9ALBK zAsHkb!DJ_yh^s|S=FV{zt^*3i(JSMXC}Zpsj}G(G)!bJ(lHCmy1$A%+3I1D80eP9l zpj2G_iC?9ASl-5y-7=tz^pphyqq`K_3C#xos!JI3)$SAzX?C z_p@=cKe5%wr79p zv+kz(JMX`rX3o_Ra;^yx?9}cU(W~V*&8~xiWb+Xo-U7E@&nz+hpwHH_?vK{n=1M#8 zd*2~5+n~tkNMH~appXy!L5d&W(E77$TWd3q->kK^*BujK4~u}c&4z6Sy5bU5Oi_r! zejklWm6{#1Oc;b6318TA%gcQdeE~VyhMK80{fAe;-$DA99}drhzNLh@W$k(rp>*ZM zA=5OKvp33o6gS7053hk`1BprhW$lI=_60Z4Gh7TXnUPX@sm0|^f#+;=N*Q{oGzWxR zqaf%iyeANo(xw7;bRI;Ae&6zQF*CAx#(rpa0!=X6pGpx%f;CMbMDo$>`PY?9+nswo zSlK?~8vj_AVq&4lws%-ynZAFVqGdUl7r(orqS$RW`kv`B-mR-P8g6}!(aSa!95{2z z{ozpyl9Li^9QUa6)~y>Z9L`!$a4%95klc!;WOnA|Pt*p^9)yW|WiL^trYXg3300QPa#rla}Wq#9Y;a%8snKaGw)s7^}sf&ywv9% zV9_*Ekseq*8xga><^By9kwE&=QrKN)EGYvg`cuq)(22AT8lqj=bJ~?twL`FVDq})4 zDrDrDvmgrgHXqg=JtNVBt2AbS32R3yhV%nz^z-<7i}z-g@p5vbYF|VU<_h2l6Tx)q z8QlvL^rS2~IO}EEiSY3x&e#uo*3uy}k+;Tt@CWR6qUuwtOyIXYxVYN$jU6D)EtI}E ztkrN2M^VbHBN$BI#QSDj+JA)aRA-hLbFl?eaxLq0Y8Gs5!$jdRGT3E299R(C@(^-B zg*stS?S&MiKP;i{a~(|Jb$CVK!X?4f3~y_l=bi@~^zs}BUEPpRO9y>2CPDvY0vL2> z0HC`O7qv_T=h`pmh~MI(*$wOQYT?Y&fAAx-+S%ShY55vS61Nu)bB;c#%zs+J0wxyT zkp>2G4<%wcPHd83CW@Fy5CVA~zUO69U(YnYpI7yY$sFOeI5*PYE^0#fgS(ls_MLA% z%XoiKJ&xMs+*f;BB5>%OneK!Nr0(y)E5P`IA&@ZFW4-Ws!d03OYM{TmLYg)Dx?)|X zRY>g-*q^{mznjDSy-t4ctyw=)CZCFg0dsW+qP+3;Zz#&@&7dp1YpcZA8X`O*JVIPUU=JWvrbGGOZ{)y#9{(o(oOcV> zdHlX>5G|yc!#<%of6a91B%7V2qr(W*FBX|Ay*ekJ3lE{}4r%R3H%OU)aNg03CjE&U_ z3TPL)0C3ot`)~mA>g_zXF-G3w?0Hs5@fGOqf&g+NRi8KZg~nl(o!0@x?pn9%0BU1i+cCo^cTY`x#8@&iK6+V=%B5Or$CdeDw&|7~HS>)lxeSFj$;Dl7eIv`LbU= zDeUc6=)OKLGy2oc8dSul%x}`ZlW|NZta^jLo;M+m} z6l4MWj+{R^d&a+(R-(Q62Kg2S8ED^RhxihP3Il->dF^lCb%%fyfpYnuxM==Ap$CF3 za?;=ad=Eju_j0n`h{{*SW|B2223Femn6MZ0{B47RG zf)D=R->>0crjY*BcPK{oF&^SMYoj-o!7cyGL?E z9tl^Nqil#2lnQnl$34v8KhrROJF7YF5U6B@sRqQi=K9tohAgRwZeDqDU{aDx$e#caWB8T z0ZBJO0n7g1dJ3J5^@eNKr}S~)S&JiPJa}{=KCfo$&E_o{Mf?%64WkW73R?UJgA@^W zw>b_!2Tk1_DrDw-Y4+Oh{>Wo`n-CWB)=15~YG#G+!u_IA?d#?=FAHob{Kb`}z+`aH z&gETUSOTJI;~NJJjoD;&DFb6DyJ9NQ1MEk)grQ>P$I>=?ZU#1|(+MNVAg(+EmgemHWBtFhc3s#$^QuF`;-H6zCHx)7DT=EW1I0We0G z_Pkt6lPx@1bxMNN9*>r^oa#&3$K#ti`EzBnVzA`LkC5z{V8fQNciiIM{x+-#DP26z z*2fP$^^PNWb3=L;q)$fQN=@AdIxB@JJ@y5qiqe7phKb0dmrAoGzmROZ)SKB?aZH+I zKrL6XRk^z_^{QMm`x$FP-^}u*2S{Bnt8KGVowU4q|E}TRTJ3fIgFe3wjqA7RmQN+fJUufLF8AkP)zEeOZ!bJL?V?P>Fehs`HF#Y>?DbzOoMpN5BF zoU2%yHeYQoW#vRpFemul!>0ITG)ioCoDTVxqdvB1yG@X*uYPl#1AEIefA9z%-7cEt zZbBOwgCxV&&%mv57W%W$*cmu2rJkPg+={Dy15J3p!y=j^*+fnD{T@Fi%GNA8mZGN+ zP!U5_7qws7lO(lZg|!d!NfN)gdPny#3lfMRvQFT{TYD!(EB4GNwqa4!_5gl=Lje=4>2bCpPxv5XMkstV2jESp!-gw1$ zX$`3I0CB;a`CS&*Bk5jcuzor8@O_-uZo4qOXged(Gzojv!hAKaKQ;5p=~T-I1%%l2 z8NJZq8I@Ahfq;DC>w`4im%J+BWHJeR7wyc>&a*Hr;_i`6SXgQ+VNvk0i* z^N>ATR4_k68pU6erZ9OdADh3<1{TDkuA!k$WhWM#;H3uhofnI2g!`TRB>$0siSLaq z@pBjcdh!!~pL`yuQltP%u%2JY+|T*PRD4xv?pJ6W0Zs8g=Ae=573s00NaPNK>U6Y4WN) zB4pb^0mPAqdCT}*ehC|t(936n-7sRHLwcU+EBn)1nqZw^EMs6m{4kwdwHQS?x-WkV zJwXwm%{DKJHk2nlDeiMxUJNqvdc>Df_MrUw;Fge<`!5V{BgbbZUQmyO+WQ0B(iuXL^*_WS zZVF=(HhST587s~j=#sw7ZU}5oFKT}mkUOuC|Iaf%4n%Cjsw1USHI{FaMal4Y9KTWBw{cqlf7hdO%v{j|$k zGxS*(4Y@G$doy#$Gval(J7M0rSy8tDy?AhBsdT%RdAw=@mrmaR&H0;Qw#?$)!US>y z0k>y$jwjg!>*MXxG&!}T2TPi^MKs(;0*8Rbrf;p(V+z6^7SSZ zFA#&l|Jg!%oj`x{ke;1~7DDVnqucdco291-5C0fcwoks*baQ>P?dHNsFkgj@$E-8c zbpALC*eZZmd!t$M_j~^sqFMPxKmp-5itb?mdW<(|+DyxzljLNg6fv3<52ReyWs~9c z<$V7-6f?25g4pN;C+4Toz?ay-eh7LX#Y=Wx=)5TyXWAU)@C>~fC zd*Qkq_+NjeT)!8uD2QZCA@I9~uPqQO6RLfRk7c&N7n(e_QL|*&Me%s|Dfbouy(qNC zXpoTv^yRPh=f6eP#-9yp>SG8^^+XlYBTUj6n(rS|5*U76|;wAIb!ib#%Mk?Mxjn@}b*vaD;MqYe4NeYe4y1;ekcQ!LeN zfhc2SSklyle33(;2W(F6KG=i6m#8qlMQ-6yR{XW77)#9G-enr@ zKlwNsBv7i?$&e4aVNtzQn1r((piU=v3SEN7S}|THYZ^Y`20!Q?xO`p}zq57S^pIK< zt*z_StO*Gt8v?he{4#em?E?K|&$yMIZunf_0Zdz~HOb%9O906hxBOq23;fOiH8NiNi48Y zgc5?NWg(ec2j1zgs#oH5mcpv`*wZd^Z_+X|u`I75)ECU5W(5#2_TBq+B`#XENCt27 z9orZZLdVQj$IxaJ5Bll$W{RrsedBa##9gniL5cU1x~P^KJ?AJhMeugfD@}u8yI*=n z(PBUM+p$6r9O81m)O0mHuDDvs=;A(Ni~|w!aDi@K0Ab;yMqU4cP$b)SyyVq)TE3E( zCrRFKA*ZyAM5l?~@P(t7d{_xHosl|7Ca9eFhU!$&{ogjVoA_tZ?jZAO>+9vX7h-bi z@M*UsJv{J$(l8w*yN^HiWi2S05hXCC zr2?%Uz_D84xTQ^VvM8HF?MFgt$y-CaNZr=oDSK0LiO?79@h~~GVDb6)J67kt5PP;e za7C$JlF?YfBw>m)lmr|F+M2!e+G>s6ru*cY*2R*N=087wOB)oogTgkHRzZ`Dij@~T zCOS&n;!`Wel#61#`fRw)n`?{qlYT88al`#p2ZC%~tU(?EuNT1^cM^joD)dso+CN{? za(`A8lh)Lw+Cmzopc;escI(I}x*;8vlk{P?%{Gqo^cUPE2Z8`2@^uj%!xvn&TA}%H zL=I#H*;av`$$qyS(uutpgX;D_-Hc}KhC%hV+rIsSi~`P%BShY%xV@_UeZ!B3Y8&BK z0qZJ)_1V9`WpmxpJP!_14a@K|1QtDzwcuP3v({W`nmhXtd- zlYCZQ*136;M3JvX92T+&M>;6PA;{S;ZUS%93IeuhwDyP!q?csw zE>#I=^#83^3N;t!>0+cG9TP#F7oa8R$Sct|nxi~xA-HKCy;m=X0m4IKko@%WkDKi&kQFlXR9>n5HS^k)!fpWpZQ~Zegl|0btOSd=oywgSaexKc)%QC zvrr{^E7wSv6Paro)on@VXs_|Hm#S2T@yQ@kNWLRDV;!zGHD4i>r4!VA)E*+1@DOQ! zqmF~nn6My~fPOouLh}c*5`?yl0`IcHjQ9S^b&a5WXfWouV!aAh%NQ*PHx?;RL@sZW zO_q5giS0z7)_N%_w4N$jXvJO2bJH# zXfo?^M~Q(^p)e{O^h4DN0SemHI^8B+mSz8aK!l#GE~Rm$*5jbw<<=`%05j()V)KMtaSTTB@RIWLn^H+J>$f;x;| z8SokVg{VN%;ZPBK?)S~4)l1R~Mjdq(5K6s0YAXH#^>;yEWs=-)2k3H(C~>AI7GJd#VDDaNv14EHB)BPmB#anVBcl>XB->-myA^R9*-OJ-~4PY8k_aJuTKUyE#R% zE_aV#czFK1Hj|M<$=(DVGlESLkis7&&*$chb;Az-qM%=gvYQTLq4v4=^ho%>MH=_au{Beo9YLyn``A{+Z;Yw61fje}B%`6n7v(F5mF+ z5=OR#I5q^e_QOs?bA5MNlwrHi;7Ep_!XvMA>o0D1EnmFcbHeOClyV-}viD`Nu3@Y^ z;P%4kiK{7RGDV4M6Z!sC)wg{UeDA?uogGvj}Ie4nPRe7Z1G*rw=xM@Bixg z%}R0e_-B@9RBP7!Era7HHK^!DmOf(@XE+qXEA#p30!u^8)~gb+^-@t71!T*AT~XL( z#!j}ISt_Ba8NlACnrQcs2>PY|U(C##3qjbn9{s_z`2w7{ZGlOhDUdCd=}GRfn+)z5gSR?8-3crSOd2-*Ihdb0dap<2sDj>>EAz?dT{{ ze=p8zAa4&3`B%-JFA>+IDDgj%+njTvm=0Qm@%7Xl|3kflSoG#qzF;Smx0CEt zz{P4C!N6;z`+Um)o3X-O8eWK36Y?B;p}zZ?`V3s4KD3XxPRP#(ZT!vsDc$KF$ILnyZj?_fJ%7d zTiUy1?dJKSZHZ)+uX^@FO>gMb09@FgP^jhyvaT|4s~|h$)Gfz!?kXZe*P}c0?QU!; zPk$tZSBpuQ_lt47e+V1e9y`lBCPkQ-8xUjE_bGL)T)q#1Vb zG;x%F((dso_VEtWVCR*bzeQ(Mx)i*r(&Z%eW;?1s{epYHg{f(Jimo&11yXQ$_b8v^ z+1O)vDbep>4#!!@Dpp2_lbE^5gCB~=BRO1mgn4{i&4jnA znftF+*1`jgrvc1(&|Knqx+I9y?Y;HdsG&mvrDw>usVj$IZN*iE`b8!UxF8`(!`&Gx zH@=o9H#^9p@TxZ`@GV7viCqC%g&Cyy8wCUc4Vq`y(KvTQ;Q+P_8JoQCu&l>3Thxbt zdoL2(}9LFv>}0L0fyFL+pz_%VKC?C7oI z$(EuGorNy`d_gtK z^4JgPkNNApwF||_W(6~s%cwhu$uOFKKL&q$<#SWQ+tCNw132VP=-`>eG|U@#UqeJ8 zVbCmbWkqH<03;2Td4ib<0j=5D6?D3_Xw(IyHH6J#vh;P`nKg1xIcyWFX+U&5idP36 zqeMMaa?w_Ygma>FD2lnKhPPGVt#wX+J0BQN zkloG1$-qNaJ8ML`yg1^;gY^_*Qq;t(gE2Cpe@ zT`Iz#Fc1+v*zlH0CNsfZ_e}qPL%)0rHIkPJu)w`2P8QBCtqq6ddIO-vzwx4dHi(!7r-JHw z>%A#b)ehjvLMG4tBar&h7%mdfcS=Vw1K1DkDxQ8k#35~oHb(duyYk0k_r`d!M)nx%FAoaI&GQW-KqQ@8<^>vk!4@Jq<)$ z(^qk*Bgzl+=lJW4?Rar2#h|4lL*XeB=6ok3hZ+u?c0yU=M)3KEG3b1wwib{JW>f|e zhX(_;*-b22Q!gyg~8D{b6q_J%v2Clz3o(tA0w#QOH@4f^qN>OK`cyZ)Q3>x1T|dYDd39nJW{W^KW-fJPaNFKPFPQ6}6? zw*1?drl><4l44}NefKvBn3BnZmwBy*%yg-n%y(b(%sHJ-k;231@eohwif3IKJms70Ru=@7p;pVr=h-o z!U8D1VU(X_0Us+mJ03wQ?)40+;8QuzpmGawaI+Ya*v8R4iHr=s!;(HZO*9B9DcQhN z8lEY&*|RneQB-YVv?=2JY=YSf!Ce-4=!pHaRBUQQe`G4pc#q-z-d_e{zgDEal4E+% z-38j#?n>& zTVS<$J*_v;%;Yege60ps=KT%5#H$USx8s+uj5Mj64lwpJ1*8V@7rs0*8+h!l=bwu` zy!jsYz%gcE5-*8qQEO$CDH1&fabtOS;p%cYc>D#103dn36)l z_FNuNLuB^_QKBKbb0Q;h5c)=c8I{Chj!{9P*e=?J_|4T*K6~+oL?nxU(gan~+WMc~ zqs3=95&0Sf#aKJRzZGO9Y5|Hev`-My$NTF{26*_Aj25Y~rKzaeKlCa*VHDQrhGWfW zyhbVN>0Y@A)-LNXpSb)-LG%60Y_vWf!|`1sNW$C+iNax1)G|GYUmvl5FV-+4d>Cbh z_ixscflFaJ9ey5pw1mRYXuF&uU_bYA8uO!ua-66|A6B}K50{Jq$&Pn}sPBAZkwHCN z2Sh)YUDBqyn!BY6&n(L!EXWlbe9DgwSLDI)44z#Y`HS4)DDS~=Ds4R-BYXz%HTEeZ ziqo!BJPkE{&ohRZBGjUPMBU0}E*a&~2R|tvwQ3+~)v{yfn1h{FX(p{*q z5}J>$XKMFO1>s}wb%*t3JUnPZ1Yn$Q0kwSc)yMYg1A=HOLX2+atTh@}f7Hy>z6$+T zOFtq{tKU?kvh9MA%9mgYg)ULG(Zbhh-j_OiD2QtCAnG=ADrNpqIx1wcmqifZFq-M zP0=?Ne%lDRaPXU#@1J9OpBzg~yS)exv{2ck!lY~=Hh|SEBF>QmASJu^CN-5<{|v|J z+rA{DcLNW9<@4=$wwQYD@FDz0oW!c^zLX5#7I)Ye`9c&_22dJUMXW7%irHca5x|Jm zN95RtKh-5XRaOr;_>=EOdfr$lNNc~`nwridfYH(A6`!OC#w)p={!`|hbe@P1vi_?X z`J!76kHNL;CYaYStp1OY`AvWj6)_p#vci#KA@H<+RiRlEjL2i6$vnT~UT>|u#=yLo z_eruE%?tGs!TEjJZnCNhzJ;6S~{wJ25dI_rJQ} zehy4tcT^3*44KF|!|~0<7EjfG&RY@|XpmU=nkm`d8<{7>de}Tm;g|$1EtoGC zjjl04XhYubbdMWFGYkw&WyXc!OsvS~gN@eLHZ{HTVM@6xgHkFiZsVDM=H1aD zW{otr&%`Y65v##RC`pXC2 zM?(U!WB?Zl%r&=67|lHOc=JeqocNfn3$$Fi0D`3G6`;=f5wi%fz`&GXqz(thU)A<| zM(}BU!03qVLkZQaXcl;rs7`_%-|KK6X~eTH%U&VQod8s*Yb&BMnzAhDOIFK);P$9% z0*a7*h&^E9hDIOf2kO-^*E1AGaEPdtfVotSks$2GVbxHwDozuQ+6df##dcUrcDR6L zRzx@Z<}KhKt8ga7yo~k9Jxw;NnMZqui`jcS1CKm_2$$pxmfF_+Ga=YgZm(s7SRPo3 zACz_)_4>E{>4P$q9U-Ja+shV&GPLz6z+VTlQx$8`hv-Tf#&(1)@%(*KARcw68(%=w z2=-*ngEe}6;qJ5Y0B`VrkG9_D30g~JuNRR9Tn*fez?<~u`VH8=xwt=ry_w8V79WRo zC1I=h#=QrJ+%PI^Dn{$NFvM&v)GNA|p;ji{9R&(RKVg@PfHqSE28}u>fNXgXo2W9e zM=fJ6-zO|$r-ve(*2~%MjAL_ z^|~VMD>7VI33j@`2l-m^4RkwOFF3rnaAQPIsu*wK-47wtOP`SNA8v~jAJK(AaSf(y zg%we{G1U?%d7)Qy^)M85@5#9l`}>=BgJQ%ly;249=YoZ6&1ybSP$VU$+x|5O`c`S; zt3-RE1zvTe36BbY9j|>Q{ecBY*aW&=ajh;;3wnq;NNve;q0D2$HP4-N1EW+O<8Sh3(#;9C|nxl!j z;56l)!AMgc1h5I&pC=>}z(x6&JG;#JUMuQc{m6ENRVXItYe+L zBN&1J_+8l}Yj(#)ZM|k16X*hyK#79UwkQ zfJv$mO^)OVt|bB;MVgQgv;qUt>MC3Gk>9`+`6I@)yP^Yy?~Li72y(4HZ}H4Zp=1 zMK3&3iXJV%)|B(BZU~Swg6{Dc z;|nX^hn(W$mUk4XJ4O#-l(`C1)b77g;$_3jd;Z?%{fZ730@cHv@t$Ok3DiT3M*~aR z^PaAMH(T&|Ui+OwtIb4ZRurKxcF_+gPMA|O$ffD;{!o$2RhcyVumfB!LXXB=+6Apm0h z9Z9Zf#sDN_0Q~5+*E)C#bw%!1kH+1p^GVUOyL&T*{4q?8%0U{vEA0i{K%B2DAalNW z6*L7N{PzGk8dX7-fEo{<_JGaD4RuU8eer!azq_I32qj6aLxGw#pdG1_w{re89o>L` z9=M0qoW0Tcs05lqxveT61xOH;<2SvC?*hzWEI)b0YQVZt!c{s93!1*BTp5>v>r^;x z$+fnDv}TT&Q>}S2^tDB$EW?}kPd)lAwRCKo2AM_Q?a7zW2iJh?9~K(Q=m;Tc$+tzb zFYvCqkcN*|%K@yweF7Cx=cjWP4s>;Y{WTLodVNxu{*#T<4Y)WP>X`?&QAGBM>0vk* zn)BLMiZbvHBWLfWV9vUfj0bwJ9_0*8I=0zNMOnKTM;H)qU;#Uqpb!UgZ3?E)IoI)s zOwQD|pzUYMIf{bKVWY{S03ST{E!gty)aWk#PkK&kyu})@cg? z?6*2s>$c@WEtzG;1q{nN{js{$ka=aNaJ+wqh03MJ`}5&h7)t2nv1Nz#lo`!RVHhrU zZ=V<*Q|L{x%@$2O`IF+ynVA*ERgIFQ#|Y)>se@Xg2~`-2mN~vj73R%@s`c9P+S<+N z^IhC(jKHIXWdC9hHFV%r7Mgf}Xo(SDxX~PoM-!=Dxq@w#?@iXo@*Nx~(a~4&BUm`LkYZ|SGG8Mbb((rNj8q+I z;S^uVzgzgQCTQ3#hgS+wu|oZ$$J*izW-ypm$x+ld`tlEhrw~ImQE?i7Zanx`?x}$1 zOtPkaztPHwP*zkt?3v{p=IZ2;(LWT?hxF=2GbvpJCq2Vg2_HQ9Z&h>1nR}EQ*2%h` z`Ia9W>U8Kk-w+XBYkyM?xV0%MTInCZBKjtgZ9^aZ4Tu@vW#V_6lW)wzg~^JqO%|WD zygIkuhA=PlZCaNgr#Q8L!u}}^`f%>j@Qj~>;IV>^-(v~7Y=CZy{r=u1H7vY2mp4|Z~Fjk0Idta z`&ZrLnj7HT2P^!4@UCz4&xHThmj3%)->Mz<)?t1Cfa82G9zgvr@rD~koB8|U(dn=S zsBXp%7!m;72Iw?=-#rHOUHaeM;)ekAJ^^)Sgl7zZrPek>og^c=vC@FLRNJY$&%65o z!8^il!T|3Nd%zI@uj(yV09536?RMvPu>i0sbQk#D6%NpUe7(JqbwV4;I12!{p>43! z1E?k+`Q8n700u12cXoVL0Y3%+L`4}$s1}1(gH-}xAOwG4AVnEaFf<@2C@3JT@MsyJ z|KDKw&-Ac#ur(8QF!hpiFg3Gfkg>8ivomtFGI3!LcX0MFayAuYW)%GYPU&6TEmkzP z?2p)y{O&Y=kfx{Ti6+S05NR3a!;7eN4ga|qWIF--Rk?eoPzp?s9vsRGs;Z zLK2{-cMv)Rj*EPK?TBKJJdM2{Y&*p>$DhE`)h3v#Qfmmjf-t>0eXq0JuV_ciYv|gz z;0}27)vMJ6piYmbT$A@d_w1-Y`hROt0>?n~CuWkSbZxKYswnMN`CbY)OJoXrvtPIa zIGxWi0QNAgg7w`nHMUe`*tn$J5dV1UN z4XUI!d8Y)eb~2{Rl3#2)54HHRFLQlYWS}%fIJTGA>@LI8ZoDmbVSiXp^%w9J%vbH1 z6|*-WCGE_9FJ@^UKE?ufke_@e;%Zt?F43KTVG7%?Z^}|(k)X{P{+7>)1ow=HHf>p$ zuJdPoWx*PMq!03Q>X9^CZj)@&v|{2?;{cy3wL@|Qb?pEl3g0;eb~!k5RPGxfym>}+ ztqttZu-iF0gm9p|y+b&?{UUDv!9~Lb$`G=9if4uj4Ac*FL%U6R>EAOuUE}tTnhLFd zaeT+3Je- z2q_Se;bA&E*?h<6Au~+iheVZgoav~48$ieEO6)#$hCGVZX!0X}T5&Bh@XrP~dg`k9 z;fa)W+B0PSF`i$b>9o*=3gN&2nQq#?eCFJKzB)W z;whXtIuBTbKefOE#`p<*?wdpnOa%UdllL#X!=vS<&Xe~&zbLuILI;Z3RtKtIHzrK@ z)zAcB<5ToeXi)OS#zomI=DjF;#1!Z6V*Jq02b6vga?|tC05Glml6u-0M*RH02ej+%XkbW57^v1@T1b`?*)6_B}Us|0ma~!e!5knAs z+gT*mfCW6uAbVG55>QuSurLWZUY!a&ptHZ{>)f6U?pB)&enJ0BinCf<)Co`^AQeoY z|AiFf|2HY5jZ7Sjt&Hql{@>OAut9Unev1>yZ?BFhBORmOG!e}WnilhaTto>?-|+9w z570&#nw{%|@^k6OCqiX(Etw?Uu$2k!0vy9B+*}T}a;6_Q8rLSYmW3L&kueWeq_bLW z6Lai+AHcT(62Z_ub+V^;q9s^OQO?u?d*(H(@Bv-^c z{UNcqVPOraZ~5}6JNM3i-P>#4MyhQ2vQy3<+8y~0zGE#fz_ut#*uiql;RmglRN5Ib zeYLWoDvj1oG!(;=r`sM|?W#u5jINBmO9qL1QO)X=KS~Xy`UAz_3t&q#N(d032H{A1 zK-t06G1(N`68TaTyQ4x_CD%Q<>|s*(bhgY>oey+Qjh*hMJV7#lC{|HRg1gyVVgB;VUxK0l({=$uqg_` z?i!rC*&K1EKr6mbgt;sMyR<{&y+2U1Ne5xw^a)g1J1ww{g-RHJupVo|uTV@{ZP(#X z!6{Z2a{VPoGbbp2n!yjA$6E&B_Xov0Iffr5Y-AzmN>dPyKD5WfdN9AeS*%@y_ApzK zbr3(Y%Y(dK2mYSunEGg5ro87Of~qy_wbkZOCvb}#gg|#QESysLz|)K$FARYXSGZs1q7zKR z=s_w*cZ_g@&w6LXb(`G#!qq+_K1fCRMRtm^$~yHdj>xTq*DU9K`fM`4Z?aU zXgaE9$opblln`Kv0bS~10zX^5L0kkxeiCjP?Kke{d;(r_?vsrkMI0;Ty>al+%@^}t zl@ViL1v>uSvp@lOW&-xik88dSMqju_@)yTO_S`lP_x^-rSM9;<@X^0NcLpJj^nLUV zd`i7Z=ft>AmX+(6Ui=R(jtgeG$84zB z7np1Nsy?2*<1{SX@PT$ZV;-sLC*3T6Yl=H%5eXwA532JBjhMM$wL?ei6vbVDlwfBU z@Y)n&^7Qw%$KwKeI&&f=sjIbN@7-zU?M>ke$l+uEK1MB)i zb90q~`^~_pn?N8r$McD_mz}T|%;_!lpxy31pux}6s|j)pKInY&h_S~E?sg$xw?K}| zbv_S6lfC{MCNMzo**)_uK)ywbW-YS>Q;i?Y-GjOv%R&6+*ykBQlYH_?nSmG zyI*v2K=68{81M2s=EL9N88McB%lJEPGuBzdq-6ZKHIiz-V77|a#rS`OsW-%pUmvM4oLdQw=ZVmieMlapH03kr^B+&OUa2T3;& zc-_5T0#cEPzPJTT7`R-20eH=-J3m35eHIZumzf`ilS3RhUe^xI5(KElZ%$42-2vhI zYYqwISs4p9D1ay)e^V;8hlQ@dJ?CU@(r|a+@xEW<;wxm!Q}cZf)2tQ8`198+a}pK@ zw3z2w3)++fmQH&z%n6M1s`L95tYAAu>y6Euzs+mE-Vg|Mo(3qkl!K_IZdC_tBjwX_VaA&>}f+THJg z#n1Al=r1)18O00ulbtCG7zH{cKWU$5N+( z7dsG*L1ZT*IMh!M&^>p#=*=Whey)WMO1G60Nj1JvrK8bXSm*Tm3BU_@-eyGjesd^87zKG-1ERh}82& z19h4Nv?_c9Lkl@x7(NIL(){1PW6G|r8>OFAcbA}TyJ3$*YLCPdVa|!?6m}OI{(Ntr zY+)(pH8M#uD9XW9G}4^YKoWaDM`+Ljv=*)j`{?}lSTj$mwtvd^GD_kWEbWo z@O`q`*wxQ}z@EgG1QmHrh<^Y|7)>qYOqkXl-lRwW%0`F5%UZL{3a8%v$hm7m^TBmG zF~rESLW>utjLJw0X!{3; zp+?z%Ctl;wlo=C`u>368Nv>WjGX%-tD)GH(A8XB8hGG}|>-fAbt2;U7X+Iz4>#jXi zz)m`5!HLCCfVqKC%8zh0Qz4@bnWb{SXn?c5?M2yP+ynzwrp zC&~fHefb*wCg4E2bGun}Z(^i*==!`6o(ArJ{W_EvE0n#F6WN_jLQRRzmL=%9^!t@L z7I&LP8|*CjV=tun*S08FN-$Yg;VcEv2R%KLMzq805gC2ERdbcDAbZg%|Lk4hFDWf3 zx=5F_0ncfQlrfN7{&ODMy=nDpaCfX$(ubT%m;_vzFk9ax_XGDOnWlsJ#ZL={k;hH%PXv{u-3w0DvOIwq{28+#;C>u zy#GdqybsxGJ7`!^reCs;=J0HvvCc7dqC2})?^n%dwl8P2Dqr)bePMNti?K#2G7doL zqPfL5q-q<)k9FY)NX+VrR5?v;QelH_?uYX+gzU ztEGC7jB;o-wl|3^LdY~^%2dfgs0^#4oj7mE1-%TDXpOc1S|qC$Spc-8`;Q>#Ct{|= zDAr7K&&6>2!kJR8Oo>+*j^;*Pu<$P^fV!)sqqym+c}Km9s5=5ABn1Tz^KQ9FbuRMA>_0g4~#ZC}`t z@_+2Z=fC#xL396fK=9&ZfpIHO>>unyx-Pg~3_G0L19qG8^7qY7>H=*rYjX*T5fLP> z5mt1!oZ&C;9jOA{ey*@*urKMk;_(MKiGl&xN4-bz%J%A;%jyJwP)<+>x*cZm zYF07)H&Qx_8sRX|PhHsm=kO+Po3&}&VRQnF%JEgshF8S+Z*>2by(TkN9RT4$Vv>|UgN0Brd>(@Ch?OtB(3EwqiBk2nb%X7se znT=gn>&jaIso}tX|3k?;2S>ti>%OsVJDG_&v2EM7&57-a?PTI)V%xTD+d9d-d*5@u zTc_TA@4sEu)m`1c=UMAnYjscajS5vy>%C+Gd#APOA=ch5{OAQlUP$;YR2Mje092cX zAE%yOX+&LU;B`0C^bqAR@yh(9d_@#@uWWi#b<0!A;gxWI%kpr{lJpMgKxAVN1(NbHVjxqPnCS<$lOm+BUJ}QXkN@__&pIZ-Qx{RL+NM*S) zFb3TiVy+=;j5jpzOQ(%EX40MY#)>u4HjFd-PK5@S^}irr?}TWPtMHm7dpO7AwUB=s_xSi^w9pz z6lnj&6vV8ZjqDwaz8vAdAN%VF$yPSY>?i{p8!~!-O*YJ-0gF*LvWR47ecFBt7(O8P`;^jM5B0#jr;)26Uy-JgiWSWoe&?cGKaM6uEMy=BcF` zA&!S@>VBmR5~A0bzM(iXb*Xxg(st#h;p}kL@YZDM6=}Lm(QTuE(yY=(l#It{B2WVO z%UDUN6vLxw^?rL%CMq$)!!51 z^6A1YtKQV8w`Y9!Kux!?i8quQop$|Aew1LsXmYs+&y|dFuNISYzhf?C_az ztc|_WK5WD0voX8Zoh9NpZPKrNCwP2X4A#1D2V7@ITiC6;^6-PVL1^`cJP&a03J!8P zQI+&xAUlQ3oWv%<*AC#k_;mp0dZOBYPF4wfA(6R3;Pw;ox~q$CBlD)kU_Je2jBg{! z%hvyt)_z=J+GqmidcK-JQX6|dj+2x{G*c^}b%mVTmoA}>%8FW##?p5~J>)^)qW!t%moZyO6iCubvr+?x`)Nvux190*!VWW;yPbFWwH;Mf(;%VuQ1M+C-JePO|1b zfjlEVoYg`Av|>OfWfZcO1iQ2NqBMj2ilSsD0!DiV!?00rl4p(=FuS9 zy$hiy2hx%$*{GX(40febP%MIf%vt<3HbM5wUe%}UEDUK1tqndH=^G(?FLhflZ09Tr zYF2NAu7gG-Q#HkH2u!*IS+LQ;PS%|DxBfxf4$2k>2O?b+3O}Ef9CQ$o&`nC)63Td# zC#sA}N9BP-Uz}rKY%Yv}1%A^An1`7jF{SaALtmX@JXc-tfa@Ljz`8+ysFY)R{_J4> z%SeQXo;n~b;-0qv+v&S!t$N{Ew6N zaFlM!1V8-c2(aBpMnY+%*wCF0f>m^#CMSc9?${=lB5?ci1Or~Hi^bt; zHtc8dT#tlHE4T-nw2?G_HnvMhgFiXXr!OTkJfW+LYy1^RXirHt@5c@569XTwU|$Z@ z-(rYJ`WX%4asSPM=>NrmM2sB2+=t&x?{7j7wy`%ba`>;V#CnAtrhP*J;&=?KH_A{V zN~;&MDqIT&lTP=5(#6aOxKx$pY2l#`16UFH4?qH4f_Fohk`p+66*Q z5myXy1*=~9z(73v7vB~oFZ}lD-{CBnKvBv8=%(UT`6hRNj%KWSB~c5Ar%~d`Vz&%A zH6{BUVwK~nxkupJLIbpJyLncfn6eQza!z^eh(vQxb$>D;S`o;e4C1EG8{J>^2YlRa z*sdYmxT~lAQ9ZapE>IIE9iF#VP5}Y*^n~KzA`5aUfs@vPKzNSGkiYbu@!{SB{z<;- z-$QMl&BnriyWkfG_Mtol@2-ch33-3ds4G1Z%?*a-QBnOZsfYX6K2t3~qYv^}e%3Gc zXQPOU-wWXZrW@$ELM61P;gvON@O_FT?N9^p;F9&)-LeMnns_VQ(C3aWX;@R zqMOqoc;(cA*`nF4PZjvap(QQ(RSt*;%Hk+kU&OmM1i``X1HjB<80m#4nO-c&Dw$ zJr@a8q%)}v!^|tVeH#|+T8#e5|F~~PioX>Qt5wXCvL)Ii`KQ0$D(GjuNT**xTymJF zUIk=-dnvfL0BQ1>4doy(j>?y}0Gfyb7LL=3kxoYEBJJe-e7MC_tO%aj6fNqdikQf;_I!Q|57$QLx+UL=`M4pl`B^%D zrhmOf`(+BRKjjeXKid+se{D;c{%2DVvDb4pb9DQE`GU%d^)}ttvZQJ?Ntr0r3W=YB zzkvHxoKm@@b`2h46U4?LC#tDdr^Dc&yTG#Hx1|!VVgF93iRZqdJ{Zdhm5>CDgxoG+ zA#+#AL7YCtHA!#Bd55}}CnHY$2uOK>X}=JI0->?5lP0ytWwR>eZ`AU+)1Gywz-V|cJX zNFR}uKsq|xz;DGruA78-MN_*unCedTDA+m@H1f7v5}D{TW+xygS7J=?c2A9eAtW2s zA2c8U3_%+5$o9X&ntnvz<|?h8;e@#{As!F)T3j!UkDKEH=#b{MMnIh2qp>h43}%VgaHdtj|*+ zOx)+``>KbDUyBmp*9sNfWn$AB5A4FR5xk|b2JBq9`W5Oyyo%TkejLGneEl_nBbFdQ(cvF_r3up)+N1$9=f8zZI@Pc>Va4A@V?Vj<>OYrS01D?fuc=q(MFY zEsy}X1>++JW&4mRWN7XSKR1o*6PR$-+D)rXPg?p?Gi_rVR|ZlEc)y7T3%7_|K-)>c z#}&%okZ`cj@YKb#x|PK9rA9{x_~)<5fyKX04!-}NYUDF;bkcLQv3H)`RSGkzqLSk?+;*FJO#^Ph~& zHIrSyD95av$5NI{>IM8!h-q%ozwUs zz@d2pixs(lQpPs>LfD1P8>H_*luOL0A)ioKM>h_&u^i7d1hps+(yhCj2}k|3JX;wR z-%xe6(>3%E@Sq-MgE<%dV^4NGzEajaty? zwr@&*6xzc!@H~TUy);mJ2jV^(CLg2gVdfrd-qu%iyyme$fYnN(LeDD}p2sxXns54V zK2~EZc+71y$ilkXpjf0VcR_t80qHoEt6!b{Ruwh@q2zt zvbQ?KK#D_I>Lm!|NIJ3u?@NUXMU;AOHXnh<}5c=3k+fv~iI%ayI%q z4wDrV{0D92E$dIT-#b3h<|#+%TqF&E`pqO2M`k3SZw=4uC)Nry9)Uc~{We}P^ovwO zr8rI*fqTQQox}T`HFo_x70asWIKYw-=NW`}oyL%S*(j*v=hLCorB$Z$3hA*>u?l8? z;p{iW&{VKIRP>RAAf;FteRC;|u9pYXBw6k_1cF6oE{P}pck}*BP6BV3dYspM1xr~(hIks1E=ane8q`5Ed7&{VkuY} z&k+r^M2~MF1L%G1J!=nAIghb#X(#}H12~DZ%zL?L7B*VR*)neM7BMIKgtz`Ip|jaY8L^^9d4k9)e)tTQw3wNoI9_;{n#n z-R%WNdi~gRq}MZ&AJv))c5Op)b`+e@MQ@@fLgTm|UhETDr1!_l*Ntm4B#7{e>3=5z z*(p6pappU95L`murAE1KR~Kj`3wuMHzF`+x_(w!x26Vbp@P!zc?3<%* z6r8lYh(90JOZXw3n}eh72RUwTugqXF_mr<6QuVX|W)KKM_CtSH!=zE=rDO zmS&D-Mh>*{M*mEA70s;v&U6&qtPM=mUyQG!GL`k`On}RGeuGy0^N_zPr z#ucdBshK$C6?$WhDmR%>dr-q=kVN2P zO3z8w2SGJV+p42qaYJy_I&g`tbi@q^hyjp)AiVM4w0EG688@zf>1^wBQ_l;7j@2tB z>G}Ob-AXkmDfc^z|IwXlPGjWVtu!yh3S+TysRF{Tfl>9C?Tdwqsq@xHG3#V?0BLsu z73hZhN;etTJl8;Wz0lBDTq(;Yo5(>1kdIa#^pdhT3#K(x-qTiM6a);*1|Pi!XgR|x zs%0$z5;&qlEy0_AtzfT>7bu0 zczT1Uw~xp6V3n|2LsYBpOFn2W!mb64FZ)iXWpxTA1ddyeZtE8YQCE7I zkZ2p#I0u*kGkROc-g^0^nUw2^d^lJ=rQe&nNNAbd;eka&!v{$796#c zn6Iru!sCcFKsv*#7xh_p{BHMS@7fvl3#v6qq*#5kDD^LXT>C8nl#Iiiyl8%V5nyph95e z)?gdbEWTXxg*hDS1PI&`S>RQLz@EOhgVtvuFvI_G)hQUgMeI)JUb@%{MjA~*(TV0V zCC#4Xcm6h+qTQMhv?^YX$F0WmYpsE_dN?2wI)}Zes!?q~7G1(0CU?{dcNs=mKhC8m zi=@B(&MDpzH52`5>Iuhnm%W50hw*Wjd-I_iVp^BP6CjFL=!Vq>@<)22!^ue2+5J(C z0;AB4h{L;at>x>@?T*z}H9*bkg;2k>$3xEB*lDpr^D^KcJKMD~Q~GtuzzZKtuNbh; zeQep?KK+8UaNX@>TL2?-Zl8kf0MgQu`LX+`9=a)fRIrg-DP22UFYV-8gqTK#yd2_b z!D8HDmfaEXYZl$WPL}^59?o3)5MTA6{tpe*3yN=!nI=CuLQpdBfKSQ69;yoxA66Cm zqHF921$uPJrHCXS<)XH%-Fuo=z83&_jx8K+f+YQkCnks#Yx!XpuS9(O%@K1JZu-l& zT=FDRvyZD?y80(xv+9RPU2bOkIk!i1<~Fy*h}D%eHjl6qaN)Eu1E_Gei z|M9L^xP&l-a;K)yI03T*hwS(49H4HIgoIg03nb2Fu)dl#{}%H1jGuEB!}I(L)S8?g z(xmD&1N4A-EOJ#I1KxB-cTo3{7j4gAd3P><31`iqN`8CbJ?@6=KegR7lor-WBG2{I zmwK|AgB4%T@jC6K@hd}~#}+^k#zM2m62hOFka~u=lWp|pd52QIbH$cIov`40HY^Kd zHYp!?J4B2U`WzqMCTJE|b{^~{1fZBa)ZbSrcG#Jhr)SH{*!guFt`|OV)OlrbH%ym< zTO}ENyT4wdCqgA!MISYS@yOR~<{gj-C+zq zka?E4N#Tw8DRPh`f#l_sEpXyUiU{yqqx#^`R&mj^-|}2T1(I?;H5&+b4sUafZchjv z0^lA9iV#~$N>R_Ke_H_JTPG2gY>4i;#eMwBry_klLbftq=QUiYrabYnEX7*L!%Xw)K+c!~ zP+UL_>fPd3G23mu!AaX%suEX{0uW0AbydMz!2Emt8pIR~_-Ft%s$mTdy&S3E?4m7) zMW1s{@uK?~y@@|ls2mwL5){Ygw(-ikQ~QkFj-k#zycva^>uE~i^M8LUOa{KB_$nYy zs^>V?UXs7^od<>+wK&c?F|S`7V;-7iD=`lr3eSo_uMG2|ekf;syzf;DP7+@%7;-{G z(cmW^JPBtN<=g-^pvGTg8O{U=je5nHTT0$6E>+OLXMZSQE`KViR4=FSqQC3ypfk~V z(w@n)D-690ipsebUm)P5iVft&MzdcgUI(0(K8+QN>}rgk zS_7NQC{aUMaEm@22A{lDlGzwwhZqTM8Xz125!auKqjvyt&QS8mEzn4{H%f95>XyQW zmIXny+M)M&OT z!N2y&;e7xulp2vS<|IA51@8%o&6BMDCsLXWUFsT`P0rFlJ)lHsIF1+!Yxrn{%Vo)-x*<9^ zt%^3OAD+x3om9rN0!-6`sbEVJWZoNocAgcFy=OUGL0mPpoSL{aLTk1y9B#m zZf&gxS^z6z>u4TC3G{C%|R5|tHX*Z=LUEugrW!81l&AAubtBg zC=0Oi*ZhO<_Z3|`Xx5i&$!1;S+M4J+*yE_er3p4orVrkGBH2rRT6<3ZG-O0uSKy@M zs^iK2sX3H98)?Q9oBa;wWiK{QfswYNDcf$P240tHG^vc0{A%WPN%hwz5>hYPPYFkT zWXaV}dGK^0wA{(yE)aB9@=<%Kr20q*$^?*d2)2L#3sQ}-SF$7_3j z;fk0}&FY(#wRC)Ulsh3K2j`3{4lK!RL~_g<{m<3kq4c;_n(tOmW2h0>i(kJ+B98#I z7_{eOdHa8n8+zQJdA^wcG%Xvytz=d!L;lo;h~2%&=eZHn22lu_zLhRWlu7obppf%V zq)QveaxScwo&k;r3y_%Ldh$fdY2Omt??EZ6K~M0ci}`1Pz&ySoGa#aCnt=Vw+$Ekn zRia(y87s!B;k{2MFaC(DJaPY>-v9qJ2wk&7y~wWi1 z4)US5e_|yIEQtdIZ*htz-XVdSlh~1R`*h%0v`kXhX{cRHms}ADSKxggr5XZ~(=MAc zAOZ&C=!SXztZt}<(n;0DG~=pRV0f>SX$g`7>> zIyK_vLZ=3DAWwJQS)5g>uPG3n_q9lVN8PjZg#C#~lf?e6_i~3KaHv=Mi8$**E!9Xs zH#{`hXKJ4R)JOTUI~v%im9!@^bNNhS6@H+o?BSed~sI}iqtB@LdFRl6{w z$7d?Z^0amHitXw|i^_#L*mA9S{87bA7RF>Y&lxyDH>rSBkZWlD6}W}MO0Kc&*qg;Y zWP(kp*MoqRxhIH|Y1$3y3oJLtXoGPN%{I~tGWAXUMkVw;7@9@~p5(XsUQBXvysSX|)M!hdrrX&^V{COqCY^A=;|yyio(e2}Y$p&0l!Lt;Gz#;YHVXn`3~7bf^O!)0$puRz z+BYVp-U5P0v;#%QuWsQi+dQ5L~ zm8jZL1kc}rZQ(}}lT&3Z>#<)q@to0g(d{9Fxez3;>cx70QUIwpOcbs8J&O3UTJ3YF zsA8rVD#(`iSrh`U2o*|)DO?={(0dP)yffKAb&N@*m6)YY;?;1GRLL#UVW((j=W79Vu9xa)T4i!EhWPSB3xrN=1&;MG)gw%e<`8ej!AY zP;>~?rwTY+awXjF0NS#FKI6rJ&n+#y2OvzHAg}B=_)eXi$eY#fX<`SjqWs9%!rDO# z(oLY%3RR3HXgiR*ih62aOZC=;9MK z?(#_>g?hjkh-%BRiHL8w{udjrSG{d+W=PO(y@91>2-iF5mejDy-b$tr0pBnxZ$D13 zS8H!aIVkKuN>g!BgU2tgi)*91<;#i$3;lEE3FI1*8LmR59W;Od{vR$=MTi-XSp-G${Q#WDwVR!&~w?;U$1UOG^^coS=bq|_h;!N;o z^A6NJt&u6Ojae>C4rmcN{r4ArLKv3)uug^_%%3ss;;@Jaj+S3LcSO66JQQseSD9?M zwJYP|GD9f}dJyvZ?wO~#H~Mk6fkC$=BEb8J!azq03RP>JwY`kZlzx;}%3_P}ATw^v zvoiqDGQ}Kk*dA1M16XumXKV;7)f1Bbf=U@qt9rd;KYX86H*fS}lfOaz&Nw5YtXtx%z|Scbz6!ExZ8GL(PQMSy_?B? zz(Dfx-*O$NKg4<}xH6)o-ueRDfF$Y{1$@AJCphc{$r_wr!VhmCrq0_Ndm!YOPRKPE zmj`%U_5kV-oiqVIfSm#i9{T=ll=EuIBPyL;b zXw54AU7GO7=d#iggHDAalRc|?e4x>^w0drec{Dl3Ad;itfe9KKUWr=Ux#hzKbxT0X zzhck(a5TM=x#)qLqwq>21ctA2KQQFTpto)i)~rdlVsNw z*OF80K6#uqe_^K>glY(=Q@n6@GK&;%F}uZq<;q)7I$+tAp1{*YiMJs6*}gQSsGJ+< zHfUMgW5gMOWaUuNvPYAc+_6gjD%b+nkw87Q1xO|EEP=l)4i!^8ObI@#eCHH*hG;$54oKx!hRsde2FP!_M9bT2YxqD9jfKWl?19m!$Iq|1}4^G3u}=16bywS-}{$K!I7f1lb_KWo#8X& zdHZbVb}30+l;M-bx!h-KJn&}8ydk91LsFUXCDNsN#ye_^;mCkc4Hb=WQD-f=w|riY zraIes)B-!?Go^F_Pm9l_<<0@Y3x1{JJcn}9*bB3ao=Gv$i@+D35SDbL z`j*{h0v<2z;XLRczncBL-8ooE-s}ATu)XJ#J_E5CDkq9$ti5j?&+7p8_xTA`y=750 zJ6dttD2lzT1!$W$!(FxbJ;bs{RYJ8Da9Cd1u{_RBmPf8tZg)oILTU6D9<|zNzS;Hi zJ|Vw;JwJ0i|F9Pt)GK`8w#XeM!i}*B`;mR%Vs|4PaG5Kg-}#d8PT}H0yZlkp4-xr-MJYh| z{?nVsHGGahK)KJp?`$C3EYf$loFi9iz_OU8sfVd!nS3BNz6sflN`(DAST2 z)~DhnXl~UlUumvuu*Y(Cejtd3o@J$Ha5BS6#qZg>dR6Wcao)_&rmpvDey#yG$YaD;Hn#tC^gPT!Ni zI)_`&yQssT^fZ%mQYN_)n&2)?0ggGK&a%&`_QQ0P*eW+8RiziceC4^l7q}_&5EO}Q zgA>~xIv6M|4$?4imose$cWrx>a#J_HHD|&EkF9;AV2%L}f%;c>B7}9kwIzXV-G?V; zO1Vda%r?{sYD(d5dpYpg(0_EQGVL6}E)wKJucs`7`Br))!QuMMI-KvkAztI2>qofs zLOV-k;SCd1R3f63^??=0VUCR!s_%A1%4si!IzNt zyDE-(s#9$l%e4`|Va#L3z%+6S5Q9_DtEC8&3L7f>d_jV&k~9zbB=g$vgAk#9NVAtC zd?+RY&-9<`b_A!r=XRu-^|-`*2y{BzhJbpkS3d%DmSM}58vPO?BbGX`yFc5I3z`ce zJESDv3H}B!t>CkEZhlF+kP?Ps30<)DIRz|I{^nZ8OzyZLEb6;3m%-~X)|F~bOKpxt zFw6i^tQ>pTTL24%M=_20r4*-6jfp7U=m_&mjZWH-e?ctTteIIH803b3Ji(rhGG*ll zEh`Mbx5xiJ6Oj1)Q@BgD&Z~txnxVz}v$7Wab^lYy!-Owti63$}w}FQ}BJztlN1hPN zc_vFRnv4e*T6L63wr+GnIHdjWB8J7t*{>s}lSvV!@r1@|w9$l=)4${MhjrEM*(C?DDQr4`L);f$m;2x{3QAOGQ$4n4r+~0{mp@3q zuruZE_HaP0l3`ya;FBH^vS8)gA}69Nm!H=+JrKy;xbS9RvbYKxzBse=3@TVD8Ih$n znJ#18Cfw(5qik9+)stG*rDjKcYDpsJ4^~A+M5jYkIS9(vY_%gAhJS{3H@2BVqC$>WD@IVA!=$0TA}Jy_l5^LU`Y772BbCeGxSZiDX2 z02AfEsjf4du$zfwN|K%96k)#}X*^B=9>%Il;F)3tH;_w;$1J7_0STmDr{GW;oVz@n6`ZxkXb{ARNk+S z_p;n$fdJ#w!=heGe%EOJSh?FP#2keFu^RbgmrBKF~~v6 z^%^Pc_=x%b`+tS~cnZ9}AHG8IjoY%fS4b_MK?A&>+VX#+o1n8!k9q~x(?2b7!cWo`6Nh%& zT#YU8`bRh3;;$eVCn~18O;!mrEwn||YYqt}tHb%%@WAy2k9WDyrfrS@#k!0c4*~%y zzGxlLf-MWGf*l?(-lja?F(^qqR`^7MOJcXRoamN6`4qzZv>QM4B|oeh!7UB*iD;r^ zXc=bYv>DZXqK^Tp9cSY0>AuGmRKTMZ5j!bKM%FXoc_`@2R%U<&G#ii<8e~OPZX6$JguM7G%;kO zGr^&t&IdpQbqf7M5%%=F^g>2(G=^Zr)yfM~7N--R=C2Zb&aP}hw11!$D(AjE4KkMC zTZ;uaIY~49#ic?$|Dy^kaEXVw7YNpnFy0wRxG*yaLMOL12%$MZ=VhlXL+nyy zl4h(-#8`sRBzkJ9r;1d=(NQ9~G`t_#Iqao2R%!ya3ZMpZ=)?4ufbYCI zD8fHe_H{0Itd))cm}y3=s;Vwui@R+>f}8{BYXoKuS~N#8d$z?^O)5FaW7mPk)E>sG zXC%U?5&!!BLh@b%9&*#WaWTOtgQRu#M!t1}*Rd4v` zZpMAS%T%C+`i0NzrwBGoO|oFVabM8gaP3Y*OuTq`qNKQhwj0VO28*(VmSnBaHlas) zjZJ>|;<39!G7h}Z15?#c=B&lPRxz6K2{fEw9<)_EKNu?c?A_}I%yZwRX;#wvC*`Ah zd-A7^r^e#Av5mBq>p500W;UGwYj}9e3NXM=a|c&%E^S=!Zyqv6jQkWQRh=kYteq>e zF2AzhcDBR-nDg*!PD-ui@ShQhV2F{++t#ocmY0HYo(U!nsY3?MPyB@(K-)lv`?7-q zFs*tYhM7-sCX3V&+DrNG;$6^M#}UEYI*apz6eX&jD$wqcftF#h*I^Nh_)xdt7QEZ{ z_id|Lk!UGIHr@~uC0nz-h8!vA8W-rpVAa8`P`U;{HP=N=%t6B`OgqYK-6tx~YqV`uGFMbuq+z{T!4rUt7gSB4z>KNpe65J8hc6jC&t>^S zO+|T`-SNs6pT2N>Cz{3gSBN!QjI=lLNsl-C;-uR}WHo0@3!9g0@dol&8t>Fl+Q1Q& z$?Tp6fK1KuOFWl-?BoKRp8WKYD@kiHE8P#!MUPtbq_L9<`&DG;S+{2+h#K!xXC6=r zzu9iu{Pl#=~WL*19}aoM%x1ts2#0)9F?EDKW>gC-#Cl(s_%XD=jc zl^0S_=D=o#eXeuzEZBx4Isg?8sTS(!;uxsf+k@Ly44opTrVIoo8n*FF8nPcN(uV@5 zaxb8w|4`m8=5B0%zjEF3KTHMmwv~NBT{AhbaAz>FB2`h>8a>k3v2A{L6A2z}9Ho^r zb`ztXs==-C-587em0oLu@CvR6czsW=U!;7?RU(hy4em(bTyru(?yOJVpq@{$xAE$P zu;z|SHoa)JO!sTTWkM8KSaIS}L4_uylr=KYdpf~oP5sS)(Sdw*wH#-spU}Kw? z=`YU%qlS60D!RZ2a>VaAg~c0-6OUhhmm=zSTm}eMs7)m@noO2-fkdA}B(atT+8FRb&Y;j`}+Wymf-bd%9d99L0#O&=CjpW5GLB*R0&WF)P}=(H5X1! zc|VG-qtwtu@2mZp2cCT4FFxO$3hC#6A0)qI`L^PIhPl20EOw-yFJ#+&#}^VjKhd5E z>zK!&xQvIY$r0*Bk|~g4I9mYn{->shbNAjHbYa^s?N!;Q!e?_`dB&j|deV|pUbTk7mjh39_^ zH3-0xP?vEGAUQr+S@Z({yvvH*XvD&b;Z``lI*a;t2!z+&blLlH;8Di|4gCmWnghVi zukcDL7l-W#n3v%@dw)Nu#!)SB3t<53XJqKZc!{xD{Z=$$ z@xZj_w$oABE<5WuQCYRDL#84KmlD1$?D9K2nD<{;NmacERw0VdBobJdxHX~f%Q{0u ze8~pe`;lY29lV>qpIQpY=NC-0ms2awLXF={#}lUDEh|ua7OdOc7s)%wyUg@{IHEYuJksok)U@1KLjKf7<9!F&2yd7fwi;w`fpkSWLyY?lyaDz{$I-A5K zj4PDES)$^I#@3@V#XLvB8}tveixx8vdPdn5MTog}KGU~Y9@C=vbf|U%n`n_f9Sm{z zEv)UaB%Xas+f&IBn;TG>6xU01QRAb36r(-bhShR2cr6~I@j3f{m&1cT8?3Zwbkn)h zrQcEi1SIUH!c4oLC*W--B-cM1Nivs_P*UTv7$GXgk!$T4V7?47K0HNo7k6N$s|504ja zr>k%nHD5r-v8RF&0z!J_zfTaDcCBYvoyvXoyN-^V=k}YaYYvCyRxbq41RK7#noEv^ z*kTUPRmrAU z^8;k9%SAz^TVy$#B7FL5g|$h!Gnb>6Nro%Dems8@eiW74WJQDKY2m9$i|dXR#h18w z17&5uHVh+MNupG{oz9y&MF``-(e|mr$@<)%`r=JDd|FQ&NQ5>tYg9G3qb21fr=K55 zA&yx|Qrcy*xSFAW_cr0fuCI+rOQil@lLR25i&Sbr*U5T{Yv}2T(cyDW(K962qbJED%>`Xy@AH51U#LoT+TZHlL{X@2E#nra!&u#eG=fc4cx1YkJr}ohS z%qGe&WqxD&DrYW}b&b!+uF5k}_R<3-urbGAIrSH+;Ln|UD6vcx0UZC(wzkOE$KVHp zLwM8E0gEN}m)_@k@CjpTMHJyCOD=%j@ZA7WuBSvFAtk~5{n_i;?(><00U>wz#>&jI z#C5-JKI~O#4mjywff$DSi}RdMrf<<2 zYC+{_Ze>j5)FK^q47Y_Ub7g>!PP_+HZw=7U%%UG>c-8!j*?DCHBT%kmrr_bq1n5s+ zgV(=y<1EZ>*284jN`}2YjQfEKiog}XPc-Y`(!j!AInp<32Wg`5uGuf-^y4Zaxy*4o z`=Nolfl1wHNSbw6+b4unf_j1Snkp`%Ns?C4g-ycsjdaR*3VRc1V*vL4laR%uqNSW(a9_k2q_m<^7~v75Ss3kU)f_XJm&uB^HHD9fC3{lQ z3PF}B+$(}T>-#N#j1u4pY%4OA^;1i%g&3(kfwlXdsp1d?Sshp*gs0LnoK96~vZoyz z%cUNxIefmwQ*Q6u&L$|>2W>D5rd{O69Q1sy4LlNwfrmo zZdjzcREgTPT8&8Y*oSM#T(5(QzQHG&v|3x{xdH7umP>2y3KBqFFSUFtid3dZhWlX& zU^Ac^RU{pPY-%5z9`)0Z>jWOOd5&?t$^}z!C7&(oQ;#jwnlaM|7`bFU7ajqM!a8;a z<`d25Q(@|uHAj@WRNmK{nL@_@f%DC2Q{;tY{m*V%2*cn2x8&b=WuFgILW_#|9FX&`0PRs z;y!Vj4hGf!u5>PLX6T-%?D$7_?wB2cV}`P)N)$*xG!NvJOw&|7x23+2DHm_}h=$JE z%4f1E=YgLI;BZKuu$Bh(NzQdVj&c+RI4TSRUFIW0FCwEJR#}%z=A-Q}8X}mD=5TO94paiG5_l7u|xz{n4 z6o(~(bTzGAosBAZIGoZJA*-oP9*5u|ecDMu|0BRK!7L7z#!O1E8ZBChBFKZHW_FCb zvdMY@Wd9OvW2Ao+u_-!ZqrGSZ2pvNTF7@(DXaVYXnBfzo923sPuye55J<1;B^oOd} z$xTXR+&MZ-tcp48ltYkq4|Fo61_K zy%FGdWNo~px>CWsZ}CX@qX0`WC?#iWHZhkZ1uyovgc#je1Aol$XS4LJNrK^4hf8Pb zqH}5!jGRy7yW`V=Mi}QOMG0KL#hkl{o}QgPL@QTOnjdV@{*Xq`kOS*WA3}V@%aazW zd^X%!7^Af}=l5NC6lafLh7ywx5bwC6W(weT$Ud;~`|1+JgKZRBtl*&{09$=Sm+UVg z@4)lBP?IqrX$^>&D;p4tq`MKdt7dj-a0Pc$ko!zu9UE8Y0zzNLVe{r$Q1Oz)T)cXd z!&_Th2TLLza-I4_d^2=xuaPq#K`z{Njuww$u`;^~oX!%vJ$V(gB}s7{uGo@1N)CX^ zUltSjL~o`leuXrdR^2?p&f~>p9X=Zp*GL#40nnZ&N*CNzsh<)V&GwLhE_9BG^-!$b zyn%I?mB>x2tw|@70)vmvBj;HdchPS}&$>Jk@lavE2}U}?@faQZEBabhc($bRJ6|_Q z6sA1$Wx0##bgWbshIWHwC}k7sW&z5D`Ia^j`9k`&fyW!-vlGr&6)HE?*JLV!a4*#n zvt6oAYr;%0%8emRmfJxUoq$HM9KTXj=l*76K4Dkq;>{@STIR+>q&V4jlm@ss%rMQX zA+#2HYz6P~`zfSF-yU~>$(N)T>m@xOy;d}lenN9Gg~$L;HyPYR%etl{3pDmN(~KgBu&u&)&T zX`@D?2W@k#_mSWmW|ryeqfp(Gu*_xrtV@E7N4AW9DMNh3vra`Q1a39&%8JL~ria2_ zW>fzGc64X;0QR<3U$;9!cmhnD`|4LtJ96*MNYuq3)Tae|#8V{gQI(ITH@)4daL{m= zkiy;?-Eq}MAjW#3q%wDPBHexQ0ymMgl_i!Hv{uYOBC-Yih~3jh>=b1kcKiD8`B^&= zaEBToPTxfa&V*OiXTWPJDYtjJ_Lk$LPP&BQ51T-4&q`S+xXWn!kppzq<*mUzs0f+I z`?M|~PB2)BaYp?W#`a!krsN{%XqGhp5R$ofBZsPcL=iX9=#ns7I3VtFS?i`FgK@jl zj@%z~!Wl4k7)bFuDxa`**6>g>oL;1*qx#gQ^B#NO^HAb?RfsLI5->IQ&m&@;TR6I7 zJ_Zz8*t3Bw&s^ukV*N|I@>xd+JLC? z(N#eywn+w^@ngXbicap)?OG8~wp%3;p=ZWf!(ixGC9*@Xt6wli!4s7sj}K~?rZYjJ zy#kR!{!_l@b5K=CD3+2maZuLu1h0e=DP*ej3?ji0P*x|4>0} z-%5@5w<4!J4#>CywC$fvE7u8-2%N-;U%_>aw7)>-o3l(EGq4#i#W^jF8}E}_dHkYV zhk~=CC9<6srhuxvQzVJ@u^bF+#7H;J2iqay9(Q1;9j_0;H(ly?w!sdqAU~g_6#ax&)zGE`EWiFR(^*)D%y?p}q z_WWd!Wa@`!J(aLeLzPvF0a$0s?((0>w>~$gzLXyl|J~fpo>o5kEil0QTjNfSRPM{^ zcqc$`9ohfj7kq`NYC zBQ+Kk4?P-K&s4*eDp%9X?r5dxU+-WpVG{EmV10 zP0-%g*F30wus8);+lTuzWw7v9|C#w;FJdRjU~%-C#7NxY!pPWjK4T9Z%JW2sQG z%Vg9{=sl)O+GFXT`^gp&wOgg#uM%>fIqlU0$z$Cf%_Dg6hmNOHiYHw&?qI-xIr$G- ztl9?1S1D;!N$vJJGW}~GYESpaBqQM+M}H~XFfI)_35Cg#?Z&F@u7|gLe7!EM!i_#O z&xk2ly?WwcQ6PDJKdDOnMv-PR5 zR~i{hnq2XUzfUE@vU1~mLmOZ}aGMtTH9$(ool@dj`#?T~F8Q~Sy$!fXvi?cFk^A<3 z)MM7|RqQve25zW^V8rmYEw&l&VHRc)@`ozsS|M%Q1tctcKG*6suyBG#J)XilH9vGI zi|O3QHP7|!QnJQM;|KSn8X0#r**;AuCgM_zF@Y_|mRO&L#<@XJR14r_Q6I}})Cvwi z)lwhISw$a{y^%Fa7r&a{P9@DtZ0IF$Tj|xO$Y2x)COQShiy*Tv`5D1^nu=nzf=P*2 zJgU&%(rT70*`3xx6?u#ML*PmN@wjwou%3PH*Gl35!?if$T;z?C5;1-%g0uI)?ja`f zkeZ%N8Y=q#TlXie} zKrP`qrd=IXuk^8_1hufdOTZSUp>UZKX&67ETs)Q~lrJc3DoflJ{ncTNTzLDUp3saZ zH2da2{k6EZ16XBC?{1Hb2jNr1r528(&Yx#|&t_U%s`%r&}7%U$go6 zqY~M@$mdI@2PZ!mQpc+k4gcN1==qy_OzLCT3F1QFhSpOzP2*B1crw^4oUR1p>+XZF z**=<)cHMG_iWF;cSl;AsZKK6h*sIlw0Z$Z{5vyO_%#j2FOHXge32A5JDnu|?2qai0 zk4xxL;!sqPGL$9QBEgGv=Ssk|Y4y@(rtL)6gnNoc6}HRs z5Rb8)t7aN(=!?9b{LRQN@5|#~ZW<=S6Z0!ww<;`gnOgp|raCa6R;fKivhc&Q3;3K2 ziNN7&z`Y)kCktK-5@AeBn#60-n1-flO*VIem8qg;bUYnY`$A--PR`Jifc{9hcyJHE zCt&DaeH@T1+z+?-szgqXi6`UX=J<8Tk=|8q2uB=pvjjvFqbw0pqMX7TQC2f=5RmvXSoRpFT*YvQv zMbWU;AELUrkBVt)jz`x>B6wY}F-q~YMmVf^RNBNClNhDR14`Wept%XL5jx@$QT=L6 z_?*TM2nZ&FGe^Mh9Re@ZIoZ^Pza@TC=9|)fYx=SgWz`&5NCfN{i6iuUF~0lbRyv;$ zD>4A8U%4%Vlsnh;-bvPl0>{*D?Dj|ZLi+Mh3j=k!veG{M5@Q#ucptWM@WtTl^&_Lc zYhKzW;U#3gX{B@RtTLUgIEfqf_hw_qCQ4kwLDiQSLi@0M^^RxR-}MqR(UZ3ehRHfZ zuu(MO_rvMpB;y=4?eQtQngGK!c%fuiB7U;ygg8}+$X4q#3b@VR>+PqMlGy>^2nF@%{zjr)G*oqGIr~ncpB4ZLgffuy=_^qp zFpVpb*B)-=O>>h;58~&K1Bc|d3zKo`-a`sbpCs0oJoR_RTleP zuQ7B(lrMeLdx;A(SBeG}A^rpYj^EF~o;r{2fi>Wp-@@u{@tcZ0cOHxac5lY|YI3Jv zP!hhB4JMz+Avf18#KKYAqh=MBkxg7Qi$e~8RDnjBRE6494gLV zaVP7OAUWkNAVUuOFoYl!QQ;qQBh<++Lk7DwIEPIMV^PWkIVh5_O;k{WfIYB55A+>W zOA4rWBScKDKsO2_C~iKmzt{RXFGQ(^NF{hmK*Tr=Q*OH3@8{H5D*X)5PE1U$2X)W1<1ceJsA`gFf>MJqzqKDd5mT1 zJ~@;S;eQ?-3%YEE|G7&50|DXxoBuQZE&q-DRACAzIuMM|RC@|2Jm7-VC<-WcLiB$z zjErEzEY?6kDz>0N*#Fn}e`Efn?ovQe{U-*75(*FG#4d5y5-U}R5(*9kEi_gBKelDp z#2I^Z1VtHO5FnucO*A$3Kd-plQ>!VVu!*q$?Xml(O*tzD638e6^8e-a->m{X61|-W z>Hq%^M9MJtMMz0d009M40RdtBPg4J#v>>5US*V~$iT;y&9&XqB@jv^>zeAG!kK2FG TyGWnvN(IFM@yP)7&;EY^K>Kk6 delta 78508 zcmV(~K+nJ6;~=roAQMna0|XQR000O8b%u*ea|A|0XOR;#e{<6YSfXZ6TVfQW@vW3Q zR)vbH*`N#puRVHK}q1R;X-Xh0XCNowj z2@z_*AZHbayrNubc+#o@Z3P&oHIjz%Dnff}D~hq=+k#?#H>@pS1 z*lqi}I|12!6K6ZnB3FH~3u?+&4%q(zmo5ohLK3o|H-yfYX8}ipK7fXU=yok8-h6r6 z7oMPwA67Ab?wU?}euPf*`?GZM4^T@31QY-O00;nehLf!U{{nS}ldb~V5_E=(OEti$ zN)TlL0B}VE02u(65HJWEf9<_#ciTp?F#KJ=0!Me8W6#Jm!CkVWd$h*NV_AAdIezbC z^7ueRlSB*x3;=3LPVR4is=6C{H_!knnYNiTBTHmgFV)r6)wRC)@B5jp-4f5YT<6Da zz0us(h%<6c%bEPRy$HrH2iyO>zxC$aHHm%Zj9qVL1khCbB?+>>f2C<}-dQ)E;XUB* z@YkjG-k6aew}-|pF^{H(XN&^ko$#)=_dg6@9t@Ap&fdJb%s}vdt{>)Pd;idON1yp8 zf~NFK=d*w9@;2N1u9I&!9JzCHMaCZSr`OgD`fWCv?QXN#%(uM&WO$ZI@-U*Fk1gN0 zvB`mL)Al~J8jthMe@-v2uNDsezP%5>XnMZMxp9Af{O;7U39N{`BhDn4Ztv@@#1t=X zekY?qyY$>SgE=%mT8`=7`M5LN8vFk27&c{FJF*Sm=gYTd3meeXI6O|aXT6I0mGs&Y z_sUwQx=gI=%s&_f)-B<%KW^LZ#2OhktSwquc2g;dMF6I|e*hG3@BiBB_8ZL|ZMTa* z^)CB&(Camtzoom%Er^oup$BUsUUpS&@v3qlasP?Q{$lwSPA<2&W@<)BiUFFvbbTwe z#1R~j?frLT9LT^vS|-p#*xqu2c58dTD;?!6u_n_%y*LgQ zW$^D@!$huncANp%cUv-;lly=;fuBE8dY7H4ep}C;q6GNAU3Yd04BYi{J+vBnTLyM9 z9{VK758A{Lo4He)*}U}MnZJ%)XKYQ*4ab-eZ%Bdwe=Ze2f($3{VQv}$d29JFbGRB( zf4(Qc=o1r&88O^YV~d!F52y6D+uncngB`p+SOo45li+m`$T4)U40LtXPs5{p|N6W^LRSB5hw8{Kv1{)Kbmd2m#c0z;BzOdQ~Mhn8u1K+iBs!!BRe z5H<*Qj-I5#hS$UoE?wZ?XCtVmV8F|(mr#K8eI8kL#*RE?H8d7=z#td!W_N9~{`JbHmTJ ze}3~y#1r1o3yLCN&McxP$gH;a9b{9sXd0Ks!l%pv#_oz5ykE?2h<7m_24wE1FVuKA z$C&$5H#l63$AALsPZ&~f7PUMBqVsO)4J~XJj{45#Xw-oWhuR|=D$^H&ZTxt@9C z!p#TFL^>I0fC2`4u8iDUxHSz)yK@30J__dm4E#>g5Pym8C;9@E5B@fo0biN}FJTRZ zR1fXo3LFu7=B|jjpu7}Bjsk!a2XB|71;gDfDQJoah#OwJ(I)&NKZqP`2(z};e?j6= z*qVzl!&+Q8^z0P$Nd*OXys`yv?_WrJM9I*Dd*I~!UD&vQDo#gEd8- z3-I$pBN$B?+mqgXv?0UcsqGp?e-zFeUwhVsDMMz4vk>#rMAI_1n`nATsZGdN6Rl2c zdSG8s2PxpOYvVnreF&YezVM~DCy0%Hrcxms13Y4YILLqE>--9=%Gbw5R z4qgp*akmHFXbOi}g$}sTe&GN*-S$fj`7Z&z>G@zMhvoN9TSs| z@HR-R>R#oK?wgQ55|21=-AaWSlq6G4>21SbV1+`f0PO4{D`XW-gFkWkA%X9rVGcac zct~h8vJK&xkNo1t?YD+wf1=VPdJp}$t^ZR`D(Sd(Bvym;x09M(z0;6r$Jm%KSXwQT zpW18=5|L9fr`=R9qInGz=QVU~5N?i+G99MH>F82uWc6X0mf_u?m$VFTx(ox$wL7)$ ziHYKDdDAn;3M|7`YkY3pgV=g*Ib2kJnN>bzTk{uMx2V~_N7qwSe|`hUEuR6;p5<$z&E_!I z`KP0FR?y5OokEghe+}8YL$UGC#>l;~3QTnYNXp15%BPT?wl?>dHQ%#j`n*^E6Q@V>W3sAdY0dhIRz z;EGJF)IBM|))g!`V5r*OKZO@=5eTzV+ya^`oalp4t&^W5f0bzfufY}E%GaLZ9NM4D zNhG0TbbL727D}8SoKg5!N+*~D=L23JGSWvW1J%3Q9Po^WgB)z$Qb0e=-8;LA_YMk9 zXBS2H&M_HTvrsxN;cHY}IdkRP7wg9E-9;e>rFNaDbx_ip zO(JgpP|`lNO$@{atd>B<`^7*^f2K6Ai#g+6@LU0I%s!*q+j3B@CjVqwM=z9Md)eE@NZ!o*LaH_#D*cWwFwG{|*@FFBIbu%5o3lqFO&?L*}~E>8CuSq zTf>7V48Ix65fld1#D2pTgl5*lY5{VT`8G?kC&5 ze=%a6R{3HWK!wQ-1CKh#C8@l4h?Dy{a4AMl;bK(IMUu^y4idJ+Mp+Lrlr(eBx_Pn^Y!A2YVC@2SB~ z!wYDVawE$1RdCyt;iBM*D`&V?aB2tsfny%u*#5PPeU+|TewCGgM>syy!sW@X?ece{H3lwizqF|+ixI~a|qSJ;i~lf#9D$3}z&Wm#BP)p?Mc z1${^qsXeZ)FM-Kea2FDEiYn6~TYdoS=;3HeMxQC8&Nx)$7mwl)#jvjq#4g%1XZ+r9 zszP0%7nnNG6*0_1GIl*?W1~(Ye^u0l%rZ9yCa^3#&ZIJUO65qQ?+njL+erm`5}^6q zGVEI5_yYcD1=H~DM7^1+0!L0;aDJ=xK=*by1YB^Y?`xp!b8wxhp?P)`&%+O8Fl8-x2fQqhrgM zIIbU9Bc%y4Kho-O%o1?v&FJo`qJ!Un#Ju*;oS`>T#ynCmnu(5w=ZfeEA@I40#5a6U zMPvvWS{@YPxWiTsux!7nfAtHQ%@FuZ#%#dI66)%Z8n(0#1c)YWJ!(=@>4BBXzL+V#{FZe*}kF^@t*eWH z!0gF>VA}`&N2QaFdV1rA*yol%qfUZq2pcg^DHJEdyHwFBp|s2^*QKJ9>jhWj zkA>xts53RG*hd?6E#W8aCw6wBCXJ+k=-mjE9H3(FJ@pQyzIjv?L@yJ6!6X8ei#F9Y ze=J6{jc0s9s!kAARkec;&SwYi6qHD&!-ewuB0VlQvf?{7e;T-QC&P!?jcemK_4}ay zOz&MhgQyAiRRovN=#Q_8DY7C((SzBtscva(j7YfuDfU#MqYK-__oYn?pQxQ(d>@@T z^zqJ2-pWH_yg>jbk1=8#hx+6ex>P`O{N(_BY_|6iPX~^9MdpT8I7!$HfaF^FYmdN6 z%>y4kQs6U8e>I;J%}dn_ryrb!&n{J%-pMVxO=1U%O!$LC?&p+wquL&5iE1b+bii*) zde_iNjF2~%B`alh9nmjVn~NrXN-4Dh2}Xi#PFmBUpHb4MG=Ss1kw_ zGm3j1P7Tw&tA;8NgQ(*;;M#Ky6Q$b9`xVdNfa(UAf0ih+u_^b&#rYw{N~K3}iv1i7 z)%dTn8nLCQMto~{Cd`%;X%RtAFb}QZl6XhdTskO}#-ql=DT{FEsWm@CpR;y0h+5M4 zor~(fDy%H3s}8TJX+rs=B%3Rp zl_Y?|X-PI$IWO7vDkmlhqjYAH0BWZu388pye;7!;lOuptJUa}sI*sxXt57KesdG>n zWc8!U;8YJQYkldFIXp4Xje9ix4lPxZO~ZXqhD)E@q587;Q$~q7oczIHguAMIJ+~FF z=Zut4a7nW`taN)eQ`qFQF-UJ0W6Y3o;e`$eh7D)InUtF{M4d)rO6g;RscTS4XWPWi ze?(@X!L-~XKnzTu9~TZ6oG)EB8?L61p`)i&^-z_YZ8_y(8xKK7H|* z(9+zPz=r%{c%hjaI%G!f8)|LREJvH+Q0jA`J|`~5*`vG1;Y0LAsc%afC6;qwPta_5 zF=D|cD$YdFk`g*MpAsJJz1&o_n4^9tfAhGvW{l#F7Cvxexu1dZ+3ToAvz?+(Kv6Io zHaHngOC2V#pTM(^6xz{y3XKU0Z9^qi8D`Q@a0q;o0x27Zo>F!%`QuMkm`A{{Ki%Lr zCD0%^oz3PnH3>3ic#KLYsdxkNS-qHZx6NpeuC71A4kdTDtGK%iGB@$MF2ummf3tmQ zB3IKhXJju-@)r8>a2{t4%PLU8Ta`+p%v3fam>jDv>~bSB=%KwN{CGL|lUbb!d?Yuw z7P+ediARQ|-SBIp5BmUM8y11*sB%A;IRJ?=l*X!fk%x0Qw|t+N%g!Y1?F2URbm3I> zyFQ{?z2Vur%d>Y>XIIbddidZ6e}vx>r!>#Y#0wgMUHn*kcf^7!fEyLH`w2tlO>*Jb z52f7o-1v;1(lf&`F<-=I^2rPCN;+q|0d0APni2tK^ckeC5RC5Mfn!paA7iB8TX{`p zu-nN20ac$&rXc(j%!==K*Qb7d6eWC>DytJ`R>^Opaf@jOO4|YF=KgjHe{n{JrH2AdGS}zB{7+cPb5K34cUwnUzWf12yh`28FVSIt*sVg0pHlETS1hC`TB7 zpxKxwlxT(nLMq6Eh23XWhJ5HLwVqrn_Wv_u^ZL9e9EcZM$fICeU8|V0I zxvN%|k(Iw85dD#J7b86-e@Fj37``AZmKKO)zA5V|8kYt#hU{>-fWz%QAj?zk0AE1f zf{KMo9?O8M$b(f;-SN)Arbi0&ak%i`T5#yR(e&XgJpXiGqL0swTuQiHZ#hA$8iXRX z35BX!P~$pKAV4XDaj{9ceNI@a1ep^|R5)BMA8v%>JTVVlA|Egfe~D=`0}}TDFBxwL z(hpwVRBI6UJa{J&c%e62LY-*M3@{|wfe&T#Lp<| ze#tg1nVnNE3m0~Tc{}GcT)rweO??n$o~#9Z>DmuNjJidkid3J&@D%OEu#Yunz;s`y zBc}#zNDuW#Kc@O>f1nbe{;P#8jr){N0M$&j!KvS1ZDQp zf1uY$K@_zFFpPCOGciZ#*dohL9@V%@@8*ZVk}7x#dw{2oe|PPECOi%BarZl>lGw{mmv|im+N+viAo>@sKK39g-D4is!Ixe{>N!`Sh#Pu_r2gBXElbV zS1m%{e5!*lddt>YGre4EZWG>0bWo8tA_bTgaixl(e-u0YtR#>CGGRwuTx(r*gwtlR{jHnpmztNrQfI)Y!R#y}S(ZlXQPO;y~v2zJEv z5-D}6e?R=Grt#dguZhPl>guvCcX)tf%Bcl|c`85up%csyLfv6l9_ETYoLpVA(?Qjw zh69MI+C_+67D@>no>MbGtv*oti-IGnPK3TXFxogYM`9KYmxsGy><`T=i1~i13iFqj z^lv60)#^goZYzUdc*K6-o{TkKc%0l=W<^Pgf2%U!gBu!1PNQY(4v!@$7Vg5Y(ML!l z=w~&me&DXi%*9F%J_(RMs-Ba3%g1Q3P<>l@1K9AKyA@AB>bMsW&~3n-w8L>`-^Ms zf9$>XVR)iARglkt%VACzkadp+yRVLRo7(e(_rtRn<*(V4RUxWcsUjz#$Y%u)xNDpU z>7rGh#K;cO9q%WQ8i82Rb6hM@AjKi`=2g}S#{fZS|0ePWCO_sm9KD9hQV7aPej4LSv z5alT1923;7&^Q9c44J^f61t1 zT&3!h6Z%=Q>NERm5fsmaC}}Y=e;|VBqUF7sX;{>=H;sh!(Qaki|^F+&X0%e zs6>|cL~W39fPseW7RNVXs-@Bef1xDTt5L8(npfE#90V`)N)R;af8?SPsU8du zE?a8iVd|zrnSt@}6G`xqT+tke?fLMO_e{0^#m5G zj z=-tOy^P5-sNxs1(s?vE6i9KeTjKbQ*Sc#sdrIVc=Y;Z9*{#Zy`TrC`$dfoM~?G-US zLrz1qiOWhdNehQ#x;fs42T94g;@1-1a6VoX354abLlG9I=9FRX?WSOn*&$#%BblcV2q@ z<(=h|f9L~= ze`>oKeMN%CX$Coh2~AQNqDvS6KNhK?!=?0b<}f9fU?Xx>>2m}gYG6^iDOmKQZAz}X zY^A-xw6UxnkIxKN1#J--kt!r#**U?_J+7#af2LV+$C7QRevdtda}HQ!tuD285|oLd+XljYXn)Ov@9rDs=A3FC$5w+_w%*ZpiG5lNoc% zycnnIc5pH07(Q?(MH~ey28TX>+xt^uOC*?7@0l$vB`{3hMfX9}FT1x};$aIf#>PJ- zf7c>Bb8Tbp6J8J8wly=_<6;P)1u41Hsp7SCd@a8i9{oNXqYZv#w<6Y@T7ye*_%giN zL^Kk9$6nw*Ri{a25H^LKU)ZP#Wb$v36nsg|%nNF;t47wt#SPQ#i6%|elF&6v#Rc*Q zNa*-CK3F9aL4OpVGa`Kb(Go@lZG$lMe+G3%ahVxP@nSHV%lW`(xfMUqglke7QA|XZ z5bxbNYPWg4fbhCo-1r4+CY1u@jX-Hg= zMH%xPZtvf^miY;P6hqN^DP(Top~53xbUd#=o&!S22w{=)qN`G-7+XF&ofw>#e=Ds4 zjgw3mvunY}T8>)ak+z}2_(N;NX5W8EwVPxLW(LZP9TLln8OuCe9Qz2 zNALY&hUsdtfdrmaNevllc%+Au9$dr!{5CAyAq==k@XLsup*!gN6LGV0&$mLZ z5awD$uZ;+-2#B+(pFDRlCnkz0e_=O(7EwnMj><6r)5hExvA(R@i+;EeGI(UNIo z!;I(#t3^~GI>1E#^{=%PSPp=>OP>GXiMzE7&Ef}H^A@(`PWsA3pLJ0Pgl?#4m=VG7 zKV({2b{|HN2~=>A`PWvrzoYF^8+o?{zx8yXvGrn3a_&yim;ZL%*{Lz2f0Jx?+G2e* z^}Ay-8h1T=$u;*4Z!Xl{56_O@T9fHVz_m9syw5<&{XxohQXq8kKA3knntin)^Au^x zUb0yUFZFfgRnYEr09SysP7lzf>m3Qmzb`!M*DVuPat6Ar*)4gaBn@v862sy<#19fO z7C#||Q+Ug9hAC*ly=-E?e-ykrcx9i2?@g;I$&d`KQw}Owb?bK8O~slS6Y|`e0e3f_ zGm(4|ZeC;VOxQ;+GLO2P4@!pQ>ydpa>8I&5Y2VLpqvW8p^mevcIXS2EvvHE{_d6L+ zpc1;4(RK%YWm>)Fj@H%TM{BoP02dPYZoU|GN-seDBz>xe(0FIAER4&Q#!KCXtk(+MkrDL2=(z0?OZqR zeItd}YZqykcECG^_QWx_wqgKmaqXtS5CA7WvWek^Gh%a>W2oY>O~Fc!E^VcHe09a+ znH99>Ci z6s6K#QkQK5CWQtMu&*8NJY2Su%^_{t91 zR1=&b;q6$Slq4Z1iRvsA%SptvO2go#G6fsCmP~3j!(ro+smKXeVxr0Sie z@UO9cG^LJ;wt$R-Plh*3q1nT@huo(W`76V_e>f-EBjBByX>0RY3cVzsg?nyQykhsM zu=bN@+`{vWIoa339PdF>>q3ORUzZ9abz- zY}cw3&~6s*mJn^vey{^7s^M&Ym`Fq#kw!VYoWw*VT4laM48@XZG}Y#sh|?~ZaxukW ze;+uLUxbBci}qWkXS$2BuB#luc8LaBh^P?bdw#GmEq4#)-b2HSpNr?jS*&8CC8;ID zjW%u&%CH>b$u_*+A;F+@H66JSr(&U71+%Ae9rRKQnulA{;4PJxCs|^*aA)G-F40_3 z*!j{r{-~RR^Hi{sp5jVQOiRXAatmR5e;ZkZ(@R1brDQ2f>nPN5Qeb;2p9!3k5{fOG zfM7#p1z}Q?5uP6O;d$KCLx~`Lbj0tSg7wML*Gks-SX5N@V2}E!cdW#s-IK4fl+TYM!x*>cO?drPiTtXO~YRjRzt)K|Lff1Orx zj}%X!aPX3WBXM@cPliY`uk~)Il07T$AeWk3!bz^%ZSH7;-j1gC`u!cP-|C8<#ifpN zsYa#Fa;eryhq<0GTU>#+XkXY~o)92(TAMNHE#Y3b*Aq!RA{V+A`puyO9sR#Vi;#7p z8;DcPWmFZy%y$_FV)-f)j>Mtrea2D8Wk6d9sBLlsf$A2q{Cf)GCX>geD4o zmSAt_jnH$L?&5#Bi(s#${jnc}?Fav5Il|^F*-G}f30zO}D^u|DJT_+YOUtvoYulc>XTEHE4DQ_*=Wxe{8k|WU@Z4 zkeyBySIAtaNjJnqr;=xQ+B30$9II$pi!PYJ73YmDey?4-Ht}!+R5(-`R3|TmwNk~m z(z16I^;ZUZ26}^=s?+(BHophz`8SxG6f#^qPUQjEn9GPI7vR^3$AZ zhp|hcmoBZ1p*f4$jW~Cuf9myI@y{Wv~RteeDBnBmo9s?yp zwogp$(kBbk)y|w-U?n8NQi%jobPvc7s^bmN-bIN8QQ9g4jZi8&*U4ka>NoI=tZ*KH z`4>abe6ciAfF`)bXsQOy`HLAV8bSD5$;t}Do@4ebsh^(Yxq?V&e{r_vWp1w&d@9(I zCGgfxi&C|#-F>vBn2V;jEas$o3*|Yahdz)-yWek%$wHBzrdO%LQ&SM-d^J5lK4z@f z5))rFj@;kdlWA9E?*P52119koeOHD;b@rTUTY(=@I5AIB!9ki;0~REXHw9>D_c$85 z!dqE_23-LSMZEjTfAGc9j`4vs`j?g>n_f$Sav1;CrTklp;XX<}ZTQiqZ_mEu(J97p zQvu0Q_B3fC^eWg&B=ZPS7Ukv0qD`i|jrp`ON!%D(+;{AY&hH|vr$$Zu5^Yu61f7W_ z;ZEqiYMUVAYKg{~EGOen{&*p)ACOs6l)j{;w5(`1j>+71e;?34zO1Z3@*=0Mt(Yf| z)ypUYgC2WPO>)F@O{xLqG{wPb&(kL#`I~~I?TOGVX zkhogiDp!c!lwNsN78j=e3gy$3P6C!xrI1DyFpx8K+Ed#a1p`U!{fjt1P_LRb6?09dc4$W*tgABDZK+ zX}6>#%~Lx4sc9mfniK?@7){+L*z4Vj%TT8C)NZ#PeXKs82MTr$Ri@A`UOa=;PZ}a!-w8_Y}jOP_st!0fc6+?E6ggE)!@$gy*Ld z%5CidYS)?qIY-3+D3Sc5cTK@$5^2$R92Q29L=|X=$DQn>*tHm9hgCSDNIbZqHjAAH z!<)B0e;%I6N38T@sQ6S+Hr_~_1dH=n2#hOf8YxowCu>DztR@&%cSGrs_vEw8bcRN| z_laj2Hm|U!1ulI4KL=Nrna|+?fhp=x4Qyy8nG?sfoQVeCSb?9eMb+54o)XQYAzBVh zPJ{6S<1=9?6KFIYac(WorG-PZTVBPGj{529f8y}s`qTS^^OMHbR!j?fTU#$N2Je=s zaW&KOSmyIR&7YHzHGa^nvG(9Dv^z{F53As7Wn!G}0D51bP%KDo%oRuQ^+lG(6= zVK$WU;~`@J>1JEcv>%24ZEXeK10CvD0nKaC+4xQSF%j5K8>L5Jd159b|3xAWUvRWo ze?RO~%n}h&Xxt(h2_H3my*0v=&tcGH_9&|^H?eM?Ma8FsCV9Ycc-C1)l4R`W$nXho zbeh~=Xv{v1wk_-f794mB@>&Rv-vc!EA_pHEw$I^RAR`a+`XV9<52Y}@j2C)RvS?8_ z3q09O<^aUmH#o^g>HZ~Qrr~VeB*H|=e>){neYmXZZpe?D2j zRA+Y+Ftw_HscxMpxTON7>XB`=I3K|}i-YV`BYc)*DaE@ZnR%;l=IlI6=Ud@eb&JE( z4i`a?$|^6#jl?->GO@!v73}4yq4n9uXl4-A3v3mMt|iGil49wO_P>?n&`7ogG2}Uy zLt^PA*)hBH-(q?ra)3G9G3J`fepEg~`G6FZ34S(#x;U{5{O3e-wAJ7h?1`?bN^`p?DGT)YSah#2;t)RNB)b(T64g ziTinxN}H4TLTz6K$0?u$qevp9aajceG0J4*UFNY~!nNxq{(B;K_L_R2e{;(LPB7Dc zCf;Z5+B1HqK@ATYPi3C)i0)+Fu9i>@bdrIfKGnv|EGOJ>=w2Hk0Mpr6M~j=iy;0!Vdwc0t zjV-ESDxD@Oo{Tv#+j}g}2&{MEqt43Ew%VSlxk%L7yX!N}d;s2yE_xcf1hlQ^V}PEy z9??t!{K`f}5z&6te|yo;w!%}iquml%4Q6>T98)t!BZB_POpO*!f5##)2j~auvoBx{ zjV%r5J_9DG8Ro44>~KP~J1dxKKyY*1XRfEhI1tl-iOya7!Et9muG$vNYew~HQzO6& z6&reFOlZv$R!32LFf~B9Gl7w}P*Xt_aE_}vgxx;ym|~Nm&|}LEu&BIC#$z}cn1K-R z3rp25jUX^afaD$Re?#N7_8u0qqn+Y!Mqt=*Y~I1|=WslX9qlLhb!ebY(851*Oh zxWVfK2TvRvKs*pIf5K{&QeE2^Ho|cMg-8RqkuQJ8jT{+XKzz%3 zxR%Lu0{#vg0rPWV>U$e5G1ThOFjb#!2-DR2+Viuki{T4&e=OCT+S$p;$*Zg3&l<1y z!RkU#v56xQ<_0_K+OP=6g&*^IL+|wlFZEs@v#k$awp!Z7C}`0BwoKRB)AVLT*PFYq zaG(Z$-01eY%|Tb+`f&AbPn!n8eDBpOix6Z#KvfO=wc&b`SFp*mIs4i8-Jkz=F#quf z`2DrvF_YAff4n35q2)&(b_V@{$mm_7dJ1`DPJyI_l2V$7MsiQYN@b8aUK@d030CXN zz(ssH>OsVw>GdJ+Ee{Z@!=AAk7hA}2&m5E7LpQD0^v_Q=8qB#gN6R;aiYPto`zkzb zp_0rRhf-~7r8}9lhg@DQ9wnMm2+i@KB@q(!6HY$r4;Z3c*fkEsvP8lWP=CLhxa<>Uefh zt9sMR4-#((ej}JL#k$a{v7#jA!l;1!d%JqFywQ}5K8vKu45&c%7g2v%H^TSz3GYZt z5qw!!e;zlr}bewM%+) z6grDX#M9ubuY4!&5YwsCSGY25RZRAVu;JhmDXBEs{VLvF9r^rn^e_~IZRWjg8xU8Owa0y&ypHWb&4l zm1k;UP-2R!fWTgfVPv%k%so$*!C+3p`cfzqhw?m{B$a=n)s&=IM_TG}^GK3ZwKV1H zo>Q||C;j4Mn238NsTMaT;tI#OBDlY4%DCsj((G#%8gV?QxQ9 ze-@<$6|Pz0+LRv-FM&h%eu+Gr6`s{mceOH(^#+fSZ?oQGXvu{XJJqaiA!YBB<(AQx zRM>nBB2#}ssIO_+TyOH|8wDeI9-Bo;5{uasRbszl66qszr*J3J?5Q{vpkS@a84~M1 zfTFJI(YA7S!zErAOHQ%)QdpS;uRhCVe-Bh)5cT6^PK+cLvHq#PSh(JfCRYUBT2yy3 zd~iln&vmRnZI=(J@v}@9bza`&G9n6a zeN5z*{jFfITyD4tAiKb$W9r^9XO==2jurU5;J)CtBzJ`?ozZme=0gnD%I&0Ie~?tJ zlG}-xK9}}2@oo3c*G%`074o75-$N&j2UI5pt~N8m+Kde2JgCSbrnY7HT67m`&n<_W z-uxFV&gsUQOyG+IEOl!^-;ufJ-q6UVtE_K3{KW1%o{uUycwhWz1-X_imXD9(&o* zMmBm-g@3O(<1Pq=s>Ha(6uCU(jyv-icch`Cp&dTK$l-rR_&3aA>$!{7e^`Gq{v1> zw8LW;8qH*((uurQ+p7_tyjR|Qw4#!2gX&hkns_YXyefD+qFt;6k8t69O7JfaQM%{W zKeeQ; zccs+clxTWF&YfGxCbf4KZKcPpLTYb?OthQS-jy|=Ip{S??R~vIsl7Qp%iI`5B(;XA zy|0t{2wc)8wReTo-joI&JGFPnIM*q)HxnAFrASV$<1tfvm+*+(q8}}_ceLP2slDkT zT&L9Dp}bRTx7+KKf7&~>AyrpWD@pmPQ+r>}lk8e(ni9#A-$iO~y(ndSZciU4W42Oi z@4};$ov8ExJwa;k!jmGzsA{tG6{hz7AH$n#N7y1x>zzFqx#vK$nq8&e3(YDm!E-l< z+(G`}tll?c1x^=;I*K`ot!%A4C#`(6NB{blHV$9CBg3|7Zsff^*WcU2p7-_~_wKpS z=^@%NVvk{qtT9dZtr8BY?uiktFidul-_zB3VA11!MGP}?x@u=NFQU^ecoj=^%}2fm6MGUIHGQ82602C#w}(g<2=Hh%`jV5oI+glq8(~zTZa#tNyf&) z4m2y!rW}f;80SC-@&2$6G-HGz5Ifr4)EZ5-88IC0e*lZH3!iw}7J9%^H?1&#KYr!F zGD41n$k#$gD|g|INbH%xPdQ5z#|FVk1KF@_YbNn6kC;1KhV43&*m=wc3Jd$k#AGA1 zf|*8OXwOMw($Lz_8F9kCXfw<~>S8jsfV^mIeZ;S$MJyZ|Pwh+b!2Us_1zOwEv)9HJ zu}#gNf1-QxVh$K_Jxl@TvdRiP3tSE(_zO>>X8{);p#h0#TQ`e9y9-mG1R9?PE$84o z0GD@Qbz27kWB7A?_OpFt5$&|A8=@KZo$=sne-h8#fz<_;9WR!5=>r*TLASn(6T|#+ zY+iILLSh0Mhgm*omhUfcF$v-dXZ9-s{HFa1e_Iv&#;FZ&Cptlzq2Is^d8WlPw6U4) z(7T6( zpDqhhBzn|BA;p~vw9sv7c|29mf3!pjkWe8$$|dTq z?&4i&Xj@60Qc?j8{2Mx9&vSfBL_M-p?>YX?62M2n*;_a$FhOFlB9k<4wZ`OorcQxp zK6c*NIvc07SS(k4;&SGx<^4Dge|(20%g5Y_aX9!5rqZW*^tY0Vn!xbj8t3{Nm#ZT1 z3#w`g^%O8wfPtR3sP4(OK8Gufai%pI4eb@`NGz7C9#_vAJ*`F}scVne(Yi!B?v|v* zTa)K@mVnpixxFiP8T&@1{Mz}qziKi6IA$VGEF-U@`2JN%&}&f-ShZN&yu)48Migt?j>Pxwfc^G0#MU>&$JYw+pBIbZ7@nbsqG8}+B2mhXfA=*0i*{=n@OI`| zcD#jM5)3;uf(5W0a^YY}wF&VqZi$C+QqrTZx_`jf&BZ7p=ws{FGD)v)_EVV9P^w6)9w9J+ZC30e3CuSIG4jk7Hzo55+!vl*F)EG zQDsx{E~gl+JN_Nq~3cO_IQ3mOy?gFKp8e~3v5TUZGd%Yy#$6Gx%E z`C7yIca`Tqs6PMYXha;(m!*+`Ix)AJRfxHMXiHXsF?DTN&=3E0fABiAMYeXc?~A-G zQaQARI=}1PR_Oh{E0DqR_d-G5QE|$!J{1j`jebk-_P&yoVY>wxcq^$f-qJl>+Ukd- z;3uqN%-qIGe^8qXFZ7>|({w(9IwHy-)5gRfy8|HySZ{rVAD^R=hiQCBaLFAIaP>Yu zH&q2(Np_k?6>xbF9Ji_@BjQ@SB@ehbq#xg6{Qm=qz>;6Am^Ug;J1X)VP2HW%4UbU9 zckN=ia4gRr#EU6;oRpQOe=}+0)jd$vndl{A^Cq*~H<8&*r_|AusnQfL=kzMSNM<)#Y6|7+9S+~#UQ1TK zM{Lroo!L!t38dOSU1qn1v+#+TMueVu2XuqNZ_fr z-)oiNExjw1*RmEV-u&my7ct{j&7>_EBzXe>S3s!0no^%C?DH2)^VZr`GUG>Q<#DE}VuPl8T6L-`%}JMSlxd)#FT9(& zAa1+n!@($6px7B|Em|psMed1{KGDK=WtTo=fCr#=imtaMAhFwIU)&%USD4rQ#HjY% z`EY8O)E&tYx2dRWE7z1MoAFeu?#0Uy24qp6CNG1!~Pu+ z^av9LZsm<}0;Qiw+1Kj!cQkaE(+6m{-Rzb>*H=sn^aa^%RJHNCNGhd-smq z5?i}GJB~DU{cdSCH#K8+zZX8`2dD;ZiOiFEo|ESz#l~#C-Df`egRaiK^LzbfZ_SO_ zgMU@ko@BeI#oM^FVcRD>3nD6m?66;E+4i-u%ZmMa?u{E@qBDNdwrDkAoB%EEOXE9% zXPKIX85bi(QUiX2Mu7FPHPKv(kR6Q}qbUy_#UxseA9xFzn~Y}FN`m7|MmTX+eVB

0!oqgg;bljc#=|3Fg-F7EBfv^5hhN$^h9Bl9NRP%xK`dClftc|5 z9WG`jsVGoRG|`tsHm4ui87v?dTonGlkc*0Q)>$SV%UoRibByv))zsn=93kU6`!!Oj)cLQGR|Js znLI$fATB+LjD;ggPq2bKU%JrMp?DwW?tCFlvy~+xgukFv7g5m>v26vKq1|EIY$7=( zH@&gav)(qYkvk80^;I_tQIwe7jzx!iLp=&dxFxK5v(q>KW5)E-~ovg zA584S5KWMiEX+?b0#w7zsA&eJ2@pP5oHvMM2n(!Mkq2)1cNU6j9Ex4nnE8~o2ywyw ziq_-{dd@ryLRQKC<&T8$i4Z;zsMqUn(PSip13i0D9)qRk(B5sesDIDxU_k$Ece;A< z9DX4n9$CS|3K7ujQ@4~x6um4RoE@wc3_WP|sjqRj-=qKbn(bY|&{3+{XfPTa%&)}o zOlb~0I$%TG36_;M(`iqcs27q`B_1ka6FIhb48oy1;YHi?_oqp}Zu|u3nAghWq6@Xl zWgO?(4|YJ4HPGWV34f#)PwsV^T~Bf*l4{C)jo0vKF1213PB!rvEtqpLpT<6LCh#bi z+6ZK~W_izaA7x!vyN*i|%!t#|a+L4+!NRoMJ(PM64KJ48&WW>7y|C3NH{$yZ-ct2K z7c5zl`aL(&iD}6wDTkp*?Uu%rO~x`2vTUC#iIK(RBu0ZI`+q_X4VNf>mE)nZuu^ga z>d8kD4X*HrNKzhG;#m^qGDr5|A_!13S%Yg`yOW6>X_1&}1id6}_zy49MO(e!1OaciBlLMz@^`N0>nt>h^b~`U@}88*Pk%y#NJ&*UnNcLuEOvBk@pAC` z0BhZ-R)d!~VN2W_=ys~_FIm}FQs@h@y1-D> z*+o%Pu_>*WjcLpKDy!MjdiZW*$!e*CvYIU|?`!Z4+S2sqDvfFJCa%tq=0$zV&1csM zW6p@lHheDRc6v5Cwi6JV#*A~uF!Oj@u8 zPwK*ptxzdD8LZ14Sw=w2y{#>po&^nIF^cxIUkLJBsszH?LOAdate4v{!XVfL0^p8D z+aQ)!<0XC=D)^f58vmVoZZ?u5X~Q{8SbH9xh<_J*Xu3)Fjc9^47;C`{dw@{S_?=Gh zL7NZCIFD^;4Trp!~M6!mzumN%;X6#FDP(wSKx~@-X?Ht~>54_06{h|#v zR=5HyTy;YWHG7Eki+ooSI>r}U&vFHIXq+~`)pQOSQ}pAf^i{K?=*{Yelt)nDi_5F5 zQh$M;QM6Ws4ppQpwi*b5P-4~Ix$eB`3O}A#gg9b#D_3sSNF0z_mW?DM{0^-fCFYGh z`&mO1hpc-^GGVPaoABRjWH?#T%94qZGfR7Vd(7jn>8Z^;kH4abO|M{4y8~QG{PPPe z2X2q07oyXWiC^Z>@Vjgkn?Xyc^y#_jJbxl@%PywaG8x{90_$Xh&u(e!q)#Q1I)oPuPJ$4H3 z;xBEl)>a$44<|!CbQ|bt!r zUDd|EMA$an!tW(ydyqXy3dDZ=0Dp3jH41OGc84AgG-IzZ}}S*owo;#MlP(+c9W z3rKkxa#ka0+De(Mck8&0D3f-$VF{wno0gZC+e)c91^LKgTT7VdNP)VK2;Rb$X`q1- zZ&WN_#p`_wOF-tRy;CW3Z-=4`6RP!SLbaYGv1X3+`z6_Cmd#^Zo{d(OynkEG3gn%d zgFt&M*q<_Ou9BP7h?NF3seQHw?@xlAYomC$86R#3{hv$%Pgd%$w712=mlG2+`GG^b zt)Wd9GsA%|n2QmQm42xmpJ*4J32LLGCi@IMs(wb_s>tn&_Mqy-l>!Ra6Z>k>h1_xz zX|B&+zCWNuT00gdYB-aH4ZnxrVej#JqPhF>F(mWOb{bB3i{)gc9)DhW(%#d0FGEti zW#f$4Tb*MgPoCQIHn0DHUVeOe(U-W+HvJ}CZTOYQ+bDI6HcnzC2)bH0I5iNr@wI(L zOwYJeuAo~*J=xL=)sxIvw3H>K#Ozbs8c@v!Ak^=+YFm*itY6iVltZKS;9+Y{B5Xm_MF>q->%RBnW48HpczRnlV9;s}EstxI$F zS4fH7`oa|t(pV74bknbdJzg0cZ8 zA-!3Nh7^*QGn~oll72a2^95KF8On;V2Eq_WTe}jeDdat8Xj7oc$3&ZoovkNHc~wb2 zB||bRiJ|ja&U&X(?H7f7<5g^L={e@?Z}A&Cd_q?;!j%vCbTu`WDe+LC#q3Lxa!Hi( zSOmA<#?--BSbwb2`4SfwOSSzn)gBb`zcbThuQ0203V@p@yF{8y3oUg72or(aZ= zSup*cJTv<0rmt`bh10KCYJyMyaf(b7rz00mzbJdUeEQPj;VYTG!l4YOUu2e-PhW37EB=9V9OPDQW!biebY#eAz2TEB%Y6Yxs%qVMYMoEy=o@PKt zV_*9h%$eIya$Qamuz$a{k$zu6%b&s;$U+=#ol0o?T5KanSf&Z}9HE#K z{mn5psG{wDiRKOTW2V-mzC@QJ#8_LU7AZ&&%zwtAWGrA=2=@OZp6fsF?dVbuy($R% z39jczQAU3c5mj2DJxN-Wwy0SMiL0eoDU#k+(Z5$i6c+Yv*BvB zJ&pgO-C73A!si&a<}GXmqtL}7Sb3jABfx8hT)?|IAQR$U+!AkW(`bg(*o>6o_Xg4c z{eP_Dv{%or*utm}Mnu9u&kXn%vMZiBfPJ5Yp%mYxQBkB*ABfXXNQ{Zpb4W*PVw>Jg z-7Mv1GLzJjz7PG7Gzec3zpY0c(8dB!bT*D|+!TdDLwtCHos*Th`% zC=t-?lWkFY_Y`boIWI|du|Agb_9_D(!>vIR_+^#^0FBbARUY zgzcGcl9Jsyph-_fINn&B>sp(uSnk^2a}-^B6Gewuc)}>U_9lvMV<_GjiZ_PhwU44} z4Ww_W%9i4R$mcdS^7mya9#n}c{Pr!y<>tF8o(5iIL zpp!w7$!pbgBEPL=Gb=dLk4`3f1%{&R5fE(^UVf`26!{*mkWdPoU{}jf zAwjDfZdV!doF$-@8*eQ>Kr-#@CcgRGif`sqHbx_26A3ERrk(7faO#C)Nv1FATwo^W zk5DBu`N;L)gSqRN^b>A0_kYMA3(I4+6k{~CZrM>ViBCUG&coLx@U@eu^6m#lxkh|r zV=v=V!u<2OO6M-!^|0EdzLP5X8rt@@whAkx$6yf$3IU7ZS|J>#=@eT~>!O_+`ZiU4 zw+yzE8NM2HH#KsSq}AYinbAc(lx^~ph3&V$=dk_uCTt(E@PuLe?SDB?)nQg$@fRP~`RF;%aj_j6~J-p`4BS;_l3ohOVv z3uZV5d($rcYTo(g9?gm8TVgZt3HLmF!@bb%lYhuon!s}kXPXz;4ppM=}M6ZU%%5Q9HNSe@Y{u6d{12jqOI)o8Mzs?;eS?bzSd}PfWl^n%kHg_ zvnX5_)tB84m;F*Hp;>C(Rnx>&a#a#F7Bo~)aQT@7mG+go}c z^EqJsvyQ|yJhz)K^V=CC_p2%2-l^g(RAN3ClyCo?@7Uv(Z*SvmjtO`4UEIW7@c~eh zr>n`*LOHsyoQ$k776wTx z)2VBa_MEEO@iV6Vei8d+F*l|4^{U%=*43{ZVj`hB3xBj}^j?}$k2H0%i^;tj{bi_a zYb$=I;5<($2vd1cs>45d#|%x{S)ln-cL95TlN`)qv;A;&e5%dhrDAVV%{-199Plz@ zPE5K}*be652Of~p^Ktl5=$-W`^w(nl{^wJqFD>__LyfQz@Na>9wVyxz#lOzZfGcGG z*8bxxLVvX|9};dFBnA%1hl*UpR9*A_`uqR>McK^X+S^Zm;hFtAy|UtAsyOm}SwFrU zFxM+*F)p*(Jd(XGq4!8#4XCEoG&E&VeD@$Qt zhQV+<_WWRBTJ9d+r-z0YGm3NK$i#Bm`_tIi#fvU>F_?#IXq*5aQnUTm;zi{I$8EL7as{4WWH2wDOvG%Lx)-Dq_%4j< zt$&qOuIg3FN5)blF(Y!dR3~4JQN2<=GQ>xRgc8r2mlSM@5TBtw+-Im!l=Q1(p~ zuIg1vP)5&PlVeTSE1~UcDO{!GzM-DKOK!oCA(^^wPT35{kE$Ll>pBwm|VF~m!-OA}dkRIT}K z@h!N9*8Hv{exjB&Kirg+nDf(NcqK;CjnTa{GB3m+HDTZP{$*k?o@{MpHt9Q{)qen1 z$c@IS)jXm_x-=)n#w7jKn4}Xn;d0CLw`rALYvXXYz;$tB9R8||!(EwL>WdkNyWe3} zo2NGp*Yyebs*S_NZU^gc9PUYSV?2&=xL0gT-x!BC#^H@|cw-#S%=arX4!32g)~g$b zdz*;sl=-Q*iMXz8vg>UkuJJw0Reww0xzP?D?{|i0YX1i=8B`m_+2eb?O#rsC0i##d z{?c2k0PNmV1z`8qDge9pWC7T{uPFdK&kOq5DGzcbu=TvZOpM*H zCIY)B@wHx21ooJ*@p$JRHx#?oh5g&@(rX@sgx#J{Oi&WEs|ndc0lVE6d4FFj5PHGo z*AZQQ%i3{!k=Gd3P1So688pw)#Pa7F+HrdlC;wV@-2C=Df<>2~9PM*7$i}u?*S4GA zFtqY6zd%_i@>AJ+H|E}N(cJrXWA0Tl_o`ZIdj&o`8%yn1WvP{_)xMaewpUzyWn-zW zJb-B1Wg4Gv*;3mpuIcg>Sbu7ZP0cH|)b@+*!W&EN#!|bn)NU-bnfZQymRe~hl<(A1 z+o==^wz1SwDO{FT<-4-fo@RC18~$WD9s& zBa>}s_o+;_Y~7D)vQ;utg~izgR@tt|8!UG>!15($TWF-B84Y%UEq`=dG<9$pY_x}B zo)ACA+!-=dVoIYzQu_4M5zGc2S^rD!EkD%W0?YqIZ7J^~of?1F+!|h}-^8v`4dH#q zzGFYx+e6ef)%L4&9W{))#!r3z7A_m+?#4QW$CINlP@-ux)Pv#8KTch^YFzW-3^3}A zjS*>Bj-@d0JNQIlP=9O|4S02Ncyayd`r_*U`*8B<=;HnL)y2C{KVO`^zaAz={8~(u zeTl32#zgrjH6hcu_yXoi*<1Wc&6s&Cd}*8J7W8*Ti;7a&>^<%DeWF7YIe}2%6H6wCAKTX=v7Har2@fLhq0SupW)f=D`D+-w;jDlPvfJ>VJ(EK8aV6o2sZ^%Jnp4 zds@WzGP^wnZi(cBa=Xn}b?K2Y?3z=uxP=4vnEim06%B;Femp#*Bd{$=oOJJ#CY!5NY@aetL7@w1GN&%sMIlRgN%`7&3}eT&xDN0Fztnqwirqs-Sft_ zzTCQ4+lQf8uOBg76?maRjf!&0UC2EdJlBN{jvhFxvpe@iNpl_*sfnwnHd=wlXwV50 zNyp2h*Z$_nu$TbG?saJ*XWPH@Fy?R`kTgqPWggnxo_UG_pDUCGM#0-yd%?FCg5tY9-# zC$id&D7aL=j00?6=B-e+7mwxuJ1F+M+c>~h9zd||1DXB241-HMz*ZuK;<_!@#!cjm zRzwo@4i@xYL8cdDS^nHp!8J3PMt-?6i_6k0jXi(Ge#T|15U*VlBvIYlScs~~&n_c( zk$-U!R~K|wQxOsMqu^O7!;mE5-_jVWtp-@dqKd;8;zeEJyL-u1sk}3*t*PYETRx?T zA<62bRGE(mzj6UOGdZaQ^^^>Sl5U!t$kjAO%a+Tkf~D7dA4mNxLH=@aN^PDh562bE zb5~x+PHmp$&#G0-QZA(^&$2|$uQtoov401-U!m%JO6Q_954juha$@V(;-t_0(e)Ol zp6_=$>*Ar`)};=4CBEtKV6eQ%+hU$@)$ez!xTfp8;#Q|}#Vv($Nl)jhf3)yC;sjbQ zaG~F=l2T3Q^|m^d>uo7`x~_$zz8GxCK_aDF9n16D?E4XB9P5*HVUMY}&IDO?Xmelkgmw4p5842wx zoMJxxUacUa)d$mDk(nD1tsthRCwg30h_@86)GkZ>Myndq#BZtvyn5m{!zQL?6H`Oh zDW+z(N_NZC95yjEo0ytSOwA^y<{OWxG2zqazH5Wnfz4iQ(fEs%McPDNt$!xgh6WC- zPrQv}I*$``Q#O!F)D4@kEb@khWk3V<_?xl_oG1pTED9%#!+{B?=Bs>LF<9f7_I1Vh z(7jlPIG<#I%=6}pib&@qC0ffD3i#32H{eI+x+5~SK6z|Pd`;B$2p!TcmU5G3d+}-n zJc;=5OkiEEIyz;yUUbS5*nh=(!?p4HU&o5jB8*B+m+93$zK8Q}y)cYTNRZSaU=tD) zq2eoY^4?v$dbGQndbE%4d{srh2pHfs!rY&hHN!ULJW)>we-iU@Y?=DKHGV6 zX*G*W(^m9HUa7zK(o!VvM$O4jfSTe!gu1AyF10PY4oTITn%>!~@qf)$@`NsTyj+1- zwt~~;#-~y;--J))pi3)1p(B#sL3^OyUK_W{zRYo83Af6gR1}l=!L_&_Ts3!`wREd2 z@(=7uwGR@!pfgUVvM#v?1lb;yX)&8$eb5sUV`z3zoX%Gxubov z&VPlzZj~xR4|odXwtrqZ1+s#(&$_#?sh52X-FUidlmZzpn5xt7tWhdsxCd%Y|H~JV z%{Iz3P;kS#3ter7ff10<`Kr}SDr9n;J$*QZoVyZh_ zl8z~rr{b-VSD>xyr&I2qqw1H_Y8I4aOR>ok^R=MjY-S4+^?zo$IcJ29X`vrfTFaCd zsRg36NQ5r0J6kF_t4qv6Nx9d|mIzTG_n%0hy|ZRk5NGPoMH=SFy(tWI5RrWwG1Ag8 zWj9`9t&U1vO-VF) zaouHs^&{Rx0e}Ck=7~1<+*`}!j%d^aMYCzo+QbMDJbnbXL?|FkBd$M&Q|Hbh zw9_GN7n#)qAnS4D24%YcXf#gmO3U6O;QpnB4Y_3Ds_-W zwT4r!H#&y}KCb4g(+UBMdk!F4|B9S%L~22iQh#1-ovrJkuww57f%8C;F%&rr2te_O z%R!0^=LLH4B;}MTgM}tLele-_AaW>3W26{fQmeD(O-BW-)Loq-O}wwMix-XLZ5#(H=4i22NvER(s`u$;_~z8_0ci} z<5P8q`aBLsfOnD(38*`l-B!0taYAv?Z0Kz)XQw`wYuEc@L6)7xfR=~^%=CNo-(Iu5 zo1F#Lk*1tf<5Q@_Atfweg^qzDtE-4Bi@n2E;}BTQ9jn9xOzsT0Vn@K`oB#`*E`OGB z{;PD^>f#g_kLU2Wv;!OSHLB~}+R_zL+17hBlzHojCL?$9Uh0Cc>C&?bf1+yCCwBR$ z#18c0%(1KreJzH-%Gh&zIx}QwZ>`DHhW}$YZaHI+NGeH9?2h)ol~k{hIv1|C=lpJq z1(jsS>;0DZ|VR^JD!vj3HG-kglz)wz}3e4!3k|q^D@i$ zIEAdDz|kq=_Dsbi2x=l{~GKy-T?L9eGAXwf!uI7I9~R zudLL?MDO;vi-|rEWCYG(yPLmrzC{-keU&aIiH%&^&xCoCFgKG~+<$LA(GeO?SCHo@ zayt2^c16a-BPfikCK&SAKEiAgQ_W=S`Df(xf*0eScFhvP&`;3urJ)^7UDqcX$llQ^ z;Yze|k+gv*vB1V5C3HxsZuBls0U=T@^Bp!5>Sdw9Id=k+7U$#%lvLM9TcHcscqLOy zO+$+~fhT}_PP!{(>Ti@^}LoGXbdryh2M{pbqtUVcYd}*UkhT^}p?MHBkzO)G_buQP% z5+v2%Jh{O~FEPVEoxw*RYz#iDQ@M+adOV`Rr^;5hic>A9MSq+xb!FDyr$?}lw8X}$ zYA7vEMzFDuZ0sXny?vx4VZ+lJMf4`#{4I>PYoib_XbJNfJeg4>7nwzy)-!SROBCEi z`m;>a*t&u7>mvdV98{5d-efg+qT{f;0Fx(Ce$Jd6$v!pY0f`6g)C&{G5gj*`w!lGU6$anR~ zoj-VpL``co_4Z5n(bue_#SKUaBNRv;G+;c27a-%=(azw^2MdrK&H|9J49C?>OS>i` z2aqwbj4i5M@!fH7hhdF(hEKK0a1`SN=cku_?c@VWs8muta*rR5F0|+G-G&AiQeW?B zXGhmK8GpUeYUuF&1SPs#qA_CjX=+6p*NG(vp5K7{H4TFK-m6!4cXy4EYd(zJ23aT& zrw$~ER+_H9OQV6mPvUHqkzBWs;*v}TIn|A4cpx{uc}3&m&YY+S!T#3&15ir?1QY-O z00;nehKoyk6oJya1^@seGynh=0001UX>E0EX|oG&)dm7{hO^>zU;zSjhONC6Ib zhKoxL>~?YF2m?U}8CU3qZcf%*5PpZuwfOd?fPGZ!d&fO}>w$t@V(M{SVV>&jCHgdf z);3r}%dUe7Aw~{<2Q<2uS-z7;iK;pCWrZ9nf8|(tw)kgjJ?lm=IJ%nNIUQA!VL}P) z!=FG||2je8hiIq}!70>w^+TLDg_;IHUHaBhtmLa98pQ+cFWkRxxYFf39YDy9;7qYfA4IBsR$9G+tF?s#W{P7?%uKQa2FxJNmRPQ;mYhoPxW&pieKy z?aBvvTw8Jo)f1!-Osi;AaxB>af8fg-3QN^nIxxxgm{i{O$(m zXO!#AwKWsJ_xTNP)vG+^r+SDrb!?Y$K9fwu1s=(n#v~(cm}F~y5i>!C{O~}fkmFFa z^{P8XxjwwWx*4EnTrl-_5OKeU&~r{u;9%&C6Dvt9zGL5KaP*cg-i=*be@j6xLYe_n z*))ED2Bi=(+Vu$=I|~?Q-`;Abo)x3y?_ltW7-RoY3k2V~J`F(|xHmb=VX36ZR9%yA zaa@8_nu2chdZ}&Ffn2Hr_xSVzJX|PC>A*dBwMl{&DET2aNFbsI~cQb9zG}mv(g40 z+nT+YaF7vONqY%#9cg5^#K<+oBzuxOm5Kt{`A37t4It-K0p--@|-}i=&Rb(`6eNpR-@B?41u|BV~34O1LP%W*&bfdhJ{LI7DV^ zB}{5`PzG6oO?6Zy%eRU^yEj*$VESbNz&dtD^S+WN<&Hu+b;y>8>zoh-@{7M!tXD*9 zLpL3vTBvC&@p@ZBz$JJz75-x_#@rG&9j<(Nrls6A#QUw`pP}T{>Yk??#hr|svNe(T z%BqMFqlZnkZ*cxi`ul$*SXZJJ)xG6W{>bj#A7U_9PrA9Y$Artp>k(>#IvzZ4S}WB# zR53<(P(Fj#YQ}qeLdCkO6a;Pqac1^d4OOl&gN$LTdnAJoUV@%7S0Y>Qbc_yiar+f` zN?en0;m3*DXNFt$v-aNzR}U_nwKt>5;2nb2=#phJ?&Si|mWzK>zO9MD61bfs20~#Q z#FR=#WVr6^Rxn9Xo%Ipj+dn*au8JrWNJMbPVDTnM--|(zzd<_v@AMUX$ntLSFGwm7 zCgeYA!_B!=+G${1^o5iBSlAX3N3=d+Nvu~hTp$Dn-jS_}-Ge7Fng;+o<$Ch?{pOO; z9mS@ga_H=4Ppf~=98aNv_lrFN@V$QkXa@bn*1vC|oYBxM_W;nu_ib|Jz|7Wg%@BD!+|tQfq}nvg&-Ae<|1a&Aly zUFAqmDO1mibKmoOA@~8AP~>PWrBskg@_Su-B|m=Q*y(><25DKd^_bO#=j2%RR8k{p z7cPG%A*F>?Y6fO0;3F;{y|pY_HG9a*-oi@6mrI(+`XJZe=BoQ&`qTrsm1uv4x3^_9 z2zTeVsCJhNvS3}&e2)*2=tn!YiQ5>29q5!rP;vYhChB*b+|-I{Z3nmnP5A-e3#zw8 zzQOTLUBrKgm~g{-?7Hc(^HYrC9o_;d45=o4e_WC%$^p@vu@0J#vyiY6W$UHata_cH z)o4t$1;U}YPtDK8xj!|S&(2}8V zVglvqE$uT7XtlZzzK4yJ&4F)RN+K1xI4z>e(Ef5t2)CBn|HRYN?_|-dxb*Qp&QXE< z@u6Z}#y)a1beq2*!vIM!>yGIiDCpX0Ce00vw{Vu!>2B|I$!?z9V^PN#%nYf9KhJ3n zqTheaV;8(5)MCW#-GWN0XM^_-7Yf~gAfXtg_)tnT&4vL}M|PF?eS4eKz!bfgsgBkm zxEw@Lu(BjASF9}~ zOihhyLT6 zjiG~h_UDad*!pRpckGRYGI$P68Qcr>ZX2u1$fxZ`psB zq5}VX?X3Nuvjux?yo@Z(aYhd73(MZ?I{le>TtYAe+}P9_>q@}VjSkNH`qEyb$rhD zj&p!8bv1T8K30_6@_BD3CETxW>WF_y^;dHVQ`5XEB2Kw;D{VuK9JN85+66CP!U0g! z;nQJtJ$WX2=CmU`-zcUc4v9}1zuez`0lu0|{1$0wlXa%Q+^|N+)?9F2<-ytJA)jNb z-SFOq9A%9~#}o2HrUzJ9Aj{pzWh0;anu+}tM?V&J=cH{3Ek;~{>kk5Y6S#jYe;4n9 z>+x@!1H-FJvM!ti{-=2199q zbqYtJb>~xiL2*xf{wW_l^uK>nG)fO*pJqiiz{J?2Q=-I90T2ud=0`g;7h{Nh585Ze z6@%t8=K-po9GJpgga2mKX1%f#c#!CLo1M>@T4&yd->ufDzAb@*Cin}{{Ck?(U`e)3 zYp`uuwHbr}{#>W+n{H?3^KFmm14rSM^F9)At4WKiM-Pr$OWQpk#Y}&t-;Up2mp(W_ zL%U)1GU^~aTbPn<7@Ko;2;D1yVj$Ev;WX3F7;J-Q7$}<$()uKIa>_M%!|owrQ0H*) z?7q7ET&z_)xEz$jo#zC5(B*8*F@6v%br+z5@WQfBgkzNjVUX@%BxI97$EeXtfI?}U zWaa)!mMvf*+EeC`?J9o;JbsL9`A;y*Q?XAOZ>i{qk0B?KavWwSI*(HVAxdz+v}{$w zbKj2!*~n#zX;fX?l+0aTl1O26fu>TSc<8(A4s<`+zF^4gC>y`}d)HcFz^mZ?UCDaX zU<1bz_msNML6>?3fg?{YjU{%4PB&!+JHculrSk*y09$g-TlTMzVb0n2wOi%n z7dzDS=?6>XzKws0tP4lFNeuye`f8r8(S~5~w|%9KHingM$O3kpi_|zb(8cw~@(n@# zc{WmGBOn<`c}SQ0xGNWkPHMSX@{KU{3mZ?51mfPa3zrnEPoU5I`!-)2lJW^ZB}YB2 zMGp_EKEDNX2P`11Ee2RRD~eG1{0bZ^#3$;XwqZ_6WUPP8){1Plm3f@69 zf>UOA3KXi>rFa2>)I4jkP`9?&k;`G9N2cB)hnCZT%ntr|?cdo0Rv;IYr{SuAoZL$l z_}!w(3}8H8Y}k?e(al%MGz6*f!Hu7rrVn$28LTQhj_0)f+2$#$FcPb1IPZZjE90w^ zG;vR~l@fn54ZMr`GE|Q#K&C+&@>+C0La@8xSatoWj-;vCuridx>jb3|V{%90_I9yx zji4I2Hcr}Qucg0RDX=MU=-?<1Jt(7Jse(HUVyuWrhtmL6AEU@GiX}4m^z%c?4jbS>Fxa|UpHQp#RLRh%aWT>HfELudPIS~93pVuv>hKe@aDP@tU6&WRDavE^fc zEnM=F^8n#pMi$Qj9mypPLNO9*Dwi$@1SEex>C)Y)+ZXGi!2|Budr*{_Vd3d{g^i2@ z`kFp$HM!3$K(wUhLTnf1oTb=9p_(t=VDA8FkP6=ICoxz%VK?$XyqJ8-u5*L8?$iZO zJS8=i+1%qya(dv+hG;4h`AiI0N2rJV!F9yMt^k=$JoC_+ko(F=hSopMiY9!q^eBIa zUQmDWg$;as)LrFq!|~hBk&y$*yYt_3J33O|O57^!__i37V?)Ry!p^?AE>MJC+9D1G zrOPpS&8H`C9sw1+U6CiV&pIe=oYFln)>x_9QPf{0>h5um^uVn;oi?_ zzx0R>murJCcN=J9E`oO*c&8ZPmTT+eQ2}@unI!ji+uLofZD$TL2xVW)=CYHj-|^1w zvEcpN5O1BvluVhB_$oAXXKdpko*|8ik&NfQHgy9z4y9=F`ISuW!(VS4-*10czjEm! zg}4Po#Q?3xLEm5s1c8}Ho$aJ-kE&A-4{MiEy{za2ouW@*cI6`a2V)MdVKKGwqk~jX zs1D|9?#6(tWmw4!-nKv!7T}PyMA5uoIE#)3>t8j=51Y?dZ2x*~0I?z-F!az)m`br= zyJ`XlNA-EygV9Iq!OkG;7N~z8x@~B{d+e@Zq09lH<|xMsm;xD=Ao8WJvNK$fNoiGI zX0x*`C=@=<92d$TexRw=akczuBUt+MQzgZs(-G29VYFj(cY(tu1Li?Nd|l*q82eB> z4Q++#c144ZYYQQ!muf2AJ1LooI~rU8ei<*>kPNc;x7=k$)J{kb6xk0|ZZ*+Zbs)ggjQGTM?B*!uYz% zsT}!=HaFyeUo3pwZ_?>S5#25Ke^;*ZNuvV0Hg9{nVm6)nldgJ8DVttcDZZH({kZbX zOATsOkVV*umi0i3+YWy|D(Rep!vyDkFH=DvB|!y!E#BQ}2Lb-@M>c9878EgZf=bi3 zfY$Cet8e!GB+Yk_h`Fb~iJvV#fgf-)3a_lxD+%BRRzsW zxg0+XFg}LgXcq7BYJxudx1M$NT)9q{qz1{`j;O+qIkt#%MjU^#dW(18e%rT)sTuyo z7Z37~4g{zJ)R@FsAp?P^MMxCvt^ggAe#3iDv6)fHe$J>wvX9|`xt=k{S*0T>a0}f( za;$#^g3UU`bV>szrpWN8D(Hd2NV}ma1qO?4AeL`qOgP|cb8ZsIKj*0R6Hzp_#ILR; zu!OuB1o6w)iS`C=BWd7XB{Y$_p=Kh>%BVb%~^Y-IOwj)^%jK z)KBm;jvzsy|K0Jn?p&t=A{oUWKFbAW8RyXuIT8k)WD$gx2-J(mfgX9=4ysyBw=}MF zSBS%xstg1^4d%Fh*^o%l1oLG=Wto@t3D%Q5x*cu%LxBTTHQ%V|>>1$X?SzYmsoW|nZ;2C7P-cBrCrDf zt<`ZP;C~!t!>o@o5L|5gq z51hq6VKPc9@iY=CI=9%aYg4*^M7wvJo!^o*!QeJm09;m#jJZ)OgGV4nOJ z#>YXO{J2>yD*Y-60NX*;`RGSLTO|Bccf)=MTs? z9*RRtgBb_Eq@WG$IKFj49pbGRNZr)0LYr{-qPM)m17LO5_;J2#d);4uVg5~5_tVN} zT}M*beaRsu!zJfsh!TW8;zS`)(=IPS>S_<~H-E-+xp_~Nj(|(DK?jd-nx#YRhMmp5 z1J~aG#c2|Cd#zJX1FxSj4%1wa6b-f?XmQUggk4&;zge zXD8**@JhhO)j1ziRDjW91^`5{GJ?09g;^pU>+fP<4>M-NC43Qo6kjrI(_3pu*A&JK zt~EN;0zTA*A`Pt3J35Z|(2I%!bd zzPzkJJ~EJiKDo<(yli(K(I{bXX@Y&_rA@(uaUof#k6O!)t;sPr4c4H&1jA`|J4t$! zH`8o~v_tJN-ZB)ph=e4*WQK|jIO{Kr!|JHix0*aA7($^`6tz7u3l2W_X*OG5^Tmd+ zukf3vORz;1JGn|S8$g4N(u#1S=Xe>8o`_57scBBHeQ0qr9$c{K-6q^tZ&z1M`!dZ%1$Mj<<;{;Wsp#|zf@>f&DNzOVb1c5Y&{v>*-F+Z zjd<5X za8(MF=n+u~uMpGsE2YLD9PNEBNZ@KRKwM}#MZ>XKf2rSxV|s*ziPn9mFIbC($5cOy z!gibCh*WYWc%_O?h7hjFkPSH;nRIk1qJs`o$*+n=HXs_n4VU07L%+-gkmlzeX~Uor z696u#yReV1LW&f+kLoQf2CQq?pfVvosZa0q_E$v1Ej_<>~l6g zGYMVYzS=yh&ZZ3fiTMR638iK4eayFqyS+#Aox$+vy(wqqP2)4R!%J{XJ40Hr-d?;( z!^2Mc(EDYQSw@seWm+_{z|RN#WKQFo_x66Z2NtotW^XxEhX@5&hMh{XcZzJq2SE2q zf9qP8=$*&j#>(DkWjl@n9B^4+KZH=uOhGKx=~0F8e`jddD@PD+X^xiOxmEHYKLePH zxI2@NE)SJ1ma)Q!C0Qh;eHWyVkH$i*gQCFBbJ7z`KuI*%T)cn*NgdB%y?MN9WTFvj z>(twJtif~zLpJ@YqpR!d6Xt2JIq)a_e-e3PSC>OxR@_YqD)EQNQlC@W(sq`6)+;A? zP$w+#m6`tB)lKFm^VR|%B{43uZgFe@HR2eA$yA`8dTi%9QykjNL+Y8vUGG)EsF-#0KxJ?Vfq43ltG=tdfKaZbWgKk~oT-VfTqP?# z^n&BC!bY>X$s+sqr&)$7IN)?P!C1@t@$RW{{`z z^PYCRjY*x4^lSjn0kWdAd(Khdf3R*yH`j6%=Xy~-SBBHHTFD4UTf6+V8+Fdj%Ggdm z>bv7NyhPZ{2um4=HvEE6DAN5WHl&jrlk_e(7nxO=>VBqXu=c9ercqN}34 zz{_WvUg+s8SRoiIe_!G^51}7b3iLnZSydNr{CzlGF;>2&Yhg~32(hV{@#-VVq9^0a zUl7wa6;B}(Xex`*CVBn5)iBFI&@d#JSRgV_yz{9o z%vlP7G8=IF!OY1fya(>Zn#q6qL-_oh<_^GY`!BHI2S41mqV4JkPJmoexS4e}5`<_!ZGnCS2DQyRi;W>pQ=T z9reOCyZu#hZoxIAneS$p`UiAPzf#P7*!9qA2>7SpTjn%1kUcy;GTn|zL}Js($_E!~ zbw7`SK6Tm15O22V<_pr^SN0z##nb?cjtSPSF&y2A&zvr4U$uYZ|Ni3&j-+LuLsXeS z5P*E-f7IuY+OM8udf@3xLKMP{g{d9Qg4KE9dCdkETVUf}IzIULCPQ7lf6Si2#a|HP z07=)Iw9uW-RkiwqPJCiP62@#3qKtJTGp4?Amonz}$L4MUJGV$)8MA$cgH0EzTL86# zukgLUsjvFX-K_i3KS2EGZ4|;0u~`bF9oJ}9e|g$*T6o&z2QYm1w%NdpB9_)hwOUuD zZ(Fa9QfbTwFC$Xp^mNs?dN1ipv8VrstYFWgkZ_gmjtN3qA?UeY*OSnyYDaiit?|uA zS@*gV74OXjNGzs4uT@%lkO89vj?Nf+eR0}*kNmcGm884pSUGT)o)tVH;Y1zZx<*AA ze>XMTH~rukJB{c1xY5Kl1^^~r_COxdhgz*I9`cp5C2t$?;-Kl~y}#f)98aVH*2V@;jNi?gf5~wi^Kr1&7TKxJ78-M)12jSn4`;KA0FM1J zRrUK$3GR_{3-S^(+SM!+4 z>CZ_hxFr8YtG9_#KEz3i8dPj|={jy>79?qCYALVptIV*x?ORDYg4?De$l-g>f8f;| z&3G)Xa2ZBz$8*%gmrvcF7r}lKPYmmP)+;}eRNt(na~K|A3h4BkTzv~M5y~!94`&{g zv|3-Yu?z&nNyHbk1>So%mXPuukmu7|K}Vav&}0yoNaR3y(f>R_fFa<5bH}$Eimven zbEz10^0^o492{M$6ESM^4c=<}f0x~P8V@1l+?|9J#HH1``VFD~!4vk9sIB%d;_e9u z(X(B6$x7SZx0}qvLFU#F6$I0#Iymk=74H+YzC#xb z;*Jq0IT+{&;($INzwws8eGA}0BtrZal;H&jA2A&+k{Kxb7;<_he=mEQP|-e9 z7%`dX@2YBsb8{NAxy3f>h+yDf+qS#dB|Q7Yc&8kM&KTq1cI?{P3PF|#dn>gMJp|o4K94$~*1M&}g)9Yb~Xr(TZ7yDRBVBNz9|2{-TO$$G;-9Ru%0+u_Vcxr;{&3)hlG;=q9-$`}JR&zrx30JC4Wh$=_vmyQBCM zic1R>^E?{PtVn;Kxvs9H4)3uFVkwmZ3#6|Am5Z&J#8JG`M6<0i|3?gYfTP#l$D~Y6 zQI{nj1S=G5O!1yaZ4%yp;7Fco=MqVzr4tF4WFG_~7ui18Edp4VR+Yust%=`X<@@g} zd|*!*mz^I39z&_Pu+5oJ;-V-R1;w_|RhFBbrw>j$S1gHFa`^V=k?l}0@+mAB0#`B% z$EgPNBi!>QZPqg(Q%8TGJo$|a7=hapE0_Ks1W14O=So+J*D90pIL$2`IkqdWK7*CWwUdt^mN_}B3Udj6C6!kkE;)815+|g!zZ(pV#0{gR0}U) zY*K&l>1Bp6X&)-;OqIb>f4FJA99B=9$~gbZ&n{lG*Hpt**z#x%9lOf>rHHT+g>@hY zQwP6DYU=m_T^{03BaF3_^2(F9TMWuVf^0Uh{(H~)a|6m z<-qf)4xNs23bHlRu`drPWNwq-rXoNkGRVBa@TbJ7s zJVt-@2Y(9i*JqNO1)bqFdj0_L7Xg1nHXulRlfILKCuzysm^dLh9_?K06e2X{+$w;~ zr@G)L+jU&TMaF?4&=dJyP9|{$0Ek|yRJXJywQ?S_po&qMFd`K{Sj10?Rq&T9D=E!a|JCcQ_ zQ3r3vXvl|MzT1>Sy_aH3rW_8-@MgTON62Y@oO5w-0;2|xXTkZNy3*ka6e=xMl%IbM z)a#XReLPAuwVC{tjw0`5m!1f9nM_-VJbeUma+~dLo1cI(KwB5&WAw7!fsak-_*~J& zj({z#N_6oGIrc~5ifgqW)Wv1~9J^CA!Hj-yF}yB_9mmJ_OmT<_pG<@s6KNO2O%zII z-#X%*)I1x@NHAQSmbY`iW_>c=%mtVBBLpNDc9*u74nld34JJrB8UtPr88tL?q;wLO zF(d>qRywyN15{U2iDBs)#;L4l4~$5{AilP2j4OXrcf{ho&5-HTg_KA-a54hnGF`YoG({l^F7l&jB1SocY10cr(u3e1=Ve!&*)e& z(ULt6?G1bd7Sar~A=-I?f91r|Z6j1hJ6PDv2ui^#5}YEw77=Jzk}8cq11MS1XSRaG zMXVJT*ap!P*BD~n;-?K3Vn z#S}UPRAqK4-zq{PKDff{iOO#Z=AEOtcfwiF67lz;`+IZ=QPGEAf4%v&vALqo#=4#%w8GmZpsBqmXt%L~r!l`f!Gj-A6 zQ%~28M+-)lg3dzI_wv+y*r6ge44<8dGZaMy?f8KlhdM7-1s3{HOX)pG99K4rj&_6# z0t1ejO5AjfOYg{tSaYY&JqeMri4A=|vKxq(Ih2m)MF^?6_rk44#e_Ur5FKt+L7fZx z0%k|nBY)))aaqgQW&c&28f!!@wKff7S^7PajCB!-FzKv$bnA^ifoH_MEJ*J%X9k-Z zsu_d5mo&nj(mxE8(}o=)!ql5f1oRceharrPZNr{B+pX%v$QITc@pU2;mp&b>??c=( zgz^u?kwF$uU)I6U1|ZR2OjL*k6M?16bzo+6Fn=~bhMPc5V(T+#O3>LdCf7K;g_>21T_!FmHL!4DA*zOq88G|N-UT6 zDg+)N%HS`AIv0N5HYpUdqNY+OeX~z$sujJi(-{kj;7rR_mo+N{EPpBDQS(H-qhdjN zh+W58B-?2Us~G3c8k@s2!riQUp`OFgN+9AoIjwDfUA8P@frkfm_rs1-bM zwOFB?bFO4}xM2xgq-%W82DWN&9}zksF+7czriP6V79^23H-GIzzH9rig_a~MW2KJm zbV|OhUORd5;dzo0|7H%E+qQlnrzL=Pc}Qhuu$Q$hb-QBrMTLJE3=q&1hYo zrJ{GRmC3@rJAVq2{{<8}jZxUdsR`dg)jJS-*B+ultS`&pBvA-~aEINAGVfeY;myt>zXyLq@n>q@*?ZRYJy0m+1>5MM-mp5pS5z;P8J;8mtIwZ0=<(6N z*Ybf3Uw`smer>*GTkf}z8~955DqD;1z0qavtWA>rk-*Q5N3W>WgfSB_Qz6CzXjenb zTEQ3K+qniu6d7nQ1o82b8}1LbudVR|mGI5Z;1!v@HFM+4K#s-$#m{MbDK$E|+L!3E zr%3LacA-V)Vq;gY9-neif^6y~`wp<-pY;4~`hU}T6|Ta#DMFEbMS-v}Ne@Lg(kkCS zXqOTpS^7@K8+VrR&J6cXiJ#zubOcbTmE%K$1GDB&!U-|oKkgIMPU*u` zz=K-mnwbPEkEmd?wTkj+BRTeK%6FRR9Dix2vgrmcfyBq*jo75bvZT&wq0}tbw7cr_ zWo1~er=R?=G+TzbMo0s@w^61?O7y8c_snbCxlA?!U~ng!UGX_ds{Iu=QBZ0Id|0#t z`xw~p(8h*t&%j~{#A*Lw7s!$rONnn5b+&6)@+4?Bn_K3 zyR$R*UmG8V>dv#iJN+>6D@{xZ+kbdgDZX8gcn80GIJ+%334YDAK_o^ zfZzJFx#Gz{KVP`3WVk&Sy^k}k$u73D{^8%CJl?N_d<}tw-?#jo=boo$nD=^m@vlPR z-%Sxk%P)tW{HaxbctoREufVQIkBgJG4ZN{$z5ydrY9;?=juI zf4W;Y4xnfG3Jo$zRtk+BUcz?ls#W1(C=Rr%gS`&l@>{Y~ky#i_Sx_Q40XfyG8?C-| zciY=w@6~~@LuDmT5r67|=oQJO9ka)u3sdTd>*bnZHW^1PE6rwIa+S2Rs@3T0nw!MF?HVqXMSoQR%c$Xi1a}Zu%zz{} zwnj+aY`*=th7L`nIy#jou0gt&dwOckWz)#t<7(=`!U*P&qih7gca|t=>R$X%%_={& z#nGe%eoC;e1I)90?Fu)?f!+V&6}hA zq0=Vju#-q?q<;m58HNGO=Q)_K&?;wfU#=O@$pAmpeIM+LRmisua;kG9-ll-WiO4p< z@`w9YVaO0`8{rDPQ+ATtyVCdu?_`i_+GZ!kgkv|HggkI~_eA3=HYKA^+5iXfc*U+> z66DW!FNqMj?v0xGF!(yRA&a_I2i!d8gy6Nu7j(STq<__=q_K8Uwg_BbKnLvU9M&JF zEC!d=)v^bxTZktk;J`O(kGwdEZTEQ;z9!`wQya(xVa{OO!)w<4Qr>5EkL~9)FN(db z5-7zz8qU92qe#p?g`bTJ_7h1qq`Tuu_rHfx{0+E2OPZ@c`yO)wUl9KnZ2k^-m~apv zAPXp<|9=H+;{QL`a@O|NZm$22X-#{_O(vwDK11V9L!D@{Hc^mDs5Bnye3i<@HbSH* z;CgT4cyH^#?^z0kLLwctwpJp)7w5OxIsQ)8&>Z)qV)BgEiho$D8Ac;h&S}vuI7j({ zecb+jta!^))H=~}GbY!O48+D-Xjn5HnX_ttNPjbwQN(px(fr(XYfa^lgj2;QUYS8K z&GC=2RGw_-bZa$k|9R4K1F(okLVIyjuG|^^MT$nS#gNST#YTowXGifRIBTcj73)l1 z29JBCmLEZOyJZ_ubF{_l9UBd0^wc!fu!nrVapjJ8{PaqZ?=AdRf$cMg&7+lRziUAEPdMw3oP(FwD)qMG{%E) zRa9ar9)?ofs#2Xlo##QD>F}~UXifYy1%G?Af9c-A5aoSyCZQ3_!kd^aLdBrlt9=3@ z_Gd%j`k9=0y^-%+wCOfhjSE-?Bo}%SdN$mzBf-xsh_hH?Z)3zHDM0~L;45rE@)zjU z3et1N>N^OabD;|KR~vXUMJYjNneih4qqLaf+Fof^jX4N6l175I0Ep6q&vdk)n}3{? zETVBPB-qzdHt(PAM_mlF4wbvd?xLwRly;;_F4XASzti1__KZ(ybsEcL43bE95(&LB zm@dW-ge}g!)kA0nsc8MG@ogkA4C~!<4f&@V0a2rcMP;&{zu6x69rx=(q8?UrH{fMU z25U(0T65&}oq9uzL`a0!5wGg>Zhv&3ig(U`L3j8O<|Dx3O5|T^mp_)Q=D*$8j0jU9 zjQ2f2^BpqxS=%K)zco9Y-qBw&)B|)&S-O2-Nkeu#Bnk^&lnyybO}k=)n{tDG*aX1x zV-Zs&ay(uh-h>V?2;aZUSXyFD{W?$`XSuwGA%GJ3DH^?z?7`oCN;PVpehziibzRqv$DUpb?$k$KLQ8#pNqv5%O6$f2 z5)|hP{|emo*BN6Kmw)Kg@11>`^8a>q3H~+@qwcxKe~=TN-nJWdTjf?i1r7y<?ggymzxG=xg8fvTKjL>j zNbAha?-yZz!mnfC$W@?Fzu-^f{V&v%Va0D76-l7Tk1CpR)EDZ@E0D=AisW@Zyx7lm z@>c@vbw6X%`tQMn$uIK8*X#H%f%e<4mfmk(u3|?)AT;5#nop^xO7tu{(IdZyaz=FCh$M4h6-QDJ;b-*Y3dte0q z52Ml5?>FAh{{aC&{=ZC6f4BWP1pYq;00uz$zl65|dISYP1^@;?`M+?t47vmbKn4I8 zKZ94!KOhPO2*1^5yM00}_$zf-p}It2(o1^@{__P^w}L3#xUKn4H_K=!}g zw@2|)J0u(xA61_(d~02e^$zq+?^dIq-+0(geEDv1VN zF$MquK>ok9r;WM>Kn4I4KYN+je$r+qRP(+qUz}`<^p%W~!#@>#DBm)jzxIzG^MmNUBYZzsJHMyvW({(uKq4y2#HT>VHFHenpRnFjtO1j-C@+jJJs zki!0S)^-_(1hQZ@18z5S5I311Jo-OVd&~!LtOY-95woE@BnQw% zYXRa>xNXOH$AXQj$zN(E*yFoO7=njd-Acw?(o&Jf%OynoPN|FI`%v z=_`!`nbSmK*y_rHxcOpRvtzWBqT2>ZFh($uOqSbZ!Ndk~@f&Q5!C`Z3D2Pva$l^D0 zzn02x!8%5BSPh%=D>DnjsqAk zZ{^i==0uVo_1jU8z?Tjzj~{o$wReQmr5ksKdI3NYXS}MNf(vJD;l127;TVHK$PNDW zyBRz!4e#z-L>?{~%+k3?C$gPHT*@}R2ZoF=;#XmRz`iliS2b&w#)YD%J4NPPA;;zw zEqjIzYiKN^Lp!5kb6Icg(1=z&qRCteAJb0VYWLUW9F2y{cy1glZ)5bp!v{zMu8FMF z9-Mgk*z46C9eeA9LEw&Pmo(t0QKGodNlPYKcVc>?9tVW7oyeiOCk7AeW#6&dcQc}` z%))qoYZ<+tOMOQepwnhwQ9qWb-^|Z%>b)1^Nq(2eeH9%;FII)%znf`}_U|$iNTcii zG=ZX0mlP0!f8Wg1%2H+`qFOe^@w|;j7?~0ez0N*aI}jjl!}34Am*& z*CwB2Q4b~6(-jBs`usgPfM5HU8Nt4K;51NwxSO@CE#oBk1$_8k2lyi5t9T=p{6{ZB zvUvZge+X$=!jKQSi%Nq!>5d)@{9|ES(l5Sg2j0O?tR?mdo!a|=QfR;Gt3X|B#`611 zC!A5S&n-mub;_d6JaMvx@}c)JJsQG>m7+HB9wE}~fIO*@xt7rfA>FvC>yldl3PEXq zp>*;-`SDj4UxV0JFNO*;qYvF6qrVINqeOo;_3@Zrke)2bWK?oe0+1R>B*`9vk-lOFx#xGbqC}Vgwz88=0Y|0IWEA6<<5YaGbRXzFI^(@i~xVcKV z1cCh5=`|M5_&3T1e%9ANG#J7o==tC)gc&ah> z9ud9)`83;slh390P`jyqs6#l|Q}+|%F_amzJcP!YkkO>LNExsC+*71^yga?p*lJnm z;X89j5ib1>TBJbV-MDQiR+_l zUe*^kr}b`8=KNXXQNtKzYQ*|~IS7h5F&Ps{GRbM>PX^tWO-`g_j~QAaQvf9`8`^f{ zFMACzwJOpQoP*p(mpGv09?(=06>dryZulJc?05*uZ{AMP&T?3Cr_V94n-k~k?bwpe zG+{3@CBfI`9WHF47kunE5v5T)wFmn(Ke5WQR7Hxk{f7r;%f)UYp~Y!`GRlv$qwTy> zbKeuS2NL*btpPK?b?prazGI&3T`ubhBS#+Ue zhQM{o4opq>_bQup@kZk5%Un%&e zy`a1}kd^u@Q>v(ZYi_2vl*BZ6$V|?D$O5m3)s2is;Tf#Lu?`b94_-{1mJIU0s%I}o zL-BCAovBxA^+3wqC}fCodSpumq8dfHjX&&JdTBUN6g6wrZ;<|f2k5%!PBB#h1O4G5 zU0K59fG?q#D5w*kdZHUiebWHPxTc^W&~p>cCR2tmZ>p+y!UOQ6XpV^{c1`)9_xidW z4}JXE)h}pW(UthKpAcsMzk$i8TB1bP$wANue4EGh`*j3*S2Q6fE1{XXcW z!K~>)5O`~q8e$87JvmAL*66_nkr;D#p*Wqrz95>y$xPa(e(vqzHxLUt&p;o4TS8LvMWFF;Ry+j6CmT%A zPeB|CySVhNwiA|aS?2Qxw)m%Olj*5-bCIfiYfHD4=@7A{9pF*g{Z*XKc%dSSF=L5cE4A=H{f3eIGt$ESN4 zdi_F8&c7gkVK(?GVt)w#5}%v4q;F11>qWy949x3^?ZKg2hWlO-38b5eyE27q5A(xy zAEDi3o3K75MuUm{m3p%}C#@o5sAzFcyrEj0P09Ao!1TW!(tov^aUjMh`0?GvApVo` zQXD1U1{LBSDb$!Q;D3V^y$R?8(9>OMZpOXAL|!O=oT_pJquwNWTN=Yi2?mN_{mO1i zm~iu&ujb=DfFfTK{J}jjq{1lw3~WX3MX`=YuBg;@A3oJX(3M%r>t^JsB**9G_e@l| zX{8m3J_aE%J!CkjZjPZOI*M#bOsM-5?ovRN&qb!RV-v<#!FgD`HLx<7Q6ITKk12pj zBjibcV{Zs9p?Tlq^dEWiM)23!FAnLsUriW}69zqro?6LG>VwRCR#eBv~Ym)X+bawo1yV4(zW7!Ul6|Q7eYy9ke+M5efd@t5`ef{`E zFIcA-BmjHD6?9ku=Nigkvo1ZIk!oO0{)H`n6(`eOIYl^pJO5701<%gR2)K0vKC+ju z(vP?+=mF_NFd$uTTl|b+JoHl#_bQxlDG%L3_$?mr*6e;hL%nJvnk^+P5w%6G`JEP~eAMz7ZQvO0w_{{ z3^iueq4lG{B)DD1o^k%vD;R!^tvnr?YdR07=agrd01agI5_SO*z^69rr6jd4^o^80Sr&rW-o|`@Qvrv6BTgE1Za6NYZ137S&j1m%dabV*TZ1hyEZ`a7 z?L|)AK&J_0P{y^-A6xa8?CgB$AZU+-Tpu4)AduWnn{cw`!o;Y6#kJLnes z&8t&KP!ZBQNS5GBnBU3f7;P#E3E@43fo+1A;DUD6iPfvuK+|2vjDaDy&nbL=b>=?m zCX!0WZ((o%XmuT)T76hxEdud(WM6-QjVC3NLOQ0v2mI}?m!W|r{8*h9IUt`6h_L-K zxzS`(NDsyd`Ey*ZrN|d%`K=cS<`<#Y!HX~Ym&`vWWk6eWwr0w8R?%_rf6v-v4iY>| z%x*47e8)-eUj}aAE+b7%x+wX7#2SQoL9k@2rNP5G9#^EH2}gcY#_e-NU1EhNO5ue& zHDle>J$@TVs0x)U!X2L6kqD&ddkOQ`XFQ6uLIaassiiJrLoI}cVr_l&OgU}5(nUi> zfRW}>C(AzFw57%uua^K57STN+Oo;h5do17`7QjU^cC+jlJl3D^oJ{kfvD z=*)F$MN|su1+VPrS7!Ww&BicD^OJT>v6nwdrU8@2#}O}u#^6LQ?Qw6Gs=VNz9jU^{ z>1}Ct%CKx%0R((pJ^xn;FPh8#K9OtcXLWaeiLQ+=2evDaq#RD4OmlkyKS0Km67U=fUKbB}6X#r$k?*L|Y6UT-?1}s#sRX!0 zV2WYoH*8%c@)w;Kq^+ICe>w|2S-i#7dm9h|n<}~hB1|+o+6I|+v}+=&Hs4Wypq>H*1F8YgyWJ0Wgn|PGVxcMxa-8;~7cwr&j)t(h5~dY64Mse5 zwC-XdRf#$1?}GSVm^z0)X8rvX7xV*oBc8I=L>;C#X*KHflyG1Z`P4M?tCQS|Cpm)b zy$l|jn-uf1mh1Mv^lbXi2I$qMzOV7Mu61Ik%9dAn&RrXSyO6x6%CKxr{MJ6|s$VZ$ z)3n!E=RVbaJ}&-9U9B9EUbm1kXT(gHo5;+buW{T~Zj@Zu$s67=CMV`O=nvw{Jrn~* z#IQ`net~d-vj>LbEDV)FCQ;y26Itv?ABtqQAzl7}+S;d`XAVyFQD7#72;F6Nv@?ss z(0t!vqZS)~+n!bL3gSq$P?rfbL>P) z$YaXu+L^U7)Zm&B-Z;pwVGTW-duLT{!NbA51dar)p|x5aN?`=j9|{36(P5gzdD(2PK=eoV;E{nY=AJKqV!`qvaUN2w9)Dq!bDs4oY5($xvN61E z8~$Ut80pQTmlkitwNbj}xsxb)frpzTcl-PB%O(6XXJNzD7=B$R87U=WZ@YvaKiY2+ zUp}c%i)Q~-(LUkZqAOc=84Q3N1viEqs;JBP6?Q!L?g&`&hL9!Gt-Vl$zXH(;4eqml zz!KcNrShd?hMRV(#s?Tg&z2T=MgP0Asql|!?-e!~ijxGoqP%UUJ$X#&Ovn@Fqi0$5 zQVm5(t@y9%sKN8@*`6?GzeNV1iWp5nOo&FBbX|6*hxZtsNUf_Uv@c+M!DRy_9G|;4 zv8n~B6<9$uuWMYnHV$i|SD=>Of)XBonSGa)Q$#%*xd?hG6>PwRXR&7;Fl5924?hhJ z-@8O4L~oAHu)-CBT?aQ)FL;v9D`*2LETHBm_WEMZB8qHjCB$Cth6%YH(LhB-nYHdCdZJ#=Y2cl7+V>SNgzoU zPXfHS6SrzqYYGh*QXuQe(lD^H?%8%9XV&6x3G!cl1{j|ReYqudr$P;qo_>kzO9k!n z15LEAQI}xCvvrQw!|+|*1H(R?qycVU6BF>}Q2b1P)45nCW5Qem z8{xjet#DQ2#9{aQw6z*FD3LGrM!c9_KU^tt)!{W}J`)i`c+XG=)V;Do1{z2dHn%~43s>FtNS%DOZa!gh&&{t|17P#QCvS?E_)CB%S6Z;rEMUS_ zn%uY``?9r5#SzGXYdvM~Ti#4mw2I=rEScXXgkbsCbFR5jJ)m1oup2${Ri==VL z@HKz<6%Bn2irON(HWid+V)BHa7qvWTwzZ}?Y1e#wH$8opJeVri5sEnB`nx&lr)2SB zkrAMh2r5i=ewX-nKRUPzDtdvLUe{{bWnMqd5>ZSdzChjz|8N_jR}7X)`r%6XSu93P zKW1at$OkM>ddcn>ntN*afT;?22h)neP{Ieh7A*~(~joy4j zaj(~;%&hNsMD^|KDs2j!M!>7XLW(v4rytd~GnH2T;Ru^_^SFUQGi1T>!n%iRL9oQW zAtn54fq8Z*ddKI}ykRP!j9SL{;t)h|7cIjwUo&c+x4t%ijleogp~39(j+{nPH6tkV z@kJabppwn6Xk#UHdQ^YPhZfp8&n65>=>8C@DBbIiM2#s6DL9z{#+_INqAqTy7!fWU z>mxF2h2SqxEq@ulFIHp477xpX`3QdaP?FK$3p917E{8w*j^CE?+^2^^#3-KFyPbF2 z8t$%xOS=<)Ztu5ra6aEEU`2{d{^4@YwWRh?39o!hyBDn8JfE~Jk*so+PrfMW4V~(M zbGu`5mHYtKRR(TlBq!{;<(SSL1%zMqXij`P>l;eb?@8fRV!zG$#W>#Hg$-7_A-&l-5xu`olrQ(m3@Au0sCc;yWTql%hV4Cnj2$Gkdc2Fhy~5Plc_rp=&=?gj z1g|S~I7z(N4(m@oVc%|`tD7F9>x_GWxjQOP1DMgE+3%<6KS3m} zZ>?8`4IT2xJwrZCT{-w`D=x~^&(dkY1qn&&ZcdoF@ijcT*+J%om%Tv&uPFjd?D9y; z%pk>I$RH4?zj$^WjB?lI_hCwru*mujN_#wivPHeQcO&7? znr3gO2A_BDtU7ZGN~RwDAwE}n!NZcokMJ5}M{gXCHx;bu%$;Z`#RH}gk&Rg}QFb%Z z(c&P-Bf}CFR&0dnAc5F`Je!$J0L-+S5PuHG7gVw=j(mZr%wF!SoXJNv%9**GN8Lbw zjE7PE(D~ado|+O~58qMm!6COp2Ty-bL%)LeHAEB=1kDmvlxKzmK+<5CCzzSwQJbAy zK&M-aMx8-gL)gqGOJ4puu}1DHg>7Iq4Tz3M@oJ-?|4|E-ShUe*;TN-uif+-0>MfND z^%{XKJ_(U^$=_1k+)r;2iem1m=4};!cx|22$_K_3WOp?-ehOi;HJ3@t=~-k{qhV4Frh zVj?z&)y({PU7wXzzZQp{l~89|C7LuWY$VH?%%tEMi}#l;pB1uou^(>F59bo)Du zy#AzeSwEDDO{?F~sQpQ?W2MnhJ6^QM1`)I1SWs1OwL3+k(g8eK$mG#~2vR>9!$l1G zM&Tf)5A&{7$?B7|FaMGd5W=v1bucv!w(|2)hU3CN;lNWKQLyC8^r}(Rkt$1-tg`lNF z1K}xR=6pv(`)UsCb^;lqM)3Ljv0wRyZ7m?@%qR@R_V)sgIj@m82`xOQv+A6bpD5+t z9{l;}Ft{s~{nPw7ri;jb>l)wHs6({f^`IvTW^O!av1zc{5>EAxWH@^HP|TOh@oz9V z44kr!$@wlu2{({}@$YcOW~xKjwDU+RWQ2UwXi#tVAAD4d9X)DvmYMp7kA|LSC>_Pf)0#2Gff38q;1*@iiW)47dOtq(?;|{F$ zW%gzi3fCfFf~igd!-XK3d+?U4?vjS(alX=b5>zu&1 zP}u1~(y49&(GTs=D!_NxneeqIKi@RFrV!46wY$&DGm;H>sbK7azyL!)yuWG&k>5EU z6ONQH&4?}U?>CRp9aDY#fBKL!QmH2<+S-}1ij%~=jzo>+DGYyifG!k{(astKxM~u; ze_2(ny8U{R#*3Q@%axoJ>Qez0(AehI`FPIjf+=4#l=i!!Q@vhL+Pu=zu$ zUS!`mi>^aR5!~;EKQ=a53!TDn6&vozV0Zquk+l_XDX^Kg+i1 zIqm}l>%~}jhl!0JEdP88XLFe-Qcbk+C=r6|pcLZ|KDt~C3}WJ!VgMBx0IwR*e|dc@ zm)>2WdyuN9ofevMe_v#AKN(8tGRca-pMnAjN2=lE14W_%yWVD(7_h;TAk;5%?F!9( z7mGXc0Jrt`Z=T3=b|K9&Si2s#Vu93Dy5}TY{G@Hb{vBwG(fzHWRKVVVs9Uu5Emxgr zX|h8gg9}(EF|f)(tgXJ*Yh3r`3ka2u!QXR9;9e;dc4bOKe~GuK8?Z~V2lq}M$xI#m zk63z5xKCg1C?GB0GkJ8jC!Gt)dR1NsASnTN7bnmxi&tU2;)(}iQFR%6%+kN81t*k( z^h;9xCfvk$-1R|oQ$0+_CJv^2VY4>im_VZpMi;cZM933vCY$7?Z~jEnvKm+Z_oxK{~V9zaKW6 z%>~n282*AGZYc$DWa&eBegdWL&fPJSzP~-1#tP&YPi-#F;eX(cApiytEzesQe~C{*eS`&&eZnX{Nd4bew71=Z zl-=qXRKTZlo( z`Pl@s7lOOY^Ux6bX(`!M38|zjPI-^ueBYi2Vn0_TKa*p6(A)&t)^7h$Dv1L6|N7){ znDGy0e>=UsDI@xSJZ{umzA(|6(SC?&NFL z-!$uQ=p|Zh@VFVjcwwYT-LQwYn<*gCmpk|2nOVnWcRBl5?BUIKzXOgj1^fM+m=?8G zN|ExrM?Y>X4>w#*78{qpz@RKe1F)}m$4j4^fA-Cx-tQ#zFeZnKLEQ%jr^z^&{!0)l z4YDW^5Y{{c0JpSPsHEWy_t+D9KpKrU% z6}ovzvNMK7Ar?tzP+N|(92GjJtc_>sF|q8@R7FWnioHT@+0~xQ18RWewjfG4M0ZAL ze@F&G-zcq|Sj;ghNEq8i+YrC8dctQX-jIlBUMq!PDW#?N;Wb)(iXD-!j$e$q9sE;H z`bX7YL7MgvLh5MmFOxnlo&=+Ls!T~Liq<#1GEW$}6`H|VGb*oPidwp7F1(fVU+51U z{==a8er7gW@Au*Ot`S6G?u10)uqkTke;$O-_tQ-xpm{COR5BCGt-! z%ONaCk=W)qPJhe+HQ% z)S`slN~X>kWzqZJ&+ED(Z%v|(q2$UjM>w1uea#by7NpDpXPalkQwfs8{0|py^T2Pb zW;)9>@UteG$kTqIV1ggIU0HWg58cLF-?B-+wKnUuNnIU3NKEOaw?Zz8Wb=jO z7k|;HztgdnQAG)oqmFeOQX-^xe_B$Y;8$y{z9R~7(@Lg0Q(`7GA70JW?4Ah1#op-* z>q)!2(}V~>JKg|l_~fdN?9>JXQI&-lUCmgl)i0@3O;tY&eOF7~BTuScRid(OgAvP? zs;~Ez7fiILGgOI{V#^ix{ysl@sKs9gPJo}b+EH{hX68~`3PnRFvl+&=fBxJbIxj!p zISv%?fQGcbF^%M%@EOLP?*~~x&RbH8^|2e|Ndyf8Mf75CII6I2B&@yL`JW@ zQFIERKnSNw=%LDJW(;kWoO9r2NkAhK<@?H}%S20~Qgi-!P1Uh-D6jwR^vUJg2i_P6 z_{isQ@1F!47TI*M8SuG0awBf|C0H(^xjH9;i4B|NVo9`rj8lT*0Z;A)BsH||6;3%t-&pu%E#S<-Z&tQ< zhT(O5BsuN+EIiOcX`Kq4vWd_DR=tQYM+ShD=-!>wP-LYVj?=Swe@;g01|G`i+xBQN z@!aNv|A{z`Ro;32GkjCrVOQh>QBdJep>G+nw%jRZgD!**Emj|qV;BBdm+)9oHDK>Y zwiD@bZ7wgR^?YMuG7k?%N0(Q8oFW*n=yFo4#5w6S5g}w1s1f) z&BYet*ymSbwiHVH=9N$8?&pfN5|Y0id3;#*6>Tbt>Co)0ykU>j zh=^~vL9%W>a>r;-t1I($enhk^cxSR%o;&XzqCnmJKwi+!e=oqd7u(_GYRan~_kzd@ z0|M3O7h^qrgut`l)y3#~%TZQ{yMaX}l+E7Y-p%hOl5OXMXkj2{ONodiUn}-lHO|9A zBJMR31snf2p5FS3!R;c%T48u*-2_H`K~jNCmDE5Y&Na94+0=kdKkdkumc)C>PqZ0^ z3sj7l^Ng&Ve}O2DcF%O_%4r5m;v~B_Qz$V^Ru%NLG-k3y5U#{1oB^~VvoXCT!mX$` zgS6->VH7S&=5JkZyn{O&OTW8*V$H2tjMxTG(A67t9-9mt-k3E*ANz?(nwa@j`{A^s zOye#oqiAAThCBAR8i)ec*)W_{yFE6P*zB*yHm%Tre{UYXbe6(htN{Ns)ROB6s>Yb9 zFurt)(d73wI=rN-Esj%(F!PML3eZVO?c!s)o8?kj(9dKX!F?V%B6%&}HKk&*1 zFGq8A$>!T~=(Jjd(~ilMcxPZd1)&8$W_aQcPzr_@JFD3*YGa3j!gpKYBUTR*fl<-305J6k#Rvg~l=kC^A*K zrQT?iqae~S9v#52vip+U?VYHXc~S==VPvwmK* zwBD55mHhp_418!fVuqNw^ut<;z9t<=bj%i@Me4jrhjGr?Ve;XQ=JT)EQhb|ad7y&N zx4?|TGqVW*9h7HfSac9dhDF{Njo6LFpHS8y#E-M8vjRfYJTPBER zf1Y}{ejrMG$kqW`E?EFURPgjy<9v@>)5`#Ht`u-a#IK=P)5YF01} zxK30f#)|K?zl${F*^^-}7w1j@D%7zNQ65cM7W5&l;edC2&@l#u&pyB!Fm^?ykMjlf z?3n8r3d27@&`iKsszOf?cIB{a__Hcbe-n<<2;9YXP(ymKfN5GpH~Z=(;1?@@D#W~u z`N2I+I;)XKdy0e6dou%vG=Kn`C(P#v}!c1GY;YUA%U*q)iVAA_B#^iUQbhgAhZtN8kzJBREr3QQ_` z>tA7r*&3)9G*1J~OuAcgWQcx(E@uHPrU-NzHBbQQ@*oyrMPiR?#$3L4Sj2V@c{r`7 zlkF+T#vp_EDaf$B?KJDrYaS*#e^%yO#u}dx{rHCCpN45T6`<9t^0d#$aA8H5=>l)0 zD~VUot!&-k@ZQ4p5nah*+=Vw^_)Jed0)kqc7D+zBb3LMJ44HCE!gM3bB~Y?L&*-XQ zDC*whGeh>bSFZ+zh#z{za^{Z(bC>GXe4wC63Jll1D-g8JlEjza?TO~Ne^rsjJj!&u zb`|vd<{)7cXtu>QIzTOGA!;Bsf1V1Z9~!QBZlxL+C38xUj}-z#`oP^1qJ&4|?q;nx zC&qO)PUuj=?e*+TWIZ>x=3hCOaB^yki=*#7#~LkuNmPlf5(K{*qh5EBN@VPB4Z%60 z(`S7jFPx3~)EXghTkjH_fBx2sSe!RuwYJtDkmsqPiWDi0-@S9TsJYb8Xi0^TY1*kIB~gE}=C;6?MjL$~}daqS*Im6S6x? zNXCbW9-14>3bXhU*p=P)gP)xp*ynk)_XKly3jY30oKgK3^Y}SQfA>nI#Ro`LbWD{? zb4FMhW(7W!Myd`P1BVQ_?lP%Vg6AME3}$akNlz;Dhi+{j;qf<^geu|WNS@$YBG6%^ zG1)*XFd*%W4$c7|ml~lj_&t!nFh+ho*osd?iA8LvxO{i`HO?@4;ekT*Z~;268`VX< z0$zBOlBNCPq%*cRfA8)Ws1vC>MWJFU_)Z+CVa0opQ+(WV4kC3& zXd#T!mthK8{nv`TY`A$(Uwgct(cwa%x;Qgllgu#zx(M;8e_%#B_inj>R8E1ntS@56*A7ZAAn?9vM{KPrl&*$q)h|gQvHEDz}E6jFJX9p zTT^cp-XN^%f2e+be~*0b#Ei`FX-_zu->KX~r8 z4xT_=lKIx7a(8NfkoWBDTu&jr4^yLXkVNlDdH!l3%GVK)K3lvDngS0F+((K=k(a@z z#>J!EXY+PN8BN7JVV?qM}!Z*)5R^NU=m ztuh}ONDzhNC%uU89L#6XCQamzoBW>Nz4#)#l02 zmloyH3@=`)dbAsAsn|AkQuDr><4>V?t^pY;78;7^2q7tn*G1D$@UFU$hWA#B0nC6s zd}UFmf5$Tx4m35rHDf_~Jrd~t-80bA#w5PLE$a;DHZm+^>9&eYeSttW~)&&h7P zAoo#=2>%JlZ9=m%-gSb#3?B#`Q?TgG8Vrk(e{p$qk9V}zX>$Rr*E$!gw&g-i>1DjGX?fR1G8fOw!H_9s8)k(A#I4iq1h~Fh3x>ploIAcGMa>PcX ziIgu~!8S^F#%rYc_VyHLXsdV;EF7DNF*P)qFOiMfO}*=eDh{-;3NK_|Eqs_0H0&0` zD}^YSp?=Y0ZSndu=uE3*$ZG3-`3J#Me+Z!(DA*0x?))ovl)!VwSyMk>sHKF+E6VP6 z%(C`#b#h2(?+R!`y7i)&6wZQ^9^tD5_a6K=Dmi4#JxUFKNxPr;mLD4GwCOuv5fEN# zep2?iwJ6A2=^s8L`X-TVLLdC}i5TCc<9C{quT8^+NsF(H7auh}J2zj4FfQ_~e_NLz zr#LmkY83{(Id^Dy#?L@-T8a~-mTfyX1D}!UmH3pvJoRe?<#@G6fJ`4n5lIoAWCxf8OW%0G)t$4iEs`&!xb? zg+3q?Yp(C|y>6;69MIHzQ70e-e^gx}_--rH^8sx8*$LB2nL<7_t`K>han6(@=|^XJ{Y(|!|B)r{poBmlVa*KYW_ zeemzQ@VmXi3jyeU0P0Q&P8k48t!)O{NrrS|CH{4(HdD7Bw|D-6w*+4V{$B5PfI|Ro zUK&D;%Kla&s-?h&q&U>JM;5U1z5UP)Dz*Yjn&6J^7dU`Q^cKux<*M?LuO&G_{)>w-$ku2Oi7M4HE-%gZH z&4-QXs+k5Ch%p`=)oMjL6mR)`{!JubrtCHD*3B3+&j|)|?t}C3Q6R;T^s5NaerQ~d zq8aShV7mYf|QLddGgQio%Jp`vNaTOsLcMY>x54c`pL01 zh(*pgic*ZZQtOmRUU>)2RNP{%iHk%EkHoU6v0BTvStXm%4X5lO#NI>G7Om%C1O8qa zj$gHa?b0NKt6gef586G74o#=b=-3{~=RY%@W&A4HE>z3ze@%L)a~1Ax0ziAJ%ygI4 zNn%0KDw2+GqI4kok6*CS=c@D61OCD|Gr+@gU`ScXFOu_1m)@KAC6#|>nqX5_V<&95 zHRnoc@>ZHEb|%z&LIT)ahWK=sO8qR>Yr{`^=H-oV(6;YQ%&y5nTQYqEmeo%3Z>Ev+ z{bB#&G3BRUe~nvd*Xj|KAx2H@*lBeDB^taSdwlS+{?(RY?eY~5yDsji>mW??r@9JW zA`_m9)(-I=DQze$LNdS)^>!d>--Np(e8Pup|AcT)hWKkyKMJ4 z7&U8dN-Q^AA+}ZA2E8n1xEDFAPfpZbRd#gk5_$r;z`^m#sZrotu0&EDVQKDEKsRD)ws{eOHyVuw#!r04HgE_ zLM(W|rRI+7Dv&rXLR3H2mpL=t8y68{d+%RCf8NmPGCdEKY4FAC5cc?DfG$Tyz9X2% z6X+snMAReGB#&d*ExsY?)$dg6c9{#5;?|L2)&B){QwWEW=hV3&%ORkaUogE!8{P5VgX zf8PGb(!sZb0Eq&=h~T96^3CG~a_`T4BQ;CI4!c|~3jEM(;Cvv4`^g#i9^~9p|5_)O zQ&ASu;lbgk9{dx%VKTu-tKHAVw%5y)|G!e~uo*E=fC2$2V*vdJDaihRDWnXI?TsuA z?VSJT^nYyFvFI3RPz1-UlCnH zDnU1FX^gV~%WwiamxHC0>C27Ewed^SToudEhzB#$NwublIrgp(;L`x|WyxRVPPJif z?TTT@QQvWj0WR)NEPadmHQuDlK-g)W9{GKGfoKTH74b%YKqPKZSWV(nwtV8of4zPC z`jWSvDpR)Xm_tRoE!V+!r0EIR5=9Q%UyeC=rxlY-J4K?eQZi7X(cF%PVtDj$-DRs; zRS%lck+yTrAa*ONUcK}~uBK4CCm(zUY-&Ua0sK`V9BB6`I+!{pn_^ocpNnF*l?f_k zyC;|3jq4symwBr4fzGJ0(p{A%e~1Ugs!iP91j*rjW*$i}m(=FD2mir=4ro~J4v{YB z<-8#L^tBa*MpaUp^cQ6f&eFS0)~va8;__qW&V>GkC3_(BoJ@^6@> zZ5pq=f$9x92&<+Kpo*Glfh|lF0)P0wvBv!J#Uxd>9e(7TVx=KhpRzP_e}bYJ{NQ=K zr4YVU$X>}Y{Lo<|3qh9}g0S?VJ?>V6`R&bO?dr4#*$S+Kc#&Q1WbNAjn+NCME=h1d zuB{=Wn?J^GbsBcbAh7{AitXX*5j^+)z&9cAQE))8Z`j@16RtpBhOtLHg?x7$k?+lM z`3FvgR&dXEnO4`&K{#)me|-)urMmgKk;ABuy6%;XNd$ENr9Hsna6!6 zz1DF}rUj_IOmKmNXh;Gs<+ffmLCcN1W;O5SO>`ohDH+=bG!Q*NGUeO-Ri^QhT!r1^ zVE|S1OnpROcOJn{fBxwdK&}uAWvGKNp7I(FDj9M?tKxM*gJc`r%`F)#uhwRbE~0G=8DU9+Qqm>B(+iN&#z zUDu6&m5Mg+;ykW^ce|+cYmn>9OCD-h<&EXFJ zhY8@_Fn4z==Wpx_zQv9*SC(T@HkQ%*!^!_Y~+})Qrk|)fyxVqyI+H>eFt>G3I6bzMQ87?23k-Ed&1f{xMj;q)Mt& z%f{B#MBL`QR|mLv?ktKGb+&xHx&4dkR?q@%G)1*h&j#pNRTZ>}A3P=|pOr^G8FF)> z==nFCe@P2yLaPC;tb>!tg`r39%_z@QjI;=2g)Gw;^1}ruX1$2^oS0)d{wXTt3=#Cd z`#I?M6_y>Svc7b<1hT5GRpVihY?b)1%U)0LO4-D|?!~1Z0PB9c`VZa~xkUTM5tdO+ zvQ)|qFI6cN#Qw3e{$yjT`E*($j`KwlV-5POf6hhpzbdmP%jqb+VGJ@orhXQX?x)&J9*1%e^juaRFx5BQ^6bm7ats9gGN}$&0l*^so=5` zv4pa9Ubn`oah&GKk~gW(HxuNyCbe^aKYkYTqSR=~abcmV$rK}WPD*(%FgUJJ=fFD^ zVoo6NU$^?--U@{DMX6^Zz-7|FYZX2D32Lje2yivbywIHL;=u7bHmK#GKrOy=f2uNX zT#&u~mm&dj(8fY{2uBSf+7))PGBD+MX79<&865Szymw+B{QK-T7(ecwiS!>Cz0QiQ z<=vPpjGaUHsemqvR;Khf47@4PY(c1l^5=h;KT8`E%GPpks<0c4q-I}7C#5w z(eOR}zWNT{U4;6Xp1t(mIUCMd0bn__tW(V#T8`(UkNH4?rGX(hyk=rcV7SOiFH_5Ql(P;+bP5Z^XcT3ar977%d`KL5q-TfBctx7#~+r zD0)`<_FmaNDNaIqSVQ4gyS>d?OGRy`ev04RuYYPgamy>m7_D^HeT8n2+26JG_bU3F zx^lkmLI3p&>cdOI8Vm?X6aPQ?Mg9N#CF*HsX#4LtBx~sG{4Za!hAvK)9{)v{`h?vk zBVx}h4b({z(5mn?G%e(Ke_{9_Fi7*ijgASsmQIvjQr&HWmd&~yHi;b~PlOpKu4CA3 zZ1~fiU9!1_nCHkO@t`ONQ_)CsP6Ki5-5h~_3(#(C*b23nH9N{96i*sHL|pnQ<_wke zDaa1=bHLkVvyqFhz8$d*F$&U}5dQ#_Fsf?EsW7b_oN#3+dukE zXxzI@Cx#eWlxy;0mr@#PqVE^cQi`YU9iznFkw%xvUn^KAh3+k?A5OvQoJFA@+pKa8 zc)4IR5bJ#Fjpn>u^+`c^$n2%cgyxvp6w(=WSm3#HggYV0XL_jJZ}_-1EpbBzsNc1h zCncRC%^DA^pJc(xf4nKr;XsiW7ifkM7{rVM8PWIzjha2OE==9`)SRiL=oCwWNF|3c zVPl_La289lVNd4rP9nk@jR*GND3inq%wGY;@)&MfR8Qv;Q|K*MUKZy zyzJOP`R}ztxkrgxQCk6@Bs;v)gSpp0;D>A(Be<*Y*Y-l+ygML#woAL9ZRHk8LU0So zRlnYWI8qEi?#Wf_H30|Eo!QQ+c@ZJb|EkXm;c4LBt3!UaMBW`amf7AQ(D>civh-^% z{cdHB#m#!re;O+b?#L5y{-rGnh5}56RX9r?^j=rjxDoZ>YD8MkcGXO|E67fC$}f8d z_)}67iZ0SQZNOuiJY@{zhX0I*c6VCs3fv8|mE=CB0(##whppAs`O&czs@Y(IbhKGw zMR#I#Kf6gW6pSNh6Z!#Y47?2On8qOtb?>Pso+b)^e<}(d?)~NR>GShRIt%T)hIk5y z9GhsSY~fZP_xm~V{`~B9ztZN@z9PULC`oXsQ9`4$n#D_+4(vNSn-&B|qfOgz3Z8K0 zpO=@b3fmj39B|Ca-Ylbmq?pfb#cjUcyH&kMR7dkt`1BTG)A;u8t{eJgcV5dI)6KUH z^QTtFe@*4sQOQx`wsXzj$8lVDkQ{K?Q1i3TaeX#L@NWhwSq5!r|LGxg5dKpJng2ru zMNI8noa`-43~l}^iT1Yt)ULMwJ$KmJ7jq*1^!5!p^<7B?8U+7RAxiaH^yM@yk@9{8 zwJ<8L>ptrDT>tsN!;ulm+J>{LwN+wcVIO7je>*uJnm?bRpPLU;Z=sBaLfIUMz=(FD zieeoEe0`Up?Paew@BFc#NqZm~j9H&HCY@vQ+9UQ&a8Ngp$1N&j53;wP(hb2oB18@w z&o=-gisBwbP^zsRJG^$Yw%6^dUSx8`HHY3&|a9=#4U8LM0=fBGyR( ze;+GPQ+*#N@yQ>F{>{?7Ve)I$y2dJZ0J(QxtV72w+CZQ;LLI3Vpgo3mI|I;$?mxu0 zn{XKyBUf9cR?B90e08Q&zZV|A<<6Th7vQPV2D+Q1roZ@8{Qz~^P(6IZg2i=L+}9__ zhz3qa@`W~P0sot0G{2x@$&csk{B51xe}`yl@<0+qE_>929j~nEJc;7jrc%zSOc*Mn z5aPyfdSaZ!!Y(m^p9lSU!N&+zM&KN8MtqN*=T$*Vef^ZC=R18j*!B2qZfa8HxKjh4 zN`{`auG5om5-bC1DZLq~pOoh0d7g)JegKjgBOaGdv0`YNF>N*(B!Fy=*j>y031`98t5 z+Sjcf{F1}WO8m;R^iuK6X>s0cU3twzVl=2AJ?WNE-N>^Z%Ab*Ig3|i9f5d(W2(5}* z>)z-$k^tWLUzD62-SM z@3+?ev)6Uq?}K-L6RR!zhgB6%P8H|x&S&fWD~i>$UG{6=Q#x5KJk*|a=kvOxo-z*Z zLa9H3R`R&M1q1p5iX+jEe-R-bwT&L4(7Agt8$`W3lo?LT?P(_r9bm^!P#h zAY}doyUf=9$t|Z3$Ylh`9Ilrs3aP=PCv>-^ob{{S&d*z#NADM0nw0E(pa%l<>-%aD z8X!ftC5-akV!R%Xvpxp0&K=wvoHRt=z41=JJRyV?yOWm;=!yHKf1~JYFZ7Z$%`S{L zts_Ro4YC@b(aGHkTf^;~$ccP@Snf^l?$oXwi%o#!<(CO`mMHXln7Ra zct2w$JZsSv$+VBoe|-QXGZT~e!T#vK-EuWCLYmKml1e2g?cv}@dcWRu|7j_ZTNMde z$glWK6vI%98!4QPGRu-9_V{t3TLIvU74!2B$NIPV8-_l6gHVh|1gK~XQ5N3H78A9ti)bVvB5p*H3*mf8q_{XEZyel1&1kh{I+!?c)vh=v) z2F4*&`Bd#GIj`_Vp!o5&;-qELjE;V53^{z~x$uD-WRm^;oRU8Er#>fZ-UXn`8S`Mi zf&v{;XTUkle}*sCADHj>8IdfZKG!^`TL4?eFP{+rSq8@t=l1pac!8)a;x|$~CWj1B z|CJdBeoV1}N|cxb?Z=peLaGSc(t8Ek)ltti5X)6moGPjfuKTWflm!^DYRrayBUL@T zt3`N>uc*sG>D2feWrQ-Xx3MRhIqeu*m4-SLS>G+Ge}@AF-1CCY;XIyI=-^Dy__`g~ ztJZ1f8|ACNEktwl;GK%u?!%w+bqH6fKyc#4rR_;n*Sq1KsbQW|hXXLz6Sc1WD%gur zo(_TYSESGGA^;xtVxcw_tXgIXnLCeNoVNqQeS>rg=5nD{P(}xPA%(kJ;QEy4PavJU zAk0;ke<|XG4mRcmIA4pvV~|z=FDdw~<|EfVn2%65xtF93;=9OFd)99x1V#1n-wR0l zePoZ7!ESuFHWBYO#k?}dFGiC}IWNG^lHTX`h7bj9yd|bpQ|8mO(>&^a{e*s)#MM-5 zoY$g!vE-fncq-1`E|3ZA&`$JLP`4UZmc)Pwf1A*dzrs73Os@aGxKv(@O3&sIO@SQ+>7!t;3)I*2501$ ze>gdPZrFNeJLOf@uzM#6n6f9Gt}7itWYU|=gGr6^w-@YuiHh6y5py}%7w)N20s(9m zqKAk^>Q&uRQl+50R_D|`s)(Ke{x%29?!oZco=sz5o|l9s{LFNn0q$1dx$J?1*cOv7bE#Au^ zZJEo+v?d2d{-*n&U5KDlhVnXX!a^|Ijl4sGn8xuA-RCXOehxL<ef9Gq8QnoTLkTlemWJPw;nb40n1f9T_S zRQFBgX%+ew<7adUPyLi)SRymi+=B^MKt)XB27jfGg7FoBfmaG?J$SoQx)mi`f0u0QWRHlI zu+Fc8O5_3LdLz``;|ESkk>U69Sb!1S4pL^#Nv2i@l^<{#7oMp?wUQiJgiE0AVyGOIu1JbBP z#J^Dz$>O$+xV5B*9OG0He`Y2WtotvR#gB5dZJ^4k-M=b`HPupn9!d_Nh% z&0H|PzZweqxZSi{N4W9O$f#30xIr#bmn0oquu(|^0rc{M;^H9-as3G=s||th9GRtX z=|Ahoy9ew;xfa+@YmxhnoqsVP0qjF%2Hry-Ukmd7o>@`gNlo4`sFH{ygE7PVkL>$qaBGo>xJAXW>97(kbcEivWJp^k(7ye4$U+ui)au znI102q8gR&f+v*0e>!fGSDab>S9yMZXm?_sq%eTIn^4y0pal>48}`jA$2%a!Q$FhP zCbIGdC01|N9&*PyXsr&vjVPJbCkUS_8bzqWz1d_G|BuZ@cy@URTUVMhqj@7NBg)dHP$@Qr3`mIl5& zM7xm>_gMDsc@xH`OZ2Z$Xx<5vX!Pl6xc!$$?GaB z$J2qaGvkg%NU92o@a8KN25#bTBD|1xbsu!I=(>&`!uLQb2h`*YaTwT-!LpWN3MkPN z%Ay$~f3*7A78+2zoT(QX$-)R$;1EawMG=VU4Y=n$e9qj<=f||4s+5pp5Cyr_IviSc zOo@Cs{)iV)T%XdMKaf$Ns-x{{?c)>(Qa|+=a{r2-5#n+BX+_!86|G^k)tADZvTMH! za&jfl6k~tYB95|I`;iO;fZ0cz2K#MnU@eW@e`JQ~+y%}dJ2J?p?&{d9sooLmGZ4y2 z=oQLL7_&(=e%z5-BR;O2pn@>VPjd|M=|zJ18PEbqvk){A96nv1CwmnZIk=?l zd2SSLV&60zKK#--mqq}T|JAb@jn&kL0E>fVMF|I7iKyq(M7;p*uTj+zI4fwJg6{HE zf9~7wEO2^YY@Mj}XBW6A4RS)+u(?3m4}iBTDC-=#hb}(Hq=84}D)uT(*p{P%E zAPsq|UbmN%_+B&j;I{`(Lv8hYTmd8s;(jSIswZN021Bo}D+Un?U_VFcLOV9@i!ve) z*XDVeSV06Fqg)*wPV3uXI1@>Mf15Hta#44c8A1NY_bkIh?{EVqPVIgPcixtn)S5*P zn9P=iWCcE{uhh&bpy=Ch9{X?$PyHGn+?=ewZ{gc~{5?L%y2dc3{}j>^{@;Y8_&

D9)w0?C!N(_U77?h^J43~j zqfl%2SqvK12!R`}dRhRkDTJp3Lt+At=M{+;j&n;_;F^{ID0EpO8Y}WD%t<<&j0N8@ zO3(+70MZUD%xn{-ZR`hmSXuyCB~LAD315;yfX~nK_wGYMt5|Qd-!l++k3p@972C|~ z7w~=%bF#f(DNxCs@Y4Ic-Y=r>1QU<-Av{ccKiBsQB0v{!a7O0Qe}8THHCvpWY66!q z0#8oc9F?x}k6#bgSBMi1R%`av3^1sSTXwq0$UrbH)3sku0CeKILNn(gd0TpwW6;Ef zXJw7UE8oKpxAZ4vh@WV{e1(1bI)->isxeC!HM);rw4vtLp-bwz^SUqP!yAO$OpC3c z=s7A8Q>uZ-IoB8|1yM-|;6sqD3}|H7X!JX_9{t|iZj#xaXr_EzakaF<>gyDY#1J~L z%qDR%<%8+hYLldn;tSmG3+}4lUJHh?Vn^?LTza>1z)>z+S5zzkPA9;2w63n7nR4Fj^ zr}`}`?6_FGTbDY6J1q<%?C+x96!+|#6XyBkz#sd5^xd%JiO^_)_uvC)+_7J^ZtlRb zgAeId&(cez85>laR*42qu9OvBMJ@|SjzKb&Zt41bT+xrx|Yn7gIX zn?kU8wc-4iSM_h$c%~pVM9!M>b6^9FGr$ZKEwY>0IV3H6GEqRU zztcIe&rLGF5O(;L{Y<ZM{p2lMC zhjf$|8-4LMbwjtLKz4^YjFVJhxdTlRnL5bPRlJK}U^6u>)~Ko4&H5^^VSHk9#-AdlMlvVBa1d(6vA&1z_!}4wn)A z@^;xK)^d-yT}302;^Xpm1+pm9gTfo92l%Bd<0lBMS13!0J9iXKS;?rye26sk`620A zU>zGEoGkd|0}x*eA6Txk6N>Pwa}lTOL3ZcYE?mQ2Wbx@3R!CBx25orw;@4NbPXkc+ zqY5hwAN||kE+CpG^_3&O0r_eWb<3-ptR83>KCP$g^V#E8{(P{To=Cz+3MV~)D`-I$ zU%;6{KE)vQVgDxBPQ#p9>w|gDY7dK^5qjf-+XF1$90YvKYXpOcm0H)pZLwn*6o7em zorW-Z^5dywEtS-VH5Y2Z&+q6k-!|qgzNMCkPL}>f=|D zp6;ItLO!BM-88~DTAAgal|`^$kOS%5=CMAJq!3>c#vms z9AO@*lEhjbT|=*j9xCS6jdPo}*M&!gh-u3fc_<#;*p-vRKX_m~qB`m;SY~D&g+rn+ z5q-au1}AcCny?Y38)lp0ii?W(!>1DO78`7uYnvffCIi!>Ldd;qDToVJMNtU1{hAIJ zYrJBwgUInj#*%vmk?8ubJs^J88Vv)bA8(8n@!7emPD0n91%_r|og3UtUOru9p{drA zav-bc4cCtBxesyfI$|k- z9UlCLXy@rW#xPs0Zfcg`ept6PGYic!xwRiWpTy;0k$hq|#f-T$$l@c!+7p z-_Zl+#r$$qv&aYvn+nd5o=#jLbUw4c>TWe2O-J=!^pUjSHv1GH5OHfN)3Jn{4n#~L ztj4CeVByLSU4I+iP&>!BB)yFb$$FV}E0R4lqK7&hB0IO?qjFKV1qHb|%mnCx$sUp~ z`PP!VUb+lyeJ;oMyaWWzBPJH(SFO=Y^;8b``mo0{T^re5Imq1$;mWK>@;1nvk-7O= z;%ZVV1=J592g`Xs{2FU?QC!oLZQ{G23M1jb>2`OY&nI)oIFzi@sUrpwKvjiy^8r3t zm4pZ8&SFv{Ut(T^`N7REXdM)2CKh5h1M?oWg!4~eZdDb&nAsb}O%* z@eE5Ex>#TWCD^u!b@O{YR2S}khGA{5C$p^}jVWvG2<&x0aRWJm=G=3Q`r?F>KZp=n zcCg|fSWhGTMes!a=0^N&##-fln9D_}&iiZEC>TyRUwBJ78q;aq6Y4=0`1FYoBn?Vs zF~C?xMUiAOH*(R*nG_pja^gl0iXG@Qh-@%i@H2-I59296U)@(M7(&Q=3RP}ZznmD3 z1QFw~u}q!)1?Rpy1bQEM{)kLgY~ib)rLA)dWp#fNRuY%Y5i(aallN=3>$Ai7IjI_z zGdX{fn6;>ZTUuP64-u|KLno@*mW>@;=H@U3i|~lwx{*_|pzH_0q(jW5jOsY!C~N#t zrl(vgmJ3GC5|tas19uX-V{rwouitX}mJN^p0@*?(;OCXL!h@1;4|1Ym^H)!K${MwmZR(g{ps_HRr+dD+aMsbKCArkeOnV|l|J5R;pfXrYGcED?WOFwyw#F*v+ZF8!8) z)ojd`i7R}_e@djbEl>=8@?A9{YS~l1dERaIbA!V@1x{2tCV5Lo^K2xL9Su_)3i$Kz zogbM-ULl1LMzuyrnN>5{3~>lpmNzw+QI#P>*<(f5M5Vz6ykX4MNxbI0rFa?wN24yG zKM{zgZvh<-cUvpS49ALIy)jIG3`1yXEGd>q$cM+-(mP%owe6#uD!B4HPZ?0afkKQY z9zr%xw)3CYR7vk?OpkS=_mjr~d=*jz4nMud_LKYlidha2`{C;d@v3L@W0RJ&ET#KG zv%m$05`SCi_W2Bm#bLyL=8KN*A=2>~s`{p!L9IDDzr7kPD0Gm2oT)UL*G%5e!!A01~u6wbhmU%6>$uY!z3W!)>puP0V_h=R|nNO zFDLvJGS{meVqYU3DT1zm$EUN0O)-s2!3z+g%oYIXgWoewC^N=n8((lsI-=6);a&v{ zHPluI(sam%8W6>38X_nA7FVD>!CxH;eE~0odFwe(W72D(O!3tim%7*(I@>zyEq0}m zEtRgxjNX7OiaE}s8?5(lz|gMAywN`3JBL=q1x_!?H4f`u!HW_^mdt zeifUi1xJEdqe@h;Gx{`=jQ~BDzxr#7D?7|#e71>du;<;MQZutHepsZ?p{G|pZDN} zYTo1Cm8}((Yu%dSyn5&rg8;9u>52Hqwi z)F;W|iRu@9bbA7;e2`nld|`Ik>2s;xbbPTJ3200lpSC8dz$ZYp9Unpsy-JlBm*$5^ z;wwbpK0Ejq4omFc7_Km%)=(M6h%WuRVgaxVMzg@*87$ ze_M!j6ya@%xZT|pf<=j(DNtgS`t7@4K%BvAZyv^4S_h*UuRlkxuZwWV=RFuYz6i2b zOjAi+m@1y#6n9tfYM@2jM{BmqZ5`NJnTlcIQDdL;tkG{1y&7=u!48Guska78uOW9H z69-9?8;Zr}Q=*Io5ZJmI*pBeznuUa*nBI2Q^$6~~+hR!;up@k%} zfW8J})t2VwoSj5ntmSqTP-qj{$J;6eTRm*eE=PMyIZ-JWR9<-ZqNOUFWGZFVy6x2* zbluAf5(Pv2NxP;4iSgBP1S*w9ntmaFDYt^TKq}tnbT?zWcuzG6t=0SU>v*y6xaUK1 zE?WE|oqmP1YJAdHWSZ(*H4_AZ&F*GU@B14sf*N2bu0FV(?PlJ2o%^GO!mRi>Ft9$; zG)rybuX}FD=pV0GQ#$FL&Z=m{d#MMQ3vL!nDa<1C=CrvXw-Ht0gOb+=9f-Xj!tXd! zcvdw^4Hy`#EDvnELwobp>u-MMbC^G{96^JSPjKW)eFtXxW2Uv`ntK=aHxDy$2V7f; zHfp_abWp4pij!vn`+xSCkze3wu`MeM6-BTOB+*ZDIfu$AsnDgS@M=iL8H^ z*RI6!WAHK|tHUAX^ow8yQj20?4--XBUo6PKdv*&Va-Ca(A@-uM=Q>l{6BLM2hSaO4 zX=XCBxl>Fz{Wwn0S<{R@)+~x@$Xa>Zx;q;dE-+|0+odl7tu6pt*RO37MH%H!KE?sx zfDT}7KT>Z95I@W)M~nBI&d0BATWpLp&4=s6WpZXtqrhcG-JE?7c0m_=)xAL}%o|^u#X5 zS&bPdySQ2~O|jc3@EcHx+F<>B2mGJ>1xty2yp6T|wOA%ze9(t)y5E|}i9_EOr+1sR zCK+M_ESyEKX(N&|&!o1?3iY}vyBPegBrNI}D|h1LM1q zb%Cn4$15MKZ_2s5`NPmjDm0V=rR%O2Xq6 zF=47W^1jXs-^I#LdfMt~qJhm$b$TDz{fGn#uDj z^5DvtIV@`#ye}FJ&0poY(tyS~BsA4M4adc4)j%tqv9L2ZwLuN2ZT>yigfQs^Ahan{lL;?9_RdFH+iu`OyY3M z?mD4pw2blzV4i;Ft!3MJ+3jdGna<65YC_4eI1i$XlAh{p8hF=5ap$>; zmSE}FhukQrBddubS2V>B0?(3q(BCeT%K&IO^>J&9VoR?$b$fP64$z`@kXf}RqZka0 zp&DJPFo0D=ZOSHQaDV`7@hYmJR@I}V(`ocl-_=oPb9C_L6<)Q%sP!#i^=x7jfiFL< z)s^GtY9_BP%HrTv#9SNf=1BY)8es&x&i7(q2KWyJfcYs|)j+ zhrUH%PVR9$%UAEkgeAu&iBod;+7ncahmMElTOS?Yf{-L4ndx1R!z-LxfTehBugCJC8{P{~uz zg>4XV*mDq<8n(TGY}y73J*4v3)fi#J{mfp+9e){_dUf78C1JVU={-p)C%^$@Ni7(n5HD z%J((KenOzWRmb!ocfCGow0+fZDD*sD0qS$3khn|t*OKG*39&_iB$KJ-&+ua@&XH~u zjc5f)n+v{er|>-Gr(46IXX|$G($h}YLZQyrY34Pig$vK>nC#hu>Cr?2#qUClIn~J>r%VA z9LGJaZJ^1h?>=oHB*!x3{JP^U6~Rqx7kMA|z*dliY^fh~ZdTb}gJz)(JyXAH7-3C} z{)v7T5IBg@=k~M-BdKzD3%jfK8Mc{KZ<)A%pG9D&&4>V-WufaQOFnKj5~meHF|Z%dHJ_YdNvq^xpd{aLjHrZtj)5Gk|kcTodJe|pBL^dt={SB@Yel( zoCCcKz?R+jG)5uGVszxSD#T~M>Z7QrC?=q2R;pJ@wOrV%Agz2wmFv}vvRg*Ze=KTa z%1?nQWU=b7;@%vvCa1kw#Qr^X@)4I_rlhkl!j9-<4@MH5k78lBx2hp96w5SHr-tvg zyWRZ(oW#PL1@NRKzD8{4pB={(`2NsF8OsqyqD$o1T;a}i$v_F1TtrZDvG<1>&J|Kp z#5^7OX?}umt(Dl``OhX*bH0tkl-e^Z!tG_{9r-q)>fy90pk!F5c`meOH-GnPGpE8q zr3YC6!!abxkw3t!IUV_ZPB@jr&}~x5V<9t#=;ZvCy3qGgyV_)6&Wr)6uLE4}L`h?A z&FGhdh+EHnE03Aup3KQ6sd`JDU<}bjg|;r^f;qYNILm_SMjM1M06huDBqJLHG1JqGS zwe)tRC8zg5%HvYIgsa=vj|lceFgq{N{P$-`A~z#w|!eSjP4XY`19l$PYsu_#>TC=6GA>a;Nn^Pu+Tn2 z;j7_ok59KyuRj~bprZTD3iKmW4$dAAzt*&AD7S1nqG?B5g&w@X!BkWzKA0sOQ?1ht#w}`DcM_|8Z;$qGqsZ|9iv{Xj)oDM5buivw0mPvj-2j` z6q3~$KD^Qv;I>4^ffWraVKdjW%%TrLjc<~%&ea=V~;;Y zQ-tf7An+cSNCvjyQ{6+NXmhF04MBfh?|t{+kWRVjN}tr`g7KBA_TyW|dw7}?8ihel zhgD4kh;(}ly_HiJ$!B8i=?u6}io39z*i|`XvfQf}s}km?GrE^=yP{^SZGWjCR!_*^7C3)~n3CXqBQZzve6Phz34z%}o0}zyOXP zq{6%j)0HLcWca!35o5MC zNT{3%BRPR_zeq$OFQtZjgE8M1YVOakH+%D~)}U-15F+<1b@RC{PJ)`Q&LBMwXCF%0 zsW+=WT>iZ!xDp36DeiGpD3&6;e30ruHZY^GlF7+&lhr}!b@%IV+>rXbbAJ);YwsnY z&4mP{jT0F))?BTHf3}pa_`Dp*WjJ=0r+kuoc)(`HzIiYFNV_>27~ltSKD}6xc}Yk| zn#oxHIwmaRV{TDo`n(TL%bs;|hw2rRB=TEO$w;|2`_1I{j;D?MZUNqLb|gXfA<7~} z-C!?UuaYE=!zZI(%EHqG3v`jLcELX6mo}foD}z%1sL`g!Hd}r^XbCf$|MH1FHUX#@ zsz2`g$gzwPMc%Gy6va@Kp&5P*viqOzx8q9sgIV@u8XFGNHqqGr8ffPVrF&*P5I#ZxNU1m9gx&G(5(^hA$ z!oCe``qbt^LkS!;Uxf*r1S80ruqF+&H8f(~kN`HCXF3=japw0(YSUn>d>Z_PkQ?N% z?6xp4n*V&G`K3smc8WN%L1?(EO$7Xg-apTyS^D*5T@E3T*tXBN&gw6b)@Fw#j?;?@ zy=q>SPszurS)0Z%txZFJt}vX3veOiTB+T>&xLDGM@+L*mukQ)HG2G^>MN06}JT`E5 zo;+;JD@RBUAE~rA_3SV`h8dN+z9Sx4-LDPClFNNgtk9M%X^{mhE|IGuiBdHepd z6Gq$*iEV)WZ2m|x&iBc)uRT~F&eMo>*_N3tYseHLv0_YF5?;Tv!1kLfg2Ndn(fVF6 zmtGut$eFG3p*3k&vvBoBeMA?I$zt369G5Tzibr1=R@QTo1BT?#-cyt{ zK#(HdTI;&dY-7l<2ooQl_yL0dgJm3M%EfNDuF+~+_xa8$ZekpB;6{$)r#}W z_Y1)4eArBz<6bHi6(;I)P6AffR(9TKg>tje^i{p8|LS%x9cC}B#@|M^Oob(4MM(t2 zK!E8cC)psn3jqR6_f95&%mPcW`nlF(f=srz?_ey*aemD244E;e+^;^Y)LIYEq;=3S z&ksf$ZMCBa*qaN_M!v&#vkJpP#Jgsy+O3zDI3TTn+5+=9O^3{QL3Zt2{AkcFn? zhuR0x{An9e6PcF^}^pd;nCw%Pb*RoRX&3=1cyI9jkm zrsiLRSa!UDJ?|~;3u#ZUySYP8^h+&m!5_~^rkJR8sy)zFC zeQz(O$gteOv4SbaVTGZ=_5m@(9=46Gav2c`^DCbj-@Iw$`UWf}AiiE`|5;X%#|H=J zixkzLlyz5RJTcSy5Jljem@rL}%+Q9?B9CNqpjO8}I0TmxlTpGWvf)P`X6$=-0*8qR zVg?0@o&=~RYC~B0k$ANBD!D#Ru|lk0jJ=zayM@r41R&YA&puz`B7cUk2*C?MfN!=B zkgWpImS@uRjIUWGeli(6w+7W7Vr%cINA2yJOde5tpkAM{OfSbRL_lI=J5RoM1fBN} zCxEHQ+Ytmm?fFYA zK7-zGv`D06n6HLKquxJm54g0DNZO(y#X(DIR}ab0}7$HwlYjjlP$PdFEA zuuoPM_RG1A-nszc0h!d5qPeN=M|XgB#0;>OkhW*V&>>LP{34BgwhdXa2X2%-d-&!K z9#?BO>0D=?_V0C93r2zsHJaC#B^vGAc|xF%NVl8EX$H za?BgpaoK2=n;$w~{J^3pOZ@YEyfoVm2k7<-#Oh|hP`wftto>_!t;CovB2(^3p;%dD zBrCTkJ~1Q&wr?zXKJDa;Vb-S!ef<#iXFlOQ5JWOcr}oyTs;_^Upm+3($^9w|67cy} zecj?ngDXS76d3`qetI@4G7YMZ* zG7c4pO_8#DXpB=#n6p=4kH(3y(c$zW@5lw*$)Jwd`O4ehT$b_aS6^F+=uOVkB0OXW z&d$j9YNw+|bc24_Iqcyv5P`6*&$7yF1oOMOj2IVtbv8Qg=2lEW^(AX=wAoJ`HeQ;6 z7e25IQKCBneEqUO2~cJN0VpS>h2zrUvD>s6Fsh|w+3{^x6dQ4xm|+wRmrJRKjR*Eopk2VvrQP7C*})1 zn?1q)O+VJE?jw^xCTwBvn(5oCAE8OFU3zIW&-<=BwdOU^+Pbq%!4935{Hx$mj?OQh+x#^#3LXm+E8>WbZvzGM5AqXqy-6dzwEF!03ea_Sv#`r6&ZVB~vF}fs zz5+IgA8l91a4rCJwO`ZdVw8KOS)DX;(`TBasYLh86b84{xFwKS3Y=J1L4LF1S$zDYe+h|5;w73=LBMl|H- zmnaYa;5AAR#xMWz4uo6K0UST&>av6NXu!oGo~e0f!LI$9FLVl;$vpEKZA?K&Un1X% zgW>&oJo+I?|kqQ*(i?{B;wKsWT<`Gyy(V;&I z2`tHJFTQ_+s|3R}FVH?1~_$o@VEu=sZXzocS>Mvw+(HzOMe z?j^FD;KM%Pn=E+{8!{0Wke+Qz8ayjJ-nRiY+R@WESO)tI2!|}V4=z#%7*-oPx6rYC!sWE z6=_T{W8|&l^!L^5Hk@%5H>6e~GZhch>X7!Hb!aHMzb5Ph0A48$*Cj#ms`!N`7vqTl zmXRC4bR11T!4g4!5&LyO;U_{quBCqqa{ZZsLhd)F?5{sh)vqYRz97YXl7`R|X?h9E zw^z)rQijsx!|a)w&2iDLEbxj|I@vBD%_IXpCSlI!6mt6#;6!xLkQM;Utp464*w; z4gulheYkm%4YXve&c?*)`%$MTA3~}_&7XQ931D3L%yT5vE`86TM51X9Y#_#PDG1R% zq|CSH3fjg&&yINsV>4=ND2v0P^i6zeA2iiy@p&bLOciqmU{aWb!=Go9Z_IDY?PChz z$?qH+AhRe`Hd;lPItCyf%c+onFJsj-jf7SN?lE0Gh#RiKWs=Dvm!rQAGJ7E@Os3iI z4QkDI7|oN~5P1y*ij~)3)3xZaZ;V7ssrmr1&gj|Tuu5ZrUE!-t=7#%^zfsLPcEub$ zuVd^??$_$}i`2i?uyH{_tL-TbuQLfpM*%gVZxZ)krbQ8~Fpge3x{>A~QJg{*+RBU#f~i^*0+3c!dd2IQ`t z$JaihPVtF?_ekwTvEJZ2xC7lEt*-BK{NT5)bt<*yDw-@i`tXZ4GH6dRuAf%0nprpM6!^M8;>n91hwDCk>P4QsH=!P+sR5LFJ| zn)Xs|GXqRn$K!J1ZBk%DhsfY$cPn%gvPR5fPG8AOK+BT9<3Ao-Ux_-Eznf2&A*y|? zeK|~vvr=X1A8vH?xtz|it>h-nOuh(MlPta#?4WeHmfaF?6;~40Yqb{KZS}4hX|1%6 zYP&kv6o89IBRG6DD7jPpn}Oa&F3nAYPuQ_YkdlR*Ons35eAp)|1o(+%>>_>Tt6(R{ z>c@uDugzuNKvJEiecgS=GWJ*9`G-pbMQW;!*6-B}a3^R0c;!;9$uQ^7vz>z3R}AiG zUT3s23vom}a=;|*I}G$O7!a`)?xnv^@lS3P2mCEy6bC%=$@mH|e0Q~L4}QY_X+2h* zuozm#OuM)`iIFI%s7Zk>i8{9|dw9(Ua-s(_Y>#1yGnO4fIZF=APl2VbAP#^$Ke0!x zytc_M@?nX?(@)&;Ntos}sm^&LEgXlO5W=zTp?^l;Tc1W5etDR%bDR-{ZX}XVah#?Ro1>B!>~Aq>U>`~hm7QMunw=&lE1WE z4>Rd562w7_2unCm^YhX~jz1*wqT%XJpki{YH-fm1{0UE~DE;6VZT&&ehCc**YU_k< z=v-morom<|F3R3TeiH!w6t4}I2ctOo7zXk8%Meq#={n{o?#YU6?xedtmU>z1HTO-PBxok|_?!i>89$8asn^SHuEi;o-;{E4oTm*$YwQ=(K^? zGQc$BpwGkiZ^^D@&|4*1Nf8&VUpIYcQv2qfylx7Ys-jmLv#qpGuj_S8q{tQPPusfh z@dg~I){SOEY+T~~cDlL>-6Dxx#sMUIz&I07p?bW2!>+?MqNpY}#xkj|-XvbVW z<^!rmEZ*oRFtzWo9?NuRxONi3-n?9xjkbRm{s2ue81DLep(Mv1r;Y>*>7Rp`IL8mz0g%oXq)9YkKIc>tnzbhG9q zc9ys)*B{21+*%RN`C-cM__JJbU>y6m<;+kkYT&fp53Sk&u-+(vuN3}*llElkU^Tp+V`w$(xOx3`XUw=82|vyFY5yVFxq=(%A~RHXcd znIgX{$|O-&U=UGP%18`bqgb?5DYz5~tP|5w`b3841^-_eMC&x-T6WRra6%EetW}e> zpwo4(Bp;9s=4T~yw&MaWXYx-vRd(T?^umoQ(O590g~0IqD9Oo<o*n{~^7sv_$~f-ek>jt`i#h3Gt>pY;1y1;sE)N3I)>ReWx=YXprYhDjGx z==I&;Xbb`Egll3m%aVSl9k%4wg&3zr80ix@oX1>=9Qo6$qTg3$ixrR~h3$mqbJut_ zYoO@BeFMFV>QbJn8dN!tzaVtU@4-`kZec(umlQe?R3+r3`uxcMp8Pvgp zM0^uN+E8lAH-_ut*V5FC7qgR-<(ztM{(O`lX8zs|b5fiKJc9N~j#@#HjXyXF&2{j) z%w_J)yB*5jHImt(EDz)nl{$kD+5=m!MBvFa8a*j$rx)Kg zX1ZYDU$}4c!jyJ&CocTg2$jL-Yk5CZ)*PK1BPICMu2IDLD;taonaB1RN4P+9W#G1*%uZx3^t zE#+CvRwK&)B8=mFw%mx6J4r6A_TCAP@G|g2=zw#GmH}jKg1z;_8K^VBTux=>7e7*N zJUn6S-qp9;Se?bp_)c&~to_op0WOeTrMmK=Q43#&TuhUYRe})1HCn1nwD2q*7*Fi0 z&Y{S77tEAewedj`W_ggCt#J&{o&!?F6TkB#H3f`?*F27uP2MScBK@ zaF01luG_0xSMuKQxm4;ANn6w=W#D{Lj=#i-z>|yHcWy2-jKZ@_!MTJ(x;A95<8HH| zk@O?*ZZV6;VS=A)pOzZ6#z!=S1h^V7>~^8KuZ2Y|Az#W8Ed+Qfo-|*(n>3x9YM#kL z?|hZyMyI7A1NRG4sj*f*9!?z9;rfSzD8N97lxQbm8@N^n(-bZ2G|=MMf|g9P2;PtX zvr- z6M9(aCiG`e(`xCDr;Yib!q+0O%|Rz+0!t)OCv*7PZO`HkZg=Yz`N^*69B$8g8Zp<3 zy+mB(QLJayHN$RP-q?8e2Uw||#HzkK%iu0U(pdyZo?9U%)i5|szE@j{+jfkkR` zu$7JoD3G77^?#qYpJ=B)&^B^FbQ$owxb`)hu9SFw2(OzcgmgbI`F5A%KYj(n3BjSx7vl_u~nvx4ju zC>UO_Y-cDRkpNz-T-xtUpKcrNwrQe3Jt?nt^#yGG&e}MSrmzWvnw7wOd%P~Yh;m++ zUvF*r-Sd~i4)1UwK)KKwTKpXNS!1c6>#tJd26nmPynVoL`0!@)i-S@%o){Wqq))!j z(gk}SXo|I%iw5v(uE(A_M<8H5LSZ>~(`npmzum=b07;wIXv*`x<}Q6UiIMD^0@YeC zT@C_yo!x`o^}~W-<$-tls`*v?o;exulf%P{v9))gAAFS~lxZimrH0+{MY2~x*~wXZ z{oN3u30+V(<{n_=jPxv7VYL;C;;y9RrEF_+j`vOugQ|d`;&scqMz?uZE0^eQ}+19G1RYpDKR3553u(&S@;gU)Y z92hH`xzjKIuI&O$^&3{$O;hTgPrcs|ftHIjZdTQV{@oZ;bDv7b2}}yB@wLB~1NyPu zcm;aF-w1b<;xY=n_SAs$w`d+dLL{58$ zmtZGBOS{PV5R;wAx3_`zVk#CGUEHyzJ0%!9`p84*IjY&Q$r~g!TUo^=ba^ynjwNfJ zTwLkyK}xn;nC4sbV?ouYtTE*EoMdhEK!6--hB42f9ClABPmQ*0-uPw+*WGm_zkkzB zZFALp5fGKWH6DNI1kV!eOXp)kJr;W&S$Oo~yakwEKljmaGR*n9svq2QA(k^j8&tkb zsfh$A|B7(f#3HjGhH4`)P7~UgE-1t7!wAZ&SzN46b+tx(3z$#QWkh4<6%8Ne}joY0h=2# zG0=sRcl|C_a)0=9y~76>ui3sqhmR9g7e;+{!9q-HM2TY3IkhnO1&99A3r^6=Hwcec zSLg;{w{)DeHwdJ0{Yqdi*#*se*ds62=iF#D&h44%0`f5j&yhxYOrZc-9cvS9zKb(B z+6c&b8w!5a4{_$FM8}VTT$}9!CG-UaI`-F7`K2T5`OF72U0r3Y|M%bcgq1APd?z~H zxQ;f~exGA0jR%a2(Ace%N$m%x^pCflNZV>{G4`HWFa#eKCuL7jm$k&}PAtBu*@*J& zJQH~ZO+d;$+jI%~{J4-r zz7b#>6@UPi3Mh_2x2x=$Vg_FID*I)uxP&sF=juA5wCI6BEZ!>Inf2)XGs*q$F-=IFJ@x-+irhRkn6qKw8oyet~vtNWfoX_VY ze;CE#1^>VV&Kq-#Giz^QEh7Frx(f-rH!9M3nhaPr2p)ReWu|(o;6{D>yoW^zmsn{TF&w?@2chetC!ySy zD=Tq#?{YBSN5j++G$(I>H8SIsg`SCUrj9~w&C~q5COF6VGD>jAYu{KetFJ5TKr}UQN-tR3L-_9@q!0?Ihu}A;@1#ndu`F8Prb$b5*yrmJSrH$4vbkWnR*;=qI-a zg@a_}U>x;W5XDh_~Bx(W+!l;^{iUk&bq< zb)hJ7Y*@RXx076cfxM3jMZihmLPEHF?rf6r>!Ir07V|ZbzE}z;dsY;!b zTcUUVGHS{+vybzG4JL#N3i}}#lMu;8Uamo3gYNzgG$-90>hF}Z%FYxV95c!(^CnRb ze5z=I!z8b7tM`Sy?--u+6@;zXS1FsmLQxDoL2{M8AyWdpAk20bi}Gljw_Zw^G*w7e zQJH=_Qb$drJlk?OY8mM&NEGZj?C>^eyYfBG9x$sVhj25Ttu?rq3;7`5irV^Ew@^v9 z40kCS&A=Z;)g((i+uYjMjL1CVRc1yG1)?!IEK;=i@IEDul~NbC$6So!My+U6g5a0s zl@#0@3?+WkcO#4u=W@NLGZAXR?S~yw&IUN)E_!7sd*0?ij&Ye z&Ah0E@>)f!)7-&h6H^dK>yM#BNOfQfoIWlT0MW=a5R8*ro;v zcT?0;y;_j@_?@c#g#^`EJ;mR(-<=C%-_HC=?!=Pr8q{A4EPJ7BRs?*7VAP!nC+v5F z2D;7Hn~-&xlwObswbY!NF6FpfQnKvxswSspowB~5yOm>~-7af4n(LbfN?mWXJkSp< z1dO2uG`#HEGdg155-J_gE3Hu=Xri0xR%)aJn8Nm@)e~BBFDjW<~zo#S4SVY-{HiW?=WV&0tL4I%$x7=2?rst z*V~JHScUKVV}1IaG-y zC8=mSot56j_;3Ewx^?>&Pn8fPbai-eau|D2lDNBf>1{!qtv9J?O?!AzmEd=rln#Vk z-a(Vcaw5&5Th&@nH&^nYnkOco)QLX5fPf+Pg1#b+?!s@#=jF&P3Lrz;tBJN$pKq?5 zUYccsa2%F2`aa)L^b57iLsMa@1AV<@b0>-1+hw{h#FzKO7F~?2+V*zV-4c}7+QGGs zR$#eJcP+%6=(MTAk}IKO{JRcmEj-!|>dmBH>cwhj68#0WugV8JHkH(jdxs=hZG}tw zPi|u$&&Gcb8+3qZa3k2w>bDUoU5HwzVjg-6=h33FPg&T~^^0f^+^lp8YUo+5@ zSwXkUQ+KVSV*&RN>yvB zA@V#$3G1NAr76qOmn&Q>GAMCkqSV5s zA|ph-jWt60tiwvzD|So2L--<=s&M%s@bHoS(beQRftFg|YKQvV_Q!h?NtQ0jWKa+c z&v}@Yigjox{RAh{n2c$6Y$*F}ocf?FYGMH!qeEI1C1+$ad`%!Y!CkKvBiuc(z&{mn z7{r^9ph>0m&J9z!Y~8?P;h!d{IF98ms@~PHF6qTyEZ>NywmF-q ze7GJToY@tY_rOzU|U;_>W)r1j2t zV$ICFk_jup5b%3B#FU9rZ0h!`!BJG5{tK1wDhC40{X1@%dTkj&BKdb+GAqn)lDV6F zYHXb^sgS>RQ={y#X}8_9d%{Qw2y25)y|fxzT?O`q>wpTsX>Fn9s`iQ-L*uY~yOJ(ChjQMCJQb z}0^S}oef^6eg=yA~=+)?@C+O5830 zzMpWw?x+HAYi}JOxS5>HNf15yYqsJ~c|-1qJWM4IlCd({Lt)&jr-XxDU^6uM0_^G< z_$ms1%B$R}+?b%MZy`0cVbkNidsiu)e0N361Q}=;Ap03jB0^NdSrF-|tUbBZr(T=H zF)INY?0|+~=3V9Vm!J%^E((zh1FVoHY14Nde$l^O^viQVp>+&89Bl;&6v;kP+^|JY z8!~_|btQDl{*XL_t4cBK2sRE57AAfFj%p1qx$qp@2xS4zrrGscAJpo~uVIQs45!Uw z1E{k)qWD>=*S(6Mtafr6MkGTY$NN1Iu-?nz{`MR&-c;IYU`gWLEolZ;-PH;SZ6Ht{ z#{iJ0Hsxh}4m-{V(uubqSbdHqTb7UQ$SclFVblCgmUXj4D!uOUJCXcu6|pZ(fH^tf zAl0>bwGr#uenFKa@0=g-)*VGfe4U-)%bw}Nsss(7`>6$VgD{@~gQmgS4J6n{e}++) ze({ALOK?Y_U+6|UrGPj?iYOx1c905DnrgLE1neb!1GFH6i)NL~l`ZYG+`&0=iwj70zjUx^K@K*Qss!QZhLA z^;V@X8Z6Pg7UZt4<$19?*t(^0C(b3yo2J7DNO+heJvz;d@oNE$JmpL0Q9`llv|25V z6EP>tPr?8SChfaVE+5mF{bYi(sOzPvq7*;!6q401%mDN6`N_MEirHze{cpGtGMBh_ z){)-wQmN9hL1H}OY?a&5%fo0+ZIOfC?LoBm7alJAEK zr?BVvDo*~Fe5R9vEhmUoPl!z78Jtg;kPr&tiPKiM_PwZN8doRbPMOP|O2#0J&gD|K zERM41U-eSP*`lK)uGDM5`In_CY<{Z_Op4R5bk+cHr_sUJl{x2m$huoj0>c*QxwHq~@Fsa)eVQ+%KW#a*nYbYGes^T#5uY|!+P?j6R^A*}|X7$`pcEGKmA#2VTk zTr%%IW$=U=Hoci%r8+bm&wez|K}pk0jl4bR6K+|dow1CYj3bM<8aDKNka{%^_H;4Z zG9%*G+yi;s%zNbmxD^=tGc@M;N|8iX{9vgg^!Y%;tmsvQX*N+$C<%I5FbW1Dx5klJ z(#%l7I9m!8$ni3Td-Jy2khln5DfpWqNCyppi|vP<+mxA-x>)0fLJ72Q%1ocX0%_r(7GC`u3UB!6;sB1q8#jD;S$%N!zR!8Z=Dw=wEI-l<`15 zp?tpe)p`&r?Q_s#N$9F+O9Whe=#|7A^vURTW#}C}mL2}qqF{Mh7Q}u0+(LR|AQiD_ zEPig9`G8VMy&0fBOu*JHFCZ>UT7H<}u7eN9k!wS%2;}V(I8t4pb%ZbCb*lKJ#ozUt zWdRnH<7xOVJ+pyE;v&@#vyq@bszV&-+Qzj6Q3g0H{A>j+7R8tB>%ttKT8@htApACg z*jAV?BpP7#3=IVs0Su=U8?Odqkl}S7 z$sS#~T?-)7P=s@*_GXf8J`1~=7#grZ&ZM>|(+xcKP0-pkce(%!CELb8l>4HvpGt#s z`)yjf>?C_Gc&t={0`J)+bnd!nrDq+*2rS1X5hquC$#OA!TKVoR|1lING)$EM;9@TG z_G^zC=YV!8c|!B?jSoCCucTm+f~brS%JP$rp~hRD&i=p;Q53zdk0Bnnp6-Ms zExz)dt?70lCDH-rBynUbo!W?~9Q(D-Dm|#V4S<72rA{#F#B{45NTD6W$qA@s{sI|{5$^m68wBiYR3tbX0_1O4nTvOD#_X%_@!tuOVDv@QyS<+{gM zGlaYumir{LQLe{dxP_!*f%D#%+7era2og}g)pv9Jw^Qd^uQbQ1SLLNX4`Q*0k9s8w zsy(0oc(?DBqW}TAW~MqC{_dqz*?F=%h|W*Oko`(_A@tKm2rB0(2`F;{Wkg!|m~x|! zy7Av)>hFT%1MA{sq@zvYOme!x&=j@{kK|)9_}zU2fub23u5JyO@;a_(`s!24FpD{+ zj3}&CN0m#&PfS*GuK^g_mq6))_#5_pnSAwVOlUOgbFkZ~e-*X2StG>HIlz2abZg--}@&@y_0t{DfZ^*#MPB}6z6 zSSm#$eRRpY6(7XQ|Pge$}zR5!b=bE5YiF##S$39 z0oqGGlJc|*vk*xqdy0!bGk_&$SOCa1tm>q`v_ZTBQ$knYG(fNa%3k9dT-|{-K z@|?aXTi;4WZ)UFm^APiZa>=B%f%68x$Zwn4sqVd4I9%u!;1+$e-u`;pwvIcHK(a;> zkFZgz=6D3sGqYP=rZ|}zM8)WdyedvG1N=BQ8r&Du9o@fsZeDLM?Hw(D3nJIBaMJGO zhRrhiS360wGL%KejGp~`o4~1hZZXjYM;$j<%qW19>9;qxn%gN7x>um>~_~s zHKt|TAI~?o!O=MnY?A_;=3l2->C*o_A*stniYBt5Syc70DupFAB7>PtO1`GTYlO2TQK ztH%tbkmn`*p>Zok$~yhlQ*2pN;ryg-vqzFVb_-&X(TLnZ<^sNA5W? zag(xWev#Iv+1hz*(1~+%nLYiN9S4Ht!a*i9>k1airS{>O8KIDywDeNg-PQ%uxkNI= zl{up*ef_t$OXOpC9zZCOZHyr;vv^E!a+Q7bzU<{ZSd0v0)@r1{U9k#t=e{uk!=EY0 zN~pU4&Y13m^MK^<{O?q30C}9FryW1n1k-3Ezw_V!7+!~juLVLmUHz3^FBa7>%Yoay zX3xf9v7Z9~sG7Syzop(y#5J_*be;u!h-S3PMEagMX0B#lcD9r7|1Ob$h_!8QBJ6Ly4f|mv-vt`RdY^6p3@xiVQB0f+UF#((;q>0*5lPw*HPqgQP_?{s1zb8`1a9?IS zj<%bcpi9`X`{{HmM;{nt*kE6j$szO(Uj6I617K^mbi(9Z9Uq^v<&k}Dj; z-#NSZ^Wd-+8?p6f9iXb&-#?cYiyZxZ5X_X0935uFFQ4a~y;T_79gli|X;lqtfUPuq zl_V}C&WWE$LuD7u6TPxRo)~faB=~sd&YdOyFk{G%cpfPzh z_$D>|Ae{LH*GrSUgOZZR&qlgXL5Pa5Ef@5}WS~z2g7cT|A`X=L?KeI3Q}yi-*3=07 zEL$=gfjO}Q&2EzmEIRulC$!Wrfd1j55_QC9XvRVb zL5eE?T4$o6IUGQgl`nUt3It&IO!2BbbxS|8REkb)$=*eQ{BB`@A<kzJ8_5yJSqQ1l=OOP}Vblc|<>@%BqRCW_Ha2thj&XumWLsbV~eOf2j2acn2s&_2Mnaub7TLDe1$d1OWzEo zqhz}WYGB%VD>FA;3GnvS;R&lc_aP?}qW=9pb8@QWIWim}^L3CK!LcCuZ@-W%Zor@- zi(IEZ9vaAg=DKS-`#HK-$J}?ND*6DCn z{Hv3%>duD%6waRTx>Oz4hhd?8J`A@`_2!FoEAE-LOy+7Ai;Xye1KP zkegS)`XpRAl%nd0qWk4kjvD?(XFiP8f$sIEV{PLRGecIdfENY>`b>)5XYf$7gNypA z`a!_I_XEbw+FKxARQ_?G&eOv|DlqK7{nI7pbc_M^2Ih38}H@# zEkwor0&75NN4cJ0D57B1`mC3@tsQl%hp$##QlpXjnYY)M0F8}t4UbMfxDEH8hB-Yg z|A2$I)yYhSnn8JmQP^~%9;qprZeC8Sb>w;h4tZ<>Yc=94*Q>5olguRAl>Ga1# zwOe<#@M{E?*QCQu7t9NpHKv%4Ed6*1@4q!jNS@VtJqi%!k?K*ioX*hVH&ThZsEDsw zK7;IT*^g{m=7cm9l=PRC1%}Wr(UaZtiJgie#COd~-Go@-@r$$4$Pv(rL%_r&>7v3P zh-zMIAp7Y7bl@QCaxv-!v38~o{319_7X(yzMR+bjVrhI7$ecLPr$^-j>k$cY%h0$d z9z=!Len|m6P=VuZUoeOyhZ$c(E6n@{zFsm}_T3~GI&jF0KU{MsaNM$$e za!|! z6urq-N zF>Cw0<=KYFSzi-b-rSuxBi^r1c;uL-^m$Fah}{SDM_cn~6*KNPR}A%%`V2L`X>Z!J z2mF|W79Lo^P44H$$CBi2o$LTduJaZn^yCk;<|b~2*lGhyt}7P!xrFPmR~_5bZ#J;# zfu>jC4r|~Cd)dSgdZ_n^AJS1Bur@V4WG?9l1sWa2fpS?*iLhorvTfytE z*FT_78g1X(H(=@mX+hGA$qkHlN`*^Ql|*aoZqs-aujk+<0YAJgn}+gtF1o5BeNjG- zuzSoKvvPp2M~ayRnA1E3#-w^fEAk773%KM#xocvre?x-WuI_gB#)>vS+7e$EC@jX0D8l*)fWn3 z8x_Yv{S$D>1&T)i^O}O;RzXH(^6DjJhf#Y+*35A=x%{+?u#9iGUzKqSrX<6L);~qy zWHy?rQ6b!j>uUWkWI#i zZm~}cR2bu_WT=gt$hsA!`>zBx$`^RCtA_4tI@T3p0@o{oQAl zOEc!@L(lQ4g^&DsLz{F#3nC=?NO}?=^H-5doE(O{F90A&kf+>elzL34XNBg+usK9> zwKdB~s1#fJTLaWPuu${i@e8a`SA?-;Prm-x#HcKAMue~n)}Q0zNbm#MkrSp%-DGAoeoCvu6~p>kGQXwUHDCf zykm;}pO+$gU#o${ss?G-`0SLce-2w&Zi<*la`+`tk8d@c(0`OjVm-H#jIKyavmT2K z_(aT0vH$dmb%ZGQK@*X%kCD_-DkD2D=l=;WXPP5Fm&FJ6On-cpPM~`MaZ$tq%aQ`;hZVNEtndbl+fU?oE%qiuHq9#_HmkHxWDr`@qZ{ zT*hRKo>&RTP10BIK??sZ=B??!6f3=WW9dUQx#uA%c^}31Mj=>bVCH`;oyT&lNDxIo zhy~s)8xM{5u){Xq`>!ALVpfWfT8c86H}4TuiExR!{SKKSx}n8f5oC99_X$E8hfAt2 zdcBSPc>SH{>F4bOI>ejAJuQN9oIbmIn)-dHjw1I7a^lPbL%5`Y?&ApH z-^rxhON2At!Mq$h>f`1P%2f=1#t`9GH#H3h!#66LN>GT%&C7d~z2}($b#lk6Y*ET~ z1rgSei$C0=lfcWV!NaQjbBKrvpGek0vGJWDiwTssirNTL7=@Ni$*S>%r9^4>7Lz>K z)wxX}nG9w7e(fjvdpPDAN|P*uRnq93{pAq7&B+i&gg=V*fjs#4>jb({|$xJtmoJ+ktDprfbz4JE*MO^r0TQDhlAe*!|6^5mh zX8RD?-I?{_%5a{j#lF7S*-r1-b8E$WHCg^R+6sH2R$iu-^w{Xn z2N{Cf>g}P*yzU%vWu#NV9`QtfO%xeg~$md4*wrr+!b`XU#DgGd ze5e*CadO(6J5|Nv2&x>SrQaDl(r-Y#kRJnNVT22`t<3ejLW$|7u_DljTh(~lw0u8? zp)^27TTz=+^4%re*KVjJmzoE|+4nz=F z-Z%=dB0(vG@A67=(YU51m?#byNMSlW;|bPMW%-G%Y*I+f@#WK_C37iemzbdKd%p5c zv*G@nFLOESPVl@wzjo@bd^&4aE2BQb`$0(wj%)HP5a6jzt@5g+nWnW}7>FKY5Mh92 z$fLCTQ`v{@aa&#8bFLrcL|;q=2SyIw#Vuo@Tm)d2eTV3azeqNU z=Yp>LoUO~e?yxfb7T@(e1_Dfmd+bf!cdYJS2xZrW!e$Tq_h6M-HtYHRsQ?8)`oFVx zTuF!F;7q_;n|%zWj?Mwz$6uQpNYrTSk#74L<6n9IxVFQlx$$^v0;vb$c!f)&;phEU z%!v5hW?KYvw*UP(X0dXHKjrNnP=4^3x0zkAw{smmL&;3p7i{eCXVg?7k@ zW^yJ_^I>b<@>TjH9wuy@k{Q0EILfWVeNQVLtbV0mC+Zo5d0`n)*Yz%fRlDH~w)pDaW1_i~m6D-d-)BymN$Ufs4kj8+xWI^VAz z<0H}rR8i}kFfp;aM^greAT4StG&~7{r}QD5BQi*;2}k zl*|zsdO2EMI)Q^0W=cY7gBKKZ2Y>z8YUt3NdtO7a#5;SZ0+Vk~LOBY`EEKpR4j0KJM zHQKD0wukw*q0EEa5q>8U*v?-#N+hF??Vj6{W46q~dh%E2O_C;`r!eeecYb>lU)KJs z91r?EJ@r~C)`tUbtlTa5@Shu{sBu&yqUp{SV)%ti1tlZ!SlP`I`Jx;sy=oqTn^$oA zQT@o(+RLQpk~4iqk?s+xCn5x|iqIET7#Q_vmt?b>Dg5_UYf4$H?>MOy`TF}Cn|R@t ze1S1S{#6vQhLQ_0HE=NYtQ5l>Vp*6xnzF`bIq8}G$RNQYpSwQWoZG1Mty-J=iy@%A z`Yd&N)Fx4{3U@)wXPD<7CE8K%(+ThK9!LY3zumxly?n<<*=M0 zYy0)Tl5>Ga8e>iav@X-DL?w&kG7nsL?MAeVU(;_W#3$Ro6TIdu*#@4az-zjEh+r#q zLY+bO#x$S!V(l^{j6`b=e<8Z9X7#vbV(ck^uFQv1E_zyf%Ye8G(qNW~r|8f`N8o1S zv?XM=M`0{{{upXm#pL~5|ay11Sl8{4louuc;yKB`YoN}$S0QapO z^fzpUn97>9Z+7CZ1Ny)j;wazC_XoFbkeX?da>%Z3kfYk(0C%3h81-7lw@WtTv{d9X zj6ZiGwktJ-D#pQk${Eg0u-t(ggdRu!3A$?aZg6<#!0p&*#whk?s}IiS*p+lYnEQEctH zu21AMiK(6H{BOi2HaNI#st!-v^^M^x+;Y>H3}JNenX z@Y55yYi_a)RdXzC7oc5Xo>ZQ4(@VsAUFc$t7er~my~=)`9;2ustEJbx*(t&= zqsvEK1AOfKkjjWlK>a95Lu|3s8h2#z&GVcL7qs;)fE>Ben@H5|!ubF$!2i^Oj#Pn?5Ix1k{5UCSXx~zJ#kX*M2LXT|#6< z)S=g#(6fb!Zesx{FMmd>7(Ov@d{6#EM1Nhs;H4{#-)1k#$+7ev>A)ZK#~-sHDhLR| zw*=N$$-aW)NP=_$sl)%^gt{Y}1T?s6qV0xDjkm|4x}`fX2*j^BRO(=H7D+iQ0opq7 zMJ_XtTQZu;DAgn#<>j|l)34KWBJo!T4WUWng)nQo1z^FALZLdylS(%uwmDJxV={B5 zumk%x;}k0NZgDG@Y(>2U9w;-mSF^v>IsmU*c%E8W?BO|CA05*2LT#r5z&)b|cDqkt zuX)q7tOo^nf@l|P`E5@3U`6syDqz2XA?fNVy$(QO=DaceJc`0KpO@;1o6kTVlDUBz zMRQzJ+w7_^nZtpwX4&Zeeqn?ehmTN+11=GzIdRQt%yPY$X7$I;bh@*>&;V+6of~86 znD`}83aiTNB^%*2Yyw@(Zbgkp8YHGB*UAOHdnZ5W_~DUgpXIwgm|E~@8q>lH zD!pzDUY49?=4yE0usVyvj7mJguSt$FHDnK;NIBEes_YG!ZJ*IkQy7xmM2NV5D+*xs z5KlaR7r+{gq{+d2SG|FRs8jyK4t%<#G|1Zm|1EcN%(&-Df6^0vpF*$OWpMZDifqsr z9FXe|pR_heQ(oYfY;sn*&y~~NU;hn^x=dBd#|}#rU@2JW_B*4$>@XmTYH4w9Q;h_7 zn#kEJ90!gIlgvR23z)$Dw2H>LquHFZV5Bfwwd-M+7QRR7?*pcfOU&jl6M-M|s34o> z=63aTAI*&1He`ptPV`_3|DA5UNju0;NvdvtcgwvYr|t{P|mYx)Nc_!L8!l>!;)O#phX3OgdSHgH8)C3=XB@(mJRdFR!!j4 zyIwEMG=(>|JMU=o^>bTW2IWLd8a(G_YVu=@6*0`sojuIh@H~XS?20PGLAJS6pd8)V z$mR#5-P&GY06RD=GGCOWS>8j>Tf zpaZCa*dn|%aMbhQI!Y5Q&@h*3pzbrJYQPmMcRbob<7oW2Nnq5w7pPEp?RIY5Jn&^< zjok*0LTo~AQ_))ZBs9Z;FjX=x`MQrhNg}^(!+eSGV1-6-&w>LNH0_Pao!7I7ZW~h2 z=ip`=`bbA=7WjNF=Ll~o2FPg+h@4#x)rQR(e>CBP%BJa%ff8CI?^>oA)NXfOyI?Yq zrq$8x=T^QG4oUEMAt|r^zRMjbqYfD}*7O%GJY+;WzRFXW&NR;P1Vrx5wtCeqK#1d| z^!&1xNErke_R=zB4v7>t^Jd?fQ6EmcXOZit9GCWzSkyeZvMFqI;?L??mPxW>YP98@ z@0)vk&HVM;hJZs9_2MloQh=fI(U>{HN4z?kMUl^xaf0sr#yUfv(5*|W*h#b73tW@b zLH@D##*=u1CI~gEY0(_dDD@*6d?(8{qAc&fLAXfX0*(U(X6ffz7#Dk3xgaZs+hiST18)?2>+hrNzE}Wa>64$coB@;+#jDdMtMk@ zC|z16m5`De+LCMR7$aI_*59&igaB)2L#QMdc@K8;1F;~w!-^GgK1^n_1scS(8{l)HO!tX!>G5cCCYk|IQkFTwWijKmF zTHAVwazVdx>ZwcHK@LnDAya6ka|5y)wB5J0Ov}_2e-jjzl$0>zowWN#MNq|ETDk3KxW{lCP7&!cP zp}h)XQ9@;~H_i`ZG?Mv6)KYkh6uCVu9|1-^^d~nk^gdDWuA}Cfj|j}*DS43VaYd<+ z6H00Kt1`TV-B({}pvZNCw!c~y{`lsB_!b-df}3Cp{M|%D@7n7h4+d@^9~-^gV9eq( zrkw=anr^ac04rxAbrQ;HG~@I8f^RH5wzoawk8xJS@ggFp=v&K1+ujsp4AKgzb?6K+3UeJ^FOe-JFj&o213*tulzJ@CSIp1j__0GXceh~;qAMtmyOv(E z%=@0g%nJLtmzdp^)dv4Gz-zTg`Akv&Vuh&!r|F*cFlm+;4jLB#LTf3xVvq?eBE~14 zc2QoG>rSXNR+)bBuCCk`9&k=bftL2>J*964vMGV2Ug8U>>?Guh-AE@CMgh9#x#l`} z{%k_GSX2a{Ldh^nzC1SehFv^f4$jtJQYPD0$G|D8=&@>0PBD_nq!lCEG?tS z?QPbvf<3@}^9%PZ9xJKQTo{C~(gZVeBMtMi0p4U+O>rqY`Rfz5e+KQ0D-ti!NIPx= zFKcBw}i-Oa*k8-dl$&i1kShM0amo+O0=^4HvU~|X*L80xL@o3 z#js_0K3_b_590VQpJpRT-@#}>PWlCiN($vTkwDB2%zyLqO zFp^q#n&h~CF4XCta$T)oDlmO92Htu>>%%^aeNnj(feU6%Kj8QQ^W5)Ofl|e`Q$UR% zWVQ(O0}rnc%ZGp|*#hWD8mSpRGSg^3Vboft!IIZcz_PH&qU)R=E2s4lHlih1<<-7$81h#NqRsfzGIBy(;18MWi1r zqPxj*bt=a*M1DAt6E7&0PpP5F`132moGP~#pVR#LG~74r{*HP1h%fu$BM;?gpNj6v zRb8B>-deCHyqyFvETTkRa<);_-h1CUacu1KV;x-6EjWf$p)kn+SD<3E`wCg1VfczG z_X|z*ZeoIN;!Hq(9@=ymnGhn#7hc=2uTg1o1HCm^W@nlNumC>C68!6JpDxyBPR7Fh z_SEf#u`d|{D6U{(MGDJ&(+_<2U1ILuj%4|m1&y<#0pUE#VDp`Hpi1V~!@)5JRIjZ- zJg;>%k+KPechMGbVT71c-KG4Z4V5??1PN=T2q$7NLW&PAcYr|DEYXJMW%e;NY~KQF z|IjZ0SV2BHK}Eye?S1)Ot8S5u{u+`_)|l=PEAQ$rcCQZO-?2**b5|TE=#+rne;ze? z+!Qau6ow13(%v;l0`Yb!u!(@6I;ITBDT(_~G^mwkTK(O(qZMO_xhLcq5rx{^DRe^Zb9Q2U;srMAor)B~xnGf!x-krDoW(i) zJsuB=`~%BJXi*X7nUdjza!aWf28FxpPk1;%$j$fgX@>TdFw04h;D9~Z0hCLFNqogx zuco)YAU8`ZEjC~l-wT+KJus_80NvG9x>#H#EI&3J*1h<_JDnr<4g;$Ga9IE8OfSLH z3u-;t#vTtwD5BwX{Mli?ze6#~W&L+8NX`(%U!M*oz{~EHj;Ge=(^a#Q-RV-@3olL9 z{dJ9|Ng@NJW<+31WPcmlZ|d%dQjjqvN1)yCHD=o9eLp zOJMTvouHz2nb^+@ry_s>Q2==dD_Svi&w6#%^$~pDhUUU?>6hG`cYo)|w9Pdl@sI>L zbzUu|?tUP49In8^$I`8PRV11n(nKGi)jE=>BH`0#e@m`= zj7OPd&F@wS?~*IfYpq$BJTzv;GDt-{x;-{KJ{cHJ*%wZkALo|{n7c9 zaHSD~-Un1p@Sj^Keg5dmqB&+uWzq4d79I(o`_t4r9%Kn1<#-l_;J#|Ew;R7)+g+=K&e83y%8 zU&z>YY+~WvK}imlBpfIpnr$YpSp6;zr)d;zEP9>Kmliu**AdM)(3vih11QGN-&;6y za%k|neeL-^n}p`_weZMD)85cF5V<_`rDWKSF!IW8kFa1ZOf2j{8N>bOTt$KW<2?B) z7PKeSriZ(3oPCY4Cf1b6NM86fYK@fa#I~oe(bK?ko{3-Ks_OBuzrsRO&NmuG&p%yM zkE{Hg{Fx@~B(h&aQ{P-oCCGc>neL-Kmja?JHSMRo_OM%)w&mVMqVu7?SOm>qz@f1~ z#fPv+Ht`cUCQO1FGD7?4?+h9hOO}P(UN$S~rXzGiNMicmm(l&0JsCV^>EnZe?U+fw zPZ>|dPL1V~Pwwn2j}%JdZnyBxkb>C}x_mitHe)dQM6xlY$PtCH^Ll3Q%7wNl)=WT( zB`NAFTVowitMfww7h>tZg*k1dgHnr=zHab35Knny|2K)X%h||I$2Dda-JuJ5UIQCd zw`G+95=Z;>y&Wc|`6(F+wJh|GfKq*>$>_V(tu9nsp6^YH?POuvQwXtBN0x8B$v@O^ zsH+*6Nr(aHK6rwkr=(oGO5X#gnxS7lzEJj}hUGLx-*)jyy4i`~#Niu-o-KM~tj;ak zbv>L{z_)HY)!{}?&5a}~UH{1V)+`KUddm{_{;b#zhoqhdYHcNeEpx%H+1$W)V=|lr zLpc6HqJS=~K&Q<<0r?qy!vHZMyc7vZ51&URdSNEJbPe>#;Pp2jnsYUEVmZ@|+@Xdl zJt#p>(xl-l2{`X^m+ghYZ6X?6{SX>70-O7&5iZ#as_L&F`ZPbHS6dB_3bnd}4M6Z- z%`a}wKSb}^wT9@`2&h?#i?BXkHyXI#;kNbm2fP&nGau4xjr2jcT5q z;3`m$&|`L zEoq+o)LS8o?_G^65S-@tV%bHUlXXd0%CJTSbpRuLQJ80ID;F>xg57?Dcbe2&^5|Y% zC*<{n;LV=)azC^10FFl}SNuzDr30gv-y!f`{|Hv^5h%=3U0iI!`F5{0+)vCK$;LBe z(ZpdS(v!;j({RO@s^E>=5BvEzp3V5&mO?Q0cjLPzMBe$#d^TdJ3Imsr#KTpigJGpB z@=dEgW+{kozQWD<>^^bB7ya!YaI}V15?WE3qdb^K-oO_5EQS_`!K5_&Wm1oA)Lcsk zC{M12VSW&$sF%6AuA5fP!rn1*gFZjcC&-V{owS~1HbEUBEG}f8IhZW{#08_RfeoW} zYR;@C|4vc&@zVvr#&+2oep*s0%@{h9-(HOvd@}sRbKsh5#z@^Ui{DGvwQjxdg+ZSr zD%pEii^|vAoLxYqwUIEA*oEe#0D^K*)0C{u#obJ*Q9G|&)`q^+@1kFpn-%t!qQAq9 zKTQN`Z7bX`rO1%XT5Iy;!Gh!2i>H>kqvsRtsZF3f2m{|vt3*%iVm(d{#Cgpk@H+wM ziw$1cuDjD^8?J>c7#EO%|?;Z6jgF|+4C$!bvSvbk$*%|XD?5OH>ZkOC*1K%lG9|}( zYE{m3w;}{?<=r~6jd4O1*ozcdxtpEHp=TgZHGF0!ON(cy{q99-#R3Z0Ir!$r36*x@ z?sHUSr1+cw@=U)6?q3^i8#s8#=mZQDy|Bt-d#1tfU=}dIq-xX|F&)#HGlnUyturSO zj_K;%YBB!yg=ZjvGfw}5`hXG_E~Rri+SRvF+kY=@zQ}P_``i3pRV{HGo@epb86g&f zCWHM-Gk;i=eb%Wj7aPtN^VQ@QEHHkEEq@)_)kGK;t#7VqBg(04nDs}J`oP{pMf>eC zs5Dc0GEd@TKX${lek<)MGikWmT!xvbxsJmE9lct4qm#P?8;v=8XR<|Km$s7^ETzpz z?fqMse5dt+Qj=Z_j`3Q**-zUQWkPk=Y%%H=pFL>=+xBL2y>vU>7w0vwD3u4KF4d#BeMe|)t|9s94(Zq`Y=hWvvQ;qO@euu(u_ z@(wyt5*UchD|4`89B`(KSbCEx@JR&HrK*!Be`YTQ_dRv;3f#Kz`pwXsjKt=|}4-+_K&pJvi5(Rx2jX;OMB(|jZ8)vc%G~`5_iv+iNm2yIhhXpJid7DXL>J}PpN%W z{m?4y4$x4zYa>ZoSP^kBJq=bGWhSEUg^N6!l)2s zBveAK?SNBbX`xz2`uT0|atqrMYUFbYG|~${6m?D~z6AVhZ>Hb0GP(gy;A2^R#cjEN z8LcCczkdBn05o58II%F66zEL=N76R|qbAG?IcRm4Zz~#QOqrd>WfBQ5ap5Z_R(^As zPUA_8bij-Hd+1K4kM-l%cy600x9Q`?)LX6F>0#%JEZ^2#mQXy_y}Dkgp#f&@Vjnz)lx{n) zXl^)=8qn(X+pXQHA2|8}22k1HtJkX89c6EbZ5DFNz6@;kpkt?96f`Qd%CN&WwTG%@ zp^Ri1oNmtx zme9+R!vy>Ws6junyPbX}O8zb@PSr>A@@fT+y zKDJ`TtF81T?1*3#iXI!tO{#OxiU$zc=PV-ak!}4X$EWSX3AHpXGMIOp*koy+8Ve68 zG$dvx6x6F#L6%)%zrg0XpQanq0>f`qmaBUKAuj9dIPxJ!va;xOa{%bz+#4aWByAMfi{0eIt2-n>AzqaQ^ZRHA)_?P5C-~N(q!?MvrAs9s7CTk zV@{ZHRFe?=SPbQ)FZ3(Ls}RPt1V0t&Kb*a z1-}kfcX6eWKKW%Yn=P$Xwocp0X(W^Caci*CIDJ;!+Tw#n_sO4lyvFSbhm5uf64Qd6 z&}_RbJ;VgMe|uiwbLA2IbsE~SoH03}wxk$)8l>lSgs-$`H&A}Xk8E1Gjd*EldPeDg zMa!D$QvE`puHL|QUVtuX5)JwJ87WIpZ&mI{NnnQ>~P3W zDp}V+lbTm{6!%#}FCDo)l;5>F%y0W$q+ zVcna<4Yag+oWab@6vFqR^h)RwKHy5!BZGpN;kbAPQ#L_xvh?M@`L{@+@gjc~eYmic z+F)?vgO5nx-&veE$vg@ktVqmCL%FM&AeFwDM9BJp%@1+@#(E$`86$-&mSUlg|s^=0SK{u@WJbtjAq5 zlb5UsI~-Tb)Yk}nww1xX{ssnMY~8^JF1`zC0{F4wudlP)$DCPyxb1g_@%-If1-s-o zJ_9XmsQ8AjLiEfk!LkE~?Cbtr4Tt}ZHKO9>xi#`~&WPT`B}e|lS&~Rcuako; zN%3g0s(-3w_l5csVtf!>Tyk#)qk>Tb{Pc8t!?G^Tmt?f;((T2^NBJ~S<=U~>=Da3h zV=>i5Dz}EUY9H1=`d(_az>l>_5vQm&sssGq-2@#-1ZRhlQEp=J0QX!w5VujPz_-`H zHQlQr0~E0a?6J<4@w$HS+HpAP-GQx_&2p3zWZ8>@{|_{hKKl9%Aq&h zy!)Y?*b?Ce+F4u}ev@lJpAz(~kMz z!LKz=PC7$21K~Q(!V=OB#n`{RXa+nP_q#z*1-uJ{5Gp8sS2F+iu6SGh0=X(r{G|Fo zuXZFowlN>k(|}%XHMNC-4c7&zWbOGc=2%n^8zvBt%X9dTP2A5mWrs|OHfI&{kC1cY z8EEps1d8N(%YJ$UVm09JSw{-|21j+MoKJ_zTiUYsIoF74v~ke1^Wvxu_%uy+kUGg` zy0Z8gyRSFchAQlapt$4U>iWf==tF;AU$Q{+uSy1k{|2}Pm5P7+$U4aVTk5P$RQ%U0 z)nDE5OArQu(q0;}fg-qf_wzmuiB58T>WtdS4rt?**X3uTzevv9sf6uybr^}RggwNM z>kaP7%p)N<&NgdLZxp1V0Ecy&m7%cFLQ8)T^`oSaR_Z%0nxEtswuPju{oh?ll?h1# zaaT-b0pOf| zuK{1zza9sgvzH^D=4TJuuPHq+{OanTIUCxoLf**VzjTTUG1D29P%8}bhD>`Hf=tU2 zHi#V4F<@vyI}+t7N**UXu0mR;ll-~gDX`DJ*)iQ1dam^f z+rO3jCwT>!=JE8wFKsRm7j?>ZEwc4C0WH>FLfUt7Zn z8l1M*`pJE$bX+-i3bkLvlw;~>)%K{seQ)FjfGp ziqp8}x3i_*m4cDcQJBu(+g?DuG<(hS#?L(;? zMNL21@x0>pRsb07cXgERDJ)-x=e3M+I)&}i05T3lrW8ntxJ*A#E4V30r%a8nqEc@^-fOLoR1P|p!X>N zCwmmN3-=+n$_H?)?TsaR^;R`m8ZmQ&0DN@78Y5Jihu(p@q4r7T>xJTW)Q+oR3%d>i zq*eR%(CjBt1(f@7=COAf`J6MmRA|>o*)oOz)nK~ZVfOc_+)SpR$B+Tb!!72O=+0_g@ZdAYP4Qy%R3usIV=oTjJmj}@+ zLyD;q`XuhrjzIK2LfTo-@`owZ;0J}Z*4l;v02*~eu%dk({lZMqD46V^Ca&x)obTZK z8oOWU?VuxvCU8CJ?2c6hz?NDV)Eg*d4ULFFk=l0dEdjD~iCYxS=+!&jB33B~B$2Qp zMRY2J3<_-1CyjG64wBbx$AY>J^>^m29K%ww3SSp$n-Hs*o9*sVKCds5BPThdWJq<9pEsGL{zA)gBUX7~>yJ+s*vIj@F6Tt39Nor~GRX7TG9 z5Jmkyz!AEa~BH@aq?sw zK=CLI%8?mm#>vzSVGx-^mBQS&_~I%0^n^6CO|ZN99^ju~ky!FcohXlGPVkf zoeDq9IX3>z!*B-)0oBx{9m8k++#fR)?;xRMgHWtzCHfn2HJc*&!9~dS-8lV1?_T4+ z*%E$cWQpH63&PwIUxa=WYRUx}6?}QlbBiAS-r9ESph!8)l9?dLxs&Z1Qif>`8T`n- z-`=_L>eiHry6!;UL+kUeCqAsQjig({WqAuNG7MrOWk%5rc9MfSK+(5;Fp_bDlT&F4?C{tU}D2mX?@GuC*upOTC3V-X`@5nIPz zQjkrL)+2%}1RS%%i2S}u>%-PHJ_274z}cU{`xXMi!#KbzF@VR`C4}h^K*2yE%LVY; z5Hm*&bXt90Un{S!#(oCp z42BGG3l~nTy%P&xE?+x59}S>qW2l$E;0cvq7l!#acJ(iWp5b{bw1mxE3!rgqYvlXK z7m69bRM-;2^YN7MSJ#sU1Yo8w0M5|&HmLp&9AWDU8?U*@iZNSDQNB&j$N!OZ7E979 zQ4oG07Pu{OX&kzd9q#Tt{bl|eG0O=8s?N#$vMH*1&b=^?C;eqJymWf%w&H}5>}Ke= zl6UAPbR!&_qB1q_)Fk+>M8n@oHu!R}HeA}B8AGW%t*AS`3<5Yu!C1vxyl7?ev2AjTkc1|Co9v;gOUKHzXMIXjNNDnl&xJt@F9 zVs`KTWj^d{-Uo@$=}~$r<&O>U1NDwLj8i3k{lLhZxL#}X#d^JsB>b_?eiabLlfyQ% zNW)@cQFJs!S0o^O?j7kp+{vVrm<>K2bVE-C36nimA~3ykxMfeQSY{vuY!z^Yl0x{3 zP)q@Z=qzGAfJjKxw`*W}6}$U*o8>@w!LfkbzQ7gCf4}X|j?xGE;D#_0#K!AY1r?{E ze7;H$(ZUES=GWbb>G!B~>40^>qv=J>Ev5}MZJg-wG`+>cr7HP(Q3j&0UE2+dRA&7` zIfID<25j;EwTPq;;gqU@fdvUuHzG7ciu1_a;qg$s?ZkFTYvIrs`sAqecfeM)ydg2V zmht-*!Xy!D-Dt5wedkCga6Q@n&z_kj(W(6X2~1C1w7`id=PQ<^zB1v_oln(`K{(e% zZ6euF3)%e|ANC+2W~ZB{izY3opn+klCGt6q!yQcOM3HkOQ(lr1$oI}2F;bxLX-;Z| z?~$D+|8m}VhA?Lwp^KcYZNkJ8bn8_nX{E&{(j;e=c=w-y|4ORn`1jE}4)m<_pCweB!~%y$V}cwIS)=!ZtOwXDn*FZ}r>y`!n6$cS&Q-I&-t`4LW46du~R^ zb17gG@x(Y5lpSPm&2J6Wt_A{boL|GK5FsIek(Qq=on=P%uF*t*e0SYblej&!4pobs z+x8ZJopDW-Feo9eU|8{7)c749vxBL0nZUD~whc($JgE`(z7{}0=x7BNO(brhc_ls- zsSSJz>hE{7ztdpsUDd6IjXtSHwY8adN*RvtBd%SjL*YKdRfY;YX4RA5JhU^^y85(H z^V}c@?pDWgdqp+IwJ+_-iS~Gpe@8aj59s-BGE)HFX+wg33DUbJnISn$YnM7Z6-OBc z5@3p4P*dE!ndI{&5J>G4gK2>;CEZshqA`7wM^3K|8n*DI8xi=!o~XG_6;$u1G!5zb z86O>zkT*P{3?54;BfvFf&h@EKfN@2g=woXTKl7(v&l|D|42b;=z(w;87Z$ffI#K5d z{O!&$3q|6W+2@Yu2g9n0eVZ*b6Tw0_&rl?&XiUdY1IDjIeN&#mTv`h4HovQBjK6!& z++Ese@K>1V;`;`W{vmTAitDV>f6D_HgNOWBh`I=zIDrz@{GZX}-i&({1T9zGg^C%j zV@rXr`riwFlid(wVYcUX^Wad2snqbT=Ws*d#U%*c1#;`>{6yEiN-islXlqfvs}-EG z9kmi(?i<;a`Fo2wNNcCesk7)jZM)C$;}xs3K$s%;3kVCPKX?*cRm!qvwAuwe{8BI{ zIjfk9iM8QYh5j1i6LCBcn#aPm@^G_SFJQ@67H|$p3iNFFsoYTn+CypvjJ0KwF?{Q zn@Sb(7=lqM5PJ5+LzvIH5XNF6E}1VPItgLY9=eI=5lH}f>^_5cdWv1d5>6}vSn5fT zF49r^U=H#5+aD-)&6XO}FEp_NHIoE@u3GsS2pFYDx9uH+EFn@qR4QMOK{7fsLtWtHeQwuRL1mrRh8%{DrSoZh++K(GgG!YoZ3-RK2F?@Y!&n)<|Nhka_gh zLPM}uXcUIskx`Z3!Al2phHl?|;H!5Y_Z*pVO~@-Kis7$I6)Nv{Z_24Yd1#tWL}gCk z8|jHEbdPwc{yWQnBMq(h?umDcZwm=iTBXMXD-y`B9872=uMG4zqNqA$CwaJ{Ee|?L zCTx0h+CEBOj9+w>E}q&1;@z;%8A^#qqo{2df?eZ`@!Qd@98-nerP{9X9pbmhRt*Qx zvLLcPNY<=Rr-Jg1nmTuJ+Xv=9_o3kaRaPaDf9*=aZM?p&Y^iAAX!1_~wYlVOEJ5jN z1mSr|v;+J_KiaJ0wtozdWMnZlRe+6WVMS2hg0naol|M-C$He<~l!aQ?C~gtppDF_0 zbS;STT{c+cE%t_wgFcPFF5;r9p+fkK5c4&F}nPbB^bPqow(@b~~HW z?XF06q-cn!BGX9c&!>Nr2x=`%l%FrEV*FPMX3+{Z*9TlwBu=8-rWhx&H;Iecx;1Oo z&`PVVb{X&X&ee{KoLU7`Ke8^Eq|u5srj&mfqjAJP9t?q#c1Zr1Wx~;!P4U&a$oVN` zN9W^H4|~nP=0`v27#kE!1g851{%|ot#=1XP;?uy{ogPl3NK^+RezRxuPS23_Wqz-T6!yHAD( zeMKXx9}$*Y8Trtb(1<-SrpkD@v3CgZz}Jm~S5ox;Sw75UntRBiIAk~sGbI4q;&srg zK#em|)tNt6>hLYjLm*O*yYbxrg$LMv2jz^g#~LHmDMr%)A`1;1)S_aiG z#G@BJZzt{^8D#V_B}2;e{!!>*->VUiknzNKqPcnS@DVk_Ly}^eh8-PR(@9m=pnhK-&*ODkFgmEjpxM%CzVcTz>ljjZzzX#UR63Nr4#=|!6qjFo^aKt6m7Y+QutkgeaAz*ryvQCvA^ zCYD4$jbCf{p|Fu6)o&Hh1g#V0c}HmS26wx%%{y{WkiVl6@aiCa_DPS-pZYg8(r77e zvCj_o0*^b%t!MdSz@=9YdC>=-a_iF_LDMKm2A(eUMYMTfCcU^@^=IS|MNmTh6$efQ z+VU-`DrC_$Kf!EXyBAMR7=@F1B!0W3k5g{$-iS6A`ZZWp_H)ZbDs20)pFJy#J%c`8 zL=ERUgj6g96EmcjU#pPhUkVyHxTX}vLgQkS7}Cc-A{xu`zs)BbiUcX3GK)MtLb-rG zsz&DD7gXgH^&Q)xzxQtzD@ps zz`AhRhJ-b?#czDHZ3&LY>R{y3$pPYRVtS(~uJMQ+&xT!)X&RIdfQ7^x$wGv@Zl(A2 zB$gGHDnq0Pza#R(1gMAd09OViOO=CZ@Z~kbMxl?StJ9F~pP61?%i{azf)K}elVJ+F z0Z2rTU`9{@xb0A}EyO3o)_$I7v#4TxglgIt!v1x5p#01{LXkxXj$ZHcWAxzWulE}k zSoIQe)1RgEK(9)#$s`W=0{FK!XnY9u@>Ts_IHOOHGZ@52gZ2jW;`^)$@W!^!$uJ7T zs*WIjv!pGt1QKyj4(q@5b?y{VZ4)YbZV&@MxBBE&B-{{c6gt&36BvWyWL;%*Ua#|L zwg4I7D)Ni*<;!|!f&~mwpSQlw&PJlRpEVLf@@0ZBm>j0DFwD$5EtACpxSosr=f&lu z6T(YYs?N-|bh*wFt~UX%9o+QPVs)`#dypH>Bu|SF4vq2 zS3QK-n2cQM4Wp;ceDguAMob~qC%)#MS(8=<0I4^CwUp&_os71bjH+c$M;^@bJ~}KI z&><(X44I3+J=2V;+to`#ba~KPzFfR0QOxWub1E|T9}u4Bcg?-_27#F@gbR$BZI-pm zZX@fu+f?L#dp!w@veF7E;usoD;1vy^5(LvNivs|8yDK=^l7Ank3cpxBEBcSZPtlARj+Z2Bv`F!D<6S-?1c#0OY3AG_&ek#&dwugcvFd;0NNYL~dUR>Ulkl?E zmImN<=v1K5+uG3gTzL@5!r3kK=mM!?^fNL*2v@uTbGtKfYxcoDbQXav?CI;ZZq7Pj zgS6UW1n|-LUf6P2xuT@FZRXxpr)UoIAp8;&JhvpPY_*NS6Gcma;|cYH122=`9f?V0 zmF?$Ns`}Ml)sADL04;X$&h^y5iPW1(Vnmg3 zS`DKrSl))FXN%$q7|d_;DZi$?3vX2;Y2vqF9r6>EbF*u<|9TTH6jr`7UbD2_v1{>~ zV8kO$!r1F^)wN1}Mt2)0}*CNuAwIg2Z z>2SRP+#n1ui|(1XUehSvH@g>AIm7=B3zrjUT3lA5v|5Ir9VUb34X2%*dBpMeqjuj+Xpcr}Y4X6hjV?8HaW?&{CLk^<0=Bm)d#VJb9h z!MFhLke!GlBu1%`I7REv=;`&Y8bqE#7yCE+%;;rSRQ1*Hj=zkPNH+Z8r=dmm2*G%L zStKmwZn8wI9STcd_&|>DDbC00pd1(L=6X0oj1jZ07|6En*$vPody@vS$4qL-(>cw0 zuA=;MkkG>y2)R{>=jDk%rW7MUTN(0Lg2Y(t*tl^45!*KG9XA3wr8a`+vmyQ+*fN1K zW;vK;9n+GWH%ZxPYF=&r8*n4>;2qoo^uzt_NrpUo*%3s`Gght*d16vYg(&DUUtaei zHsDMvtVnj$QLg#NlbrgrUtET@t-8|sT>iW(Hcv8o6fyCGbOnYk$y*A;sx8R5Q7!_A%DGP!R1n- zdSKhd*P(SWi$W_Qoka{St-5VkkW05JX~k~e_K@_%{6-oEb{zBRQBOhH!zc($nUidm zW~7w;y5wi#2~vu35K#@jn2~}v!U>SqO>-?`HrMLIiwM1;U2J*0?fh12cJC~a=1L3% zor8UpxlpNR0#DB|PV6}3_49JaX~Gpdx3BohIEf0&!m!eY$UB@1qcCFGD)*4Ji^Rk# zYU|iLE}WIFLYct<9v1mah0b)s72fYN?*+Ml`yxNx1GQ5(^`m& zQi~pz%JLT3O`K$;nz@0b0^d*oCbA(lLGL;6kYhT_G8p-J$P^{lMijz;Po-xY|K9x! zK%Q-gi16Vh`Rof;LULus8UJ|Xn%pS78vD~XVe;{KO-htNZmim}^qEc%3Ra-U`@;T! z;Sr5sD`E`}+SRT{e3wIXE()+_W81cE+qP}nwsT@9C$?=nIk9btef?Zg*-@G=J7MSVPOMg9#x<4t@tTx|dnLlShfFIrC+O94h5ldA9gxYdz~mFgUuJ z-Z>prl3_v#?8Bv`>tZ(0&FSe*+6Ut##)*|ucNNe18rt4oXZUfa3^q4z29_bEs-LOX z(qqe&5gX4EUKt{nZ#YFWkRxxYFf39YDy9;7qY?`(vr2oa*&(QOnHPFEDR6vU6! zJ3PG>b2)NvEN&T~=dW}~ZT~P0mGVoG?!X2RmIGtgbcttyHK9~z-7azg)zM^%Hn~SS zAnnUkz|F?xlD*SeyUfu^sd2VNW9whhSuY$>h1`q}6=btyQ=N_%H*uuc-v}|;mTYtr z5DHW7cbF|o^n>_k^xU|YHJY}=0v2d97cf1A*dBiUee_^If0xIVs zD^U_BOXf}si9V0PB3-U(MB=Y;fD;1v?nCa~?h@0khHu9^0C|m~fC0TSN3+ywhbH7@xCWt?Zo-V8>zoh-@{7M!tXD*9LpL3vTBvC&@p@ZBz$JJz z75-x_#@rG&9j<(Nrls6A#QUw`pP}T{>Yk??#hr|svNe(T%BqMFqlZnkZ*cxi`uikU zSE3fxz2#B<$nM=AVlY=vy1BE*gv-V25o&@u9z1VaE7dtvF-CV#K7-e4#(R50#k#5# z1a1RyX7*SORjx6EjA5&LB!drLf}S#0B3titj1F>f`xSUfT$6C&$BEfzhFkWt_TLCs z4=$XwH>1hm9fH>Al4UaPQqCP?3lL6E;eI{okT6@19@Zt*WjDi9{*KWf9xxm4O|U|jTtll)lN z77<6ZK4D3$S2J871P0!bt%}`)Co!4_06XP+^7#GclF%K+rl4}@>}F4^&m2#of%l6& z0r0(l0B8pN#MZxWp`6&9m`bI9wN@3;aN`d^p!)5Y^T;;lG3$LL_Ay zd*Rl^1a=|9O&0hyvm&~40<0Lk(VCDzF(8~PZgOr+4_)O*PbpK+igVxddm;D%nNZ|t zEu~bDO7eSMdnG@9;n?Y125DKd^_bO#=j2%RR8k{p7cPG%A*F>?Y6fO0;3F;{y|pY_ zHG9a*-oi@6mrI(+`XJZe=BoQ&`qTrsm1uv4x3^_92zTeVsCJhNvS3}&e2)*2=tn!Y ziQ5>29q5!rP;vYhChB*b+|-I{Z3nmnP5A-e3#zw8zQOTLUBrl(aKn1+y6Lg=Q;gyr z-U2BMsV04YT#_it0nwYW4w{d%kgySD>!sJMdYz%wXiT*Q!lAfN)NJ_JZ4pkI(7&Ip z8jx5kJS{w08;e9STU%A2xzXm@GpE**SP^sL8GrB)mh3IcTBb&ssQ>g?kUw7eodug| z%spq9NMF3o8O~dinzuRDw7%)q={t^5WK+JXW<>@W$GY)9Ax(~jGjg-xSZ(K?u z6}dPqqRP_%wre4Bh+HV?cIV(s%L}u4;KpE zfFPk5rT9=vG|h$qQ%81{_Hh(SJr6R@%*Emy29BTP+=YC>+gj$a+{{_&v+&8&bo;>@i5 zDuJPAckh!tk^fvMao^Z`(8YK08hC_;F?~5&`)ie1ve~z?StYLzLOF_$i^nbyhjWs< z0z&MB^~BL=|0run+uBW{DQ34Pf4&)l-7?>6jiG~h_UDad*!pRpckGRYGI$P68Qcr> zZX2u1$fxZ`qfk0{?vNto@&}1$%A0j4aJ@Mh@!>%iilc z{h4`OLNEl}*wh;9O2E>M4$k}f(q5zE_%e*#a`m4fI`ph5Iw&~>3ye8UMIj)jLM2bx zuR|cUAHDsYRlCWrqQ+4`b(xPd&5bAT{)HFi8cR+QZGd2c5r+^=rxh)MNV za|u(^yecA2xpOORLya7@L7dtJFJHm|P}JemVRb!uCVJ+yBRt3@PZ3!(#T!HHk0(uj;EPog8g6r{bn*+nEOXPOEYl|Osudy-ShpW=d2|h^3 zXMNUb&r~`j{Gfpn3SS(%qeP@onT+KpX_+!YpNg(YVtMo~G4j71{Z-;6E?Wb<30^j|dqHteeEumPJ@mg)G)fO*pJqii zz{J?2Q=-I90T2ud=0`g;7h{Nh585Ze6@%t8=K-po9GJpgga2mKX1%f#c#!CLo1M>@ zT4&yd->ufDzAb@*Cin}{{Ck?(U`e)3Yp`uuwHbr}{#>W+n{H?3^KFmm14rSM^F9)A zt4WKiM-Pr$OWQpk#Z0B&j^AFFJ~%-`yJ7V*>L5H@n38Q6n{##u-7A1%Ak;VEG}F%* zY=dVQD4P$``XqI7$~Aez?jd1N=Wy`szPkKetW`U>9F)VI=LCDu|REnp$qQ|6HEDh51$jBNQ&Fw0Z1PZ@8i z=!cIXCy{a-W+yt2QvxAMaKE%{Rl{@Nj|SPuWr}H3UE7q*U0#w%VRV6}QlWV0yX+2h zKiR%u$m}Q^zxsRET4BJe;Qn37demS8#}fCHy3RqDdIfm0ZDE+#PLVr`c*L{NLrWNISpuH|6Oe+l?i5g-$nUho8dmH&uwVE7NuX3;hgk zmF`>4!*YUlHylnGD^Y#AX3cnsmv`>q&vZC z9i{UF^Z;9O&Rh1ckYUc*_qAK) z=d}IV<|(T%602x9?}08WyZXM2)jVm=AR(R3-^~xRG%c*6$o?yhyJIW&RR04Q(}iV z3O~8L0#Klw&CZDty0PVBfGu3|lJfxJT}Bqq0UgOD4MH&zYAQRES07I>Nv%k*T&`0X zsh1dbzA1wX=7F5wE#TG^n3mn!2tGdP(%q@s7we+I1Mb;-P?VWr;pusWjf?~Onm%kb zxz8*>w4~-jY!~I6rPxEEnlIj9?*M6#3f}D}F<3icH}XKdn0(5vbAz|;)CEsGB{h}V z+~Z7gdf?55XetuVTkJuyI!=(1rXCPXMq7J)0e>llXsl;2}8U^n8@JKX%rWX%*i8vKx)%yub8_4wq|# zFn1ehV=jVs9eAf0;FfFa?y=zg+YoP^ z#*|E%koYPzbZ2biA)Xt8j=51Y?dZ2x*~0I?z-F!az)m`br=yJ`XlNA-Ey zgV9Iq!OkG;7N{S(ZD_!I?5<&<%mJb1D8~wz0vVPd@};k`GhC5LX;oilv$HKI6h6)z z7s?-gpsChzwft!#So-u+CB>rC5z`HD6--=R)3<=u?l-G% z_WdNycaVs=r@x7xEk1!Ca5D<8tl};r&kYj_;v9nFu2i6`yz96WGzwJ(%}u!+KMXKF zhTmuw@9}DaKKr+xb@g1iPM4$x$=i;o!jCz&h;v39vU-bm-+tS-hp8F<#TO6qkPZZ> z1JszrSs?>~s6|K=?5+SElYYZ{PqCR%$$rkLM6!?Jfw`VB$62K#DR2wjKXR;p1%k~w z#dJyoCZ@>nrz+@y!brQJDFp_LZ6KC!V@x>UYjbWA$Uo<(^%GGvw#2WlC9s6N83ggm z-0d6}%$sO9*LjCd>)iS`C=BWd7XB{Y$_p=Kh>%BVb%~^Y-IOwj)^%jK)KBm;jvzsy z|K0Jn?p&t=A{oUWKFbAW8RyXuIT8k)WD$gx2-J(mfgX9=4ysyBw=}MFSBS&s4WE}a z!npxRvplHUzidr!G|+EFhZ9~IhYI7k44$yQV%%xSlAhbD0!=)sj2c;jbUQy;i}jf4 z!@5^?o#wcH*^o%l1oLG=WtlEO8m?onW2kcSPmf7jG8zU{REs?5I@MU=1dQM+az{%_ z`fAsH$!pc_wi(dw$FI#Kx#y+bu3M~hCkeWFk@@?{*HyAjp>Hl~;Lyhr!c3m)V@laYGZCY&7B~?u3@r~o&ocT%w*i37-LV1ymPoxYl<(_yuaRf$fvkoP~&A{2T ze2H$BYSvc1RRO5hEGo4U?Gt7-K&<2w;poW!l2%a(QsRBQ+i3!Ma=su$yjS!!{M25> zwyaW1n4Qzt=>@tSo@WV{&uD94qes9b8=T8Z*61{T2Fr=S7}Rdrhoq=u7gitIJuLME zklnljr{#wGQ(ZKZ&K0$mR-E`iYbhT$d#XOe$ljosnSA46|u z4uN2v{1?W@L7n`#SuHC4DomdBB%?Hd@$8AcvqB&_Y{Zqqx4h|-AuZWkQ8ZWPhm0el z2_WYW$TuE}Lra4h2fw7C4edC-bwVBDtr$q%)UQIDaQLFPyu$-vb=LTCzH58kUt#`D zSNGG(XI)29*nP<%CBr4>Wrz}lKH@|nQPVCjKB$ObGdm>l#YN)vOx!rZ(`u_7&HfYpS=NDB0o#w4;AdlluiL{q$IFL5)T=$XU4 zXZu38yW-j9DG7m5hCd#zJX1FxSj4%1wa6b-f?XmQUggk4 z&;zgeXD8**@JhhO)j1ziRDjW91^`5{GJ?09g;^pU>+fP<4>M-NC43PSUovddTWd(y z6vhm$H9FJ+KGcOG4Xn{SI-7ZmlvO#a+Xg2%{%-CpELu|_UTGK|p|PY16alxM?7l%} zgAOG}c>OZN?<8gcJuX9&>i$jLywaB;xZBh$*;TMlIf!UJ*v}Bd+jo2N^#MHM@_N-`TjgN@RPaHHpV8IGQZuHwy}@rbTQ6t`y|vl^e}%5Cga)bx+| z_hIjoHP)1TN3MjM=%Ew-YXU=8z8slPP7;a5J~$|3$9_$p|1))1kK=(66O|Yqv6rBf z)yjZtD<|-ZV1ytgIg|k47*OpbFTCUJF4B7M7Ff}3%C}9+0*ySkcjh-2H+ipY zl95-K$^bQZ+>_57scBBHeQ0qr9$c{K-6q^tZ&z1M`!dZ%1$ZIoCI>)jH1 z0$+U#I1Zuh%z5G&xg4wTm}O0VY7&%v)>24PGVMCEDh&rQ`jh=bf)kn%gY}T$2o+UB zJ*75?~41N-h6!eJ64`ZJQSL;K9U| zo@ict^Z|7Aj~6~Om};qV`{mX1LuHUqw!c(pSIyR?BVo?+jBGs_-PuakDC5>Ox|$gH zuwQ2*BVN2?nDo+xZFVhm%Ke_$zX^ODOuk`fAMu%7w3n;o%)YmeMU8mZLvU3Jl;{yr z39k^-_ba8wARO&|E=b^NGC*8tIz_{=S*hQLV|s*ziPn9mFIbC($5cOy!gibCh*WYW zc%_O?h7hjFkPSH;nRIk1qJs`o$*+n=HXs_n4VU07L%+-gkmlzeX~Uor696u#yReV1 zLPDEMp5IR9Yy8zYr898<~3AY6znw6s#xU%2Ha;^6UERLgJgUy7 z4E%}t1tl5Z_uQ~81{StX&SC>OxR@_YqD)EQNQlC@W(sq`6)+;A?P$w+#m6`tB)lKFm z^VR|%B{43uZgFe@HR2eA$yA`8dTi%9QykjNL+Y8vUGG)EsF-#0KxJ?Vfq43 zltG=tdSH8S(J z23i(Wb==i;Y19V$K^3pVhyEpQY*T-5y^$G6Ci53F17ucXE3&Ty29q2r8#ercP$<&< zCpM&$9Fz1eHy4>znd*gg$h)0BTc!wKSm;mzEQo3YQIv_pW-IG7`4xFu<*eF+dV#iB z%Ad;1+t z#Y<5-QN*$btB|#gY@o6>qPxeNNLgbfB(eH-l!HQ|tD?QY%V(Ni=;4`c$V!ZCC`lJTkjC1K zmqV4JkPJmoexS4e=2qO712>9T-O!5u?|n`JHLw^^};s0{Z(;p!8N3r?`D|#2Xsxp zQp|nW_0Vbv_^01n<}@{sJv=@#-Hu5_V$;aV2N!E~KaYYwb=k-eZ?@>>3)0?K_8%w3 z)BuZ)3D&JK9Nme}oGxi!wSVLP{^JUcq-CE&RGB~!fPCcC=aAa3o@9F9=}STs!i|Ng z9nFH(dEt4@1{Paj<6b&G`1mG6UA=$Ip25Xm5aR$z*PFD^oz7LY`h!kO0lQ^ zhpb@FqL6Tv?v4pUS|RAUUe}Y*s%l4gSFQ2QM_Ko}6BX~x1xPHWKCe|;dXNF51dh%a zdwp@*dyo9Kca@~O=U6##m!1_oA>l+F-?~Oc88~y}#f)98aVH*2V@;jNi?g z$#ER>aj@1F*{RMJ8grlnG(rszXS0d`j{PxJ_4`fx71 zE5CAho!OvXc{J?ZMH;=TaSlwa=mA+*^O(x%&q*k_B>zRLw~10d#7T-8RBU(YI&NbY zBxz`BDX;IV%&@%eTS+>C+omJP;d{^E)f~-uEUs`FMs3G))Wnxh-Jci1eiBa%>wMNL zKao`5tfg}p9$*US^qX9L3o#MOE>sU^9+k9OU$e0c1jI?i7qbQ4dpDMl@*a@q(_2AD zo4?Rx5SK{gKzY&sJVAgV;DU3YW$boc^VHP zF|G^XXlBlirFXHYA2+^}$c*#oJ-M5>}!$IcO5ETT|r#d+9J{9j1 zwB$~*KM=Dqu!(Kkrhss^q!{32L%u^74C0OvC^;DD2;zV~QXeMYN5AowzkLhfK_o)_ z7L?%y2OlvVE|M82`xtV1CNF!MP|-e97%`dX@2YBsb8{NAxy3f>h+yDf+qS#dB|Q7Y zc&8kM&KTq1cI?{P3PF|#dn>gMJp|o4K94$~*1M&}g)9Yb~Xr(TZ7y zDRBV9)&X(ufvd^7~1w1->BDM5|t#ENuD+dU~*E& z6w6-(2DKi!$?MelL%0mNMO`G<;{*R+u-_2EM9v!jlhpMF3@9+qa=!mZQuGHo!m8ZD zE9v;wPgWJ}La`*to2Qd6L)9x};^-#1BK!4Uo4>-xU^|Y-?#bU}cDtka6N*a<74tkA z&a6m(pSiBCqz><~3Sudh0t=+C0F{fanZ!}N(nPbZG5<#ld4QwW-N&R%O;M37WXuUn ztO79ubyaxx>Kr|kgz2AIP$$8Om7Yni3GBN?#nlfHGOM%bu53*4o=0sG-hbdoo@(b3 zNu;F{37duU0i>IvQj?>jhf{Zbr2hbid2?zY?U=Oc3_FFL$~WHGKG-b+SeI6n#o4Wi z-(ThX?<{;^PZ`lCa_G}qcw?ltccDGv&ZqD|@)XRCB zw$N3Uo1Lc*PCHjDiC1#?_UDo9P%!c-EEob;G7HD42J|D`^CoT9Ga*w)f1o`1jSCoo z+Y>AJek$Z*MLC6{p|IHJ18}SaKPA-e1#q6pXs+pu?GQXM_iLUnY1}-Z?*YB!B@6>b zEuysV65_evZ=+vt0OTH*7;AT;X=Fh5=So+J*D90pIL$2`IkqdWK7*CWwUdt^mN_}B3Udj6C6!kkE;)815+|g!zZ(pV#0{gR0}U)Y*O&)Wri?m zA1dlhmBCVfxM{r{R!^MDIRDDeE?%?ORKr!+@@NemyUP5fh_Dicbsz^*2fs*a>i7X& z9^y|UjJ1^V%9FQS49aYTAoTL81M4A)WC1o@Ig9eNw4&K$O($%XAQe{bwAF4hQ}2?l zipnxkW=Ag9E!w}G@=KA-8?+&Px$xe}is!&K1&QFl_u-no^)*DOq=eZ+L=W2Beyk_h z@JxL+{9+SAFuXe80UB`g7W$_bfAgoGlsS*PUuJZE^+u3^(u=^|=*%#Ho4@swDj+g* z34c*@!=~m?Bv=n;6C*k*NvgtG=-Y1;vtOON*(G>TlxK_JznOQznrSDiXO@&l;Jv$5 zn!*KH0#cD zVf^0Uh{(H~)a|6m<-qf)4xNs23bHlRu`drPWNwq-rXoNkGRVBa@TbJ7sJVx~ge+ux|XOf!*o#8cl{s8b70e?g`AV_?ZzLSF|Y02A|I3YP6 z?Of~>A~fdQDuB$Vy5J|Kd>ZjC%uLJ?E2*#MpbCY^jpFno**%VDHA zqK)RrZZtcRg{DylZ^mfIhh4tgltR6iVoRnR4$JUnysk&cX?~n@ac}~o29IaK`JTGc z;R+NgEmf4C4boR+t9zh-?h-pmELezg3N*j^|^ATrKw*NsL{nW_v5 zIP-!rA9Sm?5O*%aLoG>mm$sJ z!XUo3Y>X>^Q+LGTz0Hv6)P*Hm7ew$#2fd3!e zG86&eRBmyrj$T{>`sAO?D@r8~=9iaGbB|`)aG*&)Dxq1jTP#r%jJIhW_)=6(?(~lVaRQj@JzOEt`M+#|Pw;tKE-dk@`~^TLMQPNCn^U;h6#7bRVgNFK|n>X%bAFW8Oy}I>K*>np@-P!WqGLdTJl1 zVSY;m)oq~9=vXn)l06UY4SWR_(hRgA+IfNH#L{geR7N{k*vtq@!7CD+BEA+8Xjqae zjXwh@SNO>T1nJ5ksW2IVi4b(;HtMj1t zMJ_9gVcG37E;q##It5f^b}8Q~LLxr6!t9C4Zwls}qq%p&ShhM$< zwXwOPafa{?5*hv#mkLIhZiAFFBOs6lpgr0(Ud^^h7z_tfQPEK@Jo!S|R2NGoKq zupL53Rv&mU_MaxD94Y=q$BIDlFWKI|J!`g!AR3CZlS8e!8jl`rdf8-KC%v7vy z#1Oc0ZwRK>SPjd;q~>bgv>9sJsBqmXt%L~r!l`f!Gj-A6Q%~28M+-)lg3dzI_wv+y z*r6ge44<8dGZaMy?f8KlhdM7-1s3{HOX)pG99K4rj&_6#0t1ejO5AjfOYg{tSaYY& zJqeMri4A=|vKxq(Ih2m)MF^?6_rk44#e_Ur5FKt+L7fZx0%k|nBjpisS8yEl>y18vXT-fMNbfRd2AdkH8H2r-G{Tjlc;7z8r7v&Day>}CVG)1+%tW&+fzOB~~U4mIQh`v@u3MN-}LUGnmGsGo& zc?K<*iC;0EOwm)6mr`|i+)T`YKz2UT={(PNv9Kq3`kL%{BNsv5hzW&!lb~}BP2Cm2 z(gU*x((>?QPPCH8XWN}!7tQy?_-pCl5$(#a6%u2U4xp`2b`9T7Ec`sNK(kARc1I0$ z@N{DUF^5_-+t6~#|2UDWZ!)6KHzm^=shG^0S@W_Bvu#p^biI?j*V@9wuOCvu$f}xj z`yG)j2ddxFk^jk6pHVPC2v$Y6Ep&KDw7JrocU4RP2&HhPud?v?+hhh+54X=QAffdX zv=d?L)h9=Op+ctPMK?vR(TDil8gPrdP`nYSrt&RkizOoILx#E(a1ka?XE#NpFyEB+ zz2tU_Tg)?G*tYsWO*~ldS1hHwxJDaDnJPkc(J!W@9pW--v&+8LWEq&YM*d^V#&IpT z4XELmy#4R*vhV1l&eMaTn2U|S7!WW9Eg{MC>tG6q2l z)lKboTcf4PnRH_%%K5_2{TKQ*1r8ztN9y@@u}3T#wK$U$jer zSNP+uNNFzPjPjDm5HRXiqS0S=*3 zDpWj#k4-7!QS(H-qhdjNh+W58B-?2Us~G3c8k@s2!riQUp`OFgN+9AoIjD`0gpv0YyE+F_S zhYd8a##}+<=0FOeN2sR8X)Rc75EY0R_#z|0wv02(Rq<=(UBr9X3j_q_xzO7z6dGX7wemLyc=1E%kmN}=QwsjK^yi9ts z026K)2l3Qs6~r)Thy?2{D>+O0AB>7zy`d~4qMPi4rs zN%Y zJy0m+1>5MM-mp5pS5z;P z8J;8mtIwZ0=<(6N*Ybf3U-DjlZN6n&?zfN|_)7aKTZ`|#(Pi$eO_KeQz|W0Guc+08 zF%vOUA;tn|S3}HN!584$xdulR8E7vA@$r%y?hm%Ft?>hu@XgNP6`8#?bK}fFj>Z7R z&uM!pH9EQ4m*}#mNbZ_;p+)9mV^^;ppK?)xZ0aTZ4zS^$^!#o5(|HxH!ni3yk$pvh zurWywMK{ta-#=)V5+PaoPR1K|mhsLE_fCnQ;DdAoP^p#ULxTm&OD4kaUVi85OT5`o zs(l&Z*_}>S6FQ{Ja)Fu2waD-yxeg$uO-u>fcvdOCU5|JNzk4{lEjJ7U^#Z>`q7C== ze!;5yf3q-8Uhe`e^jWnhb$%C)1HV7Kfs6Zpe$Jjvz2QFcCw@nNADZ7qiz|NhOD6IU z@9=*lA6^k&?|6Q$<`o|Vud06Dn7`j$@B0JnWnb@pU-Hp^#_I+Dm0kUOu>TQ#xJq!A zX|ni!0IU)2xnm2C8utIxe))eg4(IE$qG_{y188?v0fE|Ejo3dQE1b5CU+v$o)pg!^ zKOf;=?SS9-hN2yQkpcaljr_i)qzSlRUFKv_y^mWOBQEOrYQI zG2Oj?x?496plA6C4Khhq3XL6J!glPcRpDVM4z#O-y$;{WhGA$>VfDL$)z2$$Da#R>WJ&*nqf8>M_n%pt-Z+{%Ux#@GX1cW zN&~U@0&Ygd&J@2D9cjp;Ce551Tv(uuhJ*Sxz#eu~;Y*Mlk}v%}s)1ZH#PP4ZaWcUr z!7CBxXv~$QuH*Gt2d^azkXI|sW?gcXw6m(!= z66DW!FNqMj?v0xGF!(yRA&a_I2i!d8gy6Nu7j(STq}8RQv360m2wY!42khw_)*q)V z2A9>?dLQvioLB8 zD8)S*&c9isNX$NkpN$Ll6G=9tyW>gszlTx$4Y)r`nyWwi9&-X;5dRl!{tkGUa1bCM z3n-xf1#IH~KiG2C_SSB$|Bq=+d&f;Cq@O-R<4!}JXtFj@kV>dD9_xIS%EdN9q$uEe zZ{v7x>%i|>3WY)<9ksSrBEJ{sx7j)VPS(&I_oQO-jMj>OSgILDBU8?4(JnYg`GS4i z{(h`@%Tv@k(Q-2;*O3gw##(4tGaZ?;YJW&Glu^WWTG9O6b!$!Kkc3mkCtjIBFwOCg zvQ(aI=X7f|ZvT1Gas#l4M?!mXQ?A?@{zZyLu*Hzf`Nc+tQfEi;B{*xR;T7vlUIveQ zrIsH-cDrR8QFFA#>m3^nW%SfE)v$+rzj5V`cl`89k?$@1R)Os^hvXlRop52gTIOBK za|X1VR-9gFRY)GqjWy%vF5v+6$dsA$SDGOok*KTGFj_|rZ2i&Pm-7|Y{&Qy(hXS+! zvnL5{EfJE3kZ(UZUX_fHFK4tEb^s5#>&7+lRziUAEPdMw3oP(FwD)qMG{%E)Ra9ar z9)?ofs#2Xlo##QD>F}~UXifYy1$(rA>E6K*<$ZG|p%Kf%o0u&^#h}}(eF7r(XG7rn znVfmOk?&lz={8o43s?pu7kUwTHr%fx!Otv+vshwpW5gsWK><|YD{MgW7wFXr(sRb@ zI|!h2p$ha@8+bECDM4qM@go4Ew3y=BUTId1IS4nBMuN5gh|+`4bhMzGoRlo0aV{j- z*HSj`pYBIp46_cEyT|ULsWp^#q)IN-=-R*2-H7&#Pib`;%VZ3aNOuwmy)u|C#t(!o z&b`$`Xa%Wg{i^Y8Bry!@-E$53ryBuLqlHCfvYx-$9{3&i>q4R)R&zJtWlIKYNbp*7 zhx}Opo(|Se?fQn5#}Sn;!5OSYL`EjtmePn*o+8MA&mDuK=U0k z_gUK|Kfg6QoZiu2GSmZfOIf;oU`a!EJR}MWUX%_wNlm+AgPU@Le%J)S@?#NGC2~Ao z9^QlwFbLnj%UD`sP5nAh9cf?>1fbo9{AqpUA8VH{m$dHdbZ35z2TwWPfvx8~Kl!%^ zufM3E7Xi3Z1LwEkQ}8@K^Pb7~quZ1|4svZ}V~1oK=R==aT7`Suw zG##%A1j8 zfA0T=`33&~WYj1|v|PsS1^D|O=2sg2uI>GFU5_$Krq!J|_7}|?TEPr<-L4t_tD*S^ z5Zu|UzBY<{YBwx2EHE^}`tknxm|~Ue|9Y|QtUKxt-FLLS>t?tclZC?)Q>WHue9}lZzB4?Trp1ZAj-dN)jQ*o&k#EPekH63r2IaA zKlQXKX4uVr5X=8=TwpWG7Cp2ZW?09bUJLHjM_xipeSJxNex^$6#sv}-=L`P|-1XNP zV-=U^)bE{rn)3g4bqW4952Nn6$A6F$p5C?_c3b6EKLrj2hUC@n8UEg6U0bFbF75@a zvTKjL>jNbAha?-yZz!mnfC z$W@?Fzu-^f{V&v%Va0D76-l7Tk1CpR)EDZ@E0D=AisW@Zyx7lm@>c@vbw6X%`tQMn z$uIK8*X#H%f%e<4mfmk(u3|?)AT;5#nop^xO7tv3S96 zAalvj&2M-1Z&v#+uiMC0*AM2vg1Rlo@6*rS-R7lrz$f~9U8Jb$#mcyAyF=DL? zkPQ?tTWEsv6&l)l)uqETD(6CPjSz&QXa#w$-mE$)3O^b)DT$K#(mnY*mUDt22temv z3gpOXl1p^w+f+yKQjeq+Qa}j3tKHyDSDukQ=$eY*1qrbB9xe4fTLy?J6z=!u@(l=7 zh+|JK;3{1Xg$R4t@%w>e7-<#M?CTSRF0hREW@r)WHil=4*Tgo#Hw<4GH;>nwF&UkE z2D3OLGMM)lLT|3Hb3tS#WsARtj$pP+kSJ%0%$t1zgsH(s+@(U0P}R@Zh=gl_gV8F8PUr7L)h@`V3h}#<2|%_3FVO+6ajeaUsvt^$vWP zsOn3?&ER{)-_Qj_8*Ow?smrB#;-A0y4a9M2_8q%WUM!S@!E2xaQ}?IUB2}^S*7I98 zcgGI85LJxB@kHUzbp@0JK&wb`**3(}Aoy{zAaL}>2h11x!Y1=A7#gFoEN#C+6t4Lg zwa?=Xi7ZIED_@38|hmv3Sbapk|OiZRGp0dW;*~_VH>29aC>*z2R&<3q4o~= z``In5d55W}ETvczg8;^e6F?zR==mgz zywX`-G1NCW+iQVnC1sC|#Ta9%GBy#{*IfpL!u-=xtc#S>n=YHDM`TB*J-^`LlT*ysxUn%rfrfg5fF3R5pLm&xkmj6OW=@ z2dE4)&L=qHPaLH3IlZ1lh?~ddAH)%sS$9dwNq=Gnw^OyAyyXy}59mk6R9qDkg)rH% zj=rUm1Zg8H?PF0~na`Y$L`0=(!^Vs`!jd=j@shTWneP3yB62veMzV=MUqeEIIR>wns_p)2F zstMXROcSx5uBUcG91GfHG}(&Fr%g@TG^7-k1q&{}scfe&_tKDec;)O@u{gahc9n*{ z@u6mKZdJnh36C|h@f??Tj}_`=Tkv;D-}QxKa1%S2qwTGcaK(#Lc?YG1E%GUm90vEz zOcNlC9b_`R{E}@%32-{_C|n++j-;>$kQkeZ19F>7YTMiyBNWnwii5eHeA1V%XM)<| zl&%KG70789W~x@jF~?F7Y8;`qWwT*XbU?xKFEt_@d${u~jDUUI=XRE|>QlKW~q{2K5}69JZZ)*&zjU^Ai= z!HK(3ud%Lc@R*y~uiiw|ZUTC*?NyBic0s zd29ir4JZC_lwh)8D7yE`CdLuM?>EwwIFNtd!A?$93Dc?RJ;Z+lwu5&VFoe&MqhgU2 zJjn`*)v%12xLGNqxCyP_fFaVi8hn>po@c`!86ry%T^Us}Vjx-z=n~PXWSO_UK1Uy> z07}{Ree=@npalaj-*HWxix>e?$smnc$@hK#oD)~L50mgw@y0Wvi9`Wt-lu_qb1zK z)tyMSzC$T!0KUFvFrWk?ss#S1zv2GfQYC1M+@cr~1ePNT$$jOFB~WV)6c3F;O&%o8 z8ar|-80_KGfM{aN=D<6c1lbJP*RE*d7*u*@W*pE~qs4a9;fZA6_KmHc+{_5g#Q3+*p4xu7u5m0fAyG;GzKQC`T;bUt9(9|K8z zop3j+JsybO0>8tpL(|%&U(2?WZPxOZk}dY^qFOry4KXLyyAQI19*o4--Lm$=p;KrH z`hD)C8-;H^Oj9|jGu8(&xEM29b@0)Q3nnbQ+~Z^8{ID3i$US9mJ!lE}j%3=5A7n8A z00xV>xpBB-k2eLti75sorWq1|&=?kk*CX~~5gWYzD_|my_WFw^>q<0k7!=E#-i96R z=g$iS=Sdo{rHs?A>QfM8stmLkT-FTwA^*PO$<0l3A>kN1&vl#Auyf%(St9WIre>)z zsvUW9H>94pw{5QhDk&1kk=+7_g#y_p1$}wL3-8B1wxZ{d*Q2FZ9ZSyOHf1BfH?2l- zod2XiR(!yXKL^?yll-j*If$bG$bcb2%46|S4<(ly_TEyKL}7t>@Id)lkArC*^2B%9 z^%$TBDm_x*A?2v%uY&>wF{VS|%^8Si!XyfopCmJ%RdkoW$i7#Kd%7zh&qM0cs4{4g zBt){beC1@0I-gp|5;`yEeD9ejIML__H-KuNgKh~{hLNkR7b>cA0Dr$@DlWiYFr@cM zW%eT)nj75;x&vIh5G-0cj3kfEm2LYhW5fjm^0V3VZ8grxUshDN-K_GGO?YsGK?$wL z-vLtX6r^hDjg)M-C6w|ed)DQsB*y^C6Q?|#zo==4=}8GCHaYJ0 zb2b7DATmK?^d(c*bGq2>k$1*!D?k@G9KX^R5cXGY&`p;VLdz;vr8-|%FAM0~ZB|K3C$D3DDBJwn~ryh+F<3nSM1qk^@m?C_i zz1V~I$#xXw(LrKHy+3nQ*Y1mBYF9+A&-yYGGmzY56lT*6HIDBIarU!Qa+8-{N?-Ll z$X{e$;{%18LaTr-K$DQd-}m4F8b z8I3ywF=u)N@iDdH8`<3*qu4ZnY}BKL5TmID#lE*3zqzRAK9yX)2$9P=Xs~6Aj+AlG zB@1qf)ru3mul;7u7tG{1myQ;9y16ELXVxx>Xw)6Dy`+p4MAICPn>k5xO+t_4qzGl3 zKWb%B7Ei9AG_YuXIAFSe5!tM>b5DF*d%#sT;~Sei3oB7NKD*rd*!xE+;Ed8KnTmPx zHu=wa1?kp}fV?%B6dQCclO(^+l=(8UspS~udNn&OWh`suVMznaiH?b-eqELpnYlyt z*Fe0p5j|WVkugf`xFZRM`ZfgKVU(jaYFcq#wR8%RQZzin>eNlqJV zgDLa8trbWJ+1i~t$2aHlUNU-9P6BgeEowEW_~^`|f9uOnaC%&E`cY9lq;6n!`R=ug z#*V>s#Dd?HV{dc&h>L9X0V35nOqcVPn#;Ib)d-=rdl0#^w2WEP*a!2By-$*dw+IO* zX`IU!v)qRQtJ5GwCCh})*182v5q4;rgBvyus$nsVHSF+s+hDpBH;m!SXL{qofYo=h z^zadN`$&Ce4U#?fyY17ZjU+sAL5hQtNMZhtAba6?09atN{-N|jPK#kc^SWB5MN%if z%#b)Da}FfvlhTbokiT+q9#OSmyQ#i_D8lpB@z{GXAC3BL@7u)z2zMV7LImI(X$}bw zLY~q{CnXssrm{D7*|9xfp5nRbz!Y5(FNZN95tzDemS2H_5bUPoKrC8SSN!B)c%4kG zj-E76D_r6Dd4Rmedw|Cz45~4tCw%42H02ic&c;lVBe6_VQMA0ngBB+#iorH;w>fX7 z^G^OyBsZQ7=i0gO)k>7vN@}NFRQbvlFLBTQq60q97j#{|QNAW9<+l(^9G2z1Zxyf! z^yuejf=njJ8hsyRt-B8|t3lAAdn%J#T|S7J92P(~%?&Jt_h_PI`{IxcuzYt=9q`bF zV%1U6mB-yqB&=5HZ3)!kW|-F12jh_E_5n65AuJQS(L9NhmC$|W^8m4X17HpsLxy5S zqMj!K>;a?Ghn0=Doq3>F6uSaPMJ=l)vacd1#1Fryf_D@`zdpb|r}kI3DCIHKx0C_c zx3$bmSIBx|dt(LL&gzyp08yV;fV`zoaQY^+VMXqmV+Z2hbBU*N*W6b`)A#(NGuN+a zxDGik`gKn9cl$7@o7q}eykigswnsNv8PLs7ntBNx+9UYrgb-wd8YiXW+0EI)n~8nW z4`D6vKCo+|;}(3&)0d)aD+S^9igTKwkstqY{E&LxrGXKkBcrd)XYE$LW8Pqt^L+TQ zC#w)|bMa)azCXJ^E$M_K!rB=nv?8|X*kz1+NacZ!1~`AV5`ukFS_ zMsA;AFmO*;N`d=#MsxJ*XJ8x4y2z z^cB6}62!1CO|emkl8Ff7tlnnS!P^oU?2@1mpKo}`+DEtp5sHAAcaUweD<1m{%$JIl zE{i<=l8o0=XZ8EZ!)mQtgX!J=v#0nyjTwzY3V+(5_c{m)6&=y%9?&Ih@CuXUiczvhBf9lfCb>nz1P28T$3HuE|rn%K@3Bk%d5D z>;77@2i@ORRh|r*Yi{>?{Ly?K?=3K9g!mlF93Y}5YuP8}3DObB3CMK?Mt8|Gk%q&< zk15n74#-oUrw3|fmS1Te?q$1KhbXtd@4SrrJgQjJ?30f zyc(Ujky7DIkRk){;s@@>?=kLF z*vIF^ahgu;*U4tZf9ywlfzksob@zeEQv(gG7zY|2V2M46NuGxnwm!lAQuXe^QSk>_DmJTH&GC13O0 z?y^L~gi50zkxk23y5Dx;_+145ZLv9E_950K{Vt%$4e@{Pef1 zciJ~lvm)~}+p-e(+T>TQjMD>q&ugo45*c^3>f$or0bs~Esds9`Vv8sKfrMh4#m zO~K7Q%4-qh(^tY-!l1qYZ|>x2;7^4{0R2OdkP0LGfifBa4w#2q*7w}Ov|I+OBXlG@ z&&`4;@FC?Ml1~dA8k%P*JdX2eN_A@Fqq<2)t$YNkvW)MoZL+r@D#6hTiwMO+s6DYW zKAY1i@`hU*9QLCl*LZ-taT;}v!HK|j47%!JHrQE)^RB9a&ZD^LL`%PzvQr(T+g!jAT}DB{RjpT`4@tnKK-Wlu^y#5jynJ8lUC z${-QL0k%Xm5N6k&E#5?zsIEsc*o#^A$=6rL{dfRg1|L2b zX*+%IJ*|*}yYt*gfi1|lme_ZV_N`A&{$6|SM znsK_R2`*4BT);>4HsUK;uXFaff>+k5bE9yx_n-)QTr7;8#JUqc zrm!7IVyutRHJwx~Zjn4c)23Oto;g52T1y7;tEJxdhNPfMst2idp&aRmA@>U|wr861-h>DvN>^+B)4pmV&~HrxsV{EBfB z?KSA_WS;RJ3~=F0mhU}l&8(**@TPz-g;*YG?BG2EgxUnyVCGvIms47oAPM(U4kE7{ zg05PQKVYp8K<;z{mnCr(%iel8_oz=!(O~xEKB^RK0NW~1JEJ=dXLhBqgwH(@QK3VBbm zNV1jwZYVmhNE*DNW8vUZ+Eq^=`NJ827|v=X0kGqh0F*&z4xx!53Et3-5n>KKOe{Y+ z_Q)*yL++ZQtXy6ad6XXNq(aD8hsKOQb0gm4fx5iOg@Ym=4}!matXOd-RmivgxS$W2 zv?m#v_^_Kx?E`!joD?f?qU3_6mh9x4S{ou1YCr}-myhc~#L`zD;3v=DxAERF zl+R)LGzc;<`Xr^uj6UGWRvUPEdEh(Ani~;xX&B`=4gJRpx*9&P+*1xT=a{G0XiIYh zg(_cm;MjtVzuHD$+})!SnvP}laYNLHa*`b^kc_70{M*IREJ}Nd=!x9ddk=~|N7f{n zLIlni(i|M!BhKz~#XjhV&|8vhMX-JW03D{jV87uB5ZTD-*OlHaiQccR`?4dHQJfdd zlA@6(U7#Ee(;h9p#t9`Jk?l5?iba{E*xUV@8|wsn^q5TIY!w{dON+ZC zA9~M3g`)%`+8T>vUv&!?45B0xRS*i-T9);sqp@^Um2-x?zNkwr^mqYAo}(1~6ry~r zbcL09P;?Zf!lPc#?Z|gjA4tOsobG8$srYz2l}O6?;1eGP zI6gtO#Dip6vsi(3dERkB&v`>o_j!_!arJWpONBkCt~0fWV|^GhXJvq4R+6l9d;mNB{ zB8tqgAazvZW{$2GWV(a68R(OEFZdw%D7_IqKwyS%ZyD;xE1mbfQgbQTEm+sz@ll8B z-KDP@E)uVo1Q;O7*V|7+6km=Mri@#zw=AO*lhwC{JP_D;VZNj8o306y`RafwrpB*A zw1BQNk8*Bssbh~>i1Y&~p?L9%=;M;xj!Ub8ut1oI<$EIbofTCBAsrh&6{&LmvMI!0&S z*x+D$Mad4)zsmz){CUmb3g1R`2=2hAauGNqs$UaoSar*Ylg+i&H&pIZ=I z<7RIcph`q!9ro#i5f`&aO6MoB19w8Bdj){=koUVm=41T^l2mG0oCO>t08CB^1~&#l zaL14Evus#5m6Eb6c;q?C<=&mc`fb~=;h)ns&&t)-Vm zRrflKH^(@SmDSWd;Ff@S1((P-s`Emi%CSaYw?qb8;lV1Hj5TEVW^oGiGpL&>b(=G^ zE`pO}(ZYnx7KdR}!;SA&n0MnZS&Sz^Kd^=HR@(EI;!2>=CxkC2ZW(t z)mq7D>HlEdiGOztKY-m-`*{c5)Ch0n)=dm=BH+NNx3Nzc%kEFGk@lJ6&Nm4cQL#IxXn$cGV@eiKJGE@dKq97@Zvmo|#Og!41#8 zu#luY&wyLW{{Fi^{)jyj zq4C2#yL0jSxa&rid`nu1&zuZl0?Tu=dhfzoxt5X3@2%lO#c7k~I~4RWhWF^~17S}d z>lg52_P&G-yvY(XMI!Iw8@Rz4D@8CnTy*OjjVH@lqLNeqBy4~e3>=Hk+5u{!OKlL` zG$=pIrv!qpfT$s7_e$`Gvf%?rKbmQMkPg~k^;e#3n{!V@0OJ*b(o@{QqG`+7R30WCibza(nrZ;OJ^Vylv-6yhNX8R-b$Q zv$xIUWApy8w*~zI+Donlrpzj0R(jWXyV2L%d}H)wt)spm466OOyl*U%O8lAbgV0-LU5F-`?g_ zA$~Px^n&szCRj58+17q#SUbpov}H~H%pc(1{rF1_w$al;``|h(S;z;z0mlj^vG0vm zFjBM!1-%6S21u~L(cFf!ynOU-zo)x)n}CbqFx5SZta;ZGx2@14daHwK_QhTrGcDGm z)=hhBF?y)9$Du}%?wfjhjYJckD-Dv9HGgKAdkF|eC5WsORIqF5a%#w zhVCs8j3Ts44O3!gGNilm^{n-=DW_lKfHp#KB+Cr587&2XB-IK?)oy0EeoNxDlx?qJ zj`~6dfejsJ>bvt=Na55&vv^fi@F?ucJJb71#LLk&+(%vefDd6iz&w=F;LP|AQRQL% z`^@FdExV3l+lkB=NokEu? zRrF0aLuAI8W8`r?8mC8$Nq0EL$_tOKn4M`HJKc=Zt2EOKew`lG7)yxQ8SwngeIrFr zOg`XqorqLBh)R2q`i{`4#qZBKPLvRE&WusZBzyH>s@5XTD7oqC4N%1e#Xy!HaG9G} ziCfliX3y$KpurK_qSNcyp6k>##y{r1FeJX$9cRE7tTxKt1gax={K-w;hBtHXgjA?E z@oDk%_IlKCI35%AD0wtmaES7GT5hy7<48c>k&?T|%p}=)YZ>>^Rr`9)$D|Ix4!dA=Qu7JFFKm^x6Y;-wGi9Gu5Eb?;*5O#1 z;Bg65i7^bvFz(x7eRk-!GlPoe9N6#~WVgklt?x$LzJ2w$nhcv7@=W|j&3d~A#!sH? zqIzl6`ry5nIC*cZNbpp*G`*nYb3z%)8TWxdgMT$ z0o9WeGY)+4y#tRNtwN%qs18`=2TERWbqGa~*^^x@!jUrm?cz+=E5ZBPjVSdVfq!;O zRj>v8{wy!2UIPpO0TPW1)~QF?%|`mjR71Kk%0Y3CeAjT)Yz0xhU*{1Jei zj527@_fsBUd9}DPo*F0b0Ame~Q`{%o+t=O(@EzxC?Um7^WQ{NuWloK!SRAQGz8CBC z`A0W!kk&1a#jo`7IhKcHqpVQ!ethd}SEbSb5o`TsK#a5g$A6*Xqk8wTI-uUlVt#y3^O60hrn|%4kA)Bk|{RJyv#w7c(Q)yxCh`*>2ZuX&qrGgv2gMsfV8a zw4#qIB$T=zJRyvIG|drYY<&$v#~;xSh5c`^wdK$6^w!+3GzJ3!>h0{NX87l~>ZsSX z8KQxB)F<1OT)k*Q@BnO6R;!=T3^c>A#%IY_U;=*pSlgyzN$N@{f|a{#{P#rFH;Pn^ zmIO^ho5q)POPde~ERix=?jJw@!F8h3T@n3Oe2Gg)E}KI%N3#Qwnp9?N4o=?(OG$8( za18BQnUL+ulhf#@O^v}B>%}og_yeL9A=6(lo`RakKoUghi*m$A`pD&bqx)wCDY0Nv z45i(&B$R;VkYQf^^xeR+STicaThQqE$C;0i4kBWc!4#K)=mS#w^v*cdRnP4GU2c8F zd|&Oik;w`W_cY}EyraHS0XG8FRKCP?zv?~P+uB(#fcC}O%9hyOx$Xjlq1gMoIp3&uVX zt9M+msSiXjcG5@Fqt(m{Etm$UEF}|D?AyaPC2Ve?>A~ zx90WLUb;7j!@F-@IHWu85g4x#NTiVmnj3%j#;rOsJU!WI2h&0IP_?CDZoaM*r7Ynt z@Jx9{bOL0bt1`2zw>&sWKg@Z;+6C-fhKt}{aqa?$h0RGday&J>WaPfzivhzFNo17h zDja3rGOXueygNq5F<~fuRPRo9jn39?k!}%6Cnw`ud_#0JW{DoD%pI$08Wp0%~ zOH)ieFanSF88XwlsE}1{0u0b=0Xf=A@#cJv{(k&`1DqY=Ne7XepTx+(_@>A_HX0v8 zS@2T;QcyyBTgg$b2PT=V71_*}2%eAmVfp(Vx{tpkVcr!n=**UBxhmNbqDNOKFx&cJ z0`p{Z5^Hx6wjBw@m!%ZCGeM!YF>d8&Jkc@XP!Q+6F^3cZPyCNJDO}b3s2|#nK-rqU zU6>C&6>_I^;@4BA2k%xaZoa9xnbXi(a<%6Us4&^oK@EL;!|5BB$a*;Grrw&=ctx49 z<7wULbZiax>pp2;5ETo`7QPci{KWBW^f$`98r)+_7ERUYt3= ze{rqx1K6QvYHF(7?b`B-$HdCGHi+4t<$G6%s)FfJLHX*c^zspmC2~BSaJI^OxDP&` zVl>k5Th>Y5xOAYy7qK?m#OyT92xKr~K>VhA3#Qk1P?=v~4TgOuk*`e2sG@Mkok#g# z5K93#_E)~j*0gi*lhA}Vfv3~xN>HB`Keh*B&TYcF_=PrC@?L5hDh3JtWdhInF zua&oq1E0;pyn=Vpa4|rCUjPF$em0|zemY)$05JGaVHhDI3I_=AcW%<`;}Rq3E;oj^ zj?dJ|ah|sAT4IKKdYy&5{n+7TD~ToE9Q}&TJjU}wJ`pJlrz)ySoNeghx2 z0HHnW1CiIQl&pPGAP6T2Qc}|vC!$ySEYgsw-{FglMXJ|AU-DgA0;3V#CVA`cCGiC4 zKCr&MCV5gF!PhTGpNdk#pW(-9J=v4dJ)gvb^TRy&ln@#FVPSo*cd0xab7zMH9~pxS z45W!HhEFy=mvE98(Vj$UYxQZWvmX7fks$)3N)NJBo}hAGssac*F7LjP&yi?@2SQ+x zTbGc3ZYf>y)a~-cXEQ^aEnGjGG(%OZ=&K)za*vF%1EsOUi`t`iXa)6pfe~Tm3+GYr)& zY|Fpk)>1MEj_T@m;ETIDL$0^ty06Einf7#z8i~NR42E8QSfA~C@bCC~qf%*RX~QQ2 zRCDI%?S5)CfN`%r0tkb8b*KyPoz|#X8A^ib+jnP0zSD48h3jw3j}T-=5Wf|}@r55* z*yE5y_Q$m+U#x1s?DerDhOAf4k(gK=^S~p3a5Ku4(BVGcmD-{NjfZSWzQ*_w;732w zH4i^J2Fg=Xf`rc4${w?L@+L*;@uk`OWy<*2xyoGlj-~{inyWfA#mjjcHCf#(=jkt_dVWyEm~a^`Oi^S^ihYP#EN9Y(?$iT7$sWf zNDH)#E}mYd`I;x*^{$|uy0R$h8T#AZ$^smz`l=;5zqE|s!bTZsgqB+51$H6Jj3!`z ztN4O%E7LR29n!b;q*pdSGR|rnz~!5YE%UCt^T%5i87d}$)^n993YPTY1;aR*qrl@Z zXuQP>ZY|+$<^D9r#b*8NoTv!e208OdN!vxAai~CEAFGLm65PG*)DATVaGa2L!g`I9 z96-bRCEsP1vL|=>cr?AuO4_T?&s-S1^2X{h&Sf9Y%Xy!z=PXD=c?!;|e+h>8knmnr;7f-lujLlC)?cd%a@@x)kn92z~ZaBZBVhl-TOSkY{TS;Omiar@Av>Hp`C>X<3AByCG%AP*dfshpx>(%w7#@ zKl0q`q3tIlG_AQskBWjv)dG0{!mz~0$NZ2SI0+d@=(uPp&bkX@uBvgp5GA&4_yq2c z9Esr16-Xp@GfSKKNPbYBPmf|^#x3A|oryoqNrP5XOcU-16XUekg2ubxgwW8RRlEFOFe zWlD6(fxvy76tw)I$1qk%QX6W&AFQNv7OK}p2PEmItUnkd7tuM~phNrLSG?1WZ6(J| zxbF}6=4Uklk#p!li0s{7$=|23eymp_-fdzXKS`BBNEPzfuE7=5?ongDNe9?pM@C=43)fc>#9KQbfFhr&5I0)z+%eRX zL&@yFwB^;EXZ3y-V8?kpS6k(u=!j0D>86Y84zGIh+oQwZVCw*BiueEb9dN zq_r~K0R~HT#^aw~A|~8-z>kcX18x^2!4bW^3wqR{xL)BkgsBhVrpmTNQl<3h_C1UU zWB4f!KT!b;Gzh(p)e>qQ2&IVAFe!#V;M|&4|JoiM&qICtsAV^c8LY>a^=t!V=#(#R z;DnXJ5HSK}9AU$$ptZ=|0Y2k*-y~N1FjV_>C!GuwtXWU^IUvmg-h8p%El!o>HGsf~ zyc6!dH?PLe3o^&wG3O7T;SRJLpdx4U-K~n3y)Da;iXW)(rp;v`DH{vWe>C!R!?7wM zv z@3=Bj#5J{!)7g(Ad&d#F@7N2}7cL6Y_Ri{DE89fuLL#{M21Z~>!u{#;JJD(HQ@<6X z90W*H`rO7IJWuCUJhtR>EdBNvZ|~bYF`V&-CS)aqJYPp%9lX6!nASgEVgAv%Wl~+R z)jq|*Em)$d%G#5s-O-$ zZa9apE!clP4)gO#yhYuxrbZn~%S6dzPyf@vlyEFd3sFC7xXh90fLbUs{mpBs4SA=4 z_?|SR>hSW^P%9g#ZjyO%Oi}b^CMu1pp&lVeMZ-os%^#{%A2ZRr{uy@O5+&6t_ zEyTTlK9f}!;nF*XgGIA6h-$EuELrpru3$rCaei@mqrcB`Q}cI2>m+Z71JnUrbE0-| zk?3^o_}a^_%7yhd_3rS&asMq}0Z%o>rIxRWnEKmId?IZK?>4Mi4`CDt*}m7=H%4 zd0CRXhlF}DSmPY2c!}XBZAN|KAgTGTL-Y`=jc?rStdVAIlw=SD&p1{UV}vRf%#+A4 zINxTO&k_6VU9<^ha{*hGh=GNd5q$$^ z?|H)|l4)X1TtLMiQY+5F247OBVJfXU@{s1DfD+#xgPfx`4-Uh56-LOKcYU*M6)PiN zU&}}uJ~ikLx+wLezRe6&b^RnnRcqaY)iXKMfSf>ILA$5%JA-z3&&F(@P4r+1Pj={| zDf+^3Xkr>+w#U>UEB10vSH-QIipi+2UAQ1Cf_L#96*#I>t2zc%Wi)9Pjs<3^pC6z+y zM`Jwt-}j@#1`UcAM8+8f%T77{@-8s7{jf_1dah;KL`hyZ`AUrK9sQJ(Ue?PSB%v>L zJB+x?1|_S$3Y%gH%pDOP2CBq=vgvFS)V48D9rLDs9vVH~*v;M{kaqdoth>ROI6}^PXDPmz+&X!1H9p zMUT(l5mU!JX)aC3$e^HOE$0wM8G&H4fQ3&ZbyW-zHg+ZbaY3#am}>v&w)1l`l*s|{ zrT`sqvG!4AHnsq0i-N-+rN3PNja2Wuvp}Y_Dhm*+LF39f&hZ@tCa{bw^?$%!(W(<* zOf4vCs5@8VV7S{Id596!L?{^6;x4gGVERO&B?xyR`k_!(nk;M*Q%vdG$+Jgc||Zeon*nW&YAd zMZDk9;clHJeL5%?VbG7)7>p;Ra`!mvlU<<9;EVF$OP{5gD{eoS?aqO1x4Q#P^atRd zN(;Kb`(ky7U1xN#7sN6hnFDZRv)&1ONZ4VUA*sD3z-@+T41OH+3@v}GFTV4jj%kyr zmC5Y1v74wCLxpb%?9qo}l(C*Y(q^TKu!y~1I0=N_Sus|(yZdPWs>DJZxBDOmwwBpP zPB>?Xy>T=4^%&w7B^_O?X&FVY?{FXTvqWczwl1BD7x63Fx{Gd98h>+OYXuy#-TjW& z>z#CuOcUK)(Y{?skTld9@KbhL0X$iSKe$6EvnV{kJTBY${EjQuzN{+|#YSf8yEm@G zSsr%13I&4^(HDpAt?2uU1?Qd;x8HRuC$8x|eUsC)AelI;e&;{UQnC(5*9AMHo!K%ufuIv3ZFb?YLQC!kyH^z4khOy;Tl_ms%@6C7rpsR{g?SM%v$L^W z+!9S&E*bJ*tz8L-NhjX=9poCqppsZdLJAu@gpgVjGwSPGAxW)-4Q^XZ0fRhoK@1US z)QlF`)-pSMcJwwP&mP-bgo3V`hDkpVxS5wM($_2=(oJZ7b^fwatYUlfl0=Y1IXV|_ zGItDaXb4%O{bzX%>;}Gy6lDBcljBr>!~X3wyb^Jcg^DT|Fk({@u0O5M2tcIrhk^GM(-ogbTC;zd)$r{o4wh4|Tv^%r6 z#!?%9Z;!S8;i_qDi0taFfF-mcIR@(|K=@7XUpkfmBJ`?*Eo0%)9Ot;Ds<8SOMXXHP0gImfy60M9`?oEL58y@S&e(vHNRr2nc^74N?V7| zINK^T)|x{i75rXv3M2Zxl=MbP#!v+!9@;xT)-SVLrGzwx7aQPfEP`Sbrj>-D>@Hx* zhX!k=MZJiJB!Y!OCd5~)_8Y@|$C84*w1W*HTa$ZQ4;8=^E|O%~R4ULlZu6i~Xa1TzF-IdgGB-qfR;a zBt|@9Md8#{rBdcW00sh=F(oXRXC`nyN$56A&Q2WqXAZY`$VZogv!!gHwGjn5$LLM)o|{&j;>S?r=Q}#)k6Tr-%#R+ZJ*0O1*Bd}kBhf%?mwZ*T!dDf151-yid|89` zVZCc%=k}sj^q~3J!!|_m0DQo?%eWbz*Lfm9#*K6Pi#yAa?2|j)GHR?8w1;4=N-hrY z3uDlOEbIFX%B{xnw``)XVA?MrS~F`BQ>lTdWGS>Q?v~mg*kKj&4Qc|2-Ck2Sq>C7W zVNQt>WqJ|eiS7n{XqdE2*vk*A*D{w@s}-Bm#=a%@^dMyI zJG_N!_+B+njqZxY?cyTD5?O5w<&Ou)bh>+fxn{1yK3R_~+hQJ5gJB~a)(A&>6H9d} zG4$i=Qn-l+&TP&Q>e14b2gGY@xV4Q0<1LisujQ4?Ah!kX*&Dx2gJUS3xqktTxx-(g5E z&t{~i<%ce%+6wZNtT){)H=ye!O}v|bJzzomj<=x6fOlyJ6#z#QZd?RlI)I}PfW{Ov zf(>lm5~vhwHqCBR3A9l3)%o$bD)<}G@{D_Gr_6*(h+5sxt$k&_v`7E zmO$oMyslv@QXc&aQSL%dQCRAB{;`U>8b;5#55naZ6K*RsPy`-*_8DA|Mg z>s1(WyuY^Zo$**cX_h0lA^3mKda{u@m9)%1UFP-K7QtnJ4R2OjWpP2vN50%Az`$t4 z0Y%nn-e5PNG;z`BBKIA1?d^Rx18}DoiA7GtH8B6cyuVO$*Qwdg#)8=K&&jajt72*Q z>OT+~qtLIjb0cEq%Y~!!)SH*;$7CpcsS_<{p|#pG?^$_K){ZvfZDk;a;V|##RUg91 zg8buirjrP^bsw>%f_asG%K@0s*_lhHFLo~mSX7{G?8jv0i4r-bcct88yqyI>_5M+4 zFQB5e|7jRX)viWT(ZL>_KD(rFM?M zmU&eH>$`B=U6fElyc(mt-@smc@DL6v5?^*HZODG(OA5P*Inv0d2H1Xb@K$Ddao2{l zV5F6=>l>ckhMC*ZVewyXxf+_Jgoct1+q@`ErCu&tq0f&^=uG?de}`O!dI}UROBI2^ zdm~S$K8USWAB`VoGasJ9kH!1654?ynZ}o}oYMC8;(+%x|@+X`RHpA5XEw3dNI0b@9 z_+km2N{RQ!7uO0w$*Cq*?H!QJ0%BPM{$7J}SDekQ0)npaC~a+VWv)m{nW+-Rry8;} z@y_lk8tb2+NV3gW#D2>0$Umji&G>YqL=CEhiSkYYMsZ%@3# zm*Mz-q8(XrQVpjPSJ9^dkLc*hw9HsGA?AtibH-b5XBX8^P%$S0>`QQ>KDHhbKo!53 zPaz>^5BLE+5|#&ULdh1gWcvNf$)THY!hOd6Edh(azCA?aQQLcx=e5@y6(7$kXA{FI zFa9JP2~?aLd`px$C;)SJPrrdjd8L-xtPXH)$P5MHYKgK3iNHLS63RbZcnEr7(XgDr zKY6EJC4e`}Rk7usQ$JC7Y_E9u9rY)&RJW#z3F#j+Jbr9erfPfmrFIxA^i02sNjkA{ z7h?2fElc6&_9!(^!eL`p`+IiP_U+ZQ$`8mQpV$*=;QNgNHIR(0k9gHw94y?OBO*3H z^rAqBoPu*8%C$heB--=Pq?k}Zas z(mz0t&N`-=;tAdW20;10gt^$}3JB=zxQEAr4$R#8HanC3nV5CisQK!ZRSXyV(SOdg zP)>BsMkY>ipqy!?`9DMPA31os7}ndPxxf2tpW&ej3N32J?fb_8HG+82>mKQHazK za!$mzr(9LPmA6FoO%|2{h{(y3wMcm+L@?g+qZ!6qjOsK z>%qSu4YNU~{tBb_cND@XxLGd55QIW8u+A}P3VmHC5FjUBgIUH?qC~x&wCoO41?z9D zrg6PWv%H=I@5T-`#P}!Z-L&U7)oj*@lOtV;nW=A13Ci_gv|E0+4zrPG4VU86K!&w< zoK{oEpC!M|hOHF@x(u?(|Z{xO4J0*x`foePq8H_*>&babr++!l(1&>32E! zPh<@{OWb~(VL;X+; zh&=6=N}AzNCrIQ`1h%;&0`gMJIJqVqz)~yauprglO>ud>Fb4TYbg;|_!zE>&j78Q? zb3d}wc%}{3UDO9AZeD&-Lz-vU5|kKX=#P5;%d8d=t*Vqo3$gPIh&YXE`Kv`tF zb!3P4hQa7bUc$M`|2D_oeY<(Of!ERJm@u1AQ4V2(jW40mD7AguDIb@2>{7pElTYbq zr2>Bt3Sc&y6X-b2MdTb2rhCv9X}?Mr>Z%EuaF}`{C8Tcq3N9$)P@+cd^SI#Uyy>A) zVq&so_2@RPc5G+p>y)^HgO_A0;zW3?qto}tCIF^s`Z<6UR za-j|fSkMx`!=Sy);0$T@P4*0}6NQmivOSBSLKbCw405IB@+;?Z3rk8=v~i#GDd|~M zTY`cby|`0V-ZS{FDjAm8tV6_c?A%`Dh6};;ed`;b0HAv5pkJUPFCB{wD2L^pqM~&e zV1vw>D9j_FQdP|qmd1$dU6mWb&3gL~KerBQg0zJ^0{Jb=?6c{fH1+4LUuhbJSi>YB zC5UPcOyKI85k;0!3E`QzX5W`7H@eU3qPDz-kmu6`FwK+gLSg=P|J1i$v_^IYeE4=c z&8|J(ATOG>7dy<;t`rA1sh~>-YwBGCL{ejyqEem?CoWoI|MwPkn=?9_I_;Tnd};;4 z^m$zNvAxc_Z%MeiVjdOANIMYDCi1P`uFxuFX@r@B*G$m1wZ#!wq9X+d7nw3NO4x+r zU?@&tC#C^9sksF<>j@I(bxxHlAn`EchY2l->)k_%Hi(r=D$77PMLYmnHI7A06wCoz zv=>J5vnorG&xqkX-Y!ecY6u4C=$@ZD^)SR;?!M64EB}Af2fMU(gQq0>+)YMdqBHl z1wX>`UBL-y-be+L8yVIlB(PlJW${P8bwRga?Ys7u=n{)HqW+uvs@FPhg&<+Ru#RIg zEWo^~m2uNH>3P)?w$jVAzV;0SQDaj1o;-EKF9tcmjDSg>-QWC4KxJSQrt(GOqN)SR zX}cDbQ0-_nZrk+e6m9tdA*x_s{((_&DD6-~-(blIzn0|N;PR-(J1VjwN5O^{Sq!{C zM5(aQDUkkn;hiQO_GJPilXIF)4(I3M;+$@`v60hHyI+bpkE~NDlC8n4($b(aH*b-a zoi@O64J+C2FL#?(!$sI%{ z2_D?oIum0$A2F$AJ>PM3%37IwxK;kvoBg89lDYxe-vhC~oTF5ymvq{B{Kj0_Vh*uZ zPZuEEZ4CWK$2w7+v__yXP2dKHrNmv2Og%*O)oHZuH54VMmNP8202puslo_lq+%AQ_ zfDX%*lU|N~kXg$PQKzfd#6G!@KCSup32$qqoo`J}-GhqxxkS>h%;S9fF!bsyI&J%f z74pR&X?F=~QZ`<$U&(yrN631VUdhe^0^JjnjG9a)K|2BRG@+9eods1tx|0L?2p6Zq zc&lv>E;(+-Xf&h)X0c4VNo;IKwYpBu)m#6bS(ZuAE{Zh`UXaA+8@@7c3ySjtB-b0m z!UpqeJaabv&Y6DGuEXHLv|#TtsMvujkz;ceoQMrqYmceLxf@LxYo{I@-M8}!_IEhG zYf4Z-1*=i>j_#y)tb-6U@Dz|?o>`-|wP~Pq&1hIdLI%`%=PN_MleGr(T1U`9Uyokl zSfzT>Etm@}mF6hmlK$Iyy*myzt$tS(PkRTNeXTw91~t#L6kO|Tg#cf{fmcNMXI}G^ z#k)1SXaN32Js9uU)(jz;0w5T`_)aTuo2}oj@$7{7_)ZO9902i88nWl|_fSDD>gq?t z(||!ge6iLJTeu+v*-kXCd68np176jh#ZYkHUmcxlUlVc8vGA+I5qTK|%h!V}`E;7; z?;irzAZDJ;Ty+Jfugg66{GtbL6@~Ag3>-!e{&{%>BUcE#{;YqyP;lWjh z6~INu{8pHgmI&P&h1Bmk0UIG!-KMH5N|?hr5g1v%RX{X z69wv@>th-k_ub7R`j&g_U8o8zcjsgDjIlb#DO^p&$4p~zUM(bT1!y>zwfo!uicyx7 z6-f$C+2xabw5FIohDFaCyy(VIqud(nP& zkx9?|OZOAFPS*@LAME3a$4kH6>kVuYDhK^q9P+@46i)7-IjF_J?bgS7>mKsjaZRRs z=D2GG^i9T!d0C}tYOK0lXXwe#$$b6g{r6Pr-f8ij?7qDDJ#co%jh9}Szf;2WLpnQe zvJmWxQll41owL$kYwKwhN^&LI{+`rtG~|HetecJX9cn_2^P?~9B#&H+e>%69dR7jz zD~@W~dujpptFD_^{}B=ozs$=tV4Wli0f|0Nk3iOV2ieR+xss3HFN<+})6=~iE+d3+ zSRu9478o?Xx%gLso8O1ldm{beahaq|$Z#ry0Kev{fQs;sO2XXU>+Iz2Pb-jHf5F;j z6sp0vXCRFBOW#a2{yo^u>34j52y9ZEuPkGh42{;b_c2X-d8Aq>LttnoVK=f1aSqJu zyDH4b2mBZCXf}e~JDOS?&>h*pL0bHjI&CdhKc1~qZ1_0Zuh^>|E``Kt@hGpjX+0JK zN~W^A@=?_1b3w86OYBwNzk|bDqvtD$bV5C6q~bzHz}K)YU%loOC?iN_t~WGQl|^{z zARPv&AC=9N#T9--6xrcEr88Ie}{@7oFWN|$Q%2{7PWJ#2oXh(Pcb2zQRd4k#8-4G} zmm@`LE~BhLB8aU^ZqAoF$lyHu`xTiqK5M7`0315(Me9dO3%j-QMUk28C|J^3{8Qsv zP87g|0sp@*Ck|G6k>}BW7M~!Ra447FsJ-v2wIh&NqqHmrE|uOpL5=m2_L#HXz~>yK z=o3E6(w&bD_a1nCoTE9p1wBm*O;R?wvnof7juaQv&97pHxVs>4e5YTBd{qX`RJ?Ll z79$|#k?ics7l!L(8v6g2Qm4K0_X#Ox{ev#n^8V#^{Y%9i+<8>10DvfQh+D2 z;tqD2CY5?-U@uH);^g$2!J2$)&aH0gvkLrxQxXJxU!7t1i0!^BZdLnqafDr4gT4`? zzbNt;C2>xtz})a{&p-%M;$^ht*)GrQ|ub1GuzOhD;{{AgeG=;k!lALR7De7MA` zC*1tDrt7d1%MQG#uPXT96`p%l79@A@jZCY+ud=MM$-s7AeFHtCW7U+|SMf|UK z)br@TsP#9D$|-fc0q;ZUw~kzLpY9VU4!K@?Igv-0J%;naN~?xV+~gRz*pVd2!XyOS zUqz}eu*T6JSr(>!G6fEyc3f6q%|2E_Bll^G=Iwr$a)fukM)AvUh(aeG*J4KaUr@GJ~w(Vqmw42O^MT6A;4s)RY0wOKcTVvc>(E8!_6z~Z;X+9 zrh+SV1z89Re5^KLZ@;%br?Mxo*M;_BF6QksGC2JFcR45*PJ7#}Nap3L=af`G;fjhd z*AqsN6gs|^od-WXJ$`9f%72%GC2LW>Krd00)QZ@!)_Id`_38RrLX77)?6_Ez^o6=P zl1m6<%n?`n+>oRNGM`OMo31EmV;zrtHVWSBp^7rsz6$`~Z?uG_-HTcvYg@3OxH%$Zi1e>< zC4@5Sjf)~wG6uE-vUZ)JL0G}vM4XpTu+CASI zNjOY@Nwsi?CMTTVk8p-EMV%Z0_Iau?tq?uiSRI3n$uRC#U`D)Mqa~x3%NGD^3}ka3 z-@L&ha5K8fHi==3uFs*I5IwVz{!V1)Npv@!P3ZFyRma+B>@c2qnUeuSDt=%f;L}b} z=s-}Jre4VNLU6}oFQNxu|1eUFgF!nuEZqEfIt6HwA<-!o`d&O?1J(`2r!A2F*iM8$ ztC73^^`oxvBu}@so^G_w{&_+vdxz^1pmzr;C#$*HlirFT$3VM0d5ZJ-Y))I|QdmrgdL=TSz3dc~zTH&$rr36H& zz}~isfs-uQE9SC=5RLXQCR@qhGf~nIeff>4sl?_lKVX1W@1St5z8`gm5Pm(kCQ4?v zQVFC%BX2T^V0ax7PRHu|We(Ae08l`$zlz0y3DOiremtFdhNYs~ea8dOh`A;JGNYq$ zL*nDs#P5z>BcxMOTKQnYmo=bNH=NRDY4gEHAw5t_3txEQLPF1%z9uAld9pWeEMIJhbk(em_%!FW&c)I2it3A#d2js&`dU4d zZeJCOO*9muRO6}Cx>3I_rn))PqV6uaM_Wsxuj8OH^=wR$w;9tF0c%uTtyZ04HR>M6 zu=TJyWZrZ__VRi0N2qT2uxx1?-?u1yp6_xsBR7y1KP=&0b zVMMOkmRC6wEPeR83Jt}VzYhA`^+DKm(16VLvHgZ80baTT7&Rr?hfTDIi3WDj)NX!$ zJ$mSF!OHtGea|9Uwln+g8FRtH*Uz2%^CJglGnosNXX|i4J9{e2*tGz0#M05Yy-9{b zRj42&-kmpj78Hr`*x=tTtuX99gQC?QlNZyg7}$_AWd0j9bosIKQ-5&SZRo2BbL5PV+^J@MU#sW!h8trKUzQ1WW+Rgv|}B z>(C?fj6U?a{s9jDfqTEU`|%+}ZEJIpX3Co?loz_&jS&!rB{_M3T1d}=iQSu2HjmW2 z@Ft~xQRBoWqyF7Dzb=CQuhOjkfZeW&H6w(sgudvxla@vt&}^0}D93bOVF z8v-K_$W^nVl!2Oh-VlmWlal$RX%IxwlzI%j$78qh4fDOgA27-eUnPu%UNcqYvY)!e zL6c|}->(}nMvCSx?~N2^IxgAJOOT!(1bITb0`2EUNEv`2LY3tx15^quE!{Vf`%{zK z-)*6wd_KI!s^q5)C&!$_W^=AiSKtw!O@mbX$s)S0S76+nOrI26fHt$kPJgn($Rc^! z8OeUqh^C{#FoG&v))vy9aRA2;0PqM+627L9Lv@XMQFY+8P;*iwwM&S|@b_o~sx8e& z(dfwKf!c$s=xhudF4d2#D?L)oTBNo>)z2Gw5}Pp{FqSZsg+e$zc~PX#DZZT{UrD^h z?DC^58X7MBA_Y+oipXU`3^%5pgD;&B=jv6dh{U{0rcLz4U#9|63NZatzE0}tJHNC1 z=FFZ0j+OAxn*KXgz$03&JUHzKDAv17Z(8(^A};zbFb zwy#C1qlsMy#b-wn$EL|}_@K&7G}iB8P!KgPps9T^9^e7d(y*Pnyf>sLFx5w0X@^n2 zG{=*$I;Kf>rg-OlT>{gZBj%Ma?j&Xm*h5*rV-?jH5EOk2U|0Hqzix(o0=zEXDf63` z&Uhx$&3wCP>^zr2;D&*ypTKV#JvV>QylgREPJaLlzmdE2NR1YL#M52}`_2uQs;7h@ zfKaUN%{1FcF3@Rp6)A0JG!>0aQt(#gbt5|ZF%lP9qgrrpV&X?p-{_F(BxKCy9j*kP zj#?F1blGEHtBw3n{6Y>DdMq*UH1U{Fb~iDgh^qnik*EUf%S>UtClWLkUT|FzGHBxbZ`|3T2EE8aNQVNq>6o-}DX}s5o+a$|H1C3a~Ey z`)HY>`GCSH#+@Yc2uZex@TIo_`CCtGjThLeu+*b7NmYJ?nm%bC%; z?|!29u%AZO0jAsK9HMtv>dU?9-uNZ2fut)huLeog+3}Vae&o=(D$kyVFZv$;O)@yGqV9n^7qDK|AXA`Jb%IpU1#+N;2Q;VDdUv) zL}jy6_58LTx_CCHg5WI7Z60j=4&~xm*T(wXyb{x*e@!+ngypw}YyOJjWeo8l$5*Q1 zFK=-w`BW^uzz<1>pGdSau_SR^QlUhV$MoCW3HP+3D_|UDky<5l$D2cT$C8O^x!yAJ~`5=hPSLWnvV7u+t;C*hZYcK9{Smj3_%16glSEy4H zrp01@Q%@<1Q1GE^Xb;fMrP<;OzPNAD)3oe)WV9$_6@`J$8C%zYO@}Qdq#?r5`q_Z} z=|?4`j|EljBF?})NtZUOUMg)ACC}#KjLvWNWC!nAUt0ZuJa~z+4!uQB2fLNO;7FYO&bk?}q@+V1%ma^l z`?Tb6wcOwRn&yp&e8+%AID z%(FSYQ1o9kEuN_h%Fpspq*)TDW2M$^?ca`S_Ooeu%k1~Z0m&m1M{E_A50gVkMa?Q| zg4CXjWjp2O9Be%xj#u2&A_H6v+)Flon|$TMbVMX=&eJ`iF}eEF zu@ib8{wALlPTZWXK16$&L^9FP z|2{MDQkB?&ZTGPsdNY|*8FY>GqA`5&GdazjVuN7Z@u{f>S$-)3sMaDJnTUf{nae8JcD@!o1p17{OS)rI+Z05}zpwS$BpE?TD!mtad>$1HWCY>KweA+ z-AJ%Gp~WRt=98PEK$Qz%3d>I)eQ_R$9{-2Tw~gy&uq-`dSPO-hZg&2Cq_$=9-`P2V z6$yjrU)UB$>l?)(nGFOaY~K~I8n5@Q@amV}SPoGFbH+L67r_R2b&Q_RzAn;R2sN6M z#I%S(a(_#$zg`I2Rm;6>=rXq0x2KOn#uV*+*2@uzedLQ{!oJxqr8FH37@feu|G2< zHyQ7kZNiIZY8|)a^`mBJg4G0=-ao+2OLr_S(%3v$`HONu6{b{2T-2kHdij0h$#G94 z77UI#Bpv+v!c6H9;xxf#lt7Sv5VbCk9Z$*&*vcCWOKO$WJvhB0CQQjlB?d~iu>B}1 zI@VA0BFIjKC(o&#m?F% za+}d5D3#xZarCcaYFXhmz1~sPMAaH}g@))YUj$v;!^J)x(xYseE>iKqCg=OA1cV;S zzQ!S;2E8#S)A|iwCNx!Ce>^WyR*58#!PuNSf!fJWK073SKA-Z#cl^(@LTtmAt%;{a zY+V6*&JW9p$J+;}e3?ZT%T8Ow#pGm<)G9FMSy*&wN)OHlc3HX3mC#eUcO&5q(P$4* z@AH?!+1tT@jex|#M?S_c^@HO{fLc^-2)*EbwvV4&lNA)T^B3_)tS3PQygSbl{is=Fk|V<0Kh_kn5e`p7m{9Rl&sah8WUr^5cG4C#}wa zdd>==qBniSZ}2N5ithx(BO##aFyS&z8wTqe2S%3pQKa{AI1sU6iVvJHwoXiYx8zkn zgMK%$r*!-i38)i)PBia@a^fO_Yxa5q;VV*1aoV_IM`voh0jqL>H~&7kkRw`32Ys_)qBO# zeQu1U(@<2$aV^YoLS9cKlPr?xfrN`s1}xMjq+i;>&0;miEl)}f7FCt8FyBHk_0{%o zq_Y`5ed&x)ss;WDsPeI`PO}}%sC>#68jZGcgYqlr7ix#NnmBz~n$hP|Zf&&BC$Qjr zdKFt40>J1eP|gqii;pk(m@=oG$sSi)-k^9J3g#)5XH*z$Lh?2x$mGF7`7m}3Y%%QP z?&lp@*tEFaXp^N`1wSMy&ceNcpCOvEN$I+@5YL&F}HXK zFcix#HS?zcv<=v~U1joh@z(SF4uE7|ROY!-gA$;Ki23u;i)D21omn6_3xW>@G6L0{ zjUYPdl(vY13H9&o;BP7BSNqm@^7Y>YCX!goQW%$;?>n1TbX(VQdWop}DgTjl-r9~T zQ4swg0=x?a2=Bef@ZNjA{&DAJ6U@>@-RD&8LT$F3>e2(EWZW$;Tps2&!JTZ}KBhFJ#GF)xHM;JT*Wk?{L)OdqYWju{(^%6ixwqjTu~xH!LKYJSc0 zw%D4SZ{Wel=B-zG2Yld)v0nsR@Geqxz6fCo)qtv?vlV>$^!D7AaV?vnLa&^E%xURn zQ$xiUn5g+je76xG4^*sAs(B`6`HS?AshxSjL?&MpZitnt_#yfQ^M%Lmb#=E8fQ#QS zF}?jgH{n@|)B)5bYfPxMSOxSAPW+P$Ry_HpplcbX$YxTXuk(V$bq8q%e_?o>S007( zNX)~x*%y8}P+dAog5>q&_rc|Ub>&%l@frOX!Z*F2Gnyr}{5z8Zxl9H2CFGIwIJ6d= zmGN0Xr6Jnp3C}?)+u^-?J(z5@=O|mCH&V{l-LPxTJ%Ql|RuT=5f`%RCLujLa>omJp z@cMRLsz`?s_N7zXei~D+lnQzYh@!(AyhS0l%!$qL6i;eK$3b1ZH~$rfqv`AtB7ly zUaOE8gZc`DdW=OKwZ!{gaX(y#}h7=5J!Xn)>vo^o!PUBktzS z^9gk1(w&i7f=}-39Z}McN#htrU8m?T+rg*%Vk8obvyXv0a+`7WaiWuYNJs-yuzpf5O!&K6iVZI&kGD z1c@S-TT0YaVFuQ#S3D``?Se)!3Jr*?Euu9;br&LienFix_=4l&r=WaFN8J_WrH>a-DIo1wBm$rdm(eFn_Jd4r2N2SQ|$qRBcjyt!(f!{S|vh~i*$jzc6j{tW)z(86IXG4DRLkbs}X1JbKN z_oMBiL(|2*X;9F73*(3P9LyB8xtpZSgOqhp4>(No&oYl3ZvQToE-G|ZOdDM*Gq-{y z>fr~x)X#Sx3Zr$qCsBI(LvgjYm^~(@BfbI5Z%@kgP6%c4?C#ZAH@$i#b1ql-jjh3C zwN_S*tyg=>`hm25{Wx}pV=Wtx8g^>m11;)&pwV|#2PU#wI8-|FDQVxl#6=V2MHmsD zQ`oxwaag92lA^8RpT)-5Be`QN7y1>R4S>dB78F0rNDHKrzlGop-%$&PHsz;8M83lg zs4w^afY!5BF**R8vyj|ZhuAqc9ec{Z{qV&>Kc+CcN3epUGIB|+YS7ZO+C9*_Oq0=B z1IeLv+uqOl+V2!R;W&n;)Zta~#ZW>}nr`8IhJkrOxJHt7nD*ppGtI#_CIhg?#<)}n ziC1x1%DC5JeGpCM1NsUNoVt%bK&TxluSdiBopUuoZfoD$x(4OxW9j?w^hT6H zoocS^{p=4!xf-8HXLMpDelsZV17qGDcO-7wYZP*z=00vHXYuCnJdH!*JM}O$!eh-` z*2B;g3MH!(*x&N*gt+TaMk8uDg`xM;2do_%VZHTVT5ymlKVMIViI0bq{gBz0ha(Dc zd>$JoR@0_tNG)s@a1BVjKVK7K;|iVfcI=ojwm}&JmEYFV8iDv>Z~p)-b9^#n!}SIP zD5QEUF&%fr&u)34*??dpmsc*G;6!it;6;TpZE_~J9>blzJ;i(wAXl7>6qu6~l<3>?6Up(*r}YiKg&!L- zI^7Zdmwpl3Y+W24937#wOi3#s(-G*MGcPUvegZi^rZ))lqH-GI(EwfAZ~u#$@HXG`zWtG z3SY64P{~%395*rMIa&STi9;uHRVFXo?vS0g=a7Aufqk=+{6!(7akntwaqAw-A*>sA zFhC`6-_H-i8}2Wk1=->aF^q$cHJ+BYl)mNUrC~#kdv*Dji(M%h#l7ASB=u9=u;8Uv zKSXV(^+bP^AwN^hIAPH`Y@p2PaPT-NQL3PpL|)Z()#5bQNa4F)q-$<>T<6;?V?CKp zV}O%3;apmu`z(W`hrD|qL27}U5SPfXIQZ4m1fcORY+y9!W6vQtgk)#>GT2j>Ma<#0 zT;r&i>BR*nx_myES3E*6e#LA2vb)`_Pzy$#)T;#11B%xp@r*krv_55%o=nC@y>u(8 zDfblktsvS;|F3gNH}g)-lD8s8R&ESWOQ+I;{Drz=7IZ1XO^$VFGGIq%Gi`FXw>Ry^mlhfsGt#; z7?yl*s&<3@3KkD|;KzDDLVs9mZ_|hXU_Y4!C+fxm9&g*F(@VPe<2V}t zM@+TJgi<@7pvxpder``D{JNW(JJ)99uVJhDGW4O{mni<0;r{m4fo)Md-(WPVC7$cW zaVFbH5#t%!CrBe;Z{-w$=php+T-7gmarhsK?mnrK^w#p}qBONziQdoTJ38>%o`;=f zhIfjg*a}g1#U;BPX!NWvSk;BGXb2#iBKgz3ORvMcIigQmV*5-X%z%laI2mf47Y z+Ow3Jq69SSj@B>DhhmVkYM#7s%O_@M5|I$yc}gXaY-!{j+#w+VLg2AW*T3dWbaTq? zZmhZA77<+`-rsQi^GClkvU+sEa10HR#Ux373prb?trlh};07-?X4-gOKBgh))5@EQ z3nd4Qd7p(=NFx30zd6|Wxnz#_C*{AeC^W)m?dD~c;NdUdKyOd7Q#1_Jp(lYO_?w)~ zO7nW!H|@?$jV|OnPz}nx-cR~QnSQ&OXhoR>o~%Nsd&$%&%p74O(_Wk}`T(yvF)Z|H zzIT!^l#jDjh|iqmPb4PSTxLzyqTsj{>eY-o6ze;u@3{8jvhJz5I-%$CB3H}iA^{Wl z{&{J72x(Cq=Ez((bp)U_FEkP?u5o4ZG=3J093)X2qm=frpSBjc36U`u4TCL`J12wH z3xKb^wh;+M`bjE)LVgaJAYzVzAv`oUMeWi2P}&x72Om|*d5}8|76LtQA6QDJp1@rZ-8xUtVQNG5|xtRp%cp?#v-Cj$fQN8<*3mY<$tWm^VSGVhVo+I zrb2K@ANAM+u%#Ht8lj6@>}rnTk9M}17qD*?)HNKs3L+o|oGl959jV zp;8S)t7$+DwrzX*x2CUjOiaOX+GZ@?|q2vKs|OeeMO^3Dpt>7O+3P_=BdJAQs3YI z(B{n_)NWZx6QXx|#vw+|XcVmTu-JFrBZZ|TwniWOps7Kb>?6?h}XKzqf_KU;sJB-^N~|eaA5O69&-l2zWD10VZ2y*YW~K-e#b5pDusW zR1=z}{y6i>U>?~D$J*Uk@Kx_F&eJNn+pBJcJDcTZwNqV4)OAko%|^#m{A##gh9Q#M zn(9G>DSzY%;JM6@#(spOOGvw+!Qu44ASFXP|BOwb(l*DSq^grdG`9wOwc^2)9*;AQ_AfsuL;)rSJD9(?ZAP( z+j~-U`DN)6pPHe}KR+}Zb+e|K9OeeN);M!m!hP=xOUH!ERmm$N9oNLg)>F zJRg*YQ(KiRjOT^KlHuG;Kh}fm;D~HGvb((?d7|mFX`PXosenrlJ>=&d>|w#C*P?!6 zerb97jwc+3vtt9qSZB;mHQo*6wEAoIM#_!G`%$i~V4z%4_cY_*E~L15tMW7tU<8G^ zfQhFrWu!NO7G-}tuD9+XgBhQ&M4skIN*u-H@ZWoDW>ukAf|6j~oezY4IrdZSgZ|+Xe)O ze(z3KhZF4dbJ6K9!M_=$RoZVCxdnz$@xYWKr+vlMnV{~W_;J`Vwa0{PxL8pB<{;}I z{25flQ1AOgqFQwoKyl=h(Gk`NqcnN0-Sp7i^YjrCMDd%%Pse3BCI+m!u7rav!kNTI z+vh*mujSr{i?mrTPwA6bH8#qwRDY!QYklPp&reAf=!=jhcNt7tq=S2E;>N@z$=P`P{ECBCK$=687fa(MVJcE^LwSlI-=H&sWeA< zWNq4WYY>_9?mG?>{;Yv}$6jM6Q#E)2}o80PMZiB+9Yb%r$rFKO5>y`7s+KaKU409quijH-MH|25d zUp$X2w+1P+>CS}EG_=HeAjo^#lRw#XPu=d%U)3uys^sIaW~Dx>=!aePaM#-o_<1jc z%%`_?Q!y6&o?Z2~{TAza@1n$2jlI-d0VG4aC00(_+%rDOr_H&R1E#zX{fo?|AMFRm zjfb3I?LX@H*#x)d)M`UBa*oFAWDAhs-|`uv)N+gIa}~%v1xza+Y8o}; z8wtbG+_UlmskUyXD_!8@!tr>k7q}y?_ucsm9<-thSr!~p9odFUY5JjO7#x2$lo|Zd z^%*J6)$GwJTF7RdgJa-YD?E)M4<*G%!@@5L9~+hf$wKZ^?w8D6{_U6Lqog7krT?B= zWi+Vi+w-3lAm^?mDk{(x^Q`6T%{loMe&?M-oJYpCb;ap$BF z-9x6n>{BGp9_DRFchV1JQ#q+*z()CUKO?SS+Tp@4LiUI?si^Sj# zWy{WAA#l%E!o9@8+;J`Mo5?$B6!9M+6*@AU%8z6?(1xY9EcFSd4f_eHxL`kH|HBbo zZwMVb6K~U%V98->hiP>!P5qR_2DdyY+R)0LoeRU@>6eL02eKXzrUc9*^XzjA9?Hw# zxjN=!cvY_zg0_h+N^TLG;J=ao4H(w=RqS>n;$R7GAfjRmBuDl&u4kZ!LhN(~dn54h zCBRki{(DG*ZecQLktzO3P>Fzqxjopm(OV?w`yN` zV-}Xjfqvu~8ac-DS-ZG&pI^9wl2nde&|n_;{_44G&?WQK2{9X@BT7Yhn9q;-l?~2R zZ-oP92B4cAovsK8YI2{h86tGxh7wYh)Lqh{?AyuNQt>uh`5|WqToHkUIx714OfksC zL_EI3AAN0)O@^}F<7kO3elswOO>}haT=fgN4$WBs6#S06zo@R%n7P`GA!I=iDDPJ4;Rk{T8#BwPYGu zMA>IDT_W~qovpV>Na4i)x!2a?ZVQLi=;P?l4Y(@e*z|!wx`viSqQJtmoUDdU_{4-a zRij1C!j>Q4q6XIklkQUBzP`=nZ|oGQ+61}N_GGcMJZ_|_O=;ojTfqlpuS6m8sj*P? zI<-0Nfk9NeY?V-ntP}u3W4g&vLKhG4NKBLd17sO@-w&5j5jkLLB`^q=DqaHp=(5Q3 z$SW9jql!T}nN$*yv;I7_PNDUA%JLQ{aaazKUkH z6K4WkQ@VePA;vNOUI4$5^~)^vBu0ELfMhQE`u9VMCi1+`e|O>%__i zuXXD9Q24o`BTr>fgLON!%rFjBbZ++yJv;+;!S&fwQ0AAEws8$Yv1 zIK5?`pqGobR-^sK4LAwG+V6Oplq`sieal*o0L4>Wi)=9EyHUdcs59l0;s}RevJ2_Q z44}36d=NqBj^QGs)}JF@kdc)j+6mqSYLIGfq?c;Zv;kh?O`s#(J4joo;H9S=lMeWg z>hn7Doz^`XYQAQ?9!eqJx8w~ncxgB%X1WVew`En$^JYrQa~rzjKDc2zVwGvZXK{|o z#+*!+mT<8+OV?q!_cv{|2mNH&sKLZJ`?#gJ3=81N^A_zpSx?U{&l1H}?P18h zSYNwT&~COdhE;Q!f(%x#>602{LM}!wi=|(~S1rPhpPr=1O<+Qy@)@|&+R54^SOzsJ zO<~2$)s_et1tpS(Z!Mvx;2$lst7|06=DB}+nSRJs%--BI^rz`?zysoUgaH#1f)v@| zznZ$O%bP~zV;e(oeev%8i9z=PQ2aU2(`zhn8c_Aow-R8 z5#i=J;yxH_w9na@mbF(l+C8lU?puGChiYTXoL`kkY~>zJ(wYpZhw;c#Q`wD%*(q5_ zsWbD}MW@)Ggy32MB8j(JZefD42@xGZz1%SL14qF)462UY=K6#z?HjcJB(RRG!~L@9 z1qM86lDZP(H4oQ)H!sP)Q{31f_DRoL=I@aH!<8VpVI1(6Y9tITIhQqCdIb7iDgF)Z zQPsoM%_VUK?{=T&YKIoEp-k*i7(43fVKXwD&o+zOmwE%Sscal`bTl_)S>yMKA zngtmJe_S{O_8Y8*L{#mc;YAN5kw5}X&oKrW&-4=j20;10aKB55=bf%5=dc>IY;v6F zu9-`9#phsqir4Td@9sNt9D#W;3@#fsN{*=Qti$GTLa^uH5`dl<|o!GcWR4v{EeS>Ry zUM#ASwG%}qrto(#V4lv$HSM4m#%~}mN{zxrZ>}Rk9BNLI8gQ;(Y(v?%^SOT6txs5t_2=%2TRmg0;QmUsV z3&9VG!fHo})FG`P!{3~Y?Njtq1bFIsXvg52(PMmHh4$qeO^XgCn;rUyw{uVZxQLXq zNt-F@mWOE&x@v0ACDZ*APwaXlp_+=GpdUUxPANRXayRbI#2_Z$qL>zPNoN+Ho$+t; z;bIMDA%yg3wU|fMf1qhMAA3tusg9Ds)br8zrnb;)U~vS|Ch3g4r~? zsQ=1xokQ(diytjtLW-h?=GKHR7tXI^c3t zdL{g0>~fkg;in@b4orUtG8`#gPt=nk_)~(lBePSVryF(gov8zthJ*K8?Di`GXu`Z= z*EdE@6IfB&+o*qh9ZAX1t}+>tLy!0HxNB4zIkXV$DbIVL$yRA~`ddAf$t{$nytprB`$6`E)tZqnH*=R2q9I~6K0eI-%z@7KdabPnL2(;>umX!?s{-5F*9 z^rIp|h4cv^RST_*3?MKyq1q#JMRP~xLin^+q1#JiwnM!RX&U0)IuTJJ2$I?R2A7+T zM#kgcsWQx#UtSP;X9?-O8P`u4Z}!3c9ZYj8x{PY^p{B>p^$%!jy6$!DMHZ0m+ktROYV^gp{KLVOj*+G+XSZtilh^38VcTNdmHFSvOKh+C* z(~WkM-%yf3T)%|l?1^~Ztmw)XV%tQsipzl9wPB1^@Ww*QNeSrrCZJC3$KN9KA9k6V zIBa_cFj|>2X5>|%N(bS$$l5@BJU@C4+e|$Nu*w9PBLtG4mwraOcx7eJa~qA^+;ciq`%!eH1+U)M!i=lc=q+l~ZP>ci8!h=x@V>nCV2zP^PYcas3U44`pj8^bNd@=&#}H z0-j7v4b~I_S0NwBWZ7kt=5+HRX z;pHL$&?AhDals0&-;GCwi-6}Qk~SMBR{Hl8A0g0~QeJ6ii( zy^)y+cbc+_XNyeR_uqzUO3>cR>q z%Yt>Kh)r0D^-CSgp0*E&x~T92SZIqt?&vb*V&0^HZT#m|=rVRd;`?z|)8Od@0YS@m zkF*JLVIxfGye_z4ykoT5YxSD}XPe7ROG!Ctq87U=cQ+Nt)Vv{cs4$#1o zpMz1aift6KoN$J=ut3*!UTr0zeNwXO$-s$z{L{*Pz=qG+_j$d!rg?9)JdyY0t&21X zL}I*3QR)G|E(z$zY<%l9)KtODewlnzt@=_B$Y+D&RF7=Ud(xYJ%@i13)$|v~Jq)Zy zI61&}7i}`EZ=OhEfUc{oZ0v?kVSX(iw~{P-xnLvY`E~ zMe=Y&Yp;z9zgy?rVq01~jbD^*jEuX1q`V#UASAj6$ zo68<4x--&ppEh&Ps+8Q>$|iMksX%>H;qVYHp2s0nCr=6jIqZCHRI8Wg&s`R1LN#?8 zmd04j-SqE%Tgvf6CxY|ekoZk#6Hgo?$;ABx>3)gpFAEsc(PGUI5OfRD32g!w0+YLY z|H!RFB&Ng@Q3^!E@D8MsUL(tvK}35gb@t|ih;Z!kbbjTqguvG3LKkby^f3utknuHT z6-^RGkL~G(4@wxw==Z_!=P^UFv}GTAOCeis;2k|Wbq`>NTfnr|p+m@TR1OQQS4SKc zbr)ea8I*s|-4i52wg&3d*`>zTPu{9%HGkLo9TKtb(Aj~WVXV0-RQ33k58kI{;EU%2 zi%+TW#nhLp9?H9TtDe)w#8aCT6_6?dy2hBld%{QxmY;1#i;A#P$lNTPmVbal;WVCr zM}H!0QGr;FIua__43$|=UNw#!Ly3DR{Whj^GskcXwcR&Ii}9;6uw%jIri*dG94vz? z&e`vxzTNg`<=x)*i_iB1Ci;m}?yu<0dK&O!Tv~y){w8;|>bZ_AXYd^aIpK;&Rw@uQ}5gQF{HTm&{*S$in zv%m~mbmjUMWlSH{9`!ER+yk=#JZ46}YoeES%`eh;FE0^JjR!l^?5#H8mbL?y_$GU- zM-x+J-1C${MAp0w>5^FN!RXh_0fTE+q_g69xw7Dk1A^EFRyZ364fS-{SJ4Q;e6tCXe-yETL`L)?Ik8oyYW$4 ztIAXtXNPMJ#lN#SZ#VV_41V5$Z{EfRJlAwnhjyY|yfI_jr(gkhjA)N%jy|cGj)-Rg z^H-jx)O+$2bbJQDo-X(ZZ7l5jw6l9EA59w_Qx)>$%w!)I_&W(+O~Zb|>z z{O{^7aGbLSEpH#9eX6C2B;u>~RbQBFilsk8kr?sE=;BZ}lP!-Xy`zHI9gHJ1KYLGZ zX++%V$<;I~s}q-)nFonU-Q(+9gFvaMe(?%u7cftE_}aG}I+INJyPMNc#@$=v^e}XF z`WA-EUSg?-CeQp9e2Ft9`4#7Tz^EQroZ)6sSP$!WKvse$A}Y;YV{=vlt5J}9j=;&q zdRuroHmJ(=pU_{AP&ZzZql*a|bg`~RiiT+}oZ~lO00Mh)( zC+A9WJ4fY7%}t$JKYfcsmp_i+h8P%SB*hSFV%t^3L_&yWkIf%7!(i(9Z2JLF)Pm z=O48KQ+i$yGdQzE5T@_*xco6-T_ti|8&_TXKr{|hj+v$)KWIFmR+IH+mmnv5gE;D@ zGdb_?=cC{i+wo0 zt~CR>g;rfHl-VAbd^pwO_}W7_2{_5uLE;xKk!9gP_>C$~AQ)WR9uoYG8{=;;IP1#& zu6>DUcIlbyH6I;}ii#e|3~70WF2TE^AJ!$`5$S$d?|y4_dWbRYNe7$zJgObhcsy_# z2`0${4HKQg6b+guw`p@l%-V?J@)#d%%GONvZ|6j=X+P#MGn^w1n0s$0&Y$zhCQ&@8CI{$uGJ^^IiI?qxQ{s z`VZfb$xcK2GStrdVfcrdOxiQK0SwKh0`-X8vnlA$?8sE*E0lSZb&yh=H>W7K~p~-8YL!yG)lv=iiG`nByYP=(OB|x)bO73)4 zVG5#+l(HWAn#a>|1AM}>wGYN%ePEVyN)HjwhaUDOJ07b_C*3bhFR!>zNIe zqk2)b-z+KH7qEBDIoOycFu1Vd`iRJ6(@^vQ6eM8j=PIeFTF1GF-EqOjNg+QhVghA& zg;ybNEri~_%4wqHLNwWBa)l8M>aa7i#I#_SR(vRzJ@20ewT=4QTr^)ldF-C|Hvuk) zVRs8TC$!7=OCk0+qHu4aX6x({pt+HCJ0mkBKom6ZQ8A)NPZiBg$31;(Mk<_gU?jrm zre(zk-yW61c|y9fc0yU+DV`*fFu~ZJF6vnW^*7A}(lePj`RB*;)$Jrc!j?e|&Mwu$ z2UJ~s4?ESBoDwh*I7xuDYrjw zX4Ycn#Tid5^~n;aB0lPIrtgV_QD6b$Ds3sKtWvs2Ae`{urW^H_ggtO}@6;*hok}G; z?v7$Bqd=uo(51&7I3&-Aan42|QIF5KA%$7)k$3|qf4N$MVTCbLV1#SAwuy{}MO0g| z8mTSxO7quMZj?FAB(BvD>E6F<#IxvPC>)?xv#H3I@Yz|l#KP)FxzEet-XM1L$Hj2k zo~VQ9etLc7BgkkpFvf3gWl|Qqmb>IC@$n2R`abbdWuV>uzCh~;ns7XV%EuQ5Oa${2 z5YNl8I9htx-{e=77T-X#a@;i-kQxh9hov9(K6*ApE;elHrT}5!I6pqT!(@3g-%~v% zgh?Slpm*sf$Zm!HQnRKTXHpEGtANnSmpV^|lOMIWivJM)*0Q`p2<6537|Ja4;O!i< zR?u0{nSQBlEByTjW$$f7l^DO}3>UgV1m-G?8rBKgc+6ZpysAtP9Y%@$R zkD~gLm??h>#A8oi`bvQ%jBKm4E=r!`_cq~(z;#v*u#U^KQ1n-WN7sNYYe_XWx9T=o zaYKh3g3#8okCR@E(0$-enz~PlkcUFHTF9ANinaV89 zvUA5;H3zkF1$*``aXk%S)>`N|D?c^*vIA|2wKxd0{b7LDzyPA3YYS1WYN- zUox0Ik^nqF!@t(+K6*`0G+HAWlm=Nw$334|qgRr4Pb`W-&QpO85_#kUdEr=nyFz2Y z#4_d?N9(bKcpkUr64=G5DaFH2L0s!gWtS%u$cizD0hXZ7&xm?ybpt*jo#=?QfcAN< znYHFGN(K0rvh!c4HQ6j#%7#NRoliTkP8`g02NSNDVH|%nExyU>eoHi1@}e0ZK8o&E45$^c=8}=Kow{gvp^!iH=NlxIouVr6=g4 zE6XrIAT>~v>IS+e&j?BPU6ki)i<)=CE8a7pw~SNE}Gd8>w7r8iD{W$2958$ z0|7ED(iGTaDyj9uwI2?i85R5B$Gm$}9gw|je%*Wd0D=%~pDzlz)Ub8+bW;3ANR$eI zMvoQTrpGVy(PmyUS=7a2V$<8<7e`A{8~sD{BGVvBNL|w)Xl|$*TNaC%{#LhD%H;&w zVkcy@Q(gNYStle)_32gzBz3bbHJvdr9_uCmgz=Xz%DzlD&rDNcSw9aiA2Z6xm{Bq_ z;LKtd2ZK^N{2d-0YiQl1lyr(ApSND9Z%9?7P&MlU!jBYMNw`M^=sgO10K^Lw8xkzj zz9i7IcFh)g#rIOsne!2`)Ot>DFJjirum2GPoIhbdIM|AbZ^>Wwc;GlL=T zQT$Df@U1_X*J$6uNO$)j{G-g&S{s6Uf0m>NhWSsP9Mi1oSlNC=%|ngv1?#s*MJ4W7 zo@mY#G(H8GcPz;?H@Ge{Y7k#{kAF8w(27@_-3^Do&zqnA?jaCSV*{pW_ck8Go7gwd~Q)fC7{h5&ERFzG*Vr%LyR zf7Y2Xq|8a0%2)>viy!m*hCYuh z!{McR6_6;FOM9=o<|Rj9Ptu9+^WWTzMN(c6;7@wF)KUNuvO;)p6g1)mv&?Qh-zVq7 z#8ZbttRNaE%^@B3@flf3sTn+}YCOY#xI^{D4NYZG{e<4?t(!@n91_R%<*mF{z150i z73g#E^gaMP8u75|*rA55Xn#>=ZPBqzisoaR$!)QOKYHF0ziv18^v5Ft=->N>JuTVO z)?r>PXZ!|*r631H1?xo&Qxkk8zKnlJP!*zWd;%cu-ymUCUnRviF)zu|H9@fmupS0r zw~no;h-Qneo*{};96)~n4UO2}o4R4dyw48K=s$kEwux!qt zsI3gp5yRQx${VI<(t}GTEqjN#EWAqb((6^9N%*AX;8bIS{#eA-Mxa)y-`ZEmvd+G$ zw0TcB51DYrujA1Y)U4@(6QZ&Z{clio*|$h^7|rXtV&uqc*V22{EZ2K`;1$B7qXrgR zSq`~Ra`_2nTOiy011`kEvbzUEqgwz68C?e#c1fSdINeFg4JzNO*eVEpC4?g7$NK4( z$!P^0X-l{U~20x8mxUgRkKA|ccXa~@D81g8Oadhn`=?$2nt`gPG*b|#R#kVK} z8xq7!I-TyDK8PZJIvCgzPlL4^2Oeo1z26cLxghI2&jkM9@EznNRm;mbUZqL1*TF08UcX%CALw4`jFYCNN{;LuYrh{om3N);2v zFwQYFB1_#p(OXdv83BCXx+S|oA2*i>mNXR*I-nu0Vf^oefXtl(1(ejYp3+-K^!@n&sAGk$h-=egMd=S_alsCC>X4_Cu*P} zU1nTCp!Yjpq>OgqVU7!0d?Z@8-Bb1}ox%t~Ul{q_%kC$%+$!`ZwaoK3^O|w%Fr+!` zmT?ar%lWvH?lIGdSPxbY251fPI_En%hdi&m-bz1%8&WxEN+07FdV^BUZb=xpNaNEw zmh#0Z*`oKnF|#i$SUon(Ie=5Kn8tI)Q3v|^d2G|A3~!7|0ZjUL9w#&iaRmVqsvGyr zF&PjlQx-$K)}Wo)?t93QKJc<=TwJeoy0h7Ya2ilaZ8B_MHGYAN(fu@j#5mVl%2@Qb zoeha)Zjy6PYxu&iw0^teTSEV}j0|^1uARIr&99mxfx*PS-XZG6q`?TEmj~K^4F0Q` z#))NU|KijWWe=W7K)HzaNnI?yVCd)y2sf#@d3I-Y23Y7)W0_3 zqWskfLd8L4>bWm>ZOLxOBPA+QRchTxP1ddpEzoy~GllDnHuKrEC`L@aAD1pO}4p%4+;mW<2cXU{GM#j_dwesIX2q zrG?&Jo!s;V(K|3ZnP9$?F6GvtG$)yoPK`!BbOJbkuN~^?u=MC2@TmSL&fSXTiiRqwm~)Df8*r zL+GQErr{Oc>N$;U-y$L3M}&_eh*H;seY4|H-|2q?N8sIG2EAV=(K6LlAUVwQ*Jt?T z!&b(K6Ae|D^w6zFNKR#?6AZuA8HzGaPXzI>jcY(C_1LQ1MY%6?xqwWHXag3s`i zbnvOTVA%(0JW~`yU5}(<=^-gA7YS<^VQgGG3rN}BR*mGl%aHp8+0M{R<@ibcX_ct3 zabXLUHO`Ztul6{H@8rbnfG_r|5O@Wv6=+F}D+KoX?x3&B43yQ_4WKZFL+dJLLy6 z!pPCijUCe;6n~SlPJ_reSXn9Mp1RxH!7scuap3YMVD)ATm}0^#zLyoku3aLjXcl8i zj4S^}SY_Ap;4vsaK3|PB4Zl+Q43J*U{7x8gJ^T3gPo!ejJ%bH&6=Vt>jiZO3JjvyG zADGyfpdVHHg&8EB&b$eI=)^R-|NWn3!~&~5(Yl)i)$z@udWn^67HnR1X}WZ-A>7ZU z<8PFr5Wod}Z2rL+bWTD^Hv-8JQa}BWLIm~2M=$K9glhA{)l)m%%;BgxtOD~61Z^ko ztcZLXte_6F!kvV3w$vsWMI26!3-x|K;<~U1b1&#bbytIewGndqgqZrf9y}( z(8%FZ7llXv%+Zz;gNht<#wa?i_`Wh|qrgz*YOf#6h2Tma4~eG#@)yT(;+bnFLP&y0 zU+M+IUhIm}N%sNdt0$Ng(Ld;+xf&+-{;LC)c@YO6F+i#GkWK|PbFPcv3jJN zW4dzeSL>$i1qs$hKcMi7rC?xCsaYQ=0gDI>?3ST(^FQzU%?8_*|2$ONh-DO=--<%ed zC3c4h_qy_w*7_4=hVum0bx_=J2(NBcXBnM6x1RZc&=v@BHp9rw%OSkreH6c#g#?%y z88bD|lSW8(o+T$@LYsT{UdetBJ)nyLgYqHyA`yv=Br}QGl?78+fH@nQe~(8I-0j7( zsrJufZY^9@5vXqt`&2lN=$EvcI0-gcWq5?Ynl5P`T3rjV@r6ik+7K^Rq=sfsHUq3Z zd7XXJy)9K16pCz~0~x`OxP5UJ@b4p+>n_Pvq4ol22kMj^pyoWne-&x_IOWINTzoHp z`nr*ohc0?cO40R%G5k4-U3>TxkqO`AzLmt=V9cwu;OomETEN%Y0Eyt)MI?!&t+hj% zEv=s%9k1AE(?bDOq<84Ex&6OiR!Sn8^b{YT&gTaVWglot?PIpFH=h-4aAq80RQa0K z{7a!hiO)HkPp{(D%zPc(mlMSpVN)*WXG_>Drhnw1jo;NAY@R&jSvxfv%wUOPVIi{m zn%$~CpG2aD@6#Qodq}-H2NrT_(OszBE!mWzBwES?)gQb)Q{BbJkNfQ5@sU}d6>o@yE12l`33LrCNfR1j1Qd`-XkZA zP0mpCHS3eQ_WAS6L$ZpCa{%{AD@v7TTyXEd?PGL@$$HD z=kE{~?$d7^S;rqds-RY=PAQtG_WGd)F3{}@VvzBdouGQ5_6^(Xbm$v)sC}4prff&9 ze+l8C1j*jfn5*Cxw5p}U3ht9i2iFJ9NY-ODz*)%Ca-L-R#gRnpKNv$>k=N!{ykD!` zq#sRUIS(_3E3a8k!92pHiGFZDcIsUBL{IZOV)327fICbyRi;?e{>l%!mPq(Clfi#j z2DE?ijHUItZo73Q%ScSm?#2)7V}{%o$@uYHSrNT-FyHAUi7^$Grm?%x2N-tE;Cn}h z7{)wRGlN-0KiVdS9q$4ndV$?b3#!72>~ob zMq?6A!Ib3sM{nwn(lc%T1b`>v6S6)s24DnBg1ues`k5W4h1p=uC}NRAQkKi&xec$2;<61XGtN9 zPAd~3E1HII0*^!KXwv;jtT@Mt#@l?lWD5Plp_xgtcpG!0xoM4}*HHBYcUFs^MUy3H zeuu-tA7~Zl0gE+8tx>>w->kVZZ9MTO9tz*vrL%c{Z>#yAw zGBZSl)4k(;9`=j<&X}$Im>NoJ_chwwmeZ&OyvgQH?WI%gn%1|nabuxm*jlPC>+#k{ zH1f(T=4p$7I1iKgJOCi!jF!VOLdGOllFu1;7m5M+3kJ`jVv(Zn&ML+N)}A&Q^Nh%j zpT4hl5X466^P9kDJN9Qxs|$>6I zVxVsliAhttH4rxM%yiIGF(zc?6h+(@`;@>AkqpfQrhLClWA^lJ3E@5$Li7!0zxvFq z(P2fLt$Y0MH2Rm_(*%_mV#izD*BV$8bb=w|A&Qe`RJv8^k%oiME+s0kFn3&q(7#_x z<<}siSrRY5K^}q*3_^9FDzYC&KDBpv?g1th%vl<!d%BN+K238*%sy?w6k!B0$9OJS3m z<1btM6(?NBcNqJV{kZsjo|T~$JTvJWi_F-E`~WUC_}Q+i=!quThx%Q0@RW;TcY1s) z11c~p46oMbw^FS~B2VD53yp+-;^TUM_gLaNR3Rr7#?R6e+;`3@D}-K)+wAA>kV&9? zo#4+`4508#X55^EWoGjue8%ZlF1PWpoRZzHaq&ootKn&{Z3+bU%n2RKo4AXBlSTtM!hBO)Ai8j$!-6uWg|nv6u?6)UD2bT5=ujYGP6+xol>Hrz zvY*75i==W3?_r%|RKjASUgB?KJ9}tdbNb-=QlEq6`;V)4-;;E08CIG8_p7QB8>Y#4 z-c%CeM3jdXw)Br{eI)J&81bo)%>U$>%AD|y@0|m%S>-A1U{23hWw8|O!d24uVLVJTzbJNMq zZ#y>)9257-0|BRF*txtuFP?iiITn7q%xdT^exSO*F%6?j+VKFk-J@}pzs*s6*r-yf zF3ktb9tX*d1edBOo_j~OkYp;9?P%mBhbjgkok(% z-x9b)EX!K~Eq$h`E~PB%86s~G0O@3Sb8s#xW}>eL2Kdh&@>pq!?Vh?uJL5euyr+Z-3Pc9f=Qt|m-S)Vnv**iL5;yr4()1f@pAA%VRttd%Y0Egfnyj0MTw?zQ-1Ds zbd0Z*bmBLDps$LqtW4f$9BS)QGGAHxRu9%xGmS%%0i&%mQ$5%X;uY>(sg|>*~sUj0oj0hKFuf zbUhp=Bg;6|vyVOTNQ3j5iK-&!^AY^jNkP2GHIb*QP#_5`&MealI`DvR1k9`N<=H;R z5Bo`#Fph=6JFZtN-ai+85VZ8!BL$1?x$freK+Dq|y|?=-7~MD|rEe#u+v|CI=i*L_ z9v_X?j|XUh%H6Aw4F|JM9b-A1lkl|=TLjfzbU;~YCRZ5k;`Kl<$tbd;3rZ{E09<)c zhypT4Fbf*Yd*xQAU4Zl5xl%>wWB2@;eS0SClrb#xnei0G3IP31Ye7VdWI40d*pz~5 zUyO* z$>}>C%IyBpFo_==Hz1(k;FJq*iNsbDhIgizm4dZFj&p>Q5F*|b9u&T3_-jn*4LyN+ zai|ysQnzlT{$Vl3Iu1l@9`gBG0p@4>orq+M0^mj?c~eFvqwe7x9T}izo)`4H=^n0H z@Z;JN4$2!gNq71Wv1E&PcRHZCb!8}>mPn78i{N0lPxGX&;j~WRs)9e|zgRIc^T9;P z+d+FWhON5_C(0FErx`&|({xCRU0DPoZ>A2QT@>7kv-_sgJ%8{+{s9a19S}}oa84XZ z;5K<)C_HS&Ttz*_x8-x+R0^ACQ-}bVZNDC-eh2Y*#_TV{OBCBb^dMx#K8n%h8NOD|wRrY$u zbEPO%pRpwRx6*k~gcThS{+3Uiin-vwT$Ih0F`(b}%}HKo&=Rc9!U0MeEJgc~_!!zV!NjxvQk>N%VG+h$8#>2ZC6?u*Vha!swR@$0V1h-W zQ%wUAyBhC~#1AN7q}N)Y6*FU08H4{N5M`Ch ztjvp0LTT;Lit!}ZGm#dQ&0!<}I&-7|?vTW9enCLw-@~9mIM_>PZBg{eBNW516KWJHz%s^ z7=ubdkfaTJaU#SWHElEyDI+KV(JQH8;8`nb0QJ01qTtrYNHR%wJ(vS|C-Ib-t*#it z53@7{PqQANLMs@>{n;G^G5p^}kjYA1ahsVWc+tn)QOM95AO};?x{bAPFiJSX1F;kn zXy!Uqv9azBVnMPH2-iaIb>W99DmC77i@D_d!=LMxRq!AD!L(oLbsu4M;wSJ(3&THrlK(83hn^J}=+F0t_8(9dRwfSCyc6V0DU9Jls2RP5W)|g` z95VH|`@rWYbyfJWf7BxU*(Gg_+tVv!0GE(qzHZr9hLvkEJcQfwQmd@qu~UkWRK?0W zH7jk;sIoy@HMJptQE@c2bnR_Ir}*HL|foP>V#qonRh~!oM;v4W9ob92oMxdd_4p#eBP3t ztq#*Gt{q^UPyzpAOAVvYj!U}V~qY;2N}fpGA}puOM$w9QZUkCe)STNVKTYc6`L zheerOxx2@~TM}c_P_cLf`EU?F1*+S8JI7JZc@wfBZMDej&;Zb>;d04dbsulck2WDz zP!!CqS#^-pNHD4&!z4VnSY!7%ZdF2IF#5q6jnL$SzA8#i49A{UNw9C}oE`&M6=@Kx z&6uZ*MsW*0<7mp?(6VyrhrOO`7R7Fpcxh=n?H=gQvMz)#J1QN+p9Wk|qyyvF0bq&a zpw*|P+Z>t?VF0$f*TLq4+piYIqPyazXdZ;s3PuHxDNC7il}ZIpCGStHMjvZ4BUp4}Ux{+;F}C&b|KUQPP) zz|%4XK&kkCSazJR1nYy>Zob+*)bIQ$EaiN~{zduD(&_wEUVA&9rUSHWyO57^0aFhr zA^14MiY4XSw?7P^ zR{IFcbhW<3V_;MP4jEsj4~y~@o+4OYkIXMbJH1_8$lTq{tT59~J7fEPzjsHN>D1_h zZx@LKKv%)WH{(lTnowRO#Y%uN<%_wk5BT4k+GjTRplc@d@;W5E@TeKoe1Oru*PeW5 zI|i@1&rMO@2AtUR*8(tIeF{*FPI9`yD}KNx--b1<*OV0zL4qg&w8S;ULz?34kAg8W z%Y+bBq~LIXKD%%h>P3hR)Mxy+8Bj3wNG#pLNf_e~G(}?Zh3D%j$U0eH6gYkv0FGy4 zSn~}x^J9JA0WaUyEMnBySU7`UkoF44#=tbKU$kz}}QKivCoe2 zklne781%&bOP=@X)QeXTi9=rDH|=DMK<0Lv8>!8V-lLyZHUUR{dFwn)!YG<;vx={6 zQ)LOQq1&`qz$P!;858jgQ1c9Y2)?T>h)HcgpmB*bztP0;UYfbZ;eAe{N)rXG>RDfF z+D|YChF<=v;9_ODrxyJ?ZrBH(hizy46<>;VGab9^D(JE;ua*&vEVg6gCN0XXnD z0vh|h*$5nIh>Br-2!{K&fBW2V-yGp;DgDmGb#ajGKEoKS60OO#&6g)*ec}x-!qtUG zzdXC7qu+TZqTpPCUlS+kYR*90BgrqsNXrgr^jR<(=OPsfo8RPpc|mzIMI4iX(Qal3 zO;Wc}E&bWv`NIaD8qiY$DvYN_45oIs`R!FnEZoDMKK)s=IPigr+^uc(Ga!&5L74@) z((F?v=NIvFKZ+Av3YxHA*Jpz!qm0S`!)C<%R;PcSt+8O`e($13ukDG zKUjNa)%0GldFv&0H0+O8p8<9S5C?XgWOCnX=?)1_ptRolb>GfGXAmTtpj{rcWr8;t zs&Al~J1dfwir0zli~cBlKY7^H$KO%g+obaOXX3goYiafGnK>W&P@mEAlX=yqks7~x z6|%xOC-={pwZ!K$ISLztVg03^IcQJ0F2F`S^LOyDMrKf^Ab|(0A5OdCLX@=ABS-Gv zTmvzo{;=j-E^d;^qS`I!3heo#z?E19uO?y^+T=;>^-NVT0!a@HMiH(==)<_I9=ysn zkO8G)qSB>E(6Ms}(bGm_G@y^!bqh z!GPcc&d}I}XPh*bA<}7uOQi9H21d*sNtu)+{%wof?OYDYPQTnoTds-DGMRT}a$?4J z{`vd15+rSn&54~sStPYvIDi-R1`Z(sOcNi2BelwCcF{c`K6JRJaeA!@=z;}>{)2o< z`ke+PiV(I(TW~qdd@U$36WqC}m{Ib4d`eppX_GCSxKA>4H7QCw=^Uzz0-cIGoizbj zVWG&TvT`)aWUkmG5E1U;LS73YX;A{WRsC*Z(xuhUo>FU%FiP=u!Y5&)1rI)E`lHzB zptJzh3OvQ$S&f8S#>Mww&_Gc+G7#lH8c$PLmBS?a{W}C4#mMC^{gU&VHE*lZqtWW* zP`HooYJS}gNc;2g*aT7{>!i0vd>jy3iaFYegUkat*ElI2#6?ZdeP`nuN!H?h7qrm^ z`@^KY3h$3b|XA?;ow%C_Ht)K~5Oc zEBCH|@nz9>>D#|SWsuzPn?syPh0G_BX=uEhTRf^WXrXR%!f(TgJHvA91@;F@{eaFU zK#G@NX4v;T!1&+U0;tdL&wP)@(QB3Y*?13Id-noPj#^UJxCC~4}d0Sb>Zh%xq7=O2jLINd^k zR9QjcarCi-jr8Vx@;J`<1?4xgWIX5M?8bh>TJY8@F)oaqzP060LIG=pzf2aUtKlSY zj*cWX{BW+*2BuKDB++DR7{DeFmgi|tK&0C=I{MW_!2@@P=fI!in!`fJNN?bfsl5Zc zd~}OzsHZNH43s_`iuP|BJ}|m-Dml5KjAmKt?j%E}rH;*N38&<>`#^dQq?VySJg5ke z>+6&dE%xlrGWb!k{Mo0jH1k>SI`ze5MGS77BH}|1-^Ha`Z=$pkfHtBa@7$+8O=Htx zFb+R-F2j%6f8hN%@4&Ld%7!}equ>L?zQ9U+svdrBoPWD-(7CF}SkOw*>T6<5W<~>< zlmg&Z^{RX4x6oz)am4HUmG+e`57r;R9q4(jUrZC_I6xRmx^g>l_^B>MDkyi0HrSw5 z!3_mx9lb8I1T;fdw?>18QfdF}gLIawK!E9Jk(5`0IGHb_p8frfNjwvW#hs6H`KN%Z zM<`fBGpW6$E15ve92sBuGMbefPQtUX)&5&34a7W`JObPPU@59QtO6|U)ajXY&tB9b zN#<%ljMQx)=(6$D_gI{`WgWhJ)fkje3ZFhyK^id$;>gV2*TIHE=|{sgYym%TofdO~ z`VBdyU|zXJzTUmvRAp5u{!jCD2GGgQL=NyLJyLfK<_RA_er*6qmz66E;jlSchd(w$ zVlSEiX-nVix`FSSg4^k_#}Z!pbIOM)8kXF{qNLf1^%Jp{dp6I^k0$mmuB0gVsa$?+*p95{+3fi0XD&KN@?24m*buQ zBn~sWmFt!S1e3YeonAKkHG2OQ5GqqK{?Luv@kd|1O7cOVI-fOw$6-kJCX598RDL}X zP-a(n9dKm9bM6n+?_JCXUr5xl!G{S{z(Eq738s;Pr3Ra}w=KIR*TX{z6R!nSLiPX$ zKR%5{($d4g*JKR|g9CO_K0Uy=6(C-aalTG5o4}*CTKtO)7KF$D0t+q zygyhyk^Yny92rAat$GHjgj?=y(MZkm!rhc+HI&SJy|mqW&>iKpArNaMqR%s;S{J;_4i&8EV+~|YLn;U0xgbmd9a#NGsR5rYK+F6<@8`^o9BZ;o_ zZ=kS!fdqznIoB!GPqkgSO?Dy0w*8ljo4Ri6yk5H!XfZ?-x3aJ&PRJIoJaiV&ze0)t z7eMI0TJ-t(=J~Kb6d2<>O7&)vo|wJZ^>=!Gv24#f4b2(#0m5Z8;r?oraf>?CHBqNk zrHyOQyhJh9sTBV?m$}Z%OT1!Ot(CgCJ-~oL(*=71 z4m69w1G(;*VDx1GX9h<%S&F}Brndr$1~b~2IolSA?q%XAJa7{$w=~sCY*iJP+mD3H zZ<9N*p*Mvt1eeJxy&$$;Ixg0>CLTPy17FySBnnOML1Mq@UisY&+@!&87UQnSrVi5)qI3tdKv!_#4vxa8-$nRLr<}HGsdSE^V_FRgb#UJks2I4- z>yWP-Udcp{*l>NAzl))QV6|<+q309u+2j zfIa+8N_+VNA>bHtQfiO?o6(c{=(S(v50c1rw)sKe>!FRq`+N|7X2M1k%2M^o-nJ{*t+H>)ZHIKO4Jd#ENZq|(F(9V_ zRyF*`h5f{3W%^84jQli?uzZBGe?t2R)(i9fg677zT67DUCVuff?=47;XB^;zwGYmabmFyl1bA+V?Y(` z037AMgG_WFlW$rv9d__wmh-Rr{`j@UZp(-8*^{2+*FHvHA!tX!$y!%e% zN)QN@g>~J z*|P9%N2RGN2rhgM^rJnK&VbJ^C<#K_cg5Pnx#8u^5&#g+t>szOB|;6X+6#nOplNL%Z}t`jvhPE zH%jjl2@!19%130aaOX(Sjn3!AJ_~Kvumn4{?o(d4X;dO=%HYAtE8Sl2BNQNwRUEik zM%`%`8pk=_SYu4wq%DK!oE%Jogy1s{uVibvfk^dmN?Z!7`c_3j#kTzR<;pJ$`6UGy z#Hq-!4l7raizIh4;@m(YD>A-c>_6Bmo_`QarZpySZ^ZrRtPN1LhWXwFQm;*K#p;lc z1*}g?@+rj!%{OxAdGRb)W6jSgWM8B?84|)5;mSUG>L)vVi?Emyq4k>rX#+#=Aeo^Z z{aM6=mx^I@X}Oh=q?~8$w+WUQ<&bky+$D=K6RBP7g=;@rNqbWlcz8%Buv8Z_>XfYO zlZ_peykCSW^2Sndcd_b6r?(Ka(?O`lqF8=suPo;Fa3BWT7eJ|N44P8Spqv+ZuNE?SSoZfLrOYU4;{DSdT8kcZeq~Mhe|v$;>n2lu_krQ=Wnf%{s_(H9M&=9G z16?-&QU|tda${endHacKE!0K9Ai zWoCb;K2>Z9;g)4$PtO8@m=R_H!kT7HnAcl(R3HLK7DBFLSkbJBR%~aGsplq{_o^3n z)0$elou|hE3k#H!HE;gSnRe(Xh=DY{lj`yWFh=ADwCVyU+n{7BD0E!>0tq~){z!rV6ViM$h#PI>QpeBKrU%LYj2xCfR@7`P3% zxYRH2qc!EB+A#JU?qn$NA#vsFnPN^{jUUKsR^+0H_YJff_nzUq;~`oul1Sj$NtB=! zL4N(*lt@tNCaoJG3gsp9Q*zyf~IgkdNRN2|c>zs)8>23p;cdK&0cl3mBO#r8ny zH~AV==rCcb3~lUceqWsQHzLjWLvwoGg&NfrRQal+3$aLl9(=lXP*7Y54>cpPX$JWG zDrEQi7ERZzKH~QGTRFh8HB}Mq6$dG+sNbJMU30PSL!9 zF*oLqpl^_e?H}9;z`GA#4ZPM>%B*<79hf-bnH+$$P+wF|&klt~I`YrjHGwUSEi8Z@ zi`h^NX)+p!pZT=+(8y0U1Q9-nQJDDD*&DcwKmxcJVt^#2s{rl=R}ec%(tiGP?TLjB_)9LiJl6%Q(_$+Jk1<9eME~5LuTX>upm#`BFBQC-&=mxoTf(@r3 zO1~i-X9Sd=HUba$NcYt|b_>I88Pw+UtI~kRO`qACLep62CnB0L?OW`l#>ZY5--pw% zuO|>Gf@{>Kkk?i_>D3C|1$c34daA}ZwyS`Uw|H)QY6lqJw~9Lkr!QROJ2};f3vyC` zq)?|{pZl8K`EjqZ}?s zcl4RrD6hNFOD3J}&FZ)?G@qyt|2An&S9}-gfjBT8!8mLuVc?`v4aJkak6ZvSo7gJy zMWb4{FRp)Oy;rZZ+`i>`-=AXZt(z)wc9WBI_Lj&wW6~WtgH47{-#l;CDOGnb%QkB+ zFT&(M#~4400N#e6n&_=gIr|8zMV5WxdpVaQB)Hzzu^stoVUOv#ty)#{&h6M{6w6 zLE>eq69Zfnq2aCm0z zkOIItP-YwUQL7(cDU_auPqpt0AiyZy8;9RVE>u=iQ)bx956Ol3tlXU!Rw7LX@I8fX z7TX`fte=<(sRv2NeCFqSiCSCe^hWu^jo;d7ANkssVU@o2gxU-SWTu9ypB&$qrLdar z16DVx;Ygndg%4s{?{hA#E5<}?p(tos1|=+N4GDW6;1pC`0)o|{EOHV@dm=wB9r&6Y zBw0=W;*h|)EZ&DdXH;M47qJsz&^Zx$ogL_);b3?64f>S#dCW!!%`pY%OO<3u__BJP zQ$Qv`-Lyd1%O$P5?Gt5wWqQ|n6o$EN^$_+Nos5=?P5SzMpQh4r^3L`;0^YB6*`}cN z=MuT_QuL+QT}d!?SZoNCxye)hYYi`9nK z@JY8gNa@|PWz!R%K!0!}J!ydb(%5KSvLx@YoxLK9iu9Wd;YB8+6`Uv$7QwJls6^8OV_73?X7>t`Om{P7&Iwan3!TIxXkh>ypwjPFohQrU}!u3!RB9wEXWFkhUHau zib5|TuXEw$+|S5a*spw&tjk4z%?#b<$Bs4@azVdpf~L>bA(5k?AaCb=FgfHy0@Y!G zc&+z*&7o+A$S8bf$6cZ|&-)oo1tD<;Nb{vm$~qcj)>6}HEm4BXm`&IZZA~DD&`{{F zn+K(jBH`d0i)5uJmzW+*VB&(_G+!Ld2$P>6w3BbIu*}(SvbLn90?0cJ#w&uC4`-gv z%ImuYrw;J<4lob}ah1QQJnxwc6@&@TyDv+d1i*?!yA`#KPDv}!baMXQj~-$21!>nt zWe`NGWF5BYOP?tt;1`mOFXwqvmWmvpM@uQNJX%#U`5J&he7LW~0mYG;4cuob#ifmq z2MAhruxXxD><61tzbHx3b`KHA*gin@Bhbh{5e%B2!Jw!&XjAA;ve~toBmNAtA5gmu zn&*H!t@OqzzN3i~+`uD2<)J8FD(w_XmBW{N;i5|DJMyrqLh_vCvvmQkPk_Ppq*~-h zO%(Ro7L;@0U+p5Av3nZ>@a3WWs6tL^M!jl!`E6TNR^K8l7 ziuI;4A!Z)h@0~uhns-Qzczvo!CK0iYTFJEd#TQ9e2Mxl?jIZ)mX)= zJ|xL>=pSGI>=)1=aWdnUpD z^yKOXbY|yvKO>M<-$J{`uNp5+^$XX2t=<6l^kO22atB^+mdn4D7$8$+tKIU)U}Y#?Mtjf!&2^6tAkY#AdVGqAA6K|0Bp(55?LJ->te>4du{5Xt4O zuT(axIr#jU@)kGx!>(VD6+mJ*{&3_|4t7c`yDAjewZCji^rAp^%|yE4VF^s? z4N1Rx13^bu0S<)afIM?ybAZP)2}Y$Kc{%S?eU}kK@W>0YK&ZbGUUlx3=dB?L0re%H&l+Ntb}Ud3CE)!# zXOPV)cOn=@aQW8DEv;Z_>`CxgQe*k*E;Fo?lhH41*~chF^v zDM<%>NfcckH(mSa=wZw1#+>y1QCp=0vC!8xAvvsybTSICMY~uoU-o<6tVV5EcI)4R z`+UHnrmVVAY;*#^k;jesx|8f+$U7)J!I-&eQ39~RHMe4 z#_`QvWO=Cc(9$$@$VM)s%iMmF{_Z9;S9B6=9MjPZZy8LaFjk2a^tq5e*`aFAfZidH z9`hkzFg(>2_=52G*#>!9^|R(riG=j^r)1q9O6+_yhF#)fyBv5a6i$2P(0=yVLjbj&m>8JjX@Wa zf+8;~V&aU(H!63G$pG7PSOknw=aRGO+aLYo zPE$w!9Fm|E9Fs^8^i&Z?p8>yD7pq>QAH`Qx5X8wde$=6`k<{adh$yU|KOk?koW}Te z@aOrs3AF6LCC$dF|ISTE9aixbvog&eOQP>C*3HrO4c4fH^O9Zct!0+62dgwup3;BP z9S4KgM)koQ`vr_gR4WW%jmHK>T83829&i)jsPAY~lLvrMvvhXeN257Co7AI;;e=cC zm=f=EPb<&s3xw2~p;BG{<_&=!8E8 zc4xfP)$oo&?W-g=MVPCA(yF7~Gy2U*P{@BDt7Xi&r?nacozXj>7{EEldP?$)_h5MQx z43K_kaVWX9*08bb;PK6HkS`*{7s!F32u6;DXa9i+fca%nUzb3BTj|wgMCSQRici*AY#3c%YI{Ix27~2PV_<|iiQ7#4a ze)#1VU^uvSrt^u0!)-*k=_#DHn8DzMYOnW>^z(4|lL*mCR>R4&70?-CvfxXtwm1nj z^OAC!CPAP}ZixLX5s6{_QUC$Mln004$d{Y5ykhXUO`H9A7*9H?bfOA2eIjCc$1;#! zA^1|7o@>rW7ojecKSfLb`EFiCEee+xy!vwtD>fk5Bg?ol1xQIw8ygl;$G6YoxX#0Z zwj{0UQ=t#1_cVpnK|i*h9Z`UN&WV~82>MZKIv46B7eZU?f} zlHPpu^5v6i0GVZbbbvsQc-`%1;EXrX0Ka0-s^5&dG!^&2n;v4!kI;U71bIg~v*7%wXg}@vlu+vz zisMRtY4?JVDfhUW9lQ9I$_dbY4dc744ry@TC%_{0Ghd6OLOcjq^A*E4h65+{p?H*r z1Bqub=I`@JwwtKVmtE4{!%|4E8@}4}e7BS1qM?kFy@w2IYTw5-(*5?7hDD>nEymZ~ zJQpqFV`T^5fRgD<4^b@LfVh$fWw8i~lkoXP7&)IKj7@Q`!0Ob`P zcD#Z-$FHs%TQS0DyAeG`)4d%-t19{~1Jj9s5`}2^43ClQkhp4B+m_Lg^0)(s3xtb~x=W7`1RSs&M92>SNLhWeVIYYB7YKu}4CVo`^*v2C6;hFXWl^&1Zm4fwvR`u!c!I#Zh0B@>+pX)?o zu$-Q{nt8=B*`l1itVXZFiczlat^q`wz`u!@Gj^7lh zb>*iMbt0~3#0}ACgJ_^UU#jYXKHo>RiRBN*A!>v#W>tCi7Di;7e?Gleo`5`Ur@id9UH}H(lzUvrraF(*&f7_Ivz6I zO_>#LMUz{XeP8>3a^{RFn5%%NT_vFAzD@*BK@;6&*8*d48f{$JKUg3UcuwJ?NZO&L ze%fJ!L9D*kiyxAL6!))Em7E%@IjTRjT%U-CW^u(dkD2!0g!^-SR*96N43AEL1f(`x z;I%m|`{8KyAOaQpfKUAazxxZR@zDYkYeWyFU zR-y^hwu4y{XXAW?_bq%rHrJZ`ucP$zpoZ&Nn_4N6A!BD%EVqJOCsrb{1n+7e#ee&f z)#3$Kb!Gr|9Dc{(SH7~azo@(Fl5A3AU9^b8mbRbTEJ2(7Ega++A{9j|t5^XTh3Czb zs76*jAYJa7`P`lLzy-l>byl(UCe3u2;E;_D_8w4A(^1kErcMLs^~cN<@)hhe-9nn- z=@Dc|5BZHhdheK#>2$G+zJl-Dzd?uIVMESrUHc#p*@MgjH1L}a$7aqt65P)6uGdQa zbDE;(R*RgbS!1uZ8kUdOaP?yd#*eojlO#p*0+(LzgGO67Kn_@+R$^8zQjq1c_9s=S zMvT!P@mTgDYpt0C@&2q)sUKPR=ls1PQ-Z7qq6Ri@bEWt{^?aV4l(8*O$Xh#FHBLPz zGpim)lZMh`cn~#SUfDOJi|#Wr^>t^{{)w?3r}u@^3RhQaLb7m2BsUxeM}HK4WsjdH zHc}Aw4o5G|u-#Qp(ArQ;_iS>F3mVUKK&1C+4VH%<`8Mx8aBBEbtF1_#`1!t&B;Max z1a904D}epAzl{k_D1gN=kuxaR1Jl)OCjfsZ$U}}}YAZ;8rUqB9WZ3w7>-db_5P=<| zjsj^(wP!-DrmuvZUI16BBUX$~7Vi02a}kxV|vB+h58WOOl$V z^kok4&!u-WkXrXU_-dtlpEkod_77+5PV2y`9baHgY+uo5@rXJ?L_un+EK2mLO$y-~ zx0Di{8WpOxop1n#v>_nGQo5&6zz%x$naBqY0}SzP(n+MKj6az4I9eK* z_&W7K<$u4U)eUQvt^?r>hUp((WC!2;`Vi|Cz@Sp}EJCs=foq@Ua|BQs9jQTDNjpV@ zmLN@vWQR3TPSV-!Hl58PM_lV<)@M+(aiL0geJh! zji2=nAW<8Gi)-g@h!Ng%GRE`?9KeSlmOA|!2M|cR4qi!0N^*?Z7bgdI#_rF@=J4{)I%ByWTTR% z`IEcyqNmh?rmM3*!7`Y|`833qOO1%WHwJ>E75IO&tElOe13 zVIOnLCEPfUtlz{8br)EL@mU8}p9#lRC$T^zMLVf3E6!J8jv(p38Fx$EKz>UKI%L7A}ri7k-Q)i){c*ee_uGQ(qZpumxJ z*nnodIs*Q&%U}s%@-qQ)6j!abDxFQtFVFDhY9*$8BH@=3fajUPG!?gbq&g;V$CA?W z@QLh~&_3VLcH0f#K>DZO-blm%3)75-H|*Ht7)K|b`tG%^e9*YLq$~(-e?F1gd+2#3 z5p7@StHWv0|F{$Gyl(dOWh{dspv%B&IuLJTosd5!-L-cIg$(t1+R>4S-sprcvyl%& zLZv=6M?t5%CI%E8p@k*4VEnEfk*(uxqN!y)u^#bBP)Msb0A$AgNR>M23+NNfpirT- z-`GixuOKBW&dT2ImF)Aa{nDjV9Q3q6rx?3kPyjVH>0&Zed1} zS)mSN^2c%$Rd1<-*Jy2lot_1)iHJV2JHQCu-0a4{f|Tut((mhM6q=aD@s)J2J^VnW z=TAfa)g%!__z?PHG&sSN8&3+^DP8cB@82O+l~|NLe8Pp2FGs`aaB9E=t&+fTQJVdf zKNqI+W4Sw!q?L&Yeh^W}0Y+;0sdmrg<ssuAC{39)YI$(uu=avE*m>y?2s(ulFj;{jI^g%p0)5mwUqyk3df^(W z4^Ka2T3>c$i6R^?+w?gH+d@Bf^+N0NNcQBSOh$rvOVE`$BVQ@y9!_@Qa<5lr{b)cA z8q8+<%-8f!(=$%ffmW##PHO#e)UYCdZcrqfXb{2FF)d%y!s%7Fxg0yN9vzRrFZWAz(t?*0B9f$`UFn-rHO@~}wEzo=M_QgeWW-^dW zTc8AhSOxX{-VWLnB9$3al$Q))-FNJx_rj4me*%*iy4LL6rFUQV6#FSsSta_Nj zF3@hthm46=vJ2*@5Jy_0Bv?FIN7o29<)HxAlYVu0S>`Zkyi;Rtl&Xr2*Kv%EjwwH$ zV+#@Mi@?8~T)CS&7ZKix*La+Qu&d|9NXT?c{EvxbH+u$eaMV>G>?RVh4`9G=2Hgb; zSX^P|2^M|!tChptDp=#|TpN=lF z!hDKaq4DDKi)11lE`YRFe=m>(0$zE(M_^hlfQAYn0}lfv4}j<}=li~U<)+hF)us50~on|@xvN33zqs>br)ZSwmHWghmya*c~;tC$2&}c6~k;+s0a9aE6hZbwZN{z43CL+Q%+wejHgK-~*c(R>N2J^J{ z13|^QhOCE3Quu*?t_{f)5U8h#l_L$S&U$-*8VB~v(~^&Fg=P$<&kd%nTD-(s#slz- z`@^iy+AbN43iZ;Looz~Ogb%XXEVjhL3hh5S!0GFpmIh|vS3OC!;2?(_9g(0$8+g`B z{`2bS%{LGh;-412#C~y!I>m9zWpCkAZ)Gtuw7t2Ehr0FaPh$axDyPn6hv3(2?K;1@ zF*x>=44FdVj2P0;3dOI~u}M7OtLE=MNBH2=KLsLiq-~Za zBDcoV(a|@s*V-*bWMoJOqLz0i1bO$Cl*kIJPV=?{G`1jmHnl$m2%6M5jIehF^p~a* zTpJ}{_pYRT(?2(3Vkd(6^(-vg%NTTP!|QJE-C5D~U{lP9Y5wklJQ2I!E2{v_{>ed*SP3rL?~ z^nvxT{lZo8DPei1my^=93W{JK2z<_?Hd)_7pX)K7^GQGZjQOWF1;6FWK;;Wc2gnZw zksznBDir&`^l|r5LXF=Q7^UfBz>6X>y1s+kD)OobdioFyVEgkeIxu88IheaCcZDp* z+uEA|%-5~V7pYKw+gpMouK+&Ncwk1^OT{Kv$|Y~U%U_O8?aomA2KejDlFBw4ot3Cu zor^X9!b4oHvTrHL`055(E3vKLCRo%ffVu@~E@w2*qZ`*{+Xe|W|1JSjLjwDAUue~X zgbImujlN{q#8sOYG(mM~#oNDTo$w=Woar6>In+(acg48w%Eo*N4dEFz5eaaBmuCi1 zN}7`RsP5Fk?PjAv(E!#|J_3K|#46ih5V_EmlnW{)Ln#HKY8R9#DVub^VetE#DH579 zs5n8q8l@l?C{i;^1QEb5&j{q#?+v@T&G@6sz{Ua<_}MW_1k95&3(1c z$f0@JJx#$+9H6H!cI=UL1$6SqHClIcVkHe;LvkM14ceVjMs*kGR%VWC(hqRGHMWtb z&MMkX*G_JH)%x)ZWaw}%g1Wkm-P-F2yMOlLXKok=uXe+h`bV*LvgWJ__YQ*LOypXfFKT=_b>sd@-@u0b$Wbd$}8EvR| zD*O6N9Zw7)`B8vOHp(@55UfQo!i}}CMGKyBn8$vIC;7#?_t7fDTa+Fk(Sr1Q5Xq(M zWx}f?)4cUW`woSQ9y(f85-$}~c3~dZra{ToG9?;~heXV~Csu*z1f*Zbk+xPk!zIrP zSM%xjHxg;)nPNyya3}c2hpnk<(b&iwam2mL%4iKSruyj;b(Jl8r%}`!gaTW^w}Q~W zloQJw8&Arvz3ET8p(b25X01$|h~nDp;sm%Up1a}W{()h>6Z`Oa z#Ex>$=Q&RIG;$eO_$9sG;Ev2w*jM{H|hO*{qf8z`~W=Y&MzvdTzBX! zKSf37#n64$k$K1LIlGxSklqJX_CYzCR~$3Q8M5CsSjADW8I&u}baF0s?Vt_wBjfpM z`CZy8+QCOmLhzfTrg#(((`%iOfeecg_zq})>ogVV1z=9Yd-Mj`7{1gMiyt&#Z$H6; z3Z?SPPm3WkSA3}TYa~>68>2VoR0kQOi#K+W5V=ZzZ>#*0f#>*XYV-}QgPJ))j00G& z4%rwZFeeALjKeb=8%xcJl6)jE3VS-(H9DN$6MD?ig(S#js>_! zqPdnJIIY8|wjlIusBqq{YHt&E0$H7tdNGO$gR*PmZt z^sj&a=l4&`+hF_?`M>f1`1A4)9fqj?&-CA~|Iq*b%>Oz4_h+8Jm-An@eS16Mf4Bbsm-ElFE&i<;{2$Q!fdAvqR=&^w@qc#J``}~Iv{ns1xcF5mb z_|Ih@-i;}ba+-_cuYVk6zRk-&%c^=O{@3RJuRZklb3N*R|6l(PP)i30KM}IVR__1+ zO+NtuP)h>@6aWYa2mp76i%S3i0000000000000~S004JrZ){~kZ)AB*Z)9a`FG^u! zZ(?a-ZgVbkc4xHuR}b?*k|l`#l|t>y4%pT7ZnkR{^hOlD=-%kP_x$yjU$vQ@xwE*q zz^nu*Nh+C<85t4h97+H3e_ws~mm^=+dK~`#SNNZyzy2j3(lM)t^6!8Bx#Hibzy9C< z?O)cT{L5KqTlM$9qR>Cn6oU2rU;k3&by;oy!=Eg8g_tU@q+iYLsfBODy*}Jz(e&oZLW&hFX9WPJg>F2K{>j{pI;z zukOgxZCw7A7sY$v9B+rz=_k@_$HzZ3dD zLic~J9?8CK|BKqu*IT~)$Mt`lb@ki){jbRXx98#iMdkW`-ip5=F#b<}gQ-9JQTK6~ z|3@u;z5mz0OiRAz%aJqd^!nJ2w|e~dh5h^Jf4aS8`t{Y@{;8|~wOaDx?|;qWE&i|n z7!sU@^8ZTWH7Brtsy)`QPv|3+BlA9V=T`~8@;xf7_f3X4CVX37gE!#wrvn$TiXVL6 zBNY78A3SBKUOwu3Trxr@Se}gy#zkjj+1kkf__%JE!(rg%O_qvMrln{@ETFKWAbcHW z9`$?~KWxpg9xJen=yEozb-4Ze3ASS?PNn!F>kFy_S7n%WnLa(biWMqY>`k=dKMyWe zvh4;OMe;nENn+lQV<)F&$$@&c@nhRq=6p9-V0hlOEf}jj1Y;zre@zlS!i3QXFp92= ztQNB@!JP1qmjo+|12t%acv?cJ)&Q2RXO^7>6cV^`<>WL!cmd2BMIft>jhU;2l%>X7 z$`J}u@s$KZzAYtg<+d-%nyI4fq*rO`))FX__{;eGPd!FP*fPfMobQLdlPQiqRO#V5 zNlhHUg}oNBV~auz|7rep+(3LgGa~LsBC=a8p&4KRx43Ztb7b&Io{wfLAr(_uaix>5OF-s}yPKeVGR<=%n6E6*&Ut^)I%rt^( zwf+&vjncJV83c&HVm?axcP4OMk_&E#TX{Kev%@s40<}sBPGFd^D#h|`TUPLdMt;$ch$AGy1NeABoW^Fp!ll>n?!cq z4WcAd7)@?CW_DuaLe|(r)!aAwAjH}1@6y#&h2|a)=#cUJTbkLArnUWka9Ac%A;tucN>mSeiM*~*ivJvp<3$HrUiK&b3!#wEWn~cw3u(*pJBgWUga|jt zHqCdXSeTmH1=}DUbI7vxnI0{MM%`Q@>8zzcH7{f&&=xTxf$uK3!k#NK9g`$yzd$@) zIg<~tpu=TVYGVBLOzf36O0c7a`z=HXFi(Gok2Su*_KqiqrZv(iGkpwZ8a<}qFsnb+ zb~~l0N%`>G^C$WO#G+3=+_2x;y_pQg5Me2Wm>f6Y=Hb33($6BVLU!&Q~f_>b)~r-?$E<~UIhG-X7U zPh(~fm=YbR4J-d$G1)jz!C`5IwmzY3G5Cm;BzQFY+bbVYF3m1rbfjRC z1kgwxk4W2}e`+l^JZQ{3Z5B2`AaCI3X+tsjLp)G7WC0^x6l+fe;&N6Md4E#MN@z%~ z%LG0Lz{>8j!DLH9&>5cm&fvPyUJie z62C7P8|tXhc zDK1-wW!;n?Y@mhV{a~Ji`l{u&rX7Ml25W=A4y(S=R>iB@^DX6 zT0OiFm!EP$nzEGLA)&=>DK$JjzhJGf;Ppc4{gnSw^j)2AoW&=FEVR*< z9ATd17ZjvvIaj9JX|bnCuTFzObb{NN)HhMATwMmgoUyaUn436f5#wH-(Rg3-W2uM? zsjUui3;!4a3}R1{GC^866fO;A6v&J~8o@|?-{55v6cpqrEZ8UpAtXh^Qnr(xmo`kE zCcNO_fhrI)B*AA|I8~gbi<$A`{Js3ziIHx5*m&wv-Y1|Vpa`NJ20x7T!Ij8_e)$Sy z^j#dI=rR*O6HQyLcaV?GSRK>9!0C%hqxvi+-geL~eLKA2a1pev%97gNRs}LR#ZTY! zQ+PzCsR&ShZYn%Ui>}4n1sllq1q6%K73Fbpx(-@H9U^i^ox%=_d7hcuD$ZgbvsKC9 z1KBAhS{imD3%W59{{iV{`F+GrM(7ZHQ1)i;vdBZ)JU`NK*W#3=G6Z*g&({JvU=}2| zp(Sa|V^I7~<7|(6>sk!MWT&uMAz>kX6ubJAWP;|{M-;KREC7q%28$nSnQSjN!IVMO zF<%t0S>-&`*i+%9R^|(c% zx6&wywW&$*J5#I5?F`redYEntAZA?gB+!xuP`06Z}5J9>8#iI39Hx+u>NAppOXKI+PFL_Ne~PFpm(q})Q7~WZNXCz^KE|A9|(2SA-VCb zBU`%kWlveqEO*U4GIVBgcrPuwyTQRNFRga$#ns4{mXJDtkbAB;)KPBW zPP7%%_~3@ZC(=Dpdr7SN*$2>pEmK)-#wk}?l62N;WvnUPw90VF zd#G$$w3Z6iH%|Dl%VuQ-bujRKyD#31sgp*LK?XQKw{oG+u4@ZoY-KDodEIjEuY4KUJ(E7}>hNs6vlsdQJluj~f1w+lz#<#pic>?2HTvFeWmJ$oyH~(S^NF(X1 zd~eL8(_g_)Q9ku4nIIl^rENfh=J~j0xbP}Sgr6Vq<4d2^4Pqh=JfKM>DWIWp5I5^m zj#z#D0AA{$$SeYonk`Ci9780<>KdJCzprC`+{ieI@>zs86HD9JsC%__8j00xQ`-@6 zl@nya>(lnN_yjSR+j5~#%)t@_MwWq2b1yTgN^%9*4oGaxf+IA)E?nwL?DUxyaBDWv zsN5^_JZf}ei*%w{=tCg%MfLzIaEVva24URs`|*hz8_%nI++HT?yx{Kxfi?3m);eUh z1U5#v_zvR7?@7}M$R_=PTh~`RFMh@~n8F=mq8IhIj$6~dYrL0YVNzvdTxF9eeh~>< zLbaa83cbf}emp0F0I{q8xO^ET=Tm9^^T$h=>;%c=G>-bA9x3-hM~1zEdUV|8{sFr zk$1QLM5d&@Xm^JdkW+@VK^~weJAKg|05C*k6lu_W_=8dUk8n6^_I~wpIw}AL?Mnjz zM{_up1B9@25MAjFNMG|JN+RP`G>*W4HHVt6JO)U=8SftadJ;TPQnJk7r@_;w zo)qFbb`{j}dzHu!3p%Kyzeqy(v&CEc34>%;=)_>lz>$SSDz{&@GLWl@>LOX*!^1i z%dE4tR!}7=lk^q!0+*F~Bhz80bQ)zi`{^Li1-a(C*+y2Ef;)+GwH&ZI{`%r*e!c6r z-)tB?kPQ;T1UR@P$^pHjuy((0sk0)UJBsZNQED|7^?pbbAdtk><0==MMuH!Hg@TY6 zMJOd{IBj3%#v#2TzQ&}SLWX#n5EdQ^cD;4i*=`dX@uO}YQamb5q50ph~Xil+@kq}X6$?dkVk{BB;-%SaZ61IjzDBN z5&jX_2|);?nYp;pAOGuGaZI01O~yVprXP)r^TRaVp7zF?YFO12PENmrYLu)@-}*#< z0c6^8YZqAw@~^`+nW{~WE07)g7!+p*=2QiL%Bq{5>Z)k^%C-QBVz~^a$TGK=6i5#L zJ;8U=kPFj(0@mZDs#7#vHKn<#dOZwZcT7HBD8kTF$*}zEX?-`mH1YVN;z@9>J`qE4 z5(PpB?iN^r@cv#{9cD`k#00dp!Us$NF3!tT&h`}5jU83b&iKr+iC?yiPUL&lFc&C8am z_lo*!34}{0)mmMe=4N4Ay}5{0fD6#rqoKpg$BhOFYHRCbOp8&KGiWN9MJynBbuPjK2~fZMECqxW z%N5&W3R@!jy+S5#wT zGSP&jW8QgFbp(7sFf;U>x<9{Wc!!{ky;I~v$+ZJh+c~FTG9-3yVgc2wTl7rkUX3Tn ziuZ7^mFNj*Ncb=27AFyOGUb_84Fq0SzfvbuuP0vKmJx1zuXztuF+1t-($E6?6Pyxl z^Id_*5WAtFHpeEbwQ9Q))ps;ZsjOmo3DMwq>7%JC<_n1DmiTG6yJkUt%w+193kWK$ zsD6(&f^?=DM?J6J%FI9dh$=la+qyh7tmuqU<&ZsAQaa_;pSwCjU1e;L|2Yj2*!yg2 zkqye@GU$SrtExocN}g;vL29&3Tc>sb*?V6>_?rP|@k2PPqA5XUTGthOih022(Vh?v z@~jnNaZ)%^yNi?X~B2jil@J^?&!C) zn2?k6gJQ-vq;4-pF@6M!%_NexlY}}Yw2X6`fqO2+xZC9W_DhWOUAOqb8XPHkdjDUyVRZ0kNDY|}RxeW%} z_kxmM!E@ax^vj-(>e4pqX75bGTV zk&qqsi;bpAz|u+I&V2ypqUl_vGxGj?cD(yJj^+mMi<@=HXLS#xu)5K#*Br$?vi=L z+d2i$CU5xCwp4JieicTf&QAEzXf5USB4v7FLzjbzK?zH&o{BG}- zsZoLGyDBOKX$O6sQ39VP%HEw`Jr-(3I8w@V&g!?`&q($N@d>Di;A;m{!Qyv!XF2`t z`}YijoDy%W$z0rIUygp)=n^UH2BTk(u{Ninfc{m&I%qa1WU4$ za24K^4Aa~75z(-GEBTCRQ^$UY=6i~l77#UbvY%Q7-HrV0K+=gu=G!0g1L4*;NuDd6 z3@kVmv(L`O3{c(izT9)7o;kJ}mht{-o=OtVX2x0)bc%7>rO5r}{V@6l!a(wa!Lsxu zybjRDUmt16&A zN_T8%<1MdW!&%NGFo0i6quv`TEEqjpcE0=>+bo%R6!}xQg@6gvWvWh)%usvSY`^7; zyIT@2JzZs`RRY13rK!ju^U6@OBA1-GDGLeCtUUe7%6z|Vy^b6V9mu{Z3=2wDWkBvX zWZpzY$XxWl)D?>T*I74fSI}znkWk$1l3^No56?Yb;o6Fw&RhTnG`P z1`2~B?E1AFq;0C_w~oe@D)!$)JQg$Zon6t>awg%ac7NaHxxUIwNwt?V2m`Ceeck#P zyG+`e-g>@T$!B3YX;&Y2_AA z`Ci)}y&t=FUtg${u8mk8q?T+xG($ZjUS_RR9lL(nmwkEoXL|(|fd3ryHaM^}tYlz_ z95y5ayF|zmPhQ*J8d7|ZAvY~>3d`O+qupFO8>XR$4j={)HNUT<--yldCEc0|wU1i= z)N8hZJGca_FsuC)S@M{DE_B4QZ;D>zQ)$JenAm4+BM>71i za0>iliYeQT*jj~1={Ravl16N%(#Cm%f!`+MTnzYDm~R`{oMpM?D@Aj~tpFm!d#-KO zYHpHwC)<#i#Shj`dj&osGt?s7p%6C|Vr1!GZo^SkOwU-d&~RK{qe;ufQYNW>;zN`` z9IeEBoIKDbXk85~Ig{(LL~_wN^$`u z7Anvi;TvkJBpv-wV_}kaksiDDqShFLcfO5sra+O~dnLF$^ zc+Qe52?RX>>&|!pr5ZB)(Xw9ojA8=RJi0q(VZVE)ea9<2BDIC);!o+sIg6pihtqbU zQsj_By^vG6m(d)IU=rhF89qf3JlXvo@*sOK2Dz2k8)!65bTE7n-18%?zBiv6ejiC> z)kV2}e@)~Xf?#bJ7j=U1PIm6%U@j@k7P*Z1-lVpq@&GS*jC$UXjuL(*jt8BC{Kj}T zv~pCVmYQz8=#osP47PWjiOSjc*+Bqmy3{xN#!SqonPkxn8GDZ{(tg+#u!Wja-M^sI zYUsk;Q+!PWxrklzE~3&k82adc@B`l=TmHG^cg;jlZk9hdk=5jf(N^+!uSDb#low1? z1d+;iX6#c4ao6C3zEr)$h40p3=qhdZC;JuBI3)Muu~$^h|LcD|`mp-C9V=o6R^n_# zv;K6d(pA0sK&4W$Bho9?we?L#Urr~UWjI-;1(XLO5s=%D(TRRv7P>O9x3Hu1qQVzP z-iho@MoN(;q`X`!^zEp|4QFP0GabgTpPKr?V@V)R(FWaf<>g&D$ts z16!q?-~50; z>ufQXQ$eFz!HaURqpKV6@)?f&f>oGR&u43%leHiYW0N+D@Pv2<{ngZ?TUw6`%|<$} zNq16<6ycGdb88I*a|6DqK#YgQ_`ItI_7Onpxcm{w7{BT8I4ZJL&?(8kijH7N{@x8< zr>S4iXE3@e^(E5K?e%YM_KytaGqeBx`5L00&z`fI1_+2nC~0f9bF&nZY20JRW7%fo z zeSXc8nTyuE(&grzGqWA1kQs>V=nP&Wecw$<^jNez2|4`U0nysc!%s=$bm-b>((?-D z;0hZ^$aiDF3+V{F8yKw5c=qdtI(86gg)l;cGTd&m*(>8*XQmhlB0G>M#rmsZt3uXg zlL8-uCah&*MWb-PYD**h(?r?yGpZ7)+DsO9*;OMX`IXD()z!Lw)y6uwea3CeO?_1O zYtLMp_nmn#Fe^f4S^K&xR9{0iQxw`h{z&c9H;!N zs?3v0R8VevKFSA}Pj<&pp$2@l94O~;f5s-avwF{oz;F=+iR&I3E%}pYB#CM$CZOhg z<-iPXX3C|*`PoQ93lxgtViK$y>TI;Y*>JN=6<#WU6nepnf3!LG>K8vZJu1-eS?ebc zp%kXx4<_N;r{9IAp~fSA&u+wdNtfDo|F$l8@wYq*-0`{_p{E+yz`hrfiWfKJHFt`xcm6P6RLk`A=xqv$1u*%W<6nm_@~E0 zSn$=0$}X}ioSA#jg2xT7Essdv?~9$vtaX-2!4wEcOe=NO&1CV_=p6E`X8`&=@Yr`? z-I)5I?K)b6s!y4DbV-i@(+!0~s8W~7Q%p5JI&hT<6|inv-iTcJ8xKJjA}0};`?CTS zW`exog#rE2epZEpFdNbzB2TpO**SM$EF2tmZoD+jGHN;Hs1GzJn{OdZw}B3w0xm(l zMKfJ)GFaKbrbf+f!(gOWQ>aISMgR_{_J-0)+1Y{?lhyOP7I*s4Kipn#l+*Fn-6ynO zWOIyykfij5e3_|ZiuJEg>#g5@i<>L`16jW$`p5yQpwR0)_9cQv(a$DBf_K+M)Ep|o zdVNXfgVA}q<>q1Jjf%+V4gUsnvzf3Itj}oznRS>k*}&tV7)$ zbvHF+l4~;6O#opqJ`(W)kp%#7r)+f-`jri`P#6pQYI<>FVFi78m*4a5^y9(9`p|3r zU5Jf|7<8x0jzg3VrGfO56PL#%+C*-o@5#iToWTM+KqY1@q`4*~LFQDD123m`UK17! zol&{Lw`O&7$;i&*0*=4{KSlqR-_pWpgB|Rr`zp%MR(MA=tNz1Qt>w_}Oa+Mo&lV8I zzrE7RjS~|vtD>sLG~ULSyS;HvD!0?;n9b&do<7Yb+>tsu@7W*N z-l+|m#z(C0biRuRiX8w$K)kag?z`_CGS!FuEbMHLV7%5&f7Qoya)FU6s(re!$H<7P-7!lefPOC;@W@CtJ9#5 z)1s&cj;#oXx^g|YPY>l9cntC3I}va>IVoQaKG2-ze30rORo-5rs@2-bDAq28#1KiY z+rKr`$)`?1#e|uu-&$DIqID1jO!55w;Wex+ zw8CC_1xaDW*cC4|R~X#$!exPU;|NK7G5)2j0Av60;@o^x z*H+$0Um3kv4j{hKpqcHh2K}!(zhhkJO{{ZTqMD*)f5}1n9cIAoeZLIuZH=s)wtk5< zg4e->mo#fZ%W7IASigMwp|x}W!U-;-Fii;`mk!NYpCL4r<>N6VJl;9)cfW+8#K2)| zcFC}@=%KN0IkmFMM^HRl^xH44ozo+s9gXfuJ3&%w^U$|7_pseb@xfUVz{8BuSb6nl z9aUyHgL9htbq83f+*H~^>?lCZ3ZZZtXoEE;LZMML6_4~Ky()Giw$X(dL3D%PU#7(G1ELa8}+j`v(5WpR%G~?*G*4w((6(@?xO~8-!eR<6}lysw{ zYmg6J7jzea687GHqIgn?kW2{Uxe^U___?5`*!N|kT!&WFW@zucsXjhVA=JO7SGupm zAT>#pb=EO3D2a)qXp(uNK@=ho^eR-UG$Bu-5RX)kxLQU14I@`n;-^raAM5*Tf5RW+ zvGXlL?pFf-Vn=&a`tY%D^Gq0mN{yJc-D5c}QV#nbkI_zDE^y6}X7Uqx2iJgSP?3>r zvOmlb8yFUf1J)#Zw{p z+0a_b@0qc~Ge%0`A;)#P`S2Mh-n_ODaYH{+6gxRd+GvGTiY1M|)z+t10qPr~ zB@~*4%$kCPm{<_%de@D1ia}N`jh&V-)fXKw1$j8VJtOt*iqKz1szMCViy@qlNFk(n zy?flID2~x4ZIA-e2{k_q8HNuy&O{Y_o89c4XuXR#<4P%_R9hLhFd$Uai8J=sCb*Nk zI#DU{3u3Kqqer@v8o9eu{%r>C`cQy`mr;*fInyPB5lJgA^hxM{_Bnh6dA!L4$;@24 zZNk35y5`eg!Iei5@v(%dUhvk<<>k{3v*mW84N4G7R9^ZR{>tU2Y*6l%DUvGVotq7X z%HWtxAo%E<@c`v2dV+PbAMq;voM-`6^6NRg>w1fJfo}^(_H&!VQ|=dLbZy)N-6kEK z^+M}KFLHG2T1YSi13Q@;{D}BtQ(eBM0oQIAm*CP*-F(juI%W8>fM3BgV&geIJiLf* zx7XyI>cW+DiTFY&0cQ+qpd~Rb8dZGWDDUPAaWY1$mD;ZUEqF=-P}xZGCZZ z1LQRTmP%q;UpL$f8ir;UiUX=FXubu@X4I>{56mRWdF(rw|AEFZY=(Gc4i4Zm}n*Gj#-<~t)fp~6W6E4DzYYMZqD z6kEP1z+n_h-s`JDmbhSpsaL(GtVFAS|YImyc&rQP3S8+N zBc>alrAE1hzYZ@tU+M_I$fOj%GzzkoOA!Dw<%$<0jCOR=J7knS>CUSfsWdqNAC6wt zbAE@>-d%yYErA zZfZl#pXitzhldIx%#Jwh){rv$a4JLscRq?E+`kRL({>}q0f$Wu*DCIXauQwx^;`z% zjD_f>>02DHSrWRSsU^UiAS*$CLU3cW7$?1AF2KP?*(0T-xs|{3)lc=0U<*SQ2N7gtBFhe5x(SeD5aXDQo zGEMvh8T|4EP4a-INhix{&k@MI5RQC<5N(zerxTi(xSxJb(M$h~Y#hG3Ji3Q7lW#;&+Wt9d-Ck5Lt@aSI%~;OKIG?PRj_THD}hnK+~X{Y=9MxF6f3 zs|ym0{)E+I@u2YV`xztALIHG+{k`Z~Y4cS0dr@9=I{e}@f=irdFkS?SW9biud3pH; zj~nXgWr9N{2cJG=POq1p+iubMEpjPX#*>76Wd(Iz5n}oa7_ZI^><_qywyeZR{Q0g! zxB*$U^~Y!Jw@j?K;sXIu+GV7?Ih2DF^u|>TF#Hr5g{G3?3}at98gjq*X0=kZwY6Yn z05Bqgx09qtB({E6aot$0Ma_(HMIDtjPv1^XQMwBMeA#lRWdxuyTw7r%1vmhX;k6#| zJ6PXs`}u2~;N;bWrPXA#0t9y=60T%#!E?FB<==NXGuUii5J~5y+6B)92hFzfl5!Izp5n7mO0 z^?jC>STcC;EW!$LnkHPbErBFW4+01ypCXL<_9s*F1j1Tcc^-X5Rsz>G;mZgN9J|4L zw)c>DGxCl~PgXu)8sh0(P`DrcJjSc)k^VGJP2ezzaU2nzFRyp`b4*g@1xl{iBM($U zsfKxw95`4FdrW!EdCO*$P7X}J3h91AWkp^NUgO#bF3A>zBw{n^f^ghdM8%k^m*VMT z4E?>q*h;grUkuI>%*nOnuyKU*Vq6v6+YpW_>m7%E=YeqyN{cGJp$4^K2D2YPxkN57 zB(a$=*U@IG)cbM@K=`2`;FzYTHQ>Oo1=w0CL@wR~e%62pZ1Zy3v6qPf|Ct>~;fqbL zy#Z~Bsj(yCkVj8tSi^vU568Yk9IG&7-Eq+!AhmY5Fw?N5Hlyl0tE${`*Y{rpK`R&bF zjJv~T80HJIhmus8@UoW#v7FK1)Bg`i=ds+Z76jo3VnI$za?XgfBMXspc>0U~TjiC@ z7J`}S?oS*Yn!Ns80geh;Zf=R|p3vZ*o#SV~7oj3cDBy)8X15`%%}0xTI1D6eVm_taO=b0J@Qv_|3LBD^*M0@QQTgxt(E|OwlRSJD zhUOmd!x2#!Hn}=xbw4Y_*~?g4reN?z-?&J$1@mh&8dr3zZ}dC=nlF>j0@w9$0g~fN z_Re;XOG}%@_GBy)a4@wSSRd#TO8fVZwidHJn146TL8bEzocPB9TF*cQbm3I177W@Wrk$On=eHXqy%>BBNIQ&qB zgYG&ik}~6w+QB`>NE4}r|Or&I6JbkT5y(f zyn#SgFetBUuW6tC&^2)hg~N3g$H8u5OR<}Jg=$SLZ}p7td0ADTsQ@l>CilkFdh?Mq zZ;~J^)nmePsW+#^zT$}F*eIK(g_!_gc{V=KnKftS0YdcGhZEzqP@5NHIxtT;2inEX zJP-ar)IrKJNpI&9($a6&i%s64K_WwWi#tIxaNv3NR?9Lb&+tM+JKxGc94G`l_XU;{ zLG;=N0K#f(@_Y@2QuuAn+VITUBE9%F)X$bTh%iRDzY_TkT;M}pHJ2@5{dk12+M4g3`VjTn zS#QvA@s(7^)rb@5Lm0+r`#~3c0wt;hs<);Hfqkl%G+B@8I_o{=*oDUmR9blUQG7jo zrqgBU+T%8~5t>ak9@y30Q{saI+UkjS*uB%?(Px7|=>CAvZ~q`(NhDY)weeFz{Z{Kw z^5t?s65$_BR`Pl^aGW~*4-P7{gGEMQSiG<|iul5C^_ShF)TPg=k*^Nf(2C+5b zDd-!{FT9o|RX0*o>X|on+_2G)WPPZ_o5$sfJb|!Wy7b2Bbx>V4qU09gz-NBQRKLY0 z5mc?g_}`1OzGUkA+U{3~uNrf(Z&cLW{R3g57HtMDpj~3OdLez)C=KkS1L?R{T&w3n zrd~X04lm$SbGO*j0H2{`lV+5<#A2nVJO#C{6XkBnHxuJ6JDm!WYhk*0g`u?p{Cf@~ zyR;OgFJ*dR^P}+CoWKJ5%!UzubcaFF$rgbX?&Qg9%im|7C7&1TG6Lbjr&X+)eBhU1 zH|o>rK~9O=-FuqP<~(t@B0wpwHwaH$tN%4w8nioU^NH8fsUap7oy00Zh_?`KAp-9m zi^+xC>ObNS7o%s-qXJ7B;cJmp?- zcrV+6TMy&~%8LRO-@bAizBZ!Iwg~&_m*_pZGCe}}5~Lr_F=&wp9A+J_#OCu&kc%3v zqC(bm(~_wzqp32@w6gn^?bbydmdRL}pN-&3uAvhY_uh2^gsutlxN_4)ZTfP6a)Bm` z)OGgmk8vPU&@d5`$W?2J#fX@kete3zwhr=SSh&c1T*@ zJx2MWkP`B}m#Fu})YpVT#Mv=+neg9_00Y?=IOULdGr{yJGlSg=Vm)Vw0qwvhWJxui zfUY0qz2`_Q81D@GOeZ`ShtiGq$Dz86Vj-0xt!v3}2giKt)Fz4Tsa%GWD0yk(Ho`jz z;bsFf?em!iZQoxnPf=gZSDC`f_)?-f{c&v7j<-1PBlvw2!PPs-=w)+P=imWW!m|wp z2&W950mwOvggQu#!p&Kt4@S!cNJG=SP@B}7f(N)#mTmQXSV2@=xW@n&sjQhUZXE-5cUq_^`zbj z@G>8$=Ne;2E2w-D?OTzTO0&eu_hyD^{+k4;T*7u#s+hvjun=i{RLzHM1c5=d3ZqTt zhsN&HOb$J8F(Mj5jxHz}`Fsm6q24sJx6z-5vG<^Ez%enE_u};^X%+9>P1G?Ef-`Z0 zIX#4<(>krJsw}nXq_t1_9o)6`9*aDN8e)~qck#3}oFxf29#9W>Zm%tYQWz6N$o>c# z`q*z!H-la%exsRl7J0U7l%hJpLY$f)p5In&jb`I8rl*8M1K7op0yfS|5T;3d_Jo-& zB6&qcs9A_@Tn*yMMXY?v8@RSb$V#jUYYXW0WMT>)iMnyW=A3U#tq?Oat1V%uEpI^= zR~rI6^A4p6tr~i|{8i)5XNj-x)a80}h{teg#mO!x^gmj^mfkYG=N+&`_lk(IVU#I0 zkOw?p9J-szM{fV9?ek?dgV#_sOw3*u(k4NGM!o&drQ!aiqi)Htw~t0Hp1(NDdLzA) zR$T-6s~Yn*n#CDiPZym}u67@-9qR4?hy0Ta*!IKR3s2A2f8zW(zDCAfVbUB*)XJ0V z2V#U@5>!b~Qd4PX(k!acqp0$@auNwRgoLwIsO^}*#=LZ7L6-t0KIZz9L5&}BsMMq{ z-9)~5i=tsTS$YLyofg(d+py?+w7c!J885cjGPb_1qFXFDbS8&!VL@vtQTjH+xm*M^ z<@>wu_eAf&Y{etF^-9}VywPrxLVXZ>P-+Ou3Zul*ay|{0%oSBI_?;Z{6nct( zSOhsot}zEFsF#rZvJsC{)w-);7e3pcj>>}3A- z8t|6$w{o)ZbkLiDj$eLhk!7OXWI$>pBsWv>v(3sWFl_N!-Wc=!rYh~a>MDOg?;zCD zcd>WwnC+l^TpY+mBTMar^a#aR61=z6qCv`c>*%R3dm={nGJ&!tfWUiAZ}ucFDF5c0~$;dGt&x@F%fPn`JuxlfSE zT3SpFwNO*xz<|=uycTid;Z$QR#+0$JI`~|CPwRPVscDf0J0>z0V57!DYn0Y}(-xOYR%s z>LWdRX-9?%$o-uBB5RQ+5F)IcyOm>wo_H!h6EK#Qw`mM@(L6)yMCTLaGTZ4{VC5y0 zIt2ZS>k|IZ7Ucp<0JwAG2tm0?bE%l_q2c?y#Zq^4WRy-CK~<$Jmm)&)TA?VI{gkeq zQ!OJglr`hP`*+{qE2S(3KFB?=h;N^Hi?3n=LPkw{neZE@SCAygH&MVv$HH^ir>Z9b zS+09bMB>8^>=E<Ty^jSYtN$itcAY5C*eJp0ZJIW_E|}Yagf-kU$bXFUHeGY_+`?x?=Z0 zs<=#n|JK>`6x$Tt88231GFfGLGDH6;w$GJ&i(C9uuEVL$voHaGQ1^&`J<&EJ+k~4I&#WIl-n^hQi*i^+uE1M4!Q0j zRl=Q)h>yF)b5v-KeAen!L8hRx(6d07{`nkyu zlS-MubNsKS7%0m;Gj6%a1^o%IfB9-M`Or?~I;nwLDDeDZv=H{+LG2XOP&`!uk>OH| zWfRP<&m}nLTwc3rU0sIw=|{ZuOoHYo?Ukk7Wv>qZB#Sxim;s{r&ZAtCR80(;i0Wdh zl}1Id@49)Ql1cYy5v^-)oCUh>o1`zoEfn_qk%53$tFi^@me9AbFVX~np zlK*b|$^%e-Ex`fEOe)LO^BaR{&bo@lc*=~Nz|3uU>wna0!x@HNLszY{hsaJ1?`Sv# zBYezUD5l*2FAH3-$nwCH7FC1L%v0;zPTZ-cWGta7H3j6@aKnj2DVinNo2^EhAUpdm zl?Pvr#W;k%ez9tbS%J7aC5k#-j*5qeHr^WMmF&@-bfrfyU9OgA6y3-|dfw=GS$Uxp zQIJ}hd)14ofBE|I7~(e`VUm7F2_!9*t5*udXMTDs1zv|Ly1CO1FyHj1t1o-t@4|Mj zuYyH0@6l-t+t!c#eH`rfx98J+9is5;8g6uD?-P75n{L#pcZW`V7Kr_XP5zI!5r>+; zd|2L=^VYAIoxO!mHCt-^);8sKxA{bhvL#c>}*nppuSLSsiNSgBWeE^Bs9|)=#PUVnUm&MiL2bbOC-H zHDj1B=(K&<)oZ#hLgEZ~CL;J#3sNwDjlmlQtm z@u9W_Lw(%X1)(2y30nq*+`?noCYJO|ynj;o1~efb(^f!+udAt0=Q%&;YbVoUS3lV^ zJJc|zFk(96R4+SKpzqMeI3nGjDtjII4nqf=vG{!137yZ~x83dUYyAhxCkK22GUm%Q zLW0U|mz2hs&{HC)Eq%Q`Vdx&7c!yohc8?3p0ztS&ECP$jDh#bL@N-^t6${7;pp`3y*&zPf`L2%0DtqDopzP(JcuD>nGRFxTS{184@>w4a@ z-?TaKQq-FlUj%&SmjrMJW9^<_CLTlgMT#-=wQ3ygF@F(3E`&Y1GS`bWiuL0}NI&aQ z)xVj)6P7$Hj7abnqy0Q3-&|Z49Ko)it)vTVJTnKWeo?`=6A+O06rHoymDSzVwf$vu z7zMZQQ-)6}&K`$Fn6<@WllkPP99o+Zq>j;DMA6@ahr8}hC0+q+U;0Ejl&N-Aodi_R z5Ge$z_15C1mbLgbKOJLR7&~BU4}Oyf-z%w6uCM0E%F{%^Py#Mm-3Mx-EEJ{J34~Mq zK2s<}Ph%Ig`o@STgrx_iDd1xdvN3J zH8B$w&^V}T*-(2*&R<_(l};>3&h#7PHSN5V>!F9rh{v(DE-Nz)kCPE|GH}xB{w4`6 zKYTo{_|j^jeR`H(^ei=7e>f#a<9R|^cwB_|ng7HG6)htW{+(;kCHt$t((y{rl=EM9@AIHr&&=>HwW;P1OVAZ#j>E6qD)mI%zge2DqOekhWh<79P1g zO@BN0RsB^~KFyf7vGTRj^G_0oA>$?6-|N8t*52pL{*f5d6oVnqmO2(>jyhcG7ku!p z))SUcdNI29-(@^c7YkX-HsQMTPI3_rS`tal4f3{`Gc#lg2yO@^7(}I!NO7|EFA{)Y zHoRXS-m920EU5~^QF5Lv@zfRzTkr!)vw$Q5aBAm9<{U(_;1h{!aLNcA|rXv;E?oOp5VU_(Y;Xf-z zhfv6wh4w}Z4{pD%h0@vU|ZF z!4&+FKL~4VfMR1${;Aq-sYZ;?bf`q+$-I4Izxy14Hc)9Heh_kwieRHbt1**)0PqKg znNFcUGf+7bB1oaD`1qABkq z7{;+c)9SV-)=V`ddd(L&z#(Ti3v{apg5ebo-q`?jVG>;2>Aix_t6 z(?h!hvVe4*(CtpSIC^5dpvoK4-{)3X_)uQhCn;W*HqRllK@uw@_QPo#`^XX}(FcyP zm(gn7U^!D*!6nXN9aBuYKu>%4nbuSf$?w%&Cj2sJH%H-dMws&e{k?JzRQ$vVJ7TE> z%MNdVCv?)NQV+zZthzo!kAa-;H}-1`=ACLJrhGA+PKdU?YV$d_@R0dt&q)rLrftAk zI!<(bQ&OQHpUWViiug{)Z&S08n+|7LmD7uXKB)1nW1eSV`r~g;)y$KY&}W>uVz#vM zs!ha5i$Q!!5}2r2fu5k~17xI!O5?%soJzq3;1euBMr$(HuRZKr={i{T*j{Cafv!%G z+zCUCeP+RMvKOcm%IRl~&$u1U9qQD&?oxg)in_`m!lZ4yBJ8%;*Rx@1q!0dU-Somc z9AdmxfkKoF;Tohrl*; zLZ?Pp$Gx|gU^<(scAmD@vJ;=tfNrz*@g%O zv1v*MM9daF$t(6$4|z|JiQwECKXQFX+K{3f^HoD+{42uxVbO!O@*lupau>)?RFAsS=0$^cwqF7)#-HJ z9!%w;{GLe<-a8ir#b>ew3^T?X$6w>cd1Txf6ljs)IXT--xko6 z2h;p-wtQ`UCdt8PsBOO!`fI2BigLFjYaOOy8c3CEj&wd4Cl8Y`3n%Lp+pUP(&3}8I zazKwQdbH)620k2LCJgrQYNk~Hj?rahPl>EaX}Swi!$OXLhGShBQ~6fZf>ABik7DKz z8D@Y&?RV@`O_6P1j}yl~9WtNlBL+`p^)am{*T3fc<-%W#~&dnpE zj)rcdK*=E-ItQe}N@|yxJkSFBJ?<=!hYd2DDtE*j7AuXOU<6Q3pNznzdEE1ycA+H- z19)EkOA5}{=6PKtrcF!M`;@=UHd`j|gH5NiNdrj?^=cAN7VgnJ@(ODO*l^lL0_fb* zWYHDAP`jf;ksi-LZs>mgJOaIA1{tc@qn~{aCO>ha>|(B{fZpt^&c!Fca*pHedI<>Q z8_vOLZ5{G!wB3vlb#7w-~JH^1%dtq5ih5dPxpamSh3f*{<$M6dI2(J(897 zu%cTMF^F8D^131<Etf^&4{50?$q8+wBWRIY;C7`6*h7RvB>NMraw*Jia?<(Ep6fZC_Kj77QR)zaB& zROI3eiBmzMd&1T2zEZ!iNZf7;#bNHCWPd3YLWgB_%3`=5XZ5<(FeYy?rj`#v_zte< zrK+BjwX$G;{zfWfT8>b=#5LsViEu=##GuaSc%`m!qz#0^;ad8%y`5G}0GV<=Im6Gg zv6ybA981_n3^xsNp|WpG=Z)jK6jF4@()numsJz@2$D7N=M3+)L{c@O9a+MO`A&f6B z2|rlXUacy0h0OWP)UK5EsS;9x7jzV_isqxX9lK38}iImA31|5k3i37O~dHP|pHzes`*RcyeK zo@c2yz42~SCLDh8#FkCr zI=+2>+;;u*;oD6fca{sbR}D^j|2vc;r@82|r^07}(@5t*0=#t%KjKaYeAha;54jj& zPvf{C`lY`d14Z+`6+KIijT;&1^w)!f1}r>_RXFH?ubn+*#vmOGI}4+sa=C-{1*g%G zVQYYY)8#lxRaKQQLjYUkZ8F zs!>$r3I~O-eWb`Y$%@V+4a_DnU6@`C5gtU-FG)KmL0FC31p^`czHK9X;$aLB=k@(> zqFQ1dx+S$$JDU?;7^=hX8Z3;nnDnIKVY>|A<+aR*Tb2qtbk_7c{V8Esg9>ETOUsM^ zy?#Bz>Ix^EGOab{mLIz@#1@7tfsHQVk72)WY<9Z3xlT~~>n(GnY2%B`z^y`FP)pDH zeS{5?I>+7#+b_ApuUw~seN6BLj6NCzGl&QLe!+#m8CZV{F*T3wjURGUwE!G0}Pp`$vS9h%VZPj#cYgK zLu8JPSQsaYwA{kz72xXkPB1kLCGzXfFGg#%mfh)^GCPP$E8rJ-NN?ZO>2C@RtXFiP zxJOr=y7A7HShw~V;kG|)@1g1S>1PK>1O?d`d^c%WzbUn$C-AvIn_QrIaH((y9_4k? zUzw&hXlil?;FeWw{z?A`53UnDiURqZ@9aN>`O#!QUJ z*4z7$WCkl=i1`iEm)5%kdmIw0iuN?O&Wm5vQHl;5ZB7#&ZwOl4e#5ZQ58aPW)~gNR z>0B6g|1p{6JJb4NLe!{1Zb#r;A}^k}N!S=vReiOkDw2?Ms%ZCHFw1|8Z@mEMGA0{? zY%ktzX5yxRTc*MBDLoi>vWj^)_W2Y{7fvy}#f$!cPNq+YPxf~0yrB9tqj$#O@5D=6 zAvN&~&V-A+q+ReW6q!GJhi%#Q+ixUp=yw?{0&fO4Qc*w`K%vE;+KURD zyJL=STkCJ_i^BpNw7}o6J zhcdgz*njVsBBLwC=VooEEc%P_@Q&UkV|2xse@{o$lPr@h!$?ytuyd@ga+@wlcFgO3b*#ktUgu(Z7+ zg+eYhI^LV@-@41H*@`dOM?1*h1>YAng?+wNe|_TM+?MiniK_f0bqn3md|X{(=r=S5 z-)UI12AGX}t7s>i)6XE9@i$vF+4W;n~vU8Im#{3G8x9F0hq-xX(@$=ck5%zkBRG|#K9B(8j8mWL_A z+Cn-=j$8a1VRB?cihv1?ANCi1GGB9kEO)H9NnPotCsIMF6MAhg<4tqGy$tHxtrl@H zXphiF@egiH^mEZmT>0X*^k}}2=92huNf6`?uT~chOM7?`9!Q5+Dh^oq7%YoojOip% zyMAp&)wk+CY1`h2us>ucZXLx2ul7&*qxhREo8U3PBTHlJyn2Vn{dCmmroT;^6spTD zahLpgCv@ln;zw!gpDyyq>g=3o5^$rDzNuen)nfiyVsc$5i=!l_v^6Zo=JM~iwe?+= zODnl6bItPg>?cWqZ#oXR>Bt_Es1A>Xh7Q@$!qLqm&kn7i9@X4W-OO%KRB4IMH9#47>x&>HG{kss^K*-sB(<6~rf6i|E>o)!A| z`rsXur%JY#tWbL%dNc0`RViku2KcWX&NOO-uR z7wrb&W{BVr0IlX|U6 zPnf)>)jJb6hP_J@$ECijBdYxB&2Um7K}ZwkoYbQLkVsK;R-9H}U0Y0{U5#^X`a%jj z+E5+a)8yDx!Pq)e|0uRTDXzcqA3SzvtqKDXCn-c?oX_`ebW|0VBLllU5I6irwhr-+ z`+e*MSG`(tr<%uI<(*|J!-#>$MU#DwhACb?c8#(fV@ujjbB!3%I9^lqG+b3==?fmv z?i*m523KLHoWmMNhwYQ;Qcj3Uo49*s`sJnjf!#dp<88%qL+iYyl7F2>M16Rl2*>l3 zWE{65CjCJU;z{$BZ)OP}Is?*B-A5YLH=3zmF1J76PmGhV%d`vF2#0; zMt$ZTDno7{glVaOK{^blu!8K`Q8-pXZft=IE)DlqYzu9_Qw#Zaw4PAVx#Wu-#b6C{ z6)}oocZ&M^&}TMmUkry$i|Ix^r>No!65lJsH`L>_-)OOx4}1x#lf$`>Y@q+4%ku_f zt&v0gi3C^^hLwCVsQCcmdbH}TKT8v&Qkob<7PmSj>+*~%&O1w*Xth3WL@#w~wq4gx z>4K%Ytz7W~lE3FTOox8o+p!o8Uemvj1MP60_5cynuSh1Jj=r5$qG;ijv%xQ)t&mbI zhRNAh6qzlqx#E1CrbR6s^Vebyh{c9uIA}w-&AaO4bW=s~Z?ANLnxLwfH*G1|Hq;R@ zCOmN&m!!JZQ|-QBVn2YgxQ4v^^uek8dX)C@|K~fD91x*bNYt>dpaOx6ko;?IjM8XA zdT@M{m3TeunKwIf&x~ib1FQMqW_N{JYhRJX41=LrZ}TI+1J7aN$x*ljO58rlRds1+ zyHL8OlD7U^kHzlT0cT$mn6=M4mwcF@Rz7jKrvXBw1qjE~_$JDSZmSIS3!OSzHSEa@ z&+h|v!O_G`D)7@vCpdMewo6D4?*SpCmx$q?U$wh;DRiw&gZCqD8L>dJI)1wN^IOD8 zu}#3q4w4a;-gYY&A9%~0EYFh{Ek(PazTA}$AJ)em(1d^?Mh%OGANIIqc<>Ha6$>t=Hdum z_AVL!2~L>iOt9n<`c!M)Lm)R;$wjBURt?vACme~<$8vtMh=Fr0Cl91X4a*`=lWAfe zdMpiwBP8xyiUSb0@Wzfr{V-wPkp-1E((wfi?&KOQQHO3MM2l~WzH~3Q#k$BS%JwI< zXr#)EF4bsTBR>n7x365(>_rB7&erh`neFFJ@T(e0X4!K zxDFSO_+n4aI2>=aNdqck#ThMx-ejN$PyA(n0SgH%0q?K!My$rd*RWn6(n^@DUMz$% zw3JrY>=)?KUA|a5viE(Q!QP_;jQwr=$JD6#MIR2-}h(mNiIkek)c09<);aImi&t*_}=d^ zBqHFCs*R)w1$#Qaq@jEnWvx%yMf<~BtaSG0Qgse%oK&!4>UH_OUETY zMMQplciTzkb_-~(xFe^WjbBMdl!Vkoqg}UY3)LGPbCU3vM|LS@|4}?|Vsb)TK_LH` zbe-vEm=u2I>@}ziEXWcSL=K+rZrrB)tyAa1X_b-(#>s^^<465`w_((DuJA%{^j+IGzqZ3HDP|3p+9x5lVc{xtA zjnO{nfnc!RIE95_z1oHiDc#!p^1t& z_$vED@9sLneDQ6b)c-BjS>H5KFblJ{WC5RoBJ$YaZNR6k-osw#GsCc|okW>4jq|rW z)a-{>G00CET^NBt*#}bx#NhiRqaLvAKiG~y4q}9S^ph$%UI)nJppoa!wYDqAu7;81 zepLx+IZhrr%_R4GZk0OTvL%wEnc34zpCA2{{i7SxN=cpj+qi6=rTO!I1RhF_E_a zJ_4HhLiZ>E|K3IuK>+)t+LRD2h9AU2>4o_roq*)N(IK`fKRg}azz zPs@2awX=eZqfIG(ana>KDsyG6H>b(0tN!iruW?>0_}AV}EcU4thgBh59!U0_NaBj6 z#B;0ZHinh}ef7;Y>?jpRw~9dg^V+6%dPo3!+et0GebZAx=$%BA-QDt^A1dk>hxo`s zzK0P%2KAMR&=%0(7B6ArlfMm;PUe2mG*dH<+F|4fU54TvP3D6q=<@2N!n8cv&MOXi zv3lY6LLd2qsL*{*6B{N|Ks9CFL;|3XcA)oUPmOS&K@q;fBl%7%dAp3Bw%hwIIS2Sv z$u}V@#!HEYp^Tfr7|J7=h8p88&t%y-h6xuq*E|_x%y1Y6RsiZ4+4%?wY>x%R8(?42d!8iv@O3J8Cgqa zq(TnKOH1XZ1?I;ukx_fDKkqpGLg|Ohh&P}Vkln(=$Ol+B^XXQbt{3#gUAS#GmCTdi z=M!E-35h}vpJu#2Plo*T^CjoYS%r%;R;iC|QmxO|R7$xIVcsE{HXmjRuMpffzgb96 zr~}ZG_EL|nPsk7n(@TcAzUS>lRa^pJvdXu|)<1J(tN#6o11{HGDVxmFwQK)kIykH< zum5t)5ZaZ*Y6|7@3)N1CmUY9+h7COTZdnSXS4Wy=m@SD}9=n#Gx&|vuIc)I(f(sBU zA25j^bc|a?`78`p%-npJ%mRAB!%G8CNS@A$aE3fBxUtMFRKrV<4SOL{Y_~a zHR(x~*Y4t!pR-L%dQC~;lvnA9fBtDD{^x>Z^VjjeaeGs1L1GbDO)g3V)!#;QCRcoq zvX$cQaA$b}C5B;YM(6}5@XfQG+GXJ*KyOECQxG#hJOa5rAvMN_JD+d5i6gabp2~Ol^S%;QkdGo4|~LUuSJhHt3Yv{l;!HsiiEeb$CwEM z%9t17V_fCdve}w&>+zLtMuhG7M@ps{#g}AwaS0ovnOn|{z+r)~2dNdW)DPb@XF`Av z=%41!)+XmH|J$t~Qadk>^4fi|<=?Y}+M-d8z$mtvp>CGCQ-rdUiG;!dLh#J#?CKs( zUB)frt}o<1tPe1bs}0Cr)J7Yxn|DxY9s(xk1D|E(CQsw+et4%P6yjJO8X2R-xf7qc zMz_lSUbDKj71tt2fx3TT(Pi8yR?X;O8SM~rh8WyvSzpMDTuCZFjUNhWw;+EHsQbQ2 zsv&T8Xg@di^D)7&JvJfpHvckgG_a9;f652KzVk2r8J`3d6KR>s;$zP~J)+iQ;qyL= z!q$F6k!|b@T+ynG1Ld8@1AN=aUSFc1w}d&`*M`l zTv$Uo#S^&Wg!dvz2NC$&oss`0!^}3for_6yF^#2n%`&tJ4F{L~BOIj^uwsaBLJ$Cz z#fs`L2lLCwM11w1{BoifLu89Q7y_LA%h-+~kG{+xRWF+r>|C!6mkajtA4g}gB`FX@ z(GOyQ+p=(n!eNI~xZBqcJ*zoAEd`m8FYZNoWLoXaTkJ4OZQamX&j-JOLvMEJYacsK z#Fqz=Ee+Ntn_9dq0=;sbI&0X57*hFZaYHgccC(zw1xIRM7fqio1U#Jx;L=fNsE@jyndX;g1^UqTVcf6~dk;r{+=-PEiI zJ&5y9HdIP&!(Y)`wa@+>uNN}Kw1rA32P4L40`Gn}@5FLwhKXCXU$-;w8(as{;zfWl zKv1fZEhOzvfiJu?C3VVswo=?E~bpiD1c1)X2r8R#~ zAGiU6@ugFS+z=tln3?#|vWopIjKk)J##A7wUS?2f5RxqtiZ_#KJFjBsWnAk#KNi!K z?&^E~Uac_?7N`@ag4!QZ*}p}HX16c;5p(uTPo`V!w#b15snSOzaU9!E^YD=Fm}I`b z_ZgF56?gS%Y~{xJ{iew@jRet+ze$~vPE?-Gi}IYxQlojzR!x2n^}d5+$zx>X+B$pb zhPkrtrhp3x$YL86G~M{T1Jj%xsIc?UfX{AQ7yP#Ph4}!7=|IfLUF=(qRm z;$9Zop~>a^do(G4(Amgq-y%jHvzO}Ov5NTEqtA?(Im(%zA4aQrth<8>?z<#DqWb;m z5!gpSeRGHI37|zsnbw}gLg`Un1E-A(e&>Gxpm(hCRMu7)Lxbw5 zw{1R8pCS5c#6>IF6PKd~@Q};tm4q8cS z_T~dd9n&+eYxk<-`sN$%&cFWR@bK2|xaBSsZ3K8YBfz^ow99-QD5b(sk)?%IksFb_ zH4hOd)|iD7_Qg>-bI=6~@HA#j8cclKTM$zp1XRH(%NnaTsD3?x6FLMUj|k)&0Vr9= zFKID3)G%2c3o<(}C_B9}$MW!K79*)dKBg<2zP-@!lx3fW#ozrOBNWg8pQw4{xz%Y{ z(}0-Oci-WHr$ZN!n>TzC@CNeZw6%Yzvn>hO1XWZz!(THgg{vCMpmYbm-9R5b5O z?-#H-JlZ!crH3r<)Z>fs0a zhC8`PdQ@-qqKfg#waB-IXH_xU_yXJK&-QGQHlZ2nadA4Kv8kZQP9We5Gj=3V~+TM8a7klm0qWQWct#_QN&6KHqP zsq`QCGnLL*`3$zWn5JLzOH~lNXu7!H@XPX`>dJ%N#C`WCzJc^3~LK_ML;EEvyW_{{80C;v?(>Xelu|^p7O-Iye^kQEG=8T6Ai22SIuF- zr!^l!tOqaya`zLyyF+N0C|&**t&)>+t7ubq{>1NuOR4|t00#yXBBu`*)QG;^u)|&jEj%iV5!b5I%9>emdFZ zaW1DVk)MMF7X+iBc9+j5d|aXyQ1KOP2~Q&upi`P8{8q#)`Ooy6ceMJ62IB?)YdGtP zw=O#K^kM3G5*MKA}*Wa;o+G+8UbiKEue@o^2#ZNeZZ#pL!JI>)Sx zui(juwp*DWn_mKs_rr)WR=BWMx=og(s-NO9;d+?F9p|R$_Xv~^k^9!WbwNOv*WpwD zB0{pS+1ppOn!8(KVBhR^kB)LQ&P`(#%?2gg(pMB`3!o4;f?m8MrY(zCS&3OcX;dp+ zScu9hP!`~yzid_VU=#=woSQ=6hHFoRd$aR}lkUamjGi}?l53nRP75$IgI9=hwwSO!QKgQ#FLBER` z-COvW_u;;?oj`l3rh7XIZ3Mo$s0&-N=8Emr>C?&}s~5{g*4C5-gKV!go0uf#Pz z3<6%T)tb3Jqddi!+Ly|-9hG~d{DU=<$s+3L!nOsX36d9T!*L<5SbY^6lScRSlW*#4 zWzK|S19o&52}XKJ;ZwT5H1X5hC3{hk*ybJjLV<~ZKGAC8wL6nCyUu`w<-uey|%6gt5CfY-|z!1WVskP1OFIV{!2ve)lU+P)(~RRE~N(WutDl z@3JnQlis`!+XypEQB0C$JMLnLX9za>{F*W2Er3(nX8Z=k%efqeuH>9cYX1!v0?Iks)K&9k z2y(Azu4r(s*htn~8{8rC@0{Epew%JLPea*5;Up!%51gf}YkOD0Bnoo(#=3eHwO7PX z#Dr>4$(ISg6qu$ALF+nO!`w0_(j)3Nw@NmH{IYCuMK;RkV<|}go2b7ZB%N=KDbW@< z90>%6CRz8w*-2s-&yrvSU~VhKcQ{pG`}uz1-=7|LG+s7ZZpbqyc1&Dtqh)N^ucqODs0_{MdbsJOSWrexQk?i~uI}UXG%;j6 zNyHg$Qz!+!JJH3yAGJjZA6Z>ck-3?KAlUVY#FK8LmnW}-}z4Dy{E-#-NpTb-V5XtuYqmzd;(5=mRtW0qNTkK?Fj8zQ9ydRmZer}5@d zAZja(Ch228pw=w1t(+>;HuU%zOxq|kZIp}tc~nS_NTYL`0%_H2D1LIJtap*(684C+ z!JS3nb4fhYWjV=0g+c^dx}#c5E|tC;UVD(nfA-L!Hiw!A1s)z0RD6rv+ksAW6Ox*7 zZ$6vYmAiiTi}YoZ*L}fIbB7@uehZZQCIt0Ia_^W{oQOvipfcoWt~Kv`)-K@Ldkw`1 zG7M|c78~Do8M)=w-?Liu_F;*Oyk371jv!a=-&? z_vx`K5trf;0%5O;DyW*Iz7R`L*FGJtA4NxlBF+V;GU`vSN;5L29a%p=`XX|Mdw2dw zCq=DG^WtpP6D~%Z1=Pe6iSyaVJp;nsnepJb&8ZS(0U7iQL4XSqxbLAs_^ z+QxdB*@L|~lTJ3*>snEmG`c_W2t0#D`Y`wfm`=bOE#`?(hsE2T$R!|g zA3wJ}X<~&8n=d^GOc-QNfxOOPby|b|_*&dj7=-VLuTvl@HTb#XQhPX@>>&=D6FBW;Uv=+=n~Q ztuye7stQGy59js|m&>sdkC|c8Vz?swZxJ!D-G4fDB?}n#bna7JfSm^ZJOs7xQmvfk8?}|-0)Y30Q=s=)v%IC)~GpNYjX?yW7#ro=W zn34U5(Y3^9STz0uPF%_Iuh))GY{gVdw_yKiP3I7~Tru#E6?Ajf!^z6B2Bo#JaSVF_ zPZffuqi-zVq715**hbNF+xQ!9zpjSXtfxu(SA43b2XNHT8sgdaK?hZp*MhmY0rL2M z`p0E3szk;U;c2o*XlgvaBY{KfvU88wienUwt?gE!w}rj^8(-JdNEp^OWaGvm`gT+S zNO8C3M=lc*DVw~IeCzg&@l5*^G4i7tH|A8+MtNQu2)Z|l!T!6evR~q=8($`bv?jtf z!<`{PvezjW_u7jvI}$Mc3_uMpb#Uc>RPl1y6(If4F@j?~23MlcL60bR@MR67a5yQ? zx&MT(ro6%}v>5m`wW73JAY~XTaJk@ zPIST|HTg68EE%h44SMOu>}P55srhbJY(azPM?FgA#_%2NBc_2*eHW2sIfPCdNXmNH zLV#kQ<WrdW;SI$7pUYfs)M9bfccJEXS)3)lCw|r62@y-R! zD~x(;%*Gr-+Ik0GJ&pCJ`E|@fq>8T7JMECv^4nDJK7nhMvvey7pgCjl5tE6vJha)2 zR}`DLt70^dl}E{GBw%JUJ2~-UK+6o41pG&t2EE^bJ3oba6_i2v2)A}SBrO8N_gh*>cYqDw zNzzq!5*W;1NWg41gZQI4G%T?%vO*mPjJ{h|hJdG`v?GP%-M#;C28*XYCqw9tnI3&# z&)DSB6CGv9dH$kN6zx|;3rlwf6DQv4gVa|JHeqO&7=NJ=9 zLx+0`+E?1Sa@TL6pGNr{4D=aHfj|^DPldP@Yla3`gx~G&9mR~7l;KONbgi=yZR%3j zR+Gq3g_aZN-$4rYA0w{v|*8 z28v#eOUljHz^F>c)!ulH_zb;X0s^asmQQFytgU?=-UWR5#~?yG5vnTvTm3>w?kzNZ!z=?af@6-T&u*GPh1378cSH^@CAED z(*+)n-VICKGH_+0=@eExQu&e<)l%e(FCijn*Qg%hCg=3q_p$_JY?#KN)BR<+3sl>i z-|h@kLwbcE){3R_>8wz-X*{&%A)G7zKx?W0S*WKx!eX@3UO1ZChyY8Q{zYQDC9Ow) zir7wmPxKqas9H-?9lhmu?NQg`9HHUu%rJY&Y#Y>A9%&LbWTLVu2(Nebhqz#2_N-s@ zYddJR&dJL0iOm{riPWaI;v1B2tVH9hr+{c+9_(mHh$6&EoNSZW z+CJZ5cYqUXy?Zick3AQqL_6k`N@W|`fd~}2qFjcShywCZ^i{ILg6k~rlvcl3M+i3x zt7J57!~xdy+MBDJh%)Sj`o69g#KxY-1?qnTE8*#W=g}gDE|{gg1E4%jRCIDFVl-{u z6e6mz&ANMj3IeHBnkCo;R-pJ(3q*_$am+8F?#0kjV*v28w9id>EBN}6XhMDn)F&%P zCMQ{W;dg33172pp+?77`FN4RvS6KR8`9vGSH=ro=)o0H zckq|06Fz24B!0PJm7M)I8Os|{`Jo7C)lonQ_TBS*86MQW3rmV2=neTXfs!*bS7M+( zNPQuf+357*^YYTX>7<&Bn~{Y5kmI}vrGP=)UM)znQ%=#yiv2;cQXyaHGIZ-eO$WMA z4dfyQ1? zG7kNaYbM=Y-|YULP40XRR5jO^9vf&ObX|lGeyd^T;G6veJ2Wz?AxXVjqM`W^L_aUB zA)gV;vd%f83wX%nwiX^6R<(8zG#W)d zf<*2xz^Q@cWw1~bJpDr3Z$=CJ?wjcQNF#utp0?=M%);#^L~Vccxc?n|&8J!Omxqq; zWJS0wL&xSZV0R25kOrAblj^dl2=5*}RjJZEXI4wEC*a~ee?+(O@VDRUheM>5Q(!M` zYCnK&qsUuB5hrF=eQxfy7h!}ky`c>m13c4;hI%r@)3(Be+FSC(Pr#v{6G2i-UnW>>KUM#V-VS}CjmL7Ci0{8Cl`x}t_ z>wVdeh$cjP=O;#ix~QnomA&K%ESYT}892KTZuTDK{`1XU`zBQN}3Fv6R^LEg^^ znpiuWG5vqjvvLV5>u+{1RuITR)JsvnY<7;i2 z2=!x2Qqnxn8*FkP8VaK;M|x*tuSC>Twk<*%!*GO{M3=pKW%9+dedB$J51)Ov02^lR zjr%zqh}4}l{B7N!Ba`CLQ%^I1LX~CyB+glmLDy%5sk1NXH#MpgDnoj2pPO(Wr7c&D z*lg$klc4#Ak~Dv>h#;oFyRzL7EHYU#Yp1|1!hZW!C8EqzBQQYI;xG@Rzz3S*K#O`^ z9kw^i`rB)P%{_^?saZ(^n!V1OB%l@=hE1^THt_RCp}T3=cU(yJ_k)ek^BCtYU(DN4 zAN9tItXy?dIB?tSHgiFvq9OPF-c?nn&#-$y65UzW#9mkLBWAlzcwHXl&Jgj7Zlffb z)iv-^0*wH3kEg3NNSF24c0z&n?%l1A6}7p)l_6NCET^U=-Zn-A@9%XmP`7u&t$Ev1wBaG# z55DzFmvyh3W$66r#yz>+8{s@0x0Jan;~{_1cZ6kH8>X5P0~_&a?%UoWWGWi63D9P( z2n4TiuJsdH6;2kMTePxnKnnq0zxsW&;$znnnWLZLHpW-&wE^6fewz=XX0UgC(V!B8 zmW~eB2eYOSU!$q^R-5*q{`@7CPHaq3qVUtTDJ2n4oJwl}Aj$^Oa~{eLE$(~4ZU7q0 z__GWmNQ`%^g5E}CqGIPI%)j`1+a&wrWiSmxy}$DcWo9p5N=tlV1-^HqX#sNDbV_Ui z`r-C~jG4a71yPzp;_v4j&0%aH=#gpff}2V1LZ+ckhA(d*l+VaH~J41jr3 zIj`WmGm<+g8d}Y$b1b#(i=xq#vW8;l@1m0_~ z=Tm^|8lt0I2y1s67K5&)Dkj;APfnGyA);ew0IxzUd+Bzv{Zxl)?iP^JS~uVPu|N>~ z?orZoC$#g)Kp}2wJiqDf&bN+;8AX3O7S{71tTimfi7CFkn%-SPl$gD12 zyag=syX~`$(}H1h_yk^3siEXIXFrdLA$iyrH2Xyh{{!vi`gW+2^?xDM9-txRsgisN z**3*r`+aAz&Mjc-xZo$`x)mytl}-fz%tiV3AOc%sBP47sAK=@CY)ejH+sHCi6U`8Y z`zF`JeSRI=wT@u@fBaMmaE1xt(iKSGSTu^{mAp)q;2_;q<3_<)2UE+b_>Fv2;Mi zY2I<)L4dXmYu&o=T>zT0lYT*2bBHL(Ov>-D=PH;O-wF3M7d5%|{>%y4(Z=Rp;e93- zFAh)ND$&HaCaBv@WI(pLEeJ)j&k`6RYPA3aDJ>3P<~qpBz14Z7pkErqNoHVZBiYJt zV)#67X_G4y%f7R3uN%W5vE31$D(XAOz42RErPV%xDJAg*)z!m7bEoM=cm;OfL>;fEe8l~3voAkC1kjaeNQLBvlP61gUx#nM$T z-vOM!L3dHCTa}Rb0)KH?!+8$Yf5L>2ftiS5LBlTI{`UPxZ{ffeP%2-C)R*qE2IN-U z*%G#}$&1Mot9ImjsjWP>L9jXX{<2eO?L)z$J0kP%3Z-HQp~ZY6ONr`tfqYa^yv^GT zbbzpLG+(cDnR+pVW2=xk!xV?}r{%O#F1JX|KK)CC4Ca43D}`Es{kLc55Gcz&AS{<<&0-qarOY$6TFM z)l-U)g8(`$IhG+}E@vrnQa~Mfyzt-_^*4h@UHO_KZ)_XrpAY-_IG)Y;YFR|q4_glB z7xt)9Ya2(Z**6%%7{wL0hRS$X{Ewr-%7@|y43giW*fJUcHH9rf!}%Y0$;mEz%f zn`PXfvYJEUmf}b0Ky>TyExv-z{MuT6-gMa{Lmc+qr*(k3cx&h5R!pCtfxBNcOC$BZ z;7ma+D-`nRR(sw_8Bq?38`%wd&-*zp<{%g^$`NBQq7~P0kwaX~kzph;5Pw~-UhQoT zPaN#+?U^i!gm%||FzD7XCl|-sRA;=B|~?zBC7tbH&~__RPT!gulk4LbijHWg)g#EBi(_tAEW1v=Ecc3ml6nOcl4 zs3sfFa!&W0Y;X1~Y&ENq5m6Ownou?vS=tss^gWGM9HpunJSY+S9Pu9`RW?V!%$%%8 zl^+rWw>mx-3JkC#*eH3FV$>Ebug$5!4&=n;#3C_N2Vn{hfarCLn*gvw;@jJhfFc3yFk_T7Lq-0Db6unem40dp8}Ip^0DuErIWSe&x!kp`ZGc@PrAX9Mi-$S3 z{jaMfD~|%FF_-&Z3TNT&F#YWR=lm?b5!a;bXIB-oFjSFQj?HpAs841M(Ys)6mw1&g z(()2Ga#>cws5z_NAMwJsTyJ5AzK1r~cU*$^H9_z@nDj_Hedj&&B9(n*HGvO0aC|r> zSfMFGV`i7;a%8}2ojL@%zpN!%VtCW-ITjRFq6rva$m_L8F zj^}fA)&pqLK<11?8HSd#1=%bRycf@$bK{Hk5D|(W@I+zb{ZR| z_zS9D-W3YPXPBtE7Ty8DmP2H4j|TNncp0Szp0o>O$Fb?{UTPXo>r|V#6SnLo{vCg# z@XHbaYG!I8^dFh1P$dzN82S}c+k3|f8_W5HLc;ojp;ZB&fFweFpclMWkzT(O;k42P zk1*f_h|bW4BSjMT2zJkiO_SM8D!bpB-91{nytxIRd@Ca>(a)fBMGQZRdC??=wf}jR3@1 zkqym);p!5+>)XPTvzRNC_IGT-7VgBg@X%YnevS;Y_1%hkDmb=%?8F$Q5?nsNvdMV@ zTwA3kJa%TG>fhjn#@=tbtJoP-l^UEWLMymkA)tPKE{s?&^9nSCiia2zruL>)s}gxg z;!_E}-?EPqM6Z{89r2=2HB`{+tUg3u7ABbmhgDSWf+plJltAvb$e8Jq4lVkwI77m7 zXaVA*A=uM^cC?G8d1^x|PtXNm0;!SZAJO~X67%kvh{5i+3;@D`rS{k#oAdv8=gfN0{Mpcj-NI@+i5x}za5A^vX0rv0yhusqC*}P1PFD$?BTotevXBVD(R+W#K;t^) z3GsKnk_OjU?Ja}GGd*Vcs(d;b5JC8LUBM9Ao^SR=Kk3Bp{h8|-Iff~yzYI7Q^B`fz zsL^J%B}Pl`x>7p=UsgC3I6z4aK#=9BasFoCa3S@qU*70UQTaQEHp3sZ1X&fQoh^Z>>mA(X-BMUi;4@M^zCn?XU7z-xae)vy zAqiiH{5^yOU>&}wpG+{?LrZtStNjN1ntJvPFWF}5)XneT;T^uI4`*-G(MLfe06z-n z@0?OH@5v_$bgc-A{?|i~M>Uxp#fCSdbG3sa4s+6(9_WU6eHkU=pzJ&J!8?+fT6A$3 zOm{h^ge>T(u@sOL!a3d5y2{cv~mp=L!evfzq9aQQ>M|fqSrJ}C8&@MEpWkts{ z-?Z1!ca!7KCqziw(S-@$(-+VtwZ3T4wy3wM=q(XZRatZl3H+*L92f0!l{qXuas)U|O^V z$A-6dSz`SnErq|XMv&>5-U8t`hitl-0y?Yycg=gAU;Dcp1Sge3l}|=)cUcaiV})=z zyL{Vy6ZwRZBKIO=Z1DQJ)Tyx|p%1%#5NBot#Ug%Lp$*M$Y$z(MHWpf2)xECH3|&x4 zpSG{t``%rMP~Gdc{5ThnVJmf*Pe1V{9Pr57Q`a%cq`#-hS7HW()FoP!Yu(>KXm+|} z8-`eYxV=&G>kmWiW(OlMvsolr;~)EfG8D zS{w$(naY&;5NdZb;`$@Bz%EA9F~WqgZiHAw*oPm<{Ah-^=fO!I1_{y3 z#u}&vES)P&JoY%(%{kgb$oB7A$R5uh$>n$rcnb7FA_DM!IgZe1*d9j#K{os1ksH=d z$s)eSuBdop)a__@J$RhJ09+n#_3bUn-ojr=383V6VxzKyEe$zQs28O*)3Nn{nhs*2 zZB9FZKfsHbdrP7uvtMa#y%x8yNQ+zt5+(dM>r~X&KFa+0fs`AqP8Vd0jXTq)=Go&% zyGu32%NM->mVxG0+&fc`@F{Ks3BY+Z5tnU)*-CczyhO6LhiAG1pNY33U_j6t%`{$O0i4ce6@8i>w*a^aWa##7XMgPz?AC9MWXq(*jacbQ;AfX-9 z_ktfhh$9MTa*J<@!rahexN2hH?^LsJVBgKgxZ{U~q&fG5!z1)*p4endQCcOM`BI56 zhxlS?YKI2kN#alo8TXdaVu@;Gxo+oP|xx!Uuk7H5JM-Bk zoVPtRO1Fs9)fpX zyKE!Ux-@NVwwldrus~G#gIaRW{^1vYmH6iN<0$`uKY9k}e#A39xvJ?}&-@ggMUwId z;sNjWJM_35d&!Y`P)HgtWY%LO(Q&oenNduWNNO|CX{sTWHAhKL-9l=Os+3h-%bMnc z1UTqaOwb0>OW8kZ>%q8?WRKCgADd%C0C&O2uLTKQs9?7 zz#arZVMRetmI@Zcu>d0+h161{Vm}yl;EWL<@9sj+^h3Co=13V@=hU!1LqbKU(t~EJ zKLC_Cr>a!Ql~|!QszXXgbRDWW5TosUY@S1K&2rjUvD+jUBK=)px>-VWsO-mg=BE_V#F+^%dK66l&l)|F|HwV2mD7*mkBGZMNWdgG@Z8ipUt>XGE zW+>6EdFI6urQFg00XuE6yf7}E=lDV31pK>Q5oL6D^W4%*4Ro$7L&Y_gX$Bkm>{JL4lQ~dx5K=!}g5MC(?eo*ylZ&SItl02~k zDUpWg5Dke|kiL2`G+?^}}T{jR|ULr1e@whL8X78?E zuO`d;scP08qH2ehT`<31zr#vptbK-W2PzMeU*Yz+Q6sz*K@RA z%$SI&W{Ldi;e_Jy3Nl}ae^3D=9`g3ABOp)eQiqR|kKdMB7jz@fP;U%lMm@!JwhP)1 z`e=VRKyli{gx=P*3j*%V4)enU%{;M*s|e!?62on`!He>0gK&A0Ja;tH@aP6Xb@Npy zXR@(k^q)gWXzX|23etX!MCiH3F7UZsv)UzOD2ZVgtCi0ZD5Q=r-^mVtaKW1kj3245 zFRG=Hr->%@hdgN&jtA*2-9$e?D8Db2VO^nvp8}ydTET#~TUe zl$*lgb1-(5_5Y?yOnN`!XV|;dDlODLP}9~wFYj7yN6l_UgyjKSo_y@}?Pyi{x+8sq zx-_?xRd&$2iwJt+#7K_;>$bbV5~?TH1c#pP$Tb!Hp!!P%@csslqZv}>l*u`{PRtN& zSU%rImUo&z7K28smagK0_@==u1xC&B)>jKDve5L8z)40M1&>6EC|L?U>kyP*m%b1f&9**HBQnOrIoy7 zK~1eLxgsuwNw&>27JGZcx2G9X{9@)RP-S+}3#=-+;x}*nrR;@~x0nk*yyXV2!Oo-! zA0|G&qG`3vLx#ZT_qU^8falabOyj?H*fy;NydXbGMG<#oYpmfPMW@uPG^+JDa=&&TsBzfvB8!fP&L6ivVOZ#fxVX(99 z?#2YhNSW2oSl<@CLbm(rTB54JJCsE#_-5GmM%Ktl;j~&=<)-nwrjv>xAvJF2;Z1~i zmybgV{ksl6R2e75Vv5y|1`G)zgn35gyTZ|YRg{K`UVSu)AGlmIdZJinNcB~&8?5bV z$MCa(2FZ$?nsX?6iToWuy&o8a$%KtZh<~Mb8PT4RCnWfU8zz^!6}Ow%u=^n=Xv&D7 zWtcqHKGGQ=E>`c5(J-UdS~*3e%x^8nE&LEnkY{3T#j5@_3s8xeT1~Rs zJG5|>NKRorH{1mpbtD+N;5d}DnjYS;MdV;PYb;-M4I}o_jmwC1nfA+#!5C{2k>xXJ zg4e2MV**=!&s=yAeJvcV>G(ILHyNB!6J)u9*s^JlQT41c*$l*bjRg`U*n)h4vH+a~ zr*Hn#Q*8M1H5E4Ub7NA?nXj@g=sO3q`h7yu0ff-Hpacg>5q;A|hhkOWh)Y@@q4U(bbj^>>_} z1`N`usH2L?`hE|Zz4L6+J%ic{PxN;wJCLwik7jX!hC$5cj1K~x;J_b8^(O(h>3235 z6VTzHpLvaWg9LC^D2tYDfeey<*Pcz%wBvmy(z}@;L;St@`Y7{vkARXCPvLpJo82=v zEZc}$otP2K_D!=;|9}YrJ-oemYxs|)^H_3J*`nwNF~GZ_@WLZJGraenufJ5kH|U6} z>Ig}sPVTc7CiAki%1fv_NVe95gKvt&8#ZW?$S1pL;d@g_(>g5671i?QoFZ%rh01|f z-*$ZCD0NC#D!IPSF&(byZq{IZKYz=rsu#NFp5UWq&=O5PssVWp&+y9p7Qv|b7&!LD z#shZ0T(2ihL~C_fiCr(m5jqW(F9rg%~#3@yJ8mKK3pH5W$~DH2x3eKy?3lWeQzf7Lyo-wq?=GvjIN z`E3~Y5s}dYwgS#idoVacN)}a6>6ekr8@mm0c)|2*g8W(1jE#PMuHFl_?!;(&LU3?L6( zJ`v;A&cYDW7g5Kd+YOt{M*V@}SJ+j?eGc3KJIg@Bc@fE@JxuV+x{-GR22Llt9m4nR z4AL$Z$uTw(Z0G6YrvR~r>`@GnA53q!n9e$F`lI5=TfsK5(S^M}km0Ln{`$4cGRE)* z9^C9iP^et@F*yu zMtw+m|8pOgLhT~$A@eAKYuFE9Z$XPH&^0>lwwA90A&AF$wa6$l2+D^f_?)l|&}rNP zZ_Y<0pi{H9N4&D%Ht@G5H0d3&V}G7HW97Y#`5vsUidpm8LtCUZ$6}vBdONKRe%3|t ziqr-8h9u8$7|pE)gi;m*0>0pBnI)o2!5+Tw1pnzituM4&AIo>VfeY$$LqN)$sFnA7 zU#hmr1JGaE*V29mw+rk0StEx4HbJ?R2>Izu6V${;=E$~q`#ccG`1WLQOqdX%!k_Hr zn|pq_$;2xaKOzh<2UPW+ohFvC8sxiJD%Smdwi$)=8eJh`kw>|296Q!83rYy*w{TR7H<5h_bsFQK{92Rw*E?A*G-5eQ(? z3jwgif>R@sA7)sR8!{g8##4Q7zCDR!mmAY8ClJ6{Y#s=`KAZ!8(8$3^BxiF0I#AF~ zUNyz;sqKxGI-k7U^Ft7^e(D>hJY%&OS_DQUn5V#ZxIWq!iQ&w-K{C>`f#v7J7{YCD zGn=zsLxX_|?VtFdVZ^sb->6lAoV+f5rC^Ms;ss2-IPP4;N&xc8;_uPlEL&Bmbwuh; zRY3G#J$qv%eufA~?!TP&a3GP}!Bq2x1OS&dnZ24+LJ4m45o8-mS-3V`zvrh!2BRW1o_KD9yj;Y$Ee)9poJE4WJS0`y8j z$qY4RHKhm#|5VZs%SOA*!M#LqR|8ECectX4^H7W-_)iA42!$#`%<)dfG6DhB8_N`u z`-XHZb3?&Zt&u;ORd6zdh%e@Ek`4AvM-66nd@-QqvNk=LVK^VY{M(>ldiidzxQ^nfXn zpQp;$Wf()Fs}c$bG&~chEkMb?XY(213MWPjzf(;w_-D$cA6l;rK1V8kZ;6+K$8rA@ z!APRF>7s{K5;=Kj!oaq#EKKBqqJfnW&D%AJG?}YK{bYy`-sJD^1}ij}9+DpZ#&bGzJCyb?SXtVS=GPu)sN=BoHp9p_VP=5QY<5|Po2Ug-RCm_H+FAv@&m6Io^Pzrk7;ct;JGvTDizzm$lufgu} zLWt!Bp>0Doy@jQ5(9ljm%7QsBWW5CSy(Z^twOh|R+Vz|3DLf}F z29A+A&3QX4!BBzXIO-ow9X6 ze47URlTt+Q_v*#j4DhjJLntDVUT4$rnI_wrPlv%V%#^M(nIFya;Kc+v#;lX-NO5>E z#YlEMs*P~PKd$P{H&zWAoQdZfb>0IySAH#3a!wbpnA(`qJI1oKx#rtY0Oa+!qAmpJ%>z1ZH=kF0m>8vB9USgH-|udi@HRIKpd!Yv9{ znoue~6kcOQru-Usa68uZvwbxd%QdCWsU?Wa$-Q()4BqqhDoH;nyNX1mmm%V#JLPvU z=zb|dEu=PbaKj{h;g{;qfgS}CC@1369_!$ih+AR*+HzBy zBn1Pu-rNRdIwrff$Sftc1o2z9iRapzdcF1fN>qrD>UHZFOx_ex7G(0eF6+3mR@IKH=SK&qyNKI|8dUw~q${UW z0KUEActZb#1N)tZbu4*;j0~sYq8gzOhZXxUV;OP@&h^Y_@nrLt=mJe4^i9;Co`{QJy%MgWN_;W@}^DWghlZXHT80)W_$kF_X zuj%+UkS=}@-L3;PhJ(x2VItNC<;ZW4$Jhplc?Zi&vstiVxZ?{x@$JvzDj5FBg;)n!c`KCPq@*lb&FAM|myp!ux zS5Wqwbsg*&1i_3eI2+xYRr5Kn&L)WaQ%qRtcei$DP&2wKw-Ng9ZR*SPch9$63AFbx zpiVc>ZRQrxT-p|`oo-KxQ~XmJhQxpedR# zo_6oMKk4qdDn}pRD3;3T}l>dkNZe076&E?}fWys%uIIBMq* zYXLW9Qq}-arH6g*05d?$zuF$U&qGKA;zOPn8vlr8@a*20ZVSu(a?lM(=xM8v!2-bN z;rMuI(M{hREBZZ%O>{`)JU0|k<+=`O9CAkBWLWiYCO0~#NXC~s=lpLsVNKwF;$^&U z=u!8Ke{5Dv=#YZN`}eXGsDLUVx7W^Qx%G4Z?uE@co=gsU`DG83f1O|==E4r>k;4#F zDr+*?hG;8IW3LCctY4>H9gUzsrNs@bczxHAx)4$fCk6UCY?_T(JF~|1_2u`I{bHam zF?;Ydgk`!k{_U;qe53yP;dwZ@rp~qtgUVmjjr008kPd9xTzoZj4I7|)93Ka)&Px__ zjQ?^p3_~O1aF?B*=RAwtU7M8gG1JzAAEZ%lH(I3yHShF8zb){BxN9Z~=que*QtDB0 zuYT_)lD4p}-SFm-v57f|f6jReIFJr`BOoU9d09tMFQl)A7LI;ebe_|<5fmC;VjXrV zQ*^?7@zWkZ!k?IGx5{oLNE|j0ArUxrfSzt`%k`${j)IUycem)>oRi3k0R1LRaC9ae z$n@~_6=mnn>Ok)Rnd++5jO$@>QOiEU1i`~H^#C%gyY74f4@3(zBd((bKh zE1yiFT=wJHmG(Aq%l1j>Yt=HSl}0vGT-Jee zrgr%NsT(@kn%kY76v4Jo4~R%4#if0XLwW}Sc+~svPb^MhaJFzjvQw%6B zM|#@P(%Xw8itZub0ahG7%=vf5uqp32>ZI&4Qd+u@A37-*+I5I4?R4oe2j#$Ig@Oier|~YS%zT3rWW*{H(pa*6};gTo7OeRxWeQAH(Wm4~Q^TENIi1s(hj`z{Vzm>Gr3Evg?~fov{}kM1BD?B7}? z4jshdAt<>a>-U?(Yno}%s(uJa5KmzulT}DE_nHXpzCk_uP5wBdaVdv`lfyMQ($>C2tqXaqq`b z5oyl6w5ioe^j=7sS^n*fSpQ9CA1@ISSoJaNZod&Gy9g90UvecETyVv*NSwpA5_p7`m( zJsR*wyxgcBzCS1e@Id?wt0zWiy*>ZxV(Z%PRYjc!-YN;De{0VE#x=%>OCx-NoQH=< zoDfcN^aR!B8>N!L+eI!A+3@yH1FhU|5~X;%-zmZ4R3 zVB!-6ObF~Ihc(|-hRw$AG>BbvOjwj2G$ zGN4Ac#MkOdX#`55;v6gEh3`f_HbJNMCQ=ik?3^?Gtj@Py#6HTxo zB4XcaIBJ`U{=9E%cLZJdAouu>pLMql1%K|A(Cfv z24b~lHO+Ewf0RurTKX7z3m7U>aeJuj0Ajd$|M%rAI^jw_0%I{gLmnr3AZ`$f4m$yt zfTMB$*4C=0WYsP+M&Qf_Uu2JxgnneR!VkFbrY7V6F_(b-M246=d1=1A*{wU2Z}Rl0UynELWl=jjPl z%&l{Wf5Q@2gXVbx7X4{IUx?yIH$vfR!B8DIji}uogqM0N(LekqmPf2tIh54%7|ytiB&5dCdT58Ro)6zf98Q8%WY8p_FiS z=R5pOZbqcMA2i-H)VC+~{4p;hv0YXT1U+%QMUA2kP?J|}v@ofP2T?xrUnEWq&Ylo3 z7&;n5R{z?xWSJ!PLsfbHREHU{ovE-|!9{<2CzCEi0_4~xeG`99KUJRB<{j2X-Lfe+ zQR5dBYXz&xR5UShBssWOd-HyPOwVp|aGz%cAxEcV2NXUieF4YexyDQQINy10Z1dI3 z-r24CO0O2TJ$p<1ZC`71u`$&`M4c>S=V(8Q&jduD_ufKigKWgvQSig5s2HlIxyq|Gl!ZD+YS1vI#OawxUat``p zI&51vE$koO6#xU8VwhNVwliK5%piy{&IzU7D&&L_@v7}4w*Q@T>t7P9vwB@}oPmty z$A+OUm=-PQg>|@icwF7vNJwetZ9AqzD_H0xHKy~n2-xRw(*4T*8zsRo3s+WAv>Naq zr8IG-;5_GyAN)Oxp$AVGpF(%*Id{A8D8$G#SFRD?MwYQn)9;4|vPWY?<_-}UjW~3)?@ySRElGL^PUj?TWc#Fu1$={=_5S|kB~R#{ zTGzi&qN&O4VD6zG&X1H|;uF%%EvaU({bwPmxp3r#zgUitf#Uf>Vd!&a%q}!6{B0uK zdo&c(*@~~lEsz7Gmz2>fbeg=wqM8_id#ps7I zS#F6{|LzH~RyH~;ALWuSTOKvjnVh`gk!T(uNScOVJsg~d2yKw18GZm-9XiZfx)|i# zok(Qn?pQ0twUk?h=|W2Fg`UR7_i|{w0T&VJ33}TJ1|Ox}L{^R$F7a1{fn9qC)p^qZ zx252V%gvf!$DO$5@2D;}QHC$|&8?+Sww{=^9F>X9UPvbvi6|O&T$2EZxU#;Rc~il}siYI{>>vrS%#Knr@|#I#jKK0Cdwe0qc4 z0J9PTJ_BdMzlF>|A+TxtLn8&Vt7%um`P{`__CZ2=p+)F8){@>C?6x)M>u4gnj&D|w z8{@u~|H%~m;|YJy{M>~M$`arfCLSqtk9o-#e^8ls{HIno(ApQ5Z)nJxR;nVlkX3R~ zCp>A-pFx>sXd32b!^XGR&PdA>i}+jZKo z`Pw1uxFIH_6uC(G7Z^dsQEKju&hQsd9Sw_kn?KA;`vy+uh`oi~NaqSrlqywq9x8wl zrzcQa1m$9Uzqy9LpQ5Mq_KC|JZy4yvgI^u_$6%$y^=nyz*-{N=D&Uw%h#L)xjH0(t zmO$R9Ll^o-%lK=BaqBo3L4HeMuVu|yVxlt7EH!AA_lTlj%i|i>l5$8y zoYeK-*Sn8yJ&Z3Wdo9g0H?i7zf8h%$H%E2gJPQRY`DK@no`Ukjh}b^=+|SIA*6P;+ z9`TBsCOO|(@l^%%_gS#+T%!-&Fu_ugkW!~O`54Zr21kQNcC?VIYE_kIET;coFx4fa)M+KAZl_j(5_e#$vI zJgOnjd3P>5AoqlJ>+Hj8MjJF+D-tq-pn}=Hb(BcVPSeSus&4ZRvJ?dE1PR}pGE<

7<|6e?Wp^;CY@YFP&QxH>6^|yg8otw3G2rNm^kj>~;DWjT)n- z$vZ_Os#E=QN0xWBcg}Mve$cD6qG*GMAXiINn}scJtrMtRwI8uKTWWns`ifN`n;aQy zAUuC~sswDVQ5WgiHos5VJ5L@099kr?TT?1$?QkxJU)a7u`VU9Df5!eY5)cXR?!%!q zk>~pjK`YLObnD@6%GLB+;adfbz){3y-9UL=p`LLDsR@1z8DJ=k2@;-ZB)LE&n(QkX zZb2l_kSew<1n@E8Ii#Q_sg14GWji0Rv(V{92eqV4#*Uz(MO7NsnF|>QMDG5#t@n#W z2s-DUZw&yS?nxjYe-7k>f2@ta#u@+m=bAGe=JFJ^6ie=hLFBAm{NcX9H)7o6T8`;nc zOKyqjNN@sH%k-6-$4`aL(uHmDMmT(8(6?@JeHX}#*HG}e_wAP~?`P02s4)jTgMEPj z0K_2v4Qjf7e}!7w)>Ycr#rUt8*mnuxztL9Nw)sT+z55ewzDksyCDI_MQWlv6GGm28 z8+iTzi4LI2NaWwVZyqor2T)yudaU199Sk|q_K~~H-&mhDbbVigb z#v$E4Plqy>*4Zwrq{kvY7`l%}y*#)K_Iyaehe;V^c5&{Of(6P5`G&~&O^pi*cy`UW&At~oqF|Um!8_t=N zUbu%GIM=q|D^1N~8JwJy$-vTijcckWdyt~I>dz4n zo8_=Kbc!k-hyvls-ds+mZ&1Rkrb=xg3gCuKcGtN6Ij&eAlO{nx5GcQ-FB%ig4Yj-@ zAWYD62!erc-9iW@ozsU0*dTwm7aZlyy5+>Ee{ZbtOM52NtrN+`Nq9;Ry_Jy&jq7@B zX;5s5F%YXzFQLeL(djlss>VQ+{t zH|?W}>%w}>J38roP~zqf$c?1(&iDr))gBx& zBxKcqPVjmkz6Zc<(^t;Nh?@9?u<&M!f99oWga(#5yCHH3-Zve7GCsw3x_a-AANjie z*Il%7dMwD$Ps9)Z74fgti?WlsmAR9-u_L{L@!um}C3EY)Mmmb_Hil*nwl?M-#tvV2 zH!81x;*IjzR;iuKP=nU^$KnGfxzv_Rr!SlXLNE?Q)H3e|HOO@vg69`buqmGze{J9N zUwzBriCnMe6M5r^t~8xee^AF|`nq|RnU`t+f@+kp-N?A=j^Lzy;2Kx$gc}qT3!v~ocoVqg z;7A)gX;R2T&7a4i15eI zxOT$s#ZuMGW&5LyW4bnov@eMUbkk$CkBn!5XE?V>WOO2-oPFz?*g+PMpLPNCvWg@- zrVUiV({^$+1PuG80HY>oCDSXaRRaJLIHF<$!JG8-&0Xi2x_RQgnzH}ze}0H-meL!i z!Jlk$XR=+(NH`X1#GKVf!Nl|pdt~I86VpIzod6g0QXGE(?@}KF3bW9f+38-^d*&w+ zmuC8T4^9Igp~bdY--iOj$dX%kU;STY=pLo~h&V+eNH`@d(ttk*DnDd!WY6E%ZP_bo zb|)s@!VNt35Z=;mNbrKIe=K3SQn!WRzw3A(N=oc1i}ZsoL3bIoG1y(?f{Sl$n`k~+Ca^`w7swVfOKD`M;i8lu6pZBMc|fAuqt$)J@T)Jq;;`q6FuFX{@vkS$s+QcTgxnd7)3H`?nRdxlOtYLw>M>+sc_cCC8m z5BNdM7W(Sy3mL>q;*KcYANs98vl z?N?ag%T%S(SE@BLP~tek(lg<=!~#<(i%q7%@+*eQppx*!Bk{SD@$t&m$xBII^`sng zHKHt4N5P7pf5_?>H3(B*ko3HqBEh-y^N{fpo0kmE4;XjjwWKSZFZSa(-V3}2&t4+= zk|R#gIu2g}KV{@xxiV*XIQgp(OVBzJo4?ua*!jF&_+ z)f>uJV^rr_Be^~@08Ehz@5H<_9XH#yNnb@H=uNCNp`G_3pofK3Dpkiy~ll%i9u6wB_y5~=_sQ#d(5smS6b>pR%}T^H*c{arS!+N?6b zcyFpg97{nDJV@>Ix%l&(jsk;Oy#SCW<5pf9*CCm{J)F= z?uN=tOs8LFH^~Ix&JKmKL^S2fByC-6@?tW-f5r~IwFv^#4j^d}&B78@S21cJY0!eE zdd=PesirR^!r-sU1XvptFbMhwA{v=JgU|;bp3_=lfH_|NN>)v_Tx|l)6H9r4EtuG) z6eABOl3E`$*UF1$O5Q5Ah7j%CIwO|2%-o9*tHf$ksqljBGT0JaC^U~SH~_gCA-X_X zfBVqd%fwmoZ9v&d?I)Hl(qdS_)TRrxxjA2)c6ka2*QLs-GVSIxJODU^%4YXozUJ(8 zrsG&&<3z@Ox9A8oVuGeZdc|}~44&ZtgBv@wZX*=5+Tct6`K{+8uRTvF2YkdeDtCG3CQTpgwZRpHQ zt4F>xJaFkUk3#NNRz~1us)|)WfOLvyc;2ywdj1VbFDm*W0F0PloxmL6wK~|-EvP-n z?Psv?=J&?PmcS>Xwf+xySKE7dyBIh-a6N6e_F|1k#GW$?z@Z3+7?;$|#A9}}e@^(( zB#&S&d%0UEo@d1$%Pt)T)pl?hA_VOqx5RSL^dL(;KZ{s&L2_@3ZLTD-eZ2!Y4-nw< zYZ)Qx5YtVx#}FrsS0M_1S&s2f9cEr?^*}~+2|ixg$mQs2vG7 z9qd^c8KGjDQ{5Q^7xQF|Cb0Dmf7L70zu-v*h;hn#^etXuDD9>J%6e+Bev6>R+~+=c zM?8qvk%J!X_2ln}FHKMA0R@ z(<7YX1-im(*0=K2axV3Bh*A~NtH#g+y)GP9JLRs3BKz;Lz4(HVMQOLeq7tT&daW7V{Y+=u1emYVX(x?7FnnloKZYPg z_0h-k&u13~;d-9FJR?`D>dKXX9bTOEvRV~=ZuC4}H3DY|9HtXfA)YdDl%ITbf_3CA zA%@?D7`EAd<4Q&nDVS4%rCb})Y0wG!_XH_bW8Lqu@7ldM)a3OCsnW4P^h%qW(!U^S z?H;2($TlhRenJGu(fOJjQ%|zKNgEszao&_wRdZ`);lDs+etb`Pzins(wuy|BJy5l) z&2bi=s|$?ep`ZPtV$ZfH{j-Nku!9VbFTeQ$8LAbCRyE50t7_w~9E>w5C9AAVd8N7U z@A4o}x|FF^)lZlP0h|l|4B-S&!r{3JaMl&Ps|UIIbChy-=fZsN*XP&MrWN!@ z!k2mKr3UH|IMW!HT$OjUBkSNd#qZ1R_Na44lkBa3HCgJcaeIwMevIWQe;^plUCYcg z2EQY(s|OoVg(gFnFwm;c$==XzP`STRWmO_Fo-i^7-BE~*Kmj=isHTL+W0yPM8Oze7pLPj1oZ&a0<4tVdVIV*sSgmee5{tG`%SxfI|GG56&Y21+KR z*eg0oVAvkJbzi%k3eW4F{mCv4Rd1RQknxLE;bO)H0=C6XfmVT2o02I|V3!%*s}B8O z8_Hq4b?^z=+%mD{TxMEP1iGdD?f zPW^JnxB!Ni*%>s?WkVZ)ZrjgXai5e@oc3G?@jZJIzaD`Tq!IIfrGIq*nZYL3CbC!f zM(&zKe>cC01BGC7nyI6mk@d9JA*T`#u5`r230aVU1m^xbQ_~79-5LCoGN!TlEcBzV zB(Fc6sqLcOuC9@JPmLSSFc!=>gg!0g2=ELXpaNJob<50B*V)k6lq zvOi#s;@kEe%?^99*pY-MNqZJFHW3t~I^#uJanv}p0I_|;#*kD>0T-i)#{y!$2-G zMSsDb0>U~!Aa`` zpjk|87XBV2_3d2G=-1CqDN~7%9qcx)s1~g{X-v19Sz}+W$dnDsF02f*6apj%Rc7KW1-7};3#%50 z+^h%ndtF8Ix&SUZrxKiU8jVt{V%b`l3AJ_&m0w-ZoIU4MP>(T~S%wmtB+{Wt16ni2 zg+S`S##BMRK!;ln+TB_zK0{2<0S;K(EKKr6eO-Q^UbN_R>GuZmpL2ebso`liSU~1o zSUBN&hm7xR(BCi<>s)L&h#5)nzP4Y#F)I@SaM^hKWD*L`&Wb+9JKnNdR7!j+f42|I z_L5(q^p9ZK%8mn&zpwAMZIT=EQt$Gj+5+kSD9r{VM3sRlaVget=o0bIuMwpWh>b%f zgMW#lw#BVb=WQbM8ny=;%cvTIuU}V<)i# z-x^H<(W*y`FR<#yb+T@^MT{(xhnMCXmdI`57-3~Y4p$IhA z4&?0T&iq`JW%XX=hQz!IM4J&R+QKZfvm8QfI5MRypijtJbeFURJvQf_CV<)|TDv|Y zfN|&d)D@ zh60_>&r78tP6PnciSfOxbT@6BZF&|+>rw7}1Z$j7zq0n-(6?)ENjq=NNrVy>hylqx zIh@X)ohka`3SbILB@tyv(1g*<^HoCSDx1(UWAM0_$rFJsazbo);c>=uY(UfXCZ5aD zPB~v&m@Zgf6EeY}--l1R?W{A|yQ-&Kyf)3=rkM5gPla8WtjYX1%qhl0Iu2{oLKdH| z+`9$DA#FSc+N>uZHE3sBszst`*nnO1vmHqB&#+$eW_9sl#CPy_;NKsgCuG^uND)K~ zf>uQW8DsArXuLo`9^&OnBXItN3tmGTc8!xntDpJv&b1|^#%CdW@RuCUNc$vplj>dL zk6_Ipebf6?E60Y7jlAbM{xma;vUu?(d~yl|9!)_RoPvZvZH#mLiYQ23yGGjFM%)=2(0v+91 zqZl7bfh-IjmPmpc49HzyE+?j37$S@!XuB?n$Esm+e%J6~RcJT&kO>Gto}5{SH26H+IdR7nnKdB92V+9m=IU09G4) zzt6>*P&kKb7EB59RD9;jzts*5$kLb3fSE@8h=?ag0x{)wTuf#S69v(5wp^#-c?q(B z{DZfjLn}LXYa_CJEBfHI^7|VhIvtHg&BJf#F{k~X0!eB+Elc(sT41eKV!%&M&Ywqi^!X4;j({PBJSlw zINZ-`oWFM=yB>g8x!0y7Z<%8xd)Nk|i}Uop(`ZT;+yZStp}EwBqdq-7$us0RZy!$v zGW!`@DE*-`i52Gt8}La$e}~Im!;4!&kyo`ah4U^3)hSe!=+H(%|H3fs)Gj2nR`Z4Q z+mC!$T5WAu*Mka%aKTk?vsV~X_pSTH1hI84q`|4{E0@U0;gSBitq;FdYy@2zQhg^8 z{bAihR-kgX%OO=wU}jro^_SO+?JKy7f zhC1_4wY3{WddXziS-#GY@lDU{0RQ>yLeH9dy#}bI*7*EeZl?rpdoWh{W`P48`UOoyptpbHqRxpv4A1i2$eRZTY&aW} zM!4)zlX8a1Se=nx2oLE{p1pNRP#y}u9|uvZI(c=|y&Ao#RO+^PO{#vjt>A+`8ma&B z;v?_?EYZ43W4^k~^=Xk-dz7*4XrT#Y2>b%R9rPNn_Iq=%@7PCHjk{~Bt6W72m z8^~4J(nIyti4de}N_ey^O9)yuSO;U`O{n+F=D_(RJo%jv)bM-H!{tj040s_lHRFvu zWYO??(Mc>!Nht&Ox&Ds(PVc%$dJD1av>0n|clv}A15IC9M&0yE3!{J*=i<(wAI!NyKB+}bEO+Zw!47P;&X<;e~!^Hok4<^N?&bFU%Rm)bj-Ms&5R z^Vo;-!)CM2+eVLA3pEeE1UM>z{(GuI@L1ldR$z~_O=r%_iI^>d-?9Q+eKK4xISrqX z{ed!q9sL0B@+O4oI9qsGnfQu9Zu&1{;2;m@t4dUbQYGrHu2A|rT`6>O`}-&y-eIwN zzTD>Z6w`O>z4L;kf-ol0CE^L?NUg!7zI zoV#1~GEEuYjQ50ctXX_28|!*$!yOn&Q$}?mqKNK{A+lf`G=5kQhZo{bc~%>>>cQ6j z#EWTc|Hj)2bzO{#jDM^eJPaNOP5Bue`cAyggjf8+&-#|_OB;#V2DtYezVx~baP zyTO=Z=IKKW-R#}QEF~cG?wt$hEbZdKn)OiCAyAx|$+ho{dASfEsDqbDgn(cfV{1c1%qET<}i zb(PNX8HtkxQ^r_<^J6avUgCgsJC)7!P=Met*mTlhAqBV3a^%jX!Y^3P^F*+ZDg|Je zGTC9!Y}T-9#sV?YxM@$9?5!XOPV6tqn3D+pfVq26@elSh-UXPI3iLZ(k+M0b(v_q4 zEbOTLno-aBo0^xt-#1g5@mO*{d{V)fgC>J=hna)Dc@F9kJPe#bx~nw>UPAVZT+E)M z1E#kR^O}w&5Bp-aO*mbdR@~gL=aiskXoEXv^*cXCLx8HJMQz111fI};lM$!xz}``% z%XwhGAFEIW0=skqo{;i_AFHnZwJ2Dy=kRxZblGDbPYs4^d`rl*n@#?=<_HZtxN0+& zVLjTRT))-|FQ_FX*Ypi4@2y_o;WksM^P5W_1!@%u--?hKm0gH@C5cXUYCgf*)tfQK zyXAm#UL>&0sesk$`3TLC4N8RDCD?!;=idNjv5~};*e7#Z zJWG90*FSAPviR1(%$Xo<&wJ6E3kWTfU;6Kd4Jcd4m4ls+Yw(iwmyoEvX49CU(hp|ie$#ZN z-~u8BV(MUKlWrmflU9}bFh=ByJ17=L9eZ=QakBtRZ=8#Szo%A1ydIA$<^DKFk8o@= z3!=anV8+0f5N$c{)iWI=xn*SIQieo6DMVke;!J5l@CKv!cT81ZTC!vqY6CJv0f#fA2ddh4X;Z61ytlsDQ!OXIQI$HRwH=~f6Z z`2%T&J=NAAn^&&2m~ys&pFL3_R=_^{Wjdg}whZ{^J{Kt6%%mnv(XVGKRNGXYQ0Y#rmKMKX zk5e9ggNCoO86Gl$jQ6~p!k{Y=;RLec9tA8GvbINKcsWVaErg!ZK}pCFO9#6Le?tR8 z%vRjwa#pd0ctLS*TPf)5*tb7$EcyXkB08rD?&}*qAD!cOw*yHqM|YvT2e>i*g}Kq8 z|HXfVb$D&mYIL%F{PSD1OMm%yFQ^&SO;cZ^+JVoIk6Z5dj7ozoUlq@E=q3G%!&_fK z)k0RXMju@k{u_C%tp~wl*Fggy1ttR6iNst1w4Ea)d|Cj$98$G=-XEbY^}GguD&*LT zeYW%3TE0F03|iRH8F)c|OvQi>-kZXbPPZnvnSU;liZ3QI^R(HT4>g(ZU@oOY)*u!# zl}OK|AAtC#wgM}qrnpDUTYXtk7F)!-GW}rOunij`aBgIVioi30{*1!!^`k1m*v( z9iN@Q?W`bKF&L6q0K^Q$Hj2H-hw{{< zuk(0I+R%WYU6em$@pf6tZx^L?{iZU@1w#`0(S)*{;?@xEEb9?KnyPd#N8^Tt0q7Id z-fNg4hw#FUquoDIUov_X^H-x0iq)1zpr1a^D7#6tuKU!4uiZzmcZA#8p;_+TVQ4An zj+P5*v+)bVs0hi4zVZbMaTV$Q?%W@*KLW%0!@!kH&?o+A=vl+PoHEx3!ySfiw^9jl zlQ^AUVR`ri;NEn7H|S&o{V2v8KjBR#BQ!022RC$-J7Q>dc8 z(NMNo^_-meKblR^|jPw~y~ILt{R z$e`JS^3Ay>7JOCg{A=PAVqooR;$xa;~W7|s~jvl z1fFKiv?c{TtWiqj>X`-B6wT5sBpL>2)_)PA|1x+xx3V-&3iTEz6I@q`T_~Bgqnbxz znff3^+`qevYX28lO|_5NWm{P#^dk=1qe&G1-?Q(g!bFmspWJzTUD~P41L=e}u_Yf% zmG#iBq&tqb)RtS?KDL9Vv47Z$1k!K6_b{{vk{y%N`WwD`kj3=sG5l&y_K$&&p4k2n zTRYiFxvU=#S(A33|7K8+B2E=X+6kzv@Ry|aLD`eUWEbeb(wpEYG7Ya6uu&b(%UGt@ zY6mC2_&o4*)=9pp?wf_1V!a=_L-+eyuLDv%Hg!gr3y-07+RgS8WOf_p_rF|x<><6# z&<@vl8xile$;95BqSXXDcxsHtjdlnhwuEtg?{WTdKi*U*3mFvL-pM4Rf$ThPrrdAn zq`u0)`U84_XT5hnzO}qQLEJ$mRH!UXCe%oA9*qy@*jN%46hORD9)dz0t>1%d81fpK z*W4Ve$-c)?Gbh z-Doz!!ZVKU?}c<9w_Wi16j#r!2wFlwP^bIRTSd0A$-muX2ynpjM@`3bO`Cr~a${RS zP|(;2W*_AAvI+gXlp}iUvh%JRA9Ls<{-Mg@seSb6p^C zcnG=(mCZGpmUpYF8clH+Esr3hkwcF(Jmfh7oUpsaDC2SU)w6^_E8c2M=||?vgw8&r zZAr|K0zo#MZQsOL|J$vq$Kv(lWtN3_Q@g$(SbcxVQ=)$j1{85M z+I|EnlE3mU%nlxr-2J<()b&VBm5J+rd}m1gtLV5%DFNlG_6DEcwf&Ytn9 zv-NiING{b{K8sc`wHPo4^i+9*01jlg@rX;cHuI1UpJxNi3J@+pUeo=}`E1-G`~m$` z9pjqBA$p-SBCB>oZ!Z)IIg~X$|N6n1hzD0Oqhe;b**dN!k~4=HmrNj~pXT)YQKlh( zW}|e`pUgg1LU{F>F&(>oG=7ZZAQS3#^9se>&6J37zSW(5-_vh@V_nyE$#7ZLX-Dsk zVd&QnT&~v4iIn0cIov(_tO69{%(7dEQ(-`2QH_(C149R_PYs zZ58Rk5xPuU2jfF9nxq0}VD5fPGk#U*3lVP7yNoN^3HXRM!$<}O{2a{i^A`e@4?cLnLsGFBrZ0&@Po+v*uvchv{JdgwFde$+O_1TF40|NdNY5O=a15 z_p!{CI!a-zG@V~fJc0|aKzvSrAarmIa(l1UVc+;(PdN!pZ0cJO(w)^GkUyfNqg66# zMrkD-6<^fOQi6exvzy7gwZ!~~#KLsV9boa2o=kdHrQiv;2poqgqc!o5*}^{*wS%Lm z5t-=}^Zh#e=NuNymM^CWnLMDt(@T+Gpq1|T+K!4;M!h|x1+-%t*B;cpZX|v}#QFce z$INc9CCzI`F+!X0C>LXPU}L;jCqM|@eku0CtY>cCE0n$+c?{1n{8_adNMkrnycYxH;#jPE#gs*133tk>< z^=hWB)+%BSBhil=Jg;j{Oq)gFrz+FS(m^#sB(3mH^TAU^E^ltwmuYGfNZLdYL$Knu z44KPhN@D<4E2VxDa9pdA5-A^l0A;bxeugJvq;0Re2kerhUmKO<6E>4f zJo2WWul!3x%K*RWt^)N`#`jd}+2W?M6xzj&s?7q?nBC0Kxx>2O20n=2LyV(jBB46N z*_9G9iBekF3Eh)aogHRF9gj^_HC)9;T8gG?xaSYSty9)c!7izyyu`){d(ftf7l$xz zErWT2ML7g9SUU5j7Bv~G-#0EG=E?`0-C2Si_4fFLEk*DGJ>;Bfw0)>Ft_Cj$MW+LhBwg%F_FOtuv6r&17nQrPJgZ9AcwBAhuPzOaFL|tWgzZ`uaKB*tY&E?mma* zDSIgHHXyd3f^^J06IvM@;aa?JJZ%|THq4uLN3lCS`H_=>$B47GdP}tv`@lMqGeMk7 zp&|L@|D2T#|$~Ke|o%oI?0kO$M1~F#DPB&5)YH}Zl zdKP<1Q0u@w$!L?R^y3(uN6^lOIA$sBZ0FP}Q3@@nE7RU4Jbg*tSwpy7aEBZ_0KThu z-@)LpC`4B)m}VNuv$7jlinNg3P;XD{5<_lF)<)>;bL+VD2(~_QKT!Eb9(1WuA;G4e zA*PH61Reg?P9z=v?&lJZE~MpXNoEc_==_dlP%IWN&6XM4NN_`>gYHB?K*Y0&ijhP{ zq%zKo%T4ROmI^>IZ?<9IEYNw*kh%SRy3R6db42-eXo|FRE1D8X{@lR>GuR7{x(oj& z#^aZm@I$PlYvGFAGt+WL2G$52ubcyVYdrWW`vBhyc?$o#A?c@yKXWVLi1REEM5rQ2 zrZXArD+VbaQI{2%G!gA-X>Ky7!{#@0!>MDqc5ZvNAu)V(Ulxt)S^Gf?&1rbEWW16I zVrw}u4aPYrMqi=?hn7JOdeX(3+NWzY%I(^5E?Yl$;nW@;%~OL{at?7zsdl3G7poDf z5*|9>@Zs?Orr~3r7L4Xr+cW_b`!4qwm;ZUGhfCfl5@cVRT?5mBd9Xq3O4w-GI8Ddj zVN4d4v%L?Nb2N1yT-wRm<<-x~?d={5RJa;nUO!4Fz}~#D@IGx_mc8U$3pzZYh{(+h z-JnSL=ki$Y8sphf!x>6iUGOtnk0TJQcB5yKWDF}5QfZwa*$Zwil>ZwrK0g@!J2H5h z9J*@Bea6^&ABgL%!SUtj%MLlb*0TiNbQL|*6x5Q@)ff|KMAH0IbJ%UVGC9ULOG-WR z(p=rQ7Ipi6d}tJm@Q7Xb>L8K_&7Or)aX#!K*D`o=Yq4yd-C-*q)AwC3It@8aNtqt_u@NYg>v^1YJXhl0S*p@pIl%9N^fvunGuJ6!Mu)=+cE*-S ztARN%_R5EjX^AC}mHg~gxc(bf`9QBdB8#oyRR^-ZQ=bPUBD zRal`fzpn}woba81i_YrRHyc<*I)5wg{~WJ+*^iP?+TZqy(!|XNh`V)tM?dt?!*!5fOvo8NgcRF-) z9UYLJ>969~dPi#1_|_4AOp(KBQYyKGH}rogt`R=JfzJ{{_^A-2!daapW$326 zM2BqttjMTKAhlxmx)YLA(4UgpR+1}CwLgF_{tXyJ2OKN_B6{ghiolF~HL{rD zg#j5YMedlgDVe7Yr)TVw5nVWAovw!sgkw9GnqhvM*h8Uttqbpq)9xgOqXd;I5z(N3HQLOA&<-4e+$ zgd?|PHqnu=RUQ?f!*7&=$^!2^G1PMd82dIAVr^iX`X@b6bJymf;|qBo_@2y@1e9~) z6}Dk^f}j}$IBAk994Qkf%Byhf7U69^jOT$JWu2m}p27`Ikk!o)4|yIxxl58G>yiye z<1B(85cAMKypj3T!Gj%z^-p!6le!cq?n7&SavO2nur`bDe@U_27e-&V)U~uf%Qtt? zg_wQQ_M!`C=EV9^z5=;sKc^pBfe?;L{KPM(%dl z+10D#(MQ+@%A~MBbSZQAe`ONo&zP0t*@Q)(!^lHkTgZ+QWJLRgQccICr1KgI6ltj2 z2AzJ=up)kb{;AoySvk)9^m^j;$>syjbl1$>H1qYGynS>U^w1mM?ULl|3~u?c{8R}+1Be-C}};i%T`YLFN{?JHAqa>XWOq)RbR>C8a_~-ygM9}=Gokq?mRzrC##x= zcOH?<$j?yvH!3^spHhE_okK7@`&5cEL%8l1MDJt{Pg_0LxOhI*x;C-b!H=Q@367$51)zp z_5ebD&9M_A`;GlsfMZvzwe-v6&-V|k>>GO2rq{K(dF?Z`V3p#-4;HrcvV@Bn4Q>n& zLt=ONi0z#~w%V}gT?F0O{p~StMp;cqLy03SMnv>o-2<$!SpsI+U8{xoNJ3}j;)wDC zXzrjMW79weqUgE7HqjOM@Z&9e_-92zHViRAN~O-RapASzU#FMnhXCEsr8)P-VfZD9 zm@VRr1RBiWsgISv?nL=*OM#J}*;sL%Mo{GDl$6%Z^vv%n@6PTkMoB8C^j;E6%6EUE z{Cqs=3-p8W#ybU|_j3?-I{%FhPL7l=Wa*H2ye%$urHw~`EH89`&^)?@@`SPiNrbaz zr88cS_a+u2CB8zd^0fN{*{Nz*1^S4W`2FR3T_H*e7yWG=P!KZ~pG({udbZv4wjsXk zUb_S?sc-XPLbUxmo{noIix#}TXgiii@v$3dfgDLiqNdM-65%1GM5pb)nKVkZU{n!i z7h)&$W<1HrWq#iSr83N9YntJh%ZL$C{}$PpSUB9qP}r}p%dHXnq_4zzVs|aZ6s@F^3W~#bW#VqyuGhQ1b6hz98yT~>ueRS*lsY%{ko(8ilz;| zINGVs{cC&(GA#LSv(Qi;i-^MVC9t8JsT*OJv}L1yI};dBV)CNhQngX z{@K~_VV~Mtlm{|dtQ|vXv&W$i(Nl>##28wlwina@Au&l`(e2M2-clpYCi_ zOm?y_l$ni>6tS;|tB~9VQ&QxAokbfXJeATBPbVBx{_dQ8c@r1~6Aa#YFGK+^^M9ov z+54)0BXT1{0c3{)Qv8eKsAFb&fK#FNaQRRl92*GGlQTSUl^^!|*${%6?BH=>fTtOw zyl)Uj2T@~~4hqrWy?^AwKPw^p@OJJoiODjlqfr=GI5F>V0gj%KL{LV$wi13Bc_Bzy zJHQ6(pR{OjfHyLLAPn#HH_BV-xZE5#4Qx*c@ zD>uDxYT7xZueWOX>_zR^_dh_I$>LBwY0ONWsLCp!LsHex{aT7VW&EbNJY z0mE!h!&FDGQ@42QE|KFQ;G1$W9K^4wsa~PoWeYS|%USq8v46hZQt$GskV__CyfNjc zm+v7vu4IBkCR{|Jtml5C^xp{1&8XW)7FVtLfe(KD+kac?8Ol)f0jm#Z*Uf-U7`A{N z_KyZ1h9kCT9ilg#WtcIhHfLy1FNfnSgf3nvO8fkYEoDshXXxu@g#s#I7?sf_=O3h^ z30qhA;M_u^WLzM{g)KlR46uTP{|TM)J8g%*rb~bU z06)I^|Fo~+SIY%CGQiS;yM=sLEZm} zF`Ph~jIcOFo?py_I0B90qF?~mDKP(!*?+MOoD;J>3F-cS7EHa7QW6vZKnN88fcZZx z{@YRig8>;a!IBcYe&Ocgb-o^d9Uu8xCfWZ?`|pa2$Y4Qtbg<;0aVA(~=qYm8ue1LL DzN)HI diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp index ee623f3ff..03ea176f6 100644 --- a/toolbox/src/DiscreteFilter.cpp +++ b/toolbox/src/DiscreteFilter.cpp @@ -1,7 +1,6 @@ #include "DiscreteFilter.h" #include "BlockInformation.h" -#include "Error.h" -#include "ForwardKinematics.h" +#include "Log.h" #include "Signal.h" #include #include @@ -10,40 +9,38 @@ #include #include +using namespace wbt; +using namespace iCub::ctrl; +using namespace yarp::sig; + +const std::string DiscreteFilter::ClassName = "DiscreteFilter"; + // Parameters -#define PARAM_IDX_FLT_TYPE 1 // ::Filter type -#define PARAM_IDX_NUMCOEFF 2 // ::Filter numerator coefficients -#define PARAM_IDX_DENCOEFF 3 // ::Filter denominator coefficients -#define PARAM_IDX_1LOWP_FC 4 // ::FirstOrderLowPassFilter cut frequency -#define PARAM_IDX_1LOWP_TS 5 // ::FirstOrderLowPassFilter sampling time -#define PARAM_IDX_MD_ORDER 6 // ::MedianFilter order -#define PARAM_IDX_INIT_Y0 7 // Output initialization -#define PARAM_IDX_INIT_U0 8 // ::Filter input initialization +const unsigned DiscreteFilter::PARAM_IDX_FLT_TYPE = 1; // ::Filter type +const unsigned DiscreteFilter::PARAM_IDX_NUMCOEFF = 2; // ::Filter numerator coefficients +const unsigned DiscreteFilter::PARAM_IDX_DENCOEFF = 3; // ::Filter denominator coefficients +const unsigned DiscreteFilter::PARAM_IDX_1LOWP_FC = 4; // ::FirstOrderLowPassFilter cut frequency +const unsigned DiscreteFilter::PARAM_IDX_1LOWP_TS = 5; // ::FirstOrderLowPassFilter sampling time +const unsigned DiscreteFilter::PARAM_IDX_MD_ORDER = 6; // ::MedianFilter order +const unsigned DiscreteFilter::PARAM_IDX_INIT_Y0 = 7; // Output initialization +const unsigned DiscreteFilter::PARAM_IDX_INIT_U0 = 8; // ::Filter input initialization // Inputs -#define INPUT_IDX_SIGNAL 0 +const unsigned DiscreteFilter::INPUT_IDX_SIGNAL = 0; // Outputs -#define OUTPUT_IDX_SIGNAL 0 +const unsigned DiscreteFilter::OUTPUT_IDX_SIGNAL = 0; // Other defines -#define SIGNAL_DYNAMIC_SIZE -1 - -using namespace wbt; -using namespace iCub::ctrl; -using namespace yarp::sig; - -std::string DiscreteFilter::ClassName = "DiscreteFilter"; +const int DiscreteFilter::SIGNAL_DYNAMIC_SIZE = -1; DiscreteFilter::DiscreteFilter() - : filter(nullptr), y0(nullptr), u0(nullptr), inputSignalVector(nullptr) -{ -} +{} unsigned DiscreteFilter::numberOfParameters() { return 8; } -bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo) { // Memory allocation / Saving data not allowed here @@ -56,9 +53,7 @@ bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Err // Number of input ports int numberOfInputPorts = 1; if (!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { - if (error) { - error->message = ClassName + " Failed to set input port number."; - } + Log::getSingleton().error("Failed to set input port number."); return false; } @@ -71,10 +66,8 @@ bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Err // Number of output ports int numberOfOutputPorts = 1; - if (!blockInfo->setNumberOfOuputPorts(numberOfOutputPorts)) { - if (error) { - error->message = ClassName + " Failed to set output port number."; - } + if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { + Log::getSingleton().error("Failed to set output port number."); return false; } @@ -85,7 +78,7 @@ bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo, wbt::Err return true; } -bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::initialize(const BlockInformation* blockInfo) { // Handle the parameters // ===================== @@ -96,24 +89,29 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) std::string den_coeff_str; std::string y0_str; std::string u0_str; - wbt::Data firstOrderLowPassFilter_fc; - wbt::Data firstOrderLowPassFilter_ts; - wbt::Data medianFilter_order; + double firstOrderLowPassFilter_fc; + double firstOrderLowPassFilter_ts; + double medianFilter_order; // Get the scalar parameters - firstOrderLowPassFilter_fc = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_FC); - firstOrderLowPassFilter_ts = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_TS); - medianFilter_order = blockInfo->getScalarParameterAtIndex(PARAM_IDX_MD_ORDER); + bool ok = true; + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_FC, + firstOrderLowPassFilter_fc); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_TS, + firstOrderLowPassFilter_ts); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_MD_ORDER, + medianFilter_order); // Get the string parameter - if (!(blockInfo->getStringParameterAtIndex(PARAM_IDX_FLT_TYPE, filter_type) - && blockInfo->getStringParameterAtIndex(PARAM_IDX_NUMCOEFF, num_coeff_str) - && blockInfo->getStringParameterAtIndex(PARAM_IDX_DENCOEFF, den_coeff_str) - && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_Y0, y0_str) - && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_U0, u0_str))) { - if (error) { - error->message = ClassName + " Failed to parse string parameters."; - } + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_FLT_TYPE, filter_type); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_NUMCOEFF, num_coeff_str); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_DENCOEFF, den_coeff_str); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_Y0, y0_str); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_U0, u0_str); + + + if (!ok) { + Log::getSingleton().error("Failed to parse parameters."); return false; } @@ -127,13 +125,13 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) unsigned y0Width, u0Width; if (y0_str != "none") { - y0 = new Vector(0); + y0 = std::unique_ptr(new Vector(0)); stringToYarpVector(y0_str, *y0); - y0Width = y0->length(); + y0Width = y0->length(); } if (u0_str != "none") { - u0 = new Vector(0); + u0 = std::unique_ptr(new Vector(0)); stringToYarpVector(u0_str, *u0); u0Width = u0->length(); } @@ -145,43 +143,35 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) // ------- if (filter_type == "Generic") { if (num.length() == 0 || den.length() == 0) { - if (error) { - error->message = ClassName + " (Generic) Wrong coefficients size"; - } - return 1; + Log::getSingleton().error("(Generic) Wrong coefficients size."); + return false; } - filter = new Filter(num, den); + filter = std::unique_ptr(new Filter(num, den)); } // FirstOrderLowPassFilter // ----------------------- else if (filter_type == "FirstOrderLowPassFilter") { - if (firstOrderLowPassFilter_fc.floatData() == 0 - || firstOrderLowPassFilter_ts.floatData() == 0) { - if (error) { - error->message = ClassName - + " (FirstOrderLowPassFilter) You need to " - "specify Fc and Ts"; - } + if (firstOrderLowPassFilter_fc == 0 || firstOrderLowPassFilter_ts == 0) { + Log::getSingleton().error("(FirstOrderLowPassFilter) You need to " + "specify Fc and Ts."); return false; } - filter = new FirstOrderLowPassFilter(firstOrderLowPassFilter_fc.floatData(), - firstOrderLowPassFilter_ts.floatData()); + filter = std::unique_ptr( + new FirstOrderLowPassFilter(firstOrderLowPassFilter_fc, + firstOrderLowPassFilter_ts)); } // MedianFilter // ------------ else if (filter_type == "MedianFilter") { - if (medianFilter_order.int32Data() == 0) { - if (error) { - error->message = ClassName - + " (MedianFilter) You need to specify the " - "filter order."; - } + if (static_cast(medianFilter_order) == 0) { + Log::getSingleton().error("(MedianFilter) You need to specify the " + "filter order."); return false; } - filter = new MedianFilter(medianFilter_order.int32Data()); + filter = std::unique_ptr(new MedianFilter(static_cast(medianFilter_order))); } else { - if (error) error->message = ClassName + " Filter type not recognized."; + Log::getSingleton().error("Filter type not recognized."); return false; } @@ -195,54 +185,27 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo, wbt::Error* error) unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); if ((y0 != nullptr) && (y0Width != outputSignalWidth)) { - if (error) { - error->message = ClassName + " y0 and output signal sizes don't match"; - } + Log::getSingleton().error("y0 and output signal sizes don't match."); return false; } if ((u0 != nullptr) && (filter_type == "Generic") && (u0Width != inputSignalWidth)) { - if (error) { - error->message = ClassName + " (Generic) u0 and input signal sizes don't match"; - } + Log::getSingleton().error("(Generic) u0 and input signal sizes don't match."); return false; } // Allocate the input signal - inputSignalVector = new Vector(inputSignalWidth, 0.0); + inputSignalVector = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); return true; } -bool DiscreteFilter::terminate(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::terminate(const BlockInformation* blockInfo) { - // Deallocate all the memory - // ------------------------- - - if (filter) { - delete filter; - filter = nullptr; - } - - if (inputSignalVector) { - delete inputSignalVector; - inputSignalVector = nullptr; - } - - if (y0) { - delete y0; - y0 = nullptr; - } - - if (u0) { - delete u0; - u0 = nullptr; - } - return true; } -bool DiscreteFilter::initializeInitialConditions(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::initializeInitialConditions(const BlockInformation* blockInfo) { // Reminder: this function is called when, during runtime, a block is disabled // and enabled again. The method ::initialize instead is called just one time. @@ -251,17 +214,17 @@ bool DiscreteFilter::initializeInitialConditions(BlockInformation* blockInfo, wb // sized vector if (y0 == nullptr) { unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); - y0 = new Vector(outputSignalWidth, 0.0); + y0 = std::unique_ptr(new Vector(outputSignalWidth, 0.0)); } if (u0 == nullptr) { - u0 = new Vector(inputSignalWidth, 0.0); + u0 = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); } // Initialize the filter. This is required because if the signal is not 1D, // the default filter constructor initialize a wrongly sized y0 // Moreover, the Filter class has a different constructor that handles the // zero-gain case - Filter* filter_c = dynamic_cast(filter); + Filter* filter_c = dynamic_cast(filter.get()); if (filter_c != nullptr) { filter_c->init(*y0, *u0); } @@ -272,7 +235,7 @@ bool DiscreteFilter::initializeInitialConditions(BlockInformation* blockInfo, wb return true; } -bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error) +bool DiscreteFilter::output(const BlockInformation* blockInfo) { if (filter == nullptr) return false; @@ -282,7 +245,7 @@ bool DiscreteFilter::output(BlockInformation* blockInfo, wbt::Error* error) // Copy the Signal to the data structure that the filter wants for (unsigned i = 0; i < inputSignalWidth; ++i) { - (*inputSignalVector)[i] = inputSignal.get(i).doubleData(); + (*inputSignalVector)[i] = inputSignal.get(i); } // Filter the current component of the input signal From ae4234fb3abf96d61a67744c0c31a3792e42e701 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 25 Jan 2018 16:53:54 +0000 Subject: [PATCH 88/89] Disabled size initialization of matlab class' properties It is still not clear what is the Matlab version which introduced this feature. For now I am reverting it, addressing https://github.com/robotology/WB-Toolbox/issues/66. --- toolbox/matlab/+WBToolbox/Configuration.m | 16 ++++++++-------- toolbox/matlab/+WBToolbox/PID.m | 8 ++++---- toolbox/matlab/+WBToolbox/PIDConfiguration.m | 8 ++++---- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/toolbox/matlab/+WBToolbox/Configuration.m b/toolbox/matlab/+WBToolbox/Configuration.m index 6bb679ec4..6e460b44e 100644 --- a/toolbox/matlab/+WBToolbox/Configuration.m +++ b/toolbox/matlab/+WBToolbox/Configuration.m @@ -7,21 +7,21 @@ properties % Robot state - RobotName (1,:) char - UrdfFile (1,:) char - ControlledJoints (1,:) cell - ControlBoardsNames (1,:) cell - LocalName (1,:) char + RobotName char + UrdfFile char + ControlledJoints cell + ControlBoardsNames cell + LocalName char % Other Variables - GravityVector (1,3) double = [0, 0, -9.81] + GravityVector double = [0, 0, -9.81] end properties (Dependent) - ValidConfiguration (1,1) logical + ValidConfiguration logical end properties (Hidden, Dependent) - SimulinkParameters (1,1) struct + SimulinkParameters struct end % METHODS diff --git a/toolbox/matlab/+WBToolbox/PID.m b/toolbox/matlab/+WBToolbox/PID.m index 8b1541ed0..b9eb812b6 100644 --- a/toolbox/matlab/+WBToolbox/PID.m +++ b/toolbox/matlab/+WBToolbox/PID.m @@ -15,10 +15,10 @@ % See also: WBTOOLBOX.PIDCONFIGURATION properties - P (1,1) double = 0 - I (1,1) double = 0 - D (1,1) double = 0 - joint (1,:) char + P double = 0 + I double = 0 + D double = 0 + joint char end methods diff --git a/toolbox/matlab/+WBToolbox/PIDConfiguration.m b/toolbox/matlab/+WBToolbox/PIDConfiguration.m index 0fa0f707f..dd5e6a766 100644 --- a/toolbox/matlab/+WBToolbox/PIDConfiguration.m +++ b/toolbox/matlab/+WBToolbox/PIDConfiguration.m @@ -17,10 +17,10 @@ % Read-Only properties properties (SetAccess = private) - P (1,:) double - I (1,:) double - D (1,:) double - jointList (1,:) cell + P double + I double + D double + jointList cell end methods From 198e44b8159a8c3577761ba4248ffd4b33ad3dd5 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 25 Jan 2018 17:02:35 +0000 Subject: [PATCH 89/89] Small edits to README.md --- README.md | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 4478dee41..bce25f6f8 100644 --- a/README.md +++ b/README.md @@ -35,9 +35,9 @@ alt="iCub balancing on one foot via external force control and interacting with **Note:** You can install the dependencies either manually or by using the [codyco-superbuild](https://github.com/robotology/codyco-superbuild). ### Optional requirements -* [`iCub`](https://github.com/robotology/icub-main) (needed for some blocks) -* Gazebo Simulator (http://gazebosim.org/) -* gazebo_yarp_plugins (https://github.com/robotology/gazebo_yarp_plugins). +* [`iCub`](https://github.com/robotology/icub-main) (needed for some blocks) +* [Gazebo Simulator](http://gazebosim.org/) +* [`gazebo_yarp_plugins`](https://github.com/robotology/gazebo_yarp_plugins). **Note: The following instructions are for *NIX systems, but it works similarly on the other operating systems.** @@ -100,7 +100,10 @@ You can also use (only once after the installation) the script `startup_wbitoolb - **Robots' configuration files** Each robot that can be used through the Toolbox has its own configuration file. WB-Toolbox uses the Yarp [`ResourceFinder`](http://www.yarp.it/yarp_resource_finder_tutorials.html). You should thus follow the related instruction to properly configure your installation (e.g. set the `YARP_DATA_DIRS` variable) ## Troubleshooting -- **Problems finding libraries and libstdc++.** In case Matlab has trouble finding a specific library, a workaround is to launch it preloading the variable `LD_PRELOAD` (or `DYLD_INSERT_LIBRARIES` on macOS) with the full path of the missing library. On Linux you might also have trouble with `libstdc++.so` since Matlab comes with its own. To use your system's `libstdc++` you would need to launch Matlab something like (replace with your system's `libstdc++` library): + +### Problems finding libraries and libstdc++ + +In case Matlab has trouble finding a specific library, a workaround is to launch it preloading the variable `LD_PRELOAD` (or `DYLD_INSERT_LIBRARIES` on macOS) with the full path of the missing library. On Linux you might also have trouble with `libstdc++.so` since Matlab comes with its own. To use your system's `libstdc++` you would need to launch Matlab something like (replace with your system's `libstdc++` library): `LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 matlab` @@ -110,7 +113,9 @@ You could additionally create an alias to launch Matlab this way: A more permanent and scalable solution can be found in https://github.com/robotology/codyco-superbuild/issues/141#issuecomment-257892256. -- In case you compiled `YARP` in a directory different from the system default one and you are not using RPATH, you need to tell to MATLAB the location in which to find the shared libraries for `YARP`. If you launch MATLAB from command line, this task is already done for you by `bash` (if you edited `.bashrc`). If you launch MATLAB from the UI (e.g. on macOS by double clicking the application icon) you need to further add the variables in `${MATLAB_ROOT}/bin/.matlab7rc.sh` by first doing +### `YARP` not installed in the system default directory + +In case you compiled `YARP` in a directory different from the system default one and you are not using RPATH, you need to tell to MATLAB the location in which to find the shared libraries for `YARP`. If you launch MATLAB from command line, this task is already done for you by `bash` (if you edited `.bashrc`). If you launch MATLAB from the UI (e.g. on macOS by double clicking the application icon) you need to further add the variables in `${MATLAB_ROOT}/bin/.matlab7rc.sh` by first doing ```bash chmod +w .matlab7rc.sh