From 497fe2ac4024b52966e0faf1bc8bf5ca4bb05ac0 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Tue, 17 Aug 2021 16:06:28 +0200 Subject: [PATCH 001/512] ignore folders with MPC-Net policies and runs --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 15f4690f1..53dc52591 100644 --- a/.gitignore +++ b/.gitignore @@ -25,3 +25,5 @@ ocs2_ddp/test/ddp_test_generated/ *.out *.synctex.gz .vscode/ +policies/ +runs/ From 833ee45f0635ace08c949f43c7a49ea6bd1e86f3 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Tue, 17 Aug 2021 16:08:10 +0200 Subject: [PATCH 002/512] add types for MPC-Net and behavioral controller --- ocs2_core/include/ocs2_core/control/ControllerType.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_core/include/ocs2_core/control/ControllerType.h b/ocs2_core/include/ocs2_core/control/ControllerType.h index 65aa1962f..299bccd60 100644 --- a/ocs2_core/include/ocs2_core/control/ControllerType.h +++ b/ocs2_core/include/ocs2_core/control/ControllerType.h @@ -34,6 +34,6 @@ namespace ocs2 { /** * Enum class for specifying controller type */ -enum class ControllerType { UNKNOWN, FEEDFORWARD, LINEAR, NEURAL_NETWORK }; +enum class ControllerType { UNKNOWN, FEEDFORWARD, LINEAR, MPCNET, BEHAVIORAL }; } // namespace ocs2 From 7234b68bd6655c88f32d5edb93b54ca51f4f7a96 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Tue, 17 Aug 2021 16:09:51 +0200 Subject: [PATCH 003/512] add ocs2_mpcnet package --- ocs2_mpcnet/CMakeLists.txt | 93 ++++++++ .../ocs2_mpcnet/MpcnetDefinitionBase.h | 48 ++++ .../include/ocs2_mpcnet/MpcnetInterfaceBase.h | 71 ++++++ .../include/ocs2_mpcnet/MpcnetPybindMacros.h | 90 ++++++++ .../control/MpcnetBehavioralController.h | 87 +++++++ .../control/MpcnetControllerBase.h | 65 ++++++ .../control/MpcnetOnnxController.h | 94 ++++++++ .../rollout/MpcnetDataGeneration.h | 84 +++++++ .../rollout/MpcnetPolicyEvaluation.h | 74 ++++++ .../rollout/MpcnetRolloutManager.h | 111 +++++++++ .../onnxruntime/cmake/onnxruntimeConfig.cmake | 28 +++ .../cmake/onnxruntimeVersion.cmake | 13 ++ ocs2_mpcnet/package.xml | 21 ++ ocs2_mpcnet/python/ocs2_mpcnet/__init__.py | 0 ocs2_mpcnet/python/ocs2_mpcnet/config.py | 7 + ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 34 +++ ocs2_mpcnet/python/ocs2_mpcnet/memory.py | 34 +++ ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 117 ++++++++++ ocs2_mpcnet/requirements.txt | 12 + ocs2_mpcnet/setup.py | 11 + ocs2_mpcnet/src/MpcnetInterfaceBase.cpp | 55 +++++ .../control/MpcnetBehavioralController.cpp | 71 ++++++ .../src/control/MpcnetOnnxController.cpp | 56 +++++ .../src/rollout/MpcnetDataGeneration.cpp | 121 ++++++++++ .../src/rollout/MpcnetPolicyEvaluation.cpp | 75 ++++++ .../src/rollout/MpcnetRolloutManager.cpp | 218 ++++++++++++++++++ 26 files changed, 1690 insertions(+) create mode 100644 ocs2_mpcnet/CMakeLists.txt create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h create mode 100644 ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeConfig.cmake create mode 100644 ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake create mode 100644 ocs2_mpcnet/package.xml create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/__init__.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/config.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/loss.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/memory.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/policy.py create mode 100644 ocs2_mpcnet/requirements.txt create mode 100644 ocs2_mpcnet/setup.py create mode 100644 ocs2_mpcnet/src/MpcnetInterfaceBase.cpp create mode 100644 ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp create mode 100644 ocs2_mpcnet/src/control/MpcnetOnnxController.cpp create mode 100644 ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp create mode 100644 ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp create mode 100644 ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp diff --git a/ocs2_mpcnet/CMakeLists.txt b/ocs2_mpcnet/CMakeLists.txt new file mode 100644 index 000000000..dc5b30b17 --- /dev/null +++ b/ocs2_mpcnet/CMakeLists.txt @@ -0,0 +1,93 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_mpcnet) + +set(CATKIN_PACKAGE_DEPENDENCIES + pybind11_catkin + ocs2_mpc + ocs2_python_interface +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +find_package(onnxruntime REQUIRED) + +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS + onnxruntime +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# main library +add_library(${PROJECT_NAME} + src/control/MpcnetBehavioralController.cpp + src/control/MpcnetOnnxController.cpp + src/rollout/MpcnetDataGeneration.cpp + src/rollout/MpcnetPolicyEvaluation.cpp + src/rollout/MpcnetRolloutManager.cpp + src/MpcnetInterfaceBase.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + onnxruntime +) + +catkin_python_setup() + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target ocs2_mpcnet") + add_clang_tooling( + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR +) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +############# +## Testing ## +############# + +# TODO(areske) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h new file mode 100644 index 000000000..84cb206ac --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + +namespace ocs2 { + +/** + * Base class for MPC-Net definitions. + */ +class MpcnetDefinitionBase { + public: + /** + * Default constructor. + */ + MpcnetDefinitionBase() = default; + + /** + * Default destructor. + */ + virtual ~MpcnetDefinitionBase() = default; + + /** + * Get the generalized time. + * @param[in] t : Absolute time. + * @param[in] modeSchedule : Mode schedule. + * @return The generalized time. + */ + virtual vector_t getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) = 0; + + /** + * Get the relative state. + * @param[in] t : Absolute time. + * @param[in] x : Robot state. + * @param[in] targetTrajectories : Target trajectories. + * @return The relative state. + */ + virtual vector_t getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) = 0; + + /** + * Check if a state is valid. + * @param [in] x : State. + * @return True if valid. + */ + virtual bool validState(const vector_t& x) = 0; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h new file mode 100644 index 000000000..8190daa87 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h @@ -0,0 +1,71 @@ +#pragma once + +#include + +#include "ocs2_mpcnet/rollout/MpcnetRolloutManager.h" + +namespace ocs2 { + +/** + * Base class for all MPC-Net interfaces between C++ and Python. + */ +class MpcnetInterfaceBase { + public: + using data_point_t = MpcnetRolloutManager::data_point_t; + using data_array_t = MpcnetRolloutManager::data_array_t; + using data_ptr_t = MpcnetRolloutManager::data_ptr_t; + using metrics_t = MpcnetRolloutManager::metrics_t; + using metrics_array_t = MpcnetRolloutManager::metrics_array_t; + using metrics_ptr_t = MpcnetRolloutManager::metrics_ptr_t; + + protected: + /** + * Default constructor. + */ + MpcnetInterfaceBase() = default; + + public: + /** + * Default destructor. + */ + virtual ~MpcnetInterfaceBase() = default; + + /** + * @see MpcnetRolloutManager::startDataGeneration() + */ + void startDataGeneration(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, + const matrix_t& samplingCovariance, const std::vector& initialObservations, + const std::vector& modeSchedules, const std::vector& targetTrajectories); + + /** + * @see MpcnetRolloutManager::isDataGenerationDone() + */ + bool isDataGenerationDone(); + + /** + * @see MpcnetRolloutManager::getGeneratedData() + */ + data_array_t getGeneratedData(); + + /** + * @see MpcnetRolloutManager::startPolicyEvaluation() + */ + void startPolicyEvaluation(const std::string& policyFilePath, scalar_t timeStep, + const std::vector& initialObservations, const std::vector& modeSchedules, + const std::vector& targetTrajectories); + + /** + * @see MpcnetRolloutManager::isPolicyEvaluationDone() + */ + bool isPolicyEvaluationDone(); + + /** + * @see MpcnetRolloutManager::getComputedMetrics() + */ + metrics_array_t getComputedMetrics(); + + protected: + std::unique_ptr mpcnetRolloutManagerPtr_; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h new file mode 100644 index 000000000..3dcd1ed81 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include + +#include +#include + +using namespace pybind11::literals; + +/** + * Convenience macro to bind mpcnet interface and other classes with all required vectors. + */ +#define CREATE_MPCNET_PYTHON_BINDINGS(MPCNET_INTERFACE, LIB_NAME) \ + /* make vector types opaque so they are not converted to python lists */ \ + PYBIND11_MAKE_OPAQUE(ocs2::size_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::scalar_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::vector_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::matrix_array_t) \ + PYBIND11_MAKE_OPAQUE(std::vector) \ + PYBIND11_MAKE_OPAQUE(std::vector) \ + PYBIND11_MAKE_OPAQUE(std::vector) \ + PYBIND11_MAKE_OPAQUE(MPCNET_INTERFACE::data_array_t) \ + PYBIND11_MAKE_OPAQUE(MPCNET_INTERFACE::metrics_array_t) \ + /* create a python module */ \ + PYBIND11_MODULE(LIB_NAME, m) { \ + /* bind vector types so they can be used natively in python */ \ + VECTOR_TYPE_BINDING(ocs2::size_array_t, "size_array") \ + VECTOR_TYPE_BINDING(ocs2::scalar_array_t, "scalar_array") \ + VECTOR_TYPE_BINDING(ocs2::vector_array_t, "vector_array") \ + VECTOR_TYPE_BINDING(ocs2::matrix_array_t, "matrix_array") \ + VECTOR_TYPE_BINDING(std::vector, "SystemObservationArray") \ + VECTOR_TYPE_BINDING(std::vector, "ModeScheduleArray") \ + VECTOR_TYPE_BINDING(std::vector, "TargetTrajectoriesArray") \ + VECTOR_TYPE_BINDING(MPCNET_INTERFACE::data_array_t, "DataArray") \ + VECTOR_TYPE_BINDING(MPCNET_INTERFACE::metrics_array_t, "MetricsArray") \ + /* bind approximation classes */ \ + pybind11::class_(m, "ScalarFunctionQuadraticApproximation") \ + .def_readwrite("f", &ocs2::ScalarFunctionQuadraticApproximation::f) \ + .def_readwrite("dfdx", &ocs2::ScalarFunctionQuadraticApproximation::dfdx) \ + .def_readwrite("dfdu", &ocs2::ScalarFunctionQuadraticApproximation::dfdu) \ + .def_readwrite("dfdxx", &ocs2::ScalarFunctionQuadraticApproximation::dfdxx) \ + .def_readwrite("dfdux", &ocs2::ScalarFunctionQuadraticApproximation::dfdux) \ + .def_readwrite("dfduu", &ocs2::ScalarFunctionQuadraticApproximation::dfduu); \ + /* bind system observation struct */ \ + pybind11::class_(m, "SystemObservation") \ + .def(pybind11::init<>()) \ + .def_readwrite("mode", &ocs2::SystemObservation::mode) \ + .def_readwrite("time", &ocs2::SystemObservation::time) \ + .def_readwrite("state", &ocs2::SystemObservation::state) \ + .def_readwrite("input", &ocs2::SystemObservation::input); \ + /* bind mode schedule struct */ \ + pybind11::class_(m, "ModeSchedule") \ + .def(pybind11::init()) \ + .def_readwrite("event_times", &ocs2::ModeSchedule::eventTimes) \ + .def_readwrite("mode_sequence", &ocs2::ModeSchedule::modeSequence); \ + /* bind target trajectories class */ \ + pybind11::class_(m, "TargetTrajectories") \ + .def(pybind11::init()) \ + .def_readwrite("time_trajectory", &ocs2::TargetTrajectories::timeTrajectory) \ + .def_readwrite("state_trajectory", &ocs2::TargetTrajectories::stateTrajectory) \ + .def_readwrite("input_trajectory", &ocs2::TargetTrajectories::inputTrajectory); \ + /* bind data point struct */ \ + pybind11::class_(m, "DataPoint") \ + .def(pybind11::init<>()) \ + .def_readwrite("t", &MPCNET_INTERFACE::data_point_t::t) \ + .def_readwrite("x", &MPCNET_INTERFACE::data_point_t::x) \ + .def_readwrite("u", &MPCNET_INTERFACE::data_point_t::u) \ + .def_readwrite("generalized_time", &MPCNET_INTERFACE::data_point_t::generalizedTime) \ + .def_readwrite("relative_state", &MPCNET_INTERFACE::data_point_t::relativeState) \ + .def_readwrite("hamiltonian", &MPCNET_INTERFACE::data_point_t::hamiltonian); \ + /* bind metrics struct */ \ + pybind11::class_(m, "Metrics") \ + .def(pybind11::init<>()) \ + .def_readwrite("survival_time", &MPCNET_INTERFACE::metrics_t::survivalTime) \ + .def_readwrite("incurred_hamiltonian", &MPCNET_INTERFACE::metrics_t::incurredHamiltonian); \ + /* bind actual mpcnet interface */ \ + pybind11::class_(m, "MpcnetInterface") \ + .def(pybind11::init()) \ + .def("startDataGeneration", &MPCNET_INTERFACE::startDataGeneration, "alpha"_a, "policyFilePath"_a, "timeStep"_a, \ + "dataDecimation"_a, "nSamples"_a, "samplingCovariance"_a.noconvert(), "initialObservations"_a, "modeSchedules"_a, \ + "targetTrajectories"_a) \ + .def("isDataGenerationDone", &MPCNET_INTERFACE::isDataGenerationDone) \ + .def("getGeneratedData", &MPCNET_INTERFACE::getGeneratedData) \ + .def("startPolicyEvaluation", &MPCNET_INTERFACE::startPolicyEvaluation, "policyFilePath"_a, "timeStep"_a, "initialObservations"_a, \ + "modeSchedules"_a, "targetTrajectories"_a) \ + .def("isPolicyEvaluationDone", &MPCNET_INTERFACE::isPolicyEvaluationDone) \ + .def("getComputedMetrics", &MPCNET_INTERFACE::getComputedMetrics); \ + } diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h new file mode 100644 index 000000000..082627492 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -0,0 +1,87 @@ +#pragma once + +#include + +#include + +#include "ocs2_mpcnet/control/MpcnetControllerBase.h" + +namespace ocs2 { + +/** + * A behavioral controller that computes the input based on a mixture of an optimal policy (e.g. implicitly found via MPC) + * and a learned policy (e.g. explicitly represented by a neural network). + * The behavioral policy is pi_behavioral = alpha * pi_optimal + (1 - alpha) * pi_learned with alpha in [0, 1]. + */ +class MpcnetBehavioralController : public ControllerBase { + public: + using Base = ControllerBase; + using Optimal = ControllerBase; + using Learned = MpcnetControllerBase; + + /** + * Default constructor, leaves object uninitialized. + */ + MpcnetBehavioralController() = default; + + /** + * Constructor, initializes all required members of the controller. + * @param [in] alpha : The mixture parameter. + * @param [in] optimalControllerPtr : Pointer to the optimal controller. + * @param [in] learnedControllerPtr : Pointer to the learned controller. + */ + MpcnetBehavioralController(scalar_t alpha, Optimal* optimalControllerPtr, Learned* learnedControllerPtr) + : alpha_(alpha), + optimalControllerPtr_(std::unique_ptr(optimalControllerPtr)), + learnedControllerPtr_(std::unique_ptr(learnedControllerPtr)) {} + + /** + * Copy constructor. + */ + MpcnetBehavioralController(const MpcnetBehavioralController& other) + : MpcnetBehavioralController(other.alpha_, other.optimalControllerPtr_->clone(), other.learnedControllerPtr_->clone()) {} + + /** + * Default destructor. + */ + ~MpcnetBehavioralController() override = default; + + /** + * Set the mixture parameter. + * @param [in] alpha : The mixture parameter. + */ + void setAlpha(scalar_t alpha) { alpha_ = alpha; } + + /** + * Set the optimal controller. + * @param [in] optimalControllerPtr : Pointer to the optimal controller. + */ + void setOptimalController(Optimal* optimalControllerPtr) { optimalControllerPtr_.reset(optimalControllerPtr); } + + /** + * Set the learned controller. + * @param [in] learnedControllerPtr : Pointer to the learned controller. + */ + void setLearnedController(Learned* learnedControllerPtr) { learnedControllerPtr_.reset(learnedControllerPtr); } + + vector_t computeInput(scalar_t t, const vector_t& x) override; + + void concatenate(const Base* otherController, int index, int length) override; + + int size() const override; + + ControllerType getType() const override { return ControllerType::BEHAVIORAL; } + + void clear() override; + + bool empty() const override; + + MpcnetBehavioralController* clone() const override { return new MpcnetBehavioralController(*this); } + + private: + scalar_t alpha_; + std::unique_ptr optimalControllerPtr_; + std::unique_ptr learnedControllerPtr_; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h new file mode 100644 index 000000000..42b460f0e --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include +#include + +#include "ocs2_mpcnet/MpcnetDefinitionBase.h" + +namespace ocs2 { + +/** + * The base class for all controllers that use a MPC-Net policy. + */ +class MpcnetControllerBase : public ControllerBase { + public: + using Base = ControllerBase; + + /** + * Constructor. + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions. + * @param [in] referenceManagerPtr : Pointer to the reference manager. + */ + MpcnetControllerBase(std::shared_ptr mpcnetDefinitionPtr, + std::shared_ptr referenceManagerPtr) + : mpcnetDefinitionPtr_(mpcnetDefinitionPtr), referenceManagerPtr_(referenceManagerPtr) {} + + /** + * Default destructor. + */ + ~MpcnetControllerBase() override = default; + + /** + * Load the model of the policy. + * @param [in] policyFilePath : Path to the file with the model of the policy. + */ + virtual void loadPolicyModel(const std::string& policyFilePath) = 0; + + /** + * Get the generalized time. + * @param [in] t : Absolute time. + * @return Generalized time. + */ + vector_t getGeneralizedTime(scalar_t t) { return mpcnetDefinitionPtr_->getGeneralizedTime(t, referenceManagerPtr_->getModeSchedule()); } + + /** + * Get the relative state. + * @param [in] t : Absolute time. + * @param [in] x : Robot state. + * @return Relative state. + */ + vector_t getRelativeState(scalar_t t, const vector_t& x) { + return mpcnetDefinitionPtr_->getRelativeState(t, x, referenceManagerPtr_->getTargetTrajectories()); + } + + ControllerType getType() const override { return ControllerType::MPCNET; } + + MpcnetControllerBase* clone() const override = 0; + + protected: + std::shared_ptr mpcnetDefinitionPtr_; + std::shared_ptr referenceManagerPtr_; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h new file mode 100644 index 000000000..6ab1067c7 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -0,0 +1,94 @@ +#pragma once + +#include + +#include "ocs2_mpcnet/control/MpcnetControllerBase.h" + +namespace ocs2 { + +/** + * Convenience function for creating an environment for ONNX Runtime and getting a corresponding shared pointer. + * @note Only one environment per process can be created. The environment offers some threading and logging options. + * @return Pointer to the environment for ONNX Runtime. + */ +inline std::shared_ptr createOnnxEnvironment() { + return std::make_shared(ORT_LOGGING_LEVEL_WARNING, "MpcnetOnnxController"); +} + +/** + * A neural network controller using ONNX Runtime based on the Open Neural Network Exchange (ONNX) format. + * The model of the policy computes p, u = model(t, x) with + * t: generalized time (dimensionOfTime x 1), + * x: relative state, i.e. current state minus target state (dimensionOfState x 1), + * p: predicted expert weights (numberOfExperts x 1), + * u: predicted expert inputs (dimensionOfInput x numberOfExperts). + */ +class MpcnetOnnxController : public MpcnetControllerBase { + public: + using Base = MpcnetControllerBase; + using tensor_element_t = float; + + /** + * Constructor, does not load the model of the policy. + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions. + * @param [in] referenceManagerPtr : Pointer to the reference manager. + * @param [in] onnxEnvironmentPtr : Pointer to the environment for ONNX Runtime. + */ + MpcnetOnnxController(std::shared_ptr mpcnetDefinitionPtr, + std::shared_ptr referenceManagerPtr, std::shared_ptr onnxEnvironmentPtr) + : Base(mpcnetDefinitionPtr, referenceManagerPtr), onnxEnvironmentPtr_(onnxEnvironmentPtr) {} + + /** + * Constructor, initializes all members of the controller. + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions. + * @param [in] referenceManagerPtr : Pointer to the reference manager. + * @param [in] environmentPtr : Pointer to the environment for ONNX Runtime. + * @param [in] policyFilePath : Path to the ONNX file with the model of the policy. + */ + MpcnetOnnxController(std::shared_ptr mpcnetDefinitionPtr, + std::shared_ptr referenceManagerPtr, std::shared_ptr onnxEnvironmentPtr, + const std::string& policyFilePath) + : MpcnetOnnxController(mpcnetDefinitionPtr, referenceManagerPtr, onnxEnvironmentPtr) { + loadPolicyModel(policyFilePath); + } + + /** + * Copy constructor. + */ + MpcnetOnnxController(const MpcnetOnnxController& other) + : MpcnetOnnxController(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_, other.onnxEnvironmentPtr_, other.policyFilePath_) {} + + /** + * Default destructor. + */ + ~MpcnetOnnxController() override = default; + + void loadPolicyModel(const std::string& policyFilePath) override; + + vector_t computeInput(const scalar_t t, const vector_t& x) override; + + void concatenate(const typename Base::Base* otherController, int index, int length) override { + throw std::runtime_error("MpcnetOnnxController::concatenate not implemented."); + } + + int size() const override { throw std::runtime_error("MpcnetOnnxController::size not implemented."); } + + void clear() override { throw std::runtime_error("MpcnetOnnxController::clear not implemented."); } + + bool empty() const override { throw std::runtime_error("MpcnetOnnxController::empty not implemented."); } + + MpcnetOnnxController* clone() const override { return new MpcnetOnnxController(*this); } + + protected: + std::shared_ptr onnxEnvironmentPtr_; + std::string policyFilePath_; + + private: + std::unique_ptr sessionPtr_; + std::vector inputNames_; + std::vector outputNames_; + std::vector> inputShapes_; + std::vector> outputShapes_; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h new file mode 100644 index 000000000..ec9334fec --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -0,0 +1,84 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "ocs2_mpcnet/MpcnetDefinitionBase.h" +#include "ocs2_mpcnet/control/MpcnetControllerBase.h" + +namespace ocs2 { + +/** + * A class for generating MPC data from a system that is forward simulated with a behavioral controller. + * The behavioral policy is a mixture of an MPC policy and an MPC-Net policy (e.g. a neural network). + */ +class MpcnetDataGeneration { + public: + using mpc_t = MPC_BASE; + using mpcnet_t = MpcnetControllerBase; + using rollout_t = RolloutBase; + using mpcnet_definition_t = MpcnetDefinitionBase; + using reference_manager_t = ReferenceManagerInterface; + + struct DataPoint { + scalar_t t; + vector_t x; + vector_t u; + vector_t generalizedTime; + vector_t relativeState; + ScalarFunctionQuadraticApproximation hamiltonian; + }; + using DataArray = std::vector; + using DataPtr = std::unique_ptr; + + /** + * Constructor. + * @param [in] mpcPtr : Pointer to the MPC solver to be used (this class takes ownership). + * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions to be used (shared ownership). + * @param [in] referenceManagerPtr : Pointer to the reference manager to be used (shared ownership). + */ + MpcnetDataGeneration(std::unique_ptr mpcPtr, std::unique_ptr mpcnetPtr, std::unique_ptr rolloutPtr, + std::shared_ptr mpcnetDefinitionPtr, std::shared_ptr referenceManagerPtr) + : mpcPtr_(std::move(mpcPtr)), + mpcnetPtr_(std::move(mpcnetPtr)), + rolloutPtr_(std::move(rolloutPtr)), + mpcnetDefinitionPtr_(mpcnetDefinitionPtr), + referenceManagerPtr_(referenceManagerPtr) {} + + /** + * Default destructor. + */ + virtual ~MpcnetDataGeneration() = default; + + /** + * Run the data generation. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the behavioral controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. + * @param [in] dataDecimation : The integer factor used for downsampling the data signal. + * @param [in] nSamples : The number of samples drawn from a multivariate normal distribution around the nominal states. + * @param [in] samplingCovariance : The covariance matrix used for sampling from a multivariate normal distribution. + * @param [in] initialObservation : The initial system observation to start from (time and state required). + * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + * @return Pointer to the generated data. + */ + DataPtr run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, + const matrix_t& samplingCovariance, const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories); + + private: + std::unique_ptr mpcPtr_; + std::unique_ptr mpcnetPtr_; + std::unique_ptr rolloutPtr_; + std::shared_ptr mpcnetDefinitionPtr_; + std::shared_ptr referenceManagerPtr_; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h new file mode 100644 index 000000000..c21bc0900 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "ocs2_mpcnet/MpcnetDefinitionBase.h" +#include "ocs2_mpcnet/control/MpcnetControllerBase.h" + +namespace ocs2 { + +/** + * A class for evaluating a policy for a system that is forward simulated with a learned controller. + */ +class MpcnetPolicyEvaluation { + public: + using mpc_t = MPC_BASE; + using mpcnet_t = MpcnetControllerBase; + using rollout_t = RolloutBase; + using mpcnet_definition_t = MpcnetDefinitionBase; + using reference_manager_t = ReferenceManagerInterface; + + struct Metrics { + scalar_t survivalTime; + scalar_t incurredHamiltonian; + }; + using MetricsArray = std::vector; + using MetricsPtr = std::unique_ptr; + + /** + * Constructor. + * @param [in] mpcPtr: Pointer to the MPC solver to be used (this class takes ownership). + * @param [in] mpcnetPtr: Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr: Pointer to the rollout to be used (this class takes ownership). + * @param [in] mpcnetDefinitionPtr: Pointer to the MPC-Net definitions to be used (shared ownership). + * @param [in] referenceManagerPtr: Pointer to the reference manager to be used (shared ownership). + */ + MpcnetPolicyEvaluation(std::unique_ptr mpcPtr, std::unique_ptr mpcnetPtr, std::unique_ptr rolloutPtr, + std::shared_ptr mpcnetDefinitionPtr, std::shared_ptr referenceManagerPtr) + : mpcPtr_(std::move(mpcPtr)), + mpcnetPtr_(std::move(mpcnetPtr)), + rolloutPtr_(std::move(rolloutPtr)), + mpcnetDefinitionPtr_(mpcnetDefinitionPtr), + referenceManagerPtr_(referenceManagerPtr) {} + + /** + * Default destructor. + */ + virtual ~MpcnetPolicyEvaluation() = default; + + /** + * Run the policy evaluation. + * @param [in] policyFilePath : The path to the file with the learned policy for the controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the controller. + * @param [in] initialObservation : The initial system observation to start from (time and state required). + * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + * @return Pointer to the computed metrics. + */ + MetricsPtr run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); + + private: + std::unique_ptr mpcPtr_; + std::unique_ptr mpcnetPtr_; + std::unique_ptr rolloutPtr_; + std::shared_ptr mpcnetDefinitionPtr_; + std::shared_ptr referenceManagerPtr_; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h new file mode 100644 index 000000000..fef7b8aea --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h @@ -0,0 +1,111 @@ +#pragma once + +#include + +#include "ocs2_mpcnet/rollout/MpcnetDataGeneration.h" +#include "ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h" + +namespace ocs2 { + +/** + * TODO + */ +class MpcnetRolloutManager { + public: + using data_point_t = MpcnetDataGeneration::DataPoint; + using data_array_t = MpcnetDataGeneration::DataArray; + using data_ptr_t = MpcnetDataGeneration::DataPtr; + using metrics_t = MpcnetPolicyEvaluation::Metrics; + using metrics_array_t = MpcnetPolicyEvaluation::MetricsArray; + using metrics_ptr_t = MpcnetPolicyEvaluation::MetricsPtr; + + /** + * Constructor. + * @note The first nDataGenerationThreads pointers will be used for the data generation and the next nPolicyEvaluationThreads pointers for + * the policy evaluation. + * @param [in] nDataGenerationThreads : Number of data generation threads. + * @param [in] nPolicyEvaluationThreads : Number of policy evaluation threads. + * @param [in] mpcPtrs : Pointers to the MPC solvers to be used. + * @param [in] mpcnetPtrs : Pointers to the MPC-Net policies to be used. + * @param [in] rolloutPtrs : Pointers to the rollouts to be used. + * @param [in] mpcnetDefinitionPtrs : Pointers to the MPC-Net definitions to be used. + * @param [in] referenceManagerPtrs : Pointers to the reference managers to be used. + */ + MpcnetRolloutManager(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, std::vector> mpcPtrs, + std::vector> mpcnetPtrs, std::vector> rolloutPtrs, + std::vector> mpcnetDefinitionPtrs, + std::vector> referenceManagerPtrs); + + /** + * Default destructor. + */ + virtual ~MpcnetRolloutManager() = default; + + /** + * Starts the data genration forward simulated by a behavioral controller. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the behavioral controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. + * @param [in] dataDecimation : The integer factor used for downsampling the data signal. + * @param [in] nSamples : The number of samples drawn from a multivariate normal distribution around the nominal states. + * @param [in] samplingCovariance : The covariance matrix used for sampling from a multivariate normal distribution. + * @param [in] initialObservations : The initial system observations to start from (time and state required). + * @param [in] modeSchedules : The mode schedules providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + */ + void startDataGeneration(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, + const matrix_t& samplingCovariance, const std::vector& initialObservations, + const std::vector& modeSchedules, const std::vector& targetTrajectories); + + /** + * Check if data generation is done. + * @return True if done. + */ + bool isDataGenerationDone(); + + /** + * Get the data generated from the data generation rollout. + * @return The generated data. + */ + data_array_t getGeneratedData(); + + /** + * Starts the policy evaluation forward simulated by a learned controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the learned controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the learned controller. + * @param [in] initialObservations : The initial system observations to start from (time and state required). + * @param [in] modeSchedules : The mode schedules providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + */ + void startPolicyEvaluation(const std::string& policyFilePath, scalar_t timeStep, + const std::vector& initialObservations, const std::vector& modeSchedules, + const std::vector& targetTrajectories); + + /** + * Check if policy evaluation is done. + * @return True if done. + */ + bool isPolicyEvaluationDone(); + + /** + * Get the metrics computed from the policy evaluation rollout. + * @return The computed metrics. + */ + metrics_array_t getComputedMetrics(); + + private: + // data generation variables + size_t nDataGenerationThreads_; + std::atomic_int nDataGenerationTasksDone_; + std::unique_ptr dataGenerationThreadPoolPtr_; + std::vector> dataGenerationPtrs_; + std::vector> dataGenerationFtrs_; + // policy evaluation variables + size_t nPolicyEvaluationThreads_; + std::atomic_int nPolicyEvaluationTasksDone_; + std::unique_ptr policyEvaluationThreadPoolPtr_; + std::vector> policyEvaluationPtrs_; + std::vector> policyEvaluationFtrs_; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeConfig.cmake b/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeConfig.cmake new file mode 100644 index 000000000..28cba4e94 --- /dev/null +++ b/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeConfig.cmake @@ -0,0 +1,28 @@ +# Custom cmake config file to enable find_package(onnxruntime) without modifying LIBRARY_PATH and LD_LIBRARY_PATH +# +# This will define the following variables: +# onnxruntime_FOUND -- True if the system has the onnxruntime library +# onnxruntime_INCLUDE_DIRS -- The include directories for onnxruntime +# onnxruntime_LIBRARIES -- Libraries to link against +# onnxruntime_CXX_FLAGS -- Additional (required) compiler flags + +include(FindPackageHandleStandardArgs) + +# Assume we are in /share/cmake/onnxruntime/onnxruntimeConfig.cmake +get_filename_component(CMAKE_CURRENT_LIST_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) +get_filename_component(onnxruntime_INSTALL_PREFIX "${CMAKE_CURRENT_LIST_DIR}/../../../" ABSOLUTE) + +set(onnxruntime_INCLUDE_DIRS ${onnxruntime_INSTALL_PREFIX}/include) +set(onnxruntime_LIBRARIES onnxruntime) +set(onnxruntime_CXX_FLAGS "") # no flags needed + +find_library(onnxruntime_LIBRARY onnxruntime + PATHS "${onnxruntime_INSTALL_PREFIX}/lib" +) + +add_library(onnxruntime SHARED IMPORTED) +set_property(TARGET onnxruntime PROPERTY IMPORTED_LOCATION "${onnxruntime_LIBRARY}") +set_property(TARGET onnxruntime PROPERTY INTERFACE_INCLUDE_DIRECTORIES "${onnxruntime_INCLUDE_DIRS}") +set_property(TARGET onnxruntime PROPERTY INTERFACE_COMPILE_OPTIONS "${onnxruntime_CXX_FLAGS}") + +find_package_handle_standard_args(onnxruntime DEFAULT_MSG onnxruntime_LIBRARY onnxruntime_INCLUDE_DIRS) diff --git a/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake b/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake new file mode 100644 index 000000000..d45badeba --- /dev/null +++ b/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake @@ -0,0 +1,13 @@ +# Custom cmake version file + +set(PACKAGE_VERSION "1.4.0") + +# Check whether the requested PACKAGE_FIND_VERSION is compatible +if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") + set(PACKAGE_VERSION_COMPATIBLE FALSE) +else() + set(PACKAGE_VERSION_COMPATIBLE TRUE) + if("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") + set(PACKAGE_VERSION_EXACT TRUE) + endif() +endif() diff --git a/ocs2_mpcnet/package.xml b/ocs2_mpcnet/package.xml new file mode 100644 index 000000000..3b2da3fc4 --- /dev/null +++ b/ocs2_mpcnet/package.xml @@ -0,0 +1,21 @@ + + + ocs2_mpcnet + 0.0.0 + The ocs2_mpcnet package + + Farbod Farshidian + Alexander Reske + + TODO + + catkin + + cmake_clang_tools + + pybind11_catkin + + ocs2_mpc + ocs2_python_interface + + diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/__init__.py b/ocs2_mpcnet/python/ocs2_mpcnet/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/config.py b/ocs2_mpcnet/python/ocs2_mpcnet/config.py new file mode 100644 index 000000000..6f925d44b --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/config.py @@ -0,0 +1,7 @@ +import torch + +# data type for tensor elements +dtype = torch.float + +# device on which tensors will be allocated +device = torch.device("cpu") diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py new file mode 100644 index 000000000..55d5c4a3b --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -0,0 +1,34 @@ +import torch +import numpy as np + + +class Hamiltonian(): + + # Uses the quadratic approximation of the Hamiltonian as loss + # H(x,u) = 1/2 dx' dHdxx dx + du' dHdux dx + 1/2 du' dHduu du + dHdx' dx + dHdu' du + H + + def compute_torch(self, x, u, hamiltonian): + # TODO (areske): implement once approximation of Hamiltonian is available + return + + def compute_numpy(self, x, u, hamiltonian): + # TODO (areske): implement once approximation of Hamiltonian is available + return + + +class BehavioralCloning(): + + # Uses a simple quadratic function as loss + # BC(u) = du' R du + + def __init__(self, R_torch, R_numpy): + self.R_torch = R_torch + self.R_numpy = R_numpy + + def compute_torch(self, u_predicted, u_target): + du = torch.sub(u_predicted, u_target) + return torch.dot(du, torch.matmul(self.R_torch, du)) + + def compute_numpy(self, u_predicted, u_target): + du = np.subtract(u_predicted, u_target) + return np.dot(du, np.matmul(self.R_numpy, du)) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py new file mode 100644 index 000000000..a78f93092 --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py @@ -0,0 +1,34 @@ +import random +import numpy as np +from collections import namedtuple + +Sample = namedtuple('sample', ('t', 'x', 'u', 'generalized_time', 'relative_state', 'hamiltonian')) + + +class ReplayMemory: + + def __init__(self, capacity): + self.capacity = capacity + self.memory = [None] * capacity # pre-allocate memory + self.position = 0 + self.size = 0 + + def push(self, *args): + sample = Sample(*args) + for element in sample: + if isinstance(element, (float, np.ndarray)): + if np.any(np.isnan(element)): + print("Avoided pushing nan into memory", element) + return + if np.any(np.isinf(element)): + print("Avoided pushing inf into memory", element) + return + self.size = min(self.size + 1, self.capacity) + self.memory[self.position] = sample + self.position = (self.position + 1) % self.capacity + + def sample(self, batch_size): + return random.sample(self.memory[0:self.size], batch_size) + + def __len__(self): + return self.size diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py new file mode 100644 index 000000000..e7b70c3d9 --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -0,0 +1,117 @@ +import torch + +from ocs2_mpcnet import config + + +class Policy(torch.nn.Module): + + def __init__(self, dim_in, dim_out): + super().__init__() + self.name = 'Policy' + self.dim_in = dim_in + self.dim_out = dim_out + + +class LinearPolicy(Policy): + + def __init__(self, dim_t, dim_x, dim_u): + super().__init__(dim_t + dim_x, dim_u) + self.name = 'LinearPolicy' + self.linear = torch.nn.Linear(self.dim_in, self.dim_out) + + def forward(self, t, x): + u = self.linear(torch.cat((t, x))) + return torch.ones(1, device=config.device, dtype=config.dtype), u.reshape((1, self.dim_out)) + + +class NonlinearPolicy(Policy): + + def __init__(self, dim_t, dim_x, dim_u): + super().__init__(dim_t + dim_x, dim_u) + self.name = 'NonlinearPolicy' + self.dim_hidden = int((self.dim_in + dim_u) / 2) + self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) + self.activation = torch.nn.Tanh() + self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) + + def forward(self, t, x): + z = self.activation(self.linear1(torch.cat((t, x)))) + u = self.linear2(z) + return torch.ones(1, device=config.device, dtype=config.dtype), u.reshape((1, self.dim_out)) + + +class MixtureOfLinearExpertsPolicy(Policy): + + def __init__(self, dim_t, dim_x, dim_u, num_experts): + super().__init__(dim_t + dim_x, dim_u) + self.name = 'MixtureOfLinearExpertsPolicy' + self.num_experts = num_experts + # gating + self.gating_net = torch.nn.Sequential( + torch.nn.Linear(self.dim_in, self.num_experts), + torch.nn.Softmax(dim=0) + ) + # experts + self.expert_nets = torch.nn.ModuleList( + [LinearExpert(i, self.dim_in, self.dim_out) for i in range(self.num_experts)] + ) + + def forward(self, t, x): + p = self.gating_net(torch.cat((t, x))) + u = torch.stack([self.expert_nets[i](torch.cat((t, x))) for i in range(self.num_experts)]) + return p, u + + +class MixtureOfNonlinearExpertsPolicy(Policy): + + def __init__(self, dim_t, dim_x, num_experts, dim_u): + super().__init__(dim_t + dim_x, dim_u) + self.name = 'MixtureOfNonlinearExpertsPolicy' + self.num_experts = num_experts + self.dim_hidden_expert = int((self.dim_in + dim_u) / 2) + self.dim_hidden_gating = int((self.dim_in + num_experts) / 2) + # gating + self.gating_net = torch.nn.Sequential( + torch.nn.Linear(self.dim_in, self.dim_hidden_gating), + torch.nn.Tanh(), + torch.nn.Linear(self.dim_hidden_gating, self.num_experts), + torch.nn.Softmax(dim=0) + ) + # experts + self.expert_nets = torch.nn.ModuleList( + [NonlinearExpert(i, self.dim_in, self.dim_hidden_expert, self.dim_out) for i in range(self.num_experts)] + ) + + def forward(self, t, x): + p = self.gating_net(torch.cat((t, x))) + u = torch.stack([self.expert_nets[i](torch.cat((t, x))) for i in range(self.num_experts)]) + return p, u + + +class LinearExpert(torch.nn.Module): + + def __init__(self, id, dim_in, dim_out): + super().__init__() + self.name = 'LinearExpert' + str(id) + self.dim_in = dim_in + self.dim_out = dim_out + self.linear = torch.nn.Linear(self.dim_in, self.dim_out) + + def forward(self, input): + return self.linear(input) + + +class NonlinearExpert(torch.nn.Module): + + def __init__(self, id, dim_in, dim_hidden, dim_out): + super().__init__() + self.name = 'NonlinearExpert' + str(id) + self.dim_in = dim_in + self.dim_hidden = dim_hidden + self.dim_out = dim_out + self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) + self.activation = torch.nn.Tanh() + self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) + + def forward(self, input): + return self.linear2(self.activation(self.linear1(input))) diff --git a/ocs2_mpcnet/requirements.txt b/ocs2_mpcnet/requirements.txt new file mode 100644 index 000000000..92e16611d --- /dev/null +++ b/ocs2_mpcnet/requirements.txt @@ -0,0 +1,12 @@ +# +####### requirements.txt ####### +# +###### Requirements without version specifiers ###### +numpy +tensorboard +torch +# +###### Requirements with version specifiers ###### +# +###### Refer to other requirements files ###### +# \ No newline at end of file diff --git a/ocs2_mpcnet/setup.py b/ocs2_mpcnet/setup.py new file mode 100644 index 000000000..55a253e33 --- /dev/null +++ b/ocs2_mpcnet/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup( + packages=['ocs2_mpcnet'], + package_dir={'': 'python'} +) + +setup(**setup_args) diff --git a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp new file mode 100644 index 000000000..e4af5e686 --- /dev/null +++ b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp @@ -0,0 +1,55 @@ +#include "ocs2_mpcnet/MpcnetInterfaceBase.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetInterfaceBase::startDataGeneration(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, + size_t nSamples, const matrix_t& samplingCovariance, + const std::vector& initialObservations, + const std::vector& modeSchedules, + const std::vector& targetTrajectories) { + mpcnetRolloutManagerPtr_->startDataGeneration(alpha, policyFilePath, timeStep, dataDecimation, nSamples, samplingCovariance, + initialObservations, modeSchedules, targetTrajectories); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetInterfaceBase::isDataGenerationDone() { + return mpcnetRolloutManagerPtr_->isDataGenerationDone(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetInterfaceBase::data_array_t MpcnetInterfaceBase::getGeneratedData() { + return mpcnetRolloutManagerPtr_->getGeneratedData(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetInterfaceBase::startPolicyEvaluation(const std::string& policyFilePath, scalar_t timeStep, + const std::vector& initialObservations, + const std::vector& modeSchedules, + const std::vector& targetTrajectories) { + mpcnetRolloutManagerPtr_->startPolicyEvaluation(policyFilePath, timeStep, initialObservations, modeSchedules, targetTrajectories); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetInterfaceBase::isPolicyEvaluationDone() { + return mpcnetRolloutManagerPtr_->isPolicyEvaluationDone(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetInterfaceBase::metrics_array_t MpcnetInterfaceBase::getComputedMetrics() { + return mpcnetRolloutManagerPtr_->getComputedMetrics(); +} + +} // namespace ocs2 diff --git a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp new file mode 100644 index 000000000..e8840696f --- /dev/null +++ b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp @@ -0,0 +1,71 @@ +#include "ocs2_mpcnet/control/MpcnetBehavioralController.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t MpcnetBehavioralController::computeInput(scalar_t t, const vector_t& x) { + if (optimalControllerPtr_ && learnedControllerPtr_) { + return alpha_ * optimalControllerPtr_->computeInput(t, x) + (1 - alpha_) * learnedControllerPtr_->computeInput(t, x); + } else { + throw std::runtime_error( + "MpcnetBehavioralController::computeInput cannot return input, since optimal and/or learned controller not set."); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetBehavioralController::concatenate(const Base* otherController, int index, int length) { + if (optimalControllerPtr_) { + optimalControllerPtr_->concatenate(otherController, index, length); + } + if (learnedControllerPtr_) { + learnedControllerPtr_->concatenate(otherController, index, length); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +int MpcnetBehavioralController::size() const { + if (optimalControllerPtr_ && learnedControllerPtr_) { + return std::max(optimalControllerPtr_->size(), learnedControllerPtr_->size()); + } else if (optimalControllerPtr_) { + return optimalControllerPtr_->size(); + } else if (learnedControllerPtr_) { + return learnedControllerPtr_->size(); + } else { + return 0; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetBehavioralController::clear() { + if (optimalControllerPtr_) { + optimalControllerPtr_->clear(); + } + if (learnedControllerPtr_) { + learnedControllerPtr_->clear(); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetBehavioralController::empty() const { + if (optimalControllerPtr_ && learnedControllerPtr_) { + return optimalControllerPtr_->empty() && learnedControllerPtr_->empty(); + } else if (optimalControllerPtr_) { + return optimalControllerPtr_->empty(); + } else if (learnedControllerPtr_) { + return learnedControllerPtr_->empty(); + } else { + return true; + } +} + +} // namespace ocs2 diff --git a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp new file mode 100644 index 000000000..e9fb61e9e --- /dev/null +++ b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp @@ -0,0 +1,56 @@ +#include "ocs2_mpcnet/control/MpcnetOnnxController.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetOnnxController::loadPolicyModel(const std::string& policyFilePath) { + policyFilePath_ = policyFilePath; + // create session + Ort::SessionOptions sessionOptions; + sessionPtr_.reset(new Ort::Session(*onnxEnvironmentPtr_, policyFilePath_.c_str(), sessionOptions)); + // get input and output info + inputNames_.clear(); + outputNames_.clear(); + inputShapes_.clear(); + outputShapes_.clear(); + Ort::AllocatorWithDefaultOptions allocator; + for (int i = 0; i < sessionPtr_->GetInputCount(); i++) { + inputNames_.push_back(sessionPtr_->GetInputName(i, allocator)); + inputShapes_.push_back(sessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); + } + for (int i = 0; i < sessionPtr_->GetOutputCount(); i++) { + outputNames_.push_back(sessionPtr_->GetOutputName(i, allocator)); + outputShapes_.push_back(sessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t MpcnetOnnxController::computeInput(const scalar_t t, const vector_t& x) { + // create input tensor objects + Eigen::Matrix tEigenData = getGeneralizedTime(t).cast(); + Eigen::Matrix xEigenData = getRelativeState(t, x).cast(); + std::vector tData(tEigenData.data(), tEigenData.data() + tEigenData.size()); + std::vector xData(xEigenData.data(), xEigenData.data() + xEigenData.size()); + Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, OrtMemType::OrtMemTypeDefault); + std::vector inputValues; + inputValues.push_back( + Ort::Value::CreateTensor(memoryInfo, tData.data(), tData.size(), inputShapes_[0].data(), inputShapes_[0].size())); + inputValues.push_back( + Ort::Value::CreateTensor(memoryInfo, xData.data(), xData.size(), inputShapes_[1].data(), inputShapes_[1].size())); + // run inference + Ort::RunOptions runOptions; + std::vector outputValues = sessionPtr_->Run(runOptions, inputNames_.data(), inputValues.data(), 2, outputNames_.data(), 2); + // evaluate output tensor objects + Eigen::Map> pEigenData(outputValues[0].GetTensorMutableData(), + outputShapes_[0][0], 1); + Eigen::Map> uEigenData( + outputValues[1].GetTensorMutableData(), outputShapes_[1][1], outputShapes_[1][0]); + Eigen::Matrix u = uEigenData * pEigenData; + return u.cast(); +} + +} // namespace ocs2 diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp new file mode 100644 index 000000000..a06fb560e --- /dev/null +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -0,0 +1,121 @@ +#include "ocs2_mpcnet/rollout/MpcnetDataGeneration.h" + +#include + +#include "ocs2_mpcnet/control/MpcnetBehavioralController.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, + size_t dataDecimation, size_t nSamples, const matrix_t& samplingCovariance, + const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + // declare data pointer + DataPtr dataPtr(new DataArray); + + // init time and state + scalar_t time = initialObservation.time; + vector_t state = initialObservation.state; + + // update the reference manager + referenceManagerPtr_->setModeSchedule(modeSchedule); + referenceManagerPtr_->setTargetTrajectories(targetTrajectories); + + // reset mpc + mpcPtr_->reset(); + + // prepare learned controller + mpcnetPtr_->loadPolicyModel(policyFilePath); + + // set up behavioral controller with mixture parameter alpha and learned controller + MpcnetBehavioralController behavioralController; + behavioralController.setAlpha(alpha); + behavioralController.setLearnedController(mpcnetPtr_->clone()); + + // set up scalar standard normal generator and compute Cholesky decomposition of covariance matrix + std::random_device randomDevice; + std::default_random_engine pseudoRandomNumberGenerator(randomDevice()); + std::normal_distribution standardNormalDistribution(scalar_t(0.0), scalar_t(1.0)); + std::function standardNormalNullaryOp = [&](scalar_t) -> scalar_t { + return standardNormalDistribution(pseudoRandomNumberGenerator); + }; + matrix_t S = samplingCovariance; + matrix_t L = S.llt().matrixL(); + + // run data generation + int iteration = 0; + try { + while (time <= targetTrajectories.timeTrajectory.back()) { + // run mpc and get solution + if (!mpcPtr_->run(time, state)) { + throw std::runtime_error("MpcnetDataGeneration::run Main routine of MPC returned false."); + } + PrimalSolution primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); + + // downsample the data signal by an integer factor + if (iteration % dataDecimation == 0) { + // get nominal data point + { + DataPoint dataPoint; + dataPoint.t = primalSolution.timeTrajectory_[0]; + dataPoint.x = primalSolution.stateTrajectory_[0]; + dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); + dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); + dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); + // TODO(areske): add once approximation of Hamiltonian is available + // dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); + dataPtr->push_back(std::move(dataPoint)); + } + + // get samples around nominal data point + for (int i = 0; i < nSamples; i++) { + DataPoint dataPoint; + dataPoint.t = primalSolution.timeTrajectory_[0]; + dataPoint.x = primalSolution.stateTrajectory_[0] + + L * vector_t::NullaryExpr(primalSolution.stateTrajectory_[0].size(), standardNormalNullaryOp); + dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); + dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); + dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); + // TODO(areske): add once approximation of Hamiltonian is available + // dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); + dataPtr->push_back(std::move(dataPoint)); + } + } + + // update behavioral controller with MPC controller + behavioralController.setOptimalController(primalSolution.controllerPtr_->clone()); + + // forward simulate system with behavioral controller + scalar_array_t timeTrajectory; + size_array_t postEventIndicesStock; + vector_array_t stateTrajectory; + vector_array_t inputTrajectory; + rolloutPtr_->run(primalSolution.timeTrajectory_[0], primalSolution.stateTrajectory_[0], primalSolution.timeTrajectory_[0] + timeStep, + &behavioralController, primalSolution.modeSchedule_.eventTimes, timeTrajectory, postEventIndicesStock, + stateTrajectory, inputTrajectory); + + // update time, state and iteration + time = timeTrajectory.back(); + state = stateTrajectory.back(); + iteration++; + + // check if forward simulated system diverged + if (!mpcnetDefinitionPtr_->validState(state)) { + throw std::runtime_error("MpcnetDataGeneration::run State is not valid."); + } + } + } catch (const std::exception& e) { + // print error for exceptions + std::cerr << "MpcnetDataGeneration::run A standard exception was caught, with message: " << e.what() << std::endl; + // this data generation run failed, clear data + dataPtr->clear(); + } + + // return data pointer + return dataPtr; +} + +} // namespace ocs2 diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp new file mode 100644 index 000000000..711f4e052 --- /dev/null +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -0,0 +1,75 @@ +#include "ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string& policyFilePath, scalar_t timeStep, + const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + // declare metrics pointer + MetricsPtr metricsPtr(new Metrics); + + // init time and state + scalar_t time = initialObservation.time; + vector_t state = initialObservation.state; + + // update the reference manager + referenceManagerPtr_->setModeSchedule(modeSchedule); + referenceManagerPtr_->setTargetTrajectories(targetTrajectories); + + // reset mpc + mpcPtr_->reset(); + + // prepare learned controller + mpcnetPtr_->loadPolicyModel(policyFilePath); + + // run policy evaluation + int iteration = 0; + try { + while (time <= targetTrajectories.timeTrajectory.back()) { + // run mpc and get solution + if (!mpcPtr_->run(time, state)) { + throw std::runtime_error("MpcnetPolicyEvaluation::run Main routine of MPC returned false."); + } + PrimalSolution primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); + + // TODO(areske): add once approximation of Hamiltonian is available + // vector_t input = primalSolution.controllerPtr_->computeInput(time, state); + // metricsPtr->incurredHamiltonian += mpcPtr_->getSolverPtr()->getHamiltonian(time, state, input).f * timeStep; + + // forward simulate system with learned controller + scalar_array_t timeTrajectory; + size_array_t postEventIndicesStock; + vector_array_t stateTrajectory; + vector_array_t inputTrajectory; + rolloutPtr_->run(time, state, time + timeStep, mpcnetPtr_.get(), {}, timeTrajectory, postEventIndicesStock, stateTrajectory, + inputTrajectory); + + // update time, state and iteration + time = timeTrajectory.back(); + state = stateTrajectory.back(); + iteration++; + + // check if forward simulated system diverged + if (!mpcnetDefinitionPtr_->validState(state)) { + throw std::runtime_error("MpcnetPolicyEvaluation::run State is not valid."); + } + } + } catch (const std::exception& e) { + // print error for exceptions + std::cerr << "MpcnetPolicyEvaluation::run A standard exception was caught, with message: " << e.what() << std::endl; + // this policy evaluation run failed, incurred quantities are not reported + metricsPtr->incurredHamiltonian = std::numeric_limits::quiet_NaN(); + } + + // report survival time + metricsPtr->survivalTime = time; + + // return metrics pointer + return metricsPtr; +} + +} // namespace ocs2 diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp new file mode 100644 index 000000000..e449746a3 --- /dev/null +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp @@ -0,0 +1,218 @@ +#include "ocs2_mpcnet/rollout/MpcnetRolloutManager.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetRolloutManager::MpcnetRolloutManager(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, + std::vector> mpcPtrs, + std::vector> mpcnetPtrs, + std::vector> rolloutPtrs, + std::vector> mpcnetDefinitionPtrs, + std::vector> referenceManagerPtrs) { + // data generation + nDataGenerationThreads_ = nDataGenerationThreads; + if (nDataGenerationThreads_ > 0) { + dataGenerationThreadPoolPtr_.reset(new ThreadPool(nDataGenerationThreads_)); + dataGenerationPtrs_.reserve(nDataGenerationThreads); + for (int i = 0; i < nDataGenerationThreads; i++) { + dataGenerationPtrs_.push_back(std::unique_ptr( + new MpcnetDataGeneration(std::move(mpcPtrs.at(i)), std::move(mpcnetPtrs.at(i)), std::move(rolloutPtrs.at(i)), + std::move(mpcnetDefinitionPtrs.at(i)), referenceManagerPtrs.at(i)))); + } + } + + // policy evaluation + nPolicyEvaluationThreads_ = nPolicyEvaluationThreads; + if (nPolicyEvaluationThreads_ > 0) { + policyEvaluationThreadPoolPtr_.reset(new ThreadPool(nPolicyEvaluationThreads_)); + policyEvaluationPtrs_.reserve(nPolicyEvaluationThreads_); + for (int i = nDataGenerationThreads_; i < (nDataGenerationThreads_ + nPolicyEvaluationThreads_); i++) { + policyEvaluationPtrs_.push_back(std::unique_ptr( + new MpcnetPolicyEvaluation(std::move(mpcPtrs.at(i)), std::move(mpcnetPtrs.at(i)), std::move(rolloutPtrs.at(i)), + std::move(mpcnetDefinitionPtrs.at(i)), referenceManagerPtrs.at(i)))); + } + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetRolloutManager::startDataGeneration(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, + size_t nSamples, const matrix_t& samplingCovariance, + const std::vector& initialObservations, + const std::vector& modeSchedules, + const std::vector& targetTrajectories) { + if (nDataGenerationThreads_ <= 0) { + throw std::runtime_error("MpcnetRolloutManager::startDataGeneration cannot work without at least one data generation thread."); + } + + // reset variables + dataGenerationFtrs_.clear(); + nDataGenerationTasksDone_ = 0; + + // push tasks into pool + for (int i = 0; i < initialObservations.size(); i++) { + dataGenerationFtrs_.push_back(dataGenerationThreadPoolPtr_->run([=](int threadNumber) { + data_ptr_t result; + result = dataGenerationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, dataDecimation, nSamples, samplingCovariance, + initialObservations.at(i), modeSchedules.at(i), targetTrajectories.at(i)); + nDataGenerationTasksDone_++; + // print thread and task number + std::cerr << "Data generation thread " << threadNumber << " finished task " << nDataGenerationTasksDone_ << std::endl; + return result; + })); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetRolloutManager::isDataGenerationDone() { + if (nDataGenerationThreads_ <= 0) { + throw std::runtime_error("MpcnetRolloutManager::isDataGenerationDone cannot work without at least one data generation thread."); + } + if (dataGenerationFtrs_.size() <= 0) { + throw std::runtime_error( + "MpcnetRolloutManager::isDataGenerationDone cannot return if startDataGeneration has not been triggered once."); + } + + // check if done + if (nDataGenerationTasksDone_ < dataGenerationFtrs_.size()) { + return false; + } else if (nDataGenerationTasksDone_ == dataGenerationFtrs_.size()) { + return true; + } else { + throw std::runtime_error("MpcnetRolloutManager::isDataGenerationDone error since more tasks done than futures available."); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetRolloutManager::data_array_t MpcnetRolloutManager::getGeneratedData() { + if (nDataGenerationThreads_ <= 0) { + throw std::runtime_error("MpcnetRolloutManager::getGeneratedData cannot work without at least one data generation thread."); + } + if (!isDataGenerationDone()) { + throw std::runtime_error("MpcnetRolloutManager::getGeneratedData cannot get data when data generation is not done."); + } + + // get pointers to data + std::vector dataPtrs; + for (int i = 0; i < dataGenerationFtrs_.size(); i++) { + try { + // get results from futures of the tasks + dataPtrs.push_back(dataGenerationFtrs_[i].get()); + } catch (const std::exception& e) { + // print error for exceptions + std::cerr << "MpcnetRolloutManager::getGeneratedData A standard exception was caught, with message: " << e.what() << std::endl; + } + } + + // find number of data points + int nDataPoints = 0; + for (int i = 0; i < dataPtrs.size(); i++) { + nDataPoints += dataPtrs[i]->size(); + } + + // fill data array + data_array_t dataArray; + dataArray.reserve(nDataPoints); + for (int i = 0; i < dataPtrs.size(); i++) { + for (int j = 0; j < dataPtrs[i]->size(); j++) { + dataArray.push_back((*dataPtrs[i])[j]); + } + } + + // return data + return dataArray; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetRolloutManager::startPolicyEvaluation(const std::string& policyFilePath, scalar_t timeStep, + const std::vector& initialObservations, + const std::vector& modeSchedules, + const std::vector& targetTrajectories) { + if (nPolicyEvaluationThreads_ <= 0) { + throw std::runtime_error("MpcnetRolloutManager::startPolicyEvaluation cannot work without at least one policy evaluation thread."); + } + + // reset variables + policyEvaluationFtrs_.clear(); + nPolicyEvaluationTasksDone_ = 0; + + // push tasks into pool + for (int i = 0; i < initialObservations.size(); i++) { + policyEvaluationFtrs_.push_back(policyEvaluationThreadPoolPtr_->run([=](int threadNumber) { + metrics_ptr_t result; + result = policyEvaluationPtrs_[threadNumber]->run(policyFilePath, timeStep, initialObservations.at(i), modeSchedules.at(i), + targetTrajectories.at(i)); + nPolicyEvaluationTasksDone_++; + // print thread and task number + std::cerr << "Policy evaluation thread " << threadNumber << " finished task " << nPolicyEvaluationTasksDone_ << std::endl; + return result; + })); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetRolloutManager::isPolicyEvaluationDone() { + if (nPolicyEvaluationThreads_ <= 0) { + throw std::runtime_error("MpcnetRolloutManager::isPolicyEvaluationDone cannot work without at least one policy evaluation thread."); + } + if (policyEvaluationFtrs_.size() <= 0) { + throw std::runtime_error( + "MpcnetRolloutManager::isPolicyEvaluationDone cannot return if startPolicyEvaluation has not been triggered once."); + } + + // check if done + if (nPolicyEvaluationTasksDone_ < policyEvaluationFtrs_.size()) { + return false; + } else if (nPolicyEvaluationTasksDone_ == policyEvaluationFtrs_.size()) { + return true; + } else { + throw std::runtime_error("MpcnetRolloutManager::isPolicyEvaluationDone error since more tasks done than futures available."); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetRolloutManager::metrics_array_t MpcnetRolloutManager::getComputedMetrics() { + if (nPolicyEvaluationThreads_ <= 0) { + throw std::runtime_error("MpcnetRolloutManager::getComputedMetrics cannot work without at least one policy evaluation thread."); + } + if (!isPolicyEvaluationDone()) { + throw std::runtime_error("MpcnetRolloutManager::getComputedMetrics cannot get metrics when policy evaluation is not done."); + } + + // get pointers to metrics + std::vector metricsPtrs; + for (int i = 0; i < policyEvaluationFtrs_.size(); i++) { + try { + // get results from futures of the tasks + metricsPtrs.push_back(policyEvaluationFtrs_[i].get()); + } catch (const std::exception& e) { + // print error for exceptions + std::cerr << "MpcnetRolloutManager::getComputedMetrics A standard exception was caught, with message: " << e.what() << std::endl; + } + } + + // fill metrics array + metrics_array_t metricsArray; + metricsArray.reserve(metricsPtrs.size()); + for (int i = 0; i < metricsPtrs.size(); i++) { + metricsArray.push_back((*metricsPtrs[i])); + } + + // return metrics + return metricsArray; +} + +} // namespace ocs2 From c6fac5cf5e5bd13fb4fe57f95169d3b19338c946 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Tue, 17 Aug 2021 16:19:52 +0200 Subject: [PATCH 004/512] add missing dependency on roslib (from ros::package) --- ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt | 1 + ocs2_robotic_examples/ocs2_ballbot/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt index c8ea706bb..b5ce41050 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt @@ -6,6 +6,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CATKIN_PACKAGE_DEPENDENCIES pybind11_catkin + roslib ocs2_core ocs2_ddp ocs2_mpc diff --git a/ocs2_robotic_examples/ocs2_ballbot/package.xml b/ocs2_robotic_examples/ocs2_ballbot/package.xml index 64b1eb1b4..a2e78e8d2 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot/package.xml @@ -14,6 +14,7 @@ cmake_clang_tools pybind11_catkin + roslib ocs2_core ocs2_ddp From 68d0153d10170aec70d721fe35a50841ea369eeb Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Tue, 17 Aug 2021 16:21:45 +0200 Subject: [PATCH 005/512] add ocs2_ballbot_mpcnet package --- .../ocs2_ballbot_mpcnet/CMakeLists.txt | 105 +++++++++ .../BallbotMpcnetDefinition.h | 40 ++++ .../BallbotMpcnetInterface.h | 36 ++++ .../ocs2_ballbot_mpcnet/package.xml | 19 ++ .../python/ocs2_ballbot_mpcnet/__init__.py | 8 + .../ocs2_ballbot_mpcnet/ballbot_config.py | 30 +++ .../ocs2_ballbot_mpcnet/ballbot_helper.py | 84 ++++++++ .../ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 204 ++++++++++++++++++ .../ocs2_ballbot_mpcnet/setup.py | 11 + .../src/BallbotMpcnetDefinition.cpp | 19 ++ .../src/BallbotMpcnetInterface.cpp | 60 ++++++ .../src/BallbotMpcnetPybindings.cpp | 5 + 12 files changed, 621 insertions(+) create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt new file mode 100644 index 000000000..da986a531 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt @@ -0,0 +1,105 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_ballbot_mpcnet) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_ballbot + ocs2_mpcnet +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# main library +add_library(${PROJECT_NAME} + src/BallbotMpcnetDefinition.cpp + src/BallbotMpcnetInterface.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +# python bindings +pybind11_add_module(BallbotMpcnetPybindings SHARED + src/BallbotMpcnetPybindings.cpp +) +add_dependencies(BallbotMpcnetPybindings + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(BallbotMpcnetPybindings PRIVATE + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(BallbotMpcnetPybindings + PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +catkin_python_setup() + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target ocs2_ballbot_mpcnet") + add_clang_tooling( + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR +) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS BallbotMpcnetPybindings + ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +############# +## Testing ## +############# + +# TODO(areske) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h new file mode 100644 index 000000000..c23ade1a7 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h @@ -0,0 +1,40 @@ +#pragma once + +#include + +namespace ocs2 { +namespace ballbot { + +/** + * MPC-Net definitions for ballbot. + */ +class BallbotMpcnetDefinition : public MpcnetDefinitionBase { + public: + /** + * Default constructor. + */ + BallbotMpcnetDefinition() = default; + + /** + * Default destructor. + */ + ~BallbotMpcnetDefinition() override = default; + + /** + * @see MpcnetDefinitionBase::getGeneralizedTime + */ + vector_t getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) override; + + /** + * @see MpcnetDefinitionBase::getRelativeState + */ + vector_t getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) override; + + /** + * @see MpcnetDefinitionBase::validState + */ + bool validState(const vector_t& x) override; +}; + +} // namespace ballbot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h new file mode 100644 index 000000000..aa23cd6b8 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +namespace ocs2 { +namespace ballbot { + +/** + * Ballbot MPC-Net interface between C++ and Python. + */ +class BallbotMpcnetInterface : public MpcnetInterfaceBase { + public: + /** + * Constructor. + * @param [in] nDataGenerationThreads : Number of data generation threads. + * @param [in] nPolicyEvaluationThreads : Number of policy evaluation threads. + */ + BallbotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads); + + /** + * Default destructor. + */ + virtual ~BallbotMpcnetInterface() = default; + + private: + /** + * Helper to get the MPC. + * @param [in] ballbotInterface : The ballbot interface. + * @return Pointer to the MPC. + */ + std::unique_ptr getMpc(BallbotInterface& ballbotInterface); +}; + +} // namespace ballbot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml new file mode 100644 index 000000000..74223fd0a --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml @@ -0,0 +1,19 @@ + + + ocs2_ballbot_mpcnet + 0.0.0 + The ocs2_ballbot_mpcnet package + + Farbod Farshidian + Alexander Reske + + TODO + + catkin + + cmake_clang_tools + + ocs2_ballbot + ocs2_mpcnet + + diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py new file mode 100644 index 000000000..8893e0c6a --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py @@ -0,0 +1,8 @@ +from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import MpcnetInterface +from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import size_array, scalar_array, vector_array, matrix_array +from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import ScalarFunctionQuadraticApproximation +from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import SystemObservation, SystemObservationArray +from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import ModeSchedule, ModeScheduleArray +from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import TargetTrajectories, TargetTrajectoriesArray +from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import DataPoint, DataArray +from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import Metrics, MetricsArray diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py new file mode 100644 index 000000000..003faba4d --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py @@ -0,0 +1,30 @@ +from ocs2_mpcnet import config + +# +# config +# + +# data type for tensor elements +dtype = config.dtype + +# device on which tensors will be allocated +device = config.device + +# +# ballbot_config +# + +# name of the robot +name = "ballbot" + +# (generalized) time dimension +TIME_DIM = 1 + +# state dimension +STATE_DIM = 10 + +# input dimension +INPUT_DIM = 3 + +# input cost for behavioral cloning +R = [2.0, 2.0, 2.0] \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py new file mode 100644 index 000000000..2ac9052e3 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py @@ -0,0 +1,84 @@ +import numpy as np + +from ocs2_ballbot_mpcnet import size_array, scalar_array, vector_array, SystemObservation, SystemObservationArray, ModeSchedule, ModeScheduleArray, TargetTrajectories, TargetTrajectoriesArray + + +def get_system_observation(time, state): + system_observation = SystemObservation() + system_observation.mode = 0 + system_observation.time = time + system_observation.state = state + system_observation.input = np.zeros(3) + return system_observation + + +def get_system_observation_array(times, states): + system_observation_array = SystemObservationArray() + system_observation_array.resize(len(times)) + for i in range(len(times)): + system_observation_array[i] = get_system_observation(times[i], states[i]) + return system_observation_array + + +def get_mode_schedule(): + event_times_np = np.array([0.0], dtype=np.float64) + mode_sequence_np = np.array([0, 0], dtype=np.uintp) + event_times = scalar_array() + event_times.resize(len(event_times_np)) + for i in range(len(event_times_np)): + event_times[i] = event_times_np[i] + mode_sequence = size_array() + mode_sequence.resize(len(mode_sequence_np)) + for i in range(len(mode_sequence_np)): + mode_sequence[i] = mode_sequence_np[i] + return ModeSchedule(event_times, mode_sequence) + + +def get_mode_schedule_array(length): + mode_schedule_array = ModeScheduleArray() + mode_schedule_array.resize(length) + for i in range(length): + mode_schedule_array[i] = get_mode_schedule() + return mode_schedule_array + + +def get_target_trajectories(time, state, input): + # time + time_trajectory = scalar_array() + time_trajectory.resize(1) + time_trajectory[0] = time + # state + state_trajectory = vector_array() + state_trajectory.resize(1) + state_trajectory[0] = state + # input + input_trajectory = vector_array() + input_trajectory.resize(1) + input_trajectory[0] = input + return TargetTrajectories(time_trajectory, state_trajectory, input_trajectory) + + +def get_target_trajectories_array(times, states, inputs): + target_trajectories_array = TargetTrajectoriesArray() + target_trajectories_array.resize(len(times)) + for i in range(len(times)): + target_trajectories_array[i] = get_target_trajectories(times[i], states[i], inputs[i]) + return target_trajectories_array + + +def get_random_initial_state(): + random_state = np.zeros(10) + random_state[0] = np.random.uniform(-0.5, 0.5) # base x + random_state[1] = np.random.uniform(-0.5, 0.5) # base y + random_state[2] = np.random.uniform(-0.5, 0.5) # base yaw + random_state[3] = np.random.uniform(-0.1, 0.1) # base pitch + random_state[4] = np.random.uniform(-0.1, 0.1) # base roll + return random_state + + +def get_random_target_state(): + random_state = np.zeros(10) + random_state[0] = np.random.uniform(-0.5, 0.5) # base x + random_state[1] = np.random.uniform(-0.5, 0.5) # base y + random_state[2] = np.random.uniform(-0.5, 0.5) # base yaw + return random_state diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py new file mode 100644 index 000000000..967c16e66 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -0,0 +1,204 @@ +import os +import time +import datetime +import torch +import numpy as np + +from torch.utils.tensorboard import SummaryWriter + +from ocs2_mpcnet.loss import BehavioralCloning as Loss +from ocs2_mpcnet.memory import ReplayMemory as Memory +from ocs2_mpcnet.policy import LinearPolicy as Policy + +import ballbot_config as config +from ballbot_helper import get_system_observation_array, get_mode_schedule_array, get_target_trajectories_array, get_random_initial_state, get_random_target_state + +from ocs2_ballbot_mpcnet import MpcnetInterface + +# settings for data generation by applying behavioral policy +data_generation_time_step = 0.1 +data_generation_duration = 3.0 +data_generation_data_decimation = 1 +data_generation_n_threads = 2 +data_generation_n_tasks = 10 +data_generation_n_samples = 2 +data_generation_sampling_covariance = np.zeros((10, 10), order='F') +for i in range(10): + data_generation_sampling_covariance[i, i] = 0.01 + +# settings for computing metrics by applying learned policy +policy_evaluation_time_step = 0.1 +policy_evaluation_duration = 3.0 +policy_evaluation_n_threads = 2 +policy_evaluation_n_tasks = 10 + +# mpcnet interface +mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads) + +# logging +description = "description" +folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.name + "_" + description +writer = SummaryWriter("runs/" + folder) +os.makedirs(name="policies/" + folder) + +# loss +loss = Loss(torch.tensor(config.R, device=config.device, dtype=config.dtype).diag(), np.diag(config.R)) + +# memory +memory_capacity = 1000000 +memory = Memory(memory_capacity) + +# policy +policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) +policy.to(config.device) +print("Initial policy parameters:") +print(list(policy.named_parameters())) +dummy_input = (torch.randn(config.TIME_DIM, device=config.device, dtype=config.dtype), + torch.randn(config.STATE_DIM, device=config.device, dtype=config.dtype)) +print("Saving initial policy.") +save_path = "policies/" + folder + "/initial_policy" +torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") +torch.save(obj=policy, f=save_path + ".pt") + +# optimizer +learning_rate = 1e-2 +learning_iterations = 100000 +optimizer = torch.optim.Adam(policy.parameters(), lr=learning_rate) +batch_size = 2 ** 5 + + +def start_data_generation(alpha, policy): + policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + initial_times = 0.0 * np.ones(data_generation_n_tasks) + initial_states = np.zeros((data_generation_n_tasks, config.STATE_DIM)) + for i in range(data_generation_n_tasks): + initial_states[i, :] = get_random_initial_state() + target_times = data_generation_duration * np.ones(data_generation_n_tasks) + target_states = np.zeros((data_generation_n_tasks, config.STATE_DIM)) + target_inputs = np.zeros((data_generation_n_tasks, config.INPUT_DIM)) + for i in range(data_generation_n_tasks): + target_states[i, :] = get_random_target_state() + mpcnet_interface.startDataGeneration(alpha, policy_file_path, data_generation_time_step, data_generation_data_decimation, + data_generation_n_samples, data_generation_sampling_covariance, + get_system_observation_array(initial_times, initial_states), + get_mode_schedule_array(data_generation_n_tasks), + get_target_trajectories_array(target_times, target_states, target_inputs)) + + +def start_policy_evaluation(policy): + policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + initial_times = 0.0 * np.ones(policy_evaluation_n_tasks) + initial_states = np.zeros((policy_evaluation_n_tasks, config.STATE_DIM)) + for i in range(policy_evaluation_n_tasks): + initial_states[i, :] = get_random_initial_state() + target_times = policy_evaluation_duration * np.ones(policy_evaluation_n_tasks) + target_states = np.zeros((policy_evaluation_n_tasks, config.STATE_DIM)) + target_inputs = np.zeros((policy_evaluation_n_tasks, config.INPUT_DIM)) + for i in range(policy_evaluation_n_tasks): + target_states[i, :] = get_random_target_state() + mpcnet_interface.startPolicyEvaluation(policy_file_path, policy_evaluation_time_step, + get_system_observation_array(initial_times, initial_states), + get_mode_schedule_array(policy_evaluation_n_tasks), + get_target_trajectories_array(target_times, target_states, target_inputs)) + + +try: + print("==============\nWaiting for first data.\n==============") + start_data_generation(alpha=1.0, policy=policy) + start_policy_evaluation(policy=policy) + while not mpcnet_interface.isDataGenerationDone(): + time.sleep(1.0) + + print("==============\nStarting training.\n==============") + for iteration in range(learning_iterations): + alpha = 1.0 - 1.0 * iteration / learning_iterations + + # data generation + if mpcnet_interface.isDataGenerationDone(): + # get generated data + data = mpcnet_interface.getGeneratedData() + for i in range(len(data)): + # push t, x, u, generalized time, relative state, Hamiltonian into memeory + memory.push(data[i].t, data[i].x, data[i].u, data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) + # logging + writer.add_scalar('data/new_data_points', len(data), iteration) + writer.add_scalar('data/total_data_points', memory.size, iteration) + print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) + # start new data generation + start_data_generation(alpha=alpha, policy=policy) + + # policy evaluation + if mpcnet_interface.isPolicyEvaluationDone(): + # get computed metrics + metrics = mpcnet_interface.getComputedMetrics() + survival_time = np.mean([metrics[i].survival_time for i in range(len(metrics))]) + incurred_hamiltonian = np.mean([metrics[i].incurred_hamiltonian for i in range(len(metrics))]) + # logging + writer.add_scalar('metric/survival_time', survival_time, iteration) + writer.add_scalar('metric/incurred_hamiltonian', incurred_hamiltonian, iteration) + print("iteration", iteration, "received metrics:", "incurred_hamiltonian", incurred_hamiltonian, "survival_time", survival_time) + # start new policy evaluation + start_policy_evaluation(policy=policy) + + # intermediate policies + if (iteration % 1000 == 0) and (iteration > 0): + print("Saving intermediate policy for iteration", iteration) + save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) + torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.save(obj=policy, f=save_path + ".pt") + + # extract batch of samples from replay memory + samples = memory.sample(batch_size) + + # take an optimization step + def closure(): + # clear the gradients + optimizer.zero_grad() + # compute the empirical loss + empiricial_loss = torch.zeros(1, dtype=config.dtype, device=config.device) + for sample in samples: + # torch + t = torch.tensor([sample.t], dtype=config.dtype, device=config.device) + x = torch.tensor(sample.x, dtype=config.dtype, device=config.device) + u_target = torch.tensor(sample.u, dtype=config.dtype, device=config.device) + generalized_time = torch.tensor(sample.generalized_time, dtype=config.dtype, device=config.device) + relative_state = torch.tensor(sample.relative_state, dtype=config.dtype, device=config.device) + p, U = policy(generalized_time, relative_state) + u_predicted = torch.matmul(p, U) + # empirical loss + empiricial_loss = empiricial_loss + loss.compute_torch(u_predicted, u_target) + # compute the gradients + empiricial_loss.backward() + # log metrics + writer.add_scalar('objective/empirical_loss', empiricial_loss.item() / batch_size, iteration) + # return empiricial loss + return empiricial_loss + optimizer.step(closure) + + # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) + if iteration == learning_iterations - 1: + while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): + time.sleep(1.0) + + print("==============\nTraining completed.\n==============") + +except KeyboardInterrupt: + # let data generation and policy evaluation finish (to avoid a segmentation fault) + while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): + time.sleep(1.0) + print("==============\nTraining interrupted.\n==============") + pass + +print("Final policy parameters:") +print(list(policy.named_parameters())) + +print("Saving final policy.") +save_path = "policies/" + folder + "/final_policy" +torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") +torch.save(obj=policy, f=save_path + ".pt") + +writer.close() + +print("Done. Exiting now.") diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py new file mode 100644 index 000000000..c33f1ef78 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup( + packages=['ocs2_ballbot_mpcnet'], + package_dir={'': 'python'} +) + +setup(**setup_args) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp new file mode 100644 index 000000000..97251bce8 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp @@ -0,0 +1,19 @@ +#include "ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h" + +namespace ocs2 { +namespace ballbot { + +vector_t BallbotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) { + return t * vector_t::Ones(1); +} + +vector_t BallbotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { + return x - targetTrajectories.getDesiredState(t); +} + +bool BallbotMpcnetDefinition::validState(const vector_t& x) { + return true; +} + +} // namespace ballbot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp new file mode 100644 index 000000000..8db1b0034 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -0,0 +1,60 @@ +#include "ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h" + +#include + +#include +#include +#include +#include + +#include "ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h" + +namespace ocs2 { +namespace ballbot { + +BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads) { + // create ONNX environment + auto onnxEnvironmentPtr = createOnnxEnvironment(); + // path to config file + std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/mpc/task.info"; + // path to save auto-generated libraries + std::string libraryFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + // set up MPC-Net rollout manager for data generation and policy evaluation + std::vector> mpcPtrs; + std::vector> mpcnetPtrs; + std::vector> rolloutPtrs; + std::vector> mpcnetDefinitionPtrs; + std::vector> referenceManagerPtrs; + mpcPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + mpcnetPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + rolloutPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + mpcnetDefinitionPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { + BallbotInterface ballbotInterface(taskFile, libraryFolder); + std::shared_ptr mpcnetDefinitionPtr(new BallbotMpcnetDefinition()); + mpcPtrs.push_back(getMpc(ballbotInterface)); + mpcnetPtrs.push_back(std::unique_ptr( + new MpcnetOnnxController(mpcnetDefinitionPtr, ballbotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr))); + rolloutPtrs.push_back(std::unique_ptr(ballbotInterface.getRollout().clone())); + mpcnetDefinitionPtrs.push_back(mpcnetDefinitionPtr); + referenceManagerPtrs.push_back(ballbotInterface.getReferenceManagerPtr()); + } + mpcnetRolloutManagerPtr_.reset(new MpcnetRolloutManager(nDataGenerationThreads, nPolicyEvaluationThreads, std::move(mpcPtrs), + std::move(mpcnetPtrs), std::move(rolloutPtrs), mpcnetDefinitionPtrs, + referenceManagerPtrs)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::unique_ptr BallbotMpcnetInterface::getMpc(BallbotInterface& ballbotInterface) { + std::unique_ptr mpcPtr(new MPC_DDP(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), + ballbotInterface.getRollout(), ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer())); + mpcPtr->getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); + return mpcPtr; +} + +} // namespace ballbot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp new file mode 100644 index 000000000..dd6e5af0b --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp @@ -0,0 +1,5 @@ +#include + +#include + +CREATE_MPCNET_PYTHON_BINDINGS(ocs2::ballbot::BallbotMpcnetInterface, BallbotMpcnetPybindings) From 7bd18c3efb32089c6f212d01a3bddedc7fa7885b Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Wed, 18 Aug 2021 08:34:05 +0200 Subject: [PATCH 006/512] add onnxruntime to jenkins pipeline --- jenkins-pipeline | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jenkins-pipeline b/jenkins-pipeline index c5f27244f..18a677e5b 100644 --- a/jenkins-pipeline +++ b/jenkins-pipeline @@ -1,5 +1,5 @@ library 'continuous_integration_pipeline' -ciPipeline("--ros-distro noetic --publish-doxygen --recipes raisimlib \ +ciPipeline("--ros-distro noetic --publish-doxygen --recipes onnxruntime raisimlib \ --dependencies 'git@github.com:leggedrobotics/hpp-fcl.git;master;git'\ 'git@github.com:leggedrobotics/pinocchio.git;master;git'\ 'git@github.com:ANYbotics/anymal_c_simple_description.git;master;git'\ From 0fdeb65516cfc5f24518da0825b146937cdc94d6 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 14:30:36 +0200 Subject: [PATCH 007/512] small fixes and adapt helper functions --- .../ocs2_ballbot_mpcnet/ballbot_config.py | 2 +- .../ocs2_ballbot_mpcnet/ballbot_helper.py | 119 +++++++++++------- .../ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 63 +++------- .../src/BallbotMpcnetPybindings.cpp | 2 +- 4 files changed, 99 insertions(+), 87 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py index 003faba4d..a9926d59f 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py @@ -27,4 +27,4 @@ INPUT_DIM = 3 # input cost for behavioral cloning -R = [2.0, 2.0, 2.0] \ No newline at end of file +R = [2.0, 2.0, 2.0] diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py index 2ac9052e3..a52b3cdf9 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py @@ -1,6 +1,32 @@ import numpy as np -from ocs2_ballbot_mpcnet import size_array, scalar_array, vector_array, SystemObservation, SystemObservationArray, ModeSchedule, ModeScheduleArray, TargetTrajectories, TargetTrajectoriesArray +from ocs2_ballbot_mpcnet import size_array, scalar_array, vector_array, SystemObservation, SystemObservationArray,\ + ModeSchedule, ModeScheduleArray, TargetTrajectories, TargetTrajectoriesArray +from ocs2_ballbot_mpcnet import ballbot_config as config + + +def get_size_array(data): + my_size_array = size_array() + my_size_array.resize(len(data)) + for i in range(len(data)): + my_size_array[i] = data[i] + return my_size_array + + +def get_scalar_array(data): + my_scalar_array = scalar_array() + my_scalar_array.resize(len(data)) + for i in range(len(data)): + my_scalar_array[i] = data[i] + return my_scalar_array + + +def get_vector_array(data): + my_vector_array = vector_array() + my_vector_array.resize(len(data)) + for i in range(len(data)): + my_vector_array[i] = data[i] + return my_vector_array def get_system_observation(time, state): @@ -8,66 +34,63 @@ def get_system_observation(time, state): system_observation.mode = 0 system_observation.time = time system_observation.state = state - system_observation.input = np.zeros(3) + system_observation.input = np.zeros(config.STATE_DIM) return system_observation -def get_system_observation_array(times, states): +def get_system_observation_array(length): system_observation_array = SystemObservationArray() - system_observation_array.resize(len(times)) - for i in range(len(times)): - system_observation_array[i] = get_system_observation(times[i], states[i]) + system_observation_array.resize(length) return system_observation_array -def get_mode_schedule(): - event_times_np = np.array([0.0], dtype=np.float64) - mode_sequence_np = np.array([0, 0], dtype=np.uintp) - event_times = scalar_array() - event_times.resize(len(event_times_np)) - for i in range(len(event_times_np)): - event_times[i] = event_times_np[i] - mode_sequence = size_array() - mode_sequence.resize(len(mode_sequence_np)) - for i in range(len(mode_sequence_np)): - mode_sequence[i] = mode_sequence_np[i] - return ModeSchedule(event_times, mode_sequence) +def get_target_trajectories(time_trajectory, state_trajectory): + time_trajectory_array = get_scalar_array(time_trajectory) + state_trajectory_array = get_vector_array(state_trajectory) + input_trajectory_array = get_vector_array(np.zeros((len(time_trajectory), config.INPUT_DIM))) + return TargetTrajectories(time_trajectory_array, state_trajectory_array, input_trajectory_array) + + +def get_target_trajectories_array(length): + target_trajectories_array = TargetTrajectoriesArray() + target_trajectories_array.resize(length) + return target_trajectories_array + + +def get_mode_schedule(event_times, mode_sequence): + event_times_array = get_scalar_array(event_times) + mode_sequence_array = get_size_array(mode_sequence) + return ModeSchedule(event_times_array, mode_sequence_array) def get_mode_schedule_array(length): mode_schedule_array = ModeScheduleArray() mode_schedule_array.resize(length) - for i in range(length): - mode_schedule_array[i] = get_mode_schedule() return mode_schedule_array -def get_target_trajectories(time, state, input): - # time - time_trajectory = scalar_array() - time_trajectory.resize(1) - time_trajectory[0] = time - # state - state_trajectory = vector_array() - state_trajectory.resize(1) - state_trajectory[0] = state - # input - input_trajectory = vector_array() - input_trajectory.resize(1) - input_trajectory[0] = input - return TargetTrajectories(time_trajectory, state_trajectory, input_trajectory) +def get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template): + gait_cycle_duration = event_times_template[-1] + num_gait_cycles = int(np.floor(duration / gait_cycle_duration)) + event_times = np.array([0.0], dtype=np.float64) + mode_sequence = np.array([0], dtype=np.uintp) + for _ in range(num_gait_cycles): + event_times = np.append(event_times, event_times[-1] * np.ones(len(event_times_template)) + event_times_template) + mode_sequence = np.append(mode_sequence, mode_sequence_template) + mode_sequence = np.append(mode_sequence, np.array([0], dtype=np.uintp)) + return event_times, mode_sequence -def get_target_trajectories_array(times, states, inputs): - target_trajectories_array = TargetTrajectoriesArray() - target_trajectories_array.resize(len(times)) - for i in range(len(times)): - target_trajectories_array[i] = get_target_trajectories(times[i], states[i], inputs[i]) - return target_trajectories_array +def get_default_mode_schedule(duration): + # contact schedule: - + # swing schedule: - + event_times_template = np.array([1.0], dtype=np.float64) + mode_sequence_template = np.array([0], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) def get_random_initial_state(): - random_state = np.zeros(10) + random_state = np.zeros(config.STATE_DIM) random_state[0] = np.random.uniform(-0.5, 0.5) # base x random_state[1] = np.random.uniform(-0.5, 0.5) # base y random_state[2] = np.random.uniform(-0.5, 0.5) # base yaw @@ -77,8 +100,20 @@ def get_random_initial_state(): def get_random_target_state(): - random_state = np.zeros(10) + random_state = np.zeros(config.STATE_DIM) random_state[0] = np.random.uniform(-0.5, 0.5) # base x random_state[1] = np.random.uniform(-0.5, 0.5) # base y random_state[2] = np.random.uniform(-0.5, 0.5) # base yaw return random_state + + +def get_tasks(n_tasks, duration): + initial_observations = get_system_observation_array(n_tasks) + mode_schedules = get_mode_schedule_array(n_tasks) + target_trajectories = get_target_trajectories_array(n_tasks) + for i in range(n_tasks): + initial_observations[i] = get_system_observation(0.0, get_random_initial_state()) + mode_schedules[i] = get_mode_schedule(*get_default_mode_schedule(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state().reshape((1, config.STATE_DIM))) + return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 967c16e66..14e543ceb 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -10,9 +10,8 @@ from ocs2_mpcnet.memory import ReplayMemory as Memory from ocs2_mpcnet.policy import LinearPolicy as Policy -import ballbot_config as config -from ballbot_helper import get_system_observation_array, get_mode_schedule_array, get_target_trajectories_array, get_random_initial_state, get_random_target_state - +from ocs2_ballbot_mpcnet import ballbot_config as config +from ocs2_ballbot_mpcnet import ballbot_helper as helper from ocs2_ballbot_mpcnet import MpcnetInterface # settings for data generation by applying behavioral policy @@ -22,8 +21,8 @@ data_generation_n_threads = 2 data_generation_n_tasks = 10 data_generation_n_samples = 2 -data_generation_sampling_covariance = np.zeros((10, 10), order='F') -for i in range(10): +data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order='F') +for i in range(config.STATE_DIM): data_generation_sampling_covariance[i, i] = 0.01 # settings for computing metrics by applying learned policy @@ -61,47 +60,27 @@ torch.save(obj=policy, f=save_path + ".pt") # optimizer +batch_size = 2 ** 5 learning_rate = 1e-2 -learning_iterations = 100000 +learning_iterations = 10000 optimizer = torch.optim.Adam(policy.parameters(), lr=learning_rate) -batch_size = 2 ** 5 def start_data_generation(alpha, policy): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - initial_times = 0.0 * np.ones(data_generation_n_tasks) - initial_states = np.zeros((data_generation_n_tasks, config.STATE_DIM)) - for i in range(data_generation_n_tasks): - initial_states[i, :] = get_random_initial_state() - target_times = data_generation_duration * np.ones(data_generation_n_tasks) - target_states = np.zeros((data_generation_n_tasks, config.STATE_DIM)) - target_inputs = np.zeros((data_generation_n_tasks, config.INPUT_DIM)) - for i in range(data_generation_n_tasks): - target_states[i, :] = get_random_target_state() + initial_observations, mode_schedules, target_trajectories = helper.get_tasks(data_generation_n_tasks, data_generation_duration) mpcnet_interface.startDataGeneration(alpha, policy_file_path, data_generation_time_step, data_generation_data_decimation, data_generation_n_samples, data_generation_sampling_covariance, - get_system_observation_array(initial_times, initial_states), - get_mode_schedule_array(data_generation_n_tasks), - get_target_trajectories_array(target_times, target_states, target_inputs)) + initial_observations, mode_schedules, target_trajectories) def start_policy_evaluation(policy): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - initial_times = 0.0 * np.ones(policy_evaluation_n_tasks) - initial_states = np.zeros((policy_evaluation_n_tasks, config.STATE_DIM)) - for i in range(policy_evaluation_n_tasks): - initial_states[i, :] = get_random_initial_state() - target_times = policy_evaluation_duration * np.ones(policy_evaluation_n_tasks) - target_states = np.zeros((policy_evaluation_n_tasks, config.STATE_DIM)) - target_inputs = np.zeros((policy_evaluation_n_tasks, config.INPUT_DIM)) - for i in range(policy_evaluation_n_tasks): - target_states[i, :] = get_random_target_state() + initial_observations, mode_schedules, target_trajectories = helper.get_tasks(policy_evaluation_n_tasks, policy_evaluation_duration) mpcnet_interface.startPolicyEvaluation(policy_file_path, policy_evaluation_time_step, - get_system_observation_array(initial_times, initial_states), - get_mode_schedule_array(policy_evaluation_n_tasks), - get_target_trajectories_array(target_times, target_states, target_inputs)) + initial_observations, mode_schedules, target_trajectories) try: @@ -120,8 +99,8 @@ def start_policy_evaluation(policy): # get generated data data = mpcnet_interface.getGeneratedData() for i in range(len(data)): - # push t, x, u, generalized time, relative state, Hamiltonian into memeory - memory.push(data[i].t, data[i].x, data[i].u, data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) + # push t, x, u, mode, generalized time, relative state, Hamiltonian into memory + memory.push(data[i].t, data[i].x, data[i].u, data[i].mode, data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) # logging writer.add_scalar('data/new_data_points', len(data), iteration) writer.add_scalar('data/total_data_points', memory.size, iteration) @@ -157,24 +136,22 @@ def closure(): # clear the gradients optimizer.zero_grad() # compute the empirical loss - empiricial_loss = torch.zeros(1, dtype=config.dtype, device=config.device) + empirical_loss = torch.zeros(1, dtype=config.dtype, device=config.device) for sample in samples: - # torch - t = torch.tensor([sample.t], dtype=config.dtype, device=config.device) + t = torch.tensor(sample.t, dtype=config.dtype, device=config.device) x = torch.tensor(sample.x, dtype=config.dtype, device=config.device) u_target = torch.tensor(sample.u, dtype=config.dtype, device=config.device) generalized_time = torch.tensor(sample.generalized_time, dtype=config.dtype, device=config.device) relative_state = torch.tensor(sample.relative_state, dtype=config.dtype, device=config.device) p, U = policy(generalized_time, relative_state) u_predicted = torch.matmul(p, U) - # empirical loss - empiricial_loss = empiricial_loss + loss.compute_torch(u_predicted, u_target) + empirical_loss = empirical_loss + loss.compute_torch(u_predicted, u_target) # compute the gradients - empiricial_loss.backward() - # log metrics - writer.add_scalar('objective/empirical_loss', empiricial_loss.item() / batch_size, iteration) - # return empiricial loss - return empiricial_loss + empirical_loss.backward() + # logging + writer.add_scalar('objective/empirical_loss', empirical_loss.item() / batch_size, iteration) + # return empirical loss + return empirical_loss optimizer.step(closure) # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp index dd6e5af0b..b31ecbf21 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp @@ -1,5 +1,5 @@ #include -#include +#include "ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h" CREATE_MPCNET_PYTHON_BINDINGS(ocs2::ballbot::BallbotMpcnetInterface, BallbotMpcnetPybindings) From d8b526a28ef5299ea455281e844b63b66e508ef4 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 14:35:15 +0200 Subject: [PATCH 008/512] add mode to data --- ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h | 1 + ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h | 1 + ocs2_mpcnet/python/ocs2_mpcnet/memory.py | 2 +- ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp | 2 ++ 4 files changed, 5 insertions(+), 1 deletion(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index 3dcd1ed81..dbbea557c 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -67,6 +67,7 @@ using namespace pybind11::literals; .def_readwrite("t", &MPCNET_INTERFACE::data_point_t::t) \ .def_readwrite("x", &MPCNET_INTERFACE::data_point_t::x) \ .def_readwrite("u", &MPCNET_INTERFACE::data_point_t::u) \ + .def_readwrite("mode", &MPCNET_INTERFACE::data_point_t::mode) \ .def_readwrite("generalized_time", &MPCNET_INTERFACE::data_point_t::generalizedTime) \ .def_readwrite("relative_state", &MPCNET_INTERFACE::data_point_t::relativeState) \ .def_readwrite("hamiltonian", &MPCNET_INTERFACE::data_point_t::hamiltonian); \ diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index ec9334fec..4d02dcb0d 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -28,6 +28,7 @@ class MpcnetDataGeneration { scalar_t t; vector_t x; vector_t u; + size_t mode; vector_t generalizedTime; vector_t relativeState; ScalarFunctionQuadraticApproximation hamiltonian; diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py index a78f93092..61ebd7a09 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py @@ -2,7 +2,7 @@ import numpy as np from collections import namedtuple -Sample = namedtuple('sample', ('t', 'x', 'u', 'generalized_time', 'relative_state', 'hamiltonian')) +Sample = namedtuple('sample', ('t', 'x', 'u', 'mode', 'generalized_time', 'relative_state', 'hamiltonian')) class ReplayMemory: diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index a06fb560e..af393d8f5 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -63,6 +63,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st dataPoint.t = primalSolution.timeTrajectory_[0]; dataPoint.x = primalSolution.stateTrajectory_[0]; dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); + dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); // TODO(areske): add once approximation of Hamiltonian is available @@ -77,6 +78,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st dataPoint.x = primalSolution.stateTrajectory_[0] + L * vector_t::NullaryExpr(primalSolution.stateTrajectory_[0].size(), standardNormalNullaryOp); dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); + dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); // TODO(areske): add once approximation of Hamiltonian is available From a916124d807628ba30e06ead49aa348e810964b9 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 14:39:13 +0200 Subject: [PATCH 009/512] cosmetics --- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 4 ++-- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 2 +- ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp | 8 ++++---- ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp | 8 ++++---- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index 55d5c4a3b..fe0df694b 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -2,7 +2,7 @@ import numpy as np -class Hamiltonian(): +class Hamiltonian: # Uses the quadratic approximation of the Hamiltonian as loss # H(x,u) = 1/2 dx' dHdxx dx + du' dHdux dx + 1/2 du' dHduu du + dHdx' dx + dHdu' du + H @@ -16,7 +16,7 @@ def compute_numpy(self, x, u, hamiltonian): return -class BehavioralCloning(): +class BehavioralCloning: # Uses a simple quadratic function as loss # BC(u) = du' R du diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index e7b70c3d9..8eb8b1fb0 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -64,7 +64,7 @@ def forward(self, t, x): class MixtureOfNonlinearExpertsPolicy(Policy): - def __init__(self, dim_t, dim_x, num_experts, dim_u): + def __init__(self, dim_t, dim_x, dim_u, num_experts): super().__init__(dim_t + dim_x, dim_u) self.name = 'MixtureOfNonlinearExpertsPolicy' self.num_experts = num_experts diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index af393d8f5..534a1517c 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -20,16 +20,16 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st scalar_t time = initialObservation.time; vector_t state = initialObservation.state; - // update the reference manager - referenceManagerPtr_->setModeSchedule(modeSchedule); - referenceManagerPtr_->setTargetTrajectories(targetTrajectories); - // reset mpc mpcPtr_->reset(); // prepare learned controller mpcnetPtr_->loadPolicyModel(policyFilePath); + // update the reference manager + referenceManagerPtr_->setModeSchedule(modeSchedule); + referenceManagerPtr_->setTargetTrajectories(targetTrajectories); + // set up behavioral controller with mixture parameter alpha and learned controller MpcnetBehavioralController behavioralController; behavioralController.setAlpha(alpha); diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index 711f4e052..b269e5572 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -16,16 +16,16 @@ MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string scalar_t time = initialObservation.time; vector_t state = initialObservation.state; - // update the reference manager - referenceManagerPtr_->setModeSchedule(modeSchedule); - referenceManagerPtr_->setTargetTrajectories(targetTrajectories); - // reset mpc mpcPtr_->reset(); // prepare learned controller mpcnetPtr_->loadPolicyModel(policyFilePath); + // update the reference manager + referenceManagerPtr_->setModeSchedule(modeSchedule); + referenceManagerPtr_->setTargetTrajectories(targetTrajectories); + // run policy evaluation int iteration = 0; try { From 459bcad1b9584022cdee6173b978de3807b4686d Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 14:40:00 +0200 Subject: [PATCH 010/512] add cross entropy loss --- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index fe0df694b..87c6ac150 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -32,3 +32,19 @@ def compute_torch(self, u_predicted, u_target): def compute_numpy(self, u_predicted, u_target): du = np.subtract(u_predicted, u_target) return np.dot(du, np.matmul(self.R_numpy, du)) + + +class CrossEntropy: + + # Uses the cross entropy between two probability distributions as loss + # CE(p_target, p_predicted) = - sum(p_target * log(p_predicted)) + + def __init__(self, epsilon_torch, epsilon_numpy): + self.epsilon_torch = epsilon_torch + self.epsilon_numpy = epsilon_numpy + + def compute_torch(self, p_target, p_predicted): + return - torch.dot(p_target, torch.log(torch.add(p_predicted, self.epsilon_torch))) + + def compute_numpy(self, p_target, p_predicted): + return - np.dot(p_target, np.log(np.add(p_predicted, self.epsilon_numpy))) From f6576e61ec8c62dac344791447194f5056989dc8 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 14:48:35 +0200 Subject: [PATCH 011/512] define number of contacts/feet instead of magic number --- .../include/ocs2_legged_robot/common/Types.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/common/Types.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/common/Types.h index 646a72cc8..e7643ca47 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/common/Types.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/common/Types.h @@ -37,8 +37,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace legged_robot { +constexpr size_t NUM_CONTACT_POINTS = 4; + template -using feet_array_t = std::array; +using feet_array_t = std::array; using contact_flag_t = feet_array_t; using vector3_t = Eigen::Matrix; From 14117dfa23fec62a5d2de7187cd03e8c85d4256f Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 14:53:47 +0200 Subject: [PATCH 012/512] set mode schedule for gait schedule --- .../include/ocs2_legged_robot/gait/GaitSchedule.h | 5 +++++ .../synchronized_module/SwitchedModelReferenceManager.h | 2 ++ .../synchronized_module/SwitchedModelReferenceManager.cpp | 8 ++++++++ 3 files changed, 15 insertions(+) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h index 1dd064b01..a7221e953 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h @@ -43,6 +43,11 @@ class GaitSchedule { public: GaitSchedule(ModeSchedule initModeSchedule, ModeSequenceTemplate initModeSequenceTemplate, scalar_t phaseTransitionStanceTime); + /** + * @param [in] modeSchedule: The mode schedule to be used. + */ + void setModeSchedule(const ModeSchedule& modeSchedule) { modeSchedule_ = modeSchedule; } + /** * @param [in] lowerBoundTime: The smallest time for which the ModeSchedule should be defined. * @param [in] upperBoundTime: The greatest time for which the ModeSchedule should be defined. diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/synchronized_module/SwitchedModelReferenceManager.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/synchronized_module/SwitchedModelReferenceManager.h index a1ee327d9..9f47651c0 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/synchronized_module/SwitchedModelReferenceManager.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/synchronized_module/SwitchedModelReferenceManager.h @@ -48,6 +48,8 @@ class SwitchedModelReferenceManager : public ReferenceManager { ~SwitchedModelReferenceManager() override = default; + void setModeSchedule(const ModeSchedule& modeSchedule) override; + contact_flag_t getContactFlags(scalar_t time) const; const std::shared_ptr& getGaitSchedule() { return gaitSchedulePtr_; } diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/synchronized_module/SwitchedModelReferenceManager.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/synchronized_module/SwitchedModelReferenceManager.cpp index 2c5dc49c5..237419ea4 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/synchronized_module/SwitchedModelReferenceManager.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/synchronized_module/SwitchedModelReferenceManager.cpp @@ -41,6 +41,14 @@ SwitchedModelReferenceManager::SwitchedModelReferenceManager(std::shared_ptrsetModeSchedule(modeSchedule); +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ From 3792f47f1637a677276e8d1c62a8990bd5444f7a Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 15:07:37 +0200 Subject: [PATCH 013/512] add ocs2_legged_robot_mpcnet package --- .../ocs2_legged_robot_mpcnet/CMakeLists.txt | 107 ++++++ .../LeggedRobotMpcnetDefinition.h | 44 +++ .../LeggedRobotMpcnetInterface.h | 36 ++ .../ocs2_legged_robot_mpcnet/helper/Logic.h | 142 ++++++++ .../helper/Rotation.h | 11 + .../ocs2_legged_robot_mpcnet/package.xml | 19 + .../ocs2_legged_robot_mpcnet/__init__.py | 8 + .../legged_robot_config.py | 83 +++++ .../legged_robot_helper.py | 326 ++++++++++++++++++ .../legged_robot_mpcnet.py | 212 ++++++++++++ .../legged_robot_policy.py | 55 +++ .../ocs2_legged_robot_mpcnet/setup.py | 11 + .../src/LeggedRobotMpcnetDefinition.cpp | 67 ++++ .../src/LeggedRobotMpcnetInterface.cpp | 62 ++++ .../src/LeggedRobotMpcnetPybindings.cpp | 5 + .../src/helper/Logic.cpp | 220 ++++++++++++ .../src/helper/Rotation.cpp | 14 + 17 files changed, 1422 insertions(+) create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Logic.h create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Rotation.h create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Logic.cpp create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Rotation.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt new file mode 100644 index 000000000..1b2f8cee8 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_legged_robot_mpcnet) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_legged_robot + ocs2_mpcnet +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# main library +add_library(${PROJECT_NAME} + src/helper/Logic.cpp + src/helper/Rotation.cpp + src/LeggedRobotMpcnetDefinition.cpp + src/LeggedRobotMpcnetInterface.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +# python bindings +pybind11_add_module(LeggedRobotMpcnetPybindings SHARED + src/LeggedRobotMpcnetPybindings.cpp +) +add_dependencies(LeggedRobotMpcnetPybindings + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(LeggedRobotMpcnetPybindings PRIVATE + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(LeggedRobotMpcnetPybindings + PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +catkin_python_setup() + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target ocs2_legged_robot_mpcnet") + add_clang_tooling( + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR +) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS LeggedRobotMpcnetPybindings + ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +############# +## Testing ## +############# + +# TODO(areske) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h new file mode 100644 index 000000000..55c6fa660 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h @@ -0,0 +1,44 @@ +#pragma once + +#include + +namespace ocs2 { +namespace legged_robot { + +/** + * MPC-Net definitions for legged robot. + */ +class LeggedRobotMpcnetDefinition : public MpcnetDefinitionBase { + public: + /** + * Constructor. + * @param [in] defaultState : Default state. + */ + LeggedRobotMpcnetDefinition(const vector_t& defaultState) : defaultState_(defaultState) {} + + /** + * Default destructor. + */ + ~LeggedRobotMpcnetDefinition() override = default; + + /** + * @see MpcnetDefinitionBase::getGeneralizedTime + */ + vector_t getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) override; + + /** + * @see MpcnetDefinitionBase::getRelativeState + */ + vector_t getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) override; + + /** + * @see MpcnetDefinitionBase::validState + */ + bool validState(const vector_t& x) override; + + private: + vector_t defaultState_; +}; + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h new file mode 100644 index 000000000..41527027a --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +namespace ocs2 { +namespace legged_robot { + +/** + * Legged robot MPC-Net interface between C++ and Python. + */ +class LeggedRobotMpcnetInterface : public MpcnetInterfaceBase { + public: + /** + * Constructor. + * @param [in] nDataGenerationThreads : Number of data generation threads. + * @param [in] nPolicyEvaluationThreads : Number of policy evaluation threads. + */ + LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads); + + /** + * Default destructor. + */ + virtual ~LeggedRobotMpcnetInterface() = default; + + private: + /** + * Helper to get the MPC. + * @param [in] leggedRobotInterface : The legged robot interface. + * @return Pointer to the MPC. + */ + std::unique_ptr getMpc(LeggedRobotInterface& leggedRobotInterface); +}; + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Logic.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Logic.h new file mode 100644 index 000000000..79a0eed71 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Logic.h @@ -0,0 +1,142 @@ +#pragma once + +#include +#include + +namespace ocs2 { +namespace legged_robot { + +struct LegPhase { + scalar_t phase; + scalar_t duration; +}; + +struct ContactTiming { + scalar_t start; + scalar_t end; +}; + +struct SwingTiming { + scalar_t start; + scalar_t end; +}; + +inline scalar_t timingNaN() { + return std::numeric_limits::quiet_NaN(); +} + +inline bool hasStartTime(const ContactTiming& timing) { + return !std::isnan(timing.start); +} +inline bool hasEndTime(const ContactTiming& timing) { + return !std::isnan(timing.end); +} + +inline bool hasStartTime(const SwingTiming& timing) { + return !std::isnan(timing.start); +} +inline bool hasEndTime(const SwingTiming& timing) { + return !std::isnan(timing.end); +} + +inline bool startsWithSwingPhase(const std::vector& timings) { + return timings.empty() || hasStartTime(timings.front()); +} +inline bool startsWithContactPhase(const std::vector& timings) { + return !startsWithSwingPhase(timings); +} +inline bool endsWithSwingPhase(const std::vector& timings) { + return timings.empty() || hasEndTime(timings.back()); +} +inline bool endsWithContactPhase(const std::vector& timings) { + return !endsWithSwingPhase(timings); +} + +inline bool startsWithContactPhase(const std::vector& timings) { + return timings.empty() || hasStartTime(timings.front()); +} +inline bool startsWithSwingPhase(const std::vector& timings) { + return !startsWithContactPhase(timings); +} +inline bool endsWithContactPhase(const std::vector& timings) { + return timings.empty() || hasEndTime(timings.back()); +} +inline bool endsWithSwingPhase(const std::vector& timings) { + return !endsWithContactPhase(timings); +} + +inline bool touchesDownAtLeastOnce(const std::vector& timings) { + return std::any_of(timings.begin(), timings.end(), [](const ContactTiming& timing) { return hasStartTime(timing); }); +} + +inline bool liftsOffAtLeastOnce(const std::vector& timings) { + return !timings.empty() && hasEndTime(timings.front()); +} + +inline bool touchesDownAtLeastOnce(const std::vector& timings) { + return !timings.empty() && hasEndTime(timings.front()); +} + +inline bool liftsOffAtLeastOnce(const std::vector& timings) { + return std::any_of(timings.begin(), timings.end(), [](const SwingTiming& timing) { return hasStartTime(timing); }); +} + +/** + * @brief Get the contact phase for all legs. + * If leg in contact, returns a value between 0.0 (at start of contact phase) and 1.0 (at end of contact phase). + * If leg not in contact (i.e. in swing), returns -1.0. + * If mode schedule starts with contact phase, returns 1.0 during this phase. + * If mode schedule ends with contact phase, returns 0.0 during this phase. + * @param [in] time : Query time. + * @param [in] modeSchedule : Mode schedule. + * @return Contact phases for all legs. + */ +feet_array_t getContactPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule); + +/** + * @brief Get the swing phase for all legs. + * If leg in swing, returns a value between 0.0 (at start of swing phase) and 1.0 (at end of swing phase). + * If leg not in swing (i.e. in contact), returns -1.0. + * If mode schedule starts with swing phase, returns 1.0 during this phase. + * If mode schedule ends with swing phase, returns 0.0 during this phase. + * @param [in] time : Query time. + * @param [in] modeSchedule : Mode schedule. + * @return Swing phases for all legs. + */ +feet_array_t getSwingPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule); + +/** Extracts the contact timings for all legs from a modeSchedule */ +feet_array_t> extractContactTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule); + +/** Extracts the swing timings for all legs from a modeSchedule */ +feet_array_t> extractSwingTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule); + +/** Returns time of the next lift off. Returns nan if leg is not lifting off */ +scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector& contactTimings); + +/** Returns time of the touch down for all legs from a modeschedule. Returns nan if leg does not touch down */ +scalar_t getTimeOfNextTouchDown(scalar_t currentTime, const std::vector& contactTimings); + +/** + * Get {startTime, endTime} for all contact phases. Swingphases are always implied in between: endTime[i] < startTime[i+1] + * times are NaN if they cannot be identified at the boundaries + * Vector is empty if there are no contact phases + */ +std::vector extractContactTimings(const std::vector& eventTimes, const std::vector& contactFlags); + +/** + * Get {startTime, endTime} for all swing phases. Contact phases are always implied in between: endTime[i] < startTime[i+1] + * times are NaN if they cannot be identified at the boundaries + * Vector is empty if there are no swing phases + */ +std::vector extractSwingTimings(const std::vector& eventTimes, const std::vector& contactFlags); + +/** + * Extracts for each leg the contact sequence over the motion phase sequence. + * @param modeSequence : Sequence of contact modes. + * @return Sequence of contact flags per leg. + */ +feet_array_t> extractContactFlags(const std::vector& modeSequence); + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Rotation.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Rotation.h new file mode 100644 index 000000000..effbd7c05 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Rotation.h @@ -0,0 +1,11 @@ +#pragma once + +#include + +namespace ocs2 { +namespace legged_robot { + +matrix3_t getRotationMatrixFromEulerAngles(const vector3_t& eulerAnglesZYX); + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml new file mode 100644 index 000000000..c3f6c5f80 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml @@ -0,0 +1,19 @@ + + + ocs2_legged_robot_mpcnet + 0.0.0 + The ocs2_legged_robot_mpcnet package + + Farbod Farshidian + Alexander Reske + + TODO + + catkin + + cmake_clang_tools + + ocs2_legged_robot + ocs2_mpcnet + + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py new file mode 100644 index 000000000..5c52da1b9 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py @@ -0,0 +1,8 @@ +from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import MpcnetInterface +from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import size_array, scalar_array, vector_array, matrix_array +from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import ScalarFunctionQuadraticApproximation +from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import SystemObservation, SystemObservationArray +from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import ModeSchedule, ModeScheduleArray +from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import TargetTrajectories, TargetTrajectoriesArray +from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import DataPoint, DataArray +from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import Metrics, MetricsArray diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py new file mode 100644 index 000000000..7de37d21f --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py @@ -0,0 +1,83 @@ +from ocs2_mpcnet import config + +# +# config +# + +# data type for tensor elements +dtype = config.dtype + +# device on which tensors will be allocated +device = config.device + +# +# legged_robot_config +# + +# name of the robot +name = "legged_robot" + +# (generalized) time dimension +TIME_DIM = 12 + +# state dimension +STATE_DIM = 24 + +# input dimension +INPUT_DIM = 24 + +# expert number +EXPERT_NUM = 8 + +# default state +default_state = [0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.5, + 0.0, 0.0, 0.0, + -0.25, 0.6, -0.85, + -0.25, -0.6, 0.85, + 0.25, 0.6, -0.85, + 0.25, -0.6, 0.85] + +# input bias +input_bias = [0.0, 0.0, 127.861, + 0.0, 0.0, 127.861, + 0.0, 0.0, 127.861, + 0.0, 0.0, 127.861, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0] + +# input scaling +input_scaling = [100.0, 100.0, 100.0, + 100.0, 100.0, 100.0, + 100.0, 100.0, 100.0, + 100.0, 100.0, 100.0, + 10.0, 10.0, 10.0, + 10.0, 10.0, 10.0, + 10.0, 10.0, 10.0, + 10.0, 10.0, 10.0] + +# (diagonally dominant) nominal centroidal inertia normalized by robot mass +normalized_inertia = [1.62079 / 52.1348, 4.83559 / 52.1348, 4.72382 / 52.1348] + +# input cost for behavioral cloning +R = [0.001, 0.001, 0.001, + 0.001, 0.001, 0.001, + 0.001, 0.001, 0.001, + 0.001, 0.001, 0.001, + 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0] + +# dictionary for cheating +expert_for_mode = dict([(i, None) for i in range(16)]) +expert_for_mode[15] = 0 +expert_for_mode[6] = 1 +expert_for_mode[9] = 2 +expert_for_mode[7] = 3 +expert_for_mode[11] = 4 +expert_for_mode[13] = 5 +expert_for_mode[14] = 6 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py new file mode 100644 index 000000000..6915cd21f --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -0,0 +1,326 @@ +import numpy as np + +from ocs2_legged_robot_mpcnet import size_array, scalar_array, vector_array, SystemObservation, SystemObservationArray,\ + ModeSchedule, ModeScheduleArray, TargetTrajectories, TargetTrajectoriesArray +from ocs2_legged_robot_mpcnet import legged_robot_config as config + + +def get_size_array(data): + my_size_array = size_array() + my_size_array.resize(len(data)) + for i in range(len(data)): + my_size_array[i] = data[i] + return my_size_array + + +def get_scalar_array(data): + my_scalar_array = scalar_array() + my_scalar_array.resize(len(data)) + for i in range(len(data)): + my_scalar_array[i] = data[i] + return my_scalar_array + + +def get_vector_array(data): + my_vector_array = vector_array() + my_vector_array.resize(len(data)) + for i in range(len(data)): + my_vector_array[i] = data[i] + return my_vector_array + + +def get_system_observation(time, state): + system_observation = SystemObservation() + system_observation.mode = 15 + system_observation.time = time + system_observation.state = state + system_observation.input = np.zeros(config.STATE_DIM) + return system_observation + + +def get_system_observation_array(length): + system_observation_array = SystemObservationArray() + system_observation_array.resize(length) + return system_observation_array + + +def get_target_trajectories(time_trajectory, state_trajectory): + time_trajectory_array = get_scalar_array(time_trajectory) + state_trajectory_array = get_vector_array(state_trajectory) + input_trajectory_array = get_vector_array(np.zeros((len(time_trajectory), config.INPUT_DIM))) + return TargetTrajectories(time_trajectory_array, state_trajectory_array, input_trajectory_array) + + +def get_target_trajectories_array(length): + target_trajectories_array = TargetTrajectoriesArray() + target_trajectories_array.resize(length) + return target_trajectories_array + + +def get_mode_schedule(event_times, mode_sequence): + event_times_array = get_scalar_array(event_times) + mode_sequence_array = get_size_array(mode_sequence) + return ModeSchedule(event_times_array, mode_sequence_array) + + +def get_mode_schedule_array(length): + mode_schedule_array = ModeScheduleArray() + mode_schedule_array.resize(length) + return mode_schedule_array + + +def get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template): + gait_cycle_duration = event_times_template[-1] + num_gait_cycles = int(np.floor(duration / gait_cycle_duration)) + event_times = np.array([0.0], dtype=np.float64) + mode_sequence = np.array([15], dtype=np.uintp) + for _ in range(num_gait_cycles): + event_times = np.append(event_times, event_times[-1] * np.ones(len(event_times_template)) + event_times_template) + mode_sequence = np.append(mode_sequence, mode_sequence_template) + mode_sequence = np.append(mode_sequence, np.array([15], dtype=np.uintp)) + return event_times, mode_sequence + + +def get_stance(duration): + # contact schedule: STANCE + # swing schedule: - + event_times_template = np.array([1.0], dtype=np.float64) + mode_sequence_template = np.array([15], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + + +def get_random_initial_state_stance(): + max_normalized_linear_momentum_x = 0.1 + max_normalized_linear_momentum_y = 0.1 + max_normalized_linear_momentum_z = 0.1 + max_normalized_angular_momentum_x = config.normalized_inertia[0] * 30.0 * np.pi / 180.0 + max_normalized_angular_momentum_y = config.normalized_inertia[1] * 30.0 * np.pi / 180.0 + max_normalized_angular_momentum_z = config.normalized_inertia[2] * 30.0 * np.pi / 180.0 + random_deviation = np.zeros(config.STATE_DIM) + random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) + random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) + random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) + random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) + random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) + random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) + return np.array(config.default_state) + random_deviation + + +def get_random_target_state_stance(): + max_position_z = 0.075 + max_orientation_z = 25.0 * np.pi / 180.0 + max_orientation_y = 15.0 * np.pi / 180.0 + max_orientation_x = 25.0 * np.pi / 180.0 + random_deviation = np.zeros(config.STATE_DIM) + random_deviation[8] = np.random.uniform(-max_position_z, max_position_z) + random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) + random_deviation[10] = np.random.uniform(-max_orientation_y, max_orientation_y) + random_deviation[11] = np.random.uniform(-max_orientation_x, max_orientation_x) + return np.array(config.default_state) + random_deviation + + +def get_trot_1(duration): + # contact schedule: LF_RH, RF_LH + # swing schedule: RF_LH, LF_RH + event_times_template = np.array([0.35, 0.7], dtype=np.float64) + mode_sequence_template = np.array([9, 6], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + + +def get_trot_2(duration): + # contact schedule: RF_LH, LF_RH + # swing schedule: LF_RH, RF_LH + event_times_template = np.array([0.35, 0.7], dtype=np.float64) + mode_sequence_template = np.array([6, 9], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + + +def get_random_initial_state_trot(): + max_normalized_linear_momentum_x = 0.5 + max_normalized_linear_momentum_y = 0.25 + max_normalized_linear_momentum_z = 0.25 + max_normalized_angular_momentum_x = config.normalized_inertia[0] * 60.0 * np.pi / 180.0 + max_normalized_angular_momentum_y = config.normalized_inertia[1] * 60.0 * np.pi / 180.0 + max_normalized_angular_momentum_z = config.normalized_inertia[2] * 35.0 * np.pi / 180.0 + random_deviation = np.zeros(config.STATE_DIM) + random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) + random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) + random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) + random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) + random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) + random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) + return np.array(config.default_state) + random_deviation + + +def get_random_target_state_trot(): + max_position_x = 0.3 + max_position_y = 0.15 + max_orientation_z = 30.0 * np.pi / 180.0 + random_deviation = np.zeros(config.STATE_DIM) + random_deviation[6] = np.random.uniform(-max_position_x, max_position_x) + random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) + random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) + return np.array(config.default_state) + random_deviation + + +def get_dynamic_diagonal_walk_1(duration): + # contact schedule: RF_LH_RH, RF_LH, LF_RF_LH, LF_LH_RH, LF_RH, LF_RF_RH + # swing schedule: LF, LF_RH, RH, RF, RF_LH, LH + event_times_template = np.array([0.15, 0.3, 0.45, 0.6, 0.75, 0.9], dtype=np.float64) + mode_sequence_template = np.array([7, 6, 14, 11, 9, 13], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + + +def get_dynamic_diagonal_walk_2(duration): + # contact schedule: LF_LH_RH, LF_RH LF_RF_RH, RF_LH_RH, RF_LH, LF_RF_LH + # swing schedule: RF, RF_LH, LH, LF, LF_RH, RH + event_times_template = np.array([0.0, 0.15, 0.3, 0.45, 0.6, 0.75, 0.9], dtype=np.float64) + mode_sequence_template = np.array([11, 9, 13, 7, 6, 14], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + + +def get_random_initial_state_dynamic_diagonal_walk(): + max_normalized_linear_momentum_x = 0.4 + max_normalized_linear_momentum_y = 0.2 + max_normalized_linear_momentum_z = 0.2 + max_normalized_angular_momentum_x = config.normalized_inertia[0] * 52.5 * np.pi / 180.0 + max_normalized_angular_momentum_y = config.normalized_inertia[1] * 52.5 * np.pi / 180.0 + max_normalized_angular_momentum_z = config.normalized_inertia[2] * 30.0 * np.pi / 180.0 + random_deviation = np.zeros(config.STATE_DIM) + random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x / 2.0, max_normalized_linear_momentum_x) + random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) + random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) + random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) + random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) + random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) + return np.array(config.default_state) + random_deviation + + +def get_random_target_state_dynamic_diagonal_walk(): + max_position_x = 0.275 + max_position_y = 0.1375 + max_orientation_z = 25.0 * np.pi / 180.0 + random_deviation = np.zeros(config.STATE_DIM) + random_deviation[6] = np.random.uniform(-max_position_x / 2.0, max_position_x) + random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) + random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) + return np.array(config.default_state) + random_deviation + + +def get_static_walk_1(duration): + # contact schedule: LF_RF_RH, RF_LH_RH, LF_RF_LH, LF_LH_RH + # swing schedule: LH, LF, RH, RF + event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) + mode_sequence_template = np.array([13, 7, 14, 11], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + + +def get_static_walk_2(duration): + # contact schedule: RF_LH_RH, LF_RF_LH, LF_LH_RH, LF_RF_RH + # swing schedule: LF, RH, RF, LH + event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) + mode_sequence_template = np.array([7, 14, 11, 13], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + + +def get_static_walk_3(duration): + # contact schedule: LF_RF_LH, LF_LH_RH, LF_RF_RH, RF_LH_RH + # swing schedule: RH, RF, LH, LF + event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) + mode_sequence_template = np.array([14, 11, 13, 7], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + + +def get_static_walk_4(duration): + # contact schedule: LF_LH_RH, LF_RF_RH, RF_LH_RH, LF_RF_LH + # swing schedule: RF, LH, LF, RH + event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) + mode_sequence_template = np.array([11, 13, 7, 14], dtype=np.uintp) + return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + + +def get_random_initial_state_static_walk(): + max_normalized_linear_momentum_x = 0.25 + max_normalized_linear_momentum_y = 0.125 + max_normalized_linear_momentum_z = 0.125 + max_normalized_angular_momentum_x = config.normalized_inertia[0] * 45.0 * np.pi / 180.0 + max_normalized_angular_momentum_y = config.normalized_inertia[1] * 45.0 * np.pi / 180.0 + max_normalized_angular_momentum_z = config.normalized_inertia[2] * 25.0 * np.pi / 180.0 + random_deviation = np.zeros(config.STATE_DIM) + random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x / 2.0, max_normalized_linear_momentum_x) + random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) + random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) + random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) + random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) + random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) + return np.array(config.default_state) + random_deviation + + +def get_random_target_state_static_walk(): + max_position_x = 0.25 + max_position_y = 0.125 + max_orientation_z = 20.0 * np.pi / 180.0 + random_deviation = np.zeros(config.STATE_DIM) + random_deviation[6] = np.random.uniform(-max_position_x / 2.0, max_position_x) + random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) + random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) + return np.array(config.default_state) + random_deviation + + +def get_tasks(n_tasks, duration, choices): + initial_observations = get_system_observation_array(n_tasks) + mode_schedules = get_mode_schedule_array(n_tasks) + target_trajectories = get_target_trajectories_array(n_tasks) + for i in range(n_tasks): + if choices[i] == "stance": + initial_observations[i] = get_system_observation(0.0, get_random_initial_state_stance()) + mode_schedules[i] = get_mode_schedule(*get_stance(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_stance().reshape((1, config.STATE_DIM))) + elif choices[i] == "trot_1": + initial_observations[i] = get_system_observation(0.0, get_random_initial_state_trot()) + mode_schedules[i] = get_mode_schedule(*get_trot_1(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_trot().reshape((1, config.STATE_DIM))) + elif choices[i] == "trot_2": + initial_observations[i] = get_system_observation(0.0, get_random_initial_state_trot()) + mode_schedules[i] = get_mode_schedule(*get_trot_2(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_trot().reshape((1, config.STATE_DIM))) + elif choices[i] == "dynamic_diagonal_walk_1": + initial_observations[i] = get_system_observation(0.0, get_random_initial_state_dynamic_diagonal_walk()) + mode_schedules[i] = get_mode_schedule(*get_dynamic_diagonal_walk_1(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_dynamic_diagonal_walk().reshape((1, config.STATE_DIM))) + elif choices[i] == "dynamic_diagonal_walk_2": + initial_observations[i] = get_system_observation(0.0, get_random_initial_state_dynamic_diagonal_walk()) + mode_schedules[i] = get_mode_schedule(*get_dynamic_diagonal_walk_2(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_dynamic_diagonal_walk().reshape((1, config.STATE_DIM))) + elif choices[i] == "static_walk_1": + initial_observations[i] = get_system_observation(0.0, get_random_initial_state_static_walk()) + mode_schedules[i] = get_mode_schedule(*get_static_walk_1(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_static_walk().reshape((1, config.STATE_DIM))) + elif choices[i] == "static_walk_2": + initial_observations[i] = get_system_observation(0.0, get_random_initial_state_static_walk()) + mode_schedules[i] = get_mode_schedule(*get_static_walk_2(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_static_walk().reshape((1, config.STATE_DIM))) + elif choices[i] == "static_walk_3": + initial_observations[i] = get_system_observation(0.0, get_random_initial_state_static_walk()) + mode_schedules[i] = get_mode_schedule(*get_static_walk_3(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_static_walk().reshape((1, config.STATE_DIM))) + elif choices[i] == "static_walk_4": + initial_observations[i] = get_system_observation(0.0, get_random_initial_state_static_walk()) + mode_schedules[i] = get_mode_schedule(*get_static_walk_4(duration)) + target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_static_walk().reshape((1, config.STATE_DIM))) + return initial_observations, mode_schedules, target_trajectories + + +def get_one_hot(mode): + one_hot = np.zeros(config.EXPERT_NUM) + one_hot[config.expert_for_mode[mode]] = 1.0 + return one_hot diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py new file mode 100644 index 000000000..138ccd374 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -0,0 +1,212 @@ +import os +import time +import datetime +import random +import torch +import numpy as np + +from torch.utils.tensorboard import SummaryWriter + +from ocs2_mpcnet.loss import BehavioralCloning as ExpertsLoss +from ocs2_mpcnet.loss import CrossEntropy as GatingLoss +from ocs2_mpcnet.memory import ReplayMemory as Memory + +from ocs2_legged_robot_mpcnet.legged_robot_policy import LeggedRobotMixtureOfNonlinearExpertsPolicy as Policy +from ocs2_legged_robot_mpcnet import legged_robot_config as config +from ocs2_legged_robot_mpcnet import legged_robot_helper as helper +from ocs2_legged_robot_mpcnet import MpcnetInterface + +# settings for data generation by applying behavioral policy +data_generation_time_step = 0.0025 +data_generation_duration = 4.0 +data_generation_data_decimation = 4 +data_generation_n_threads = 5 +data_generation_n_tasks = 10 +data_generation_n_samples = 1 +data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order='F') +for i in range(0, 3): + data_generation_sampling_covariance[i, i] = 0.05 ** 2 # normalized linear momentum +for i in range(3, 6): + data_generation_sampling_covariance[i, i] = (config.normalized_inertia[i - 3] * 2.5 * np.pi / 180.0) ** 2 # normalized angular momentum +for i in range(6, 9): + data_generation_sampling_covariance[i, i] = 0.01 ** 2 # position +for i in range(9, 12): + data_generation_sampling_covariance[i, i] = (0.5 * np.pi / 180.0) ** 2 # orientation +for i in range(12, 24): + data_generation_sampling_covariance[i, i] = (0.5 * np.pi / 180.0) ** 2 # joint positions + +# settings for computing metrics by applying learned policy +policy_evaluation_time_step = 0.0025 +policy_evaluation_duration = 4.0 +policy_evaluation_n_threads = 1 +policy_evaluation_n_tasks = 5 + +# mpcnet interface +mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads) + +# logging +description = "description" +folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.name + "_" + description +writer = SummaryWriter("runs/" + folder) +os.makedirs(name="policies/" + folder) + +# loss +epsilon = 1e-8 # epsilon to improve numerical stability of logs and denominators +my_lambda = 1.0 # parameter to control the relative importance of both loss types +experts_loss = ExpertsLoss(torch.tensor(config.R, device=config.device, dtype=config.dtype).diag(), np.diag(config.R)) +gating_loss = GatingLoss(torch.tensor(epsilon, device=config.device, dtype=config.dtype), np.array(epsilon)) + +# memory +memory_capacity = 1000000 +memory = Memory(memory_capacity) + +# policy +policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) +policy.to(config.device) +print("Initial policy parameters:") +print(list(policy.named_parameters())) +dummy_input = (torch.randn(config.TIME_DIM, device=config.device, dtype=config.dtype), + torch.randn(config.STATE_DIM, device=config.device, dtype=config.dtype)) +print("Saving initial policy.") +save_path = "policies/" + folder + "/initial_policy" +torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") +torch.save(obj=policy, f=save_path + ".pt") + +# optimizer +batch_size = 2 ** 5 +learning_iterations = 100000 +learning_rate_default = 1e-3 +learning_rate_gating_net = learning_rate_default +learning_rate_expert_nets = learning_rate_default +optimizer = torch.optim.Adam([{'params': policy.gating_net.parameters(), 'lr': learning_rate_gating_net}, + {'params': policy.expert_nets.parameters(), 'lr': learning_rate_expert_nets}], + lr=learning_rate_default, amsgrad=True) + +# weights for ["stance", "trot_1", "trot_2", "dynamic_diagonal_walk_1", "dynamic_diagonal_walk_2", +# "static_walk_1", "static_walk_2", "static_walk_3", "static_walk_4"] +weights = [1, 2, 2, 2, 2, 1, 1, 1, 1] + + +def start_data_generation(alpha, policy): + policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + choices = random.choices(["stance", "trot_1", "trot_2", "dynamic_diagonal_walk_1", "dynamic_diagonal_walk_2", + "static_walk_1", "static_walk_2", "static_walk_3", "static_walk_4"], k=data_generation_n_tasks, weights=weights) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks(data_generation_n_tasks, data_generation_duration, choices) + mpcnet_interface.startDataGeneration(alpha, policy_file_path, data_generation_time_step, data_generation_data_decimation, + data_generation_n_samples, data_generation_sampling_covariance, + initial_observations, mode_schedules, target_trajectories) + + +def start_policy_evaluation(policy): + policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + choices = random.choices(["stance", "trot_1", "trot_2", "dynamic_diagonal_walk_1", "dynamic_diagonal_walk_2", + "static_walk_1", "static_walk_2", "static_walk_3", "static_walk_4"], k=policy_evaluation_n_tasks, weights=weights) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks(policy_evaluation_n_tasks, policy_evaluation_duration, choices) + mpcnet_interface.startPolicyEvaluation(policy_file_path, policy_evaluation_time_step, + initial_observations, mode_schedules, target_trajectories) + + +try: + print("==============\nWaiting for first data.\n==============") + start_data_generation(alpha=1.0, policy=policy) + start_policy_evaluation(policy=policy) + while not mpcnet_interface.isDataGenerationDone(): + time.sleep(1.0) + + print("==============\nStarting training.\n==============") + for iteration in range(learning_iterations): + alpha = 1.0 - 1.0 * iteration / learning_iterations + + # data generation + if mpcnet_interface.isDataGenerationDone(): + # get generated data + data = mpcnet_interface.getGeneratedData() + for i in range(len(data)): + # push t, x, u, mode, generalized time, relative state, Hamiltonian into memory + memory.push(data[i].t, data[i].x, data[i].u, data[i].mode, data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) + # logging + writer.add_scalar('data/new_data_points', len(data), iteration) + writer.add_scalar('data/total_data_points', memory.size, iteration) + print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) + # start new data generation + start_data_generation(alpha=alpha, policy=policy) + + # policy evaluation + if mpcnet_interface.isPolicyEvaluationDone(): + # get computed metrics + metrics = mpcnet_interface.getComputedMetrics() + survival_time = np.mean([metrics[i].survival_time for i in range(len(metrics))]) + incurred_hamiltonian = np.mean([metrics[i].incurred_hamiltonian for i in range(len(metrics))]) + # logging + writer.add_scalar('metric/survival_time', survival_time, iteration) + writer.add_scalar('metric/incurred_hamiltonian', incurred_hamiltonian, iteration) + print("iteration", iteration, "received metrics:", "incurred_hamiltonian", incurred_hamiltonian, "survival_time", survival_time) + # start new policy evaluation + start_policy_evaluation(policy=policy) + + # intermediate policies + if (iteration % 10000 == 0) and (iteration > 0): + print("Saving intermediate policy for iteration", iteration) + save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) + torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.save(obj=policy, f=save_path + ".pt") + + # extract batch of samples from replay memory + samples = memory.sample(batch_size) + + # take an optimization step + def closure(): + # clear the gradients + optimizer.zero_grad() + # compute the empirical loss + empirical_experts_loss = torch.zeros(1, dtype=config.dtype, device=config.device) + empirical_gating_loss = torch.zeros(1, dtype=config.dtype, device=config.device) + for sample in samples: + t = torch.tensor(sample.t, dtype=config.dtype, device=config.device) + x = torch.tensor(sample.x, dtype=config.dtype, device=config.device) + u_target = torch.tensor(sample.u, dtype=config.dtype, device=config.device) + p_target = torch.tensor(helper.get_one_hot(sample.mode), dtype=config.dtype, device=config.device) + generalized_time = torch.tensor(sample.generalized_time, dtype=config.dtype, device=config.device) + relative_state = torch.tensor(sample.relative_state, dtype=config.dtype, device=config.device) + p, U = policy(generalized_time, relative_state) + u_predicted = torch.matmul(p, U) + empirical_experts_loss = empirical_experts_loss + experts_loss.compute_torch(u_predicted, u_target) + empirical_gating_loss = empirical_gating_loss + gating_loss.compute_torch(p_target, p) + empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss + # compute the gradients + empirical_loss.backward() + # logging + writer.add_scalar('objective/empirical_experts_loss', empirical_experts_loss.item() / batch_size, iteration) + writer.add_scalar('objective/empirical_gating_loss', empirical_gating_loss.item() / batch_size, iteration) + writer.add_scalar('objective/empirical_loss', empirical_loss.item() / batch_size, iteration) + # return empirical loss + return empirical_loss + optimizer.step(closure) + + # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) + if iteration == learning_iterations - 1: + while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): + time.sleep(1.0) + + print("==============\nTraining completed.\n==============") + +except KeyboardInterrupt: + # let data generation and policy evaluation finish (to avoid a segmentation fault) + while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): + time.sleep(1.0) + print("==============\nTraining interrupted.\n==============") + pass + +print("Final policy parameters:") +print(list(policy.named_parameters())) + +print("Saving final policy.") +save_path = "policies/" + folder + "/final_policy" +torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") +torch.save(obj=policy, f=save_path + ".pt") + +writer.close() + +print("Done. Exiting now.") diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py new file mode 100644 index 000000000..b4e181253 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -0,0 +1,55 @@ +import torch + +from ocs2_mpcnet import policy + +from ocs2_legged_robot_mpcnet import legged_robot_config as config + + +def input_transform(u): + input_bias = torch.tensor(config.input_bias, device=config.device, dtype=config.dtype) + input_scaling = torch.tensor(config.input_scaling, device=config.device, dtype=config.dtype).diag() + return torch.add(input_bias, torch.matmul(u, input_scaling)) + + +class LeggedRobotLinearPolicy(policy.LinearPolicy): + + def __init__(self, dim_t, dim_x, dim_u): + super().__init__(dim_t, dim_x, dim_u) + self.name = 'LeggedRobotLinearPolicy' + + def forward(self, t, x): + p, u = super().forward(t, x) + return p, input_transform(u) + + +class LeggedRobotNonlinearPolicy(policy.NonlinearPolicy): + + def __init__(self, dim_t, dim_x, dim_u): + super().__init__(dim_t, dim_x, dim_u) + self.name = 'LeggedRobotNonlinearPolicy' + + def forward(self, t, x): + p, u = super().forward(t, x) + return p, input_transform(u) + + +class LeggedRobotMixtureOfLinearExpertsPolicy(policy.MixtureOfLinearExpertsPolicy): + + def __init__(self, dim_t, dim_x, dim_u, num_experts): + super().__init__(dim_t, dim_x, dim_u, num_experts) + self.name = 'LeggedRobotMixtureOfLinearExpertsPolicy' + + def forward(self, t, x): + p, u = super().forward(t, x) + return p, input_transform(u) + + +class LeggedRobotMixtureOfNonlinearExpertsPolicy(policy.MixtureOfNonlinearExpertsPolicy): + + def __init__(self, dim_t, dim_x, dim_u, num_experts): + super().__init__(dim_t, dim_x, dim_u, num_experts) + self.name = 'LeggedRobotMixtureOfNonlinearExpertsPolicy' + + def forward(self, t, x): + p, u = super().forward(t, x) + return p, input_transform(u) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py new file mode 100644 index 000000000..0a7b41b85 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup( + packages=['ocs2_legged_robot_mpcnet'], + package_dir={'': 'python'} +) + +setup(**setup_args) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp new file mode 100644 index 000000000..6f6de3255 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -0,0 +1,67 @@ +#include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" + +#include + +#include "ocs2_legged_robot_mpcnet/helper/Logic.h" +#include "ocs2_legged_robot_mpcnet/helper/Rotation.h" + +namespace ocs2 { +namespace legged_robot { + +vector_t LeggedRobotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) { + feet_array_t swingPhasePerLeg = getSwingPhasePerLeg(t, modeSchedule); + vector_t generalizedTime; + generalizedTime.resize(3 * NUM_CONTACT_POINTS, Eigen::NoChange); + // phase + for (int i = 0 * NUM_CONTACT_POINTS; i < 1 * NUM_CONTACT_POINTS; i++) { + if (swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase < 0.0) { + generalizedTime[i] = 0.0; + } else { + generalizedTime[i] = swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase; + } + } + // phase rate + for (int i = 1 * NUM_CONTACT_POINTS; i < 2 * NUM_CONTACT_POINTS; i++) { + if (swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase < 0.0) { + generalizedTime[i] = 0.0; + } else { + generalizedTime[i] = 1.0 / swingPhasePerLeg[i % NUM_CONTACT_POINTS].duration; + } + } + // sin(pi * phase) + for (int i = 2 * NUM_CONTACT_POINTS; i < 3 * NUM_CONTACT_POINTS; i++) { + if (swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase < 0.0) { + generalizedTime[i] = 0.0; + } else { + generalizedTime[i] = std::sin(M_PI * swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase); + } + } + return generalizedTime; +} + +vector_t LeggedRobotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { + vector_t relativeState = x - targetTrajectories.getDesiredState(t); + matrix3_t R = getRotationMatrixFromEulerAngles(x.segment<3>(9)).transpose(); + relativeState.segment<3>(6) = R * relativeState.segment<3>(6); + relativeState.segment<3>(9) = R * relativeState.segment<3>(9); + return relativeState; +} + +bool LeggedRobotMpcnetDefinition::validState(const vector_t& x) { + vector_t deviation = x - defaultState_; + if (std::abs(deviation[8]) > 0.2) { + std::cerr << "LeggedRobotMpcnetDefinition::validState Height diverged: " << x[8] << std::endl; + return false; + } else if (std::abs(deviation[10]) > 30.0 * M_PI / 180.0) { + std::cerr << "LeggedRobotMpcnetDefinition::validState Pitch diverged: " << x[10] << std::endl; + return false; + } else if (std::abs(deviation[11]) > 30.0 * M_PI / 180.0) { + std::cerr << "LeggedRobotMpcnetDefinition::validState Roll diverged: " << x[11] << std::endl; + return false; + } else { + return true; + } +} + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp new file mode 100644 index 000000000..0310481c5 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -0,0 +1,62 @@ +#include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h" + +#include +#include + +#include +#include +#include +#include + +#include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" + +namespace ocs2 { +namespace legged_robot { + +LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads) { + // create ONNX environment + auto onnxEnvironmentPtr = createOnnxEnvironment(); + // path to config files + std::string taskFileFolderName = "mpc"; + std::string targetCommandFile = ros::package::getPath("ocs2_legged_robot") + "/config/command/targetTrajectories.info"; + // path to urdf file + std::string urdfFile = ros::package::getPath("anymal_c_simple_description") + "/urdf/anymal.urdf"; + // set up MPC-Net rollout manager for data generation and policy evaluation + std::vector> mpcPtrs; + std::vector> mpcnetPtrs; + std::vector> rolloutPtrs; + std::vector> mpcnetDefinitionPtrs; + std::vector> referenceManagerPtrs; + mpcPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + mpcnetPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + rolloutPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + mpcnetDefinitionPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { + LeggedRobotInterface leggedRobotInterface(taskFileFolderName, targetCommandFile, urdf::parseURDFFile(urdfFile)); + std::shared_ptr mpcnetDefinitionPtr(new LeggedRobotMpcnetDefinition(leggedRobotInterface.getInitialState())); + mpcPtrs.push_back(getMpc(leggedRobotInterface)); + mpcnetPtrs.push_back(std::unique_ptr( + new MpcnetOnnxController(mpcnetDefinitionPtr, leggedRobotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr))); + rolloutPtrs.push_back(std::unique_ptr(leggedRobotInterface.getRollout().clone())); + mpcnetDefinitionPtrs.push_back(mpcnetDefinitionPtr); + referenceManagerPtrs.push_back(leggedRobotInterface.getReferenceManagerPtr()); + } + mpcnetRolloutManagerPtr_.reset(new MpcnetRolloutManager(nDataGenerationThreads, nPolicyEvaluationThreads, std::move(mpcPtrs), + std::move(mpcnetPtrs), std::move(rolloutPtrs), mpcnetDefinitionPtrs, + referenceManagerPtrs)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::unique_ptr LeggedRobotMpcnetInterface::getMpc(LeggedRobotInterface& leggedRobotInterface) { + std::unique_ptr mpcPtr(new MPC_DDP(leggedRobotInterface.mpcSettings(), leggedRobotInterface.ddpSettings(), + leggedRobotInterface.getRollout(), leggedRobotInterface.getOptimalControlProblem(), + leggedRobotInterface.getInitializer())); + mpcPtr->getSolverPtr()->setReferenceManager(leggedRobotInterface.getReferenceManagerPtr()); + return mpcPtr; +} + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp new file mode 100644 index 000000000..2db8365a5 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp @@ -0,0 +1,5 @@ +#include + +#include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h" + +CREATE_MPCNET_PYTHON_BINDINGS(ocs2::legged_robot::LeggedRobotMpcnetInterface, LeggedRobotMpcnetPybindings) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Logic.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Logic.cpp new file mode 100644 index 000000000..30d38e831 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Logic.cpp @@ -0,0 +1,220 @@ +#include "ocs2_legged_robot_mpcnet/helper/Logic.h" + +#include + +namespace ocs2 { +namespace legged_robot { + +feet_array_t getContactPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule) { + feet_array_t contactPhasePerLeg; + + // Convert mode sequence to a contact timing vector per leg + const auto contactTimingsPerLeg = extractContactTimingsPerLeg(modeSchedule); + + // Extract contact phases per leg + for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { + if (contactTimingsPerLeg[leg].empty()) { + // Leg is always in swing phase + contactPhasePerLeg[leg].phase = scalar_t(-1.0); + contactPhasePerLeg[leg].duration = std::numeric_limits::quiet_NaN(); + } else if (startsWithContactPhase(contactTimingsPerLeg[leg]) && (time <= contactTimingsPerLeg[leg].front().end)) { + // It is assumed that contact phase started at minus infinity, so current time will be always close to ContactTiming.end + contactPhasePerLeg[leg].phase = scalar_t(1.0); + contactPhasePerLeg[leg].duration = std::numeric_limits::infinity(); + } else if (endsWithContactPhase(contactTimingsPerLeg[leg]) && (time >= contactTimingsPerLeg[leg].back().start)) { + // It is assumed that contact phase ends at infinity, so current time will be always close to ContactTiming.start + contactPhasePerLeg[leg].phase = scalar_t(0.0); + contactPhasePerLeg[leg].duration = std::numeric_limits::infinity(); + } else { + // Check if leg is in contact interval at current time + auto it = std::find_if(contactTimingsPerLeg[leg].begin(), contactTimingsPerLeg[leg].end(), + [time](ContactTiming timing) { return (timing.start <= time) && (time <= timing.end); }); + if (it == contactTimingsPerLeg[leg].end()) { + // Leg is not in contact for current time + contactPhasePerLeg[leg].phase = scalar_t(-1.0); + contactPhasePerLeg[leg].duration = std::numeric_limits::quiet_NaN(); + } else { + // Leg is in contact for current time + const auto currentContactTiming = *it; + contactPhasePerLeg[leg].phase = (time - currentContactTiming.start) / (currentContactTiming.end - currentContactTiming.start); + contactPhasePerLeg[leg].duration = currentContactTiming.end - currentContactTiming.start; + } + } + } + + return contactPhasePerLeg; +} + +feet_array_t getSwingPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule) { + feet_array_t swingPhasePerLeg; + + // Convert mode sequence to a swing timing vector per leg + const auto swingTimingsPerLeg = extractSwingTimingsPerLeg(modeSchedule); + + // Extract swing phases per leg + for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { + if (swingTimingsPerLeg[leg].empty()) { + // Leg is always in contact phase + swingPhasePerLeg[leg].phase = scalar_t(-1.0); + swingPhasePerLeg[leg].duration = std::numeric_limits::quiet_NaN(); + } else if (startsWithSwingPhase(swingTimingsPerLeg[leg]) && (time <= swingTimingsPerLeg[leg].front().end)) { + // It is assumed that swing phase started at minus infinity, so current time will be always close to SwingTiming.end + swingPhasePerLeg[leg].phase = scalar_t(1.0); + swingPhasePerLeg[leg].duration = std::numeric_limits::infinity(); + } else if (endsWithSwingPhase(swingTimingsPerLeg[leg]) && (time >= swingTimingsPerLeg[leg].back().start)) { + // It is assumed that swing phase ends at infinity, so current time will be always close to SwingTiming.start + swingPhasePerLeg[leg].phase = scalar_t(0.0); + swingPhasePerLeg[leg].duration = std::numeric_limits::infinity(); + } else { + // Check if leg is in swing interval at current time + auto it = std::find_if(swingTimingsPerLeg[leg].begin(), swingTimingsPerLeg[leg].end(), + [time](SwingTiming timing) { return (timing.start <= time) && (time <= timing.end); }); + if (it == swingTimingsPerLeg[leg].end()) { + // Leg is not swinging for current time + swingPhasePerLeg[leg].phase = scalar_t(-1.0); + swingPhasePerLeg[leg].duration = std::numeric_limits::quiet_NaN(); + } else { + // Leg is swinging for current time + const auto currentSwingTiming = *it; + swingPhasePerLeg[leg].phase = (time - currentSwingTiming.start) / (currentSwingTiming.end - currentSwingTiming.start); + swingPhasePerLeg[leg].duration = currentSwingTiming.end - currentSwingTiming.start; + } + } + } + + return swingPhasePerLeg; +} + +feet_array_t> extractContactTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule) { + feet_array_t> contactTimingsPerLeg; + + // Convert mode sequence to a contact flag vector per leg + const auto contactSequencePerLeg = extractContactFlags(modeSchedule.modeSequence); + + // Extract timings per leg + for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { + contactTimingsPerLeg[leg] = extractContactTimings(modeSchedule.eventTimes, contactSequencePerLeg[leg]); + } + + return contactTimingsPerLeg; +} + +feet_array_t> extractSwingTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule) { + feet_array_t> swingTimingsPerLeg; + + // Convert mode sequence to a contact flag vector per leg + const auto contactSequencePerLeg = extractContactFlags(modeSchedule.modeSequence); + + // Extract timings per leg + for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { + swingTimingsPerLeg[leg] = extractSwingTimings(modeSchedule.eventTimes, contactSequencePerLeg[leg]); + } + + return swingTimingsPerLeg; +} + +scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector& contactTimings) { + for (const auto& contactPhase : contactTimings) { + if (hasEndTime(contactPhase) && contactPhase.end > currentTime) { + return contactPhase.end; + } + } + return timingNaN(); +} + +scalar_t getTimeOfNextTouchDown(scalar_t currentTime, const std::vector& contactTimings) { + for (const auto& contactPhase : contactTimings) { + if (hasStartTime(contactPhase) && contactPhase.start > currentTime) { + return contactPhase.start; + } + } + return timingNaN(); +} + +std::vector extractContactTimings(const std::vector& eventTimes, const std::vector& contactFlags) { + assert(eventTimes.size() + 1 == contactFlags.size()); + const int numPhases = contactFlags.size(); + + std::vector contactTimings; + contactTimings.reserve(1 + eventTimes.size() / 2); // Approximate upper bound + int currentPhase = 0; + + while (currentPhase < numPhases) { + // Search where contact phase starts + while (currentPhase < numPhases && !contactFlags[currentPhase]) { + ++currentPhase; + } + if (currentPhase >= numPhases) { + break; // No more contact phases + } + + // Register start of the contact phase + const scalar_t startTime = (currentPhase == 0) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase - 1]; + + // Find when the contact phase ends + while (currentPhase + 1 < numPhases && contactFlags[currentPhase + 1]) { + ++currentPhase; + } + + // Register end of the contact phase + const scalar_t endTime = (currentPhase + 1 >= numPhases) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase]; + + // Add to phases + contactTimings.push_back({startTime, endTime}); + ++currentPhase; + } + return contactTimings; +} + +std::vector extractSwingTimings(const std::vector& eventTimes, const std::vector& contactFlags) { + assert(eventTimes.size() + 1 == contactFlags.size()); + const int numPhases = contactFlags.size(); + + std::vector swingTimings; + swingTimings.reserve(1 + eventTimes.size() / 2); // Approximate upper bound + int currentPhase = 0; + + while (currentPhase < numPhases) { + // Search where swing phase starts + while (currentPhase < numPhases && contactFlags[currentPhase]) { + ++currentPhase; + } + if (currentPhase >= numPhases) { + break; // No more swing phases + } + + // Register start of the swing phase + const scalar_t startTime = (currentPhase == 0) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase - 1]; + + // Find when the swing phase ends + while (currentPhase + 1 < numPhases && !contactFlags[currentPhase + 1]) { + ++currentPhase; + } + + // Register end of the contact phase + const scalar_t endTime = (currentPhase + 1 >= numPhases) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase]; + + // Add to phases + swingTimings.push_back({startTime, endTime}); + ++currentPhase; + } + return swingTimings; +} + +feet_array_t> extractContactFlags(const std::vector& modeSequence) { + const size_t numPhases = modeSequence.size(); + + feet_array_t> contactFlagStock; + std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector(numPhases)); + + for (size_t i = 0; i < numPhases; i++) { + const auto contactFlag = modeNumber2StanceLeg(modeSequence[i]); + for (size_t j = 0; j < NUM_CONTACT_POINTS; j++) { + contactFlagStock[j][i] = contactFlag[j]; + } + } + return contactFlagStock; +} + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Rotation.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Rotation.cpp new file mode 100644 index 000000000..b05cddac3 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Rotation.cpp @@ -0,0 +1,14 @@ +#include "ocs2_legged_robot_mpcnet/helper/Rotation.h" + +namespace ocs2 { +namespace legged_robot { + +matrix3_t getRotationMatrixFromEulerAngles(const vector3_t& eulerAnglesZYX) { + matrix3_t R; + R = Eigen::AngleAxisd(eulerAnglesZYX(0), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(eulerAnglesZYX(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(eulerAnglesZYX(2), Eigen::Vector3d::UnitX()); + return R; +} + +} // namespace legged_robot +} // namespace ocs2 From d65643fb5147774ba41094fd5ea7fc2ad56b9ab4 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 15:16:22 +0200 Subject: [PATCH 014/512] add missing description --- ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h index fef7b8aea..1cf4b9fe9 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h @@ -8,7 +8,7 @@ namespace ocs2 { /** - * TODO + * A class to manage the data generation and policy evaluation rollouts for MPC-Net. */ class MpcnetRolloutManager { public: From 230f073fb9348c0439d2258e8ba152f73bf64bbe Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 18:35:40 +0200 Subject: [PATCH 015/512] adapt to generalized helper functions --- .../python/ocs2_ballbot_mpcnet/__init__.py | 7 -- .../ocs2_ballbot_mpcnet/ballbot_helper.py | 98 +++---------------- .../src/BallbotMpcnetPybindings.cpp | 2 +- 3 files changed, 12 insertions(+), 95 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py index 8893e0c6a..2973846dd 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py @@ -1,8 +1 @@ from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import MpcnetInterface -from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import size_array, scalar_array, vector_array, matrix_array -from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import ScalarFunctionQuadraticApproximation -from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import SystemObservation, SystemObservationArray -from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import ModeSchedule, ModeScheduleArray -from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import TargetTrajectories, TargetTrajectoriesArray -from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import DataPoint, DataArray -from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import Metrics, MetricsArray diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py index a52b3cdf9..101e0fc78 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py @@ -1,92 +1,15 @@ import numpy as np -from ocs2_ballbot_mpcnet import size_array, scalar_array, vector_array, SystemObservation, SystemObservationArray,\ - ModeSchedule, ModeScheduleArray, TargetTrajectories, TargetTrajectoriesArray +from ocs2_mpcnet import helper from ocs2_ballbot_mpcnet import ballbot_config as config -def get_size_array(data): - my_size_array = size_array() - my_size_array.resize(len(data)) - for i in range(len(data)): - my_size_array[i] = data[i] - return my_size_array - - -def get_scalar_array(data): - my_scalar_array = scalar_array() - my_scalar_array.resize(len(data)) - for i in range(len(data)): - my_scalar_array[i] = data[i] - return my_scalar_array - - -def get_vector_array(data): - my_vector_array = vector_array() - my_vector_array.resize(len(data)) - for i in range(len(data)): - my_vector_array[i] = data[i] - return my_vector_array - - -def get_system_observation(time, state): - system_observation = SystemObservation() - system_observation.mode = 0 - system_observation.time = time - system_observation.state = state - system_observation.input = np.zeros(config.STATE_DIM) - return system_observation - - -def get_system_observation_array(length): - system_observation_array = SystemObservationArray() - system_observation_array.resize(length) - return system_observation_array - - -def get_target_trajectories(time_trajectory, state_trajectory): - time_trajectory_array = get_scalar_array(time_trajectory) - state_trajectory_array = get_vector_array(state_trajectory) - input_trajectory_array = get_vector_array(np.zeros((len(time_trajectory), config.INPUT_DIM))) - return TargetTrajectories(time_trajectory_array, state_trajectory_array, input_trajectory_array) - - -def get_target_trajectories_array(length): - target_trajectories_array = TargetTrajectoriesArray() - target_trajectories_array.resize(length) - return target_trajectories_array - - -def get_mode_schedule(event_times, mode_sequence): - event_times_array = get_scalar_array(event_times) - mode_sequence_array = get_size_array(mode_sequence) - return ModeSchedule(event_times_array, mode_sequence_array) - - -def get_mode_schedule_array(length): - mode_schedule_array = ModeScheduleArray() - mode_schedule_array.resize(length) - return mode_schedule_array - - -def get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template): - gait_cycle_duration = event_times_template[-1] - num_gait_cycles = int(np.floor(duration / gait_cycle_duration)) - event_times = np.array([0.0], dtype=np.float64) - mode_sequence = np.array([0], dtype=np.uintp) - for _ in range(num_gait_cycles): - event_times = np.append(event_times, event_times[-1] * np.ones(len(event_times_template)) + event_times_template) - mode_sequence = np.append(mode_sequence, mode_sequence_template) - mode_sequence = np.append(mode_sequence, np.array([0], dtype=np.uintp)) - return event_times, mode_sequence - - -def get_default_mode_schedule(duration): +def get_default_event_times_and_mode_sequence(duration): # contact schedule: - # swing schedule: - event_times_template = np.array([1.0], dtype=np.float64) mode_sequence_template = np.array([0], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(0, duration, event_times_template, mode_sequence_template) def get_random_initial_state(): @@ -108,12 +31,13 @@ def get_random_target_state(): def get_tasks(n_tasks, duration): - initial_observations = get_system_observation_array(n_tasks) - mode_schedules = get_mode_schedule_array(n_tasks) - target_trajectories = get_target_trajectories_array(n_tasks) + initial_observations = helper.get_system_observation_array(n_tasks) + mode_schedules = helper.get_mode_schedule_array(n_tasks) + target_trajectories = helper.get_target_trajectories_array(n_tasks) for i in range(n_tasks): - initial_observations[i] = get_system_observation(0.0, get_random_initial_state()) - mode_schedules[i] = get_mode_schedule(*get_default_mode_schedule(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(0, 0.0, get_random_initial_state(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_default_event_times_and_mode_sequence(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp index b31ecbf21..567af03a7 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp @@ -2,4 +2,4 @@ #include "ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h" -CREATE_MPCNET_PYTHON_BINDINGS(ocs2::ballbot::BallbotMpcnetInterface, BallbotMpcnetPybindings) +CREATE_ROBOT_MPCNET_PYTHON_BINDINGS(ocs2::ballbot::BallbotMpcnetInterface, BallbotMpcnetPybindings) From 2cfc318e0f95bdcfcd4d92714de42a6c057ddebe Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 18:40:01 +0200 Subject: [PATCH 016/512] move general helper functions to ocs2_mpcnet --- ocs2_mpcnet/CMakeLists.txt | 21 +++ .../include/ocs2_mpcnet/MpcnetPybindMacros.h | 138 ++++++++++-------- ocs2_mpcnet/python/ocs2_mpcnet/__init__.py | 7 + ocs2_mpcnet/python/ocs2_mpcnet/helper.py | 80 ++++++++++ ocs2_mpcnet/src/MpcnetPybindings.cpp | 5 + 5 files changed, 187 insertions(+), 64 deletions(-) create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/helper.py create mode 100644 ocs2_mpcnet/src/MpcnetPybindings.cpp diff --git a/ocs2_mpcnet/CMakeLists.txt b/ocs2_mpcnet/CMakeLists.txt index dc5b30b17..eee405aa8 100644 --- a/ocs2_mpcnet/CMakeLists.txt +++ b/ocs2_mpcnet/CMakeLists.txt @@ -57,6 +57,22 @@ target_link_libraries(${PROJECT_NAME} onnxruntime ) +# python bindings +pybind11_add_module(MpcnetPybindings SHARED + src/MpcnetPybindings.cpp +) +add_dependencies(MpcnetPybindings + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(MpcnetPybindings PRIVATE + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(MpcnetPybindings + PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + catkin_python_setup() ######################### @@ -86,6 +102,11 @@ install(TARGETS ${PROJECT_NAME} install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +install(TARGETS MpcnetPybindings + ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + ############# ## Testing ## ############# diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index dbbea557c..1192e1b0a 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -10,73 +10,83 @@ using namespace pybind11::literals; /** - * Convenience macro to bind mpcnet interface and other classes with all required vectors. + * Convenience macro to bind general MPC-Net functionalities and other classes with all required vectors. */ -#define CREATE_MPCNET_PYTHON_BINDINGS(MPCNET_INTERFACE, LIB_NAME) \ - /* make vector types opaque so they are not converted to python lists */ \ - PYBIND11_MAKE_OPAQUE(ocs2::size_array_t) \ - PYBIND11_MAKE_OPAQUE(ocs2::scalar_array_t) \ - PYBIND11_MAKE_OPAQUE(ocs2::vector_array_t) \ - PYBIND11_MAKE_OPAQUE(ocs2::matrix_array_t) \ - PYBIND11_MAKE_OPAQUE(std::vector) \ - PYBIND11_MAKE_OPAQUE(std::vector) \ - PYBIND11_MAKE_OPAQUE(std::vector) \ - PYBIND11_MAKE_OPAQUE(MPCNET_INTERFACE::data_array_t) \ - PYBIND11_MAKE_OPAQUE(MPCNET_INTERFACE::metrics_array_t) \ +#define CREATE_MPCNET_PYTHON_BINDINGS(MPCNET_INTERFACE, LIB_NAME) \ + /* make vector types opaque so they are not converted to python lists */ \ + PYBIND11_MAKE_OPAQUE(ocs2::size_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::scalar_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::vector_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::matrix_array_t) \ + PYBIND11_MAKE_OPAQUE(std::vector) \ + PYBIND11_MAKE_OPAQUE(std::vector) \ + PYBIND11_MAKE_OPAQUE(std::vector) \ + PYBIND11_MAKE_OPAQUE(MPCNET_INTERFACE::data_array_t) \ + PYBIND11_MAKE_OPAQUE(MPCNET_INTERFACE::metrics_array_t) \ + /* create a python module */ \ + PYBIND11_MODULE(LIB_NAME, m) { \ + /* bind vector types so they can be used natively in python */ \ + VECTOR_TYPE_BINDING(ocs2::size_array_t, "size_array") \ + VECTOR_TYPE_BINDING(ocs2::scalar_array_t, "scalar_array") \ + VECTOR_TYPE_BINDING(ocs2::vector_array_t, "vector_array") \ + VECTOR_TYPE_BINDING(ocs2::matrix_array_t, "matrix_array") \ + VECTOR_TYPE_BINDING(std::vector, "SystemObservationArray") \ + VECTOR_TYPE_BINDING(std::vector, "ModeScheduleArray") \ + VECTOR_TYPE_BINDING(std::vector, "TargetTrajectoriesArray") \ + VECTOR_TYPE_BINDING(MPCNET_INTERFACE::data_array_t, "DataArray") \ + VECTOR_TYPE_BINDING(MPCNET_INTERFACE::metrics_array_t, "MetricsArray") \ + /* bind approximation classes */ \ + pybind11::class_(m, "ScalarFunctionQuadraticApproximation") \ + .def_readwrite("f", &ocs2::ScalarFunctionQuadraticApproximation::f) \ + .def_readwrite("dfdx", &ocs2::ScalarFunctionQuadraticApproximation::dfdx) \ + .def_readwrite("dfdu", &ocs2::ScalarFunctionQuadraticApproximation::dfdu) \ + .def_readwrite("dfdxx", &ocs2::ScalarFunctionQuadraticApproximation::dfdxx) \ + .def_readwrite("dfdux", &ocs2::ScalarFunctionQuadraticApproximation::dfdux) \ + .def_readwrite("dfduu", &ocs2::ScalarFunctionQuadraticApproximation::dfduu); \ + /* bind system observation struct */ \ + pybind11::class_(m, "SystemObservation") \ + .def(pybind11::init<>()) \ + .def_readwrite("mode", &ocs2::SystemObservation::mode) \ + .def_readwrite("time", &ocs2::SystemObservation::time) \ + .def_readwrite("state", &ocs2::SystemObservation::state) \ + .def_readwrite("input", &ocs2::SystemObservation::input); \ + /* bind mode schedule struct */ \ + pybind11::class_(m, "ModeSchedule") \ + .def(pybind11::init()) \ + .def_readwrite("event_times", &ocs2::ModeSchedule::eventTimes) \ + .def_readwrite("mode_sequence", &ocs2::ModeSchedule::modeSequence); \ + /* bind target trajectories class */ \ + pybind11::class_(m, "TargetTrajectories") \ + .def(pybind11::init()) \ + .def_readwrite("time_trajectory", &ocs2::TargetTrajectories::timeTrajectory) \ + .def_readwrite("state_trajectory", &ocs2::TargetTrajectories::stateTrajectory) \ + .def_readwrite("input_trajectory", &ocs2::TargetTrajectories::inputTrajectory); \ + /* bind data point struct */ \ + pybind11::class_(m, "DataPoint") \ + .def(pybind11::init<>()) \ + .def_readwrite("t", &MPCNET_INTERFACE::data_point_t::t) \ + .def_readwrite("x", &MPCNET_INTERFACE::data_point_t::x) \ + .def_readwrite("u", &MPCNET_INTERFACE::data_point_t::u) \ + .def_readwrite("mode", &MPCNET_INTERFACE::data_point_t::mode) \ + .def_readwrite("generalized_time", &MPCNET_INTERFACE::data_point_t::generalizedTime) \ + .def_readwrite("relative_state", &MPCNET_INTERFACE::data_point_t::relativeState) \ + .def_readwrite("hamiltonian", &MPCNET_INTERFACE::data_point_t::hamiltonian); \ + /* bind metrics struct */ \ + pybind11::class_(m, "Metrics") \ + .def(pybind11::init<>()) \ + .def_readwrite("survival_time", &MPCNET_INTERFACE::metrics_t::survivalTime) \ + .def_readwrite("incurred_hamiltonian", &MPCNET_INTERFACE::metrics_t::incurredHamiltonian); \ + } + +/** + * Convenience macro to bind robot MPC-Net interface. + */ +#define CREATE_ROBOT_MPCNET_PYTHON_BINDINGS(MPCNET_INTERFACE, LIB_NAME) \ /* create a python module */ \ PYBIND11_MODULE(LIB_NAME, m) { \ - /* bind vector types so they can be used natively in python */ \ - VECTOR_TYPE_BINDING(ocs2::size_array_t, "size_array") \ - VECTOR_TYPE_BINDING(ocs2::scalar_array_t, "scalar_array") \ - VECTOR_TYPE_BINDING(ocs2::vector_array_t, "vector_array") \ - VECTOR_TYPE_BINDING(ocs2::matrix_array_t, "matrix_array") \ - VECTOR_TYPE_BINDING(std::vector, "SystemObservationArray") \ - VECTOR_TYPE_BINDING(std::vector, "ModeScheduleArray") \ - VECTOR_TYPE_BINDING(std::vector, "TargetTrajectoriesArray") \ - VECTOR_TYPE_BINDING(MPCNET_INTERFACE::data_array_t, "DataArray") \ - VECTOR_TYPE_BINDING(MPCNET_INTERFACE::metrics_array_t, "MetricsArray") \ - /* bind approximation classes */ \ - pybind11::class_(m, "ScalarFunctionQuadraticApproximation") \ - .def_readwrite("f", &ocs2::ScalarFunctionQuadraticApproximation::f) \ - .def_readwrite("dfdx", &ocs2::ScalarFunctionQuadraticApproximation::dfdx) \ - .def_readwrite("dfdu", &ocs2::ScalarFunctionQuadraticApproximation::dfdu) \ - .def_readwrite("dfdxx", &ocs2::ScalarFunctionQuadraticApproximation::dfdxx) \ - .def_readwrite("dfdux", &ocs2::ScalarFunctionQuadraticApproximation::dfdux) \ - .def_readwrite("dfduu", &ocs2::ScalarFunctionQuadraticApproximation::dfduu); \ - /* bind system observation struct */ \ - pybind11::class_(m, "SystemObservation") \ - .def(pybind11::init<>()) \ - .def_readwrite("mode", &ocs2::SystemObservation::mode) \ - .def_readwrite("time", &ocs2::SystemObservation::time) \ - .def_readwrite("state", &ocs2::SystemObservation::state) \ - .def_readwrite("input", &ocs2::SystemObservation::input); \ - /* bind mode schedule struct */ \ - pybind11::class_(m, "ModeSchedule") \ - .def(pybind11::init()) \ - .def_readwrite("event_times", &ocs2::ModeSchedule::eventTimes) \ - .def_readwrite("mode_sequence", &ocs2::ModeSchedule::modeSequence); \ - /* bind target trajectories class */ \ - pybind11::class_(m, "TargetTrajectories") \ - .def(pybind11::init()) \ - .def_readwrite("time_trajectory", &ocs2::TargetTrajectories::timeTrajectory) \ - .def_readwrite("state_trajectory", &ocs2::TargetTrajectories::stateTrajectory) \ - .def_readwrite("input_trajectory", &ocs2::TargetTrajectories::inputTrajectory); \ - /* bind data point struct */ \ - pybind11::class_(m, "DataPoint") \ - .def(pybind11::init<>()) \ - .def_readwrite("t", &MPCNET_INTERFACE::data_point_t::t) \ - .def_readwrite("x", &MPCNET_INTERFACE::data_point_t::x) \ - .def_readwrite("u", &MPCNET_INTERFACE::data_point_t::u) \ - .def_readwrite("mode", &MPCNET_INTERFACE::data_point_t::mode) \ - .def_readwrite("generalized_time", &MPCNET_INTERFACE::data_point_t::generalizedTime) \ - .def_readwrite("relative_state", &MPCNET_INTERFACE::data_point_t::relativeState) \ - .def_readwrite("hamiltonian", &MPCNET_INTERFACE::data_point_t::hamiltonian); \ - /* bind metrics struct */ \ - pybind11::class_(m, "Metrics") \ - .def(pybind11::init<>()) \ - .def_readwrite("survival_time", &MPCNET_INTERFACE::metrics_t::survivalTime) \ - .def_readwrite("incurred_hamiltonian", &MPCNET_INTERFACE::metrics_t::incurredHamiltonian); \ - /* bind actual mpcnet interface */ \ + /* import the general MPC-Net module */ \ + pybind11::module::import("ocs2_mpcnet.MpcnetPybindings"); \ + /* bind actual MPC-Net interface for specific robot */ \ pybind11::class_(m, "MpcnetInterface") \ .def(pybind11::init()) \ .def("startDataGeneration", &MPCNET_INTERFACE::startDataGeneration, "alpha"_a, "policyFilePath"_a, "timeStep"_a, \ diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/__init__.py b/ocs2_mpcnet/python/ocs2_mpcnet/__init__.py index e69de29bb..134e00bd3 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/__init__.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/__init__.py @@ -0,0 +1,7 @@ +from ocs2_mpcnet.MpcnetPybindings import size_array, scalar_array, vector_array, matrix_array +from ocs2_mpcnet.MpcnetPybindings import ScalarFunctionQuadraticApproximation +from ocs2_mpcnet.MpcnetPybindings import SystemObservation, SystemObservationArray +from ocs2_mpcnet.MpcnetPybindings import ModeSchedule, ModeScheduleArray +from ocs2_mpcnet.MpcnetPybindings import TargetTrajectories, TargetTrajectoriesArray +from ocs2_mpcnet.MpcnetPybindings import DataPoint, DataArray +from ocs2_mpcnet.MpcnetPybindings import Metrics, MetricsArray diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py new file mode 100644 index 000000000..fc3b95508 --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py @@ -0,0 +1,80 @@ +import numpy as np + +from ocs2_mpcnet import size_array, scalar_array, vector_array, SystemObservation, SystemObservationArray,\ + ModeSchedule, ModeScheduleArray, TargetTrajectories, TargetTrajectoriesArray + + +def get_size_array(data): + my_size_array = size_array() + my_size_array.resize(len(data)) + for i in range(len(data)): + my_size_array[i] = data[i] + return my_size_array + + +def get_scalar_array(data): + my_scalar_array = scalar_array() + my_scalar_array.resize(len(data)) + for i in range(len(data)): + my_scalar_array[i] = data[i] + return my_scalar_array + + +def get_vector_array(data): + my_vector_array = vector_array() + my_vector_array.resize(len(data)) + for i in range(len(data)): + my_vector_array[i] = data[i] + return my_vector_array + + +def get_system_observation(mode, time, state, input): + system_observation = SystemObservation() + system_observation.mode = mode + system_observation.time = time + system_observation.state = state + system_observation.input = input + return system_observation + + +def get_system_observation_array(length): + system_observation_array = SystemObservationArray() + system_observation_array.resize(length) + return system_observation_array + + +def get_target_trajectories(time_trajectory, state_trajectory, input_trajectory): + time_trajectory_array = get_scalar_array(time_trajectory) + state_trajectory_array = get_vector_array(state_trajectory) + input_trajectory_array = get_vector_array(input_trajectory) + return TargetTrajectories(time_trajectory_array, state_trajectory_array, input_trajectory_array) + + +def get_target_trajectories_array(length): + target_trajectories_array = TargetTrajectoriesArray() + target_trajectories_array.resize(length) + return target_trajectories_array + + +def get_mode_schedule(event_times, mode_sequence): + event_times_array = get_scalar_array(event_times) + mode_sequence_array = get_size_array(mode_sequence) + return ModeSchedule(event_times_array, mode_sequence_array) + + +def get_mode_schedule_array(length): + mode_schedule_array = ModeScheduleArray() + mode_schedule_array.resize(length) + return mode_schedule_array + + +def get_event_times_and_mode_sequence(default_mode, duration, event_times_template, mode_sequence_template): + gait_cycle_duration = event_times_template[-1] + num_gait_cycles = int(np.floor(duration / gait_cycle_duration)) + event_times = np.array([0.0], dtype=np.float64) + mode_sequence = np.array([default_mode], dtype=np.uintp) + for _ in range(num_gait_cycles): + event_times = np.append(event_times, event_times[-1] * np.ones(len(event_times_template)) + event_times_template) + mode_sequence = np.append(mode_sequence, mode_sequence_template) + mode_sequence = np.append(mode_sequence, np.array([default_mode], dtype=np.uintp)) + return event_times, mode_sequence diff --git a/ocs2_mpcnet/src/MpcnetPybindings.cpp b/ocs2_mpcnet/src/MpcnetPybindings.cpp new file mode 100644 index 000000000..4fe2bddf0 --- /dev/null +++ b/ocs2_mpcnet/src/MpcnetPybindings.cpp @@ -0,0 +1,5 @@ +#include + +#include "ocs2_mpcnet/MpcnetInterfaceBase.h" + +CREATE_MPCNET_PYTHON_BINDINGS(ocs2::MpcnetInterfaceBase, MpcnetPybindings) From 2b2c5032ff97ae72bdff80950bfa211e3fdf3944 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 26 Aug 2021 19:19:47 +0200 Subject: [PATCH 017/512] adapt to generalized helper functions --- .../ocs2_legged_robot_mpcnet/__init__.py | 7 - .../legged_robot_helper.py | 184 ++++++------------ .../src/LeggedRobotMpcnetPybindings.cpp | 2 +- 3 files changed, 59 insertions(+), 134 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py index 5c52da1b9..4d878b2d3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py @@ -1,8 +1 @@ from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import MpcnetInterface -from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import size_array, scalar_array, vector_array, matrix_array -from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import ScalarFunctionQuadraticApproximation -from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import SystemObservation, SystemObservationArray -from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import ModeSchedule, ModeScheduleArray -from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import TargetTrajectories, TargetTrajectoriesArray -from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import DataPoint, DataArray -from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import Metrics, MetricsArray diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py index 6915cd21f..20ce315f5 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -1,92 +1,15 @@ import numpy as np -from ocs2_legged_robot_mpcnet import size_array, scalar_array, vector_array, SystemObservation, SystemObservationArray,\ - ModeSchedule, ModeScheduleArray, TargetTrajectories, TargetTrajectoriesArray +from ocs2_mpcnet import helper from ocs2_legged_robot_mpcnet import legged_robot_config as config -def get_size_array(data): - my_size_array = size_array() - my_size_array.resize(len(data)) - for i in range(len(data)): - my_size_array[i] = data[i] - return my_size_array - - -def get_scalar_array(data): - my_scalar_array = scalar_array() - my_scalar_array.resize(len(data)) - for i in range(len(data)): - my_scalar_array[i] = data[i] - return my_scalar_array - - -def get_vector_array(data): - my_vector_array = vector_array() - my_vector_array.resize(len(data)) - for i in range(len(data)): - my_vector_array[i] = data[i] - return my_vector_array - - -def get_system_observation(time, state): - system_observation = SystemObservation() - system_observation.mode = 15 - system_observation.time = time - system_observation.state = state - system_observation.input = np.zeros(config.STATE_DIM) - return system_observation - - -def get_system_observation_array(length): - system_observation_array = SystemObservationArray() - system_observation_array.resize(length) - return system_observation_array - - -def get_target_trajectories(time_trajectory, state_trajectory): - time_trajectory_array = get_scalar_array(time_trajectory) - state_trajectory_array = get_vector_array(state_trajectory) - input_trajectory_array = get_vector_array(np.zeros((len(time_trajectory), config.INPUT_DIM))) - return TargetTrajectories(time_trajectory_array, state_trajectory_array, input_trajectory_array) - - -def get_target_trajectories_array(length): - target_trajectories_array = TargetTrajectoriesArray() - target_trajectories_array.resize(length) - return target_trajectories_array - - -def get_mode_schedule(event_times, mode_sequence): - event_times_array = get_scalar_array(event_times) - mode_sequence_array = get_size_array(mode_sequence) - return ModeSchedule(event_times_array, mode_sequence_array) - - -def get_mode_schedule_array(length): - mode_schedule_array = ModeScheduleArray() - mode_schedule_array.resize(length) - return mode_schedule_array - - -def get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template): - gait_cycle_duration = event_times_template[-1] - num_gait_cycles = int(np.floor(duration / gait_cycle_duration)) - event_times = np.array([0.0], dtype=np.float64) - mode_sequence = np.array([15], dtype=np.uintp) - for _ in range(num_gait_cycles): - event_times = np.append(event_times, event_times[-1] * np.ones(len(event_times_template)) + event_times_template) - mode_sequence = np.append(mode_sequence, mode_sequence_template) - mode_sequence = np.append(mode_sequence, np.array([15], dtype=np.uintp)) - return event_times, mode_sequence - - def get_stance(duration): # contact schedule: STANCE # swing schedule: - event_times_template = np.array([1.0], dtype=np.float64) mode_sequence_template = np.array([15], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_random_initial_state_stance(): @@ -124,7 +47,7 @@ def get_trot_1(duration): # swing schedule: RF_LH, LF_RH event_times_template = np.array([0.35, 0.7], dtype=np.float64) mode_sequence_template = np.array([9, 6], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_trot_2(duration): @@ -132,7 +55,7 @@ def get_trot_2(duration): # swing schedule: LF_RH, RF_LH event_times_template = np.array([0.35, 0.7], dtype=np.float64) mode_sequence_template = np.array([6, 9], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_random_initial_state_trot(): @@ -168,7 +91,7 @@ def get_dynamic_diagonal_walk_1(duration): # swing schedule: LF, LF_RH, RH, RF, RF_LH, LH event_times_template = np.array([0.15, 0.3, 0.45, 0.6, 0.75, 0.9], dtype=np.float64) mode_sequence_template = np.array([7, 6, 14, 11, 9, 13], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_dynamic_diagonal_walk_2(duration): @@ -176,7 +99,7 @@ def get_dynamic_diagonal_walk_2(duration): # swing schedule: RF, RF_LH, LH, LF, LF_RH, RH event_times_template = np.array([0.0, 0.15, 0.3, 0.45, 0.6, 0.75, 0.9], dtype=np.float64) mode_sequence_template = np.array([11, 9, 13, 7, 6, 14], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_random_initial_state_dynamic_diagonal_walk(): @@ -212,7 +135,7 @@ def get_static_walk_1(duration): # swing schedule: LH, LF, RH, RF event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) mode_sequence_template = np.array([13, 7, 14, 11], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_static_walk_2(duration): @@ -220,7 +143,7 @@ def get_static_walk_2(duration): # swing schedule: LF, RH, RF, LH event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) mode_sequence_template = np.array([7, 14, 11, 13], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_static_walk_3(duration): @@ -228,7 +151,7 @@ def get_static_walk_3(duration): # swing schedule: RH, RF, LH, LF event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) mode_sequence_template = np.array([14, 11, 13, 7], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_static_walk_4(duration): @@ -236,7 +159,7 @@ def get_static_walk_4(duration): # swing schedule: RF, LH, LF, RH event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) mode_sequence_template = np.array([11, 13, 7, 14], dtype=np.uintp) - return get_event_times_and_mode_sequence(duration, event_times_template, mode_sequence_template) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_random_initial_state_static_walk(): @@ -268,55 +191,64 @@ def get_random_target_state_static_walk(): def get_tasks(n_tasks, duration, choices): - initial_observations = get_system_observation_array(n_tasks) - mode_schedules = get_mode_schedule_array(n_tasks) - target_trajectories = get_target_trajectories_array(n_tasks) + initial_observations = helper.get_system_observation_array(n_tasks) + mode_schedules = helper.get_mode_schedule_array(n_tasks) + target_trajectories = helper.get_target_trajectories_array(n_tasks) for i in range(n_tasks): if choices[i] == "stance": - initial_observations[i] = get_system_observation(0.0, get_random_initial_state_stance()) - mode_schedules[i] = get_mode_schedule(*get_stance(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_stance().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_stance(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_stance(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_stance().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) elif choices[i] == "trot_1": - initial_observations[i] = get_system_observation(0.0, get_random_initial_state_trot()) - mode_schedules[i] = get_mode_schedule(*get_trot_1(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_trot().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_trot(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_trot_1(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_trot().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) elif choices[i] == "trot_2": - initial_observations[i] = get_system_observation(0.0, get_random_initial_state_trot()) - mode_schedules[i] = get_mode_schedule(*get_trot_2(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_trot().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_trot(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_trot_2(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_trot().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) elif choices[i] == "dynamic_diagonal_walk_1": - initial_observations[i] = get_system_observation(0.0, get_random_initial_state_dynamic_diagonal_walk()) - mode_schedules[i] = get_mode_schedule(*get_dynamic_diagonal_walk_1(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_dynamic_diagonal_walk().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_dynamic_diagonal_walk(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_dynamic_diagonal_walk_1(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_dynamic_diagonal_walk().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) elif choices[i] == "dynamic_diagonal_walk_2": - initial_observations[i] = get_system_observation(0.0, get_random_initial_state_dynamic_diagonal_walk()) - mode_schedules[i] = get_mode_schedule(*get_dynamic_diagonal_walk_2(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_dynamic_diagonal_walk().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_dynamic_diagonal_walk(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_dynamic_diagonal_walk_2(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_dynamic_diagonal_walk().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) elif choices[i] == "static_walk_1": - initial_observations[i] = get_system_observation(0.0, get_random_initial_state_static_walk()) - mode_schedules[i] = get_mode_schedule(*get_static_walk_1(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_static_walk().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_static_walk(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_static_walk_1(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_static_walk().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) elif choices[i] == "static_walk_2": - initial_observations[i] = get_system_observation(0.0, get_random_initial_state_static_walk()) - mode_schedules[i] = get_mode_schedule(*get_static_walk_2(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_static_walk().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_static_walk(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_static_walk_2(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_static_walk().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) elif choices[i] == "static_walk_3": - initial_observations[i] = get_system_observation(0.0, get_random_initial_state_static_walk()) - mode_schedules[i] = get_mode_schedule(*get_static_walk_3(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_static_walk().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_static_walk(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_static_walk_3(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_static_walk().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) elif choices[i] == "static_walk_4": - initial_observations[i] = get_system_observation(0.0, get_random_initial_state_static_walk()) - mode_schedules[i] = get_mode_schedule(*get_static_walk_4(duration)) - target_trajectories[i] = get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_static_walk().reshape((1, config.STATE_DIM))) + initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_static_walk(), np.zeros(config.INPUT_DIM)) + mode_schedules[i] = helper.get_mode_schedule(*get_static_walk_4(duration)) + target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), + get_random_target_state_static_walk().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM))) return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp index 2db8365a5..64f847634 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp @@ -2,4 +2,4 @@ #include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h" -CREATE_MPCNET_PYTHON_BINDINGS(ocs2::legged_robot::LeggedRobotMpcnetInterface, LeggedRobotMpcnetPybindings) +CREATE_ROBOT_MPCNET_PYTHON_BINDINGS(ocs2::legged_robot::LeggedRobotMpcnetInterface, LeggedRobotMpcnetPybindings) From ab1e2a3de95bd59822202cbeb74325392e74e958 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 30 Aug 2021 10:56:57 +0200 Subject: [PATCH 018/512] add Hamiltonian --- ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h | 2 + ocs2_ddp/src/GaussNewtonDDP.cpp | 49 +++++++++++++++++++ .../rollout/MpcnetPolicyEvaluation.h | 4 +- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 36 ++++++++++---- .../src/rollout/MpcnetDataGeneration.cpp | 6 +-- .../src/rollout/MpcnetPolicyEvaluation.cpp | 9 ++-- .../include/ocs2_oc/oc_solver/SolverBase.h | 10 ++++ 7 files changed, 96 insertions(+), 20 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h index e52a691c0..f4c96f085 100644 --- a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h +++ b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h @@ -102,6 +102,8 @@ class GaussNewtonDDP : public SolverBase { ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const override; + ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) const override; + vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override; void rewindOptimizer(size_t firstIndex) override; diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index a56b77f40..2c781d5cd 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -318,6 +318,55 @@ ScalarFunctionQuadraticApproximation GaussNewtonDDP::getValueFunction(scalar_t t return valueFunction; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +ScalarFunctionQuadraticApproximation GaussNewtonDDP::getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) const { + size_t partition = lookup::findBoundedActiveIntervalInTimeArray(partitioningTimes_, time); + partition = std::max(partition, initActivePartition_); + partition = std::min(partition, finalActivePartition_); + + ModelData modelData; + const auto indexAlpha = LinearInterpolation::timeSegment(time, cachedTimeTrajectoriesStock_[partition]); + const vector_t xNominal = LinearInterpolation::interpolate(indexAlpha, cachedStateTrajectoriesStock_[partition]); + + // perform the LQ approximation of the OC problem + LinearQuadraticApproximator lqapprox(optimalControlProblemStock_[0], settings().checkNumericalStability_); + lqapprox.approximateLQProblem(time, state, input, modelData); + modelData.checkSizes(state.rows(), input.rows()); + + // augment the cost with state-only equality and state-input inequality terms + augmentCostWorker(0, constraintPenaltyCoefficients_.stateEqConstrPenaltyCoeff, 0.0, modelData); + + // state-input equality constraint cost nu(x) * g(x,u) + // note: nu has no approximation and is evaluated at the nominal state as it can be very sensitive + ScalarFunctionQuadraticApproximation constraintCost; + const vector_t nu = getStateInputEqualityConstraintLagrangian(time, xNominal); + constraintCost.f = nu.transpose() * modelData.stateInputEqConstr_.f; + constraintCost.dfdx = modelData.stateInputEqConstr_.dfdx.transpose() * nu; + constraintCost.dfdu = modelData.stateInputEqConstr_.dfdu.transpose() * nu; + constraintCost.dfdxx = matrix_t::Zero(state.rows(), state.rows()); + constraintCost.dfdux = matrix_t::Zero(input.rows(), state.rows()); + constraintCost.dfduu = matrix_t::Zero(input.rows(), input.rows()); + + // "future" cost dVdx(x) * f(x,u) + ScalarFunctionQuadraticApproximation futureCost; + const ScalarFunctionQuadraticApproximation V = getValueFunction(time, state); + futureCost.f = V.dfdx.transpose() * modelData.dynamics_.f; + futureCost.dfdx = V.dfdxx.transpose() * modelData.dynamics_.f + modelData.dynamics_.dfdx.transpose() * V.dfdx; + futureCost.dfdu = modelData.dynamics_.dfdu.transpose() * V.dfdx; + futureCost.dfdxx = V.dfdxx.transpose() * modelData.dynamics_.dfdx + modelData.dynamics_.dfdx.transpose() * V.dfdxx; + futureCost.dfdux = modelData.dynamics_.dfdu.transpose() * V.dfdxx; + futureCost.dfduu = matrix_t::Zero(input.rows(), input.rows()); + + // assemble the LQ approximation of the Hamiltonian + ScalarFunctionQuadraticApproximation hamiltonian; + hamiltonian = modelData.cost_; + hamiltonian += constraintCost; + hamiltonian += futureCost; + return hamiltonian; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h index c21bc0900..ca4751843 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -24,8 +24,8 @@ class MpcnetPolicyEvaluation { using reference_manager_t = ReferenceManagerInterface; struct Metrics { - scalar_t survivalTime; - scalar_t incurredHamiltonian; + scalar_t survivalTime = 0.0; + scalar_t incurredHamiltonian = 0.0; }; using MetricsArray = std::vector; using MetricsPtr = std::unique_ptr; diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index 87c6ac150..9ab38f344 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -1,19 +1,37 @@ import torch import numpy as np +from ocs2_mpcnet import config + class Hamiltonian: - # Uses the quadratic approximation of the Hamiltonian as loss + # Uses the linear quadratic approximation of the Hamiltonian as loss # H(x,u) = 1/2 dx' dHdxx dx + du' dHdux dx + 1/2 du' dHduu du + dHdx' dx + dHdu' du + H - - def compute_torch(self, x, u, hamiltonian): - # TODO (areske): implement once approximation of Hamiltonian is available - return - - def compute_numpy(self, x, u, hamiltonian): - # TODO (areske): implement once approximation of Hamiltonian is available - return + + @staticmethod + def compute_torch(x_inquiry, x_nominal, u_inquiry, u_nominal, hamiltonian): + dx = torch.sub(x_inquiry, x_nominal) + du = torch.sub(u_inquiry, u_nominal) + dHdxx = 0.5 * torch.dot(dx, torch.matmul(torch.tensor(hamiltonian.dfdxx, dtype=config.dtype, device=config.device), dx)) + dHdux = torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfdux, dtype=config.dtype, device=config.device), dx)) + dHduu = 0.5 * torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfduu, dtype=config.dtype, device=config.device), du)) + dHdx = torch.dot(torch.tensor(hamiltonian.dfdx, dtype=config.dtype, device=config.device), dx) + dHdu = torch.dot(torch.tensor(hamiltonian.dfdu, dtype=config.dtype, device=config.device), du) + H = torch.tensor(hamiltonian.f, dtype=config.dtype, device=config.device) + return dHdxx + dHdux + dHduu + dHdx + dHdu + H + + @staticmethod + def compute_numpy(x_inquiry, x_nominal, u_inquiry, u_nominal, hamiltonian): + dx = np.subtract(x_inquiry, x_nominal) + du = np.subtract(u_inquiry, u_nominal) + dHdxx = 0.5 * np.dot(dx, np.matmul(hamiltonian.dfdxx, dx)) + dHdux = np.dot(du, np.matmul(hamiltonian.dfdux, dx)) + dHduu = 0.5 * np.dot(du, np.matmul(hamiltonian.dfduu, du)) + dHdx = np.dot(hamiltonian.dfdx, dx) + dHdu = np.dot(hamiltonian.dfdu, du) + H = hamiltonian.f + return dHdxx + dHdux + dHduu + dHdx + dHdu + H class BehavioralCloning: diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index 534a1517c..9c2adeff3 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -66,8 +66,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); - // TODO(areske): add once approximation of Hamiltonian is available - // dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); + dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); dataPtr->push_back(std::move(dataPoint)); } @@ -81,8 +80,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); - // TODO(areske): add once approximation of Hamiltonian is available - // dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); + dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); dataPtr->push_back(std::move(dataPoint)); } } diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index b269e5572..d99c34191 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -30,15 +30,14 @@ MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string int iteration = 0; try { while (time <= targetTrajectories.timeTrajectory.back()) { - // run mpc and get solution + // run mpc if (!mpcPtr_->run(time, state)) { throw std::runtime_error("MpcnetPolicyEvaluation::run Main routine of MPC returned false."); } - PrimalSolution primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); - // TODO(areske): add once approximation of Hamiltonian is available - // vector_t input = primalSolution.controllerPtr_->computeInput(time, state); - // metricsPtr->incurredHamiltonian += mpcPtr_->getSolverPtr()->getHamiltonian(time, state, input).f * timeStep; + // incurred quantities + vector_t input = mpcnetPtr_->computeInput(time, state); + metricsPtr->incurredHamiltonian += mpcPtr_->getSolverPtr()->getHamiltonian(time, state, input).f * timeStep; // forward simulate system with learned controller scalar_array_t timeTrajectory; diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h index effef8a2d..9f4152265 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h @@ -181,6 +181,16 @@ class SolverBase { */ virtual ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const = 0; + /** + * Calculates the Hamiltonian quadratic approximation at the given time, state and input. + * + * @param [in] time: The inquiry time + * @param [in] state: The inquiry state. + * @param [in] input: The inquiry input. + * @return The quadratic approximation of the Hamiltonian at the requested time, state and input. + */ + virtual ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) const = 0; + /** * Calculates the Lagrange multiplier of the state-input equality constraints at the given time and state. * From 4e3d089fb99bdc9d0b03f91a323085f795e43097 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 30 Aug 2021 11:10:05 +0200 Subject: [PATCH 019/512] use Hamiltonian loss for ballbot --- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 14e543ceb..493b0dd19 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -6,7 +6,7 @@ from torch.utils.tensorboard import SummaryWriter -from ocs2_mpcnet.loss import BehavioralCloning as Loss +from ocs2_mpcnet.loss import Hamiltonian as Loss from ocs2_mpcnet.memory import ReplayMemory as Memory from ocs2_mpcnet.policy import LinearPolicy as Policy @@ -41,7 +41,7 @@ os.makedirs(name="policies/" + folder) # loss -loss = Loss(torch.tensor(config.R, device=config.device, dtype=config.dtype).diag(), np.diag(config.R)) +loss = Loss() # memory memory_capacity = 1000000 @@ -145,7 +145,7 @@ def closure(): relative_state = torch.tensor(sample.relative_state, dtype=config.dtype, device=config.device) p, U = policy(generalized_time, relative_state) u_predicted = torch.matmul(p, U) - empirical_loss = empirical_loss + loss.compute_torch(u_predicted, u_target) + empirical_loss = empirical_loss + loss.compute_torch(x, x, u_predicted, u_target, sample.hamiltonian) # compute the gradients empirical_loss.backward() # logging From 327eb2e4b708b8648d9147b0369a0a3d8fb9ab35 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 30 Aug 2021 12:17:53 +0200 Subject: [PATCH 020/512] use Hamiltonian loss for legged_robot --- .../python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 138ccd374..d3b13a10b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -7,7 +7,7 @@ from torch.utils.tensorboard import SummaryWriter -from ocs2_mpcnet.loss import BehavioralCloning as ExpertsLoss +from ocs2_mpcnet.loss import Hamiltonian as ExpertsLoss from ocs2_mpcnet.loss import CrossEntropy as GatingLoss from ocs2_mpcnet.memory import ReplayMemory as Memory @@ -53,7 +53,7 @@ # loss epsilon = 1e-8 # epsilon to improve numerical stability of logs and denominators my_lambda = 1.0 # parameter to control the relative importance of both loss types -experts_loss = ExpertsLoss(torch.tensor(config.R, device=config.device, dtype=config.dtype).diag(), np.diag(config.R)) +experts_loss = ExpertsLoss() gating_loss = GatingLoss(torch.tensor(epsilon, device=config.device, dtype=config.dtype), np.array(epsilon)) # memory @@ -172,7 +172,7 @@ def closure(): relative_state = torch.tensor(sample.relative_state, dtype=config.dtype, device=config.device) p, U = policy(generalized_time, relative_state) u_predicted = torch.matmul(p, U) - empirical_experts_loss = empirical_experts_loss + experts_loss.compute_torch(u_predicted, u_target) + empirical_experts_loss = empirical_experts_loss + experts_loss.compute_torch(x, x, u_predicted, u_target, sample.hamiltonian) empirical_gating_loss = empirical_gating_loss + gating_loss.compute_torch(p_target, p) empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss # compute the gradients From afcff55b6a65e3e6eb000eb53d699fc7b95523ae Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 30 Aug 2021 12:19:08 +0200 Subject: [PATCH 021/512] use rotation from ocs2_robotic_tools --- .../ocs2_legged_robot_mpcnet/CMakeLists.txt | 1 - .../ocs2_legged_robot_mpcnet/helper/Rotation.h | 11 ----------- .../src/LeggedRobotMpcnetDefinition.cpp | 5 +++-- .../src/helper/Rotation.cpp | 14 -------------- 4 files changed, 3 insertions(+), 28 deletions(-) delete mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Rotation.h delete mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Rotation.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt index 1b2f8cee8..9e28009da 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt @@ -39,7 +39,6 @@ include_directories( # main library add_library(${PROJECT_NAME} src/helper/Logic.cpp - src/helper/Rotation.cpp src/LeggedRobotMpcnetDefinition.cpp src/LeggedRobotMpcnetInterface.cpp ) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Rotation.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Rotation.h deleted file mode 100644 index effbd7c05..000000000 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Rotation.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include - -namespace ocs2 { -namespace legged_robot { - -matrix3_t getRotationMatrixFromEulerAngles(const vector3_t& eulerAnglesZYX); - -} // namespace legged_robot -} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index 6f6de3255..b4c3bb360 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -2,8 +2,9 @@ #include +#include + #include "ocs2_legged_robot_mpcnet/helper/Logic.h" -#include "ocs2_legged_robot_mpcnet/helper/Rotation.h" namespace ocs2 { namespace legged_robot { @@ -41,7 +42,7 @@ vector_t LeggedRobotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeS vector_t LeggedRobotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { vector_t relativeState = x - targetTrajectories.getDesiredState(t); - matrix3_t R = getRotationMatrixFromEulerAngles(x.segment<3>(9)).transpose(); + matrix3_t R = getRotationMatrixFromZyxEulerAngles(x.segment<3>(9)).transpose(); relativeState.segment<3>(6) = R * relativeState.segment<3>(6); relativeState.segment<3>(9) = R * relativeState.segment<3>(9); return relativeState; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Rotation.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Rotation.cpp deleted file mode 100644 index b05cddac3..000000000 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Rotation.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "ocs2_legged_robot_mpcnet/helper/Rotation.h" - -namespace ocs2 { -namespace legged_robot { - -matrix3_t getRotationMatrixFromEulerAngles(const vector3_t& eulerAnglesZYX) { - matrix3_t R; - R = Eigen::AngleAxisd(eulerAnglesZYX(0), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(eulerAnglesZYX(1), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(eulerAnglesZYX(2), Eigen::Vector3d::UnitX()); - return R; -} - -} // namespace legged_robot -} // namespace ocs2 From c4a400dd8457a0d6c3ebbc6afd43e780224bdcea Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 30 Aug 2021 12:34:33 +0200 Subject: [PATCH 022/512] move leg logic to ocs2_legged_robot --- ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt | 1 + .../include/ocs2_legged_robot/gait/LegLogic.h} | 3 ++- .../Logic.cpp => ocs2_legged_robot/src/gait/LegLogic.cpp} | 4 ++-- ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt | 1 - .../src/LeggedRobotMpcnetDefinition.cpp | 3 +-- 5 files changed, 6 insertions(+), 6 deletions(-) rename ocs2_robotic_examples/{ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Logic.h => ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h} (99%) rename ocs2_robotic_examples/{ocs2_legged_robot_mpcnet/src/helper/Logic.cpp => ocs2_legged_robot/src/gait/LegLogic.cpp} (98%) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index 4fcb98c73..b124cf38b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -84,6 +84,7 @@ add_library(ocs2_legged_robot src/gait/Gait.cpp src/gait/GaitReceiver.cpp src/gait/GaitSchedule.cpp + src/gait/LegLogic.cpp src/gait/ModeSequenceTemplate.cpp src/LeggedRobotInterface.cpp src/LeggedRobotPreComputation.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Logic.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h similarity index 99% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Logic.h rename to ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h index 79a0eed71..460b88037 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/helper/Logic.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h @@ -1,7 +1,8 @@ #pragma once #include -#include + +#include "ocs2_legged_robot/common/Types.h" namespace ocs2 { namespace legged_robot { diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Logic.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp similarity index 98% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Logic.cpp rename to ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp index 30d38e831..8e0cc32fc 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/helper/Logic.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp @@ -1,6 +1,6 @@ -#include "ocs2_legged_robot_mpcnet/helper/Logic.h" +#include "ocs2_legged_robot/gait/LegLogic.h" -#include +#include "ocs2_legged_robot/gait/MotionPhaseDefinition.h" namespace ocs2 { namespace legged_robot { diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt index 9e28009da..2342d76a5 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt @@ -38,7 +38,6 @@ include_directories( # main library add_library(${PROJECT_NAME} - src/helper/Logic.cpp src/LeggedRobotMpcnetDefinition.cpp src/LeggedRobotMpcnetInterface.cpp ) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index b4c3bb360..187df4588 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -2,10 +2,9 @@ #include +#include #include -#include "ocs2_legged_robot_mpcnet/helper/Logic.h" - namespace ocs2 { namespace legged_robot { From 33b366fb7588772d48c71935cefc82375bf7931c Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Tue, 31 Aug 2021 14:25:02 +0200 Subject: [PATCH 023/512] increase weight for gating loss --- .../python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index d3b13a10b..696a087b2 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -52,7 +52,7 @@ # loss epsilon = 1e-8 # epsilon to improve numerical stability of logs and denominators -my_lambda = 1.0 # parameter to control the relative importance of both loss types +my_lambda = 10.0 # parameter to control the relative importance of both loss types experts_loss = ExpertsLoss() gating_loss = GatingLoss(torch.tensor(epsilon, device=config.device, dtype=config.dtype), np.array(epsilon)) From 41a181f8134759312248d1ae2a3b617abc0e274c Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Tue, 31 Aug 2021 15:14:08 +0200 Subject: [PATCH 024/512] improve efficiency of Hamiltonian computation --- ocs2_ddp/src/GaussNewtonDDP.cpp | 42 +++++++++++++++------------------ 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 2c781d5cd..0c033c422 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -338,32 +338,28 @@ ScalarFunctionQuadraticApproximation GaussNewtonDDP::getHamiltonian(scalar_t tim // augment the cost with state-only equality and state-input inequality terms augmentCostWorker(0, constraintPenaltyCoefficients_.stateEqConstrPenaltyCoeff, 0.0, modelData); - // state-input equality constraint cost nu(x) * g(x,u) + // initialize the Hamiltonian with the augmented cost + ScalarFunctionQuadraticApproximation hamiltonian(modelData.cost_); + + // add the state-input equality constraint cost nu(x) * g(x,u) to the Hamiltonian // note: nu has no approximation and is evaluated at the nominal state as it can be very sensitive - ScalarFunctionQuadraticApproximation constraintCost; const vector_t nu = getStateInputEqualityConstraintLagrangian(time, xNominal); - constraintCost.f = nu.transpose() * modelData.stateInputEqConstr_.f; - constraintCost.dfdx = modelData.stateInputEqConstr_.dfdx.transpose() * nu; - constraintCost.dfdu = modelData.stateInputEqConstr_.dfdu.transpose() * nu; - constraintCost.dfdxx = matrix_t::Zero(state.rows(), state.rows()); - constraintCost.dfdux = matrix_t::Zero(input.rows(), state.rows()); - constraintCost.dfduu = matrix_t::Zero(input.rows(), input.rows()); - - // "future" cost dVdx(x) * f(x,u) - ScalarFunctionQuadraticApproximation futureCost; + hamiltonian.f += nu.transpose() * modelData.stateInputEqConstr_.f; + hamiltonian.dfdx.noalias() += modelData.stateInputEqConstr_.dfdx.transpose() * nu; + hamiltonian.dfdu.noalias() += modelData.stateInputEqConstr_.dfdu.transpose() * nu; + // dfddx is zero for the state-input equality constraint cost + // dfdux is zero for the state-input equality constraint cost + // dfduu is zero for the state-input equality constraint cost + + // add the "future cost" dVdx(x) * f(x,u) to the Hamiltonian const ScalarFunctionQuadraticApproximation V = getValueFunction(time, state); - futureCost.f = V.dfdx.transpose() * modelData.dynamics_.f; - futureCost.dfdx = V.dfdxx.transpose() * modelData.dynamics_.f + modelData.dynamics_.dfdx.transpose() * V.dfdx; - futureCost.dfdu = modelData.dynamics_.dfdu.transpose() * V.dfdx; - futureCost.dfdxx = V.dfdxx.transpose() * modelData.dynamics_.dfdx + modelData.dynamics_.dfdx.transpose() * V.dfdxx; - futureCost.dfdux = modelData.dynamics_.dfdu.transpose() * V.dfdxx; - futureCost.dfduu = matrix_t::Zero(input.rows(), input.rows()); - - // assemble the LQ approximation of the Hamiltonian - ScalarFunctionQuadraticApproximation hamiltonian; - hamiltonian = modelData.cost_; - hamiltonian += constraintCost; - hamiltonian += futureCost; + hamiltonian.f += V.dfdx.transpose() * modelData.dynamics_.f; + hamiltonian.dfdx.noalias() += V.dfdxx.transpose() * modelData.dynamics_.f + modelData.dynamics_.dfdx.transpose() * V.dfdx; + hamiltonian.dfdu.noalias() += modelData.dynamics_.dfdu.transpose() * V.dfdx; + hamiltonian.dfdxx.noalias() += V.dfdxx.transpose() * modelData.dynamics_.dfdx + modelData.dynamics_.dfdx.transpose() * V.dfdxx; + hamiltonian.dfdux.noalias() += modelData.dynamics_.dfdu.transpose() * V.dfdxx; + // dfduu is zero for the "future cost" + return hamiltonian; } From fe9bf73168603bbb94183d64ef328904ddaabf8a Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Wed, 1 Sep 2021 10:50:15 +0200 Subject: [PATCH 025/512] Hamiltonian not yet available for multiple shooting solver --- ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index 3abdc7050..d6013cb70 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -75,6 +75,10 @@ class MultipleShootingSolver : public SolverBase { throw std::runtime_error("[MultipleShootingSolver] getValueFunction() not available yet."); }; + ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) const override { + throw std::runtime_error("[MultipleShootingSolver] getHamiltonian() not available yet."); + } + vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override { throw std::runtime_error("[MultipleShootingSolver] getStateInputEqualityConstraintLagrangian() not available yet."); } From 8824ffbd55a1d433f987e0603f614de37fa26662 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Tue, 7 Sep 2021 16:37:42 +0200 Subject: [PATCH 026/512] use feedback policy for MPC-Net --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index ee3c91427..31ca8fe92 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -84,7 +84,7 @@ ddp preComputeRiccatiTerms true useNominalTimeForBackwardPass true - useFeedbackPolicy false + useFeedbackPolicy true strategy LINE_SEARCH lineSearch From ed17e58ebccea28c482f8c762791894593d6d940 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Tue, 7 Sep 2021 16:38:21 +0200 Subject: [PATCH 027/512] reduce number of threads for MPC-Net --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index 31ca8fe92..effbe10f5 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -57,7 +57,7 @@ ddp { algorithm SLQ - nThreads 3 + nThreads 1 threadPriority 50 maxNumIterations 10 From 8c414cd10924ef6266969a707134083c3c6f8532 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 16 Sep 2021 16:24:31 +0200 Subject: [PATCH 028/512] use same relaxed log barrier parameters for frictionConeSoftConstraint as for inequalityConstraint --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index effbe10f5..42b38ab4d 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -238,6 +238,6 @@ frictionConeSoftConstraint frictionCoefficient 0.7 ; relaxed log barrier parameters - mu 1e-2 - delta 1e-3 + mu 0.1 + delta 5.0 } \ No newline at end of file From acb5c2962f8d52d8c831b8940854161100ad708f Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 16 Sep 2021 16:38:15 +0200 Subject: [PATCH 029/512] evaluate nu at (sampled) state and not at nominal state --- ocs2_ddp/src/GaussNewtonDDP.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 0c033c422..cff8b0994 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -322,28 +322,27 @@ ScalarFunctionQuadraticApproximation GaussNewtonDDP::getValueFunction(scalar_t t /******************************************************************************************************/ /******************************************************************************************************/ ScalarFunctionQuadraticApproximation GaussNewtonDDP::getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) const { - size_t partition = lookup::findBoundedActiveIntervalInTimeArray(partitioningTimes_, time); - partition = std::max(partition, initActivePartition_); - partition = std::min(partition, finalActivePartition_); - ModelData modelData; - const auto indexAlpha = LinearInterpolation::timeSegment(time, cachedTimeTrajectoriesStock_[partition]); - const vector_t xNominal = LinearInterpolation::interpolate(indexAlpha, cachedStateTrajectoriesStock_[partition]); // perform the LQ approximation of the OC problem + // note that the cost already includes: + // - state-input intermediate cost + // - state-input soft constraint cost + // - state-only intermediate cost + // - state-only soft constraint cost LinearQuadraticApproximator lqapprox(optimalControlProblemStock_[0], settings().checkNumericalStability_); lqapprox.approximateLQProblem(time, state, input, modelData); modelData.checkSizes(state.rows(), input.rows()); - // augment the cost with state-only equality and state-input inequality terms + // augment the cost with state-only equality and state-input inequality constraint terms augmentCostWorker(0, constraintPenaltyCoefficients_.stateEqConstrPenaltyCoeff, 0.0, modelData); // initialize the Hamiltonian with the augmented cost ScalarFunctionQuadraticApproximation hamiltonian(modelData.cost_); // add the state-input equality constraint cost nu(x) * g(x,u) to the Hamiltonian - // note: nu has no approximation and is evaluated at the nominal state as it can be very sensitive - const vector_t nu = getStateInputEqualityConstraintLagrangian(time, xNominal); + // note that nu has no approximation and is used as a constant + const vector_t nu = getStateInputEqualityConstraintLagrangian(time, state); hamiltonian.f += nu.transpose() * modelData.stateInputEqConstr_.f; hamiltonian.dfdx.noalias() += modelData.stateInputEqConstr_.dfdx.transpose() * nu; hamiltonian.dfdu.noalias() += modelData.stateInputEqConstr_.dfdu.transpose() * nu; From 7ceebe313db0427456c2aa9a1d465458217d890d Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Fri, 17 Sep 2021 14:09:10 +0200 Subject: [PATCH 030/512] reduce number of policy evaluation threads and tasks --- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 493b0dd19..ab850cf4a 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -28,8 +28,8 @@ # settings for computing metrics by applying learned policy policy_evaluation_time_step = 0.1 policy_evaluation_duration = 3.0 -policy_evaluation_n_threads = 2 -policy_evaluation_n_tasks = 10 +policy_evaluation_n_threads = 1 +policy_evaluation_n_tasks = 5 # mpcnet interface mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads) From cad357c58b73da95a0d2af2bfd9897dc67f717f3 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Fri, 17 Sep 2021 14:11:42 +0200 Subject: [PATCH 031/512] reduce number of policy evaluation tasks --- .../python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 696a087b2..6afeb2611 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -39,7 +39,7 @@ policy_evaluation_time_step = 0.0025 policy_evaluation_duration = 4.0 policy_evaluation_n_threads = 1 -policy_evaluation_n_tasks = 5 +policy_evaluation_n_tasks = 2 # mpcnet interface mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads) From 3e33496932bd2abaa905f40649f4f595dc534245 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Fri, 17 Sep 2021 14:14:08 +0200 Subject: [PATCH 032/512] simplify Hamiltonian loss if query and nominal state are the same --- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 50 ++++++++++++++++---------- 1 file changed, 32 insertions(+), 18 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index 9ab38f344..3a8c295ec 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -11,27 +11,41 @@ class Hamiltonian: @staticmethod def compute_torch(x_inquiry, x_nominal, u_inquiry, u_nominal, hamiltonian): - dx = torch.sub(x_inquiry, x_nominal) - du = torch.sub(u_inquiry, u_nominal) - dHdxx = 0.5 * torch.dot(dx, torch.matmul(torch.tensor(hamiltonian.dfdxx, dtype=config.dtype, device=config.device), dx)) - dHdux = torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfdux, dtype=config.dtype, device=config.device), dx)) - dHduu = 0.5 * torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfduu, dtype=config.dtype, device=config.device), du)) - dHdx = torch.dot(torch.tensor(hamiltonian.dfdx, dtype=config.dtype, device=config.device), dx) - dHdu = torch.dot(torch.tensor(hamiltonian.dfdu, dtype=config.dtype, device=config.device), du) - H = torch.tensor(hamiltonian.f, dtype=config.dtype, device=config.device) - return dHdxx + dHdux + dHduu + dHdx + dHdu + H + if torch.equal(x_inquiry, x_nominal): + du = torch.sub(u_inquiry, u_nominal) + dHduu = 0.5 * torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfduu, dtype=config.dtype, device=config.device), du)) + dHdu = torch.dot(torch.tensor(hamiltonian.dfdu, dtype=config.dtype, device=config.device), du) + H = torch.tensor(hamiltonian.f, dtype=config.dtype, device=config.device) + return dHduu + dHdu + H + else: + dx = torch.sub(x_inquiry, x_nominal) + du = torch.sub(u_inquiry, u_nominal) + dHdxx = 0.5 * torch.dot(dx, torch.matmul(torch.tensor(hamiltonian.dfdxx, dtype=config.dtype, device=config.device), dx)) + dHdux = torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfdux, dtype=config.dtype, device=config.device), dx)) + dHduu = 0.5 * torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfduu, dtype=config.dtype, device=config.device), du)) + dHdx = torch.dot(torch.tensor(hamiltonian.dfdx, dtype=config.dtype, device=config.device), dx) + dHdu = torch.dot(torch.tensor(hamiltonian.dfdu, dtype=config.dtype, device=config.device), du) + H = torch.tensor(hamiltonian.f, dtype=config.dtype, device=config.device) + return dHdxx + dHdux + dHduu + dHdx + dHdu + H @staticmethod def compute_numpy(x_inquiry, x_nominal, u_inquiry, u_nominal, hamiltonian): - dx = np.subtract(x_inquiry, x_nominal) - du = np.subtract(u_inquiry, u_nominal) - dHdxx = 0.5 * np.dot(dx, np.matmul(hamiltonian.dfdxx, dx)) - dHdux = np.dot(du, np.matmul(hamiltonian.dfdux, dx)) - dHduu = 0.5 * np.dot(du, np.matmul(hamiltonian.dfduu, du)) - dHdx = np.dot(hamiltonian.dfdx, dx) - dHdu = np.dot(hamiltonian.dfdu, du) - H = hamiltonian.f - return dHdxx + dHdux + dHduu + dHdx + dHdu + H + if np.array_equal(x_inquiry, x_nominal): + du = np.subtract(u_inquiry, u_nominal) + dHduu = 0.5 * np.dot(du, np.matmul(hamiltonian.dfduu, du)) + dHdu = np.dot(hamiltonian.dfdu, du) + H = hamiltonian.f + return dHduu + dHdu + H + else: + dx = np.subtract(x_inquiry, x_nominal) + du = np.subtract(u_inquiry, u_nominal) + dHdxx = 0.5 * np.dot(dx, np.matmul(hamiltonian.dfdxx, dx)) + dHdux = np.dot(du, np.matmul(hamiltonian.dfdux, dx)) + dHduu = 0.5 * np.dot(du, np.matmul(hamiltonian.dfduu, du)) + dHdx = np.dot(hamiltonian.dfdx, dx) + dHdu = np.dot(hamiltonian.dfdu, du) + H = hamiltonian.f + return dHdxx + dHdux + dHduu + dHdx + dHdu + H class BehavioralCloning: From f2ac23c7e5fd9e5b92ff1e63dab6c0436e0f8540 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 20 Sep 2021 15:55:31 +0200 Subject: [PATCH 033/512] batch processing --- .../control/MpcnetOnnxController.h | 8 +- ocs2_mpcnet/python/ocs2_mpcnet/helper.py | 19 +++++ ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 76 ++++++++----------- ocs2_mpcnet/python/ocs2_mpcnet/memory.py | 72 +++++++++++++----- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 23 +++--- .../src/control/MpcnetOnnxController.cpp | 9 +-- 6 files changed, 122 insertions(+), 85 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index 6ab1067c7..d46a00301 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -17,11 +17,13 @@ inline std::shared_ptr createOnnxEnvironment() { /** * A neural network controller using ONNX Runtime based on the Open Neural Network Exchange (ONNX) format. - * The model of the policy computes p, u = model(t, x) with + * The model of the policy computes u, p, U = model(t, x) with * t: generalized time (dimensionOfTime x 1), - * x: relative state, i.e. current state minus target state (dimensionOfState x 1), + * x: relative state (dimensionOfState x 1), + * u: predicted input (dimensionOfInput x 1), * p: predicted expert weights (numberOfExperts x 1), - * u: predicted expert inputs (dimensionOfInput x numberOfExperts). + * U: predicted expert inputs (dimensionOfInput x numberOfExperts). + * @note From batch processing during training there is actually an additional first dimension with size 1 for the variables of the model. */ class MpcnetOnnxController : public MpcnetControllerBase { public: diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py index fc3b95508..25fbf0bf5 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py @@ -1,9 +1,28 @@ +import torch import numpy as np from ocs2_mpcnet import size_array, scalar_array, vector_array, SystemObservation, SystemObservationArray,\ ModeSchedule, ModeScheduleArray, TargetTrajectories, TargetTrajectoriesArray +def bdot(bv1, bv2): + # batched dot product + # TODO(areske): find the best implementation + return torch.sum(torch.mul(bv1, bv2), dim=1) + + +def bmv(bm, bv): + # batched matrix-vector product + # TODO(areske): find the best implementation + return torch.matmul(bm, bv.unsqueeze(dim=2)).squeeze(dim=2) + + +def bmm(bm1, bm2): + # batched matrix-matrix product + # TODO(areske): find the best implementation + return torch.matmul(bm1, bm2) + + def get_size_array(data): my_size_array = size_array() my_size_array.resize(len(data)) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index 3a8c295ec..ca3c1f207 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -1,7 +1,6 @@ import torch -import numpy as np -from ocs2_mpcnet import config +from ocs2_mpcnet.helper import bdot, bmv class Hamiltonian: @@ -10,42 +9,30 @@ class Hamiltonian: # H(x,u) = 1/2 dx' dHdxx dx + du' dHdux dx + 1/2 du' dHduu du + dHdx' dx + dHdu' du + H @staticmethod - def compute_torch(x_inquiry, x_nominal, u_inquiry, u_nominal, hamiltonian): + def compute_sample(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): if torch.equal(x_inquiry, x_nominal): du = torch.sub(u_inquiry, u_nominal) - dHduu = 0.5 * torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfduu, dtype=config.dtype, device=config.device), du)) - dHdu = torch.dot(torch.tensor(hamiltonian.dfdu, dtype=config.dtype, device=config.device), du) - H = torch.tensor(hamiltonian.f, dtype=config.dtype, device=config.device) - return dHduu + dHdu + H + return 0.5 * torch.dot(du, torch.mv(dHduu, du)) + torch.dot(dHdu, du) + H + elif torch.equal(u_inquiry, u_nominal): + dx = torch.sub(x_inquiry, x_nominal) + return 0.5 * torch.dot(dx, torch.mv(dHdxx, dx)) + torch.dot(dHdx, dx) + H else: dx = torch.sub(x_inquiry, x_nominal) du = torch.sub(u_inquiry, u_nominal) - dHdxx = 0.5 * torch.dot(dx, torch.matmul(torch.tensor(hamiltonian.dfdxx, dtype=config.dtype, device=config.device), dx)) - dHdux = torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfdux, dtype=config.dtype, device=config.device), dx)) - dHduu = 0.5 * torch.dot(du, torch.matmul(torch.tensor(hamiltonian.dfduu, dtype=config.dtype, device=config.device), du)) - dHdx = torch.dot(torch.tensor(hamiltonian.dfdx, dtype=config.dtype, device=config.device), dx) - dHdu = torch.dot(torch.tensor(hamiltonian.dfdu, dtype=config.dtype, device=config.device), du) - H = torch.tensor(hamiltonian.f, dtype=config.dtype, device=config.device) - return dHdxx + dHdux + dHduu + dHdx + dHdu + H + return 0.5 * torch.dot(dx, torch.mv(dHdxx, dx)) + torch.dot(du, torch.mv(dHdux, dx)) + 0.5 * torch.dot(du, torch.mv(dHduu, du)) + torch.dot(dHdx, dx) + torch.dot(dHdu, du) + H @staticmethod - def compute_numpy(x_inquiry, x_nominal, u_inquiry, u_nominal, hamiltonian): - if np.array_equal(x_inquiry, x_nominal): - du = np.subtract(u_inquiry, u_nominal) - dHduu = 0.5 * np.dot(du, np.matmul(hamiltonian.dfduu, du)) - dHdu = np.dot(hamiltonian.dfdu, du) - H = hamiltonian.f - return dHduu + dHdu + H + def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): + if torch.equal(x_inquiry, x_nominal): + du = torch.sub(u_inquiry, u_nominal) + return 0.5 * bdot(du, bmv(dHduu, du)) + bdot(dHdu, du) + H + elif torch.equal(u_inquiry, u_nominal): + dx = torch.sub(x_inquiry, x_nominal) + return 0.5 * bdot(dx, bmv(dHdxx, dx)) + bdot(dHdx, dx) + H else: - dx = np.subtract(x_inquiry, x_nominal) - du = np.subtract(u_inquiry, u_nominal) - dHdxx = 0.5 * np.dot(dx, np.matmul(hamiltonian.dfdxx, dx)) - dHdux = np.dot(du, np.matmul(hamiltonian.dfdux, dx)) - dHduu = 0.5 * np.dot(du, np.matmul(hamiltonian.dfduu, du)) - dHdx = np.dot(hamiltonian.dfdx, dx) - dHdu = np.dot(hamiltonian.dfdu, du) - H = hamiltonian.f - return dHdxx + dHdux + dHduu + dHdx + dHdu + H + dx = torch.sub(x_inquiry, x_nominal) + du = torch.sub(u_inquiry, u_nominal) + return 0.5 * bdot(dx, bmv(dHdxx, dx)) + bdot(du, bmv(dHdux, dx)) + 0.5 * bdot(du, bmv(dHduu, du)) + bdot(dHdx, dx) + bdot(dHdu, du) + H class BehavioralCloning: @@ -53,17 +40,17 @@ class BehavioralCloning: # Uses a simple quadratic function as loss # BC(u) = du' R du - def __init__(self, R_torch, R_numpy): - self.R_torch = R_torch - self.R_numpy = R_numpy + def __init__(self, R, batch_size): + self.R = R + self.R_batch = torch.stack([R for i in range(batch_size)]) - def compute_torch(self, u_predicted, u_target): + def compute_sample(self, u_predicted, u_target): du = torch.sub(u_predicted, u_target) - return torch.dot(du, torch.matmul(self.R_torch, du)) + return torch.dot(du, torch.mv(self.R, du)) - def compute_numpy(self, u_predicted, u_target): - du = np.subtract(u_predicted, u_target) - return np.dot(du, np.matmul(self.R_numpy, du)) + def compute_batch(self, u_predicted, u_target): + du = torch.sub(u_predicted, u_target) + return bdot(du, bmv(self.R_batch, du)) class CrossEntropy: @@ -71,12 +58,11 @@ class CrossEntropy: # Uses the cross entropy between two probability distributions as loss # CE(p_target, p_predicted) = - sum(p_target * log(p_predicted)) - def __init__(self, epsilon_torch, epsilon_numpy): - self.epsilon_torch = epsilon_torch - self.epsilon_numpy = epsilon_numpy + def __init__(self, epsilon): + self.epsilon = epsilon - def compute_torch(self, p_target, p_predicted): - return - torch.dot(p_target, torch.log(torch.add(p_predicted, self.epsilon_torch))) + def compute_sample(self, p_target, p_predicted): + return - torch.dot(p_target, torch.log(p_predicted + self.epsilon)) - def compute_numpy(self, p_target, p_predicted): - return - np.dot(p_target, np.log(np.add(p_predicted, self.epsilon_numpy))) + def compute_batch(self, p_target, p_predicted): + return - bdot(p_target, torch.log(p_predicted + self.epsilon)) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py index 61ebd7a09..0fe845883 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py @@ -1,34 +1,64 @@ -import random -import numpy as np -from collections import namedtuple +import torch +from ocs2_mpcnet import config -Sample = namedtuple('sample', ('t', 'x', 'u', 'mode', 'generalized_time', 'relative_state', 'hamiltonian')) +class CircularMemory: -class ReplayMemory: - - def __init__(self, capacity): + def __init__(self, capacity, time_dimension, state_dimension, input_dimension, expert_number=1): + # init variables self.capacity = capacity - self.memory = [None] * capacity # pre-allocate memory - self.position = 0 self.size = 0 + self.position = 0 + # pre-allocate memory + self.t = torch.zeros(capacity, device=config.device, dtype=config.dtype) + self.x = torch.zeros(capacity, state_dimension, device=config.device, dtype=config.dtype) + self.u = torch.zeros(capacity, input_dimension, device=config.device, dtype=config.dtype) + self.p = torch.zeros(capacity, expert_number, device=config.device, dtype=config.dtype) + self.generalized_time = torch.zeros(capacity, time_dimension, device=config.device, dtype=config.dtype) + self.relative_state = torch.zeros(capacity, state_dimension, device=config.device, dtype=config.dtype) + self.dHdxx = torch.zeros(capacity, state_dimension, state_dimension, device=config.device, dtype=config.dtype) + self.dHdux = torch.zeros(capacity, input_dimension, state_dimension, device=config.device, dtype=config.dtype) + self.dHduu = torch.zeros(capacity, input_dimension, input_dimension, device=config.device, dtype=config.dtype) + self.dHdx = torch.zeros(capacity, state_dimension, device=config.device, dtype=config.dtype) + self.dHdu = torch.zeros(capacity, input_dimension, device=config.device, dtype=config.dtype) + self.H = torch.zeros(capacity, device=config.device, dtype=config.dtype) - def push(self, *args): - sample = Sample(*args) - for element in sample: - if isinstance(element, (float, np.ndarray)): - if np.any(np.isnan(element)): - print("Avoided pushing nan into memory", element) - return - if np.any(np.isinf(element)): - print("Avoided pushing inf into memory", element) - return + def push(self, t, x, u, p, generalized_time, relative_state, hamiltonian): + # push data into memory + # note: - torch.as_tensor: no copy as data is an ndarray of the corresponding dtype and the device is the cpu + # - torch.Tensor.copy_: copy performed together with potential dtype and device change + self.t[self.position].copy_(torch.as_tensor(t, dtype=None, device=torch.device("cpu"))) + self.x[self.position].copy_(torch.as_tensor(x, dtype=None, device=torch.device("cpu"))) + self.u[self.position].copy_(torch.as_tensor(u, dtype=None, device=torch.device("cpu"))) + self.p[self.position].copy_(torch.as_tensor(p, dtype=None, device=torch.device("cpu"))) + self.generalized_time[self.position].copy_(torch.as_tensor(generalized_time, dtype=None, device=torch.device("cpu"))) + self.relative_state[self.position].copy_(torch.as_tensor(relative_state, dtype=None, device=torch.device("cpu"))) + self.dHdxx[self.position].copy_(torch.as_tensor(hamiltonian.dfdxx, dtype=None, device=torch.device("cpu"))) + self.dHdux[self.position].copy_(torch.as_tensor(hamiltonian.dfdux, dtype=None, device=torch.device("cpu"))) + self.dHduu[self.position].copy_(torch.as_tensor(hamiltonian.dfduu, dtype=None, device=torch.device("cpu"))) + self.dHdx[self.position].copy_(torch.as_tensor(hamiltonian.dfdx, dtype=None, device=torch.device("cpu"))) + self.dHdu[self.position].copy_(torch.as_tensor(hamiltonian.dfdu, dtype=None, device=torch.device("cpu"))) + self.H[self.position].copy_(torch.as_tensor(hamiltonian.f, dtype=None, device=torch.device("cpu"))) + # update size and position self.size = min(self.size + 1, self.capacity) - self.memory[self.position] = sample self.position = (self.position + 1) % self.capacity def sample(self, batch_size): - return random.sample(self.memory[0:self.size], batch_size) + indices = torch.randint(0, self.size, (batch_size,), device=config.device) + t_batch = self.t[indices] + x_batch = self.x[indices] + u_batch = self.u[indices] + p_batch = self.p[indices] + generalized_time_batch = self.generalized_time[indices] + relative_state_batch = self.relative_state[indices] + dHdxx_batch = self.dHdxx[indices] + dHdux_batch = self.dHdux[indices] + dHduu_batch = self.dHduu[indices] + dHdx_batch = self.dHdx[indices] + dHdu_batch = self.dHdu[indices] + H_batch = self.H[indices] + return t_batch, x_batch, u_batch, p_batch, generalized_time_batch, relative_state_batch,\ + dHdxx_batch, dHdux_batch, dHduu_batch, dHdx_batch, dHdu_batch, H_batch def __len__(self): return self.size diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index 8eb8b1fb0..f237f21e8 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -1,6 +1,7 @@ import torch from ocs2_mpcnet import config +from ocs2_mpcnet.helper import bmv class Policy(torch.nn.Module): @@ -20,8 +21,8 @@ def __init__(self, dim_t, dim_x, dim_u): self.linear = torch.nn.Linear(self.dim_in, self.dim_out) def forward(self, t, x): - u = self.linear(torch.cat((t, x))) - return torch.ones(1, device=config.device, dtype=config.dtype), u.reshape((1, self.dim_out)) + u = self.linear(torch.cat((t, x), dim=1)) + return u, torch.ones(len(u), 1, device=config.device, dtype=config.dtype), u.unsqueeze(dim=2) class NonlinearPolicy(Policy): @@ -35,9 +36,9 @@ def __init__(self, dim_t, dim_x, dim_u): self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) def forward(self, t, x): - z = self.activation(self.linear1(torch.cat((t, x)))) + z = self.activation(self.linear1(torch.cat((t, x), dim=1))) u = self.linear2(z) - return torch.ones(1, device=config.device, dtype=config.dtype), u.reshape((1, self.dim_out)) + return u, torch.ones(len(u), 1, device=config.device, dtype=config.dtype), u.unsqueeze(dim=2) class MixtureOfLinearExpertsPolicy(Policy): @@ -57,9 +58,10 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): ) def forward(self, t, x): - p = self.gating_net(torch.cat((t, x))) - u = torch.stack([self.expert_nets[i](torch.cat((t, x))) for i in range(self.num_experts)]) - return p, u + p = self.gating_net(torch.cat((t, x), dim=1)) + U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) + u = bmv(U, p) + return u, p, U class MixtureOfNonlinearExpertsPolicy(Policy): @@ -83,9 +85,10 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): ) def forward(self, t, x): - p = self.gating_net(torch.cat((t, x))) - u = torch.stack([self.expert_nets[i](torch.cat((t, x))) for i in range(self.num_experts)]) - return p, u + p = self.gating_net(torch.cat((t, x), dim=1)) + U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) + u = bmv(U, p) + return u, p, U class LinearExpert(torch.nn.Module): diff --git a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp index e9fb61e9e..c92450de5 100644 --- a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp @@ -44,12 +44,9 @@ vector_t MpcnetOnnxController::computeInput(const scalar_t t, const vector_t& x) // run inference Ort::RunOptions runOptions; std::vector outputValues = sessionPtr_->Run(runOptions, inputNames_.data(), inputValues.data(), 2, outputNames_.data(), 2); - // evaluate output tensor objects - Eigen::Map> pEigenData(outputValues[0].GetTensorMutableData(), - outputShapes_[0][0], 1); - Eigen::Map> uEigenData( - outputValues[1].GetTensorMutableData(), outputShapes_[1][1], outputShapes_[1][0]); - Eigen::Matrix u = uEigenData * pEigenData; + // evaluate output tensor objects (note that from u, p, U we only need u = U * p which is already evaluated by the model) + Eigen::Map> u(outputValues[0].GetTensorMutableData(), + outputShapes_[0][1], outputShapes_[0][0]); return u.cast(); } From 2a7be47898553055d0f2f08c61ec0b71192cc0eb Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 20 Sep 2021 16:15:06 +0200 Subject: [PATCH 034/512] batch processing for ballbot --- .../ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 29 +++++++------------ 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index ab850cf4a..c80f9d98d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -7,7 +7,7 @@ from torch.utils.tensorboard import SummaryWriter from ocs2_mpcnet.loss import Hamiltonian as Loss -from ocs2_mpcnet.memory import ReplayMemory as Memory +from ocs2_mpcnet.memory import CircularMemory as Memory from ocs2_mpcnet.policy import LinearPolicy as Policy from ocs2_ballbot_mpcnet import ballbot_config as config @@ -45,15 +45,15 @@ # memory memory_capacity = 1000000 -memory = Memory(memory_capacity) +memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) # policy policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) policy.to(config.device) print("Initial policy parameters:") print(list(policy.named_parameters())) -dummy_input = (torch.randn(config.TIME_DIM, device=config.device, dtype=config.dtype), - torch.randn(config.STATE_DIM, device=config.device, dtype=config.dtype)) +dummy_input = (torch.randn(1, config.TIME_DIM, device=config.device, dtype=config.dtype), + torch.randn(1, config.STATE_DIM, device=config.device, dtype=config.dtype)) print("Saving initial policy.") save_path = "policies/" + folder + "/initial_policy" torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") @@ -99,8 +99,8 @@ def start_policy_evaluation(policy): # get generated data data = mpcnet_interface.getGeneratedData() for i in range(len(data)): - # push t, x, u, mode, generalized time, relative state, Hamiltonian into memory - memory.push(data[i].t, data[i].x, data[i].u, data[i].mode, data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) + # push t, x, u, p, generalized time, relative state, Hamiltonian into memory + memory.push(data[i].t, data[i].x, data[i].u, torch.ones(1, device=config.device, dtype=config.dtype), data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) # logging writer.add_scalar('data/new_data_points', len(data), iteration) writer.add_scalar('data/total_data_points', memory.size, iteration) @@ -128,24 +128,17 @@ def start_policy_evaluation(policy): torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") - # extract batch of samples from replay memory - samples = memory.sample(batch_size) + # extract batch from memory + t, x, u, p, generalized_time, relative_state, dHdxx, dHdux, dHduu, dHdx, dHdu, H = memory.sample(batch_size) # take an optimization step def closure(): # clear the gradients optimizer.zero_grad() + # prediction + u_predicted, p_predicted, U_predicted = policy(generalized_time, relative_state) # compute the empirical loss - empirical_loss = torch.zeros(1, dtype=config.dtype, device=config.device) - for sample in samples: - t = torch.tensor(sample.t, dtype=config.dtype, device=config.device) - x = torch.tensor(sample.x, dtype=config.dtype, device=config.device) - u_target = torch.tensor(sample.u, dtype=config.dtype, device=config.device) - generalized_time = torch.tensor(sample.generalized_time, dtype=config.dtype, device=config.device) - relative_state = torch.tensor(sample.relative_state, dtype=config.dtype, device=config.device) - p, U = policy(generalized_time, relative_state) - u_predicted = torch.matmul(p, U) - empirical_loss = empirical_loss + loss.compute_torch(x, x, u_predicted, u_target, sample.hamiltonian) + empirical_loss = loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() # compute the gradients empirical_loss.backward() # logging From 49afddc4a057b8f49b8ad6b1b006afee6a02a89c Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 20 Sep 2021 17:16:04 +0200 Subject: [PATCH 035/512] batch processing for legged robot --- .../legged_robot_mpcnet.py | 37 +++++++------------ .../legged_robot_policy.py | 34 ++++++++++------- 2 files changed, 35 insertions(+), 36 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 6afeb2611..5c1bb2344 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -9,7 +9,7 @@ from ocs2_mpcnet.loss import Hamiltonian as ExpertsLoss from ocs2_mpcnet.loss import CrossEntropy as GatingLoss -from ocs2_mpcnet.memory import ReplayMemory as Memory +from ocs2_mpcnet.memory import CircularMemory as Memory from ocs2_legged_robot_mpcnet.legged_robot_policy import LeggedRobotMixtureOfNonlinearExpertsPolicy as Policy from ocs2_legged_robot_mpcnet import legged_robot_config as config @@ -54,19 +54,19 @@ epsilon = 1e-8 # epsilon to improve numerical stability of logs and denominators my_lambda = 10.0 # parameter to control the relative importance of both loss types experts_loss = ExpertsLoss() -gating_loss = GatingLoss(torch.tensor(epsilon, device=config.device, dtype=config.dtype), np.array(epsilon)) +gating_loss = GatingLoss(torch.tensor(epsilon, device=config.device, dtype=config.dtype)) # memory memory_capacity = 1000000 -memory = Memory(memory_capacity) +memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) # policy policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) policy.to(config.device) print("Initial policy parameters:") print(list(policy.named_parameters())) -dummy_input = (torch.randn(config.TIME_DIM, device=config.device, dtype=config.dtype), - torch.randn(config.STATE_DIM, device=config.device, dtype=config.dtype)) +dummy_input = (torch.randn(1, config.TIME_DIM, device=config.device, dtype=config.dtype), + torch.randn(1, config.STATE_DIM, device=config.device, dtype=config.dtype)) print("Saving initial policy.") save_path = "policies/" + folder + "/initial_policy" torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") @@ -124,11 +124,11 @@ def start_policy_evaluation(policy): # get generated data data = mpcnet_interface.getGeneratedData() for i in range(len(data)): - # push t, x, u, mode, generalized time, relative state, Hamiltonian into memory - memory.push(data[i].t, data[i].x, data[i].u, data[i].mode, data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) + # push t, x, u, p, generalized time, relative state, Hamiltonian into memory + memory.push(data[i].t, data[i].x, data[i].u, helper.get_one_hot(data[i].mode), data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) # logging writer.add_scalar('data/new_data_points', len(data), iteration) - writer.add_scalar('data/total_data_points', memory.size, iteration) + writer.add_scalar('data/total_data_points', len(memory), iteration) print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) # start new data generation start_data_generation(alpha=alpha, policy=policy) @@ -153,27 +153,18 @@ def start_policy_evaluation(policy): torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") - # extract batch of samples from replay memory - samples = memory.sample(batch_size) + # extract batch from memory + t, x, u, p, generalized_time, relative_state, dHdxx, dHdux, dHduu, dHdx, dHdu, H = memory.sample(batch_size) # take an optimization step def closure(): # clear the gradients optimizer.zero_grad() + # prediction + u_predicted, p_predicted, U_predicted = policy(generalized_time, relative_state) # compute the empirical loss - empirical_experts_loss = torch.zeros(1, dtype=config.dtype, device=config.device) - empirical_gating_loss = torch.zeros(1, dtype=config.dtype, device=config.device) - for sample in samples: - t = torch.tensor(sample.t, dtype=config.dtype, device=config.device) - x = torch.tensor(sample.x, dtype=config.dtype, device=config.device) - u_target = torch.tensor(sample.u, dtype=config.dtype, device=config.device) - p_target = torch.tensor(helper.get_one_hot(sample.mode), dtype=config.dtype, device=config.device) - generalized_time = torch.tensor(sample.generalized_time, dtype=config.dtype, device=config.device) - relative_state = torch.tensor(sample.relative_state, dtype=config.dtype, device=config.device) - p, U = policy(generalized_time, relative_state) - u_predicted = torch.matmul(p, U) - empirical_experts_loss = empirical_experts_loss + experts_loss.compute_torch(x, x, u_predicted, u_target, sample.hamiltonian) - empirical_gating_loss = empirical_gating_loss + gating_loss.compute_torch(p_target, p) + empirical_experts_loss = experts_loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() + empirical_gating_loss = gating_loss.compute_batch(p, p_predicted).sum() empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss # compute the gradients empirical_loss.backward() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index b4e181253..08c7dc85b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -1,14 +1,22 @@ import torch -from ocs2_mpcnet import policy +from ocs2_mpcnet import policy +from ocs2_mpcnet.helper import bmv, bmm from ocs2_legged_robot_mpcnet import legged_robot_config as config -def input_transform(u): - input_bias = torch.tensor(config.input_bias, device=config.device, dtype=config.dtype) - input_scaling = torch.tensor(config.input_scaling, device=config.device, dtype=config.dtype).diag() - return torch.add(input_bias, torch.matmul(u, input_scaling)) +input_scaling = torch.tensor(config.input_scaling, device=config.device, dtype=config.dtype).diag().unsqueeze(dim=0) +input_bias = torch.tensor(config.input_bias, device=config.device, dtype=config.dtype).unsqueeze(dim=0) +input_bias_stacked = torch.stack([input_bias for i in range(config.EXPERT_NUM)], dim=2) + + +def u_transform(u): + return bmv(input_scaling, u) + input_bias + + +def U_transform(U): + return bmm(input_scaling, U) + input_bias_stacked class LeggedRobotLinearPolicy(policy.LinearPolicy): @@ -18,8 +26,8 @@ def __init__(self, dim_t, dim_x, dim_u): self.name = 'LeggedRobotLinearPolicy' def forward(self, t, x): - p, u = super().forward(t, x) - return p, input_transform(u) + u, p, U = super().forward(t, x) + return u_transform(u), p, U_transform(U) class LeggedRobotNonlinearPolicy(policy.NonlinearPolicy): @@ -29,8 +37,8 @@ def __init__(self, dim_t, dim_x, dim_u): self.name = 'LeggedRobotNonlinearPolicy' def forward(self, t, x): - p, u = super().forward(t, x) - return p, input_transform(u) + u, p, U = super().forward(t, x) + return u_transform(u), p, U_transform(U) class LeggedRobotMixtureOfLinearExpertsPolicy(policy.MixtureOfLinearExpertsPolicy): @@ -40,8 +48,8 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): self.name = 'LeggedRobotMixtureOfLinearExpertsPolicy' def forward(self, t, x): - p, u = super().forward(t, x) - return p, input_transform(u) + u, p, U = super().forward(t, x) + return u_transform(u), p, U_transform(U) class LeggedRobotMixtureOfNonlinearExpertsPolicy(policy.MixtureOfNonlinearExpertsPolicy): @@ -51,5 +59,5 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): self.name = 'LeggedRobotMixtureOfNonlinearExpertsPolicy' def forward(self, t, x): - p, u = super().forward(t, x) - return p, input_transform(u) + u, p, U = super().forward(t, x) + return u_transform(u), p, U_transform(U) From 5840a43cf4b482a09f55f35e059514c253770897 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 20 Sep 2021 17:17:35 +0200 Subject: [PATCH 036/512] fix bug in MEN policies --- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index f237f21e8..b10aeef56 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -50,7 +50,7 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): # gating self.gating_net = torch.nn.Sequential( torch.nn.Linear(self.dim_in, self.num_experts), - torch.nn.Softmax(dim=0) + torch.nn.Softmax(dim=1) ) # experts self.expert_nets = torch.nn.ModuleList( @@ -77,7 +77,7 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): torch.nn.Linear(self.dim_in, self.dim_hidden_gating), torch.nn.Tanh(), torch.nn.Linear(self.dim_hidden_gating, self.num_experts), - torch.nn.Softmax(dim=0) + torch.nn.Softmax(dim=1) ) # experts self.expert_nets = torch.nn.ModuleList( From 04db5e474a10b676b28a0858e2664580f9a84be0 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Mon, 20 Sep 2021 17:19:19 +0200 Subject: [PATCH 037/512] small fix --- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index c80f9d98d..4ab5762e5 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -103,7 +103,7 @@ def start_policy_evaluation(policy): memory.push(data[i].t, data[i].x, data[i].u, torch.ones(1, device=config.device, dtype=config.dtype), data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) # logging writer.add_scalar('data/new_data_points', len(data), iteration) - writer.add_scalar('data/total_data_points', memory.size, iteration) + writer.add_scalar('data/total_data_points', len(memory), iteration) print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) # start new data generation start_data_generation(alpha=alpha, policy=policy) From f75f64df34747a65776c402fabed1e2a11ee25de Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Wed, 22 Sep 2021 17:30:20 +0200 Subject: [PATCH 038/512] fix bug in event_times_template for dynamic_diagonal_walk_2 --- .../python/ocs2_legged_robot_mpcnet/legged_robot_helper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py index 20ce315f5..99abb6622 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -97,7 +97,7 @@ def get_dynamic_diagonal_walk_1(duration): def get_dynamic_diagonal_walk_2(duration): # contact schedule: LF_LH_RH, LF_RH LF_RF_RH, RF_LH_RH, RF_LH, LF_RF_LH # swing schedule: RF, RF_LH, LH, LF, LF_RH, RH - event_times_template = np.array([0.0, 0.15, 0.3, 0.45, 0.6, 0.75, 0.9], dtype=np.float64) + event_times_template = np.array([0.15, 0.3, 0.45, 0.6, 0.75, 0.9], dtype=np.float64) mode_sequence_template = np.array([11, 9, 13, 7, 6, 14], dtype=np.uintp) return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) From c4e074c84d3cfe417511196a48178f0760450052 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Wed, 22 Sep 2021 17:37:33 +0200 Subject: [PATCH 039/512] reduce partitionTimeTolerance as this causes jumps at partitionTimes when running MPC at 400 Hz --- ocs2_mpc/src/MPC_BASE.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpc/src/MPC_BASE.cpp b/ocs2_mpc/src/MPC_BASE.cpp index ad460314c..a67c5a1c2 100644 --- a/ocs2_mpc/src/MPC_BASE.cpp +++ b/ocs2_mpc/src/MPC_BASE.cpp @@ -157,7 +157,7 @@ scalar_array_t MPC_BASE::initializePartitionTimes(scalar_t timeHorizon, size_t n void MPC_BASE::adjustTimeHorizon(const scalar_array_t& partitionTimes, scalar_t& initTime, scalar_t& finalTime) { // TODO(mspieler): Workaround for when initTime and finalTime are close to a partition boundary. // Times are rounded towards a smaller time horizon to avoid very short partitions. - const scalar_t partitionTimeTolerance = 4e-3; //! @badcode magic epsilon + const scalar_t partitionTimeTolerance = 1e-3; //! @badcode magic epsilon const scalar_t deltaTimePastFirstPartition = 1e-5; //! @badcode magic epsilon // current active subsystem From 689902dc1e9ec63f4125b291aaddbd6553173698 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Wed, 22 Sep 2021 17:49:35 +0200 Subject: [PATCH 040/512] unify how data generation and policy evaluation start rollout --- ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index 9c2adeff3..66596ad03 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -93,9 +93,8 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st size_array_t postEventIndicesStock; vector_array_t stateTrajectory; vector_array_t inputTrajectory; - rolloutPtr_->run(primalSolution.timeTrajectory_[0], primalSolution.stateTrajectory_[0], primalSolution.timeTrajectory_[0] + timeStep, - &behavioralController, primalSolution.modeSchedule_.eventTimes, timeTrajectory, postEventIndicesStock, - stateTrajectory, inputTrajectory); + rolloutPtr_->run(time, state, time + timeStep, &behavioralController, {}, timeTrajectory, postEventIndicesStock, stateTrajectory, + inputTrajectory); // update time, state and iteration time = timeTrajectory.back(); From ecc35b5dad714bcf04a31b28a669da00db8ad36d Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Wed, 22 Sep 2021 21:35:46 +0200 Subject: [PATCH 041/512] fix: unify how data generation and policy evaluation start rollout --- ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp | 15 ++++++++------- .../src/rollout/MpcnetPolicyEvaluation.cpp | 8 +++++--- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index 66596ad03..8661ce3f7 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -60,8 +60,8 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st // get nominal data point { DataPoint dataPoint; - dataPoint.t = primalSolution.timeTrajectory_[0]; - dataPoint.x = primalSolution.stateTrajectory_[0]; + dataPoint.t = primalSolution.timeTrajectory_.front(); + dataPoint.x = primalSolution.stateTrajectory_.front(); dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); @@ -73,9 +73,9 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st // get samples around nominal data point for (int i = 0; i < nSamples; i++) { DataPoint dataPoint; - dataPoint.t = primalSolution.timeTrajectory_[0]; - dataPoint.x = primalSolution.stateTrajectory_[0] + - L * vector_t::NullaryExpr(primalSolution.stateTrajectory_[0].size(), standardNormalNullaryOp); + dataPoint.t = primalSolution.timeTrajectory_.front(); + dataPoint.x = primalSolution.stateTrajectory_.front() + + L * vector_t::NullaryExpr(primalSolution.stateTrajectory_.front().size(), standardNormalNullaryOp); dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); @@ -93,8 +93,9 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st size_array_t postEventIndicesStock; vector_array_t stateTrajectory; vector_array_t inputTrajectory; - rolloutPtr_->run(time, state, time + timeStep, &behavioralController, {}, timeTrajectory, postEventIndicesStock, stateTrajectory, - inputTrajectory); + rolloutPtr_->run(primalSolution.timeTrajectory_.front(), primalSolution.stateTrajectory_.front(), + primalSolution.timeTrajectory_.front() + timeStep, &behavioralController, primalSolution.modeSchedule_.eventTimes, + timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); // update time, state and iteration time = timeTrajectory.back(); diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index d99c34191..fdd1023eb 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -30,10 +30,11 @@ MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string int iteration = 0; try { while (time <= targetTrajectories.timeTrajectory.back()) { - // run mpc + // run mpc and get solution if (!mpcPtr_->run(time, state)) { throw std::runtime_error("MpcnetPolicyEvaluation::run Main routine of MPC returned false."); } + PrimalSolution primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); // incurred quantities vector_t input = mpcnetPtr_->computeInput(time, state); @@ -44,8 +45,9 @@ MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string size_array_t postEventIndicesStock; vector_array_t stateTrajectory; vector_array_t inputTrajectory; - rolloutPtr_->run(time, state, time + timeStep, mpcnetPtr_.get(), {}, timeTrajectory, postEventIndicesStock, stateTrajectory, - inputTrajectory); + rolloutPtr_->run(primalSolution.timeTrajectory_.front(), primalSolution.stateTrajectory_.front(), + primalSolution.timeTrajectory_.front() + timeStep, mpcnetPtr_.get(), primalSolution.modeSchedule_.eventTimes, + timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); // update time, state and iteration time = timeTrajectory.back(); From 9ece3c212da132dc967efd98613b1d34c6822bc3 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Thu, 23 Sep 2021 08:44:23 +0200 Subject: [PATCH 042/512] clean up comments --- .../ocs2_mpcnet/control/MpcnetOnnxController.h | 12 ++++++------ ocs2_mpcnet/python/ocs2_mpcnet/helper.py | 3 --- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index d46a00301..2f93cb655 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -18,12 +18,12 @@ inline std::shared_ptr createOnnxEnvironment() { /** * A neural network controller using ONNX Runtime based on the Open Neural Network Exchange (ONNX) format. * The model of the policy computes u, p, U = model(t, x) with - * t: generalized time (dimensionOfTime x 1), - * x: relative state (dimensionOfState x 1), - * u: predicted input (dimensionOfInput x 1), - * p: predicted expert weights (numberOfExperts x 1), - * U: predicted expert inputs (dimensionOfInput x numberOfExperts). - * @note From batch processing during training there is actually an additional first dimension with size 1 for the variables of the model. + * t: generalized time (1 x dimensionOfTime), + * x: relative state (1 x dimensionOfState), + * u: predicted input (1 x dimensionOfInput), + * p: predicted expert weights (1 x numberOfExperts), + * U: predicted expert inputs (1 x dimensionOfInput x numberOfExperts). + * @note The additional first dimension with size 1 for the variables of the model comes from batch processing during training. */ class MpcnetOnnxController : public MpcnetControllerBase { public: diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py index 25fbf0bf5..7ff833ce2 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py @@ -7,19 +7,16 @@ def bdot(bv1, bv2): # batched dot product - # TODO(areske): find the best implementation return torch.sum(torch.mul(bv1, bv2), dim=1) def bmv(bm, bv): # batched matrix-vector product - # TODO(areske): find the best implementation return torch.matmul(bm, bv.unsqueeze(dim=2)).squeeze(dim=2) def bmm(bm1, bm2): # batched matrix-matrix product - # TODO(areske): find the best implementation return torch.matmul(bm1, bm2) From 5efeaa20b8b0ec04cd47858c92bdb92e7cf88f44 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Fri, 1 Oct 2021 18:21:06 +0200 Subject: [PATCH 043/512] add mpcnet legged robot dummy --- .../ocs2_legged_robot_mpcnet/CMakeLists.txt | 24 ++++- .../launch/legged_robot_mpcnet.launch | 33 ++++++ .../policy/legged_robot.onnx | Bin 0 -> 71976 bytes .../policy/legged_robot.pt | Bin 0 -> 79865 bytes .../src/LeggedRobotMpcnetDummyNode.cpp | 95 ++++++++++++++++++ 5 files changed, 151 insertions(+), 1 deletion(-) create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.pt create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt index 2342d76a5..bf196dd6b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt @@ -64,6 +64,20 @@ set_target_properties(LeggedRobotMpcnetPybindings PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) +# MPC-Net dummy node +add_executable(legged_robot_mpcnet_dummy + src/LeggedRobotMpcnetDummyNode.cpp +) +add_dependencies(legged_robot_mpcnet_dummy + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(legged_robot_mpcnet_dummy + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(legged_robot_mpcnet_dummy PRIVATE ${OCS2_CXX_FLAGS}) + catkin_python_setup() ######################### @@ -73,7 +87,7 @@ find_package(cmake_clang_tools QUIET) if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target ocs2_legged_robot_mpcnet") add_clang_tooling( - TARGETS ${PROJECT_NAME} + TARGETS ${PROJECT_NAME} legged_robot_mpcnet_dummy SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR @@ -98,6 +112,14 @@ install(TARGETS LeggedRobotMpcnetPybindings LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} ) +install(TARGETS legged_robot_mpcnet_dummy + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch policy + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + ############# ## Testing ## ############# diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch new file mode 100644 index 000000000..8cae13cd0 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx new file mode 100644 index 0000000000000000000000000000000000000000..8f544076c7ac8809c6d9793d3615c126e892af00 GIT binary patch literal 71976 zcmeFZ2T)bX7B))G2!bd&H7AJ#g%zxD0@cCQZmC@E+u&R-rH zvT$m!zO1dat6NgJoQj2tbf}8-GA$W<7k#C^A@imNhEBFIRrtr5rK*Zt;If&Ebfxp9 zWmM*=NSsu(X9R}MoHt|gyr58P8|$S(GiL;csF`ZB6BV zIa+F{$Oq1u9~`J7B^5^VrKK&~smM(U3jO(6T}2^uVc@()p8u$?qoh7pEg1(JeFgu( zdBKzIOyzz#{({57<_|dR{tSnm^*?Yp*#3sY{%_*2kkn@@!C|MbJR)Rz=-j|%lO0SI ze>?pnMZdG@VE;Rtj(^T(n6%`w_9~r&mdy`Z82SsHizIx`nK>^gaG~vwc=Z3=`deTQ zzXj&>Hv^Lp?BE~?%<(sZoqr?vS7?rZ4()fU{}kHqoICvn&E;=G`$?qp??k%(Ig#PN z5b69!w6=dqBoJIAOuFhT{@PS+C2juKw)!hL*FOjM=T!ax z_k&7Dn;+mL?f-A-$nuB89c}fMf0MYa{l6#E(e{sc?f#O?pVohn>1g)@ufyNODDci?X~pVr^N{g92_-<8ep zWIEaZB=g^NKEFJcoc={gj(k4crge*!^AE{3O%)Cz*fI0XhAPlAQjM%%9fZ@cvMe{ohs6FJwwOAg3Q4ko`Y* zKu-T6n;+HxIhjAL{{ZLwLpJt*S2n+s>HM<;vj2+?$oXHCAoVRGf;-H`v zL6hyBO_hKD^pBJ@Xpx6ZzO#NSc!%yM@6gTKpP`b%W#?ED9R*T3|av^-~*fA)8< z`73{?U&wX+gTL)x`b*+#U;0bpb+P+{zr(-v?vZIaZFJr}DKUi+oQjuR0IA?K?WxSS>w34opuBP;$FfA#mQ=U@) zkN^Fzj{lzG|8ey{uKs%l{(H~=zqBjkKL35pf7kPWb^Q0t{g12varNIb@ZWp>ztgVr zrR7w9$=W3Op^LL5f0E2_{Fb#jN@h6zDQlBVal6|5%-S67{!P}FEG==g{pT#q{-3h2 z&>u-y@Gm)-Bm)chLCBRO-+xcU{+sh3-}_I;Ujo!qkzkh;$@I9ZoxVb!Y11YR*RxPt8hsukYc{3+5>Tx(=yB7D` z`vPQ5cVb?S3$5=eL$xCsuwif%oQ^$>aoK9_(nd#cONlER_qG(adJjR1S#v?^v@^cA z+l)DmP43BR<>;9>43`v%VAtK{kg93MV+`)2(xY22@MtxO?<>u_4cUeRHa&ska_gY? zmP(?2N0<5zjKg~)VxX&2Ppb8z96E3rqIzvCM%`4xMLOybdtHT=B!!ZYrEVy99Z~D@ z61ccu6I8lBVw$3hr0jeO(QITB-d2C9dHLiN}0c(by zfm%$#?0~N%!Ni&5YHSCMD=kES+9aG~P=Yz84Op@&5H0h3aHz)(GXGemaANR7XscO3 zUaDPy@xh)LI~nNSf^+O~>j)~+T7+h)Eik}<3lmklQ}yTSIQLFGdGTom+};q4ZEyEM zSix(zdwUmxe?up_-@qMhzH|Z?TVvAe>jup7wxSRFG0c0$*{Mo@h<4S4cykBbHaHip zmv!R@&lceh&vxv_kyLVeXg-V_ph43$ZP+c7aP-dJj{8CnKuo)yOx9SQZ@zmECu`?G zSS>i{9I+A#-$9%i$kz_ zb|JdR%hF~0H$!!(2uAPk3%bPvq4VNyOySgVaB)vz-@aUj`rIlZWnmibn3B$vVz!Ev zO*DAEQ7}H4dKNqT9~Mrmj>VY@eeu)&OT^PigNNPx1Y?`uVy)~Gu<p^ zl`Zx%IRW8Q7L$OmBG&TE1XS8~qKobZ*zI_h94g#G9<{Znx8@mfTfHW>Z@^1-v6t;f)C zawra4Y(!mK969S74aSo?pkezum@`3+zt}Ju8!u(SgT>?B*G$TV!%2oPyZd=}4fkX0 z_@sAOym|>t87^ttvrC}Ak0i#-p)h-L3VMx>#`w{r&{}l{wtb0*eWj1tyk*hM@s
S|S<{to6shm`AhG?}`(sO65Jaohx&Z5Cq0=z8Y=@C5r> z5iZVc?}uzi492Ri!s5q$gkfY5oAGQoT$$jAFFOW+it9A2|8|^A%H0ng!V@9XccH~y zpWc3EfVZ#o#M#IDp@nfAW?zngtXoHguubjobN7Q7ci}PVcW)}(9_R=4i{e@Q6cL8% z?13ZY-Kp2@1*F{pirM$x37?)dLd&xn@iE!=IOf$y%xLHdHM2#`ZNPpeH~N^!*l!E2 zR1t&Xsq@(NsyPYwL!8oJLX$_v;tCZ9Qa9xa+thp(x@06`Ol&PNFUnzK1G`cEQ*+n} zR{=*%*u$RQoyxNAbwcgY+hEPv7sPYuewK2lg=M_9<{zfrhAGpW(D;2CJQ2BLc2Zw) zft@nsM08+r3D+QDt_$Y9^I@qylEC85OOkli6xJ;@K&|x7^whd}xYYb842dJ?xL`6Z zsaA%J`M&VBcYnN~r$)7}nux2~^+z8Tg%;Ekz0?b11{{ZetC5v_oeKuT zro&efggFV)RL3_TOxrzW(M>(^y_`B~EIP{8SM7xa$7oz)rN!SRO@%P4nYg)!C7C%* zlf|AYBwB0b=%D$L7`fvS_{Dx;m%cjDukpii*k)DwV&``@@skS8a}L1Ot75^~_aW2R zoxp6itbjc|n=to&DtPd*P;HY5mQ8(Kn8`O*f63~+_QysPZYjZH9Ljl)eHW%}y#N?g74{-n?Z7Qy`X#sbfhxPFX(Xu8TZf>24n#HOx z@ya>W8}&sjYM2TSrWfFXebMaM(|tmw|5psrtb=F0^m+GZugH~4ZrsF5pl`O=W7MjS zFk4Z97dJiveCrBnJyP+Rf*n%RyLfBQdg!}r5T1Us586zuXwidX7;(`WH8v%}TlF?{ zcvJuZH?Fa?h?gQ;UvHSUREj$9xPV$qqoKrOE`~knh6b)0&^@IH6Sqgf^Cu6msI?z> zZ*ReU3qQch(M72L)`-@mhjWV~`(a+`ZfxK08Zh&*eB!N3M5b;CLMZ3uRyGtOCWMK>s$4|Ty91ppG_|XTAEw|&%1qYzqsjtv4HVM&tJyL?j~{NEb{2aUN~;KNsKw5PtM{kVmEPkv!lA0Cm?m(HrQ;4aL8uGlWi^#@$U6AOZQ z?=*kfVS6?9tT-bMJ8HpiUzem2)4S8)W2$uc`|0J*wReu$UCP=)5mFj>8Z7+;B7-U zTHWSBx7xpxw1bs!_niV2#+%Yr(yh?`K`=jgBnL$&2Gapa{rS$Xk=Wd|g+;6Hg69iN z_}eG0yq%vBKY?TU;y_QDS1^m~QhPpnoGLfFF`Zu9=}k3t&O-dU5w!SYBBpE=lLz{p zxxaZAiqAj5NsUlizpRb7S1@Oqi(kVF#T`#NiL?`vz(=iUVV zsl!z6Vtfx|GDq|3mPqb!xi=qr3V6qWo%rJ10=zRqonJ4tpfxe6RQ6MUn)0m~VvVkX zp3Qo0X*G{7j#x&0duPDLHHA2N_ej1}y%s7pGWlb}e7M#*9`7&fPXldZ(LZSiWLJL2 z@*`hhUu_+=)J%kJm2ZjY=y`XlrAsq9CSl68rMPGBP*@h-15K??y02W6$EuT?*;-n~ z96rp#0jsL;TvZK7Pdy@DapNFf)gBHPwTB8%q#UW;w`%-orADXJ3}Tk9N|M>IKWOS7 zy9EasZ#Q(MSvOE(?N$usPjW^>+@P+=_XA z9KZK7muqaZ*u3rBeaj< zS~#TU#`CULp+AqI&3DIegJ3B_k}c^l>v&YtIY*vg353??Gd&d>!S$O7HMFWAQU0G? zaHKwU?wya*Q~|ubd*Lp%s{&in8I?r?;OIjebWqsAF1+o*`tANeUYs_^W6>fZ<;FTe z#nKmFSA9j<Uf|`0oZp?@wjApn#Tnch{Hvf(4ba<8mxZ-ciJgc#_t=)n@!#6k&d2pWXmA_L{Ov0 zwhZT?gWaL_gA#3vE1+FMcJPN=J21U#huG@yJofz51*YFq9_tkQ!yB(B6?3)cvYz`& z#APsyUAexJ%@*Bf!*BWX%rEo!=wV;sg{qE6>NcdAvU_>h#7dO8Z_A_Q))31(J8bC_)2Q^H1?UV=CB z9f+~2AG5R4VogiSE3Bk;lcaX%gv^JpiEgPT7(Fw90YVw6D-;P3AqC+NpF{Ua5%idR z78=ieSQ&8C)1yW9IEN-1`sv+QnDAW*E#lMA2jgIdT`^ocvj|>18sy$}CdJ$iYhYW8 zH=3%Xkj1JE5a*LDer6`d1CA^3fu%1fwr>wkgEZW?&hC%3)1@(HSBluKWhK$`dE=&# zsxHp1@nC*$v&o(BXIZ9(0b99W2a26*A!Lyh_5386z0!5AoGqhJ>D=5Cw|y|C!elv` zs%61e^o%{Dr5vQ3^RmM3{E?|h6-k;><_vjcglg#%w4G6Nodcnv-sRrm#+ZZv+=Y4YWS z1Fjez1P^a^h1aJu+0zb@Y$rX*x{OOih2xz-!E1~ABZpkpVSp*v^z#x28rotLNhiaG z#=+XH8^jrw?N(wNlB$q7+ z+{K?9+DEE~HxjQ~%g}nY0~*X3$asbenU$C1^*BgkDt9ayMwosA4Jh7-qXn4|7Ms9W%mz3*_1dH4T-Wu+eY^!+ma zK^RzR8lYO4*?u5@U6G5y3!f2Vg^oDEcNk=v=Ju8=r9h~})h1IG(pFlCTD&5Kf`{d^VpdS5f>HrWpy zo!g_gN*yW6)rW#B03qNI)I1H{EpTSx0T+k&(2^xcs;0uFXwq)&Qxcf1hO@OypYBrsn*yRA5%B;{oyAvuX zCkU3YAtY?cIx_c}0wkBZ0keHTzBY_yRqbxE$s3!5^+L5^ba=mb?34(>zkdmAgldAV z13eC9cdeAlm`Ghu^~X+GQv8%ohstr$HYm4gGkjNhjrE(mafkhj$f0{~e6VgR?Qr!S ze|~!uoZfzwBz^9WpBHD*FV=}vXYFCMl1%Od`zNrShgS>L(=Ejcm$O*HWfgo>=t|VQ z-jX4f9)j%-ebG*>mI|0kYF$j z4a2H2TUgMWVfa0+2BRvQ$SM77*2n!6>R!2ogJ!H2t55UfbySXKIojcq2oF{^Rs~0O zJHguGwOG5}dax}qL0qx;74a}CVw27)LE)^ktXk;~vv;n8(e_XA-f0Jn9)E$Zl<0lr z^9I_|x(e3ppU+!zPO`_F2_MkD1}YCXS4ss%pjqS&dc3d{@81{&;j`lT)-q*mvZ{i{ zuK|+mS7Fz3zb>^)C_^m)&Ib9uF$2_`>4e z6oG7%RG-k{uV_&0OMcfSpXMvg;p3lug8dVAVeYifnAm9wgq|46I`l7xt%*m)-%n@2 z^xkpQy?7IzZtTiFXK$eMh%&D)4j8{=B(l3%R;Y7B-U zVxgiH^wKC{b~b8I9T*A5gWrfh=JXL+$s~Z6%Mp^`Foq<|)+7@v8_BBX7Gm7l2)u_W zLh#8rRv}$XmU;9bqwO~_>oJL>q2&r0bI=@XTRqr{p5de-(+uMt=(A3@e8etWK8S0x zY|*iyJw3K|FDj1>qiRCFM`+0#+WG!aGU$#AHE@VQv%51vPFtP!34I0~I%Lu$=>fR+ z`$6(@^#;hkI}V~=_obbjZsDY1D{#Wo%P_iO9jSly9tNu`;){Dd;EPlX`>Ylr^xx0O z^>YVV!^iV%Os^Wzy|uo=fCwGd)ocU?D{y8}VhTgPRkO(H9qEe>y{W~%omjB&1eI^= z?UB+^#^c`bGBVOwnqSsv#tRlh;B-$FXH`r8uQ%tTGYO}Gvwb3V8zd_lhT7mFsOW=IC&c*jk7b!Zs{FN zT?d%2?Oqae^(I+b?1)L&s5x@B4tako{a z!%o$UP5ck>CetUVOcpbj<#DL(%2_?BLYFf!;34`-rr++(Cp=n7sM>n;arK9`Xf-U4 zxWiP!cCuVuceY3MCR_TbP0%nfBU|m~iJ$H8$2kk^QRYh|IkEDQcx7G*5si_h?Z;n1 z+R&T#cI)c#;6oUd)^XwbUVA}GqX*x$iO{*n-a%`88QgC>Nf)^xslXU5eS4mciJQmUv>ID{Ggd2UbPhG3~uS>0oLF zT2Eh)%@ai8ogbeF)Ju;DsX=7ox_zwm;X$F?^D+0T*u+(1JUk-v_RN^%7q5Cx6K zcUs{@Uh^KhC+zZX|Ba6=w_jzQZK4O&T)hC9%%c*2g*l>!6a$yTQUVYFR#Ph^C!?55wm$`y>|S};8Qg5 zO%XcY(Bs`~Kj8vnLy$?*;D-CPNl3*t@d@E1E_Jzy_72nN{Y}!?;(H8So07mXOIoRm zWESO5-($7?_1)n?%a$0OCIhXC$lLw)cw~bM)gILcSM(l?L)s6(IF1e6_)ReDXkO6?uTEk_FBIAWLp0;DTNs$S(P6=-z%UL^M7V7Oi%H!GDcFa&4L z3&DNYMuA1g<=}4ZMnWeUphBuOjx4AbI~+2F;gxA*bh;Mz$5@DZHw>SQc13R+H@G-b z4-dHpkO$skc##b8*9qZi2G@hjQNr&ICM>Di>QTN|r> zZ-eJw>9}VkTxM62C%}#s8=sNe-w7xAwKryvr30{nD}b#MjuO zv>Z(`-;qwb%G@tH2nW4W;y3f#@sAejur0GlyngOTR)2ghc0MS8(-T9ynwCp$v-Fmsqk2sP=^LmOebPSO=$$P-lMILh-Sg zE7&hdCidHNnSS#NrmQ-Zg-p1|CP{B)>WiK*DH9u-JG+dfo-^SY=hOM1_pfnL>v%f- z>H|`rr$*)t^Pr=pRQYhDDq)Rv1U;JaTqtc2!TR?dxyQ5;!6sp{>nGVPcGk;+F?!52E%@z*?TrPRUa9vD3ok_}aHsdlm1sK~VhIiNc!GV$1L_Jg%-s}$&%Ka5t zda5+WtxClHagWH|U1vxGw-Q;%CbJy5+bB2woA{L;V=D&_VY|C`K?mEdu;6eqK6uqa zBBnVadQ@WQozKjpZ-30!TFNfyl){iGEnIR{0dB6CMobzuvxqUB5U(il0Lx3{+d?A} zSnvvmsgJ?Hxbaw4ZqDm%`(Wi@F|k|i3VOqrvXIUdr~=zy)7DvR+sZ=lSz*Gz4wV)b z*=TVs%Ll}!pA~J%7o%H%0qy6hPn#lY$c#}GCdekhyDBAW{3@HQS~CttkN1UF!$)Fs zCl#(dq!YY<6iF*>>z1d1mA0>YRNvJQkWdd@ zAIB^Ap*>xui>38;3Nf{P=){C{(m-b6p7SeE*{g;rtaHtI@r25Y?#EB( zlQCU73El;%=sE5%`!adHs7P6XDW8ZUT|0gk5|X-#x~9hqHvQB{$m1j8#f25(Wy`k- z+b8)8m37*fT%ZZBY81)9wO_^Y!f@7kz7&}?TbbOS9>?@b9NCR$$waR#Ms%%Yq_|<7 zDu#84W_|6>xOX?4PkQ$WRuSXXuH?=QUHrQ8u-L6S&wbg+Ht|6%fkcm}7W)S0uuiez)Y7I<`r z;HID}o}3*+MjLty4epCr^BNiPlCmM#TSg(ww?AFZ{Y-pT_)UMqutZE8pG4Z=u&lGS^*0g7<41L*?t@}g~W`@vv>@N4# z%m{JY7k8ok*CDA ztykEgH4_Ed5ov6#{yyS2Rh2APkSAld#j{Rh3fQ&dDuQ3a6Y=)wp<>lhXM|bCJK53# zIq|V_T|5+!AzXPgQ+TR1h3r`=PtLC}!)DjftkPCmEPtv4J7cX%^0YdTC&M(D={N)S zJhV(WQEDo5Z3HrD*fP;nudnXc#hX|LnJ&)D^dLT`HjuM%N@SqRa5BXHlCV{3y?FDW zM&eZK&5o}y6Bp;DRrnMI2xSAKDmHGD6}o^cIlRV@1S;CNDnz!4Y^Sw})D8nHIoC+a zWe1XAss5lXXcN5(DKe#Zy7<*3fqg$J1xHSVi5I8aFr%}o!lXhO7Nix86Ru^jP3>p0 zlbiEcVaN=!FgOAI3I>z>u5-nfY8~8<@<6t8MlPG*O({?1;Z4-id{Rpi(Mv2?xXBbQgv^yc*0pZu_&M`n=(0-4b>|b`smIgBJpK$ z@Qstq?c*_KHfo7j&Q2DK>f*@ao7vJJ=V^>6iy{la^>#(@?=Ep&N>uT zDU#gp`NS=+)qU%PJ~-v|KGv?~2+_=sWzVeJv9$PG>`?w>csap^n0=4I@OAB2f7@_a zKJ|=9?ejKrzv@-RGn^N?clDXU1PYY{sad`oz``#mdlS0)`DYnZ~d#bn>> zM&@@yGTj*Q>+2GKJ`DJ6RW-n6wSjx!*SCUsqdpVv9A-sX>tK6|Hcsjm?bd1LHnIMT zg=n~`C%)V@6rQy|#a1pGKnBtFo_pedui8EZE{YhHI@;0*gX*I!<*L*DKfpea{wwt9uby z`7RZEQcqN~H(`ZE`^fb2f$-?40%(sx}vM#}QEY!GP*)bEo%Y zi*V{@Rk$s?5mvABhxaK5X=%DPO>Wh}^UJc~&bUCKGV>Mg+^-6_H6IRMKF(YMZOGy` zM)ZVG=bpb-1}13_<$kV_!Xkx9Lh2V=etq~}*zaX7YWRMZM9rBB7msIvRl_jiJg`PQ z^KCfU=N5<0*CkR^wRFP4qZw#$Sy$qTMp;F#f>_@$A0SP^Zg%c%`7^F>}u$ zn(=WgfB*IX*4|%B-_NgS?w;GBuUImBG5GKGED1bJ|ee3&$9&z}&q7 z%(`Bi>*~sSM1pAExK&8| z@P>JgP=Y-x{K4dt9eB4narse&VueM@uyE`b!ObU8c&#y#*A^3YIw^suZhhctbl8fn z^4LJ*w|;d_2*x|7bIYCW znf=GZ!g+T^Qr_Uq4t|tuXU{T(Llee8Wu7H9lMMZip6*2ZKGzafd7h&$o21~`5Jw(6 zdmuMRr}SD>8NQ$24g0xD^9>y*@nFe}_qp^#cp|_EH%f*BPgePW*l!&BYS0behKoSz zLkZhmk;PBW&J*|VA%&BI4&fBl!_3SwQm`aytogxy5)n8bXF2F%(8g53BS{%A?G9ue zR_nqC{oQPe{UsuI$q#PGWMgiXKfkZ_6c#^@WkE}((q{`bc+`q;czxB7H=a0+U+*hd zUfO8L&y?PU$41U1&Rl_GX+Jo!fvv@s_3uo#R|DI5>5Kn>llNK$;JVjUu?O@ohZwwTH<1at21Y29vXI z3h|Y{D{-th0{? zY#S>U9TGm zwk(W+_xKF{{Un5(uPt?t&9QGUJnhHWBD81tTuYIUOvK0U%m%BH;fn>imFn*bp#rqt%p zNl3bVlA=83U6rXo@Usd1;Bx4+}|c z-#0|HxgDEK&C#=EA=$7HMa%shVfK)|7+2!o-A};7JM#Q|>mJ_4s*!hbOr`}Z8`;%6GSoOUi61s-M?0Pt_<*V{WbGWcEy8YC0r?f$wr8lv-)rL(AYGW zXcX=sVe#$g+g?l@Tx>*N-s}SEa%Oycl%~h+$m`T6@CfUqd5$QLZeop}XTn;u({%B8 zhssA`ci_}LbC|e30Ef5Rzyo$|gy$1u*coaG5vm_Zw(~OFs1pk5`ZJjCbwBKV!3}2Z z9|TK}8-e_py~5g4gHa)VFL{1T9+uCWElxOd2R;bi5Ymt*$OROTy(RM8=d&4>@AsfH z9gov<=X+DT{B^Xra5!$NRO1b%18{(%1x|L&c8nAIa+>~eWL2~AeSti5lUmG)IK z`_v0^R`U>Eg-7BU&Buj}FEeqM@>dN0yq)(gD2Ds~eQ1+QN4z8d2x?Ni0rto6H)Ubq z7pF+qY?P)azbksIs@OtZ-kpT=2TwB7^`>NJZ$ox+`A*`vQjMBxZAZDBX0&?VjtV2g zBs0|-FtaoSXFM2*om&QBL`)}|8Ju+9Eq&E+y?Y#nunUjh4tyyf1uN2+& z^erh}=|<;YdxUq_J*8hl3aNqSMl`*X1gF;%x<{@UWxh^e!g{2)c4~89jZMVQO@lV} zZ$y2s3Ap~yMVMYL!Z}ag;mCJa89XW@!hR`U#VojY;8^%l>5nzxkwjz9QR1OA9_{X* zWY^lSA>(7C*tg4tWSi1Ca%52lD0G;?rW7qF;{KaqXE#}DRJn*QOH!(YEf@J`8^g+z z+Xhop_bgFn zW8YI&GR@3~KL;3;Y8OnL8qyWo4qK{n@DBzS%@V!8T(xcqTX{{7okam8a5;Bgk* zuF4Recv-`g*_Q?Uc7_br3?ygQ-X>#o`he}rZGze+LJGj*Jm3Yf0(A#OU znAQ6x8Sir&jV_#nl%W-pcI}0WkGj!?CzGJ#YYTe$bbEl9^+z>ox}(qxlHG-mN$1Ul zqINlZneJ#u5N9`#3tP2?GTC6}Y^BUkiq`QjOSI|Zl<}39H!j5){#~en&ONM8G`WyeSR#)6S^3bvk`Tjz*g1@FmKht;yxH1=xN`1$=Iez}snqsomZ4>`co@a%Rag zYCL>7Ry1khiEeWt_Q6G*kv5;&>`!+eu{EAubkd;x&c3Oro|DLyZZL(A!3Xikt|**X zp-zSrO%VsDyb&Dh--)-~(HFhV)koXxZxG}i07I~WmB~w9Wz!BOjs+L6aoho>bTI{{ zOw}h3N8e!{H?zquOL^$pLmoG2XYxshx`{u(e&BAPT83Th`n!kfNM02RiC_f-b#UeF zb1*n?9qS$?*}nef0Qj|qQ5DvnUray13S7H+e7k)Lil$#fcQ+-PVP?#wv%6p~opVA{ zZ5gd{t)RU=45Y8lhqJfizLT%84ptXt!sAhzX!+WeJ}qM?=dlcxz7e!~xSw=3HWA+` zWVpP^0KC4Kf=P2d_O8_>K3cM{tmPJ&VzW*>HGMQ0dEObGWu}v@)zL)R>@h4#>xd^m zT&D_hj+IFTUeIV{=aIIH!~1>}WZ%`d@S%M>>bgZlQ|dd=W3e^pb|VCXZ&(m<)C`!s zKLEUr*Q45!e%xWi0cdp^11TGqi|2-!K}(G{-W^%JvKD+!Ni&g2A6majAX@1XOJ!PwNzD$*n2W zziuE6vaEM+i=9Vy*niyQ4L&Fk`D4Kv@xKvhP?>g52mGVEa$8vJ;U#kdFEFkXv1`jbZQoc&C!C% z7QX1&zL6C5j3nb;YQWqPX6RWw03wqJ)TgJgJsFbMjJN92*AG*ff?L=V*G<0IcX2!3`S zKc)xc#O||U;oak8k+K|pF0~k(&-LIU@`cQ+PGl2wICl3I*pS{n;My>P9U63#wX2#4 zJLyO`Qyj&%6`0a9wWT;lHjc-=^TPp8Ybw{xeucq`mvNYA0jj+<#L#;sD4m!KH;3OR z=LR-YuWKGO#V-Lg?$3w#k6#73YpeL4o@aoM?Zub$IzqmEm*(=_vuH+o4Gi&VCXTXG z{|{Yf9#vx$?*Gy#4I*hy^IS@U&UyEKGd7?U(m+LqNXVR_i71-WBt;rDis?LGoE~-YPQ@=23Jb-|RW%&)JB14}A78+}%?Gn^zXx zoLzz3X!c_M%&2#~V`oP$#94GRzqZSuCDSJGD?eCsfjjT=y-#Zke){j>CJrPCRG()F zjz=vOT=(kbqYx)S-XDM7qudhy_EHgEK)C?@E-XNqrj_V<;~eIl@@&#%d;~qK-_LbU z)hFf-T5!)n57pW1YQ88di}yt-l8l6ACTxQ-8cJ&*({g%Yk!Cgu4mUz`ce}%*no_Rw zv=w?Cyc>mNRU>Ta#l0wzA}wB*nffQIkfjUDY&+N|@E5+=WWbR@&pz}#;Sraf>c-w# zZAC9J?_t=|1{Vo-Lvk&~j&bU2kT=E+moMSqpTe)hpfRN9|HMBoj$zi!*Cdy(F>r2O zAmg%KhSsfog#Y2o=t_PmY&&oX`aMlanExo*m&1T>=XYdQ;|q8dgZbCSQn%|mF!+$q zJ&j#~|86)ALt7=NRONa2!pvf&0uo?1{6&{{QyXvf2ju|doBu{@nHDoo%NV3Z$8^O#LV`ugh?XX%zvwYRyNNXcV%h|JM zH1=c7FW<@d>Q#Btw14AGo~AjGyE~;E;983p&`vZJW@RK0vO*jA z)`lUeZMDJ)b}QBH>1By=*msNqP4U#E^HC{oHLfPs4QFwpHIKHN zALr6_SF@cjbZBwT3+7kgL-=}&lJVomzzn@-VB9&CE?U@)FW<-}E>jMZL~O&Z7Pg85 zCBKk|Lk_Ih#9TbMU>e$9`4q?4xWKQRa5i5-nS=g&TFQg7+j%IwMMcRB6%8=H-yM)=APojAr; z2$Ms7nQeJn`CeW%{7s?<_cm(x!GaE)8?y*9fF1bH2CQ3 zbOGpY;TexH6~wxJ6BNDq?KuC030MAVm>+gEiLdwVFP~;CZSJkm5_HaI1?gtlj#pxb zaBTc_0FL{)kt^5iQQYZ7GDmL$1YTc7ZfWb21&dBGo+^W+;r?;1N2d>&OV7kA1$kt@ zr-1P_ZD0oOUk3%%2f|WC8PReMBwzPUf%UU6S14_aO{GiF1Fbalde%Rb($h$)p8ONY z%v{2(RW~FV2KPwK&Rk}2Z7Q>H$ufAm>nvO!34;^WO0YCc6rW5^2W=N4+8GI*R# zc8WAH>!mhwxoO#G?Y-Fyv%i%*kg>V>`C$UR^GFl^l9Uc3%jeME(htw(*l9UH= zT-xUnY;su(&v{`5S2_S4S~-EUc)A-^4$6S`lYfjfbfTh1E66tg&2ToLo#cdOlFe7T zA;a}K4hbD5dlJ;qUC}|ZBv2V^IcQ=tNd-JtJQXkhIZ75UlclfU7~um+=OJ#&IBdIZ zKIRo%05^jTFv$N$&dPLx?Z-^~^H4R>bd82YTM6dl8ykF~Jqb>YDqzpYV_@Zt8mk3*=R}du zxIU!vtRLy-?dMEu6_KB~19my)hZ3KtATux|$3<2$C!CWQ|9hQW)RfmG<}*jGmW7j# zm6#hWDkO4R(wNEALJwpAaOd2Th^YQlydmc@*_5Kq`M>n$f0?7s40>n~^DoPY$=gD% zJ^whfF8(Ch>3SC}wdx@s&U&GFQ9aCB!#5=Ih%&O+I}!X6ZZLN|by2I;WI^VYD&gnt ziOTvnqh~LYP;Rd*=~%9cFF&7zRIHQ8pG%whXVOQQ=%slKzu^`4(yWznw~auzMXX6? ztTH*Th4G%&5$@6fP2!p~jC!9$LyuH0&Ynj||EGDJl7$Ppb;T0`3%28AUk%PWuZ4xu*gxiLQty*g`${q#(~2^}1Ss&!3}kJTM^c5U zvQNRoTzl&qLB?lgG*q|=<-JK4(rab>Uglk`4MU z9Ot*8V()E4Z2%G9wB+XHn(^q0YB|@t$q>0Vx)FBfJ0Ap1pQQS+U>AhxO;rI<`1 z>S27`+L^_iR+$BVmq+98zrQeP_gCV+lS{b9ejjwmOP&}v-XTp>4e<-njhtzD8`GbZ zjEfwku-~Ie+>0DNGUSo~!|xYx)nWiI2b}Q^k5D4(y#gl%XEm=41Z0y`Mz&?zptSLR zNc)@~ez`e-$##=~KT%@PK2{9+f|o-krg)Aei;6upa9H{ncycxc4sQrUdYet)bln5; ztuqa67>ovgje8)o-m__)%REpW{foER++YfuFXB1d)&t4BOwuYI!j9_Aq}2O48EFPI zWG;zI(=EY%o-$l_8%I>!0^r?Fp)sQW7Fr)zf}gH%rjh~EFuP|W7~9=P=drN3wJH{- zE*`*Qd!6zBN@f51p8fwH1A;2HPUG}+4ak@MRy5GM06pxSi3RJ&@|$#G3BT*5uu5m*Ek+IjwgY`{_(g1r zTR={;1sC5Qz&Es-A?=?T zmS1Rs_tw6H&XXDV`m!QWJrPEl!gqju!3l0NhsfA@^*HAI0#NTf$g@1V0Vl1MBJM;5 zpF5F=qV6um2@eu*n#FW@yX6FPKO!7rUd*N%kBit0wT8ocvP=&AD;?`3miY=t*7~xq^23m#P7|KNYlWNE>#&A}5_{TdBU^p%HJKFK z1zW8vprJ$&UauWyyz6!GDWyw{)TMmLm$SppzYmb8lBW#bSV%?^6+l<@E%~`invHpo zKz>zN!k!;niL76J)5i)!l=b;J*}U)w{&Ok?eH*`=EDGsE;@m+R8r%Y>=gn?;`#PO{ z87;IUM`>%%f%%F%Amv>H+roBXi{T2qy6_Q5MvCEr zXRqOej5Lj0TujFHI55i(r|>PE)nSQ;8SekvgXXR*!>bPz;@Xzu$YJOj%sg|Dh8Ld} zianfLYO^hnbA=*(J)xW)UU7jwiBqAwT+DFhi7yasMA(t-C$Xu#IeY!oAt;K#?3mqe z(ECaSoEk0Syh5y|$OxidSt7<} zA{pm%m!w+RlX>m_gjf5TY@Vq>`V2~Gut^JC+-^cIsmeLcYzw2o#*>6y4{;i>rWK2? zdXHbdIVY4O?V;Z@f?59)HZ*_>hOQ^gaLQ~omdf}B@csn!B_zU{aq6tEb|X5alFZ)J z`~xB0b2xHP4}z+^SiurUdgEAp`DzmOxjx zmNBn4iINIaE%*^Aihj<2$`tdo@b@+w5Z!DJoiZi-;P_(P+7Uu`Xdl60qk%0mtL&*D z)Y+-0xEE@gHc`95JTSdvMec_S1I}Mtpg;Q<->p~z3qlUU8w-Cn^nnscW`DtM{W<`b z7NJsIWgMw{9X~EM!U^8T@zd|BWYdo&u%BlJQ|>Opslr}*r~6{2v)2JO>X)OhZ)Rcf zSy?2*-y6F2i$Hyj0^K$-3AY;ygW@xyo$g8O!mF1qYYF<~MFXW$v0nK;=ns&i5^5f# z=5Pf5>U$7}9MFT=z2`tvU)))E2q}fEC&7F4$a>=l^ep-=v@7jL zqaMz5KJN&zxK+&D8K=PQJtK!}Upu29uAH2S{ekR{xnyd>>B@Zabh~ukDD--v>UuUW}-8SXW@M2&gFpVzfVI0Pvy;a<;|;;9Xk}6jvM@Y=Pu4i$%!SXmvQ2ZjF8%3v+35*QNb zn>miI^}OWhsRlG=y*yKzs0)4Hd3aRT5QnC%hp-E$*qiIrY45c$bm|?WmKVni*r^&7 zRPk&$>->H?-t)+mrIU?Y#wKUO+I5&6?cIW#X)!n$ucL!Xx6u0lU)Wk#N6JO)1Q-2R zg3kRKwDL<6N?oo97JhRmq!~hj@OPrj@gRv3nS@WNO~!tc*21XmXO0h8`|c!#Uqtv0lzc^rg0nU1q%+6vxHGz&ce_IaGly7P<)DeL6<8FE69bO&i%K z{!{Vj_eV&tWgdLq)5&cRHxszN_&|!Kbg|q(Fl0=wxMLsy)NF-OZfFuq+1R4H%Fn$xmSe|i53e*?*SUn~&cpMISV1ehg zErlN8L|0^}0Od7}z~{U$NP2%ltOZ9v0(Yb7UdN zDu*6h6$Lhlk?`<-00cM6&|9mum`8(%Dg4?(iWlez9RHj^4`&~xR;f?Gym>qM8~Bx6 zRY-|1V&>Di5i{Hc#e4>3|M#b0?}_`GOQxZ(3ue9 zsfTdf%5qw?r=2~MGL`aOL-Djt&bS#bg})*FI3+igx;U=H-aIv~qx2@)yRZxHxS12} z#2}C^mZR&V&oh5&Wk~#`b}~iSBOI-n18am7ZHb5!wTPWevm+GooP|8B6upd_;FdsU z{~L$j#k2^D+jFp~_;pN%jKRfD78Z7iu%}TJc`@w?TiC7D^0g`%{T&Pg=|Xk7yK@fJ zDB8h_9%zHOqEpmfu#9cqE=t$L_rpH(GAhW)Mt{W%@Ta*#ze0d4U?Rzt>MaBi5;`5H z3tZ2udeqUrjh-=3rmG`+$tvY*WRiLjM#6H)g$PG9OOS$gEgDZJH)vC}+m`UGsg8vF zT|%ZjbA+m}`%sEL0nUyG&z29!Grza_MK>RBKfR07z1a`7(;JY{{5|-%7{WIy^U*-F zkgQp`!t>H2Andkt6H5+|%x{`txn?c1KXPrf+QaV-akPCG)` zoyF)&%tgF4qym2t*-2imN}>f~v*~B?TEvr*5Ee!piJ$0hPM=;D2)mh&Ae zY2mwmoHq3+u3YRuHC~1SkyL{@AFJuYl+WOBssLSSccqgiIwEx_!mE^=AbY$4S=ALs zzGiAOwQoELoZpGIanhJ=3qk(c6nWpvBD*`v@fnRq(vu@hX6?R(+xz19N3L$fe!mRq zkmysoKuw*ER#}1l?mUO4)#gyPp@1{n*H25<{%PLqpM`jn=JVDx4N#_AoLx8mHNn&8 z;4L_fCR#QF={`W)8*9PNe;LYo)QXz>hlxqsN#@VN_hjJ5Z-hSRA#?X2oO5_7lAUmr z9P{pm(){}nGEo;LBp!nZq43Yub`RxRL#U007v25q2{;SOFK$CsC zKrwDve?yH{_jhq-$Hw7yVbUz`;4o@4S^&03lHk!}TQL2mh!fYP(CU4au&DY58QRFA z$x(*%-77UZ*WZJC^roAHe2gb)I_lh~qzS06W|Z^`Cb0I0+PLLyi2VK0j(u}1sQcn0 zY}Vg3Ex**oS}c94S?R%2Y#s3c>u$_q!x zA7ORvXi)C|gR6aFvGL~=uBGbb+0YbmGpJ^)6U zx%i>n2=m;PMJDThlke{}@ubjU20JI z5_Qy|asrj<{$)Hb*OSE$jks%*UvrK3Rk7xtIMAAELw?pRAmRqTsPD%q(r0LjZdUgq zve}8ACl$UFcE#J42(7t%$>UaY_m0%M8(1Pa8cX!fGyoYaF zPjZ?c<;B`Ayo(DJrQmLN6Z>oGUb3`uDSOD{3@-iENj~4-$|%uOunC`Jjz^qku52E~ zUG?kmebWYLd~gr1dF72eqRg2X_8CzduZwRyEWp%S4%b|?!lvIe!1hcV8PRDV$5)$^ zU+uAEL!>91a~GDDtme>>!v(bSlneGqG;BHAQ4Zn4@5$WWYC2ouE`9T;oYV~80Jj@O zw4+eQX=W9VEF2og*0_qolQJGF6{!fv1X<|jzAx;vU~eejC_`7;<>J2&rLj_3A}p`E zPNc+Q!GZCDBX55aUEcw;mOmYLtu7_+8t))9uN}S0H|1tEOaP^MvN#}9gzbua12T8b zsOZNH*mow=qWM&sTJtH~kuhqn=+ma3eoC@j+BDj|Ro&^OcpSUocG%>w&iidZ*I9ZTd4!=?+uJDqj|eSiJ}Ox)fqZ4rLRH4Tpv`!R$y4c!E8jNFQ;NR?2$ej3G zAaOheRhK(sgK=`4`@KG_y<7{QTdTlmHjE$(WDboN0lUdd;jk?Zoa{0an9#WkI~FI9 zyJ4&Ne;O|^$s?-BHun#z5lEqbv`kxuG;fgFy5@{n5PqN7k zsVVFkX?ciS8wb(lf8e3d8aUsqK&76)Al?lzD5~;4N;e7>daJKu3HvBeK2!j&l%vsU zg?SLd4&%k`1*mUkHx?~*&Vj2&A?&YTKJe+L29>zhSgkjvfTy;HQUh@4~p=pm=P?=ZpKD6kKk>& zEASJ?(;9)DK;loJpleT1vuN;r!S+xsp5>D9%#R9_X0an;f)%=T{O!h-yxJpAE=0ye z3Krg<$uDXT<e)5ao-{mdBv(kFrlR0(LF_+f4D_fkd`~b*9@2Chnkf0 zgc%#h&Sw+&o-zjfiOz}qF@kGCkXq3^FGR#~_w`S_V-;TfhMJ$vB{uDX?3X_qU;p#x zdwgO!rR(DaE`B%oua95i{favwFo1XbE6Q&Lx}hrkAd_L<?$IN-wzm5*r(CMM0E%0~pz37rB1q0|1b%1QEQAf0?j&JZjl9|U^_zljl(T2wo;E=VQwAaT#O#CfI=~6mVmbxEKu2e+V z{q^909H5HLaZO@2&O|!-9O8yIpj~1P%wX~bwDDRB($3K(?SDc@Zp0YA$-oYtQuXvzowWff#`qOOQjJ|lm-=5uqfbiEo;z)dVB%|g!7KA+{NwIz{Ig?J1!;}Rf}x3;j744@|KN!L z!MNFTd5w)l{7TDefkMq>W^MCDfk)M8!9A6!{0>G};MWr?kPGH`lArH2PxjYh8Z1Nv z8{f1y-yTlo)t*h@rMJ29Q!EPkt)_WR%YQ#1mi$JtD^neD(f#Cd3MaVFCE zJc)GH#WB6rQsnvV7bu!{jm%B_CKy|B5^}yZakAeDmd5 zY+@v&+r=gLWzj7>;C_arKQg5!u`w3md%pi9GXgzXgI_9HA7+#L~)0KO6B z0WEf<-xyLeC&5U~SbBI?6^K32f|iTDq{v6Jufb2+R&e+nJfv5!1o+}qMBW^V4G40NH#F+A-NSOd@_#}y>0`0Cq$vf=s4EQ)|gF8 zmt#GQzA}*yUkeWES)kx0qv4s!N zG!7TrfqJ*$CkNScvo=};WwQl_CLl!CT|M) ze)Tz8*A@=G(e22m)RM$b*@WuFI=Ri$vr)+LK5}JY9n7e*Cz>-w@zw`^c&)?;PTu&M z95`2w?>5zO85edz`<1ES;@}Nt=b-%MK-#hQA5He$Kia}W9ZPr14y*O5Dy0J zC*!Bc!gu)@P!Kzhgr81<^}i%hr=>e`7L7o5SvpKsM=P1E{|jvx{*4wTPa%T9IXG(b zTqygpgT8(J42jEH(RYhpp?A)%jB1)3K0l3zop2KKInWm@zQk}*4(~{n$rZR5nFmF0 z-gA|{3c&44K-a9>@Ro5JY#{aG+!6L%PJu+)l5KVRTK%5 z5rgzU{xEHmJgA$&|UgsU8O7xN1oA=S9>B~9i z`e)4Y{0x|UAqEB632*GnAy3s0ayJdF(Eedl*wOU}*$bv4nMq@?Lsm5BkQ<8w zsdSKc0mhAn^<5t*O;(=fPaQAfsZrX9-4wyBqC}rM5^zP<# zJYi)ZX*kzGPMm1xOulSEDpg9L^JW}AU`UDbKSO%&iWnGePNLem*0A}ICDi3SfiC6w zpt{9``PE&^Xzw}BJ^JnnKPNsWvBG-``>%r^yWu3h@VOEtjGqNXcJt}{nTk-{c1o!F znSy_~Tf()Z%Ve8V4kP|0PY|4bnDgf(aOMvoW#o>MC2w=k_};O|g)f7WQUUR?#Q4*n`?a23r89E2oscbe3(nCTENuHPDc$-M992L_1tv(FqHfo(2f4t zr0xAo=Cno(cfo$4pwuUdtjxF}IQv%|8T6_$C3Wc}>x?}5Qt?qxZZH2E;0V$mx!7$SSqJ@S=iUjC*2!`@8JE~;UC!PO_3Wr1-H*%Oct+I!%H$1 zP?c6Jjx&D7{jXH^zwg=qNo4~TNy9r5$Gy<2LpS>dm<*phPW_H04Sn!UutWYR6X&i$ zd-pGZ=Me|V)Rrb3xNjA+;eHB{++;*v%rGObaxNj02g#&!=3DMt&=_d_EsWLdInRYZ zV9Azp)Bnq0{9nuI{|A-zx^vZ^Y5b!yc^WyKH0-CF-){=LMp;a!2Aaqj!|CUh#q?Y7xgZWcQVw@js+-J%j-I$D*3}@6!;pY137r5g9@( zOzv`vybDS0Ee|TuFO0`#Uq#+>v1rk+1mPJO)^W>Kbhe|)X~?gxWq!$2Hu-iotZ|ox z?CZs_PdgcB4Q12B`32PG^blzEy0YUohqBGC{rKZ2xfZL)llYzf2hM(#51U*mOE13G zVJi>&(f!I7@r77Ki^kp};}e~sJ5ichvf~6kXx)tVOY$M*g)K8(p#&s-b;vt~tteh# z1Z%g8v1u7sp;+o8wCN?b%#zi0lF<})PZDz|Pg>ZAExd%cocsp!bq-?IQwe7Mz61sy zKViUI9)CADLcd(t!ye9D2p@DYXq*$88z)FnIsNA-PVO8$d2t__vn_DPl%q)Y$uTs? zK@Du|r*mtS^zpA{p7`-fIdaamgcSc+kLFGvK(DP4NW46P)9Q@qjH(j0@cahaJ zN3VcfnFsFhUCb(#pMgw9N6?%r_yu!M}zNH^$?I)v*diZ4{*nN>S+ROlhv zO(fZAI1pH`lVIz)8dj)ff%MV0Bx=MKX`zeAZKW#oeON@+=N#px7taA1$(Lx2hA?0K zY7V#I7wU&{rDr!F8n(ow^%D(s^!S>p=<3wpA zYMbE0ZZT4Wy+PB+yygl7HO@4tE?wANx(P`ST$`#Jp(dOzZa(2&V)*(4ajWMex#?Tjb~o$$BT#o zyZP25s4cQ-x#ExMJWD@txg5rBJy#CF7gNaP-u+OcdZ49h;&C=oN0!~1FW^qk-pX=T z+AZ}Xp6rurm%+2a6(8_+W&;m|f*{~BqZ}vf@hm@rRT_TsSFI0X!YY(-PsCSJc54!F z$6pC{y%dKIwOo9xW(8V(n!y)C_Hjo-RoR35X54c39+nQdKwsrl(T?Z~^i${l|4{GH zpMHIKhxvUNGaOA?em$otV?VMi+Asf!y7K%!XZJ1c-@n9 z9A++u!}lD=fcRMtwy=C17U0dGwTxJ!= zzeZyGr--jCPY-XbgHYc?_`%c+xVKe}Eg8BB{1zW5-D(FZ4wzHQILyY2JRoaV^TA@5 zK1AqQp@>KutYceB!p3xw6M1EL>^^V&deLg8TK_HTwfRVE=N_QDw%bOc zo*Y%M6B72@)-6?bODVdz4wuI4r%TNZoJs=Eu~$RK5&iU1%#ZVMiXNTQGGXF=YF@O( zNw4)OEwL+xHC|`gd*jsDgsrcz#Zw0-qtBPovf4(}dtMex3Llxp9rpbS_7KJ{NfW*2fF0w-S+Ae7GNw2M+6t&>LGtv^si(^zDK)j3OhQyarFQ(&m0l-O|TMr$|6%6GQguhGKjgKC<2^Hq_owjjjyd3niH{ za2#dgtKLaOuVX!RE7*;naQNq+D(vdLV*Xg^ z5xPr}1V1oF9nzC zY{$v5tVQe)W^`}DtB~0(7gbc~f~s+}X68S{`(r@E=M|83!vzrVxq?cpxrn#FeF1us z!Zlu-Z^8U?`EcQ{K1{R=BzE_@(2?8#CVguub6`seTHk68qSA&~^&AW7p5m}o`Yigx z?_o?gub~st=5tM@BCNBXJNv%BfbOl1X7eTIV%LA^xT@{~Bl*UeO*naimcDDGb&ZOw ztcfJM#{L}lc1<%(=`y7|_lH5cvK*Vf?<9SpE?gy{=nh8PQ!$P^L|lUJBafYSD9^%= zSjemvgyz(eDXDuD$iYFXiBK+LMF%(ErJ8mM zbVr3Q=vGvdVC_q6PN@R~&g(*|x2mYwbxqbw{sMRDl_EX2Y%b^Y!;*ft<*@G^- z?~|Fgx}dkF3bom(U|+#K@_XK8aw|a|M?JrQ3S9fReUG0o7RQXR!|kiczfe8hZ3(Q4Fm(97bDb@Yz)@Ijp3@O>j97Oba~cIO+YdX%YI2aKo{c zY^~-j`uBpcvs)F7En=Ub=KTtEFxnPJT-b;G3I@^N@8eks(MUKxF66MQsc>IVy*e;X<|D_7hJ7s`;I1_~fX6K^|uXxnRHkrnxj_#a^!4`57L6`x?EQ^oa>Lx|)R5wKmgQW!U^<9oL)nlr&YxBYuPydtr7y zCD?Ck7`a`QjnX8tfGH8>)*Z zwb<4jpwo}X;j+p*s6y8VSKG>Cx>5($cN-Cb^(7cRW&+KtRay747~=H7mhHZ+1o21R zNzI<|m|DIg*H^ov()VA{MmKx>@Dc-}cgJCoxCP|t_#a56Xft{D_ZOF_IGIk=RwpZ{ zkV^Nzq6z_nteB?<d+=JUqX=0#OMm2Oyc4#4n5=yc+F0B^cbz; zY`?yN5BFc83m3bHT%Q~oFuRU6!4&3u#sU0v+leOmVKF9U{yXlt?jW}&Ad`7))<|04 zE+(~oF{s?>IpHhqM}~#vq@jKyRp56DlU$-~qUj>+yiKSODpG6l`z2uK?fU~ewI6cX zH`1X12H+N!we% zuQD7vY`%%DoEp)v$~~|Ox2BVen#k6ibude$12t={A(65kR4sTsOqQC6BdW$QH9iwb z;p4Svq0Jcldrk?t>@$V^{Hz{@f9c|0G&o{j^Gv$%b3ENo7GRauvFy~~qgY|-SvJDa ziA_Fu2dq|lGb;Zok-_BicvM52E)B?Fr>!z#_4aDQ=hzFFtv|@0AN~*T;nkCk^-u7z zUKe)N0}pyr{s37g95rv6txpXfwUREmQFME~B(x|x;-}*~xYb|Ni0;pDQuVTk=!Inx z@ynC&Tm@Y)+ZMpyG!d{e_jltn=!DZt_cK)bMIDX#;Ys%jF0v0|g_{88N`qNdC*v<$ zNULK#zk9teS>gjRx@5T%MV>mVJ9?Eo&sI!q#R^c9Kv_ zq}o$`Sj($8r~H(FxsWEBk3!ZG@u?QtmoC?%JkI6>J>IVSG3Iu3oP z0CS`Qgc?k3?%Ee0azZNw9iMCr7k?Ck%BL8buVqFv2erxV<^Ql)gjh@Zcv~9Ukpwrj zl5lNV8Q}P_c+gga_OvR}2dZ&Mr1Lrn2^~w9x|QKc0rQYVU9`|Q@dlq!zYb@7zJsq6 zWhM?U0JVZ{A};O0^~dIMb&HCKIe#x42vWj=p*CWAs*<@ktpe}4T!thJr{W($Yq*{5 z59$3i?s!p^8Mt~$x5PWL?8Jkm^z-sE`bE~6E{a*ms!M-_@oP8IC{D#GXl)LgWgUW2 zlZ0XR*ll>uKpQ)DNiPUQE}+xTKj6}jodEyFw_NhZw9&c_KRMS)K04hb6`PW{y>G3+ zxlbSG^Zao1>^ww`=Yyz15Lpu?hC4NMnJpfFiNtj)y5vy`m7BeR+PE};!vwk|+RkNVRV>9Fmtd-!YUMo)V5z`*AuJoT3r^*J;PsjQb`JL{|Q z?c%*qUfGWW-kMOK6Dhdw{5p7~X+x$g9wp-|T95=U6Wt5oL8$u@ST8zF=oXj`-yD8( z%F&CUD|Zq4B`Xi5LZkGzkMrSc>PgZ$-JKo69@OyrE$m>t3BO%sO1FKKWmUpo!se!V zP~jX3CBdf{$X*1qgm?IpTOPvhw6AFAt=BlHTA3z4lNQ>LG-;83h)Bw zl2_lq6Jo4|%bpc;TU`s#@8a=r)q`@I1FtZ@Z?+&~5n+}p)E*7ynNp|0VLacfg2vyx zgO6*6vGYaV()s{-N-uoD@gGNEOCBMAnhw)Ta;0?XxheG8*$n*jiLUVfuN(f2#L^?e z8G(kKaE0#s44Sh>nQT*d3L=+};T2&w{`aozf6qbwlgdu}qzV&MW%0D3SZwcTj2-SO zU>jkpU`_Q>2n>_pe$2_m-S2g{V?JleO0lCjMKzO%6zW0y_#hlJ?2Te)C_zh+pRfgy zkA4}>fa3{QxpGDl6-TdyF|{T5wBj@3F{@heI-?ftOf|&9d8676qOBmd0PrBQ`~XIsRpgo9-iiqZ;Nv{p8F)^+SaVy|S)Z`eY$L zdZJ77a+g_xQ)UtKP22dWzMJf1~dE7OTp8pZ>Wp*A&TT>9+QO*(d5wgP8?-c56=you#)5`WMA1p zI#V|ggHNUqTlkku<4MxI{0!z_`%NtSNe?T%*2i`mH{lE8qTqr^6TEe|rR#dO;=S)g z7%Njbm|a%|PZh7=6+1wL7=E2LezYq3kIJ2;HTSUdO>mK)T^ZQW%! zv|txZORmPVj%`O)+aJO>`C?q4X2v#unm{eIj&r64dB`BWkLV0uf!~#vNFZH?-0O|$ z%~Hg9Op(R6M09C9Jx2luU0HQs4}5TaHa0!x1O{{7pg4h0#C~NwY2fVfm_$2}KPtkC zM<%lG%)a7IeFs`w7E5l*9|F&3^0X^40=e#-Do8I3Wq+on!^f5Nm?+OD27k2Zdeg~3 zcFlpf)Cg9segp-&O~iddJ)MufESwK3XMRf@z#oE_!n&mrv@qc|lDyW;1)mhBGn=pB zm#v#{T=N-ZKe-b0Zy&?;vqr(;(@9d2X$_B6ZV4@piom=YPqkQ%&8v>k71 zaCgJ@2EJ8u*6NTy&1KfQ=sEQg3c!p3JBYS!x{M~XG-)wwqGc7k8leqOx5j#XX16)$#P1gE54;oVhcXuaKJ z`tfusThe+7#0{Ns{8|ZW_2mju8kS|Zh&W*KRS#RNzXIHs7;|&N-5cjeXvts(00Bx_msYHl4$^N^67f#$?=_><8IO!Ylj!EaEvS zh=@-cBB}FJ$-|`CxOHO>YJ2b<%~{-s`nN>mISJbdWQf77CFR8En6MMsEQdYXHOaDf zvzenLm24{71><}klYNsGVu@?R=uTri9*sDIkN4X{M{6^w^2!b;ubkMO&9G!yiO7F|NGKV1 zbgt{8$HBJFFKg3(HI{15c!ogv!~_n?)SE+%lMY|M6r5E{pM-@gj6UeLGI9 z*C!_?y~V?}9oTnb4!I)Se7#`D!S9`l;pnVvIzvx})zQ+y)#~?f!M`4?(Gfw! zM(6SU7H<^9#!tn%%k}WUn6zf^v<0wFRSxeM(1&E(O_<4>2WuBfK=jiRCc0W4^;r0pPJo0Pyn5z+UkZQ1#bFFz8tfQad#)5FTk4qfa4l?@I4_8ax_qeF|JBLn`f%@%=~)%@wp?({bK{rPFg}! zcOoG7ZG>(-$;R5<7jbpob;9q@BV41ogzQeqx@OQ$e|RV(sk^(?@J(e=}(Zpw-pTw+7p9`I|!o- z*XrE7Pa=CSIlcF;KL`;e3dPSnrKbE{h?iZp`t$8iDV1w`P~pX=V7llQa(WIa{ynk4 zDK&|G(LtNKEMP!wCg!M3*4)$w@*ko4xt(}Z5r|BL4X9d$Ji;sTk-pTK4)(G9Pt?^# zgQ#?41*#+%}dA%HG z?2Lysu1z3xEDg;Key5Cf~mWrzNOR zT#1)}uctO<370~S=SFCSrxhwY){4~JxnaIf6)}1s3M7|4MZYQ@5}VHtP_{8K)b-E% zi7(e>z|EAuMCh$5>Y`2pp>R>S?kVwzvY~>hXUXLR%SM-duCoLkni4>Z@0Wl$wE~Lo zAV6A6{E(s6GQ#ZXb_$-P2z9+;6yx=f$PsR&=D4fbrUjZ1J=;eawfa#GORZtU`D((W z^aRL0EI}-rOsBq1T%>{v5v6&k2$@dCP?nFCz_NR{S#?j7iQZe`)E_Swmi)eQ)?ooo zue0_VvM>ELs%%~i7>u0+C=P1L`MnB4B+V;Pic z>@CW^_Xsg($pd`ftw%Bu1o)CsNVEsN(zlj4PSvel4$K=TS>JkO;IyF|@EwVxx^u%= zwOZZOcC%BI`2K42S|gFIE@lUQDRE)p6_rE;?ZC1f9H%7JLZ}8$OnmST2BGn}hz?o> z{*6`NRkrOzv(dW;mkyq1<_!Dae6d$o1x&Rc@!^q7?{^I1wIqksC$B1 zRMh>;=v%K7wb}Oy5pS`EXwxo3>+B4vYX!Dw*mRQGx-F3^ixB|F>eB#!z8!e^%n3F4 znBYBYF2E_yXT*KCqrht+k%$P$BKBqkutPYWBLUF>>W}bgLb~%J5Pr)G_7U4a*5p#G z#qky`vXFqB!OHlUSvpL8yA-X}&q0yDyx52JB7pV)4{ER0gmiTe^1lwz|ES9T*PQ(y zRrZK&0PDHjFfn2V=u1qnf8*m&V)Qff5}dHWaht^ut&|IXR(_?vE`I|QiY?*3EiIHa zxtM4$*n-c$F+vX7p`g^C7ab<6iKaJKi6^h$u(Q)&QUN(hgo;%z`d_lL+UlNeCypKV z{co!58rMwV6lX@JY1E*aXTz)`#`#Dx{5^gq8IHvDFQL+9958IV8#;Bkk@$V26SPLg zlDkhd(Bs5L=zdPSe!b{*bnEUt@SrGyHj2AQS7#IC#Emf+85T;W2F#FoS1!?YL3?P? zrGc1x<6hcg)gQDL-NHxL4->uISK*jm6sfo+3qNaFNngQZ*sKbHQxTE)dl(DqoV^NC zCpe&T(hXutaVe|`*2TtKIq{{9Jydi5E@-&V2=sMK;opHw5ip}d99?{u9{5rPAI~X~ zf<(!BPGJF;8`t%!ZWR%7?9_sMXGBa$uWeCly z8UfC889;m4XKHa%2HDy>0~?iEas5Xw@)J)O!5?uA3qIE%57hSH1T#5O$DITRQ)@}P zw(Z2sgfrbWpiQRAtHFS?^Efr$0D6SAFlL%UT5_QQG<7w=Lw#r9VE+Tyw!;9N7&pOI zoazQen>T<+DRmr}pG-)#1pt}e-{90YU(6%gi#D!7^o|N?I3+>i7e=eFO%?|>KADCa zvKGj=6LCOvnz?yPB*V@tjIyr6Y%JrCVcyw%=+c47wrOh=6ctUnueXNnK4Ew&#)Y~o zBZ}w$+R^U|^f1xIk5wL@fRtetEpzKQdq(aQ*kz@&Dsa`d1xk+T)Ng$;inGa2xZ}_>SljXohnO%_ zv)v@RIRBIO8vaUl@avJ6PnN(pv#BssO^LRWy@oC3Gf<*hHfeG*lfLR7g1879vhPec zhEt4*^Ttx*@dk_&p3V`fv=sJS9e|z$&w};(R`7_EBf4{}2ok}ay$(!Vz5-?@7ULb> zoCGKHZdb--fyO~Quxo}FTJ`xbJ$j8yzU>CMa?pX?{LF%G8<+#HxqRw}xKis~&wn6i zbo=Pn5=&_n4P#o%2*E#=USJ_c$iP%77KCgrM+aKmXaPmxdXJNd@P3Oa`IVt(KXthb z6RevtZ*~Q3{?)U7Y~wm;@Oqdi+G>P5>}F2dzK*i%5TxJoJ^kgQU(+{`W!aZ%iw#uOX-(6%r&+k zfb?%%Pkt}-rf(JHVe=3*IMBmT$1irGzZu8KlVz>wK*CP!UnNW0kBNfmv>NOivW(3- zCyVgredwLDE|PzD8@c@|AykgKz$kx3tTj6YivB)jYwGMrL(UMddn5tYewU@+4pFdI z$Oyju+KlxJFOkQFE0O-Lt@z*yb3C7&hYA)O(P)#hL4L<~^s+gco{TR7TKD__ztAz_ z=U4;E6o|x4&H2zNwH0l0DS^{qF>Sn0k`N4ifC83u(2^l`(D2$vp#8~(kmwJlI;Rgq z4QqM4=5{LxESSd@a$DOWUL&&{<{sctIY70@6q(ZaPxXG$!J_VuMhj?bKufY=KoNk1UZG=L-Rs1 zkbm(KQev$s+&*=V{`KJ{F5of*7Xr2M>^E)l=#zCgL0cBiA5g+ImKlJ*)ByEbhXMzq zjmW7QVaYlrYB;J;v0!043f!MWIkzD;Z9za_(qFWNYx+fpF-tf>h<4}UP86) z*T_5Ru`vII4?4->G1$AM9a#Rj4?Fn}lDZ8ngX4MYX{AVK@Zj@xy6x0abfDuOYv_e3 zw7U6_vJsadKEE-5xgR{x&-@VBs2>YH8d?MADW*ioiOXOji3N; z#vnzz3%CLNh9awFV33(J;FVLL-GVux1#1A<{^rJ~Z8P92*DD}JL=jsrmLQCETksVl zHD;9|g+d0jpdHh(`-ab&F3n$K@KE3dIWoQt|1K-W$0-f+QByrtu*U^GH4MkCNy=7V1}P8;gH4y&83Ct5PnYloWYFs zj`opW?Gzmqlt5~9T0p$j75^TK#orH!!-?TY=(nIqTSlG2yo|B^X!0`sqc;@b!xl6A zx!ac*+J6#D@ZJJDe(2IxDX&nULLxCEz5@NbmW$KG8C6Q&1#J*`k_xHJhK!ZQ1B2Nd!%I862^UYu)p^I|24Z{xx z(&z(^j*&LAf9dtl-qU-MuaT!ih3PD_3v}U31UVOX7Pm?Vf^SUbkmuYecJ`2=n?5s{ zeXh6gPfs}1{)dp_PKr(ro`W5N8lY&v3)J*95KAd3`r+~&z#VLacOR|6r%PqE)ybe%GrHI~oBUJD1FaSu=wE#K z^i4^5dR18!67{MFzgt3R=e#@UwsS6d&L<83sgnU^*PO}wTN_cdbsAhA{}6JT--03k z7`I#^ACm*C8qB&_*0gtn;S!pk;~0CeV4{mpQG670PJ7ukOz0;;d#be@gieUv)AZOYi7 zvs0XPL@a<5HdnyMUsKrCU5Z}uwgj)tIZq28w!=L>vG7kV(`51?lK$tl223u_2Hiph z*gusY%Np#Vv~?A+US1N<95zae4&gx?BObSp?E(@aw?N2UKG;XUuHw$yh51&U=J;HrXL)W z>&KtQeCeMX!?4zoyKqh21itev8-zcb!5s0y;Fv-ZH2!%4cTd?vg7yR314gOi8%X?z zF^%syY)8$xe@EjgrQpJFC(s``0o}v6=mUYQgS~MA9w8gsf&q z;`H=*a_>$<%#pN;9QX4iPu3rSiV;~rszC;O+{C!G>pFZK`vidwKu@1e1%*}htk+!H z*w{%P%ZIFlnyT5TxkMUA^&Y{)MZ!og>K!84=A@O5H2vFT14BoC4TCzxNq>=C*k0v~ z@rQgm)7lf-OY@N)zct8~fI(8)DWB;Y?Z&4RCQ;+tBKToomKu8S0*h72ur)i^(cFi; z@r`6N@-*ulS(C{N=x_&AqBy}6O4g#ye^WsIBpam`g(Jt#ITW^78vbBbT$GCfGLeY~ z_U$v2zxolft|E^dOt+wURWW`i70HD3mOwX|aB@MyjCA0Q!_BYPz*p;bprh%gG+Q;5 z&R+4c-12 zQ)!{=;8>gm4RdFJ#4%EAm=kWan~P^KB@!IvIt!w%vr+ z!#&9^j}9<#{2tK0?MMcGs(`(&f1#K%C$jQbpe_%kQ__B`81DQas#rEkID;|Up8{b1HHne z-Sl;oHJV52JgG&`T;=J`wTXD*UMjYHH-zq-3dKb(Dsb4X?Sp`(GR zL0!5eVV=^|11%N#vv+J`PKJ_i36Rrdcq-hWeN)wWj=Zbw?sq10)_ z5zR$PyYk`<3|zWHuMZVJu|Wq_FM*q>?f6rb4%#xkigZZ|!Bq+M4Bl*n+TdXi_`M&X zDv{5`Iim!a(Q6o z$BjP1>+)YRuwY&Ko848=l)fDER5K=PM?U;5?gjYRUxT}Lsi3CoCfHt~63y<-!|QoN z=-(l7WU%~SeCXXxI2ir_GX77jY62_+zi;KrGyX5he4BPyK%nmEBMIm zHgVm0J?#{;67xA0GXsJc$xlfWtEB$o;DKY9M-b3Q{Y@cPZW%7ox(2Ex?!vcM<4DoS zi=a)67sYq9!P~zsq0IEJtaOWfcKKux>%!l9c4>d7zSMXE)&6iN>ztYjHR9M$9ZyMP zC%)!j)fpUR3#a81tsKqlNBvoB{u*0Kb3}rOvE2slkMt6Yr~Ik656bJ-jvb={CX=Zv zr31ueJ5{iC*qMEzN)M>W2;s#qiJW=%RVKh8>zIM^{S4r5 zOE#qu;{-PKSz;#pM#vxTL1C(oz*N9S7+P})dhC&g%ExX&h0t4|xO);+w2Q*88oRJx z>`{U<>owGTQ~=-jaf3K%3dT6kqR&!Nh=Vu*ubz&^8`#kgy9XDMM)DYJyFQ0p7Ph5#Ku(1&?fG z5DpJ7L4J|1#HP?zJk=>mpZdHOyQ`Yg#e47Jbw7VV-^vX7A?GFJv~Dd{ecV8g^0O(i zz#9Cq*$?)g_9MR>+DwEP=EJI!JMh(2d$GI0XH1kWBkc||CaV2R!AVIzygPCn%q!>O z5Q!Qzx8xRlu~h@FN@dUtUBd9i4=sv4xDlT;5&)SGdtg_MEH0r8$O1wd*Io33;kgub z=5!za#q?> zSj|@$`c4&(2fPMRssV#nd?kwaHFtpBzQ)*oLJqv2Fh@pPd{Du$ zDD=IWSxrmJv92m|A^}F7yDxTw^6D(3c8J^pVH*-yN6MtZjoJyYK{$+Qf^2*6PizQ? z9Ybszfe(aU(lGJ8*$spV>J!UW77(n!2#Tjxh?xEvgmN1=p?x(!mf`fHy13%mfar!^=~UJT2eLRmYwa#-P?%2A!AJh1EtqZH?LDAo>b!qJ$M zxV>XN-fbofuZxrt9M`jfq#h5lEK~=({KBY;H#x)wZewEp$#P&DGecDQaw3uL6x1kd z$2#DmicV&WVDUpCDChZJDpxQcaX(y4#MYANyK^SR&94I{Z*U@sj$ESB=Lr}Q=K#}% z(Nvu%AJ)hdp{zqa;mkxOp2_8BOmGx>@W2HG#wR1Q56XbOYZoDvxQiOrvcrDmYB}iHuLGmsoB`1aEkJ<@k8l*#qpIvEh~9`G?)&#aCnvI42Gp9ArW2nLPgDGZ-hEn`P1 zXoG*en&4LPKUDZ#20A@EjD1VJN$un=f588U6&YOr6NgBCgDgiqcN#W~k+fn&x)K&f>XykGT@X)BP1hmO2O-r^;&i&#s9 zXIbOgt1dVxMiE{rl*SKwPg4!3hZ?gR1`B2a#Dvu%@YdlQI6THd9NioRe${^iyQ?fH zn{PJkr&A)}>it0?{?KFMcTObBV22b{tab|d#mpk}=2E~t&yQDyJw}4{sGo&#WrGJ z$x@s(CI?T;wX-gy-DIUU+f(nDl8NX54_43M9P!lOP+zGnf^g(}NW4>N7 zMIZ9VQAJD_RkbY@+A3ecP>hoz1RJTdG7pJuPf}2><1$w6TrnlDCcqH6y>M6fC!jpx z3ziP(Q?GgsfSrrD@L&lI7`=?L4=Y5m-N8WN)MtdE9smLD5g>hP_y620`(JbRe^gn) zVL2RrbR(hiOBK%)hl2rwI20$dlva8q0dteSq7!m=f!XOwbU?)&EaUP5*EH%`JLh`P z{tb0RV#^64cSH((ZC_2;K6b>MF$b6dvV&S!PEbQ7kZ@4ws{3D5S)Ko1mDS%O0E#su z5bLBC)b_E2Permo$$edXp!fx~g!>CMaYX(Q zeq7(FLRuVVC|VEoXzyoEcoGg!)W9xipqzo8nJpq*x+CaMK_*z=P#0#uszVcdFbs`YM@~l;A%XDsq_@oscPmgDN!1 zE(w2-o1OsoOcgltZ+gC&N~;o0Iw z{On>3ndhvBpNaC&%>{m>Z`#WGGur8JkF#{Q zEGM0|CKW#$UV;^;v%!ge101tAmr&z7PIoyrgZpQ6@P`-!bmP$j7?HaaymLN<8gMJ< zZdgZ(&nw^y-Ba+9VIrBhcL`m0FTtSfZWzw`F-s=Yr;|%`SCWmp8;F~iQpo+kV&Tf; zM)jZF`N&JrUtxAm7S1_xpZ;B8N0;fZ!5=C*u%V6#xuJ{;AC@b`$9D>m|8_|-WJe+7 z*KGy*9gM+O!#n7xI$-zZ2*av#JZ#=0$%yZvGxk_JOI$cB0T(s9AKHD3_6 z5EmgszGAq4;~DbM{yzHaHfcQ4*#*{7K6qpOC0eZcBX+&qgk#q8(x+v;$e<8Q*m!CO zJljzYqbZW*Gr)8U!i3dt zV9j(=KJh#YhWgKv$v@o47XNGLcJ5Sfs@V4=%lLU#E7pTv{-M2L%3^E^XW@)(G(l?c(;Q=%i|zU>K!OAOC(?4 zUyF0Nqku3-2kd{Z@Pt?wkQ-IR-Ax5Rbi$gH+BS-QyV<}7jd=2})+H3oE+B^<*w!!N z+iaj49!+}Ayo4>?2axg?CGwYqEmq$2ite-LB)2|p$7>rj!KW@Y(ygZqKid`w2A`He z-Wvl*O0%A|=p#4T9ni?y)hR~>7H8M(;=hV+q~tOQ0!KV6n+m3c6i{BhGI--%gO&(B z1QkqQ;inyc@Oq_HxM|G}np(A<4hqSp`wBJc*(yuuQi~8;Ywvlw;@mX)_Ui$z)mlm) z-E#}SuDeC(aX~2gJ`-3rj)2$=W%zc}Zmf1a7`Ey|)U)C)8k)I@I}8eO(g_({H7kxd zCLW=Iy?sFb))qWHA`Y8=KLOmDV)*g>mGJz@i%6x?6k3c9qh?WmXe#*)*?#YYuKo_B z`HgmLb!mk`>Nv*#_j$HIK123yJ&0fK|A1Y+b+G))N%GU-92#1=k#>ds@CN@bm~G9N zk4XixPha+7iutljP%$02e;!GCm_1ddA zBIgD3tah=RJt$d`S1rxYC}@Z*eZ41Nnuk8|KI8k+eTAe!Kn_9h|+w;4Gbv zcb^`@PTM`8$>1R%DXUD|u8zRIC>ak_U&eEX(;;7R3ATURi^hx}V{u~)=6$4*@D?4? zZI2*aN6SOIhPAk9)huy7_zAl7=Of|OT|;P0H^ThvRm@eo6a>zu(Md%t?h?>4%th=QuO_oQmCZj}R z@IHS#P%zYu&$g>z(G|r&N@6|ml9&g1H>|NE$1jj6dk-i^@4*=dy@~V41^#T#gE(|6audTts;46`pEit$kLsSf(CX<9-&c7_DSX{-LPJTe1FD5f==d zQ-$l;9pKZG(-5jBgXF9VtQ6=#=Q8URf2)UhgvlkfE?rIon@nQwW=?#xxsN@Q+KOBG zeu1CTu?z<42;3(91>E(#f$lbF;&aL3&{&r_Mji5rI-?x=(e!q5O?Ch&LSLghh7OPi zSKNi(w$k-BQ|Y9ivKUm_#89%;PvRm07h2>UiJ!{)ayO-0_N3DdFVSTmf6%?>49JOGKJv0& zJTz(k4LvJ=5IKLDKJEHVuEf!_YR3{ozsVLv7dx#IpaIOeJVZganYD`5mR zDDjHCdg&JaI*ajyrTdASci>Aee2?Aw5c9YWCZw-|(b)l$d0dx#R9FDx@IC!Bp-0|zecMQse9 zjo0H4;Leo9mB~_&we~4cl+(i%2Gz*lZyq;n5M=f+R`kSDW3rIpP+a#3CqvklwD0ma zFiwDn{^mrIg?Cp#le7=y;nQq*`qLsT{d^gCzt;d-2G!w)#y3c?I}84{I}S?*CYfR{ zEqLB8h8mJHL>x^Mz~baJ%3=B?46!a|KEnq`WkoRc_8!`OTnsGBhy`2F9{h305!c7} zg7Rj5QowI7y*%YSN?yLYe)ee?DgC5~Zt~no2KxwL^TJp72%4d`S(^S)|@$9+M zvqXKm1^5_fiGvfY(6XaJ;MXCH!Y#EyFVMhNf`#ZE5+uW0?ddP#ZE)nMKOTOmXYeLb zwf;N%8(gr{rmq=4gc+^(aCM0odS*aiHop|G@sUI=QrFSbCAXOpx)J=A&lHYMq@qPv ze}k!4o0u~u8qVh|$5mzJ)a<5VuvDoQowjHs2bPo*r@h*Njh-;px)*|Y9GAnCJOE4y z5s=5+4inrQ!6mK}ElA&(3n4d`|+cM$lMIfGxpv*_%f9YAB$ z2?^v~MLUTn$nwiDcvV#dcKmrp+^piDc?Fl#hr-`rxcdct&VMVtt<{JGOF5ChlsjFK zw4LdNevVQWtI$r)R%C(EMf!!66E?SLL`73+?%yUuO}=QQ zH8t+kJ9sXuw=Px3>s`k2o)R~#cB=-ScZ&c)KHupe$x*BX2JxQv8|p84*wXu7GS1xp zsaN*D_8|XJWw)uhqCuW5=+!|Jc$(K1RopuZ2L;C|c?nglM{I@TyW{YeenD8DEQuN= z#j%@hBxX(S0nww36{w0Izxp`Hx;-rfmluV>ST6^(JJ1w}nAn4xm0cjDo*9l>BEhqU z{djrU0@Zm(8JhZX;Oavg{x?7!EzNGZ;ZtUdzTYI>*Dc|mp!0bC>&QTyNvsel)=$sI`Ey1 z3FWs{hU6)kVpb>-*y8XrAUJyzM+$4{>tDsQ=WF4Li+$+kZYBf5 z?~L`gu7^+gcVVOI%i!3=a_Dwh0#2Seg0JimhX>Xl*Z-ZGi(z;Rh~UzMos8L6zET$M z6jq`d9bHg5s|an$$iaqZ`N%yQ!>poXLUc#95I!`Qh;Lq~#uF~QWLB3AY=j*I@g@tl z(|4fq9(x$59))AJ3Xr8C=GbC?JiZUjuwQ>O&N6$A^Zn7GZ#ZV0FB`cf#rw{q0A-BJ`k=W2LG zqX^d2yg=D6J#g!ZXuM{NC5Szxfes2d66FhgKq^)V?-+cIL?Rxtj{6!?F^vZ(Cub)Z z`Fb8?eVatkZXNm_771$jFEF_~Kgcb53H{QgkVO7ElxJg3{9-ySw?6@}GAt6_Q#GeP zL2-g}CWl&UQ4hO(lyMLj4-hgAV-R2zbYk#D9n=YS!WfP4Ob9~^1+N<<@lFw zAzs*Qh^v>~#vCse!OTOd*mph}a;6#q8}4v0UhxYSHR;3s=r&a%%NPN*rts-gf^gLy zA3U)8I=bP!1abw6z^xw9Fruvx+D-Gr4Bb1h%yBVX&c_fg-LK%kOi7;VnuCnuHHKBk zcEa*>C%pf}R`U6SDvZ))$gB|w+QL5=ycd^&FTJ0^ob?^3caFlvBbUH!KE~iWOrv+Y zLomPiGF)$Mj{i+M(UoB_Sl@^lyPxWj0y?wcXXh6jKHQIg8Gk{i3Z7zP&m<(*-H#WX zAHrx|Uo8JfnqC;Jz@|w8*(;^Ine)jD7@IQ7u-2;2VM$@fJJvqP`{1>I-e8={t2%pL_>urtJhv&%hs)^ zCCCI>qzQv#JPX8)IC+q*(? z=yNgxD*2aLAx;gTJ07Cc6RoTdDfQr1{d2UWECnP^T%l&a9)J!S-P8e&Ng|=54)Oj> zW9z6(0q5FMN~w4VDa_~t#}i)Es6!FAQ-e@;tPnQw;{qS~l0kdz zZlENhi^z-$l8ksFTa$K_}uX=^UA{Ms(e)mwrD zSFJ!9Z90HBJV5v_x8;t$!Hz>3pku<341H1SSOj4a*uHiWwcWtU3>Qd zop98^Wp!#`jhiK!ndOFVOy9Gv@(sLy%@K6Q*czV#M_D<-%22@bDs__K7;s;agc1&n zn!3sp+%~@oza$-^Dq7mWrp3SE-Qq)lW%mcYYL5ekvvb5_|12;Rz6=)Ta*}QX!^n1c zCH2emDi})xu-g4M#iPU&YA_{KCVNjJj;TZ{aEmIq^`H=)&)5QYzXmYsVK=+s>?Lqz z9XB4^bscH0D?&mCnm`@pgx?M4VM~e))MI!-c=1Ij#yFuqvt8jGy9(%ifkv9w)??!y z7T)h`4J#+Sv3}MY(A-iC-%oI&0OpL6Qn&$n1cF)80r!Z%XI{Wd`H{F#Mx6Zez!PRi zYqKToBZ<>_f5Dq$&RFN<4zRaD1+JFg0`K2bClm4#(2VMJl)U9Qw#|?y_w5+RslSxz zG1(4ui@yxKEm;B0|8_%-or}o$*Id};06$$X&ke(RIY{T{KT+T-O?`267Gc9R^keEN zjAV?;XTb_O>C_(By`TdV!^cp)PdyVxS;p`hYAHc1#I7t01i`1gS$EQJvxCx9z`Gyj zNNgY(P5tO6S~3g?uCtn8R@#$#mGYjoZr3t2x}O8XLW&Y7R3^-_wLxXdI&}7~5cmv( z!7eLNaJ}A{ed_THIM1^P9o%t`>Q{6Fh5vdelfjGNZ$JPm@7g_L$;~=~VX{$^;r6KP zf;2GW{(x3I;{x-02hc&+QdYD>JhemXEAn3N%hFikidH`k1V%A@FkdJRc+_u4?X$PQ z^J*Q?S~|k2@GwFqeVQO;un_oc{so={h=Ex16Uaxo9BeF+gle@mRGH#-LOVtqnVw8R zx{stmkXj58+cSz*>Bob%;1rbj1Hlg-C9JQ{ih(&6fzPk-v)Oeetk;!-NWy3p_!7b3 z@wQnasnlPTmKYDJ$><~=C~bh_ipOx$pKP@JdL}LsBGKnR;V_|S8q6$L!c)$jpxB(5 zCePmkK`A#WVljhnbv^|Y`;&;qY;nBKSsm5a97G`-*Mc)l7WQn%E|k7-lL~L53ET5r zXzrRmmOFU>u*7eJ>8=%oLG@EsgWVrO{>NnyWhwb!P10jxrH zQbRduBqF;tF|4b~k;FUM2PnRDmWa3iN9=6xA~Z)GYh)Ul^czjJoxgQmg zwn_l2?NKcfi@ik2cFTaXM}v_=xG+&r@qxXuHXjVNEl1~e2?8y@W;SPO7I5<12X;AZ zq1d;>h=w^wCZFAfF4}7ovjefL-w6n1@jL=oxD-%IY8U!FXoX&&K=i%Cg3!2_13tPO zrHXt^AfG@wvAp^-YqB;G%*xk-2~91S+iD3MT!rvyISnYXis^Na6N9!IAr$A{FQ_$U zD>Bkj#A!_n#LN{T*j8f*d1|uY_>eOY*_HG^6O8|B&i;=o`{QXN@hSc~dv1IY{3|nr zUc8Y)HdciYo!^2U|M|z3b5)|v($S0XCbULW7C1B+!h5E& zfTQ6dB|TuqQ1ETQm8)%R%L9$b#&sX~UsPG$|6i5;8MFZS<^F&z914WElL48uxF7Z1 zl|T{-e1zHO0BlzCi8aYy%R2Og8(tslXGSdpv`gbU{oxXVpf{dAk-%9md94Q5zjh)I zu}tx@Gj9MZ)|hq+5+-BxISh^+OMs%KBUt;l0&P$nkCj%x!HcIo@rR5nbXKDkHdS2$ zOPg1avQ4d+bv+VxNitnvf;W-s0V~+KX90fZ<_0#uYmxM0S!nURg3uVQ1tG(+z{JNK z;_@gQ#%$k~!U4!xcOJ@%&%h^-sz`MnVO)HQA8e3}BQHn-lv$etqq%KKYP%SC%li)7 zB_`v21P92wx{VM!&51vpI}0ssg`v9ad5WJq2VKxIrs;zcxPaIUD~&9$>Anpx1fB(R z^6%-Zj$@R|;?uy?ECU1wY{1VyuEuDS2Hd~x2=mu=8M1fglizxWNUeA^Qd2*fe#hQH zO0Scw7xr)v$&~cf(u;eO~jow-x^oB(qE|kQ+ z;}i7bfiL(Klkx2^y9Z5*-Rb0(CHOVgBt9wnqqpa-Ge_=bsCw`lzq^|RKLd-BGGw~RZMxtyCL1$TNmjidd8#;)j{L-l8=k$ya#7xB z=&u^SNYB%U#G2r~C0kK}sx;p3HvfO>dh=*1J*V>-aOwoG^#(iIb$q#xam9_k!%!;f1Lo)r{JX1ys>~kf$cD zh9CXXpdNM?)l1u>`w=>Sce>a z8i*?bilM|rA5_aa=smNAr0~~4=$3wrp|&E}JvJSuX}*RLw)yz+ha_4gNkP#G4-B{5 z!`=O!iN-Fu^j2vM%OgaYR0->J{#Ucna=mJ}B)pdE>aVDDj9SiIYUJiMLBB{jd|H1B(e zf<1~E>-HozPOFohG-9PlQDTU_aU@^lay1JCg;QDk2xYcy-jL{{oFY-)KY?xNkRai1 zAIXM}C^lPZK2CbGm~K^^$6jfXg1xHQWLcF7d*WIz`K&pe6vxJ*N9sypVajb}Ezfcm zb=w&U?wla#We}bFyi8E4FqNjh+0426FU9H7D`AA4xaiP_HRQwwF;V^&Gd5Ks-SLLI zH3|01h1kz;$eCwLd3tUM%>I!e8bo>0Rnta36$gOo5mh2}axA-cZ5;Rar!!r<6 zd(aXt!j6S6>M{hoxjc3dC6*lr(~V7Fy1JUHc&j1mwoL}5iWofMB|}yROVdxL2}mC| z2t4N2;zwSPCH!8(W(j#pVl)q3dna2TwBkEkxwMg9Fu#eC9Xm)?-78u%-J2FoUBP~J z-OHYEcn{8Qn^72NOhBW8X!YKP>RxvecvFG4O@BzptqzVos|&-m|Bd{iVHogjW7YW$Cla<(K!axJt0citzIV(8QH4g{J}wTXR#J~L@i(}of3;v>9=@S#7Gl{Q7R{=z5G&xy-K zoi^s+W-Uu^Zez&3njq-=S0j2HDu!S8w1PmxoS2PRi4U)Iy@C_1cl>gB*21PWmcwHk4Tly5`|PDe?B_0?`+;OGS^PgUpsbF-vJGBbKFYWJ2DVwD2Q;jcM*&q|Bw89 z@&n%3^ur@hM|#ZK3??jEj0RdqAn#QV*Zo+GmA-0E)D6$D2Um_GU(WqxZzL%ZfnO}U zL(i8reAC1GlM`u=%~d>@+(WZkJViOWrmXY8FCwBR=)sRhq*+#ps7xP8f6nhE$xkj* zTX{***y;khInIfmd@-M0?`A-blsPg@g_+!9vjyag%p7<$=Qa+o7x3bWVfe%^85bL* zGFqlCWW$?02>)Hketx@|)yWDH9nZOnS-riYB*A$WRg@er?2lt5L`9J9YsRj)S3&$l zUZSqkhehc;(rkf75=nioEHW88f><7yPm=P_(5>0;xFNd%oQ#ShsY^HcI=%n`nk%8Rp$mhq{Wd*vT%0gdu50}LIJ~{F=@j0y-+s+D08rgRP(^#KR-^kDVPf3u|9x}JD zh_(3WLPLVqlDQ9;vO(9R*pse@Sf@BCwtw9|y5dw7ilu+R4dV`Td*3AS`PnjP;4%$- z+Sb5%hf(yy)mUbQrwwLLEkx;PEzy3IVpZ**vhBR^`<-%$$Tm97aYEvIHtt9Rn`p6s zRo?v&_@y;oS?7l9PP6qdmK-6i&+hh!OM3U6Gr+X_xaUI~ zu{}AT#zsyherNwdo~j-1tsB9Nbxqg%qE@ zSndrcET*v-RKS3S#D2tRbD`|s9Da4;rrTN5$*giKFRr1KUZdq0G#@UEHZ!4gZUN6Dw zur}S1e;ukv-QZjur@+Vf*)(gkFUF-k6Q;X{p?IAy8mk8jlGVPzDqdl9qVE}=RUgUf z>cp{XaaN+JvQdtH<6MZ{#0J8yR%6Zlr?D5)*Ae#Eeo{I~yFPxqylCmNQ$)(Dn6z$j z7X3B~CNrCT>EUk??#=VW_jo<--&u7TS#te1ihhiie$tF(_Ezj`sSnz`^@we z-BcaSGTR96`Ilt>ZaR)rl`K(s*p|Lnr$gY*L6o^yZXY~yGMr1x5Q-g~fd^KKVb~ge z6`7(=cjy$6u21SPy8QvwetB86aiD{it~xBT9CeVSROPYn;|oZ>LmpAQIE)Nz8(V*C zvVv$4WMbdwK_cJwkoK$Tldf~Iq*lC=K0V$IdAa{!@3%Z6-D=EU5-G#e>@T3!H;PJU zpTQ@OPE-8_LKvLBis|^c9xVobG3T;RLE3&f+9KgfPkFwBte3pGG%uG%PCLz9ee{R! z4-ts2C98_04W83j(+W}4N)U;hB3+mF?j}i8w#5Hzg;=g@V zX}TE6{NPW`J69o?_1BmDy(=ktdqsts556Sb0YsEH!yoK7f5v~Be`v|6 zQWEj9gUqrYP26H6(E0ssCTw^p{Z)ODIlJI3qS0Y zefF?)G*RweNy|Rh!{6AuG}CFMsK5U@PL56|3wfpQ75Bd=rBF_N>Samou2cBy_*)oJ zb(sXNcck$PKH%UEZ?aQboc`XxprPesoMkziG!*B-H=BpdlEVv_TQMhzZa-n2uWTfd z3Eu2FEh|xbpn#|bRpE@_aN=RJpZyV~?I?dQi`;s)km>V&$>$GO;^{duM60ZYYwDGy z!{=@V$0CG;gh&*G^pF;@Rf&V?-;joACmo08y}MIXJW{vheN(>$t!czrH$5?D`Ip z;M4bLoZD$=IWv>Axvu9LCNQKUb_|K?Qp9?V)uavO;qSm<5>uedUbv{io_?E#(??Te z)OHCzPu&WG_uGj7QfIIjJc-u|)M;^p3vCZI!i;fQ;6EV7kY{yNozLU`H%GlbcaAt& z`Ch-ib$6~PMe~iw-9V9_|8nSU-(zfF_ju8*)dix9x<-x*;s}4HxJZLz7O`tCwlkG_ zZ)jM)8g7~11|574_G{!k+_C!_Z;KBWjS24i&$+Vyy$AWPt89eVOB4+A!@zAK3@?3$ zd*dr$n$;yH!{!D`g>GP6EI$DgU9XmOJ zy7)Lcy)zG8$8^HH?>_XU_&&13{2W~C3J1%30lGcMP&?}l^ndWm{-3w^ ze_drAt_$dM>P@oct8m`ojYK%J8BHhhbUxz)xJ&5)XAxEe1A=5&IkOgT=FOpQs}s;P za}jy8jKiu0nxd|qDMUW_k=@!0UvWp=Mife?ko5&FQE}kA@YIZF#Kp85#Vur?k6L%D?SMuUPq9o zIrFIfie|y4SM}V5H@oS=Rqa&LcMEgyh%|NMNr}Tcwvg5^9ePJ=2QkyPrqt>X`L&Hl zYVG_CDbKzT?YhRsmJ~q0+)}c*ItlyCgCMWhk@U3ZlI*2# z1p*x}oVvJ=ZYoY7Cw{D_>(-v3{u>%-&)6BnAjzCo-(4WG5=P_CwphH8z?X^ZpCEC? zF3xYkaPFqmQCegkLQT)9(9|>$*Zb%uuBfjib7z?dUkQ59*G`9=oZ5-|YI^an&S7jn zbsM_OoY7NPnsp7>h}&mciiEsiKZ6gf`tkD<)q32IqN7hQJzZ5mD`h%p%D{f|_Kh50 zN)u?rkTw|*q|^3ukHK=kCDz*PCp}i-cuZd&O!jXg6Yn+Av5KE*_&9&Mx0C~~rBPJ= zdYEYVg=`!bw27=PlNEi^oXc*WZzQrfG>Vode5EC6{W!7QmfrSf*-0~IvtM@C&^M0y zWb!QDJJcVA-?mL*)s@Rh+xz?Er@=+K#Mp#24wa$5`p=NCFRhsTz?m4t_Tiq+$z)FQ zN;2bLKI#pRC8Y|-a4$EFIJw+|j7OTnBX7GKDRxF@}cuY=WPrTSgw5?O9N!T*t zCg_5V7JG=-x;A_@{t;at97CM+q*?j*o>Vz*DV_aG3BM(eWNiwp*@usdY1fNrq2Sj% z^2ex}SY4E1fANNcGx8C%@^UtHFOV1AAJs$O#NA|yq&V^+V-N9E>LTZ^^WjKyjKQrT z2wvW{BH06fsIi2X=JzRr zYfk$L&g;nuDn6-ljTZHS>BkF&6OyB$Zdo;B6x%KcGJPh9^-#pVkBQ8Mj1k=8(SHS< zOlF3)C%QqhtI?QZkRDzZ5y~OqiK19~k=}SyumIyNiH^#HVfC-BP zZx3lP&tzT+J7N`>Tbn)W1_DkA?eFU`Tc#^8bp2xiIkSrqUT$`%Joi)ZE83d-(D+iI zyx5bA$k3z0X-6n5^uWKn61d7D4cZf&LALCD1ADESpuf3=8uu&HxXtmP{Y``FHIJbW zqViFEOC|F#H~}nNzvCwlHB!v4g{!Nl!sOCiqLZ-~TU*R&LC;Zo*Ju(=DHuV^+SZf3 zTs&FKuZOSvSqo$3T?Er!&Ii>g5sb#WT+Dnh3#aO3z{m;HKy}ku4Cp_C*K2Pu_oPQ~ zi)Osx!e_=ZC9h23k^yxvk~YLR^K;CeqczNf5r$mlv>Ha|qd%zEeC1wQdZAh67wEXQ z9i%Hxa86F?(7e2XaVX?a;cqHuX;;sv^E!b8?S_Jzk-QNnLjWyXR-uso5PUxrh_bdb zxc#Twz~HF@ybL@Gxo2|FNqbQ6%ZTF^ixn_u{dRIM`;B12qikqBo5vJZO-A>=m$)f! zDb{Q*hPn6(A1q2^?w|4E77Rr?T$bdmVYhWr)V z&aC;V57)XMbF;mFPz$YFn7XD3{8bfU*x(Q0>`^Yz{7(_=V)?+A^Yigo@K$VDC5unT zE@d1Kjl*%C*`QT0mHIB~f-IW>JX|mUul2uTX;UXuO2xuQ>#b0da}p(ZzJQugn%Xp0 z!LiKInDOZXlP-50K=Ttb`qV>ayLmZtYmO03e!K&Q&U!%I-gvI_gEbt`SqrsNzMP9p z27Y`b2}46HIX!y}S^fNoaN^vXxb%k#dT8$hb3HpoRcaQvSWjc79e>5Wh*;oo)o?t- z@73otN2WtmU?+wM)Jb02a+DeS40mZy#zz;VX+_If9(iU>o39^(sgtE) zRY6(Z@h|}?x;qZ1j+Tb(Gj~8GdOh#mPKP4>Nc!!{9n4LbhTV&m>4E_#E=nC zeZ{#RzV6K7re5;=xeCX=iLyU?Fb7&ruEVMiml*YXU%~yvD?zx^XMx;NISlg@!@=ej zE=J`r7A#7I>a^X=Mh!nE_nH?rm5qVp-wl}K8KcOd!cHdp#C*)_4nUI?)42QqdEDw5 z1Tx;XZ;-)>m*u#VXKWd-!N1(U-PN?MVm+totPREaM?vaU3(oJE zE3ELi!0a@8%q;dV#)?G;@eTeEeo|Hk-#| zt5m4{6GBB9()3Hldl<8QK5?`eWMs2c*p&)jz(_D0Eq6oTnR|L(K@>ngj#P>*hi@xx=Q?r>kv+TwK;A%y%M;#5uFVtAAh*|!Xtq*oTG zy?Y9N@KzV4d6ffB{KNRYO+a;?7rc!>W1lZFho%d$On;v$I1k<9%Dk_lV#rM>6?@J7 z4_8^+|Nm8X`JzvtQ0+jz$h1TK^bXG8%P>eaYJoMDMNH=G4BV1%fC-Aaf&C$)IquwB zjDOEhu64PbxSJjUn=XuaIto@i;q~RM?yz9&T+%T0JUUn_305B6hre5f!9bP_CjJ;l zZvHkWwJ|f%W6Ls(*u&#eAPOB`Tm;FUPjGVSADq+0j{}JZIj8ZSWK`03ZlLZe?(@^6 z9&iYRhDKza^mlyNU(3g5?}H4Dg`|D04VJ8{gTTeE4)g1`k+jpYG$2z2P2#+0-;$ex zvL$?GT;^{^ciUX3wMay-avy3l?;g&RFoZLCzo2;KDA=(r1Jc)zq`P_~XoS5JzVy#W z`9?>g1XZky04oc9rs~0DknZe-wWqw` zzP4ioNPf`ggLH0r7(-ROB6<>C}^t6d`g zzMV#mg%sDNXL02#=JB53JeYK(7anUp!B@UhsLsMRtov1sa&j-wFI-|#_A2QzF|JvY`=5+!@eLGVj~W8PA@|CeIt=`|=7_?EE=x2E5mqrjul z1mmke!Ikx)Sk|pUj*gR}9W%BP%V9=*sK`BJ-WG1~k6q3Cy zY2(uvJjSeN_Sf`5x9|~|k2!}PE9Q~`%^B2X^hL-w`i)d#7d*0A&)grJgL&6jLE^^w zkYR6##r}oxEprx3+js_Sv^V22UEXWWHQ*v&mTd6gGZusYLHWRN`qZZm6MNGDer3be zdFFIL-j(R49l}JijOKZdVKl0D!0W{h1HWrVa^34EaBmNbllezDbm~;03meMNNjZY+ zyXb))YgWU_1>;C*lMS&M;Xp%SH1Y?Ab}8 z^>8mN4?K$c;+BvHLM)p3tuE6cl=yB)fV17R3DKSf##+`i#XpJ*`*(%86DmP6dPd>e ztW#LEuLZli%)#7gEYaRPm%8SvQl+L=)VvV`b50b2#ba|ahR@2KtC0l}v;V-P%eAoU z)^KWIQ_7sUd=}LYXpy10-=J(KFP2&}o%-Itg=Lzaz%H|-$1Qo9UX?C=al8uytxt1P zES96s_Hm?EX+5nN_ZNM>*5bxee~^Jk5UxAG$=16Py$(qnYLuYsysGg~YCE&$*8rw^ zsL&nDzhc15XQ$i<+i?l}C3x@1 zmn3lc+sv&I$imRu@pK`d6PdJZGL^YiF02z*#(?A1oP)@S^nACXaYrLbo{6f%pDG>= z5-LVcs0|AKEPKwyZ}laie9ee-Rwa=}%3S=xFjQy`0eb%_Owef-Za$_$Qqh- zXB3i!Qp8N#6O_v8aDnqX#-*bT%>LzquZ<(5N@svIqe(^&{NcoVAL7|3XSi%>XWZ6k zhF2yD2;0{qoZBZxLMuX;resU-B>%YYxqI-=z62@jIHA*q7%+_8*dv@rUG z!$>(*+LbOzCCn#K!>(IsX`ls`iyE=I+5>bpMB`A91iUV(Mkl_$YFH9FxKe|N=`JU$ zH0FYu&Ia;ywma#l(xP3%?P0O|QNhw(&+uHJA4ph9qgmB2oNY22EB6V=!Gg2!NmmsW z7J1^^jXfAQXBge4DMNS32XYA!_3(3M6z=*KgKECf;PNOAB(z#!*1>kTq@Bj#&utF+ zkz%-f$7d)>Eo9u>y-;iaRl!SMM7J3afFne3xCv9%F4(I(!-SWGazbD@oe8auM2@X_L^KJT7LFFD(ASQ(m`u zgMQmpxM3Or$*tXtg2r^}=4MT$zONwjzg~gRnry+hO{#nj9q)%dy%-|eO(;xY%`$epBzF5K1)s9GmTrh`94a#YlW8=tReY81B%c8 z=kRp@GNx*~xbT*ECZlar2k9ZR$m!0BH1)waSh8b4ub}FojYrV8|0uvpJ6i4b50$(> z!q&)4$WQ)-2ka-3@^>waRfrpPpSy@Ae3(PZexJkoQDP)gVFWewR>hgFibS&;V4dwA z?&`kHAedhV?bG-StE&Wm?Md`Wm<^5nH-qFK`YV|1sK5*p4#QEE)F z!^GgT%&9}kP;q59OdR7$TeJ6Z!VPL<*6DGWKW7Y8-x$Z#HY}$*dUNsH4HiUeZMnkG zA)K8y71!saqtg!wa%L!+xgWoX&X^d_#VbYP=+n~7yxr#HVpkYW*=m3%Zy)9+Z92l= z*+eiaqZ@jzE0Tl9^Ko0D4~RheEY*#rttjvV=C=Mt0 z?(zjuCJAMKeMdhBXZ#~4Nw54o0Grx=awDBCaJe@U7~8@!SY1*H=hw`k8i#zyo%#Q8 z-cUZQXlaC*)PN|0GdNVf#&J_Vf=@y>WFG$je=G`sPgX~U|IEs~xCw!sI@HQ$Hn9z| z!#(?_)Y%M3L2b4Ds zrS?6cXPCcr-5?d42a6-LsmE469MHBJ&*g6B9@ypJ{#0EI-(CiL1$tDWgoV$wA=KIY z5z~D)8@4{Mpst@q@SXj^%}<<)#_UQsd2cvfv)2TD!zAg{3UL~u)rq-R#mQQ;Wag@7 z4HS<^hsDML5E1Z8U~9XITP1h{abjQa%5^X5qWlu-Wiy1b3;Muwq$>=wdn}BJ^7uJMFxOF;zR1`Q{ayWdBrpk=$-85#?|yi0Za}u5d4k>k+u=i_ z2`x%iCU&m2f_srAJZadR7N5KiE5b)ntxp4Z?ePI-$Eqdd=l=1O+dH2Aq6fHV^JCyb zY8kivg)ecrI98Y+ynyx|{veofwjXED8c$c~&xU7<+ThZzNN)Fv6byVXPyYS3A&R#A z>cpm+r!||A*D4<%_SppZJZ}SDZ?3>C3W}slc{AFTWP#Y#Hs<6HDO$eFiku31g<9E> z_$@*V_FR1_zJsqYt+f3u^8c zaYlhZP&;HIU95WoPxnv4jax$?$Y46nX?4Q*XMAS8#|&)Qri7l6O&ISUkMnCXxpxj4 zV7;aYO)fuVN@k0}LzhX^`9mQVU9u(Z#j_xD%Ms|ww<5XjzTmUYoDN->0tTO#aoQ=$ zBved{vo_xX)Ac71-x=%Cw@m=^+{y*<#|5O{q7Gx6*Ku>0L@phk zNs4`VLa_6BZj50JoY<^PkK1R!ii6oO>z)F2R;}Zl6kovd)plf7aw0c&pAcosN6`Q( zz#BI#X=y+TtStG!eV?_FpAjW!+ai6c^r{ifE~SHl{1E>37{J9VfV4cj%p}TdlQWVf zxJ-Wot(S`-J0wHtpnExbof;2KgYQtbZoe?zbTN4_HWp4B?!Z5Xsmhdi&vzAB`F2G;oQv7{38n*JEv25R6T!Tjf3SzD4KyMq|Hf{hl&9fJa zrd0mtT-pELgZ$T3cK*$eaLiGi-puacMt@f$@8fN``l(@feRKl!r9MI>haAXx{Svno zXj2uFTqg6UD|7GPdr0zUs7+@rUZ@Gd6+zE&?Fk#ZlS78Ybaw{S4n%`)qA6*O=x3JI zKB&8S6^+5hwQ{;#X-#8r#HdqE_OsNW4@@-}3^q+L+1 zG=`jglL0S&Oc9useFx2qM&3s?0{k95hq))KK*B44JJv9SY15|D{*Ycy@cT8g|0>}n z|L36oZUT8Jt;y`(?#}yUT_H_q3^#gGaQNOHp;UaXAT)X}l-rjw+z4svIm3s~hik)U zPP^gGJAO4X!5G}rx=?etG@RJ^gZX(kgt_9EgdZMvGhVmEncMf==yvxszU?i7&q@Yl z%=Ka9+KF4-PNzqB(btM@di4mel=*OZ6C~)HJ$KMYr5%<;oZ&3qZRI9w&H#&gd%AYE zKfErsqCKupV5W;YITY81f7*lTmxq@0fmArT7cE9-4hNWFBtzn!@Hi-103D|4G}Y@o zbSEw5W{Jwts<#0mN_nVUohL1G)1-L|whPXTuZGxfI`mBQ6mEjyVp7?99rdcl(}O&H zPn7Efql$+U?(Gb+B=#iJaySiEoJ#_Eo#!y2DjmIebV9FV2CS*O0PCP9d_4S0kvCsCgA6IllCZ+ zOXCT=^WTI*`$q$k?lk~5M_17@yApIvOu{u+#?whpbZAU{9zVL6QkF;mTyHU@wO`^; z*}4F8_oM?eo8>$`O7VhjCYa{H z=aY||uWJ!ro!JH#nir6uhFRp==whh;aTvGgw4FNDl4+I=pJ8UqG6KaCoU!AJsQe?h1ZuK*0P@jDxx=J=(Y-9qz?O;dXsV5`W|^3LYncdv7VP9#A7w zB*jT|u@;vR(+hI8x9eIq0zI3ZK-Qa%fG~Y82wiASuA6>lG)s@d`cvL?bMa-6h;`w} zOIcE)VnmMfD-xwUqv58uCXd6KimE-y_^`2>bN}OlF(w;vjAS%SkyWH`vU2deX%dWl zIDwixy8-Pg+Q60HN4)kEIy$t-PHIG-?l=K8;bCy!ARWU#b>M~hM$l-yjO=Pp2X|8y zcsa5bUXI>#H=a*{LbjUUgA ztT+lIPRfx7QJToymLYnH4&>L1emL>z6?}8iLA_8O_W_Nlacvn9pCm?W^QJM?C&!RL zfgZ7#DMc3_^Q7NO2XOyHPkQ>PDt+Ad8|Kc}M+?ye_~91(0fR=gaUl!u zW3$sbI!R#{?XCK#Ywr_SvR#1~y2RirmG78h5)I4Dl<6b-1BVhbAw9@~=I}qi=Hgan zXX9z`7Rr&t028|8aypbfuE62bt}vS;@|iDp+VHb@5i_Loi@UyH3|+qDGdNnGU=Ccr zCGcCB&F$F3d&OS^Egr>kiDzZ7w!Mz~UL4Nk%&BD_57mQ|R1REUnjr|>Cr0Drn!)Dp zNHWM9V`2(taLwb!&>I&=k?g8&yk@i8&oX@Uf4`W68O@mKMa_c*%AkpKTsaz;&-&RJwi|HxQUgO#li;AKK>#gxd> z<0&}cpg_`6?S<4)jCefChlN)@gZ9e%g6_3bv1x-XtuOq+yfe~3@$@68Fd$2Itr|v+ zV~^sz7t2v$iw*3!VnprRayUO^p+`cW%I+rgazz5O>RB3OPCAA)ZHZj}X-D$y*ClYd z>q^_MwK2mD)}nlz4%r;`gXxv37vzUp3Afqm(+oaq*eFRFlt&CFe(!l4%CLW+<^B|( z`aZ(ab1Ptj;xc~CD~HSR7vdRm9pxm}p6cn3qCj6@m9P`K??&)E%}!*hDi!Rx*zZ@k#Ry~&iN z8&=7Z)e|Mi?x-Xz2;l2`au6ubh=3LQ--A8hi_~ej!G`O4utdUuTwHtDYYX6dycV@x zRSKDJQ&Fr(l3IZn`KBpGGREeD{F>op!`&G)eESn_LZb?KTB3zpj$FpAKYrqlN7JBX zi2@E+G^G1{tC`yk$Cz_bUj&n0k3`G21$euw4U1haqx0RZsIl`m-Yb`*@qcuwda*B~ zdD)&gc3Y9HCGR0F^eUq>&5rsTXp!p&$5EGyN4Y`vtuW)9fOuDa$C}_;PG*BFs0^2( z>P}xcM^6#slaL_j(i}&wht*>6C}c`M7{a}S--Sy(KQZf{#)J8dPK@_+AyekA1CuaC z`gN`h&0H~>Ubx+jqC`zPd%QCm?)?Pwp65aL>Y1eRqdm5M9FN@++O+(l5(MRULYIL! zIjvg&FK?d2zP}PgX}$r8yI+T3;{;mDvb1o%2gZCeAkto`_$#oInWPy4XAab&W!O%v zN!2IgI~+*&3_TLDG#;7F>Tq8AQk{-cmGB5gfxqo1P?vR~>Va=iiPNO_)t^AmpAsgm zP(W_h*pN9%nXs*2AIEr)BH_zl;*pREB)n)ResLH_`+Wli!^bVLW8RKIp;`s^_dz#2 z4Ui{qZY7}BnLFs=lYrLu6yb7=K0UL8U$HDrf${T{se(Zg6sh}?&V4mF(yuYq5=W68#^ zJ8=21XbACY#S0Ny^se^|95rPLb4YIreUx%j_|kSf$+o}8tmuu!ulI(L-aQ(0k*b{d z78NBq1zB+wSvd(=asEr;e?Q#*|2))Op(ZC`Z~32|TK&IIWmjnO-)pU+A~%A6S%v@a OW%*~avd$7ChW{@cAL-5j literal 0 HcmV?d00001 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.pt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.pt new file mode 100644 index 0000000000000000000000000000000000000000..a7a9f89e0242606ce6624d3166c8d94827e7234c GIT binary patch literal 79865 zcmb5W2|QKb_y2DuC8ZJxjWU!_hFwsKtfHWxp`q}{ ze_a%G6nq%Jpx{LTL;ZckK9&nZLS39YE6n`JTy2wz=s*LWMKd!j&$RPiWJOZ zMT*fPB~uSBJu%}G93C7#U!|pncrC&r{G&nxB1QW0U@ub-P4dh0eaKShdz0FU z4E)3qr1+NM;g+pKE&U?G#iRrQjONq;Q4|p?*e6s|)SDE^Fj{0}8l;>Mq$0nXaS*R? zpP&vwM6kXRQ9q%me-JOufS@jugZSkR4C2>i62z}yP!OM@X%L^4nW={&$;wJ({%11$ zhS8iT-#3~f3zEg)Xwi^%Sy-BSsFB}Z91uJ|NGuxKdI|3+ad2p)rZ*G7Ro4ISO{6KZ z`Y%JszKD!qycgMMimXjdJ=DoXBZ)XZ^8-XS{I6)b5Y@#~`Jkp~SZmtDD(itJqnxGJs{jYyPB<{%b13PpkhxkJT^fKUWI$O}`!H`pgq zLRZ$`u&0lqlSmYuUjpeU8E?zWVD4Gz} zS{zrtI1?qJNkY-&*5XY0yEtyG#c^*fjP zh~^4K-mMwW`#WQw){K2yGxlrE*uOR7fYwB8M1euvewiOF3Tih^uxZfHf9Px@TF_qS z5Z6+%D3sG#!&P^!D2x=6R4|;^IzlX3C=`i!t(kaHq);Rd`e)&WiK5zHaFMHqShQGv zL5XMySGcA8#g>Uh%Y~v9{KcZoPSWP#os7L{qe$XqFT$N^YlFN^5C0@HuUih*E{3 zwAP%`|ITSsYfc%hIc;vuX-jKPnI6hylIHek(T;X$X1AuflTUM(M3f^G?QTso_wO|GTGPyLO|zgi&BE3+ z_xx^1Mf^>#^*+~nYx~D-OjER%WKtY0+Se|V{jHgl@R=Nth)RW`gRPky`a6@ut(hEY z&E#lnCdXPcInMVXz6)B5%H$imJX%!IZZ7Gc8+y1%)_(t?Nh4ImBFt?-Bd#a0d{3(6 zH{l7fs7fe0$#23_@uJg0(V0JXw5{lD`zxMvH4=-es1lMeYVr~4TvUD+XEZqceIsW&aGv~L&S-KdznwE3Ih5bgnNA$a zZ|aN|htpk+NWJvPD_wq*3nzcaF`dZ?k1j;cf4q*Y7;WwhcP}Rm!aF#e=|X&Rui;9~ zMS_C;{R6_8u3XeeUJx!y?m~H1Z@^*O@_Tb0zkEXu`Q;mN$S>cRLw@;v2$}}TU$0DG!qH4W zdF1yWPfUNg`_DVk2xb6@x2rKTkoeromoXvm`xVR#B98n;%$O3A7o9O9BxcNs6*3mY z@}(b4D4rQ2k6IG@$DVRvhLUjmDXoZ4Uf+y0Ir1-Y#)b&MkeIP0R>;^9 z%g<&{D4rQ1k2((}?qI^EC9Jb{%k@NU`CUMB; zGnqp^pD7&j`M42`@(q$V3C5i`(TtGTc6m&dyX{M_M7FrK6*G;TXkUow#OIm@GlRhI zzQN2Sj=Xg++Y39W{x~Mm)QTPy?jn<#+$_3&pD6yq^&SM zA{Wk+!xWk-LBF9UEpXInln3@x+(+jRXR1eIt=LeBW3{h%e@P zLSiO~SRs>4EMLqNLh;N7d2}POf9{=OOe%@DpEHg4T;oV5$JWNNi3^dvlR=LBbejo@ znJvT$nM`8&>9!JzXST_sS;YR{JKG8KTJInvVY1~Tc5+c(>s=hSX`RD)ywkoLtg8A4*%IW3P>=TDI~UC?e}ma`Pvt8D6joqLK3E!SRu2Ii}LyG=ddlG63*lE zIlv*GPbr6dJ_kAE^EpJYt#KSCPBe3b*mijwmAiiwe>iiDoM>N&&V zj{J0I2#J}q#0r^n#PZWs6N+cf%cB>F{ayTvgn6+q5t1-9@)4K0C@*#`hizilaUL&r zJ%_y5S2*OwzRDpl_B9UwhsI&UTqh&{t#J%vZgAJ)8plln(abGk+ZF9Lcb4BgcQ}-9 zp1Xu3Oark(<{lU2*K?o4w)H&VJbpb7Ipo*V$RWR;M;!9&c}(zk<9I?int3XZv^9=r za`%tDW5+xvC)(GsiTLuy@q$2G<9JCNzHz)F#22%fkeF#9R>-_2mM`WTLh;OB^5|P) z|J*zF%sUcqKj(YmbB*HzIkq;Ak6eiColoS*PxqOSnE67ikoih1KixM%@yvI5^aruO z_s&nkyw<-6Nf-rkzY{Ww#I|a!#9^D(%ACh*t->L%wJL|a)@mH`TB~#T&&Huag3(L| zV%ybTlN-s`z9Wb7+IJ!(VYG-9GM%|7Uwds1+w$qcd3-)yIpp)v;gHX#8;5*8-3hie zjvmB`W^{>dmq$;z`$zFdFnZ)f`$FgwU*0%+5ol{12E^eTM{h!WH4F)f86#qaj4`o% zHTn>WXZp&c{fPau_zp~e5^q1}0OE6vV<0)UHVzXmM2bI%9Qo-?35gjqVug%3vHWxv zgyNaO^5_s^e;419FfaB{LK4PGKEj%d@?zU?*e3Qc&f~=%&LJ&1+2r}~D8pm+Pfx8yhI2;K?GXi4U6^-J~@|y>6DBnDgkc6R$6*5j-lwZ$C4%^n_ z%z6BJMsdimXEcZWdd6_bug8Vp@5V8fa5OVc9%*YFRQZTT>q$LABtA)k+!Lq4A<4*7f*5o~K5i-{A>EFrdC9!ur!AH}y}mXQA6hDz1`RUdX5;N~if2;f(GA4@F8)Tsyx6IPButuo zL^>Dc#ookWo7fqg$BVt0LtgAH9P(mka>$FlmBas`aSUg+k&*w_IBc0L?pj>q*iImt z*+Fc(qGfYu`OUMFL;2>}MM%Qr5G!PMb5VXhxg55wCy(>^_2hHNucv@Rem#X8^6S|{ z@OR@VA{@=^l}FkdN3q=fWA6-S_K_3q>$soz^2Sj@psjHnAP(O+N(u4BJV;2)93oc8 z943}8<`F{i%u#vt7_oou9b4u&iMOA#jQCvRC@06(#!i+ zKOIXbo~e{aPZ0Zi?^F@ywLVEm!km(iIL$?Qte6DfG=X0Gy zKA#&L^7-5(*w#315ht3tO>Db7?#SIgif_lLZ*dSe!AC$;+Z${=wHPCF8*7>yx8vuNtpNY5g)iHFZM?c+r<9FdA!)4IpoFu z!XYpAR}Oixzj63KG!9$lI~nhngoA0j*f(*nNIRZTjS7@yMOGR z5lm-tqJ15;i7#&)T?n)_j;_Sv8;1@dzL?zziJ9)i3Yi|n^2O986wmaONA-yPbMH7X z`Xt_d&R)dl8ixTnwlq{t}=_il&C-(Q=89P-Cn{wEuwHfE}TAOpoYi+?Huk~OKd98%zJqI^EK9Jb|S$9a4{_8jv0jNp*Z$ALpWA4h_1jYB}3Xoe!T zT^>O0{uZZaV;Glf_o<3=n$ojaj;MktR? zCH8mmrxE7Go=!-@%#e?m$whgwxvxzB7Tc5ac(G@3$cyd8Ausl94tcTXaQHto4m)Np z8ToIG!=CZxuEjNuc?6;vA7a}T&6hjNZyrAm<(tQ!kc0^!mV8LeMfvs2=df+_1aTg} zo?s67^(^3!Urz{!{CYwO{%#y$grk{od8DmzM9AGg_Kpp+kep~=M-lPmje{Z3);J=G z!#55wA-kyLtg6?4tcFNaQM&0v5^F$nN(ui)jo|I$=5!eLwW5t5h5S@5-Vgjb5TB@ zEgZJxlgW8}K3h5D^V!BBpHCKtd_LO=wl$6&#EE9IiEWq1PPzL>@rN_J$cgra$RWPG zaqK40);MyB!#9pRLVPvy35l5kVueg0v3xc55Q=At7=7<{+^`<`A*`bcYGWGe_jnqs0C${xQP5*vAP;m@@f@axTh? zUBO|S*iz2p#g=i%i;Xzs#l{@+VzV6n4~@f~sU#!+OXDC9cSP~u$C#6ExBmC%0htr+ zuBk&lyK~i(KO__iNOo2j_8$*O4z+S|9{P{RBmd)B$^V!0GN~FaH*Ut;@L5=x zqD&t!R;LZmviRcmW3c1L4)nt>5m0^1VQhFbAO2O40NIL1P^t43{IzN%T(QIw96PiJ znKrQ?UMms1aGQ7Rm0nK{1 zAMPGE8JjH#g9_F5xcPn)-eLPhklN`Gc204}iwdQ1@BO7Py~{v0-rynDczPF(FFy^| zk5Xg@OwPkDo1epzDr?}-?Ug|5o*q4Ed?J1@H6Hf2v!HdF55exN66ko-6HD%B;K&|Y zFyWRaUA$2YA{IMhm0K9=TwMg~4|jo@{hy*P(t2?4avIRFeFdK=72&J*&!b_gQg~fC z4h`DW6P^0L62D$O3(pN~gjzRGA=91$+&SGIzg6l(zdaa>YtvU^9rFy?lME&LyWLS> z5N-$W{uKm=Y)!zdQb&0AOdJYYw2_i&`oJy*hCuW26g+=$6U-6q#-S}bvdwEZfIXv+ zW4kArD8+6N%$Tl(Z@g3iN=CDAM6N5iT>k)>I&HxL(+#1thoe+1eFgDt3+&_+1AF$? zMfa``rZc~(;Ok-CX~ms^V2j#Ic>VT7ytrfz{9qM>r*#_x6`!nzhbbji>BDJ1huYWPf?ob}ai~Et z-qG&{E?(h-&3C$DH>cZRVO1q{YT_gK^Q;K8bgF}1LC!eA8`1}IYS6PUQ)#JABp#Ul z7P=U))SQkcwAL#v9CmL#X#PAOHYBgbKi`$Yn4C9`4-PPJ+Kry{VFLlS`qmSUu!<^UJ5&v#p7O&{d{OhiMDp z&6`i)%I7;N&lmBS>edrKSsVv5zgmOv?Ss+BGBr4+uM565Nf$5vrbZuIoB{Xc?!mM2 zop8p~DKzZ*2o^Uj$91t5vh{ClX*;uvaEgNxJUikb1kJ}_bVx29p{h(TIh+Yki=}Yp z;Zaa;&v@87Y5-C@cM^^eq@f?*Zo%5@6I2?r2^aWgAdUFlvJQQ<*Qr0O;rA(*1 zj=_WcJHzy=36$G3JzC3lJ@(Yj#g5B`Xr})okk(?1YrmX^2c5s*)vvoCWdj=eygP&^ z7MP)gT}9a5rUaJOd87OJ5G8f(jUD!;!B?L<(ktI6vHODoeA{6F7@8m4$|I ztadEavf2YOBJ%NwRTVJa>mF?S7!UUrdE)18wP4+fi*Wb4uWW;jQC@uB{uZ~FD;Y4pSRJ1FevWSkIL1IIiwp|6e7!UbLZ zrPZ5v;9lcCfxs6Huwm;PP*|6Z)J(VGu6D)J^RAz$=8Vp8*O8Z$MzIFFUS$t;=t3GC z@YDm?H?5*$soEjJM$FEi>^? z&jYZx_Egw&c`B}Tt-~FkO5pbM6r8HC4L=%Ng)Xw`P_on(YM<;2o2(cdH?tN^dvpqY zuZWdpcb$UKIN4yVvUvvfGeZ4aacEii4T6W)AdHBMZlK`Re1qjz+O!Dj{!#^cuB zhi?VF=!TaDxZ#=w4mmj*oApV=Sy$uWw!7t2%;wJctI1KESoaK!e&7ci#!rE@k?YZV zUnv&%D1l{%Oz5!deT|Ldk8b_&M6H~P}4(-ZJA&EepZ?=`f!=^`}Rl7izCE&$WQ9mvyX z0Nv|cC_3dp;i7gK@me zNjN$hqvG#j(7-(qeg^?~N0K7lW700zuk#DE_K5}lprVDfBg@gc69-_D?OMEOhz|RH zqaTbJ5{xqknS)^eE-2w#F3?$}LQhz@8m}&R3a2D|L|4As(cjk#v3q7my1D2Vn)6wc z-f2G%N3TeL_LCkV?R`neYWp%+V(|oLKTL;CtS3BewG|$!dkl4YodrFcPRsf}oDa1Y zU%(5x^`Xs#hw)_VbZisT57mC$FI&{B8J~GshM(S2VRJ5S!Wtjq@zo#Qu%UT5@Gy@< zM-(m5z+4knytE9fclrqKWO%`c2S&0weQ0)HpWe`1CkP%;eTnDdwt#9aC73MT z15)3Y!1Aj?_Vc`L6^AVAu!-$4dVr@lao9r3`Y%TaWw zM*>bOl;K@&J7C>GYq}=tFE}1+)5CRZ(Y;UJ_;V@?x7K|^lYZpm%1gUtAHxb@a;_$u zC6MBadsHY#$`4A$KSD1$9mKxiJ1BZmhSzVdh4-CzW4$hYvFW91P#HT0_4M2T1-E11 zr!ntP)4n@kPEi_eiFd)ZUdHgmp&!(oUJ|sSDFQov)Pt*U8(~XJU3}wh1nzJp8Lpn^ z0CRUVn!J$kre z%aS_Wcr+EmV`pJ$NbFkeWCE2dhN%(4DDp>4u18Ocag$v@8*qVv~*mUx6 zNNw`N%dOr*0S?Et>yKjdvl;Nt?qJ+yPe(ZCS`F5p@l7VZ;RhQ7bFip%EqeK)l-fG& zJC5jb3BDZMi#2)q8eF^L$o3sV(SL0ph9xU>VTigKyXWy!h~GKDO@q?$OEnuz_q&hp zmaK!L_D;anf0e?YeTUG6ja4|V-V$qXPJ!>Ve&XRzbKtz&H_@iJ7OC~5anOIU0&QPV zhjkXOg~d)`IOh2PY~Y{`P0|W+O1=cXdftc&zl?_C^55c8<|AA_vk>=sXH1{Xh-J;n z4#V*M`*7FMHz6|hWar$y0+cRIhH*YBwCJn@WR^?N#?3#m-xe2mUR#OHu#iCWfEpBd zd^LKpd?5Ypt1_!!nv3(+h$z|i`OwVJgL*Z-H#>duJ$!Kb4LD0hpVqy$2LBY*fQQ#v z+%$Bf?3b<)Oz|FtFE@MP_4zYF$hd4M`EUaJd^(ID`0s&F4tIcRC$`|9AEWTBAG$E> ziyHQiD~CN~rf_^HjZarhgr^sMKtqqmz{M{qup{9H$T>6t$%^CfSx^YiNA3k5?k|En z!8RPv#^TRxPk1t-4&zU*xQ}^0z9TvU57~{PMkj2Ob5p-M(t3^qlav00cQnvwnCv z-r1Tjh$>KUoLeLX-*d7-hIuh~ST=@wr8^pxo2~~Bj;@x~TMa;i?Tzp?`~Kjt&60`} zojSqjRb5efVg(fzY6@PY9z(-2Z{w+kTY#Tv5O7&G8Q&=>kkuCs1<(AJfuPO%0kjm# z9_6c}O5rK$XoEkn97j{)wfCf61R@5?P?P&T@G$X?EN$Zi`f|Wf_9XQhH(T1kTJ`5BuJMzu+lw zdPe~kuQ`bu-OW*$*Bca_Nny|W&%mO@6GMLr3eI+5C!Fhxn{EnZ_v3uv&(Q7oMWiaL zUVH?9phnT^a})70{2im@~CA`}MvFZK$M1&sHnL7Q-PNOYdguhE8QSd-lTXtIDv3mK}Yd_iAYP;S;)8 zWk@TV&%^#8*GZH0L_hE;e+xmf2g32jv2gvzDRkE|HBk9I9cErJqYWo3;M>=~ zL-)8yq+HvL%~Ntfog3c+dVGI&(YOt8+UvX2y1-m4Shfc&23B}fdLlwT#=x^Q45*7s zk*{|ZXuQMXSo8w*cu;_x8?4w@AWX7$WNQX*m!)t9?K> zU)a;aZd+l~q+uYc_&$ua8Hc}R7GZ0TzJd*5TI{R`3t-yJ@08B=?rg}yO;mQ;ZDe;g zj$U)zn2w6r0$qlxu|Mjval!B%cnMUY_0bjF^r0BcUSI^v`kuivdKj`zQcpbKv-(5TUq6(0@d^l({xZ5Z^&lp!aq~#oU!C751U`E6yaKE8>s|@XH z_$AeJo=(j-FhS`9uc4B~TdAh`8!5Nxv+>I%2l2U``#w;0 zWipJp*C3n5n4qg(+Sp!v6|GbVfP4D-;pkPzsLZY%pt=4oIIb}T1a>|LwtW+$4?TXM zQtceH;f6$(`aU0;oic)B9u38(>oAD-)sYRkzYG*NX=3AsUz7%}2f25Akl1S>J~+1q zWrk#+g{QCL^zjd2P)IXco@fAS6`Rm>Z(V3)ZwM{aw4sa2MmXo|8#HZpC-&L%L2ym- z5iIG`5e^qmgpS$A@q~e)xJmE}9n`GH!{27Wukrhl;rJMwG+dQ>``iW|uSv$c-)KYk zyGq#i$8FT0nIc;-y97RP@T9M$Wa9HG%5eJCQrzQIISgOtg?9`(g9a83#%kM?1y&7G z^t5+?wErPR;L-6O(0kAb`em(_nU{72vsCQixNWmQQiC3_z$R$Bf-kjmY%=9_tzNdO z&odPJUWU%>5uvxmP}Y>2gS3n^AeCVRZpQtP9h#7g6bD$r(9BaPVuP{F>&XnkhcO0J z)0Ez5@Q2yR>}8IuX2*6aIAIrBJiZpp9C{1=bx;UgSKXrKn> zyJPR{D462eTV|bAjRxK-M=mA%@#c^BDf3>#WWrvGV8q~)z+}K6JmJG;f!Ek9sc^zf zFu1lCRet3WdaF?Z=B=YqGjkrj@tX?{&20i3mIk7#1y?C=>+RBURBB>3c zE(}Gb_v1k68#`3Ft_rQ(y$?CM_6LiV`+`BEwxRdBZ)6X;*?{va&w>xzhJ%2S#YlSY z2RdR=h+{i!L@v|Lf-$<;h@STu)ZD5Dc{9YQ*JFS@Q&TGD7$$(z`bDza5hh@K*#ty8 zUy}`K$p>nScB28x`cTvX$lUvmgkHYsaOTNhvQ_J=Q2JkfviZYIp))%JWJs&fi@9_0 zLWu=debf)`7&=g@=er4Y?4ysZC$qR>$QrEEM-!`!&7w>q6Y-Y*!zjrrJESjCfb*ti zqy9Usz{(NYXtiP-sJOfWEBhCL4Q73iL6`tm)o-Q>^+w^Jb7J7%Q>buIaL} zMh_4#?2JsyE~9cQ6Zl9mTxMQWO*JN`z+8KEIO|Ri*`v(<=*751-28fyY`-$bPacnj zi>8F2PBVO9vft*4`Q8}VB=14u3>%QzWfm|#)E_=sIG5ToK^6a4rHpGjDC2v6b8rS^ z;TTIAQg~zQw;X%eGO|aa4!!Y=Zf{;{Dfkg z^Hfg72{=Z)0HcgkD0S~mylDSIR50TqYN^2B#b7m9kYdmr+nWyA4#)@HLbnwbA1kJ3G-e-wt4h>I{5c4LTjRI0OXa+|aDDb(CF=rqjE1rffuNDeklD1K5_F z3X$>-S>w|VG;Q4i4U`vw&&R$Yo4iVFqMsk}+06{DD1C+NccsH~vvaZj&M;IiT7>PgcghyZ z0P5g+6viHzgbnJ~I$9nKg~{{P(PX3t5A93>t7>P$#2bk)b;t<(lG#aJUn;_}k4Ce- z@8;6!y~63~$@406ZB#1b4vfR+Jqoddks9q^=)wm5aAr3nYp1%MgXub55xRKo0?gH3 zPs^Z?4x4xzF^j$FijHB>Gvf)oaMg#7{-lP3cZ>o%b!MaeIq&fVW*mJyAqri&bRT?Y z<|0w%CbYt>9 z+W7Gqw7S%YPCM8MMp-R`3r{S;QxlD7ymvRz`VBnrWIk>T3dFs38RH2Tli;yNW&Bu1 zLB;vDtT4`w{S@LP+aEX%-P{>2yM1E-Rvy?1&H>7x?p-j9IC>Q%z_GA-a2J+cp99hp zdeLukb=j%s(&);RiS*CzG}{lq!&I0Goc(J%PAyAgU(8WreckQp0WYtj%pMRPJ+T7! zP?-w9XL^Cu`G@depUYT%MhO;&ErCOt_ridjJ6LU{GR%JP4DZy`U{_AdLmh6}Q*+jB z5Lk{iz|*H#z{a1qfQ#=Q7_cS>mR4>D_Akz2=L2h5QC=bWOloXp>6Rg2?B1^IGo2!K zozHQ$L88Yl8D{}Ac7DJygAm;kwF{be9!#Ix9glODLRx9rbMU%Y8*YqIaPp|zimH6F z@KBEn0Nad$&pUiUO*$<=yniEo=F29UUV9o1?V~AO_0$qjHr-H1k6N^Gs2y^x@j{NJ z&(K2W-ss-I0@ly05}$}NV^8X9IXPE{v3{mK*wGd$?A*XAsN(+`K7D?Oe6GKo{haPc zd+oGg=gR!>m4*tu+ads}r+>ou^-g>*F$o8DG@w0=%fN2E6#9bU4;(aOj0~;qkLT?Z z($eV;?8&noV9ga}YJbmXXo&p@WNp_Q-KnjnEX}?mahNq)o~I8ESR}!o=d#&jCjIEE zU5Bv~ANO|(I{O8FK0&^lyY0sAq*v0BmnFzoYYp9Pld01zx6SmaDL3G`T}!b^VJdzW zAA!n`7QpW1XX&s<6)e+0&(R2euC~F73kTzR&3u7hLLRKD3t|@;uZLl8yepme*|E})u}-)0ZsU^j z2-dbK3!9%Dimr<~uv0!(;>Io81QYU9p)_bMem=yD4!O}Ct8MuJCp7lO_~cYj&`%ex z>~{{{%^L(0XKaV}KkK82FVpdAM+@Aib}HO>^BAh1JdLWqZI8nT6oHS=M?sZQn^C&g z418s^0zR6l&5FAv!y3~QFmGgx(>2v4Fkt5F%6adEthf3`*uN+mH_p{!ReMcACF_>L zkDhB`(3<{u^zB2q_w=!J9(B2+wrs@#Em-<&ESM)NLI>i$0J}{SaJ}9l)bp|xsuftuM6Ob>Vq_mE z_%2`{tL~&t*m^u?=wSBz=PvBPOZMQ~Gg~z7=M%U*?F)NxT|AqXybKGcID-RS58~(U zr=jIx8~FB1Z#;ilPr7G^u`txnpDj)<0YgmIz+ELyFvIPEY)^!X?8<-Uu znK88;oW1o##_m1_looWR3X=Nad!HTQn@%P$By|A}JHDH~5NyESKC8r9T^>~VXxl*A zs`p;D>p>Ac_Voz3{Ej)z_B5^Rkg^5O4~F!c+YGE=^RT^XFx%AqB6vO56Gt9CjEWR& zr6;_6ao_7D!1qHr*f^&v9x`hTi&IT-BIy%*9M+;$3O(TNUOnNg9`o^=;qPVC3j;X3 z#2Vi_d<_}y-7On%xDY2FYmqg~No7|a$iey3&*Bk#kHS;o_mQ{V9xT3m0iyUxz%XMu z{lF}PmT3G%o$=WQZqDunGe1?q{+VyVhthrYZ0i8rwPON)9IOwDU+jfL+{Q^?z1xP2 zPt9g4%ERdfFDv-w^BvH?(iOj7UM~w(v5<~`^cTug?+3LW`{DGS#j&^w= z79%f;u}S zt|z-?X;(PL%@KBwnJv?qkcdYA>Ih%OR!Z|~GNHN3If!1E;&HZ}u-g>{x)7{FkA^kS zIaiG;KkiEbKbyj^MveiytbPpJJ9iPSxTYFQa(Az+kD400jZ8Uv3n zbpWHJ8^DsOUD%$-jM-k7Eb)W#!zk$Ibkyg81Kt;Q9T$Li7;Z_x?v;N5_Z_z@9$gi{ zsKO<(eis^X>4;-s(8T3%yE1_1i+6y>73BGbq2uwxg7vsy0xj#7zXoM|GQ^gFb8u)$ zGbpT`1|nA;E5(>GG78u=`4g(1q!%#PA~9%)Ff86q9ZOlGYNqc zNpSUhr*d*b^`^XfOPruoyk@PiC`~N3-u$ zN`a%gDvtb;A#1AgK?fuosgczg$Zgscm=`#TzG@i;A7uT4me~QcpuZ2+09WaqTOZNa zLw3;03un{y+F`K6TxI-bjRDN>Yy(bQ?_PPN&X>)3)d*9qHo@X)_H6GbAsDIk!eid< zXWcfu$M(Ck!ReauY>#dZzyRk$|L#tBOP7Jj|B3{C%rK@(pSvS`WHAuSG$36a1iXw{ zFyq>0w5+ZO9_U?$Ze<3t=F2X^ntKw#`ZM8h);AOOh2nj7QZEBKM$ZS%y8IBA2Mor0 zLbGKS(T!~W{2vtyy*2~K9upk{%OA7URTVn8%QFN=jDZYHWs@u`FuJ;ittl(TwqCPA z=A9}~(b$a4s&`R8*1krMzkCPaul`{0xLaUBPB>jx5(w&A7Z zyXx`7m$R~}Al9b0kX`nz0gqKqMK24Aph=(Y77R}y?8)C1qUovy(uvh_fuiwWuZ8`#RKo)#?^A0pHj={Q(Kj40k zIB5EQgRJsO1H7k3p1CPq&EEFb0ORxAuwbzkeif_&c0?A)dcB^D-%u{_rdA8}a6Apn zg1@8BuMOdw0ZzF3o;kYo))J}3-eMP@m{0Ft_MIJX)}iuJ9!)o$o+t1UO|-apCB4L{9abCB$8&u6l->-x}exda-RPc)Ik>R@Hb4uV=;h}o zv)>)Y(+xTW;A!qu`s9wSER%5tTq>s6w<~pE?~)1dR#X|B4iC^C>EGc`oBp(LcqUA` zxSG8+QGvEyyc#a6c4U>x9^ob3_Q2aaW7+&U0N3{ENe|wKBc>ZQKWUaMe$%&!R{8Mi{{6IBo>3a-q4;YNT z)x@FDmG1b^WL12B=oq$TT{c_4dmNmSWKy}Qt_X|B>%`^ues;8KE&KdV5h`guk0+eZ zW$SV@ot*bMBDWVE>5}n^xM7bYt-i7=-X`4)E<}HzANb9{Md6yPukBX&^QH#uz9$LK z*>?`9E5~7bN{H9L{EGT{J_hpzBjA;Q0`#ivG61eM;OP!?*{EYWxVvj7I5$IqzOwQ; z)~vN)72k$JPiLyq@V*9XEnx9w%|R7=?-{f2zN*o()M4z^NCT%QDiS($mlghSSs5F> z8whnaD$`fj9EF;@4?|VAD0FXt0WRG)pH4nq@7PJ98F>zlqb`6t>GT(iu~nTN{GKYK z>VK)h3l|R~n@Nk1UiYrBU#bCA&1}XI$I0Jx#>d##M~>reQFUnZUs-Hv;C(uDSTc^w z?_Bw)rim?|RR<3Vj>^1kZ?SA@EIrs%m&Q;4j~v?y^B#3z{XeR*&&3;|xN;j?%09!Y zg9l*!umC#qc{a}K3Ha}EPnXSmK>p88+|U1?&rgQ^51*e*9X~`ScF}`NdW^tsVgBhKjdP^!9p1K3|Q77pyFC7XQh9yFd~1P{&ErVNj@fFWQo zE<1tYgwEnr()4>X$%=yhKzR<}dhBcAQFS(Fen~{s1LXb+9SxfPnj4NcoJr z;}Y_j5cl(M>+1NA&xMA&III8uTxf6SwQ$Lfg^urv3-Dlf7kDNu8TMCr28Z0rf!YID zxYEQ(wkNCx2iwwcfrb!@`e?J=DneoCoN=J>!5~`Hm4?X*)o5wM7Ia>{4}CzRGnUMk z;FzKx_@+l7K2o90sy}>$W*mzJtBS9Kv_T={Sifu8A*-k#^;)}`oUMYsw5kKqs`g4b4BRKlsT}Rd|x{G z{(Saa@e}O4?gE5`6X+cdv*7tPX0&;#Ev#KMl&yLD8hhocvj%skzy)XO@l?;b^sCWI ztb5sHn6@e&=@(|=GlPQYZ{Ixdy9>Qp)sqjG3OK0b4t`lQ9t)DX zuq9Gksp=1J>6PX4Dir;$OQS-J94)7Jp}y=jt59CAEcNMkSP*4e>{z$NdhJ5S@MBxgRpCaMxWgjY+JR4kXWcuhZLw?mSLIf2E{Lr>#-J zII|zB`4NP+E>#kkJznH! zdC1zK_OBYp@x7ZJuk6y7euR(90}Q2(0q;gTj=Yy3{dsGdbZ**6N4w*m6=q7kq~^C3 z1qMe`q;vefI3|p@5acR%mHrZ3bl4@m<+xw{Mfn7YrQ_+!MCswO&Gw%kr8_vL|8O{N zxy$i|VOp8Pwu2Rm-$Yf^SyB~0JbqTV4C^E9_RLQDbntA!_Q}YM1n+=2x7MPT5x|y{2a+sH6HxXWfdEYWc|=)!tvL z=;>ud9UGz`4S!x!ajAKuW9h*}$F%ARf(=7*1Xbo)<#WC_AWK0RT9RP^WHF6sw?Qd2 z?P4T6+|3#Ee%}MtAC^$J_p76ammYx_$Fs;K>65g>-d#BJQ<29fXa^YY=7+>7w>eAGxGq9L^uZ_ZD!O3O`qU)uw zW<3q1wJqecg-=NLu@T+UXo@#x^uR489oTiF_F?4)BV2jnHmY&0#!6@qoS7AaQ6bhE()E>4bu2>}UkcD^6G)u$Y9d6)u6#!jbA^mf3Mg17i^z8*9@_6#1J ztIEKOM_}oGj4y^MupU+R^yjK8c;UY9@bepe zWccbHbUjjp-#rV1vn(Or@-hhz4y*=0W@gYRY$?2E{}q%h8HL5&>#*7}ijLRv0ivFc zwBk7%yh3LcD2tKM7Q;>H)HE%6%Ak+b%A3!mYx@rYDW7jpuGh!IQ1=cfYq1$xe?l8r zzLJX;k_cWI#} zTO5^cy2;2?d*sR%9i2v5gezl1yJ)o4b}JAYcR+4_L8!ZM8p^GYMLN$~WcyAP$nI!# zMox~3NIk&>L~b09245=$ug>3+4&9JVnY@dY-MP8|J@VWH7JDhd^~`I*H?t(LElvw< zo;MBNO8-Uun0y1hTD%e^Zk!C#)CPbYPx9UFECra|rxUek&?hu~g(uY7d`MvLKM5w( z3nR83w*AWs$6BhTzqjIk;EE6jUB@0Y+=yrykZ0K^Gs^B75@O`w&ep@ZPbf z)OtW?Xt{MWT#}cJ1m%a2-ItCy!?6$geC8ntt`=d>m>S?wV2R@TM1Z5p^|FN{wt%4R z_t0s#!+7LAJ7nmr2qSOIg7Z~dU`qHiwB}F|tSvt*OFzB@*POP()N{Z$5Ze45Oik&7q~2rTs)a6i&xb|q%V!UON~fXhtEo@HtFhxL{ZuV@ zWZ3`5+M9+`{r&&`GDXHpLNZfGnaO^ywRVO=rl^#pQHoS(&?Ixpn9O6wjHN+jzt>tc zOB10|9}TD^8mLIa+28Mt|MkCqH_rE5-*ay4yZf@Qz3u&4ujk|WSXPh=6Pd9MYl*?P z2=#1wyO;oYoz07|<~5McNjD-3*vgCZ+{ zx%x+vBGQZN+PcAW%k}KC`d7^Q{L^H1Lj<|PZDGx@9Ng8LghC3Bv<}7ce|2crO(8OG zPa;*#HX?hDFC>?;{vnU$orq$vE;HF#$EY68W1o*Zke@SO;7I=5g8sI1B6psF4P({d zgscrIF>|6$*3(I8Yca`vtOh4s7LwbEH}GC=79;#Em$xPLB)g6kf$0zmw=S%}h$)3y(izrtG%BrMv5|kq_}Alqch2~q`Lp@|!THJk z-UVu4H-}smnZUlS&p>!C2UM#)0V*#XnNfq+pv842|F^fC^CA2)xF-J|KheR^S2l_= zpU;xg10T5h8>G2giUx?L*IZh3H-^q0c13ePW&-VFQ@GyKZlP^E?!rx$bC65lPq|xvDX&|_~V5(z4X|VonHSDuL`ok(^tom;j%}}ibx6aEMzS=YG!M5ZZSh^?2nUx z-36R`^G=YIvMhEm-w-Fnc+tkAdMNEcI+!*?0PSme1+n24I%QKV=}1vTw-Zl8nf1>| zGblh(&XV-DQ8KjNF$djyP!C>AnG65C+zxi{zJ-;;wqpa`N36%{BAk8Cm5RL4g3_6{ zz-p;T;4yiE|2Hydaqum0p}mIt!Ru19Q<))3x}Qn5xk!@CJEi2PY7)%&kVzAq3aIhf z4@9NM0ZHr$Kur#B;MY;9=0#y=;76@5tohb8D5+YKw!BkE)hE4Zlza|+K#1yE27T~y9k@SA?!2VfjtYxVknI<{_wka3Du(x*X&NC9E=;Cy^eCQapyBY)6 zuRjbIXG?G+rIngTGHU1oCnI`5#SryPh@c+R2QWcw9yL9&1_jSkB1blw;N?wKfSg-K z6ECHL#*j9W_{5%?6iuT_8&5HL8kfi;{=C@Qb&)wf91ocgdy?7gN$v%I1oORh;p3~) zdl-A6bF^xqXAIxZ(h#S1&^w zq5}xe=LRDm1If?z@lfH)PtI2VjZ9#b9DH{0JFd7li?Dfbd7%Tsq+KZ+=A7FEe9tnl zW&2S!K0p!0a++ZCqb?}6y@9^Xs-f-S4Ro|43JpZwra!%U;r<2PL~uA9H&4E%$$|&z zo4bI14>cvd>PF11$JucDOH+90Xbjxhau06br~`NWlL`YDNWq}PCn3%{fWpdWkcEeR z$c%H}@aBXgKx%w35$t{rs?l+f>Kq9ZToCx1UILUw+j-Od(qX}d&q!nMW3s%Yis>5m zgieIK zg$6Gzxbwzt0Png+&~sT5it^9!5B3(}pRo$WB6BlzHoQ+W`%i++mJ-Cvs}v0IxhCap z)<}6cl2rYf0QJ3}(C<|-#BFRkir@Ymhz!|K>23sa~1T%xegM;NV2tk zKFoF(p(U;d;mRNGB%$ z`2why@USh`LAQ36!n5QnTAgf6&2^OM<}F7^S^6}R2h!oK)rnZM-Jd!Z9D@5Cl*lcw z1R$Of#LHX4l9}(aSl9O&tgC1c`MWL!w0pf}elOC*G;})N@?<_}7n=pMY8F8lrcd@} z)nMM51$3eD47B~kBSze;6Rk<^MZvwgtw- zco`_fMB;6CqriG~N#ZJ#1ve@tkV=6?ba2ljI@e5w?yu4y8dbG;i|TchRc=8xEPo6X z@6}NKJ9Ck{bOU?+?R0w4dnwDESV+fBt&q?9o%oF69;$xUhhE3W;4_;Wu<>CAHr%_6 zjFfwUzMK8Ha(61L@Ma6n$iBy!b?qsb!Ml&&xWB+j6(@*U@gQ^Vh!6}Jlp@!=9N|vk zO?0MvF>U@T*gWgyJhErXTlBm30{SH#NgmfbQ3v%Us98*bPCYe;>$~L-99FW1op%qA zhm%RDNM=5r`{EC@|DcTCjW=Rdw_8xFcqua?84qtsc4O;%k4ewD8qjK{0N3)m@UP`J z@Vyh#@Yw4HP~gzZ9)0l-V~{fsTHL<{w2eM9y~o06=e?&iXrBl?dU9j)7iCR)rX+$^ zoeZScmT=J4<}4&C(?;y0x6pzWc3jQh#?7GZ7`&3R8P(0Tp??}A(fpclXb|}iXo`}d zgW;y|V8c=9RWJztjLRYsJvEZNmxEKHQK1(Da_1R43&2gUuvXh7f~zPL1*P7$3= zyL^l3fzF$FiQi8$kT!?vKZ=7+S2m;BilWUXRxyM&Y#>{&E~Uwvj|1WZg^3qQ;~2f4?7k=oK(&8C;4sbyXatf+nnsx;QXT2pCA zH>(r>&Uu(;be(+8(Irj3ipZrR0&~BZqR#trBreSbpF1oI>B5irj;{+S|MVT~ax{le zuQO!IBMB%FDYd?FnNa06L;IB#jc-gSkv$C$(Qc8prPe4mgyQ!pr^phK52dLVr7I^1dGuWGS z1=zVstK8M}}L{pp54QbZ{{jCB;4@i#D%j6#iBN?b(;%XJujP z>2n&HY@LTRkIW@wkqr>l$8wy4{=&mn^>|nPKQO1q9<%O53EP@ecm(d$+9?)p>pm zKbHCo?)!_9=IM*!OWAhT_xmZV@iPe543uEaz;rBpV-{R0qe1ld`k*#l9-7vD2%ZOp z+yR&KRIKk3jhI+LkMLU1lSoAxv{a1f*K{!JB#UTmq$^o%D-8|S`(ihhtMEgdcyo4_ zBH0yw6m1P&Nv|5spi1u7QF@IDaG2K*Z>`^jq+}Ffz2{aq1hQ}|pR=&63B}p&*T9Mq zJ<_BS#b~W61KyHctgmzhZ@m?YYO)Uy8?|8IF7H8S7{sGJ0uE$}up=(JqD3BsCDEQQ zBT#6*7ZhJPhuiOMiiRHgp?OmjP+j_Lu82|sRW)3L7CwFpJ(5Rhs^UUgpAk>rs|cal z3Xq8L_xgwT#en=Yim(5}B^7N_Ow8FiFyN^Sv772cen+dax4*5yg(}G)Z}xoBGEqtt zMk8pxiat#rRK@q#|Aj&ag_=`kO=&=TB56}egmo1a1jdQOK~n+x?AmntL@@>kblkz) z1H`GPV+EY$vmA(A3a72Q@8S75cgXoQ<7Dkr%FG;gCQ1dJSXj)JeG{3>Uh*ix3phte z^hP+T%bpdK5=jtYmQrt(9GC!I<~%o zev@2AJtExD9I>xN)^8U*#wu_(`emUEqwOFik>4YT+zajcTaoyx9>No70B2u+f#qL2 z2>C19e0?>fjYgN?KNmajS8f}w+MUQA8Cpb^^=iR3bKOI-gG`9unfIjY^BQzt z!<_ulainKlpOOBtL})mvLf0I(0Sf+8QAd3(ykB~RR93%%K0~^6O<^+Zy}W}wpKFZO zR({5^Rn0)eDjjtBSdjpiRm6Xa38W`1$q$QPtbDi!d7SM5CMBgw`L!|lOz=~3=+t*` z;NCm9u~wcYy%K}-`s3i)WgDBeTc^T1ipj)k!&3Zqd>rHXDzM^JDI4NY0DhIql3T8n z-Lv5)^Q)~H%opHOV*%!1FjtRq2Zy1PdlijqdkFJX1Cf)!5Uux-rnKQ3jQcu5f^#wc zef1>0E>%uFFRIbo7f!>M|7h@k|2oOvkw_ZP-w`OA@jW-6PSdPy@_4VzOCoS12W|?y z3;wkq61}jo=fBCHE&dPoCnlqcWQL+7H2Dw-%@@yy7LR10F~1A4t=5xl2ozx_?6P6! zCv`Sw%>}$!C=DhnrelF3P4Yl?Ba9ec4I-Dwk>(OFes?7wOzJElc_(hMm5eAT4fi90 zb!G7E^jFx`rk3~abR9U5q67JOD(UFSZ~E8kitIjr;=jqCE&qS{bKIgqtLB4it{z|7 zXI1=3-|FQ@AGXM5+GV@kUe0nAbvD|(g|pG&lvO|XQj^#_a#b%iqp7Ry2rFQ@l~Y{n z%^DPRaGDz@n?{UFcy{Y57)OogoJJ+cc{#d_Q#_%-2Hf1yBzC5V6F$?vX}$eh-uSxr zOp&w`EBQf!b0__+Rc_>Ij+B->`yxz~(cj+BtLls5X=xU6ezl%&D(~OI`L6xS>XdzU z6J4sz%%+iS*y04v=Be&1@Ad~y#%;o;=}U6NY;u?@hu-jVPOoP~J9{_}< zU=u4*nwx)``TL*^N{(tmxp!L7Y}ansAaRT|2wWvYE~a$Hvk-XXqX4r=Pm0)Hsv$3@ z3saYrU-(&QHBMAd1<5iT?5n}g^)&pTeYp#P6l$a#dLYtg@ zV9~y(L_)e07AWbXrqLPHKqZgW)6NCjsl8Zz@Fw|HeI0M0-oT}PK5Z)ptgD(NyeFVR zIigEWW~;L%{?yBAq6Hgu@chNB!@xL(78J zRYc-8>EmR@D{1;T;UI80V8}}?3P3+krIN3k>minR!rH%8sK4H9fuo+0i_tT!6#u`;}iIepREfQJHf-gfCFaKLucMO zs9vFkK0k@!%` z4%g1i!Z-QeD}SB}{n%ye?=z^}eh>?NUe57axr-MW zX9zXcYr=lPQ%$Q+Ig_J`QgDC27D+PQ4Vm2K#LrEHgug6f4!Z7#?vWbc=D})q%2)+H zDrQfF4vLc9AA|wCR7IL1_)a%A3j}A4gA0KtV7!e#RC#{+s@RDV?Cm4X7`Hl;xys%k z?65Y3zjDF;u~$sk?^?Wg?Of=dy$Ie^3M9JY!oZ%Vi^o*j;Z;9%7-=R#O>aAsJ4eT1 zclRTle`=h~edh|F8}Q%5SE1mBZ72u~$s&PCN>J~!5g6E62GTkW36n1X+czHsdP}@; z??4PF2v`O~?#{+*%PaB3V;vF{9tb$&y(ICvD7hjnfIb~k<+Hy^(B+;Uo%pR!Uf%aa z!b1yy>8Yudy%Yn5FJN48E0>sdhmnf~XPJ;6*GRqLRMeenM5HV8z+Q;Sn+=BW=dOjs z{?!6}WnKh`@;?Ao=e)p=-Z_%^3Uxdb`iH$MZqFo+L;;tXQ?ONDC%zbXp1ml};tS4K z*wmcAY;Aff)8~1ZvtoQc`{%78_?CW>v3w7PC}O8=0N7+|2wq|$!`Qd2U||r;EVL}c@qsN&p6pHbm+(d2(jr5M@2z94?}o6e z#hu~xBNv%~s$C#=lM*8{mdU;w4P(QLF>AQ76fYf%XP4j50Ww!EaGKgunVxge?2kY{ zj^?@=&L&Agyi=n9c)M<5wtf=DoP+VWi_bc@L~!uwTe2km(`{x|iXgl8MFAF?WSNlf ztvL3F8CIIiW6=0|Y`xQl?*?eba(A7WpZgL(oR_)PT#*xamq55ES&`NAwJkaZiGVHfMnI&mOQ$DW9EG z5QH-QI@qAP4$$0C5NfrTt?|gfVXo)M{j2NPO)3@aL;Gsxzzk<#jjppxKBZxoeU(;P z{4Vw;-z-)y;T-GHvzhs|ToeSq(8sDV40xMW%se{w*veUH8{4EY9XQ+^;|xAih2u+h zfZ<wmUzH(_qrguCU+BDz3kXpDaq@EmUv=pLK+Y?2PkF3|-A}9r()5F^FKV?<7p` z?r;#9l#l5#dGO~`9TT$4o~?cn&B~q9Vc!^U0(%6r@$#Y*ppVbA-0rr=;cW%1m~a&M zZMYQd87*P2NSUy4jVJM7k2h-(oX#XU&Sf5$R^xeYOW2YkS3JCQjI~>q!dAyi0^jBg zAW`TB+V6Se>w9d;+PO#JH^F;My%RtVUI581;j;A;(LBa6(#MeF(rac$O8diBY_H!U_lVuDr?Gwe1E*e65@fqHK z7UF-+;W?diod28rdGY^){CTr$DCfTVFf(Eg=yARW!stdM^QoWz`P$`35-k)k)yW4R z&%a~eO@9is%9g`*i(6S|B*I+hE+$8wTI1EGk)Ue77~X^~GPj?eVQxKn%FE4cXG8N& zF}hCq|9*YqXRH7I=loe(;D7j|`1R-i?2o#fx|ycUScd*i#?f#TPFK87q!CZdQDZ?O zT}#qY`vyccZy>tz(P{c|zyqCoB1oHDOqs);K6FQZG*wKoN6BkU(b216bQ>=pkyarp z)FDkMrsb$XS4xg!C_euI5-Vc~9Exnk<+Lg2U78KLbL@a*eCW@GK8tBNB2{`@Dc6zHZpYU!t zp`|HywDMOJ8PxoWW2LI##~^h&`hGE$c2%V}$XdE@lQk+V@}~;OgnD{OQ0<;==xL=r zlAU!A#z%XgiXTZNwU`BOl$7bp`TSn)$M5i#%wF^;^begbIv>mK`wWMt=97`OSQ^O{ zr;|1OY+xiE?#d5fSF9?e7Yt?4*)tTj=j^2OzfD0$`U;5hKQ3sQ_Z)O4e>L6FqKy<{ z)6lB!PSRwof{u4^kl);FWD-1`_I})q7WvIXci+g-R|~>vz`fU`!*?p>PX7W24!F~y zz#eGPC4y`$W>Wj7$H?+m-gI1U3$;{x1qHJ`>CLZE)Uv zk{)eX*aWm@*LC`pF@pYwO`G*+oyF%a zUjbK3W2kjv7`>RwpwWsiFg7X@rG-wQg7o9G>DXF2OZ*TKHu9p5@;|T!K1a6b4>LW& zXW$phIHbKehumwONz=&}Vqb^B-k4bOA&LXd4xRyNqXKaLsS4(swm7U0HzziBg5^Y=Qq-E|#qe0=~8ytxV=xO2g-ueQWVa3QzUVgZO%UPulVrZUP8LV;?} zcd&Obn265m!A5g2b+1){-;@w}Xf01%as-G?PzJf4^A{!VN(8gU`8ztLRM?r$XK-}+ z*+!Bt5t}T8ejNlo7&szAEL@$8B}IezPp^VI1KKd{;Wc1UQ4Q8|=8~gN zY-oK$9XR9K3g>(=Yqop%iR7YJq+{bf*wFf!MA-H~(=`Z({r*S;hu@(N2}^V`s2o0> zOoQ15I@C$6ggE}r!YKy1$hI<@p4lIPg%}s~`am~<-}wA_g*bC#0U^n4zZgBLOm@x= z#kayIfxeX!-0ba%FZq_@GF}h%u`q|bkE@YrwSKElre zxXJJQ^8LvhRwIjhj`Ts_FYrWYPxE`BwC3$cdeMaWYx+bDpl zm+B;d2#Xr*)4GF7YRfhU1f{^n)}`oOv>I*mI|-AW?+~%vTI%p_XY&`Md64^Lm?^ch zCLM0B!05tS{DFpj=9F<@;|p(vDjiz=QR^9p^_x{&U^AGdVmaVZPnw&(HpQbnO?|4n`Lp?EhvS6VPGf_A* ztP}st@S4ga8R?U*_YN>o{97mfXHm2))zpVq?E@I1WUp z&AK^^ROD40I<?> zgSIqg&~fw&-q4Qa?j1YL?d~}Zc~vGTMoYf=Q2b}`=gMAIM|=hfct4Zdw=t5dh%~q| z$zjO1ONTqL-IZD~K}2rnT&~+j0rYy(njF6y&UHz%=dLI?hfcOm;+0a3;5m$8LYtR1 zt(dsM_P3m8Tt|!W)~~{_v`h)=sHB0yXHgve*^)#ktPm_LHzblKpTNDc4b6F`G1O2( z0x9!ng{FmS=zaM-bhE4g&T96jvy>0eHZ^-X$0LM#FAydZ20i4dLCZw9e+ z2&P+ZaroW>Pq0^i0qLxcNA)d#fUw}5=B6&c=84@g5FR{3Bfiv-CqI`FUY$Kz{NV)M z5be;cFy{nm_-zGW|IUNrk^JvNt#a%gaRvXDR)rEGQ<0L!Qn=#VVfwkZofHWz0Y?v+ zlF30+wB^=3l5DC5fBWc=`sGuF%OvF(PkBui88q+&zmbKjs%x`Mv7aAuTAhCXpumx`ug?7Em`XADo&tm6*p| zV;VBvGR_05L93S;^r`s9XE@ieG42mZ_ULP*gVv#xdFQ!F)9+Ey3l6ZhcntU_Ex={5 zg~-0vADvMeqzbjl&AxBG;hLz0&4Z`fp}~q0bSX0d7CzjAgE*pGuf>nR@}Wl9DX{^W zU*~YQ73fo)*sb7d|5^HA?-uOS@rU#N;Zo>S*}}RgsxtjgxiG&s0DmlufH$oYz{@4h zVC%O>P}5t8J9r#x`b?l3_c(bv&6!pe&gHg9K13s5 zmyz$)Wn>$>2wl6~%oeQ;086jPzXkw~8fQcRRWQnZoy$eF;SvSpx)^Ma^1%Ax3| za$d>BsoBRthtrI0OGF z$tM|#{QO$ekEq_YAXmh5frGRw{Hznp8OdFZ`#Tz#_Gi|N(V?ltR7`_3wF*)>s7(DA zcA)soRJvNUjJY7Q63u8?K!mrS1I&|~?3m;~L`KbyyKzx8$+EGbL#d|FkIyN*PI-rS zeoQ0#(g;|uMzHLK1fGh?Blu#`X|lHBJ89D4f^@HyWPH0i2}|}TR-KDM!qZmVxNZkH z#QlM53(RnWW&yNuumGdUU97)r6uH`$L4B_IBA3aZRR7*TbZu%0fA*H8Irc|s@k9*z zm3WZcRXGF(`8{mW!=K3309AUspYJgddJca)i-x9uFxFnl($w%@utRDQDD4Xb_0O&| z;;b@lneGn!ff2lXZ7$hYrAn?zssXFRGk~XvI-EPR6U;lW2rMQ}lXtGhWM79mx%43( zC}}K2$L`wGGMilVqd^op{asBzPb;LAb2O=Zbse4+*bKh6M$oMVm+*zH`RMST4DzE% z6;zjOMU8eh@d@V)I6bKa3ObyF5r6nxaPBnZe{}%=Z4^ZG{X3?}%ZQd=SWT?w&L<>` zfQqHccyd7oPL1fn##3D3SO7y^L-dKl>~?VAz$s$*E`mDN2@x5K5r7Z8YOaiyK)|a4 zPVsofgkC&DGDVHRKXD7`vTrur&Q3+nW`$5B>!3xpJz>dqe`Px3c{!PxcZAArawE_7 zB)}gH5g?#9mi`Hx3&uopLAP`f*`Fps)VOO|Q*&)16#>AJURUrSG!5uVKLjDWUEyKD z_rTJLhbuj1U_Uw=xh;`G+K2PdJ23_B_^m6*I^F>#uAE?}Me;e)7I}EIY&}AP^Uyzt1gr@C#BN)F$Pa$s#eI_-`^*0W{;FFAjt+MMtC3yMKT3%DcvgY^n`E1> z96La#&UQlgzq-&L4O{3zrWSJ+yAap)VszssbNt%*GwUv91rdElM)Gfx6(vISZc!zy z)bxW*vsy@6dnD8fsl^9w3s58xjniTQteKWe)YCVUn4PD9q3;T48#0@J*N7tttZ2qN za)n@zt_RT1UqZZ6w!(MoO6c^&i%9igKXKjHijMqtLK`^ZXyfPv&9Xm;-ndVY;VmMyx-izS(vNyuyE5+ZO)9(@hjiGrFpL+zLxpnP4G1XL1ox9cpt zk#Gxx4nW8ErGetQX3i5KQ)1(-Ni-v7LPNb=e5YK6#Pw_@!= zZ<2j~^&wHHQ{@?U&ZEK`caw@#d$f;p7}aNs0UEs;murs_b^Qix@$)n&9OL1%(rE12 z`3pyhsK6oq&YJbp!nUeOz~j*byMN(k)Kpu51~MI~m>wZ7lwbxVNDO5 zuZ8cAJHqq268MhM8o1+!4Vw`;4}M8>gwvYDaQUP8WWUoD#wt%1gjxoIgU9s1=82c! zjE)U()~bfeoubs?Mi!dpBgI`>7KGTdb7Cv17G^3yu z#&%AphR-9RQ&1f4T2=|qM(;#j0Ucm;+ZAAX!4n;NRSSEz|AY$j1+i1WU-sntOjadC z9_!KpTr2a5*$TSA_mN7x+suYtwdW#)SLJ||UJ7vh+S()?l}fwMPNjy8iPY=~f3|;C zf<{k5_$=%iIce=r`g&xM+xS_W^Qi!t-D<%1wrkQ(jTAC^C5V;cBRtO}a^mI|GSg)gi%(w;d&QrE^)&;ySmq4#ccB!B8q?sI2K)ntwqInU z{pv|;eIL_(+#JR$@ZhhUnQ*g*Io&j{7$otTma%vruI=SL=t}kn*fAzTf6g~S6Mv^r zp`T!5t9o&@82=OWRoYNW8*AN$3pk#~-tSQWdM(5J2! zNCXK%Hz8f}NHiCeR!e|oZ(Q*OQw5x%-Dg?9Wen7xDkkf7w84;Z709}C6H0`IL(9Yy zRB!u1xH%|}c8n!Z?$*=5)|k@2XF6Qek%{P2)WN<#e5+UGx-$d51B@pB?b6+v{+c zi0{9>nVg3Gy#IpVFy=2-|380#!HPO&$L3bNF>M?RoDf1P+r`L&r(D!w`5Kqqa={z) zj)TgyN90wV8D2atkNi$Wkh;WPZa1*q%aX_8#+sxkV%=Cx<#%e<&B=OTVGOw_muZlOPgKlTQ?ab*! z^di6K;W>>ACO!l&-_(Lxj9ffQDBpY8;!hz_kkYAyI zL8&TKQRom{X&nI}$bzp;bjT*nC}?|c4JkB#46p6Dz?^l~r{3{1$u!S0{^X&6Bv=(j zUil{p@AD<1Qh;vRzZ44PSCev+5^z!JGJJj}5zUGX0}m9$a8kzuc;WMLoSpfOlj&H< zs~Ich9R1nMt9sLErTjISeblm&bJ#$a9r1j_Zaba9OL-!|Y2t3-$z~KXcLnb7uD!|O zNz}Wth9gQ$yz4U1IMTz2eA~}Hzgp9z@x_-79ZO}?tNNIeZhF9Ocq?yLoh8s!m9-L_ zo8IJhIiD#gN@SPW*K=;3{b4nKxtx{U%x6vVet8TdU&KpZZ^-7c**m9)6)G0S zgL9gAqkGoix3MdD8#+}Pm7;w1a_VyEtg;T&9}p%BZB3ZL%pm|3<`VCsTfDojmVnyH zW4ZZOz`eVYEqk*Q^L&ii(Y)WhGyD#jnr9X+4B^7>%X#dgcyD0*dO6|uff>zB&v2C9 zHSjIe2u9W)hXHF<;C$b6P%H8rDC-`>wU1`OcZ*h$kc2IaV9pb0c&!LN4G{*3DlClO zI*I$0m9YS`3!d3`f-K*s!talw@E^-mXlJTQKCEs48hLA>nIxa<)maBWKRtv4 z_;;!dbwPH4+kCuV1frtqRPbQ!Ari&A3&z7;A&@BsyVTmqA~hk>TVM-kH}c)VE9HsY zg9`R-MkHhd_kb-6kM0z(gYFu42&a1-87EOpP|h$jrm5z8XgvkYK&rw7S) zI}4)cN|J>`C*alORZwR>gtpsliESAm>-1B|mBVpxvk~8y({db2$h~8XBkz)LowMlP zehuQUx0IH7T_N*64#D8_S+qs)IQE{WLG*51r=KKvtiqvs^72jy|E{YL^md~K6S1Ta z)&;qfGxA=eWa&%SAnHx!Bu=&#eqv|OwuIb|yFf)`1ZjWHKghVc!baEoaMbn* zSfcxyJpb8&eXl!_RqNA9%48`C9eGBq=g$E}D@w_jRVH+d$b+um5NzJ3NM;EL!STD5 z#J-^t?i9NSYZgnR$&Lizb3ho2AX8vo;*A?i8=1D3+9=391x7`m!k&4*VAj=G_;mgq z*nhkmd+t(4vb{&>m1AzyY}DizrxLCN+@Z=VPab~7cZSs zN8)ct!-2r&YK!`3pO|{{{YR&?>VT~m@4-dBarnbU{?1!PopVN85KHoN z{6>WeHn6jrb(cE_q821`Hdm{FiiT0JKsJi$ZFq3?kHUMP;EQ=KlD&-OsbS{BogE-b z%8HShS;TM-#jv6c(#-hBV>ti1AoRE>K~x1p*e;-;??QF)1tyC3 zbEPw=Nfm+B-jN)4p*&9Xs~X(2ToWwsh+?&Wo3R{sQ^wOqkh$QlPuAGW!n1N!jKJAk zFvn69FE3sQR)s{dwNLYyqrx`K?_1M>Yy1RL7c7Y7x=-VqYHl1KKRq0jD@PPJM&P{r zUTnTpAr@{CVGawnV^ zJx?a`CHNdJi?3ew1Ba4Qv3>7+z+1J7QBGOK4x6}X{IO{;p`(1?ROB)=x%WC?MwW{_c1Rjnp-LOJeQQ9 zu@8)(*NG9(z5OE&m^Gc;le7Y%>U+pk|JUSliWpqpAV?D9BuG@uEb#dILa4VO6WZ^$ zhf!&#u;GJAEI9HTZ(TM9R+U1qb;umQD80|;ruZJRowBg4PKFn!WeWa?8G>_Ve{k^! zRp@4HGppv z1*qIcZxYlPN%GE~1HLxzfzI7ku(7U%e^-qP+_?EU-mO>;yBG~7I>(tbobe;4;n6!3iYAlURpfZ1XZ2R=6sf;DxHtjnMaukD*0IMX=5 zByGIGe9w#JaNU*JGK0N1Bz_V*+^GV>za@x#Q~^7!VK&yjGmF(dz6D@s6D)7^gS|BC zAN->w2E=O1Lbjlc{UqAP77%;9{Kad|jw6TJm+UK!*ZMxr6YU4gx^i)n^FAjmROH`#-#EzGi8r*Xcg3@86r8LMd^$-j>wkaR`A0`o_Mfq0)4`}mm; zSUE+A43tyA58_ylsA8PZ9S*ei_G27(6-b(n0F}LK{_S_)8IpMTzu-5F`F^th>-<(~ zSe-;~F=BK->ye4FXwb(^#EGinROgx!%s<7ylT!UMu-|tc`{?=u8KFQ>vZ$G}^4BxG zenAtH(z=VuA5q5d9?fQ4Z+H^Hcpv_3+QI&v&am&xA+uVm>))KXh@{B6|IBa52>cI! zfHB`w_8-qrR*NM;nPCj(1erk7JTNMZxK0yKT}6?wTHkYk4*B4d*zvO&BGa71rH z*7_>maorTV-HIY*A?rzVr|$nE?aia9jQ;*_Gnq4l%ppn{Bb;mR%NS9T43!2&p@`C? z$vkDs5SfQUWeP<(*WTAD2@Q%$(QHZ@l}N~QeeboN-&)W8$M0V2eynx=w47tHul@PF z-|yF(blk--6mOf+!4J3L2{=emgYM8$=K^}@z)kq|#?tS@ZLx)wDa?P`f_`mL$El6+ z=<9Du;Br$KOq2w{&KPyTS`|YV_>pw_RR#QahUAz@MZzMlPC9(Hh(01-i*449pt4^^ z^dr$ax>Dp5$?CX+pTAp2cXKAN$Q~P*wk!=D`@S2B1+FBw@2jP5+LJKo+6Nf<;0x}~ zGlK*7eZl;_WMXM)CLJ83hvi}{aDZ<%7@K+y6TSvoEX%E<#QynFaS}mvTCOS%uQ4E> zFFpzibJC#K#A(t+HHPlI`^Eb=wO4`l`Dj?z%_P_{JZ*s2_8cwQQgq&-F-(nLs}Pr9`8QVTR{CQV0w4x@Y3 zc<5~9Ed21RAlCYm4-O1i;-syG#7g14^mES+aPyc6ew$>8F5kHYV+$96*S?2QE4~kU zTeV4rIZb@J_cy#_b&fo@Rgi9JO0%rG9*y%p&yZ=H98%C!nrz$LN?c+5Binx@L+QQN zoS}WfWPIXBm|u{G3wGb6Cu>~jDhp-&wx$PLnb?vms(A4(^)kHIO`QDauE=nG#ZhSQ zIxyg213p?cA|E}#{!kzZubmWN3+>K8!nb|#{v|WS>9ECce!nIr4OY`=;|KVS9SYmt zUZfAFdXuq53Y^s=pOBz`6zrDW3GPiqqIRphNPCeENB@Z@y|gG2xw-8}OU1bG_&@-b zYFdv!TCJc>W}DHafI)2UaEy*<)dl?;$M6&-25w2tQIn-PbdP#A3b|ndhMbCte7+hq z9BvKmH5XdUQrVzYJem}#8Ud<0N^C)6Q!*er1_lh}TgDu_Wx3ZN8R{;d1r5UC*ik`- zjQohKysv<`P`M zmjEO|4q*R#ihs#J2kPIoaBq7lko~ouRI>k$CIg&dt9~l^*Ek+UuuI9&TN^ms!mBM! zV-m^0>2BEBy94QrEG2&|-hg$Cp3)yS@sM_RpWr2}2H^ekm1IC)6@F+R2ZkS1L7~fo zNXdZ1;(o_R_8xCzxj$2&13sAYlQ;+m+4qXS+B-#~(5w?WO8 zeDL0N3hORigxi%}X=;%!9Uhrae<;)Eu$KwaR~;j1=rQxZAXV9{`HPG?XcCC_xmzWd0wPBve5!{yHMLO8`Amk0f38t%YyGq)}+5K9G$Z z!DY@e(5f>SnK#XVW&PH8!Ixa*>PR3VO5M_XtPdPl@V1zX@kZ2i0gyF-Z^c|twbYhRN z<(%>q)^k*&!!BIJrByU}TCV}CrjL>4LeB^}r?*&pmmv1Pp9=FE($R9cS#WKvAN_2g zApbH&lHET8bIE07OQ|4~-{C=gW*CnRGRDljFikD;|3`$MTTEqICsXI_b>Y_UQh1x} zDMEX8F(|A31|p5_LE)vvr1AVa82X2adAqadu{Zm$>ZBDcdRT+bylR13vkB1}YUY8+mbF3le4e1g_An?jfJY!JGd3#ZS>VIjpD@TDadMBLW_)7R5+-B%qFS@vUZ z@m1ubW*0CPl}T2EHBh|%7#(|tjl6EHAj6v}Y*X-quD-Drb%h2&b!Lt)4w}U`a=)Xz zOAdsdY%o?ew#KLC9wTezyLits2i*Ed4%x4H48rTqf$PmOFi}kk8kHRdy1TbySvm#X znA-*B-EMQ#fS-&2NAtldZ0xm;md`aKZ|n<0Qs3*Cd{Pu@57y$;UgCvOv&*42 zy9d0#cLYMc43LpmgO{H0pbMFO&(U?a@i!)`d4It|8aU??TUYa7pN8$(szVHw5 zMJ1VG_3eiCDkI=};AM2Z)c~K&P=Ge3%wyD}m}s#spzr+INGj(aCuQi1bkFDxa;MaF z7`#D+$ekp^CU%pbIbMAhpK_@yyBy@IJg2E$|Z{%eKdWNgX99FOY-m44wnGhXnwYtU%QJ zS%P!uIl7o>g0-&+LC(Tw;Bu7$d5p@YbHsURQ^`O&$10w#diR;`KWRz+DikKqnx#V9 zj!76;_n9d8yNImg7?F=hF5qu6%gF>C3G95onEa6w3;PxYp@7&qylJ=td2P9Y7j}kG zLJoJR8}=(e{^n#V`pPcwgXTkpO25#ne`i5v{7M|kT4G==ti z1HN2ET$iB)SxX)OEp;L=^{NSN-YX9zE+hkUv<1H#^~9Xieo);Z zLW+iNr59$NLKzF?I5Q8TNtJt-==MN2G9pA2uPu9uyJsMMDmjP0EsGs8X9!M? zBY1bh2}h)@LlQpW;DvNAcHgGs~CfmUG6~ zpWwX9D*B?$ZFu2+6K<%GM-MFt%ob4s&LN7ZQ>hX?5Ugi9QNQ6=!fW97Us;H|WD@*- zYQ#Ka65(9ILVT^Nnwl~C3KlGFMn@dm$U(ts;z-aF;A|#|jhiBofagM(Sp>iuLIxBu zGi6$UCy3`gKvw=OqH|wopycWMA(Lv?3dFbqYv!B**g`<*F(~l ztbdZ6DcPGaF-9GaYE7Y4S&^i-)@G=#B1T`HjbysH3rYE>rdZ=y6?t=MI#vx_wINDLX4k+M^AS_HF|*XV0!x;9_OPELF(-0BXQ8md@n)p)c|X;Vvw^h4?}K$ zT3%9;XuGIGksN3HNGBZ!(d!>I+f0TQ$Qo%3-rqTNZflP0gD_xiOXC3n68lo zC@rUlxpFN@H&ajCQg|47FW8F}vJXdNvR@b>xW>spe$WC#c<>|jgna=qO{&eb=v8UCVAec5?_sA*beWGu|?l= z5bv(NWYCp)ysxGaiC7;);y+AaNwG8b+};eA+i!wD3q+va?=+Ab)680|FM!3i=b)Sf zEgbAt2t-2rlDLRU3eGb!7Wjy3NpGBOD13DbH zz@x-$b~4m9xQ?CGoN<$W8K&OsX=GkVa_#zq2kcd;iO2V81N}yh zN1y~}{Z&1z>-Q6HsR+O;Yj40)0kI%FWQ-11{EnA`VZ7zd3Qqj~4fOWz+5g%P*%TI^ z{om-%3}5np+@IL5^hd)2=IH58TX;lh1FC5XgTrD!DUHR;u^C|pe{N2}BLiZPlc9*( z6cup5hB(Zc*a8y2Gk&RSBKYaMVOISgak%hOG)xZiK$}mj0W!9mz?Hh^AdtbmD1_n4(AUv<5s6d@Ml{x-r2v92-i-501@{gqtpT;8AB0dbU#+ZdqjyKRpwI zikt!{tt^Fl3SUr`gM#Gs4Q1$fehiLY(gGrlg-A=yFMzj;2l6yV;gtCIAa+eYe)e-G z{#cU+m+`S+oDZWnQ<1Pbqy`0%kdCg1g7c~G;Ma)=+`y%Y4yc@f>o3eA*N#9~T%U{Q zUNISTWi_ng$4{;un*!Z+EK)q`-&>%)ikDj@fNBtR?0lE2>M6lmoM7Tu5uJTft8c>b4tos5* zgKePSbTYYDi3d9`3db5pO7Ot8wbn?9`DBfvU(Raq%V!%1Z0vx8c`P1JNu-(6(&q>(f`W2v`3uoX%NTWv*+-Y(+&8SpAeb% z+!?mP9)ft82cOW5P-n{~ctS4$C)tUTS0mSA$L*>3CUn4|106We;RUwv3x#*qT*Be! zq~H_lhtTeq3d~TNgPAIpbcb&`+AYvWt(mw?4Ro>SH8qJ;+V^~FrZbXtZ%Znnzi&H1 zE!QBrZr{az;`FpdBcO*>;gC(7lzGO!ODD3F+<&lo=IV)n0}<%rq9W?Wt5J*3 zI|bOKha1@68(&gkD+`IJEuUD8U5*4;cAhO^5l;wY|6>&{=0PrpA>00<3F`^VpXFHO zKwT6LVAmA3u{BR#B`VjXu|~iBWw{RUvZ}nzSW%luqKL^!O1&61^H92S>-jlR%Fi>B z%J_1a`nW!X^_#j%%=vDi6eq5>ND8~Lh0I#m!B))9Cs92w43v_WV%OmpNGA3+ zYwsZ|DyeM;was@MjC(N$@;*%<=%S6rqT|2~k<&~TITZ5A#-ksm6k1%Yjf$Mt5P!7*P4@DH02 zbhz{Zwh2r}>b(Pa-uE_4G(Chh?x@i7!!>wKx+r=5g(tp{l@8b!l(0124bJ_nKsT18 zVyBw|^s~=LU>#2y{(CY5r!Ui{V>TDSqkE&^nVzknaYrMJKOPG&HQJ&96HRm~Jq7%e zlp|!L#Hn>Jf`GZ59qJ6XMUL5$V2{8&aXCc;SP?)?F8Ix=<`cj&mp#$v zvx!8yT_LgXqC4j8uRvmpq|k*f6F}@5B#tiNVZGWc2p-NpgQn3LNSi$ZcI7(3{+i#c zgUqhF&!&K~cC$biU%y2MJoRx^%Sxae;Dn}U_+Y@!JHS-uGTyGd8y&M*j}HSMR)M4r z6b&q)4l-;PzVnK3u?M3YFA4sU&jzqz-y|ill<8?=x(#i&9zOkaLT$sZzZPL9iVgIVgg{LB87R+qzJ}QT zu+gOk2A`&p!9`tc)5pTw53PrFzk;zv-b>KYSq|U);z7rmXOxoWWzZ)Y!BRQiMEpJW z7{(XJ;WAYPa^zMZ%uig!R@@Xv94Yz>UheV5CI?-?*4AZEPQx7DY|Z@=BCp zz87z}ph0@M{=`{7bm(!l9#kt*1zuH1!L@&Tp}re8nfihk`|S{+IU0O0x}S^mee?yL zcxqswfWi>AQl{@x4`3YQ^$r74bo${fuy@`Bo{JeroDdF^=aFC-HqDe67H8L0od6Ms zgISF^_3ZHMW#IMawMc$21O5IyKy+TPB6!0Lz>G>D^)&MhOWR!nec#T7VHrhL1|Vae0uu_5cdesRo7>ig0DK zGgYOvkyw?q3avSqiA?XPfbf+`L~`GEw8$bAbVX#MbDt4>yT5|<@nJbwi)G-W^CE0^ zO9ksiofukdy$FoNGAvSiC#011gEE#EKsPQtBW^8S0e@=k!Rb@^Xkle8z9defp{W>{ zcIgk8Ubqzh_I(D**D`m_Q%xW|^9n`qGpuai!$4~wooLHfz}migh;w5niZomTjxl_I zupW1mGk=AOX{U(|r+CrqMGLHca0g&1)Pq0Ir3lN02dq|?DMI7(S&-nQcuP6`F7fT- zI#lSr5{<6h2bMk#0rP%0sBQY^kc)B>t3)S`c&&B|rCyyOQaAl0+@3rq48F;sz+PU; z;;a)$$gwB(f6PLK@3hdWWye`vcbbuWay+Hhs|vz=B9KRnBvD%PmTlNv3`V;aqLc1o zz&Nyn&2u#mYzy=P?jGh8yFQv|o%Ljx70=O`O{<8R!DQBC8bWyjcffgGO_Z7S98C_d zLyyr3G}hxt=$|P7@BDnIOCh#USTu)N*f7MJXg&vKG@8LL17lct-wAm5i{m5e`cP&O z54K8?ha2=GDW0t(=zfwNvNqPj+3oYh^m%dEb;Ak@+{lAJM}2{ed-{Lz>lOO;P5y86 zXNFJq|670l{Gg3^pIXVD{mBjgs*a+^FO`ttTJS2fp_w z8eXk_L^-D9pvN)oNLf}5c(hx=rZsAStMxXeGU&k6t2l%6C0%T%9c{?j-wXVQkI;Jd zVZi@Je`Yvk|Kt4hC43%;s80cNE=@vVnTk-L5+{Wcy!(4R^lNaNusUc7-hFSe21tTkBT*h|1lwxI*UCCMZUF3UZ8 z(xB|sZ+O+DCT&@sikHg0#Qc8(@!JdM>Abdec+GM_c(p@{RBOMFS(S0{xgyhfDRu=d z-?0up+cFP_`1pYHWHVB^s|Fn()e!n$n?dB)WMCVz7UJpz9L?+*7r;Tt({c)GC``k9 zcdn6o0+P7=un1V8m_nXb1Sq$;04DNnAgPV=;FZv8>~byxdl6iqsKlO-Kf;6Go(zLd z8ziBg+9^teuK=AkwxQ{ri*YHj8rE4m;x%3?U?dC!vl?&c63=nUkN*f*<8T2)9AAMS zy_3VpNFQ#u-_4x0XCd3InEcc~N*brGBn>Pw=+|slQbk*lBlU*mIUVEm#88674wQFInWSGDUpo=P&y1;0S)o) zRWir6o-Q52WLs_)$>M~Phs)#Wxc5A`^}4DAnK)itvV;Hy!UOQM3gZdhUr)${d15j1DTbNr2mXxn z1KH5$NH#4LBIc~m&Q2im!kZEA^As%eT8fOaJxLqYHehWF9?nz4M0$g+AT4EojDBOV zl%D9|qPr_M(rKr6!Pa$cq(X@-IXvHtidO9cIng<&ks1IWI?^F4F&)5dDlqq*58Ca0 z5nR$y2hx>2_{EAXu=H;R=;MEb4y>lZX{B`JApa3at}a5~{_rBhWIj-K#U33oIzv7G zosG0@3h?9daVWNm0v{5`$?_+f&>%{ij=f$<3if&QG?j8i6`y5aWv@%`<#}rfW`1NVv-t&WHBHx1e>_lMg9|L&(WfmHV?F6q(_+h*3dGjY} zff$LLLPlMGf#yIein4IVKbMJ+c25?;5BHc^M?}?<-b_U?C%p^3Z z>_+)uDQ8EkDdd}Z4y4{jgH5Y1Lhs%t^vj(jW^vHV*S)1;l(mf+>?|A!q3}+ZKn<4t0&o1a>ocI|I(iJx4%f3 zbSIN>iqFWEl6z<|o_X@Yg(BMKUJhWD8k6QHCCD!KdHS2M7?em{M&9n>fk*7V!Qh@K zGEdwCtsLEkkBYgIcRTq&xO5)eS*t@{X&-_=6d`Zp9FdeiNM~*F8KYTH;}#{4f(2-$bI|bsJUrtyzeL=SERiFokwWY zvt^(qlXaRZB&QJ1&I+I#(gt+>>Zz-rJ^hs`i@qZw}IXCaR7?%!n9j#k`k@Cfntgu$Ga5!|@e8J8LFCV$z6lUFRh z0BgHLh#jX5f!tN7IP?V653PYdk3@0T+E)-h?xDyARj@$$A3XQ9jgoQxhMYeu8Sa=7aHFqk}v9I@#e^nB?b3PRqmSHrpit|v~uXHSu^z6M#QKqO=K67^`$0^uk_sJDOzdn6m; zm@FA+a55M;+~LM6BSf+E!*I;oBu)i1DC3pIuTj&KChYig5r`$p67;wecsRNnj31PO zYZ9W^eX=bqu0=&S%8beNf3v2w3W8{}fJ<~v+agYy+b2@s+*3?W6w!#uqx-Ykkcf&u zI(6bLDQ2Nc)<&w*XMI*+ndM`wu@(GuTd*X#?E;evedR^uoYSX+n!nT2%XiYxObvjY zi4cBtjDRm1eZknjM*7VGF7)fnL%@3vSNxkSn#CQ2LMx z$tU=nbw~aKbw9c1lw!y->}ozZBhTBOIlCbB-P|pJ-fm7o+ceS$MO_;h zJX#2j%#@NpJ{=-ea(wBE{Cg;8XoyZ`-5`;axMg!<9LYnMfpoVOJI^J2G;nkV`P-k!Pr6Cd*5~Ur8Tuqf$4?zSK&7o^&8LeV>5SFW*65t25Abtc*1LW`p

zCg65qS;qfnghz+Uu*%e4JoOhq{p&$wZbT2Rdyz_+6}aPr)6?jsff<-k&mz+;QrfIBfOj zy7=yjQkY)3lNOlLp+5+D(T4qxF)v5ia?eXiOg(u8nF?dP)kTx^IIxDa4hbaZ(yw5P zGZM6Tb``!YR}SK9q;U~6g{wbk(^U+YvrkAGj#_JxH&(MS@A_e|>bx=zE4mNr7d@bC zE%m{-co&?b;D+MPy=SM}h9d43H>54=&q|T`3HCBxsw-pfQG@J4QdK36l!;qKM^!Ge z+`PmFnk~N#Nk18Kg_i?)Gd&QJC5fayc;)y2I4>96-B0(#u^1u$AG{Cja5OY6IgC^-d zpndmF^yjh&C4A)o8EG0#$}X&=!~LZ2SV$qq?AieFT5U=Kc%y!1Z&PY_1GPbaoFH3JqX(qp7kuOr;F=ewPIb z-Thd3_zoR1*@MgXo~Ml#Wx&+hLh?&|F)Xqugkm=Z;N&qS&SO1MdMn6AV+vDHr0W$P zmr;kk*JEK5cNKnD(FY0({(Zgw1YymAY;3dK0S!k5p$->mdimAuVA0-ncy;zi z@Wb&Ce35yT__xv$Y|y+%bQNo2^V;joo>vI;94uh*?(hfs>o%Z$rZdDlZB>R5y%l_W z?F|~{A#S#n*Dd?5|#MX<7^&rk_mz zyMC3mVa^TCJ?EuA-Ic-yQ^T;&8`6c2Ucmg&5A;ud7GJ6=}L%srtq@TXj<@q7`dS40B7-{ zoAl#XC+HQOsgUb=KJBWsg{&Xj0zC~h@Ne&B^r@AGsE$vaJkK8uCHnT@${!ps7yBG% zTP>u=$L}M(=ybS+@igCEGl%#@FXK%dA=q@P2K}x01SD(kK%XE>9PjxRO@%qb6a3tG zCYV5)MsLstBNur4Vj-9?eMM|P?@2t4sfMcKkhH!V0wWWg$w0+bbhi%+O8eF#NB<+x z-ZYW?6Qyh^@*)R5e!qnna~`HRPpeSfMnR}p*-5nx3E>5Phrz32W4#wz7a`|Dl?7Tsbb`22vh^jwX8jhu&OV;RuD?gfsss{@_a*1;~@AnNuq z0`|l%hEcs@h$H6*yO0Q&n>-I=idD(xn{s5`r*mkn0!D<)Db^3;qhRV~7xda;4GgEM zQG2m0zIfXPcOTF~nM-nj*CZDK-?v~{UR(O#Mp;h5MsB$0i#q4w=>j@cew1FLA;!#q z`S^)j2|3oclz!}2Oy5-1vh<9DjBauh`^Ri018#N`RcfO+v`7XWUfTtFm^;|th3+Wq zbUTAO@TV91kNsEw-|K-M;s1^P%Yv+;`-0) z|HkM0Z}|sfhR^pu{{AfPv+xJ(4D&>4k^A`&$X?fhbeA)1Snadulz2a77+MAR9IAQrG~Tke8}qsyCNeJYV>^b^E6{ToJ`7dCPZh)9 z3Uw2VS7pJ!#~A+Gi60>K{ZHr|<%JWfBJoGoBP=*o2D^u*;L0m}q^(^7d<%GS;rDGo zM|w3ar&j`pyIHiRo*X*9=RCH_RbdO*G+AhJ49QRCN5PhqI(Q>*1*zLEOy(*&Lsm)@ zL>FIx1<@G1Ddoc!m-?a46B+#SzyS2?D8;}3)T76WHBh`yAB%1;2IImz;I{f?G-lun z3WqG=V0Qt`+wqCTQdy6Tw~gV07Zc!>KS4Nf-!<$Nd>aocIYNzO16=>ylU~J+M$=uf z=s^N={@wo$l6Ie>HhV6h9`WVkGK2kC_qr5LJ4aJPuOFe^oF?eHL5DrU8boepDzMu4 z8H#8eLUStTQFqM~(7VDKIjQoKw%#G=_&Ot+&15bznI6VJ)8Db;n{h6LrZ)QLcdBO`n$XywC%d;P4!mqGaLa*jplB>fp$U zO8hAQ7b5GnV($e?PG z1co0g1|cs#v3IFFg*nNW!T$IKI4;>0yELWZ8ryiJR9*~=7y02@Zsv~Z-3jgWlF2Ir zr=YS)D%J_z3GGF>pv_ai<7X?MgQ_NJ zx`W{%ub;evs`rNAHoGQ>y;7j3bv*Qn4#%6=7x4afYSP6Nc_HX*<$Ll z0;M0TOmGtJ69IK@tQ%^=tgGLpsa8V{Yi&g-ds#{}XxUj$XvKE3e0ATmV(rDy*taC2 zIa89_rZC5PMr60>j<6`DYZZiv*97ZGb2-)9ZwXRw?FU}V&zMkt0>uY3Vi#m@!v7MdG zda#sadoSC{`jn+eycZl{_r!`4j}JMuOnTR_&0ne!hu4Y{IOq)vUOPpw?{rvHU7u$C zjW(gawhpr-wmDHJGu1HL;Q|I*?9tq*1gfe`4iEZg!o%UCAbeFD81LxB+T#*9?od2X zo{+<89gA^)R1xAnTt&R{PXLCtztMMl8F-P|ch}b&1HJMBsFE3u9(Eew;=x?}Tx%sx zEtbTUT|qFMiig{nz4zVOeLzXXhUGBq0i^X$5^|plPNrXrSryobS{lDGUE`kj3eW0wW2pz%%=4Er>If@}H6N_mR)SBs*yaTOB{n@>=~<62pbwVp^zx8 z)kAB>hS9;o9jNipMc|4?Q2*9*#LH{zDbM*xi#xnbCh&A8e@j7^MZY0WWDWATafZ?Xt)Qbo>(oX0c3<*$od= z;(ru%?iE7sly(r7Sxe9or#zroY>eHu_JSPKNp!w=5`0wuh05EWfhxXO@Xh2XxRhUw zco=4d44WUDw$_4@Yz370y_rZCt^h#(JE2hXia2g?nRvWW3#YsZ1M>~`pd~z>diK=> zoK@Ken)uu(8^KKU?KLl$pC@6R%VOyF;R1WP>m#(|j})?3jsON~W`s1~24G|2KsZ#4 zPy;7DE$(S91u=zEEI*$?s%$QgFkeHR6w{V2uOJwol&*65g~?X7yv}@diI4=cW>{T*?O38ViUD%NXjuqdM+7 zX@^8zQjn0R2nZD0NI3^;;$1`Pl>EYU5asg>?Ptlt!gITjpwfGEN?8xRZsx~VI~y22 zp9$``UjmHv_`%-d%9e^y7A$+d1Q{#vgS=}`fmC!5!vjbMW$KZ5;_g#akl=v&wn<>m zNh>N!EE3A^KTE9r=*o)x3y@~AGiWSW1^M=e69XBxAkD=XXzdjud?-(lX0;H9HGQCl zB&vazyd5}lp^rT!Vn!V{-UhnvB~aNfV<|H8E1|6@PZ^yDsQ#H6_&t^ew(pW*ZRJm( zuBOI;k9V>`9r0fD+BS5Y23I(V$>CI!aki5DN3{l%C!s0WKF z-cklWes4S4s;>)M-MFbi=G@sx8Q*f6-iwq*qs$vJ@^!a8F0 zgC(3Rj6$8b&{Rh!6(e;X6>m)g_2*6#A#$6Eg7)>Ot#UD__^m-yWG;ePrO$}GD;}t@ z&l~CNcA$#9MbJ?vUm)nb8qTs6=}zSx1m7M({G>6DP#4EwmGOS!;g}+jDSrfr%v$P~ z^er?dQQG3|r2u@lmQt)dQNViIjFg_NLgt;Ta8a-zT7E~Es=l_GSU)vK{X1QcyRHUN z($>o0Vo@&O8|g$IgRbnW_RYkJ6>o@bUKi2Ttr=(({b7HXkOgkDTIfL2Z?q@loP|I2 z2TU9eh2xLPk<3L!ymT@L>zAL!6+@E1XfrpxpBAOaenZN4rV-rlx1o+--^9vtQO6f8 zzv9=%wNPU35jYvyh`3gzfu`C0n9k(KKQq68#m7CMrRfwQlp{s%5&a3YSPPI*STyU3 zhB)nDrH7OD*CE}{*f6b#~C1x?jA!-D&JfZx$*$Qs{7@Fl0=qD3k|L%tlzdlsM@ z|Bm2UgB3XasT~-}Go~$1)?(v^B>4HqEPBN7vee66sn_QVu-;XSH5i^^LWLClHd+Q= z$~%B|PU*sWU0bqWa~ir>^#R)li?K|RHa!)=c-H$qW6i4#AbRa8Lk}Eq zUTYC9wNQn78GT)YX9~I+u4es5AC3%Qcu^u;T>p9f-|pxCjh}7I@SVv2GYD+`-+k@Y zRKvkBn^DQ$r_`^8)#$zy8|Xd!wF{z->3&@e7^!k zUH^pQzc4$3mI8{~P7MOnUUc%^BCz`{lUaDU26!sD!rR6-kcEjjYfo+jn&}h(lR1JY z>CY1QXvP3G#jHd2hj*frXBZ|Bh(Z>?42!zbmG?I zvF(pomD`za*6bNV^_VMYGE742FK@!8?k|u#k0!WQ_!nHZ= z3JrS|A(2)~D89)XKY1XGKbr4Br?%+f1M6!Tb#fmnO}&mx(s{60%So`xP@Sml*8}{| zhQPj>_2Ao&L3CMsGcw{?k4^VTz}G*+si7zlc+#g6WpCAj4;la51|2n6$Ulk{_?F>R zp=UtGvm9Mn$_owtJtz7U=Yd~9Es=RV3G9y=0_V7Qv79dDgGJ8Ev12yF0G+-GR7ypF zin%>F5y1~P8K+S5jr{m^BeOvly^bmUeNcJ}8*+Ew&O@oM+ zQyWoXJIP84@c^0Tn&_feDVWIK031TD0aN8eXs0TZ-=}V)t!^X?wr9GG{r`c>lMC>> zO)V&C=o|ol^T0iK13W2W3sui$p(MBy7dkH{kPJ(0>~3viKD56n%iiD$=cv2|k{_{hG9^tRYSU1>AGLPAjXrcl>=nhf# z9s98mPy;t#&x5x?C4gwnu_$>55WJm7us>Zv+&A|y=esWAdb$zJG%lq2f|gOA&T~VL z3ly??CWW`$u0&Q6C#kWU_Q*cK4^(?Df#q$c&{Wa_9{>tyEXoj!ZsUiWds2~9L@qn> zwh?SSkk8sTq6tp* zilRdQ-6ftL;DMQgi_pHD8dMw6iTZjCfPs|~R6gX2Z40Eac-uoH{~!izyix`X-x$Ef zOeejoTn;$tG7DDTX#%GnFTfh6<;0ac4M_H^BAj=f0F@^gpIpFN?Dq08s+4yEekc@i2L$pUJ>xu7e6-34!@fOYs(_k2!g#9u|Cjncc!IfxIi~DGOQ) z4*p(+<8mWmp^mi0Y%RmTIlu+4$V{^7UXIDnXu{U0xSL%05?-hkj9@0ObB_X zFgZ(c54}PSZ?92#{MP7L>k4#tB@2>cgKXC^E_mSTexfbK2spui)bD~b=xIa(D2|x} zeqH7$SpmW{)(xs8QvjDn4_GV|md3s5yqL#e8P@E5jEpoCfzj4hR9|loRDz??ye|*< zc&Q#)G3TqC5yVq_D<$mKtVfE8_gKS>fASDI3oIG#tImNy zBseAv2lKz9>?MXsqOuu$SsDXApS}llQno<5Wz4y{vIQgr$5KB^CIOeq6uR7?iIUGx z0ydL#{Ft*C+|B4l19wY7Y35S2Loy1zOP>Jo?Y&^(>Rx1)irAx}e?WR@7OHNrL1~mb zTH(9`9Q=5e${JkDw9|Ki>{FsZ5^0B3lWkuP%283luCt)CSwvA zqKp|ric~1dv+jElAyXQqqzR>YqBLl)&-;hHkG(%XypQ9%|Ac2)_qwn1yw2BoV6_op zZ>lm%cO&4wVh^N!m*Yj$_ttx@ZO5->YU0T!Z@{5blTqy)A=0#I6}FkBiFR~xOr%|2 z;QNzfB=21WI^8YMVZ0xXclJ@zDsIHt*@_5{u1CwiHN%dY9Nza`id^3-m$AO&3sGOf z!1<;nvtsEiyz;yt%DgR3hHtDzs#|Gv&r}?B-n{4uvt-tL%v8=_G3M z`rY`l#d3&!Bt&w1RY}gq+32FeZhWO=D<%B(Gv4jB2LJYK1f5sM@V8(FLZ<1E`(kP+ zCpVrt`Rg^#erASJbPMB_f%%n?nijiyG|IY zvx4Kt8nNK=3A<+rYpIL-1o^iGPf?n4>me=36J5G*L{c8>f?xO;FY?819DjEsj6C1X z-4~UJcbOC4q4O>@>CePPd(!axfe~1C@&x{wdkVXyi~_^$m|3OWfSl1k5c@(A2Yr?z z^S|ALAG&d{Oi&M2B?=*IpoTujs1in_iINOgAqMY~LGhuiUHkvsLFz}rXlEAo`jQQ> z$%a%pPvFUmKEj@;Q;>iB7d~cdgwDOYMa>CvCi9oBB1b zR9GSLDvHvRmxuCoMu-p?Wy?mIwv&nN8Z zzYM8+EQJ8OtJv7n10Fl^NJN^Cq}`xQaiSx}i3WT_AO*1Z$V^ z@vmvK@kZON@Mr_g91IzuCOZe>^&xA>8E*bgniWQwG;Tv;Jq&<=1R0&4N(|%7@wvqr zc;56ERGRxMrd~%KH&$sQqY`nVk}89xzwo&G+;gbhF@xV+F@!h#s-U#9>H)UQWV)S~ z;g8nN z(cNB93PLRNsRf*z?$&IDKYel>W&qPqf4{=}mB3s1OZ^2#O z2}Et4@?+yXsE1+-NF+xCdo6wqFKY6r8QPX;>e40noA^4o*s%uF4hJAMQ<3abus|Vi z$~X&Om4TmIAUfC<)FOp&;R*%*-6$Eds$~JrvzH|=(-Yv!!!I~%LlRyqH6M3+CBO?a zJ+$xg0Pb784?Z**kTb_+k&TlzuQ#fM<8+&m;tP*peb^MD@o5aVK0QW-2l%1!gc*c6 zJcIlq$C&5KW1%jkjM?|n3(dPWm7gEDf()Mgz*|uL1uyoTLDuUmhUcrg;rf9n=HU8d zyy3kRn)p2z$yjq*&fF@Fbv_HdR`>vM&kf^Mm;x@+v$MgpC8A1E(`Fjy_2zf z)J+^eKOQfyImNuQQvs_jXRyJICsfH|0eCXcggAaE!e_2qqdUc(a4IwlUggh0dGozs z*)}usug(y43XD@?bxfE2j0%-c=2a>=!DqtG8Tmk?I1tqpTi!NGvMZrcUZhWfgf+|iylvngGhj0SRK6yS>}rP)vl-bndw_Bx=a@umu+IgWSZfSF}FWXAA+`&_b|+Tzdfca zk)fSi>t%JWaOY!gO7}&!-V>w<2m`0bQ zm=|Y7#s<5gXJ#WPsa9V*V_NZ_{D8Cn@aX@6|82<`#s5`5S>E*#3hk9iSI%8VZBz-p zkGEzTEJJX++EI9$(vK(GgN{}*a)?c@5ctU4;NYAHS_nGAU!4R}o|`TyaXKcC^4 z{NMd=Splj4kN?ea`TpO)rtXH2&m9iJy?31EVmK4#r#{50 z5+ZOu;wLr!Ac$)ANx~nV_E8?U1*x81XR>d8DtDhg1D_}BA@z1q)O!9l6XDR0uX)WO zyN3I5bJ;THwBaQ3=FmO7OyLgr?Z3>JzuUviR$Tz*4Yp+K;??lFcn*2xGyn_dDWi-d z-T2R)K=Sp81$itShI(TJ$U+H#1^Qy>$NKC!1vu|e8c>UA#@(e9 z*J+&s<6JMwc2^`$?CFJ7($XmAO&!j@cAq-Xq>2u5d+uplu0gJuD5{Lw$uYN{QuBSS z8I|F7kZ)Bcg|!8E-J-4Jb5=K&TPaGl`Fmgmi)5S}$YM1v({z8}Bje?C2DdEihPt*D zXk()%YE>(Ss-Guu=(Ic7NqGTUvb&G@c4G&GWUW9m_q)M;*-?D_WDZvFX7C=v61bn8 zfp1=%gVvns<<71cFp>GhBnQ36)1|(_>l;sTSEvDO@`;Dy*Gg!)e-NJ8?2O(-3lYYo z6QNOR+^!9t&1# zo@?6)x6MTU5-qsVrJkyD-$?c*O5r#^bw=fD8idh)#M|dIF5NyIEy}n8Vt)+Lk;Fv2 z$S;(c2>J|xk$pgNn_$p+7dhAZ7aCh1U^htwfxA1H-dRU*&LLUMEOjDJP2S_youcTG z428m?q)3$AZ!lSzfirhCzyhBmAO%|JU-SC2^1XBn zCm6YtOV1R^)3?82>2e)x&KknUWv=)(Q^4eKIfMhCejDU{ka{m z`*l11rci~4l?R!PW|5fA55zKRN@UivNF4Ubm-@k6hS^^NQM1nul&cqocf`NNAEsPq zUN1V2OZG{l>GNW7fWjzFHi&_>vt&s>`HBA}o`STE<|LQ<{Hng4R7BGySj3k^iT(z} z??xJwJuSx)uFcf${rS|_d)@f6;2G-Qv|mj73U#v1?=#q2ou`hq-{$#j%3;C}ar}YT zK#Hf(Ok%YduDw&wj24Gcxl3xPr~ev2SU4Bj*QE0{L<*4jBW*DEuRQv}Wt(D)7BFqP z>ZIe^6qIwZ54Y;CgO1EjUh&o|I4q|YlZGm|zgq-7bS$CZ=6lBS4wojO{^722G4d?M zmfSt50@n>Ep_||LgI;_K{67c-%%U`Ei-QK5YhCdQ%u(RuU&mRvD8&VirViQz!U)7!YfL$!N{lWISdkjZ#x= z`NUoTx%B75%I43Yx#uG~8Ej1&ihfe>^i{B6S{9Zb6GsOEM9Iv!Z0!AV9hMHA z3*pWB#I`$^@xe5_nxsR-4c^#5)CXtiuNy~{5loTC%1Dr=;C|$_bq~o{N0GzlGy zPQnHL-1&ZdBgih;59<@&gDrO#saJ7^;C5~Bo1}-X`DR0UgEYyIsKJ3&Gf`Je9`^9o z2H)9Nxomhi=*K%iU%?ApXLJQS2FRl1BzJHtQpS#Bsw8tDib7k8agLM+T;;s2j)3{F ztJaCUEsy7STBcAqtsg&rU_l0oCnLw%<@j-!7|xKNi&k$ffcAI|VjWNlr$$n+z$+m# z2L#Y}RRNSfH4mh=NTA>c3rN_$0miUN0X-|xz@b?;@SdOJIK1B#Zuv=L37P36(yNN< zX)L6!M1SR(yq3onBL%qUVK*+GcLO^<*n?Ffe&gPAk|h3*7EvztqEv6#BKy8MXiv#| zII^RKn&xUlR_ke?_7l2f-nDGz2R*`1zrsU{Dn@ZlU@apSEDj11Vno^DD`W4@Qp=7W zwzHSV9N?a$)Dtm8h8_nC)Gmu>@t5E=4qsTeu6UX9fC z^kFtpl`Nj&h^HU^1l})BL*M3wsOh6E?)*3d_f66y=dMkLjrsTCp`IYRq*VYzT^I1% zzmw49<$CDIqk0T;9Y8}?oD?m0!Li@y5x)32A+hjpA9vlj`>RE%-_d-@XTKdy}5l9 zYh1pEU6viiR=qNCBUXo84(GI+HOVl;Tb4-cCBYeGFLXb$2Fu&O#WuIR|KrbwuHX3S z|HOah_)h=7{b%)Cb5Ps7k1%$79;u1lfW9BBgogv3jBKk6($lWMMt7>2r1&lHD{m^= z`7Io;6ODl&k4{{-UxPeYv;a>r^`kPhO-X-p7k|il2FkJRrPdF|;cva7Xz-BAfA(J` zTkg#H@BZ`u>Tl=9jQ`%>es9H9_^vOGW<3lg{M{EJ;kh63|4%8>9jbvjO^ppnfr)t+ zeq+HgxE7wlul9-}H1Z&y8~+fYw3+N7i46N_rw%rz{t?M>(__mHin0?t z11x;+3w7n}O+wy$L}C%9#J7gasy?hn`zkevx!4`7!L*|y?W6e6*&C#8!8pG8M+8dc zrP-w^Gth^H3aDYa8SXlugJ_OBZY^t0K2@b+`K^obhe0QzGf)gGg(8^{*KE9}UxukO zsK+)2pHX|Z9J}rKxd!c$crMG=#jcxFge;XL?e7`gViWv}m?-`|qR}BrYaIK41}2L) zBwk*{j_C5}Yf~I)@2?BkHDA~gd%!c@LBfN86~Xts1_fLD!}jC z?qGFe5z-~*jJ~+Mso$=+9UEV+V5-m({Ge|?4&uMSOZ&ap?B6@s6-)l04^qnZ=_Q2D z7P`bn`ZbVAFU;8^!iT_U;4C?RRgBdzK1=(pu|@a1Pmtcc{=b(;=L{x zr8vc-P0GiJ%Eda!x>$j|b?>1sB1)hw<%um!6!6gA>UtA_3y?awo63Im8B{Cfkk&g5 zG@q)1u3|ogqB$M?)q6B!O*DN&B!_5kf7RfB-O>K5P!o%W%;=xr{~+Ig;>3{aS92!` zXtTvR^r$@qeHdQA)7wIme5FIEPFFBw%ZUs->}$hT{@#v0$l$i>jetgj-YjHrQsYl zEO;||HTD^~PmyHrC{L#oPu+sw&2vb^(oN{q6Fv0#RyKaC6ob#xEqL0hWFp$6f<#^N zc*Aq@vC@29Wai|;tkX0nLvRw7EKWd&!_QKM1%BkPzcL=e>HHf!D{`hk8*kc_gxB|S z9HI0_NVP(c9@x2yoz$D(aO;GQ{Xr41&nc$p!!`=2Xm=2Mxj=^ASMUs7(lkJAVbvth z{5UB{ts>MO1w`*Wg8b}c(Y7`I)V-IM$Xe|l`J|-7rmQ-QIyB$FWRFIs%S9T_iDY0T z`G7i-eu6POB0)UfyvKej9_ZITF{G4S2yR+GF+If*2ev*#57L|HmT%?AZAUblbL%53 zP+U*%Z4{uVn5?CfjCUguaZ$9qA|LNsSI)kvy$m;(4uVVnJ~I2@9HOz!4YQdxT-R_K z{}?-;6jiz+yLaVyhR(P8+p$NO+1=vELS`~~nmUyf49Stgrf7Jksf>2Z_F?Z}ZPa|D z6o3)iE$_yDuFofdyWfGf=oI4Xu>@u7 zsIj|t*RahGWf}1-CnU|WSZ5n5k}Y0~iRPs}NPx`({@`P>AeV=(9Wf#?U3pkScM?93 zeurAc$l{FKCdez<8BRGF6OH0Hh)J!7k1tB`!~mwY$;E@}R~EZVI>O#(ndpIVC_7rT z9U0y`VXr!z!s1^T$(Cxk!&5s+ za_ht3t-(c*{o4rECNuFGA#w8du|6_i!H1<&AK}Ua>b#;${d^Vc02D03b>2+1CBE%z z(9Q`r5??WfCaw&_sy^X{>zUs6a||}%M_lLft`(N_W@8z)R3ZvV+6dAU3*6|`KT52P z!9S!Z;R%o|NqhdDL`P_DA~Ve%I{Ca6_0~mUgRBnbX>}GH@|jAq#yP%( zo-UQ0DL^ddtDq;bg4p}vKfcl8SrDxM9B{@eW-2OwKn6&`D;C*%jSK?T>Dm z1usmmqUwsj_?^8f@g47l<4PqQi~KApEgSs01>wA;zn?>59HUK|YuULzkg^6{R- zS?Jl1BiI*xBbE9AxVd}|I#p#tymD2Ds6-NFbTk+u_r${XccLh*(hgzsm++@|C2pMD zkJqLQQOBs8lvZ60vvPej<>g#~QqWBx<8Jn~xq1x(nSQiwn=?_#5n$Vtl^RwZF(bmK zvd}-t&!i`7IyN*I{&=M02uZpLf5ZDxM=N86n=gM zEwaLu{_%Hj!|#24%W>G9FKn~wBe%t7UY{)JW~BR6D3yWp{MJ9kxgGNFx7DyL|s6dSh=ku{JS~4 z5yx?gdh-OES&iOkl*kplx}xa@9EdbSBT1uA9^Uk23Hg`MO5z%ZpiQ0IU+1+E)d%;; zg3SjRnT?KY7cETZSaDhCXnS0>Qj#p6-i#*_74T$JHB^$E#MFm<0(-48yrxqe6>sgt zH^m3ArF9otVfmIk=XP?BtiD6<=bm@(_r3Jj@dM~k%}(~~=@$Gt3)uPct*D`Hq(NX) zG|`VaOE(o)k{6Gcqo}Qk?3pSFGGx+-1|v)9197^v2rED<-%uo}p1Lf(zY(@?(L}-W z64YqYEy`tdG0u4+jOvq=QN-~Ra5%#Z46S4E-L9+n?!wFX`fL+ALoS2%DbDBaHf;^N z%2e$;{DN7wS(N^eVNbsi>t)(vJ=oqRf7B8+#9ol^B>cX~w0CGG*}1+HC+v}6f4tyW z<3pkl{)8c~4Bj!CmGK{s4^}(dVTg}iuzr!vAF;TFWS@m`rkO=OBoW16L1J!`ib6j^dtMsrjRZa-^Q+f_Y>9l zmXrLAQ%SPQdKB8c7%Mqfqch3ZnSZx3QGjUz%{zCB+#1xwzjBY$J3xYM(E3JfHLMWmr)MmbU>}r*uujI>Y`(k#ZFzGlDtzvb z&%Nt|%BxyvLG@)OtpvkhP&Ql;*U)?Mk zBD;^Vyp8f~S7{}V`4mOxZA=B8xfH#mt%uR^s%3|C-w+81!#3dwMA4STJI|iEf4>S74)>zrpb* z606XQi55DYD!t{tf)x%c*= zl`Tf3+07PDKCOmK%4U#^Q7=%^H9-n*KR}i*pE>{I4Mn@CaT$S4;1_!tkH5M~RXe_B zUFuJVXAS*nwmQAh_8g&9bBLz+ELs-2S&izM z#QcLhs$L#SrpLdf>_m*Q_S8A(NX}v+8u<=>q~sxoPHT9qVhICZ)YiuGt}co(^_)0f^H9mGymo6NQ^Y@?IElo6}PYV36LpJbP|CYqEg%WBprLU8>f zrkQFXdu^T&%Q+kA7pDZtSC1RWb|@A7Pd?$wrGxmt`_KPNKjGw||E{0x(Ygh0CS~xg z#*$n+ZbK_BcOVg_3&|q+`6TUTAFubXEzUlVsE7^0$fx)iy2R=98gh=v=8-7blxza& zC>lLhHsl!0d*HE`IbJF89L>Gsg0FGe(?dZ5&|qE$(=8oPd6Xid&zWGy3pc62Z3F+& zg=&6{?)>lm^Z&|UbNKK7Cx88;dm%O*eU4ikHE`Wh34Evl!QyXYxKMf_yE@Gcjq~l1 zuDBuDh7jy|l0a9k2 zY0$#tWvSuodkHDB{KMpCMKT%{5}cutO;qf7q!8ovWjfYJ?~X zz5bTpc>5qZo9#>zTbH7+#%#u>EuU;V`ix8)S7EmYCqh)D3Cis{j<%i3C7W)RBdhP9 zspTpgNYl(rvgW2HieIQsE*?yVjX7N?WBywF>Qoe3J@SXy>34xVvIg{2Cy9tmDAEdv z53u3cboz|@IW)7-msE7GpdU?|Not4wft!;LR(%tRgkcieuG@%b@1)q;nnft%ZX+s@ zKSn1cd7{BxdTh(rUV@ti$?*;oR*BnRw(j|e9njMMi@5;b# z?;Yr3dJj^3@)BQH{R-pP??Bz^{doV|4dmyoAQ+rJfp3nQk=vh?d6`M~@q;v9bT%S| zV-kJ8^CSS$FiB9%+P*Ig&J&$Z63%-l9 zPYNfqKMZA1Np=}?Y`Zt^Y1&SvUZv37H=5*OMm{_)6d^nV1*B@pz1w0%A#R~6Jjo=w96Y>TN=$O&@K~*@tTZhfOeUe=;`k91`uciYYzJ#6vY1pyEo?am!#V+M~ zBoEDwrmvrPPgVBv(Xr7fTyK2^WtOzo_NPz|?sPH4r=!K{xm>PL8s1a#U=ZTk$Z#=`)9Er6=J5qDj2;4Gba=JYav7I z{55DNM|FBAx)nArr;)Ev68hdKPHTV4g#(9Hp%tq=(5I!|WJE=hl~|;LMu%hQ2J-~^ ze$g|$@zQiw&DMhT{63Gph>X}RToY>T{VGQPgA1#iCPJR6D56*AMd+?KN0^NT!sN;9 zg?QQBeN4`*G&COg6$?zLu~}Is!P3K>Y<0Op%pP8Y=!5T>pYqx$F!VEu4bMO(iXObU zc?XeyQV-G-4#9iZBtY8x1o#jyNq#>}Bs{K%Ofo+cu9;oOHpjj*;l{35J>nShsy&WU z-S)tnyEORu94B&h&O}OvhlM7Y!?wE;Z0x4*u=dYOwD0gFY!;FWTLc@)f$nfZUo9a< zAN~+IN)OGyYYzDJA~xWU5*b%DBo(KYu&RloIKXQ$8Vw%@e%nFvNEVZ2=RY9hI#V<^ zav_a{$gxZyWeG;2pj-IGgOMPJtY)HT3B9wdB-nd(?C%jD4E-k!TbN(z4Vc9O*fS)jZts zvt}#2e6|d%dOC>&4{zbUkvWCu&PcOa7nh8uw%%rZ8}9O&ZWV&hG!@FVD4jU2s>8q6 z?&OzAh+>K6IMAE&k4j3_C#_ilypWzaoE{><%~vyt)0IdtsnJ2H@#^HYwgyolx%}ea=czHZZs^Kt1ODM1 z%p{9u>YSl0wJ7Tp=zcUnrZHvAiT&!RXH=1B4ff#tfZteoB$QC~2SEFvEet$XC4ctU zF?Kf#c>7u=n5zmD99edg@d!MPRgOl%_15ip-SxASov13aHa8wtXu49pybwHcXcDuh zs*38WFU2zB8>t)n5~!VW4QOO zB)YQeCto}%j%W63E=UzxQzl<-1HD9=(%HC*46+rlF8w>WoHiye+70kUPf2)^(+SiL zZF2eEDk9E53A^&2;7d)CnL5i zaD%xZBhsdX+h8Rll2w3o4EK2wMn|vaSROGu{Lb#?COa5+d>W(38{` z^Q$1c(G1VFNroF|YWah+tEp3t#SkAVj3o=?u%5FvwWVq?xb_GT(LQnhzm@smklf1T zNKPfYRi{xhM=f|?U&do4wPa@chi(qxSx(8mD#tfhpQctP*f2rb-zW>4CZ>(cZ0{=G z00P3*yvqfX_;?_e@EgS-|6d{46^!%F_&SjFpD*LdU$22&;Zgi<_GFxq$T2z#*P@TU zHlY76ol$J_uu{J$pXUv?>$fs|1BV3b5Mh~?$0;MU{D=% zMfbQYCl-~SP$X-nAF)%}Qo^Wz*iAG~8IzCYi_tsz8#r%x79}w?mGS9tfoHc8nW5|N zse}9bm?#Kh3ATn zPa=x_EySmK2+L4rXjDvv=x*PLDmo59>vI=$HpB-O+|Ga_$E5HdH3vG#BM0T#`mhSI z;>fUAhF&FA&Z-sVp?RHU$d*?I`@g4?S*I_tiJHl*{n12n)wUEH9-9g4#mgDPR6SBW zPliOYQ;ibcLhF2?RiQ+6vkG<bc8)Lcv_9WEi;_ffcVqY>JCqzYeJ;K@F+{Rg&h3*m0`9vUZ1pv1Oe zB6wDeV_Rsmt6xW>;xP?cxr9eQ7b+tgvZT;cp%ip5)(efExJp-^RHW0!mC5aWxoBY3 zI;tW26FFZag~*vgboG7{wpnKeVPCZ9W~z%V?LCsk*37*`1m{$%n4#S_&;Qx1gr(qg0tq5Z1SAq-Mh*T4(AS^xAPZc8&Fe)ro4b zqLIs!*f^kfi={}tnGM-`aL~4(#0YJBrjCZUu4TUV&n2&S~3@zIDN`Ir3h>>fo{RaMA7I!cwkwI~Dc7(9h0jDzW2A5LK~ z+>D-ad#B-XBhcMfjP^`^TYvZN87gD!3wB&4jxI)cA)TKV$aP{aE3&+p5e<<*yI20E z?A(*7&$8ZZ2ZuDcmCEfipLuDkRcAsUus=>Go*6@1?f%gM)}riFl|pvQKmvWW>mprX zuY|HRW|YXCPx$1`ud=T0wY?V zn>g_|$l9B@aB=PLY1V>E3vT+en=<`EOcCh&t`SU`e;a-X++g|w$z1FgZ1Ori)16-kNc~`OD)f9pa z2uVGkf!{PwVI9v~U^nYR6gIL9@hW%1&%^!oCA(&z1xM10!88kaxW{Z}1+ z?9GAT>ITR);<8awcc8;7bn(e=>F9J!I#FGnh#cP4AfJck_T#+S4RM~c*yAO9lH@ar zmyg@gyM7LnVF_LK=3fJL<uxx{oAARutx(XSc_gtc z48Kmh#s(A^VtKnCNPcj+{S>1?vc=JvEh&g&SD8Gfi)J*VR|6vK?eUB3G?PJgnpGNI zw0{sk9GFCFho-W18k6ZMwEz?Ae|ZXe?-!5T zX(JRZH3qrX`$&p%C_1UW8GH1364N_&Xe-wNzqd&XiL_s$4!12NqKTK_eZL3_-x0(l zwGW^%$`u9o7Er?eMR2J?gmwR=PpWI|=`B`GboXsj+M+O)K3BYyoNAC}dv)iN`C^7- zt!+O0GJ6S~5dH$svAm5t9}+BwqG)efj_I3u6y?V~;Ceb!kiEzgJL9EAP#!*uBsvab z%a`Wl_p>2vGVDR7-Cqr%X<{Vj(l>0P_z;5HK0v2jEqREBnav|jRLaL}QZDg_1pX>z z=M|kpFT5P+A)6^=Kx7!To^VB|CZ7FL6^T3(#OOmQf^^G2G5Z7cd9=;<7Igb+3)VfR z3+GKWaSONeZ?#{VHc%)f!ly@w$@|GPau7{WiZ+N(wj{Ex`0H-7xNasQ$jhf zR+6LFwlk?}JJ7~U50F^)T5#>(iVt}R(B*HxLFIlsdgr5I@+|f({X4jr){sjfx&x`` z)-j+j3ZEmB{`g|*7@_;mE3zK4r@+%rmL7B-B3(;C!C&#{2@dlK4m;Gg?+0at5kk;k>rgrZ+O0ICc;W*Q~D1Gv*1~E2< z6%lJAr$1MbYxZ9`w)PJuxN|yLIIPB2>{OsrMboGjr<264$A^rJeSnIFQ-sGl60Zk! zy!m4Lh*G5i>OcD!A3MB)C&^_VdvrNk@$)MD@5FvAm;DOvtn#Dl#trB_1uabH-%Pr$ zRm8qhd>y^y*9`h_?-pe8c^&H(;E2Q$^69{rO03+AO3@U}Y|J1m8ikmi?iA?(;>PEG^jkZ4Io~tF!c*ukT3VflZjbEI`hi zKDAp_=t0}4iPN47*RnVFSfG1Df525JiGG?PL~=}@z-srWMC(pBI=`?khzcYQ4f&# zH5=}C77_V9_2}1lGQv<0d@Yje05@; z`xk9;^+$_HFRl`qhg1~ z?R4hX=a)PUsVXwf!kHJUyZ~bi9%4Gl3ZnSV+ES?c(z-3ylAkT?NS`y2%cTZly zMYhxJ|2}O)WdV-}&rY1q)YoPObElD6%H7mZM-|;@TTjm#bE7}qj>6wo{ewR!79A|k zMz5F45u?wx^xG;-gzfhc@xMS!Ue&-2|E@)88!ak#%MWasjaQWdk zG$ucpw$0q_}CDbxXoy7jX!c}>LJp5oY*sw8oueQXEu8)7B@5_ZA7(&a0VT^V|mwRD_c8 z$>XR=`Z1a#Ab=MB)Fpe>E0BL<3B2tRr7QZ^(_UR}^d_So-o()z^d{|iF5~kTQdX6c zr+pW|Z=#9gwZ6ft4tOHj`5W0rlMY_kxp*exmKvRzIF0VtilD`Udx*-f2zwvTNLn%N zEY?+TMhAX#^V=eBKiIaHjCNn8(QomF8Iv>E2N@|u?Ry)u>?UIQ6(^BMgd*{8XvH5w zYmjcG9X{P@^?!ACK0r}aaU9{C{*xfJ1+s6Jr$Q4G^a2sx{KtGi?P2p> zoVY)k*FeN+KcSnQX7hT>`4_wes7{FCQ-ZZ1@|Vs0iKGj>$H}eW3oZ@xTlVswHTZJ< zKeoVh$5NqmaubaIqt8~H^B2rsGz3$-UxjYB9)txKo1vn-9$a{Oj6Iz=!EHRho~ztc z20X`%;KQkz7^m}Z39bJ9KzJ@dFb8z9vv(ANQhkW4sqO@C zZ}k;w-%W-t!;c6nRZ8e;+H-gQva=Y@=YNbJi{ZQP-G9=~e#9r1U1WE?UBVZ96$ze- zi3RDGXLkQ0WnF$m{dVa$j?RV!d|1%S^ z_}=5k==^1~!IZyaXC5+tIn-6^0UNB}p5`D%$CHvytB?+iN+svw2kQ^BPE=* zhY@TD$sWx)SGv%r=ZV?1gp({~c=q~%jtmi!5)SLcE&DRrWeMsV0T zO2U*n(QqC(Y_*dxwBjVbt?y3rD5PTt=n%6VfJfi=<;s#N2mjuhS}>r98Fxk*=S+ F_Fv!ThTs4I literal 0 HcmV?d00001 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp new file mode 100644 index 000000000..797e82915 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -0,0 +1,95 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" + +using namespace ocs2; +using namespace legged_robot; + +int main(int argc, char** argv) { + std::vector programArgs{}; + ::ros::removeROSArgs(argc, argv, programArgs); + if (programArgs.size() < 6) { + throw std::runtime_error( + "No robot name, config folder, target command file, description name, or policy file path specified. Aborting."); + } + const std::string robotName(programArgs[1]); + const std::string configName(programArgs[2]); + const std::string targetCommandFile(programArgs[3]); + const std::string descriptionName("/" + programArgs[4]); + const std::string policyFilePath(programArgs[5]); + + // initialize ros node + ros::init(argc, argv, robotName + "_mpcnet_dummy"); + ros::NodeHandle nodeHandle; + + // legged robot interface + std::string urdfString; + if (!ros::param::get(descriptionName, urdfString)) { + std::cerr << "Param " << descriptionName << " not found; unable to generate urdf" << std::endl; + } + LeggedRobotInterface leggedRobotInterface(configName, targetCommandFile, urdf::parseURDF(urdfString)); + + // gait receiver + auto gaitReceiverPtr = + std::make_shared(nodeHandle, leggedRobotInterface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + + // ROS reference manager + auto rosReferenceManagerPtr = std::make_shared(robotName, leggedRobotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // policy (MPC-Net controller) + auto onnxEnvironmentPtr = createOnnxEnvironment(); + std::shared_ptr mpcnetDefinitionPtr(new LeggedRobotMpcnetDefinition(leggedRobotInterface.getInitialState())); + std::unique_ptr mpcnetControllerPtr( + new MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); + mpcnetControllerPtr->loadPolicyModel(policyFilePath); + + // rollout + std::unique_ptr rolloutPtr(leggedRobotInterface.getRollout().clone()); + + // observer + std::shared_ptr mpcnetDummyObserverRosPtr(new MpcnetDummyObserverRos(nodeHandle, robotName)); + + // visualization + CentroidalModelPinocchioMapping pinocchioMapping(leggedRobotInterface.getCentroidalModelInfo()); + PinocchioEndEffectorKinematics endEffectorKinematics(leggedRobotInterface.getPinocchioInterface(), pinocchioMapping, + leggedRobotInterface.modelSettings().contactNames3DoF); + std::shared_ptr leggedRobotVisualizerPtr(new LeggedRobotVisualizer( + leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + + // MPC-Net dummy loop ROS + scalar_t controlFrequency = leggedRobotInterface.mpcSettings().mrtDesiredFrequency_; + scalar_t rosFrequency = leggedRobotInterface.mpcSettings().mpcDesiredFrequency_; + MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), + rosReferenceManagerPtr); + mpcnetDummyLoopRos.addObserver(mpcnetDummyObserverRosPtr); + mpcnetDummyLoopRos.addObserver(leggedRobotVisualizerPtr); + mpcnetDummyLoopRos.addSynchronizedModule(gaitReceiverPtr); + + // initial system observation + SystemObservation systemObservation; + systemObservation.mode = ModeNumber::STANCE; + systemObservation.time = 0.0; + systemObservation.state = leggedRobotInterface.getInitialState(); + systemObservation.input = vector_t::Zero(leggedRobotInterface.getCentroidalModelInfo().inputDim); + + // initial target trajectories + TargetTrajectories targetTrajectories({systemObservation.time}, {systemObservation.state}, {systemObservation.input}); + + // run MPC-Net dummy loop ROS + mpcnetDummyLoopRos.run(systemObservation, targetTrajectories); + + // successful exit + return 0; +} From 929cd96423467aea7992035ffbfb666f445b1592 Mon Sep 17 00:00:00 2001 From: Alexander Reske Date: Fri, 1 Oct 2021 19:34:32 +0200 Subject: [PATCH 044/512] add mpcnet ballbot dummy --- .../ocs2_ballbot/CMakeLists.txt | 1 - .../ocs2_ballbot/package.xml | 1 - .../ocs2_ballbot_mpcnet/CMakeLists.txt | 25 +++++- .../launch/ballbot_mpcnet.launch | 20 +++++ .../ocs2_ballbot_mpcnet/package.xml | 1 + .../ocs2_ballbot_mpcnet/policy/ballbot.onnx | Bin 0 -> 580 bytes .../ocs2_ballbot_mpcnet/policy/ballbot.pt | Bin 0 -> 1831 bytes .../src/BallbotMpcnetDummyNode.cpp | 81 ++++++++++++++++++ .../ocs2_ballbot_ros/CMakeLists.txt | 24 +++++- 9 files changed, 149 insertions(+), 4 deletions(-) create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.onnx create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.pt create mode 100644 ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp diff --git a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt index b5ce41050..c8ea706bb 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt @@ -6,7 +6,6 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CATKIN_PACKAGE_DEPENDENCIES pybind11_catkin - roslib ocs2_core ocs2_ddp ocs2_mpc diff --git a/ocs2_robotic_examples/ocs2_ballbot/package.xml b/ocs2_robotic_examples/ocs2_ballbot/package.xml index a2e78e8d2..64b1eb1b4 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot/package.xml @@ -14,7 +14,6 @@ cmake_clang_tools pybind11_catkin - roslib ocs2_core ocs2_ddp diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt index da986a531..ee0ef0bc2 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt @@ -3,6 +3,7 @@ project(ocs2_ballbot_mpcnet) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ballbot + ocs2_ballbot_ros ocs2_mpcnet ) @@ -64,6 +65,20 @@ set_target_properties(BallbotMpcnetPybindings PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) +# MPC-Net dummy node +add_executable(ballbot_mpcnet_dummy + src/BallbotMpcnetDummyNode.cpp +) +add_dependencies(ballbot_mpcnet_dummy + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(ballbot_mpcnet_dummy + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(ballbot_mpcnet_dummy PRIVATE ${OCS2_CXX_FLAGS}) + catkin_python_setup() ######################### @@ -73,7 +88,7 @@ find_package(cmake_clang_tools QUIET) if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target ocs2_ballbot_mpcnet") add_clang_tooling( - TARGETS ${PROJECT_NAME} + TARGETS ${PROJECT_NAME} ballbot_mpcnet_dummy SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR @@ -98,6 +113,14 @@ install(TARGETS BallbotMpcnetPybindings LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} ) +install(TARGETS ballbot_mpcnet_dummy + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch policy + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + ############# ## Testing ## ############# diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch new file mode 100644 index 000000000..ca53ee47b --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml index 74223fd0a..56162164c 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml @@ -14,6 +14,7 @@ cmake_clang_tools ocs2_ballbot + ocs2_ballbot_ros ocs2_mpcnet diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.onnx b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.onnx new file mode 100644 index 0000000000000000000000000000000000000000..22a85b67e9e0b3de400ef6fa629c2bae077b7161 GIT binary patch literal 580 zcmd;J6Jjr@EXglQ&X8g@)U&i&%fh9>#c06AXeh*JBE{jHpO>6i5^tcy24QJ&bFm~= zWEM*>E?{KhO5kEN;o{B7%u7uy(koBROwTCc;)ZgQG82o17)_5TltC7trqFlEl1{cq1i{ zi@2l=+ty{z@K?@OiF~{E+!5}5C#bvq6u;(0R>Ib1ermCW@v(pN#b0L=0ZYT0zfl)pk^|IXeTBCP5^>e Bn

2gC5@ zz29$U-oJvxH*;KD8}}bcab29|>ByQh2Mi5snevRCAMfC%>f~gT?&|)qI$PEq6KBe< zZRt-7M=i(Hyh5k|*S`R<6VQ}a=r!2WEXQ(6z%MHaqN`=i(`HR<dOkGkx@+5}jxE>m zg=3y!dZsa9=~#exzYHymK%EYXMe5y(=wX|(m9jXlqn633<zv&qH+)pz#IQkEiEi20 za#U3yCjtmhLShj%%FwPPDSBGdXXZ7}P?77-_|U<alS((WS-x5fY`fkEQj<ytwH943 z0_<eKE~P7KDtl&?_ZDW}q;yiVk2P$n2KimpdMmSbD_f}5b~PjFQX|>S+-?hFG3y$E zZTfIKJKUnQ5UkCj4_kHYlKf>HCsXyxgqCW01h|7F=FSt)lSce9YGw>~r5VM%X_XL$ zZB+_2z?SU`s%O$T8nu)O(7TQc$%5~C>fEpZcc;@zD|wes-lLUF*d9WJJozJjEIbro zhY}+`gV3k&WY^09?38*j-1Df!NxYOe1!*ZE#ZE#03+$o0Fx)G{?q#OzS%QoV0}3f+ zZraj(7(~5@T#L4&kah@$&`ycRFwE!{VMK~8!rqYTJ`DS0*dJ2ezXT7+@L+(#>N<cn zAW|#`7hzO}F%%X?7NsUpm}(J)InALkd*dif&mk0MWP;T=DZHYo;lLxAdPqtr8_7G} z0M`!l5+6+a?EMaD26&jcMB4}+VTm!US=DmjFd?BKr79tUBNU%0NUd$jk1m%^lL*pP zShu-5xczm_JusLbA8Baqx;|e|xzt*@^6s&dV<rC`akKl6$e(_Br7zX~;b(kr_WR6V z;^gHQv)tc3qr2LhR{6e(E3bU=QsvOTWaXQMSWb%nv3drc$;rvpivFun`T4iEvhum> zmE&jM6f5!3_k_tM@l4^n74J1ZckS?xxmP<EatqzxiX%TAsJ#Eh#q7DnpW>%qofql1 zr!GAH-18(Lbp7AA#rkg#<;M@+%(vIV@y1g}vd>O!TKQo9?J@d}ulN3Pm1dW&_1}?< zgny0>E!Vlp_>Tzm)~m0VjyI7T=_*Jux~<LhXhEc?p-vhnd?Pv<S32)%@&hTbD0*P9 z5gUy~J@!k|sCC$(Mr`y)bVAqUiFgfLja@-%Y1v)%=;8OQ`O<Mf<;MPdsB)hZ3MvFg Yn~3|EPE8_o02QP-)0zp2`iH*%0)uJrT>t<8 literal 0 HcmV?d00001 diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp new file mode 100644 index 000000000..c51b8af46 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp @@ -0,0 +1,81 @@ +#include <ros/package.h> +#include <ros/ros.h> +#include <urdf_parser/urdf_parser.h> + +#include <ocs2_ballbot/BallbotInterface.h> +#include <ocs2_ballbot_ros/BallbotDummyVisualization.h> +#include <ocs2_mpcnet/control/MpcnetOnnxController.h> +#include <ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h> +#include <ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h> +#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> + +#include "ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h" + +using namespace ocs2; +using namespace ballbot; + +int main(int argc, char** argv) { + const std::string robotName = "ballbot"; + + // task and policy file + std::vector<std::string> programArgs{}; + ::ros::removeROSArgs(argc, argv, programArgs); + if (programArgs.size() <= 2) { + throw std::runtime_error("No task name and policy file path specified. Aborting."); + } + std::string taskFileFolderName = std::string(programArgs[1]); + std::string policyFilePath = std::string(programArgs[2]); + + // initialize ros node + ros::init(argc, argv, robotName + "_mpcnet_dummy"); + ros::NodeHandle nodeHandle; + + // ballbot interface + const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; + const std::string libraryFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + BallbotInterface ballbotInterface(taskFile, libraryFolder); + + // ROS reference manager + auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(robotName, ballbotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // policy (MPC-Net controller) + auto onnxEnvironmentPtr = createOnnxEnvironment(); + std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition()); + std::unique_ptr<MpcnetControllerBase> mpcnetControllerPtr( + new MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); + mpcnetControllerPtr->loadPolicyModel(policyFilePath); + + // rollout + std::unique_ptr<RolloutBase> rolloutPtr(ballbotInterface.getRollout().clone()); + + // observer + std::shared_ptr<MpcnetDummyObserverRos> mpcnetDummyObserverRosPtr(new MpcnetDummyObserverRos(nodeHandle, robotName)); + + // visualization + std::shared_ptr<BallbotDummyVisualization> ballbotDummyVisualization(new BallbotDummyVisualization(nodeHandle)); + + // MPC-Net dummy loop ROS + scalar_t controlFrequency = ballbotInterface.mpcSettings().mrtDesiredFrequency_; + scalar_t rosFrequency = ballbotInterface.mpcSettings().mpcDesiredFrequency_; + MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), + rosReferenceManagerPtr); + mpcnetDummyLoopRos.addObserver(mpcnetDummyObserverRosPtr); + mpcnetDummyLoopRos.addObserver(ballbotDummyVisualization); + + // initial system observation + SystemObservation systemObservation; + systemObservation.mode = 0; + systemObservation.time = 0.0; + systemObservation.state = ballbotInterface.getInitialState(); + systemObservation.input = vector_t::Zero(ocs2::ballbot::INPUT_DIM); + + // initial target trajectories + TargetTrajectories targetTrajectories({systemObservation.time}, {systemObservation.state}, {systemObservation.input}); + + // run MPC-Net dummy loop ROS + mpcnetDummyLoopRos.run(systemObservation, targetTrajectories); + + // successful exit + return 0; +} \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt index db81eb18e..2a7baa6c5 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt @@ -41,6 +41,8 @@ catkin_package( ${EIGEN3_INCLUDE_DIRS} CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} + LIBRARIES + ${PROJECT_NAME} DEPENDS Boost ) @@ -56,6 +58,18 @@ include_directories( ${catkin_INCLUDE_DIRS} ) +# main library +add_library(${PROJECT_NAME} + src/BallbotDummyVisualization.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) + # Mpc node add_executable(ballbot_mpc src/BallbotMpcNode.cpp @@ -71,12 +85,13 @@ target_compile_options(ballbot_mpc PRIVATE ${OCS2_CXX_FLAGS}) # Dummy node add_executable(ballbot_dummy_test src/DummyBallbotNode.cpp - src/BallbotDummyVisualization.cpp ) add_dependencies(ballbot_dummy_test + ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ) target_link_libraries(ballbot_dummy_test + ${PROJECT_NAME} ${catkin_LIBRARIES} ) target_compile_options(ballbot_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) @@ -115,6 +130,7 @@ if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target ocs2_ballbot") add_clang_tooling( TARGETS + ${PROJECT_NAME} ballbot_mpc ballbot_dummy_test ballbot_target @@ -128,6 +144,12 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) From fc66a21a2d0466792723ec58db3457467b4eec27 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Oct 2021 18:03:52 +0200 Subject: [PATCH 045/512] add mpcnet dummy --- ocs2_mpcnet/CMakeLists.txt | 3 + .../ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h | 81 ++++++++++++ .../dummy/MpcnetDummyObserverRos.h | 38 ++++++ ocs2_mpcnet/package.xml | 1 + ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp | 119 ++++++++++++++++++ .../src/dummy/MpcnetDummyObserverRos.cpp | 25 ++++ 6 files changed, 267 insertions(+) create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h create mode 100644 ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp create mode 100644 ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp diff --git a/ocs2_mpcnet/CMakeLists.txt b/ocs2_mpcnet/CMakeLists.txt index eee405aa8..9bae9aca5 100644 --- a/ocs2_mpcnet/CMakeLists.txt +++ b/ocs2_mpcnet/CMakeLists.txt @@ -5,6 +5,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES pybind11_catkin ocs2_mpc ocs2_python_interface + ocs2_ros_interfaces ) find_package(catkin REQUIRED COMPONENTS @@ -44,6 +45,8 @@ include_directories( add_library(${PROJECT_NAME} src/control/MpcnetBehavioralController.cpp src/control/MpcnetOnnxController.cpp + src/dummy/MpcnetDummyLoopRos.cpp + src/dummy/MpcnetDummyObserverRos.cpp src/rollout/MpcnetDataGeneration.cpp src/rollout/MpcnetPolicyEvaluation.cpp src/rollout/MpcnetRolloutManager.cpp diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h new file mode 100644 index 000000000..fd4e721f4 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h @@ -0,0 +1,81 @@ +#pragma once + +#include <ocs2_mpc/SystemObservation.h> +#include <ocs2_oc/rollout/RolloutBase.h> +#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h> +#include <ocs2_ros_interfaces/mrt/DummyObserver.h> +#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> + +#include "ocs2_mpcnet/control/MpcnetControllerBase.h" + +namespace ocs2 { + +/** + * Dummy loop to test a robot controlled by an MPC-Net policy. + */ +class MpcnetDummyLoopRos { + public: + /** + * Constructor. + * @param [in] controlFrequency : Minimum frequency at which the MPC-Net policy should be called. + * @param [in] rosFrequency : Frequency at which the ROS observers are updated. + * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). + * @param [in] rosReferenceManagerPtr : Pointer to the reference manager to be used (shared ownership). + */ + MpcnetDummyLoopRos(scalar_t controlFrequency, scalar_t rosFrequency, std::unique_ptr<MpcnetControllerBase> mpcnetPtr, + std::unique_ptr<RolloutBase> rolloutPtr, std::shared_ptr<RosReferenceManager> rosReferenceManagerPtr); + + /** + * Default destructor. + */ + virtual ~MpcnetDummyLoopRos() = default; + + /** + * Runs the dummy loop. + * @param [in] systemObservation: The initial system observation. + * @param [in] targetTrajectories: The initial target trajectories. + */ + void run(const SystemObservation& systemObservation, const TargetTrajectories& targetTrajectories); + + /** + * Adds one observer to the vector of observers that need to be informed about the system observation, primal solution and command data. + * Each observer is updated once after running a rollout. + * @param [in] observer : The observer to add. + */ + void addObserver(std::shared_ptr<DummyObserver> observer); + + /** + * Adds one module to the vector of modules that need to be synchronized with the policy. + * Each module is updated once before calling the policy. + * @param [in] synchronizedModule : The module to add. + */ + void addSynchronizedModule(std::shared_ptr<SolverSynchronizedModule> synchronizedModule); + + protected: + /** + * Runs a rollout. + * @param [in] duration : The duration of the run. + * @param [in] initialSystemObservation : The initial system observation. + * @param [out] finalSystemObservation : The final system observation. + */ + void rollout(scalar_t duration, const SystemObservation& initialSystemObservation, SystemObservation& finalSystemObservation); + + /** + * Update the reference manager and the synchronized modules. + * @param [in] time : The current time. + * @param [in] state : The cuurent state. + */ + void preSolverRun(scalar_t time, const vector_t& state); + + private: + scalar_t controlFrequency_; + scalar_t rosFrequency_; + std::unique_ptr<MpcnetControllerBase> mpcnetPtr_; + std::unique_ptr<RolloutBase> rolloutPtr_; + std::shared_ptr<RosReferenceManager> rosReferenceManagerPtr_; + std::vector<std::shared_ptr<DummyObserver>> observerPtrs_; + std::vector<std::shared_ptr<SolverSynchronizedModule>> synchronizedModulePtrs_; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h new file mode 100644 index 000000000..345ad4403 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h @@ -0,0 +1,38 @@ +#pragma once + +#include <ros/ros.h> + +#include <ocs2_ros_interfaces/mrt/DummyObserver.h> + +namespace ocs2 { + +/** + * Dummy observer that publishes the current system observation that is required for some target command nodes. + */ +class MpcnetDummyObserverRos : public DummyObserver { + public: + /** + * Constructor. + * @param [in] nodeHandle : The ROS node handle. + * @param [in] topicPrefix : The prefix defines the names for the observation's publishing topic "topicPrefix_mpc_observation". + */ + explicit MpcnetDummyObserverRos(ros::NodeHandle& nodeHandle, std::string topicPrefix = "anonymousRobot"); + + /** + * Default destructor. + */ + ~MpcnetDummyObserverRos() override = default; + + /** + * Update and publish. + * @param [in] observation: The current system observation. + * @param [in] primalSolution: The current primal solution. + * @param [in] command: The given command data. + */ + void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) override; + + private: + ros::Publisher observationPublisher_; +}; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/package.xml b/ocs2_mpcnet/package.xml index 3b2da3fc4..e6b0e9e4e 100644 --- a/ocs2_mpcnet/package.xml +++ b/ocs2_mpcnet/package.xml @@ -17,5 +17,6 @@ <depend>ocs2_mpc</depend> <depend>ocs2_python_interface</depend> + <depend>ocs2_ros_interfaces</depend> </package> diff --git a/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp b/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp new file mode 100644 index 000000000..be82d7679 --- /dev/null +++ b/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp @@ -0,0 +1,119 @@ +#include "ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h" + +#include <ros/ros.h> + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetDummyLoopRos::MpcnetDummyLoopRos(scalar_t controlFrequency, scalar_t rosFrequency, std::unique_ptr<MpcnetControllerBase> mpcnetPtr, + std::unique_ptr<RolloutBase> rolloutPtr, std::shared_ptr<RosReferenceManager> rosReferenceManagerPtr) + : controlFrequency_(controlFrequency), + rosFrequency_(rosFrequency), + mpcnetPtr_(std::move(mpcnetPtr)), + rolloutPtr_(std::move(rolloutPtr)), + rosReferenceManagerPtr_(rosReferenceManagerPtr) {} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::run(const SystemObservation& systemObservation, const TargetTrajectories& targetTrajectories) { + ros::WallRate rosRate(rosFrequency_); + scalar_t duration = 1.0 / rosFrequency_; + + // initialize + SystemObservation initialSystemObservation = systemObservation; + SystemObservation finalSystemObservation = systemObservation; + rosReferenceManagerPtr_->setTargetTrajectories(targetTrajectories); + + // start of while loop + while (::ros::ok() && ::ros::master::check()) { + // update system observation + swap(initialSystemObservation, finalSystemObservation); + + // update reference manager and synchronized modules + preSolverRun(initialSystemObservation.time, initialSystemObservation.state); + + // rollout + rollout(duration, initialSystemObservation, finalSystemObservation); + + // update observers + PrimalSolution primalSolution; + primalSolution.timeTrajectory_ = {finalSystemObservation.time}; + primalSolution.stateTrajectory_ = {finalSystemObservation.state}; + primalSolution.inputTrajectory_ = {finalSystemObservation.input}; + primalSolution.modeSchedule_ = rosReferenceManagerPtr_->getModeSchedule(); + primalSolution.controllerPtr_ = std::unique_ptr<ControllerBase>(mpcnetPtr_->clone()); + CommandData commandData; + commandData.mpcInitObservation_ = initialSystemObservation; + commandData.mpcTargetTrajectories_ = rosReferenceManagerPtr_->getTargetTrajectories(); + for (auto& observer : observerPtrs_) { + observer->update(finalSystemObservation, primalSolution, commandData); + } + + // process callbacks and sleep + ::ros::spinOnce(); + rosRate.sleep(); + } // end of while loop +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::addObserver(std::shared_ptr<DummyObserver> observer) { + observerPtrs_.push_back(std::move(observer)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::addSynchronizedModule(std::shared_ptr<SolverSynchronizedModule> synchronizedModule) { + synchronizedModulePtrs_.push_back(std::move(synchronizedModule)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::rollout(scalar_t duration, const SystemObservation& initialSystemObservation, + SystemObservation& finalSystemObservation) { + scalar_t timeStep = 1.0 / controlFrequency_; + + // initial time, state and input + scalar_t time = initialSystemObservation.time; + vector_t state = initialSystemObservation.state; + vector_t input = initialSystemObservation.input; + + // start of while loop + while (time <= initialSystemObservation.time + duration) { + // forward simulate system + scalar_array_t timeTrajectory; + size_array_t postEventIndicesStock; + vector_array_t stateTrajectory; + vector_array_t inputTrajectory; + rolloutPtr_->run(time, state, time + timeStep, mpcnetPtr_.get(), {}, timeTrajectory, postEventIndicesStock, stateTrajectory, + inputTrajectory); + + // update time, state and input + time = timeTrajectory.back(); + state = stateTrajectory.back(); + input = inputTrajectory.back(); + } // end of while loop + + // final time, state and input + finalSystemObservation.time = time; + finalSystemObservation.state = state; + finalSystemObservation.input = input; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::preSolverRun(scalar_t time, const vector_t& state) { + rosReferenceManagerPtr_->preSolverRun(time, time + scalar_t(1.0), state); + for (auto& module : synchronizedModulePtrs_) { + module->preSolverRun(time, time + scalar_t(1.0), state, *rosReferenceManagerPtr_); + } +} + +} // namespace ocs2 diff --git a/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp b/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp new file mode 100644 index 000000000..dcd544a6a --- /dev/null +++ b/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp @@ -0,0 +1,25 @@ +#include "ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h" + +#include <ros/ros.h> + +#include <ocs2_ros_interfaces/common/RosMsgConversions.h> + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetDummyObserverRos::MpcnetDummyObserverRos(ros::NodeHandle& nodeHandle, std::string topicPrefix) { + observationPublisher_ = nodeHandle.advertise<ocs2_msgs::mpc_observation>(topicPrefix + "_mpc_observation", 1); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyObserverRos::update(const SystemObservation& observation, const PrimalSolution& primalSolution, + const CommandData& command) { + auto observationMsg = ros_msg_conversions::createObservationMsg(observation); + observationPublisher_.publish(observationMsg); +} + +} // namespace ocs2 From fbc188e63db761e801c2dbdc79765963fac6b221 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 21 Oct 2021 11:48:35 +0200 Subject: [PATCH 046/512] add raisim rollout to legged robot mpcnet --- .../ocs2_legged_robot_mpcnet/CMakeLists.txt | 1 + .../LeggedRobotMpcnetInterface.h | 9 ++++- .../ocs2_legged_robot_mpcnet/package.xml | 1 + .../legged_robot_mpcnet.py | 5 ++- .../src/LeggedRobotMpcnetInterface.cpp | 37 +++++++++++++++---- 5 files changed, 44 insertions(+), 9 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt index 2342d76a5..dc4814f63 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt @@ -3,6 +3,7 @@ project(ocs2_legged_robot_mpcnet) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_legged_robot + ocs2_legged_robot_raisim ocs2_mpcnet ) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h index 41527027a..885ed3967 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h @@ -1,6 +1,7 @@ #pragma once #include <ocs2_legged_robot/LeggedRobotInterface.h> +#include <ocs2_legged_robot_raisim/LeggedRobotRaisimConversions.h> #include <ocs2_mpcnet/MpcnetInterfaceBase.h> namespace ocs2 { @@ -15,8 +16,9 @@ class LeggedRobotMpcnetInterface : public MpcnetInterfaceBase { * Constructor. * @param [in] nDataGenerationThreads : Number of data generation threads. * @param [in] nPolicyEvaluationThreads : Number of policy evaluation threads. + * @param [in] raisim : Whether to use RaiSim for the rollouts. */ - LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads); + LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim); /** * Default destructor. @@ -30,6 +32,11 @@ class LeggedRobotMpcnetInterface : public MpcnetInterfaceBase { * @return Pointer to the MPC. */ std::unique_ptr<MPC_BASE> getMpc(LeggedRobotInterface& leggedRobotInterface); + + // Legged robot interface pointers (keep alive for Pinocchio interface) + std::vector<std::unique_ptr<LeggedRobotInterface>> leggedRobotInterfacePtrs_; + // Legged robot RaiSim conversions pointers (keep alive for RaiSim rollout) + std::vector<std::unique_ptr<LeggedRobotRaisimConversions>> leggedRobotRaisimConversionsPtrs_; }; } // namespace legged_robot diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml index c3f6c5f80..e340a2d41 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml @@ -14,6 +14,7 @@ <build_depend>cmake_clang_tools</build_depend> <depend>ocs2_legged_robot</depend> + <depend>ocs2_legged_robot_raisim</depend> <depend>ocs2_mpcnet</depend> </package> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 5c1bb2344..feaaeb5c2 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -41,8 +41,11 @@ policy_evaluation_n_threads = 1 policy_evaluation_n_tasks = 2 +# rollout settings for data generation and policy evaluation +raisim = True + # mpcnet interface -mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads) +mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads, raisim) # logging description = "description" diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 0310481c5..1ce90ea43 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -7,13 +7,15 @@ #include <ocs2_mpcnet/control/MpcnetOnnxController.h> #include <ocs2_oc/rollout/TimeTriggeredRollout.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> +#include <ocs2_raisim/RaisimRollout.h> +#include <ocs2_raisim/RaisimRolloutSettings.h> #include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" namespace ocs2 { namespace legged_robot { -LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads) { +LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim) { // create ONNX environment auto onnxEnvironmentPtr = createOnnxEnvironment(); // path to config files @@ -33,14 +35,35 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr mpcnetDefinitionPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { - LeggedRobotInterface leggedRobotInterface(taskFileFolderName, targetCommandFile, urdf::parseURDFFile(urdfFile)); - std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr(new LeggedRobotMpcnetDefinition(leggedRobotInterface.getInitialState())); - mpcPtrs.push_back(getMpc(leggedRobotInterface)); + leggedRobotInterfacePtrs_.push_back(std::unique_ptr<LeggedRobotInterface>( + new LeggedRobotInterface(taskFileFolderName, targetCommandFile, urdf::parseURDFFile(urdfFile)))); + std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr( + new LeggedRobotMpcnetDefinition(leggedRobotInterfacePtrs_[i]->getInitialState())); + mpcPtrs.push_back(getMpc(*leggedRobotInterfacePtrs_[i])); mpcnetPtrs.push_back(std::unique_ptr<MpcnetControllerBase>( - new MpcnetOnnxController(mpcnetDefinitionPtr, leggedRobotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr))); - rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(leggedRobotInterface.getRollout().clone())); + new MpcnetOnnxController(mpcnetDefinitionPtr, leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr(), onnxEnvironmentPtr))); + if (raisim) { + RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout"); + raisimRolloutSettings.portNumber_ += i; + leggedRobotRaisimConversionsPtrs_.push_back(std::unique_ptr<LeggedRobotRaisimConversions>(new LeggedRobotRaisimConversions( + leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(), + leggedRobotInterfacePtrs_[i]->modelSettings(), false))); + leggedRobotRaisimConversionsPtrs_[i]->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); + rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(new RaisimRollout( + ros::package::getPath("anymal_c_simple_description") + "/urdf/anymal.urdf", + ros::package::getPath("anymal_c_simple_description") + "/meshes", + std::bind(&LeggedRobotRaisimConversions::stateToRaisimGenCoordGenVel, leggedRobotRaisimConversionsPtrs_[i].get(), + std::placeholders::_1, std::placeholders::_2), + std::bind(&LeggedRobotRaisimConversions::raisimGenCoordGenVelToState, leggedRobotRaisimConversionsPtrs_[i].get(), + std::placeholders::_1, std::placeholders::_2), + std::bind(&LeggedRobotRaisimConversions::inputToRaisimGeneralizedForce, leggedRobotRaisimConversionsPtrs_[i].get(), + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5), + nullptr, raisimRolloutSettings, nullptr))); + } else { + rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(leggedRobotInterfacePtrs_[i]->getRollout().clone())); + } mpcnetDefinitionPtrs.push_back(mpcnetDefinitionPtr); - referenceManagerPtrs.push_back(leggedRobotInterface.getReferenceManagerPtr()); + referenceManagerPtrs.push_back(leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr()); } mpcnetRolloutManagerPtr_.reset(new MpcnetRolloutManager(nDataGenerationThreads, nPolicyEvaluationThreads, std::move(mpcPtrs), std::move(mpcnetPtrs), std::move(rolloutPtrs), mpcnetDefinitionPtrs, From a31479f8d4137cf4df7fc57660f663e36aa192c8 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 21 Oct 2021 11:55:23 +0200 Subject: [PATCH 047/512] add raisim rollout to mpcnet --- ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h | 2 +- ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp | 3 +++ ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp | 3 +++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index 1192e1b0a..cfb871021 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -88,7 +88,7 @@ using namespace pybind11::literals; pybind11::module::import("ocs2_mpcnet.MpcnetPybindings"); \ /* bind actual MPC-Net interface for specific robot */ \ pybind11::class_<MPCNET_INTERFACE>(m, "MpcnetInterface") \ - .def(pybind11::init<size_t, size_t>()) \ + .def(pybind11::init<size_t, size_t, bool>()) \ .def("startDataGeneration", &MPCNET_INTERFACE::startDataGeneration, "alpha"_a, "policyFilePath"_a, "timeStep"_a, \ "dataDecimation"_a, "nSamples"_a, "samplingCovariance"_a.noconvert(), "initialObservations"_a, "modeSchedules"_a, \ "targetTrajectories"_a) \ diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index 8661ce3f7..0ea3e2834 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -23,6 +23,9 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st // reset mpc mpcPtr_->reset(); + // reset rollout, i.e. reset the internal simulator state (e.g. relevant for RaiSim) + rolloutPtr_->resetRollout(); + // prepare learned controller mpcnetPtr_->loadPolicyModel(policyFilePath); diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index fdd1023eb..9bb9fb62c 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -19,6 +19,9 @@ MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string // reset mpc mpcPtr_->reset(); + // reset rollout, i.e. reset the internal simulator state (e.g. relevant for RaiSim) + rolloutPtr_->resetRollout(); + // prepare learned controller mpcnetPtr_->loadPolicyModel(policyFilePath); From 248bb272a4271e079de4651ff6e078a8bc1ee3a1 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 21 Oct 2021 12:40:37 +0200 Subject: [PATCH 048/512] add warning that raisim rollout not implemented for ballbot --- .../include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h | 3 ++- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 5 ++++- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp | 8 ++++++-- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h index aa23cd6b8..26be26212 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h @@ -15,8 +15,9 @@ class BallbotMpcnetInterface : public MpcnetInterfaceBase { * Constructor. * @param [in] nDataGenerationThreads : Number of data generation threads. * @param [in] nPolicyEvaluationThreads : Number of policy evaluation threads. + * @param [in] raisim : Whether to use RaiSim for the rollouts. */ - BallbotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads); + BallbotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim); /** * Default destructor. diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 4ab5762e5..2ccb7dff0 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -31,8 +31,11 @@ policy_evaluation_n_threads = 1 policy_evaluation_n_tasks = 5 +# rollout settings for data generation and policy evaluation +raisim = False + # mpcnet interface -mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads) +mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads, raisim) # logging description = "description" diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index 8db1b0034..1da459497 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -12,7 +12,7 @@ namespace ocs2 { namespace ballbot { -BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads) { +BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim) { // create ONNX environment auto onnxEnvironmentPtr = createOnnxEnvironment(); // path to config file @@ -36,7 +36,11 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si mpcPtrs.push_back(getMpc(ballbotInterface)); mpcnetPtrs.push_back(std::unique_ptr<MpcnetControllerBase>( new MpcnetOnnxController(mpcnetDefinitionPtr, ballbotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr))); - rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(ballbotInterface.getRollout().clone())); + if (raisim) { + throw std::runtime_error("BallbotMpcnetInterface::BallbotMpcnetInterface RaiSim rollout not yet implemented for ballbot."); + } else { + rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(ballbotInterface.getRollout().clone())); + } mpcnetDefinitionPtrs.push_back(mpcnetDefinitionPtr); referenceManagerPtrs.push_back(ballbotInterface.getReferenceManagerPtr()); } From d15bf3afcdb0eec034f4c154a5b9a2192b1379b6 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 21 Oct 2021 16:51:51 +0200 Subject: [PATCH 049/512] add tests for getHamiltonian --- ocs2_ddp/test/Exp0Test.cpp | 67 +++++++++++++++++++++++++++++++++++ ocs2_ddp/test/Exp1Test.cpp | 72 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 139 insertions(+) diff --git a/ocs2_ddp/test/Exp0Test.cpp b/ocs2_ddp/test/Exp0Test.cpp index 6b6638f16..866d8fbad 100644 --- a/ocs2_ddp/test/Exp0Test.cpp +++ b/ocs2_ddp/test/Exp0Test.cpp @@ -229,6 +229,73 @@ TEST_F(Exp0, ddp_caching) { EXPECT_NO_THROW(ddp.run(startTime, initState, finalTime, partitioningTimes, std::vector<ocs2::ControllerBase*>())); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +TEST_F(Exp0, ddp_hamiltonian) { + // ddp settings + auto ddpSettings = getSettings(ocs2::ddp::Algorithm::SLQ, 2, ocs2::search_strategy::Type::LINE_SEARCH); + ddpSettings.useFeedbackPolicy_ = true; + + // instantiate + ocs2::SLQ ddp(ddpSettings, *rolloutPtr, *problemPtr, *initializerPtr); + ddp.setReferenceManager(referenceManagerPtr); + + // run ddp + ddp.run(startTime, initState, finalTime, partitioningTimes); + // get solution + const auto solution = ddp.primalSolution(finalTime); + + // get Hamiltonian at current solution + // expected outcome: true, because the current solution should be optimal + ocs2::scalar_t time = solution.timeTrajectory_.front(); + ocs2::vector_t state = solution.stateTrajectory_.front(); + ocs2::vector_t input = solution.controllerPtr_->computeInput(time, state); + auto hamiltonian = ddp.getHamiltonian(time, state, input); + const ocs2::vector_t dHdu1a = hamiltonian.dfdu; + const bool test1a = dHdu1a.isZero(1e-3); + + // evaluate Hamiltonian at different state (but using feedback policy) + // expected outcome: true, because for a linear system the LQA of H is exact and the linear feedback policy is globally optimal + ocs2::scalar_t querryTime = solution.timeTrajectory_.front(); + ocs2::vector_t querryState = ocs2::vector_t::Random(solution.stateTrajectory_.front().size()); + ocs2::vector_t querryInput = solution.controllerPtr_->computeInput(querryTime, querryState); + const ocs2::vector_t dHdu1b = hamiltonian.dfdux * (querryState - state) + hamiltonian.dfduu * (querryInput - input) + hamiltonian.dfdu; + const bool test1b = dHdu1b.isZero(1e-3); + + // evaluate Hamiltonian at different input + // expected outcome: false, because for a linear system the LQA of H is exact and a random input is not optimal + querryTime = solution.timeTrajectory_.front(); + querryState = solution.stateTrajectory_.front(); + querryInput = ocs2::vector_t::Random(solution.inputTrajectory_.front().size()); + const ocs2::vector_t dHdu1c = hamiltonian.dfdux * (querryState - state) + hamiltonian.dfduu * (querryInput - input) + hamiltonian.dfdu; + const bool test1c = dHdu1c.isZero(1e-3); + + // get Hamiltonian at different state (but using feedback policy) + // expected outcome: true, because for a linear system the linear feedback policy is globally optimal + time = solution.timeTrajectory_.front(); + state = ocs2::vector_t::Random(solution.stateTrajectory_.front().size()); + input = solution.controllerPtr_->computeInput(time, state); + hamiltonian = ddp.getHamiltonian(time, state, input); + const ocs2::vector_t dHdu2 = hamiltonian.dfdu; + const bool test2 = dHdu2.isZero(1e-3); + + // get Hamiltonian at different input + // expected outcome: false, because a random input is not optimal + time = solution.timeTrajectory_.front(); + state = solution.stateTrajectory_.front(); + input = ocs2::vector_t::Random(solution.inputTrajectory_.front().size()); + hamiltonian = ddp.getHamiltonian(time, state, input); + const ocs2::vector_t dHdu3 = hamiltonian.dfdu; + const bool test3 = dHdu3.isZero(1e-3); + + EXPECT_TRUE(test1a) << "MESSAGE for test 1a: Derivative of Hamiltonian w.r.t. to u is not zero: " << dHdu1a.transpose(); + EXPECT_TRUE(test1b) << "MESSAGE for test 1b: Derivative of Hamiltonian w.r.t. to u is not zero: " << dHdu1b.transpose(); + EXPECT_FALSE(test1c) << "MESSAGE for test 1c: Derivative of Hamiltonian w.r.t. to u is zero: " << dHdu1c.transpose(); + EXPECT_TRUE(test2) << "MESSAGE for test 2: Derivative of Hamiltonian w.r.t. to u is not zero: " << dHdu2.transpose(); + EXPECT_FALSE(test3) << "MESSAGE for test 3: Derivative of Hamiltonian w.r.t. to u is zero: " << dHdu3.transpose(); +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_ddp/test/Exp1Test.cpp b/ocs2_ddp/test/Exp1Test.cpp index e0244dc46..716ad3e7e 100644 --- a/ocs2_ddp/test/Exp1Test.cpp +++ b/ocs2_ddp/test/Exp1Test.cpp @@ -144,6 +144,78 @@ constexpr ocs2::scalar_t Exp1::expectedCost; constexpr ocs2::scalar_t Exp1::expectedStateInputEqConstraintISE; constexpr ocs2::scalar_t Exp1::expectedStateEqConstraintISE; +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +TEST_F(Exp1, ddp_hamiltonian) { + // ddp settings + auto ddpSettings = getSettings(ocs2::ddp::Algorithm::SLQ, 2, ocs2::search_strategy::Type::LINE_SEARCH); + ddpSettings.useFeedbackPolicy_ = true; + + // instantiate + ocs2::SLQ ddp(ddpSettings, *rolloutPtr, *problemPtr, *initializerPtr); + ddp.setReferenceManager(referenceManagerPtr); + + // run ddp + ddp.run(startTime, initState, finalTime, partitioningTimes); + // get solution + const auto solution = ddp.primalSolution(finalTime); + + // note: in the following highly non-linear system means more than quadratic in the state + + // get Hamiltonian at current solution + // expected outcome: true, because the current solution should be optimal + ocs2::scalar_t time = solution.timeTrajectory_.front(); + ocs2::vector_t state = solution.stateTrajectory_.front(); + ocs2::vector_t input = solution.controllerPtr_->computeInput(time, state); + auto hamiltonian = ddp.getHamiltonian(time, state, input); + const ocs2::vector_t dHdu1a = hamiltonian.dfdu; + const bool test1a = dHdu1a.isZero(); + + // evaluate Hamiltonian at different state (but using feedback policy) + // expected outcome: true, because for a highly non-linear system the LQA of H is not exact, + // however in the LQA of H the dynamics are linear and the cost is quadratic, + // as a result this is looks like a LQR problem and the linear feedback policy appears to be globally optimal + ocs2::scalar_t querryTime = solution.timeTrajectory_.front(); + ocs2::vector_t querryState = ocs2::vector_t::Random(solution.stateTrajectory_.front().size()); + ocs2::vector_t querryInput = solution.controllerPtr_->computeInput(querryTime, querryState); + const ocs2::vector_t dHdu1b = hamiltonian.dfdux * (querryState - state) + hamiltonian.dfduu * (querryInput - input) + hamiltonian.dfdu; + const bool test1b = dHdu1b.isZero(); + + // evaluate Hamiltonian at different input + // expected outcome: false, because for a highly non-linear system the LQA of H is not exact + // but still a random input does not appear to be optimal + querryTime = solution.timeTrajectory_.front(); + querryState = solution.stateTrajectory_.front(); + querryInput = ocs2::vector_t::Random(solution.inputTrajectory_.front().size()); + const ocs2::vector_t dHdu1c = hamiltonian.dfdux * (querryState - state) + hamiltonian.dfduu * (querryInput - input) + hamiltonian.dfdu; + const bool test1c = dHdu1c.isZero(); + + // get Hamiltonian at different state (but using feedback policy) + // expected outcome: false, because for a highly non-linear system the linear feedback policy is not globally optimal + time = solution.timeTrajectory_.front(); + state = ocs2::vector_t::Random(solution.stateTrajectory_.front().size()); + input = solution.controllerPtr_->computeInput(time, state); + hamiltonian = ddp.getHamiltonian(time, state, input); + const ocs2::vector_t dHdu2 = hamiltonian.dfdu; + const bool test2 = dHdu2.isZero(); + + // get Hamiltonian at different input + // expected outcome: false, because a random input is not optimal + time = solution.timeTrajectory_.front(); + state = solution.stateTrajectory_.front(); + input = ocs2::vector_t::Random(solution.inputTrajectory_.front().size()); + hamiltonian = ddp.getHamiltonian(time, state, input); + const ocs2::vector_t dHdu3 = hamiltonian.dfdu; + const bool test3 = dHdu3.isZero(); + + EXPECT_TRUE(test1a) << "MESSAGE for test 1a: Derivative of Hamiltonian w.r.t. to u is not zero: " << dHdu1a.transpose(); + EXPECT_TRUE(test1b) << "MESSAGE for test 1b: Derivative of Hamiltonian w.r.t. to u is not zero: " << dHdu1b.transpose(); + EXPECT_FALSE(test1c) << "MESSAGE for test 1c: Derivative of Hamiltonian w.r.t. to u is zero: " << dHdu1c.transpose(); + EXPECT_FALSE(test2) << "MESSAGE for test 2: Derivative of Hamiltonian w.r.t. to u is zero: " << dHdu2.transpose(); + EXPECT_FALSE(test3) << "MESSAGE for test 3: Derivative of Hamiltonian w.r.t. to u is zero: " << dHdu3.transpose(); +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ From ca8b7092e553a83b45c6c364b5572bbcdd5b919b Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Sun, 24 Oct 2021 17:37:57 +0200 Subject: [PATCH 050/512] add raisim option for legged robot mpcnet dummy --- .../launch/legged_robot_mpcnet.launch | 3 +- .../src/LeggedRobotMpcnetDummyNode.cpp | 57 +++++++++++++++++-- 2 files changed, 53 insertions(+), 7 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch index 8cae13cd0..3e7c5652e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch @@ -8,6 +8,7 @@ <arg name="description_name" default="legged_robot_description"/> <arg name="description_file" default="$(find anymal_c_simple_description)/urdf/anymal.urdf"/> <arg name="policy_file_path" default="$(find ocs2_legged_robot_mpcnet)/policy/legged_robot.onnx"/> + <arg name="raisim" default="true"/> <group if="$(arg rviz)"> <include file="$(find ocs2_legged_robot)/launch/visualize.launch"/> @@ -23,7 +24,7 @@ </group> <node pkg="ocs2_legged_robot_mpcnet" type="legged_robot_mpcnet_dummy" name="legged_robot_mpcnet_dummy" - output="screen" args="$(arg robot_name) $(arg config_name) $(arg target_command) $(arg description_name) $(arg policy_file_path)" launch-prefix=""/> + output="screen" args="$(arg robot_name) $(arg config_name) $(arg target_command) $(arg description_name) $(arg policy_file_path) $(arg raisim)" launch-prefix=""/> <node pkg="ocs2_legged_robot" type="legged_robot_target" name="legged_robot_target" output="screen" args="$(arg robot_name) $(arg target_command)" launch-prefix="gnome-terminal --"/> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 797e82915..73060803b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -1,14 +1,19 @@ -#include <ros/ros.h> +#include <ros/init.h> +#include <ros/package.h> #include <urdf_parser/urdf_parser.h> #include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h> #include <ocs2_legged_robot/LeggedRobotInterface.h> #include <ocs2_legged_robot/gait/GaitReceiver.h> #include <ocs2_legged_robot/visualization/LeggedRobotVisualizer.h> +#include <ocs2_legged_robot_raisim/LeggedRobotRaisimConversions.h> +#include <ocs2_legged_robot_raisim/LeggedRobotRaisimVisualizer.h> #include <ocs2_mpcnet/control/MpcnetOnnxController.h> #include <ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h> #include <ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h> #include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h> +#include <ocs2_raisim/RaisimRollout.h> +#include <ocs2_raisim_ros/RaisimHeightmapRosConverter.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> #include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" @@ -19,15 +24,16 @@ using namespace legged_robot; int main(int argc, char** argv) { std::vector<std::string> programArgs{}; ::ros::removeROSArgs(argc, argv, programArgs); - if (programArgs.size() < 6) { + if (programArgs.size() < 7) { throw std::runtime_error( - "No robot name, config folder, target command file, description name, or policy file path specified. Aborting."); + "No robot name, config folder, target command file, description name, policy file path, or rollout type specified. Aborting."); } const std::string robotName(programArgs[1]); const std::string configName(programArgs[2]); const std::string targetCommandFile(programArgs[3]); const std::string descriptionName("/" + programArgs[4]); const std::string policyFilePath(programArgs[5]); + const bool raisim = (programArgs[6] == "true") ? true : false; // initialize ros node ros::init(argc, argv, robotName + "_mpcnet_dummy"); @@ -56,7 +62,39 @@ int main(int argc, char** argv) { mpcnetControllerPtr->loadPolicyModel(policyFilePath); // rollout - std::unique_ptr<RolloutBase> rolloutPtr(leggedRobotInterface.getRollout().clone()); + std::unique_ptr<RolloutBase> rolloutPtr; + raisim::HeightMap* terrain = nullptr; + std::unique_ptr<RaisimHeightmapRosConverter> heightmapPub; + std::unique_ptr<LeggedRobotRaisimConversions> conversions; + if (raisim) { + conversions.reset(new LeggedRobotRaisimConversions(leggedRobotInterface.getPinocchioInterface(), + leggedRobotInterface.getCentroidalModelInfo(), leggedRobotInterface.modelSettings(), + false)); + RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout", true); + conversions->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); + rolloutPtr.reset( + new RaisimRollout(ros::package::getPath("anymal_c_simple_description") + "/urdf/anymal.urdf", + ros::package::getPath("anymal_c_simple_description") + "/meshes", + std::bind(&LeggedRobotRaisimConversions::stateToRaisimGenCoordGenVel, conversions.get(), std::placeholders::_1, + std::placeholders::_2), + std::bind(&LeggedRobotRaisimConversions::raisimGenCoordGenVelToState, conversions.get(), std::placeholders::_1, + std::placeholders::_2), + std::bind(&LeggedRobotRaisimConversions::inputToRaisimGeneralizedForce, conversions.get(), std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5), + nullptr, raisimRolloutSettings, nullptr)); + // terrain + if (raisimRolloutSettings.generateTerrain_) { + raisim::TerrainProperties terrainProperties; + terrainProperties.zScale = raisimRolloutSettings.terrainRoughness_; + terrainProperties.seed = raisimRolloutSettings.terrainSeed_; + terrain = static_cast<RaisimRollout*>(rolloutPtr.get())->generateTerrain(terrainProperties); + conversions->terrain_ = terrain; + heightmapPub.reset(new ocs2::RaisimHeightmapRosConverter()); + heightmapPub->publishGridmap(*terrain, "odom"); + } + } else { + rolloutPtr.reset(leggedRobotInterface.getRollout().clone()); + } // observer std::shared_ptr<MpcnetDummyObserverRos> mpcnetDummyObserverRosPtr(new MpcnetDummyObserverRos(nodeHandle, robotName)); @@ -65,8 +103,15 @@ int main(int argc, char** argv) { CentroidalModelPinocchioMapping pinocchioMapping(leggedRobotInterface.getCentroidalModelInfo()); PinocchioEndEffectorKinematics endEffectorKinematics(leggedRobotInterface.getPinocchioInterface(), pinocchioMapping, leggedRobotInterface.modelSettings().contactNames3DoF); - std::shared_ptr<LeggedRobotVisualizer> leggedRobotVisualizerPtr(new LeggedRobotVisualizer( - leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + std::shared_ptr<LeggedRobotVisualizer> leggedRobotVisualizerPtr; + if (raisim) { + leggedRobotVisualizerPtr.reset(new LeggedRobotRaisimVisualizer(leggedRobotInterface.getPinocchioInterface(), + leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, + nodeHandle, 100.0, terrain)); + } else { + leggedRobotVisualizerPtr.reset(new LeggedRobotVisualizer( + leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + } // MPC-Net dummy loop ROS scalar_t controlFrequency = leggedRobotInterface.mpcSettings().mrtDesiredFrequency_; From 3ed2efcb51e235a4aabd6ab05fb5f890e68d10a2 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Sun, 24 Oct 2021 17:46:12 +0200 Subject: [PATCH 051/512] fix default height for policy --- .../python/ocs2_legged_robot_mpcnet/legged_robot_config.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py index 7de37d21f..047130b49 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py @@ -32,7 +32,7 @@ # default state default_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.5, + 0.0, 0.0, 0.575, 0.0, 0.0, 0.0, -0.25, 0.6, -0.85, -0.25, -0.6, 0.85, From eda46281932f9056baf92f9e245051f3d63a7813 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Sun, 24 Oct 2021 17:52:01 +0200 Subject: [PATCH 052/512] normalized momentum and position error in base frame --- .../src/LeggedRobotMpcnetDefinition.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index 187df4588..8483f5a69 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -42,8 +42,10 @@ vector_t LeggedRobotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeS vector_t LeggedRobotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { vector_t relativeState = x - targetTrajectories.getDesiredState(t); matrix3_t R = getRotationMatrixFromZyxEulerAngles<scalar_t>(x.segment<3>(9)).transpose(); + relativeState.segment<3>(0) = R * relativeState.segment<3>(0); + relativeState.segment<3>(3) = R * relativeState.segment<3>(3); relativeState.segment<3>(6) = R * relativeState.segment<3>(6); - relativeState.segment<3>(9) = R * relativeState.segment<3>(9); + // TODO(areske): use quaternionDistance() for orientation error? return relativeState; } From 3324fc9a72c3f38b3a31582b654d52e1429235c8 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 25 Oct 2021 09:48:25 +0200 Subject: [PATCH 053/512] add input transformation for legged robot --- .../LeggedRobotMpcnetDefinition.h | 5 +++++ .../ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 10 +++++++--- .../src/LeggedRobotMpcnetDefinition.cpp | 10 ++++++++++ 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h index 55c6fa660..f96cad2b5 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h @@ -31,6 +31,11 @@ class LeggedRobotMpcnetDefinition : public MpcnetDefinitionBase { */ vector_t getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) override; + /** + * @see MpcnetDefinitionBase::getInputTransformation + */ + matrix_t getInputTransformation(scalar_t t, const vector_t& x) override; + /** * @see MpcnetDefinitionBase::validState */ diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index feaaeb5c2..dc150b0a3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -7,6 +7,7 @@ from torch.utils.tensorboard import SummaryWriter +from ocs2_mpcnet.helper import bmv, bmm from ocs2_mpcnet.loss import Hamiltonian as ExpertsLoss from ocs2_mpcnet.loss import CrossEntropy as GatingLoss from ocs2_mpcnet.memory import CircularMemory as Memory @@ -127,8 +128,9 @@ def start_policy_evaluation(policy): # get generated data data = mpcnet_interface.getGeneratedData() for i in range(len(data)): - # push t, x, u, p, generalized time, relative state, Hamiltonian into memory - memory.push(data[i].t, data[i].x, data[i].u, helper.get_one_hot(data[i].mode), data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) + # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory + memory.push(data[i].t, data[i].x, data[i].u, helper.get_one_hot(data[i].mode), data[i].generalized_time, + data[i].relative_state, data[i].input_transformation, data[i].hamiltonian) # logging writer.add_scalar('data/new_data_points', len(data), iteration) writer.add_scalar('data/total_data_points', len(memory), iteration) @@ -157,7 +159,7 @@ def start_policy_evaluation(policy): torch.save(obj=policy, f=save_path + ".pt") # extract batch from memory - t, x, u, p, generalized_time, relative_state, dHdxx, dHdux, dHduu, dHdx, dHdu, H = memory.sample(batch_size) + t, x, u, p, generalized_time, relative_state, input_transformation, dHdxx, dHdux, dHduu, dHdx, dHdu, H = memory.sample(batch_size) # take an optimization step def closure(): @@ -165,6 +167,8 @@ def closure(): optimizer.zero_grad() # prediction u_predicted, p_predicted, U_predicted = policy(generalized_time, relative_state) + u_predicted = bmv(input_transformation, u_predicted) + U_predicted = bmm(input_transformation, U_predicted) # compute the empirical loss empirical_experts_loss = experts_loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() empirical_gating_loss = gating_loss.compute_batch(p, p_predicted).sum() diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index 8483f5a69..b08b702de 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -49,6 +49,16 @@ vector_t LeggedRobotMpcnetDefinition::getRelativeState(scalar_t t, const vector_ return relativeState; } +matrix_t LeggedRobotMpcnetDefinition::getInputTransformation(scalar_t t, const vector_t& x) { + matrix3_t R = getRotationMatrixFromZyxEulerAngles<scalar_t>(x.segment<3>(9)); + matrix_t inputTransformation = matrix_t::Identity(24, 24); + inputTransformation.block<3, 3>(0, 0) = R; + inputTransformation.block<3, 3>(3, 3) = R; + inputTransformation.block<3, 3>(6, 6) = R; + inputTransformation.block<3, 3>(9, 9) = R; + return inputTransformation; +} + bool LeggedRobotMpcnetDefinition::validState(const vector_t& x) { vector_t deviation = x - defaultState_; if (std::abs(deviation[8]) > 0.2) { From 1f5639bb15b340aae154eb14a2d878ade1487a4a Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 25 Oct 2021 09:53:12 +0200 Subject: [PATCH 054/512] add input transformation --- ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h | 8 ++++++++ ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h | 1 + .../include/ocs2_mpcnet/control/MpcnetControllerBase.h | 8 ++++++++ .../include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h | 1 + ocs2_mpcnet/python/ocs2_mpcnet/memory.py | 7 +++++-- ocs2_mpcnet/src/control/MpcnetOnnxController.cpp | 2 +- ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp | 2 ++ 7 files changed, 26 insertions(+), 3 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h index 84cb206ac..8119b4ef0 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h @@ -37,6 +37,14 @@ class MpcnetDefinitionBase { */ virtual vector_t getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) = 0; + /** + * Get the input transformation. + * @param[in] t : Absolute time. + * @param[in] x : Robot state. + * @return The input transformation. + */ + virtual matrix_t getInputTransformation(scalar_t t, const vector_t& x) = 0; + /** * Check if a state is valid. * @param [in] x : State. diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index cfb871021..15a158b67 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -70,6 +70,7 @@ using namespace pybind11::literals; .def_readwrite("mode", &MPCNET_INTERFACE::data_point_t::mode) \ .def_readwrite("generalized_time", &MPCNET_INTERFACE::data_point_t::generalizedTime) \ .def_readwrite("relative_state", &MPCNET_INTERFACE::data_point_t::relativeState) \ + .def_readwrite("input_transformation", &MPCNET_INTERFACE::data_point_t::inputTransformation) \ .def_readwrite("hamiltonian", &MPCNET_INTERFACE::data_point_t::hamiltonian); \ /* bind metrics struct */ \ pybind11::class_<MPCNET_INTERFACE::metrics_t>(m, "Metrics") \ diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h index 42b460f0e..97d1070d7 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h @@ -53,6 +53,14 @@ class MpcnetControllerBase : public ControllerBase { return mpcnetDefinitionPtr_->getRelativeState(t, x, referenceManagerPtr_->getTargetTrajectories()); } + /** + * Get the input transformation. + * @param[in] t : Absolute time. + * @param[in] x : Robot state. + * @return The input transformation. + */ + matrix_t getInputTransformation(scalar_t t, const vector_t& x) { return mpcnetDefinitionPtr_->getInputTransformation(t, x); } + ControllerType getType() const override { return ControllerType::MPCNET; } MpcnetControllerBase* clone() const override = 0; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index 4d02dcb0d..f1a536ca6 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -31,6 +31,7 @@ class MpcnetDataGeneration { size_t mode; vector_t generalizedTime; vector_t relativeState; + matrix_t inputTransformation; ScalarFunctionQuadraticApproximation hamiltonian; }; using DataArray = std::vector<DataPoint>; diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py index 0fe845883..29b08b943 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py @@ -16,6 +16,7 @@ def __init__(self, capacity, time_dimension, state_dimension, input_dimension, e self.p = torch.zeros(capacity, expert_number, device=config.device, dtype=config.dtype) self.generalized_time = torch.zeros(capacity, time_dimension, device=config.device, dtype=config.dtype) self.relative_state = torch.zeros(capacity, state_dimension, device=config.device, dtype=config.dtype) + self.input_transformation = torch.zeros(capacity, input_dimension, input_dimension, device=config.device, dtype=config.dtype) self.dHdxx = torch.zeros(capacity, state_dimension, state_dimension, device=config.device, dtype=config.dtype) self.dHdux = torch.zeros(capacity, input_dimension, state_dimension, device=config.device, dtype=config.dtype) self.dHduu = torch.zeros(capacity, input_dimension, input_dimension, device=config.device, dtype=config.dtype) @@ -23,7 +24,7 @@ def __init__(self, capacity, time_dimension, state_dimension, input_dimension, e self.dHdu = torch.zeros(capacity, input_dimension, device=config.device, dtype=config.dtype) self.H = torch.zeros(capacity, device=config.device, dtype=config.dtype) - def push(self, t, x, u, p, generalized_time, relative_state, hamiltonian): + def push(self, t, x, u, p, generalized_time, relative_state, input_transformation, hamiltonian): # push data into memory # note: - torch.as_tensor: no copy as data is an ndarray of the corresponding dtype and the device is the cpu # - torch.Tensor.copy_: copy performed together with potential dtype and device change @@ -33,6 +34,7 @@ def push(self, t, x, u, p, generalized_time, relative_state, hamiltonian): self.p[self.position].copy_(torch.as_tensor(p, dtype=None, device=torch.device("cpu"))) self.generalized_time[self.position].copy_(torch.as_tensor(generalized_time, dtype=None, device=torch.device("cpu"))) self.relative_state[self.position].copy_(torch.as_tensor(relative_state, dtype=None, device=torch.device("cpu"))) + self.input_transformation[self.position].copy_(torch.as_tensor(input_transformation, dtype=None, device=torch.device("cpu"))) self.dHdxx[self.position].copy_(torch.as_tensor(hamiltonian.dfdxx, dtype=None, device=torch.device("cpu"))) self.dHdux[self.position].copy_(torch.as_tensor(hamiltonian.dfdux, dtype=None, device=torch.device("cpu"))) self.dHduu[self.position].copy_(torch.as_tensor(hamiltonian.dfduu, dtype=None, device=torch.device("cpu"))) @@ -51,6 +53,7 @@ def sample(self, batch_size): p_batch = self.p[indices] generalized_time_batch = self.generalized_time[indices] relative_state_batch = self.relative_state[indices] + input_transformation_batch = self.input_transformation[indices] dHdxx_batch = self.dHdxx[indices] dHdux_batch = self.dHdux[indices] dHduu_batch = self.dHduu[indices] @@ -58,7 +61,7 @@ def sample(self, batch_size): dHdu_batch = self.dHdu[indices] H_batch = self.H[indices] return t_batch, x_batch, u_batch, p_batch, generalized_time_batch, relative_state_batch,\ - dHdxx_batch, dHdux_batch, dHduu_batch, dHdx_batch, dHdu_batch, H_batch + input_transformation_batch, dHdxx_batch, dHdux_batch, dHduu_batch, dHdx_batch, dHdu_batch, H_batch def __len__(self): return self.size diff --git a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp index c92450de5..1d0ee87b8 100644 --- a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp @@ -47,7 +47,7 @@ vector_t MpcnetOnnxController::computeInput(const scalar_t t, const vector_t& x) // evaluate output tensor objects (note that from u, p, U we only need u = U * p which is already evaluated by the model) Eigen::Map<Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1>> u(outputValues[0].GetTensorMutableData<tensor_element_t>(), outputShapes_[0][1], outputShapes_[0][0]); - return u.cast<scalar_t>(); + return getInputTransformation(t, x) * u.cast<scalar_t>(); } } // namespace ocs2 diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index 0ea3e2834..fd3ad307c 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -69,6 +69,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); + dataPoint.inputTransformation = mpcnetPtr_->getInputTransformation(dataPoint.t, dataPoint.x); dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); dataPtr->push_back(std::move(dataPoint)); } @@ -83,6 +84,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); + dataPoint.inputTransformation = mpcnetPtr_->getInputTransformation(dataPoint.t, dataPoint.x); dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); dataPtr->push_back(std::move(dataPoint)); } From 1e39a476e3b20b45f65e17d0bf7260caffa640fe Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 25 Oct 2021 10:40:04 +0200 Subject: [PATCH 055/512] add input transformation for ballbot --- .../ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h | 5 +++++ .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 10 +++++++--- .../src/BallbotMpcnetDefinition.cpp | 4 ++++ 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h index c23ade1a7..21bb01b72 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h @@ -30,6 +30,11 @@ class BallbotMpcnetDefinition : public MpcnetDefinitionBase { */ vector_t getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) override; + /** + * @see MpcnetDefinitionBase::getInputTransformation + */ + matrix_t getInputTransformation(scalar_t t, const vector_t& x) override; + /** * @see MpcnetDefinitionBase::validState */ diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 2ccb7dff0..e3886cb95 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -6,6 +6,7 @@ from torch.utils.tensorboard import SummaryWriter +from ocs2_mpcnet.helper import bmv, bmm from ocs2_mpcnet.loss import Hamiltonian as Loss from ocs2_mpcnet.memory import CircularMemory as Memory from ocs2_mpcnet.policy import LinearPolicy as Policy @@ -102,8 +103,9 @@ def start_policy_evaluation(policy): # get generated data data = mpcnet_interface.getGeneratedData() for i in range(len(data)): - # push t, x, u, p, generalized time, relative state, Hamiltonian into memory - memory.push(data[i].t, data[i].x, data[i].u, torch.ones(1, device=config.device, dtype=config.dtype), data[i].generalized_time, data[i].relative_state, data[i].hamiltonian) + # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory + memory.push(data[i].t, data[i].x, data[i].u, torch.ones(1, device=config.device, dtype=config.dtype), + data[i].generalized_time, data[i].relative_state, data[i].input_transformation, data[i].hamiltonian) # logging writer.add_scalar('data/new_data_points', len(data), iteration) writer.add_scalar('data/total_data_points', len(memory), iteration) @@ -132,7 +134,7 @@ def start_policy_evaluation(policy): torch.save(obj=policy, f=save_path + ".pt") # extract batch from memory - t, x, u, p, generalized_time, relative_state, dHdxx, dHdux, dHduu, dHdx, dHdu, H = memory.sample(batch_size) + t, x, u, p, generalized_time, relative_state, input_transformation, dHdxx, dHdux, dHduu, dHdx, dHdu, H = memory.sample(batch_size) # take an optimization step def closure(): @@ -140,6 +142,8 @@ def closure(): optimizer.zero_grad() # prediction u_predicted, p_predicted, U_predicted = policy(generalized_time, relative_state) + u_predicted = bmv(input_transformation, u_predicted) + U_predicted = bmm(input_transformation, U_predicted) # compute the empirical loss empirical_loss = loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() # compute the gradients diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp index 97251bce8..9116651bb 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp @@ -11,6 +11,10 @@ vector_t BallbotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x return x - targetTrajectories.getDesiredState(t); } +matrix_t BallbotMpcnetDefinition::getInputTransformation(scalar_t t, const vector_t& x) { + return matrix_t::Identity(3, 3); +} + bool BallbotMpcnetDefinition::validState(const vector_t& x) { return true; } From 46e443db46f92f6757b2f67757a7344c2a67df04 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 25 Oct 2021 12:45:22 +0200 Subject: [PATCH 056/512] new policy for legged_robot --- .../policy/legged_robot.onnx | Bin 71976 -> 71976 bytes .../policy/legged_robot.pt | Bin 79865 -> 79865 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx index 8f544076c7ac8809c6d9793d3615c126e892af00..6e2ef510dceaa114c6897aa40b15aa8e7c48c940 100644 GIT binary patch literal 71976 zcmeFZ2T)bX7B))GS(0P{K|qn5&hE7l6cCIUKrw)d3ZfVQ!3Y8d3<OaLq5@*XtVlS$ z8q8Tl5Cb5HSuvuR{m;xD$2<3*JNMR9y;t>KU8;8Nea>2E_t)R<uY2|C-pI*l$j(lP ziJm)ox}K!Ht-D9YVJQVG1@RaK@pughM>jn=@90^R!(t}dnacd*%34uDDl9%?p04;d zaS4T43L+<kZqveIB4$mSI4eBH*3Nc*c*L~nF$(H`_;ON2*gVk<-SuRA!)MN%Xm2X@ z%h_60K{_mI_Vh3<F|kB`o4B~Oih|Un@R*;Ul@(-S=7!Ch=k<^Bv_<8)Ye+cR>B)=? zn>Br+gQ?Un=U;F*+5G{B!=K@Bu>A)PC;Q)UIQ~r>R-*FkMK~Pv<VQqLjhPu1Kheol z_P5JFQuI5UPL98`>HOzxCW?!0>!@H59zQ#LZp<%q&J*z&6)`J3Y_9!}`sn?+^|y*S z{Z=uTzgaO6!A?%1iaGyAu<LIG{|e3d&!PQJ^`AofopYDppt=1`Xg`T`{hdhnKPNKj z7b0E%h}QluiTr8(jYu~=*?+WDyT1unM+Cu5#H72P?5|DLUexA)ZL7b6bN_R2e@^8O za6hPYw)+82)c*gLj;w!3+}U1F{x^x+JN|nzo$ddK*WoY8{Av9Mna&PB@H+iXydpB4 z9W*4I9e<GN{7+;$|BGaP6#wUB{<QuE&gloZzpI&_WIF#O)AcXN{1+WL{w0||t-s-Q z`GNQE3h5UzT|{KM{vgx+pUM0e+59N}&&m90{SDj?**N@N+59Ba{dY1Q?Eiwye^HXt zUy}LL`VYJ=Ka}L~ca<a})5T6h!o~hKG95%i%I`9F`4`#zDE`mM{Av9S+z;6}{9W1n zPNs|FPcr|_;PXqj<nk{{a{fy)e_DUT`$I_%e^*JrkSQ8{T%3Q9`D6I`72Lnb=11{= zPUcVRZ{U8&#^LYE<|mo1Kgs-y0m$WFl;rZ4Wd5}NhWCe(9RIG8ej!sd0J;1afE@pM z0CM>k+59N}&&m90{RcSLAF^@$yR!M6OxK?SkmFwrK(7CyB-g(r^QZMUyg!uW_;-~g zBGXkg0J;7cfE@pM0CN2o+59N}&&m90{SDj?**N}P+1QFg4N-^g>a3?UYSz3tvEkth z!Y4Yqn#%wF=^r6!_&iUEZQ`;DB7bL5pXKWE2Y<J}^cTs})%6el?tkeoYI&}1|LpH% z_gDTdzmV(x2Y>s&^cU6F&F&BW4u9qEDypxW{U7`t|I%MnUpI$8_&fbu|Nf%J;Ve2_ zM2D;Ba1$Nw3L<=>!(McV>gM*d19tkqJ7CdMk@)4QIQ`=3^oLSi{%xtoqB}c^j$e?9 z0*}FAF@s~HCOU}%kAGY$Sc{C#BBP6*!ie7*g_E0TmhzA5|Eo#3{X*#<9<kHEf5c@P zB4;-}MN#ZHFD7hO%tU89(_iL_zkaaJ(NK_%3yX>kw_dFwCoZQer>iFJpQs@wcFs%e z|M9>7mGR$G{6DV#$JKw&z<=-g|Ce@U((k{o^WXLSUm5>BbN}P&e_Z|d4E*<=|L?S` zZQ@c2zeH`K_|VN&6hDbF9KS_v&Y}#*KSgb#6t}zG&#2AW;on4Unc^Zx`+ttY9RDc_ zi}?|RP5&hZ6GdR5KL}|k|NeU*_TOCp_}zax|5Cy33L@;HLzEtOchHmRH)YC1XD3rh z(fRkfJBm!Ac=$)%Me*=&buW_<S)Bg4J}&=MpB$MV6<RIxYg8%sOH?WIpFsU5`!59i z)$yNx|DWuCb?<*B|3BU5-`W4ae*b@0um64j-!bq*bfUSSyJ#-xE?T<zRdlYuicUlA z*VVl~kr6R{L@P(pb7QQ(>B?!#nX8F={n@&mf2;1NTLl>M5x_RB8rpiRGZkM+=G&q~ zd-m%GGY-nJ3z{!6<lt%2FYqv=Z&`?a&9@Sx)cg2S*8s<?d;<AWiu|aOSKMcDLhtl4 z;IkGDhnpj$h|;S_+@<$Oc%^KIuxb547!gtk5;47azFmXW)?VS9nj>)4L2>?~-%C2S zs}+}YPKA4Z_pz|(F$_7>ok_en26Z7jaK33EU*A`irLE}#!)iI)bJ>y)E|*|3H`c%? z{Xuy4Q5JD|Y|aj+15E8E$;V!vP_e?~GEVeMBqp{#Y~tx}xIL_h9({fboCht3*kA+P zXs5$;eDC4J?3-9%@dgxX(;!DF58h%e&KC}Yh3`(lINLeG@-Nb~q)VM;XUa4FOf~Et zehySOo3g#G!K~K&DXCrck(;qXjg2+nm_wEs)D6vooL*T(+wmZoXLA_$=D9$~_B=9S zdkr|z&!8IH31!DO!FS_8Jov~14$hl^EP6c|_gRsjdVfE3_P)%uUo&L&@j)1zup1|E zEim}?11^uv#G;93Fs9uB+usWKDf$&;f@v1KR4>N6vn_ZDOFf*w(w;96v!}fRI^olb zqgBEOYJAI;5VX2~k=r`_G;&}AGrrV7uWjz+&h^cBzo<=ElB2{*>ze83$r3PBVJH}% zUILEqYsu)q9!yTZ7FES^smsdea7UsT2F~69*ThxX+C}Bme-Z=VF-M>&Zv<5@J&I{6 zSBd@5dQja!ps!Xc)~f2GPozE{>m&)|axP&=ojzLRK7>m?BWXZ4DOk``pU;nLs*<>G zAn?Ec8t$EPr4AQz@J7dNylFcX?LtnH{qwE(4X&GD+L$BciSHYHyR(+IM%82IliOAE z#j_x=_9G!j=hC^2i|947N*JI#6gEfY(D9i&p|<y7eB%3^x_L@6d|}LY<DHn|FdeF@ z`x$iyO7d6AyD(6)l3sAxj1&8vBc*lSSxv4wjK43=+kUbV%F9&ahLBlw;PHKUUaJP4 z7PM2%DJE=}bS`}?)<J#D_4vM<((sk79M2Uc2@3B!U|47&9&l@d04H%?OXd=4N^3!D zhc&eh(!r&(Mnk}xC8VL*f_?v}09&t(B9FTFh5MxgnD^@dP%k=3u5*bbb-yN~Ss#TV zU9VtA=UZ;Ugrmas_hPXAMk;*}X@#kLHtyVb7_e(3XxB6BZ;%EZHGAP$rXd`Q*M@q9 zEo4mdV$|6g0B7D6V1;J^RE=&W^H&W+_uF!n>pk=7-DO2oHq{wgAH9T(iyC;;avNrv z*3deifxP^_B5Y|>#<|o7pRBUw$$}#K-BucI8?J<l_hWFnV=>&nC&%|lHm3Wh%tY-s zH}JCGFzgHs<YFI`!>+voxV@wh7mbQwF~=5jt2`FryR)V6aq?;yXEvNX3qL@~x^QeU z%cU#M_U03BO+Z1`KKC;pzC-zv7Ssr6f^mi~>D68w@9bSlCpzfz6SOCwnS?q&uOki> z7WUxxOPdqBXn&Y9Ka|=XlV*c2orHPry&y>}6U)C|gGmRLV}FgA5W7-I^qn2sr^4vQ z*W=JB_A}L)WQ|Up$)Mu0N*L-B2(QkGvzw`%7-%>M*L^q#35!c1p)(9Lmp#CH$#T4m z>SuDI?hy=aQDG-Het^gegWzqh4U}BV#qDj8AZC&dFJ68n+t%O2siPi|{Svdm;@Ehi za6z86=On`FNiwWbV<p~r*oCHnJ@{!3)<o`O3-#C|dJd-Pcy{P_D%&B&%g1*U$ZVPl zrTl!1c)15uELK5a@C?k2TMxT7za^{NqHqJR#lDSuM9s2J!(GddsQE~TS*Gj3r;u~p zyZCI}<hc+>anc}@u8yrT^)U2MKBi~Lu~E)Wuw#xcm=1XXE2b_)tM>PxdAw1O`t_@@ zQdScM4;Ij+wWa*-$zSOVM^pad&_SquJ{6CSnT8L>y@2!6pTp>```9AbhsQG+wGlTb z8{#;)9^eSN_F<^KSC_9-*5D_UOY^c8gPE3h8YHy5hsd->aK5U;!hGb&){GnUL;fz< zM3+HWtpZE*enAg?QNpB#flMu~7;VhzgppGNp=Ej@1lfuSFe-_?nJ^EhI_NU@&QZ`m zDHaYrDTJi(0Q5QEh?X;i5OH?{Tpj?J;3S|yI_50?$p_dwrVx%E|3os^o<_mBr=aYw zzz;B&fE?+o7^!_3oDZypT^qVXcjL!cw#O7J*EfN1v=$#;`hh<A(g&`dzmA>F3oy2y z6h&(}{(!tU*LvWR;LDcNSXP86J@-7-y!QxPcJ8KbI}M;y^E4La4<?g$AAyw$eev#c z9%NpY<C>WD$P|je^O!a)FcV|vYH#4sDX#pc4OfV!b^}gtIgDHS8S*FhUnfg<?x0e- zJg@sQ1HYeLii%lnFl0s-Bu}%2-sZ`8Xv1h&^sJd$M|MDG^Ly}X?29(#IdsvyBoH*7 z<Rnv9BU!3T!re9K6zOZw@>UlY%@M=I3nMY+v;haT*XR?^haebGff-YD_)9t?acY%4 z->K(IAMTjXEAC#6d!Lqq_x=hxHP4KXFzp6iRi|N9YYVYXWBBCSYjC$eMP4r6fQlxn zeEW4B=4lfQcy$Q&(LG9@+7{5R>yhlK)plXls!CL!Ispnohr&$p63AF_0@sy4#$n&Q z!8ufg7f!H%^@sG(*HRnL-T8_E9u24~z7iC#4FiichOGODk<8Q>V4HzCoVYE?Y>E_F z>|S+Dx7bA{`ANdf5LpsEq7z4N4JWDJmcxmN9x&N25nR{Iz>EWr(f^t)&Z*7=Ys`f+ zu9k4YXc2a=Z=>DrrC^KE6&m(cNOBToKrKZB>PO$j6yqT@^)S#y1C5w+?~70x`x!n? z5@%|%E=>5oj_67m^5?qm#5s|2EY%^OboCF1{dwDkWg#-`WYlY#?|1|H5f}b^#}^vC zb`{KWPK4x98(^bV2kqyQ0!PnW#K@(Eq~M4>FEc=m>v`xjJZV~oE&3|h-S3@nX^1Xd zIo=;m9;?Q60cVhR8wP<=dc0!yePpqV1kBsM5ntYL=O0Cs!o2L+q*(DGdfe!Ox?U5& zz3U0d=$nc2R~g~kCmq;zKpGBL-3AFs3-V^JHcEu=p$9z<<5$y#(D#cQZeJ?JzId$@ zTs|~c)CY+3)1NS6|1=BIx^~gqyHmkyW;@+&co@z|#e(2uqVVjU<8ZKjJ4(;~L?^q{ zb4e{FxVK7*PkSGS7Q!uX<ikF+n`z8foiW0q6|ab~n>|jqJ_Hj48RX~}Q}UW+k)A2> zNR{)kq^}yt2A`pWl#O^#{cAWeLI5xCXV7sGy)f<AwW@tdr}0W=1Fd<#7dK5VBa+(U zLT%F>@TGYgzFnKg?dZP@gicnR-I)zgKdT+mKhK4gE6(BC#ZK6~yB?-5c>{6P16Zc< z75X_sj(_Q!0e5aa!mmT+u{oo!aQwmz=<c!&wipTplG{eJ%tDT4>?lJLE)M!3(}i1> z-J?fLgYb0GRvf!d8=dX8;P)5a)IfC`#?KxMfuH6I@2s1MYP~|Se62g`Y#9k9pFLnq zf;}l2_5}4!hmzC@$H6H+7glx|Fxjt?_%)~s4w|1Knit+<*0e`dx^NV%Q{4_^fimhX zmgFU0?Zh5;kK)or-n@ayZ7|G!LEfFc3|ICagp8?cq37{i;C+Dyi(&3?UP+uc&07j} z?<M%&RcnxP8;Dbn4cM*c5%_jT7q=b1jds!+ICR)0j2@o>Di3!<U5YL{@O2%uhNa_! z&KFSk&6fq#`SUVMG;rF&-C*6O&6G4hL*;!9h|3W5hZdzouJ3B$Q^8HV`@)%o)tCv3 zqZRq;eP#HuNCnb&7h+s^ceZB!YC2bAGl^Y!it>RbwD?dmxPRG2%kLFJ;hd}J^IQpM zM(u;J=uVKe*#_2g<g`VzkN<mvpnvf8G(Uc`v^mSt>d&9*W5?1~-a@OjL4265AOEE} znI%5cU{*FhthL;UpQ6!^DOdWjeWNbH{v8%<hHg82*7M<CawfdE#7)%etHE~lxdF+% zJhN_*mXrIn)PJ7y{8*p$ua@qy#E*Z~FOnHLc=Pqs7qFBpZT9d`46hyG&u=)M#L7$^ z*!4v@?D2<8epPWS`(PBo<Y!ES=J*+`yTVma@CoEAf**q88UeeMAj3weOYm)<8?frY zJAARd7vFt>Ijhu-<HICA;nVC`-liMRl9l?PSr0$HXP5;avmy(0Kj^R_AkUZ9@vLjR z86Q8aA2UHa-hHz(FDE<zMyGc{<3lB8wcrH=CFKJyG-8`B+OZGGCOqk5%-8hp!=5NV zfY~aRy!3$(zRua1Nh#&xm7qA#dQMR|xfza58^NE*)o0J+C7E5&0WxEm4l6!#6JNG{ zh8-{7;-IW#*5a+j7a#OvYaZM3>-)XNcc$k+`fwj!P||^`Z^W>{pG<h!W6jWe(n9|I zrAu^`ybyIH;@Pn|zM}cYOm=3<F|bfeVeapo`Df-{%xc(hc8og$@n+&|+`d}0lw8TI zZricoNqt%K2uJYqSjM(ZRp3huyD|BAU}8T0e2VJ`Ha>qN`z+Ce-`8gzuPC9z#CCLp z1><M%JxWXYA_*sKvJc{yMjeK$$B*zsG`#t;t-k!e4~qQbH%sBUsPBy*tj^EZNoBTC z%lKitj-g4sKEFtNB_AFp#jEsnWp8)6vBUWZJlr-SlV+&1GJSLA>#_noW*xzhYcA~g zoirwRlg6*;xq_LwCy~xU%h{php=_$pIeZi7&A+c1Ks<x@g0!MP4itQc$MZ(<dpT3S z>5VcgdT|~OJC9)>Ck^BScj;qH!WA%9G~%<r<l*~08L)Z%RkCE46uRCXPSZ+4P-@OA zye_B7C)HYF$Ju+-UP6_Go_Ir?OP`XioD%x=!c~+znhs{K-{K0J%h;S!06y!IxlOOT zXo}8Guy&UdWyAiUsekMi^fy_+Dq*W&3FZ5=lOdbpaC}S~%?vt>=fIb2zFv#gD@^5# ze$5B`<;GM0)*kAcYDur|Db#+HEcLx)N#9`*ofF{;`$p){GtU&zZ0&QBb<YS^xK;^o z+%-Vo!(l|Xn+ALLR3Eawo@Xu7uA_RN$LKID5ob@hfYJLSVZ}8quGxM+oRsV!T}LL9 zdG!w~)|qR-C#`&L#qBG?!3J;KCj|Mx!oI^GB6KJ=oYfE(?`srzK6i!Ptpjo6yL|e% zWIT65Z!L*kl_7lF>IdqktAw3_D`}XD0nLzgfK-)MlKxNzw$Qcw48cMYS8a)b)3)%B z9U8E-uZ-t6{pI`^{*>_01ZyB-hvAz>B19VA1@$;v)O~e}d{@mChN$Yp;H6cdy)F%} zxt7q6Sp|^1K%7imn+mxh1EIZf6PY||6?JrUCW#t#!h=yZg3ymQh+6NR!r9&v@%EL= zbb!ZQvMfK58X0_~Z%^w&_$qB?ks^fAS2OrYcluSk)LvzgngifPhbJRpmary8lt|g* zh1a!`(8ZttR3a|X^o}e%U$TxsXCA&%7bou;dtu?U`8crj8@l;O^Dq4x1zHnl;`<jy zc%Wxym0#K<8gG^DvG?nATHmNjb7ov6v-ehVspq_?-{I-xwU-1c@}Eg?#1nGj(He*l z31w`^bF3%wp1KzmJlF5JfsUu=3p=EeXxA_#u^QEwnV|{PC>=E$c9LUf#&bJ1y(L~# zIr?SxcA|M?w;-%R07ZjtaM1(y(=)q$LEE~9Tbf~8RoW_v4&K#VzQhW0=e-TyKfIX) z2rd#`uhZn%{V<fcGk|m?dqYK?Ims2spkKNwj;!e=l*kp`x9B84ajZX^e71?7YZQb} zawp;KmnpRNu`1s)stnKe$j7?{nPfq^111P8;nvMWI5s)}BW;7xY5QxkWaw_}eW8#V zzcL|bE3&XRDF^Y<iV$bkO;8agL599u=b<WgicC=zM|G#`WU$N*a#hU(z8bwF>ykdu z@nP$@g--SOwK0bUP2Y|)+*3VuD)aE@#Aon+&O6dluD}+IKEr7!o`SheJ=mnBQhcrX z5;$PI5RX^~lg$C~K<4eC#Y6M(NM#bVFZ8GRSqYWix`m{vxD*czU|_k+4hEtn8Z0%y zPP1xm@ce0X@5=)6MMsl<r;kWka6gFbTTKeH18DEBi>ZuEFD`EPAbwl%Fg`ohfk}wB zu)q_A>`>4av=J<ad>uPJTF*z+^E<PIW@T>V%|gVT4^Vq)Bn0ku#<<)an3M6H4B9mh zuZ%Td8y))aLWMXS=T=4q9m8;cdtY3m`Hn<v7!N7?{ZUQr2)WmOlx{Ywr+qh{<Cd>? zqpH_+z)RhSGYV9s<ITg^MXjS~RGG?}WkdLYaz(WC&tk*!-PzU+8Fbg|418SuhMGOI zf!r+x*o|KS%^rKI=BbsDEe>DF){0$pvCJ`=Fr*NQoV4J|#f#_?mJicYyW=2jQ+Qqe zfQZl5q4Qf!;cc2Jw9!*kFv%9SZG1}4&pt>ioi~!|Gsfic9bMwFUQ^iW=Z86JSNNHi zZTRLn0o6koG8QdIvkuguui7+z+Q1w%S~D1*SLEX<|4A^R_Y0g<E<xXSO0p*BXxeLN zFLth*4lkj!2_4s~u&rBOL)Hp$>{T7c8R>4r;nA~6YI!xCcc+qOl^)=}%zsYQ$F!0a zb+#z?#tS|P3%Lnfdg9QrHpEX!lO#KKz|FWVe3#(6V3?dX)7xB!sqHbGd!-6r`NExV zDwvITw>x0voE;?Unj)Vt<vd6q?afcLIYg&zy^R4<I?(6&clZux$wkRwR3YOcl$3bW zH#NQaq%vtX?t&Y>9aKX5fA<hf@N*+AwQGpf_IBaOjy-hKlT5ND*amxWhUEH`rYg(R z<utN!G(3{MhVgH&qv7O!o=XxaOqg|&9lgH~h9AE}oqD__**U(LlJ$VJo!tcWC-Uj( zogc~Q5j(i?Y8$vIL*>9CcqcVBPvlCs$btM;DOz`{gm$^if%WTUXpip8gb%YTiN}-+ z#QgbnlCD`zPY;u)wtcdw$FiF=NlJ+fzqp0GditI^HLane4))>Q7b~y<QK6Uw6|8M? zHcKp94c|uz_*oHaXj}6#R9T--u392Smz7{b$1~a<dQG@>^;Mb;o>1v3B$+1%V{OrU znynWCF}E#A;2wR@jonOpIz_<2NoR!1B<-PoggA_FE3L|KtRrzF;^@4L4%#B<MyEM1 zr{#k+Nq}uKxfy$!)NHs1sn-VMo49!p-de*K`&F?x9TOI#qzWN!%P|K;2|;}yTzEr^ zJc<xPL!BpNdS{Y}{fCpaHc~J}(h{C7dnBxT5D&KZ4WMD@0G!aEMLvt`aqXcegeMY< zgz4voQGd5e!NSmSRMGUEuugL{$}1V7#Q;^T%CsR*XH{2uDc+<`R@;c!8g0gf%x2IU zz=wWJ#E6BZ>_NZTJoQv%--@#Fz-Jx4lc@1KpB%vE&Aq@McEHWT{z&5sfbTaI<iy)a z{fRqxxbP9Y9bJVfC0_8fON(q>xt6n=D@6~zwuFS2i^zh6quhP{@509U_lVBLEMbt_ zI6C*BDXL#TF1R~t0BO2B1RT~_;TC}oub#7k9huGX>r(7Ir`UDydiUOefA{;K_%?z) zRqr5TY3exQ<7z&yQr~mOn^W*?{af--$p>tA^y9TX@}T*c6l+qG<g2oiV0q3b6s{h^ zX9eCQDJ$9_`qM#DZl;c&ALA(L{f;<}cY-dJ3R>24FQ>Xi1v!n2L@sMMd8*R`M-Nwq zd&Y5W<lxcFwaAN&H2g%b`I@o|?<DvugC{f9OAhStRbA>eT#n_eE?`Ds%XlTN1c);+ zV%lA6A>cqZs5({S^-fC?XO;#U{T`rU_w7_7L>{g$ChXGsQ{Xp34D4kSXyZ+Hcs5TT zH;`)L{dhdJ3TYx=21N_CY;O|FhU3&^;!Aq$P6Iuq^^x@2rqB1y3S%EA=UHxGTP^i+ zAG!%sIi(p*a4+;cg?Zt`Cf%MH?^fhpZ>r&KosHB~MS%P7?k0PqC@%aq4D!svuw<S% z7Ou+y73)G07;A_LW{>Hzn;M`rW(|q|)JpVHqpO~;>>%s+c*E4a$}oPXJ{~MNNLNnM zAb0BD3eKPEEyO7=uw}&^&=9WUooeTLY9AHz++>x_8GlGabJwYW3d>+bdl1Oi_DAK4 zEa-i{mQGGsMbie?&`l>*NOIUMT6}9P7k)R0+LwK!-^0&B`a~_r-ZX;@kUC6LqRR!2 zg{#OIi|c|-=PuDD?e5U8YY*9aq?W#@wu6JU2dSF19QDg`L<6U0(ioJApSsOtZo}V! zr|m?~*W>$C8_!8&p)X&e&u(3IG@(Bn*}eznitnbOYevENXT@;yt1P}P?@i-9mVqyR zBEh}RQwyhoFx6sq#j*=+)IGTj%6n|XMCK3U_BYYI2V>A{M-0y0`<WbBr;gfV%}J#G z3wmSAr7HV_S#<r<E2PKA-Q2xt?<%z_o#+fX!dItlV#?DWuxsi=c?I1^Bx)V+!7}~% zyu|kyUf={b6FP*wUmAg4t2V9;EQaTMY*@>j+vM#Ubv9MJ9X_5{#?5&d7~DFCZ`A7v zCm!s9v2#sW=ocB>E8~XpLynW2CGWZW)6#_BN_&A}paRS(pNNh*_N3#992)x=fd2PS zG_tG&R*RiM^s=E<JsLeDQv`gV-*|Gvrjc%qxyBuJmjQL(SUO<(E>2!YlRx!f3yh>@ zU}XE5HV!;N@}hmgS??R&o$Lk|Uh2S&u5@zg^GYb$aD(f$ZMTri{l*z*IfMQTWjg#? zLgmMiQ_!YD2Xb%nQ1kvdow7s|^B?JO^;eQfrKSf2+t1`Ry02tg9|wcgt2Nc_KI=V= z`rc#`UYq$o+m&#EY&R(C@<)wZ8)2fFBnv(|3|>oZg~Y9$RZkMzNNDT;er)|0Y&_vW zPWqjI#z>^wzw9J+8w)@`Wk2e^9St69y#>}iT1d>eN@AAF(^KM}sCmo_pGNg0QQcd} zKnH7lS6Cw)FwKsT(NTO;m^E)6%b=iD$#dAgvHS$dVSJ|81b%n0IyI17fnP%+=-x6% zh(4PJ_4BslnbBcT{Czt<j~ztkCGQ1Si94i!e*={4*9^V%Rfxx(M09NaPBQjRqpHIV zx!L`yh}ifVYHH?Adiwj4;LGPpT0c=jf6@lQQ^~;~_8^gjK5e32w`@S~(S8h^JOsWR zC_$A5h3XPq2B%CEJx7%;V8bg%kw!Cpu-c@|UpuSBJC{mh!jrl5p~NP7X_cd(wq%9C zFf^5LGo9$naaO2XG#nS3uO(J(<G^id3w^yghHw|kN&BasByd*_chz_&oy=>)pq3|O z#qvI!Nr5bC8P$@!f?Q$sECoEj-k$DWXu`NIEB2+S7UaK`GE1Fp)lr2D>8q7pIO|;s zmg^Y7TopSsDw1NI$M&PUUKi<cVLA34uL_qGq97;V2^W3O;yl}CLiPF_9QA5Gv7=+4 zW3(s<{ctl`Q$CDNdoK;P_ug<-M|dck)l43RMM0<RF7i!fqww+j@tn9}3in9H5_fbp z(hr+wgYCrYuzRRA>ut5I`eSN;vRg;O^Myt=Y_hxqH_mIaPwHCy)SD{UD!&e&H6t36 z)gUwfEo#5qi_h3gI-Cyymm^si)i9mZUt15^>(^kxB4s@M=^=@3Yvqp3SjMePJ|tS$ zQ^Ct)b_yr$R|B8)K>D>~3-xkw!qOp)RL`ziaA0N(O*8GoPHddR^EGyiOGsfeLWfkR zXymeSi^j6KqFCr#R3Fw=kW)3?<UZaTl}MEbYhZxhTRPJ^5GR~ljq)#~*t_D3P%SqR zx36r+-JAQ7&#p@`yX7$kOWS~MKql4xa)a2-@T|K0Yy_9=l1ysq14#KI6Uc0?pcZ~6 zc;Mt4Ze!a-VVSBnk@CKb*`h__2;BnMvC`7B{(3ueRxaY5QJI-PR6_R$-N^xOZJbxH zKui2ip{(CWa(}uUX^Tn|c6QwH&^$W<Z!a4_w)M&<LkDPKa-TKyqOA)pe>+IH{ALXm z$jD&d3&mAL>N%}Dcv~1@zLol%Zzek)zaSy*FKJbL1GTE}APqN*=!r)K<bbakGvmhc z#}1moWPexBcMDFy#^oXWrtB!xNgDy4&dWirn=E`uQGxRfI{Yig(^R?kFfGISs+sB& z@wIs(Y;HS5k}eI!NaK}Q-Q@)%Ei>`d_Iup&QE7NDsE$e+Hgb)dnrUmDD)bIqB}`ta z1;cFya0BfJ!riBbiDJJ~f|5c_`0854y=E!IVqPxq8*+-j*uSFM%&CDNCR!YMk$s%* zi8SG#oJ!;MhEK=+(_e$mU|^p;j|yGQb@?HeGTHbLE&RM)6YI3+@x3RjqF6u!HP<kM zoC;(7G~Su7Z%|>@dE#<LqAbdvzQ=0+>$}7L@lpHbDK(B2T)of}rpa3g*UgZiWqn6; z)7r%#ah49f{^|h8CD+2;K2h}2C{@zxw2v4}N+*xgqoFWHpDtYzMjz~ZK`d97(lmP+ zzGv-Gp}9#YF`ILS3u^GI>^!VO?50N$n<;iQ+q<31x3A^2MjJEhN7|x|^uNIS(=^S` zT&DK^XhJ<}7Gc_w17uJ^CMQ?6iM&0b!BwBH!QnS$U`63U(K<vQ_WXo3k?s2(>pN0$ z$g*UZc6uduZIubpySEL8T~UKjITu>7bSET5oFXd{nrN5}&z&wh4X|e}cAu;SbHkLe zMn0VjZcc*B_x-3v>OPn|sR19<EhSItOUamcz|bi&IRBgxSG!u2?s!lOZklr-|9ekx zI}e~eez{1tJrQ=V7Zwf4ht7?P?C5Sy)*>AP4Vq;XKFhPsZ_a^JQ74^me1hg5HvyHf zySUwdE}rpgq;3(W>~&}!=8ro`BK*d}nwk<+JCz_L_M1r`x!qVEu@QA@-Uxd<t`<tZ z=n1^k5iWr(6xIw*qZZv{*@AF&<`Ytab-~7XE4K$uICu*CC<gLP@jI!^u}n~o9LS}w zX%ZAf)Z@a?Qdm1=I4m3`h6ZCElgP0z;jMu;bShrJtwaHzpT0u&4Ox%1qWu*WAJsAN zj6S$s-U?dXtk|YAqo|{AZ`RR{bmPrH;n;2yh{8pADBl-=c>%p)NZE0^t1}ipZgrp` z$~8p#t36IQdsOJNS_+LbCZocH9B!?LI!SCvhcO$Ja9(X8x<!XEg)zo(Qb!teZdPIV z+07yxVtj&_BaENp&W5j+phMb49Hr*MhrK>fR(Y6uG)VDY$1KUgB?IyFmAk04c0Y!m z>Y|eubfDym4KTeR9M`vwLFb$ca7wU;Tv=@djV`vtytoIOHuMZxqIQR#X}TepC@Bh= zRa{|7q&uE|G#(Tsb=jQ@k-&;~;;8xhU^wJHd6`#BZoPdh>~!yeTYCBMljW^ocjX>> zDqI{3MD{nX=E#DckgIGM>CFv<?%^W{`*t1uMJWKaxk|X9=_wtxm*Cw}OBRpegzs|4 zW2VW%9tHP>Z&Jg_^r6e>)U3m}blXWXaPe#~9%#o}-hZUF>YK4e-3`xuB48ToOS-i< zli_(M(0I-!7!@go>05<ha@rYIS`DB&8eUi;N<A*Mn}wTvTS-TbDVG=Zg+%nY4jH=! zQ8w^2oZQp~C)yrR&&PHc>2(oqty>SKPtq`b<t4JJQl9TLKgvy>vko<T9)tYwRd`l$ z8`K4vqq503n8}$k{hT1o*=+|Flg^SX19f(PK_)cGf1z=fhWtufCHOpA0187d;c~YL z#J&4<e0tjj&xO3F`vgW<UiVeFzNMHNF7&18s`WIpv0dmVeu69t9tyjHc9Q4qO|-x2 zWpXN65zjVS;R)|ZcKw(GWRBcJc0L}5^A2c}3_Cmi*{#Jmd)!X+tt}*9UlDjW(g3uv z4~%P)<)`1!rriVf(&clK;r2~s$jCc}6%XWb`L5BVZQcQ_bE>2}4$cv-w#|mUQfEQV z&5G$8ZlGg(7_lBUR$TtbB)Yq316jTB99i3cH%2}BKzihVq25YM;pFfJVIR>t@%Jf# zxcuZcE@;hobbL4nYrCcjodc!7_iHW;^q0k%)9i5EGk<PSeLBgyP)eEEb~yL?Iejhp zmR@iYLuZNasHstc{WPBA%lEzDxltM(iFpsvX2)P!@wbXy`+Cyp!vVi|^ulArid5@9 z#r@`UK>p@LT=J<B;&0?a^+!)$ZJIqMc#8I9#O+6o8berlH5*pE_$K_6b{^*qP^6V@ z8E}5}LU`Y~581+{a3}j232F>RKe3af?18c{+*%Rdnrsvf4EqEJbw%rzBY4P`-Aqjc z+8~56m{!;mbPV6q^!Yonr6ZZPTAhT^(<`eaL*K!MLJ4RoG!*#vAI8EJc9Gilb;1wc zp2AOC8$@YYOPo=;oUA|9uWC<^G4zH<F?nZ@T{$9Kn^Sknp#s-7Ve{ZAMBhq^8{u1A z6+A^%5ay{#S2>4rS87^>)2@fpeO}+mvwfk|xGjgQ9y*+O-#I5Jo_>Vvxh{d`0(IQ~ z!BTkhizMl;v4Dh5Q4&fI-$_dM_Y?F<FQU>mp~5@odkK&0T@!lubEsS%vsT#s@oS-6 zt_G2IDJRA*Q>j6>^W>S~F|tDL0!^JeyNVnLAhO$2X+gj_?)t!9!tC%8k{6<jr+ueU zKi5hwZl5Jy^t?&WGX<hDb32_Col3iWQ%Rhlg+{#85%zx)#MuQWS4ob0P462YB6efU zsTI3KbX(2HwB4IYMUyeDcq|IH%~xOsO{3d19O;2LbGR74jgBPhFsWV^?=CUoEZZ$G z?|L|x)@4@8sC052qCIzuCC_jsz00WKNflvT$7P|4n+;c2E`^OQl9(-HPaGLX?=H=x zn?>mbhXn^X|KyGIc~4W$_x=Ks5@K65scoom{LTwxj$8>{IBF|RKiELeiJ8zwXCH3m zD1)kH9yh4Z*hj=|u>pZ)&xlICD=rXI5IEf{rmX`<RLuz<&ka5mU1cPlOaol~h^JXB z_fqo=Idtq1*Kpbh=SnDmYgfA9`*K;La_GEJSumSyee}L6l++3(xA&p^;XQQN(nln6 zRWs*Gx6+2D0&=cVlB{T8g0-66iSmdu^xC7nRM0Prv^=>^D&A-yzA>f?W494mF>m;A zY6_{-w85qK+JqH(NtpEFJMq7@5+2$Ej!>!)cATu`_N+NfF5Ept#+T-D-{(+HsrU>x zU{5=#F5HKoENw{kDh2THS?ob4$O@td?4$$sEu{T2onWwQ9XWj92+?kl!8dk8L8j*t zqFYo<Y*w75N1~f6pQ`235tey0t*w9#&RouQ3tugqVJb&ndad9ROxKX)gKvn3XBIc+ z-gvTXjTG6VX;12Qtrz%I9}?p1xn$bZ#X{AC$uwB!5V>urfxTAMb4v$IqhAw`lB);S zaSwuD)4S)yY2wLZ?&O9@+Cq1e!sTMvXZ33`VB`#;hFv{98PiG<+P2Z1&+pTBvlmg# zGu>#N`FJwU?gLFfz!6o$5nS%PNHTDjp0My)3!U_0wTIG<NTMk31Kqf*!oh=bs9CZX z4YriVSg{1bohcv4!Kz}i#LIz{lnvmJM^aZM3EK~8fz0SpczR|Po$pme^@3$^jjkT| zLfs9HwC*D@-!f^?l|0hxS_wI9DcX-$s#eu;H-tV|F_fnGjiMtqe<WQe_t0Gd{ZXZG zAax3TOYW}~qbWT)NrTHg+AT_lllI<Bj()D8iciF8apNA6vqP3#e`8+NFzN{HZY++s z+%vf)mOE&B>_xg$5I`0VR^sUJ^K{7R1Kgg85*Vd<g6Lch<I>NF_LOqEbeFaa&c<#i zyRy9MUR)U$|M?@Qv^ZDTvuPtS)2k)3gsMb)>_TDUE=^(m#?f>_>R8UjAcb5GeoT+` z*vWM$KdRb#Igc#pI7`<AjwPP@J?U_1IdCy~#5p~)$Dq~b<nH2H&h<u+NBekBPEw@# z%O1{@f3#Itv*#E&lCYj`x^jUyHYJg=rI}=^_C&62)eLHV>^!OaCXSKwUkb-97)94? zX%gPZaHa9E3YMPpr0<KjlIv3K^u#bnl0HQs+<0lP&>~_qdUY<NjgLKHS>#CS&{jfb z@h1cd7BhwE5{fwLu`f(M=7cL|CexH|jb!fDm9%AJD}C3#pFUwxf*D$4K__4hwGpNB zmJD+RZjb_W^8TFlvR_}9`15JNZ>y@I=T2M|6c)ze+?_J?hT~4Iw#NhFloE#*@~>14 zo|cK`ItMw&WHTDwXb!m+Jl<HJAXF&!B)!x;X}XvdEsy>nR7?sXiP0acE=Q-6Qx?bR zv_oRTlPO{JqGKufH><Kg{uRuRe+ML5l?|1?Mx01F*I;?HD(^@D<P4uD3>(o(4eN{O znPKmRHDPB+xS9;Do_>>f-HeCnFU)vzt7NQ+>A?yDO{$}xC{}O!G>}C!UIHU~36>A# zSktf(<7TbIsGyBRYF;kB_TG#dPvbFprUkCEGZU@$NkdA4yeJc|3FB0x>DBL{@L~Qk zT5+%v`vq9Qj`$Z`NY-#F+%LwPEa`=ds%(ke0~KHq<)mZpE#dlC>Ugenjc};PRyqkU zgXO+W&?jsRfBR!MQoFud=o6bnT1M}I&2M_K*`2Ss$as0?9F&ZEs@^gC;}@~;NH1JA z;{fTsFPk|GIE!3a1fD<KohA0xV3D&E$(_3WpnJ=OJf%J1VOs+VD>A9to>t=4sLz)y za;66>XjMw{UT&GP6PNKSOVIyoKjF$XyoV|7BEBz0tIuD@@x*5WE55Li-)Au#^e*MH z2S!imyMBZ}9gvB`>>Z$dK&5adA48w)J%NH?3tXyqn94Mn!?;;OE_D4Wx@T6ou+qJ} zN~$PP*q&`oudO<WThoMOgO?Nk)$tp-ZZ8k&<0JX`@&>R`VLDlK+W;@G`5@RORAWyD zUJz0xCo=2OD-21yhw7^?p^?vJ_}*Q$`rFZ6?DMKzetFM5sFd&wKfZV7>z4H5kL2IQ z&YPm=^I|d|9Xy?P5p9VzJflSScw4edVrI<CED3F&4`*vyeBk7cT9Qz|9IRty_<(2Y zsjpWdIhoQM2Yyp#4-6}-?l~CYk|p!#nUWwV-8dea8lp&TloduwL}NkPDSD`)2~G^s zVfOI@`RG<8JxtX2Z#T5q9Qk>C`QtR4_|gV?p0#G>!`HEa8TQb%*$U%VrBT_MQA{a- zCjrtSxY5s+jeL0uU*&8hS7vmILSP}Td~OY67P|1`25ZwgF;!$CMUX9Xh$c+-LS;dB zT4Ux-XQsrHRpl};r=UZ)@0BI)cAdqHK1nl~uP2y}p+a@SM5u25pjB-<!jJiwPhzyr z6CG4MnMYB#>gGj${Pg5c5LNzJFsf^auxQad{?@7sxNON*a(QfTzE-O*Q=fVeCbXHL zkL4Q9s7xEYmaYJKEhBg~MSz(r`mqasCA7G(lK84hW0j#YbZqm1!ig*BhqVR5b5DdM z@nwZ*E59|rCrzFAn4rl@%4DhyX0C?7%5hBEJBhbgQ7Qz*^I*NSfO+jUW`U~>Sy-Yr z!zOXjv{)QZFW-bA)6H?gvL?J4VS=~kCjgU9N3XGKVBmx^__TNxDYY?#T>n<OcTWqg zZdwW^ueJ*J+V`OTvnS#C00*LZ=q~xVX`@iOC04}gC_eL6I5g%-^2=Ut#OIBH)%KEW z`A)kN&|Bsk-QMjo+@7L{wzfU^Fc`|t4)w<6zAC(+^emWcPs94{H>s@V5^~^*NOOIR zF(*v4!ZoKApYAAw6Mc=L{QU}kQkXu;o^1rOJC@+Ji|5HeFIzY=Xd4Y19xA+A<<G91 z4Wi$j?$eNlbY7DclelvhykSrV>_4}}Q#<GYb6r0do;z-%?~k{`_(>p275GBg#~M<0 z_66=t*oI33K7f2$ER4(@3GX68A=+>{Zs{<_53(`v{NoY9V$F8gU+2v(DSQ>)e4>W2 zenIrv;yWZ<;VSpA%Z7|qtf2KlXUWGqL+P+9_h{?tS7g?;Opi_(2O8Cs#8R^(@K$~z z_qZSpl1E#!{JyJr_el>~`K!JJC-=s!Cw=*i=hK*;j}brR{&n!tpMrhFLNG8g2RiBx z3t|o)qqlW4$)i!-uw&j(YX9gM^yuY*9YQ&%xn+lOkH^F9<d~`#jo0Ae&8KAKtU&H` ztO_+RS4MZ!7sPj(H_U#!iyjUf1dU@>v5@F5{PeR*II%f|j!vn7ruTjLhmCP8@Ocsc zHhwppcvefFy;EjOW;V0dpwaAX$s3`(@he#1kWA-{84Y3mwvoJ~Ok8a@h_`>5h}N?w zu$u#o*{m6FaLdO9%x~Qzq2kSMWOAzvtBq14=HtY0Nkk_dJ6jzmjrHJ1c%}%WPdy_$ z-^_sRl~#O3bPZ@~THuDtbY`x8hiQqsvFp}IbVOT}W}a85^nUOG_473Nuc9rc#dmLE zX_*Fyg{tz$Gg8Q@H@?j2xE<o_TChFdhe>H|!Gu1YIPsY=e+*(t$tFj5woL(ES&G4q zb45hn>K%<>&16nCM<!Uz6P`S#h2iV%pm4_taw|y|wc|tJwxkDiS3QN&OWLas8asIG z&zAJeJSW4uJzNDzBQK!aw-A2i`@P`hox~1(mciu;)x<t}D{bq!4(~ksOl$5*;E=Pj zY_!Kwp&^OnVh0Q-4GB4zwZ9zNJkJp6(gS2u9@6(pGJMnO#U#6JrBL?idf~^yvCvyx zo!9Q&6L);>hV$)~k&8BdtZd&$lvpzbm!9m&tAFbv>am4<QgLtQeL0``Xx>2ed3LOc zTg|9^CO&GsPQDc>^1d@u;LuVd-g#h#Xsc##TpZ!SOUt*?{obeOoq_^fI=efWS{F`t zZPP~wX%`y&;wUY(F~cE!&R6OvEGNkiZjqYsFk(KwFOf*UNSC?xr`nKD(ihzn<tNqn zS>q+x!0xG@(@s|KiAhU56)P%HZ1MnJmkROB&Lfa@JcJI9Y9>p9PvF=mF4+6ke0oX2 z6lPA7gvo1P5XYSs7%4v<vI5IF*XPAl)o%y6|Fw)BaTvgi`m~al`;=h8)hEQlQXGRn zsepg1XxGh0BWxNym7g`@E^RqCjJT9_hk}YYY!e&B8eHy@JOjqsu3uzj21ZQIeG9ry zabeYY(tHI!7JVcAS@hdC{A-0^-Z!`(ZOl&LJ{(xcSEMxyw+)rS?A<GQn>J-Ov@I9j z-!{a1E!!a1>>Sm&o<qjeN038V$LV$bJuqu+9#!8U+Ld7wNTw#A7S6vppPtxcMrS<r z#@Rzm@O+P4VVs-|I>$(|>$|?N$bQfHhL;-bb5s?}H}B1i&8qO-+;(U_^;LLe_fQyd zI1b%UkL4S7_k^i^htn*lN^&c^57b7kg<;AXxWuy$Tszu|d$LUUF%P66o%d$FRrEos zwv|4#4PkhB5g4n8rfMl`$hwo;K_=rqoX|@qQv1WWqwh0#<xOsEeRc%zY#WAt@5=f3 zSYtN+`Z+GFD~0V)T*|+_7Rppt_|TRLJ9f!$J8L-a&d8Ke_;%8IE_dBijItbv5~>B{ z^Fsr?eftfp5o<$@+gjCn4+7Zr4O(~)uTVvQ2SN3o<yALc<>K7(>q1u>ad@rkNKa1h zjqzHRykXlgYBl09tqIJ+d%6=LYIr8j>0bfisezt@*WM$&T>G&&v;M5Ur+~+e14#ek zzNk32nr-#Tz&FzOh5or$X*Uu{g*_h%lG67IwayL0=AhGf;K~hrt7eSL3diB&ZhE+` zWdSkVy^giTS5dzDFqw1eA=+D9ghyG5FtS@KU9w;k1kT*ZW|j!JjRviPkIGUk#Vk{l znk#_jnN>s%a`;U%ucGk+Sw2QZmXv={;@xgE;pI_kI3YU~q|~BW=@vk6-$u$8gki>M zW9*Jy#3*lT#rgzow##58*w|;&j<{{O->R!>;+PjOw!9OAUOR$q(jJo33vqySUtm2} zkyNdE^6=seR9@u(@5Va{Z@sOcVp~%{E>(>ckd0*ghK&SO_f<c7w}l^8-isxU9>`iN zQhAjjQ^DCOoDTV%h3y)p^i|&1s)y0iY*5Tp()ZF?+T?Yf%3fTF*LIx1(Xm5uz@m{f z-Dn*Y%!@+P<C$FP>>hmgey=z^OKUJJE@FrJ^IZ9dATXNJ12zON7gp@_$7ApPV6JB= zJ(O;b^E-4P%O{Li*f$wWK1h4snSYp+Ja?%s^3(%U>s9<`Ce4G33QHK0&Ko{Gz~<kx zWd32Ya5erEQ5luO&a+KWDcUVLXkR}TYhuDT*VRGM%t#z=9R_2R_KQ-^rF5`#rEt!8 zHO^_qYYYxehuk=2(W_s5@ayCx&^r8rjx+4eU3}v~FOD8Tw&#@Nw>3jqN~|L`B&t;_ z<a>)=o6^8f<`o#H7)W%ce!?XyX7HLreVEaqXE<q_2K#i@9TWZTV8+36l9e+YlcmGq z=-D?^VNES2+ho(7BT{j3;}ZV$)j6PdPKBKdyUo?)7jU<ph*sImHi5Un0gAz*mpn3~ z;Dne53-CNi#p2J9>AN~`&P-Vrsoae%&MyUf$zGn;4^A<s?2j-tX$7VR`LNZ89^#VF zS@5=^ADfVLgnux4HO;;EfqE$iLBPl`K5|_j_G$i0%ICZCbL98qfa$kTxlamAJd{q< zE{+8&lT)aq+CV#Ml;Dbm0~@&`6{^ebaR(YV(99Gqcv*9-%FNybolQKc?1BtP&wm3i z`8)8<SAdbqs_gTbE*KIb$#012#w+ZR;|(+8*f0}$o?HHo2Rmgl>ZmR6IbkHp4_F9> z6>(&|q7M018;)OgX|inwGr{`Z3^;mCoy|Njm+#!F&8K;VV%O(wG%+Q}qhI0!GV_KE zUU=sO3p`&}oqcKxvj=41msxk{7_&8`_tVwbw0a9Jn(4@2)|cc}3)W!Pg+uJgyAXCA zvRTBK)4cnreHbjem2GLWVI!m0vTBhg-?nYU*Q@ey?;%IrEVB&8E?WmEdL?Z^cTL#y z`5T_=@WYYb6hjLX;C9#yJ~`4$II&0t{Vp%%ofKL)Nv}6_<fF&*+!uW)SsRa<K}YGv z@5;gr@fV4O^<z3fn9O_5?19{;vy`0Dsjdk*gT*;we2M*faGFH8*tIKYYv>ksC+jLZ za_241DBcg@B}TA!?HxuR<l$>G2{`^pg(WYO<Z=wA<H2b>jJc+YACgyK;=V1cd$BBb zOg~FPL^}a)%9}v%f#zJHVn3>Sc@s=FPUMXZOsFzwVupq?EDE<`4&zezGjlJ|b$-&G z3tw7SKkZ}0zV-2Em(RR|xKG1q9B0q+d`ek!crH8PSH`}q9?QB_y~57}??RkdFD&RD z%F130V`E24FbgL~Y|Ougf(%irE%7iewAjmrM2#g;EsC&dzyvl?UDScS)5H5JsiY~X z1qSZ0;74w4r2gMjASkg1pK&~iPid`ZD-_3JrDZryx|E8A9}bFM0rTKz>91#&RG*!2 z?hRpn8lLfI6g@pM-|!Q@dhz~y)cFzLocMJPQTTeJDyd05LFSH2f`%XoK6#Ru=ghBT z_=7{rK}GT;CcOGe7cIEVd3(FCC*8zIh^hd%p7)7${Ap4Ae2r|pvY0%*Tu072pMi%> zTQKhHh5tj>oB!n$ef|FpXcm=<MyW_NXi!~e?~^h#kS0_JnPtpO(VWtpO43Y&q*6-P zb@tjNL?SYzh$a~$q0GM5{rx=d$9@0szW;;sIKw_`t=H@ITyR-cgDSYJg6$qN>A8=} zTu|r<IP)<Ns^aER`=xrQgg$_IXf65m^(r>go5~9v+Cx;GKY-b<skH6-W&B?05!57J zfyM`8sja*UJAGv|o+YLQ|526wpE<~XLG+vs`B|Ps+jfd!ZIfgs<6s8aS8R)`^rhfK z>JH}Vh0kPavMo6J1aq|+f008<H?iH)#3_1NLeH5ZVi&XkU6C{--a9gJ+4vT+WJMw7 z=Tda?PMdJN6TqlQPs54#t+}HX`xq;}7`pXDf<17t7$?71g#Sa8{qM{BU#e{0?C*S$ zjGH#-rU(DmSS`NNV{e|}*~K=0=2r`rHOBECeGInt&z)eCJy2uwPhLe}kgLXjkBbFQ zW*)G)dDoY>aGQvAMP?d5;AZR9K=jroGiVxb&K}lg%jb{0$+>aXKX;rETu!jD8JVld zbKVlmOF0y4V-$6spL=_`;8;<W&2#0q0+EyI0vYuN{?0mOo|COE7nUH;L|vZ56R9z@ z`gD=c>-j4ukoXtIdzLK2*<aw>^d%qUm3Y|N+}13$e%eyNJGxr@YVL|-ybG<FyuR7P zJQ-erP3Lb(&Nsi3|C%io)Fo%~2I9SKxM)9KY`UUNY2qZ}xlj}|E4sL?FHC?mWT4E& za(HdoMA)Aki=MgXa;1NcF@lvM_}}ywXtzlO`Esy@OWl-1PTgI^1q67YQmqT<mvRru z4{0YC4rZW>t;wM9+K>R6ZqrqMh!b!dz;==(TsMvuTp1&Z?NS^-JE8{NcykxU2^HyM zwOVkADIv30oFV_p<mjxFOAy-s9amP@qGN(_D5!Q7)!BQT9skLgh)FboNsuNxxFU+2 zHZ%wE+^<+uIP$U96Js1A&cON<mWr*;go-`oI7lIldsUQ1Wd0=Jh@sO&S;U6@^jMPp z8}J5NigythUk1m&p1^$L8*qIjl3AK8P3Ib(gSXpk@QWG;s3vkSdORJ!kqxGkf1U;H zO^fl1W)Cd$^dIc}IDy^PY{|M>oagh!!mwD(Hd>_j1_@1n=&+wNZJRxX^~)Lo(<m`q zw#Ew2-#CkwJqyNl8!G4u#Yb4JJObm*SLlk@cVSe*40P{JAR*hw)AKr|C~xvwhz%%3 zH(qGt7xs}f&2%SrQ3|6btMg&;;z@YXF;iN)Nr`HHQAUA(LhxDZ5O#M$6J%_806)Ao zVnKi|n_?(Rn|%_&Th@y_Y6xdmEUm$^E(oj^=R@-=AH00REhrY2_XZJj*|MycNX)*L zglg>osi8gWh8}{iHPph{9ct{&2XRFIrW|bEQ;C1>6l2*`0n9*)VMNZ1dao6lF;&!Y zxPJ$6(|b-XGvRoa$ziVFREr%Qvlzc@m!b<CFEU^J!=Qgc0oXkd1|*h^V>c%b!Ou1N z_)}UUmT370Dv-@yyw-vt%K|nR`+#YAH=~&N8OI1d!x!z_xOwXweCfbinu=7|%;c4{ z)2|c^rRC_5crtBwE5Ta(r0Lf=A^7%rAx)w@7fko8U}Z0Avdb>Tu~ilGSikdU@T@uS zxZxk3pkkSXC7(MWtBvE>zxFfPI~V-1go`%TFpPt%k4I2Xwgg`6wgS>_<>KM_Ze&r^ z5!e~qi)Y+$!pm}3K;hEmY=vk%Yy74I@2HZexyPQdPG#P_xyjl5{nyqBd^B76rQzv< z<vm)w;h)Vm>anH*opeur{KC+>>hw&T)%&z~iH<k;URM_Jeono|f6!Ya@U`1)WtN9| z!=JTywL8xU200U+(&1>Ep0U#0IjdHiTZ_AS8Oy)(wRJ>m)-5U(<hCXVUMZOIo~ah{ zCW}uN-1EG^zgc#L@82&ah!GRzHBM>ZWzYh_x;-|6b?Yx%dHWXdBPxw~zwk-{&Xr?U zFH#kZZ<OX{8m*{PIk}!!FYG@HAHNgSo=N1%kr{$jYA#%uw;%6rf<3eA<9+_<r%Swn z)!JP39Zs-xvn}uK+Ofze)__dvo&^k>K+N6uLEN!-=zPK~VQRktsr@*J5_>(-l*_&7 z+x1dpcGVsQH%LPQuHqdER)(ZinxuT(Mbxou86NSvO=L2^p{GAZape1juuyQ7bI^5T z_TSfm%5*7+pD6<O(-KMJv~(oZse*M5`e1&KA?Ce0h@2hn6T5#mh}Se9BCjKf^S{P$ znX=kYp>2+n*G0gl_}3_Dn@|bQY!Z&w*I@7aDagN34ij^6ZnEPflrZZe$McBC->;Qo zy<t@}q@V)-Vj8%OH!!RkD{No1B&pGaBe+m=6w%-*_;6eeX+Kj=ir3l^t348^^4Aj* zq^v;aPn-<DnQ^$d@EN*T_7@jFa>f;fNu(zvni$D+p&}m)8xp=Uop)a%DscyQswFUW zS{h(s-GZiXn#u*_q%vM28c<7~ktM4`$Y;}uAn9ob-bYnX<y0PU8!Clmc{-k}YlH7? zG-D>5X~4O^rLg-z2wt`EFxT_zKDjnN0&*(HK-yPD_>-lG6hyDXhBuX@ZR#TAud4`q zKdz$QKAQB+_RaY2nMuU`@@cZB(T6Na&cPZhML_1%2v(gE4u;eF(0Ac;GcWqi?VWoJ zaQ85?%1Df9PHM1O^_eA6cViivS}ypSQcM))ck#CzMuKtcEpR~j51y%&&>54YgsNPl z(3}hzMzc$TbZ);P&^fgjcgaMOg<<Cyx>MK|9MvaH{3tZq(ZKxrx|0c!@8ln{Yb5*T zogi-$6VZ~ZhnV0;*O~q$4umWiBr6ubWa18GkYtfOv|{!R&gkrX68zqf)Ni{$Y`<lY zM{=u)l$R2+o)UygV}${wvnKe1#u=pF{gO0Xy3I9N+!MspK~DU#Bj<a`8BJ?3B!`aN z;K}y4GWRM@66HtN@X|gWtg_R`dvBfPYmbda(Rm0f_Un?H1)W&5O$0j6dtndvZOA$K zI6?s#Op0|Knxk(_;)Uwu{L>~d5*bL;1-l^Fq>^mk>x7@}@kA2;q>*6$TjuQWS)}6C zLAJT<MlMH|l9emxlVj;uQI=Z?DoOzIch(7FeW9B9q)K^qgL>R5ZB;Z?aF!TV8*`bD z-jGLGrJRgQC4Ynw!@kDl{Ez~57+%mN2ykjb=hnX$3?0uB#M#(#fAuHe^!1bzAE{tm z%kDEb{xosecRD(p=g2#}Yb+C$(`mD@cM^HM*BS@y>*e%bg`!5sMd;e`f9T5;9W=Ob z9DLRo!$q{6WDaB>Cb23~+*X4g#%IPL33N*#+rlQ{ypay#J4Fq3cJ=e>t~a2v#9ZFy zID2$eGl6NUTZ$?!Oh=j1uke@p#}bduYGQWWi=6S!<y>RTID>R$qHFS;DbTba0bMTS zX7pBw9L*;wZFaaIvJo?RI|RG$reK97xt!k}A@O8jHkMF)jtqOuz-h?}{Iz-lP`f?M zaVJffcmF=~c~%TbU-OXD<|z65q=d}Z@nhQ6mGCj)Xmea~82Y<Q8L9tR&BaX1N7=c} z$h*;(=vf7jldsB9TILMy^yy^g%TYgM!8^g-ILN@pf1Y@sm=dJy@+3+#T)>(?4xY1* zm_-@gh-%-!rB|)c(KE++v5Y9!QDg{6@gutT;S_)8_1#$1IGzMe^dzeHU!f57X>_a0 zSF}NYJeZV!N5;o}8T07v_}BI#QdAPbO_~`FN+KJe$?+2WnyQ13t_(-kzdxXHS@*Gc zhCKY{BFUh+FG(<WgC&DH_;k&5+?FOBfBSEMY40=8(#HX4;qV?bqH`8jcWUCX!}hT4 z%@#aaR2%Ji)B(u_@i=!;280Y{f%N?rP^%VWe~T<c_l^|e()Za&;ow}>)lU*?dD)Ek z=gC;DrU!at`;oJ688Q4EigY)eKobv6Cyke);8TMb>pO=*+2gXg$rkP8QD_o*t2dpw zx#%WX|Lj4p17_Il5z-~HtgYGqsLK9No&Eo}0c*?tToatEJivT<eIJfrcn%Ssrd*MK z8S~*-Dvaq!Aa52I;BVbN41!2r+3j1fN1+KVwVsN-9Zr#im*2THlf;SI`Yi6w@Il6K z_$qhdQ4Z5}yq@rPdtjR@)Bi73_P=*Z{4Z5D^G^b@|Ax`X*)Oo5MI3*Ayb@(6^a!5s zv_h+-eDN2Rg*c$Z7MC}>F@p5P*lpcI@DKE54^C`iAAa1(zIx(B?aklQ_|OS-;9ox5 z`(YfLvD$_9bUz~fWEV}EuoCLMDcU>fDgJh+NSFsxAf?+yVb+QicryPSj4pgbUc8<| zr@7>y>`TdT@0KHUzMTuHF9H~rXadK`2(u+B<fDTU`cZ*k*iVLxdD%v88Epj7p|McJ z#zD8N1g<zRjpn(;(-S*BunF2XX^&e3erE6mc>hk)C&U?>PTGnigU^9mpEP|fJC&7c zFr^}!eCXb;&yZQFkIlwL(cu$9^*HPvNX<S<Z%JoB-31fWSL=)4ZrzU0%yoh)Q+M<* zO$O`NkHb&<i@Aw?S!7C~4-TEXiE&=8hP2}l8TeTZ6^=6Wwm|~??R2vp$PA+j-s&{w zMIPPi`i_Q5Y@nasn&Fji;^>^HP&VM}7<MC;!L99G$ZEC<%XW#-4QdTg6Fm=CgvwE8 zp*f(XU<=#(z#QiGG-352MclgN6ntI%16P!bV;jFB(nu$fY+VL@y?L5czv>_fQ5TTx zfG3j`qK=n^6~S-V%`WNKLY*8lZRI<*g3s78x>QS=YPBV@^|f-;!ng%*+_@P3sJc+q z>P<AiYaIO{SAtF_4-)I^7=lM$k|USI=_joZIPM`PIVOK0FFGF7!e_9_JI8{|8&f#_ zKnGt=JxufrgocaEHe}JK3$}f+<T09#7k#0`V4xKJ>Bt~9X2I;)ql(D)>2Wf1+>&~~ zhFJJkJ-6Px(;F4EjbVGtPU2<mo9U#O!`MM86tcEmL=hjJL)GSu_}OSNvhxZizoRE0 zc5e#N#p7Ak-gs<teFGjpIuVvVX2G=e0otMh;AGqhP5G1XpKG)5qU<r~<xL}4B<_ih ziOS%gyT-FFcB|<lH;ft$YRRTe(%|8fM;yL<f`6Tr>>cJLJ7vrpZhUwQ+BxKe|2$fb zUrnl@S7(TTXy;WtEH?zABAcOSb1AO!+K)4i<%5=cKGGKnBGJ0%!S{Y79=WRnD$6fp zXDor&>@8tC&;J0ynlpmr>P2|_jRYv4mJi~>lG|yv4)(pdmA$Jzm(~2)MV88DW1XrX zS~+|II+pp+!%c~7O3O{`uOz^u$p!TJs;{88su$g`Yr?5(_p>J9o7u4HIjr*dB{=*@ zAetn9h0L;OgthyH(6_5bY{1Ael>8%uy;fwx4*IRe%4G#4DfKeeEh+_eZxX3g)&iHt z2`K7aBp0+(SYDP$VeO&`c*)pCj@UV)x${F=hxN(qzEzgksbVc9n^kCvfeZU#Y7E<T z{4{=?{1{)B$DBMCunSr$>9t=OP?)uZ@mM|wvc;9?I<69Li#CPjK1K9&TLXT3T8uT# z2!)_0RRs0Bla#-0oS;_tPV7C(8qf$dV;~5L9qUBhkIKm`**#=hd@Wi~R*Ky`;vnqF zKVs~ZL1(p_qdjr1M5V>u_VYRi+@UU7zvqiQJ?GVji=tP8@`4z|_!-yNp;R_ztP`Gj zE{4_$&zdWTMDYBcad?x>MQria7+m8o5!F67?nKdAe5J{iaxz<KVSzbB?_7YLV_le8 z13%G^RoTS;_fH0Yo6GHZ*3CcExtL5_?9HkAh(a7vL>IR$058ujIB?NZSRL2bmvufS z{SLwGvZ>cmhf)E)^KuI9ZY;tvi|vqKi98dVUPG+6zs6thsbe?ESQwvpfVlnDBq#1= zk%!+FF|wNRFu83yOrG-v%T4=-g&|<%y1<T>d72^C>>HAh`GoxLg=3Z3$AsVFMN~R> z5DoWlp%we1q4~xj$y*+QZ?%4bxkplw#(P<6;WLGvH2*J^zy;q98VghE_1XJ1*)}Q_ zm`$ED%+B%>x@=D6aNY@>u>Hd*iaFzmrsw_Q40BS+$ZspE^*{;7RtO<JeKVn=XNV}y zvn07ad34VDF8EZaiFB?_hN6)ZxcG1;4sE)PWL)K7=c@s1W8H+j?Fq6jn87Z2cm<}} zC!%cKa(1?~2-xjyKrc=C_*_E=IIZ6S6Sb$YUv%EGTT&C)_jfNcl1naf|LV>Wu4O-) zJCMt|KDB2p)|cT0n<Y?`_G{cZ+X|YeY~*g2b%J(JAo7_XhHMJQ<0RGXc-M&s#J*R9 zyX8$e!M8hP`3_wW4EB=gQ`F(XetCS=xfbqd#KEoa#;`@fr~YRC4|YwLJaE48$obCz z^k`_%^E0EV-sn~c*?*e6c;bkx#<rm0!1b_ey+5G1G0?DXB{F}sgbZ|^fy=QmaIrrd z%*2yG!~G{|KA?&hoQtIuS1CAcnhCob<)C+(CJxTa!;eniBZoYqYOl`{C9Y;6xV791 z9pSZudz=>QUOs~qg}AcSMU(Ie?Odu=tc_kgPC^QfrD(!CaTaac4ht-$>G9hw=#8;8 zG)KhY_e+1kk>Voq@5F6Prp=*SDz3o2>8D`7VJP18+JarCyB<awdyv|RD)vp21KaRR zji^{yqMZ#(P`psxUwk-Jm>o96&=^d|3{^png&o`rF^7(i7a)7w5c&{UhqLR))w@nl zW{>BehKY-PIIaEitjpd4yr3u+c25WPc2NS;U%ic1Zjz-ZuAc_~^UBPk*8^B*^gS7~ z$&}6Admer3J%PVZd<MQakXNHPjSG_-!eqx()@E%f!7Eo2=bSXqpP>oYpD%^sxo$)) zQx5m66OL`to}mG29qOzoNqOCFct%7Som^H(WrI)hudLuvyQdoTnf^7rzMSADW{{>| z8Dy75m6CARLSj(A7;4TXv;Bc0^uo+N;98a|RBZL3bG-$P-jISlLVjR()l0;xFo5ZJ zHkXbhi{e`4Gu)-M*0|43$Ov=&#cbGol6)9FPd>?H;uvNS2L7z&*@u}>a|a3b&C@{e zTi?uHdwv#IZ%bnHG}Ng4Yg^nrHVFTV$$?>I71}j$iT>SDf>x!Ap%Jo(xU30>r>W}n zo5DrB`C$ZHJ0Za=tI3A6>2KkgYZc_0#sP1v6iIoJhyxZH;jx1c$xpW<MEAxaSR%~Z zHp}OrfmN00{;U&F6WatgC!HkYf9tSv@hjOLnd8t??`+%UAIu&F9vgajE<4LNl)c#b z4>sy$z!d{&w(Iv}>Rnj~lMgL|dEK|sAK$gq<wFfl-nbFZ=#8Tz>q_wR$Xw=v*?VyH zp9sQZ1TA<!4vT%bjCrQd1o>xAkrJgSrm^z}SGQ+7rZ!iQU;0k;bb>zBZ?R=o6ibt= z$7zhC^%Cm+P=;CxGFj18ne5TpDmrFMHklgRMaA0>)6ddZArt$tY4tJetN9`9?Iruz z=+;Hxh|gl5v$I(ySXKyuy1@kr&(gw>wS>z`WL*aC;3-DkxM>fII#QybL^%gv-`mA3 zO$_5^cZi^%Z8>CQycD$GIl@)_^@R2k4UBhq!x*7cYT3mc9H4WByPwc4bOP{Ea%?yI z@#gq?kN5u|LOcqsSJ_4!19Y(UO?jGiLWEBGIEaonUBbO91EFp4R{YL91o(F`q)Ph1 zkk?`qHcN~idA1Apf37D2|7$4r`%=6{_7h%EKgwCQ&B59B<3VllPN+Y2iil=4qwVL^ z;Y(gRr1@o&A+!~ac1~r#UrNTU`z7mhts;T>-9t~h*}(A+_4q>A8633cHLAP#2HTvM z1zfs>O2sE(2|5QaRj{Nv+g~zBS(^Q1be6PTt-xe>CyXZfQ8jXm46PcE3^q&PR~yff zU+VjqS)SSezpQXf)?Kb-nH|!)BMp@o*5VlceWoK$7DU%-fXnA~)a6MyjTl3)%lZTL zZKY!MmnY9=qs-RP(Mgm(qJh8_xUk!0Oz81%1-N|eKU75iSl`s$50%l1?Bk=Q@TF=j zV}AV;3i^}<%I^Lsc3~V|`sE(Xe<+W87c}w`L!{9B{a?^R-A`mkyad)c-B0#qy+Cf( zbAhjIL`271!HWIyoKfKq#J$i%5AR#hnP>w|g!^oa>b&~ih36@gKZb37BLpo;{sXDz zO;kVrA-1zLViT5zV~aT-@!O77Ah-1&Ndp9bD&L^E<ni!g{|93AJ0H3BeIc7|*9)?0 zmEp&+nb7!j3qCr13j5pT2hq=zh3p4~yaV_)qxkd_(Jp?Aw*F$VXw^K>UHKll98QEd zsRH(bOg-(LwiLqVHqfe-6KtIy9|HYblr?$K2xLkds(fEYD;5^eO;eWABwu6t^3D$Y zTT-p==+l+pP&<r_^7oRR!V$No^9<rXph^Qz4#0W;dQy7y7dcpINMpKg!P=vmuuf%= z;O{R;`k)&*ZZ3+gBbBh~YdJQ;@*&>mtc6#-)ug*){AqQ^J&;>aSHEU3lLjmt!!*t| zr0!9U^!bmkc=ca%EcK|GGGUUGt(ea`3I7-K1190rj+rbvHHKDo@Py8u6RhRz7MMNg z2<defvC0@lIJxOD>CQJ|zJ2mT2I(^N(Yt>F^^Xq`U)@;fz(Zu{`xXecN@W&>IpTpW zrm*`|1qw{+1g<I@R8t+P?V}U*+h3)yub$tcQb!ijiHA!`%${U4QRrOoUZzbuwbVc+ zLYnn&yuzqEt;R(L+33yrDBN*lHbj~%XN6)VT>54X!C$sw{dt<$>|q9y+EvE<8@WSH zf7hnn;?uBR&TrCJ7*EzTpAv%UYSGf2kJ0Q2stnqYiQkET<jU$Nu?Lji!8EmH^sbyf zZF9|M2hw`kwL1-2yX|gN=J8?rcalF#J=eg6Gh6Y!vIrXA`X1ZwSx4KaUu8ArJ*a+G zB3!UCV!i*~!E%Rtpre|=q4SxTW>sRBnF3~ERTYS+WzoNvKQk#VHu!DPFJ{t^9g~&p zidw9XptAZn+^RZ{omXPU2~O-K?Pq22mQE}9bKKCj`t?idBKH@&eko@*m$cx`Io)JY zNCDBaGotbh!8lHC7{6+2BPOTUfyHY(n(m>;zU3K0{A4NmV&xc?IVy%9UOa;r-cX=D zbxz1wtDBezM=c>iRbZwf2|V3iB2{>SG<n@8GYxu4Q}<<*xlfKQ9_29aj0U7kQDDiD z18nuD3uL}VFFT;UmzK>^VWVHGvyb~d+2wARu%v7vCi_2QlM|x#{74p(Wbc8(%4?`j zBMF*j<-&$}T8v)<Lyw%jf(yGMaDqM`x{9AdD`yXmo8tr>(&w3iZ2~4Ksh9K}?&FmA zx{!go$wH7~CvnfdiGB&$H(sCe;Mt@FkoEHj4l9trw-i5fE=@^n)pcI|oFr9<3rQ#E zg`OC$`6YC}a7T+iPoUQhZ3m|vTk!c}H`2ABfXI%@!~QsD-t7~?$d((2z0@y5+OjMd zHCM+PoqR}lHN^i>mHnSN$p2GiQ!mW{i3j0m%V4`;p3pMA{Ms$l>zB^_df5oLBLEg| z*^G>JJ}||uLD)=KJS?g>g?=2niG3_gNzBMAZnv!xp7QO9!1uls?mpv&_dDD}U$Z`O zt9J^UKyZVv3dXpzOcyr9pG3d7cnDv?M~@?fWYqthmHqF_`(LVT?XGc9c$|-4K4ih6 z^t|Bnyu~=6)&S2}Q)ItgxeT&a671vgO8nT+R=lTl1g!g=AZe46@OS!?`_vsSNI2-q zrokz=$~%ko-4C;BcXFU;Sw3Fkxt2y$Ng#W<b@1`+8)lnE3X;!lMH(WDK;?NVoKMch zew&l<2ICO~ep2vt-9A<<RSY(sO`*4?cd|QY?!wkvjd&Jq6?AgtFy#3vu*YwN;(%+1 z@u0NOn4haf_eEyF3-ea)$67vO8oQWJ^L}BGa($LYB|&Se1N*spEc<SaI?f9yLM4ai z(*5dvP;1;lR9oY)VXQNq7bwHpz0$JTHSim<reN5=U7X$P90fz$o!DDa>#+`xk8AE^ z<AsV&cspN`-d0;qLpJ&|&n-jQ+@HJIDR1>b`#p!gsVtzCjptxwTOZEe{|9@h_~Lyh zUD;3CtEr!SFihJ*&_Q!w!MXfDHmUEg+8p0n%71UMohKh}CU~*bQZUK?*xVCg#{5NZ zwXA~WV)?(fPZj)GY9NR^mSAI=EW>Lkoy0S9e8fAKmcu_l90j4)5rRYOX`PHv>5l$; zr>>ySo*&dH!oUB0B7cH?zs*y-7@o?h7QwLPFRKURqIiXug9YflmSEw{$ht$#mjn%$ zY<RknwmjR8V!qq2?Y!MvWq9hp1pIf;n|Z9qbKX1c?>37T=n5W42MeaPEaj$3mDzNq zUgSNnn!(^#cGd}fvHZeQiURR#R@N5xr1=w{Pq5Ks{R9UM8U(e!ESQUB8+g~X_0dta zSb<s68I;w*5@+8jLQF{ns*CqU?28wIOwl5;$?iM%va5zM7(GR1thz<ajWwXDzLjy2 zwj`$mrI75qS4@h{ck3lC<B9U|+1T`;8##B#9KG#VKr&|2(d-)^dG_1(^ZIrxqhH&V zdA0?U;HdW${?ztug3i6+{IRM1HuL#^`G*@~_-9*8_!EM<`G=p2^PcQ2<jtNZZgcOk zGUGB%l=09H<pnA!3#`s-+N6EDFJQ{Y3Z@=h%nQ8s%%*wgIeta|A?wK%3k7>U>+2$e zLIi#8B7$<L<kyP(2rjqv@bqq8;l0qQvuRjU#@p!VZ_})@P>}c{nrE4#z`vWPV7;{O zuFa_F4#CeS5}bUd17BB|<L#^2&Y7n?6sQEb@nRbuT8}F{#57&A<+VP`5Qs@;3cfm| zS<V)HTKC+dkr&h?W3zmD9M59?S>BQ#dw8x>%6P{%F5xCCP2_SXN}?zI{!EZo4a$pK zEKK5ilGa59NcC?Sn%c9A@viwpGy@-#ea4fJW<3L=#vjPz9tRlJ&_jvyP7`gLPs{^7 zcjk3_7uRmRfv6^UV^{A(#PT-}dF(Jj{wH%$noyBc4Yy>jDw(4xA?LU^qVr&;)^&VB zFAoRCrV)kTQ`oR&k8xj&JpL*z!fyRA0|t1;>?z-wkYarYhs-g6FMNOe+y6N-G5Uha zby|_ms5yJ`xEao|@5T2#JXp))orH#|qLrm*;9aDUdEYZv$mFiUCNn)zQ|A?up}ZPa z6g`FX&~em#XeTnZ9Ea6z2hzt6B8mHKJ)$rzhR*ZDY+f;CVE;@ucxD6*|4|G|eUh-b zvIzZ2lw?;$55RZ1b_goDipGeg0YB|89`bhMebt-Aj=gaUrflAXUS6BUj!}{%N%dto z-tGz7>6r*TdK|9dKA5UC1zW!nVPA*&;#-C#c)8<2eCFU(+H&*+Z7#cqJ&Mv%dbNOA zE&QFY`5uEtd7e1rvme{FXczmvsQ`P3I-yyLv)J3)197&EDn0)EHFi8_!@}5Me6;^8 zR0s9qn!z&by(E)ucXno{|M`f$3M&~^yEw#um_n4KZgayG>iG4&XM*j^@_6}LvzeDB zRyIkU#b{YTG!J{{bIx_)+{XEh%o6Qq+@b7F-jy4b<cQQ9^2@E9_tQ+Acqy(y?@#LC z<MS?a^Tv;5?yt*ZCY_jpb`+?Pew8OS7pBB;WI;bCkgX@16yBmM#WPWceGb`ED+D4s zhYGxx&fz{uw=iC*8NA@XQs~@AOWwe%M=0raCue`Gn9R=?XL8<GGxe9Ab9OC(%sti( zP5xqr=KK}D|94N3p}-XKLl)45*OAOlzhTrF^PK6`l_P5#3Q@D`W)k+x3xC8W@b|}C zqILWSGTM~N3s4%*(KL<;h&s!89$}dJfdQUg-UET>es##@sX~|fc>K8WGnb?<LH0)# zan-wfiSq61WMtoU!C}pArhIKL68{#4i;q}B@Zu}HeV3!ba$_~;KK2OGes2jmYo6hp z=))vJ-kaEMJkK<X#h?XOHV{jvB_#2lfVal$6gHkOj>|_la=Rv$Ii|c8WUM(sRFw#u z(Y_ERDipzAPRKPre+`#}F9dPzDAcBshpp1n$+}Gqyg72U#CmTMRz97^Xw){NOU`M; zSm>?#QZ9|7J}E$MRXaLt6hN~2vxsw@H_~dV#4C-qql~y9@Z@3gqEZrk2HXjeUIFCo z;<^{?S$yT^Sgf-?7#|zBgoC^@L9TEGIc9s4T+~^>-OQIDxn7ZcrBC79_y(bod5@5y zyPg66?|k^J>x%ac9w*MnLvd!p7&xb<hIidxLD+fs$hVAz^!}POV&IvLw<ueX9O<dJ zzd9M;wm%I+nhxkYKMG<T{K40_h2%%w2en>n=H5ydGCru32r^fZ_Mv>dOZx)D-*O1s zzP-q(4y&+K=yo~Nwt#i5?&lfaI7AFb7@U$LiyKd;bEl)Vu&2EdVibf%)#iAzyyq5{ z4sJsB7bKxyX@m@DM*z9EiW|rX$4z6Cnb}%p@PIcHu5@SN?=?@k;f!C%UtJ5@C5HKN zIlkz7`4g0;`x4Km8=-7!J!)+);-WJ}=tL)F(q7R;tm|~KMUF7*Ywkyc<a&50;g7|A z<UzdhA{38&N0-GMAjsnw7HFS@%I(Ln=(9{*Bbg3XokeK)a{+9<I1a7SEg+p61nA)B zOzwBDBGgF=m^9mVH0*zikvc98MKeN~7Y~QgT~WaQ)+rE6Au}Lug$`qus189lX2M<e z6W94c*qU=!vE`sPR(rw0?cM_PJm?C*(_vzgF&VrzClTu>)7kz0gXry-Hd1io9_*j> z8;!lY5EiVk0{`82@D&vvujYV0{v~!9z8ihVT{<_(%Bo}VYVI8rf5{!^332U(E7p*C zPh&`bK{3gAbOvn;`@pk`*u%(tH$=}=Z*!}r&&7LFDwvXntIX#kl34Fm8V@(S@~Z8} zatiga+zR#@_x+R>sZe%Af1~FStuKF2iHQYy^3Vx+a4eH~%ARQSnlW6!AamhNGr!E@ zHQ%MyOAz5WMR2D!9Q{fjCa=DW2}3BZT$I8ju3};X>XCXPAYK8eu(*Mj>|<;7qfY{R zSBRmZE_H5A?sLX`jyzLb(n<9E?r;%{`j|G=%gC$j4>xvM0GzY%Ca$w3k<=M^c$9gE zSrrt?{QTihEc%*IPrDxat&PY8=V_q6MgdP1A{C83TcAfTGMI_ie-q8a6S#lN^O$^v za)HgOEvU%v6tp~PBX_>eMX%M&$UV<2qMF{0{M)WE8&2Ovk7`aq!^Iry?%;peV52ym zB|n~7?=SRN_N=x!p>ECjFXN*!5oi1#RoVZkv;UteyMMU{@UO20n5e+^j^Xh>*6reM zO0J}?Kk~S)wKAw&*MO?7jUbA%y~(_#6<Fq01>fQHdrqz3HrKI4mP<V+j|<>Bx~l(@ z^gZ-K7Im{>rFad;FMP+frd%ZdM^^T~m(%|*RkmYMGkbkNpSWnG5%*J;a7ML|^jgir zSz}Ie*<u`dyQ>O+-n0uHnqI~zl`DYs=hHOl*EDwAsr7jJA8EE;KB4|g_IWnDY7Q-5 zI<~%3V<lexN}V0<(4{|qJK8E3rn1|vrm-=`o0)S_BdpDa5SlP~FO}OYq&-zGptCF% zkcxB}TjkUZFnzNKPQ8(c-;^se+0TMd)}AOvW=SWLH7}Y;b9sQq_sD>M%sk?|vH;l* zb)pu9>w@14U*R<u&1vD}sVH}n9Gh-82piVMvZ2%6@ZNzRBy`OUytX+Qw|R4T`>rbX z=cOFHclrrZxZ*OFw4Bd^rW{+%F(m%@a=fG^hBP86_RvvDxEp#MU%G2aG<Q#;b;6_M z{fpn|+TVHPzevNwTgJlsDIyR)Q53)63y{qOcbgjCV@^$4=+2qCncX%q3N2Z11-@R- z!?TZ{Vtq<u*cDbXwl7zhk*h~U*tc6Jvbmz2>@v$g(2OKmU0ER!^tS>bZRV`ZwR8AV z=r(-iWGbj1)~2rebg3hjK}Sayu?=I2Aic<vUD$Mpnc(A&PX+HL5t9?3=fPY!_(Tq$ z|0{}jZe>|<?k#gCO9v-E&>|<YPU5Q>71(BVBKSnbfR+3{Hg;tezVUoKKJisZDSdf{ zOt}A)S+P7AzgqSS@0b>XYr7Y+sax}i$Ru%k-g*enGXI4dt5uj(kA2u&TS!C<d<cgu zEkLrt6rbpf$2%8D;S1lw$dPyh=9%LO9JEOiuQ-$_DB1lHsjaj`8+S<I{g+fo?n6V! zy09Im`82US-!6Pkq>|PZ+0~0(;IT&@yVHp0sjPQs3DwHDg70iy!@6VyL&^w&zQHCz zfzZ%7;bkfPx#bmGIa0+w8uYBs2)4s9mN9fs+H_iQV5jX}0yI`Jj#lP~ft|BF_Piht z3vTTdQj6=j?EVEHb>|oANY%%^lA*-p`bmZp=EFYB90r{rTef~e2s~I8Z5z7a1l_w^ z!uFhA4|p49v)?=n>9w9Eta6z$O%d$}9oKVoBvOvHZTk$r9Jb-^^{Q0;TMU=uwGe+e z`4YE&?g1668f-tSix~B9!b0dEZ5DY(P8P{SqPY*QOOQu1yti=Yj>&@Tt4~P6H36Lx z*Mc>UdZgAl9T^DAX7;!!dpP?XdUnsEel$|bwymm(O&82$ca&$c>O5)s@s%oTI5U%- zd}24WyH6%{4()>SgevqweF#2`+eIW5hp>XK7ri}yDja>Ih~lrEgOvJV>?d;_x;ty} z1fwp-zS<Y;PYt3}zBXDUR)Z3!4s%wbr-<^=TF#<WAGD&rkxO5{qg85+SRR_l;$7FF zw%}xa+m|lfQ*xO_>r=@3mC0PmK?e|sVRrMRJh*>n3{BmX1sgj~3fDpXCRtq#5a;j; z?RqZ@PlL7yRj>*iQT>LbKR=DhRpG?2ItG2q8Vf6jS@7Zyqw^bDNPzZu{M<_uCL}SS zaH*U;UR_6;We1RH)(F|ZO&JN>bZ{w~MngxUsb%*V+pP)3pgJ<b&f7MTy_g_e4HWeX zt_y<je2L%iVJpMlxSqo^O?!`)EN_7K9dGahUs*bnS0&i%P>XH1JSF>gd;-I!8shvl z1Vmpn!#yKqdT<J#JWO}Me>>lzeU)Z7F7lEPeNl((3#Q{qGVSErY87t!Mi!ZEQDYQ+ zTG_#I!K~fJD%N1bU-s_H)inB9DXV!~!FCLL4o|!;M+I)-Y_Cj^t&c-D{ViPQR(G&j z=<EMYtl|{lW8YbDa`eS}bNe`*o>aDaUodSsmdE108L)oX2x;ce7Oo5X$oNmYO!5x? zCAVa^6B(0QvZ!x>dHzrf=kmNsZcZrAbMTuW=)rXO(XC5ools}bzK^1T*3arUh`qpJ z)<+1b+e3@YLfDCEy0ph5A1}T&N>83qK=saDpq1gu#+vWPDxratnIS@N%4b5`%<a_8 zeJ<^}{|R5;CREftvS@2TF8#Jov3|#`&CKIFyNR_33%w$biKog(a&R*bn;L4t!O<r6 zQJo9+eV!?h)Srfa&-}t9TT4*h&j8GyaGL!kcN4D*m`)!UUB--rA5`6m#L;`BKvgUU zKHACRL4Gm>&iDj_cD>m4=xMTFtsEctbBI%L-iVJrPJtT})1YL>HTeBa2M<0+c=;J~ zcG-&-GAG#xtu;tT(l*;*>*3YdacdhIh!BV2pEJm^H4o9D{3Y0C+-)v*g(m$FB+Kfj ziQ-*rm27|fs%PIAiGp!zAY5C$m8C{T_{yqevZx}3Mn8(8ee0BI>FsdHJ4w)lepzbk zdJ`R%)u&lTgCHkfj)SKcLW|-CTo$qwYCbl>*tNMJ6|n%sCPX2Jpc{<8oIWxrc!^e~ z=tE=I8M5rkR&=>u3GN*o1CRDRC8$XUrk&lxUf358wUr*Wld=oxks2G?8pwmW2V3y? z-bgSA>BSX4BymEeDb?W%zklgS{IKXN+*OsNUZ*?oeJe*i%^{4wJJbLV-ZqjCMIrEY z*#&I+#fg+hL}JHHh<H4`!(Bc89lZ_kC$n#C#nzU7WbLQ{geS=%vGMN2<$xJJIK2w# zKiv=O1M@ImnZYt|4%jkZHEb2<K4s)?7gF1waX8Q;8JE{f(SGJMTH;rN(rmiWNWyCL zYRE&l8t4c-*|r&PzorHSPmJ;Vu3(&g=@ss9d&K#D?dLYkn2Em&N&5cGHpHpTC%NPw zFJ)m4m+w~2eLiuH^u$IY(=+-wYjrE{m|QW@I-iNe-(R=cFtmd;t=o?W-*!;3aFKe? z7ZtS9O_ScX*v4iW1>=3y^Mv=-jU9Vs4mI5v21Z(;IQ!^Rvb&}b%%jS%x^*PiuxT}Y znOF=*gl?kTTki;YEyI?R3~;eLfF4z*a!brIPzf^;R+q@(oLd>>#IChAn?AfCo{KH; zj~UivZC)|SX^;ff#SZLMUJx}}^_lr<7D0OqLT%Bo28euZWLti0q&19yD||Kr8Kpnj z(p}!R=Zkkz^Wz&xW_KwmH8!V<gClL#xh6I$`!&t%&I9+c*8pFBh?^__;-B1EsvI;1 zCwjd@ZzO7vy|M}@%UdwUIcJI8z#Q~#s}7dXNJi!FB$)|)C&=guZ9MTw0*xI_!LNFw zK|?;N{`Ko1Xq=qI2KN1h!1`v^?dlyS%RdyWuDwG=pAXO#+L=gkM>e=NEn{qd8q(Y) z^|&?WA0GGj3jN+XiF(o5$aRu34!1jiPo>Gja8NHZN6iEjon^30?H^KoqzSn^(+9_K zVr0XyW$4Af4e-X+9meP=K$Oo9B678f7+eac-}J(%$MkUanV5lX_HvW@Cwf)TqPu{4 z+Pl*3^TM?*-m28+SvFf0{FqF9odavyRiOT@KHFy53D?zU3Tv1n?Awb&xJS~Lbje*r zKMmwCJy?R(ckX7-{Luwtm0C0}Ruw1TnTrXfsBC8t@hF}vMB0s|N~Z-Rf8iU>KhFsV z1~|}RXAKfCj8UZ506?6Ctx41*GVkws_H}L^oOQZ@&gtue_?=sD++Bgb%WDCCS2LWs zk|gNv-HfLnX~Emi>tTMt1;|r>#+AR!#wRY9kj!Pru=a*TZUvK0UZ)5#UBPeB@{%Pu zw9W{<k$#8lTL197lim^KToIUCK~aT?2%7se4kcZ#!n<e3Ky!v8n|t>bn_uohMV&OL z=+R{~%E^hn?fx6z7%9RAY$<F%8I3Q;>#|Z8byzRdjwJcv%sEwS#P0sf$r^5e2zz-d zcDWAk74ZUNbuV)LP8Dch{DhlaW&l;FL1XSNCI!h0(4#C(p-V>=R;W7TWlCOf&Q2Ac zn_0q*eo5r1>CXn#{9vd2xd~b~C2SwWH;}}fGxTNHJ*-i#!?t=9(OGk@LAisBt+!kn zZEL)W56GLc@3(w}jRy9dXjBu#i1y(JH+SKAZ5*3Cb`_PGP{P*QoP@Ni0(^et1beq+ zJzDIt9f|b6C#u3Vgcq!>$kmr(;8o?07RtD=g9qorMsWw!@k1AXY^wx?Q-iqm`~_H@ zaI-$I{h95>SY3)XIe{Sh6Nox>f=JK<2y6My_W4*cx%%z++(0!}{agv>UklxPGFw>N zp+p#St%w@<Fszj@xN@hkl<e6Z&6bV$!R{?eY~ZK?zPwmlh#FHOk3DL*@a-Fs;GiNt zzoCp<np{JEq^`j~Z%oE-^F7GDFXq%Ut_LYNtFi0e3{&IYFYLc^X?pnEHri8@1})j* z@a5+^X7#ptR7moMJ3S`s_?p?+_Q@-BcBUVDUiAR=aGVa;M+dN7M+hFf!H|x8m!b-z z8Srz~FQ{2x%$xV|4ewu|P{~i1CxZc(QEpWxTBYzA_4f{t@O>+&{Jb(06?%#EjQc>k z>~v7^-d>#4FHJ)%PuWT}C(|bv?bvvsqCE9}7gp1`4>e;#aCgZJdQH%abK@20{-0GK z{^&BEzL5`Gi|xru%iC!Fvp#70qE5!m>4)@I3({n*4hPr7qDp%ya=aFSu73=&_$Uy> z#Hu2h`ZDs=shH?4JcO5g7a@H@C+IQ@2Z+=9hn_2K#YPSv@B>{!zprXxgD!aB{#l1< zlyx0gcDz7qhHPl3eIS(Fd4nc=QK1TsPlf7#E<Sid6R$nvi(ZB8qI;P}-0MFFORW%R zeXRWGNtqn{`jiVmrf^+afdy=tSWZSJ>!QUs-Ql=;0C_J|s>eSWkHhjWA^SN)SYlN? zxmzQ|c}^V<O4~M*Be_jjYUv8rD$mgNtN#}6$Ng*cd$A$wIa{24`tbucIdC6+pRGXC zSOsVwTLEvbHi6CU$ru;Z5V<F{FtA0P_H-1W$jCx?(moDe-4>x@I$?0cA_@FMR<dt4 zNAi*v>f^_2{)?X^bkQx_kCv~|z~U|^d0!@K(IKHLV6KNF9^HI~l<GO7cK;VN#d!+u zUuRqIe$m`^qlY9cYFN)c`6j`BlDDN(&Z|QHw%JgmU}-yfzXZEl>o(3jIupv{@*qfV zCOdgj8#WWjBRzpJ+fuHL@7@-pE>9zoY=$^|l>duOtZoqRp$llWsXcbJcSEOpMPUEV zx#0A<M4-N09~)a2lM7dJ(3sRzM#3x<8~gHXmq$0jtGp_DHHozy&D#Y>r%U2ubq%&^ zel^X@6|$kihS|vqO*HWqus3x2u>S@=_REi^Z{~f&uMXeDk=NGHc|xAj#}s`!(P0L@ z%)5__M4eg7`!C6_Zy!;}_atHxRL>|B=%eI7JJg}Lln7&2SW@CXx3*y^Sf?!p??hpb zCT)UWEY@U=jB+r?MbwACx2k{TF2&BA+KgLE{OT5(z9Pp?UWAqGLy|m|(rHVj>6cd< zfT%wLpU;Lc=XwmLKk~8pP#Y{L5UxMH#F0hODsaIx9e<mn#{EZC_J8Ie|4)_8e()Hf zDVxY~%RzL+=N!ZyF~Uub+E9J5M~LvB4CX~g(42n*WY3Lo=ChV5x3oTpuocVTo*){X z4jPMNy7Y*3`xZ2?BNvG-ze-%viiy)|PwYk2iHw3PR<i3RS3Z2?EYf0$jH3vo#5_le zIk*2WRrbFx?|-SX1F}v6Nps5k=Q&Q$o8MX|aMcv3KffrLKB{9a+HT2YHcVz-8-3<g zrg`yFl{0uRx6a}IYD(F}uldKj?-a*4oy`^We@L~?d~M4!%?M%SzH19ohstam)|%Sb zn6DS4dO8SFm|^~#_EUl}Y1ag$4v{wN7M&OTUiMj#G%}CT^^4$%nrH}yLt_MKzv2at zZ?_AQGenpzQ5C#tQR|t}<r)kzeZbSGSjz2JY~U?Rn8lo(n8^735#hS;@8?ZbO5p9g zdXllc*D5F;+s*qrs>YizF-wq9F^O9#J%uxE+QKRQoyfg^9L!&MOplW(-_7h<SxQ!R zx1km1HPJ(-{oJK7al(h>iBxOCg-T5r?7Mh{)D~-TU%d5T^y^);Nem;E+#5vMI1p-W zPBVjTr-Tb{R3P*4bts$~069N($g6u<OzRMjXP27Bs5Ly|HrYO6kYX^techQ@S+6E5 zH6D|Y_6i6=3CKEX3nV>>L>YSFI5}`UUGcFB29MolwAM!;nfPsZ(5DU7i&>%8J?{9I zi5i=0D8|-4{)X7_nfRu}d2CJ&le6cJ!@M7=bgGOw_VL_>Co)e!ZQ)w{ujmn3q^SYP z8(%V$D^=;O72k=~N=v#x;vZ*IlL_-nN|9~ZT6*Pr7)a^tg=^<tgNx!fWLy}D&2<Br zk_#B@2K(ULk#zXDJp(I5s^RDEi(q-_09Gn|gB;tB!Z=4Ycx>mzgh@?>bZt3$w4wl; z=&t5$xp(C0qn|KjEyW(JlB5Q4yYMmLhPzO!7D30cEC>#kX7_!6%;?5g!OaXscG7MQ z{&X1;kW(%JLw7!m7|4N|)phvst`!;wci^x?hO|wHfVy+nk)Nv(MBJ^yu>U7r_MgTW zy7!G9dAO5@6|&Po(E6MCnLCXAQzfZD_A8_2Xb3z0PJ~zOO3;(rfIdz!W!Ekf#g(HO z%)Ct>(XMuRSU97QY}z{l#}52KtM?`038J02Z_^z#QT+)T{?&q3k8XsB&_VKiwgRmG zw~6MC`GO7_jN(gqkDwxdC7#v(0`8s8Bac!R;B!tUbkJ508~+JGJDv$)|K>_iHeinz ze0YNv2F+r{93|Ptf0E$5IEa{jZzNaUUK5__dRSV188)GBpxc=aYps4Dho^GzrTQj3 zO?KkBaC{P>phC5LC!+HOdH7`5SemQchOD}bY4&>wzzY__x2?{?tYj0+cApP#QZA4+ z#u{|%WCa*;e<{pXeI&I9+whk~=Wx$*6HrfY#=&C1MazbgJ<UZZ-^PF%8@1uPZhACb zdmDSpr3&p^kqBG$G>K7mD7H4c#-uy%1+!aLc-XlBeU|?~lI9r0rJJYO<>frq`OzW> zY;gk*oldwBehji5;!)2eIjo#^8%ci7Lyv2caHm~2_{?|=(R&VnVB-wj-R^<z+hlNE z<7eQkox1pXbr#Vpdka2l*HL-34W}pm8pgD%F&qAt;{BOt;pXsHyn7+WF(O4|I7Jo< zH7xL&F$SAc10fzu5BvT|Bx<<<INRe7^L&#c-Y?C^?9-jFeqSGoob?)Y=$k<DEDd<t zN+H53pG+B;0zspNXu`0NY@L5cAR|&tZYYUB>3MB>?CuZ~7b_%<K6wU(**WO*IvMJI zQ4H>$OoSV^Y%!5bU;|fk=uPTE*tFdio&}b&E`hm3_lW?O7S1Qh28sCFF)MoSV<L90 zG-9)#Wa3_}Tws-J7|y_zKG$$#S6#|RC%vbzL+Z64Xe30bdOEa}B+<(r_c6ZMiqgYN zvD26LNaWKeTsA?6dX#>Ip>|)=>wT9bDFU&{eZjnyZX<^){b2D<J6s!c85!&E=b{JG znYZ(2ks@zV^zOY7l@t+%!x<~`<%0~-$eu`!P1J!|q6dh*%O2$ZERpQ+F6Ml%)*;of zB1XaNE1J`6%<1qxlPjrX>C3u5M4G#VbR2t7@|P%*r?LpI&ZvbgPwy~fIG;<&{)R>$ zjxzo;r8&p)Q1axIGXAw~2wyRsf_v?)fZbw61al_C&*O27&i_Z(nRrtb{r%sNF*46& z$WThA2>0y0uS_LTC=Em@N)nnh8$+TpW(Z{{q$nxw+56lE2^EnhG^bFJG!gyIZ++Kq zJ<nRd=lKuL;+%W-`Mf{x*UL{IE<~?b>MVQQD2XBd-j{$;y(jRH-vP20QaA>UV7chm zph{E*rVaIhPu~&8m|fo9cgYifSF|VVG_;V4x)?BlD@k<aCQ|+P8F9TN2#0n10aDK) zoJ;A<*iS*YVT1=o%C*Qj@6&`)PNsu4mI3e62H?=neI#Y35)hEwfj#Ek#G?-pHf1)! z<i<MEWV9L<Eh#5icqwU5VQXp0@9Z2{n#IQ+9qPr!7nHt#AuskuIn~>I6O_L^0A?Iz z%TW`pw4%Hwr&9c0L&eu#DsL=`-1b_9^Ou%`tTht!@2YKf(^cH)FQuB;WcwAGDo&!k z`<GK{(mhnN{CB$NuMD{xVo2`q(V!l_zrpD!I0lw2?czzjFvS15IpFn{8rmbEhG(Et zN)K2dO8#XNb<ALxZWGl6n{~~x!r6WFSla+~&7c7&+p58mF-3Cgog@|M#-~kWSVYpm z7~SMMA3j)V#@qkyJoRXAG2Px2LI;c2HwKocfNptJiYs!E9+YYTj|IEz7JV;<d2W8h z{dEl`zb}Rwk9Vha|45P*YU=c3iEF%K4-vcF+K0fa<U9&bN|0y0H!+Rci1)X%@QK$U zdSCQ7r8BDoi*|`oC)O__hi_wB=TH^QNu7yzT8aU$%Z5-ye}Xy__kd#`+d)N*nsHP* zH0f)8cd&AJ0ZqQq)a<fSp1RyOTA*NndYy0^9Q^$WXFZkR&HpY>8M()C-nw(CuH$pb z(p{mXQfWC9JN$rW;JAh|t`mpf;TK-a8EZ;bB#0vm4^sE#IUsavDbIqZNe*Ovr7cd5 zQWMiFI7NvWSldOC)}4756t?{YJw?V;y|o3swkw;Sc~PC(C4CG08qWY(%Axp3&Qacd zUqo9e>H&$xOMni`H#qq+2KOJ(r(Dxzs17w>Aa?R9Ryng9TR&gLc{Q|~_mg)Oe|@(J z+%J`dX45l(st~&fjN3V{-=ClZv(8b&wfDh}eU;?l5+f`pd6`<I`>^5cTnBvnMFplS z?=|*}io&Zi6!E<UX4G)H4p}^vgQwj05_PFou<<gV?mnV``=Z^5<!~L)+NlT2&%5I6 z$$z|4#xWpk#|<noZvj5K&J8~sum`bIdORJ?LRfWPfD-spML%m*!#lQ~qC`G4-jEFL zY~+vDfpY;PKyk|h5Nof11&4BJDTAWM?=!=2S?NB)%rU|<j|;%o{A+fBo5v~B+_m`m zCrsaJu)>}9t!Nc0oRZNyLfIWPpnBeI!7Iux;A6Shsq3X~^zs`QfZo*!ir4*v(hSMO zPptH?K|?bwoxc!+N*_4w=m?mj8%#Y@u%_jcBk=I4mmK`#DDPVmmlk-)2MwBO_=0E- zWl_o+2WVLk({O~^w)zLy94F0_Qug5-+nNokx-DoVeVXb!=M3Hq5AhZWZ>6;F#X$R` zM);<JFT^abq;G>PNSc-h#NOG0AESiTJc^JxVV^-#K_9iXZZ;6Ft)qK0`FK}Ne4}63 zEY?_%3KFby=t_}hAaI0BuI3fuE8Cvq2PZr@>0RwSIpK9=`WkPbKg$_j6qW;vyEExE zWiohGz9RV_Q`!GIXa6Ua)m?LuZdmY$9!M}?bob<tZ8CF6lgDeybAA;__$p1RLf_FF zeI5h9c{5m^Q!u&Q9fz-6QiR>dSKy;<v#Al`)zrq{a`@HBaw=-UB&B{J7w<^efL+Fw zaNN92|63}%V1DS<u+1AH|63}nr>8<h&mU(NK5C~w^wxk)SC*3Tv>aM>d@1~`uZPXf z>5&!OWAKY(HpWDmJSkCQvZs6E7=Azbx?G7n^|g@H?AgvtEi^;9A$=U6ql^?JRiTTc z1?n682Fo5wFo#y(hPUlA@yw|(=2F=xnbB#4VjaK2*kxu6{UL*F%yVVJ+;rj9saiZw zOAdZ=y+N7>RLISei|p-ehl8eRkXDq7rPHlg82D_qW$^)SJ-Zf<Mj3+Wh!afrgBWyW zC>eP<c{9u0#&C4o3HZ$MICC;o5q8NRM1G;s@NefsW})Rprt|m{^5RJ?^CoyR8olL( zW?5LV9kMDyZHhu+SDw;aM)c7EH!1Qsl!7{TGK`dJJvDFJOV06fQ>=2S0<XWg6qX2y zW8S{6pria9$Y#^W)c#u9#P=5Eb!|S3Q>-B;Ryi|^=H4SB*A1A1lm>FAW^vQKI7D>8 z6#jll2_5<*3_qnSFy+owC`@TH&i)<@#p?9H_Xn$3h`%k<AiSRxZZ3yLWzAS8^el8v zmqY#ne3J8eHPckX;7>OtnETyMFz}=?bQbX>brI{JaO+Vphzh}VIFqPZ9b=!OyWsup zHXwE*5%KfQkp0Dz441=4_ZQW0?dR@cYW@<G;WJ1+x|=Zb1Lwn!=L=Dwmk73dCde(> zwI9~Z4FSmt#l%>y9u2PjNnU0}qo=~XWaT)!@9!Ur0<wRiJMIk1diN4oHou3wkj3PU z*g-rfH-_cYHObWzncy?)^qu`$kNo3rAQg&LVC(Xy*jUh%E#5n#tc?wHt=203ub<0N z?e1Eby)hHMX%>NHZ)mc5`UJV%av0_JN$}0?tzdpe&Osu}UD2x5u5jRmG%Vj=0<zgp z-ZdwdppcL>sBx_t92&hzH+U<9H$VOY<FmUNA>~hC`R#bTzxx5ce5xEaW$puN{T^iM zbqwe&@MMzrR$&Wf3?IGhKu%;l#g0mv^m=L~@iopRKf4;3bzc+FwL3hrzT_k1NBrMz z&Sm(ge;d*~t4Z=L`^Y$GB2RMl!7Wt=ggbZRV=r^b<~`bs>yua-0WqeET_CD<j)MXQ z4}f66F6O;C7jMj4h`+D2hyE6O@mC8c5VVL7^q*bBd2@rvi{IAJ!8--tuw4p6Di1JX z9TMp7;eFiI;oV4J|4Uf$H-@1!Uo)SY<e2Mc2Ek~A7n=Eqhjty<&waGd2(s!u6q;Xz z=wKP{+2n_0OW0MkN$eOLNM235f7rmy*l5CCq9@69|9Sz_{Uc07RW4vB+t}@!JanCM zA-3iAB=*y9V3$+}4s3daHY&B^b4wzr)raEXm#@mO&VVJnN$n@oREK!8w$!4Kc^%|! z@M(DZpAgDoWhN^_^GMF@aN5-^3RLSYA{%!~kq1?c;1Cky9r0Sq)L2YIa<f@V-@_oh zXoD)8pEw6?JNk-zj+2E8Rb}D&zR&p6STkOQuX8H5mtdvGy<oyS5%|oR3#F1<zzOk1 zI3*$&k38B=BHH4K`wK@l)q2bbnFdfFPYp5wLN=(vvJU59bE-#V1@oZApES(zL$<F3 zQUBJ1K(W#k%7_eOeb(LkGh-D{HS5F7nKo+5u^zOG2$0b53*?k^6}iyj4!GmfnV9yK z@LuFq%5dC-T(*e=`&xce)4ryHTKV%}S;q~k=lv>b&tYL`VAGFw4Q^+CB)vteofP>$ z#bo&TeiqE+oU>@;xFwUH(GA<jm6#~yzi3Ro260Mlq2<PT1bz!<eiRIl<BrYbR{aAw zqpy_tWTJ)m`<_uF^NupZTYZ`BD{F}&=O29oTx70V6Wq7AoZ7H`oVMDUhX3iX1E9X? zM1R*CT3k|`cnfEO7jN%#sE=W2gZvxf8*d8TzpUWu-jGG8OBgucK8w87N}1w$V<6sD z1g^Tfft@mIa-Sv+!@8y$Fs3;kFPoE(UXQsm@nuQG<-7(`yCTJ$;)nppEAre$yK_j{ z5<(tVG-BtfIAjp91}kzy$WCoBsCh091O>7pn{``(`{HjvXbO|^-j{g&-BEBiHxDaE zsPG#uNI^|79U0Y>k*>E!C|ydAc<xkUdLP=MmBQ@jXs9TII#zIBdG*2zOT~$@?K@JU zm_#Cl4TxvmerV<T1|FytBHSQ*czpj7*kLUKL)ncKe@36o@>L|`8|RarF(Fb|p9>y# zNrU5CXTmz?-+0$DcWl2*4|`hIQHpoYa9p#N!`bVO;-X9iW_fKH*ECoOwd|1R3&}zz zaEmMR`uZZ`YRD1=rNfA~iZWSiEeXHdyAki%A>=o>L-p}>@!OzbY9vyU2|1^V8jp`q zH`j%dzdLHkrH&5lbM+>0@>d{D+j6P$-zlK6aTR$fun=#$TFskP-wp)cT?Lz6lAxkn zE-uP7!~TB4@W-bfveo??tbv+jV&fM?vG>6v(+<|ZeFO=DdQx@n3f!Br5_x%4!p|Hx z=H*%mbak2%lVPAoTvAoY-)wmVA01lgm2F72|E+?ezw%*w=W5bra{wyo&ceN>Ma<Na zexUBeV&q~4!0##_yU5e^ph`RioEPZBLzhmFUe!Ll)c}Iv9Cy?r9L<;q)syi*!~FUI zJHENZ0jfwKl9)!HAi0E1rjsw@C9n+k`6w{jH>NRtg`SA+*vky1Ni(~)Q_yKm4m#uK z44r}=5Veq_@atkpWX2NMpJqM+)&W-Js(&-{F?J1fi9JD2d!a+Bq}2%j_#hBJ=>mDr zGjJ4@f~6W#=sz{qaKdjsO8qH}UWY3)l7CC#t$KH^rY{FcH1!kb>35hji!{&^-USf& z?gv`%xDeX4&0tJ_jG^kTP_$F%0KDodgd{n$$<FPOsLMNn!EJ(M;ZAY%_xKx>Yobi% zkH>>~)w>vYK>$u;U3z6V?Wu}a<2e3AFg|+z2~hJfqH}aC7#Ei6{U_!_L;fZO@;dM< z5^=FaS?81Zakql`AJ?B{I-TB-<G**oU%P72p7{5ay>BgZ`N?kPT8cmOeB*59inKJE zib%k7?;A5Emoga9yT<4ed+yLIH(+9&G@07-{Y2o`Gt#o~EfD|ii&t9kaHXOR+-9FZ z9ndiV=Z&@lORu|NfyOBN`?V9)9Ek%LR@~&7FN}lH{P)n<(hd18vE>&(w&n&l=At`Q zswmNSC9EsY06E%!z#bJ3^!ig8(|aeI-04e$+h6yP&b!e>Cs`0l3mk^8Z>U1<-ySmi z+J085Qbdd&IRNg;O!$IbGS<~<rMQcf$R^Nb=U5yL#!~0vT|#BN;8<bc`cMD_Z4P6& z3-m#2dn;B^j)b!|u!5=!K|uLr4AM>w<m<m;$TveFX8k<{#_(V|^ZceT^JefO40#+* zgnQ<bc|Eqs#;}Pj>lz~7m)P^gqFtzalQ4QPU_st)&4$iym+_(Lt0Cuy3(C|;qWNpb zA-#Ssi4aTR6@ByO+1+cz6KWjtHBS^a_h^H%C(c0YzBVxoPy|A{?c|j?AAV@EL@(BF z<@PAoqS205bhvFLx_N&!(hk3XJ~K(A!RI*oV5QCPKCH}*5K98wpM7w`)B{<v$?t{W z0H*k<DBO~#%y@GzfSfT=RPisCd-d>faNGYJPK=O4mVN`^VXqrl2IO&i#5?>?(v8<P z)dOxg*Ww7HHuBTZmM$~C0tQNR$?KCJ(8`pxWH_i6h2APd8@lDtyf=|VS@j<IR^!E} zTr{FDzicO>Tc-2>%~n9`@2EgCK}%%0(Ge<6E&_g&&WyE)C$nq@k6zZ9O``XTqTk#u zIC<C&Wz|~aC#}Zh=lUXiDe@-nD$;@vj9=oXUQcl3)Ft45P?YWvKMRH`WU<fsnb7Iy zTITz!pD1Z$K6;&RiiSJh5F77R;ME&t`1zYQ`;JIJNGOvS*=z6@c$F|`|0NO*9mRA7 z4<X%+k5SUs)4)08G!v|?jsBv$L`LWy^fKhb&VdQY6$>G1JR4#$%32)~cf%z1`LD^! zrlSpW@nxS;e4!+jp8ej6+;^`8g^MqMw>`q3|8Wo8C)faMk1Zfh$D_GVPZl$-m#1;7 zd@dlSuo4Ot9EF;H$KjHSXt=6UkbTG2(uPfm<Ym<>xKO$UB)}Ma#oCKJD}6%@HXnqt zKL0@CojF8yu_HV=nhbu-K%l8$I=Suoot1i3gXGpXG^l65Ki|JpdTA=yxs%<v<W1mj z4iD(VmR(?PZ34WreHprz?a5^Bp!nuacH}`uIdM3#7p>_3gkELj!Je^NVzd#U;;1Gj z&ul&OXr?q1{`V4yXUCb@DTqWHTq5H^C!x(tK72HOjihY8$Vf+dBBCM+u0A(`+lT5& z#Fp1|LTWFV7WEdK`u3R;W|3SLA4Q11-4^OGYie2a)f{`AwxN2z#}Mtv&#>jPD=K|h z$epuIin;aLfa$%i%cRA~ptR%!ur#a%7+g?60_UHD=4dZ0@wpnfT&f@e8;Uq>mGN+o z!YGyOFA87DOTZVO`S{3V8h+_3Ch9vQalv(Y66V#8-CMiCpyDCCpDiSaHvQq5CGG^# zUmftPv!*b)H=ew74F$~wVwA_4HB4WH8`QPRhnuUF__a;}NbMNg)eSmFev8I3IoIRh z%G6#M#Tg_nkr9MrW&^Lyl41P6Dx!H`#^KYkO)zxoA>n(RBZuBpfy#~@#C%f>TszVa z56=Aq#>Shdps6G{(G*7&xNN0La{=t0{EBU!s*@*Bm~<sKQeBzGFeADHxUIiQMReOR z3Nb72(7jfw-CU6y8vl_=TItUu><L5kFzX-xejh4n$TB-#+A#w);wUQI09p<elBe5k zkfT=>xWUcA3+&TyTVXSu8)d^>lT0Naf@;a<CuYdtVG-Vxs|GiEeW#1xn~+k)HF(@! zn4}moypT#~e1qM*_qYPc)9)97%H?BJ`S2`W8Mg%XFAjr7z2Yc#RFSF56k>*??l8QZ z^9ZrMY@2j%G{BA%PW0v>eenh0aLNuwav%Vq%mK*Q_>lE6zlfM)8kk?j3LGD4F#RrW z(B5bUGClPf9CMaqz~mBm%B2r}&Ax+u=X6n7$LfG5>mc^B%*OP22v?0;V{$E@McnOf z2<NCljy#JC%~5Awnw23J4K9EAUnS=L!(WVUKt33;?1d$xb?DrXE8*;8x6_=H(F>g* z_BomZoH7TA-Hr;9le>jn9qcCWtyuT3d<MzcpNsGOIEK4T2)vUyi#g5iI-S-~WwuRB z;5HpdqEBamwQJ6Ua=&Z1q&5qO|J1~mlL|n1SQ#ttc>r?O@`=N<NCq~oAZukU_;(Ie zG5Ru92u#an#8U2)X}ceQ{4y2XpK1%P>m{NaF`8&hcM&Pn^k5REr;_&Vt6)Te3Tr%Z z!N&)s;PLAXIPBdY;GV=_nOHOMKc=$(H3#`mD%-f!AC%8-13`wbXydpH*wHmkJ@NE_ zXS;X9m|tzAwnz?M^ooS1uRI0o9@{}*UJ8Bek^uZLG!0fdFgU7H5|-U~O$pxAg3GT( zV)DR*oQbW*uP5c<?`?B&sE{Ps9Y?{B2?D@+rX)48c_nMcQu*If+5dNW|1FhusFcMX z3p+WSltSnuA^}I|1h819ar)!Wtz>WVAF?9)6)FDp7E?h3xceq6*m)IA`d;Nh**WdR zX44sH^xzDB<qTN|-zW*Gokxibn8K3oz3^e=0>;1SB~D*98^%nJh5|3j@!a20WShwp z=2Y0Aw-NzhdCM8-P3x2J!z_9s<1g%sbs%>*Yk=C>0(i?}4RV-f26rA`2v5okzynhy zP@n&gboZQx8|!xy-=90ku1&J=$BYzc)Hp_u<^)0IoO+@qc$t}T$^r^L>Lv*z0!Zfb zTG+|Af}PGfP_2c-_F!dDYm^pKd2)y>TfGDJo|8a*FV4aM7B}NGznRQ#FU1bxhQ#K^ z9|(dt_*efgyKTyRpz)#%-fXf#%u$wNYuO5AH>omDzbKJJ)17e9G*MEmBuARdve5JP zv+RFJjdZ$-GnD8D&Uc3}^7gC(-t;5SZZd9&Gw^vEFT+iN6W({(?xxD-#!m(t?b<I~ zZphb@redq-a8e)rY+TfXI7)A&=#{SrIVJ+ybj&FWN?UxqG3d4}-IjfsqhDFgTU5E9 zWAwPUQINZs6Z*!~ZjKGxbMe#Qh4r6o5W1<wStTLNGaoPK@Z1L*xBXZ}XOTY6f{3rY zmL&s?XC^szMZaWt{6?e3UFmUjQ^6!J?Oi6PducK6W5O=V;bjrMJt2eIMU~MfWKY@U zHPTdz*)Xpt_8({Qm>j+Sfg<H**~Qa-<jlJlH^{5#L>x<zqjr(cr0GwIlbl^bqdaAx z0jap#ps3*?5OB7l9vv-$v*%cWPxdh!1CAT#IQxq7|B#I9{bisK9>L3s^>MB1LHbT; zJBW7MO<i%N!0d{-*kh{;{ier<_w)J*&goDI;&ek0EVe<o=-@9ZussUjd1nUiGPenR zw*+42i;*o}!+5VmDimjq6Tu)vJEtut3K~9*BHTRkZe=TR)k+4%ozIC*UOY5$JqR{{ zW^njI63`glN!nfvf_K$vH08P+?DH)n4PP#S&5IGd<vR?TU-8LddjZ0nq-p<Y0z}Us zo$Pc8#|u6?!TLiS^e$YBM81ATzuXWAe$Ko{{zXmzxYZWd(<LBqZW`S5c?BqVv70{s zG8{I)N`(14K9PlWi-_6ueNeV=8u9TdqAkNB;orllI9X&r<uzeTR;6{2DVKR<N0Aq2 zXv+X;TH6S-H+#~KwM)Q<-N(qB?3;A4>I~*!?h{ybxeuG|+=}ikeM|pNS0fj=c?=N@ zK}&!+3EJYzoUV*v4u5N+<#(NdE9(TX%NKjJVfZj(^-c}79<iiS9}mLFbZsQf$e=q~ z_Q**265e<C7O8=c@Ct``@bG*WnK`eSJc}+OgPU2&kx?;TdVUaC&P&9%bIo9N$SRuI z8H~e!oF+>y4v>-JPhj&A5mF*{3Y!m!g1k#y*uvsfUa+{e!C-Be#lL_Lg{YIs(R1WU z(M@2^QUO}pg5l9KXPA~FrX(V90o)n507g!E<D$@i_@`qX&S$X;VxRv~a&PA2_qxOI zqULGBU9%d<?4L=FvFHuGvK6p|?b=$FE5Kf{C2VRH0EbNHGrL@G;RnjP@Ke<w9?rRq z!)E5f;2mr~O>a7BasLIRwPVP?Mn(MC%m`A>i<pg7wkRfJ1(|!d6zhF?P0z^JhXGPW zRNhA;MprEc)aXky{hbS$i7T5S_v$IQCruCb{UJd9G$8_cNoWU4nC^C<p)P(x77mSZ zwD*OOb>%ym!5VKy?@t;`V;9tWOuV4p0|x?sapC>VDLA628|;vM2Ar*?qYuGV<dWn` zvTtkvrkI`NDe-lvEo|S|?ua{mIA#_udiB(H@=F`1_VsGYVH&$^{Hcd$I-o*t*|?9= z{V<I<sQ;vT=53?W|GLn7N3tl1t~YqASWV;fim$v6hrZD#wjZRY((3Ti6<aAQgN0=L zP(R4fH75p(GlA})9{}~S)aE}I=!|t$^tfdyy~EHH_>SJ7eCCZ(-&W3}=S-RN=I_0T z?-o?x57%c>4{1?+O|uE?$@@!3?F$9%b%4JAt%Z^~cak!!iN{&PTj|7Tab8#PZ+hay zG@QIo9ISTSK&n!w=oLkdcu&6&^*q=FlvNZ^UAykvNoec^5-VB8fNdGJ&hw+8$rL>? z_bRn{Rt5C~WZ`0GWq3K$oHlqK2fkM45uG1~#D8lh=E)~;2J?CJvspRB;7dDrntq=8 zGh&G!FStSth~K8JMo(ZJ>vDM84ie9d-Qa}DdvMTLAIr(ffV9ySSf@RmDj_*wVqGRL zpzbb+iYnxx>O@{ivJX|=;R7}o3xXp4Y+zdj8qd9xChOL*$ACYxX_M=|4aCQuI-rmU zL=InXyea**(XD0?XWIr-+s8#KF&unNy(ktVp0$rD9f5L+$2p7({zUR7!xf=mvWBhM zX$njQ@Tt5_DLhWYMI3W3rSY#r7xmqVqSh@KZS;#u1*yD7>PA}#FL-7k=;$#4D?ew@ zZTU~A4~akMU8)*ji$XL|j;rFeOhi%g-t&RB#ZoG=-xNzPh@<Xhx#GKa$8gWX3ViFu zFYst=5kZk^c=ntPxchw~+4lJs6|}n^e2(DolFRI{vDggkEw!H9Ig*OszTJq8?<Uc~ zA-jm$SRUE>bscbZHpBV-TPO?FYoycb9v-VF^wK{$wA_~U__4`m+|`MI@iC0Q*{Tys zj|i|_C=a^rlqbRuA3}8dD46}v68!rYPi|RF1BQ1s;Hc^|s^_vb35n&v;YM+S(vDJV z@e=I1iw}#U--D7wE4($w4DNQ#rxQ%|@#QO7Ku@L``?}kL9gD8O!m>!<^u(6FU~mc7 z>y?6GVGO!_^+3q4ZQyT?0ZAH?1^nr{L|M`vF6<M;5>I`BoR0=^T&aL|Z~uYgZ%s$c zLKA3lX$r@b^wS?#^kDC|%VCP{Zg|a9oLbWUgw~SwBaR+(p`@u4e%dPqtS)~Cp<yl9 zJM1`EQ)9(Cwa?(DIh%;ZLVfrt@*;Tay^pLS`JD5^3&^juo8(IAJP@}$51+j0OybPr znQ-MG@GmKfQRNzufbu|4P$dq<N*lqm2Mr|YOgs6~xgK78@tab7^bE?awgn4+M1uIj zgLvSF4e4Fv3I22_&~Gbm!2Op`K*d>~;oPOm;Kc53yh)=Cu6)>mxAczF&FQ(koGD+h z%H?^Zx5y|EFO{Yj%v)wl<xS(P4&G0FK6i_HZZ$$ZaS5f19reks0b{#$(y>%+-#J?I z<3Y~qk|^r>vNN>1v??9F^EdB(^jd1c-lKRZ<`d-}E=%SvFXK2#C;~~tRrsQy5*6@F zhkY(Zuxgz?s1{2Bh1&&yQ&SJ+=haDDYL-)rQ%8AsmwRFhPa!C*&_Kx>kJ262n(?=U z1K_qRk6wGb+U|tD5*@xc1{B2ZqeN7@X!Wd};7so&s$lRp9-s5Pan5ujJVo4Sx}l5W z|6YUD7OVvqf|If3D_73*@ng9Bpa;=?5{$p~w1elx0Kh{Ec*E%eN;~}!d7o=T90Ofw zzmK1(v-Ahv#sk&Fcy}_8fn54g*#c@$UNqTOCyZx!JfV&o2oeVeH@fn`c4*e@f|oU> z0GC3Ro;-FBh(~<{v$fA-rK!CCxhwl$=j{KavImo0$@6+k81g$A&KbPKaan<RPp7#u zR!kXazGp}x_U#3nn|q0Q{~X>0-TN>mOb|akIuGmpDF)|453|OY`(U{-A~$0d>5UVT z_(ksszIuB-_{Z9QLdv7*|D`Ky_5Y``hvl<?z7)G4rs2TMwRuX7SR0Zz#19TfJK+7p zy5zQ5Dcf1$!nb4=@(DC1S9!OI<lLj^RXytgotVS#zM{yuD$U^L<4V#$7(kMS&w_+7 zfHt;2Bn2BkG9zGuijY4?ippZhPPJ5m9c`eMVku?3DvWGcT8xiYtc2AWTS>*vRAPLu z3X9(l1jC<hQsti-fToHxG>lmSOob46zIqt=?GLBy_<k_8N|M-x{DV}y2iBOGMa<>y zk$PSu>{mX44l^>`UR4YuUiOf{yK6~9>Iw2;vJcK~X(7MzwjuwpOv=M|HkdGsV&pr= z@mO*m&KtBs*FMf;zIm_1*Ke1R8Ke4yS}B1zA~KB3EG=TPAsY+SWHVQt4KUsQocevy z3HpS-fESXRsaseRx76;#8?Oc+*%e1YooybvXZr>{>BzzkQ9HODt_%^cSd0Q5&qEr4 zYvGOw75w_NFcSSO#PwT@83Tu{aOM_IwD(L5(a7>(9u&$TWbg{klCCE!_C_Hyp%r-d zuSPgiOB8vqe~7VQKswW(VZX3a@|r!${SIrf3z}OFWE3NzW<p&fFjXcdf!TQ1L@aO_ zis5M}L=f-exy*>hO1sXK8T`P{#@v75YW%OF??EFweWkz0z_5r2rl~cPu@l$kzTWYg z9Ej+E-XYEmd25Yin`B|SY9~)5XCCv8p8~H4IuN-&mb{i5h=zxXN!F}NYH0Rs>~c7l zTpoFejma$d;lN!G`)vitGgXDgQaLQk;sTZk6orye>nQWzi<m_%jbx91CL{hlg6mkP z&mZ5OkCc`LGM|gW;JM*@<m?hf#v$!1GoEr9g=YG|MU%}?PrHj;^_5^QTycZr-Fgt2 z7QmC&q)`dXgg!I;$&D*cOgMiGK9EL0*7ppKI~)&fr?Fu6=p^uar4zZnsF1#Jv6z0l zeI4BU=nSy^HXq*`am9PfS+ZW34D-c)332Yc&7cztnFf~x@^PIT3P@eY+#gSeURr%X z^zlX58ZwMDf2QM|>E1*-U@LyTSOe`p5Qc^=B;c^E9@-$=00XTjp|V;#w9(OoLl5F1 zZ<`UDWbUISUd%+|0crT2$^fout;1~%?8MD+DSox)9!|CXN~<Q8;UmY+Qe*AC)Lki7 zw2*L#Ir~M8f8=5kval#%UIa#<&hSpguzeMtXznDMy)n4!&V6)BQ;@k8{ss#4EGCm< z6Brk6BCDL|lbpFN@J%KU`Yp3Svm@jg(Z%t+lb&aA)O#87VA%~UNVUQ4ZPGB-aTYwb zbQ!tQlfWw(XaiPHQo;7rCZJ^SoR&%YM;k>Z!iE73zH7b}{rq@|`BA=(d8uiIw00gM z5&HMw4y1@){Z&LWJInFZv8zyPSOd-yX@-$y+0b+Eed7P8i^Q<E(&39D<QkC!)-TP# zr&aRsfSV2s&W|KpUX6hJ$>OBr;YMcn3=`Or;17fBmtc)Q(@5vbFsiC&1q{wDp|24Y z(s7{z*Hnn2!x|F&yRxTX>wz@xu~j<UWRHs|a!muQep1cs8@x<(*WZHjB5%;Ux7)df zE1w|~r7FlD%cjJay1{(Wg>bp!0Q?c3hqha9LHB=nK;0m=tG~<(ep-AA8nrdygjOly z5wRORSltPvW;au|6Ra%YV>Q*-{}U{H{ThSm*4XHPF@tufkRFNmFwoABG2yaQNtb5) zE{$CdJidhqlM+Uazv>u!Hh^{XY2{sZ|H<BO<!~84gdpz`5_BaLT|Ir03|FopA6e{- zv_>H*O25EFERDw}RXsrGX}06;DTUSVzM+nq?q`$#0@CWVuTgXN70wf!i+xxxw=NwA z)FnHC%gjVt^0FZDeUyls4^Ky8xou?Bz><lun8!D`Wy;TeHiw*9z6L5!6F|oPXPEE# z?}@=<PpF-Ej2VxZ0}>2)nA6il^b1#_!KM@{!G0&Sp6Lb1%mmUU`UVf3ae|+^weS{f z4>c!QkJ`^hYUe}tw<op_gy?9)(DjFDvm@c4#nu_m_dW$GcR=2S8>yJX#Nr<@)6v{3 z6HsU7d~6jG%)PLa_0kd*u0od%USX#KV_N-~vpY=TDwBK^<;?}9-RZO`JBl$^4+GkL z#n3T}z0RB@@MysvJa5TXqUPwrY+J32@>??DjBW!sJ`e>2>J`bz{Ah++ejkT>_X0|6 zF)>t-BMI8`;Z`BG%XwBD(C&wkNV6N<^SYCy#l2*j7h0na2`l++W5V{nzl{01rBkFn zi&atA1;ZDE(}{iADE?(PNk!}xA`_8C<iIRU>NK*+1D`aaa;=T{7aTx$Ek6?*MuRbv z`#|ch&VbJjak2iTM_`@MI8DvC4)=Xjhc;@$aK2kC_T)ar@jPF8HaqD)THQ=N`E-<Q zla+*R9h0O*P?FK#Fh~w%Y~j968b<nYcZkZr8s_<mN$eh$NuETmfSj&-r1X*?+H9_h z?RT0(d(ATZ<)S;;@7xA@3YJ4_i8Ca)@ET5858)ZXIi$Ra#U+^}Lz6u%_)2yK7G=}x zVLw;$zFrN_SbGK7=4F7&k~l25LWv0e5`x!G_v4*gPhj4U8~C>+LlR#^F&UgtK7ZbC zbU|VOHBW1W%E^CVXqp#NOTlQy%#DoJgU{s0dwINRyai4f=My-$lPs{WBh9O&VS%hI zQExp*zxX!`-cV>Geacnj1e=IPxIG4<$CpE=us!gp;Q+=g;%sSvI6QoSOC(bVu*$eN zT%K)1Hqi%(8*>>y)01J;M+?wUgbfaT;KW3<&f)t`97Puek{O+mDVo-^MxGT2I=AWq z&BYl^LYg9I?CYn`wx;8dq!Y~YLQ#bN9D^GrIvI^-B}P26g7yS1#Q*&l=%44$m}paE zD${|k5;g^Yf~Udxn*#8hmL7bh{1O#lbp`txTaf)FZkVboCoiVb8u!)<p>UB-u#k>n zz6w_}o~g^B!YL2(4|HRv=SPWiUL^B0OM_fhy@8Hi+)11sr-Rf?amJo?6PkvX-~vfy zNWPUpX^ENes$4!6JLAS~5Jf`uYwqO!w+xtmY8AX3C`6pgRY{&!A-)mv4*V#cz>*Vb z)Y2_4z!dj7{w}OaUP;H{z|R(NO3DC*bPvD=uTJ>ls~7jX>q15|HjBAz=Zud3`bn-l ztA=RU2ELxAg`_&Hu+O(+<lk34vLrqUZ1Z17x=m(7Sur6PyGjU#+-rbHE{XSDJcK;b z2_f?f<l(8Td}?=GA-MStvCh9*ka=r9aj2Y4>n_^^F6>=}cZNxkmLIu*-`&TX?QcNJ zb|AQA(-5p?&wsmi@wu@>$C-`a*Pv6+p1_Cd2eFi!KE1J`1Lz(&4CY?eVU*XT;%#i7 z{pQ^3u#xTFXLp~Y^fz<x{^`kN-=TK$%f^a$*qh=P7Jg8weFX2Qn*cd3_wij@2(KSM z2VP-8cusK|Z_~t0dQqh*Xc-R2lXogG$9pktD1971;zX1ALI2=#r6lxncM+QAv=6MC zC&_=Q`Gp+f`7ljh{%FgSBw~>~7fwdjkm#KX#N_2;VxAgCxo;p~DcuBx%Zh3HeF>1B zcZV8`)?z96=H%hQWBAb$bu8DZ4MZ0w(nGUi(fdDRK%vADzPuGeBo+vRm&^ZvHCB+y zOxlfC7blSCA|-f_@M&_^_B-S7m_VU(u6RM97c6+J$rT#h%Q%ESWU5?*k^S{Z+^&|w z++V29eR6&Y6Yc*HrtF(TN+x_^%eGihS8k4V=beMasSc=m{Ryb=S`Y5Cm?N%J5^$wg zlX;dASlF|c(Q<nN##GMW!1IrRoX&6Rw$Lmb6(5Jq3hvWVHj2#Rq8@OU<qn+nu|%2g z_L0_3DXe#>7B%e(Vf%O)@X_Z%5|kuNmP9WDt>W6ugTP$I!y<**RISNu>s`u-jAcN- z5*_4zNfC7_w-F`n5?&vh>^G*CF%x29Ov~>lU~8}%crCCJgq*L#+b;Kk4v#u~=ac|B zpY(+mF%cssmv>{4#Qi`}{XEn@)(^BLCWwRfLuy9DO4RpKmM?W=4cFfD6#Nh^N(8-+ z!R;1b(4vbz%!BNi{I&D{U}<v=Cg9_7)LuGFf}%#rolsA>-tZmSQhJ|w_)NmA6)(}& z?}wQ6HWj3F%Sj@lItO1|e;;I66d=cqDZp^#IZm0+(EcgkNQ{FxNhp(}re)v8Z%j_1 zewMy+MEWs#dd(Il^~SLGzZze|>^$?=<2K1`%7UmXmxM%VLBFa@^7+qlAZq!FSV_j> z$$)F5tWkgjS;kY33Om8%;2-*1>p0NFf2csVq*9Z(gzP*jP8RFTB}cbU@>~!ibDb}e zceo8}np~r<?|skmHg`iE&RN<sQWdrzSVb+kZi)q1v&0Os1SIr1o-s4s2A_HVMz6|h z(5{&>$ol9)#^a6@sXOe%xRzv+8af9d%UpC(xR1QdUj@Yl50TMV$B;|oUv$U24jm9T zBDzvn;Pv+`3#RKOpz>xQ&#`5M*N<2>g)wnZ)`H4@;k3**Nm%>Hm8e!uCsW8CPTR2s zbiG(Z)Kpi(b*mj9{UH;j4-C?)JTgg8{7!EAgFn<<jq^zOaVhe!>4JyS4j}D|ud!#9 z3tC%ijiR-V!VZ&PAfZ`}%$QdRb53KH5HOvbs-K1m21SvKXD!qSmF2X$%Tk4@@+8Dd zok&$F!8<yID0;vb0@<6iYKJ&k@Ff{!^v8njs|^UYKT7Kf9wTDMSqlN#j%q)@MYr_K znM>L<|FU>F_bob(c(-M_&koMv`q`gBh4&jthG;X2vJ&T?n+jpF<Yl-qimRE_dUfKx z@-?r1M=Uudc7y3DokEAJCArDd>zJf>3TQ_75INAX7%oblPHdedsK)Dt@Y{P8*q)+7 z-ZfS7N-Ho3inM@*za@#SjT=4bkq=}p9Aa4r3gn+#G<tWSh#_9`pjl3qJO4OBK_yp7 zi9!O@oUaEz29M#kryrrS)pOkRc@r%C{RltPo&_y#bz>RMR(#L*GMwZIAUx~|TlAH2 zz;R*Ljp<1Jld~nsJ|;L@=qNmPs|~EEwj;3gG!8jr02?l};VVJM04|FIU3b;-wykY+ ziE1;ms`4~C#+oBe`#)e}ABr-es`<dsO@?pBCadeC<(YIFHAY-zI^Xn_G~Yk|AL-XU z!#dl&NXZ5hwDzzPUXyhg8mx6?8otFd_i_RWzB)<fh|FVxY@|uOixTuz`+=W)(M3yc zzXGA@`$5M1V|cvKoA@XPQqcz&!$#2zoc>k?9$dZx8(K_KahD3f@)4FwUig&DnQ-Dd zCFUTBS**39T#t-Qo4`$Ddr-W;5sIAu2bH{Ph6{BbLt(pFqzUwbdFF`Z8TOGl6Gt>R z-3WbjzYE`dF-Nnx8|nM~GWdT?W&djq@}E?8P2fkIHZlfy>oTC6T@F6eaTO~CX%nf7 z%H-Dljd+7)B@sU70uOBt0QH}iQ7}La^gFYf9>t&3be}f7A~qMCQ}3o_QZ{45@_S&i zU^?7zHxa0=Du)wA*+jBtBb?lrf$!>_#Ukfk(;<(fVWiWp|1FjMf0y^)Qd!rdS#TGK zBom(t$>1+rvN|%FZ0$TpGFeoKJ+}rFeHDj0IK?D<{Yx_T+XD)<Zvq(;ccE~a1<bi% z4b9cQ(Lq)cOkZFMiyz{Hf^z~W@pLLIHJl5p)zop)`~$FOS}_@0RD>TmY5|4WF;MZj zC`6V4B>27<$6@##kma2L4WlQClE^X`GW8i!b>j42@j!T-lSAr~&%?bF;mlKMHE`@- z3b|ObjyBr12!865LnbbkWLTmC&(GQp!xEhuGUM+No2pp$m2e#&Uq0B7{n?v*I533Y zp5kJ+lR~5>Lx2&vK>=`n78A8&Et)4kNkozsz}lm7B*-U|%y7*hPYWo<HEIj)O0R+I z#G7Eg`2ysqiiq&i0!lSP8RondLTAH*Nyb1Q@Le87>>sjI-{OZbEI*z(9M?-)o}VZ4 zroF%~>kAR5@DTBvJq@L;4<OAVX(amSb+YwW28jr`!ZZ8*lKyVoikIlNfu_JrAUxv| zP$>~2GCHToMn_%j>@EhM5FR+P>;VX5kx(7`i-_ApU4r)Iz_RZ*@m7B)qBi3iutHW~ z%YyCTadR$w`uQozTk?gP3P{1THa~?u%ciJM*#f-la|~1~9|y57^vU`^b4jq=9hf@D z7;XL(1|Pp=5j8vG$-i`UxLAf=dNpsSLce5TneO?l81e#`YO{lro7~6=>0>19p%|>l zQX(@&%*cf_j2}C^#6iLHafh)Td(3%5|G4LkckmSPa&spJt2eQ<Noi>A7*E>xQ~2GM z&F~^%kya%D6(ld`)&7h^`@b@<rfmXm2@^!t$$c<T{098HLyt7ti4aYrF6eMM2C6(g z0Lxo=Af_$@zAl|)kz4}IkLbnlRDuEe;d399Mt@?LJYLZk#}1IZRz)<cgiE)aJkAl4 z3b!RkhHq$Tm2t`xuX3zq#W~^`Upf71PMmK>alHMHZt!w{9^jmsGU5C-`pElzOUPF0 zt`eQwaD(GLB*gO^m!_^N9O4*lisLQaqsl8-qQzsdw5?uqBBw~EgSV|Yl=Jt{DV}Ff z2IugzK%Uutb4v5qRHLNq0iIvZ9^NkP^Sl?g>Uj<xN}P(zpLte6-kdFkX&mO+vc}Di zD2`E`IFH}DgwtJ9VVCIC&YADNjGkQ}Yj>(9koJgw+E~`|&yKm^!@0g7uaTD%(3o5? zjrV+SCMP4dw9(pN2JdZIZ{y!>>p9C!ia?KRI!#{h#W~yG<C;(Q<Py6mw=-K7d|hZn z)Rn$sHHW!4K;a6wRF{tr?wNyg?q|S%XcIZCwHDg%h`{>NSAgTpvt(s+3K3A>gAdxg z18>*=0r^@S5OJa$4>Jqlh5;!uqeBp8Wf+maexh(VUx-nikxrbiTqmoZ7<0^1wMduR z6M~&6IB+wGdOlPKJ{Slx7HeOU8&+B*r+WlQ`1XTk-h8@rT@!c`|9>$dlcf9M4oXL) z8v6eg1Z9cl@Vd@YC~w6F=zI=r|5Ql28*k#58}f*wPZ*rORFYMHpGCsE!r<2n4}h%C zJ+f$r0<-2EhR*%xpy!AML@#n+$stFud+$xy`}`_6^o!GY?@lF{L&>8RK^3rUZ#G0B zQ`pKck!S|&A)#kt(dt+|^6hyf6&P7ejwNam)r=0Ze|a_K-1`c4wqK{OeidfQvy)+g z^jVPHe-gyotDr5{&C&h3T*~~YGD^9&lEt!B5LtmUM9^y;cp<05R6s7XCpd@|Ol3ji z)Wvj&^G?vR-iFLJlxlpXUO~#FPUG(NPpN~30r34-H73a>8%}t7laY;4Od{h>!bcsT z^Ph1r%CE9p_biLIePRu#+<uDER;i$cH{Jrnj>Vkxlp1hcadV^MAiuF)as%~ZFOC1+ z-Ea4&>MeHISx0T%a?Nf`X+KrEx1Hl6iAb(x7)F0}>DKM0oCV6lbWdOgr|iyK-oC#T zc!uU%USsVzCY#UGA{Wi@BhLm9u-<_7&es4R+1}&Wi)obA%|1Hwz#u3-Sp(WTdMIHu zLitOm07->2p#4D-CA8%OeN9&t9D3`<iw$l9rC1be&OCv$)NH}L%^T^;ZDv4bcmc?{ z{)t{&TtG+uXa@2HwUl>@G8vEw1rvwEsf8vD_~iR~Dm?cDUNt@oOE>!h-6O5wdY&2y zoNWXJ7lq)f2iM_W+qL1ylsQ=@_>s=|`G<PdJdJ35zfZN;tRWXK7r;64Lm+d)4~yK^ z1ohv};;NKC*i~T<I9qs{lBU&3)HsFzSrmemjjymy$x$4yUXESn=HR)$Gtlx~&ZO`~ z8Z{QILQ1s{Q<WAwi14Ykpbzb*K21*18%u@p^+n=v^|6N-xK@MxR`syWMIAIRih)|Z zLqNa!IIr>6Vd#-{i$40`6Sh1x$rFq(CNs~U#<zZs0KK}Ow2to?@O=JKvLbMT*e$Do zi`vqGl3oG0y@oAN8Fr8n18s_DC_w&7eF5G-&EYMY#U5Uk#T#~qlZesHR6xoM_$j9e zd#Rg)(|2`X$51qg`XfweX+IFcZG{JPb)n5)71Y@GntWPkP35hL!&+XAV87@n@H|rm zs>3x<%BKW;Bgur()KY|Va)jU!*UxaYct7la+rfHRRY;7=0O`wjf-0LngUj;H@R4~P zakH$U0+##0&i-Qj<<vBM@+fN)yB3cl1cS)TLK|i(?HaxI`bkFZ?|gv122o#A70507 zeW2z3L!3ULPn`J@9OaQmjkm4Use<WusDl%yaM`U&kh@Zt&d;?3+3nS|kzfP;>EtFV zqDF-%%}wDbnUr7&Eqki>hynHZSRvS4@|7o&IG0prU&GC(1t@x!3-$TZIgXX7UL#vB zq8oIJut#JxU323-7QZ5ar>+;$ksX>Kb!R1J7i6f3<n7d{*4g+JHxMWJM}b+}J#cua zD%kYL)z)fv9=*LJ6AW1HrHvkT0$Zn5)Ex79cv@8jFB4w3Yud~O_WK-h(b!e0DTHr# z%~BRL1~9e}at+j~3K`gWt&r}zrA+vhyQ$;pCDg)YLf{<#49MO)3-1^_iH}qm!a3pg zz%gYy*o#lW$Qj%5wLKrGw@?m}>=QuyoEUw~BZ7zyisH*bKS61_K1q;k1lu+2hz9i- z{C@I+jfpg9%fK)Ao_jAH^mc#>mU)B^ZN}80p_$Oa^ASBHCQJNTMuhI>$GjWIe5i!9 zTa>Tk0M9X-1_3RbIFFi#fl5{k?ee~cvXC^SZ-1YOZ&oKYN@EGUUZ$64c4{Rx5$^#% zm@Xx4#;Lfs<{rLiu$_8-eht-@W(7XXImYW6)u7AOJwV+`U0VD1B;b{AqJs<W;8_V? z;EF^l@HaGTJP{R$58B7zq-s%WXvWRPf1UPl{D=zsTxx-w)vLhkdv}0Rm<|YKq``i> zi}=A3W9-DrA1AGIK&{kO{P&L>UZ^F3g?-*q?y)quYkiLMV|<u8%w@$%%U4kyGpvF7 z!JE{Y&yx64fD_p7qfRVi_EI<droi|YFDP>H67_Z69(LTU!0TCDg#YFk<0f$)5Dyy0 zi|^&(t#LE(8HYRIl3y#3Ytq9(vRm-_md)_s`8KdGuN7n{?8UaJi0u7sOM{68`1c4) zeD3;0N2`Xh9cN=q`Ksdou`Byu=j{KavS|lS!ZUrkSOF<>TAv-pMk|Ye#Kt4g<U|$y zCZU1)S0ToX78t|n>R&nHkO2ZMZ|Q$s1`Y8WJiwW-KKy8xHZ&363jf%t)1Kki!IQgJ z@m{k-^iuDoxLdpZfAh*(|Np71<eGW#Z+R)$W9v?i?4Qf%{tf^mcP+6{M?O_wS4FGb zTZ9+qU2P2b2ni=S1KKRLXU?fPA^o>^N$IJB+~xDs_yetJ<eS7cX4Bds*uQ2ESU$N5 zzYYD#WZwQwW{zc%Aomkw@t-GT2#S!1n^y>Amx4}gSxr>s))LBOEwEl755?>Mavlk` z;|0a>V7(vfF{(dJi)MTQvp)~8jyNvdS6YoNffo4aXAKlb7r=~&8`Pr1%4m1V6|#Hh z3siXJI2H_WK@#tbxf6cw2rZ(4XulA!yf+NfHoF6z3wZ?IOl1<6>>#seD6;LdkL0b( z7+8C*0!zrLGtYcHiTpP+;`(SNV>ui~&TVsH{#^2fLuySx@=6L`DK>(WOVmjFl?ghr ze;J+^Cq&3omQ+m4VBbQPz+ZBfo_Lr=g^+i|-7^w7kpN^Xdx&fHyMQqrTunw*@|et} z?nuI*n}o%T(dDN%Fe!eSBzmSM5&IZMmS0n4<t_hcQz1Xj2V-lySEoJ63m^+--89Fl zH8!xYG8=4vSqyvYT*#0;7tXro53#~!N?kIJHvXXs8#e=3`cas~unK4SDI2IR9fTwL zmEkhi(`XO-DK+a%4z!tfoUig2aThJs;y*Lkjn?eD4QAg}N4@`+F}?a+L`zyz$4zhH zhqA2mELjwSw}q5=aW(94UW0`a^WhQ865yY<gX(`Y6GqZ&IQar+vAj72Qicj~#x*Zo z(yD=S8=p7Ej#l9Jt9RpP+5e}mGY_XK4BI_Y=2?c2OqD4a8rbW3mr|)T2qi<5G|{9v z&BmxuXf8#iNTvvTJ?}yyi6*HO&67|PP2{Za`p&t&bDi^jfB0uzuDv(5wV(I7@B4Qj zp<h0Ekh=vr=(OQ7V;wVxKd$V<YfZk&ZufL1b0zJ`py4%S+MGz<I3tD6yK{`J{OHOL zS-F-}h(7Sr;rhJ)QxpDL#!h~Hf-P~cNQHS?Q|Z?Hvmkk+hFD)@c<B;tve>6mIHHyk z>Hg2uc-RYwc?b|XOo@!uwukEj4E5b#Au18_PX_bPamUV%qix<{V18ExM~@FAzC8n2 zpwdM6Z5P=Oxw_6SI}h_g^6q@s=XTOOTb<pTznr~ubv-!}zmhkWd_&jS=Lm|?6uRtf zBFG50Xgp>`W3-~_N@)jFn?z{sC^d30S&4r#Xg}#od4&tMeq~ytFNx++9cr`nFZuVu zO&B2ym?gigpheiQoHBU>HV+OkIi<!lIek2F)==Wp<)83xLeBE)s~XwypE8C1V>z~f zQQ${q%a$KF`jZ+aIYOD_8~%sAjI+|vS?tURmba-9j*@|8Y+T|4nvp(*v8!X)lak{I zxfqK#jGxfG6(LT;l%<G9_(l>o?HF{teZi<7FlNsw+=eY}`o#8HK1>)mO@HpQA<p`v z>D|?qIB&#LBBhqdV}v;?H?^2Z=J~RTyHt6dXe0{{E@Xd(`k>6bG3>PqQ%L541N=JW zA?0fwR#Wq#uJp~V46LfHqpmX#5+9TA^!2+68Z|18W^6SeaV1^U;Giz=sdtE88Lv*7 z$wS<#sZT%C7kJ~uBP<Gkh7LzW^g^c4MVhc3%qP9WC$5TggU)K|cKsYanSF^gw11=N zZB4w~_7y~OuoG*%)sa2E+>JGv_Jt06Y)%KCeTHVQ{ovl8Ll_nuk9zN4(%j5isQegD z3U*(IrHMz$Ud?#=;vLVtACU*u6Sd*(o8QE{?l4}~iR0=gcEjB(Tgj{-Yuvo_F;{PM z7iJgR<MMwVG-T^Y*r5Ill2>(cC)~vJ?Si9hwXlhst~av$poFEfCEr2rhfk-O$5UA4 z=Wg`m-OaRM#C_(?Q={_Y(X)B6`ZRW$haTJQQXy=XhM=q^0V(pHPTGBqfW}hl*z$+u zuStb%PCB%tcRKSob1(C0<Xw2X)D$Kb&n5+J1>hBN7+$N6!EcHw@G*a%NU=3S)W0?m zj9WYL)haK#(p;9G;lGD9_E+XNcPFq1d|z?<RF=_$7K1R*{1`Rdbbwv*xrlAq8b-7x zjKKBwQAEL0iGGVnCQ2%TA<{x12j=wRr#EtRz*&n;iR&R7Cbp8T%Nt-rfhAnOFoK+} zRO8Fvh%sQ9EtQ`afJf{S(9mWuN$-?ko(=fJ9)+D`K*;9Jd%Ti{_bz~E*`Cf%hL<{% zm}GYQJ`Xbd_(iIqX2P4)<%&E;YOucVzp|Pq?t-(MEp?9Xr5jIbv90fqkj0nI)2_4( zLfeHrrIZmrZKWNp`n{N1$1Np1ZRUQd`-)^f3?rl7n^O<1=`_it1OA)^yfgPDH@%)= z!lzp^xA+E3|2URp7hIu5MH%cZt+TMiJGOkJ@i}&uUktl@&rv#RY$iK@e?DqG(qP5? zDg1-GKl#XA!}!rZM$liAQt8021R5~A4^KV&%}n3L(*+LNG)K2q@K&wh{Dq9)^43t4 z+fW3vlMiyizC-Zi7omfqKM)K3mkCGhD{#;zn>iTRB2pL~0gbcX;16~^NwOVZ7G!O~ zS`}Nc(`Kacc|kMT7auLz$^c7#Om;ab7$3=o$>dX=9oKpJqnT(wCIo!6>u7TQO;ouj z&Hj~*rwjIpc%xxD)F8s08z-+o>cw%afa>9UeF|u`Oa;n@zhbJ+nZWqFlR;}*511Hz z!E3gz^bl9YdDRRdKF$B|tdSDN^{UW+>UQ+{un6|b-~qm6cO{#Uoxxs<_zpAEREY7T zCU*KRp`2xq7agm&g_i!3EEgk7?Pd#9*VHm1DPK<azfq>@!m7{YM-sKL@f1fF#M8!} z^TIkQpR`)`Lw2quo)7%UtsTArIwsB~4)g|>9T)*MTG9;r{v|X>_k(rNRQlzg60<Yo zpEyEtCNH6r%kQg8Am%3N&bbOP6y}uDaybS5!60|u?Rg&ih<Qe=zqMmt)lc@wf^+Qc zafUQY^&WLo<;gpV&mf&6%WJF^lZ90Qv`kpxU9_);60tKr3O|GqZ$9Gj+|eRUFVnKr z%+I{O*+OE`VhGfKBpuVD!^E%AA(<LooQ%^5T3ICI(4m>cwLK8J33KV88D8b<MYiQ< z)Lig2-$<St>F{p_TwO=h7dq!h5!t#*)%n8cO=MH!2*~&*0;N0dB)IK5EH31rA~^%& zZXKdcdFSZk0}d>g8A%u0e!{GU0W`qLf@t?Q!Sb_r_=@v+7@;wOtf$kQT)znYq|x5o z&g0tXle7un2PCtfI>h8;1F%-s3M6WZ606ruo%7vxu)`LH5c{P~q(UwQm#Qd}ni1|S zE*1#yH6!@^!Iz06(Ps8+ETavcF=TO|4Y%L7ica~c%3gEn#<4#qkl1~LsEiunz0ceR z?J*kErF9$_AGykVyznQ>E*3)3%3yTda306ysWRIghvU>M5zrCPNz&|e`EUJ)NmytQ zo&0&Kb8Vx6vqfS8|8<-if4Ihk)4cPQ!q4%<&&jm>=PX@z+I?d_t9&x@dOsoCEsq^E zFM^+SREu`Xd$SiWj^%B&_wgzkm2{M7CJnQEOb6}UkFn2;s8q=my5@`zJrVkjyKpR( zu|J|oSL8*2#jjDMOyekw{<Hx<tan89rTrN3<peRQOC{?pw>Ss+gtCKmPw@)3B1u|+ z6pQ1B6ZLaK=1ck!?dvb4kK5fy<ID!;@Hi)GH1s}pD}G=y{5Em#zW0FdAwPOeECGR2 zW|5jP!-@6cB)HaK#GSuugg@*p@Y<6|;(v;vle&}P%-ATVKgJw;=C)u`)_cg%%;Ii- zx&?Q?TCfvs?Xbi%p1s+e%lbXI$Cu}LJAcZW#C|$Ag>ph?`?FOl<U-6r@<3|{ee*|& zehrHT_eUqOU3V6FA7VkebFVU2zaJ#3e;3dNwOT}5EKTczBFJV4=azW=W!^3yNmCSu zkPprO$daYgspocix@M#v`LVte+mtpyM%Dvx`4$hWG^FUt{&pcdxPy(Y+sa?Jr2?#M zIQh#TAiavEbhB3}_oaRwO=3cbv4lFAarq(*I&+8aH1?#KWm5D>P&J;oyNVQl*h_5J zZNUNwGkRog1F@dC9}KpQr7!gz;PTgM=)C-pY5sSe15{y6OpU3x*)|M|jsWjZ7wOG+ zhoRQ0)G6|q1RefsBzvYbf_C#q_<t5*#9-Mo_I}@-@^4d)lSAntWdEx^lH#JruCR*3 zfk$5K-+67U_IM?BTAV9$pG_fYP718wvk?TIw^GUB<z$TIO}eS4nK`yLgD76m#IDF& zMD$97`ionb-{a1q_SzycP1Y2b&JH1WKOKXmg}a&6cRt|cYi6WjLb^CHHkY{@EX6x? z684#AH0odVF0cG7&1-ktl=~~q=0oL!S%1kfq}u)<e=swMzrI|Czx1INpF%7ug+2zG z@9MB@yazx@Bu?AnK+ekEgg<dFz+deuYz~Y<=guHJcB_DEx2^!WA{lCbx{G$bW+6DN z4r&MPp=PflHSg7Bykd>G_v1|%_3Mcw!KjkodsR{7CGc||Wu=w}hco=5wp`-5^eMl| zc7STlt|YD#N3po8osXJ#pMRxmNYhfsP^AS<{HUXMY2=g{^iG&2UDULlmdDESm*-qZ z@6Q^X`~4ea+2F?*)vrT*EM9WE)=SVugC_FFwtfb+p397Fbrthv_e8>=3V9Lk1tHnR zu*lz&oLc#oujwQFg#38cNN*N9-Pn_rc4@-R?rP=ti^BNc?r-GX)Ho6ou2;T%#8-Cb z^VRIYc6+`rq!AvR4JT5|T<OM{=5)uNWqAC<VCd7FNxRg(k?$49;BlJ-^>$DeC04C~ z*Tw=mFa0`ATjxzipPx&WwT$Wbt&Pljom8xzKb0$Pa;9^pUgE}XGGV#1pQux(J2&x_ zE${JQFZ+4UJ9fKMK4eUE=I8EQ3*)}YvWmgRDF5g-`>QWiXrnO(Wo{u^wOO9k&a9;q z{HEY*{W^&G;y^^c0%r5u0|B`wMT`DqQze-Mf#Y<UslDldYrm$6tDTxa{GcD#S;gRJ zO?f74q!iS7|A4eFl3?^km-_WoldE(pA4MYg0O2I7c=<X%^JO<_%d%zb-o2tnot<cz zNa(BWzRZ593}hWto)F*P8;IAi^>pKq!{XdM`^d3fzqnJM{h-t15RTou1fOaCC062G zF7<5)xg@2{X~*1x!_O~p#c!lpn?E+p5TRJfR=ysOOg>4?wol;Rn$N@&+xp>j=6E`I zc^p6G$x(iDTb8Kcz)3b~RthhEwVyt(+sz1QBN`Rt%`S^%sq5>-{A!Obwmc#VJwvWg zaA+sjCP}j{?-%hMK4;M9;|ltCd!@imk))?%2RpB{lPIswPUm$a<f!l8Z#a>6#iZ)_ z?6f7<aD0Ip_|~RihkY<@J1wBu4mb&J5mRUyw4n5K+8#Qh{sBMvq6OdhO=41{X$yNh ze+@ge{XR*}lHiNHB%A}{pOU(WNM1A8$XWZXn00aL!w0LA+0iSj*#;*d<C2~EZ8x8@ zdAHB7D|*xDD*ryXZoZuEbh}6YTUGYI_8|YM%6{+Gr8z>IL}p+OPXG8G@3&jhk}fG) z^F*3<Ncz*)rvjNG+fkH69xPip;jj4Mi%9%AVGy|(s6e|))wx&sgK_qjX>{GfH)yt$ z;kp!Gaa(6U=kA%Vhwm?@)8<J^OpEG2xN<d!e(|MZNofT<`E3gQA9`i~&&&J2RN3+w ziuCkqU#esfLg)5xrb|{-lVMUzV8`_ixaeL2;*}H0Pi0Bgc3C`)^xHuq*Lac0vOVzm zOCE0YR3aTVrbMIq9Zq$*$Ry<}(mSovWVB`(>C%kCD!F8G<D4uVWM<9|R9i9n7ffMo z@haNKD3J%UUEJ-X=eR#bJaq}0%i8RACFWipbk>^VBsWzKKWv{wxxPYpWPK4AzbVGl zWdqE;!Rxv9$qbd0*aIi$7vj#Y6zb0D!!6GSG&u!yf#@2I{k>XbGiL$$da#z{)R>Z= zoi1EjPcUiU;D(ld*)&r@h2NCA8x&Ver+W=&pn2jxGJ^^&p=a(<BlnFsW=<;k3@s$0 zb3UDFd=}R!<`cOC(e%Ta&E$FCZ~DznmVdrz9eZ!tIzDJt7_F)K#4SypLazO}L|k9* zC6|u{kY5AN{A;1seK4a&G%7#QfYHyu**$~aZiuIHUIyf6K$2)`a5|9~KbTHzJ&dA? z7@BG}i=KA4Oww(Y>C>U#NRm?`H8)Nmmx{HC^PaEd<ED$8`^ukqTsE2vuDpy@QUbYl zvph*BMX+PIAzPZ%MylPDAW25}=nJ??4P8%j?T_X`p4||>qez1G<4bD&bPOx=a}I{w zC)6-!IGIrY1^g>)X>9LYYSlOv+eggDotBqTvGpOe*FV7r>Z|GW@AqKjvRgDH^%und zhY5Ef(#V<;y7IF!y2}n>Pu6`QdhTD~K}IsB3ZwUn^u_S$$W<D<;}`w(!jhkG!IX|z z9YYK4ZTO6X$~@C-%MNH=#SJCMT#HrX+jgiDxzL$x`fhdJOnANv`BAXsmLq#I<``Ah zJ4|L6`m)s)SsXsNiFZCyV)OYl=IS4%Up)u$S)YW+$g~nztD($3c93V+&ko{Sm*omE zcOft_Vkj{gbC_o5{2>PV#dPXGAjC``K_AVUPfs@gCiZ^k@PeWy>$^6W@R8AEqnSF~ z4AWp~-6=G@`~nYeNCf3>7y4oGDIsT90s0fqiavT)LwJM)JN%CiE@dwX`MuTjy_qy! zCGAMsW=rt+&78IjK1rkG$B>fEdx?Q_0M&4MK^h)c6RmwOxJfZN<lf~pjyoIx6ORS6 zjweQwKxZv#UQxhxZ%KmDXNu_gPe}XL=<%Z_T_&vqk<{VRVp@OSiI01#%KmH8fqrWp zwmb_-oK*ll_PG&d;<vN&eqJVBjvgdt^c!lvT?s1d#?b^HBR0v2(x@jvB)!y-T%jwW zpiz?7yPHaKR;t38ieA#U`Z`_vR)_VM_U5%E_mhWr^oi+}W_tBiF6#G<=BMP`1ED{d zZK!M$cPUk1$b>ZU_|<t-ck~ycocCRvLmfos&cV#F)wZG=wHl&dTeHR9rQ4j!jb4ft zud8zMo!H@|;yzt`rX{azn_05>il158ysL*r+`Kv`W#j!$2aUFfRBTnn8V44MzCR8S zB@Wn%oA-u0g;w7ZS(JYiS$a!}KmXXxSPy9yEjg9wcyinKvW7N8X3O_3N5$nj+|t~n zGSdt>rg7*uan*BKQTd|<Wh?d<aMsbWqHd>tadhukW~-ALvp(Xecs;YAbgfujtm_r+ z`14hS_)lD@c-+s`OjlVnv)>?D>_7ONQ`2pK=E-@6D|0ntM&7p*-<&;#arj&-_D)_d z&X0c#KVx5@bzn2M|7s5;Zp#M0c1;+&^eL{2@PyBXtLVPhd+_k)7<hZnh9qAa2ls>R zAl@tq=6ab4%GH6ghOTyOU3~+di_+-N^Ea7zCLDK2&B08OKev6UB4@b{x#v3_X}^;a z<W%ovjQ0IxYBCim8>WMnlZ8IkPCLl)+s=iXXpy>O<3wXM0&wlvN6Z!dAg=1qJ}h<W z;kLZI#nkvrVVc$t#;5&>xcX~1Gv76uGwfN)-73q#`oZed?^9hFcl$LHc`5~-<h%fb z1+uX0h`gZA*Mr3FTaFUtR@|FIarjGqX_=Sd3cU9w7dnI{_RHct=H$J7%zAHS+<3DT zeOfYcRJ{q-7pMc%p#~#vE{3N+)QRcTE9mh(nzM7N7i)?4bGy0|m`L9y(YgU?V!K|Q zdHie(7$4XJMOzZV<A({9o7#~!KT|aGAIyD;XUN`~0Pg0=VAAMmLu352ne`Lu!Jt(K z4b96-tryQh^`TZEiXBVWZV}_HwG7QVU4}_^OK2)x#%$av$OOe7MA93^Fz=Iez<zor z^cR1@kzO;IR=vHjBlZCnia+D!t|IhpZ3c@UH<(MY!?}C%qv?a&Qt0#MDw@As$zAWb z$K4LOkF^i-x#{;Rxy37#iFx>BwAGjdugW!$pHK#ap9RB$*IKYai!fK--(qgcgJ{jd zIo$AvLG;G6y#glZD`w_tGKmu=!=`1~ut3^^`F*yE>5x~U{LDqn=b~9$;jezKLW^MV zYYkF7s0B{Ed%~%1J<I)aSd1frqq!@Sit%d&Cw4rZhF`~Q7J?>XP9rdtlRF}d&$bmX zAKUGjo}~kvwd^)7`2J>i`%DHtrj7#1kr|@)3zfnupqh)f)GM2MIv9iBpTWk^dXcrW zWEu3xkv*^0!hOROyzy{2y|8&Ad~`P^HNyRLcJ(lkW^V+B+MGqld`DcJB2RyGO+b0y zPG)sZ3rrQv+Y0tkqWOhm!QkUZ=3BO~&f8)M4PA9y)}K@2e%-S~>*F}ln~5u!2*qn~ z*-e3cT%d>=%L=*KH|1#k+FY)xS%jxYCBm4K=A5i64?$P{h!m#(=61<^68|Z($M?;( zB8MfeoZ~17GF$N)Cw1x>=TtWc2i=&#?7q2=yR~6Hb87zr%s<kCiz0=9nNE?@ad#79 z5|;q~PTyg=H-*5=Y(w(4%bl6ob6m`h)+4u#Lzu!xa=37u3{j`&%f_W%#mQ>TOkLYd zJZ)Wq_eHaqduxTY+RPrt+&7QQjq>2eF8T_7E%8iV!e^%q5;7P#BwJ*7@*<OP+7d*+ zi(ypZAh2ATiD{2R@dPIeGmc#1;=bmHX;+Y=VR<iO@OK35t(V5V!*>h)y`eN?V<cme zHXELdT8|t0vf#Gj3Z}GXD0Q}q<${jZ<HHr*qLS_*utGr1c?_*Xspw|W&q=#MX53&L zweK0)Tn#3<LJ42@(>YjiX9rn*dKdiNv=^=Zrook`2Z(6jZ^6Zrf<qt-?AEL&b|*sc znAS1yn5M*bT@_p^_cMfC^a8>+m{2eA4AN<uhJlYFK+$S5%2&wIi*E~=jE-ogdwv|r zF0`SJMf+&B<1^e}tprK)x?$D7T{I<G3dy3CAi2>SCp31#;azj-QkmOAcrFx13T%Z@ zhv$%|?&DB@W+X|ljsl;$O8VL83BC}elYO>EB*<2mZg81L=T9^wyIvTOV{v;4`>&rn z@HU1ndd$(8TeJlurXn>yc8~}ZN4mo56jgk=fw(SE6*_<)<G=dzfI*kYLaBW)_BtgW zpRa(+XU0R;zrRqsb`-IlD8pNu{D4c&#_ZYB4dm=JOX}sB4iN{IK)-%9{5kxN2H7T4 z=uD+s5_L)SrRU_}+jXp^?jDw{se}DLexZ8?OF{*S>cCrPUUZ`kGSUj6RBJv>DSkvZ zi4-XbWT~Hl&3~Vj{jWLuKULY&Zy(^&lP&n_sS!CmHJUu?c#T0@Uqbz+Gi0NY1u7~l zkbqH5qMcJy#a5Qfd1E~buE}C39Qk%0Eq1NNvOQYNo$iawvaL^<gliYTa&i?e_%)bx zMBavJ{tN#nRd&MvugXSVlmplESzwji!XzAO!jXse<3Sin`@Q6ljqU-%WHa)$$(n9B z+KX-GlOb0ofIhN};#R~+)5tP4dN2F{PIE})_O3dEXA(<5Bls}PjJSm5ccxLZjl;+= z*935~QzVNbbaB>#KM-pAiJQ@P2rudDVCK_{@K4==_&dswO+j~H_RV<a)Qx|zZc74K zn!Q39%b}z*{Rlq!s!L7iWNMKtjQHWBNy5y%==nLAdtXyjicvPSeuM(d9To!B5AOqI zN7Gr18>u+B9qr!FCQp1@xeKQ2Xw}Q%wC`&tQ`gi3D%v;Up#MMa$yEyy6!{+KySQTd zb7kCaIgPGY^8>3vzp-td4ee;QCu*jn$j#_;(62L+-Y=d)24{{ZKUH6F5<5)kL6@1L zOO*#<=7VIcsCfr<yA_FKcP%V9RVd0lHjg}8;789*os8*AY{}@UIhb+zh4{XH4Bojm zT-X-bz`&QO^xu$8@b7mj_!kVKcM7lJ$l2pz)7ia34`DNU-<BY=u2WH~!91KIP@*Cy zS;4~(Y22ux1~~LnBdmIK+0p)OE%vP|fzsG(%;Px0L}7FWhgTSqEyb(hoqjtkc~6;( z(?<{oIYrcx@*%4dBgiBffNm{0>b=RHvvA5{sxC-V)yODvGV~-9KUoEDu4Cxst>a0; zemNLzxdN7$sglLqd3f0s1v0}Dxc2ZxFvO=27I+(q$sb8<%v*$oPuxiS<v7qE<qF-O z?=dMmjc}7kK4ZGtNBn4~f++EJE8N;H4_W*LT)i+2U-ZdP<y1xTBFz{)lYc<}JySUI zF&#hOx`3mljA2&NHZsmC9B-fa2t_v|@ZUyx((h9aBmFMJ%6n&U=7nT<=KKY#>*a9x zX+>tlza;F-O@dp?7}8Yh1l#>ufL^Z^sGPCPaCJu-TGPpCZH{NYbd=%7HM@b2{lN{J z-Cj0BBMjQ4t6;S6FMK2QO#C#&ftqfqLN{Y7W@EZAp?U*uPnV+i_!czUJ(grjFXU=j z9!eAv(8nhS@?Xf3=STa*nw$$6EtC-CPK`sZpa;u$38Pf!VJ>uXE2jQaq^p$e=$-Sf z^keraJQ_Hk-U{EuBs|Q7)_Q$d|N0q@UH=6F6PJN}i!q)Ux|x33(~GSs-vKA9(94fF zX6gA=)c@RYm^1SdO!zkfYt^UIqj7~0-V;a*CahxcwlcgsS^$gv)1ktn8*NsuhC{O2 zMDN8Mh&hvl^~*JgX{QtvK7Wkr@<)Kz7L2SXQUv4CQ3%#zsH3(eO1!BAjh&rvV%#@$ z-aP}q+7H2n6{f_nY#;7=4um`DD|EA7g)v#C_%KM7sBV4CH6PQ)`-{rZdfa$?Ggt}> zb*ylkg${GgOr0LqiDQmD(1Sr`rdT^Gnl9v?!}DjE@H*H9msuxc!YmasadsD6YT5^C zNv1SO-wcjNs?hs4vY^l*8S1CTa_Xr**j^~@1g}(4epDEk^bRA>GTnsQ^yySBQHlvD zHYCAb$OJ7^rK+dTV%DCqWbXGKw2igKw2R#s6zhcV@(1vZW;_f!D->NE_Q!oH^_V~Z zpJ?XQ0nyphz1$sd9^0q2L7)AbQr#nRcy7TtD9Y(U*MwQ5%}$D5^sg0-c{_vlR;iH# zsYb;1sUux-qYNFFp8~sihFrgmGFEOG#9Uaxb4>wBoQiW1T#E64%{lks%wU$A<vEmo z&Hn{%DPpGOu`sGDzJn57Yp~s_L*2U$!2^S@P=2zBk#@F$JAQ+Nt#}BW-1L*#_G$(8 z9Y}{odh)~~FdgGhc0=OgCc&^B4cB(=74nY_f`cWFYu}*;TgNNXMIjfswP)v2J@sZT zJH;4}hJ>L{_#LQuH=VZs@T8~DMo{m{uUrKqeA2y%fyLGG^lkM5`hH<Dc=-B2${B%d z>G}YQrrOif3cjN5Y4+%qnhYb#&B<IvU0OVLC0KhNhARF5_iK$lZH-o-v*{Dz{$fQ^ zpFP3o;3v4WGY}RUC}7h{4r-^&hX$7jOml0&EvM_T-AkV;B%WmsTLPYOHKd(3%5<Sd z306P22v&2`#n-;y6(#;rCUfiy&~4mwQokmJIp$x4w%?vXn74os%2tJe$b2+ZI0Ex+ z{c*|EZTMt}6rF8#OYA3afZ_Kdz*o(JcqvbyGQV56?8E7d*Rpf)_Phh_k6}1HyI9E1 z&&H~?Ga+ivANbrUPlO%{s5+Pdw+wx_ZNaCox~Lc85+vwIw{Ey;)`?BGKVh77B>Z&J zVD7Yi7u|TSPQ*t2sJVO*tZS-)$Zc`>D)1|8-YpQ*J%h2r)Sd*~HitV0-D$L*0-f+z zM26{`(&WGsc=e<fIW$HW_&0qRDKElZDY9s>QlJj|?1d>0w}XC9C)|{r2P)0}r2d5> zX+GhGDnhpHt9T*p(Nv(-!-O&|#nYl8hQq0*dn1}w{^a)6SHaC@OAIxhKwC;bL4R)) z*lP)H5uMkNFtZlhR%y|rt6p<E&iv$loph&DyYGYAI!$5aRLH!V>rZcf98FVypBFE` zFq|k|DS^+5r!coF6WfjqhY({&`1E`RJ!a%Vb}DQa`zB>^Zd(!O^`>L~_WhXfE*sy? zHl|O1jKfnII%La-T!9?j4f4AUMDcP@VM|9grdQmDNArVm<GZ0`XmbX}sHVUa`CM37 zH<UcT(2mctw!pjTt&o-WsjM-`fv%5UD>_urEe>3E7`I&82;=e%z`972UJ<S2W~xje zg>oU-w)Q!?w*SF|#2DIcW>5A_+*uZD6HM>Lhd^tnI(430g70i5k#FJ0P}R(d8gDQV zejizIk98rdr6$uMawAE#st1V=I?P$*4#jIdlj!M5apGHnceqtQ6v)xGFsymb(%{=d zKc4+&Yzhr^Qf&W?gU-E$uJjKu=A#Is@;1?Ir#pf{R)!XT<<O#JBtB7k1(SuJPvw0* z;~~F_iq`~@!X-bT^sNU;d;S)S-Yg`SjE_Ks;$V7v&IqCqFoQGyIF0P;nTHn(G9a_N z37`JSWF%JVk_5LjNS6;`9u-RAHP^xTfF8$yKa#|&ayUJ3{t0~aQz15EQ*eJ;30T=Z zfY4L5;zT`9%s%x2A2fOsmq%IL)57IM`n?Q2`&*N~ZOlM4lZBOn+A(2ODN2oXA|B0# zv~l_j%3t{bjp$A$Iq1^$^Et4x(1oQk58>Y9#aJi(9qe=)@PP4jtab>-u5I%m7`(8e zI1h3<PceyWdzl>Hxm0^~FT9zSh@QnE^wZaeu;J=foDynDzkNMOo1~V)DNe|}nO39w zfmCo__W-5kgK@QaDbCj!SJt+K#S?S;@aa<*6esM!gIZmfw0kN&-jx9@cOKx~MNP1k z3I_Zs{otP-27+q_=6%Z)8F;o~&qhU%nKcv-T&}`NOJqrkhAk{GnnQOF6VWwMmN<HP z4|6Q#C%7%GhUQ@th=Q<y4|d)K$%Y=Z+FCfWeMrD3>OOGxcRDw3i*US|Q-;gtn~<G( zh62J-c>eQgsGlJQx1t8H_+-iac{G~Ld3YAieCdGA*Y0CPgCbrHdxDFX%hDxL3&<XC zVMXq`7Bn<R;nFH=u$ou_FV`W2D@DSIo?J|e6lTt)%Q#oTi@9n=8LoLe7&|wwq8(Rf z6U~Jc-2Aaq;oFi;IQ*yx@&_FS@%1UFz5f%=TA_pnTs1IBMsyI|0E_5$xG8ghLDLG% zb;yL=vOl<fNDCGpawF%=_Tm^l9Z;K4%IObVN1Xi%n7{}ldU$ywHq{;gzWM{?ZRm&3 z(WNl==rwMzlMX6;e}mVW+i=90NBHOR6)0+VB;f)w%w@x9awp~mYFm_GsPtY~tHRP1 z4F%Zcc^o>VucPZTS?oA^0v|P~P#Milc<GZ1Y1x_%|L&L&3tvf+{B|px>@9+G`YgBm z<Zx(75s@LAKB9kcEYrH?JG_$r2?ag+wDj9<NWxZ7_iTd`*$HrrM8o}F6}ayjhnku3 zaQA&K?p<6Dk&_gtyh<b1j@6^FL!<HPv_tUJ|082)Y)6fP1G(j)Ur{A{1{fU(W31Y~ zz&`^G;=Z^NT!i^h?ZPjpQ<_A_b!3XfxEVFq?SO}l=dm(Dfrbn&f#NL;^%UmW1-`R! zz)DcirPrdO{!aSy&vJTvc>!~5@(?QD6h&IXN70LC7Q>{(jdX-7Lnc4(#vcFWB=pG{ z0oi9uy$459J7Yc4cj6)B^(){)i!>bgE_C~jJIowhE={$R9fWXEJmmQ`LTT6##(d^k zCN9Q|geZ5T>IwxRT&+#dhpdFZYIz`8(8atPd4k&**bZQR28cp5mK{sN%Hj{uDp?GN z{C|sOW7g6~+2+JXAjJMmpGvMVceyq1>%i~ZM{vztMQ?Yi(cAA9qDR3aeEjN;@P4z$ zyuUxOIOZ<iQV$1F{xq^lQc%U0KNX#kyo#UxwZNEnYG`76AG_8Hk9m`W*eDr#RWcM8 z>;@<+yjk+MO`HC>SOx25#?r*OYGCr)0vD!z7tjB&7hD)kqA*I8*2vnyN=FSEEs~%M zx2n=Bfo@E&!1NisD+ZdV4_7kN7tL;J2w>MHuG;A_=nhhav8*9+-o66jL@|)HT^cpr z^H2l)sEpr3xLILDH_cKcmKh#&s_i{4L}w5B_s@V6vPZdqu|K(UeoJB4YXfrp^&Bcc z%8G_1SqSTFLt1m)fsVRA6Q3JuW09KzmmT^GXL-*iidM&9$8JqjZuW-mjt5LbKa2fG zQgGUzJ2)+J2sKstF7{ibgn3RC5Pon3J(?mQ5*@z5uyggyuM3*o5*G<7l#9TlX-#m` z_7$g96bCffg_$&^3|iNzk^1ryaYJt(^LWH<?uKI>D)S>r{`Jk|^CETX=~E9SZs)mO zrloKod;)ncR^*r`gUQ%C#~JYkAKbsG3HrV!K*K8qYLzpR^v?Z<(^G>uHng6J3oZlM zo*S_9=}=0$MpIv57TLM(32rJ%Wj<dprn$vlWa1Aw;*M3&DQzn>=@jCkb;Yog$%V0d zH_(aS)o5O7G1R23BU^ILLuknorhU~puH))atX|WMi2^P$tZoVYaiN0ywptNZ4a$Zc zqDxL^^aW;((?{@^D8$0scI4^Z6dabEfOgN8GLr;(ujRI*=vvz`Sag+xZ%WrV@oFE~ z)T&8Y*+`I*(S%3p2~6O;6OdE+S{$kEOOg*4fb<Co`f$J<`=>;~(vNn6d*}pga2!Nb zZIZA@QUbzu7?Yuy>zSU1awLnZ<SrRs5@+7xAgb7aa4uRzHA<3ZFbU{opT(6J29O=y zA28ig9^8chPf~x|f1j28uRX|rs<LLnpsw!Z4M%@j6St;6@U){ACdM3r@RgB_{X1bN z{B;HvoLbI>Ip;B@bQj0(988)cPQg3l-?%C67A(bMAfeTOirxqD4ZY9!1$HoA+T&>8 zSsjp@{DrGYk^>U4AAeLd3%Iihs3VjmuP-~p{12+^|9N@;mnu6ev5ncT*Z?Ev4#R+T zlkk*@JW<{<h1w;27flX2gzAfO@Qr&vcQ~gT&o=MFM3<lVee`Qsu||)KT%$y^FNQ*N zo&e*Qk|fgaSbWnP4t+14L!y!$-F5T`D7p1<nrjrv=A4PJ>8%gwd43RhWBbwjLOJ{} zUI4m(gmwQNHPW?McndL4MFT}sh|Ufhz~yVuO7SrUSRZwi9GiqS=PXIqU%|LR2k_lS zLy*aHptk*Hw5!IMOzH8aK{soiezXr2pLHG21q`|hXO$T;PhNpaxJr^~tKI15d`-c) zd<83BPoZ0+_t3DaPF$${2b3~?fimu<m4?IU7eRHrt^5pzZ@MFpJeT8M-vVya{2H|P z-a>8C7BD|-y-|=9X{xUpRP7u^Z7y%2Zti=rkX=RxvxFvxE`+fo4RQW7G0K07#+^P5 zP%LD<)`dD_esUws<a-#ep}Sy!e8;@731rB2DJps+OKOYbVQTh2G}PenU%4S&T|_Wx ze-JtHeF3R_@(AyQ%2SVRs&wRv!(xM5c4U}+rN~oCglpI4VoFvqjI>vw?{p>-mEPsJ zMJpNtvQL232UkWr>jTuBXBms!7??jCIZI1DNXc(ulmwXm#aeav`1S&G;sz(O-~Jvh zrfnmGtxw?EI4|;5XDis``!MD?Cz$7x)oGat$NZVq4bxqMsLQJ?&NAMMShJ7OZ%hYx z<X(ms14cAKQx2kBu7g*JFwT9pK{xHU@St7=FO>;@gYmA^@^2UXk~>JYRkXm*Q5}#b zbh&Y3Y)H-K@65AqM_PM$4GGgc4}M$fP;I9!ZE6iA@%pkft^O^zX`L>OPB;cx7))Xp z8$n?3P-5fL0eRuK8PB@KL{CMAelcG_A3U5$&*fL)%_+i>TYn9=V^1G6$i7CKho2y{ zX(ai_%tOibW5mN%m(#!XEkX&71D5Y&xP*y)=r1~j6Qg3ml^ahN_N&shN_H4k5X&6Q zv8BfSZ?LJk4;L8PlMG>O>|T8n?dcx4F{>ALHG~R?GvPQe`wM24O{88PF5Hz576PtI zo0Hs9fNM?}kVE`z@VlXkoU0#h+-XjXFMP!<*E_JRVKVV(Nr1~%tAH_)qS?z_sQbey zFgH1p&VST_hla05FEd_zT2YS7%5ftdRr4VBoEK?n`G;Xs2#s$fRO*)x`E_V9iThe6 zYAsTt>FdjIb_T+SE6*U;Q}BPBJPck(+{yWxSD?1BmAiCN0v)dpCdVE-&?-G8+BgtO zdNSrhn9g&kDY^!6P4`g0R1pqT-UL(UPJHblOZ6t$k*&uwIPS!Lw9<Vfstn&kZyyj! zvO=Dt+0`Prr;!Jy33^2Iwwif)-H>#9jHFE~T~YRtHuhx)5SLX9(TvZ4Gp?&JQm+d= z9gT>-ZZ$@nK8fb)#)QxG!5h;i!6Eaj$Xm7Gg$!qMb5R(s*`rAIT9kr<5DMu$Y(_LL zq~c$Va*#QvN)L^3M{z(9JvVYNsrs!*_b~OK7xS7MwdDd#UrxZu@Dc>``cyvF3XKof zL7~+ROma~Km5>kkfm5a5?kb43$6SKmWHAoB7(yQV%hHk>58@$|=*ckKalFwk(F|=K z9bf7bH@g}bqR+$J;37Pyy@bA;G=xfpnV`GqoIp6JgJr|Dscu@QXr!ACJ=OdeUN0Bw z0u!HMZ&)lwshE?bRT1Q0&N8Rx&8FD@*PJM1Jz+ep;<59qEU_v%fd@?q*zH^e(w7{G z|H)-&Fi4JsXHOyxHbY5%*L#f8K7yLxIVUA?KeNew6g^pW5x33kb-Xx3nfN_S!SZn) z)NjZ#a5s@6a!ZwQ%~Vr5V7s%7zq1Hj2fDdQjnirGaYK<L?LjG@O!&QRA}M^WM(ym& z;CYG^+;Be49EugfEFZ6<f6HflJ+2gooO*}SRrS!=5r78$FX3&o60><uj!;;V!wpqY z;pFc9#0Qc~&@|~9dM<Q8j4FncCZ>2|;0117R=~|wK7ohtjDu;m_r&uzT;SF;N{}D! z!n$)C-~`c6)H^#4h6#;SH#otl_(}nH_l>5DU-WUe#CCMOK_U2R>C>G{zA_UwJ5sp^ zMs)wj{me%{EgbdoCYD1N%#~0fs!ye<^B-5bUQn86SDBEvI&t{0ZJY4_Z@|1X`VIj~ zV~ApcEInQ^2a*pMlg`ys8Ov5x(ti3kNG561Mu!+^2{k7g3ct9`zqN_@ogI4hKgM+h z@>K1rV0b)q5KB$Z!9Qrmc&#?rP+r3*#TVe^cNWB6LxOCcXiBF0iRkpIc;;T;5d0MV z5N?+=GyScjXx*?@WCR>birE$Ly`@f;?oy}2l{hrB+asEP*@zAsF_JcJ7)pLP4kepD z??&sajm!i_fo_>O3&(x>2WD-<NN8d$6qZh)va<r1JB9o4`xFy8s6dMDw(`ecckSu= z&C_7}O(PQF!NcBuJ%sjCOtg_5?pe74rj{<^Hb1?Cz3JxUqR)I-6Kh5mUdk2+zC8-5 zi*#{uqbz-`Do0jd9Yu=9_2b#2vY1rWjam8gn5avqIV+z=%=S_u2djtCX!AW_{L75I z4~=4)UOd5_LS|m}z5!96{+0`=F=AX5^l9pZNkns)B{?uui*E4R28-4xLC9GJT!jko zY7yf0QNGNo)<RIPR3ddEbKLa)CnUZ2j<s8LnDYf^F*I@pJj={sWDjhCP1?N>yQ32l za+ML^wPDH7eIT{piuR-%l8Z`(xWmYw>s@jd+V-i_i6_QG?XW61+BaNaUF<<Qoj#b; zB1x{y{eVpo8Z>HVHwLI{(7yOYrv;xwop#(Zq;qSsL9!>ETd*${gJh0ioTWYezPJj* z#;MbklX~=`j3zOdX-g~X_Az_(hluy+lt4qC7VX<z0(^KUXLZnkj(3%ydyh22u+<MS zbC5OB;HKh*p}Hj5RD$si7Krq(3`lkp#cq9R{KyZ0(%Z3g&QBFMF5n8EHdiy%Dbhrz zWjPV;{DWo7M`GBtJ`7iTg-7eYLJx{akVuZqUmeGsSJkFF!xLd_M<4iis-attD!ROP zA+4U-_($Ce<T@6Tx)uiR$xI_TPd;$hrnh3D%U)2(*pBz?7Bk02A=H(f#L6OF(l)~z zCwetux8SxfNs7cz$sw3}D+On-j|3~xH4LlM!43&c=KZIO_&a<Xj@f?$_RpVzXX~7V zPU_X*{309A*H{V&3WT{9g8QOnI~q0%=p(gjoObC_bUW?}vx8!=<jp^*Ow%S$_#2pA zqd`|T=#w(nS56=|f-HES52mX(Ggl<dVfX!cT<({C?t{?N(JAl4=qe=QE7f;6^0Wh4 z>UtaN{+<`s^~Pjmq&Li1tVhZYwcyYnM?iV`3q;9N%;0yec)gj&<Chw6*567rI<JZP ztJ-m3S_+)o^&Qo&w_$s+h%Bsa;UvDd!}Q~y;FH5$oYJO89&p8FCl-$;k*VI~J!?s- z1avF+#+m-dy>YMfb*>{>m2z7dqB}|mjRu~BJ<{vY=d~`K{Nfa&J5`TbTh9?S?$iA5 zv$Fp+XaA=vi>sYrzlkk%DZGbi13u6Yqs(dEEfCHt`eeL?C8O4}UcdtF=RV&0i{BdG zfn=>RMlPzvN0v>Hk<F0w<NwgqOQ`dBJDRj7q%v2|?*{Hm9i+@Eg5Sn(z*j?Fa+A8U zyrQh6x~#mEtfcUz_#YRy{XZ9Vf;8o&9PR$Qs}BF~RoNgN;d{ratIMkj53BgUA6B>~ LD?3?ARqlTQmylYp literal 71976 zcmeFZ2T)bX7B))G2!bd<Bq$0fNRGQ#BL+}J444zBC`JsJ6G4(75=B6gsGx`nX3pu= zpdu=w7%@jsF^drc{%7Wn<DL7@oqKDl-m7}AE>&H7AJ#g%zxD0@cCQZmC@E+u&R-rH zvT$m!zO1dat6NgJoQj2tbf}8-GA$W<7k#C^A@imNhEBFIRrtr5rK*Zt;If&Ebfxp9 zWmM*=NSsu(X9R}MoHt|gyr58P8|$S(GiL;cs<i*ZKc~zLTqL=mtG<F)(A>F`ZB6BV zIa+F{$Oq1u9~`J7B^5^VrKK&~smM(U3jO(6T}2^uVc@()p8u$?qoh7pEg1(JeFgu( zdBKzIOyzz#{({57<_|dR{tSnm^*?Yp*#3sY{%_*2kkn@@!C|MbJR)Rz=-j|%lO0SI ze>?pnMZdG@VE;Rtj(^T(n6%`w_9~r&mdy`Z82SsHizIx`nK>^gaG~vwc=Z3=`deTQ zzXj&>Hv^Lp?BE~?%<(sZoqr?vS7?rZ4()fU{}kHqoICvn&E;=G`$?qp??k%(Ig#PN z5b69!w6=dq<WK8wM7roJ{-dSZ{7tx>BoJIAOuFhT{@PS+C2juKw)!hL*FOjM=T!ax z_k&7Dn;+mL?f-A-$nuB89c}fMf0MYa{l6#E(e{sc?f#O?pVohn>1g)@ufyNOD<RX- zPD{qo{s)<k|3s$azewgs^?y$0PwQ{s9DacNyPEk)rsGdCo&SQ&f6<ZsUy}LL`Ws%S zA9(++kbWW4NkXRc4>Dc<naqEY&5!E;oXnrr-@yHljosgs%}+92e<#z<_Aki%7bQ9T zC7D01|G?|?LrHdjS4k2woouvZoNRw1(@xT*{4R5+f050P>i?X~pVr^N{g92_-<8ep zWIEaZB=g^NKEFJcoc={gj(<t!PwQ`Ze<;cB?<(mRG9|r_lj9FEe{?^;g8LWQ{HXrV z$^2>k4crge*!^AE{3O%)Cz*fI0XhAPlAQjM%%9fZ@cvMe{ohs6FJwwOAg3Q4ko`Y* zKu-T6n;+HxIhjAL{{ZLwLpJt*S2n+s>HM<;vj2+?$oXHC<ouUp{<Qvv_lJ_~|E`iG zWI9VaAm<+)ko`Y*K+gXnn;+HxIhjALzk&N98~eX28*53bA$g!XJL;>AoVRGf;-H`v zL6hyBO_hKD^pBJ@Xpx6ZzO<r>#NSc!%yM@6gTKpP`b%W#?ED9R*T3|av^-~*fA)8< z`73{?U&wX+gTL)x`b*+<vH63)-Cy}TOX790{e!>#U;0bpb+P+{zr(-v?<Z*-j*{Xe zDbAAOA}Ova5`2<kD=CuLTz)=)9schJu;i}D{Bl<ue(`kpL#<B#wpJs_mF*?v7o?KJ z!#6P0ck!Ib4wA&<AEzpo5~HKU=%lYQ;<rZO;365N{Nw!pY7#ELQ2K{k?C|ez@gXgV zvx~l}BzIgC8aOX>vZIaZFJr}DKUi+oQjuR0IA?K?WxSS>w34opuBP;$FfA#mQ=U@) zkN^Fzj{lzG|8ey{uKs%l{(H~=zqBjkKL35pf7kPWb^Q0t{g12varNIb@ZWp>ztgVr zrR7w9$=W3Op^LL5f0E2_{Fb#jN@h6zDQlBVal6|5%-S67{!P}FEG==g{pT#q{-3h2 z&>u-y@Gm)-Bm)chLCBRO-+xcU{+sh3-}_I;Ujo!qkzkh;$@I9ZoxVb!Y11Y<I+)5z zj=#rlFEL5-;UBR}^5NfNKcpbBIQ(-wPX83oW`!RC#Vh=pRVw|GRVw@^Q2)vP3ju#s z{?qsWll`x*{m<n8r|bMX`~TOk|L@}Z-~azR27ZW6G8S}|j0IgKOE<rY&iPl-X=(nt zx@R?OW~h~9<tSufsAZe3Qb(n3n$n(swr)4*p6;`R)3DO$1!iFl6eY^iy+(@Eu!iH= zEr&q$gEGC}8Ul*__oLy#?eJztEbxklAk*#=em>R*RxPt8hsukYc{3+5>Tx(=yB7D` z`vPQ5cVb?S3$5=eL$xCsuwif%oQ^$>aoK9_(nd#cONlER_qG(adJjR1S#v?^v@^cA z+l)DmP43BR<>;9>43`v%VAtK{kg93MV+`)2(xY22@MtxO?<>u_4cUeRHa&ska_gY? zmP(?2N0<5zjKg~)VxX&2Ppb8z96E3rqIzvCM%`4xMLOybdtHT=B!!ZYrEVy99Z~D@ z61ccu6I8lBVw$3hr0jeO(Q<qaO)@+2^4&AcUS0%OWg}RRv`*~A*ERSmavV;Y{s7dk zonYpj+_7D%Gq%VW)0VPutWRBoT9)bJrgRzl)#)HHnCAqy-UNeHW-M|URk(FBf(0*0 z62vNjplM)8R2uzoMz3ZlSg;l6ywnnJirz?y`yEE7rp+wTsRyKwl);8)azv)fI1DND zA?GjNW9A+i7&OulL^^JwP|<V18$Hp(BMdrq?#S+3?L{|#k;AKVJ5cGo=_Etp8C<<F z7?<vu2=8se&|kYhNH;~oenE!IynhG&Clf^`eF8wq?*g<8>ITB-d2C9dHLiN}0c(by zfm%$#?0~N%!Ni&5YHSCMD=kES+9aG~P=Yz84Op@&5H0h3aHz)(GXGemaANR7XscO3 zUaDPy@xh)LI~nNSf^+O~>j)~+T7+h)Eik}<3lmklQ}yTSIQLFGdGTom+};q4ZEyEM zSix(zdwUmxe?up_-@qMhzH|Z?TVvAe>jup7wxSRFG0c0$*{Mo@h<4S4cykBbHaHip zmv!R@&lceh&vxv_kyLVeXg-V_ph43$ZP+c7aP-dJj{8CnKuo)yOx9SQZ@zmECu`?G z<gI5cdV(Qduiu5yFb$V`)ncu3KlZA6Ij9|pLH))E9IO(?T8nbf`RNqUH|xiSSdZX~ zyI%v#Nk)+3Z4TNycA~w;aJY7@3D!Kx6UIJ`K|#9{HZ6^S&7W<_ye+-hha(Em-(&z@ z@$HC9zbMc{OVeR@VKI)|?t$qe{3x`42qn!c@j`e{ar|pX>SS>i{9I+A#-$9%i$kz_ zb|JdR%hF~0H$!!(2uAPk3%bPvq4VNyOySgVaB)vz-@aUj`rIlZWnmibn3B$vVz!Ev zO*DAEQ7}H4dKNqT9~Mrmj>VY@eeu)&OT^PigNNPx1Y?`uVy)~Gu<<tI-S(N$aCm^t ze%naxEF12YFG~|&g<+3r?I87XtT5DHm#RC)<5-PCbXz%`jh^<9q`Wl3`qpYF^K8Y) zSDH-LfI{HgavZ$Fg2m?VL}$A_u<ycTc6U3lg!Y}$byo^J|D;OSyq4jmGYPaPcN3;q zydpA1+i^{iA$VzogSt&INe|hME^8|wX8av!{tyGZc8<j-L+i=<RcB%A`jxQYeK>p^ zl`Zx%IRW8Q7L$OmBG&TE1XS8~qKobZ*zI_h94g#G9<{Znx8@mfTfHW>Z@^1-<osUv z98iLrc1^;DReBI8<dXG)%Ag$Qhz4b*FoKO^TW!1Ju}(@bpm80!A<l)Q6{>v6t;f)C zawra4Y(!mK969S74aSo?pkezum@`3+zt}Ju8!u(SgT>?B*G$TV!%2oPyZd=}4fkX0 z_@sAOym|>t87^ttvrC}Ak0i#-p)h-L3VMx>#`w{r&{}l{wtb0*eWj1tyk*hM@s<H5 zu5`kJd)Y8Mwi>S|S<{to6shm`AhG<yt1R;VWcuopKfNDwlg&Li1Y;MSgZ_`Z(<^<| zaYy@UqSKplP~ZC_nf~-P+|GPWiZ0|b1@kOy?^Gf><MUB?k=_pS4?Gi;N|boKT(MAI zn*!Y)jYoW6&2X+iPIEV5mr`_~(lP<5)d3vnn*wt7GMKnIi&!!sE1c}1uTch$d^sB1 z#_omA8Y7_7%4DqfxqzyVqF~Dz0Y*q=;X|)u>?}`(sO65Jaohx&Z5Cq0=z8Y=@C5r> z5iZVc?}uzi492Ri!s5q$gkfY5oAGQoT$$jAFFOW+it9A2|8|^A%H0ng!<RDa@xCNP zc_MCjkqv7rcM#io0-JA<3KNGG19g81DR~~asUZ^MmMBr#ZWc60ISfzs>V@9XccH~y zpWc3EfVZ#o#M#IDp@nfAW?zngtXoHguubjobN7Q7ci}PVcW)}(9_R=4i{e@Q6cL8% z?13ZY-Kp2@1*F{pirM$x37?)dLd&xn@iE!=IOf$y%xLHdHM2#`ZNPpeH~N^!*l!E2 zR1t&Xsq@(NsyPYwL!8oJLX$_v;tCZ9Qa9xa+thp(x@06`Ol&PNFUnzK1G`cEQ*+n} zR{=*%*u$RQoyxNAbwcgY+hEPv7sPYuewK2lg=M_9<{zfrhAGpW(D;2CJQ2BLc2Zw) zft@nsM08+r3D+QDt_$Y9^I@qylEC85OOkli6xJ;@K&|x7^whd}xYYb842dJ?xL`6Z zsaA%J`M&VBcYnN~r$)7}nux2~^+z8Tg%;Ek<mHoDf~P4>z0?b11{{ZetC5v_oeKuT zro&efggFV)RL3_TOxrzW(M>(^y_`B~EIP{8SM7xa$7oz)rN!SRO@%P4nYg)!C7C%* zlf|AYBwB0b=%D$L7`fvS_{Dx;m%cjDukpii*k)DwV&``@@skS8a}L1Ot75^~_aW2R zoxp6itbjc|n=to&DtPd*P;HY5<rf-3OTUJ6YOWTW+@AsJOKWkKwlTFBz8{C!rlMV# zDXafbDqf=h0#80Wf{(7t@q)8ysPsMtFMrcUL(8LNjAaBnAZ^Xe3cK^r<wsCa?E|@) zJ|6Dx?ZdT<Dc^0}87#GeVXyo%G<_+{zh%B8r`2U(gQ%D!zuN;xFAwLR0<tQ~tuJ7A z$3s}h+%P!G7zaJ~;OB-9B{x1u@f{~xp=VAE3Di8u%5`EfrAUnVLv!FlnJqoH_zet1 z4eHRbp56I489ya+$h`28`F`7um33RiALj0W4TUN^+g*fbi{%72VJbuoe8`@vmEjcf zmF#Rfg7KT`;jZUa)YUXW^SaZdGQ2<QG<GAn-w1<`{ok_Y-8ad^ohkS-W&qZYH-f72 zZ^A_VD7LXV1U)|JLgbAuXl>mQ8(Kn8`O*f63~+_QysPZYjZH9Ljl)eHW%}y#N?g<N z4jwVkph;g0Xw$kuY}V_}JZnlaER&7_9UULE-g5yT985+yR0I1GFG9cUMbN#Z0G*0u z#QFPt@$&R!vNW&(R92Y7tOyx?uA&>74{-n?Z7Qy`X#sbfhxPFX(Xu8TZf>24n#HOx z@ya>W8}&sjYM2TSrWfFXebMaM(|tmw|5psrtb=F0^m+GZugH~4ZrsF5pl`O=W7MjS zFk4Z97dJiveCrBnJyP+Rf*n%RyLfBQdg!}r5T1Us586zuXwidX7;(`WH8v%}TlF?{ zcvJuZH?Fa?h?gQ;UvHSUREj$9xPV$qqoKrOE`~knh6b)0&^@IH6Sqgf^Cu6msI?z> zZ*ReU3qQch(M72L)`-@mhjWV~`(a+`ZfxK08Zh&*eB!N3M5b;CL<Gvw1vRd)aAg!r z+SG<qGX}sJ4H=%^GYTw&&avr-BiYlHX7tNvS+2LQ5Vx&cAc(ij01Lx0!t;Th`N$!6 zuxw-ljFZ!&9j~mzwgu<N{i__Cdnbv%ckBX*ll$WN7vnL0`)D%TI~StfS7G4C{djL$ zF*NO028F5&Z2Pbn$9?Mv*{upVE#fHXh|OW(9E#NygQ0rKd)E7K7%Y7zket{CQcymK ziAy4|h7`e>MZ3uRyGtOCWMK>s$4|Ty91ppG_|XTAEw|&%1qYzqsjtv4HVM&tJy<SK z>L?j~{NEb{2aUN~;KNsKw5PtM{kVmEPkv!lA0Cm?m(HrQ;4aL8uGlWi^#@$U6AOZQ z?=*kfVS6?9tT-bMJ8HpiUzem2)4S8)W2$uc`|<qdJ0F@QbmrgE8`1KPypqzdwf^&% z=g0c&pi21zDN}fwmKXj0C6<TLFh1<b1|HtvM33Zd;A1hJUYJaI*AtZc*QfI5O(W=@ z1{q#4#6eg&dJ><RvyST}dDD$!9O&w!Yxs0g4#m^5T=rr+Ue$5{GHPu2(@~qDWI<0l zQ_^-mWbK3VXXLT(LqGnY;S*l_ti*@K22tDL$#6l*ThdSI@ZO`k(v<DK^wg;#eEdBn zI$=^P`cE4`{fg(}`W_!4Q(2WqHWRv``XyTL@S>0J*wReu$UCP=)5mFj>8Z7+;B7-U zTHWSBx7xpxw1bs!_niV2#+%Yr(yh?`K`=jgBnL$&2Gapa{rS$Xk=Wd|g+;6Hg69iN z_}eG0yq%vBKY?TU;y_QDS1^m~QhPpnoGLfFF`Zu9=}k3t&O-dU5w!SYBBpE=lLz{p zxxaZAiqAj5NsUlizpRb7S1@Oqi(kVF#T<ND8_gp;l=+w4U*P1+beNYjTRd!J9xt=e zphfu{E@aK&=I^EHs)xDQ`SuX%HNk-9=ZxYrtIViwbQ&Fb^)go2b)hjA-RTU&OlrTd zJ-_pO3hh0k2R-wk1HacRjL$xMA1?+ea*y_JplRuF{%r0IaK0i>`#NiL?`vz(=iUVV zsl!z6Vtfx|GDq|3mPqb!xi=qr3V6qWo%rJ10=zRqonJ4tpfxe6RQ6MUn)0m~VvVkX zp3Qo0X*G{7j#x&0duPDLHHA2N_ej1}y%s7pGWlb}e7M#*9`7&fPXldZ(LZSiWLJL2 z@*`hhUu_+=)J%kJm2ZjY=y`XlrAsq9CSl68rMPGBP*@h-15K??y02W6$EuT?*;-n~ z96rp#0jsL;TvZK7Pdy@DapNFf)gBHPwTB8%q#UW;w`%-orADXJ3}Tk9N|M>IKWOS7 zy9EasZ#Q(MSvO<FCO%o>E(?N$usPjW^><rrOFK<u;_jkljHyzWU*`k<a^vZsdnT7g zA0+S0l%UtuP!uiPL*njDgn(cTM($q_D#ba%hbC2GWn0Id*cpl64c^KgE>+@P+=_XA z<qEoReH8BgHiI`Chl7#P0_;=n!^a%3qEM!aTihMFLH;J%sqqGku=3+gmLI{oS&N$O zjD)41p5hLRYVcWjMWnUb5Mxgq1^bj5{CudK&71p(w6@+B-ez}!?`~_M^ZrR_QQ8&z z*z6Z<q`E*~IX&FB?Kzq7ygwOc)SpG_FQ6CJ$-?#sEp8cN>9KZK7muqaZ*u3rBeaj< zS~#TU#`CULp+AqI&3DIegJ3B_k}c^l>v&YtIY*vg353??Gd&d>!S$O7HMFWAQU0G? zaHKwU?wya*Q~|ubd*Lp%s{&in8I?r?;OIjebWqsAF1+o*`tANeUYs_^W6>fZ<;FTe z#nKmFSA9j<<Q{yDa+*g(zgiwyaSgKucI8E@6?mJwIg`8eMmVse8u7_XP+ZX)C+v?T zBd(UivQh)IRlL9q#Al$uIF_y1aSRU+YGu`5<;nK;YJB*{?(kx;7F2e#VRfrafVRyL zJ8RhDtFk24e%4U-qOl*E+$khuWy8dayffL7Tk7J&<`vAv{571()Z}wE6jvH=i=m3A z>Uf|`0oZp?@wjApn#Tnch{Hvf(4ba<8mxZ-ciJgc#_t=)n@!#6k&d2pWXmA_L{Ov0 zwhZT?gWaL_gA#3vE1+FMcJPN=J21U#huG@yJofz51*YFq9_tkQ!yB(B6?3)cvYz`& z#APsyUAexJ%@*Bf!*BWX%rEo!=wV;sg{qE6>NcdAvU_>h#7dO8Z_A_Q))31(J8<Vg zMQ+e@2c0;{6)ufT$8|UFLaNOSQqbKTYFRpt49td&{bC?UXFYb>bC_)2Q^H1?UV=CB z9f+~2AG5R4VogiSE3Bk;lcaX%gv^JpiEgPT7(Fw90YVw6D-;P3AqC+NpF{Ua5%idR z78=ieSQ&8C)1yW9IEN-1`sv+QnDAW*E#lMA2jgIdT`^ocvj|>18sy$}CdJ$iYhYW8 zH=3%Xkj1JE5a*LDer6`d1CA^3fu%1fwr>wkgEZW?&hC%3)1@(HSBluKWhK$`dE=&# zsxHp1@nC*$v&o(BXIZ9(0b99W2a26*A!Lyh_5386z0!5AoGqhJ>D=5Cw|y|C!elv` zs%61<p*nvw!G)K8nG3J7q<NwJ74m+Q2|LiooJ{RB8*kpufUvQTaFx$iyfFPFyfs|_ z4XuZvH6ar2K088g<y~e+YIiW-S@P^!MiN;#+y}=cZet(6c!G6pg`nkTObRN^nX|qR zF>e^o%{Dr5vQ3^RmM3{E?|h6-k;><_vjcglg#%w4G6Nodcnv-sRrm#+ZZv+=Y4YWS z1Fjez1P^a^h1aJu+0zb@Y$rX*x{OOih2xz-!E1~ABZpkpVSp*v^z#x28rotLNhiaG z#=+X<JB0kaiMTdEhc&TO(J^;N+<GgM1qCIrDGg4Hrd|Wx5@~v8{sJ!3kXv~=#l&N^ zJn_gX6zEzPEhtzxoA1k0r_1v0!d^pjIJLMOx7cpLyreP|mS_Q;9}N@TG+@r80O8UD zdvMmfgs(c7@QvfvLapHw($2R^T<4`misjQ7s9X`EQ&Yq?(aPxi<T;zz$rnB^Q-ltQ z`DCEcCq_GZvf!G#a3%FPJ^6MQ44fCivrfO{QiT<CX_XZ1Xf>H8^jrw?N(wNlB$q7+ z+{K?9+DEE~HxjQ~%g}nY0~*X3$asbenU$C<TF(byL8&*mJ=?{)o4l<!zb;yAeNGAN z)Z5|hq|TVvq>1^*BgkDt9ayMwosA4Jh7-qXn4|7Ms9W%mz3*_1dH4T-Wu+eY^!+ma zK^RzR8lYO4*?u5@U6G5y3!f2Vg^oDEcNk=v=<x*$_wy51HN;Ar`+?eYU1DvajWhj+ zqT${pAjLNmQR599l<f{jqAfA9ZVOX3m0_z=F9_qr8Z7O+75FFiC!6~9M1#U85}8y% zUf$o%+U_u6j%EzeZQ;yE#gi<(n*mqkLiziGxnOZA#Y6YeM$SGn(AaKFUwcJy#ROfP znDHIFA`@}1bsD{;<w3PJsNe>Ju8=r9h~})h1IG(pFlCTD&5Kf`{d^VpdS5f>HrWpy zo!g_gN*yW6)rW#B0<p{)2g$8Vn1R0{^m(cRMeVk*HAZ*YzNS#JV-Q89=zO-~T6c1| zK9%ak+VRtC&hR70dU@=)b(E?!U1r<VwCJMEX}GXF2}kOtpur^zZeCZ)4wT=P3;-5l z#&&f+GGqwfFxZH0S2aU3$IIC2W+HaiOULYr#pJ2^9-{HBia1;KV3TB(@aYXRupKQT z!#iJNrK|Ma3-;a;b#k5#A@a}J?9Q$@;mjnM-!>3qN<Ki_fk$NAb-Bu;86!O$%%`$z z9x=FJv^|~CQ=M;D4hIz%Y1%euAWiz(fxDQ02W@3PLK0P|-xnFW!f+;zijm@KBQK!u z&Lxn3uLc?phcoldwc^|5o5`9nopE!knB4DbDr9Czq1}K^aC4nB8+rA+$Z}{e5@L3b z%~@I_J``5qzH0Al+<9DzCfE$+qtC^9#LnzR?)N?j<t5+PGY?&y(Aq#Ybl8SQ$vd!c z*J>I)I1H{EpTSx0T+k&(2^xcs;0uFXwq)&Qxcf1hO@OypYBrsn*yRA5%B;{oyAvuX zCkU3YAtY?cIx_c}0wkBZ0keHTzBY_yRqbxE$s3!5^+L5^ba=mb?34(>zkdmAgldAV z13eC9cdeAlm`Ghu^~X+GQv8%ohstr$HYm4gGkjNhjrE(mafkhj$f0{~e6VgR?Qr!S ze|~!uoZfzwBz^9WpBHD*FV=}vXYFCMl1%Od`zNrShgS>L(=Ejcm$O*HWfgo>=t|VQ z-jX4f9)j%-ebG*<JH(<sp45F_!wM$Phvhp*Q~A-}^y9asm0y-!$15!*H0JwzxTMsL zMn1?Q%WwPfhhJTw*Y<4wW$!E&=Xwq9nQ-(nN`v#R^Vxuq81`A~6f0P1hi?^QK=)G* zh%em&w*x<8(3{=j@+bq;mr_H=fq^KxnC0H=*(wN6ZnImpoyemjb|iShdouCjVKPyF zIXSv&G*6Vu1wXeao)@N4d8oR8TijHr?7TIFSI$1fB9AO&h8AN{lj!k>>mI|0kYF$j z4a2H2TUgMWVfa0+2BRvQ$SM77*2n!6>R!2ogJ!H2t55UfbySXKIojcq2oF{^Rs~0O zJHguGwOG5}dax}qL0qx;74a}CVw27)LE)^ktXk;~vv;n8(e_XA-f0Jn9)E$Zl<0lr z^9I_|x(e3ppU+!zPO`_F2_MkD1}YCXS4ss%pjqS&dc3d{@81{&;j`lT)-q*mvZ{i{ zu<dkJlqr?xQdH-dHYWL*a=NmL%$;cnA?=lg_X(#3ulGY))=fF6NYH^58&b&HG#g^! z9LX-$9b^{=?_)=;t!SEN8XZ%ZE6Ht6RW2yf^{AZ~!ubgW`XbGPK2dBCpIYb38;%FS z$?zt=yZu=dU2jK6hGyXWDcf1x5@r7Jz5?Caq(GM66yrnlXJW(B7#xt?5qhLmVc$Un zBZpS8S(aK*mfuITJ+zfg+*QdmFTAPne7>K|+mS7Fz3zb>^)C_^m)&Ib9uF$2_`>4e z6oG7%RG-k{uV_&0OMcfSpXMvg;p3lug8dVAVeYifnAm9wgq|46I`l7xt%*m)-%n@2 z^xkpQy?7IzZtTiFXK$eMh%&D<dW$Za>)4j8{=B(l3%R;Y7B-U<mZ5owb$U9INt*z~ zT{neQf#qV`h5boQvx)dzrUuR_-$`nvdoumSM{%IkXA+V0#ADtCS?V!yIBBbIz?Z$> zVxgiH^wKC{b~b8I9T*A5gWrfh=JXL+$s~Z6%Mp^`Foq<|)+7@v8_BBX7Gm7l2)u_W zLh#8rRv}$XmU;9bqwO~_>oJL>q2&r0bI=@XTRqr{p5de-(+uMt=(A3@e8etWK8S0x zY|*iyJw3K|FDj1>qiRCFM`+0#+WG!aGU$#AHE@VQv%51vPFtP!34I0~I%Lu$=>fR+ z`$6(@^#;hkI}V~=_obbjZsDY1D{#Wo%P_iO9jSly9tNu`;){Dd;EPlX`>Ylr^xx0O z^>YVV!^iV%Os^Wzy|uo=fCwGd)ocU?D{y8}VhTgPRkO(H9qEe>y{W~%omjB&1eI^= z?UB+^#^c`bGBVOwnqSsv#tRlh;B-$<dhk^Y-y1FPh=%v*@<xibC5N(Ek@M-TP&NFv zu$+vRx<$JWR_0f>FXH`r8uQ%tTGYO}Gvwb3V8zd_lhT7mFsOW=IC&c*jk7b!Zs{FN zT?d%2?Oqae^(I+b?1)L<R)MDLby}t2Pg@4sRfe5g&)uiB=XKjlS>&s5x@B4tako{a z!%o$UP5ck>CetUVOcpbj<#DL(%2_?BLYFf!;34`-rr++(Cp=n7sM>n;arK9`Xf-U4 zxWiP!cCuVuceY3MCR_TbP0%nfBU|m~iJ$H8$2kk^QRYh|IkEDQcx7G*5si_h?Z;n1 z+R&T#cI)c#;6oUd)^XwbUVA}GqX*x$iO{*n-a%`88QgC>Nf)^x<USh%i6@((X5vv2 zAS*+!x|F%TdvF#8jZK2{3p8+In>slXU5eS4mciJQmUv>ID{Ggd2UbPhG3~uS>0oLF zT2Eh)%@ai8ogbeF)Ju;DsX=7ox_zwm;X$F?^D+0T*u+(1JUk-v_R<Yoo%wbr1Gvb- zc_3Kx#nZ!ay^c0$u6cud4|rpDxB1X^FQ2u#XTxc;?-1fN6)&o8lPpqw1f0B)*$=g& zI<*ea)Wrc6t&fZAPF-X<8(T@Aj=3yszXqI;H$lx;FNM!_X9O?%{!m<>N^%7q5Cx6K zcUs{@Uh^KhC+<eA(n1e|y-NI`)mhS#?ac!W%FtrfXkfK7;O)I$d~fJBsv0H@D~z1@ zu~SJP{aOJooAd&ey03UOp#!8W=z=%iYT;cW4}|0?IHIHmy!)jIE)RB)uP=3R@$w)t z{%!-CW)pxnBf8_-W&J@uYm8{A%r;hY(3CAbmBduoFsM5(2R#~N@avZOyx2C)L;7+a zkI9-uUs>zZX|Ba6=w_jzQZK4O&T)hC9%%c*2g*l<Vz-#5f}4C#X585UqO*pgLfA9% zapM-~bJm&M=wwgsywT_7_o9jFAY+_NFR`-^`e5gMZOr#&8F8eI?sErUA(!fhL(Yx9 zEOt#-EFNLQ67R<ebHBV}4<5gT4vL>>!6a$yTQUVYFR#Ph^C!?55wm$`y>|S};8Qg5 zO%XcY(Bs`~Kj8vnLy$?*;D-CPNl3*t@d@E1E_Jzy_72nN{Y}!?;(H8So07mXOIoRm zWESO5-($7?_1)n?%a$0OCIhXC$lLw)cw~bM)gILcSM(l?L)s6(<m&704c}Z@=1ve+ zObvlCh3jGOk&a-X=?N~^6fh{!iljmW+|aSWFRPZ28~(Oztnxi@y5Pl*C|<-hv!zI- zZ4v2a-v@SuYa=bTVxGIJh2S}N(DHFd$wvBL;Qi?|&Cj{ajvvnrX1(9vV<$Zg*x}*I zY)vJQp>IF1e6_)ReDXkO6?uTEk_FBIAWLp0;DTNs$S(P6=-z%UL^M7V7Oi%H!G<XK zEbbxpG<eHYt|j3r?^bf~bQO7Eep0+;vmQIT6tTemt2ldenAo61nRLogK$D)kp?d38 zvbiLW%<GWNUd-1cXU9i??g|A6J^EBsb$&JO$n1~tHN#<XRi@bGR4&emy^CF1JMp<8 z@gzLV3bN}qz?@Cl7*xFr9N)bbMs=Es$9rVpq-sNm8LW?cRwt5aS#^*$U^08SDICT( ze-+Q4xErrI6S!@XfbwnHR4e`pv)GYJxB3)AkNgQ(a&9!eCes5CHGL-o`;B3`Pr^X5 zlLF?NRx#`O0zb5W2&7%Cf^F^B)3^jbXk5^hpKNJBc0w9A)LQWlgLe?;`j>DcFa&4L z3&DNYMuA1g<=}4ZMnWeUphBuOjx4AbI~+2F;gxA*bh;Mz$5@DZHw>SQc13R+H@G-b z4-dHpkO$skc##<f6&KaX!jbb}Pij4A-+aK1>b8*9qZi2G@hjQNr&ICM>Di>QTN|r> zZ-eJw>9}VkTxM62C%}#s8=<S-3u0*)3t_hi8#J>sNe-w7xAwKryvr30{nD}b#MjuO zv>Z(`-;qwb%G@tH2nW4W;y3f#@sAejur0GlyngOTR)2ghc0MS8(-T9ynwCp$<jbSG zxfbZ!OS+(S3M6W<F6|poNA8Zc!qG4T^>v-Fmsqk2sP=^LmOebPSO=$$P-lMILh-Sg zE7&hdCidHNnSS#NrmQ-Zg-p1|CP{B)>WiK*DH9u-JG+dfo-^SY=hOM1_pfnL>v%f- z>H|`rr$*)t^Pr=pRQYhDDq)Rv1U;JaTqtc2!TR?dxyQ5;!6sp{>nGVPcGk;+<wPh} z#O#>F?!52E%@z*?TrPRUa9vD3ok_}aHsdlm1sK~VhIiNc!GV$1L_Jg%-s}$&%Ka5t zda5+WtxClHagWH|U1vxGw-Q;%CbJy5+bB2woA{L;V=D&_VY|C`K?mEdu;6eqK6uqa zBBnVadQ@WQozKjpZ-30!TFNfyl){iGEnIR{0dB6CMobzuvxqUB5U(il0Lx3{+d?A} zSnvvmsgJ?Hxbaw4ZqDm%`(Wi@F|k|i3VOqrvXIUdr~=zy)7DvR+sZ=lSz*Gz4wV)b z*=TVs%Ll}!pA~J%7o%H%0qy6hPn#lY$c#}GCdekhyDBAW{3@HQS~CttkN1UF!$)Fs zCl#(dq!YY<6iF*><LKpryKLItZd}jRj{Cjq%lCjKp9!VnLzm0MW-q+BPO?1S?W7G4 zcgoYcC(+_*d2)EudN3PtahW))S2DB)w=k(TJ-&H!I<fcl!C77h(B|q960!QZ_)~jh z2o8M>>z1d1mA0>YRNvJQkWdd@<rcA}lj2D7^EmP%Mg_lIjYoIiE<`0agB-Cr%3RjJ z6EE-S&Gc8S2meDGAZg$d^5h~V6US^o&BOgsRo$J*%Gi=Q7nS+Cpm${Q*9Wj{tqikS zluc$zSK+F2%aI=3hL$hBqNdOl23)iy?C}sj{c8to)wRHqO~bfuyaJo<KNQ|CZzZBr ztMQr!!x1}QK+nUy!6|Gob~EtiBMuE?^cY1PIUa&L-@&Vmsl;?q96$JK8+1wU1D%g- zLG7tun6}LsuzWiVHn*9;%$}LJpnN0AGMYr+ADxVwtUciPg~j~#mX~NyAWb!dHQ?P> zAIB^Ap*>xui>38;3Nf{P=){C{(m-b6p7SeE*{g;<u&!dNJ2s2i;(6|~4k-}7uihko znHpKEYeJ&yi<zFU54(H9k&UueC*y9*V92AH?oZS=F{kdyE`d>rtaHtI@r25Y?#EB( zlQCU73El;%=sE5%`!adHs7P6XDW8ZUT|0gk5|X-#x~9hqHvQB{$m1j8#f25(Wy`k- z+b8)8m37*fT%ZZBY81)9wO_^Y!f@7kz7&}?TbbOS9>?@b9NCR$$waR#Ms%%Yq_|<7 zDu#84W_|6>xOX?4PkQ<p3z3Z%#YT45+029`LdZr94DZ^FJp4Yx{it_Wcs$FW?7E@K zURh_cdc%9<(7PmY<`FOPAV&xH&uwN{vw6DvqWUP7x=2|v4C;aX4ulb_{U(qUqmPe` z4X_htiweK&6h7#kBXyfk5z*RtVi~&=La~L0Fl6&4V*1h?dPf$CQ&#Fj{d;XRYOH6u z)4wpC=1>$WRuSXXuH?=QUHrQ8u-L6S&wbg+Ht|6%fkcm}7W)S0uuiez<Y>)Y7I<`r z;HID}o}3*+MjLty4epCr^BNiPlCm<Pu3|e=Z=EI_TNWqG5AqkMFGyq~4$X6&8DSzm zapo3l?QJ9MmN6wc`uoK$T#h{j7h&heKCDOLdUAYGo8Z^8+3of~7x$iB^+a==xVXk{ z0>M#TSg(ww?AFZ{Y-pT_)UMqutZE8pG4Z=u&lGS^*0g7<41L*?t@}g~W`@vv>@N4# z%m{JY7k8o<Hc?dbvah&w-APuqay@Ifr~(HTd=e=oM7i61b|R|z&VtpO^Mnpl6HKO! z!$w|00#n}+ubiHE|BN}?<EKJyW}PL%JY(E<X&fqv){?xlSIJzjkD@mlV#Q_~V~On8 zSYgkJskovvPEhZ=jIH{ljky8lLN{}H;?wq)EHa5F?Q?Z;h_ad_O;r=S=hza>k*CDA ztykEgH4_Ed5ov6#{yyS2Rh2APkSAld#j{Rh3fQ&dDuQ3a6Y=)wp<>lhXM|bCJK53# zIq|V_T|5+!AzXPgQ+TR1h3r`=PtLC}!)DjftkPCmEPtv4J7cX%^0YdTC&M(D={N)S zJhV(WQEDo5Z3HrD*fP;nudnXc#hX|LnJ&)D^dLT`HjuM%N@SqRa5BXHlCV{3y?FDW zM&eZK&5o}y6Bp;DRrnMI2xSAKDmHGD6}o^cIlRV@1S;CNDnz!4Y^Sw})D8nHIoC+a zWe1XAss5lXXcN5(DKe#Zy7<*3fqg$J1xHSVi5I8aFr%}o!lXhO7Nix86Ru^jP3>p0 zlbiEcVaN=!FgOAI3I>z>u5-nfY8~8<@<6t8MlPG*O<rtnB*$c`qM1S48S&<Og{;Hm zL?PQlooMF^xM0IU=A$_QO&83@+Bv`+nH~X=EEcGaV$&z4yRUTb$g+CIiVY7pS1hwh zWHM^&$>({?1;Z4-id{Rpi(Mv2?xXBbQgv^yc*0pZu_&M`n=(0-4b>|b`smIgBJpK$ z@Qstq?c*_KHfo7j&Q2DK>f*@ao<ZcwhppmXabav{Xu9ym_8`%iX2Yyq63MZxheVCm z<-#V<LJ|`y$5iq(S=au?!pyRv#JgS!6|O6ZzK_i%eG+TLSt^}zp>7vJJ=V^><v|SV zpqD7__TrrIsogF?<KZq=Jw8VmALk;TZTwcSb2%<PrJ+L>6iy{la^>#(@?=Ep&N>uT zDU#gp`NS=+)qU%PJ~-v|KGv?~2+_=sWzVeJv9$PG>`?w>csap^n0=4I@OAB2f7@_a zKJ|=9?ejKrzv@-R<FYP9?t>Gn^N?clDXU1PYY{sad`oz``#mdlS0)`DYnZ~d#bn>> zM&@@yGTj*Q>+2GKJ`DJ6RW-n6wSjx!*SCUsqdpVv9A-sX>tK6|Hcsjm?bd1LHnIMT zg=n~`C%)V@6rQy|#<o1_B`9}J5|63%B6m;6i<^!<7gn#lE)FNN#TJdm?9vV(!}l1o zD6Mj#_QWIczgd<2@l!BAehx^oD(hRi8P2a+$8=(3Q2W+N98=!J)CC)Ouh<Dq=T3u? z+BKx+o)_#`G=>a1pGKnBtFo_pedui8EZE{YhHI@;0*gX*I!<*L*DKfpea{wwt9uby z`7RZE<?QA0hKbxGHXaYUZf6$jdypwgyTlerU9nKIy)cY-1gYgNka6o7-oBI!vf5YJ zZucrYsMN|Dj|GX>QcqN~H(`ZE`^fb2f$-?40%(s<V$H2vg`GuW_H27BEC1Yqk3IR6 zy|?bb#i6$Fc(I;G!KZXs7xo;ydisE&#w=X(C`FX_XcMfkt)c4fz3{M)J_#D?&D^@1 z(j(o@fP3~BVi|l4OnRq5<**vjw!AfDgM2Zjeh~?Mmac#$qvT+2=`faIIFu%=-G+0Q z39NLQCCrE}V|xer@oTQ%iR;sRaxr2FWpQl~f9erR?@(m&>x}vM#}QEY!GP*)bEo%Y zi*V{@Rk$s?5mvABhxaK5X=%DPO>Wh}^UJc~&bUCKGV>Mg+^-6_H6IRMKF(YMZOGy` zM)ZVG=bpb-1}13_<$kV_!Xkx9Lh2V=etq~}*zaX7YWRMZM9rBB7msIvRl_jiJg`PQ z^KCfU=N5<xndV$><0*CkR^wRFP4qZw#$Sy$qTMp;F#f>_@$A0SP^Zg%c%`7^F>}u$ zn(=WgfB*IX*4|%B-_NgS?w;GBuUImBG5<Uf&EF0K)ZWn!$<pw^J{6v_D;V%15Y)3A ziOmZJgG*}2?dCJ=l8FhN*R;e%4PA&*o)MnDeV#Q-A7d+X?5Nx)C+=C;8?APa;4kM} zd%SbH$~(sFW}cgZamZ?O%u}=EyAn^)iBDubH2Y7ZMM<V`UB^clATx&UuNzKI_LvFB z&&3KwF0+}LTa{o<)=-nqli;!ELgu}0A{igAM@Co4qP<HWcs5iOD{NeF*6K#ryls{+ z=dM0Hnl)M6a?1%H&&wvBcXNr{nDIO(bUt^=-Xe$_11e>GKGED1bJ|ee3&$9&z}&q7 z%(`Bi>*~sSM1<?o(?inOyv9w?)-!}Ic(I8Qiy3gYX98Is)lAM^FN7uSo>pAExK&8| z@P>JgP=Y-x{K4dt9eB4narse&VueM@uyE`b!ObU8c&#y#*A^3YIw^suZhhctbl8fn z^4LJ*w|;<GZr`btvjV@}DWA{06Gso$?V))d9bmIz6xc{sjJG^G#Ro=T$JFeh++$-2 zR*%Ty%j=%PNbi}{&|^Dy-F+S0gP*dvtq)<1R15LWkz;p4jktYPE7>d_2*x|7bIYCW znf=GZ!g+T^Qr_Uq4t|tuXU{T(Llee8Wu7H9lMMZip6*2ZKGzafd7h&$o21~`5Jw(6 zdmuMRr}SD>8NQ$24g0xD^9>y*@nFe}_qp^#cp|_EH%f*BPgePW*l!&BYS0behKoSz zLkZhmk;PBW&J*|VA%&BI4&fBl!_3SwQm`aytogxy5)n8bXF2F%(8g53BS{%A?G9ue zR_nqC{oQPe{UsuI$q#PGWMgiXKfkZ_6c#^@WkE}((q{`bc+`q;czxB7H=a0+U+*hd zUfO8L&y?PU$41U1&Rl_GX+Jo!<sh4Qa3eBv#H5{R?8%9a+;gWL8|a)5i<?J)d+B`g zY3;3wZ40`?pyQ1!RBbQ}{ov1*&sicqlD`Dn^b}xgfhz3Xcgua9a{@oFWJ;H$tOT85 z6gy5Yq)(TBq>fvv@s_3uo#R|DI5>5Kn>llNK$;JVjUu?O@ohZwwTH<1at21Y29vXI z3h|Y{D{-th0{?<lEMu=X2F^$T@!0voSmUnz%G+phd80aMuDcH-D@@5C$@cPjg{5q> zY#S>U9<i6|I;a<WlikuU!t3qBc!6Y=SZc#-JagWvvUG7CR!O$z_8Az)jm(DAHf=** zw#)!EpGRPyhS#`QIfY+-V#$@m?fA@t*F@i!O+*{Hcrv7Y6GkR<q<dSZW967|2>TGm zwk(<r=DmvW&Aqu$3xO=Tv4r2*l`DSyO`m*Rnngm&D@nFx9J$|l9;_X0%F^GaLQZ5V z>W+_xKF{{Un5(uPt?t&9QGUJnhHWBD81tTuYIUOvK0U%m%BH;fn>imFn*bp#rqt%p zNl3bVl<jGshlaOCN`^}A{7AnjesbjjES)1oBGZ$|rD>A=83U6rXo@Usd1;Bx4+}|c z-#0|HxgDEK&C#=EA=$7HMa%shVfK)|7+<uQJx)Htr{`I)_HG}ced8#Pj+Tu$)3ZO{ zmKVjxS>2!o-A};7JM#Q|>mJ_4s*!hbOr`}Z8`;%6GSoOUi61s-M?0Pt_<*<eu;ZjY zm)?Dn%(_&^Pg^AN;xUFWB6JvLjM0F@)s~>V{WbGWcEy8YC0r?f$wr8lv-)rL(AYGW zXcX=sVe#$g+g?l@Tx>*N-s}SEa%Oycl%~h+$m`T6@CfUqd5$QLZeop}XTn;u({%B8 zhssA`ci_}LbC|e30Ef5Rzyo$|gy$1u*coaG5vm_Zw(~OFs1pk5`ZJjCbwBKV!3}2Z z9|TK}8-e_py~5g4gHa)VFL{1T9+uCWElxOd2R;bi5Ymt*$OROTy(RM8=d&4>@AsfH z9gov<=X+DT{B^Xra5!$NRO1b%18{(%1x|L&<qtB4(YWRJnS=d#cJ?YVO$etWH9Yy3 zj9AExx&lGU8^J^Vrd!Y91!ULqUKsEBh}l1|#V>c8nAIa+>~eWL2~AeSti5lUmG)IK z`_v0^R`U>Eg-7BU&Buj}FEeqM@>dN0yq)(gD2Ds~eQ1+QN4z8d2x?Ni0rto6H)Ubq z7pF+qY?P)azbksIs@OtZ-kpT=2TwB7^`>NJZ$ox+`A*`vQjMBxZAZDBX0&?VjtV2g zBs0|-FtaoSXFM2*om&QBL`<Rk&}SVW>)}|8Ju+9Eq&E+y?Y#nunUjh4tyyf1uN2+& z^erh}=|<;YdxUq_J*8hl3aNqSMl`*X1gF;%x<{@UWxh^e!g{2)c4~89jZMVQO@lV} zZ$y2s3Ap~yMVMYL!Z}ag;mCJa89XW@!hR`U#VojY;8^%l>5nzxkwjz9QR1OA9_{X* zWY^lSA>(7C*tg4tWSi1Ca%52lD0G;?rW7qF;{KaqXE#}DRJn*QOH!(YEf@J`8^g+z z+Xhop_b<Z2C9!y5dODBleGyXI^{Q0lkz8K!6z04x!y_Io7=GoWIOJ?M9Q;U+m(16q zNeaor$$shRRzC)tKfE9%m7CC=d!ql9!|nqVmtluebGWtR23x;&w7bd%4X8IAM>gFn zW8YI&GR@3~<ifa#^l+;=U6OZFeD+%gJzOL2QK}S3hkfo&*EY-qtJi^i;B-$=jLW6L zl5Ll6Ll)68gU>KL;3;Y8OnL8qyWo4qK{n@DBzS%@V!8T(xcqTX{{7okam8a5;Bgk* zuF4Recv-`g*_Q?Uc7_br3?ygQ-X>#o`he}rZGze<Czv6tgJf(rsSUXRy+(W$J}sL7 zLH2h1@~KMh|8NI=(((-RR|PS?PYQSTzU?+H-<P`RDN(<{lX>+LJGj*Jm3Yf0(A#OU znAQ6x8Sir&jV_#nl%W-pcI}0WkGj!?CzGJ#YYTe$bbEl9^+z>ox}(qxlHG-mN$1Ul zqINlZneJ#u5N9`#3tP2?GTC6}Y^BUkiq`QjOSI|Zl<}39H!j5){#~en&ONM<N<#aI zl5t}~17wCz;63_Xpd)sCfPqPGAp7|+XzkO4LESgv^!$FTYRG<?XP1R57a8)%T`F`y zmb!R4J4E_TEF+d@UXr!>8G`WyeSR#)6S^3bvk`Tjz*g1<Cy9+Qpz#!$Gi{Gpdae$P z8`O&L)0XkQhKtE{9Z6rXEQ!_)io(&Y_rW24Io~j(JJya1U=iD2K!={`^l{uv*eDs& zy$hO1`k5+F=#v9&%F(d+s27V>@FmKht;yxH1=xN`1$=Iez}snqsomZ4>`co@a%Rag zYCL>7Ry1khiEeWt_Q6G*kv5;&>`!+eu{EAubkd;x&c3Oro|DLyZZL(A!3Xikt|**X zp-zSrO%VsDyb&Dh--)-~(HFhV)koXxZxG}i07I~WmB~w9Wz!BOjs+L6aoho>bTI{{ zOw}h3N8e!{H?zquOL^$pLmoG2XYxshx`{u(e&BAPT83Th`n!kfNM02RiC_f-b#UeF zb1*n?9qS$?*}nef0Qj|qQ5DvnUray13S7H+e7k)Lil$#fcQ+-PVP?#wv%6p~opVA{ zZ5gd{t)RU=45Y8lhqJfizLT%84ptXt!sAhzX!+WeJ}qM?=dlcxz7e!~xSw=3HWA+` zWVpP^0KC4Kf=P2d_O8_>K3cM{tmPJ&VzW*>HGMQ0dEObGWu}v@)zL)R>@h4#>xd^m zT&D_hj+IFTUeIV{=aIIH!~1>}WZ%`d@S%M>>bgZlQ|dd=W3e^pb|VCXZ&(m<)C`!s zKLEUr*Q45!e%xWi0cdp^11TGqi|2-!K}(G{<W|*)Mpx;=+15(3#VZj4SIosl!~K}S z%SR--XD_0WybWYNXu{pzU0C>-W^%JvKD+!Ni&<v&qRWlEz(Vt}==p;bUbMRdHMo$7 zg|o{jbxy&NGnKiU{|P$j-Wa%35{j+uTH(0b4RDZ>g2A6majAX@1XOJ!PwNzD$*n2W zziuE6vaEM+i=9V<jAPj1SKr9Saoh3s_1$D*+i_wZ_k@jG<p&xArtvDPTKC1pG2#`c z^=L+X7kbNR1yu~G$4;MDc#QE2quR+kn5q5=SoK9Rej6g$b?e%I@6R2i@I|Szd%HA# zC2b?>y*niyQ4L&Fk`D4Kv@xKvhP?>g52mGVEa$8vJ;U#kdFEFkXv1`jbZQoc&C!C% z7QX1&zL6C5j3nb;YQWqPX6RWw03wqJ)TgJgJsFbMjJN92*AG*<S@B`Ae9C~z`=8S2 zTIUSf|BW5h7&@Mpsu!@4on2_l!F(QiH<337T_)bUbcpx6A>ff?L=V*G<0IcX2!3`S zKc)xc#O||U;oak8k+K|pF0~k(&-LIU@`cQ+PGl2wICl3I*pS{n;My>P9U63#wX2#4 zJLyO`Qyj&%6`0a9wWT;lHjc-=^TPp8Ybw{xeucq`mvNYA0jj+<#L#;sD4m!KH;3OR z=LR-YuWKGO#V-Lg?$3w#k6#73YpeL4o@aoM?Zub$IzqmEm*(=_vuH+o4Gi&VCXTXG z{|{Yf9#vx$?*Gy#4I*hy^IS@U&UyEKGd7?U(m+LqNXVR_i71-WBt;rDi<IiT``O8u zAsP&YkPJo1Jo7vEch_C_{&DYr=dZK&I%}PC_WOLk-_Ju;&P?N9+%*PPkFkRT^Ok_P zk_x;QCUR3|okd-xSFrRrRj~Z^ipZ=A#ra#dl7Sru+10KK@ng3PSn-k9QW{jvI<B~c zE~prRY=jl9OAuwF6mLLHn_<f}QBRQV`i=9NCn0y`0vwrt2Er2$fZM<(Dp#XR^I{gj z*!xqcp0otWY!N1W|2Hc8-*b@vkm$MXI*)RQYvMDsYhOIscFqycW@ciamw$-d?IyBW zxwd)6<8;EyNW_m<m-4@@jfdC24ihzRBP=R@p8PbwM^-PaMOF=o$obYPbR6`td|4j3 zv(X-^eip&!7vlKkm3&mBCWEgu-+_&K!B}+v3NmN+dGdcy+5bA;|Ddwt%EvHA+8^*n z7F0A3eQw|r+xz^NQ@r_WqBir-6?}B)ojI2uJ^F&5RIJ3U|5+(m7k-83I^SGSx2D_i zbGiX@^Xz*5x$|52TIz=dos-uz&CSSY4*Odn_#0Z^JkLOmA3Zxn5HF`INbZbl-uI}x zY1Q?!yrC|2{`)^EjCR#${?>s?LGoE~-YPQ@=23Jb-|RW%&)JB14}A78+}%?Gn^zXx zoLzz3X!c_M%&2#~V`oP$#94GRzqZSuCDSJGD?eCsfjjT=y-#Zke){j>CJrPCRG()F zjz=vOT=(kbqYx)S-XDM7qudhy_EHgEK)C?@E-XNqrj_V<;~eIl@@&#%d;~qK-_LbU z)hFf-T5!)n57pW1YQ88di}yt-l8l6ACTxQ-8cJ&*({g%Yk!Cgu4mUz`ce}%*no_Rw zv=w?Cyc>mNRU>Ta#l0wzA}wB*nffQIkfjUDY&+N|@E5+=WWbR@&pz}#;Sraf>c-w# zZAC9J?_t=|1{Vo-Lvk&~j&bU2kT=E+moMSqpTe)hpfRN9|HMBoj$zi!*Cdy(F>r2O zAmg%KhSsfog#Y2o=t_PmY&&oX`aMlanExo*m&1T>=XYdQ;|q8dgZbCSQn%|mF!+$q zJ&j#~|86)ALt7=NRONa2!pvf&0uo?1{6<mcx1j9P1RVe4JbtdLNv~u?6W!}guv2C~ zdr7eunm3jJB;AF*RyM-x!vqrdT)@AKsv-7-JkqrIkEUV~vR-%-^R7PwgCtWLdEJ(c z6?ejm>&{{QyXvf2ju|doBu{@nHDoo%NV3Z$8^O#LV`ugh?XX%zvwYRyNNXcV%h|JM zH1=c7FW<<mR!vsSbSbR9oeO&+n(!#T414xW#HNn5kUV=M7)N#E_Jme6&g?DZC3PT$ ze`@TBbC1X}<1RG+L?CZ>@d>Q#Btw14AGo~AjGyE~;E;9<KJwLrRcw?*Ex$8xPL~O* z=`I4j_kUy0*sVlNIFczIBMj{He@3jZ^svJv1uuU8m^r+r2>83p&`vZJW@RK0vO*jA z)`lUeZMDJ)b}Q<CL?MO@kx9==$;>BH>1By=*msNqP4U#E^HC{oHLfPs4QFwpHIKHN zALr6_SF@cjbZBwT3+7kgL-=}&lJVomzzn@-VB9&CE?U@)FW<-}E>jMZL~O&Z7Pg85 zCBKk|Lk_Ih#9TbMU>e$9`4q?4xWKQRa<KN%;irVjQNL&Far)BV%!K1gG<5S(rXanS z*k6lf4_we@w{ObEbFC!lpN;}3watfJSe#WQT`>5i5-nS=g&TFQg7+j%Iw<f3{VU<H z?^g`1_LQT`%uE<Fzi8O7?J%3nzl?qS?g@f-pA;NAoXO8=a1uOwa#`^3r;cEyf4yKZ zVw%9-dMalXQ_R~|?8r#k8#BKr%oaQVW5)&KMER-zQn>MMcRB6%8=H-yM)=APojAr; z2$Ms7nQeJn`CeW%{7s?<_<GZu1Y7Pc;2#<*!&mvZw>cm(x!GaE)8?y*9fF1bH2CQ3 zbOGpY;TexH6~wxJ6BNDq?KuC030MAVm>+gEiLdwVFP~;CZSJkm5_HaI1?gtlj#p<s z;ahvp;^!H(^4-0Y1b^;7<ncNm^Y2zQ@E#7n=efGOHiu5WEjW5un?HV2bkpRBI6>xb zaBTc_0FL{)kt^5iQQYZ7GDmL$1YTc7ZfWb21&dBGo+^W+;r?;1N2d>&OV7kA1$kt@ zr-1P_ZD0oOUk3%%2f|WC8PReMBwzPUf%UU6S14_aO{GiF1Fbalde%Rb($h$)p8ONY z%v{2(RW~FV2KPwK&Rk}2Z7Q>H$ufAm>nvO!34;^WO0YCc6rW5^2W=N4+<x>8GI*R# zc8WAH>!mhwxoO#G?Y-Fyv%i%*kg><dn#Q3&-jNXRZG&}#OW{fh5GBtl)Y|2aqx+t4 zi?<k(Zhr-E*w#%VMONc7onY7()xhK^j>V>`C$UR^GFl^l9Uc3%jeME(htw(*l9UH= zT-xUnY;su(&v{`5S2_S4S~-EUc)A-^4$6S`lYfjfbfTh1E66tg&2ToLo#cdOlFe7T zA;a}K4hbD5dlJ;qUC}|ZBv2V^IcQ=tNd-JtJQXkhIZ75UlclfU7~um+=OJ#&IBdIZ zKIRo%05^jTFv$N$&dPLx?Z-^~^H4R>bd82YTM6dl8ykF~Jqb>YDqzpYV_@Z<Uh+gH znOncC20wH3W$z{DzzuORyrjDpCJ75$8xQ+KzR^`;aM~OskBRZ8J`s>t8mk3*=R}du zxIU!vtRLy-?dMEu6_KB~19my)hZ3KtATux|$3<2$C!CWQ|9hQW)RfmG<}*jGmW7j# zm6#hWDkO4R(wNEALJwpAaOd2Th^YQlydmc@*_5Kq`M>n$f0?7s40>n~^DoPY$=gD% zJ^whfF8(Ch>3SC}wdx@s&U&GFQ9aCB!#5=Ih%&O+I}!X6ZZLN|by2I;WI^VYD&gnt ziOTvnqh~LYP;Rd*=~%9cFF&7zRIHQ8pG%whXVOQQ=%slKzu^`4(yWznw~auzMXX6? ztTH*Th4G%&5$@6fP2!p~jC!9$LyuH0&Ynj||EGDJl7$Ppb;T0`3%28AUk%PWuZ<Y= zG?BSA)sXe|DPyWOjq_3yMbphMAwzv5aQl+Q_jk?{c)2Y_=6BVYrmh#{qjWP0I?R&c zEjP$$Z~!_NFo;qj){xfq-AthMaY5nb+x&XDcIL{kfQ&Y*BfE~Yp<Kl^%&5#|QhNL) zS8*Voh{_8eqYj@Zwc{<JNA@>4xu*gxi<V>LQty*g`${q#(~2^}1Ss&!3}kJTM^c5U zvQNRoTzl&qLB?lgG*q|=<-JK4(rab><m+8zKwdKQ)Dy|~^10|lXfsm(_KCZI+L(Wv zJ~M4W24qdhC*JVCAto(hBAM5Q(OeZTZcfQ3ru4KKvbkwRYM;gmiJU!YIowJjPgN1# zJUz5iOa~dwIz~P#k8;l@+MpYe*U*<^wrKT?N+P)ahnzP%4KZU=$=t=)&>Uglk`4MU z9Ot*8V()E4Z2%G9wB+XHn(^q0YB|@t$q>0Vx)FBfJ<ejJ1WE2VMfAl~arjuodFsr- z{wp)^(o6ris3WZ;<3ljFcA7DsNxe~~pp^^-1i<DvGmv<pi}S4v1S%_!l5ta1!P=cd zv&8|JFhvF=o%0yOtqG8=H;svlvL~wHA~<MK0ns}#6-7>0Ap1pQQS+U>AhxO;rI<`1 z>S27`+L^_iR+$BVmq+98zrQeP_gCV+lS{b9ejjwmOP&}v-XTp>4e<-njhtzD8`GbZ zjEfwku-~Ie+>0DNGUSo~!|xYx)nWiI2b}Q^k5D4(y#gl%XEm=41Z0y`Mz&?zptSLR zNc)@~ez`e-$##=~KT%@PK2{9+f|o-krg)Aei;6upa9H{ncycxc4sQrUdYet)bln5; ztuqa67>ovgje8)o-m__)%REpW{foER++YfuFXB1d)&t4BOwuYI!j9_Aq}2O48EFPI zWG;zI(=EY%o-$l_8%I>!0^r?Fp)sQW7Fr)zf}gH%rjh~EFuP|W7~9=P=drN3wJH{- zE*`*Qd!6zBN@f51p8fwH1A;2HPUG}+4ak@MRy5GM06pxSi3RJ&@|$#G3BT*5uu5<a z`_Iuw|I+*fb9%?q+T0B!PIDe^me7Ku{y9kVvnN;ICLnU3efia8(WGQIAHAs`W%f&b zK-0WknE!*y{?|^4|3PJ)?@N*Ob6Y?u>m*E<QisdGK9C)6HsPujQg~~SH@EAM1QF}* zLp=YLc;Vg&B!62W9kLFkf~|dMa%uwmRl*k=3n_)qgfnz*M=mWmA<MS^Is=YvnIO!Y zIn|k+LwuKo#INx%W8W_0^y#oM-IP`fQ%-(C$CA=85zFWLUyWs1>k+IjwgY`{_(g1r zTR=<ZEI#;(k~kR^QoXa2RBTD+mlOtZclW7+V|g8(?K_9gou`P~PW{C?ffLz<S5nF1 z_Y<7dmkzP7Rchd|rz#t7w~mcJVgyEJr7*5aidsIKLN&9znA>{;1sC5Qz&Es-A?=?T zmS1Rs_tw6H&XXDV`m!QWJrPEl!gqju!3l0NhsfA@^*HAI0#NTf$g@1V0Vl1MBJM;5 zpF5F=qV6um2@eu*n#FW@yX6FPKO!7rUd*N%kBit0wT<kOq@b1}8}XJ{A!)hca~d4A zB-vG`=hBcrGiesFcIr4`&UPq=lUw(0;v$73EQh_=u=%}Y%g#Wyd0ZIw&3uV(cCTRf zeUpTB`Ll%Kl4Yd2@H4n=abYjSZzo;1pQ7Je0tgj8t*)paBF9=on8(itphrkEKed%1 zjfThM{>8ocvP=&AD;?`3miY=t*7~xq^23m#P7|KNYlWNE>#&A}5_{TdBU^p%HJKFK z1zW8vprJ$&UauWyyz6!GDWyw{)TMmLm$SppzYmb8lBW#bSV%?^6+l<@E%~`invHpo zKz>zN!k!;niL76J)5i)!l=b;J*}U)w{&Ok?eH*`=EDGsE;@m+R8r%Y>=gn?;`#PO{ z87<jzKK}-sTyO~s{yGbujY%XA+^bmafvaSHtrnYpRu*rcx*Ko4xC<5~Xt7XnO88li z;xjMTLVw5_P&=s&ZnqL~L%$dda10h*Ye&6e?di8rC$4&p3wd~ABiH*tAH)n~@p2@F zIzFw#o9eq!Ja)#z=Hn?nQiw9*)!5M@WxD8kI@^*wpZ%-EQeF52OqjT^FMAx)8q?{} z@-cL!rvt11wwvTk!npq8UYI1l2>;IUM`>%%f%%F%Amv>H+roBXi{T2qy6_Q5MvCEr zXRqOej5Lj0TujFHI55i(r|>PE)nSQ;8SekvgXXR*!>bPz;@Xzu$YJOj%sg|Dh8Ld} zianfLYO^hnbA=*(J)xW)UU7jwiBqAwT+DFhi7yasMA(t-C$Xu#IeY!oAt;K#?3mqe z(ECaSoEk0S<kOK$T36(OmCsECHnZ@Xu|LS*gb}o@I+eZp<0#7>yh5y|$OxidSt7<} zA{pm%m!w+RlX>m_gjf5TY@Vq>`V2~Gut^JC+-^cIsmeLcYzw2o#*>6y4{;i>rWK2? zdXHbdIVY4O?V;Z@f?59)HZ*_>hOQ^gaLQ~omdf}B@csn!B_zU{aq6tEb|X5alFZ)J z`~xB0b2xHP4}z+^SiurUdg<C&+}<U|R8M|QEF3NpYkLjy=x!ThY4Ve73$rGBidE4W zvqU`kdI4<^CMvoWr_uQXdQKtNe&FvHg@Uu*h4cg)$!@(9M^?%mU?(0mcJf=8#a{M! zfUlp70K?O1@cPgu(o|oHm71=xVK16-UcMuHP4xmhE?ys(Dv!XyK?h>EAp`DzmOxjx zmNBn4iINIaE%*^Aihj<2$`tdo@b@+w5Z!DJoiZi-;P_(P+7Uu`Xdl60qk%0mtL&*D z)Y+-0xEE@gHc`95JTSdvMec_S1I}Mtpg;Q<->p~z3qlUU8w-Cn^nnscW`DtM{W<`b z7NJsIWgMw{9X~EM!U^8T@zd|BWYdo&u%BlJQ|>Opslr}*r~6{2v)2JO>X)OhZ)Rcf zSy?2*-y6F2i$Hyj0^K$-3AY;ygW@xyo$g8O!mF1qYYF<~MFXW$v0nK;=ns&i5^5f# z=5Pf5>U$7}9MFT=z2`t<i8H&khrueHB`9>vU)))E2q}fEC&7F4$a>=l^ep-=v@7jL zqaMz5KJN&zxK+&D8K=PQJtK!}Upu29uAH2S{ekR{x<RMP4l?=9RC1SZ$%W4opuIDu zVE%7D9gsM|8q!W!Ze>nyd>>B@Zabh~ukDD--<Nn#`VV^R@FAL>v>UuUW}-8SXW@<K z6>M2&gFpVzfVI0Pvy;a<;|;;9Xk}6jvM@Y=Pu4i$%!SXmvQ2ZjF8%3v+35<j>*QNb zn>miI^}OWhsRlG=y*yKzs0)4Hd3aRT5QnC%hp-E$*qiIrY45c$bm|?WmKVni*r^&7 zRPk&$>->H?-t)+mrIU?Y#wKUO+I5&6?cIW#X)!n$ucL!Xx6u0lU)Wk#N6JO)1Q-2R zg3kRKwDL<6N?oo97JhRmq!~hj@OPrj@gRv3nS@WNO~!tc*21XmXO0<C$F?=r&{ubl zXjYu!^y^MTQo{)MWO*8mJW~iIORmB6ih6u`<5Lo7Uk2Om+{EP20;HL_hka&}$;L^4 zV6Lt>h8`|c!#Uqtv0lzc^rg0nU1q%+6vxHGz&ce_IaGly7P<)DeL6<8FE69bO&i%K z{!{Vj_eV&tWgdLq)5&cRHxszN_&|!Kbg|q(Fl0=w<UZ{m25spWHcTfJ&zPU%gv%$j zDDI@#NxTV4^f$7LgRI!8-kX`iP$$q={X|#y@1|#z55tPDjmYdDfKk3C_)BDv!}8bR zRpB${oj@L9tCA>xMLsy)NF-OZfFuq+1R4H%Fn$xmSe|i53e*?*SUn~&cpMISV1ehg zErlN8L|0^}0Od7}z~{<vwzpTB%?*)kIoLUq+67N%m2XN>U$NP2%ltOZ9v0(Yb7UdN zDu*6h6$Lhlk?`<-00cM6&|9mum`8(%Dg4?(iWlez9RHj^4`&~xR;f?Gym>qM8~Bx6 zRY-<CGevNsm<Ud*lHzn!RB*7OKT!>|1V&>Di5i{Hc#e4>3|M#b0?}_`GOQxZ(3ue9 zsfTdf%5qw?r=2~MGL`aOL-Djt&bS#bg})*FI3+igx;U=H-aIv~qx2@)yRZxHxS12} z#2}C^mZR&V&oh5&Wk~#`b}~iSBOI-n18am7ZHb5!wTPWevm+GooP|8B6upd_;FdsU z{~L$j#k2^D+jFp~_;pN%jKRfD78Z7iu%}TJc`@w?TiC7D^0g`%{T&Pg=|Xk7yK@fJ zDB8h_9%zHOqEpmfu#9cqE=t$L_rpH(GAhW)Mt{W%@Ta*#ze0d4U?Rzt>MaBi5;`5H z3tZ2udeqUrjh-=3rmG`+$tvY*WRiLjM#6H)g$PG9OOS$gEgDZJH)vC}+m`UGsg8vF zT|%ZjbA+m}`%sEL0nUyG&z29!Grza_MK>RBKfR07z1a`7(;JY{{5|-%7{WIy^U*-F zkgQp`!t>H2Andkt6H5+|%x{`txn?<Rs2N75@A@H=)57!GDblldUy_7}kBIt98OCsJ z9KJkZGhW0O0gXa6^n2C<%IzHojaL^CbTJX{zp6;T4t#;5%^&I2_a*G?$WS;Gz6T$; zy@*CC@5Xt_*Fd)HFPp6$LO0DbVojIa#H!Oy;nT|QRC#+E>c1KXPrf+QaV-akPCG)` zoyF)&%tgF4qym2t*-2imN}>f~v*~B?TEvr*5Ee!<xxv;I<V;*DGovk&EL_}$i`UFz zyDh`;v%G)UvS2mK*INP7sGB{J`+~h6n$L;}dtsdlVR&qS7>piJ$0hPM=;D2)mh&Ae zY2mwmoHq3+u3YRuHC~1SkyL{@AFJuYl+WOBssLSSccqgiIwEx_!mE^=AbY$4S=ALs zzGiAOwQoELoZpGIanhJ=3qk(c6nWpvBD*`v@fnRq(vu@hX6?R(+xz19N3L$fe!mRq zkmysoKuw*ER#}1l?mUO4)#gyPp@1{n*H25<{%PLqpM`jn=JVDx4N#_AoLx8mHNn&8 z;4L_fCR#QF={`W)8*9PNe;LYo)QXz>hlxqsN#@VN_hjJ5Z-hSRA#?X2oO5_7lAUmr z9P{pm(){}nGEo;LBp!nZq43Yub`RxRL#U007v25q2{;SOFK<iBv7wR)9ElO9>$CsC zKrwDve?yH{_jhq-$Hw7yVbUz`;4o@4S^&03lHk!}TQL2mh!fYP(CU4au&DY58QRFA z$x(*%-77UZ*WZJC^roAHe2gb)I_lh~qzS06W|Z^`Cb0I0+PLLyi2VK0j(u}1sQcn0 zY}Vg3Ex**oS}c94S?R%2Y#s3c>u$_q!<U_aCo%WvkDNj_EQXIgJFDp6(eoHq=i#>x zA7ORvXi)C|gR6aFvGL~=uBGb<?voUXOKbPj-aujayLch+cgn%Lb>b+0YbmGpJ^)6U zx%i>n2=m;PMJDThlke{}@u<ENwDp;i+aE27WK0j;d2uaUz56$HH5uD-yO?DMuPk82 zf*fgVPXhfb+5vZZ2H9<uk!<dY8d(2$CHxxs0skR&Z3^<ofA$lS_h=?ws$>bjU20JI z5_Qy|asrj<{$)Hb*OSE$jks%*UvrK3Rk7xtIMAAELw?pRAmRqTsPD%q(r0LjZdUgq zve}8<Bb0yrc64Q*OejUKiWae#@^k5y%&X{jB||?(PQV&9Zn$TABVK~fus#{T@n0J~ z*2^{rC*Immd*+L<c02du-EBNNrtt;rnplQ=PsGrY<p}OhnarAJ?cfJ3kmR%8;iPNO z09V?3GLDZ9le9a9g4ZpUNb>ACl$UFcE#J42(7t%$>UaY_m0%M8(1Pa8cX!fGyoYaF zPjZ?c<;B`Ayo(DJrQmLN6Z>oGUb3`uDSOD{3@-iENj~4-$|%uOunC`Jjz^qku52E~ zUG?kmebWYLd~gr1dF72eqRg2X_8CzduZwRyEWp%S4%b|?!lvIe!1hcV8PRDV$5)$^ zU+uAEL!>91a~GDDtme>>!v(bSlneGqG;BHAQ4Zn4@5$WWYC2ouE`9T;oYV~80Jj@O zw4+eQX=W9VEF2og*0_qolQJGF6{!fv1X<|jzAx;vU~eejC_`7;<>J2&rLj_3A}p`E zPNc+Q!GZCDBX55aUEcw;mOmYLtu7_+8t))9uN}S0H|1tEOaP^MvN#}9gzbua12T8b zsOZNH*mow=qWM&sTJtH~kuhqn=+ma3eoC@j+BDj|Ro&^OcpSUo<W%@_M+~$+8Q=-2 zVr=(;dMtCQ4ofWDPWtrKp|)zRQ1I5tlNA{zzUHyaP1GS+GPDz>cG%<JX&lq}PZHm} zRYz<V>>w&iidZ*I9ZTd4!=?+uJDqj|eSiJ}Ox)f<vOeU|+BHwv4bze!wnV<=Mf)Ia z^6S9o`1M?%T@R&cF|4_<G7FfG&o|`a;umA-s;?6C&9+p$t>qZ4rLRH4Tpv`!R<k*8 z3LtMXg8!Au{`VZ@KdI~@R|{_2Tot@)(sWqp9EYF!Pe9>$y4c!E8jNFQ;NR?2$ej3G zAaOheRhK(sgK=`4`@KG_y<7{QTdTlmHjE$(WDboN0lUdd;jk?Zoa{0an9#WkI~FI9 zyJ4&Ne;O|^$s?-BHun#z5lEq<d1v_l2bKM=<NXgRTOofC@5<lI`&3y9=ALu$)$}B+ zC-NFw^b}$ReTpLuwYajdc35Y};`P#AIDCo%o!GnqZ&>bv`kxuG;fgFy5@{n5PqN7k zsVVFkX?ciS8wb(lf8e3d8aUsqK&76)Al?lzD5~;4N;e7>daJKu3HvBeK2!j&l%vsU zg?SLd4&%k`1*mUkHx?<fM(N=jgw9AAc8qTT(GqB}{cmJgsk85(wWa`mv8FWkdO7*y zu^cxEDWdz+we))BOPG21CdQ}T*nCGneB*!#Yno<<@9wao?H}KRf00m;_1FWizuF0l zd;{2bv&T@+Mt7VZb%>~*&Vj2&A?&YTKJe+L29>z<j0BA&g3jWN_{O2xL}L5_95PRj z8hKwshqfL?hmAJVgAQ}i!{aaTzfbM>hSgkjvfTy;HQUh@4~p=pm=P?=ZpKD6kKk>& zEASJ?(;9)DK;loJpleT1vuN;r!S+xsp5>D9%#R9_X0an;f)%=T{O!h-yxJpAE=0ye z3Krg<$uDXT<<zDY@z2u_O>e)5ao-{mdBv(kFrlR0(LF_+f4D_fkd`~b*9@2Chnkf0 zgc%#h&Sw+&o-zjfiOz}qF@kGCkXq3^FGR#~_w`S_V-;TfhMJ$vB{uDX?3X_qU;p#x zdwgO!rR(DaE`B%oua95i{favwFo1XbE6Q&Lx}hrkAd_L<<t!iGZRMBzFEMX<qm?m& z4Q|Ff{{$UQL_>?$IN-wzm5*r(CMM0E%0~pz37rB1q0|1b%1QEQAf0?j&JZj<AP!A7 z{N{x2$vALJI^r4bAj@B>l9|U^_zljl(T2wo;E=VQwAaT#O#CfI=~6mVmbxEKu2e+V z{q^909H5HLaZO@2&O|!-9O8yIpj~1P%wX~bwDDRB($3K(?SDc@Zp0YA$-oYt<vDA| zyC2$lt{TI<u9K>QuXvzowWff#`qOOQjJ|lm-=5uqfb<zW`wPC!CS%kDroEzk_4;JN z^57r5cvmxik(i?3AODu)NkI><T571tEzXj6r6oa7*O=w-{Y8c&FXNBn1<RAXA<gtg z$75%kcaCmv?yzK<|M>iEo;z)dVB%|g!7KA+{NwIz{Ig?J1!;}Rf}x3;j744@|KN!L z!MNFTd5w)l{7TDefkMq>W^MCDfk)M8!9A6!{0>G};MWr?kPGH`lArH2PxjYh8Z1Nv z8{f1y-yTlo)t*h@rMJ29Q!EPkt)_WR%YQ#1mi$JtD^neD(f#C<dM&f~)>d3MaVFCE zJc)GH#WB6rQsnvV7bu!{jm%B_CKy|B5^}yZa<zK-=v9a($kZO?Leos3zYZg_SDxs} zrWTSC(T55vPBUxzn;G5vy6DxRpXid`6@Ij2CgU{rBX^!Hz;{kt<HIi;g?>kAeDmd5 zY+@v&+r=gLWzj7>;C_arKQg5!u`<idx{51>w3md%pi9GXgzXgI_9HA7+#L~)0KO6B z0WEf<-xyLeC&5U~SbBI?6^K32f|iTDq<vl+j3GvNX<jrO%FV@^-3bsEpiT!}vLM{Y z&T)IJztDF(9dh0i9J=Bl9(q;E^(Pjy^(U86-=BbMjv3<+l^Al=QU=aA2a$(Q=Hi`6 z+j0Erc)YA6nC7l%g6!wXct*?;)=(uMCzpPNx)K$v+3*^lT_#0$sy-rDtc8yK^M*KK z{!gefyec>{v6Jufb2<pB0d9qfP@h%@_Iqm=Y_9r^zmKXC&3Aox-uZI)^g0atSz^e3 zn+WD>+R&e+nJfv5!1o+}qMBW^V4G40NH#F+A-NSOd@_#}y>0`0Cq$vf=s4EQ)|gF8 zmt#GQzA}*yUkeWES)k<a514uPUGWCbu_SM&2|0360a*?$Ck_!2$XJCVi>x0qv4s!N z<nrg_)Bb3puzd^eiBo_Zs&c$4;Ye8|-Jjf!%4Q7eoR~8X)?D|pbY_iwBpTcyL-sV~ zakIZX;kPkMk%U_sqoMhfyG<k+(@9^+@G=RqWbO##ZdNZi`fv*;F{y~#QNNfm+b9N_ z_WMY#T`t<DJ(esC4k1cji%C&u44LqHgsZw-$~}^nCr&(3A{B3lwx+rg^Lw@E-HjfB zRZ0P4_$h{a)V-d(@I8ul`j5d!w!G*6GD$?oV&zCy&|=(^@sIiA{(!vO8A%dS-BG%v zJ}UGT5>G!7TrfqJ*<tjJEZOUe<+9H49ab&C@$FXV(z0;;^1LN_Vt5)!3?(x54lQKO z+$!`w-4Ir$9Ab(e#c_B=8QQNifqbGJB%<dc(n@_nR!D_HdG<8)BZ3lcawh-Xf#smK z$%8a)x{3G6JY}BWwIH{i-z5%0$G?S)Kl;p@EU?y>$CkNScvo=};WwQl_CLl!CT|M) ze)Tz8*A@=G(e22m)RM$b*@WuFI=Ri$vr)+LK5}JY9n7e*Cz>-w@zw`^c&)?;PTu&M z95`2w?>5zO85edz`<1ES;@}Nt<s*zl>=b-%MK-#hQA5He$Kia}W9ZPr14y*O5Dy0J zC*!Bc!gu)@P!Kzhgr81<^}i%hr=>e`7L7o5SvpKsM=P1E{|jvx{*4wTPa%T9IXG(b zTqygpgT8(J42jEH(RYhpp?A)%jB1)3K0l3zop2KKInWm@zQk}*4(~{n$rZR5nFmF0 z-gA|{3c&44K-a9>@Ro5JY<sLDKV+^dt6h7Pj0Aob5>#{aG+!6L%PJu+)l5KVRTK%5 z5rgzU{xEHmJ<Q0eV@|!lD7aGFj*9Ok0Gc{VR9=3>gA$&|UgsU8O7xN1oA=S9>B~9i z`e)4Y{0x|UAqE<pE^>B632*GnAy3s0ayJdF(Eedl*wOU}*$bv4nMq@?Lsm5BkQ<8w z<cGNglWwSSn*sNJ9YOtFQdn4V#{p6Uf@L4I*`%B*bSx?XB3#<h<lm2(8M_Y?NIHs! zYwhsG3=u$E=ArQ`l*o>sdSKc0mhAn^<5t*O;(=fPaQAfsZrX9-4wyBqC}rM5^zP<# zJYi)ZX*kzGPMm1xOulSEDpg9L^JW}AU`UDbKSO%&iWnGePNLem*0A}ICDi3SfiC6w zpt{9``PE&^Xzw}BJ^JnnKPNsWvBG-``>%r^yWu3h@VOEtjGqNXcJt}{nTk-{c1o!F znSy_~Tf()Z%Ve8V4kP|0PY|4bnDgf(aOMvoW#o>MC2w=k_};O|g)f7WQ<gI8H0~oN zlya4p-vukai=Y*YcXB4R`uNuGRd9cEufT4&UvMHmi$H5Baj*Bokz-7d+sFm}1e+?x z+$onkC>UUR?#Q4*n`?a23r89E2oscbe3(nCTENuHPDc$-M992L_1tv(FqHfo(2f4t zr0xAo=Cno(cfo$4pwuUdtjxF}IQv%|8T6_$C3Wc}>x?}5Qt?qxZZH<jG%w`_voG`K z1=gW<n@@}zy^Crqa*6XE2a@hS5j{82fbkwPxa{uhWbeIo=$4!c{8*pCY?vM|m@Bi6 zD8EP{`F=VO^r#F?*U}}Nya+t9u^~qns^g>2E;0V$mx!7$SSqJ@S=iUjC*2!`@8J<n zzMTDqUhKR^4w-A?u@N4`q}LB>E~;UC<RF@0=tT_I9K>!PO_3Wr1-H*%Oct+I!%H$1 zP?c6Jjx&D7{jXH^zwg=qNo4~TNy9r5$Gy<2LpS>dm<*phPW_H04Sn!UutWYR6X&i$ zd-pGZ=Me|V)Rrb3xNjA+;eHB{++;*v%rGObaxNj02g#&!=3DMt&=_d_EsWLdInRYZ zV9Azp)Bnq0{9nuI{|A-zx^<RX%u$CXi63NjTMvrP<D<$c50KVlH|CSYb9B*TqVTV- zls^=G9<?gJArA}*UOj$4Wkyd!@y;P9vp{*L;KG-n=ch>vZ^Y5b!yc^WyKH0-CF-<H z<|?~p-3`2Ax;pFr;wRp&qDs&15M|@)@4$hD?yT!|eJ6?cZE!p)o=yIK0-<kTaffXf zjUTk;k~WWleD)p_HoY7l&z=tRFSQWA53cyzV*^^(wUkq=e@B*t&mxN62VuD6F5?+H z4tlowI_;m>){=LMp;a!2Aaqj!|CUh#q?Y7xgZWcQVw@js+-J%j-I<AGCWx@xFFwO$ zYA_XDaS%FFRoQDv8CYR?544~HR&IeTy=I+)XRcFcZ?)H>$D*3}@6!;pY137r5g9@( zOzv`vybDS0Ee|TuFO0`#Uq#+>v1rk+1mPJO)^W>Kbhe|)X~?gxWq!$2Hu-iotZ|ox z?CZs_PdgcB4Q12B`32PG^blzEy0YUohqBGC{rKZ2xfZL)llYzf2hM(#51U*mOE13G zVJi>&(f!I7@r77Ki^kp};}e~sJ5ichvf~6kXx)tVOY$M*g)K8(p#&s-b;vt~tteh# z1Z%g8v1u7sp;+o8wCN?b%#zi0lF<})PZDz|Pg>ZAExd%cocsp!bq-?IQwe7Mz61sy zKViUI9)CADLcd(t!ye9D2p@DYXq*$88z)FnIsNA-PVO8$d2t__vn_DPl%q)Y$uTs? zK@Du|r*mtS^zpA{p7`-fIdaamgcSc+kLFGvK(DP4NW46P)9Q@qjH(j0@caha<H}>J zN3VcfnFsFhUCb(#pMgw9N6?%<fE1#mS>r_yu!M}zNH^$?I)v*diZ4{*nN>S+ROlhv zO(fZAI1pH`lVIz)8dj)ff%MV0Bx=MKX`zeAZKW#oeON@+=N#px7taA1$(Lx2hA?0K zY7V#I<Txlir-<jhjiPg!;_&k2d$42fIHy>7wU&{rDr!F8n(ow^%D(s^!S>p=<3wpA zYMbE0ZZT4Wy+PB+yygl7HO@4tE?wANx(P`ST<Eky8LAeP#vIqJgS*1(S=+sH%%R~U znAz$A*)2=q)|Pk3c$o?AYLbTsH>$`#Jp(dOzZa(2&V)*(4ajWMex#?Tjb~o$$BT#o zyZP25s4cQ-x#ExMJWD@txg5rBJy#CF7gNaP-u+OcdZ49h;&C=oN0!~1FW^qk-pX=T z+AZ}Xp6rurm%+2a6(8_+W&;m|f*{~BqZ}vf@hm@rRT_TsSFI0X!YY(-PsCSJc54!F z$6pC{y%dKIwOo9xW(8V(n!y)C_Hjo-RoR35X54c39+nQdKwsrl(T?Z~^i${l|4{GH zpMHIKhxvUNGaOA?em$otV<Kq(4Me~0vV}e!YvyWKE>?VMi+Asf!y7K%!XZJ1c-@n9 z9A++u!}lD<BxffZSvmnM_XNO%nlEH!;$9^8-4VtJjeSBdC`w-tixb^h^e?jrX-IYm z6xU|rg1}K$cUu?CE2?1b4KH<y4tEAS$F1z<omqIvo;>=fcRMtwy=C17U0dGwTxJ!= zzeZyGr--jCPY-XbgHYc?_`%c+xVKe}Eg8BB{1zW5-D(FZ4wzHQILyY2JRoaV^TA@5 zK1AqQp@>KutYceB!p3xw6M1EL>^^V&deLg8TK_HTwfRVE=N_QDw<d$y%^W9P>%bOc zo*Y%M6B72@)-6?bODVdz4wuI4r%TNZoJs=Eu~$RK5&iU1%#ZVMiXNTQGGXF=YF@O( zNw4)OEwL+xHC|`gd*jsDgsrcz#Zw0-qtBPovf4(}dtMex3Llx<w-u6~2UWl!dnH~l z^)}5OIE+?K9|wMZ#ptClx1+IUCaW<V3l)Ebin6}<?4KoZu<)}YdnDu$l6X6d%KOho zXnQuElcn6^WUs}3TTqK(YAmekFvhuF612!80x$mN1<$$+S-;sXm~KrH3Yp@dAWS&0 zdX&P3p0NQ5>p9rpbS_7KJ{NfW*2fF0w-S+Ae7GNw2M+6t&>LGtv^si(^zD<PW{2|V z)IY_1)i>K)j3OhQyarFQ(&m0l-O|TMr$|6%6GQguhGKjgKC<2^Hq_owjjjyd3niH{ za2#dgtKLaOuVX!RE7*;<yQ;xezeF@PD_n4V1_u*g=WrfxbU6>naQNq+D(vdLV*Xg^ z5xPr}1V1oF9nzC<PL&nLk!G+hr-}%C%&Dc#1U4k$E;H7yoApWQW4DNRvvREt^w^q> zY{$v5tVQe)W^`}DtB~0(7gbc~f~s+}X68S{`(r@E=M|83!vzrVxq?cpxrn#FeF1us z!Zlu-Z^8U?`EcQ{K1{R=BzE_@(2?8#CVguub6`seTHk68qSA&~^&AW7p5m}o`Yigx z?_o?gub~st=5tM@BCNBXJNv%BfbOl1X7eTIV%LA^xT@{~Bl*UeO*naimcDDGb&ZOw ztcfJM#{L}lc1<%(=`y7|_lH5cvK*Vf?<9SpE?gy{=nh8PQ!$P^L|lUJBafYSD9^%= zSjemvgyz(eDXDu<PViM`V5b3CoI8(H;$q2~8~f369a->D$iYFXiBK+LMF%(ErJ8mM zbVr3Q=vGvdVC_q6PN@R~&g(*|x2mYwbxqbw{sMRDl_EX2Y%b^Y!;*ft<*@<FH;|L6 zo2ky}0D6f;;htHSapRpVGIg6XjFkGJ7gq*I`KEMEr9YTt<=)~?YJG?%2yT;pug4_0 zECK9J4Kg)*#qicaIk<Gs4Q~)%K__~hqAefCv`l(x1e--)v45)1vcKhHp{ssAb=8@{ zwn(c`sWf$`)xm%9u-Z)AePbuw`IXETDi~AE$A7WQkOuqudn3_acoplPn#+vH9>G^- z?~|Fgx}dkF3bom(U|+#K@_XK8aw|a|M?JrQ3S9fReUG0o7RQXR!|kicz<QMFi;AS( zw;s~)?Gkw3;h>fe8hZ3(Q4Fm(97bDb@Yz)@Ijp3@O>j97Oba~cIO+YdX%YI2aKo{c zY^~-j`uBpcvs)F7En=Ub=KTtEFxnPJT-b;G3I@^N@8eks(<zX$jZYM&twhRRvY@s8 zCh}|>MUKxF66MQsc>IVy*e;X<|D_7hJ7s`;I1_~fX6K^|uXxnRHk<vnB#O1!h*`hE zA29O!1s=OTl7@y2l5=xYsHo&*dT;e9y0iNVnX&dKygaH-&F;qI`3)=C$*Phq(|B<} zF9gD>rnxj_#a^!4`57L6`x?EQ^oa>Lx|)R5wKmgQW!U^<9oL)nlr&YxBYuPydtr7y z<H1*g`HS??$n6%m-FljRYEp&oUu$-HTYrt(?|KDMy>CD?Ck7`a`QjnX8tfGH8>)*Z zwb<4jpwo}X;j+p*s6y8VSKG>Cx>5($cN-Cb^(7cRW&+KtRay747~=H7mhHZ+1o21R zNzI<|m|DIg*H^ov()VA{MmKx>@Dc-}cgJCoxCP|t_#a56Xft{D_ZOF_IGIk=RwpZ{ zkV^Nzq6z_nteB?&ltd+=JU<s{oe991*@k!r>qX=0#OMm2Oyc4#4n5=yc+F0B^cbz; zY`?yN5BFc83m3bHT%Q~oFuRU6!4&3u#sU0v+leOmVKF9U{yXlt?jW}&Ad`7))<|04 zE+(~oF{s?>IpHhqM}~#vq@jKyRp56DlU$-~qUj>+yiKSODpG6l`z2uK?fU~ewI6cX zH`1X12H<k!PC8}DMST5B3)+_4fabW@VK*xZe@+YQTHSlutd13Q-qQh`cKZeHIeHYg zReF*e2HypXobPkndKnUv#X~{AMA2Rw3z+y}I-C+SL;VLNAp2el61!ZDq{B;)Q#fJ= zq|;f;!_o9u<uLvpnLtnRCP4l6L#)$Rp^<y2jMmPc44vU2ur1MvPO4TyKd;>+N!we% zuQD7vY`%%DoEp)v$~~|Ox2BVen#k6ibude$12t={A(65kR4sTsOqQC6BdW$QH9iwb z;p4Svq0Jcldrk?t>@$V^{Hz{@f9c|0G&o{j^Gv$%b3ENo7GRauvFy~~qgY|-SvJDa ziA_Fu2dq|lGb;Zok-_BicvM52E)B?Fr>!z#_4aDQ=hzFFtv|@0AN~*T;nkCk^-u7z zUKe)N0}pyr{s37g95rv6txpXfwUREmQFME~B(x|x;-}*~xYb|Ni0;pDQuVTk=!Inx z@ynC&Tm@Y)+ZMpyG!d{e_jltn=!DZt_cK)bMIDX#;Ys%jF0v0|g_{88N`qNdC*v<$ zNULK#z<ahlwphNJEYxbkL-Au<a__0a#^`<Qsx6CYll26u=5>k9teS>gjRx@5<r`T! z1yx+XbQOM!a!8vneK5Uh7s>T%MV><cL$lU?Mt?~OS|;m6%+wmls;j%$s@$D0Yswbn zrM!qvusFhQ7IB3c;%=m*K_Bi$Cez*zBUsGX5071}?liE>mVJ9?Eo&sI!q#R^c9Kv_ zq}o$`Sj(<gcu~qHnyzX|>$8r~H(FxsWEBk3!ZG@u?QtmoC?%JkI6>J>IVSG3Iu3oP z0CS`Qgc?k3?%Ee0azZNw9iMCr7k?Ck%BL8buVqFv2erxV<^Ql)gjh@Zcv~9Ukpwrj zl5lNV8Q}P_c+gga_OvR}2dZ&Mr1Lrn2^~w9x|QKc0rQYVU9`|Q@dlq!zYb@7zJsq6 zWhM?U0JVZ{A};O0^~dIMb&HCKIe#x42vWj=p*CWAs*<@ktpe}4T!thJr{W($Yq*{5 z59$3i?s!p^8Mt~$x5PWL?8Jkm^z-sE`bE~6E{a*ms!M-_@oP8IC{D#GXl)LgWgUW2 zlZ0XR*ll>uKpQ)DNiPUQE}+xTKj6}jodEyFw_NhZw9&c_KRMS)K04hb6`PW{y>G3+ zxlbSG^Zao1>^ww`=Yyz15Lpu?hC4NMnJpfFiNtj)y5vy`m7BeR+PE};!v<v<_@W6v z>wk|+RkNVRV>9Fmt<Y;vzJYtAKJ0B>d-!YUMo)V5z`*AuJoT3r^*J;PsjQb`JL{|Q z?c%*qUfGWW-kMOK6Dhdw{5p7~X+x$g9wp-|T95=U6Wt5oL8$u@ST8zF=oXj`-yD8( z%F&CUD|Zq4B`Xi5LZkGzkMrSc>PgZ$-JKo69@OyrE$m>t3BO%sO1FKKWmUpo!se!V zP~jX3CBdf{$X*1qgm?IpTOPvhw6AFAt=BlHTA3z4lNQ>L<MC<dpyrU7>G-;83h)Bw zl2_lq6Jo4|%bpc;TU`s#@8a=r)q`@I1FtZ@Z?+&~5n+}p)E*7ynNp|0VLacfg2vyx zgO6*6vGYaV()s{-N-uoD@gGNEOCBMAnhw)Ta;0?XxheG8*$n*jiLUVfuN(f2#L^?e z8G(kKaE0#s44Sh>nQT*d3L=+};T2&w{`aozf6qbwlgdu}qzV&MW%0D3SZwcTj2-SO zU>jkpU`_Q>2n>_pe$2_m-S2g{V?JleO0lCjMKzO%6zW0y_#hlJ?2Te)C_zh+pRfgy zkA4}>fa3{QxpGDl6-TdyF|{T5wBj@3F{@heI-?ftOf|&9<Rx_W<~RQzRQA7)_dlp? zywxDDCBmoa(A*ciQ}4}qPu~S_g|lSN&&liK&(qT34%lDh2f3#42At}erC&pnX?j-k zy_<VEk?E`Wr>d8676qOBmd0PrBQ`~XIsRpgo9-iiqZ;Nv{p8F)^+SaVy|S)Z`eY$L zdZJ77a+g<v@BVL?Lizce?9e#=^~_hiyx0uBoW3&mI8u@^3mFhpyx1?$*E_-g-FBwA zbRd}j)!-Q~%_X;)&edQh(^xLjF_FJg%8L_R8{%hO1MaAqEMI!oF{WX6zu;KLaz?Vd zm*1|0Is3m;`3Jqlc*+tI0tu6`+<KErzWI#|rn31SbJzo+vk6J)`sX7s-Y*HqlpF?` zR8?>_xQ)UtKP22dWzMJf1~dE7OTp8pZ<tfd9uP0#n~wLK&KF#&CD#h`z&dOMy*;!A zyXT%JndcvH{H5vKr{})pesTh*>>Wp*A&TT>9+QO*(d5wgP8?-c56=you#)5`WMA1p zI#V|ggHNUqTlkku<4MxI{0!z_`%NtSNe?T%*2i`mH{lE8qTqr^6TEe|rR#dO;=S)g z7%Njbm|a%|PZh<fd+Kk}v#XLM>7=6+1wL7=E2LezYq3kIJ2;HTSUdO>mK)T^ZQW%! zv|txZORmPVj%`O)+aJO>`C?q4X2v#unm{eIj&r64dB`BWkLV0uf!~#vNFZH?-0O|$ z%~Hg9Op(R6M09C9Jx2luU0HQs4}5TaHa0!x1O{{7pg4h0#C~NwY2fVfm_$2}KPtkC zM<%lG%)a7IeFs`w7E5l*9|F&3^0X^40=e#-Do8I3Wq+on!^f5Nm?+OD27k2Zdeg~3 zcFlpf)Cg9segp-&O~iddJ)MufESwK3XMRf@z#oE_!n&mrv@qc|lDyW;1)mhBGn=pB zm#v#{T=N-ZKe-b0Zy&?;vqr(;(@9d2X$_B6ZV4@piom=YPqk<xlCk@UZ}f`cQ{)O3 z$VstsE?TJJUITO{Q~1i8iw{}+Y2I!dicEIdF@lQilwW8FNAxFyxXE0S>Q%&8v>k71 zaCgJ@<yFw|J^?>2EJ8u*6NTy&1KfQ=sEQg3c!p3JBYS!x{M~XG-)ww<zX*x5_^J7L z&uYX+6YB8-K@HX^(`QE?#0iXSE}~GuQ)d6qU_9eU6iBMcuqA%=5HwX4n?7GC)F?cL zbpC#95L-{o{~5yfLRXl%_7k$&QU>qGc7k8leqOx5j#XX16)$#P1gE54;oVhcXuaKJ z`tfusThe+7#0{Ns{8|ZW_2mju8kS|Zh&W*KRS#RNzX<G#a_lx{3b9@?h0%Mr3R1q$ z$AQNu0^e>IHs7;|&N-5cjeXvts(00Bx_msYHl4$^N^67f#$?=_><8IO!Ylj!EaEvS zh=@-cBB}FJ$-|`CxOHO>YJ2b<%~{-s`nN>mISJbdWQf77CFR8En6MMsEQdYXHOaDf zvzenLm24{71><}klYNsGVu@?R=uTri9*sDIkN4X{M{6^w^2<l(^6XL8PhA{j6$Yo0 z!|)>!b;ubkMO&9G!yi<luzgt%NZmS!yf&rsFZrnBIU3%$J!mZM6KWJv>O7F|NGKV1 z<s<67vl4c%EXI%JwZSW2i=?}xqR$b!lyB#PHJ6MBJ?}Vp+9^sL%s=uY8uubQ+2_D* zHl#Jeg>bgt{8$HBJFFKg3(HI{15c!ogv!~_n?)SE+%lMY|M6r5E{pM-@gj6UeLGI9 z*C!_?y~V?}9oTnb4!I)Se7#<vMc>`D!S9`l;pnVvIzvx})zQ+y)#~?f!M`4?(Gfw! zM(6SU7H<^9#!tn%%k}WUn6zf^v<0wFRSxeM(1&E(O_<4>2WuBfK=jiRCc<L}_KMX- zS0XAo(a&XMpR@~zMM%P?A#sH3Dxf(=xYeD@L0fXZqqAWN_{gmFSnJXGCh3GCvMfNJ zv1wZXni|Vc<Q@Zzf9IhcpPw<2f2xV2uO{}&wfcW_oryP9(ck}tBvUHNlp#YXLm85L z_CBU$rW6gDl{um$4Q7#f$UH=-43Uy@&)!E#hDs8OMx-<#QxWnzzx7?e^*n3+p63s^ zYn`?3xp#l|=ly=a-mp=^3#&|X12eV?nv{MCAGpZDP@ToJc9S)3zAz2DyW3H2(lnL* z)fT>0W4^;r0pPJo0Pyn5z+UkZQ1#bFFz8tfQad#<k;?&J9ytqC4fmoCgApLlV;l5q zU5)mX+(a|4l<>)5FTk4qfa4l?@I4_8ax_qeF|JBLn`f%@%=~)%@wp?({bK{rPFg}! zcOoG7ZG>(-$;R5<7jbpob;9q@BV41ogzQe<h=p$EfKw3R{)3wE?+JBm_GtsUw=M*P zxu1bDlJ8LaR|_0-Lk|52_(!!aH6!9i!hp@nMMyuV6IFXvQq@Z-bldtKm7KLeRa{9X z1{}|^wofZl|GsE~sVkQWlNW}>qx@OQ$e|RV(sk^(?@J(e=}(Zpw-pTw+7p9`I|!o- z*XrE7Pa=CSIlcF;KL`;e3dPSnrKbE{h?iZp`t$8iDV1w`P~pX=V7llQa(WIa{ynk4 zDK&|G(LtNKEMP!wCg!M3*4)$w@*ko4xt(}Z5r|BL4X9d$Ji;sTk-pTK4)(G9Pt?^# zgQ#?41*<fb4}|d8gS&Z~h{7|Ol&8!-^mor2P_8*jy-{$Yeh@zhNufNz>#+%}dA%HG z?2Lysu1z3xED<afSEURhLW!D6D`MvMd18F=YJ7b)$qGLOK<9>g;Key5Cf~mWrzNOR zT#1)}uctO<370~S=SFCSrxhwY){4~JxnaIf6)}1s3M7|4MZYQ@5}VHtP_{8K)b-E% zi7(e>z|EAuMCh$5>Y`2pp>R>S?kVwzvY~>hXUXLR%SM-duCoLkni4>Z@0Wl$wE~Lo zAV6A6{E(s6GQ#ZXb_$-P2z9+;6yx=f$PsR&=D4fbrUjZ1J=;eawfa#GORZtU`D((W z^aRL0EI}-rOsBq1T%>{v5v6&k2$@dCP?nFCz_NR{S#?j7iQZe`)E_Swmi)eQ)?ooo z<heEvIN2U1oJV;O>ue0_VvM>ELs%%~i7>u0+C=P1<fQhz%R^kV6yf)~1x5bRK@zh$ z1UbDQ8F?BrIr4fW(rQKgJsk_8_Uh`t@)#ke`K=*eYB4xtFh=kvEJ0&iEl{_A7tv=B zNm(7`!==`OFqL?QWU^MFx_cJ^C+7i1xF*1|9q~}p_$sje+Y7cy<WjR@oN(D^C8SnX z1?cPm5TJjGDz{HV7j19h=MVN#hox^&FU-n_Gb^lt5!pZ)j;11;(>L`MnB4B+V;Pic z>@CW^_Xsg($pd`ftw%Bu1o)CsNVEsN(zlj4PSvel4$K=TS>JkO;IyF|@EwVxx^u%= zwOZZOcC%BI`2K42S|gFIE@lUQDRE)p6_rE;?ZC1f9H%7JLZ}8$OnmST2BGn}hz?o> z{*6`<ekaVRvUlN>NRkrOzv(dW;mkyq1<_!Dae6d$o1x&Rc@!^q7?{^I1wIqksC$B1 zRMh>;=v%K7wb}Oy5pS`EXwxo3>+B4vYX!Dw*mRQGx-F3^ixB|F>eB#!z8!e^%n3F4 znBYBYF2E_yXT*KCqrht+k%$P$BKBqkutPYWBLUF>>W}bgLb~%J5Pr)G_7U4a*5p#G z#qky`vXFqB!OHlUSvpL8yA-X}&q0yDyx52JB7pV)4{ER0gmiTe^1lwz|ES9T*PQ(y zRrZK&0PDHjFfn2V=u1qnf8*m&V)Qff5}dHWaht^ut&|IXR(_?vE`I|QiY?*3EiIHa zxtM4$*n-c$F+vX7p`g^C7ab<6iKaJKi6^h$u(Q)&QUN(hgo;%z`d_lL+UlNeCypKV z{co!58rMwV6lX@JY1E*aXTz)`#`#Dx{5^gq8IHvDFQL+9958IV8#;Bkk@$V26SPLg zlDkhd(Bs5L=zdPSe!b{*bnEUt@SrGyHj2AQS7#IC#Emf+85T;W2F#FoS1!?YL3?P? zrGc1x<6hcg)gQDL-NHxL4->uISK*jm6sfo+3qNaFNngQZ*sKbHQxTE)dl(DqoV^NC zCpe&T(hXutaVe|`*2TtKIq{{9Jydi5E@-&V2=sMK;opHw5ip}d99?{u9{5rPAI~X~ z<D19H?~`fJ-|Z$e5>f<<H@QMp-~Gf-*Kahn@d{n3t`1L)Y^R^y@&J5cz39pLeEO`x zXDGgv3vPa?f^W^FpxYMK^gZ4zAd&eA>(!BPGJF;8`t%!ZWR%7?9_sMXGBa$uWeCly z8UfC889;m4XKHa%2HDy>0~?iEas5Xw@)J)O!5?uA3qIE%57hSH1T#5O$DITRQ)@}P zw(Z2sgfrbWpiQRAtHFS?^Efr$0D6SAFlL%UT5_QQG<7w=Lw#r9VE+Tyw!;9N7&pOI zoazQen>T<+DRmr}pG-)#1pt}e-{90YU(6%gi#D!7^o|N?I3+>i7e=eFO%?|>KADCa zvKGj=6LCOvnz?yPB*V@tjIyr6Y%JrCVcyw%=+c47wrOh=6ctUnueXNnK4Ew&#)Y~o zBZ}w$+R^U|^f1xIk5wL@fRtetEpzKQdq(aQ*kz<b9{0*ZsTcII?Z;t)H&_l&#)kvt zHxEGbpdw6t@d#|bQ3m#~*5C`Tjp>@&Dsa`d1xk+T)Ng$;inGa2xZ}_>SljXohnO%_ zv)v@RIRBIO8vaUl@avJ6PnN(pv#BssO^LRWy@oC3Gf<*hHfeG*lfLR7g1879vhPec zhEt4*^Ttx*@dk_&p3V`fv=sJS9e|z$&w};(R`7_EBf4{}2o<w?(Sgmn(EE}s4wwC` zKbj&zj}AVk&*wGcy|;r6&aD26w!U$qmE?Wt{yl!QOuHqPTf`3|vUup#JG00D!Pjut zdIg#k?}7WjQh29I3mmE3Ny-%qVTFaQc-)L6+gNSzdN~`Gg>k}ay$(!Vz5-?@7ULb> zoCGKHZdb--fyO~Quxo}FTJ`xbJ$j8yzU>CMa?pX?{LF%G8<+#HxqRw}xKis~&wn6i zbo=Pn5=&_n4P#o%2*E#=USJ_c$iP%77KCgrM+aKmXaPmxdXJNd@P3Oa`IVt(KXthb z6RevtZ*~Q3{?)U7Y~wm;@Oqdi+G>P5>}<it+k4PI230X3#pJa{PXmkWP5AEnCW<$* z5LE452@Poh?Attq+rtO(kK>F2dzK*i%5TxJoJ^kgQU(+{`W!aZ%iw#uOX-(6%r&+k zfb?%%Pkt}-rf(JHVe=3*IMBmT$1irGzZu8KlVz>wK*CP!UnNW0kBNfmv>NOivW(3- zCyVgredwLDE|PzD8@c@|AykgKz$kx3tTj6YivB)jYwGMrL(UMddn5tYewU@+4pFdI z$Oyju+KlxJFOkQFE0O-Lt@z*yb3C7&hYA)O(P)#hL4L<~^s+gco{TR7TKD__ztAz_ z=U4;E6o|x4&H2zNwH0l0DS^{qF>Sn0k`N4ifC83u(2^l`(D2$vp#8~(kmwJlI;Rgq z4QqM4=5{LxESSd<zZ1ahp8M!?cNTf{ei}VZ&cVlukp`zGQw+L$Qy{xki;PfMRUa7h z3;erxic(s-g7g?#X)tgo)IgkEYp^WgB6+M!$zaCSmewauViC_Z26l%y$i7)4e5p0q zz$VqqV0+#z@^Z^8+9`M+yoC`&K)q?*_L;}j=ZBSq?L+}`9_NNd#S&0SIu+#igt6AI z7QkE=^MQJaCPRK71<#5P*5_zP(3<@Gq!d#$U#Bih4wbAU8;kRxXuUfvDs_f_Dr-hd z+WXN?8@Ta|S}z`3Fox?kIl}{Uie!#e3Su4niCWa=G2Idl|CUc+{?*&CT)7kd`i%%Q z_f<fyc6Wfh#0zNXco8Ks8REX26<E*Qmp=N0#bk6H!Kw8daA#QzS@ZB8;O1<uuj_KD zpYe`>@a$DOWUL&&{<{sctIY70@6q(ZaPxXG$!J_VuMhj?bKufY=KoNk1UZG=L-Rs1 zkbm(KQev$s+&*=V{`KJ{F5of*7Xr2M>^E)l=#zCgL0cBiA5g+ImKlJ*)ByEbhXMzq zjmW7QVaYlrY<QV5<1VJ5Uim@Ny>B;J;v0!043f!M<to~RhnL*+A&JyUXYx&flC<8i z6#4k(S9*5O2wYuLPHwS|g#jJWwAQ-{{Krch-B8LVBNq4L53=i^Z?`Bd^@<5$F&V;^ z9}19bcN`r%_Xx2=H$#=ET#&@K1nWjTB5KpV5Y~eZpk=QPJaA)*@mTGqB6hsMnG=1a z61k5|Tvurjzx)}^bK4wN6izZ-!y8a>WIkzD;Z9za_(qFWNYx+fpF-tf>h<4}UP86) z*T_5Ru`vII4?4->G1$AM9a#Rj4?Fn}lDZ8ngX4MYX{AVK@Zj@xy6x0abfDuOYv_e3 zw7U6_vJsadKEE-5xgR{x&-@VBs2>YH8d?MADW*ioiOXOji3<kzEi)KsTuj&QwE!*N z7ckp9oJ=;BC6f%wVaD@X7;LRxA1jiE8;|vY-DorHHg_lQs{Mn3gM)}uWG5|WQU>N; z#vnzz3%CLNh9awFV33(J;FVLL-GVux1#1A<{^rJ~Z8P92*DD}JL=jsrmLQCETksVl zHD;9|g+d0jpdHh(`-ab&F3n$K@KE3dIWoQt|1K-W$0-f+QByrtu*U<K-dhGwf6OEo z4{RpI;{-`T`aV7&q6IGq7lSU=J1lo!gbaTpLRXblz<+BOlhQU?c-FxQ${hO&NE1!0 zBDM<(BMKc{W`H<8^W(tOG2+jQSRy>^GH4MkCNy=7V1}P8;gH4y&83Ct5PnYloWYFs zj`opW?Gzmqlt5~9T0p$j75^TK#orH!!-?TY=(nIqTSlG2yo|B^X!0`sqc;@b!xl6A zx!ac*+J6#D@ZJJDe(2IxDX&nULLxCEz5@NbmW$KG8C6Q&1<SN<#`l(H19Kr;_)960 zHInUsK6lg-FMEuLje$$BHt$+o*TPAQKq=Z?y@QNNPo^Dsiiz7y@8pVy8!)%)EkL|( zq$UO4<7Kj24Gw99;|ybC`bV-hbYaxTzQnJ{^JgkPor=MJS&W2l$Filh+TlBm6ujrg zZ(OHj0IuxaiKkuV@Wljotlzl>#J*`k_xHJhK!ZQ1B2Nd!%I862^UYu)p^I|24Z{xx z(&z(^j*&LAf9dtl-qU-MuaT!ih3PD_3v}U31UVOX7Pm?Vf^SUbkmuYecJ`2=n?5s{ zeXh6gPfs}1{)dp_PKr(ro`W5N8lY&v3)J*95KAd3`r+~&z#VLacOR|6r%Pq<BLP{U ze{KbETr3CItn38qD#gL(nH2ohb`w6`A&2jLj{y>E)ybe%GrHI~oBUJD1FaSu=wE#K z^i4^5dR18!67{MFzgt3R=e#@UwsS6d&L<83sgnU^*PO}wTN_cdbsAhA{}6JT--03k z7`I<GAL;&J5G~y2B<Z2AM8V#TbjfW8Y_x_!;%8uR!&C~*Zb(DPA-!nRB3n4=LD05- z>#^ACm*C8qB&_*0gtn;S!pk;~0CeV4{mpQG670PJ7ukOz0;;d#be@gieUv)AZOYi7 zvs0XPL@a<5HdnyMUsKrCU5Z}uwgj)tIZq28w!=L>vG7kV(`51?lK$tl223u_2Hiph z*gusY%Np#Vv~?A+U<d%`KiGn{fK;F&^aA*K+ro34LqN}pjc(enKrZxZ(#}wjR6LhM ze&rQ2n0|7PG>S1N<95zae4&gx?BObSp?E(@aw?N2UKG;XUuHw$yh51&U=J;HrXL)W z>&KtQeCeMX!?4zoyKqh21itev8-zcb!5s0y;Fv-ZH2!%4cTd?vg7yR314gOi8%X?z zF^%syY)8$xe@EjgrQpJFC(s``0o}v6=mU<W!2hst{k@<w^pe$9<nwVG`cLgq`Ycg_ zSX*qc?S7`{?XWKDv;IZx;MIpD-GfJR8}asQTy$%}O?Xq@1=fi^#HlYsp@LrpI@82K zll<W*H4?ybzHBUa<p_@OOahw6wnG!Y)z~cC0(;%4M{e0%uvf(%tj{&XdlQ}E*L~OM z<#E-d%-PS__H+w*e%^{a$XZGsnwX(8%+8YiJ7)0kSTL&a4ZvR>YQgS~MA9w8gsf&q z;`H=*a_>$<%#pN;9QX4iPu3rSiV;~rszC;O+{C!G>pFZK`vidwKu@1e1%*}htk+!H z*w{%P%ZIFlnyT5TxkMUA^&Y{)MZ!og>K!84=A@O5H2vFT14BoC4TCzxNq>=C*k0v~ z@rQgm)7lf-OY@N)zct8~fI(8)DWB;Y?Z&4RCQ;+tBKToomKu8S0*h72ur)i^(cFi; z@r`6N@-*ulS(C{N=x_&AqBy}6O4g#ye^WsIBpam`g(Jt#ITW^78vbBbT$GCfGLeY~ z_U$v2zxolft|E^dOt+wURWW`i70HD3mOwX|aB@MyjCA0Q!_BYPz*p;bprh%gG+Q;5 z&R+<JfhBLC|LzrZ<}m`=)(b(26HDovNkL%MNG<lAF`~~@R=}Z47O+x<A2n~>4c-12 zQ)!{=;8>gm<g4RFCGE=C-|8NrpCb$c^t`~?AXRW=<|DYOWDKko%Aiyy4{iQ9gXB9P zXkc1=lBBLn(#7Ai;r7($_|c03_+9q{m>4RdFJ#4%EAm=kWan~P^KB@!IvIt!w%vr+ z!#&9^j}9<#{2tK0?MMcGs(`(&f1#K%C$jQbpe_%kQ__B`81DQas#rEkID;<md*mka z)-k4b`BX#rKm-V?CIXA`mO7!ZWV-wM5?b?q9If-3neBV7krS68?78>|Up8{b1HHne z-Sl;oHJV52JgG&`T;=J`wTXD*UMjYHH-zq-3dKb(Ds<JuCNO3#4WFy<Vzy&Ej&3}P zSK3UVrOPehhox`8{_;Uoxa=yiz!cYoO|E5Wd%OofT&t;Ymm1tsGeC4-(uFZ%Y&e&- z5+1SFr4J8o0r8AdJsESrz~rtEc`x%j?3i3k|5er^XBaXu*NM}#3A0*>b4X?Sp`(GR zL<m$}%7GJ`X2JBSH6+giS)5<yj$C3=@mGscN_y)@c%bS7;6KR)?YLBMJ5M$!D&q&+ z`fbr|Z84OlIG|T^bQ07g6*44dMet)&DadGUg!~tSp<Y}xt?GIf9yu9BcTC1I#gi$( zWD`v<q&m~&&U3W?+q-l{b{#G6SBZ~rF{e9@{{hcUHdF2Q3K)0$7PLEvIRgnt*y=t9 z74_}uJM%Z`aY=LZKW1hBYY*}tRrb!bD=vJLiOTZ+pvCI$aKBg^<D}9-&hdJ%+n)wb ze!;9hrc^tfYZbnx77I6URe|9ZYWUpXdBUu3J#bxyKpzJe{&PVR$vW}F!q(%cu~P|~ za_=Bg*H)rAs|J>0!5eVV=^|11%N#vv+J`PKJ_i36Rrdcq-hWeN)wWj=Zbw?sq10)_ z5zR$PyYk`<3|zWHuMZVJu|Wq_FM*q>?f6rb4%#xkigZZ|!Bq+M4Bl*n+TdXi_`M&X zDv{5`Iim!a(<Y0%XOeJeYYg1taFd!&;2?j`WkX-qTf8}J5Ok(7nJSKa)bL{gD~npN z&RH#ndt(_)#=0DQ^Gz&z8lMAM?;{brEC!y*9K_2!8o^5{3dk`qx6C8f#OL67B>Q6o z$BjP1>+)YRuwY&Ko848=l)fDER5K=PM?U;5?gjYRUxT}Lsi3CoCfHt~63y<-!|QoN z=-(l7WU%~SeCXXxI2ir_GX7<FGFh4y=L&>7jY62_+zi;KrGyX5he4BPyK%nmEBMIm zHgVm0J?#{;67xA0GXsJc$xlfWtEB$o;DKY9M-b3Q{Y@cPZW%7ox(2Ex?!vcM<4DoS zi=a)67sYq9!P~zsq0IEJtaOWfcKKux>%!l9c4>d7zSMXE)&6iN>ztYjHR9M$9ZyMP zC%)!j)fpUR3#a81tsKqlNBvoB{u*0Kb3}rOvE2slkMt6Yr~Ik656bJ-jvb={CX=Zv zr31ueJ5{iC*qMEzN)M>W2<vmMxl(6$H<!pOh@-Zd)vy|`|It^r6ww!1*;BX5C5IqZ z6|fWbYZCt=UJ;5XuIXRcu!&tMC#wIC`yiF4$6~GJi(!eNF_y^I6_oLiF48`Ssjb3w zgaYKz_c-E992mY#oRaLNxC*(^H_1BogwJmDC2~9aV5bZrU64!NO}2#A()&Qo8E&j@ zqD6d5{{f)b8thc?gxzYZ2k5P<kchHA*wKBHD(?40>;s#qiJW=%RVKh8>zIM^{S4r5 zOE#qu;{-PKSz;#pM#vxTL1C(oz*N9S7+P})dhC&g%ExX&h0t4|xO);+w2Q*88oRJx z>`{U<>owGTQ~=-jaf3K%3dT6kqR&!Nh=Vu*ubz&^8`#<GuOXfAcY7H6r<V-3YRlm7 z4z*xy&K{^Ez?kZk_Q79o0+GkKH~!HDsSS3@$bT^;3(Ar~+nzui#%=}E!L|@AD+DKG zpJEMJF8m?S1g^f%&?9%Q!Xj-qs4r=ukn-{YN3G&;h_wZi)$9U4+St&y<PWgp_Q!fE zS5e4o0%!4N;4)D)*xhN5GwrT}@HGNh{YNx>kgy9XDMM)DYJyFQ0p7Ph5#Ku(1&?fG z5DpJ7L4J|1#HP?zJk=>mpZdHOyQ`Yg#e47Jbw7VV-^vX7A?GFJv~Dd{ecV8g^0O(i zz#9Cq*$?)g_9MR>+DwEP=EJI!JMh(2d$GI0XH1kWBkc||CaV2R!AVIzygPCn%q!>O z5Q!Qzx8xRlu~h@FN@dUtUBd9i4=sv4xDlT;5&)SGdtg_MEH0r8$O1wd*Io33;kgub z=5!za#q<ydn@N)`u^6R=)WI|F#IfPlrRd7TY}hkZh}PG+LN!}^<m2Iu)3+Cbh*>?> zSj|@$`c4<B#7|(0*-~_aI}xv<P3d<h(r|LC7|@@-0VU@CqN%#2*g1ccTD@8i8vHx~ zZiI&5mv5Q-hOI5UZ~Gihn7jstDt-9v-wt%F!3yu%e+4Jb7U6)A9&Ds62@19s;Ys~; zXc3YFZKp_Be_9-ia&W=v)|=R@_9pb?t%l`Wgvi;BSa9GBH(E?;1Kn#*=zh_C;^{|4 z^5l+07#5y{9CPMi#)C-sM!6Y&zSNBzPsowNAI{VFg6wD=?+85MHV9IM^zh}+*I-9@ zIq*;!hevcI$oPZjuu16}WV)yd$2=B-<sNNV;@Wz!Bg-3Zz55PHr)n{%4q3{)T?o9p zBLH`r9A=4U*aN#v0t7wbCw844N7ZFQNb%%-w)KE87<c0XS6;ng37JU($0<H4N27+g zp;ivWe)14dPy#tA?L@mILfIiQ$8pA@J;?Gm06kW_P-nszHSGTm{ZbnMF}C+n;i~Uc zxP~Pm^Gy~N3@(Oq*DHu6fl5f`=K_eS6`;m5TBwP!=j`lUKykin18<hjQ@OC3I;X&a zDkyKD8CuOgoAaEq_a6r5zP&*aZ4arYbs;EEkwlh`1{4@wjg+m6P;#FT@``supRNB8 zz|@79UBUs@Z#YPuzxENCTU3IlgZ-4=_GMH+Xbe`Htt0lCoC3@HOIdfKcYw=3hLHO4 z6M)Z$M8~hx0FO8yWItL?9j>&(2fPMRssV#nd?kwaHFtpBzQ)*oLJqv2Fh@pPd{Du$ zDD=IWSxrmJv92m|A^}F7yDxTw^6D(3c8J^pVH*-yN6MtZjoJyYK{$+Qf^2*6PizQ? z9Ybszfe(aU(lGJ8*$spV>J!UW77(n!2#Tjxh?xEvgmN1=p?x(!mf`fHy13%m<!%z_ zrC<;kHx@@<160s$B8>far!^=~UJT2eLRmYwa#-P?%2A!AJh1EtqZH?LDAo>b!qJ$M zxV>XN-fbofuZxrt9M`jfq#h5lEK~=({KBY;H#x)wZewEp$#P&DGecDQaw3uL6x1kd z$2#DmicV&WVDUpCDChZJDpxQcaX(y4#MYANyK^SR&94I{Z*U@sj$ESB=Lr}Q=K#}% z(Nvu%AJ)hdp{zqa;mkxOp2_8BOmGx>@W2HG#wR1Q56XbOYZoDvxQiOrvcr<EnXzxP zHD%;4LHXtUV=sRmilgnXz?WPRgtM<0%j2&M40U7H;-%bBSbHgoKkJApqdwq06R%L0 z-BRdSxE;^C97kh1c4&6G5B=c#h^$uf;1Sko{KCu)vnIU3tuG(Z*s)jauuKZ?9n%A1 z=EqqX_I>DmYB}iHuLGmsoB`1aEkJ<@k8l*#qpIvEh~9`G?)&#a<f?Rx7+UbRseK@B zrw1lEzazFmDtIfvPe-<GgnOe$K)35p<RQ8oKNHXg0dhWgiF+Tuo5%|-YdLXT6h97& z5CyM(t3%Zd>CnvI42Gp9A<ec~#5ppLoVQJaT}2Q$|IkJ6ik>rW2nLPgDGZ-hEn`P1 zXoG*en&4LPKUDZ#20A@EjD1VJN$un=<e0Ysr)o(<d*NL8T_gbA(o%xC1^v)drVPLP z9R>f588U6&YOr6NgBCgDgiqcN#W~k+fn&x)K&f>XykGT@X)BP1hmO2O-r^;&i&#s9 zXIbOgt1dVxMiE{rl*SKwPg4!3hZ?gR1`B2a#Dvu%@YdlQI6THd9NioRe${^iyQ?fH zn{PJkr&A)}>it0?{?KFMcTObBV22b{tab|d#mpk}=2E~t&yQDy<xzaKtC3=}D5Y}g zC_vU)Xw}9))E&|H=+DCl5UD5(sk~xpl;<gxhs}`XyFQlN`E%4q>Jw}4{sGo&#WrGJ z$x@s(CI?T;wX-gy-DIUU+f(nDl8NX54_43M9P!lOP+zGnf^g(}NW4><B}|4n@zwbs z)J#G<RaeQlBc?b}Rh<pUAIv6p-g^tqww^`4?GjY4-&4Z8z#7!;Zbq3dQNS*|mq>N7 zMIZ9VQAJD_RkbY@+A3ecP>hoz1RJTdG7pJuPf}2><1$w6TrnlDCcqH6y>M6fC!jpx z3ziP(Q?GgsfSrrD@L&lI7`=?L4=Y5m-N8WN)MtdE9smLD5g>hP_y620`(JbRe^gn) zVL2RrbR(hiOBK%)hl2rwI20$dlva8q0dteSq7!m=f!XOwbU?)&EaUP5*EH%`JLh`P z{tb0RV#^64cSH((ZC_2;K6b>MF$b6dvV&S!PEbQ7kZ@4ws{3D5S)Ko1mDS%O0E#su z5bLBC)b_E2Permo$$edXp!fx~g!>CMaYX<xoZpSN4=h9Ui`_76WFL8@e;VXawe-!c z-el%ylAJ4!g7#Zd$lfP_zB2I%f7}mn(76}nCarjUaA_%E@iaop=mBzT&_;Go!f>(Q zeq7(FLRuVVC|VEoXzyoEcoGg!)W9xipqzo8nJpq*x+CaMK_*z=P#0#uszVcd<Z%3* zOK5mX7}(wC1Cv+1U`x0hU}=ZbIW8ofR4R`D&XV<dD?(u2z7{%YE{{GfbPF4se?f&4 zo9O2PmGpJ~ZzQYv5$^hEMZc_{#QevMVS-8mIx~6%3VN!M``vF*_qUPItEvx%JpGAZ zX6eEHR%fvAB$iMrNTI#M)$!_ZeeC9(2EPC4f>Fbs`YM@~l;A%XDsq_@oscPmgDN!1 zE(w2-o1OsoO<o}FWWwo|$A3wy;6~V{6bVH%g=uQnWm4n|KlT%h1*4u!{~pl`g81a2 z4<$e<+{nRWcLMQ*<#xC;h=5!o|6toU2U2|t1$C~b!SvY-hIhSy+*cmDe{N$u^-VYi zWavI4_6Bi)w6)Sum*+CNI;w}uPTD|YUMCd8Q1#~RM9I-l4Bd>cgltZ+gC&N~;o0Iw z{On>3ndhvBpNaC&%>{m>Z`#WGGur8J<CT^0OTr7(m%vYQep^r5DCwgiy_Iy>kF#{Q zEGM0|CKW#$UV;^;v%!ge101tAmr&z7PIoyrgZpQ6@P`-!bmP$j7?HaaymLN<8gMJ< zZdgZ(&nw^y-Ba+9VIrBhcL`m0FTtSfZWzw`F-s=Yr;|%`SCWmp8;F~iQpo+kV&Tf; zM)jZF`N&JrUtxAm7S1_xpZ;B8N0;fZ!5=C*u%V6#xuJ{;AC@b`$9D>m|8_|-WJe+7 z*KGy*9gM+O!#n7xI$-zZ2*av#JZ#=0$%yZvGxk_JOI$cB0T(<JFsZqjM%(+~2-^=f zez-=Tia$g~<cZgB{_+hiaSeqpMGt}}lOd?d@G)AqO1WO6-H}$x3qd<~dLSi14m{TH zhF9FP#$OFL&^mLqsK{*qZ!<eX2REz-z4B-9A4(8B5T2)g7o^i2a%sq?MhASh%qOzB zE6`w&5!|M*Tz`&A0}Vo9B!Af#AfqhBUZSN-x`l;9x6j!I;b$Hg9M_D6>s9AKHD3_6 z5EmgszGAq4;~DbM{yzHaHfcQ4*#*{7K6qpOC0eZcBX+&qgk#q8(x+v;$e<8Q*m!CO zJljzYqb<egqdhUiN})w~<B%@<_@WB%H8vkveB*~zt&ngE9Hv?%b>ZW*Gr)8U!i3dt zV9j(=KJh#YhWgKv$v@o47XNG<ayS-l1Xq!R_+omoWdnXDw*j`xH^Dzm@uc-rA#&k$ zI(`z>LcJ5Sfs@V4=%lLU#E7pTv{-M2L%3^E^XW@)(G(l?c(;Q=%i|zU>K!OAOC(?4 zUyF0Nqku3-2kd{Z@Pt?wkQ-IR-Ax5Rbi$gH+BS-QyV<}7jd=2})+H3oE+B^<*w!!N z+iaj49!+}Ayo4>?2axg?CGwYqEmq$2ite-LB)2|p$7>rj!KW@Y(ygZqKid`w2A`He z-Wvl*O0%A|=p#4T9ni?y)hR~>7H8M(;=hV+q~tOQ0!KV6n+m3c6i{BhGI--%gO&(B z1QkqQ;inyc@Oq_HxM|G}np(A<4hqSp`wBJc*(yuuQi~8;Ywvlw;@mX)_Ui$z)mlm) z-E#}SuDeC(aX~2gJ`-3rj)2$=W%zc}Zmf1a7`Ey|)U)C)8k)I@I}8eO(g_({H7kxd zCLW=Iy?sFb))qWHA`Y8=KLOmDV)*g>mGJz@i%6x?6k3c9qh?WmXe#*)*?#YYuKo_B z`HgmLb!mk`>Nv*#_j$HIK123yJ&0fK|A1Y+b+G))N%GU-92#1=k#>ds@CN@bm~G9N zk<DA+Q@%9BI?Qb8vrI|b126EVC;|M2`8}+SDueqLR}!&pJm|Sx2x;eT4&R?$iTqMD zfN01UTxcT#4O_g?mV2{6rPl~A{h5h&SP+Ppk~28;y$1w{A2Rr398#}oNElRz?}CCO ziioS~Bcy#x$U6tKVXDya`aq$LWN_^+dQ)vRbT@oSSIpVcN#zoB$M7;bCD;zT)_IWI zPTV59IRmMY&tbrf$vaIP3MDp<KZb#ltI&j>4XixPha+7iutljP%$02e;!GCm_1ddA zBIgD3ta<dQ)$f>h=RJt$d`S1rxYC}@Z*eZ41Nnuk8|KI8k+eTAe!Kn_9h|+w;4Gbv zcb^`@PTM`8$>1R%DXUD|u8zRIC>ak_U&eEX(;;7R3ATURi^hx}V{u~)=6$4*@D?4? zZI2*aN6SOIhPAk9)huy7_zAl7=Of|OT|;P0H^ThvRm@eo6a>zu(<cN%V9?+Y{fPG= z9o@laFu&#xR=1F)&t_c11!XjOLA@HwBz-5h@OBcbEkEFOhnHa2)_9m*orF|Z&w;A% zy=c6ef{RroNp|ln%p_IFx`HK8?0^IDgTaSYi)b<9`3$wn^&b(GC_$wd#Zrme*2BG{ zE3lL3d1Bq11Sq^U0zx)DfqY8&q}IYm=r_$o8h5ei@9*8Q%x^=O_pAb4d|L+<=c3Tt zDl1eIXhyd<N|45U67bk+V|XIm2CZv&hSV&5aK4@<sZ(f(ZxrZKu@W+H?r$jm+$V@= zz=yLgIKjO#kytxvCHqTDJ2I`+g?7bhz;81LPMTefc_k~r&$<W@+^P&_?k3^NVPz5- z^x{K8+T`<EJMcX;g)9fAQ0Vp<I^rT5?R&6+463EDan3Kgyv7{0`FTM(=GvU^HHT|5 zM^RRh8KExfjb*fq@cH={$Y{-Dd`!g*H#}dBwwb;FL6wQ%ZmkH6mR$ii75anqNA_b; zIu6y$9|j9M9|F&vi}2-=ELeY!2W)D3hiwxMK!5p5<o4%_@u`7qaKtBq?%?Ag?rgn) ztiJ1!23EUhPrGuu%Dag4UigW{mQz?n!ybr_zr|+>Md%t?h?>4%th=QuO_oQmCZj}R z@IHS#P%zYu&$g>z(G|r&N@6|ml9&g1H>|NE$1jj6dk-i^@4*=dy@~V41^#T#g<kL8 zgTyFZ^l(iL`B>E(|6audTts;46`pEit$kLsSf(CX<9-&c7_DSX{-LPJTe1FD5f==d zQ-$l;9pKZG(-5jBgXF9VtQ6=#=Q8URf2)UhgvlkfE?rIon@nQwW=?#xxsN@Q+KOBG zeu1CTu?z<42;3(91>E(#f$lbF;&aL3&{&r_Mji5rI-?x=(e!q5O?Ch&LSLghh7OPi zSKNi(w$k-BQ|Y9ivKUm_#89%;PvRm07h2>UiJ!{<LkpYg;P9@Oczx6+w5Y~}N;()% z|8(}j-=>)ay<G;KTa=DuU!26VYQf-B&@Hsog@-KM9}a#iOF=Q7bhK5o3+#NB1JKnC zi25`~)cba!YbiQ-T@^2^U)~9Bl!=pPs5ClVh>O-0_N3DdFVSTmf6%?>49JOGKJv0& zJTz(k4LvJ=5IKLDKJEHV<clvEctk{%j8a~PZCdlmU+EFBXO$Oni<rmugUx8)o*KNo z<t)W(_K2$4wgF`Cj-|qG9tOW?Zj>uEf!_YR3{ozsVLv7dx#IpaIOeJVZganYD`5mR zDDjHCdg&Ja<A<?ZUpI@zql<3!mykFi9M<zZgynm7kR?6uY1hBZY3Q8=kG`xSLhacE z=g>I*ajyrTdASci>Aee2?Aw5c9YWCZw-|(b)l$d0dx#R9FDx@IC!Bp-0|zecMQse9 zjo0H4;Leo9mB~_&we~4cl+(i%2Gz*lZyq;n5M=f+R`kSDW3rIpP+a#3CqvklwD0ma zFiwDn{^mrIg?Cp#le7=y;nQq*`qLsT{d^gCzt;d-2G!w)#y3c?I}84{I}S?*CYfR{ zEqLB8h8mJHL>x^Mz~baJ%3=B?46!a|KEnq`WkoRc_8!`OTnsGBhy`2F9{h305!c7} zg7Rj5QowI7y*%YSN?yLYe)ee?DgC5~Zt~no2KxwL^TJp7<t(HhxFwP2X0~Gk?kVtR zd>2%4d`S(^S)|@$9+<N4Ao#yQo&%@b&={of_ch8y$}t)Y{uqRlj`xxOurJlb7R6>M zvqXKm1^5_fiGvfY(6XaJ;MXCH!Y#EyFVMhNf`#ZE5+uW0?ddP#ZE)nMKOTOmXYeLb zwf;N%8(gr{rmq=4gc+^(aCM0odS*aiHop|G@sUI=QrFSbCAXOpx)J=A&lHYMq@qPv ze}k!4o0u~u8qVh|$5mzJ)a<5VuvDoQowjHs2bPo*r@h*Njh-;px)*|Y9GAnCJOE4y z5s=5+4inrQ!6mK}<n_OKbmqHc6g$&OntGIy{9kUs9fJ2sSC6}}ceNrJcH#g|J19xM z`@GVC^-s9|kLZ0E9WIB56#pRY)DZHJ;%+D>ElA&(3n4d`<dR~qbg_JA8F^nR2}=tp z;^e=Kn#N|j5)ZrwK3cE9^H+zVEZt8yFhxmbFS1demmItHbR;y~a-1NzD}a$;K1g@Z z!f=r*Eha2nzhhkpxxU~x)YFh5WiH)@Y0w*&U5KaC-sjNBIfGWq4#C|uQJCNV94@YN zVQBMFaOG-s%#mq8?$mX}b-Aa|p{2*Mcv=+JY)?g&sR>|+cM$lMIfGxpv*_%f9YAB$ z2?^v~MLUTn$nwiDcvV#dcKmrp+^piDc?Fl#hr-`rxcdct&VMVtt<{JGOF5ChlsjFK zw4LdNevVQWtI$r)R%C(EMf!!66E?SLL`7<ew6VJ*9GCeB-1(2v=OTZBL+@39y1Fpk zII9SIR~BJqvu~^?^AEr_G6FC3wE%X73~suA6YLm}K*xVVsMuVLPW5I3_TX2FSg{wS zO8kU|N5s$z=N+_8Tp1~P`!jsGM3k=l!;pmShsabN0b2d59BuhXfxL3`IxY=na?Bsk zumwKVBd){8Nw1p=*uCNo;x{^jgnsG3tNAwAk?EdR-DVHRbNHe9R07BhuVqPS@L<9H z=_ox)5qs~<1p@g}#LpBHJhp`k_r1cv|8E&6w01{_FSLRdfgyD3=qY+dAt(9q=&Jf_ z{mS(_J^hKfv{<-K^DefLwZZo^3NiI*oGf9_P&U4m#OULE6n^qJ1a<T1W}O!N`k)ck zuL;4~DY}4Toe&hIlAr=PPn0bFj^cQ)fujxW<Ynn(V#~NFN<c9n_Mj;YvJga`X4ZI7 z^I`aqGaQ@gT|hs=!szIFSp!bR9vsE9gseG|Lif+VqvikprO}pLlH>3+?%yUuO}=QQ zH8t+kJ9sXuw=Px3>s`k2o)R~#cB=-ScZ&c)KHupe$x*BX2JxQv8|p84*wXu7GS1xp zsaN*D_8|XJWw)uhqCuW5=+!|Jc$(K1RopuZ2L;C|c?nglM{I@TyW{YeenD8DEQuN= z#j%@hBxX(S0nww36{w0Izxp`Hx;-rfmluV>ST6^(JJ1w}nAn4xm0cjDo*9l>BEhqU z{djrU0@Zm(8JhZX;Oavg{x?<j|2^J+Q)U0X%tM3E8=y+-65RJh1jcJrBKaYPV(5Jf z)*h0h_Lsy{$}jiBg%h>7!EzNGZ;ZtUdzTYI>*Dc|mp!0bC>&QTyNvsel)=$sI`Ey1 z3FWs{hU6)kVpb>-*y8XrAUJyzM<feF`yvNCWW!I-wXBDGw70=;oxD)8J_oK`vjTPG zzNQQYmXLRC3sFFJI1XD|2ST)XNdtwSfUAuYa^4BWahE=U2-9rbIerj-tw?|>+$<P* zlu==+5LoV0fxJjahn0lFh4_zfcrqAQb10w_(t*%AV-D?T_JsMjGx7XeCU3b$7E8M< zCe6S90WT|Aq)_q{D2`MEr+v7PP7@ytmfr?LT!qP3bz!7xc_y|K*Q0&6y94%nRoER_ zj~(p8pnNb5!dIx%vsGEtbpI^K%r+prT^=LCE)2F67r=P8a<s?lClv5DhAuO)<Z&rZ zY_lo|%b&iA`<*_Z$j#AsVy_Pjcl`if95n_mmv7)X{W`#TFj(I#LL7Rg=TdGy?qIv| zMj$LU4jdkIpeo-PeDsDaNjzz1l#?E$nHC7YNJYXGLn2UhWe8RNaXr;@&5t^oI)o-{ zWZ};r#t;~91!h|!@K{_lWwTU+-CM8-A7Qd*;a#e@ty>4{>tDsQ=WF4Li+$+kZYBf5 z?~L`gu7^+gcVVOI%i!3=a_Dwh0#2Seg0JimhX>Xl*Z-ZGi(z;Rh~UzMos8L6zET$M z6jq`d9bHg5s|an$$iaqZ`N%yQ!>poXLUc#95I!`Qh;Lq~#uF~QWLB3AY=j*I@g@tl z(|4fq9(x$59))AJ3Xr8C=GbC?JiZUjuwQ>O&N6$A^<DhnBhw-rl(+)68$E+tC!}Gr z)I3a)HlUlGlh6^K9?Ep`2G!rjqD?EJsf5vNYPKbW^<+;xq2a!tpj73F%N~)esiwp1 zcFR|F@@^fh60<booJc48F&)j4+VzXoF@KwII}wbot;(Zbza7&5agc{yaH^U;dgl#w zRxOtZ-Sdrgr_F)@Dp%Oc^e+)SY5!Qc5}e5H6l8C^ro(Dyxw0(s%&2R8ZtRNOMz+Ga zQsTO40&8gGFKb6X7pv@$9xK$IB=VR{(2CcCdJa-IA9N)OP%e%kRPxVL)K_aC))ZAr z%scO)BqvMjg!y)|dG+ep-i9`;3??(?RmH>Zn7GZ#ZV0FB`cf#rw{q0A-BJ`k=W2LG zqX^d2yg=D6J#g!ZXuM{NC5Szxfes2d66FhgKq^)V?-+cIL?Rxtj{6!?F^vZ(Cub)Z z`Fb8?eVatkZXNm_771$jFEF_~Kgcb53H{QgkVO7ElxJg3{9-ySw?6@}GAt6_Q#GeP zL2-g}CWl&UQ4hO(lyMLj4-hgAV-R2zbYk#D9n=YS!WfP4Ob<LcJA>9~^1+N<<@lFw zAzs*Qh^v>~#vCse!OTOd*mph}a;6#q8}4v0UhxYSHR;3s=r&a%%NPN*rts-gf^gLy zA3U)8I=bP!1abw6z^xw9Fruvx+D-Gr4Bb1h%yBVX&c_fg-LK%kOi7;VnuCnuHHKBk zcEa*>C%pf}R`U6SDvZ))$gB|w+QL5=ycd^&FTJ0^ob?^3caFlvBbUH!KE~iWOrv+Y zLomPiGF)$Mj{i+M(UoB_Sl@^lyPxWj0y?wcXXh6jKHQIg8Gk{i3Z7zP&m<(*-H#WX zAHrx|Uo8JfnqC;Jz@|w8<lWbfI3qO)urs9aO8O;894|pNYvZxyeIB~=$7xu}nSlSE zOU6km>*(;^Ine)jD7@IQ7u-2;2VM$@fJJvqP`{1>I-e8={t2%pL_>urtJhv&%hs)^ zCCCI>qzQv#JPX8)IC+q*(?<PWb6r2E#1;j17Nd17|4{HJHe1{_9r*IZq6fSRghom> z=yNgxD*2aLAx;gTJ07Cc6RoTdDfQr1{d2UWECnP^T%l&a9)J!S-P8e&Ng|=54)Oj> zW9z6(0q5FMN~w4VDa_~t#}i)Es6!<R+?vB`c=QS#Zb>FAQ-e@;tPnQw;{qS~l0kdz zZlENhi^z-$<aufZ;aBqkxxe}fmThpOelMM3m2>l8ksFTa$K_}uX=^UA{Ms(e)mwrD zSFJ!9Z90HBJV5v_<z&6xy#zd)>x8;t$!Hz>3pku<341H1SSOj4a*uHiWwcWtU3>Qd zop98^Wp!#`jhiK!ndOFVOy9Gv@(sLy%@K6Q*czV#M_D<-%22@bDs__K7;s;agc1&n zn!3sp+%~@oza$-^Dq7mWrp3SE-Qq)lW%mcYYL5ekvvb5_|12;Rz6=)Ta*}QX!^n1c zCH2emDi})xu-g4M#iPU&YA_{KCVNjJj;TZ{aEmIq^`H=)&)5QYzXmYsVK=+s>?Lqz z9XB4^bscH0D?&mCnm`@pgx?M4VM~e))MI!-c=1Ij#yFuqvt8jGy9(%ifkv9w)??!y z7T)h`4J#+Sv3}MY(A-iC-%oI&0OpL6Qn&$n1cF)80r!Z%XI{Wd`H{F#Mx6Zez!PRi zYqKToBZ<>_f5Dq$&RFN<4zRaD1+JFg0`K2bClm4#(2VMJl)U9Qw#|?y_w5+RslSxz zG1(4ui@yxKEm;B0|8_%-or}o$*Id};06$$X&ke(RIY{T{KT+T-O?`267Gc9R^keEN zjAV?;XTb_O>C_(By`TdV!^cp)PdyVxS;p`hYAHc1#I7t01i`1gS$EQJvxCx9z`Gyj zNNgY(P5tO6S~3g?uCtn8R@#$#mGYjoZr3t2x}O8XLW&Y7R3^-_wLxXdI&}7~5cmv( z!7eLNaJ}A{ed_THIM1^P9o%t`>Q{6Fh5vdelfjGNZ$JPm@7g_L$;~=~VX{$^;r6KP zf;2GW{(x3I;{x-02hc&+QdYD>JhemXEAn3N%hFikidH`k1V%A@FkdJRc+_u4?X$PQ z^J*Q?S~|k2@GwFqeVQO;un_oc{so={h=Ex16Uaxo9BeF+gle@mRGH#-LOVtqnVw8R zx{stmkXj58+cSz*>Bob%;1rbj1Hlg-C9JQ{ih(&6fzPk-v)Oeetk;!-NWy3p_!7b3 z@wQnasnlPTmKYDJ$><~=C~bh_ipOx$pKP@JdL}LsBGKnR;V_|S8q6$L!c)$jpxB(5 zCePmkK`A#WVljhnbv^|Y`;&;qY;nBKSsm5a97G`-*Mc)l7WQn%E|k7-lL~L53ET5r zXzrRmmOFU>u*7eJ>8=%oLG@EsgWVrO{>NnyWhwb!P10jx<f|3RJ*0+))ZBs63m>rH zQbRduBqF;tF|4b~k;FUM2PnRDmWa3iN9=6xA~Z)<BhPLwO8>GYh)Ul^czjJoxgQmg zwn_l2?NKcfi@ik2cFTaXM}v_=xG+&r@qxXuHXjVNEl1~e2?8y@W;SPO7I5<12X;AZ zq1d;>h=w^wCZFAfF4}7ovjefL-w6n1@jL=oxD-%IY8U!FXoX&&K=i%Cg3!2_13tPO zrHXt^AfG@wvAp^-YqB;G%*xk-2~91S+iD3MT!rvyISnYXis^Na6N9!IAr$A{FQ_$U zD>Bkj#A!_n#LN{T*j8f*d1|uY_>eOY*_HG^6O8|B&i;=o`{QXN@hSc~dv1IY{3|nr zUc8Y)HdciYo!^2U|M|z3b5)|v<E`QC6bJldIt-SUKc_6>($S0XCbULW7C1B+!h5E& zfTQ6dB|TuqQ1ETQm8)%R%L9$b#&sX~UsPG$|6i5;8MFZS<^F&z914WElL48uxF7Z1 zl|T{-e1zHO0BlzCi8aYy%R2Og8(tslXGSdpv`gbU{oxXVpf{dAk-%9md94Q5zjh)I zu}tx@Gj9MZ)|hq+5+-BxISh^+OMs%KBUt;l0&P$nkCj%x!HcIo@rR5nbXKDkHdS2$ zOPg1avQ4d+bv+VxNitnvf;W-s0V~+KX90fZ<_0#uYmxM0S!nURg3uVQ1tG(+z{JNK z;_@gQ#%$k~!U4!xcOJ@%&%h^-sz`MnVO)HQA8e3}BQHn-lv$etqq%KKYP%SC%li)7 zB_`v21P92wx{VM!&51vpI}0ssg`v9ad5WJq2VKxIrs;zcxPaIUD~&9$>Anpx1fB(R z^6%-Zj$@R|;?uy?ECU1wY{1VyuEuDS2Hd~x2=mu=8M1fglizxWNUeA^Qd2*fe#hQH zO0Scw7x<b<sy%Tee{H=^=FZPzi}X&?P$0Tq>r)v$&~cf(u;eO~jow-x^oB(qE|kQ+ z;}i7bfiL(Klkx2^y9Z5*-Rb0(CHOVgBt9wnqqpa-Ge_=bsCw`lz<F!nv)S!vYIz5X zb4NR1FI0opGb?eJ>q^|RKLd-BGGw~RZMxtyCL1$TNmjidd8#;)j{L-l8=k$ya#7xB z=&u^SNYB%U#G2r~C0kK}sx;p3HvfO>dh=*1<G0`67#YhDB~yr^Lgsy6pWCbw4OB{l zqEbqOG-)y`bBZ!%iYO(evhVA&Ns5vpDa}QbMh!GO_gT;Htmmxr`#yj8=U&!cn`K|F z;r)KSF~!q{rvA|&o>J*V>-aOwoG^#(iIb$q#xam9_k!%!;f1Lo)r{JX1ys>~kf$cD zh9CXXpdNM?)l<U=@})I1Gl9w8djTcyCR4RXDr}T%gvhe$EH!qJsJ~;eUo^{9Mx^4g zP4w7&yy#oAn5eyMjwp5aa?&`ni5-1RoqaiU0rRbvLsoPa)^IQ2d2>1u>`w=>Sce>a z8i*?bilM|rA5_aa=smNAr0~~4=$3wrp|&E}JvJSuX}*RLw)yz+ha_4gNkP#G4-B{5 z!`=O!iN-Fu^j2v<QMBUV`Tl;k^tK@}k1`g;o+}e&%znU5KDL*H^{$~8H5x?6Jr9af z4jv}`vClDgo;2HJ^@^DMRlu-ead2ZxEPKFhCUHuLM*Z*6WY?althWAX@-y-i_cpN$ zxMMn0Vqum@V`~UmF+Cl3h8SY3tq)f_?mQI9MPmZwb3vhEG?(0?@vHSk$@3Ix*{L#- zt%rx>M%Oga<y*Dn^OhufXh%KKZJN%GI8r21d)&`n=z7d5pV~%@xs!BIGK!tlxJguT z^cLneHn3S^+e9HRwME)=6zS7Srv2xB!n}^p0&C4WlyZ|NRX@zhx%-FkRcs49w3a5< z)DPR=PF+V)eivG{{Q|=mDH!G8O}|W#XWed(BG0e#qlLV#WBrVYju$V_r#GWTWYEnV zuBZ(V^Rjl#f${ZA8*PZx@4KM#BpQ5di%CFNEk3m_#X$4Zq@q}h2=D397X3c@&GG=L zx44UbFF9QK!U*RSN>YR0->J{#Ucna=mJ}B)pdE>aVDDj9SiIYUJiMLBB{jd|H1B(e zf<1~E>-HozPOFohG-9PlQDTU_aU@^lay1JCg;QDk2xYcy-jL{{oFY-)KY?xNkRai1 zAIXM}C^lPZK2CbGm~K^^$6jfXg1xHQWLcF7d*WIz`K&pe6vxJ*N9sypVajb}Ezfcm zb=w&U?wla#We}bFyi8E4FqNjh+0426FU9H7D`AA4xaiP_HRQwwF;V^&Gd5Ks-SLLI zH3|01h1kz;$eCwLd3tUM%>I!e8bo>0Rnta36$gOo5mh2}axA-cZ5;Rar!!r<6<D*> zd(aXt!j6S6>M{hoxjc3dC6*lr(~V7Fy1JUHc&j1mwoL}5iWofMB|}yROVdxL2}mC| z2t4N2;zwSPCH!8(W(j#pVl)q3dna2TwBkEkxwMg9Fu#eC9Xm)?-78u%-J2FoUBP~J z-OHYEcn{8Qn^72NOhBW8X!YKP>RxvecvFG4O@BzptqzVos|&-m|B<v0O`MwdNA!ND zO|QpEkX1kINZw^c_seCt?ZZ_Ju1v(shf~0!h~l^9e5=kcs)FtYvb<`M%KHbgHeZ)h ziJjV_k+y4D^ME;Q>d{iVHogjW7YW$Cla<(K!a<f)pF+oEf@*g6vBUTe$VFL@$-^b+ z{3Ht+b4ZPtM{Iy;m&NI%eF{{ybuX2yRpQp1)uxjQ9%1dEA!+_m42p^BjHureTHmaI z{!J>xJt0citzIV(8<kI^?066OM`w{y?s}2kni5e~)2RAX-?!|rv^$jjmM=oyyBsWN zLV2BF+_m!wtLUK1Rz>QH4g{J}wTXR#J~L@i(}of3;v>9=@S#7Gl{Q7R{=z5G&xy-K zoi^s+W-Uu^Zez&3njq-=S0j2HDu!S8w1PmxoS2PRi4U)I<Jl#J!i{7Bi0zDE?X2(6 zcWA+KqE4`D4Pk8_-*+tjcaJ?W>y@C_1cl>gB*21PWmcwHk4Tly5<N(fgo}S9*hxmU znBJ?yO3B<6>`|PDe?B_0?`+;OGS^PgUpsbF-vJGBbKFYWJ2DVwD2Q;jcM*&q|Bw89 z@&n%3^ur@hM|#ZK3??jEj0RdqAn#QV*Zo+GmA-0E)D6$D2Um_GU(WqxZzL%ZfnO}U zL(i8reAC1GlM`u=%~d>@+(WZkJViOWrmXY8FCwBR=)sRhq*+#ps7xP8f6nhE$xkj* zTX{***y;khInIfmd@-M0?`A-blsPg@g_+!9vjyag%p7<$=Qa+o7x3bWVfe%^85bL* zGFqlCWW$?02>)Hketx@|)yWDH9nZOnS-riYB*A$WRg@er?2lt5L`9J9YsRj)S3&$l zUZSqkhehc;(rkf75=nioEHW88f><7yPm=P_(5>0;xFNd%oQ#ShsY^HcI=%n`nk<R$ zBi`4#(^MdKwSlb)Hb?Wn5!}&<J|Ntq0Add|LX){OeKI3YaPv())#+te@nfq+&)nBS z>%8Rp$mhq{Wd*vT%0gdu50}LIJ~{F=@j0y-+s+D08rgRP(^#KR-^kDVPf3u|9x}JD zh_(3WLPLVqlDQ9;vO(9R*pse@Sf@BCwtw9|y5dw7ilu+R4dV`Td*3AS`PnjP;4%$- z+Sb5%hf(yy)mUbQrwwLLEkx;PEzy3IVpZ**vhBR^`<-%$$Tm97aYEvIHtt9Rn`p6s zRo?v&_@y;oS?7l9PP6qdmK-6i&+<vho1gGmQcQIJRU6tD_>hh!OM3U6Gr+X_xaUI~ zu{}AT#zsyherNwdo~j-1tsB9N<kJyej+MtrRZ@c9LzkF8vc^Pt*(LmzF%Pnu8bNh& zKC|nrB3q$)oLY`?6ov8LCeKe@H1aMLjaQn(#0DOMt{F|VQ!A919ByK_{^eNTMosQD zdxm)BNQf+-B+zSe>bxq<lD_FJqB?`C=-@9PQ_ihtkM8TBr|+e3cDeKDrk_9YzPTNI z(?7(fmiyA7;PKf0QHR7{T@JCQMv<x$lE5SUh~8a(OTQ_GeJ+r2)Qb_2<0l@FI0sGk zD)&>g%q<m8e79lEMFwmOA0@a@kw@2!A6@T1QJK1L9U$vBZD$uT{-jZu&L*EUB6>E@ zSn<jl@^^GD-4`}QXL!W0Nh1<b?Yt?zyORR6O`IINUJ2_WhT*=^^9he^BvvD8vFmy( zxJ_FMGB@gl`gcv~6|+K;Ubaj$Y|upXT-IM?@!%Gftk-s2d4B}uZa*Y^v?E<K+mM|f z>drcET*v-RKS3S#D2tRbD`|s9Da4;rrTN5$*giKFRr1KUZdq0G#@UEHZ!4gZUN6Dw zur}S1e;ukv-QZjur@+Vf*)(gkFUF-k6Q;X{p?IAy8mk8jlGVPzDqdl9qVE}=RUgUf z>cp{XaaN+JvQdtH<6MZ{#0J8yR%6Zlr?D5)*Ae#Eeo{I~yFPxqylCmNQ$)(Dn6z$j z7X3B~CNrCT>EUk?<V9aE&0Af=Io{kx%sqQ>?#=VW_jo<--&u<f2t|q8%A&El5~SHA z6~ty*Q1QE|#5z3?uB}*xKg#7fxf7vmq)jxdKC(=-ceM)b+n85xcjg6i*XV|bjyp&8 zYes@9_lyR8JudRs^=6;UcX#w*&Dl%;3fK>7TS#te1ihhiie$tF(_Ezj`sSnz`^@we z-BcaSGTR96`Ilt>ZaR)rl`K(s*p|Lnr$gY*L6o^yZXY~yGMr1x5Q-g~fd^KKVb~ge z6`7(=cjy$6u21SPy8QvwetB86aiD{it~xBT9CeVSROPYn;|oZ>LmpAQIE)Nz8(V*C zvVv$4WMbdwK_cJwkoK$Tldf~Iq*lC=K0V$IdAa{!@3%Z6-D=EU5-G#e>@T3!H;PJU zpTQ@OPE-8_LKvLBis|^c9xVobG3T;RLE3&f+9KgfPkFwBte3pGG%uG%PCLz9ee{R! z4-ts2C98_04W83j(+W}4N)U;hB<RlIc|!kXQe0isLOAwPoY)*&PYqvWQkRL-@MY9` zY@V$unpm*}My;AjZ8KlP=NX&Hy^IRx-y}zvWq6foD=?(?Rp<D%sw{MD$`wd14ThYV zvv94=Z|147E{`8w1Roy-z}X=}vZj4vJm*alO<!su(ru|>3+mF?j}i8w#5Hzg;=g@V zX}TE6{NPW<r^bkOOi2>`J69o?_1BmDy(=ktdqsts556Sb0YsEH!yoK7f5v~Be`v|6 zQWEj9gUqrYP26H6(E0ssCTw^p{Z)ODIlJI3q<P7KWI_iwY40~2bR9!_U*Ba6ewGm` zF-RvAMvG)Z71`mZLhHwjx+uE!aHq(uC54FH%@NHVyO6E!TSyj|8_@3o6GXcvS>S0Y zefF?)G*RweNy|Rh!{6AuG}CFMsK5U@PL56|3wfpQ75Bd=rBF_N>Samou2cBy_*)oJ zb(sXNcck$PKH%UEZ?aQboc`XxprPesoMkziG!*B-H=BpdlEVv_TQMhzZa-n2uWTfd z3Eu2FEh|xbpn#|bRpE@_aN=RJpZyV~?I?dQi`;s)km>V&$>$GO;^{duM60ZYYwDGy z!{=@V$0CG;gh&*G^pF<wAY9=#AExZ-hRz%Dn4)4LSR&hl(aDa)XW4QNj0!;Q!fNQ# zNEGfcPG(%5*V2clj?!f>;@Rf&V?-;joACmo08y}MIXJW{vheN(>$t!czrH$5?D`Ip z;M4bLoZD$=IWv>Axvu9LCNQKUb_|K?Qp9?V)uavO;qSm<5>uedUbv{io_?E#(??Te z)OHCzPu&WG_uGj7QfIIjJc-u|)M;^p3vCZI!i;fQ;6EV7kY{yNozLU`H%GlbcaAt& z`Ch-ib$6~PMe~iw-9V9_|8nSU-(zfF_ju8*)dix9x<-x*;s}4HxJZLz7O`tCwlkG_ zZ)jM)8g7~11|574_G{!k+_C!_Z;KBWjS24i&$+Vyy$AWPt89eVOB4+A!@zAK3@?3$ zd*dr$n$;yH!{!D`g<Imu!}Ez)-)86?){LVh@59`IJeXY)M=fO>>GP6EI$DgU9XmOJ zy7)Lcy)zG8$8^HH?>_XU_&&13{2W~C3J1%<BF<))7P->30lGcMP&?}l^ndWm{-3w^ ze_drAt_$dM>P@oct8m`ojYK%J8BHhhbUxz)xJ&5)XAxEe1A=5&IkOgT=FOpQs}s;P za}jy8jKiu0nxd|qDMUW_k=@!0UvWp=Mife?ko5&F<TmeLohvhw?l;WFZ%03KRRcye z@sBpCsMjJ-{GzZxw~RDs2f^<lFFdYN>QE}kA@YIZF#Kp85#Vur?k6L%D?SMuUPq9o zIrFIfie|y4SM}V5H@oS=Rqa&LcMEgyh%|NMNr}Tcwvg5^9ePJ=2QkyPrqt>X`L&Hl zYVG_CDbKzT?<jwoP#H;I3vN=G!6MS$J4hy-kYZik637#fq<NnfgNdrGNMrIb^0HkZ zGMub|+gBc@E=P5Q!(3_|4C^h}xAt3MVe)BmKHH2ny(Y&V)$%5S<S2s0_h5K5g~x?b z)S=`73B0XF?}xr1tD6hy_aD`GOY0O->YhRsmJ~q0+)}c*ItlyCgCMWhk@U3ZlI*2# z1p*x}oVvJ=ZYoY7Cw{D_>(-v3{u>%-&)6BnAjzCo-(4WG5=P_CwphH8z?X^ZpCEC? zF3xYkaPFqmQCegkLQT)9(9|>$*Zb%uuBfjib7z?dUkQ59*G`9=oZ5-|YI^an&S7jn zbsM_OoY7NPnsp7>h}&mciiEsiKZ6gf`tkD<)q32IqN7hQJzZ5mD`h%p%D{f|_Kh50 zN)u?rkTw|*q|^3ukHK=kCDz*PCp}i-cuZd&O!jXg6Yn+Av5KE*_&9&Mx0C~~rBPJ= zdYEYVg=`!bw27=PlNEi^oXc*WZzQrfG>Vode5EC6{W!7QmfrSf*-0~IvtM@C&^M0y zWb!QDJJcVA-?mL*)s@Rh+xz?Er@=+K#Mp#24wa$5`p=NCFRhsTz?m4t_Tiq+$z)FQ zN;2bLKI#pRC8Y|-a4$EFIJw+|j7OTnBX7G<S66|}E04V-|Nb<2wX=-g%=wD!=|wbP z_*kOu6Hec~_)GixS;%sHiDT*`NyX1D#!R!DNXMTgTc`EX11)vbu2-Jy-Bth_@4Xc+ z*SSNolFA_@emIRwnoDQbrqENa@i?}$fE0{cO{>KDRxF@}cuY=WPrTSgw5?O9N!T*t zCg_5V7JG=-x;A_@{t;at97CM+q*?j*o>Vz*DV_aG3BM(eWNiwp*@usdY1fNrq2Sj% z^2ex}SY4E1fANNcGx8C%@^UtHFOV1AAJs$O#NA|yq&V^+V-N9E>LTZ^^WjKyjKQrT z2wvW{BH06fsIi2X=<vg`x{WysywGGj89zfCuSGB-`;<?#bJAU?tW^~?^F&IoffIOg z)kfOnR!gXVGFjjpPyD0zQXk<F8uC;~8q}pl`5P|_j?2tpzMm*}7%U&nO^MZJ>JzRr zYfk$L&g;nuDn6-ljTZHS>BkF&6OyB$Zdo;B6x%KcGJPh9^-#pVkBQ8Mj1k=8(SHS< zOlF<wD*-q5%yGur|C=EELMhkvz!6d|g}_1@FUX9FMpcpyF}n{rd~vB2bbMBU3G0&T z0)|az9_NHHmvc(Ex>3)C%QqhtI?QZkRDzZ5y~OqiK19~k=}SyumIyNiH^#HVfC-BP zZx3lP&tzT+J7N`>Tbn)W1_DkA?eFU`Tc#^8bp2xiIkSrqUT$`%Joi)ZE83d-(D+iI zyx5bA$k3z0X-6n5^uWKn61d7D4cZf&LALCD1ADESpuf3=8uu&HxXtmP{Y``FHIJbW zqViFEOC|F#H~}nNzvCwlHB!v4g{!Nl!sOCiqLZ-~TU*R&LC;Zo*Ju(=DHuV^+SZf3 zTs&FKuZOSvSqo$3T?Er!&Ii>g5sb#WT+Dnh3#aO3z{m;HKy}ku4Cp_C*K2Pu_oPQ~ zi)Osx!e_=ZC9h23k^yxvk~YLR^K;CeqczNf5r$mlv>Ha|qd%zEeC1wQdZAh67wEXQ z9i%Hxa86F?(7e2XaVX?a;cqHuX;;sv^E!b8?S_Jzk-QNnLjWyXR-uso5PUxrh_bdb zxc#Twz~HF@ybL@Gxo2|FNqbQ6%ZTF^ixn_u{dRIM`;B12qikqBo5vJZO-A>=m$)f! zDb{Q*hPn6(A1q2^?w|4E77Rr?T$bdmVYhWr<hBzTu}&xxeYQ`2Z$dM)Q*oqRC8>)V z&aC;V57)XMbF;mFPz$YFn7XD3{8bfU*x(Q0>`^Yz{7(_=V)?+A^Yigo@K$VDC5unT zE@d1Kjl*%C*`QT0mHIB~f-IW>JX|mUul2uTX;UXuO2xuQ>#b0da}p(ZzJQugn%Xp0 z!LiKInDOZXlP-50K=Ttb`qV>ayLmZtYmO03e!K&Q&U!%I-gvI_gEbt`SqrsNzMP9p z27Y`b2}46HIX!y}S^fNoaN^vXxb%k#dT8$hb3HpoRcaQvSWjc79e>5Wh*;oo)o?t- z<f#Z&2ljA9f3q3;38y(DD;=iucLH4W^n%sGqqsvRj@j@XNVkMBt}YDZ8Wzbg-RUP$ zI_9{bAE?kHeGw!)ZsP2!`|#GR)lAmKvEVpf2qz7OGshicxa%|YX<LLFD$Gts*#+{j zPH_(Bz0Qy>@73otN2WtmU?+wM)Jb02a+DeS40mZy#zz;VX+_If9(iU>o39^(sgtE) zRY6(Z@h|}?x;qZ1j+Tb(Gj~8GdOh#mPKP4>Nc!!{9n4LbhTV&m>4E_#E=n<yXoehM zroWymi2Ma;nB)yLxmH9fWH0mLpev-#o(e{*WSKy20i-&Oq&sS#bG^zZ!C%u2!jE(d z2j%U!EmIdm+tmav^L{MHW_)0bwKO@)!+_PDdhoq36_zYl6D*QW;3`t$ASr4eh&?{T z$?bRzxLpDt)?DL;-(avk(u$ieVM6=bE^^Q}#N2z%kNJH8Pz_@^$wHnidvysenqo>C zeZ{#RzV6K7re5;=xeCX=iLyU?Fb7&ruEVMiml*YXU%~yvD?zx^XMx;NISlg@!@=ej zE=J`r7A#7I>a^X=Mh!nE_nH?rm5qVp-wl}K8KcOd!cHdp#C*)_4nUI?)42QqdEDw5 z1Tx;X<d0BG)S|tVky<H3Z`Wiq`bre6riL)BeOjPadJ~w8D(<W5CG1l!tPAiT4MB?v zIYG7p2<}|KvA3<zzQu~>Z;-)>m*u#VXKWd-!N1(U-PN?MVm+totPREaM?vaU3(oJE zE3ELi!0a@8%q;dV#)?G;@eTeEeo|Hk-#<neTKgSW9!zrx=6=ApEn%epW+|!_Ytiuo zS#(P20eZZ51X%isi$X*S9Q(k63;JCH*B`iW+t2w34$ao5#f~57qp4Lyc~v+Wh^#>| zt5m4{6GBB9()3Hldl<8QK5?`eWMs2c*p&)jz(_D0Eq6o<P8cYOra4Wfi6N)a^j#+S zU5g^?#jS{qz8}fGz7ke%jV6MAA4V!EmFADq0Rzoa)Lf8@=l_M%KjvmM{f-;F%AP86 zjHseh&nA+0pa0-Z9zmpEI+uHNIG0YYpwxgz)Fm8O5j}}kBPH3PxNOjrRGYf84-9{j z*-qWydSeV#%QqGc?&Af|-S4Pj#aW1+c$hxuOQOpIZj#;`(`a^MJ}q?6C9C+WuYtrM znQLJy_>TnR|L(K@>ngj#P>*hi@xx=Q?r>kv+TwK;A%y%M;#5uFVtAAh*|!Xtq*oTG zy?Y9N@KzV4d6ffB{KNRYO+a;?7rc!>W1lZFho%d$On;v$I1k<9%Dk_lV#rM>6?@J7 z4_8^+|Nm8X`JzvtQ0+jz$h1TK^bXG8%P>eaYJoMDMNH=G4BV1%fC-Aaf&C$)IquwB zjDOEhu64PbxSJjUn=XuaIto@i;q~RM?yz9&T+%T0JUUn_305B6hre5f!9bP_CjJ;l zZvHkWwJ|f%W6Ls(*u&#eAPOB`Tm;FUPjGVSADq+0j{}JZIj8ZSWK`03ZlLZe?(@^6 z9&iYRhDKza^mlyNU(3g5?}H4Dg`|D04VJ8{gTTeE4)g1`k+jpYG$2z2P2#+0-;$ex zvL$?GT;^{^ciUX3wMay-avy3l?;g&RFoZLCzo2;KDA=(r1Jc)zq`P_~XoS5JzVy#W z`9?>g<P$(|-;krP?N{Qig_CKh*D3yHwiXLh&Y^X>1XZky04oc9rs~0DknZe-wWqw` z<L4eMSMozk2`_51QkgvZvX|?Pk|z;?Ett8;h_vz|s#zv_Bv1Mcj+UB0Q)D|qZ9yrX z7%xf8|J`M}wT58znkpt^dm@BH^+KBXa)D<_4vg}iNM~g76vLkvL8nk2bnHD~=RRrT zGc}nTs*$FT%FJlT?j@vggE);1Q=uQ%%b>zP4ioNPf`ggLH0r7(-ROB6<>C}^t6d`g zzMV#mg%sDNXL02#=JB53JeYK(7anUp!B@UhsLsMRtov1sa&j-wFI<e?bQ;96hyp%G zZ~%9&79;+>-|#_A2QzF|JvY`=5+!@eLGVj~W8PA@|CeIt=`|=7_?EE=x2E5mqrjul z1mmke!Ikx)Sk|pUj*gR}9W%BP%V9=*sK`<bzm!P5gC)_p(gWHu<>BJ-WG1~k6q3Cy zY2(uvJjSeN_Sf`5x9|~|k2!}PE9Q~`%^B2X^hL-w`i)d#7d*0A&)grJgL&6jLE^^w zkYR6##r}oxEprx3+js_Sv^V22UEXWWHQ*v&mTd6gGZusYLHWRN`qZZm6MNGDer3be zdFFIL-j(R49l}JijOKZdVKl0D!0W<ba9_n|^qN|6`Krgz<aZ@(Q?#U8wZj?luG?^M z`U2D`cYtC+HQ3uYGo9OAsdG&nycQPY<b|%pRMif|0$rHFJbmiDaTf8;G=tgYV<~H^ zN-9IVp~y;sj$~CZ@Q^kMkkO+L9#%5zZQVKBd~up*tpm?ZdpVEh5Go6LaPiR)JXwDX zn8qxJV^0A|LpCFPdjiETuH?(9DT>{h1HWrVa^34EaBmNbllezDbm~;03meMNNjZY+ zyXb))YgWU_1>;C*lMS&M;Xp%SH1<VVz?;R=#IGX-RrVbfMmAWI#?TzWO6N>Y?Ab}8 z^>8mN4?K$c;+BvHLM)p3tuE6cl=yB)fV17R3DKSf##+`i#XpJ*`*(%86DmP6dPd>e ztW#LEuLZli%)#7gEYaRPm%8SvQl+L=)VvV`b50b2#ba|ahR@2KtC0l}v;V-P%eAoU z)^KWIQ_7sUd=}LYXpy10-=J(KFP2&}o%-Itg=Lzaz%H|-$1Qo9UX?C=al8uytxt1P zES96s_Hm?EX+5nN_ZNM>*5bxee~^Jk5UxAG$=16Py$(qnYLuYsysGg~YCE&$*8rw^ zsL&nDzhc15<B-4c2W&S#5AGkX^SP{(se2Pc*a_c-Z~uOPps-$>XQ$i<+i?l}C3x@1 zmn3lc+sv&I$imRu@pK`d6PdJZGL^YiF02z*#(?A1oP)@S^nACXaYrLbo{6f%pDG>= z5-LVcs0|AKEPKwyZ}laie9ee-Rwa=}%3S=xFjQy`0eb%_Owef-Za$_$Qqh<nTd%>- zXB3i!Qp8N#6O_v8aDnqX#-*bT%>LzquZ<(5N@svIqe(^&{NcoVAL7|3XSi%>XWZ6k zhF2yD2;0{qoZBZxLMuX;resU-B>%YYxqI-=z62<U`3tMt>@jIHA*q7%+_8*dv@rUG z!$>(*+LbOzCCn#K!>(IsX`ls`iyE=I+5>bpMB`A91iUV(Mkl_$YFH9FxKe|N=`JU$ zH0FYu&Ia;ywma#l(xP3%?P0O|QNhw(&+uHJA4ph9qgmB2oNY22EB6V=!Gg2!NmmsW z7J1^^jXfAQXBge4DMNS32XYA!_3(3M6z=*KgKECf;PNOAB(z#!*1>kTq@Bj#&utF+ zkz%-f$7d)>Eo9u>y-;iaRl!SMM7J3afFn<?GzndYGJSHSC+8Dpj<Z1JvJ3Ejd<?wX zeHBcS7ZSG#d|jPX2MHTuxzEQ2Kul*4%g-8O(%}IR@}9KUS!3YJ!FGIcr4R};#^cfv zQTR0d8^mAhf|0geXqSS*H(@^@J?s#kJnMp~+&nb%HiJ#C4{(Qirt_(}ZIHQ30Y;!0 z_K5AqoE4q8CBzDtTf4F9z#P)J=r?W{(GJNMUCB?40+62UKxS&BGQ0dIQc;s8r`E9( zI#q8&`nM5+optx>e3xCv9%F4(I(!-SWGazbD@oe8auM2@X_L^KJT7LFFD(ASQ(m`u zgMQmpxM3Or$*tXtg2r^}=4MT$zONwjzg~gRnry+hO{#nj9q)%dy%-|<!@;d-Dz{*+ z87@2_PO@&v(pQ(45zVzMxidwUv^JfGER}D}T9-rkQvQKZc*GT+j=RUa|IDx2B8`Zl z*-zn*vg!1a`V{)aU6Y;_<iq407oqgaK0!(`p&z%c<FsFfbC;?esCHfwqqJf(p0S({ zG4~~D&OJ?<9W<F#scgn`C2JX}FQ0L<?^66ds{!<T4&c`f9E9oU(@vSuBs(XbIsEH2 z9(`&~(#9R<JY{oW<J*s@v+69Y9a1OjRhlsP^cARieo@$WvKPO$ox{(|OVFWHk2J1u z$ADi~QFXBkh98xuy-QZnQ_JUo$gG)Bt7^sThm)b`!$(-!p3CXZu_La}CCR^mCe*l8 zz@woos8o0XDNPNeCEnS>eO(;xY%`$epBzF5K1)s9GmTrh`94a#YlW8=tReY81B%c8 z=kRp@GNx*~xbT*ECZlar2k9ZR$m!0BH1)waSh8b4u<!Y1jPD47x6d~7ou3+=SLP~o zYVCkVBV#PwnvU<Eyak`b2k}QvCQeWN4jiw1vQEDY+28*_%yU%?`7BS}zutktak1bd zZUBoCB?+30CZD4<DW}!Q$nDUeCht-}wM*II`v2}h>b}FojYrV8|0uvpJ6i4b50$(> z!q&)4$WQ)-2ka-3@^>waRfrpPpSy@Ae3(PZexJkoQDP)gVFWewR>hgFibS&;V4dwA z?&`kHAedhV?bG<RhxU2As5Ohi2yxo%ra{(qQn*;P6Rcb0$<~w2`1#Wq`=|Bp!u6|z z;fa(j*~fX%*tgQ8t#>-StE&Wm?Md`Wm<^5nH-qFK`YV|1sK5*p4#QEE)<m|NVGM>F z!^GgT%&9}kP;q59OdR7$TeJ6Z!VPL<*6DGWKW7Y8-x$Z#HY}$*dUNsH4HiUeZMnkG zA)K8y71!saqtg!wa%L!+xgWoX&X^d_#VbYP=+n~7yxr#HVpkYW*=m3%Zy)9+Z92l= z*+eiaqZ@jzE0Tl9^Ko0D4~<AXfO0o~!Zec$`2DUOPTLs_>RheEY*#rttjvV=C=Mt0 z?(zjuCJAMKeMdhBXZ#~4Nw54o0Grx=awDBCaJe@U7~8@!SY1*H=hw`k8i#zyo%#Q8 z-cUZQXlaC*)PN|0GdNVf#&J_Vf=@y>WFG$je=G`sPgX~U|IEs~xCw!sI@HQ$Hn9z| z!#(?_)Y%M3<Gc!6(qQBSd9v12?)(cFXHWu{^yh%zdoyb384BuKKjEwZA2P;kF067m zkEXM{;O@TFfTp_Sh={`J)H|5kpT#i^y>L2b4Ds<Pg6E3eoW!1dOpkBG@z=iKXne-_ ziiL63s|rxEZWUPQ&0yTKZ)4D@Lui}T1k*p9$4^tHlV+_=Am3hsdSycVr860W?AOBm zK$hDb_Lfm{3&s^;OKA~5?k1UqGm{$D5t-W@fY@;Q-7t+#h_%4-*@w`@@Hvz2*~8W8 zkHLm&Ju<OGmTIIaqC&rb@8_PusnGGlE0r%X@K+_HccczrjWO5e=7S$?-DtAgTZhq> zrS?6cXPCcr-5?d42a6-LsmE469MHBJ&*g6B9@ypJ{#0EI-(CiL1$tDWgoV$wA=KIY z5z~D)8@4{Mpst@q@SXj^%}<<)#_UQsd2cvfv)2TD!zAg{3UL~u)rq-R#mQQ;Wag@7 z4HS<^hsDML5E1Z8U~9XITP1h{abjQa%5^X5qWlu-Wiy1b3;Muwq$>=wdn}BJ^<uhY zRETu87W&S94bN+Gnelp4$=JDb@T=@{sJgKfS*KkPlch?xt5}k-H)Z@`S(AewYanWa zJ&fKbiN_YG2s<Jb>7uJMFxOF;zR1`Q{ayWdBrpk=$-85#?|yi0Za}u5d4k>k+u=i_ z2`x%iCU&m2f_srAJZadR7N5KiE5b)ntxp4Z?ePI-$Eqdd=l=1O+dH2Aq6fHV^JCyb zY8kivg)ecrI98Y+ynyx|{veofwjXED8c$c~&xU7<+ThZzNN)Fv6byVXPyYS3A&R#A z>cpm+r!||A*D4<%_SppZJZ}SDZ?3>C3W}slc{AFTWP#Y#Hs<6HDO$eFiku31g<9E> z_$@*V<I7LMgX;lg{VpeJ(8BQ{X0Et0&5nfqb%*kkWw0tPj>_FR1_zJsqYt+f3u^8c zaYlhZP&;HIU95WoPxnv4jax$?$Y46nX?4Q*XMAS8#|&)Qri7l6O&ISUkMnCXxpxj4 zV7;aYO)fuVN@k0}LzhX^`9mQVU9u(Z#j_xD%Ms|ww<5XjzTmUYoDN->0tTO#aoQ=$ zBved{vo_xX)Ac71-x=%Cw@m=^+{y*<#|5O{q7Gx6*K<iv&%kKa{rI!pk>u>0L@phk zNs4`VLa_6BZj50JoY<^PkK1R!ii6oO>z)F2R;}Zl6kovd)plf7aw0c&pAcosN6`Q( zz#BI#X=y+TtStG!eV?_FpAjW!+ai6c^r{ifE~SHl{1E>37{J9VfV4cj%p}TdlQWVf zxJ-Wot(S`-J0wHtpnExbof;2KgYQtbZoe?zbTN4_HWp4B?!Z5Xs<EKwF;fOp=%yqw ze0!}!Am+wWceQjZiRRf4f*8U5*imRYlBeNo+C$XmO|bK`4K=*uM-+xeQ0s&yIAWoS zJ)0im(<yy8%x4@l_^jl@6|cY^Q(o1~?t$ji_wb7U{obCkkWAcI2j1Ne@wJ;4R!Z5E zrXP!O$S?~>mhdi&vzAB`F2G;oQv7{38n*JEv25R6T!Tjf3SzD4KyMq|Hf{hl&9fJa zrd0mtT-pELgZ$T3cK*$eaLiGi-puacMt@f$@8fN``l(@feRKl!r9MI>haAXx{Svno zXj2uFTqg6UD|7GPdr0zUs7+@rUZ@Gd6+zE&?Fk#ZlS78Ybaw{S4n%`)qA6*O=x3JI zKB&8S<N_01Jr{S&r9kdS6G5X!KK>6^+5hwQ{;#X-#8r#HdqE_OsNW4@@-}3^q+L+1 zG=`jglL0S&Oc9useFx2qM&3s?0{k95hq))KK*B44JJv9SY15|D{*Ycy@cT8g|0>}n z|L36oZUT8Jt;y`(?#}yUT_H_q3^#gGaQNOHp;UaXAT)X}l-rjw+z4svIm3s~hik)U zPP^gGJAO4X!5G}rx=?etG@RJ^gZX(kgt_9EgdZMvGhVmEncMf==yvxszU?i7&q@Yl z%=Ka9+KF4-PNzqB(btM@di4mel=*OZ6C~)HJ$KMYr5%<;oZ&3qZRI9w&H#&gd%AYE zKfErsqCKupV5W;YITY81f7*lTmxq@0fmArT7cE9-4hNWFBtzn!@Hi-103D|4G}Y@o zbSEw5W{Jwts<#0mN_nVUohL1G)1-L|whPXTuZGxfI`mBQ6mEjyVp7?99rdcl(}O&H zPn7Efql$+U?(Gb+B=#iJaySiEoJ#_Eo#!y2DjmIebV9FV2CS*<g+#ScH1Mq~N%glN zK0BVlv*@X~Y1b9-3e~17!$;C<Cx%deZ3*;FR>O0PCP9d_4S0kvCsCgA<f>6IllCZ+ zOXCT=^WTI*`$q$k?lk~5M_17@yApIvOu{u+#?whpbZAU{9zVL6QkF;mTyHU@wO`^; z*}4F8_oM?eo8>$`O7VhjCYa{<GWMJHVdCw3ut-6HM8CO!M=y3VyBamgZeApvzUCt2 zm=7bTA~*6FqsNT<Vp~q*)pZzoO`RUAEx_g8Yw71BZ8&1#Fs3=c3so#rFeO-oqj~>H z=aY||uWJ!ro!JH#nir6uhFRp==whh;aTvGgw4<x~3^HePH}~~&D1;qZK#U`%L#Oh0 zOg@~AD)TtpI-vwQj~>FNDl4+I=pJ8UqG6KaCoU!AJsQe?h1ZuK<IOE5u+lFcieIad z`2itle8r8tiISw;q*gGj>*0P@jDxx=J=(Y-9qz?O;dXsV5`W|^3LYncdv7VP9#A7w zB*jT|u@;vR(+hI8x9eIq0zI3ZK-Qa%fG~Y82wiASuA6>lG)s@d`cvL?bMa-6h;`w} zOIcE)VnmMfD-xwUqv58uCXd6KimE-y_^`2>bN}OlF(w;vjAS%SkyWH`vU2deX%dWl zIDwixy8-Pg+Q60HN4)kEIy$t-PHIG-?l=K8;bCy!ARWU#b>M~hM$l-yjO=Pp2X|8y zcsa5bUXI<!<oKErlV^GkQq9IBVE9#R@Tg-hcm~mJiSih`WDKYAB^|=qC3K!&9+s{* zB;JS4fy|!?BrY)#y_am^{)K#o;C<ada~h%7Z4)iO_7@ti-Nos01cEo;;O?2lVfG$n z<mS54$CKXUwT;8beMN?ZN6OPkhu<)1;UUc0R1Y)!;y@mB$xzfF>>#H=a*{LbjUUgA ztT+lIPRfx7QJToymLYnH4&>L1emL>z6?}8iLA_8O_W_Nlacvn9pCm?W^QJM?C&!RL zfgZ7#DMc3_^Q7NO2XOyHPkQ>PDt+Ad8|Kc}M+?ye_~<hYZ*c`&Hop$muBpOv{0dOu z!;dhtj=}x8SHYMc{e7PJU`CB0O}sUh?j5O0ruVB8@?;x&_*FoCz8~C){|ZYiN0LJt z(dazOm<|iq#Wka<;B~1;U^{U&bk$tO`}-R)sDhx5yFXnjj6uhb$>91(0fR=gaUl!u zW3$sbI!R#{?X<Z8n_pkYuPW8}O1+m0GT(=6elRMIR-<N5_hI;_#mpdg1}66hlPi9q zB*!2UL*x7K!>CK#Ywr_SvR#1~y2RirmG78h5)I4Dl<6b-1BVhbAw9@~=I}qi=Hgan zXX9z`7Rr&t028|8aypbfuE62bt}vS;@|iDp+VHb@5i_Loi@UyH3|+qDGdNnGU=Ccr zCGcCB&F$F3d&OS^Egr>kiDzZ7w!Mz~UL4Nk%&BD_57mQ|R1REUnjr|>Cr0Drn!)Dp zNHWM9V`2(taLwb!&>I&=k?g8&yk@i<Ze+Czir1dQ@a$To_0`b1S(<b?moU)uo}1dv zTM?KcyjdYbpQhT=j>8&oX@Uf4`W68O@mKMa_c*%AkpKT<lgiABl_3o(Q@Dli{xGr) zGckGNL$3aUDVecG8vf|L$7K#b(AUtKv|e$h3g7>s<ad^;x%x3K+f4Z!;-`3W*;$yA z-^7LRb-8?fH%LGK4pY~v(0A(cv~#~XdARgBE)DE~R^w<eetiY|KV{+U2q|*&2_NL; znT(YevhYc=E}3WS%(z``#aGo!!C&er^q7FaSyz?DkFg+QrfAb(i#o2bP#Y%kYqhQ$ znIQLor}6rYM(sDl$jJxI%$&h9!WHS`$=oI>az;&-&RJwi|HxQUgO#li;AKK>#gxd> z<0&}cpg_`6?S<4)jCefChlN)@gZ9e%g6_3bv1x-XtuOq+yfe~3@$@68Fd$2Itr|v+ zV~^sz7t2v$iw*3!VnprRayUO^p+`cW%I+rgazz5O>RB3OPCAA)ZHZj}X-D$y*ClYd z>q^_MwK2mD)}nlz4%r;`gXxv37vzUp3Afqm(+oaq*eFRFlt&CFe(!l4%CLW+<^B|( z`aZ(ab1Ptj;xc~CD<I=?WEeJ}j9JpELOx9|$B?S`F!XgN6ZLT+Ew7kE6rzXI8`osX z;a4N5W7s>~HSR7vdRm9pxm}p6cn3qCj6@m9P`K??&)E%}!*hDi!Rx*zZ@k#Ry~&iN z8&=7Z)e|Mi?x-Xz2;l2`au6ubh=3LQ--A8hi_~ej!G`O4utdUuTwHt<GU^rRq2V<c zY;8<#M(3i}JUv)E`8@CA-2q1NPS9QO94|~fht8{%NlKC@OfOVN=K)Qc^(2xZYl<;j z-V4t2C#V~%++kC#EA6X@7q(7KWib5_KDcX1pA;(*=gAfLAY29yjkF>DYYX6dycV@x zRSKDJQ&Fr(l3IZn`KBpGGREeD{F>op!`&G)eESn_LZb?KTB3zpj$FpAKYrqlN7JBX zi2@E+G^G1{tC`yk$Cz_bUj&n0k3`G21$euw4U1haqx0RZsIl`m-Yb`*@qcuwda*B~ zdD)&gc3Y9HCGR0F^eUq>&5rsTXp!p&$5EGyN4Y`vtuW)9fOuDa$C}_;PG*BFs0^2( z>P}xcM^6#slaL_j(i}&wht*>6C}c`M7{a}S--Sy(KQZf{#)J8dPK@_+AyekA1CuaC z`gN`h&0H~>Ubx+jqC`zPd%QCm?)?Pwp65aL>Y1eRqdm5M9FN@++O+(l5(MRULYIL! zIjvg&FK?d2zP}PgX}$r8yI+T3;{;mDvb1o%2gZCeAkto`_$#oInWPy4XAab&W!O%v zN!2IgI~+*&3_TLDG#;7F>Tq8AQk{-cmGB5gfxqo1P?vR~>Va=iiPNO_)t^AmpAsgm zP(W_h*pN9%nXs*2AIEr)BH_zl;*pREB)n)ResLH_`+Wli!^bVLW8RKIp;`s^_dz#2 z4Ui{qZY7}BnLFs=lYrLu6yb7=K0UL8U$HDrf${T{se(Zg6sh}?&V4mF(y<TiTIT&{ zuk3&K+5dHw9n)e(nq5A^z%3VA6B9_j?LGxvPiAq-*A$6?UL{U!Kg%V>uYq5=W68#^ zJ8=21XbACY#S0Ny^se^|95rPLb4YIreUx%j_|kSf$+o}8tmuu!ulI(L-aQ(0k*b{d z78NBq1zB+wSvd(=asEr;e?Q#*|2))Op(ZC`Z~32|TK&IIWmjnO-)pU+A~%A6S%v@a OW%*~avd$7ChW{@cAL-5j diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.pt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.pt index a7a9f89e0242606ce6624d3166c8d94827e7234c..277990a2d695715d0ec8365c44aedfd78aa0681c 100644 GIT binary patch delta 65153 zcmZs?c~nl{`##<v%_15ol_W!yp;A3(-zPFeBpDOQOquCr%#$Gvno~)dDI^ssJ<r+q zE+H}{8H&;*LLw;?zP&$x{r>o!wf0(PoqzUPXYYOO>%Ok*{?*>^S9`x)zp>)q)rsi- z3&1?M7#hY8VgqK&v)OfObkwwI;GL_)%7?V0d+r@F&Gj0DCmliylVoDx-;C|L`sfze z3JLvG`6U;=ay_zyz8<5`M;%@WuNL(qYG1wZ;;{F^<AMy~O>{0Sa!&=hy<>R3Nt0Dn zJmxHGw_s4NteAh`*iKh;*W;1SjnL@WjHziKz&URyllyW5D%~@1pRp@nWud`>L%Lyj zu@YW>G@4(2Q;zk25dur}=3&|UC}RD=gk1{<*f?08_j%+|6kzlSS3CL<BlB5o^_{<X zeoY$9|6C1L^G<@#a(#@QsKaz-H{$Bk;#Zhx+5svR!Ei?HEPTfb+$Xrep&z$krTH%5 zX0HOh&^?Hqj#Os+-D0@3@E&NKGiF&+m$M3!kE9~#7v~)?kgd??*yN~@P`Mxq&Wwp7 z+Ea4L9<ythb=Df(&z~h8=S#qn_JD>@ClnOM!9PP+%zbYMx#B$@$aX}Nl|8Eb#^$Tg zIsOsX^mI6@+P@5!AGm}bTpi5+_KrKtwqe@pySTS$GB$k|@$2=9h=*|$v=2(hw>wRF zxzWRLU*IG@QDzby<J1Yi1M;Q9yMcV&V|N_e{D4bdcn3K!1Ml7v7?Wa4UOzjB&1nsS z_`(@AcBQhG_N<eG1u6@`@XisK(p5p0x{hE<dKIW46Hl!JKf`OebeOX<2A;}lu+YOd z>D;vpX1iU7H)j{oL09rIc)$}fX+afe#1OF1^2Z7dJ)GsG$NN~y!^$%c(Y;a+P2*eO z;jG2fX>dO{I7*LC`0tHaD%Y$pn%n#h8gEaflgrQGi?$kkWxf$7x|fox`^NGyQ{!Nh z+jY`9y92*xR?zybRoK~DBi$z(1+Epp2+7|~cfUMLpOPTKNqqsF+j@qsip+$H@z=0* z_CIQ4FVFCcAwQV6WU4MYR71B1b?3<Qk8gIPtGrl3%dOAh>S_1LmCB*4Bz_RAYL?~A ze~%TE`xj%3`*u2~FdOe{mB7cuCOTxj5xb}mPd~`C(OD+Lc#F7T{A#Ymb7@CKsm+sd zjb|$6*t`KJOIcp4|3e(2pau19<LJ0$Iw;=06r4Jakmt3g?B6dHNPfD6ycZ9(faWVR znZq|H7?f5@o^gJ}|LPD%qkakQ-CrT2^E-FYBVUMa+>2E&{OLQdvFOjA#?07jfZdBh zyNclq{a|P-$$}e^!{Nq$ZKzU7B5t+5sFUdgcYh>ek$oaam)4VgK`v-pqa=y8PoQs) zrBOwHD-hSeZ-<BnnwUR21tX11XyvRqymEFL)-|Z(ZaNEFgUoqyFpd5*SAd$~f$*St zFK(Wa4$X~9{D|X*^y>O;sNL}bA33^Ur>86D^X?{G%o0J(kyJdqWDDDS!<P%PJB&Zd zuE4K#r(osCh2+zQ97@7AVBN@gF%2ji&-+z-peQQa_U_MraPvqVYC64vmBZWVlQA4` z<#2_ro~+AzXnWvDxk3D%w*OG&&<Osjf(e<pV=nC4=Se5tP+;>Pmcky}F>q8S5^w%_ z3Ttyt;tb7g;1j6U_iZA}9>8eqx0Pt=(?fOEjzi1N<1oNl93*(oa)qz=WZ5hKPIMhU z55s=mg9E--;6UdZ7;@|#HXc{v`)l-&Tb1u&LEQjW8v7Hx%ICrNcr&>0G#<}4c!7*j zIDBdULsFt&;l?HJ$yK?XV0vQ}Q7KnuO=tYz)Y|?`q8W%UTDsBLbp*d@@;IXOtB%@< zFZZp5aX6MO_(v7n`ti#92aEd0ZG<cQKHSoN83vdJf$MT_jQ=khE}r{NPBm=B7+#D0 zUHP7ljJgAFNB_bh?{(Pd@L}-V{T}yY|7nb~KZHv-1?V3>2<!V-!GgR543AJ^OROv* zW0x)%JAZ+IjfZe-Qx}Mb6uuPs|M??G6o;Vb-9aj@xWZpr_lJ5<G3FmEn1|Z;{W0Hd z6TVyd1@3SD3`@J4u}+kYg^`S!$(oRu|2TN&GzD}gtwHT9UA|IXllQo(z$=>0XIc)y zaG<UWyn<hX)e{}IW|k63j(9<TCR~I#dJNW7sEC=L!xx&@tA<CP&tU`qOGmSjm4eqs zSE$>Z3d_u8M7Z@R>+slv8z<{B+s-90<ERhhwWh++4Nf@g{!1L~Ex?wyG4N<6-~me! zU8ZBg_P73oEVoq1FZ@j+L+_yI-bYZMtHRGTk%Kb|PtZ&I5m@Df!o?WzP#9|X0Shi0 zqa^wb2urp24Of2B*4_#5<o+}4tUZW6)A~^yr^M$dJ8<<m4@JF6cd#H0QDOIeI;8PE zSZ7|MHkta+IphweCCn%5E?tK}6$^ZOl8657H!);yG%}Smu)m=V2S>`VdlfHm!TPCu zT+CxKM6CTBH`iUmq-n$X(yPyiI5UIx)8%>H_6Yn}CPvk$25|Q7hU1&eVZ6z4%!^qH zhd<TQab9iES=$AUFD=mQ<{5f;&ruM)Eal|=PazTOk`1<+biKk;sQa#qhj+=KU-@F( zdq<yxil?;Iz6C@xi!frn4ljPFvlur@C-I%bX4968eZ1<WQ<(Mf3OHOXq8rbS<hK|P zhHmK{2&%6m<ANEsKK%x^lWvoC-xyRi(%_q(=`ee<<$zC|ae{6>`DmU<yPtWnk7LgZ zQ9%+Mw9x|+Jr}?>*$WU6a0|n3en6MM4q)XufEPSW#Soo03}=tl#(S^-pp)Hm)Rhec z)u%3C8Zw*>U9^}P8v>;0o4~Cad1jWT%6zg0VYul<ver=^Ub!oh9g8|~Y4Qf*|Mw)^ z+A;#xIr_oW5O0jg`G9kun&YnGvoH?h;qKJYP;PJ-hgLPv!Hp-d&fqa!^G7U@Gk*PH z;0aBrTKX1G7&=q`Yd{arF<|QBAHWr#9{9CZmJL+2W<pmb(d{>!zc(}!cX=r>|H%oY zd&UO1diK0f;NG8=Zv93Rro4b@#G1d~)=PJU2EleKKRCW524csy(P`EvAph<I^b)6% z#Ossz{xb)1qr`c4p!H1{*69tvp^iTUvAZrjE}Q|SH;OUL=`Qj%F5ud47_T}soA_GG z!JhN6*#5$nf4}7l>^Z%Yq^q`|-HUFh9OD7D-K`|TA`<rn8Q}NUHtfz(fNRnkkdrqh z9lNzrZo_4oYj+L*7#{+QUK>0w?#INv)5AoM@^<%ufh@ndm61swqae8ZBCWaP57W0b z(M!Xx!QFm7AS(3}%3c>jZqs>G*!i2Tv##Qf)?L6XX+J)=>pwIVlHmH!Y@E2wkeA*y zz_fs`#L#9EZXTBh9-;`6-)l_1u_!X?#D1je33$O`ASf=sE2i_*4S0LKr?`5H2-=$? z=*lf)F!;t(Y4*`O_&D-8E$Pa_xOD|YURzesHqL<F+D-U9^emS#;}{5*W4VcUW1wn# z6NLBdhQNS(80u?@wU?@3^N|ktuXrYlG<-~ZwkYxKvm@Yj^?UrYKpATzEQD2uV$jxF zoB~P11(AHpQWlxY(TI!!BpYNw&waCybgYqHH(rK!(vopSm^NBXOu~O(9H_oV3hv)I zA6$R$7G8&~#(`rz@n)zk>Le|O3q5w=c3={@;L?hE#tVqQM<H15kB7i+eWv(F9{((p zLaxbOGNil<qc**#3gXly5T<b+$U$`+<}1(3f6c@ZZ}U-n*n!tKs)6CBzmOkgkKpmu zT!`2h3Zn|E!J(W7Qx{vfuO`bIpA|!8mmEJ{8iJIIA(kUzaPX+>@b|SYrWDrTL<LP; z;PMc6tcrjEEtjD3gf7eZ69)Bb!tq__7Z6weoz0vo=kooJXyT?rmtb6jHd7nY1CnM< z_%EUl8BMPcC5uzSN6{;M`^Ac^DH$oG?@;B7vkUN7+5iZ@l#2gt7|KHSoua!n&k>)% z+mv@TqUm|Z!M67zz1f%wsk@%wtj}t=ZEH5H+0hA#W+~v}q^LBgw^4DWr<#n6r&_e( zSc7OaYfI1b!H)bn1rrvfHG{uBVIm6-tj4jS%lI|Aj(l(Japw0)lZ`c-#p-Wb^6NFH zF?ER}%U<#ju4b4rZ`~&788(Y==Ztt+xmP&MLX%yb@B)tW%Itsh4OMDWiB7Th6sJ!3 zDUpf4Ye()blJqDTN`^dl6PcZxXZL4DzH~`xsHp0TpRISwK)a-t0=s`2I?}NzdXi6Y zQTk%se|C2td5C6t$=K#5#7cJGeRA_KX|+r69Vwc;i?`eS?Tct=N~rDcZGqBj;dXXi zQwNKjHiw9!_lMY-2V9V(++Qj^l6hTh_eQ%_DsxI-s;XZi*<PeAnrm+&91qvv0<LL^ zWD3k|+OJ4N4S&_8ivNy_UPq}44wof%O;P(qmloOE-8aaxef2P1bZC|Q&6H(FM3<i= zh?*vJh*U-Cb~U|9f=61O<UOAyEs9DIwS>9Z2|;T`A#sE4vLdwT;#sn2kSnegw$x2T zS`tqZ=Ba~c_8|0$3L&puQ-rKPM>wgw4E!5iM|Mv1r{DKI6k;|e)6<V;3%hqOB3Xu) zNso2|O*{IMUfvf^t~`lC&G%*$$!NRUoc)4SSdaEvN_g8MNP456EKHAfL?izKa_7S% z66&7~M+yz`s+hY(CoDTl|7EMQ@zGcDSo2TF%P%BHr2UC+;Ywz_N6Zgsx1e&0WjN8- zfNxtCK+l+2qI^mx7)&F)?I=0U(f=&2i{_c!sszm4l>@$-vBJB|SgQIb9Q?nYq1rNb zeEV}H{_pM&`0(NmiUYDy+!+WGi#sU3-F1>%5T(MVnw`g1FFU9!aKwD7j@{xo_@L&; zhW<W>MjPirUHKwVef1Bwe;LSom0R;JRu?1^x#J)g?8P!ieIN%$_Kmepta8FIer;kG zP708N?A10fW5als{n`(T*5|TigR4L<#~)zR4Yut4BkWe3B1V(P1L@JN1K0)QERs6Z z6GL`qkvnxpQ0H)x#ZKA|&O?r~iL26Z-aIXsePj~L+Bk$6eAgz2{~U#LwnzD$;bj=V z`6>Qdz5%4WP55XtSysLx0^QV>)2fma+_D7)pyo`_W?mYWzgq!I2j0Vrwn;eFe=47y zD1J-i917_%!)>Vibr-+Bfx@klLOi!kkH7mgl$zdE$4$HP;P-Ypo{y2@7%~sL)Tb~v z&tiP0qYo#%tLef~Z|F7d1dN|}Kxm$1$anXf2X9|0vzc?QaNoU;WAngtoc=Ts1}y2% zZ;JSezgL?=du#+KKKzF|Voc(%+<FL@XoZ_DuE0q-_1xfyZxAf~hTo0uL;04;aP>b= z7DIISgeZ4bvo;IORMgp5`6%{s;UzHKtHL@bABFoD`bz$s6r8kc8LxK5fM0Sol)s)k zjbD4=ER3K0QRw)!7<H^8LFtVnvDwg{|LZW0e{k6w#EQ;FFx)H@Z$9rL4M~bHZ{aeG zy_W(VGZxa>0S9q=NF$87GZ&VmEW?ZiOZi;cFy7)rHEg@C!BURA=I3U+iKa#+Nqlat zk*+X!BFQ=tCtccLDC+oKZl@nINopLoSQ0ktSW$jlg59dUhN6f$cO=Vi%ohC~@mTV- zu|O>Kn7+woN-Bstz8Q)Nx1W`^2@^#_4g}dXD5wbMZJyZOn^!N2U;0yGWGq{-W_Fe| z<w>~oo#qtLYuyadQ2C+K$BQpZ?q=VRcsDCcgXLsJrNc@@@hn}sW|y6G&AMwgZXW3p z|2zv(54cMqMV(tUTUR=uR7DtPzN|>+lz5$}cvERn#`BNT!m|+~bvj16QqNg9?zUF+ zDBOYD@ufo2-F{WnvdT!ve;`N~Y_b=%dMXg}kg-&&emrt~IJI=$i=jt8k_+ME`>LuE zqW5bbiD+C*hFxnUKW=A{DK{O6Ux^Ba!*$VqKW&U$X+U%OUm?{?mOz(y`F*OI@PoYi zEej_<&B9sIn}VasLe8hc81v$kF>IU+R>Vfo(vfjQxkd+!_pJa)gBgh4?ITW(6?FQ) zJ9PQT6;#7m3DP=)g#<Mt%r&xvs5Sn$G3-5w^opS?63WPifvdr-BAR%Ysspu@7lzK! zBH`n&2x8HqF!*^Z3r2P5lCPRN_%FCb*l-7MrGg^A*HDR>4?GAN2Hk}D4TA%r1@z_F z9D31nI<?uQNb-7KP+x6LHe=9G?B)8y#f;bFO7>s4Smgw{8IiQ%Xb?45ttFW&0N00i zay5_M5~lb7YV^XnBE#WmW&2P}MsFM;>`sp1mdgysLi(D{Uv-pzn=}ZO7EedFLpmgH zgb0Q8dHA1d985K_gU1`Da0AblKuWJNxV9XHl^YHS4Lud~)&PG@&g+M<orCdD;$WgF zdmGn($fK1bW)p9d!MNwkO6Io0fPL7y2_Bu*qORA@(AA~l6?A@7G7NW@LDkb;pgZgY znvHHEKl|3sGV`afXX+7z`VMZTxg1v>Sz@>H8&3lsg>Wn;N7@;Ek!sGUm25shr2W@f z!S0-2qDeL-L?Lnrx$Y7`CdaFC2DOT`X6qfP@#%R`t9p{oI)0vG+iSRzL#DJ$5<t4e z)g@d{=XUO>MvY|u^isNaS|DwWh#>QC?&tifZgb7^9VwmJMwiWd%Z2Wbr%^JgWZ8r} zg88`_)bEoSE%v%h?SI76D)m)VdHE1xJIt44h0G*v=O)7E;b)0y{aadc^}bMM^;jCl z+64J)a|Dm8PGsamGrIra9g$eA`3d(p_Y~Exx&;fGM7VOgDeSp-PGY1GM1oQY3~n}| zchhS?wo(RbE-Z&du3p3`N=(S^crMzuh)g!Mpkbft=!`QHvFqevsxRGveiQTP);)9K z^{&N4@t+Ej&S>S%b(|wQ%d4rE^G@P?Z~=99pFxkr-4v6=g_lTXIMTo41F7xhe6C%W ziKe%W5>^`Nk`dB#)I8rpNT~Wis}i#WRp&fO7bgcE7CDlm>H63)vsSu$ZW%eh?vwPZ zI8hpEXD|FU9SCvjm>}Pk%eiD%aCiQc3E(lB97vrbI<P~53rMc9+t8>*-|w*n-@W2S zVbr^0q;$?~a!dS=d>>{^+Gh2~Z^Qcu{*|Y=|B?>S5FKS<%h(2P#h5mFcwsd4I<5t& zUDecMm>#LAZ59>XE+N?wDWXlG4&<gmI9FD*faG2tO%g`mkSy>Hp^Iwr=@jvDde$vP za0#9wjE&Q#CKKOq=>}GGx471s-VNG<C%e;VbmeqNKUoS~>Ne@lN70};KSfykpii7? znE;A|-w?BgDL8ljGU&`7h-~^UPCVBDr&UyN-^K^ixYf@DBZ1MsFD}s;#%sBk`a|GI zfhP37c%1y*p-uFEtrCJqrjewSa^hC%K}Xr_rl;Oz6LD<97~#yBDDL~AwZuvkDBRh{ z;f8;UVXxc}jNY-B4jJQ&wvzt%hOgpg$JY~PbO*9-+K@wM#i9^SR;bQ2Lo)aadHngb zWc%%%AZrmueFrV3x)txpQT>r@i*qMgZ#n=c=KLfUVh_$TXe;z=&7_%^`~|IXC&V~J zW<8e8xr#j_jNy>`31Zv(ne<Pr0Qq<g>=jPZHcJm0Zn+TWw;98kg3(YJy9KMg*W<`f z@npgC-DFnBF4ARu4p-F}fI^1@dVSaoLuHM~uBvK`N)LmS+3|SvYa*&tJVd>GIlfnB z7I}Oy1F}9P5zT#l;)BatB`g#raq{1Wf?h!bHmEfdCy#7u_U#xkSszFS?Hf%?uLfXy zi5%}SnIlR4lZ2sGFKN}WNZLATG<SFQU9|n(K;G{jW4EhM0!XyA<^M;qxBiRVYOg9I zW3SrB==Gz$rY{v||G6bSmG>Xl{=Ndmm*1fO;z>fLcQ*I=NDTI?4yPaHrNfW<6&%5n zqU`(kaF=EoSztQ?+#FBS@V7sO)mrjYZ(XABpkp6r)^Ss~T$Rk#ii@da=OVDXF`E2u z+9%G>o_osl6%u{J#Yw)OjEs^}-|PRL=!s)hqvtwUixd8Y6Neu_y3T#anGfaR+jDo4 z6y6|xv)zWQRQ7=HI<sK+C40yzU&u-0=E1@>&(Qm@2fuGn8UO6d2L9cPxy-@x6AL>w zkhT0v;~PKs=i^s7vxfRA+Dvz_$bs%y?8e9*tyl2lK_+=1uSv7E%HsHC(fDG<c`<g+ z`atX6Pi7;XlS$ImD13Zx4%W0z#hAL?98V2U;8gL@Umg0zaR~X9OR!_DD(&~SlHN1l zfU;i|Fq03(dNoDJ{cj{obq-^J+dlK*Mt51mLVtKY_B)FHonkMj6HL<D0w?{><H9Bt z_FioSuUs;T$!rv_V0&u6VM3NEOi>769f9Xi^Y~*_o^XiWQ;El-%M(dcp$D{X*$QW; z&c*AKT*<RoRWL2?53iap3WJ&w>9C9yaBS*E&S|M0F$yKL<##^j&QWFe$A;tInuYc) z3CEeHn?4J!OJ!SJKC)wq>sfp26mb6#$|j2gj`6!Y`|%q<6`s7TB{mauc)nJKt=B8T zf}m-TdrX}<eV7do(>L>tPc3n3LmB9Q9Sl$ApT^ErzaTe99_-d;(o&{HlT0|$dG`#> ze^*Vz11=M_mc?A+QGHl)JQI6yCqKV>Gn+dn!Cs?!3$9SeW(y2enAor~f-f#qXI2&u zVZ-)$_(#{7>E>@_X|?^?FZE00OjH}Sy$$Hs^_CvID$m*tKSSs<pve>eVrozr>YW(F zM{QR?)en>K%u{2y7IT1(8e2v~5-N#RlL^{4h0y0@G|c|a=-8Gl@~1kU+D-A}&m9^} zJYI>#bX@=W#cNAK@JIgCV#^vgl3v-5Z<ullmbh+WTEPdvQTZ4qdR-y@pWop1O&j2K z_eC;&xgYHf8c6s((bNP6@Vbp*V0U{x4Co$&OP=#M>B&>FSqE{hMGcmvX~Cac<6(AE zKl1jjInI_}OpeH^!tWgec<1TjRjg_ukkYY*bmK-9T(lyUI(~1*e>Hjh1MU<*tltM= zz=>e8{p(!#Q?(S{Y2~t;V`Naa<|cHge??iDP1vw03$8Esf%qe7Xy}?oOl5p&kjVw~ zs5l8-kBm`g={0ZyMOeM(5?^!S7fM&3l}6>yhOKwPF-JTy4dwfI_uL7_;Bj{g|44r- zZ}7X8E>KGX<LkaGuOkqvm#kn1$|Csahj+nyh!nb`(%GApoj7V`Be^rZ3}QTe_=zVr z@yGKg^V$RE!-<23iI&C<I^L=jJ$?G@xtr$v?ye;y>Q_8}D{~^>wssY0XQ$K1m}?+5 z$;?82PbAILHbm#rfh6GLNx^q}AH%$)3`Us)VZK7CK&LyAsWXo8j_acMy(_I@ZmuVz zn{-(8SZDt8h+w`}d<LFJJ%?)=AZUP;pZPG4-Rg<QjKuleqNS5DNqz`hBjkZs&?H>C zB9on|EP>WDa{Q$DV`B7uah;N8R~r4dQjiw*ZHzsK__55NjA`*Daz|=NeN_$}ueOVh z3@aovv$J5~qEI~k;vcn`8_&kSv?RMiU8v4OSNm^k9HClYws_Zf4R(Hc6J!RtqxQ^T z!mYI^E+R2}u)<szcRrXE&Nz=Z_RGMGhW@b8PJ9KdIxWy8>?+l5S||i&dcut|7bd7~ zVHxR`7_@yRIE6TK<6C}{Un`TSL+@`6eoPg%y{?z+ubD?j&T|uVSIA;08BDHt&BW!4 zYw^D;i~AUIadCFdbK30a$1fRin^X@;hX-$mvHH?X2%a~cti7bcg~Sz5Tk+QS(D_&& z7Al3{fQbL-!oLPI@Npu2_G31uW)Ox$D@Wtd$=^YJ<UgQMImBh=be6q%3gM^xAd0H( zWbNY<pflk}-;umRvZl6?j^@oQ*Cz<e@3hg>rT%d5$#<N3Foq2Oq{ggP3}dG(gNVPD zGu-!8z+uIvyttwu$xbI1_^8Pp{P^V~d6U!0f?IeEZvEU%g3rz&qf>hXv*Z}s)oa5H zpALbL+&+o5X&mM@e5He@S<{q;R5p2CEw*PE5aU}zF|#WWE*?mLV`cY=s*5^qf7b$b zwq?Z4ffC#FG5q{zH*lmw1W7W<;U}obh;jO!67qJE1kRUK<J@)IaFEeR{=0E2zd0tH z|Mchzr!@bH@UQ4R6(0KVDJ?0y%PR-oYF#$W+@weXjNU`d1RE?Lwn4a`U4upqhslZ= z$BA9W0EpDx3OfRyQisOj!aX-8NPj$_OShSzw5^ej9;S~^eKg>vleiEc3=hS7KP_;x z=8EFGX}|c@wHhdRgb}AdE!Z%8IJ+<|h>hyrf=7MM(7G3Mh>gNS(s6hl?pWuIB(xuv ztZ^rnRr6^}%~`w_5{y@xlW>ZBBo24|P0Rn&g_-9=Snf@Rb2pB|ou%s7IMM+8Qd6Po z%wxJ=yeOdX_IO$9GUX^d$yrVgie92?s3Gr~Glpg!b>Z_fwII+ag&AHnB6ZIriRPRv zGVr53PrSC`OzS@UzW<PXu&~8)|4{g};1?dem`VQy-Um8zGTWSc10Rn*jXq|_VB>o$ zeu>FC>?&;_dV$yZ4`q&gNsk^C>sVQn?IrU`*sG6Z-hpGJB*qM|)Q|Q1dL5IkrsLzI zmRSAeGA8x^N<JShf~4a9#V(_xcyZbp95io*VCbX4JMT${nVBKDb2RezGsC&&d@q)_ zQH=%OK7-yDw7J>uTfn&c6YaNg5}&f?0{PJx2tNnCMh`eF78ML0DI8b-3Uu2D-p(_N zg8M4!lpKquV+`>2n+4c0bs^PDP=|&!eL_y`YtmwC%$x=*F;V?O7~`MKhGu6lHNR7m z8_Ps&`m5pWwdpNbmqVe9Yh!UY+V~{_S@eWU1|3^G4-3vm@y&;2*yVA%&?P&C|E3}~ z#hP_iENFc+EIRrNTy?Kfn~dFD_3NpuD@qm$wa*GyJ#C?BdY_f!(!;Iabc%lNzChbm z6Cjvt!<IjuB8TG>nWdv5|KZhPT)VEEzxC!E<a<T(sl)Y{#(R4xSMY^@!O7U6t;1?t zuCl+|E|HaSa-@sSrq1HkC-7B_KKr401vWkN$6JAl+>(MMj2+#IuU)QV%A`;fDJawE zx(L`k%N!Kip3&b64^opm`*FTbEG^eaCM_%TNX7U-EC?yXyIQB{fL>!>J<OeNP!(fC zv6FqRw;z89Mf|aAQ~B{8$M`EX|8T>oc)T%Ig%{WMK4)%u88~$RY@AkqpZxLgWX_)p zAZo(~7}FTay4GBRHz!lL%Tqp~i}xVx!xJ*|Q-6^Ad<{gCUQ5%?ou-$D1aPG_zl5S) zTY=f#AZz2clUD;xndw7&ZrMc@n)p1Hn`1klxjj>5*3txCc4Y#8sPH=Lw>e2nM}*We z`IiUSH<g>10BiZ!;$Z&WjHCSh`Fr`GC$n)5oP!nTCh(k}TAxmTNAT@~)r_N_R7i~A zom(EjF!OpS+r^XW=m5N=oea14)N%_Vjtdj2Wr(j=GVL0mj4vM?6t4eWj4v+@2iWF@ z{j@CDk}Juu+gN-<s0gozgHI$RDx{wOa(6)SqEG+OUp|1W)A6Enb{m83T@98PD8sbA zv=MRHRcLfSjFt1Yz(>oYD0u`JqqG*kE}ut^kC$TyU+;kCZ^cyVeT#(rTmY-p+F@pK zw_sg48Il|Zpx(UgSbXF(l}#)sThHs`cX4VQ#;#4GU&$6cR5ODAc{K{2_$U>p*qlVJ zw}G8nXoup@#c=ufS@2!`o)lgA0CpGD5VGbo<*-OlWRqcmrZr36`j#WwDtx>7Ia+x$ z7ih<J?2cT^^ym@#b>#puc9SB!+i;%t=<nsmFE&E#v4P;kM}qj$lIg_ofePkb_Jm+b z1y>!ahO(Z+(fQjN=KSIW^Y2H&dEI}-m05Dd*M?5u1E#ED-CB%QvBM~&JM&vrCo=Jm zbjb1iM>6Rj+q)({m=`pde|{(nzh772EN{0H-}Xe*cJ(G9vqE9P_s2NnnFch@EEPo@ zRTh&OKHteKlXki-Oc9LFG}FC_b!4IKRFoK*Q`rGFxXdR^Fwgi!gt}4WS%noFN7l0l ztl)!nrxiEOy1=-!eth|dSk|NT50%R|GSjeUFx}dm4_|NstR{bf*6Nk0zU3c{MS_3w zK9JC;0a)krncDQG5tk<Mce=@boiwpf8-E=cho!GJ!=cf`_+IB<)HFd2lb&XX{)7A6 z;8*R`=wd6`(!+!7^=WA0{)sprh`><gbpEnxF>4&T0FO^CVb|RU+B-epkEZt+Kk;cP z(qWY(?^8C*ot4fu4qLz?JuKMU2iu@mNw4V8D|d7h7j_Wyv^{jY;uJJ+8be)MblKrk zEqKAZm}VX7q5Ja8Sa9t<^gLvMYjoNu{H&vKZ40T`QWk7a4guZw>b$@8GuZ272rF9+ z*v?>YmS6oC)n^tJuWn0VyJz*|N+*~x*MK?f&96>a_16-VtMVClT#50yGkEdbzRP3A zZY_wZ9><f@{n+(t5&4@I$Xic%h!fi8U|i7^(CIfAPi=fo>(k7+AMI<&*f>>I_3@un z|I0HX(YJu{{R#d0b2Iwc#Bj5Z&w-ZBlW^zhTyi+F28HWMs2ekf*;fS?Z+#cdzk73! zDIc801|7(v!Mpm@qd_-2(QSzlt1;9=Reu%UyYvR9KX(;mj!hyTE(AdJoe6ky;!=Ks z0mG|5EYa`#7BHP=08^gD6XhM*+`p~|^vq8qRxdviMkV*sri?JUy8JX%NG&7_wm&Bm z2I_KTeFA)x{~}};Yw`aL`G_O+mas?armWIMoW{4rHu9d^&G_kC7c$l72iRXNZ=Nk) zjhD}EfydeYEbPfAaM-nmy&Qd$H_%wbOcNvUvW+?K_V)p(A85epe2V)oBmhgy1Ltv4 zZr1hdD5ICi{$BgWMLXL;Yi19p^>sRz80A79+8!j?#i8&-cN#zK(iA}|4%|auo>PO( zH8%K1Y-XST{w;G>{|hePbGS{H9>S*NdOG`PIvq9LoN1K!L8y8Myeq4u6VI$ctM}7c z+@eu@tH=z)hAOi<_kKKgNDiJ|ISaGyXtIW)xx~V-o=)u3UXJ=+$0<5WC^BiJ${Ckw z+42fHZfqkJm(^b*347J~i`@c<&JM@uVVXQW_#dC&ewoe~-pIEY?P1yDb@-t7`uy|e z#r)ERS3yZF0;tb7m>4KqEIG+zq}pTDbiYN4hDT!A_!L|}&5&DL!m)$rZa_w@KZKh~ zu=e6Bd?GmDoK2z9YLyFIx|fuTjBFIsrUOla_8w>2QZ%&B`mUj_Nq0%lT|=_GJr!ST z&BVmt2jO_SBHSDNO>i!Y<gecr6;F=T#n7X1^a2h=q5Lh@*SV6}-v+W<`?uoUZJXi3 z#f7wXW;#{t)<B<7C(->tKVmQRhvoX$Fm_2Ic3bMh@EQrmxePS>KhO2}#i?`Z2g=CQ z5BxuYCU~*6IOghPRD61ZY;Jogoi=GQEWLG)G_H-~dft{IRPV-Fn>P^)<Imhh7hjmt zM;~VAo+iJJ+=UfZlW1_)J7K5&5E%C3h18=$8S2k21Rux8q%*NyShc;cB*cZ-sc8W< z*(SI?>=fw{!tlg033={cOaCX_IILVJn;&hz#9myuqd#VdCGhqckB(Uvq~E5^gWZK= zVTRsd{>P1LsAi+cKOZne5_0Sb?8@pw+vd3#xp8jajrm(>uRkFT-{-=|;%U4oItQk% z2Y9^)$(Xq$4dyTQWd7F`iG%tY{L=b?^BNvaG*X_B;WD#P=S>V=h)RLAn<B-q-lB`( zT4n59vzM2Pk;4t=qS<|w?fmv}JHU2}xyY(AmkrJ9z*G-SUVP^m?7npX+EfB@Yl<P; zdomvDET0I!JSBuHt>xOM^?*!{DbE5T@yQcM{#(8R|8ccGr0&inmkvy2KKe~qXi-gd zpM-*0h!dN3SgguVe`jd7qoo%UhXML*mFM?31>o1MbNPG9>%dqffr1A~Fl+E!*eX$C z_w|;tqZ_=rH`d4al;1n~VXdZU^hqE;bY`-=((~Bm)dWdCe_)Z02kbrN!nYf(Vrw=0 zaO7r6_E~yJ&!_#di}`fZPP`>c^2uteNJAVpMOwGOTB_xJWNP4X3(4$OLmNN!5J~UW z5z;>k#!5qvgxgJuQWceCX^E!HsS=%!O_uye=SYv)`b+n(V@0ZcR(H_f2Sw>k4ieuQ z8A-*@L6U(E&33P*2a9x0Ka_S@_t-q`A0W!O<|iee45hQ~o-Ep5epOm>)lOt`(q1IC zuf8Z**t1o%bBn4-zeg(h_@-RMFM1>TX!O%=_DmD$Qx!ky$cGDr5z5(iwJ}#jPi@9< z@NT+ocvFZZ<Md#u{4E<>tH&ymL2m}y8SrbR`^J_?3wx}%E7|Ksw~b86A-xdkl*qFr zv6`n&9>b`qr9UYO^B{a(oisr<Q%pBb|0%q!E#Ss>pQdA0-lLWl!?CRR3FoY0P0t)w zCTbtwanW`^ZRa}=pxWXIFlpaHdVaqpX>Hads#8Xj33tAT9K3u)O*^$o&sJ@beYzGN zavLTY@zP6Lv*(0FA*R`GhUBm0KuNIV+{1~Ifxh*U18?L-FZN`JCQK8{+dY1+%{lj% z<rbM96CEC+Ew#B|U>Dn7A?0!uq$Bpt6CJ+w+OB;2c}Z^be%qnBv!r_#7Z;uMJt}Q- zm67IPo}^HIh4flwgJ{&<8=^YnBD<2+*`f_|yzR<$W=SLJf<)HInvzGUnzjp?9@%wI z+9v(|LQ&92aFm!B9~YX$1zQEn=x0(L--V)(l4rL4Gxl?3x9mkvUdK!2loF(!j<MDg zWM37%u__h$mZ{n;T^cH~T6az~|JN>&%dl+Gkqz^Op+g1<DT9>Ai)L@m*RX)3hR&k} z7K`bV+37_0Z#Egxu!D0e_(Kg2Kc{;ww1`15hus#R>GK9h6n76FMIxr1p+<J?+|yC6 z-20cc!b{urR5#oWT-^3k>s}FAv~42sK9xdZ``k#~6V}|#A(mv=(euIw*=ab=@HPaF zN`=EAu~f5n7=L`pb7%_IfKC+|e#_@E*dnsvPkW5RXxj&Hbn;mIF7bw5?>A(k`FE0I z{6tKQyDj-s;wg~q&<KwgE#j@kHIyCKCGJ^g@#D$9P;$dm7*bdO6UQwkWi>Zwy!I+w zmiY?fj`e4*U$+wr>;9m3|1f+0^dxniFp6r93}(~T0-t)3ap*IS_Z#QWPW-xvLz<Lu zQ(h+d6QRVf3~Irj>MzmvlK3X+Cl`y7*uU`AZK0@h)OcRu&OIEqX(M@iYdqg?h!Tw~ z&W5n*FUa=A5h!9}yai1-!f+VaevskcANPQJW|v^;oPBV1-w5{bP#`PMehiB;<49b- zlv~wzWv~7jOu9vj;pn%ueC_NV{Liv<SR^}_j2}Fnzc1c;7?SLCnfT3nm~-BaM}-bJ z)O-%}eH)>mE!%bvP2gWTIq{?ae1YW|d7SR_P$GF2O@}Do7dmqF;r-*+(ydEUMQMf; zxVIB+>>_I}k|n!?MBtVtI2D}`Hq0pH<{P~h_9xYdZrsVE2bCw&o`pH0-&5r2^1-W# z_|vITAf9$jm^MIxt5}oDX$6iU+tPJtv(5{<%fo^NI<r}jsuk0Xnyuu<#c?FwA(`$f ztRl)z$E0oxCJXH<54q(r@gl#!%H;ePYf;O)DiZm=MsPTCk<Lhy=aN6=bH!KR2-6=P z<{tA4$<Xgp$mG9$r{K|P`t@+MnEp~ja{2v9Zu{B}QWN}!Ycx@(o+TNi++`Cz-m@IO zz(oA}tCbpxe-ZPIF{0f=1_&%x;C2U`6BZxjxZ;);(Wum?(#1adm?F}}TKxg=y!4w8 zX{tzl0y2gCosCrc{%zW|_qOzaK|Pn_*+}Gn9EXbst<i6u_=afjwIH<KkT19@93)1c ztTB1@Ye)_{K>an`==2R2xN^B*GV{iIYCU&8jd&~-tzLc_EM~|<PM1LM7ld#}v^`PP zR*(i<m*L}I&LR<-nfO=e3!7cI1(#0DLV2SAQmK;)HgR!u&BhYZWc5O7yC)K~&%|=W z#f9bMs#7ercu<Mob5tOpT@zETza$6Dchkh?MCw%JMhwgHz}<W+i4XO~#Uh|}c}lpV z#g$T(Wk_4+71i<Q;KpwSFka^eM_R6euUi7DXDp*f?C;Vm#xsSxX^J#u`ANx;_7lQ@ zl1SXO%Lpf~<Iwx31jW53F0i*vOr6BXAR)XTp4ZcZ9Tm$cKkYI75kHGntd6B)7bn4H zZ7Z6rG6I_Oqu{>78T@MCNPbEJFxb%>JuDv5w15iKYqaGayF1eXzFAb7u#&#~ng%<J zE_0I2`@z2T3a8tl!?V8B%h}4Ayi0zw$n4I3F*WPrAUatMO3%azXMzl2v4c6`H1Cql z<zaMb!#z;(D<ck<m9Tk87i}@}NBVfB&=P+F$`qow35MDDR5T86)F;Bvf>%OEd=K%~ zH^i5U9g@&w5ArkT1&K9z3p3aT%pOrpp1jNyf)Zrdpt;)gWo|9CEfSl6RWjw&EZ5S$ zXfZxh^alAA8YrK41uu5}B-i8|(Ra}ikQ$xBysbw-_H_ajD8->oO(yC1mX2Gl^d~D# z(rL|lDcSceLFjE9j73UPF4q1f>F~bCDU0PXbIdWW?pX(UB#ZEGjV873R|cWWjJYWh z`sgdZGY%i|?Ly7xzH(f+3D#{!pjRj0{l;|i#`gx|s}4Fbekd;A6iIDgjOKm3+eqv8 zN}7J>G5U<}B?^yb;ml<==)Lm++|Ut;3jQ;N9=U7y)BGpY8sDYv*N@=4sSillRaZ#u z3)5ySTTQ3E3Z~8J7ioOeSt9m2{#j(>zl&4-X+~b_-WOJmo(g-SbGb_;H@R;Im0;An zSP_)Fi1Hm21kK_QVHtl*_<7op=4#I&e}kq`!|#8{rHNMb#k09&k-&2aryc0<#wnbz zyN$biwp@~J^<Ls!xLoQ#XPERs;R(_c)j{9=l!NIGE<%8&mRQIgR6-h*Ur6cl-6Z2; zi70A?z0I#CMR3cNBVTLvh1Dr<ILpZz+{H^Zbky1hg8%F$u2T0JS)TnzP*}1X&s(`s zmkCNl`K$(3B|P9(`kv%||MI3*O=YCv<tWl?MCd@Lk*L2~6GnXOqUPVMNL5`tH|TaR zH8`+-pzv>LDwn32Bei?CnPjd#jSpW`(g&SW$$Pyi^zq_EsvGx`cvs%y)}MJqstQhH z$(3Z=dcS`#c7r^O*BHR9^FBqq8dli_>e~w5OC%&)#_9hkf3@R)YyTfl_M_)gvCq;) zD7o#4I7pLk>?aa^DcT|2RdQ!8zfy%-PgRm*GM4Fj`qRM^+~~9gxuAM4SK@f)lc1M= zU#On1Cd8cAfONb~ZkoQOP0yAStD*_$E?*!>GCm4VqObfPwLh2!rk;HU?El99pC?<s z##&rGyPUt>VoIHd$5PkR)_7JogErcXhs1uTgd{nEw(htN-!|?b`$uPULvl1x<=Yt+ z*)x*we|jB^{-eSdYlIhnPrAS-U7yTy7AO?g40ng6@AUbOY7_RWcaFWLSq$%WGnNmw z*u<R==;H0xA7$Y~_b~NMMbtEJrkIVlnn`oxRPD86*5l|+G7xho0zTwub4jm#N#d>m zPIZ0_mpCnmi*<fV1~jOmckndo;hs+Hzt)h4nzyCBv);k#E0!!{=m?UcrOwAqZ^QMT zA^fq?3t>;oFM4eC9q=sogGx66w(hvj|Gt_GdqxM+jAhqA$yz*vM+0>}U*Kq%cqz<( z7)(ovGQa<j5<WV18?HXGrUpB;SWzE>e5&guxBgC}_b-Hzj?D`AX_yS27$ggI5-G78 z=xSFWdM@ax^rdn}Y~sBJ1(5kOZ(!%`RG1(>&9BG`=9k&1+P_^kh2A_U!?$i3#HYyC z@Jqzjf3Tb=@g`~#V%MulX{9Bvdh0w~J>~^BPQ{@90VC$J*M!XhRdT3%Heb>&6XP<i z`B`QAxq&NO;k4gQ>OV9b8=g+ZeJ|AE!e3d~zJ=%Ig;wrCqA^50HKc)wr{HFMF4(P# zz!d?(Xrr;04{=X~J8uSnIIz=?yYlud9a!;-Tej2>-Yw~YZ6l9DVf`#VW=krS(UNBu zY`?;I%N|miufxSG+6$IO$6(RnXSm<m3YAJGL10Z7Y@ewNmwz0m2gAm4ujd4U??xq9 zwm(98Y3CQB=Wb0lY*PlGt2#90nHeTt-U_iR%6O4Str*VB<gud6>BVxFMf}0%uFU^U z4DWXA5;Kgy0S~sU=AGmHFuIFkQ(Kudy)V@{@NE|Rz4;xV*L9t*YFk_!?>8NSt%KRF z*wHNgzwP#qD6){jp)4<14yQY5z~amDIP=~f@?>C<kkmXAl^^tw>KIdKR60f{-WH$Y zgsC&==W!iq>}$^#4?K!bmju}#n;FRV>{PTrKdJ%U%#!#Yi_F-qhWWg9wl<5FZAN34 z^Q`NnI;-^hhCPm6P`^%>>Hi28l9$hd&!^tPlWz^EV^aVQ<7=sT^G29+i?DK;*Ys4T z21Zz}fTC~>GRAGQa9(^w4b|SY6GfMBa$4RHSI-$m3!UP~*go#ei)Hx(N$1Jy$5zGN zCzb6hub1(0(sBH@oCIE9q{6<u)8);^CGbN7cj8Oep|r^Hr8Fn}I{B>s6+idiL6ru7 z1x=IX?EZ`qc<94m5_ao6Mi+O$TGiWFUsDJJ&1*TaL%s((oNgmA5+gEOu7E_0=n!mV zPgCtfg@RR<DH;a+pjSJ8l9hU;pn+v{-j3T?n0~6b@_Q{bT)M`Sb<uR4dz5f#pCd}; zfZwE*iWLv~v6zjCxS{$~U(a4IO{^`!P{(&<$0s#><-57h{mKRZ{0}tl%^5M!n|<nF zelYowsDSPrJT8}XkPGV{(%nV_;LUOa92m)==G7efd{q%GS8E}Y61%96mp17u+R-_C zBs<m>#H{Q4*>4HIh`L=}{4}pY{FU&&-mQRlcw6cVGZcIA^A?W3b30iyDfSbYzqABD zReyk|9%^iyNPJzo$FUIXH@~7j+uG5rtbjUo9!1%@a(ryA&GrqG&}VUu@VBOw?9H13 zp(n4><)e#;L;7gYQhiBpt<n)jZ{W$4&3fG66;JrK{(k)Q4cGax>;LkP-mYRnud{fA z`<nLs`13I6wmOq8Ji#}r`r5B>tY^KsvV4(vUpW>;^irEpP5jby4(HDCfITTqf^kC( zpTF0SJv@@iL(>>s*I`Z!(kApZc7EZ!M_!|;`~K2<YFnx5#6mi|sfBy<%n(vUZZswN zm}qg^52^3d(fF(0gpCi>=g)l#V25pA7q6GAgX6XbDJ|N?GN&Bn2gRB&al@iCn0K$6 zojR*Yik)iFFy4g^vGf6*V}}_xMuy$hNWjW*TiHU_sjQ)*9d3K|dFqQ2*^~4X_G9nh z;%)afanB#@q_#3VHp)Dwi*>%xeVaru$;=S<b(ir~Mb6;yCPAuXI+FB``_4t#Dl*aU z-5?owhVN0oD~2_@N3*Bq*ML)8i`O5VgrGeEs4JI@U#6=;n<NSkk7>uY>5X82=nVDI z%Ypy?>=!hhHo&3h(RgQ2EMD4n3wwVUL)#kyOV3*JOX?od$x-ISb8H+@vGc+$2UfwH zEtRCjUmiPtkD*IeKO_6o=7U}T`$Ebxu>t$+tHztg%EAuMA@;v|iun)bvS<-=7;mlG z!ZUMoxUn*d&d!ZyK~({)X^l3^x_<&wPf;?kS&iAd+$9IpOj)9N8>-9afZym0d^mVL zWFOsv1z*Zg!7~Mw{b!=wzyRXtdx!H@Hzi}!-xBv|Q!K4LOP7dmY$4Z*hv4Hw{jh4+ zD@w|YapbvO{N=r2SeUoSUMnes9W1b8PY#Q4>b{3Cpz$P*J=zGlzmy<6ZxS<>^x=TY zNqCmoiH~%Z*zz;gP+>C%Mmip6ANQBw)7Db@IrAuXF1ZYozR#sO{wHD1MnV_8dLY~s z|0Lqp-QIM<oh@K%y_R}*kHr&_>O^jUD|P;F3hWzwotVDz!F7jI0o>zx?!$lfT<386 z!Bbyx>i094{qImXyeJBCij`S2cZSSgdx^x_)sn98Rpi~*MSZ<n2l0j1CfIsQ57S>* zz^7V2h`ag@suxxXYdf2T^<&0~;ivm&^ya*Xpf`i2(0`)nS;<1$!W`jS;Cb2*a*|9s zYYK_0o`{a9U!;Z?5{Ue#+ji@}ZsR8v`9NE1HIq9bQ@pq?m*p)qVE3)O_ylu5*qcAC z@Aod`6>dyslQtYjbHihhbm$e`S&)I20okB$dr~ObxQe}v5MRWDeaWJfdmkx%ugd4p zcyzY@k5uKw2=k}JlS|wnTy;qulJCXSz#X1;8$Z|4#q+G-*BD#snR<~Xmnfm`JV*Yf z$d{R~{Kj=o@n;QVkJ*!s;V9Et%0K+IffaC4A>(xy#%KNJvv#=IU%0rPS&G-wg!(L+ zCAP3+^ZZWQ>kDOkK+=1bP@jsf3bzohJ%jSRzwlc)$FzO>LB#Tp<bz@XanRO5Z4E2V zBKaJh-ZGi|*kTNd!=p&fM<s4xQy}eLW(0#?gtL(DXn5Bcgu^u=i{HQZ#nPdP{Nbj* zc(}NnUwHEYm*{;AbUhz1vFw``w#+Dj4BnQ6E@ewN``>0PWqvU{3H}HD|K4CfpJ=h= zYyxr7(uNb$|AYTW*O~uQ6}5lgkj#l>h)khWB1$-Wt#za*No7d$q*NNwpz)X`X)uOD zi4;YV%Gv8$8YEGqXh3O_R7jI(c+P!4zudok|AX^7d+)Wc{Tbd@Qi>Y%?t9JI=vl+C zS;|=X>K|IVy9#+Uo5Re(GIaHRAN1niYUmJp!XOh3h*|WDN?og>78fJgce6<5H9C?s z%UB3A=UZ1anU#p3)^s}acAm@Ha<!SqLLIiKIg^xxHd6KXS+M+xHdK5tCk=Mb;kN!* z$yBqOd?@U}uN0QiR@Fk(Z=s6W&U09Q^H!4n#}urzuOg>IIyj+b3Z{&q3!C>*uly<4 zCqRyA9v9QoE*<=aQ*L-~&<xf)Ya|Wo6=4*;pdBDyUT7UNoI3r@CGWFO!3nqX=#05J z$kyBik*5ahJXH(jt@j}P$}w@<>$Q0F?pnMt*9?~jorhEU&3w_zOnl(-Ihx_K9~-Sc z%KLJu^nGG2(h2>5=AWC5!>><39ZH>OYTcjmwqu=CD_aVt6f;yTvX(+qp2nkNmrL+g z%Y$$)Z6?XSf0vvt@?z3%BboFb9~R^0Myfr3<H{dru>~oBjd8K~@?le=SZG2PpeIP7 zJd!)3<ABK4zr4!0)ez;Z#$+yE$H7tyz)F7sy<JlRMupwD%3};LjWRUozB|oHn2sKw zGyu_*-74U#GYk7@E`T#rb)d!84l3U&Aa8>WB&h5c8S&>17~YW=K0JJb9?eQ;FC!k{ zkwqq?&g(22Z*vohW+)36s;01pTi5UowTa~8x-M8_;mk|NRKY>%H~8V5E!e4nCkb*( znDWqb<f>yFq+Ao@+#d(X{d0aI<nFN%Nxl6@btKL4^A7g(+DjQ&P~wSPls(Ajol{_q z><skmmnrUQxC9zWpK)F8d02YnPQ|Gw&BDS%rVRPJfjG7sq}`r_)V_xhQQJq}EVARW z&7a^i?WI`f%O%KtFUj*MuOq^qqcG^^S!S__BleOyOHH1rfCg@jB^Q3Ig01T`$=;t9 z__DhZ8gfaKHhPuuksH?_@y=m5cl8B+PC^;|mAoAHSL)&qr@iQdualT}{41m}OOLGV z=w()~zmk7NN^IBn0QRaZ1!^;8;cNd&ZfSrM8#-12YF=5BA!U|W*z^va7`uw(>g-@% zqM4)N_Rn@a^;sB}TRo2b=u>1GKhvOp`~Z~s<qMp;It2grO3eIJHTpT|GRiK=Kua{f zptrBvY2-Fvrsi}3#e`p^uLggjty4`<e(-C2?5z?Dvr7^x-b-Ljg;U94iJ@#<--`82 z>Y;2<7;ZZ^hTRmu#@UB8m}q-{3CKRaj7P62hxPf+bg^AEn%4XVs=n&e!8UIpwQe%4 za?ppJ%MYPT&WcoY6@lr7gJ^P>SbXr14pOeTK%cthQ&X1^JiAYdzL5k(eJ0O<c*B3F zMRPr#Fyj+`Xi8b%l3KFwycd2ueiw^zxDIyDUZCYYjw1Hlc`ux+=|DrjYBP<QPbC3> zY`pV;0bY^5484ok!h*S5`1J-GtmrFC7TK?2amrcveUb-2hNRk>Ga1&Y7ttTOrpWz{ zCy4Ze=tqfDJ)~&}jyQc0IotGL`6Y+x{j$r{(Rc`G2CSvKv#YS;9A9F8YMk)f2GKhH zSN%=amp_hpTgsBBU7xV^j(XH*slif+20W1~hK_4h;8?ATaZVXkZMq8W>-5>HXE`W3 zIuDwj42E~rQcT7q0(MV61{=Z_la95~f&>?H+_=0A**}rQ>U_4N`O8OQS&uluS9L?y zBMAmf@tTQ$u1%Mm|7Nq$lMOFe;;a$)?Mh*VXW=B_8ZQNKz2QfizRQztH6a_3s{^M4 zEa9w%oltkXJXva3jWhO)g`)UVuupX?(H-7^ZN+NHOl(DJi;VF7Y8mG7G#aU-$wHUf zUu<n(Nf-8<M@uI<<GIfB(DBz&uzm9saQkvjEYhEEj;$Q>>G>;JXi#!8CvO{$t(FOd z^JA;v-Ki3G?HCdMJhcV(j8?$;`Xfonv{H5|`xqS>(MxnSs@T!HKq^h%;0>$G@v76Y zti$O$ez)rmj=s5^IZ5CUU5VyQea0AkSx}EANY5g6^)Knb_bwFHcZ^!^s}OM-Ip!!~ z?^N_`*c>XUWMKvQdVa-?IpC1u4hxS;zTcFr@e6kYGGRg%=J}|K$dC3F?>rUBSmS%R z?%b;DE)(C;{c(k`m^`8h#*B@cqr|?xTMbnIF)aEr4s328#O&8;JgKJvrsqh=6&HEx z8mkTGC#K@>HbeFJ|Cpej|2}^EA5T^(*iMxBuo0mV{#0c58SP$l1`h3>fU9O2L22PD z8o5OmCY{}lZ2q;=z{*JOi{V6mPQ^Y-ihbaLI2Ik>Cx;KVno)-*>rnfqY$QGZ8uduY zr*2EV@dBn#l{MyK&8cnl%BL=Va>^m9JW~o154NCTS=Ij)Zu)eX^vlGGV4<j8#Z9a* zi3$FB4;H^ZU3XnP*Fdb_QYaq%)5Jmgi5-`5LzjC$;fvr>$^t>MR+`}DdK><)fuiH# z<^KfrZt<MkiEQ!PPst7$?}dViX<?jdpOHAZ=Yr#m6%!pDC;5q!y=RCMx!&@QCrRQ# zDL2IhGol?=y5@?1`+N}}`{5+wOjkt-q^(Dad&3WkQw9!;8>^p)6Vjx(buq<)Q89kp z&-o)cI`N@kWbqt+`>-1VpCjYB6Y58~Rez-Tw)*V?W6dLiZP(&By9agRe7QEkx1V}~ zq3S2aX~o0&#Y!XiiB;=(&A;mW$Hvfdm;GkEa?w^UaB%@$+}416b43Q|k=u6u;-GlR zrSL{NWswqpCIYq<UZGd>4f(GN&EV&^`^aAgBkk-;s%5not~ws)J~t%sZ8NkXW7loS zGY*2ReiQoc!AY*JM<AG*oWkkdc+C3?A9H9}D6YOei`qLZrHe;4(y%AR5QdH*hnRJ6 ztSK6$iOeE#!rmdwx2ps`@2}<z{i2Za;Q;)3Q3LqN*rU2YPds3)N3zGskgJW~5s4g& z@5tv$NJG2m2}$9=>6Z>OR-S|xd2hk$TodTItib=yKBle)BOzhUOHTKa4!i5yN9`Be zvFY;vc*n8~n0BrJ2`{W*S8hjuNYNx1Zk~A$9>cyPtGsAD$#gGw?mUL6pWncl-Ko&E zF%4@(>){qpSD0VWjy3Z-(99=$VDL;mXq>u$i%>L%R3lZkr#J^&n=a*rd?$VSxF32P z6v@sK1!fVy1@CtrPQvYL#n1MigwRkWvaPR?Gd*YzchZK5$ndQr%SS6qfvVOy80T3I zKP*&1&;B<2>a2rXpEu!%ka4Wx?IT=se`b01$bHn)J_2v(HYNW?4r0L_X7tf!0oKS& z1##VPu0OjMZ%9^PVwG>4-pp~Z>90Dxd!h-ivTvZS5fjM@A8CB)XBy|^--Wh3Q3IDT zqCDy!`~&vy_=A>iJBEi!KgVzUYmmBr6Y3qPMN5CKfvE7$w8c^b{Qmi~>_K1APK%%T z;;F|_e0nh+|KtTcIDU#gPMnU<xLLE$LNjdjCk$<Bj)ZHIG~q(KGoJpb1G(%QPh@5) zkX!#0V3zwnI<fB-y*BSX6^JJK!JMMY;E%q8>GM=rVgC!wc&Z9tOYgwb1UG>PUrwVm zw3*>Db(EWP3dcprv23jdWZ!DVGC#@#p6&wQ*Uyr4B&)#Ea~gCco~O&LMzZy~8t}vO zrKC&MMX&B`z+YX@;8*jlK|l2#4wV5uRwbMU-aCs#ryVVr)r1Cof1VjjH3}eiJxb6v z-=nbJ%z#eF495<(H@VbV!C-sW9{0}5L0{B9(PK7NaPiJ@GQUVbW<7R=y|wee%j7v! zM(&5q8HdrU;i_0Gr5Y)GIfWX_j^XE1+hEa{Mu-jE0pc}daN83vRPUI^w+<O2!Y4PI z;@hPsso8}Out@JVDk^p4&1B!hpeK6V>c0hed&UX4)B6o?b;0<c)LGh_sDdTl6)YGt z2v1@bkX&wtm;E|Q^|FI-rq>^?#eW#yu2hc6)6L+w?G1_^{~kRvw}yoABjITsgDCsc zbVU0I*!MFJ4eb@Z<$F)ph?S-CX{DwV6yzGQ{r7vg_(QVzOj9%DWoDr-E0vjNp$y!Q zI|`L|g_x=yA$ynds3X}0{5J}rd2a#n*qcpFo5V0DZyHUoIEugTw`UK!j^bIDCXmdg z4E)+K8;DjJ$6L&0EhFcVB^NVM+`<v0N2Grh#J4EbEFBHC=Z>+<UiBCk)}hqM0_^tn zBa-Uw#utW~Ft36x=y|e?zFv5r9vcSKG5ZDgL8*c6y0i-1H&4Y^4_-!A=G*z$&#Byp zY2)eHh0>_=V?3T46@eo;d;0a0G98(zPWP*uz<B8$)Y&5tc{U%VqD>3)`DNFxBb|t| zoQCZ;WOL7oHxYcHSCZw}%j<ur5`Phy%zT9szQ)i~+OBwM+ErNhw1%U-r}@Op@95{F zpWKGAO8m^CaN3llg$Dw9@Rf-p@M~v#AnWX@*hUxnMe&@;Qd2yIyhIK8l9?hW43>Le zM5D_*MQD!37L+oDqhX|ns)x3rg28I|@W)ov@s$Y1O7hyfE_%|hnoh7-ZzRzkA&)Gv z4}_jy4Htet1-FaRxZAiLkrAmNxR}7c{gB2hd&F2SZzP=dJ`PMPmiJ$ki@cAUp?$%- zAkJVol2Y12=ZwEWdmj*L#YC%dY<VeEj$VMXX68XModtDql44ry8%O7zY4m-w5%;|C zIcK^&QyhLRk87#Af%0DLK|`V?^{9b+yrzb};Jm`!>*u;!xb$x!aLa2x&6t&kk{2rS zeFYmFm9^dZ&u8?h<)+I#mwkZuZlBBPsy1`68ejS5-)eA26krauJM_2*Z?6gJPaH(E zXFU?DJU6HR8U?6rT`@mrMX}h-=nVhChHx4$D!GGZ-F(epeYD1SA{};W7yqs119!#j zI?{5`#W~+J;pQ79F37!tw^U0(@ZlR@xnu&q?_(|A{pKw9aA!7OR~f*sRVXX>%Zo#e z8akXvD7TmIRJo2GOFwd)`ZXJ;yDtTgwqj0WR~XkH>A@TSQi6H9BlyRPSH#(K<Q%sf z?n5tQ(>eM_5uUc(pggGo?=Potlh;0eS7<+HG^(Bsek9KwSvD2+-=YpK8?~t*-hc+% z$|J8Lb1Y{%z@3b^FK`O4=R$g|MFQ=5eg4YQ+f-}o2_C-i+?d=m;t}d!c&QT~xVETU zXm4K!O@5*%p72$J8|@Jxc<mwN9vvD7v$k!7^TX$2`Th6BX44jOV@nnASNvHVcG8X; zD(5d4iuZE08UnO&{TZ>1SReKzckwnyd$|GSbAqgxL~7`&#2Xuk?x4(?AE-HNELUb{ z!!LT2!W$Hh;I^sWL_hlzQL@%XDoTwO*DfLaWK9#KIAbO<l5h@=z6hi3drUdE1U0T+ zcL|a|T0*r?Zl`w7<_lha+%EnhE}>m-R-@W8L$S4TBGQqO<bG+LpzZAu-Y@wy*L|rL zZP|4m9!NgVsJc=SH`VyT^;6@V>8<DID1ZKLd2{bzTrxzH-koI4btf3XjK8V$ug6Xp zp>hwcDyrZcMZ;)os5{tpm!gruCOGe`8%_E1Pkd}_7)su9jVg|xM57nG)2APtQ21XH zv5|fzE;uX2NqsKhpWf4@Th<@r<ldEE6Dh52D6i-(MW<KvAkB66QMl7ED*Z8yS24>f z|7x(A=APLF%wjY(I3$JdWn6LeThq^3r7fb*Iw*hh`efQrJDJz!wsLAFBF-_|jB9?g zj?T-?rw7xna#zo|^K-A|Ba@N=PTcsN(+^0ZO_NQi+4U-3HDd}z=Y2(ZP;?KnFkZ_& z9cIUC#0JvtV=n~sd$jn=0U<B-paNajKTPunr*bxD!l^$$6op+EaT^wVM{6Qf#VT69 zf`jW*P(h;&PgIX{t*2ektL~5Dsj}-i!@FVFDS9-$F>DE@5`0PP%Ax4MpmZev#sPir z1sE-{;4L<<{)7&kXyw+IjzJ2SO8J(g3c9U0vV7_0QD7JtkD}~S`SWsBNJ=DxlJrb^ zdBZb$|HvFc!lOE|y6j?5Ug(WXN4elaS#>m{F^ON8t48N%Xu^L+{zkc~SO4S528ia1 zj29O2*C##XKSY@^;~lB6LCpdx=d^L26ADmNmnsx&e8aEueT<fl9|CtjuZ5z<2zvRV zCT=`5k4D#w;d*2jaI5;%>C2;eT*#z9+=xACbW7Aq>e@e?MvPzm-_bs@sodiK_GDEP z-9smdjQ7tVJx&2^q>VFqylyv(OweNOm$nhD^=>5jY8Wdsv0;z)9%mhW$4G3>cGhRG zo+)lvi(5lBFkPiOtmNWJt}kfEwuc0)cGqC$r7lBW4K#wd=sR?s8AEh;o3e799b~0U zKXfK-C*y_@7NKGZql{+|vz6u|vMnY7>-0(VIIKtt8wmS(W)umT>%=5s5#n%SGEv}* zut8-$e$uYOEOvKc->?h}dkk1=jWz3wFe1p(klYw=$v&vQ!&`@pCUQlKNyFsHY>-MC zJn-Fthkv5L`?ccIwO-_AnjZTUBFn6Oi_nH}ZI)9aA%b?l8o+s7U%@>gg1wN8r#Zzl z*rARIB+=;$d^fp^<t|$iLGEXWYud(U_Zt!UE3J6^sy*aKZ7q#e<Uv~|gk1@AmB2VR zu{*1;VDq7o%;EcF^3ilEvzR-NUExY_$S7&H<U&1+mOa2Mo?Ej8tE^b7mo1(latvqL z{z@cw<S?cff|-<y8;P9a#g=ABlK(Qs<bq`|QI^qWQhCGh?xh=uabY1jEi(yPCi)W5 z#>=?wY6<bsa3*D0uH-_WGU<@0jGrVZvXI&8WT$o%o3JsQ%+0?7Mj?7+uhs#wX5}C< z#A*urns3i8XY3*({M-Po+Mv$L^h}wnT?}^ITmp+9+Oez62brMvAc--HVWS+v&^PyJ zcIoO0=I?SBdOe-Vj~mmGa6ti<Q+9)yf`7Op*oPEwCZwfTm7VUY#+N5AWWQF;B%b+t zux-}^Y^ZENl76SdkMl?IiNrdzKYtKRc|OmJ9?V?~g93Ztk%A@(s~-)Yt6tNIGHPhW z&0aLQuoL|}o=gASs{@77BY4!8uMjitKD0)j#4d*;xYRE{X{7eq|K=Vx`Tc6GkdczA zkoo^z^oaQ)6BBJ1eD)BV^01D7*HVmDU!Dd1hf{f-{#m%s)P!1}Hi3CU2~Klb3Z<Y0 zO*y(OMcI>vRkTCb+~LB%U71k4V-x#3#hRoAv<i@s78$0bgI%ZDkk-yGIQM}f+qd8r zzU7oe4gPLs7jt`INW*9nKCKIf&$bpZ{#_!hN_S(M-Hma{-%E7-NOj!db`7dNXv2-1 zLdo&$Nc{)t!99{jRTCzoV(}PBQ|KLDe`*o!4KYWdfk#;5{V;O*V=VES?#*Vqf1{x_ zNAS~WhuG1Ln)s2%Ub1v!DE{5>fK9P2WDSR!;Ca&}_G;}K(tA_nMMl|7mQ*4OfLk3x zHeY_iuj?@-d)!sv@kS0CIjS)goic9xh8KcEc~(^W;yJpka2C#yQJ~^oU8p|q4N8%? z>hbNDc*`X>Ij<`daD-+t9GUOJrjEM{a#zjRUQUmAaHE6?UIG|A=`a1dZ#dc4A&WZ_ zhOs=6O99zDd<{+cx)#fqnxL=u=R>8d1G_G}8#34A;nBHO)M(==?2@2PmP=JYYTE)< zS<Gn14MkSlI34>P9gAJ$JfSpjF_yg-jXFstx{3`zck)5WTj36Rd#eV?UyC6X8P>$9 z@F)`sDoE|rVxiNx9jy2_5K*FUC%pHtWE1=*;P+=UiJzAob$lu<%-OaZ7mo`-vBR=q zta=&gT=WB8B!`kGvMu1#FDa}n4<{>9evsQ9j3mE#fo!YZVsXF>cqPA=cB+4)8VUMP zawG|TS~eDsX)}R;6)WMKW&v71_X!;<Z6(1-Oe4vwu8a6fBj;EA{4tkY+I|VAtV+VK zs^oC)D;^do55TSJ{UoDRvBLW9JoY2Vg2>HvBl8!y;SbMMao+A6lp;}Y79N>N0s;<W zy(<^czTO-Bb#E>7>icgr_SAMJqt$`t-ioBV8}HMiV|lnTX&2IMp96o}!cfZzPZqmV zqBq;HZ#25d8IB}Aq0@%z^UF9NSTZ&ZemuI)7I(#vE4Rh4EayG02>egUtO);V-$3+F z=|hHXEA*pEXi76hH+2}=>e5ILzDR>LI}DjyQ#enMJS&i-;x4#EU@5cvNP5LK_I9F> zu1cRmzxp`g<u*HMm&j&1@}F9POrKt%>Erz2d7mA2_Ku_19A@Ew^Lv<ly&}1@f0uB< z)<z<=`vpGtJB)GqZLFhGon1ZIiFyOQh`~cK*|ul5@Zqk}IHC-bjTu>lU#liO75f0z zZ7w0J<qzTyu?u*QcPoX7t1N{xO_YQlUHO!6?_r|Af;1$tX{h@b4ea*U6&&)MAiSdw zIUXoQdsaUrtA^jBr)LIn3-(3e&s|!$)XV@EsO*M8Iv>TO)?Fe2<Llw|+T-~6KN*tT zB2RoarbFtOt-PCi2)bZ06;=hS!2N=9w2#P(MP9R5vCSZ&K1KyUc;HW`uGA6X2{9IA zL-b4d6fqP}(HV-DwSJ-<->T?*dR1_KQw|;exCIS(#~@#eaabj`8XZxXO5+08(w>K# zAh0G9Jf2UJIJJ+NjMWP6{jpBALdKq)vn{2mbRyR*H;>(~UJlnSmJ)}T(xiR;UZi>6 z3aiO=Q`0~D=#NCve57ODO4-R8?(eiRR3|3|8@J}eG1UUdul7K~eq|O`=Y#JCm2l?$ zmJ*V21lm>I#|`R=N0&6tqS^J=xaPO>xgGmuv6+24+19y<eLwJ;ESRoY@k3s%B4eoy z`(ts6^c=Ef8HtU!u75ZS(fTc6ixv~X83$}DS~VW<m$mHsi4SmSS{2+ZyN`#oo?#u9 zBT2=sr(DnYXx6=c3ESjz2{Z-&_-m+;mDmAo-I>R&+|<ucUVoVWGm<Ept;%4!?G>+} zqyXNsN$C0OS^@WdGg+ze3YJ7#VUN%AgvQr~67onEx!gKMymim8?D5}_DAG+1&%d)$ z;+W_QpTu<I(#mT%tSXYuw#XoD-#l1k?g4N;t4DM%tFU7NIW+CEhH&ckRLGqP@c3Lg zbt#A-W`PT-rZ5144dt=^=?LWSS0I_D*CUS^Uy#gS3VAIr=(0P#=uTQX)e6+ExSp?q z^^r0eU7Ra|N3Tbd1QiqT3?9x}9yk&oS&8hjaWEtG^Mo(GT5$d>1<-PM138)pAV}5> zJWF@u$!@Rko=Y+y^moFCcF)B1c5-;5Bz3i%nZl?gn$W*$0yKY<fy}Zr^x%;yI<(#Z zm%8-PZL>Y7(`*y!X;;c=-aaXCOBT(=V-`o#tfXOV?xkFz)!N~tdW%Md%uvky*155^ zt5d<vd?HI%-3;E^TCm7Y34e8R2k$Wf(1&hwtrf=fwSPC)6QslfPV12JLp|J$#T()G zmSVVAUr&8YZlLMQhe73rG%l|%4waYBhZj;)=<1RS;!$OFl0o@R30mWN0BgFZ(X2FU zx_qfD{@&3H>pi~UVyq7Xt3DG>axXlzst4~IA|j2-py2dnyfe;+c+EMFKMCB~i$#j0 zWYBPyXl4Sg@!Ie^MT6jn`=)rMn8T*u1$gk!3|!Z+03O-z!NZM5(H5&L_IGAG8ZkXb zAlefyh58D79fOXSp#p_CbXKaBe!O@DT69|JdNYjHrh1TS*-$pozYO|+byt*qaIBc9 zxQEM<3IeOpBajA?)J1F&or!aCtM4#ocx@1C&GaOE{Z97ruqxZOiNn(urjnCOUGQ}O z`=A>Tjk{(j5$p0L>`Br?5wcq`8A_H{vG?H%v1|Ade$aCxC{WdfibI`9;ixMXKTD(` zTpU%o9>@PGw!;HUCy@9bvZQUR7E}6t2H!065b7@x5XH)Na8bU^PEOS$P2zmy_vSm9 z^f(hc)(l}*-@lOyk2aEEnLW6~O@=55#z63<Ao9pt6vb$bG)xIrAiod2B59UdFrhyZ zjlZys;e07PC~^qSz2U^2d)ZGTkF2H9XPc0&?`S^N$cDK(WusqV@2+R89tLfGT|~~+ zmL#7&P!Vx+ZN>X#r&z=ES8%9r8~(ZN64?>?mUCKii4`?%XIJ8uvuCTuu*<5d<gX|& zijJ!t%W^IzvcY%8l8cf-L_g1rg-_RKm(I3>)X%3-J>@l0__~Dp*of(QO?$k-DT>=; zWQNX;-h^zu?x0C}y^@nGSYlpBp!|6^#1p4P;LwV<c&x2ESu)e1BKxtO(62m=+?K$o zW0v^f(!4~JYWNH7(4Ip?Z5@YM%k31n-Fg^rYHNmuJE33{D@|0T_T#o|I#~F-8OB`M z4V9a+VC+L@B=kwb&m~cM<B|6`;nd-<8a;BHmc12yiyud~$>fUHhRY(i2U5s?&1NQ? zWQy+9-J{xCL3q?k17@7>kF<`45yN=D3e%SizL?91*s{CBnEBoW_Uwi%d(~Np10HV$ z+2#o_zS)7;n^(f@M<2oaqGaHhx{WlhmL>N;*ud-cDcHrmi0)HffCb-ONs`_HzG6{7 z=9i6wK=~+f))#NF<K1#Ppeul`^uf5Q*%0M6xuB7?hG4!z6Ui9Y!OMvi_+6#QmONj! zUf8U4iS*XL<hMGUCpT&r5W}tc<P$pp*L@F>case(8ux1n1LY4O;g424U^R!>N<t0! zYge%BCxh|2bS>sB%txu;29tCD!i6RK520JjPt%w{bz-~p1A5Tnj%Fhbnh^Mg{!?-n z*Zgfp*IX{qz|l4ELuBs2=Z?LMKAcH|wxjQePuwEt_P<0n-pnN{8`a79S3#hqa~Hl8 zdok_8(R|U1IvBi8x#HiLVPx5DZEP)VOKew7!<v7lqNRUan4O#_n>|F#&u&P8(4B)x zpYRd>v)`R0U$Uc3_r}7HWm)uM&<*-1Yb3ru_5~F^@oJ($e=nltdk6FN3a8M=b3>`` zG6OvQ$0GLi<qvY8X98)<up-^{ufX1WK6?2|3xE1zD0yE*5h9ZWqn-3BCVAzsQ~zQ> zz=yC$Yd;d>RgcMmuH(oh;5b`rXh?pOJD?_W7kimk;D!$aSSTL=x?+2<=`9xFWii|F zfux71I5~w6HBX~OzP&U*C!Qbkb~4m@oJW~6^3m&NS=9cx8Sj$5jxQaY1k(?N3ZER! zW^P4;gay9&gk_$`GAE+3{_lP~^IRyNe_mSh%Du#!SH{4Lf|q!TYBh?&Vf3<{7d$=l z3e48*#Y27np_tnu3m7_M8a~<^i@pybsPcp|+;aPh^eir**n6*dRL0Q1CBM0ZGw~=m zSduXo56~~p_xb&{kI>FbQTX<z+2l%!CrjGGRZN`j2=^27!1>5dGOxXZyi82T&EGD; z=v9bhhg7n3>t*brfhybj`yz^zDEKLH1VTk-7op$(D7Jr5fgko?fw(n=Of|%lfc9Wi z^2`!%`d9{m>)QCJ_!cxM<TX0><r60>A<x;omjhGBb=+geEIPGoBAs*Go@@CU28KbO zaCMOzIrAV>Xt6<s-E1>sEmw`%;V?CFI5rB++FXsy^0kT7*=MLK)QgG~KV3ks7tg_p zm05zC^O1PRuwE{9`C$A~LlHm!R6#|LdHlIG8%6{N(GyoSV6#^p^|;rFIyLvv-IDsv z;L2ZO>zH5^D&auCJY|JrTO=THw~eUkggiHA;X>AW&K(<1&cJIf46nE}eFf1yC@G=) zpN77{;VkuPgb4e@x8M*#C%6U$g237yUm2ywmUn5A@t^zglW(i>#=j4sV$NyU_o@J$ zuipX_SBK$6J?(h!xL@d7e<kPt_W&NKjNneVuIJ9AA>8<<i`qXK0ZmvI9>tb(kCMjX z#L#-=zU&4U*l5p&h0UWM@809;CTa>dMvC6E13t@H)Q-)B?^a;Pzt-a6dPCWk7mn;h zu>uL%YKCn;X2O$A_GFq@0lFqkrIVZv)0)gGJ}tzaT~UgMcm9{)Q<F6@dyqv}r|IHV zUSIj_x0Y~5b0O__l7%>PCJs37La#|$CUY($c=ELnX%`*j^14Tfb49`&+&*J79^Ilq z!h1DYL6QvnsB)W$Q_m72fezRwc#{tj<?KjHIx$t4g!ad6VM-rX5R&u(GkafH7WNb5 zryWKU3eMBp5A|5Pt2=fYJ%m^t`-Bd<s5A6uCO+ocin~&7(<K&<xa5PS$aAv<c4eDF z`Lh^{=J(rCxRN0OI&QzdRiKLn8WK)!>InA2I+wWW2`lFQ9?ohX{A9)}GEk3g3(n~+ zC8s~Sfnb*;iRltco*Vf~9w!SlJ*g8Mx1592v~^I@*$8hZAA*w_iIBQGjo$u#kTzNZ zzMV9R9hYSKj_c{L4FdzT#t4fb^msB_wD2s-TY5!;$WEqPf9O-&Kf{o0w-(jfaUZ2F zssQJwK@3;UgGEDaDsJy7V5VvX1Pw}I@^Q5=X#0JXk*iJH;~mgdlNfR>OrLx+o(h@z zb6AveJk)KPj{~E$af!PtJ=Cd!4_&=ZH^2FXJPt6b7CzJ({YQK(KU%o-fAR+RlI%p~ zvzDX0F*V5F{3Sm&A`#a=>gSp~=ipO~+i}>>8n~3Djtjkl@bSw}(Bj9Ac!@ZUKX_3J zzxy}{pLb?7q(KSiUTfo|Z;ZrquLM!JZwV*EFVMC>8n|!6IJ!|r32l$y@cSq!WM`nn z^{nxMm`&RM6(x=1b)-Kfh9(O|&gX~HIa3-0g1AiVE2oHiEmlYXWBvU5AL}Jl`(H3G z^d)5fd`&t3547>dfWYTvD73yz$3rdZz<%{fJo^4g`qBkM(%@bQxHO)V`(TB08h7Fc zL6g|>tQRz4wq!quawwL1o=3;^g}?^Ozf^F}p1f9Ef#z196k%`P6t?b{KtmFL<JNFz zxGh+SbWfeYH*FRY=Rwvu_|O!5RP6)a^EU^ZR{Vp;=CgQJ*>+g+V+(9sJrsW*5{E~Z zf8%>o{jpYR8H|)JVndGEVCjdA5Y;0^)IKf34Hc7dgNqT?trke?-D>1s$Vhhn=tr2n zK(qz7oK_^Q&rjhM5@gc!2~{wr?hJKSFbDf<zcBI_(4Xx;9XDuIAid|g_(r8YVbKyU zv+X@RbhQq9@_9JKSOw#$g9gKe;p$M8n@pb7oswJ!U1)GqVBFw$g0Ie-;q|FuboKXi z$3GDt1s^_b5GT416KriQa=anZUQ^y-w#u<Czvy~~i7FR<!9o!K@JIR7W+E8=T7~y% z>l9c@8S-JrY&b)O{&N3Y4tz~Yk-+r)1@YALy9J{kx0Fi@X9zaFvU0Srm(;bE>WMeE zAH6PfW29idqO5pgf1W_>(OJIX`+PnbS_P8=yTsKqKa`*RBXG?6sU{Xxl#edomJq>L zp7<j^{3c1zI4fKHK5851{346r6qU$r<8t{ULytM8m-Ae;b+<Sx{GVXPH+6p5eNE2Y z_L11|p^Nx#M5p*%0}<HDMLPyPRpmQk{s^|o^oq5R9_Au$p{(l<kd(`0?qPHm9%C^X zbvT6y%mnU&L)wEyX!*NXTDDvb%g`P=JKL0Aa@)(_-dKl1-M4d>-8eMn+&DUCy(|B! z*;oAI>Jh>5jfyb+nlzeWPiWTOpPXM^2)+Hr8sA~JfWMQ2uU5#zI<IcJQ!ySZutOm2 zPk5KZb77dCZ@HW>9p3od1GkZ}D7)bq7^RCMv8CHyv=UWG#LxpsuR9oOo_C@*7Y_5B z+g!A3NiJOfd=aggLGaBb-Kgqi1?+c{0(O+=mk*Kx6SD*ecHK%ReVUHT_6f+Fts^0* z?J56ar62lXa2Ngs4IsSUftK+($Zy<Xy!z8TbmIAT{_KmbxawsL&Jb<sfGMR@!CHA2 z9-27_e0{Tc+s#4v_x^YqE4Q2T8gPL5hwI_5>v-6b<t6yI?gLaVD#s~nJo#ytW}<i7 z4}wL?4L)0E2-}<1gbRvVsdeyra(C8izAr%+@`dRPqyxxIG!gvQEn&ychq3)%DtV1< zC$Ued6cxFCb|Ndg_p`}wbjdxDEf@c|69*+25>=)~ZjW>#qqQ#5UHflBF@8wrIY*)g zXCHyV_$qiBngyL}hEUVd*>u*~PGmbihTclE#uozS^DKBR-TM7F%q;u>J=q;-jYtl1 z<d4yb9|xoKi$Yv2;R8LF@B=&78j5goMLyjZFarMco`$BZ8)%{gZBbpb7Dt~v$*M(G z5EwHF2m4LJL4Unz*2aJI$Fx$KA>oC{fBMa-znVbb8h7JD{o^29xB#i`Hh_Z?Zj4Fp zJe(uxklRWG`xg0`5+`;A{%AFUZF9Rx?`s+3j)G3woqCIIHV~!ZwOb^0OA}?N_V|fZ z4a4AHxh8#VJsNW^Q`xEl2NISz560a&Lrp%n@k269@d}kJF8%#zW~>{AicM8nd&3kq zaCr?DmK?)74x8ZCUw|}@14yMGAX_Bp^hReM8`CD3^6{I%a902<&I@Lp#gZ*6zYgQW zB1z+-+0qM}+;;~2Q;2KV#L>X4Mzm$<Q{*yPnY>$D02h^x!me*0aGdo~@$d>GZk?n` z?<n%%_lJ$5SudYB{P|oXxYV|Qa~>oK&UZA6tv+b;>sIaJjNc6c=Mg`+=J6Z2gx{|G z&YomW@zE=~UcR_o`COOy-M%lp=*XtM{NKZ+bk@A}++?#U(7&%8B^plzvl&UqxN|8& zW#QbKU-^9E;sU<k_6)zp+zKt}y~g>D@8!PujOQ)>P83hrSxE1kI7i=IHQ*lbgXtCh zO0*;WHy^TVBdRM!eC?NNPVMwj&b&C1CU>vrV?q_gkFxvtfg^)NG<KH)S}<)T6vY4K z=VeW!JKANqXKO7{?zt1(qiwa0ih4VdqK^cl;gC!1(wFks@-IIyu7q1N>KykSCDUvd zEnJi|kvDr5fx0fFgVA?$$zqcvD%OY+bY_V8r=wEA>~kG@l5m#$)niK^PrA%~P`JgF zgbq+6J5e6K<%q#EaXUI<`4;URYf9DC)zIPIdDN&bfy;qZG_W{HyrT3D3JJ*+lM6B8 zoLFD(LcK3qlP!(1D#jp(0#ttbjVdf&Y>meL8pB&&U2+|Kowz;2VvyYatK~OTUzfWV zPZexfY31-Z%ZK95Htu=0Ja}Gu%o&NK@;I?zKRxj)Nc?B3CYFxXbFe<np}#9Cxb)R= zV!`!78g@Fa{I~NX?(1}pTRf?^d}&BLiWir2*J=X9YYqHReX}L<`IN}lWHfQ_V!rd+ zbo9`=VWCJXqCi|d5W;DAPe6t?v$&vkE2=svg1ejSM(;Qtq|Fb`QPIukKheW)Q-K8O z(lHh*Y2(`%*zoBl=fAxSeF_waV{;wpSotB;TV)yC7RA%ouUFBrcMkAt1Ga(gw{%$F zwHUd%SksL5b)1dP6=?9fOTU!?KkHX2ufA>>eQddgK58J6MMji<aTozga{|#^nRM(P ztO2qQ9*8ix6^+LHvqk^@MZ(R=gOK?hJ>0AFlxr@sgMe_!f|_y#Acv#5MRX>0+g5?I zLf@jCn8|c~sx{v3mcd6^nbM-m$;d?Q0$t+afVND%j5BkC(DWt;KHuyjEi*ZTx@9SP zw8R7j{M>+kr<%cmk3&&~vN32WiJb71R%xpEWC>FD)q`n1!^rkc-)ZDcWx}RdVw;P9 zX;@A>|9)OG^?p4U#~E+OSF9AcnRQM4NY$k<ZO%BXWTir%w5XuTMPJdz&DGR<^C7gb zcrr9hI!P-nR)ftHQ``|$h+cc|g87gkINLo5ejdI7m(Pqx5u&;2^ym#2h?p43wrcsK ze+NRCj&L-r$n!%d3KX#XnR4{>{&hHTvJO5sEW?G*`#8;qPqF#}2Q=k-5Q@y)OFw+K zhnA_H=vVzP{`L85cz4ketU2lv9ye<?9@ySUSL@Yap9l4HT}wY-m5?S*{ksIscYRjw zE!T@g3TIULN#kcbaOr~t3)b%DKApbFJ)7LaHMwr&v!|KDwhv<+7psPIms(Hr`tSD& z7UYC*S7)E(Jydo0(BMAt+t5Yaq@B_9V^|00v2`eno0}_`uBeHW%;(cW>EYapr$&;; zQ;zDCnxYHxaVT?>6q;Vy%q{h5;BEEuMBI${Uh$o|p47%u2Fnh+&S{M8<?F9h(JxVZ z&@DGHzv$Kl$0Mf0`K>d;(24L}oSe=henfIGI@xlOJJH!k`z@Z8TPTmFf5DySuRr1{ z`W8~%NsCba+E{A)(oOKJ{~*oVI|q!L*3vJ{b?8|(LU`XWy7Kr5&M;y1K6smE57Ycy z`K9kaai{oq;#GStz}W4vNDT}5hq;ru9qFO4p;VR*nbX7_GLr^pXLtVmo=w=g%9YM8 zk3+7R5-9k$yGSAAJsM+pmJa`${$DRtar)OG|C2WuduqODZ>$?UE3?G`eX-c0^P<3Y z9u+?s<i;kmTvT<}90GUkM1mVTVPd<5IN!Jyhi#UoPol?DlV92B^v3<DC9D?B9ZTRw zxF)}9K#4wY>7gaJmZ5*64xxa&(ElczRh|1}{_nhja-z~{JJEiPWMrx$i9YH%vvKxM zxE?!mcm+#wcc?Sn-E9oFtj|cQS3>+6MiF1Xu}~tu1xn+h$;-0gEO5Z0qVcjOa~nQH zvefWAw0Ew61Kp=k)Mi9h)jfa{tKPF7G{6OFoQAC2FbLL-2Rh9jPu4ucjh(+4R?f;6 z(dctN_(I}(I2RlbWA7GFg<3z<-Eo7<>$r~ewN<fs*i2+4L*UthZnSjwR?e|vDUL5t z0>^-Vn2Vf4^$e0>qWWDZ6PM$5ts`VVQxmr6P#pN88T{@ng6r{;T}yvj@tEpr_?f<e zEZ>~O%~>)A4VZ^8jRsLa{T7=}(>o`VEAPj%FW!sk)mynRq}LQUA4MXNQ)6nQMuO$a z6e?Am!b)7sC|~!C>nog&eK$VG`LR{pO{!0;FYThMN>-4e^P*9yLpr(Z@QO6mCsXH; zEy8*?1`6k9kQI-|6Ft90c*}q`Z96VY2KUJbmx^XkX6C#e8?5srJ5PpzUh*7vKU0km zvzK_3Y8lMi8A7aO=F#mx%dx@8!DNo)ulTkWK|{h*x^(jyXp;<ceVeNt{m11YHO(Nb zA5~h8thB(=FNJO!2uH3T!^C>S0>S%G8tc*XacqbiQsMV$tnlAf-HNWkZ;_~6qP6m0 z!|>+7KvsD#i8(453fs1{!Jfc+>>c33;I$nYS~(Oa=roAsQpdA56><2ov@@u;O2C0> zex&<jHYATa&wU&-hPv)ggQA`nbS#X*@Alk5;a}#VbSoV^RwY#eXvwFFeuJ@6$YO3{ z-&8iWx*T>aPZBYOXMw_LrKT1An=;7o*?#O()@FRV`!1ZCsmYuVcd`Ds<78u!FP{3R z3Y!={f|4bQEdR1Q?r$`~#Oee-dPSAw;3VujWI0^BJe_T=_=fMR5;SzlNgA;~5<3i% zY#9hWfcku<!_}#oe12gz|9aD6yz}8n<nTo_f!^(LqdW5?aLCPS?6cEMaA~;3$dM`R zx@#1?U+hj+#4l#G{R!A>WGfo{xDek9=qCC<5@>LOH>j>yPuphbk==VXlWrSD-0fgO zRt~+6{p|i=E!{e7Z={bu-jBrM4WlJ4<}P0GxdBmFahTrK{y>ZGmC_o~b&0k%Z5DmG z@GgzF>*94{a;fOxDehZc3wK9FvOXm0B0Ke2w?b5SfY{iaV9)&mNyF9#W?na+4pcRO zeoGjAbi0-u)0bv9x4yzs%`@Q7w*g8sSHpal36MIj8ox>s<E680$e2J4Hh4y)_^9Ve z8uC^R?$5qPMbcaiZrq@X!>5hH2WQQO%gs^ZoDVf<a#K9o6kmykn?2*z4*cUs2gTs) z9|ZKy#P#II`-|**-eUGbe=->vybl6R@8T^)lf3+`NemkD=--1Scx1O89wk?WgK|@_ z=gwMK{_7EhNp7e8g>rBO)REl_Yt%7cLxlIZ8{xGXL9p&+52}q-fcgik*!CfoxH@V% z_IH{|^?nV4h8LT;g64U6ZCVb01+<|){~RqoH<;|#Q>?f%^ccRk=dkeLd?R7(oI(<` z@H)QGbb;;aECS<YH?fA?EAr;`CZV~{Q(`&109SlV;S^@M<BY*m@Z4#l5BPgzI@x5m zj?{jigN^+qFpAk;xMRjKJi4ZmM%`0^If2{B{RIt3WlR<4Fd*4~@%{o=-u?qkX=|fM z*^Z9hGnSDp+R&`{7W+AxGfSZakLp@Q-yD`CUk|Ngn^j~<`Oi}3BynG-`Q8&3dHj&v zb9s1nMSvs|;oSrNmp77<<42+U{6csyVJfNWWkOa$J`0=`NssExK@G<x3<pmYI^xbN zF4}6h#QZ-2_onYE*WZ3w&_vUy?=D4T%txRRN)5=>Acj{el7=M@V`$ZWWg?$e1HERp zEYN0rh1pH3inON|aB}WKtTjlAj9q?`iN0pM1+&MV*f8cG>kqU*QD$N)Xs!g)Odrx& z8OKFA1!Fq{F9Zfr@M!QW`tjs++|f9auA@#^|Bn$K|D&7>ejqu7!dp>*ks;o=Y(H-; z+KQ?jT<8SvW9a-AEY80cPX#QTeh*V7<1P<iBZCQaa==<)e(-J)n*rLwVULXHJV$LD zc5f*=wZ#h0x6B|R-a>SyF@d*|h%*yMY(|Ey*?3yA<cFpnKo2MFpyOw*2i<9|Y{LRA zl2M(6hcueu{tqEYs!UU|6gZS|b8G2V?-s<#&j9mb>JVi(0k4;l6qHXXAl_p?k*jjY zJK97Ia5&-xtD0g*-bMLT)O?e5TJm#jh4GocP?ijAap_w8yi*yRa(n4d$3I-)P8k>o z%7Q(kD3t1@z<uAtpnatVmY>)|?$~|;d#1;%)!#vB$q@W>pOBhfe25my^z+=1t9aM@ z5!hZ=7Ef>wr=G$mG*Y~TA0v`T+tC-QxTcP1*f3NH*VO-kYH1~Ay0R1YC9V^;9q1;e z5w}76Uom?&?+^9ZoCHll^RVF2T{v@5nyi_qL!E*rVkiAv`nk{pcDvM|<`Z+Vo#IJY zn|X!CEyMVvv<2i<N`PRNvDk7)HNBj2jt-W%*WF9qK=igumkwEU89AgUqVqWsRC?ZU zkp3xyuN-fu!RwDu@%L-A&z3>V^AMIO*jQ09zK`T9ejrtY?qRLiUwGqTFQOYq$q<88 zY~=k<@cpf1^=W@K{yR1U@VEw;<Wve(3smump$;(O-f8~%zft(wuo`I9Du5$3(jo|S ze~bnnnv17z-hrQ(f1p$X^P9Co0q@@<1f}>7RJ&gR&rPw1)%;#?XGQd>i5eTxdxCrn zw5J>IPiKMmEGm`^M3X|PSZ37om*-9Fi03(iUG5np{TYcY>aZp%Z*AvK-AkYW2ad40 znS%-Wbr7#oY+!m-qTx&-=^XEgTw(d!&v<$Ia%O4B!QUijzChLr{aQN+PguQzT2wbv zQQk#v#pKI$$ygiMo#Rfq(mZ(n_i*{nG8wW}t^rNq!&sN>1?Cw)7Y{o&2mYZ(I{jHR zxTFWMC&_wHqH~Qz7Y4)h#|bDtNr5@7CD=-|HHV&1(!%g17pp27;1cx=Du2>lk|qkm zBd&Nr?UzKHaBMy<@{<9VJRL|MnMtn&yg}d33{a(k!`!TO&(UAuRr*y{2VSa1P`^($ z_^*l?326L)uX{D%=UraHuWnPAet0q~a&#ewe*S>VPcLB7ZI9a|>r_Q5^^>XZmxGes znkFza@&MYfd@(dyj=@9aWpMa>860r;IwtA|#9tKx;Gt0fOgN!|k0obt+aog3jW+}~ zdR;<EHz$De`7yll>>Vh7=X@HxSp}-Ur=g0*R`HnSW{|su;B~7%;tP_QaND*DVfe>G zY}MC=<k-_D5q>aYFI90j<yW1nN5*^hqj5z>Olx60-5}vL+!%KimrLLhDUGK&(=`IR zTR9eX?W=>I_LE_blNEh#vlOe;_0TP)11QzCmfmr|`0An4=p~iLr!@zOR}b9ar=GV$ z)!kd^pWElCz<UO7u6l?*6okSA|9^PyaM1zsVtW=DG<_FZJYK2dh5l#QC-!BPUdze4 zrUPITI}ZN|Du&SDVPN^<F-(l#%z3N?G>fmqvbov3)2=AYkH5`zhK`ghdYA|g_8z1U zXO5ui4Tfm&j2QmosBrT3*EcjQXBvKSGXNAP$)Xo?f1!nwF_&~;J6#~kj)G@$Idq5Y zaX97hl{r5KEOXk8PV)1@Cm!nyWjc2<=Zz0ofvYTWx*A05bmLg<lo7(Fvol%f@&`C> zmj&buEWy<q!cl47L~1<#G|rB9CKr|+!KQ9ys8+)95>7vW-1r4B-ZqfRdR}59-J8%i z?UU5+>|-QSH|pbV$&8{QkrCATL@lpkugPX)HKRKcw8kl4Taxr<7u;)5p(gt-k;-iW zl7eX>e)y>q{13>&%+T5Bo`NB}@0Z5r*u=5b7xdYNmRU^hTOwYXV?;bIYLW)68W?Vv zBW{&=`{nVuY(QR~RrfWa^=oxeo0N|T1)MFV8;V*{{hU&I`<N7*J@A>Avy=zRqU}^J zW;c=^aTXgMY)6KQ1K@1<fE#k%hqS&JTA?CZD0K2XhTnw_25Ikuc$3X%GPTf`-A^&7 zSTx}mRh_8ER=ht%>dth7e@HLf-sp*!nZJQ`XKG=N?;o5z?*&=^RkV*Svp)xC)*S^o z9Sd5xtQIBOoFLOy#Ub;aXEbgC<Cn*MfiPzUh{{#x2BqAhuPl#|b_r@or1}`1Tyek$ zTEZkxfo_GK^;!0N&Mio)OvdC<8U%!l#7hg3;M1?UXt3=|n5+~||E#zIx#d#eZyU)y z%xpk^M4i9*FZcS9KK;e{NqSkuF*6}JS^;Jljf3b-f5fhYz&MvectdNbzU39}>dv<k z%<Oh-Bsj%;2I=6sJ@dIqSFNa&WQEEQ`6wduDUw-RZNN{x`^d}OVzSLZjo3v`VRLS) zK<WPJ%q=GgiuqJRY|}`gY%7RfWX#9^kFWEN$Ljz8znQ&dMM5PjNdwn;zK(_z4Iwf@ zNJVL@G_Eo;GO`**p%fJn#dTh<gNl$MrBWIiB!#p?zw>$j_5JJjzuV<p>s-&*^YM7x zACkg{$k$i#w0+GeecSaqeL&KX=*gC&8}B&sQO8T5PRrBI-&PR82bx2$7?VY6I!JBZ zc1B@X8eMC5AR5IoWSm}r#I|jMjxG<Psj&umtzCqe_v!SBkNu3>s&o<*wafNI>u<_r zQV}issE}S|)qxHr9pKZtmtSM&5_{V7+C2J*P7G=@`w7vFnnd2T7-gNr90Nm!q*jU1 z+5Hn}1?OvM(iTNt^GZc3_qZ|%cF`iTB@@x@DY^8KkKPCr>llqTNiyei9Hf1SfURq% z5xgLV(HD*<5{X>Z2H8qq8|b0y^)1+|x=d}UB>#eK51mL0ZYkP6J!oXRZb1&6d#{G1 zO=zU|&6TXp9}i|Tlof1`s;*^^S7{N~HLnF#+agG+#7*{j;W&NxinMK<%ystITNPS< z`zLasZ7Et9Cqu05q^O!3)6wudb@U)XoxHtMEGWE$A!z4pl>1AXSX(V;##WsH1^&fD z9PdMg{9As6etRH~B`(U)sHA8+Gm)l)3Mxo}N;J}*sgHU$e#0$KdeM@(&+wgrKvejv z9Y57IK(p%~V+G!3eAl}ajR}NkJm`#?rmA7TL{Y93b}{u&$(qEuo8e567!+UM0;{js z5L9>)2OpY-sxP)+etA$l;Pa8t(V&GxHn%VZ8jY-5@ku(Kt4KNN+sa1VpTKU>I0Ms{ zE7aO>x~k6+W%h)XCM&5fQ)~W8uGTl|ANin{!)_aKAqDHrXwSnFu}8*XG|kh2tsahI z?`8!MTrozB#7)^CD>+hSKM{Fre#f1k_2^}{Uh!eeiEv1p8IS+ux)OJ_0P4uWrKn~? z8a~mZjt)Al#?xnyQIS`(!D)zN?&m(S<^8p@wTsQ7r3|=!mJ9l1NbE1Z^DT^y@-?J) z&iqXmylO=Ar#wQUHU{JlyZ}=Rnw*~AO(M+}(<Ubj>E4wM=*?#fdWNk(bMJ$q!ha&@ zvDX>n|3{N`sF=t12<XL0L*GE)m4=jTvT#mY1)dnBOJpyrk@|c7c)evY5zV(phc@^@ zRo@B<`Dwz3C0rSm>JLiBy#=q1I0gAyj~Ru84S4#6yD*k5gVs01f`;1#^fxb)NSFDe zG5<8&pnndF=f7rx+vU(syWRg?&f9@s!k#Ct+wvV^GSF_=N&XJxlK!98WbMu)Wb?y( zlFpHK7TA_S-WN%<jdzx8_jyUa{aS@Y9t1+#-v%U_G#h1IoQEtlhnb+cQfzlX0teBm zh3tGGI`-soR5;xPUD4FS$7UWt&&AG?ZwvEq>tY>HnQ;`UKAV7OOFy#lo&=w_Xz(p4 z3UZ+OYYLetz5)f04<PEgBr_@*fD(CG<a%5Y+WU7q`$SF?;{PR(%Vl1SVaP(%*R4dG z*;|r9sY`ff##R&>Yge5fb(dI`L~wWf8#vLazdCckmApUj3HPMh;^iqKq%2K{6~9RV z6dADlwt3Q~%KR}Reryi97NbOh+|!A?LmGLKO|cI9HsOvFWynkN4mx8ohhD5f6H&)( zN@Ir_%IXoJ&xLLzX&<}6+i4$JaG%@pp1qGk&qT3@BVUlFXGO$RtP8)a%B6X^hse4a zVsw&^A88a%B1d9wkj+2S$PT}9!K_~|nGwEmGhU|G0(Sz^K~(-Ks27M3g(<1Tf3Y53 zvQh$d5&`g6v_b#}H){*eBg^mW5qf_XIzLi}H~ZQVP5DZgOV5Q(bGE{x##89Yz!P$M z*=K6pF992Dc!Hj<7^k)<X5-xhN0H`*KM>J1mH7NNAsdx$qvJ-#G=D>1D0<YxQI>W^ zk$)$&&{74?;cD@K+VVLAD?Fabb(~&=@fI627Py=w%f*w>`x59<#zZn%d=|Nwgz=+A zFLBVunYhi^hMQ&HFyHUG;%x#|>||lbVy!zIHB}B-ERG^Awd45hrVZ#aaCEi;ptIwg z1lNA-<I~|^SX9>X7jFs`rsu_VqX5a9Xmp!Csj(3!+J+ryQRz{n{^S6<&?JDP*VEAJ z!ZD67C&Yd~vJ|C8PouxP--E&<eVl#c6?6I90dl%ol{P4_WtvhFc_OmgtqFhdrjE{e z-g(su-aJJ~o@Ck=-Um%P-mqb$AiSM_Q*i3X0bc638Sj^2ui#g`h_!6PMCMfWO`hu~ z5rOj`IjTbC5YI3$Qs5Y-A;?~)BVe(dwSHqPFHfOO5Yo7XH+m>l;QTy|clc?5U{<&V zrTuffMq2TJU|m+2V7G3OpsT)0uxQmp-lft3!Q3EM-lp6n9{Y4f&4zZ0XUM-UDX48; z#(P|L$tKqB0dMBY70irmMVr*J0A^LxlbZ8Q|7_Tc?z|gwPS*&s{A%JZi3y(VP3NUW z6xPg}CNJnY|Dt9z#E0i(mIu!rPB7&4UYxb{9WLuzK(2CTxLuiw@MXRs(VF-LYc4Xu zek$c~_4*lnFw6*N-AhCKe{>)@tmBCmY}<jS%9X?7$>+$L#sngy6^0L5y@ej1-*85U z2Ro84;6ZjiTK`d&$hQfjj5I?sx^4m*JR`zt$e$of%5M<2PGg?MaUIg3*-5Y+g+A6D zqn>@b4)3Q4v$H*4lACjNNY>*akn;WjD_m=tLa#e~=#2Uw<YtUKzQ2u{B7Oz=jtay1 zSPOJxiX&2<TMKki7JAT^OCHzM;g{=Alf~|#NXAhbSx!4gi|!6ZUoN(SqWfL4aGMJ2 zk&n@m5BbP>Xf~p|vQWXH#js~@9eVMs0uKG;)!e;Z3`UePy*lU;I=?p)(ZT$2Ja=6z z(e?`?TXG`kwGsMc_}NY>VCPv9AFE9?(%MM4(-msTi&yC3gBwi67g6>?W*o|vI|p$e zQXp!9I=$(J1%2=ODas;7jZUas!(j$55k;XKBJAP?T}o5fOURZD+Ze>PrDY)F<4c*~ zCA*->$BN9DE?e`8uXTx>mpzFe`#hlzPWMCazG$+?tTNGGXIC=hzmJV&SCZ{t7ojD; z|G?MU5*x3l8G@~UJ$M%uj8nSmml#q1dKg@MmUkkd3=&m0)Ts8?);y42Pj&5O@MuH0 z&F_*PylB^TYV)Q_n{N}tslvSvc=pmXIb|7&=}|p~-@MhFH%CpBc^;6)JAb=Juz&Ot zme=kP)Li?6$%Z0E{PHZ^?pzIiKGPW2Gn1f~i}HT!5~JqUbu;M)`r&L!89ZovPKnY( zl&_RJNUP+)gVtk|$foy9rJe>H>RB#`*mwsD@dT_rIT>eYT7&5Zf2KHO7AOqP;X~Gq zKF0HGHna14BPeHIqg<QR$VY`O@b~a`YQ9-DPI*^FZ9kQa-ToM0xkhi$<2S>N)0!k; zh9MGO7>p|pdf}g2b<xnc1z927%cTAIO}%OqBRV7Zs3t29a=A1c87Y5)^uOz{_$_Uy z8a{_h5`JR`l`uG$%RfoUF<NBb9}54QoeOJfUg0SPF*r)=0=75F!Y1DGwA1b-B)2Px z`nFM>6zU$Pif3;lqN$$HO@~u`W8;i}p(wtwP!g?;zmLJ;3WU$CLM`@M(75m@(h(ej zsaFyOHT8$ls*HLjrnL`Srj7}Oqt24aMJI7R|HlyMU;n{O@y>x~Gabq5fWO3M#U-?` z<pfOB&xTtbT!LzP8yT9WO9`e6kx|*t;QGS?)iWH;vs4kU-?N?U__~4eOOQu>S$D9D zmIa(_n1b3q9f5tnMF}If4uWl)(E&X@WHqWz*L1%oeO~jZ(;ksn$E61N;S;`sb503d z**=L*=!?cTkD0OBI;zMhO9b&92GG~D;pjt88`mtXPL8U7B;9B1kUAHfEmdBE+AXe= z<(6fXpOZU!_~9)6oGOM>Vz_#|$|$@;IEYNnwPMGUDjClkDXiw`OrXC6QD2U$kopDt zq3PazeB$p^K3P&L#Zwz<uemi(i^`U{O&$E3iqF><!>Ki*%$ZY`kon*WV<=qB^rZw+ zJId6_M3V&GM6&`crL%x~!JkGwiqC}&1z!Z>u_mNAvl2I+6rvaddurfnK5wqMehrr` zVygA>@T#3hn6jJiuw=Ou9>0;x>}=D9<Gc99m~+Zdf8(}Nsm(KRs%-#1=DQCJwywh4 zw`f4%ZwKqSdrmW33)11E<zB|{{zI^~bEAwbOtDzWB|$p6VRL7LEiBl-80USfpzZ|M z+EiL9LX975y+f&*a=WB}9#-Zu9rbFYws;Sfc%p!szd{7^YjYrTuL0i1@8>WKm!>15 z?F(RWf(-1%sc5JCR$LkOp6WqLh-4;%ZoUK)ziJ1W&_4l}2K|7-6H`gFQVndKWJ4xV zkKk8l7bh1@Vk`qb<GU+gFhM;Zsf`Nl_|OJS9r`pG&30~QK1nDNUk-$#x8afCX1qHU zom5YGFa9XtFV1AZuPKn%-Z%*A8AloWcV*OU>FLa^k;%C3%CQ<bEQNg(UI=ETuA%-$ ztwQh39Z8Gvar~m}F1|c%EA_0%gK9~d3-68M1sz`}F&DH}!SywIjP9>75L^gkHfG<( z2GK51E_EDyr_ZWM-WPxmE;x#hU70|AlCR^}{Cl_n{o$)~-%lOLTI&X{@7{)qp;KTB zD+l2=mvQSdV{FG2n~u%Pf@`uBc=Wdtp06W?McsR-l@Say%**F}|1(G(wryoDIk{17 z^7BCJU>)T#AdUO{>>%7-i&!4rOVxFc!=KMCNc{3u>Wfzxw{BJuJYO1^hexxF@f}G4 zNCpk!rFT!`&5`mrXVGoAx~>_N?&#wn#ZB0!X#+Y~)B^iYH$$4rUTl4wCVPKbGw^o~ z{x!sb%scv+BO0MxdfymR-Wvan*agH!=Kn~t#jG1Y=|Bp~>DI$4v>LDZ>0xZRCJ&_i z`N%A}gn1KPP5rwh!G6s)MlxDoc#?<(p{5?@U&plSsP(HLC$t;4@76_TlAF<Q8!g6p z`wi%9sK9$?9bz0^9r0t`2k<}h=Q5?|dH+92HokTqU)sYIjb12(Fzc0sA8x|x{qlpM z21_i`c803DUc#u~U5J;SuBh?rMT8fZhO8VHu=$#H^wgdPQkZ(s*2z?}_G5Dr8I}rR z13f>X4<2FQH0FkTw)|kzZ~Y>Zzh#i1mC0o3?@sawiIW|5<pgnNpyW+!iH4FVpHOC= zFmH}BlB^o#wF^JMbIwMA&pNJutLh{(A?-8F82HH59NMD&g;%g8=s@qfd7%1r4ocf` zlUjIKjowpGPWJ5TqI1g=v9O;#E%nye_V2otG`*042_Hnj^6nr?+OQI)Ts%!s-ElT{ z*)}pmUX_cR^^zWY{x|T<zl5cfwAiQa&O~{57IA2w%vuhHlKc>R_V-n9^hxs$NS7zz zH4;NOu0WGKDF4gEeprD`BSi>#!r`~cEYv-pYm_KB$NasYK?ReyWTo>?+K%|q){2L0 zXZ^}%&HLAquj;4SbjOvn)U?MW^yoL{!pZe)0)JgPIWk$BNc4shr%Da3ndl#5F0zjI z-gutPtCP;83lzbi&H`(cS)ttGOxXJJEP8R>o_tzhiwr7#5mqUsw4@^$<L?@%W&@zY zUQu$Ct0Gq(w?bNSL3qapHMGLvBpt>bsSI+mkd<j-t@<O{cA=wA?bB&{Xuik(TQH+R zi+=HM1^Z&EEzL;JqY};Q@qI<EvoCG}f}UJT^6VAVw!{OA#GXNX%L4FC+D3h7pNw`g z9=tO`=diK`1qq*WaayGdE@+-apQ?FQ6Y=#Dez$fHewulLeCl&X_sXweo1GUavzV3a zNqG-eZQ*5lzl$v&trE6GqLUlZl9iFHenC20)|`bl_1d!&Hie=(-Un7JT$}ZNYQSDC zILK~KvOrFC8L(Q-j_kdb3&J-Vk=c2Q6}zd9{5<Np(`q#m8~aG~wO&BXLx2!1IW$Y% z60ZNFi0837-pyBku+EyZOjhn(((k?ljPCIj@QiuB$n*O@T&~cG*nQ{e52c#6iw>r; z{?bnD@W%nvwL+ObaxI8%zPufsh~LEO3-^$1mRC5PXffIFHW?(ido#{5B{6EzWRut& zth4};CPO8Zm?p<Q5j}=R(_dq^y@S-t=nK5nM1z>`{fovwIC4W|Ikleu%M`k~HOd)- z9x!iDpsrNwleGMK$X10*(n~*KdxCRW<;{=id3{A(v9}amPRX!_C6cuXnLmkciZ#>- z_pskAC2Zv;E~gzLXx6-uJ5~DD&~eG_q#%DWHM@nPPYcgQ=zJ``q5p&&sSCEzk{3ZL z;k(d|C0W4#{PqQ<oS;vimAMOH{o2Ul>NS}EFPr>4YL0BRXOMeaOz`R{PmzdHGK(XO zXeqlYBwXf6Cm&K|HKH+Eo4A(#8RCH@R?nob7A{6b2?^{r`3be5bG8tpiT0%DP658% z)I#hX5|M|&SMugv9f>lGBL#bPP~6R7KG8|kWL+j5Czs|aqb~Fi?^V?%AIS@R<5UN( z41b2_oZykdBCcCEX+Ib(c!!_Z%aWZMTZrTJv-ru13+Vp95XtZFWTp0RM8e`WwEkXe z`ecwJZLs7M(Ryq|#B-nFX>YvX!Jp%JM_@dj^!^noEoy@L-gs1gn13Atk~7f})p+va z9ZS8RQU(oH>hQMbH*#-D#TPZ=m{zM1xOZtUTJCR#_XIp<TFvjliYiMSH114-_fChM z%FiHe^DuMDQ9$0hWzr4Y%5CYS>9vVMCblN*XVezHloXvzr{$kJlGFG0kn$;QRL@hr z+Nv2VSOH&o3BAO55<Rl0j$0s2z>+2iM9_P(;P6!hDgng0`wzOdH3Rn9XpoyfmQsI< zj!=Em@4?%EsbE#L0+sidgKI=8yiuHqhh)>C_u5gO?8^w=Sg0@Pzx;w<Z+0b{j3n7* z-h5i$Tb|uBl0+wXzGjXpY#@ooqL}Y%ltpxRC(!FZR?^*jcObR-Q}A}nC?w+|M}{KO zkemW%i8SWumsft^z8)#^&sL32kNb{xTD?SjgYLu5auc{-I0a?bE3vgb0_?NFf=I9S z!6#-XVO?`^l>bGDdiKv7_%a94KQ4~9`tc?b{=*HPmAKeGnOx1cMKNjg{-e%l^2zf= zM#+FRXes46PgkKm-w)ENr|yBRqXn^z|3P-0R-<3OKY{!%oFl_|1&9oAF-j3VcFCsM z<o0hrVip&GSklG(QugFYe9%IM?~RDF+ES8Y@EQK(0&ZUQidou9QQ=F?s5|U^oZmYO zm6Y=@k;=*f`i@#Itap#CovweDUhWk`ALeHg!&yc2nq$|nT89cP7)xi{@BL&W4{5P8 zzD*&&7G#irzmka0icx&#*>7s;A(pt!Q72b4n>drzM#h`V^##2Q!BRUbVMSUZ6X-bs z_kQA<RmOaAh4%*T+<gfW%}c06zHc{AW<~@&THb@d(c4jq#k?ASGh^Dc%9viVERQYo zcc5SNn$Yz=ChW|TT2wwSlHMV4jc6RW&Prz%Vau7p;Gy0^(pqm}g$H8vU&(mlc9h5J zX=xCh2q$K)v<zw$#L*mxiv8hHP8uZYuw?ja>h@U!n0Id>@YR-l2LruN_^O3HInLZ> zTpK4KkFIf?t0#x!ekhP}<=N!ARs?-X{2$wWxSmcbDWI=Ld<BO*1*G56NiRLbH4llp zl3A0&NcAt_S^=iX>=hiOIHLv$OV^TPJ@Q1ETL2n-OCiSQE`sRtc=G7`Ic~vp4ZSoO z<3mZQ2|nlB%Y;te37@T2p*iFRQ{o!|jcQ^P{r(l)7aIdJe>?JNT#h<eFfND?c3_1x zO4*|=Nyx|`-?mgHhQP{dQY$6Hwu?Hkj?c^J4(b^)8ydi6w|~+n+|JT>=jxJT#RtSu zkwx!>K7!a4NmeCPfY#pT`;Z!Lv3K6G0d5LxaYy)Z9MRK@PnOQ$sk%<B$te2BYEN5> zjJtJ#cuyxYyEUlzts1CEWtfq$nL_F-xg<Jtp}78bu41#49AD;IyPaoIo1?S{zhNJt z=Xx6KTMo_mIqDNx`K=P|-K=O^IAb^3{b&jl4DmpY-|U0}`>%sv1p{?y1vu``anf0K zmOM_FLo-E@#LuD+7q9gpJ~qZkeXJ9La+}$@b7eR}WeVC(me|;T;`(-@-I;?Y)v-s) zZv5USjqdv_K&S5mZE7Ziq87{1lSYW`HOB+A*4kiX8PJL9q|$MKf;?)R;zaYYAJ<^e zIEB3?ei2zCb&9{MhTM0FL4LmG%rVc~WO1(|eRa_Yp7nD+iajbyB$N>Ao^uz}XR44z zFXw{(iOa0>3vaaHd<9f)3dGhs&*8aciqwAV$=L2v1bp`Sg7Ri-vO{C3Xh(=YS@_Y; zw&{_Mt#NV^J2+Q~&!#pSFsjXi1b)s#UN%!}e=gUgm$d1##kC7DJLxBsIF`|(t0UOu znQG*Vv^#zN{4CZ&{V1!TQcnzd4rGVPV<LL+7><3WM?`KeCR=kn$f=NbOkq|AWqCrC zY%Gfa<6nlTMkNzw^zFnSwp(N6fH55L=@c?(;b)+2CSkVz9wD^2<{4JzP9)0n5ux$C z$w>Ju7xEJ8Afsc|<ne$bdgO4QN}X#%^d`395!nw^f!A*4-PiBndEAR!6$pXv;^nAu z=452%mjYMs>oMo<>EUmd#`x-!NaTHnA`3>+AZJz-H5Oxpzpv`XDaG%hfUjE2-0Hgn z_Xdq=D~s9qrb|41tE-guYJb4iUU9eWD_%hNon1^Aqm}ft%?hY6CK0u(O&~pg<jCNT zXmIK{jR!QBqxZqaXr%Nqb@^)|QvB;i+?v#ox<HJy_(z~U5YDW3{Y$+KnoiPXC!h~q z<7j=rQsS~-n(((ypM<_`|APDFc0xgMJ1iQChs`P?WYgFH7Z^N1$F}TcuUilf$~PJP zWfRa3*=n-KwVL_Vx|*a=!AM_787;ebp7`f9lY{y$q^L%OJn?V9r|xY=RUeKZi)~@J zTxc3O5qck)SseqNeY40b?KyC9umQeYe8{J|#;-Gg6=;L0`ow+OKD;A30^Iw~lUwgn zp~<w`CNfKiO#U^U&Z&+dBkT!w+;|7l+3<{R8(mpDwD=@Co*#^ky&gsBizd+<P2=#t z4p;i`>V8^%o*ca-&Yo(c(@~y{4DI!73WDb^iSXoFG}Gi3+5NqX$_nKdAlX7yJRErk z@m{MCZ$UTpd+u4R9$JZ(NKVB8D}vFzzAOl+I81G6{(u)=ordnu&le=emQwcuMc6rC z5dF+08f#y6udV+m#;T8)*LusXU_+z>X>Z|~sKGLkO)T<fuLnu67d|xMrx1(fLLP(p zS7q2R&l!M!GZHTen}c#CZ^56q7vQaQ8TR-_VcReMIO|S1GhkK+Qk4?KGJBW|zo8*; zM+-Fldw^Ac$P%L;nv`p-9`k;l0i}FB871k}vqvt=@?1HjPDgP@ZD2UXuIn#FE&)&3 z-4_3d+KPH)Z)Hg1Lj!Eo>NfVZrY_;<Wy~aUZZ@o8<~<U*co}Klp-R?u?kBaelI+Em z*RlIY6~?LU2HGJ07)On1AP?hL%%SZ<WSyuLo3-~ND1E<3Sv1_HJ{`6~6jnel!d)S_ zqzcw~yPz|h-m;COh@F2ep4OYRoL;K$LW?cx#Cx2SYTGJzus=qI_~@No9Eu5_R2wv9 zkUsc)3;l1uB|94Y2-<VQk;n#nvdh7U9N=%jCqIb8sHy`QRvJQI>$2c+zYuYsBhO2| zy%FB%bDYTh>m+ZRJDPEB6_HodC-e3`qPA;f;D$AJOjV~XS!s8HnYG)1W^((8%@-%e z>a_*y+|EBje_Z*F-fwda3ao6|RR=?1?vNxc8>o+^JMPlIMl-njC4G=*)}qaOq-ph{ zCNke^F@B@n0x_TFAf6`&mmO;70Fxr5@=pnolStzDP8X@BTXS&eV4k4CrV|A1V|bfs z44$DXP3@R20xj;}An%hf==Eq4FaGxibeY((Q7D4-;m*0T7q7DpuSQURu?5}o?lsA@ zwIMY;u7i2xBK@h}m!6~W1bP16iCnd|lU);11*QC>DC^KK=FCSg_~LvV&pNUmKU4jS zOa-M(#@k?YK}4NVkGTV>&kLEV9x>Ydk2y7gD-W}fZp9}So+kWh`{y%njU4c)ePfVa zG>?b}#j%T@WU_nui+SY<r|FdC>8#-OG4j0SFvSs%NR+=jy&;k&_HX>yEzZMqZA27y z3BE*N&H%c)K#X4WejWSSBL{o*ZX}QQ*K;TpVUisyZo6r=P;Em=KC2laMLhowVJp@i zr!=hL(@WN0#q-LQz_TeGf3^%H{n;FZFu{g1jZB43QMc;sJU*Gy+RiRKZ_GX#5?T;B zwVS?sZ7Xdz(1tRKh1g10AzPpLr>G?&l2r}VvsHg9pcmPU;`S|R^o&gn^nDvZbJJ|u zeYc*|Wp{JvjX(0pX75qBZWKfgI&wq7|28G3g3o{WKay;7`#k>F5lwQ1>q#l{ZNy7^ z-{ZCc6LND{gfu=8BcFx6$(u92RHcO>L6M0yYv=zJB)*8mKj(|0^S&}<xLTQceN7y% z2wOt7t?j|n0w`u!_BFG2#dGGt)a~%~#ZuC>K#uBG9EVGn{mCa!A`ljn!Ka57|93f} zgA0VC(~dl}of*#7pgb<8Lz}&Ym04kA-R?Iy)xDhDMfQ;0(e~e8Uufp9n<6A6G)3q? z_?dLaJbvvmS(3fQlgQ}=lT~AT$oh>9NK0fr9Ju}&&O6-%!6qy8Q(l<1*bq-5y$+zr zt*$7th7TV<mEm13a_F=9RHV}I4%;m{Po-RwCCx9z&<xcYG^`qhZ%d`28)qen=rkkx zUxO*7T{sm&t2UESN)EM44l{Q%&+?f+l`L80zlt_LY>$jwoyqd8CsApJ6#lS(0bxcf zpu_Au_Uoy_85{ml55%`K0}Cl4EX0S?Ybx-;;dJ7}Xu}<s``F+N5I5dc68n1#&wQmD z8cb|LR~o0HpI;U+dEW!kz)nYO;#ES5WE9xl8HYi3<5F@&m%j`fB_Bo02-lUH^ML3% z?ZPuxW}uJIjUv9RA$Iz?c$@4sB$W_NKIH5{&qsfgq1lq`^L5+k2OGAr{>yie#>PG- zAZ;<a`sV_&e{%#~%<@6M{@Jo`tYU~brG!-K`-snsXJG48K<?g;CsM9D=%-H#&n_?@ z3GwHN6RVf0m{%7=GNvsj*>f(Ud<%K<bm9<7u}LOI`bp?Ql{&KJ52D`P=NYF>Kk-S) zXe3^L5#JW!2+n(?Q9i1K1Cw>>>Xd%e;FJO>65Ma1&t;-(pUn((tcJ4L6WGs{LSziT zB4$r#(h@&c;@~z!bgxWC{Q0e)z`M?Z#Qs=COdrj{15?)EgC-ZT?8}ER(E0?oD{mo7 zzdnHJ8}5+cj9(D{Kd0_mOd=a^l1(4wv6JKk`gF@DG|A}`v=^k|3~o4ok?#k6CoYrN z1HVY$3lnyJ;Z!nZOAM*7G-nGE<yorBg8rv=8Smtb-qh7tCAR;7B9aPmpz{wav(vbL zSa>Z8*59$FPsd~t`AMm0nXV_@U|h`L_FK5Qmmu?x*|=0YlMK3uvc-K|!eq%!2vw1% zAJ37dx3BPLUv4PnLhoF_V#-8hFf)~uT=|1^w5y2SKVOJhI)!vBUqeoJ{YI8v{Ij@F zR+aV)Ek$f(G}<*y8E)-Rp-Iabtb6eVPTiRd@*|7L2fs61B(Dy%t#Wz2E)5VKAw*C9 z<ADR{3tZH13wb|HjBFOOM*S;<SUhAzy2VeEDCwE#=AI)+$JU3a*t|gZA2%Siqc4~R zF;~!oi+K!_>H}8%tU%iO)C}Znt456K%9)X{6qu1yNzV0QGP-pVYq;PddigJs%(>u4 zTH9>cxTlKrc&7%8nQ73q#Tdnz`jD)TkFZ4ietPxKi)h%|8O6-#Ax8V<puS};N%GL6 zQ)~!{dg71rt98*OvI)u`3A2;#WuPmY6oH>p_XCY?xlTggYS7+d?yS1-G4!xm8%@2` zMJ~TC#oD7Y*u_^K0N18X->>f%49nHw;Q4uid0Wbe=8R8BzU-^u3Yo(*vJIrNwpj3P zG^y}@?JW_wSMRf_)qBPB+jiT=)9SO0g40q#PIp<&zG-QKOJ37zR$or#F{@knHuCz% zY!datcnTJZ0+obyyswXac**}P1YJkMZ9*FE@QiDFc_!{6f{))0Q)Uyoc<aw3Tc6(d zwdQ`mE*185*jhG7g9#{2shL_JMLn80B)I)tl2_Z|R<rR~Ib#+b%Nwy76GZ=*MeVgw zqP9n53bs>j)u94qfu?J;_0QM*2*IDY5W(D^Td3ihXzG|wn!sE9tWD=#Z|cc8im9=m zMon*<Ex5H}F*WC7lfXSKNN_FwG5m~ufz5oom}8f}L-M{7@ETBsSpiS+<_H(~sJodQ zeZ$A8dt%`219OyiVJ@@<&W8ADDX_|Q8Ygrm)Z8B)z%RGlfaiQ(9{G9h78Os0;{zfq zaS_j(*&iUwm~6w$^Mlr8%tj8bG#sJyj{c+?i)0DCLj#*EOvAsw%!Vso`<ZY9HPn(d zmp4nr2Zzq;pe||qGq?X7#nm?7nXp%Ps78;)ROfba{B$fCZy6k+*4Rfgy59qsJ2eHk zRa}|yz4}^en7ePN$TR8i<jM=sag&4%C!{%9zZHZI-?0{|HD!8^$KhYn0X43=8}Wml zQuwS%n2Ulk>hy!7)OL4yyz5pq_UJCchOGv;wOkpf&q^@mmLEL*ri`ZAUBb>^qnX(@ ztpYW{G3L-n5*6v$$=mi%3|VYf=2MTKg@JwoA1cF=!TFm3)J~m^`n{&&Y2M<@r+5k- zY4l-koeo5gT+B&~cL}w9ek<s_)WEt%wbf>RE3xuKQ{ctUBB5add?%D5#o0ADW%hcK zK{imk4ssGh)d!x~&Y9Hv6b-OkS_ETNpYU{72kPadBXA&=-;OH;AMwTEO6>Wv3yi<r zpf1EtW*$h-AnkWWut(2jZ1idqbN%xJ=5BBsZfd{AEPYVV_-&L&M&S#wg~|eWU8{oG z`86Q^ED+q@sKHJ(L|uA+hq@&Vysc|jGLs+rlN-;Da1fnATvVn?CC^_7yEl}8o0u{6 zo1c4|`Yf$LSci4g$I9hQ#ji1@P7UF}H!7$~v>Q&nd%`I0&1HVg@xxOBqnS$!s_<YP zBd|W1hX-fw;i4%5M#VRSkvbuXpY1EBdIv12?*ac9Gs%5SVA~#e`%D6QGYmm^dI4{s zu%6o*G%)cdlWOd;199N{9FA=h(#kWl6|RBrQi%UL6xwvt@r{R*N#PzV=ylRZjohtu zMS~Vk^+yB_G0(-;*R1iDbZPQ!cs`c){6cNH(hYW;%Ui}Winpd>7U=Z$QbQ%&1~AM7 z?hm&x#edES#x!%0TJKz5kJUyhLiQ?Lbd;eVm&;<64HbN5#Vsk)8d}QS?&9HW!(^Cw z+K7>~XTkr{AD+z8-^?M2KEa<#OZ>j8i8p7xJ!5SsgjUF2Wkk+AV{BSPvFMFu)Ztr4 znL9hzP-l+0;cF+l@w!MZnx;`{bJEEG8N?;Qcy=>2vO5?YN_5fNVJFJr`$+*kV-mWn z&kv?5I;8O0xe`d3oU56eaTzaE>Y`fu9dNeUP29#?PCW?aHgOK$DI?D^rZmc#nYC^Z zyt?D5vZRkTJB1{$?}QSb?&<SXQnm^3epi8Eg(#Q=6ydzbA@~#{3Cm7gVB!X^2*|L% zwQlVXO6Tts@}pG@ADMg@ZR59vkb+&2ltJDKcw)F6?;I_LyRsXp>c)x0)-;y!&uqmH zH;(Xbj!b}!9GA{{Vha|D?&AGia2O=!ietm0&#?LBKvc?AQjR=biR+pVpe@;l;P37u z*z|86TzZ;-ct?M8HlTDo0d~Obt=rM;Q^7b(Eeo8N$kF`a%bd%lt$<5YyCL?z0dW;9 zLtm!mVc(7jkTu<drR$`~`L`8R!RKgdWKA3@sW2zjl}Aa5^)q~|K@L(@kHF^fLnJ*- z1fzAEKzNrsp8x0zq#jyD0wnHo!MhNc&XE}mQ&*y=PIIxgLnKNvivo|9dh${42_L`U z<)fn(ddS~Glk8lynyj(XMTcJKpsctfh#ntf65hs;b&nb15T?#KHD!r@RwClak7T3i z86x{?C$e`_<O;DL<MGyWfc-C^wIWAh)^&n<pKpYVIrE@+{4X?x8X^lT3D(Tu8(gr} zr*o@!qTD4W#MLD*A0iUg!<cph{7HRB{4LT5e90hT$(pF)!gG}Pb{lP?$*1YY7C83p z7j`P3Q3$7TCA_s|c{lo@Ag=<d)z*;ost&T7CrgkoO}uo>|4Y{@xnxNGUn%#c8$bJP zI}SMAjbA_2L#cMrsN?e+?7#OFwC>J9yY!5)th@~JG3?|Ww962fngp@>lZ=^8<B4!$ z=o~ga6pCy3YE<*cd1}Mnr&Q9_LNHl)8@v4yN1r3_!V>Sb|4F$4`h(B@zf$h2dHl%p zQec0s7);Z;sifnbc>3{UI1#3kF;^)}M}G(1v}tIt(~RuQ{DJ$87DB0n59u(8Vm8Kz zk;ob)@*q3`FPW3f9NC<MbCPd@N?<BDL|nkN%}dC%U0O)XJ_&4Q%c6A=ns~X}9|)P+ z$1EE?jxT6y;G(DJ`7o|L2YFjdpxyq>u;Nxcb>_x6Yzs>QlWDK9gvmtoCI19|GN?%m z$U<VA#tr)6Gf<Ml5$y6Yka^!&S&gI2N$V6DSfv#V4G-IZ&@;$#$`REi?#Hv=uRu>c zUowSLw~^bgCX>;@FH}qCcTiBj1&Q9{%#+J}W8@$C9<NzskMp0)<NYQ}$aW<!Fctlc z`?r~s&s~;CX{sT*6@3=QG^Ufbs>MjWXde2h_<|8SFqI@Oa^PL4PXvedG+fvC4q6V& zBH@uHa641ME6Q4pp1FCEvvvz{{(1{E!|n<$xcEZQrX7QuuTJK6N#^j6|H+PwPuLCP zzcawQT$D6dT*cE@%!A#zN4Of!F6@3+2ra)(crSHU<HZ~}Dsq7-Jp7Qy7*5o|6Z;;) z=8lWjmT#Nz=(d|s9eb5}9LL!$^m6dzI$ac2wFTa355W5OggU=;3YsG&i`7It(B|X_ zv_Jx2L`{nD-FI6u#x}*&?Lsl47#W35hn%M37b@Ue+bD8z?>v-rObTY0Y=rgG6p<fu z4qgpMfrM5PGZ4NGCU{hUo4c+6{Sn5G%GTkECyproVjO52+QZ1l2UPk&J-plb8Z~u` zhoIx23@`caOSrRN8j9IMyk%`3elaS+C-NDx=tZ7BxTJl9u?JHjr#Byeyi<s0i0H%e zlznKfX*j-nsuwD6Md0yW(rC=17N&b$giQ}}utQ-QJhT0T8(O9C<ZM}L%6JMMElq(t z8z|J-WCQ!Xx`AA8;?SM3)MRCA64LmEQQH$wefnI3cWpfkY%Kp9qqSn7W|_(k=oh;U zGdzFc9+78)r@?c`)UeywQJ)Ctm|>jMuoLgk7a<SWZmf5B7Ag{3%QVp}+>}Ye9v)ZV z+6zhaJabf_$}B=NxHgVbyEx30f5)|lxKZj$DigBsCC(U^C7b1Eljd{wq<7>D&h%YF z?(oBRQ%Mht;AN{eY=83%&)WV8e3LhTbhkb}HE|E=<Nv@f)4u{<s6Z}uFjT;~&BXic zWLW8N0p^cS!A;6bNoHIHgn#!X<?}aF_^v#>%Pa>!?|i6p9>M0Dw!m>obu{V4N{Gox z!L30mXzCXcsCfPuD@&gMKC8~zU{9rUPNhr;RHKNsx(OEQsRxyVU*Oc-A#8hi86LEp zfY;VdMY=Ud@uBB{nA4tIN%3WvSv(a#^jAcRdml4hS?aiLT@5yyI}i7Wi{J_kQ@qbu zgSt9RnWSpOQ776bfoRQC+_XHJtYx0V^JhiyCU6lSZ!k;4Ny`<G)rw)b(0LS;Ql^p= z?P+i_Qh~JHD29qTY0zpH%P41f;DHJ;8+fgVr44t0!4EC;tjG~{>n|lr$s&|bl`aZ& z#gzYAMWUFUi;MZQ(5kQBu|=#I&O1MX{bOzLyKDb&k7_)K=5n<!sowag0>2esTQkmc zxcrZooBe}nc4zUxl71Mq?5Wl~A%)MnorTIP-?4qta@0Rtgq-(o;>~=!jQqH*gc36J zki}DLvi?R5whlT2vsddfW9IU>K1`G<+{iMWJ}HcXZ6#cYafUrt+8{@qW|q55B!ky} zfn&OW>VC}4>wMXFa8uI^EcR*;r{Uw!t}_U=r#mSzTXSgk66Ln%!Ek!_Pio)mjd(O6 zAJ$EhM#jGRIR5kqB>Q!8p6_V5dhiIBmb}jyVB(m814^)Wo-A1xT*!pxt|F6^yO@%6 zeViG*1ABxwL*u)pWZ;_%$<B=+?)8HVzmDR5<$7YkuR)r;ZEz#+*QSB9rw63xaP&+2 zcBr(oB-t{aypbiA*fk>!rqmjtRkE6-YStz&b4i8UYy$IZt2TKVEkjn2C)|z36lFYn zf};bU;D9f_uvSL~cWz>!Y4IAkzbFFdId<c)>{dMBs!e2)bNN)N3E&)iUGl|To~%{5 zi5uF_gXyY#!PT$#c*%d{(Mrp5>^OHRYTcSnWqDU(i=k(*!<~Z<l_<i$$ZJ?v<^-&< z@W$)y_TeWJM92!$I|46h9UT530z8$Bk*oZCBJsPMDM`(zTsNGBx98@Nu^5V(G&>eb zu5rZk+o28+#s32zzeppll?84m7Qh`{4`yHB8Qf6$1LBf|$aKdMxHaty?!4QF<HRE2 zr;Q5L-2at#<GC^t=#633pmngVvk@Zq#o^b!gRti?hgx?D#C20Gk<VQtXijt@(UWAz z{J%V;r9G9T`JTd;PphHh{F$1-_Kf04X&ye5E{Tmdag<_@Be3}4e$f8@1#Sti28Awf z)cQgeb)9m=3S0<oP_UMKSCt_RT3pwhY&LI#?qs6s^axL_|H&L}y$!dzOmK+)e9~Rr z2V*~?z*3F#nrOU%B!?#4zgdlBZhpfY$oa|qI_*UGb|Y<|v`v*;Lsd|(S9z0Ly)#J0 z?{k8n!pTVP(oOg%dj^-@F2emMCPT2kHS|4SMzZvr(LtI00?(9U#&IvktAFI<Yx|Gk zq<1Cw-3op3^xIr~ra%LQeJJH%(<2~#Scexc^%TNBm*D)mHt1Lrh<Cl4h$eOwaC8mD zbXY813Ts;?qUVJJ_*ronyj%Jbiu3wv9{JB9+oMBy$IC|qz8g|;*wtMy_nHovRjQIp zyiJUQ!hBRA6^#2spJV%hKR78lh73%zL`SU-*2J0zk_Yj@@G?Z1*e<+@-<dB!L*ZFi zaheU$->JjBBE|54<}X59L>7_>Qqxg`qBDy3Pi2ftC*rH$7m(})ae_O(&CKR+GAOfu z2X1^$lfb)NH>Bkr+!+#LBRlXLi=KT8!}%XzW-kv%mF*@aHqD&9R)SOwGT8X$bo@l_ zH7w+wpu+oB%2|3d5p4BG73;r2^;>6@_xvrc<oB#a7xYg+gseEZyK)MW@ma<g^)5k& zzOTmT%L|}rq!T~=Q$z`E(nLv)d5|w1Om$R<;H&oHxSgEDK7WLfYyD)BaPA58dMP0D zS?Ty#-c2x_-3}pVngq#{TyV*m54io2J6hCH%sj0KLSpYFNbYY{^7c^y=HqFSu!&Pf z=5MaXBC~9ebC)i8v~(F^FMWeY*a<C|qe-@}VZhW_69Ob2!h^?txJB$M%+|b*6ZDtj zhB<+Fc;9LW1Xo;FRR&kSoS~9Ke^6IER}u9sKcHtxGIps7CVhhsVdv$&cyWjc85&F^ zogx8nhEe8|;HeGRDIo)Fx3yz2=|H^2s2Z=)m|N4op2nwEjpC<I7hyrt0i38dj8hKV zk(0v((B0gQ@2%^Ey@d1eFCGK${2jo#ZeaCL5l_dZAAjE^3lhsG;)IL0@q+b|C|$(@ z-1JtG!&*GDHOd6fSo)pHO8*It0bGSsm)3kF!|m+@Z4W`3t}|&c;|^~hlJFB{56JzU z&#VsPjyWr9@P;)8=wO*H2ZZD<;9MToE)alY<$W;jGok);%s?w2=0eV=&#>oe8;-ay zi!bkZg8hOd$@(Za#CPY`=k}qXqH2f(ZkvIrRUN$Ah9R6U7YV1nm*TugZV(OFz}R!H z&dnQZ@YcuT_{*Nn<n!efNOf%;vu2hZ46WadCuj2DnrJ2nt}n*w$NKQ{jdECrX#grk zkBGtzFpeI8TM`Kro?3@X=M+I{%^$pdLN~5D?ugD#JA!9U(g3CT)r_{*He~BnPVs#s z^hj#ZBiz}P0BpktDBC#(AET>bRpwPj+(rY-eC@$kyZZ5zoDTfw;w7jYutwn=bZpVi z8K^nt1y(n{i9^JWK&S#uHr_AC!!9S`v)FZP|4b5p&OC)X?kf-p)!q0)-y+n#Hy_5E z4Un;?FiLy77f%1E<ilBQnmK%WGIXc&(1hK+*gG(mdb#y0yq5k6<=?eQ_0VBR!7oAC zr5{d}BtaI6hPEFH&}JWpRg2=`-uqH~#IF@17swE4g-5t))+7=;F&bZ9avYv|_foq0 zvx#1yFB23ph!skffnLH6%C!FzjO(Z%CqI5YEaHYnrNUp(BDa9d{anNo;5}G%+W~lJ zeGb<r$&g_2n@|--5f^UCb@N<-|Cw^iyI3fe)jmjm{s|%{gUYF_g%gN$XB6rVHzenC z{9r-yE;7ZQLJOad;P2i+DC9{F$NsY*?&6VTw*DkEdg>vRjmdE7YU4co@9ShTe{L$3 z7$ip2<mYf{rFbawdIZ%wCQwEWxl~-tG!!g9f)zK)aG7j%axQoi{8cIg;qqbX-Skt; zF5dwFqZ~jo(YPin1=m-7fS1BmaNPU1Kr$wjbd(q&bB-nZGv5wfrS36X-?xC*P%qdQ zZ6^G?UzEt*cWbe8c?W*{x|#dXS>m$4KXFyeJ$y$w9C+83pxwfpnjZ9&mm_=`_l<YM z%y&xIz@iNghjI^ki-FiE336FD1iKvusHwPh^KZX8`F8#`Y;%Yu$*YvW;I}beoA*_) z=ED(KM5!VfLq*akIU6=vtMExQPl&ADt4J>SI#PigA4vRA40I9?=B9%uo_0%xBf@qv z4K|NKQ&bUV(YnZX|3--8#X#|XF|6uThE>3eNO(PjTXlM5_i|ZeQs7MNEFLhy8hq?M zwhT^5W->mrellmh0zm7H4m$Z}C6P8XB|B1#xlK1;mo#3VLk!y-@N->tT<Ivol!W}k z%iUKXS<@^ya99<~ce%sJ=XUD;7>&nHq~j%jn(>mz31q6mSAo|$Ib3E_2jPiRNM<^R zTbwflT4!6SUxlj7`b9#7>nMSaC7p1~;x(gI83!b75w&1(4ZPf@gj#EF3hwiNj8c!M z++}W9w_tg8I=Xg!5Bj)HnYehg!cE6>%%Q2(kPto}Jr~F_)Dv+utNA1)*y(|f?e2up z!6dl<T85ZjnT~#}8plgB{24l=m5K|j0m<(-;Na7VgbdFhp4@P9aN85SyE23NSg21* zt6Y)QH!0+VZv+2}m<89%Q-RlQtAc}6Da<;ulURLKB4rs>(3rOkg<UxZAvf1k1Dnq> zpD$<PhOJ#VnL{t`XjxCb71l9BTV!FgXbBwPU9ic~=HNFrz2GfWfh+FLMo;ghW399# zJo{MywSW@=yYE`_uJ&iay2}g<$z5dxTRix%`=u(OB_lyZLKQlclPKTSr{GHc8$qPH zCrV2#2eDH^<l#RjJhnIr0(xh22BcH4(^?cMny29J!a}g)fIga7w4M6?Pzn_@^~?qR z3xc9M3`AAwAZC#oQj8KN1ymAtwJc_C>iVDqBOh?Si8MHId7zXr#s2@i_7NSgM*pvr zJ2a0!jT_mOZQLRAml<;G`~y!vH-S~m2?*a5Nm;(*_QQkAaQT@aW`}JVRZR{t>_KtV z6>$dM>Ho&N<L*EJ&H^E|`&iaJ5%-Wb%FFjN<*GiH_~vSW)WT0pV~P}@h-3I$T^EN; zn~ycPw(#vWIn@7@Wvzn>!Uxli4BPUTC-+nPW$(lERa)3*+X8&XKpM%1Ehe**zVa6O zAIHk;uHYW0F(&oO2+r*~ijx=p#9wE;fsI=yq3K)Ykox%$h%V#s{360g>>Z7JeuTs5 zi|3FmH=7*FJOOf!qm1fSS+wVh73_ZN0h3%laA2}y*uAh8zUjO1LGur{;cr$#!+zXn zgL=yQSGgEz954qQv=y7mKE^&~nbyLyQgGv06IA?{vvQDs_}wmDkSLo&EXJmh;YM4u z_`5stztv>(ZD67x*M1)3BYGKf<temUT80SO3!^1l9LdLPs+__360Uo*n1qRO(3RU? z@LF|lESD?z1@ibXsn^vapE$+wuKY8Yyt|p>eFotpp5@H$HI3NPJ&c&=xl!LN+%YF7 zk_=BJxP8!&m|qMdj!s8#1-*fY(}<*ntc6+Ab@7@d0xUfgjSqU<hbk@;wk^aKUrT!g z4(xZzb>boThrZ&n9rMuy{(cd{>yboFRq<d~GLCgsSUg^<OSV)ZoN~+`o%rg8>YsGr z<`8M(yibu#-<T@UxicGSS=RGhM0hwfv=pZoSHW~k1@cbA3Mu>u!eMIB;8Su6Oh4FD z>ct<R@f=MVm&U-F$(S)QnFQ(Ax+yu1u7AEs8Th?#3#n5#7@p<+_i#RM9}+h^g+t?9 z(V)g&Fu&$O8C^L=JzuCyY77|a&+-viy2zg_dR@$z#JeIh`Z4yJ`5Bx`FT#s|dL&6z z3ZfQW2iKe2*!IyJJF35h_ErUap@w@K=GhaIzr*lLDiQ6g>xQ3(pCNC@aXv6J%~9i? zuhg><Ytodu741+x2VP+<Sm~f9>3kW2;<Y77Uh7+MRLic8PRfE}9Ef85^uRZ8A~N^* z3}xYWDVG*MG)X~%d@^z)?GLTU*=x7)t;O6aTYD>WfIkZNCEsB4hkX#zIUS8ttFiF* znS#lRLF8{MzniPcnS*POQcRN7DE8)^!B$bRV9(4WYsVBxsN8HERUS(vUa=thV?DUD zYZSZbTA~7OJ{;L{3tJLC+*tkt4&4voU})Uzzv2@vs<9%j&Wo5!AB;JwmpUUHR*tux z(Lu-A72tJ45i|B)`2Y3w=3zB`Z`^<LJkM#?B!x!N*=y}nX%ZR?rGY{yg$$9QQ;JA5 zY7`|Il38ik>)w?J8PcFoDVfQfdHC(VpX++A-#^du-(J@~Yn|1Z+WWrW@7H_%HVZud z=z9_;xHLlQ@3>(9{{85LRV1Q}Mc`C~EBJQIKt4IiFu!wz?9*RM=9#h$2j!&ktkd3j zq{R=Vmd?Zd{eMa93=C;q7(~7=#NYRY;XUu`c>~q*aAa*gnO#oM^OFzI*}0rPq$mr` z%l5@(t%FE;{Qz^ULWoSiB#s67-S*I;BM)8wR^icd9~7(g2(?z9L3{4sCAxKTXm9fs zWWs+(hWw>K$I%vVDkx`|!rjD5yOY<vB@V9d<<hjG9ud>3YIIli95UIjgL%W(sK=KK z@MnKBxWCeaNOfwG-=`K~*GLMhrIn)+k4TcN^O4M*ZY03E+SkaAgGIzbWjtn&FC>@U zozXsvD#BXzlcVK)d?hrN#0cbY;>0>6%lSy&Wtn2tqX)<@)kY*)ss#Hc`;vx5OQ6(D z9Jl<C0|9j#>FgR}%;SzC&*d0785~2)SY41wwIbuQ+R#O-%OulP4k<=GC(jvy5`4HN z%ZuN241LXMApiO#aCe9l)VBI#e=c@Ul8Pr&jkfc=G+8qJu`c$uZAB8gEb>`aO-ePx z;jyy>h{PHbUtTFkKxjh|`kJ79_#My8TMLRGcB7%?Ty$o}1M)RCm82+I;LOM!`0wcm zr$-M>$e&*pSXNNkL(R2HBk$fzVXN9gl5T>L?Y2lHc5FHhDT*L^qSAQFDQA3ViaI|3 z@fjB_m`&6^F;4Ogf2a+<=1|m9K{n6)I=#Y60S9;Fl12xA2$qOIzQ!V0dZ_}5aWjE` zHrwjio1w_#-)F|T%M-p981RH)l!z=mjs!n8Ps107l)=`n9zDtxL6`XlseP$@?DOIh z3F&`Dh8*gMMDbH1)^ZzljVvO1e;%XZhw{|MIj2d_+|!J@q9P-G_Zzt<98OF!&ycx+ z_JpL=pdw=vQuwcrY>K$R_$U;TteXzV-R5q?{B=i}m@Xmw#g{8~Zbky)$oocg4mqH4 zTy)iCh7&Rl%94cN6CkYbH*>YY7S`%rL_r$5ur2&O<+yP=NZ&Jp-7j`iFM>6Q`Qt03 z5q(5HLW)?aR}A<+Jzy=@X`bC;jEA-MkdD`zx!(aj>ao!$v`Bt3mfJ4{1x<5M&ff9( zU9=lzIUrENZx8-J!kL=TWxoscud=|Zvfr7FKQwW}Q(H3cPd8a}K?anoI6>sTbW&$h zivFU9Bu(QrTG!Z0$){Z)C!S8kcB(>n<1`cO8O#IEmNe?_Zwc~pX9v1o`;huGU=D5L z1_;HGT5?TKqM)lPc<FW((3fY3slZmin}5Ox#u=JH*E)6lWx6`v@G5~!s$5SwQnC;? za~5%U`4^eK9*0+D+&~xW96@T<BI@SF-Q?2@V-UR{0tr?j<ohi<Si8|3#a}VPJN#Lc z_(zAJx5d;>BU>U^xdOS>g)$p^Z<4P^EO5ob`6wpU6bBwV)gV|roQDpCYLl=oDHu|c z#?e*gxZ2?lIg}?wGFv{A%JY6y%CUot)xs`vYMwk!zcvnbS_shi@22?Ksub#eUk};F z+3ls;^{|TPFcZ~kM0v>S!U0ETtTxUP?^V};b@Mi(&=`3Xbx4*(5?M4DN=UqU5EVIa zQGisM<#8L&f^2y94Q2LyA~!Z^QDqknkyXiF=)v*Rl+@lhv_bPLO5OSn?LVtP$kW%P zRy_%c?6!i@BL=ua{vz3G6vBKBKZIT<slc?tsp!VI7L@l}pQB|6h_u#kG^bw}pY(Z7 z?(a~Al$oE&A{ABmotELW;FVyN)7Gm7;L~~v36CCO79^#TC6d`>kEI=a3Tq*;4l0ma zqyrU_YFKZk4K%kUQ373w27y*Bx^qqgekar-cFQ})DqRnzdI&*cb{85K-9e6vPQt2; z8#$`3jdM(dsDNc0%zjW0pSlm^v#uC<!Tv+?!`3iI@J$gFX!)Sthu5fUxnfwWe>vuD z`$_7Tn~_-e-(-vOAjxZck46a(FX2h!`O$l*G9^vewj~2ukNid<@05x6s1k8~=86aA zo+3Y0tdR6bAa3iY&|OJ)e7fg3bH;OkTy#xDvgPsQu5B2VZ%$BKeGzG{*2aR@UIAp< zy!+%cr_Ha;OeQaLqR4@(xn%a*WMsuVLt@*s$cT^{_3UK@`LSg)nY{Zl+CATk9BOmo zV%nn-zwZ<&YqjJc76kcB<TM!l@x<UE2S-vq!)VqmCEf)dX!ep_r1r^Q)O=VI_pq1A zsa923c}Ex5dkhMkkhCFQ@a#M?iQY(^6t+MK?S9PJH-DJtT%gB0nT3?LYz7%rdP>X= z+T*1j*Gb#2GOlDl9-AcxAg?eTT)(fMsDH^u3d{Ql5iX|0pAL{q4_Q)h><*drtC<*; zsS(}Cx8$FDE-Kyri6~!sP2Se<aNv!8M(7h~6!$E6iC)^@A~Rm=;CoC>ePP%HoO~bv zKcg*i3x|DWp73GpS29uT5;Kyc1k5H1YnyYv)5WDoAhw1q9MT4tzG6z-O$R1Tn#1c# z;@~3OHMV90C-9UN65`ALPXIab&jJC7c0#+2ZNT;7U2^!}LUd=B0;6{80(U#2i>FSs zq?AY3a-g8y%!{kP$cL_{Nce^VNe*o$otF1e`6&t?DfmlF=5Z+?!xQk^{RgO%WeJFR z(}r?qRihu{pZu>Ut2d!Z<Nw~1mCup;YcEjFZ$}ffQ;5xcN#b$KoQy&MiH-EYr{`*t zYj0%0c;8!;{X`$9IyNx(AM1k0g>a;$s=|(PFH!cN8|=V}PEelo5^Q}FIquF)SaT){ zr`}Vi`fbkOE21Ol+r_15eb@84{U$2-r+P7!`tS}jTldrSMFH-3>l9xBjtujG?p771 z{B#eG|JD(o9Q1;`(PHwlB9v+#w-0UHlgWG@^u)^J_b^}lcav}hU6Ltji+3svqKxMf z_z>Agui|Y*agFBSzv2-6(zY9_rbzJr>g=Ky(k;w+P85uUF5tf^i8xB{F;<S73|d@W z|DR$Z6RarE!LtPoSYv-0+;ikU*4ZUEd5bVwyl*Ei4EDmqJ)E?D3CGK}b3lnI9n86C zOJ3NI`$);^IJ4GuF+99;o47eWMmvfViN5X+^7*P3j@YBYnx!y!!QD)H$s^~+sqJ^! zC^a|yZiLUGaC4lVB1_M&bb(IwB$z7DEx`(rw@AiFHr%M#k1$7$|DY_4Mtdee*@{hE zQp8`F<}OaZ_w3=RFOGq)@AToUX;y=0+7hzaBG>8EpOvJmN0Ah)x{KDHs;732xuGwt zL?2y5K$iZ60wZ5>M#6L)$)zj@1;2$ZB+b`qli}x7-Uqv@n;5}}SQ7c>3uvQCBV=W& zVWOV_YTd_C^|esySpH|^vYaBn*B(V`7f<rSyLtG?tozg&w=?A0Ne`?sv5697PHw0g z6a`z?MygmO6+QZD2~RH^MBcnMq~&rIRX+^}kw<Q5B-s_$b(o++o&<PiC~}dC6Bv=g zqXMLM<O;o*!(%ON?W5P}2f(YsCGhZl3Tc0-NSj92K&w$PJTA#XTWW0~o@b69^d{o! z1>O)fIS}lRenbgAC&B7w88P@^h+6Bi!3x!4;RUid&RCUK7cPQ)-tTu(6ly`s6oo<A zz7x%sIEL;jIpIJ0qiBDU;5!Ko-%o9|?}9%)1#G~|qp)iFPQGS+5nNjQ27iqmMFX`m z?3M-BC>7ZjyspoPT_O;p+f5SiS%a-`abh4k=)D=E^_8UDrwA_AC*k3(Gf8oFe7&ny z8R{#qg0pTcHa}s_iG9sMSD*`Dj2o%hn^urh?@Q2cfu<VyM_!<sKxZ5^r4g0odJ`)y zA5}tP0u^_17F}BIOXqI1WkrMsSpViS_I$u0u;lH+#hTXinsM%2c(?-_-!D(i>n#H~ z{tPq(QP}^a1H=^`gM!R|Xx54Ka3@HY&9j}vIj7?wWLgd3jrhWu;VF==Hi`t12SRbm znVF!ZbP+unzC;chcabRvuT#5nrV+(XP3)rVPU-v0l2DUywoUyK=-2OL`z3>E?^;<f zpP9}27EfmnAIKyp)@H)X#!tlfjU_HTR067WD(-ixY*?>UgAUpL;i7K1G!uzx5<jO5 zAF)@*ciP%uYIv~#uiU8(voyxzkG0Q`@LEl>X>dILoHia0kru*RG8vcVy(Tx-o+I@R zd8qN40oN4LLQXkq%%G<y6e!)HV{Tj0{juJSb68;6HBuz|YzKHKt)N$U6@Y&FY<OE& z3@tO4;6+yNU`eeA`T9ka?Q>W|CiqNXud50)X%V@6u**?k4<Gu7vr~m(LSs0kuT=_u zYu2KjwT)!`qk58Eb(Z?Q=`B)TIEYd^CWF)`7yOecqZS=91)m^G<SD0ylI=!tZ`>jJ z8}A>_S4x8&cd!F?4sW2Q*DKKV@21mzC2QgFQCp~wJ%*z1$<p!5ZeVf2B`bQd<$1E= z@I&wx)5MeB{=@%pD{2$<B{G?9xT3<9d}^|ylWN4-kek!s^Q;Oo<DV06k>@lt(4LOO zjt(>PMk-1Et1NUV&=QO&J=D3ZufhCkEs5$`fL}=7gUz2iL2s!uKOvsrMXRr{m!6)+ zewQB+d!x@tkXzsZDcR4^P-{N!YAz;MkN-dm<{qO$l@3tLe59~h;BjJLl1Nn@lETu5 z#Yo443i54gEY8W1AV%6dnCH3m$Z6S4Wb(EXWvDfhtG?1?@}z9yw4;|KiOC_qwnON{ z>#xMVFP+RkH;JCUU50gw4ud3I#||y!i06qld-1>dJbHCxIvMKU0SY<A=&m`ynwkoT zA9+KD!|pMM_ueJBn2+ln8I*NpHnbo4N^-{q;|&kZk$<HQ*4lWGnA%6;b4#u<J49_b z(WWq-JFTiA)2;)>&5a}LGe^h(PnE22;I2Mqt0C{n$*6bZRn&Aq@C5I?Hyb|1t;Z3A zO?0_u0~@Dl$Zn8R#KD}9z5uT#UOMiu^}afK5y{~04nEFu&El+derTVi7+xl7j$iM1 z!E`*^j3+<U#oaz`u&7%d{SqEW4Tm)|&51{tjO)H+=Cpd=*02yFJARbuP+I`fa(ZCq zrU(rJt|k3?bz_5zY%j65C`Cf)8Y~sOmc@hKbl49LZ@0CIedfHDCVWNq?~z<M`%9Dl zf|Tgvug*gM!3lUKK8AbCW&qh-f@$aF9K3QEbu~YN3uQ0x=LHRrQsIJMe^W<?ckiRD z*NKtyAI)+6`)z1T{3YtC&Ob)bv*`)aY|CZ>riGAo=f;C_=VjjG`EIE1_8e@RZUKjQ znzVAk0ruirir$}M&0l6cLXUegif3v+<vLlB>}%x_B$TEMbARlmi|Tdw$3C^-H#wit zb2)cB`Is}U;Z%hCtHszpB~iK|Hwo`5_zBG3IdryvFG@)r65usI&!JjlW#IpaBgkqL z*@fD3&W}1$V|t2GJfr}WN*j{O_Cv2VjbMSFJbq#j$IhF*fOS0L!p_tCN$z=!XK#** z(04dmzTz!g_QG8);^M~z3hpgo^}~12a_aFo&QPD#__hywpUuUJ_Knc_#T>;Mr(qRm z!86d)K1xJ_rE%wW#NNtj!Jc!4u#IFq>FJ%0->lbx1E>+Xz6>Q6!F}lWtTl}K<X&Xn zeVG_8eov}7X-kXx4>X}vhn|uh&VDBpzgBm0quBe?Fpb$s$*t<cPr_~xygmY1X4tR> zCuM2JUL|;_nMcMe@ZgN#@kw+ln!wh-zPQLZ94gieL)rcUtT3?*`ES&Nc;lC3N3RN& zTbPbE{~Sc)cCKl7yL$xXRJ!8jr{r<yaUD2UagOX>qJkcEedgV0v0`A^I~YiPgjJaR zw0-+JzQ!dXe!fL6W$-->OdOYEklBIf4hLZA_8B0Mugk_(H`>Y4_&p?Tb`#0JqJWaa zACU3~K~%)!0Af?~m;8%p#Tkp$ac=%9<R*53q^zms*_G`<3(Y!t`PXlegyHGf`CBDA ze6gLpYqZAa+RqWCiBiNf+YWT?`%zE8PWY)B!%p)X#r(;O`5!|kH5$aGv0?Aub4;a^ zTI{9x8TjJSO1w_^B**HUk3-*-<K91#@VS-~_Bidp9`F+dPPjqL?7i@Ev$b_QZVr*@ z$u+pvs1y>}nK<N3A1Qja5L}LJfOV&Sp^N*KK_kcnMd@%dibJ;=Y|dqq9KjuA^y4J; zWW{K`dc8eaC57n5G(kQqzw#M-PuYi-(RzWR_tQ>n)=au6@he1>*yG;#5ytAhJ{~uy z0sH*R@!LvEb|Cg4`kb!JE*BogKW@mw!J<qE9E_!V#+l;gXO%c;ogo|cTLMl=OatjT zmr+5&SL*4CH0E#B1gz&TgJWwKgI$3Q8hIrJ1_F0otn=?DiK?l<dxfq6xLA?~qaJ=# z3Xk^p3`O@Xd&uDp_oz$LC9tx`M&h>e1SPGhMz=gWgy#`stUvh|>G5htMQc2;!??fX zWb!n8^Svg%|1AUE`n4NZ9Jo(SC_Twgg&cz|+X3sWk|%!m;_H9RTLzYOnpjZSOXH@m zZ^^O*H7I_eNp;;xM)hh=IM61BR?*(g9)1~!Ek2|-4!h^@^{4c*A}$B%Nk`>ii=-+( z@@*!lJjla~l|<RVOTPG{*kPP__)Ej9#32;6(Txu3`VBqJw&;pyGwz83a`g9c)R9+$ zbyCiN*5?J-X`icrH_>PSZ3wAH#)UL#5$1zhwF|tCHbv3e1IWvEB8-+bF>WiY5n2#U z_k~ZSO*XQ)WKfRpdpd|-B<f3N2`!>e1}YO>(Ny>o97#@bF*0jf({R`Nqi}6OI4=Kp z6y9!}Mb;;u!j2-3(2N<nAnME|py((dr$>TBu<QSaGEc1_ihg?3TIU8N6xu|_8&5~3 zGd)n??Hee~ITXn(Ily}@IvWc;OGII>`-sZ}OFZty8Sq~^2me0HC8TuAG*-Y44!tYO zpI^0w^{bzcdW>~AGMPMmuT_qA;Bp?~Uvb`Uk$iG%j~%bQB9*5Xwi8i;7<&>EVgXu5 z{9wDuK4dWzf~PGXARiBIK-A4zH2l*P`JX7D?sEA#OKA-}Yv2`1-8G3aERh6t{dQDT zQphxNsiHS>Y{<#2hAj2Xg8kjsj-~%rvF4hkjnQRW$%ozFVD)GU)M|1>90hC8KO)9{ zsXhbK$9+Qrqno>6N~j{fB@>McitS<Bzif&>6oVUc3Sj<+O~{%o#3Kum;r#Q1D81H~ ztoSO9CqMZ_HC&`|&FX&iB0L&@k>t3W3VF=Suc4H%UJCU>!W@o$>mlC{uEmoVcXIWX ziL8}HY2%NbGtfy*5&k=sMx1Z{h?7mK3D}>?>h$tn1sIgx4{!Pb4A5RIvFS5tygvnR z*!RSb4#rb2W<zxMO4M~P2j}K+@uu74;lj`7XwA?dRlRBlwVTssDQYRe?S;pgC1;ed zdxk&xGjfQyOtpuqIXz^YbwBTH%m$J+eiGZ97faJk)+`mD!mbJv%xO$fDP%*o1+nY6 z%cH;1lh|)11q~|=pTd**i9~+33V4tEOkyVb!=meZLHeB-J6hg`8>JS*(cQyv@}M*N z<tTvMftL^{Zi%(Lvxvs;`^b6~zv1?qxm5DhWYpB<jcT_U;;jBUV#c|^&R+RM<qa)n zY7{k)n85Wm<XT>UNUailY_~bTt8<ujkUv5@fIMsRTn?r`(?(}qHDG<045{#J0ZGpv z=;=x+G!&i2d>MJ<q}I9!9`0~Mr4x#gkDEHj^-U*jlc$o}&$F0ay-kECApuiva_SMW zx1{6TLnhMXFmb=pkB+^3hk~cSCxV8}-Nd461a<cwA<ZvJ&{+?Ta6$#q)#r@y(wUC@ z(Jjq5Z&xs#pBoLDX>&2(VHcKCmBhcfeD)jNn)C;|t3<y20;z$nh8X3=@X;g@9~?T5 zl5Y7xl)-Lj{N{q^nP)-E(I?cd`DySZpo56&^-w+eoF2VHQGl)d_b|!3)v=$Y8|7u= zg&%Xk7+L2QUPYN2{^Qs{ePmOR+4@4-Be;cbn^D(jY~M}$W)I<axtB?0lp+19C5;~E zw-U~*{D?JYWA+#S660v1MbEjF#fAo}!>^-i(4n!OwpywPLf+lPM8y~v)EU6fPzSoJ zTS<W(E6~X3pA`E)Jz4$yS&G8;g6K2Sgcxk(-MwjwS4dkh`&WsOnkfsY6~jU}akVDt z{BRaaCAZ_plcLG3`HE=J{xs5El7U`ktifd)bci4!oIE@I4w>(*B55`fv}yY##>6lT z8OL6u0=hlxzg$p2)+-~C<uYrM>pDyo+q6^a3k?3ZpVq_)!YO%5!hFG*HHh$=wn19L zSv0F8i;}9zN1vNjsKy&j;MXgGQ_IfbOV=i`Z<{9~$tnM!Yh)+P*^!J_T-{CG+hd5v zJt+m>JDi?cYAUG{9LGtKEhsg<kAz#&)YT(bF|J$(+Dql|x^Q`DlFpz4`;+kPr=G-Y z=V`odNw)x=b#SiRt}3)}GlsBb60qsIKGnXLD|0++$J5keaq&M>JnaU?8lk(m@1!Z< zlR3<hImP%(o-BLmq#8RQz5#cu)e!tknmzdGI<`OZg={itCdHQxu|oJ`I68A3T=VQ9 z(;~;SAH#~EIOGb7^bEr3O%<TjB8X>@%|SFt>Lk=g=7DC@C&uVyBP06G6w_iCsd#oP z(=<Dcn5jy#TOyQM_uvZX2sD5Pg+{RGTnkK+^{4wbA14ykSy(>Gi^@pv<CR2q!Pc-U zyl;*l-a20hbQiuvQ9<wVXI)qPMfN5fMl$gB>K$}?P7bsS?gT^K4`uM@!uO`#K8)2> zE!g~P^NF2@6+1EvB(K+>2~u5zWZI;0?P-pm<ZXrL)LbSfzHG!l4%?Do`6eX(#|9R) zUSiz$ig8G+r69AYfZFGzj1mVj@WKOfu)e(vrmYEQWfmIXE1Ke1v$p{vS`Ts?5uye0 zLUuSbb~@{~SA@(N=3b_qh4}j^cU)6{fjD)G(Js~I=v;yqT)p!c<o2C`u$FIR>6Q@? zeRlw_EQx@e!G+*Za1*!iD$$+2`nYH6WMooq#IEqUh7yz>k!yYTd5cB4%VPycoDek~ zuD%GxvZ7k-qnlBfEk7=R`I~gG-khiCeNj7l@cAY4Wx5d@n&3_^mA1eq>$&Xf2w^DU z&VO<=0XE<jy36_^E6NLNN6ba+-%gmx-S{i5lY;|&ugUyV2p(6Nvzs9T(Hy|LdVDft zRPvPhv@-&&^w~j{XI}t8=@sO)eJwWdvStUq3Vx8u$_HUUc^X{*iSYO^52VULUHpog z!5}st&yNy<jKd5zyy}2=TeuNT6&I-BDvtu|)sXKoh(-#=Q$^9gQKV5P&O9-TuwGa3 zmHZ*xJoJq4Us^+yOB;T$KL?M0l?EBRZ=pT)()1UTOVrZX{h(%A&Bf40?15I<Qrr<> z0`i6-IEEU}>J$V(!AWauwxkth>ngKPw`Acy>E9&IT#w#8Sq}eNz{4^=w_w+_MQFNq zC%k?*6|M(=C8v4%P}}i`$r&godRsk62B#p)>KSG{g`3f~Kp%V};5d3a+(%|8-bO9S zvYe6M0tC&jQEX?mEzX)(iH^Svf%RuKP^Pst{pP`TSQ~O2Jlf09pAQI+&eO$Q&>0Tt zljO2GG>EqMDY7dz89(fm$C*XdQ1?t4cAZ#&hSs0u0vPMbv2(G^-pRT6lvpd4nr6Z3 z=p7(IM*6Igr3F<yFNvHyasch!dR>6_%{U3sFTNwA;@`woPJplYbu*J}TF}2`{;=yx zDHV_&3U<$DLHoDmjDx=z_V`nXy=F>6%nEA=c{7um)s=y=Z*oms<D>XG=Rf@@`kCCc z7Xk;7f1svP0nRFK;r&;xh^?On7dL#xYmBS$it@j8Cr+D^m3{&YznvyPHL^gBTCd@Z zNi3G`T?`37>+$CMg}CttpH^C71M&P9=uF%hP-)V`yYJ@W)OUZGpJ_K>y_+nlAIiix z_HM;rznlhks{lXBeS-pe0>M-03aWV~&qPd=#h(rH7_ac3_?(tL&7Vu-T*-sPkf(te z0bU4cWu{nD?<>jJbQ}golF6XO6}(_&eS>J&C_Ydof(Od<cr$1CvJo;TQ2X$H=DRDO z`FXgTtIL|hs`_0hr^UIU(r6*M?^KRPb#v?I=4w#N_63CJIK=eNUWRlm#HhI*<qd($ z6nWu%HL}McjJne_z^v$uAcE5_|InM$VZ>ml0PXehL#~gm^U7CVM3tQ)V8T;|GvCdb z-ru4~o72yQEt6x!{f?umGtRt88Aph?Wf=45#suc_xO<GNvu*vZ4f`1Fmme9aLKP%F zwH6snT~2gWZ=g4N)hJc!CfT`eZ38;%jU<omBqiS0sZOs60wy=20u=>o!BvkH#M7~! ziaX8qF!{aY1}lRUVvds4Yj%=v9y?JSZ-7L;*JNhA3ZSe5lN&@sK9Z*f=aKb76Jo*M zLRy2yXvN8csIJd|)V<^`txZxPlcbSS6+3b^&IGq@E+zAjGG5Xp36B%>Df3}7DC!hM z;PD+<^%4qSsF7@Ayj}DfWoT7H^sXo{9V53Hg=v;lN39t2OcmvV)@_g-OOeNdEOL-5 zD%fs0OU+EqBX3Q|Qyx#Zpp@Xr4NHc6n9$=lQLI!2**gC)$vD?dt_vBG9tU@7_k7)k z9ZvU&d(aCsZM!Zfn0kX01jUZ9MM#Ec|D>D@dd+Q!4Gg7bx2$Q<7f&YMj-H5bypejZ zb`71cenE9#)rWN=GT8B32Jhc4Nu+T81|!c~iw?i|+7O1?8PTJY2z{ZF_zGU2s6G9Z zBRNdE`%2LD9#NFq&GPoCX(Rc$*T}sWrwGqE8x6ebM1s0cDgd7h$kvUeNK(iZe{Wfa zI@Gu(+mj)tt|$qT-u*)}AMD1@Z7|H0t7As4v{RMo7tqbe=Q-_LA@whoP;%whD7VUC z)L3>JewtgN+&wbb$$h&MStQ9@<93|5o!&~Ev+VI~#}0Ji>_wz8AOWAOeXxXSf&gh9 zDMyy6t>ogG{`%KSg=DUI5lI^=A+xh~QK}JpnN{PZ(0i9uDt>%AN<Q}qIq|cpg-=4! zj&w0psb+&ZPUP_18_zSab{$%=d^@9fE}0WsoktJ#RA9oME=u6Gg8WImgzlc*Pdy9# zNFHAoCW%+dsVfJf$N<M1MrFH%VA9@?$Zg&#M#Z{|T-h*);)hDf@wZRO=-O>W?V2j- zFbPE=*566SSqdrY&7}(0M<K5h<CwBH17yj&y-sq+qL8e#J65IcGP7qD5aVPQ5@;?C z8-?O|kCuH$=NigUf{QJxsBxo!2Gr3YijSUG=R{cxIO9}I1KH%#K*j}1K)TjA>YXyD zlNdaWHvG*Z0e6bf1kOcr!CVhlS1C1&JPsz$Qhi8@=X^4kQxkl<Qb|sD&j5uoFJd3| z89m)6L{f~tpzf*biE6YaCGL6<UHa8TWM2uB@}5dma7+?)ellt3<}f4L2ErhCFg=S( zFh53yH@0yueQ&gNwj4$LZjd=w&r+3(MIc(O8EM`Qr!ubPpb$!noZ#47Ye5wxch@#N ziL0SD|N246Z7*a@`|^<SxOTLfQA8R+TbaZYYD`z&0<vgl5H(dd1>FvONve&GQzP;( z8V=tsLR&^!NxFZK0P%H9iJ!O>o@)4lvVX(LiuanJ$J^T}$NK?J!=Zdil>6@Ac5)!} zi^EJ)Wi`4OpF{HR+(dSLNvKAUg_dh9riS*cA``1`ppL)79Fg)p6S8GKNk7!b+|P6* zoADkjxXveE%MYVYv0>8eYlkwH@tC|@rx>%yWedRN%MQ}>l8<*p%_Fu$6=*fx%#$&T zVKPKyVg5@GytLXLQe%=yifRv9cX&4$$QvZ1!)M4VHk!9eJqT-hrxQ!AmY3k`h^bjJ z_zOMrf5OL{t(sr{Pg7Puze-0~O>n*WF0ZUC4%Qu)AouN#Q|(62kbO!V+$_G+FndK7 zm}s7(?2<Vac8>`zG^62uPCO%1&PNlJ_#{Kfg4C}0&d4SOqr^2o8g8%2KrLpM$%^wr z%$1aI(q>onzpm^RvqJv=qbr*~OP9O(2ot}D>`^V%ZGNet=%P0+@LSJ>&mAOsT}Q|@ z->*zl_%#%vBtaTi_9B<w&3NTIW7@<b8Jad2u_gY7jcZ=XHs=5IVk3KQVSO7Bwiwq! zQ+FQ3t=<jM0eMJleIa~wJqRkVH$!ra8SJ+<=30H?I3-@1tG=rVaEO99x%)2+f8Vr& z)SasbXKyonZ1X!RINOgfXM||OgbA>%VKS0>rhwVVS~PO%0h9BA(|1>;Gd@m-$r8AY z%}?j!N#P6Whd)$Ndrl+czA*_6EU3f>KTTlQe)&j6ZI)&o0+OM!VU)GG+y-SAC%}$X zXOY!uK`v|S)(R9y?YVJ5n@zM*VWZY2qDLKPu-1dA=ru9L&xg8!smmftm4j$nj}Be2 z&4HY&BMmA2r>GtB_EhGFY~GAN&dly~+G#wTKpyY82K1j08o4iG%Wvk<r_KEExLbwn zGyPX&)EUuNy|cjA#unGQ)iW^ydINcNsu_5JW*``Mfk^b3;E>e}6_&GyRIaXN>ZjK> zh#g5}hI1#9dwb5o;WP#vaIvTV*!@MFHqux*G>YCNt&8(yR-$bWb>VjUciss`iGAgD zlOb~UX!WfR5S;b|l=s{MefQh=pSEJ--%BUhUwaDaU8a>Fkc)o<KfXH99SIZYi^Y%O zOE32xy<1AJ30z4}T{9EwU6Uh~uIB75A!F9XI0+`d^<&cq-0_uT?I^x$7oNCLg7$us zLp)r{(3KP`@cJvyKGUmjcw(y$2?^`TwTb{-l^2Tpx}#Bhv;{<otbvlW7IMC>4>!-! z6tFg%y=X4@78x0ikzcrUW9jvD?aMS+{N56qwoYVg{r0n7nKt;_K?~TtCyhup&1dDj zY2+;)40)cD*?I3<;6p(ky0hvFcOlHc?za>1!mU&3kl7lfLx>|!1Rue<66Z<$QWuct zX_F>nR}z!58SSZ+z_BHQ5$5y<b2#a^n$`a)&Px1gW;OL>8sit^#{Tc>jg#kkvhF5J zSkl1<TLnJrbVRkWf14-0GWjQtuKmTE|7{L)WZQcB!JeD2BjGT*9b`qft50E-m!HFn zh77^oJe|_7(ZDW(R4lEokKZigK~}0Wd(*Rml$X^b4}qdMH0W`dsZw`bwm6l1-&ew1 zf5o80_jPdY#YDO?O__FDq{dd%NHprk?8W}|A*{G-5^a`R#bDVRc%q<$bvbFk`tQ|a z!xJ@F=o98{l7-<aM-~cRX#!hz^g(Z=Aw1j^k6G~ya0yDsUW=~5>+(IQ%2JOj%g!7m zrz!_XW1j#UemKmWvN0kv*Diq@-nK~X{A2VZKaUX~*vP%C^XaSy5xA#Fl-}_%58n3p zH`<8qqrX@;V=IZj<f!Ux{BW5pOrC5+hhrbM)yEZfc_`4ls#a`xlp~!U?In_G3Fzz{ z?yEP+01Cpn=2q;W0A3%f!Oc?)aP8MrdPz8^6ke;3C66V*y|x?3%VjdYIIEO|`-L%g z8)mY1S_8;G`==zhJA+nZ%Te5QGg>bo6Q8-Bz}E;k%R1()!*A_M$=A!nICKf-N(Uae z=0_7Jhkggg<4Zx{{T)lEZN&2m=i$-FFuX=juo4c97{GVQ4fySki@fb>!}v^xD|<`k z57YZf2{w8LkT;w=I6~$w_57P93X-iOT>-7=$0Hx&d*=xm-1`BozL(|nMZ%Uu_a(7A zb0gtFF$X3tNyEttCbGp-_R!OpJZEb^OhK^J3JzcKp!05|vE$tJ>19tlvAaNL8B7ui z2LGr6JkoW6x8Ynhd8n0zUd&g8k@Y^r=0!C&n&1Q@j1+EqU=49EL-E7p4Gr&l?&0UX zuhG2K{?yft3dE#V9;T0fhdfre;<cYokPH5^aL>X$Z19@j^vYH_Slk~>7NpeSzOR$$ z=RI+(|Jx(<=glXvp!rQZc{3`{CdBl!g8>WJ){0Mzw800w#WtD5E?j`aol8+sQWosB zo<-ZdPK1eT7qPuw2JGrppE$qW7S?nB3r4nA6)hc<VB4dWkV%LTBt(88L2H#^Nstpg zm!HC{X?cT=e_Dl))?3hZYnrf{ni(9Z&tOfI9|>4>;c0B=L_nI&pHR#VnR?e}-$AEH zh5i%4DIFd^fT|i5EEJ|lU(QTHEuTDC`^(mx0i+#IzC4K)Q$GaplfJ;>HwJVyNB6AA zx5IBrW$*`%p>pi{5hQIfN+Q{Q6q`$-MP}=nE7j@{kz<X^jy0nPNs^$kSrCjLiaKF! z#TF1x7;ZdgVC!@ySCpT1U4owWd=E~VcN3=l4W@U0J%wFdli2gWBw&|JBeGd@m<*Zj zhevOIk)|giFsD_LUEp+y(L+(xMmImy9bW+1XKL{f{~8jnI*am)fP9scp!@c2N4Z10 z8Ohf<%#X4lY$Z@urZucg;n*)#*kru}wOM+yHK%`oNcu7mTrs7U|9(Tt8_VdVax2#L zb}{R&b{~}2TeE%CUY1B_!Hb?w^!JD??GclL&kOWv2d_-97Po@!kxsO@^dLFo+Cm<c zlz?EZHd@{hK~9wFfUWpc68P>Csj@T%!JJ7q>NRC{q2y-|P*X%WG6|i6L^9gQ4#ycp z0~e!=ZM|G=Qi)z2D#Chc@8qwzQb#8yCGchI>Op9!8?8kcxOV&^&b}N>{G$6&LSQol zy_yPEA2yL&GUIW~3Q@dt-#cV?+zg_mLvgl$E#>&OoG5x8Lr?$I2*^cSH&%brAbNjV z4sW^p3YnP;Lm+37p1F~e;QY{sz6Hza)pH+{f$P3#YK=B7sf&Xlq4{k0)W@humt}`K z+t?aieO7AvA#hwam2E5%r|ak-@Q9kprQ>~~KgtBs9)ZrJCpU%qes(Kem)6ge`ba?T z$=$T&kU*aG87jnIAL_vqE>NJ*_&QPPEI<ppBGLKm%cN7M60hD@M3fJ3@(fFVv^@DL zv#ECzY0ft$t6sap+Bt@B!>Evnld=Sd4Wex4iQjCL^IN+6y$bs)x`8b=v0@F38(?(Z zFz#>p!(2S+gXdm|gXvd;=<buIc)4JTAIY|_M-Or*;r6I~*jHW!68Mwwy-R~onQcff zd?t=FXjj%sK?jSq50dATgITz}4I3zMw{0ouX#bU?SR(T&ZXTD6#Lk3Mm%e7w^7+%) zoZLv-VRATlj@HtfHyW^^o!6=GZz=3CSpog|UKp#G>P`mgtXaV=&!cSjjp-~}HXlAO z$)O7OzlLaYFAz~ILBF2s!o!E3aJtYCs613}9QVwd?c`$4p1>U<JJXieSh=g={)a+X zSKG-rS_<QjT6W~hN-Nl`ZcghB`4Wq{FG-VsHayW<jHCUsAa+I_j@aqXpS|x1n&9Zn z#u?8LuwAA+8uHxGjPfZUyRMNv?4Ajq#Gf)V3-59^xG2JyKIbKIQ6TEqeW5?#DxAG@ zA3iG?z>cyIc&R!L_77}9dMEd@Lz_9^cH;#U+wvT2%-ZmaY*{={b&w=%$;bXNd2CDt zkIK^><o%ErV^fT?xQbl~?vD|05hu8S&X2hZ23sWQ4GNN|_O~28?S3EJp05Opa(7}e zr8R8TAq;qWDXQHP4w;;GR~x<|{i4HlIq@3o3Ekb;(uPBJ#+AYui*F5!7rw(mwO=6M zqaB`{REd%%0Jw=y!K~39v{StcJ#SkD@_TIYXs8|YK=8Sa2pvwrQahDcF8v0D9>_zW zc)Ib$=pov-b^@EYz>6KM+es_TS&kj-BgmXz*)Xh9MLrb$X?VUyoSn5{Iht~-mGrsX zAd+pn;oh-kSg_Ft+_udl8T$Ki$@*v*e>sb)T5CjWJAa_YnNP%e<ww}_Gia*zdjJ;b zFEhdi0(UWW$7e$IS5Lf-Uq#Mm*ubU{O`Po>PRpELiVeSu^B--xfGXZjZ9KvshsRIc zL;qqq@6l8RHhxY9t@rvYyXlEJJ2PAo-`(7T6y~R}H`sh!&#5S9opxq78XD659Ub^c zOceM{495%Q&T#eUDl%KVUckhLDpB^UK0;tv1}=<~hn04d;Lp+|tbXAg3DMK0+CDjv zwgq$1(Slm|o9@G=Y_x;!M5RWVVpm#1Q3ZaQ)IprAKhj+O6B1Ha(P}>Ktp52ou%uLl z{n<Jl5<MS5=DAvwUEl}F;t}{#>n9?U-VVu@x#aj<!A{uTlR$sG8;i$XS75J)Kct$9 zOQ;90xE7mnK6ceTOCWF}6lO)^W+5lmn}3A}ZN7$9o*04H7)drtUX|TmT!n2!C-5ge zYhmqkf8gawsjxG^o!xu>IV6Ow#-Hn)*+oeg>1PY}lENq7iHm#y_MR6`N9~`){uFF_ zPv~MtI#&7&xUGBu@{>~V;`13usVxXw7`A|%VmBFSlEZh*Y}t9ocH+jGC)C-V10*X& z9lvj?ZZNhn1P4Ptk=&ArGm1aq_w*zD*MkR9@`~)QYv1skU{U%&v??uADMjmL#j(DI z(loVel*ZQbX#S<iw1B^89xC?UiuLN^P^hdX`r94>zfY*KrMfYA;^-=T>7Fthb9Np5 z<*){w<`M?qewC8MlmI8^#Ahhxz69JHwZ~id9~)X<PsVHAvf(#JEm&xrj;vnqg}%Lq zU|WnGeOpJARxC+}?3?G=SEIpfC(dOf7ha{O&p$1IK*_`Gp&?6l-kN=EBlq3>Jd_6? z_Y}dY^Bki?Vh0Y|u^+=wG;GmU<Ggr(;rfUt%yT6WRw9ERhOeTNqg<H9M-;&Gwt%*m z8K6X6K9PAZUXts-b#TSL&7c-=iRAs0XAW#`LuM0S5;rE9=EoQT^|O_r7J+7CQ}8t? zFA$<DY;v&u5=3p>mr4f14zZ82@3I#keTG%#XE>(2K0dYY5lfyG!AH*Ka`}Y<o4iAm zD$rdC=eR)eh4&QUdvYoyo<78CmrKIP%2pK2=>d8<p}Ccp2~{TROcZbD<D~|Pw1KW6 zk;iI~sUnZJ2_h_5+mICc+PYh0zo$5V>-&j~uP5oVe<#gkZ(ke5aX)=Y9A(26xmU6M z5ru5CXAS#%ZxE~6@Bw~#J;rfD6QD#pjIDX+%LXkFVa@FApr`mA@G`jyT;c`TYIcg9 z6CH%22V`-++ah+cGWSb1Iu4$4h?Tyi0W9!3W=79D+(TymRlos>98B|a5}h*G#iq)J zK)rbcEV;E4%D$fi%UUOTwN4IePITC22P+)zslwlUO_uMJ^@(2e$Az9*sZ7uPYftaD zjfRhTil}L4Gg=ptgu4So=;S3r{Fpxr>2q^xv4ZFoi2v}1BVOI6TwSLM*jK7TC|Hq) zDbuHD;^wQ|W%WIjcV|0#eY*p-I$Xog`wl_epPO(&OOI8V6T)RaPh^k2(_psm*bPPR z%HZNYXV%rn6rE)6finstzrNIRF^ERI9Uo$emiJq*`(?y-v|PY%)ViQ1vl_146KDB~ znzVWF9&9DT{UaG`%eLRq=>7k-Wz#2dm60V=_oArPBG}M6n=0H?NH(0{<BMj}@b*9q z)m!<I7-jRpD=3k<T=*Ax<_r@4%2q}-a4HNH9Vaf^-B7iZC0Q6#j87}vBC`XI;rdb* z?QQGeN<Ql-O&MdH**=}w>ac;DRxg6C-<PE0s!rhSH>&^JkFMPu;iLKd$-<>#%D<n4 z<UgG(JWWhnYD6aAa0+)6{Vyvz-`84LBY)i#VPmnob2_{8lWc@F^UqBY9xs-B!qF@L zoh^6v?bun@kfoRM4eYt0g*7*P;Hhw7Z0>ZiEl04~*ffp&^05>C|K5E0Z`$D4@VgfK z_`hj-Hr%jVymjcmrwke!rrfW$7@Ip?EN1DC{(P|Ec2%EQ75d)-O19iXjP_~09a}&% ze~vA;D`~gS3jZyTHg;Ct^}9&^d0TFQu@K;~Y175H{N14cp3>Qld&pQ;*ncw;#)e}V zUjJ>kVQe@Cs{ZfUzK;z#Q1s5RxzokQAkg_C_S}ZX2zCE0aK@f{$QU{9zXjfm4ae-! z|1B`Vfg5u6<Zbyr4%`A`CW`;29T__@W^NeUsb>D51NV1~b@1~grgF=3z4MfOm$9R< h3SWN0*wI+cEx&&3XzU}E|8?w$doz#b+d2xj{(pT9g^>UN delta 65153 zcmZs?c{Eqw`^GI}8B!`46H4Y#i1*p|kxHb2W>l00C6y+P-Y6kMAw!9hsUnFI-e=!i z8kJ;<A}Wn4>eD>;`2L=Mo<E+m)?VxUb<RF}opbheU$5)_@m}`Fd)bmO6Y<-)TG*}E zi^uRL6eP&-=k*kL-J1;Sj+cYd*FpT#9~(hoS{dqI%7g#%qk&aD1L?ud__Jm*?Ac*S z%1cjE;~qw$RASI2PmRqv-wQGZXYfSkB)-8=njaR?hHWz=p*Fe}V=|OQ1N18Jc%cLJ z{&Wsbxy}}&$+{qrs<p@7$32*7`$CkcT#C*Kp18e00%srZgybP(S(Nru9Qg7PxK&;! zu~P=HF|%{gCG|C2mE8~Hk5?0wE)9N`TMRy#7X^lPX1rQ&DGX=QMCpMSMs^IuEhAJQ z`k^9Ucz7$>7~+Vs4-wT`wnI~ycnByOzNAAWP2|G8B%)^f4qix~!IsCj=>$0mw8})# zaYshdYrjMB{XTD8wz?Zs9$cfwBSknk*&aVikLEvK2*-xxP*gKb5niN7^S|vb5$z3j z@aVq)us9lxOj-#Z-H4z8+YfVs;&K?GtxFW2&%rh3J&-L9K85T1)P&Rnhsf!vm(lJ; zI!&+}2Pq4rvF)uaksjrZ8*|*ry{0GB*eMNHEzkwY2uI0Q$vePyGjwtagONJww5!#e zPyZ#0twF>20Vh_IH2JsC+CCFQikHF{t1z5DY#I!Bu@A~PX(s*UGt9q{Akm-f2jYQq z8sX!NF~BX_Kv(2g;+~gZ@utUZxPwWU;rE-w>D!Ym)jUwW|B+~|T!!nl3o+B64GZ@y zN7EDT=;72(Hq}&f*JeJ0zc+(PpK>Gk1UO@~FYxEH>*=c>^LUBc795-W5nQwxw^Yf9 zS9zy`L0z$=_xl<#bjBaRzn@AVEc=7wlk=Nle%naCOk0Fjzed6&>(RvgcRU_*wcwvk zrFh~kqqnN(!vTjO5NkXUb7y9u<&H7zQe6S&I}fJq70IO5<0LF_QRP#HSkXuN;pm!? zhb3DtLeyY0Dl=M+r9XaxzQZzM-=nwmz!I@8K5RIP2Vf=coPG!I44O*cU*8GJ6;Y`5 zJOXDbhS47dnP~rJ1!#?(N@rWnW7~`#fax+lNOCoXVfklpg6dp&@Zbf6zCOWuy@^6@ z*hqX45&`Kyt;vSt=JacYJWSJf!TYn+G31v#UmlVIMLDO@JI@JI#PjCxF!UJ|_Uy*S za5EwHgDr1oQU`M!q~YeI3qX3yVQ*j#PLh-1ca){W^{o<ER5k@PPP>85wlP%x)>W7! zN}_*$J%om=D_qj%BbdJ;g$|55B@EJ6WheCl@U?#(>de2)UE3RtYvrfl_p)Z<tf$Jt zI=+M0tLGEmk$DYPu4CDllCgX^bYst)Tykff6>~f(!za8C!*MGILvl+r=P_S{SFw#n zFV!4$+&z~rTKSA5_32^5kLz&3`3LTMKZMF?^RWC=DbCC{q0uMLpuJ5olr;L%$9X{G zhU%ch*(7-PU5O9<AkEILC1UtEXbiW)<UNrt$ivVAU6`&K4k}isNy^4NoFuM-D4#Cq z`5Fah&v@Z$j|OsZPaT{(xEq4Mgu}Om8Nzh^YY@I-8}SP(pda7rgW}&aI7uTOifrph zdCqb2^6yao(FQ$ct@(nMxb)GAd*|V&Um>QRU50IY#G0_2%OVGt4}w84wy1r<0OnC| zddk`eYeo(Pm*@LQyO0HkcPX*$k6uCB4G;9(rpG&cv}JV40T{h(IO-0)3+tE2vflVb z_`LZTbZ_$!g)Ym2%ZGI#(CD5>Ra8T-F8hq9_il$3bNgTLz(Sbj-v5wCJs?o*n}pLB z9l+Q{3(-<(4gUQV3nk}X(G5EeP}@h^n6TRpv!7%@VDxo-c-WHP{htCq>)$FtZgVT$ z_tckv|9w9HG^&FJU7C&2Tk2ujD<l5?6cx-Lx>8b`nu%Jj-^l7WozQvo11V_CqVmSa zaHw6O<hJ`at~X^ch)-U8%MC0X$YNzrbES8ZV9ZM&#4pz=2F=HnB7NGNGy<wk<B+$w zh;FlzKz4#OCZr!DrWD97y9qEwFAW#;EyBNE=Rrqx9*o?bhz;(IsPr-tj^E~Bp42gX zHob<{v1EwcX$z`X^`XaVGe#_Gp!1(y6Vu;S;X>BXIY?(mVYJd7JpF1i=SgPJHE-v_ z{Ux^8r|t)e4lA+Y&sDN4s|?h`L#U<CEV6OXQjG7-0C9CbvEIPZO(w~()Z;YpqCQAE z;e@Ge`!HtvKwf5y37<JA3~!j5qw9gk@KL12cfQre&iiH<cvU<VO-9FHMoR=7dsNAV zr4GiQMwc+A@fDf+#2-4{=0L-iSQ@)Rf?G!vLq(|(KfN=U48F)?#*@$7_qWgC<J+4; zjm#HZ{QeuJwVA=qKnZnpDWkHBY9yoQ9LL>?0w~<NhlZ`jBzz9yiZ*>daX~chQk+Qc zidWpHsXcWtDlGw{qVEvnf=ueQd<?I3YdyW@z~Q_l#q`Z%e|qf6NF25(7eecLiL*x; zP3rnc(>_?TuPZxY#Y#II{pAR}mWVLp@Dw52W)NgX45u-14<IgR5}x?%PLszS29vHn zlF(`Z`$M!*Ek%dFwSNPOLyRk7b__w=U|+uQ`XET#Gz&hBpN5T^%KWfaec{UBY3NQP z(S&yfIk`j{=WM_yH=ARW%T<`V7ir<|Akg+)4Zq1M%#0hrkC=553<kfU2VR)r7g-fl z-BL*pUO5kOwg+&#g&O;O*dM|y)?)fNQ?hpD5HXFul|$6Tviyuq`*2_WOPCY=l{Wvj z<A2A_MbC64zW2;Oy7apuf5P4m_wI=X`&rMZYEc}uI=%~v&0b*E(`0aBUU1#&D3mrn z2Q{smWMt2ELI3F*Pzkw%>xPZyP3D&2Z0lsS2{WJ#U(X5KwR-W!+X{U7P%O)`>yF^S zFHzX?XBg_5R+7c05%l5!OFA~kh;7|jfeOlBNk@tgJUu^|sg34Y(P$koRSSUga&OU~ zPlo+D+DB?tq#<5%nk0TMhRT+??7QEws#42FG_oznyVMZ_B1hwlcTTK+t_NxVD#h}z z{Q$GfD6(8U<Pt3%5sgU&0-p59gvJZjeEqinzztRTiRuls>zglrPh@bk@f)4>Cl9Od zo)W$W<wJaqBFhj-u<o=h=g9d(q}wz4M)?A+Aiv3(7Zn(r+5nH8Poc&TeKfvXORB@C z(UD$<K-3-v-==+{Jw+X4>6s)H_eHs2gO46uDgDDO)rzEtdN!idR}I+LJ_;=@)v@j4 zMjX@}5BvNaAm>CYz1^M)o0J)Jj2Ohf|G67O)w{4lTa`cjOPhbOe+FImL5Ce%kqA2m zM8Sv=?r2%uh~1YG5z22uNkS7$&Da7)h1qC#T3R?+GE0mts}o7c@-|T1Wen>gq*;B{ z7&M+e5x67%xZCO@h;Rco#9l(vn<>z7YAp^qtprQ&*Q4gbUxK8~AG%j(V{pj<`u0r; zcXa-5+&JVeyfxQiMsMGf`^}C_--6@+J3av;_ozdlf;>C@{3YNg2RJe=8Q;o_ZICy3 zjE{;B!j!W!u=c+a_^WTh7j)NPM3W_|rY68AmA^RgWj6S=KcGh<`Xtt~Tw!I16mOs3 zh-x7RpwKA@!(NX;Z3k5_N-DsFyhwQWx*H3AOa<4xk65z#E9_oWfLfpQ_?s!=%%q|W zHk>QMp;I3K6&ri8rH`74^xfGIv0Ro9zUcs)cSq90sejQw%>{0&O0yKRNHAShPgh^w zN8juo%m4Z*!!%2BFn512Cmde`Cc2BccWydt!R#))u%HdRWi@&A`}^^4a6Nh2%CKkr zVd0<pC`j;~g7<oTFg9-y33SbZ$S+q=y!=}kK3RDhUX%?2`73Go_v<$F{-X{VKjd*` zL?w(6jKOU^kJqba!u9Q6==jTF5b~BInbB<|yL1K>3M23)DS+Es&XO;Wx5Ei?45L^$ zerF@$>c&RIZ|*qSG!Hw1FG8u^6mDwtVMN!1;Nh+?a9MVUf{LHAl$4)xvYyGPWM#XF zevh-=*`7la_*v(sG7~v7*0^Ufi%6Qnue)o)Ceexft~?p0<<f-Lf&<w2BlG#;dDqdb z>b4M8X~H@m_G^mOMtnex5<mBg59|2s&L87+*q@Z=_`myk*t_Q>pS1H6E1i+%V(Z%+ z`(~Z3dh@wlL?^b2UW{HSS}oorD$V+6-|p-p3j6R(6rVd>SogC~vNrg-quWe#N!jWa zhtEl4xjPruiApbS7LC$AC}|qGy3!>%xhm-IS;^nPOI7Y;RYYM^10>NZ29kuP$f~^^ zEtM;8U2uHUtSx%~=Mbk?^jR$0^1M)za6#5_rI{4h5mq5Gd&)%9Hh^Q>;@zCka7tqH z(n6TF!&mr_Heci{e&=}XLf_>`ix%$JHbt^vf|{uCKWo7^q(QW>Ku_}1XSXoydAvmP zX{zM7c#-7R+;$NOaFS&F@p0@pnkU+pFXg!GjD-Bo&LSzMh2&|4cpCRkeJZUqjwMen z?Gu`;#!z$nQF!0pkd)c%sJf!8412{xX>v>z7qs4(yh$vl6Vlspo^BfP4<1KccFo3) z;(Vd0U_5!{zl#K<o+Ds6S9q4EK&$6o<1TfsB$lo`xAj1m#Hasc<5+?Acy^JeF&#qE z;Tilr@v8CcD)%0HEo~r2vI`SR030H<dB24K<=3u)|IgdNdW<nC^C$ecBF(LysY|cj z<gj$DFXy^Vk(I6JfPZj}T+htMt^2QHx2Gu$^7%mbrgP}k^qrU$dm*gkKy-5un{i7Y zdme~{#}Uin@A~8TW{VsXD->SDFWeMfVOb3B!rw$}UWcbYsX_FQi}2Jymt9W|qXxGs zF+_16e|2a(R&B^bjBmg_<8AnP`D!%ssYG;*oWqC{gNd%i|F(`CV(oDU9B)0rvGJxX z^wvZ^Le>f9mzBbp25sIf-3+ql4Q4+d8}Yi*@?w68d<B|K1engBVvXbHu~aWDjIF7_ zfhu<Vd7XWr`{f(0tI_3UOc&v*yGL>N&Pw<I*KqglVPNV|f(cVMps~0G>SJn%lG$6# zh;JmS|FrlMr5$vMaWk2D!q;(E?g>zLQe<BA4_;eoCW>E7F-k8SV!w*#@Ix!)N%ik! zNN+abb!SUqd+TrTjMzeD8iui4X$LyE`!nI)4B2+qLoomSBkthp92D(3O+ttjPDzfT zbh#e!DhVPATTAE)-x|{0!7!Y@p(CE;Q|C@Ac1=zRyrfmxAx~X4ljK8<@i}T;egWdF z9a+8D{J4;0u!?Vbrq6QIpK-r@AK=$IMu!ZM#>s|H(74Hp&GTr1Yq!&=>*#|t4s7^U z{c_J&{tJEZ#-5)$>?riinn1P{K8A1`SNxTJ2CWzCiw*^;Fz+YpAZgKWPVM+`7P#pM zmzC5`?QTZ!`!DP9+cu_w%XoPv{?nL+`4cm72gvf8v>AK86p|(DMnQ%C4O}=vm-R@z zaLn~!-1{q>o%2*-OUz6-Gw(25zx5!WAi4&Nz3)r>cb$|(9ZV6Wmpe(GbY7D@_^B^h z;d4pSvvY#P&e}>CAD-*DHP?ZYw=?E`t4)=(p|Qg(r2(SD{|<?TAu}2Tz4sfc#)<nx z>JOX*ZX!DR1$uE?Gq#B4&Mg*g9I#(xIH^*y`Ti_X)F4HX#>YKX%R&>X?AJf8x)IkX z@%X1BA{Qn}&|tHpv9zfq!tI+R=k;%gnI}wyGcS8ZK{w(>hTr~*Sn{H(_OqiTO*45( zl3AL=4d+gg^+NF!QO4LB(d>osl0U5v937h;i5iN^9Ut_(cXXTWRuwq%uH?u;J<*Vj zVU;6yMoLnCqeI27{dnAa2EBgMjzkv3(P@Ti=zD7=t<xJrXU#jsd1~~~^48<RL;Wsd zKF}F7vNGsCPYLH`TFyOhy@jfpZFtf4G#xeFmwpxR9gXXzfRH`V7)%G|k+xBZ<kgga z<j}(kTGaVZqUgMUTcd46lgHkt#UV$zo;8O#j|EHc?T!n0t1k#ooEwgdf(F3Jge25+ z9S8MCUX!tp(rAcOCAUstgK#u4jjXvpmE-o+&^ARoI991d{wxf|=!G_5;Gd7;>v>3r zdlr$J=GhR|)hW#1Y(!gpRMCEG3k{W81*i4>aj&?XOCLH2Of^q}Yj*-!J@^(m_G>Hs zGU5*{QO%}@+9HL-&v{^SZ4^v<HXg4xB8gg|CRjY)MGAWqQLpnKHxQdhPRDY()n^l4 zSXNKd15@ax>n&nTc6$l|fxUEhj5cW)&_fsas>3LIT`-eZ1sB=Fu=M8#I)8~Wd-Zx8 z?2o^Qk)xGh;?|kqm~|Ovj9rgCqJQ*)ViQjMm;yhe&QV>rFpQfh$9;Tl1DEUL@ze)Z z@O&hVEB>_8PQ?Ua-I8K>;^4*KPe{kxvSJxn&{BdUu2sT@gFcu!?gkxOV2<*~WJFe- z68chSmBg@g09mZmMKqpt6N8L>f@#T6;w@_ruE)GdT&D&xLnC@zY6W*<dOYWIzex~} zenrDS3-re6VEVBTgr1yisxoRIa4Dn6gNQ#u>5O<fV2l;4PrpVtiVx`tJ}(xEzD(2R zdgka*^Dj%N$=ht9KJz%YHu@wDacdxp#y_P0U6@PU#SghPMz3jTtU9sSGYq|BZgclM z4M@%Sk&=|_Mg2?eNltffB2S*hlcVj*v~j5>Tzfi#XjsS7KUX)2&L{P8VT&?2QTa>Z zg;@<Zd*V(azAI%-QzF#q#ZdseYx;z%`*o>Xd@pJ548w;CM<LCf((X_0!f*>$Qg_`G zeY3Vff|rh9ol#52KCGlJ#pf{f>toJTYl1LWYXF&Kew7%F8HY2zq>6l|XGrGGSVYVl zw7AOVXY}L1D&lvLr@fnR(+~d3Na?a3F*&qzHLY3K!ueVsmt<eNE4rjo&t318&=2Kn z>5kZ1a&+iw?t|hrnt%L-aCUz*9WZ#BXzRg?v}A}SK2-iqPwqZTmV~KrDu-L?VY@=w z8(u?_gCxZF%Vc6bE`uJf2J*?jSE#RfElK{YPTpj1BpI)hM2SPxq4TS_fsCauICt$h z`u&UxIT2Vzw7-24E|XgB-^R~et>0L>I^dII@4q)(V$3k=UJJxUW3DhQ?-Q3_U`A~2 zjHe}!BS^^yJ6e6PhK8OhqK@u{Bt%A^jGJ<depde=JQ-#~Zin6^Uye;At0osx$*n*1 zqFDik4?0X;#Pe^GY3f;&_xn!jAJ&rGg<GlCb3(lm6RMW#Mw9EBXN2~RM#QaR2IZaa z3l@EOM1K1zIz~nlf(H@7Q-3n}tVo7MSN{p(*czJrpTDqXf-yL=g(O8%L*Fc0hMOYI zQ0|!lWR4#z(O7YWDvj1e>)8yeEcT;VZL}iFPtV|tw!~nX;RG&HY)3VNrND1q7BxI! zMM5X3(tQIWNY%YPD6{emIb<@LY6pp+rYV&x(3paMmxjUj-(R@I))mloa)D6My_iJJ z9ZZcY?$JssBX~AogJ625mg|mBfE;@T@a`BPJWDsEZ(L)r_x&ufa83sC#q;T~eNG@% zUbr0M{Zp&f_#&~1KTWr$*pS2_-bAm|5MFFr#-+`W!#`pftREzUUH(fk1sF`T<Vmil z4g@7hW9NlKcyN6XG2CQ=1!ZmYTT>!g-xG#DI`>g=ooD4*S9jF>@E5k)+~%^YuE4ad zE7u`SxkeMuKEUngHqrcrPibEjk~ik^ke_6UcJAtUYlafln6V7sh4h#8-F0N0Zyr2e zKAp)gn*jXoVQ6gIN-n|xzH4O!TFrkBGJB@~pQJw<ct6HTNlMB|sbA6SB(HN`C_cMo zf?#McmVVhcp7~m5kq1rAAXzs^RH+|9Ma?e=b14O%X=BL0L~qHo_93j~=z1Ed>kd_N zqwt7NI??^?DO{?RP?gVKqI0LiXx=Und41^vw@=|eGGU=B_djL-r|-~ZKdJsgqJMa} z%g9Pe4IJ2i`oAOD%0wx7#uPj8^i~DB&ZQcMr=G+~3fg$>*MD^T>y1#fTmiQDEfjV{ z$x)e>F5>930zCGp(af#c?2UCGlWgfCBM-;$U*){OSW_MrtDR>qjYnD531z<i*LifP zO~F=qa4It^B_dZ3>RoY=v#VEh`gG8kZA>h|(I>x<WATY%pfZ_4_sc;%Z`}vlGTX`b z@?X>@w;D$&UVsBH8I4rbpyxuG=-JH)qP%QBp<%BkI-Dtksb15Vi~CThJ@ps#eTVTL z*ALVA@70{N7ro(MX%yolPfb4Bb}b(rI}XR0<)c!O0<(NFn(3y_<?inBm0WqdA8wBl zS7G8mGZ^e)0((l{VbjTExV0n)HBSW5%HZv2mvuteBoI2t{St&<oQ2v=2OKRgtcUnD z3UoHrfYK9jMBK0lV%lOL(P9$5-F$*;-5HGG&!#e+M>%}5)&_n-ykB*Wjcj$qc~`u> zxBwkS$@41<Tv)&#XO=3a)=rHl%=t$3U|M(o4&<oD@&e4|gJ!nV%^|*gl~NFRrM!SU zEz9}6-{f&^<`i;5Z3#V>{TXL$cI7WeZ==n3ACupkmr*vr6PI6lLyy%2aF3oo$A{Ba z@}Fu?6P@x$wDrm!xTctn|MCYp$)vo5&=N2HWo8f&57n=P$&NL6$9gU3SPbV2oHp?1 z?!TfVBARiF^;s;>8;Y;i^l}R?8NjLGS2=~NnV6|!3)6q^r{cWF9Bjy@eQ~O2p!t^m z+%b?3Z;PS7&RXK`A6uxh_od2@XN^ee=cja&M=bm~b%=Z$vXssX=pwSh0T$?AEye=( zsnu^^CGju9<f|`c-o}%&u7c$6bjcIxINCP5h}V05gYGLC#V1`*hHX~6VAGWyI4@=t zk7rNyFZTyH|6&bx2dqY|lX^I#E)L4OW$?Mc!GJZkY;J@d`xfXVoLlWmADq}Aw6~2x znX$^Sl*o|APitZ0C2<RhhUw62K7_H@Y?2(U#ec|AXY+0)@zn`4`M<+?W`LiN3zCH; z|Bho~MG||nRGO{uwCBgXZK3HS04`nGgCk_;!S8e*lDMW6&6nRpg@whqHE0J|^qz%P z*&Qe!Dg#+hUf~JFfh=@>E*<pHo?CkGkjPRzT^kq7F@x^E4~ff))39oPHk4E!C-!e{ zqVxF!EI7BI-)@;+U6N)&rk@?kUa6g72bW)Fosk-BhpQQ+ocMxa<0#*^?If5EHs^1h zio%?oz)SCXP2Lx(!r?F}r^Ss&Y0dHs9KZMuL7OS?de9Hrqt-{Zo;%EoZ~QpI^9Qce z@uL+b;+K|$vl&K}7B|qt<L#(>y$^LPc||ul>(H*T`OM#>8n0|KVOKR(oSbWen7{D| zHq}g)En8g!vMb-i%h#p-?)7f=J=vf4Ibp+=3I5pJS%s&}R)IqDH^lcRuq!4GS1W1r zUV0VeltzM>zoYvH0~Ssb=mA6YE1Jto7C5l0HwQs|vkZ4`<SS}ne}!7x>CldbTF%ns zC*2xkO?T&NlJjP9F!ELwD>pLWTZT?xGoKqe1>F1r->>w8sCEx_f)C}l+>4|uRQB`3 zju<<6d!+K$=Cr}BlRMF<AQ4|hZKRc##Q8A1@+KejtO_zR9r&A?mwBb=F_5p`hX;D> zsnPmmZ2c@pZfrTtz1lc{o;4kXKYRy}pEDnGxsIdYd#w!)*kq1Pig_ac=v=623}D;! zVj<{*Z?$ug9g_r3cY2uHj>VN5nQc!7nqD1GTZ0F&IbW->JMEZghB#LaBmoETwS^BK z*ft#H)4srrZhge7^GLpdI)obBf=9XIAZFolc>G<HZv3$T_c@y3=!SW4_(3^snmwOu zYPZJ?W6qGTucv_QlvJARvk;s2N#Uh*Rkn3lJk%Rsf!xVqPWR<@z^X+{s{KCAWxfiB z!SKvp6n8IEVRBk?Xz{_F@YU-81nf7&sqLkxvtT;E<sk<eO?f0x`Y$wPN0H$H>!|;3 zL%Pm*CwUUqfc3-okq>jGvzd;u)S@nzyQico>^ZLjC9kFvKj92LAMu0O9hrem8r$i} zdsehTWGMu@OURzdqe1jr#GcEY5c5W?373vHXScr(VPo&wlV7iFsq5btusi7ot2-FQ zlHzya+&RwV{Ll;d+VeVCmf66^A3C^Z*GPWkpy{yQe<dr7FD4d7`{87<6Qp=N5l(M( z5t_$Lf+YoK$&Qn61wHOKx%u#gz)qDD>2-s-{5S*b`tArHl*L96n79suE}!D>tkq`i zH>H`?y>Zpgj*aE5bk4G&7lQfe?<c|T4pW|uG_D?$kcMm40{@|XGgPr$v^QSMdWP4L z_shI+OIaB`BV{YO;<EzvTZ_qxFO}r*(xGVKJ&j?a5ytfYFHSogpkk>Ja7t?=c#l|v zAH)+s3)~xRm{@F$U1j&F?%7kqn6d(lFYgmNmnO2%^VyiU;3iHwdkL;>cualmPUF^l zcYsFCBDyKN`6nhReB{9YxEsrlkq1k(ApKhn7^Z(DUrLJjCDyBOs8TdOU#m$9-<$;t z4_C>%Psga<wI!^oas%J#V+G<5-#dt5wL5;^-6X7+HIukK`;X=-7=X%ie@q@(D16%2 zi+TgY`5^s3m^?Gx3C@hD9vZ^HNwyO6#%$o{`;F(V7H;CQ1D$Y;<|nqQdlx&eeh`;` ztsrLq5XWWeqK{lMJvjIlzRZ5Yy^{>ah@yBVTAqncx8ub0`Y5DvJr7V(v<{75hjW(a zXA@QCMHY{LM8iP|0@^IVy><~kl(t2wH(4a3vJXEO2J`Lh1Nox?^3?~LoSCivBwqcF z0`roY%2&^<73^?6{F<hWe&f^Gv6W)9i3`OCt;^8ALXp+28pU<=5H9;`HO-wR)|WW^ zIYAywJ;KHx?nLvdZS=42S9(J=0e3q~L7a>fBo-+M`WhPGKh%e6`dT8VGn$G&%;Y?! z+vqT>7Dr$5o0{}$KwgpwHMWX^$Q5T;&hC2t{2?nQatnkB8>d4R^s~T#Za8%GFmrWS z0Sg_qgvR_kWRKXR8Sl+7r+RUII51a*tqZ%z{V7qT(O2u~=#TfwhvI3tI?5RqNGULj zh><L9=TMmD;RwUSmI!JyV(8R=O7JecT9R9z4yLlVfW9$CS6gNDXqMs&h?qW`(8*`F zjH>=xltBLW1mVDJZMLgv8q>+y&JWmMi{i+fQ_N1Xgs<8*fUS=1#=YjJnIt`p{FTju zPcCT?woDm;$_x319*857n7-3x;o-|mq_KW0J3m&PtqN_YE7fn(5!yLGj)uYIoepG* z<Ph00ZwMP%uE(_QTH=$+G8*uA0UiCs0gHlKF`s-wNQ*|#>i@*VGqb(wS&Im^73>fU z?sQ|xq;fKD=59DHL*RB{CV5`f&-KT<;nVzB%%8yv!}9jilyACdxq2zCFYYA;4c^40 zpnt-)L)nFf7c}NU2i1O|$QiAPgllS>V4g?{b+Wa{?<upGuty0iZp@<ON*wIFF_e9M zF8+c?sy?zC@ALSnp@A3`yc_o4oyS7ecR@zNO;kSpmrv6RU>n`X@umyzfaZi#P@q1W zsc%ap-8Uk!^UHLmRK1_-*AtS~luN#ZUx6(FXW@%f2z{|Ko@L2QWuIkBh@*lWZuyZS z^wcb;=OYhulWS9`$NXl<6|bJcw^#<jlZ=00nYD@+87{|xq=i3m^cmk8n90j*TEaJ} z2Em|ZGWcP?HslSqAy-<5S6^&g!Lr|VL!#9YD4cK4bY27^mDj>)AI~w5L!Z(9R2I2j z@5V+9b0FH711pC+VcL+fbY*iS{hFf3mAv+(crk>Cw+aJ+uMHqRdW<Z*pGtQ%o`Lf^ z74%{HYG%5t4(huiMX@(FfcGyW_GZ9iHcLyJ57Sr<-uIqj<tlSLy*^7Y+uP0Z*8Hj3 z<daGqN6d6wUHP1GEwcRDA+IP*ng*L8k;Pe7A#K^u>MKgn)@KPx@2DYF-MwOJQhSp7 zbKpIF{^K_x{}~eV*?y3Ia1l{fJ3@~wY{C4@Rty+sKw{#K;m&?O>o##W6KVpOjm})Q z>sKdEmr0~=^Ur|Ma1)FTmu2hH{^9dns9twli=FFk7R-(*LA~Gx83%ev?YLPuF*Y7M z9!*5kZ$lw&t@sc-x3>`Iox4rnY~aaAu@QUsQj59x%n>?Xx6pu((KJzCTlf^OM!JeW z&~AwuZx>Z7EUhK<?~i)$O1EIM=g0D?e^*!k(w3>VTzrln*pm;|JKMluLn<G<<OFtx z-)BG4v-zNK5qLHgv7RFr0as<f+Xo$>E!GP|_3l4#ZgB({e?BBsH+Mppe1G7iWFKqy z9Z1}AJx~;)g74PKlFTjng4X+G_<?hQ2P%EsQ#YQNto=>Dzt@EiW1O(I%aq>zXi4S6 zAF_}uYxr}!els_dLDhG2dA{fREMCUXfkixwVSfiS;=PAG{MN!yUVL<UF|K>G0)O@W z!2jiOD*b%m&pt{sI-FtAa7+B{T1?I?&?e;?C&+1ozntf_OLYE&al*}!uY`(LP0-yP ziKDD+=+ClQRCcTv>H2Ysb{UzHJLlR7-Q>jYzBQZuc5vf6)$+;9oO%4!%%f~`$_;Y2 zkYgW1)x@AvJOdtXtAGV?o?o2&8~)lD@^dGqLtNcH_Hd>YZyT}?cGWsE>56B#W7uhE zKM~IImJ)0jJ(4$1-7fN*B`@L^2Gi!Au~2B|$vJc!q>1;kC9kS2iTs69l5x}=EZ^7A zz^-WO=5QN7^+#@^A6T%AnX{er;>7piuJs6~nc}(pM2`l@9;$#1EtUK)t37m4;UYe2 z&UwiH)kHtHZsCTrQ@9aMa>sWTaMw3|faXhUq1Ch;E86bE>X!?lQEbkI^G~SO5CgdV zAPbna3KU-%52oLAaN_w|+NWPmkFPSPzv?6C`cO|Soh^rt#p9>3zJpn;>69zXi8HD` z(s%}g`#%?#_H%5iTmyUEafTN6-o_cXb68`xqLXuxBlUQr#232_z|PZ-yh7+uJSIsc zclLhapZG7tGaD4y3frUb_rX9IemV}97Tux>G7)Ic&BfTaKdFJ&bFxM>37S{s({~m3 zh?uz7lb4yM!jy6~9PX|R%TlEH=Fr!u*kHy6d|VG+&Rn(b<AKat#PFWtxT>>VdhF9r zd0t4Iz*@FwJH3#N<kz3H!Y}t^aMY)<pmtb>Z`pqd6i=0boX0lWHAWjtiq`P)*P9%b zrFyBCc?5TdG)fk{2|=qyJ2CuD6u72;@^GiFjM~iFPBn%P1%pIwkW25yjhFj%o?bb7 zfAKO7+tx@^|I1({s~_|0C&Xh!-r(wI^*yZ8yAeu7mjqwihm0kL^XA6tJc0-=mLG-O zXM@<vuX615*2A#1`WP!=uTai>3~C0g;?rMeVaCWI<o{)$*DL&<43Uz0GUWdZnuGK0 z#Pi%Ngsm<buw%p|^q3wAkA2igaE1X`j~|G}5sLV4>S0V7_EaKw{17>JW;%>jQW0j~ z?*hH0qo8z+DyLiCM=XdrR$M`tF<1eDHrorzOH5F$X$Lsu#?XeKm7+ftm$`&KO)~N5 zA5tt)AV)IJi~eWa$lUN8;F#>Rz)5^|@B!G7xykWUVLqCBy1<R3crcWD1r`spL3Ip+ zP@_@8>7aUCYs<sBfpZ~vv??1`wI0?lbtT<T#__>Jd5D*)r8_&*=xv43{P}@{F>*~L zhMoC?4;HV+i&d&j;psEFusocI3tLIjxWN8A;SG?p6Qg=mmKa~EhmitRcMRZrVSarU z>2huXsT^yP6uiEl-7E6aUd!kx$tb@2wIZ)@;T_f#XW=j2lttV+L;uWK3YGl;arWai z>{j6mbUt_opkM}{>EI2w_nYvhiMG(NeLSoG_#S<76qxp-Ik4_V6VCHm#=n~?%{(i_ zvmr?wMKuf3@y56S{@1U?`00)gle_wa`t`-3{`?JaJ8CMG8?qk)+zr{dg*QpmmLufg zxJ~SUy$gA8{2BcFR1deuyO5LhHsDuf%a*t0zz3OexFD?x#uazq+cRz`iW|a;CAJc| zKfaRY-F{UA{97g40!KMoiWdywew;O_l8Kd(EH@|<Z8I)(EQ#&B92y=f@o04x<<tiX zT2?uti|oJ3w_jw0Z#&~0b2X_%E$@=U>_f7mgVoBC#G`#8-C$)=pvf7>d-mYa^h8bM zsW?_NY<iqXT5_|WqMogC50G-$b?cMkv9ohU<;6d%@@(oQX)k_Oh+qBl5zYC;3&U?I zNnE{ei(VbS>i8@2gk&tf6J1w-D=`Sv5c!$(I$ld%?08rGh3HH8TgMND;ga<;j2(Sq z^aUxMQH~YQ7dr+H4zKj5CRLv5v68TuCdt_T(D6ZyleFV`68$eBSu%gWELPfxs$yD3 zg74-e;%Ky;E`6aXrq0KzMdj09k@b({&^~n=t-lnG;n8*6@T*DO>BIZ*<cgu>mX9Is zS3z=iQ)H!#&2&02p_B-{>&Xrod#)$pGTCtR5YbCFp!I(O=+T|hB9rIa9W6_(9UA_t zcXZR~b!<MVDftS|D_7}C99Mms>NvS8TJrbdF3GYa@nlE4%U)F`(pnPJ_5mX8O9_&t z{y!X}-ONNeGD9W*M0E})B@Z3XDZHti5ozgoy*frxR*`D|{aLbuWAYz|%a$h{-{>Y) zI2^lB74l(QRih<W^=I+lDwhePCBt6XNnV;S5gnghE4m=9DM_qIkh~eD%UNWUi4L4t zCQ+K|;#eWB$PpD<o|C8+kL1==U6ITwDv;dQuo5+L1`_Xw5fT-D!BPJ6{i=~ZqquSl zDanS{^;LI!4?C7zh;dA+ogq48ku9n*&8S@ZyOUaqD(H?BZ6btq(^J|d-2A#NP&Uk& z=zJbQo5~`&_HzpK>D^}}%<(35iTfrQboL~Q)4x>+C5D;gWq>CtmK+fR6HTzY42aoF zPtv)unjYHOMY7KpaI3qkID=LL@-pftx$1pg6egd-ISu+KT;#LhUV$|neCB|q6$9bU ziwj^fuAgq_=D~}cI(R<&JWc8_WhX(M=ce3%!hY6EgTl$x-bl(fNyJbe%Y&q$ufOj9 zM%AB>;`_Rd@o>rr>?<C`4o)dTna)vIeWjh&yVs&L9S4gt!Z7OSQP6FP!N_IWtj9GK zgBRO6Y>V*e4<Jp#^!F43mmh#PFY|@&xLp3y$tBF|CqnTtV+ha)r$;Om@%(f@`k>PV zLgK~SAbM3aEXng{N0(P(+S3G>9KL`z(#V8_{Et|crvbX<ui(NG1s0;&L9bi)r=Twy zLCnmbSY&iV5<9F(wD{*Fl$=AX3zA}sYwY>&HO;uG=r?@-ph<P#b%FcEGx+IM5O`Yx zroD|r^VPNF&!QBb2JM9VV*8(@c*hjnI=m6(%Q-$uWjP5R>BtYbWrKUv#H1oDk~f=Z z%qJ$P@N>p}<w764k{mF!APL{wIQLdJSnoNAW`vl~*ej~U^376ezjG%s))44C>jzv! zb{iRa<|+NOFN~^g+YApQRq?i_iepi~r!1A^L+^^yIPusrC+@txwa~I8iCaB5l=N&@ zq`NCKgsERTMYY@_A~z$E)6sn_+@<oI>4>kicZnQb;L^v<HoGJ_@?f(dHzG&aerY~u zwm}AU?e@~6wnxcUy+PE&KY$LOJD=t>g;TXxeL~T-e4%6DVCv*JfGR{Aku8VasQLX8 zF?o0Up=A7_EY9dtxX{tEjz04`LPC6`A$Ie7(JzxYax6lHruxl?hspoAKeOBDyO2;C zb9gpMk{?5|z4~#dw-jWJR_3;k`$iY+@dB09Qjz`2SrA=6o?KlL3@<KPl1`%nBKIba zv$L<Jt6hr7`y?Y=aVUz*?GQ%_u>3UHr>{mou|~S{;T1CK@H4txArQ}`O&~vZGAfKr z5xv{L6ty<ap_Lo&z+T12+|vdNTKBYp+V{t>EfjsoXUCBe>oJ4D@@OjT$c?9>%2I0g zLkUwHN7L^&o|3h-!RQrMPZsA}(umO;$t9U4Vbi2E5+FX_MX!65;p8Gas_Q%ewzPS} z8o545*zk((FFgYdm1RQm<sDdm-3ndp7oypjK29!TG?Xq+Bdy8BG-zZbWV#(AQ6Brr zfU`!><F}6v8Lf=p2T#VV2zMG>a0u7^k|#};v&r-UJBe+oK9|~9Lr0GJMb`KBM{wpP zjHVLbX&~O@f~UW1XK!CUA+pNj*}Hi!$-C)poMxg5T%6ztP7u$1_Vq%GFX4jN{v9nc zxsF#tGcf1%d!f)v6@|Sq<fe5kY*y0Y>mwXQ0WO-nUda*K=li)IpPG}$nFjDKHIKf~ zGC}>7V(O<TgGqmU#5iH29ZpUy<4(Q5BDr2tPjc_aAhG&DHC}v#9yw29r~eKQ%XQI` zJFTQ+(o$jir6=6d%w!ySIULKKt_ZetIBe)jr;oJ{2zQLelYPCWxV^c9*hwZ4#SzkA zpBg6EAB})zgL{P-lNKzVF&XdwiY4963ZT;^Mp&lsT(ab&9v`1xM2?ALaHnfM8TtDW zH+k1Vit$HCZ;35jNtQyg*_{koKAdj<X^581Z|R=Tj>7VZmhk-7KWMo%R+w<Szh!52 z4LRigfV{gi3Dj2j((=+qdg4UAVDe=%(I^^@`mdE>zY(MA|BTrE>oPb_yeXdP9ks?y zQI=Si-igiXGf{K13HPg|gwxx7T<G}jhChcrq7nT{hX2<nidcUVE`Kg0F+-+cj_ph~ z(|IW7)}F$Q=F#wDwk6(-zecw@rE{`hG9>;<2L&HN4pM&f14iKpUGO%Y3~3)kTt$i` z;m{&(txhWubAe1KwEQkv{#}YJpC2Nal#GG8-z%|oQ@g~rw_9=|I+bEgKAnBZ8$zW` z$c(<rA~l;L&fMv!a6s~$dw5TgWZT>nnO;7^&E9E3QjhlviAA%xOUjc-d8ZV0zj{fS zWEVsdej~ZvJ(bqJcjgLo!iCFr9+LdUVliEjd|PtiuPhncuF2(<CDGLLgUOe(A0=nT z4kFIx`9e?HHIch-8L7AV#LZw0q~z>TI(@f2O`1K7JT=k5A#)}RX)U+tp8IP_or(tj zSeMMLpA;=|QCv&apB<u^-umd*ahgmTWk7|&QrKZ*Lyvf9!;vRfIiKK_SE-gOM^%Pi zqlVKmY0HNGi_8h8UoQM2&q8j}D04j+v~vzMY4^tBD_Q`t2Z)-{TxzuX0L=JkN@nn* zp?>CkI)9B8EJ#r$MWZ4h()fw+KaIWkZDGLwhsnP2TPR*OZy>&-k-{^>GIFQuIhVXR zL(slw$pYKHNwyC@!bQ&3VeR{7;nST5s8w|(`0icFt#3U<<u{I_&nBDEm+4oDNm~MK za(*j(^OMG!-~EA_-4})6HlA)iWBPxU#;3t%r~Uc??El99ACtYk+)g~V?gF!zrj3<y zKj^C3ha@aRL<&c@kx`FkaGxxmk}GqD^>1_eqBmg|Nsanz+Ga#y)sTIR`%r+nA#a?_ zd<Q%EXTLy0Z(WvsJCcp;ox|(COCw{&1Duv9-r!fSy$#DJY4fw6{e*2An(V^%0es}8 zd$`|YHt%+8jMz!;eJvgrNAn5aPZ09$D>P0FV$nU;Li{Fa%;fKLL6gqF@w7=e^J+Eq z{?84*JsQi(nimN}FTJA+f~U}-3lCs#UIXVDp@a{&cscDGR$HBMiep8tQ5d)}OH?;5 z1`p?@3q6@uG%nJcRqQq8kA$QUMKvjY+ZFK>pjQ5D!14pwbXb$W8J`TQOCMr2$>LRJ zDYKi_hroHQHeXkNi98yh3x6L6kc}H}P@T{KYGKkK%v+dEkJim$a@}KKaM}&BP$h!Q z`xVnaNsf2ee1lwQEOL6|T~<9a&x%jDn}(}rD`VQNT->Xd0I6@%Se$rf7PBdMgQMEr zc%@B&e3e@_eEg(RJwEg#yc_eMV7GEHpHQgGuDsIc3lDm;ed<@>as**HgX-vzxarsu zH;`Mf{RAAat|I&7MR@4hL~fF59?E;^(|4*{NVH@euGuETCnn#(T!oKVYZzBOMcKef zQP+(9i%Vx>$AK5|PWDyUeDWL4)IR{c=Wv|z`znr|^An#h91P#b#<DM$ck>5RJn%mQ zK%LS>yquZ>QyKG=M5>fx=d)I<O0$5*(MO1K=P@$PUJGsPCJAeXkAYuHJmJv_6<X?+ zM{|FyBQBGklULRR<zB=>V%a!0xhPM}XJ1^;9=bX5=RRCVmD6*e(W^gFeC9l+aQc#} z%;!WkEQ~jv*9LNmk8oMfY|_}@6p(wl2%L*TKpYswTBtlf0eq34dlDyluEOP7sW|Y+ zTPp6GNJfz>WX1|j?D}sWU6+1Dn3OvW73E)$)jC-a`f{4E{-jt5vrC79`&%)aRv8IP zm+l6Kqe@N@gSD#rQj3`ROluZ0%8GyXLym8sSPyXn$FYeqi}}stv~Z8#1nOROmSFL8 z7GIV`Dz@Cg_%>HIAzP7Y`6Y744a%^gU!T?9FXf_oV}aY^ifPr0ux|4^V!Xrznkxrm zoA`DS{bx9qKEJdFCrot4!r|+Q*~Wdu&`=MYuXOjfLXG7&)pcM=j!pG-A7Jj5-spNQ zh~H9r2K}!bqSxB@VX@}^>Y`!C`A~gjeoLlAD44p17sl&VU+VMZJ8xb?&vG}|zi>M5 zyFU;m%dT<ik$^v!#)3xqPmy@#IzKMx>~MIv^D8}FHv)y@FC{x($YP_`Q8-q-oUAI~ z;7Y(=AvRExKOm}t>W2F;FyJzKnO?*i!!EN=P5by45jWXS?=IMG-ip$_VYK?!Q+7yt zC+ogV*w-Bsu}j~YyU~0UhCZGMyY@!H`YUx1;AaGDJCno^WUd0iyAJ|Q58*@e)zEVH zGE^)6LRZA?Au8V;P`b5)6!K9dX;uWp&F0C!lpLZX-zXWnCIzy5Kkx=yo6$YzEO)<m zkyBXkbhLHY!fy&mg$27aSV#LdEZ2I=&**Whe*f?qKXk||A|rZCMCyat!3|{?=p~MV zHmhX3zeS7BdvgOt)r&EIi!C0q2Vr>fK|WfljjmZGLW><^aHrvTvNO~M^e5)iAn9g$ zBI7g++Pe^5&0EEt8}pX5+k9jtF8kT8EeWV~C*8@w+PB)+QH816_9OPY*40I}ix|1G z7V^XQu|?)%o$`E3`5WRuB|0W4A4HLJoWed#t5zGfkD2Feb~3Da%<^n=arN8_{Cy=Y zK4!~Huy}0mH16{?vZSPfv|m&P`TqL3erq=Uc|Zg0(^kMNtGg`i`9ZQ`k`j7*=aLuw z@g1Gj&b&@<1fKo-gIwx*&;MBvi5{Pa^05IOMDFbrCLZiFm5^;|FfCQR+R1Jd|7}(Y zpj8B}Y&3?WbLCjhoSiWL+gyCoY{Yv{bp^I19&#ZPvx53<s*Xc^;CUOAvz`V%rY-{e z`BXCJ_ZaXvw}nbg5n=1H476XDLtalDN>+vS(XPD;%q%K{S^dcsX};c8JuWm<>@>IM zBtOvHn=4!Ni0J1?=u~RN-&mgu1^AI)c*us?8ELT<{(CSlMG=pa6u7Z4o*Fi;V>7aL z!8SK7yx|>32Bii|j!zbF*sFA5&T9i<j(jlw^Epf!y<c*F#v4+$V<`1+GbW7#M?iYf zcz{qd+?rlQC5y%8%+f}U4~S{t2HCdoiw|}2n`K*gl^T0?Z1o1d@#G-hBH|5j26ymf zz|`t18X9a?krFF*{zn}DjAg;@S#)i07B2gImdUNY0^8m`Lqqxge#=dDII}bpFaI5b z!)$%2?fqsFdvqC>v?ZV0zd4Vrt1-s`1C2maT*_mTrz~z6c!B&8J>*O`t!8S8Glj~0 zDSo=)Z2o<B7Ta?!jL(#J0k?liP*irAlYc#(k2!gQ<-e<7WfentWfOURwOy(3c6Akw zZZ>5h`+_h@U4>8Ddy+lV{slLN&c<=u4g*9+QCI&~GAG2AWLS7p3&m9uabS809esE= zN%z0OJr5a+7Nr-7Mq~tCeS05Ss;`W5RMWvvGY-#4jb}ZZ8knxFD%*b601eKbqyBnV z`Sg5y^mT70nsr6Y?3OM+ckpH5>dT?5bcu`L^uv;UpXkUhTe_Z})ZE1M3zo5~R16QN zT!V^xsnlxgbTRhjdy{9^pVKoNlLU=!f0}x<PBfzC0a26OrQLHM(S*}6XnU%ME8Zgm zTY6OR>irq8UUoSfHun^({wQ5N;_*1#G~gxw=iCMU_uvR@zBH4$=}+dX2Wl{dL~W;4 z{(qrY%Nbg3hv2<m34FGyG1GnY7hK=y@UOm$E2y5w4H$FEh3iv}g&WGP)VZ!1+lz}x zt*r)lN$%6%?$>BtOn;;1)5|2wtxMSZsFSleHV*9X-XLSGKX6^*P}WlSfCX=pgS`j+ zs{hk5WG8dN+1Z0ZtY)%^Us;{b%d6f&*Zux1%X6BO;UAl7az_m1$5!wqV%;h1?`37) zxF`%PB05RcK2_EeHW7AS-V5GYJ>>8AA-tUFXgsu4L{%rOAnJ3Kan!my#IyDTad_fE z)vu|*kiIcE(L)7&7O7(U=;!pm^I}*wHIrO^>Bz=SOyj>T5c4)0fcNhCfqlQ9!Ju`a zEHJ2tmbx5b;sNp_+5J_gSV+rtI(f}cd~rmZnKeYi%<>idNKN_b367D-F8kujN*8u$ z`JVrWt~dXu@{9WaWy(w?p)wRQG$=!IuD$mu4H8j`25C-6lLn2)Sjw1W9->I0G>DvQ zuT3gbqmb0QQZ%Q5icsHk-``*EUp{|;<9eKPowL`q*6aCPeAB#FSmRy|dl=Krc_(h9 z0atHUuo5lU-sj7=CB39KiX)L=pB^cjoyoZhG{M>32zA}7gnKv7k(XvgxTU5-_^Gsp z*#^A@QQLbF3`q&WF)_>W$FUc&P&5F=XGd0AUkzt=84<Xk@Bz9oZW%7NR>N$aA^0^- zpyH|3@GZ>@DmLnng#{s0_}!W`-P44~#D(<Y0S(M1f2Oq?7oz;HztC1ETijaB!H|c_ zSSrGWzSQVL+Gn=Y&wu;*=wYMS2z}9Lx|T_%(y!k#b?*;EW|0^)hseYE%v7Xz$s3<a zHo<$yVis{#hOJ$8iaL78!c%$)7SE2I;rgwJxBm4WzPG$ZWmS)<>I+rWVNr{=fdSWh zG916$m3>3)iwqa%{F%=f_krK!eTsW-QBH4uT0%=+grM`nb}A5Qh9Q%j^YnV@2&OJ* zlmxqmkZ5yvJa1Plx_L&oaz($GEI2d>d-YrS<oX22f)2QKcrP<pT7_$WR-#?8*U_AX zSFzI+27~7!;a$@~lJsaTTkx_2pSbr1KTS-;b%l#)y>YMDeO?Q{tBs=}N%m+>{}6O= z+9VK-_-+TeG8X7{xEv%m$03<p#YkaC9un?Aq(dQrOg<jW(h9%e-owXOuKjQ*-F=h@ ze;JXmj|Hq`_9$rFu^Dzn3)#qGP4uUxfyV5vgcXH5@bv8sc#5zbebH`)DLbaJQD<(@ zz!YC_ka~nF^fuAMDy~d-y@m)z4IP2^6-jd!myMt~&wY^FG-*6ACy(A*W<Yvgm7*O# zAM<an&%pK-vzS{?BnzW1So@|Tv0R^s)jcnheKUk4_Q(U6vd)9k{#S^MM_t3;#>g^H z?~}x8!vr$^;8^GhEyJYrh`@QrKYYNxlx{8k52v*`k_}HpuB<^Vocc=I(A#DkF_ULE z>0{Mz=$@ZER1TYgUurz!H~u<7$Nkwsi#pHH@d2l(?5&Y_uKGBz*yT+c%)~^gWk0@z zvW1-sFENETS6N8kB6d(*MV^G}upM(1z@n&;Td9)6ibGw&W40Qev}z-D)4PE`Mk-dO zikfv`Yw#hmVdoNdW9o3GySSR1DzZZJCv@ODtF{tVbsb#lxdDGdDYQ<KK(H$cqN$5- zqD5WiP@xyb87<92UMfOrp?jTfxD!N*Qul&`!A`VT%bg9M6h*d6&4-z?PBibj5j;E` z%i6woVHwjESaHc{VTYGB5q)~#LnaK-CMBmv3FUO7nZD&RGWqda>>k(65_BfB(xfQ% zUQdRM(#D`5>9HT!9f2|v8Lj?L2n7wQT*SH2*uPaB<_z_Q!Ek-P=I1h+trv$fMwvoY z-&xS^4q=&k7VOjqeR^-zKP<CPrZPdpn)yG9fd;)85iThx034}^KUhn#r#FYOCpr;G zs<D=C_E%({P6c?R_W~q$HJH_zy~meE*TSV`y|8>J<3@aO0o|-7Dy!hizYb03uezV1 z69oq$e2pd+f2^bCxrJP_)dhUuRsoVTvBZ6AHt~D!x3ZQ^3$c5V1<YTpP#HNxM97FE z`K)JE0sE;kkGY4qk<ki2K*MJ%6Y<)@H9jdMY3gPaA0x@Ohwj32I_ikx(l!uFm7#O( z-*NtrMu2}BmDL`Yl~28j|GUyie+V1sg>5nX!B10Q-U}m~X}<yo&rV0o)ES1Tuc4bH z@JHjAaokSVzf?|CJB2NM7ROX)Z(-9Mufy~$T5R>38~D}huQ*>P2`;*BhtwTb;B)#t zG=E!0?v1mBza~!XwChvo=!wCW{d#QKQ3s^$H<UD%7UO$o55oDv*Vy}$8C#YehhJRt zg=b@@QG+GlsK$j#BxipLHGA8G|H7r<H^d5y*fBf!J$-=J3U-IbsqUy>MGf+A_Tb+? zoZ(meY1(MFkbK0h%%t}&o^HAgf7)QqcKuKx+B-U7`;7%~VV*zat<U8k*&Q4twf*Ux zt+4;ZFSPgWJG`b?i^aZDz!N$m@ws_xDmKqbz_mJYV841UecRhhsi~d_7rZ*l2hPtz z17|hhjw|E0ufEL<G*lu}DM_-*-xhsHH)p~RU$FDy3oNqX0nX45AkI>sSgE%fV`V>a z<c}`cnNI28jpM9ZHJ^E2F<>>9PvV#Vjgy>)P4KTPltoE;2xDv{3Y4!WS;{6Yx=Z~f zNZm@qYXdau(f>4P<S(yj`#&>Tr9fMeRkscd*HOV%A49S23{yP)p*o%>AqzGYd&24f zIlgaBDsK8}$fqs4OxMXI;y9gCR4QjY+}BuxL%w*R(3zT0d1i$Kg~&wxCNm-9*d6{n zCy&ks`+#&w9zHkh6?Jte7QZ`Lg7(InU`YTI8rw1}{^xQ+TP_VbcCs>1D2kl&!M<|e zvKvR|zOm2!YGMEKvp1jPpmc3c`U}AVJwra+wo0&O{t5dI;nfO-cW}czA*rId;UF(% zw?U9w?8Q&YY7|tK_g8dHJ0qU6vVe0M_e@Z(iv=&c=LvHAw0ZyAz7-0ma|FR794l5i zz7_Ybe9z^mIrAzXl?An@-iqwgLr)4+jkNgZhvhko%^l(kZ^Fbz<Ff?=b(bpgJJt(+ z8NaeW;h0*%=8oY;u~7c-jA+5Sp^JHO%|}5}4e*H;Dgp(EH17KT*W$F3t2p_lHo<*O z%-jC86ddu8vDcE56U&(?@_uH8f{FDfxx$KO?zk&LmygAu+8$99Xsn3AA$i9^DP9NW z8{b0#w^}K`K#O12R?p3T)+v7Z@;#U9^@J{#ysE|`JAt^mgx2Jw!_<H-^y%nMyfF0~ zJ$3B~FYrv@yW5x3me^ywmPZ7Ag&0y(dQRP@2h*dyjaXz+3hgF0u%`SsNWQ&=HpXwE z#-eU>2+jFRt?cDldge*)-~9%x(mft)zB9r$TesmdWf7D~-GEOEt(ouBKz#7C6gS0O z6=q*8f|tW&*~0h%`ZTDJ#uz4`ICTNtI8Kc3kMqHf`3qsX2FCj9bFu0NBV5;1fc>+A zz$&&FJEZMKQ+Bt4vf5cJ%F?wU72U(xB)tsY+&CQ>C%mABA8x}yVKrUNywJi@Q`V4= zcvk}zd{=55i)2^m>JRhD=;f~Xh+i@`PZNUiocAa~JdKRJtwFEzwpcpa2GkOzi0t8L z^4a1SZZw+CN(w@0gW6G8^h%9Aj^2mn@3j;s<oJuopA!l2V_hkxTF%sXP>=bUj{+Jr z2O{G45#7=*wAyI|e$jghFEdhsYXRrE0l9GeeZ436ddjh!WA~7JO$ERHv@DxdQG+{g zZo?53myqqKLNL0QhD#m3!SwFaH1E_@c&>eyZjvNexwjfjkCh`On;*EoP4ZKjOK)Sb z>QJKUsE4jMUx&t62EXj*;-ixWD|TD^BeNhIPJCfE6Xcjcl+h@VHJeN07oXuK)n(kc zzR(HVo-cyyUytGEIcLzC(IfCgcVpa?9SSoQ0nhw)8C1@VfWJGB<A(Al_@`vxESu|$ z4{SsNG`tkMh{PAMVSy3(_9Q|)VOkaP7r*4f{;bC{qeLLDt3>ivl)@TI9c<q2hBwAM zhXg?wHV!SN6aSe&Z_a#}<<pI(>@0w<ijA-$Da<}nY(sSX0`Zc0?vOk5E#6;bf&6Sn zu^;E+N#4zBkTscyBYou9l%Kbe<`)$r+9@?1(_iE9B)=*kYtLgRX#+ZSsR1|s^9G3P zb;heRMu5O32v0oV&gMj=V$)@xP|@dNWTzI1i_Pbdn+p1{d}}POh+P56nv(K7EQu}} zxrWMGeWdZu@w7E&Hom#_DXM$Yi{>nOfnM(n#&eGCrf^aQ?k+tqq7%|2EM$c$cD+BA zdVQYFCDM4h?Mx6TFMCc8jda6uHDAz!@<{w`-zA*!+7=$&te{0JGSQWETa@%?99}ae z0CHmk@H45=kn$lP1$uem@7f}4TksTy-aUdAZ;KaHFB^^LjPbzt*C^r_$~q8t)fFx8 z@~6tTML$s6gLSZf-C6uxO&=C#>d^$pc+|6R923|$;;~CLV7x~Jylfmor%(JL*jIiK z*{HMwzukmgY`KE1f3G0ZRc!G1P!;epy9M@AIn-Zu8f!SilU0+~GmpVVoY-**ey<_L zI!5ot(WOQ-d*mnl#rhFmJ|czQ7D=o`equfLxnUaqDm)8`4#{lhcqL+}XNZeOH{+~- zPw|*X`>4#f1%eezwu(a|E%CTj<8g=di3*PsE^tUk74PXVf>`Tqm`h&(K5lXl{4$T* z=eh?k4jqSX?<?eo^c2uT3XUMNPad{?ltuXJ1*iy-Xt($jB-)wMi!KKo!%+@?Snt`j z8w$tHP%m#aZd#oSj2+{J4j(Yacpx3^>3PK+9xSFamXF1YQ>WnDx&dI;D~lY(X0%7| z5x(JLh(m4Un01W{)E?@^EiDge=80ZD{hcd*Hc9d^z6wIOW(T2wz!V6G)y3xDrlQU* zc_Nh9WC>iR6n?aBA2Of0g1+dCKw19taA5r?x;+0p?R#tjJAwm{p!WsDRLjG4H7W9S zzrG};s*4xiHD`T;7Vz?(Cz1U$5m}!Y%J{1hSoSid1$WZHy5%rj$vVdc{=NyNmP1KP z!c<T@pMiE^3a?jN;y+tOlfm)TM0$Nf2nzGti}go8rw`vbK~#Yu{S@?<uUB;BV!Fc6 z!VyELeMS?#5^#yXqR7+BF4y^lw10f@sRZtg=K;Z@UQ_<>TTArw)NyY1gPGjT?0$Zh z#|1<$Oc2L>9ExQxd`8JD>gg9%cdj#jF*obbnF`-+JE*%Tz|ii$&7U|;6Q0KQHSj<8 z_;8P!%j^dqHt=R=mebtB4XF3*Me21A^UBMkkyqjg@nH{Z{<w;epUw^N(_Ccv7vwKz z-tmxYE#6OOYY6!=qbx4q$W8k(dmF_YvtRM2hjh{lQ;P)`5|vPhygRy<HI2*No62v| zUr9xOmbaoJ%ddQ^kr)4o`^1e_%R&nN)6m!_!ysizEH2$#gF?T@BSjr^UKkO|m0oh@ z`Ys>je#ni2bEAkLd>2AZ6R)EO2TUZ8{|rcqGvj6AkD{GhtU;i00ZZMRMTc*3riB~p z=_Ef{oV~q-`|2Hu5-v2*Z^cz2Zu-GDylYexf39N{_x7AVI)CyH7kaUTKWr1n86DQB zXy9)03;AIFZbA_!SZFIg(3DTt{8XWG<@qRPQVy@Q8d0mEfpn&oI_KE1h{rp4Zn9k- zjS8sZGBj@U1F~1db8{>)y}Od1RUgQED7xV4gI75J3tLh8T9GcN-jmF~>psi}=ThEs z%~?9PCyHNi%LJ)kzbvR|NZ{HohV#Dzd<5fH775m>NYgFjvXGbST5iKvc`Dc!MVlqL z+^P@(J$Xk1PJON6mc~o-%b#aanSP!N{8dMf{I;RG{TUqTT}5YYagdPYl~l9dnftRl z8byj$*xEn#@8WuuU9eJO9@;DX&MC(YrQhc`(Uv{UT)XfH@4Q|KF1V=TMD8xtPt&3m z*AF9UX>ZBknnUV~W3i>fY2@;!4b9Wd<omUyv3iFIom5zYSb7i&vfsrQxhK)Xt{36n zjg|aby#oG$V<ES9xC@#^s(I1OuZeWw?(_CW5^8tt#uVN>`XcY%wvHQEAdhxFx1jnF z9D19Q%RLl5ws+Cp#8-?PhUVYy5qx;6k9%kOqMcnad`o7Spv<a;U*x!p*I9LiKA93P zo~-SLzM05?#_&sA1oIHMcK+Z;PYU6yw*dEITQCZZ&7@4EB|?9{mT-Yv9r?oN;k@Pv z6aMwIwP?F^DqWCs484&AK5AMVX>dapuOJ(S1}*2J?cHbi>#A0KWchLWq0Nh*zVj3p z>ok_TZ(T?yxXt9x<har=b9?wX^Wyo!C>6A^G6^YXyP-#Sy=e9J*|2=<A^cPNE?4fm z9tHf{5YI&frEx1#g2W+G_o&LqApW<;ZceZ1Fw%IYfL3zzP+E^7SV=vjL!5N6bg(IG zbWFyH&lKr6`wV*ITYz}2T?DdzBTpY*vBd1kQ@ZCIrvFo8r!|ds`9CvRKaq!MookTb zp5Yg+%Mr1o5}tqRtx)c3hvbvr>I5;<wJ9^nM4vCc<KGQyMMilG@X8r=ybF=zs)aM) zVCyXEVI7Jt>`|a=$rY}q^)z?qNvk+L`4JzKae^~*&ivnht9Y*P-~W3it9sHo*jjWg zVI4~xK9BtCk7U6lm@RrA!$O|elJcw=wj558N2>`Ne~Yjs4^Ohso$ln~6KPhl(3(5o z>CJpI!<kOJBZ*sPO%B~S%r=QL3Dn6jnMO5M_2U{&uCZhvJyLMt4h!NdaVY&tE5Q%y z<-p;MD|`K<7h3-)h}fdYjb!TL6ZnyWqr}`cl1=d#PZIK6NUbDbz4Da;@$&A2B^%}v z*Ma~zKH)b`Ra9nSACM$?+Xa*J=aF|Wrjk@*!^R{IA@33$NUf+AcRw*AxBm)Bu2ql3 zhjakH?2#wj%5mi2kUl)@^=5XbJQKvXoXOmH(HwSuAPhc?|3QzaUcjGs7_#p7GnksI zKD!0W+3vNoNOq1N(<WBTbA>W9Zrem!&pQ&0k#})axH~EM9SaG$JbJA=hAlCbz?+|c z;XCTP$itw&Y?!<$)!6+Fe;JwyT@6QAs8Esh7fD*=U8nHYOn-jS(rk8FWT{Thoo4t^ z+7@Q|a|k*3CJV;=w~);98cj}TdNAKAW1@W|kt}U#f{JN+<Y=RS_>4^@Ry&8W7oXRW zDLxZO{cBD3YGN?+zuOLt8;3ICu<y8Y?_%~TpbbxNmLm?+M=-}$5iEG+#d<Z@GdqJ< zSUS~{-To2AMAl7Hm|HDoT5Hb3m-{>5g}W+yT5LvYj-Dhky>m#y;0GM3--2~4k2CX0 zTgmRwAmTD58OI$y1FMUcvc0MexKcfpy&HQPw~mQ|_Mkar?bJwE5}%LLEB{HtdHuMg z;T|#9h{b0syOH?D0|6PXO_H_ZA)$4zJ6ycvhJ%hwfN_)V2o3~h)7vLLP!Xx3*1!B< z?!hX!UsZ#LtS{&Hx0OQ6h{d>Zgd6u>%7#oEybZr6sgQLwPXBwed%vuHR4FYbRVn@d zv*=+SqOqG(kynf(Nt#khOYeRWtaHewqr?A$yQ9OYj{Q-3L0t-mE%K$i*52lRt!qN{ zN21BHt<~%YHx>IGu&%Tid5&JZb{*X~8^LD99A;P2InrJ59Uln`C5b_OB<s{sRv}u> zMk?+H*{Lg-lh$uKj$VZI7GJnF+0!EY-7b<C&q#y2btBj*_zsRG6zz&Q0w2Q!*k<2p zl-MnWO-~eXKaCY}X|OFg%#ntpQ=js+ub1MPD`%nh#-H$OzvNApOx4!QU1M+Fmf%|h zCgjKTALL_C65iu`9?w!UMsm|OWAmM>xX+tEFn;POc4_itysJyJh~2&DkCejN=$(Vv zY@e_L>&%hC(;u0^#lDmDvXcwDu8@XwQ(u8y1;IVxTG0IJ7o94Zptt%@#y8a+!F9%a zI%qx%Ngqf-*6JO+Tuln8Z|lRiP3ocYr3`r`AI2$1oB`E)Q;2ujQ;2gkBsP8otxhZ> zZle2(xW4WUtoe;KNgO{32kje##8@Hr537>|#B$l_f7Pg_xf-u&--|n6-@x}53(?je zv%y(<vheKmiRj3f$*?~=fg5u_2<f-|Lc2cf1o@F|bm~~j78mQ`pSlDd%+i8|X;R>@ zBMGY0{*joiF=%A3q-~^|fJIHGB)JhYN$Wj!BPjG|W1mJK_j_HaDDoKbvv9!=B@)7) zQ9k@N{gE*E$Bq4$V+UNbGML@kiusvoO#k90ai8HXv~-pk*%XjQ6A#&g>&q{kLa-t9 z#D*i&)*Gm{(-<c{xQV706r$yVv2f^#11l{nL8o2oM0oUfo60#4zCt>A1&wR&;<CDL z5Hh<BTQ4K@@Ze__@Z}w8RJJ3>cjV*N{zRNQ$%Hu@oB^l76dFG%oy<O;%1-YIp)%Y; z(!RF^@J~rdxj>P-H4z}LVSqDdV_?guAbKabA6eKt<8@x1^y<d5G*8?{y{C(8@wTG| z5N^<6|MjFU``USr9n7kQ6_<mBdq@4Db6UNa$@rb@_3}Wb|8M~q4pGJtY4U8;k~9*e z`UEevFk(dKDPHxChb3lpxa-mqVwk4}M*rr(4@W}o3-04{MPghSCXKJyH3E116zc1i zgT>pVIcdoaen}LQhHiJdp{0Ea*tvZ>bWXZ0;WZ_$*VP_m`due>|IGk;BD1~ny-Z@| z=7TRtpKUvPqN~WvrZ_OGSrq?X5P<$gsR`$rM5B=DMbx{_m#G+QRQm6T$K`c%$-8g^ z*5Gp-$GOykLV7Wq|87g=_o)-G@W~hM?3`I3YIJi&Q!g*4e<l8v?lBVh@at~mls*lv z{a3>)9LYr`D@Nd%Oa*q<zK4h5o$zUsBwMhLBT7XVS+ukSR6m-6)z;s`HI@2s-Byu3 z%8;~ca)QX7+ZN<w?lyMuY!=K9nS|dwmB%9vH_<;S8_ABsdg>jw1oo5|h=}|5k*GJR z6n2KFiv<S^C`?~TpKq|G<DXxqzTfgWv-LhWa*r`s_5Va?|1^j#ZC26u8!$|`sf)&a zG+@u(^SDiI7Jl}w7VLA6l8s+3QTwHHVD<3%Fqoc2bL3_*I?Ys=-T09{sy)VfV$UM0 z>w!pFZ6o*jyQrF`svLou+HCBVSWl<<<l|l>#~fCU=2Sy((4e7>Y;=elo_XdavVP^m z>Anu;n|jycDK6t-?B#m2KW7khf5oBx<>j=aC5@~vPh!1f0N*k`BHYz;QrOaV5{oZb zkq9HL%KcH_(BJF3coW6p#Q*&W;hQy~LLCtqCsdC+Og1)~2>Uj>GJ9?ZXl@xRbXy}u z+WTk0(fVNF!bC^mqO6PLcwIkTqFRog;Rr6Ma&E<<zFT}p)g{igJBM!gA&bxE>0%ST zM3ntBOkgoe1!N9qqsjS}pknnE-OXEFnPDBlER~hXnD{xYVzL2wpFe>}#7|jxWThV) zIc6_wFmPm}-2<7|L|N#Y)CS-GIp7J?Hel}oW0K)~k_uLTrgf7Bfz^fMKSkZ3JZc^o z7J0EJt(tiLP9wV6?J63t`vA}MJWS&ybbNcpaIl-dldZoakih4jXqUxAXex{%rB#2C ztaNQ<MYE5nvTs`i#`{jQknctC<j*`1mpH<VkH^^R@cEV6qmMz^pgnFM%)pAFl3zxn zeCic)oerw$V`aIaM0ea=yy)iv_U*+Z$dQ?e4(+#w{twn<{hbLAXKjE7y-lEWK?+j7 zAf#<Bp~z#_RO)qw!srSUn0Z_hIF}PKY*)0J__Z%1`8$t5gfM|bo0c#ic?B}<#R+1Q zEMfKbk7jmX#*ka*-?9GXU3gS!5t-o<je{DGF{|gr@H@bo7MP@y2)Wnr!e9d4*)o!i zc`N}6B~)zPCnGGgEQUo7+@#{r>DVkX6P-{R3bqk9xw53UoQtT_1J$jt!QKTwB~ij< ze8l1hklNi&Ovp+SKjD%vcGz7ee|bJG&h0^du@h<Dk!<2v>_<-Peqh?gV=6bk{z;3% zCRctq@d!^^bcS3_j>g#!w$mK~dEtr~57B~8<+w?CHL<N02sdR}Fq0!2(2b6B?EbFx z)VuMoK=l5>T<m<lieIRs&vmp4apntu`Z+rU-?ooNFK4=-4L={^@m?~*?h`UNxLsZN z=C&LwTj7N2wjBcTws4Z*U_ed?i*U-lG92tOxiVTa32tv}N6YAR+%n&fT$}V4@9*rS z(wa+{;p{>*@F<EJyB&gR^b2c_(idTo;|8Q)XvBPjrLmLX4RZY@3%gxY@Z-&=&`C{W zaFNsH9Bk|0)T~L8w%r&S^2Q3g36Dank_)?#Jyuwy@_=;xm<PWK^I#J{h1{&E<a3t$ zqq*1B@$Q$YMDERWq7$P^R9QKAYg*w$8hNN$@Ei=wHA#4@NRyQm7URF;<cQuvE9m#| z!ulKEAu`(%%(Rzc4a(Ei>Ov~jp$z*IzjMDIM04R`$5GwLJkHW)7ETE?;5?G#@q9fs z7NRT-_qHYDwy*6Z;33Z<MR8<GlM{wHo8i~@X!y8B2X}utf&>2<vjvg6KtY0VtQRGy z+pjM$!c}#S(9yD!d%tQ2=qg-9i$B>i=aY|VyHPyXr!$=XJ(CGZI+982cpuQOpAOd* z)6sl2SNzT7h@dOogLX8QagUzP;->B&3f2naprTHiY2q==Z*n7vN={%N@_F24^(AC@ z)kKipd=YU^Zu31V|A9!|V2*IjlyFFKaA2Pjtg(+|s@@*|j&Av!2)h#jtug?raXDJ7 zXY~+2pK=nG7yN<>6CpaaVhQwaHiW}*eqi4;14Xyi(ejnPXut3`EzYu`(c`nQ!~E%} zJFc1ca}9$VZ<3hz&5dMX{~u;?_dm8g;f$nC))28Y$3rZ)FM<rj?1Or}{pf=Pqm@7K z6*l<mvziWxltShi{{1u@TmPlhcnQxEf(LM;>J)VLO#mu=TFoi)V_4O&#mEm$#n*0* zh20nQ;iifKvOh2!dCD2$u_Kz$giAVTdf!QS=Q<5`HyXm#k5Nc>++-rE7df&#hjjA0 zOddP`^I+eUvf263<C#`r2^|?wiGJ0Eunk#P>E#WX<iPeM_+6oo3eRjH<#TS+V=hT} zSZo!Ro_`UC{FQ`T)0K$djZXTnT$-@=@3@>5Q(69H512L96d(mq!Q3&le_|3%2x+6! zhPdJ$e~!6|0xdv$)FZTa?+LJc7s8xMWI%m-7ovM#Rh|!5CTK+g9^(Fr3%YU|lI5qO z{~{-|c|RS5O-(w2b=pBhW4;k=dcwnIzcFn1vwRqlagb@Ob%Uqdqw(*u5aj>j2>Tl_ z7WK%bqZYLs*psLX2EygMwXHFzh9D$5_`((44@yL4Y7bE0HdlN=`aQC97Sr?Y!>JD& zMcihp65|6I<eh@Hu=mb&GAn96iCNOeD}_pekX2gvP~Iv+q)o}z2f1v~qjap1m5Z}) zEN5zaU!#qNui@4Ao$T|}FtA#84UetphN~aaQTW|HkctgP8;v9mD2LBmq2;GL=2#$F z6!ewfG?Bn>2_CU{ts6h!_mTcEyMPXTX+rj0Td`l54D<H9fcC7_sJt%P%Z83}Cii|U zWWUSSvwd7K70g%&uB#N-nzgpH-Q^p<Siv3>_7u7@Z^NQ9GORx5JU&0(2Um=&g2YFm z*eFm`O!wAEF`^t!6OSNVq?8VZr`AElmJ`Tw<03peaFk?Sf)fN3R8rq`8Qf;(jw~`~ z!ix9}_}$7gY*@?{qQ9>LTzA)zgM-dwwLp=q>F#4Gj{C^##eMMQdoV5D83b>utZ++X zJn>DLO|FQKKyq>{S+Qg$NQq8pkspCu$d1Z&*f=5$jj7fL|MLL#&FAo~=sOfOBG$V* z5#^Rt3ZBSVgM-(27#}hMTbifS+I&5TY+DCk&T3G*$mf)Z=M!feJ@#w%M11JX6G?5a zL-uH9;)f+00A6IXRF^H-T~CSl|C&PTf;x$wS2hx@eAxoKjC$zpXJ_$?H~swk8xKIc zL|<&#G=a&k*#-p(j%2ss04YsXKrGyY<{Ni|p+y;;{^umh?h(_(v*FaUX@G{w>ETb3 zmM-sOL}%;ABKL=V{GQ3{NJViL=}dNF3g!UM#~hIWszb4_emME3>qtBlVnk5;WGsF> zVKJ4sotfA?k!Al2$NTeJ@t$SFS?Wd(yH=`U-K~o3{0R}#>MDbseY4o!OU3y8Q73%K zOqteBU50&sJMc-N6Y%#KC#+PVK=U7(!XD@AoPCA{3bG48`$XnwUEfP|+Qb357!~3% zP4aC1trVi<Em9TE&D%lvbE8?_$8@|X@gCfKkb^(kzChjY^Vy-aXfiyj4j*Y6#w?$O zV&@%^w0Yind@g(oY4&eK-J7l>>&u>G|Epr$w)qd%HkGE%{{Q&n?~{4GKrL#<I%%=` zS8fApM!&kw(`_~m{L<}LFutLQRL$d&ljuiXg<4nwYdJTRS(e8zn<tW*|LGaheH`Pb zhi}61S$^=QO@p}go}+1Bvxv=|GJ1FOc-Ay79=fk5!h+}T>D67KaMs6+l~mQB?=E`y zo|yuOJ!9e6?e#EXVK-GAwgA6SY(=YzI%%%@Y3|=-RTS1UPGIf-9}<1qe1#A9DTTVy zH(bk6TO6e=#sg_1@H%%}wzhKyij|}=dZN6Av#)I@*Hb^@#vVEL$JC1S{Tsq$w(e%L zC4Iygk3<wCJcj1#hG26=DTuG>N4>ko68RejkX`6UeWDWKozqudZ_Z2XUGf4c?~uW6 zGG_2lK3#;)7Am88uU+Y7Yi*ij{Kl?yeGe)<kqfI#jM1lQ7f?#=ZLEAa7~91hW9FOp z;dMJASz}K$6K*((W=~`6U*ZNvmK<Pvo?T<b=@o2z;3e2JV?Jx#^c&rqJ)M7eJx79d z%%ICek`s~h#Pxmy*x25kT^&5nevH<b|G$lq*GI>l`9FDs`b1mN)!xmJdoz_5X8ope zll|~2?fa6Hpbgy+YlmC*Fys{&ENEXFhLdHq;LN0GJW-|ueHlIp4s;&m9NR6>W_60% zrDWjuq0!X9OBv_ZZ=$!GOkl3;VlHvqB|6|-Eile$MTfl(bGhH<(>paQ>0!Bz|LY7C zl<gTJJ{f#UD4MjWg!5fjN7p3wQmJDyL~pYKOl%dBM!R;JcV{78ZGIG;PkackN^Izi zUM=EtA_Pj}DkZwUE`FlFJ5t_ugO+G^a0h0^;f(tR(9(ATLhGaO43G1CZ=9svIFOEa z3ZB9AuujyJB;f<4vT60Fe_%S&NpQefPJ~xPOB^o~GT?k`G;N5@z=HpdP;p@t-kaJ9 z>i)OUBR&!tO5B90>s+{w;6ZBesS9Gh-h>I+k0b_cTlT^2G^$A+2J%-V$bhF3e295~ zUcN4c=A~w|=G<&>H@ZaoS7d>OLJ0d6Vn~9=|A95n&*RSU8@RqmMDdOUJ*FeGA1|2| zf-y?L?N%nRc6=C~eRmmT+djrOeJ^w8Tr8MZ)Cf@W%#&0B+C-Vx<Fv;7f#5eAL0%QH z^?T-Gnao1Sw>pEa=w8FmPREduM-HR=+6pwb@jkx%?I=x6ekVwF$`%*(oE04UQz^di zy2*Zw=tms?uxg3mz$7!i%kwqA>12{P{)v>JLbzV6k(ABVOVx^RzD^S>m%8$nUAkP9 z>pWE6)yB#F+`~V+QB*PR`$j&fCxJh8;SG1(%^c17vO&DH#15J1YuHPVJyqd$EtAX2 ziQ(rtmI`j4`)zN!K+|4x#M25bpA3%E$`Om=R#|d?BOY_cThG`ZnmA2-$#A6oU)j}s zmYqN_PAN*DNxus;=M3i^-rG{^1HjMGsNjsSyuJUrot*cV<J_*%ZM;mbEd4OLLfpN5 z8GU<Xk$82JKBt$H$zMxYfL-)fqSC#xFnP8W_aXTc!rEiOE9Z{5-qjAVIa(sBX=;xa zx18tmUT>jd?`eE@#-RALgiA8;OrhC<Li}rin4c2mg{HMH013;<jbHnehMC_)KZB;? z(9)yWf4Lqu-FOijg<eE?Ej_gO;Yj>$%2Ei7Ue8IVJ;9bYb8u^*EQ-<Nany!>+A(Gf zmEyMI)4PwsM6oDc{4S&k|9Tik|Jo(sIoA5{(W4BF%UF(WR3x!bla=^e>wfAlS;_FJ z8S@j}Oz9puOmYen(Ea86AxvD4dV^gtQqM(O4H{sIfegIJnvF-5OEjoUv_SKI0sl5B z6!QVw(R$|?2yt<e0Labg(|s}Cng1KPiDdVHo!My`@`Qslg%l_pISIEkxkIYkITSut z1txzwhHu0z#U`d0&)z&6X6GSTX%P?C4@Ba1Qzi0|s-sw0^Bp%Wv>tvojbytz#(|&t zT$Z=uI!yTd3Gcj=!m6Z?Qm+Z)!2DJ<`>HJFwfC38%i2K6)|@~hdb?&i7cw&&m+V*! zr?pmqpRfbCLUrP{RuTbRwGyV~XX9&Ko6w+XCWPpg(t)8D@q;;2Kr2z=6lvDL4?bD( z;?AkCW0nd^t$K=^OAR2O7m^%K4=N4^;_ytK-@CgVzP%p{!H#;QE*fZ3NCn>eTnA>( zQ54ZrRq6QY&s=IzA=%yK>Q1-&Z-eATxhSIFPVl|-9S(hNi_K!Y!KwcOEs%`|EjE`u z-<kvo_1eh3w*c!7{GmT96k$X5SANteJ1qRX6%~Yrz@ul9hYMF%T<&@gch7!;W}3Cb zvp<b=W3@9ZU3CiL`_Dp9m*^?XG98U_7M+D2`(*4Cl7U@+5?r}k2S!TC;NJT4;8=DZ zZ&A2{i)N^i{>EtJy;qjX5o=_7#*3DpE$13u8j~H1<8fH{3F?_KfKzT9!L6pXxZ`LG z_1tPmG+rEJ*F|p3W?KYw`*xy4H9I)oaRxVr7a@O<*$=$VMwi5{J^-^XjHPpjlt9!i zHC*I>A9T-Hpv7t1@SJPUsa~R$L^NZ-&wr?fo?lhLOJ=VX=%lzKw^R;^?kIChcmJSQ z3e~9bj&iZf8x8csR|%bZ+$vCW9F07GD)AXpO1XkbMM(RzJcm_vsh7zTx>Pq*EDF)z z1Svz7(*?f}dg{ECHpPACzwCKVzfF3BqFl>quGU9Be98h&|APU|>6F6*=Zd+Z`%S3+ z=YJ@wOojiEQpb0Hzb8)5M7;F#`>0iMkk7<d_ya~#w3y$9EJLq|_hsDU-S>P!2R^jY zi2GH1&4dt|WlZP-PoaoMUq?|>m$NjXU5y6B`qB=U-yE9j!}Sl9LKYKO^9Rqor1PCF zp@z=ayxk&oJ}5K_CiPcvD`)RQ!(LwyTs^iJ9sl&6PTsT?DQzco)2UMAAG4jhe=XwI zUUCNSfKHkylz2lQkA#)AjcD0U2XOB;ME`Zqr?X~=w$q%Ak@VvgNpo7yP;lB<nyN@< z&gI$#d_YqnzgY7k3Y!=wSXZcr3d*|CM2#@+McMruf3@Eu?Tu8tP~`<@cj628vDO!b zsoHbuBXT&w{s>;aOpWXPETWm!(%AiqGU!VO^35`_;v!#N`bbrTemLmRcR^<KvWN>4 z|5@UKiW1~-p;xG2u}p>_{8bUHSTG(fXbj_x2W@!4Vr$ORL7Ka~*aDV0YT$F47dWYN z>1ec_JYA4G87&PA<BMA}xI?lI+~A#I$Tg~uE7>VcHCs;7+Xij|Zy$5IBV7}8)`ZZE zdn@=%)odzTCC5dV5i0t)A(fX^wn04w(p0xGle@J24(ig8LcO`i_==HAFeOWqcM08s z`?@bdU#7Ao#>vwgH+<0k*aYhM!W4;@F6G9=FXg{jxxwfslJai43qNa*E+3fjS3K-q zC>(P?g&)a8a2s}N3;h4+<4|8oXZ(UJ*05HjvHLtl^it#tSl<1ZHoGZe&)h{Y=(CA_ zw{fHWz3udq(o5<*LLRyVyWxSOF9^D~p^I-{((fA|i^EcRSn=HsY0uvzNO5nc|0Nco zr>ku6vDUrlm{A=vvQq}Bvz4?YJrdJaO)6Vnj*e(01C<!CKm1&YVwU&=krsWVVqqeB zrlQP_+@Fe99P2_Yn?F<kk;CAwiaiQ4+zvzi+TmKf0$xxi4Kb0*5Ed~KJ^nQrn@>!} zj(Z$&SmFt4dB2}ZcMZ}F^Lo(IvlwmoWJ{l)y(fwLNc2uyG;l+Sx;WCv8vRwUL>Ke^ z(%g^w*z4|E*eSZOjaVl%Q_rl4kZ7fc-8C}tN6jF5(aHp8=DfyR^b6tnuSop&`%GNS zPr|Qtq?qO!FW6BY3K{1vqKyvkkxBhhTwYQo*~y`Y*Q|R+x9Q~LW^No8p5_8&r+wf= zlrcV<s|PpQcJtNrDgWK=3;O4%!gV_jL850KAJE$GQr!CKk?33H2ehojiC_3(p}66v zCOTc-$;Ga@#r?`SA`mVf!{<%fMFXSy>HOLYNOn*ew8FA@rLs}fxOOCOc62?WE>=`) z>Tmw)$p7f?st9z%SOfD}dHh%T20jZM>4N9&0^frN_?P@E!HQLH1W%0bi@25fijel* z5brj8C^(dKUXWPp&Oeu|Q#lsoFL>HHz%}fdX>W2rg7Z|W;+{|H=VpJAhSP(e_`bM@ ze8nY6{O6}MEvZ<DvOCkcCD)&!ef9h3&WE~uTVMk>KgR`CEUTrdK9R^Ryp2oraiuS^ zf6(HnX1-)zB6c<1a0;-tG|#Et=J)AWar5q+q?w-Tg3N(De*7dAiETarn!{fq)9#%} z@r^zI_^CHqGDHSC^BIzi&Up8*TpHaHjEr`5P#SpysaSU*y<N-xSCG9WFy_Gj$s3sG zc!*TL7()2^shrt2bLh(pM{k5NG)7;MncURHnI|MG=nStR$K98xx0xSOmkB^;rc?@+ z3_PW)CRT9qbz8a2t}*o8!%>{;El-e+@|Kj5jr_l19REHaa~?*`{~INiQ;}Qw|IQmI zpH#}V71_^FL3x%DRItMeTW?>08#L2Uez`4p=RM$u%D&~hPpQDagUevi8+FPaEW=@4 zE6J(Xy(oh(W9R2=BdHyP4CF;(_ZcTi+a1JCb-#j_s}P6|JRsAoVqvx71tgHajrmzO zsBg73b-NP=d4a2-vdN4%t)(KGUS-F&-SvVV{D$Y>EX6|86nfWj2<Ouh!Cpxg#M#fZ z#p#bLX!mkMh`oB0e)*|^+{%?u&*=bM7jB3I*5NF}hp-bDbl^`vsk9p&g0oiEG0{L4 z+pTsH9Ol2Jx!u#)J(WxBobm@EsJ#izFP+(=${tYOD00AYW^r`y*L7HR%OtYO?;>A5 zkKllkb{x|186KtC;n(#W(7!v;oJr0}wk>=zj0(30-wjFV<L_o1`DKf}S?VQT^{)?q zL_L7TrRsyI*pf8s?m?N!ad>6VA>yVV&gyRcA<n_K@qLpcSkqF2@k@^r&9}-BC{m3^ zU$->lja(ZNDUHY5c@<(*kOAMX?uYILi|`T=hh;SX;`<*w$mAJ3wmF@IllxO78ux$H zZ^WAN15;!94;oP@#r7_@LL`Nf#_3^O`Q!BTS34-&u@E~bc+n_{7;?~UB>DPE;^tGz zC$%TtaQ@7Dc%Ni>lj!c@D3Z0o4(^VWXSF$jWM|Td%DvXfc<QMU_-)(++8(D&q(4}& zg(mj&z1;{F_Guq$F_30SV-w--m!V+Xn~t`=7DCjDOm33WCf4j(i^})fz>6p$Ex36D zM`S9Z=Nop>YN$so)f0%$pb;Eu`H64Nj3@CchKg9l^*CYSwJ=Eg)KB6nlgUuq5#;u= zYVN!w@UZGzG#;^OR%M5u5;=P89Zt_kgN$|M>{qcHE3_XAFNzysrp;_Ju}}uq8s@^L zC2HjF($Nw_sTvJzaYnB_9MHR&SLyo6NZg*GflCg^ixt)-P^GF3;6JXPJG4)vi~so> z0kNFU=%RMqB@V>5Uz}mPV%Ly}ES<{fZ$HqXn?v!Vk*m?2o)B6y^A??;Wm-Aqp(iuR z3ZYAu_)`;ADfs@{7lvPVfp;?}GMj-ide-+1%yZn!f~zf1+xWfkn^#3QGzR%!ImxWi zFo|w2wLu*Vvbl7TY%%Q=&BF7HhS?ABNvK*aj3^hrMf#>=#6zuYiEmgq_U%X)hVQ*0 z++-PzEzAed6(tck>1dLWcYs$--AmT2YG?1}=|NXhGn&9}hpClEnRe|<*nGSOqAV2H zZi4_K3Rxi8Ub-0XYb?UY7HG5ePouaIYC}LY^}Vfl(~x5HE;^e!eNe_F^_cV8|Ans` zZHsRu?L~8EP~e<@A(yL8_|BGn=>6+`B;k`UsoRqdA#0=YRCJnp=*Tg-1=VoZa3X#< zz6SrUi6t%#YUJOOWVjPq$3NFzh<j?)*@=>2T-Q!j>}25#A+lw(cK1;c9`aL6pKg1I zIu~q0F=L*iqQZFcq<kD?$VMU!l#IlGA49ixGcx>Y3@tS|Xk@nw88h!I{pGt5S5Jv0 zf2@wuU~vw4f5WwMh|+YSZTK;=rSB20Yw@P0Z%xQIT~{!j_L#N1OOrXb9>Tb4OZ2LF z67hXn2zTd+j-bwlLab2mhK{kU6byMOOIm_%3zjw+^854BE0!vsrUfT6B@B!w^cy6i zpK3-ltI`y;ZY!ljRjW|(@^tiS@o%s&(Sn+>iy5zF!9*eHtUY&1rPypJyWkYUtX3Rk z#Rq!nhi^Ba%u121Uw#puR9xijWU$1``ClqpaJvgdPb`GXHOpYqxnNvxkLlCl*XaAc z^Ux^Fg%ex#p`>4jN_F3)Z&tLU@fT-6Z<h|P`E>`$T57|s@)7vpj>FXK(p>EH^$V>X zxks|>_XBnP*n~Il@gVaH9)k1H;ljip0RMAAi*D^DZF5$`qg5|Jw0WBijDOTaUaiew z*x8r3<-W!R%1d#&izISCzYaGjB~igzNnbuq68Z9e0MjB>pjGm%7#CTHSIS-DqVLPo zdxjyz&2K*bZ{G+ScybCF8S)l#7i!{}b=&BS>;1^AZ5AkgPNj>TII6%K3fFvmih^|3 z2!A_-h$<~-a>8PrrC7Dgn97vA#B66ixw<+XC#r3#+^;s31eYyk)5@-3znPC%@qjBk zQKZWnzo@g5!EUg*!k^6BdXcn9@8`QZ!jPi`-RoWx%1!-o3-2GTMY{tR;!C@g;E0bd zI9(WxGY#$|9SPR=WZY?p$ao;RSWy<+HR`$K2Ko=hN>{P=zRhe)?K8+!@*r<zT5xuh z9btPE;F-lm7Mwm@xQ`{nvfb~&Ymq;m-MI#hHZWzbqat7@O@KF7j>Ev(WUQ2z5AM&} z=y!)(pyL2&aJwGWm|;VFm#g9lY&>?W9tSmA{oKLeJM^gN_e(CIrIee}dmCq`Ye}A~ zFQEPX$!x1i2o`m|XEzn9*s(?>;o#WcFxkm~?MpcWIfaZIntTQHPkbaZ6q>kE3tqs4 zwL@WZeJoDDa)O$V8bBo<+vtxgJeD&XO~h^eIF*=@ikzWX+uMWtB+=wu(X^7(=zY9M zYx7@D6tByloE6Q-&$GZQz77Ykkq5a61G*^pViyXTb_Xk&WD~1@FL7Y6gwkCqU?2bU z1N~n!an{{pdiYrdHXewi&q|zW>3&C6=c!8^lyvdNQ4V-(_(D3N`YxTcU^`^nSrVJv znNW~p%SY?#<AFb+(9y0cq6{fP+95BzLjMR@pBN#2TlbL8EwjaLc}Xa6x)hw~9|a1d zi_zzb2ozjziu$gdfJ<LY2^F@%8Z~Qjugnd73_VGT&|Iu`c`u7NET$`OOeCT*9vm{h zv7*xX^nPFfHk3R<X9o;GY3f&+cGi)bJaQZ8TZv}D!NCV~*4SIH(aaI5?~S7K<~~58 zOY!JhnI=AFFdR?I-GeOFt%8v(hL#SlMgNvmp)E^>!14SvTzOp{O{;khu5sRY&-kNc z(LFiX^(Gyy+aAFhmE^gra}H7Gk9I`pyp(NmD`F+v&XR5aK7;l!9yF)8BOTF?XRwbm z#+#4i(wdKlD=wPrk)mUxNTg;ItlZ;<a^APVzK3Qoa(EsZqicZzbO%vZfeUy_eM6}R z*OBqD<&d&^8+VZU;Lo+0IN<qzC_d7bR*fwsx6HS}FD-fGqbXl$)L;!(E1j8ksvRly z+ee3gy(9rzLaC@`n{nmEvobhzz#LDI>|uX(XE(-^6Hr20F_`T4V40GB%O2+{=#n6I z^@_t7TA0eMm@W<LYum+riS<yg^bLL1i<YR)*5P@2Z_%|a1@v0AB^*f5!49^PL)9aj ztC*F+ZuTxBW7C6(COg9#-+PnQ!>{3Ou6m-%g+G%?ps6-CnI;jyPu>A%ReYG{a{>+H z|I&ZcD)5)3kH8{w8XZzPn?JESmVMr^9X|9*3=wWAEc0wKHF&TC`X>dWSE7ql(MO)- zt_nxLOvhkt`D8lBvKcM8n}O)*iIjgez*X*Sre{vtz=RS7Tsf=>6%>ke$X-5)C9BCW zTa7I&dFD}8`0^8LJ0K+8nM&liT`Znm`wMTm^oh&(qeU)NP9qQAra+gbIf*n?hlTao z<XdtCeySBfeIo|Jy|b3CTwV&p>h|#pjyL(zc@t6kvS>c+{95#l$<oX*-SpYt<LKnk zNf0Q(dxwk4@8a*9jnHM;2Y3XI;5&66lha2p!tX!;-}V-PK;D*Ke4S4qE*w|NSK*@N zi%I^||Jdd~lCyDJ8eadXlnZqibJFjBz^Cg^Veg}raHs7W-nw!keDMgO!=3>O4=Cd| zwmjwXZQcqTWxOE$@)X#w*hcS5RD>c0|20T9bu?T`7=s1l8j!J}9TW?%&^>{JP(4vq z(wT8)-HHw*SE5@vw>_MMh!?P(!&-5Siah(^MM&<o;dpk^3$k{%81H^H1oZByqyJV2 z@d8l=RNro;sx4{wm)j<s|E5Q>!p;gGbc^EO8_uLsHQmT*#~Bgt(fbI8xa3LxNC|&6 z&;<V3b-HYmHd0TCMl<Mgc=_HFDr4JFQLQpj30%R3ojgbrhK;K1ZwMoLch0h!ElWu7 zb`_YP`xqYeV|K&$1Ub;R2!ygf(eEEiv6<&1{tZhbcDLm5$(5_o|G;bByYJKQn1_#J zMW)=zjSO{u>cl;s<#f-NoqUaWBsiYz=PHw((98V`AUMvMs;?KJZ)<>tFR(^!XbL#1 z=F;a>m4w&3v$s0;ao74i@a2)6ur=Pi@}u|z{^w@R&Nx)zl=|y%C10D~6>=a}9)lKc zA5H7VoTCjxFH3eVcEK}|(p>ztJCP1K{R{nkJWX=o9K(Yd!=R+Fi0_~F1u2@8(cMnB z$(x}?-0pyfXrY}3SX~dH@}9%+$t;BCa+)Yh^4J*X>xquaY$fOZWU<ue2{gK|p3L>X zK$PDW;Ki!tWV8P@+&0RXgl+YPq}8Lz^NtZh!C#F^(f`NSdB^4Wzwf^(TGF7Tlu8jA zH1F$tT}5R?MkymiAtHN^yIp8%(WHqK6-D=TzOG2f2uWm9NLCpotKao`|MmUr_kVx% zsK<TY=j%L<<9YOIT_6V{3~7f+FHTAcW&KPR5JP<x{=}b9p<_3j)xK{_jhai?3!{^$ zzN!g5{3{a*Btk*x(r2)7;(fS%e>pMWUo+l98`!ivnfPkGp`?0S6mfP~#V~TEFj*xR z(${8CA|1$UtEtt_GYw_UkM|IJi;?We=yEYhBO7SRzIZ<ESr(50NBHrXp|s_E3{~2` zhZfeX7Wx!oh{mu9RO+Y`>t*XhYqGbZ-=I}gCoP6r-c3RGlmzG)_y)F=1VC5FZ`{>8 z4=ld;qH@kL^kS}}``b@&|7-!w>%GOCJS)WyR2jniMLeQp!5x0jcEN8x^~Pit2Fc*| z5o`I<q`73}pxc-%KZf@Wn8_ZWyq~`_)0a;7sK<iwiTsqcKIH4j7qC{zpWhSJ1%A)Q zz=R2EeEn||@>rvQnoWBzy88DbOl2c!|EmU&l#Zm=E}VpUZNqWZM?y@l7vk2(nIL)d zj$_mpVTyS8N8-Ff8}9_n<89(fSgrDQa!)~vKhrDVu{}H36dPrJ!aGCW{jxEe>R(Jx zg$NP-7ds@%FKbbD`6_n($$q-Fv=Wsj??BZq8*(hygZeC~CS#|1lCN1xWWtXGI2utc z8g4Owsw_#z^cWMm!7Ce-b4M^AlU=B|+d-DTx=&&IuM)_cwHBA}y8#W#9a!eSl~>P` zVPE(Ut<8UJR_nEHJM$+kmY7<eqaFqxw8|onaxcHKMUp<wWAhp2%avS=*tCj3&0joO zGm*TzSWjy8LTP5QEl8QF3QfjI#F+id6v=mD+`xSBue-}0)ITg{9KLE{0v>|cWo|?~ zLj~7On?<FsFDI8|BIq>xefS|foIg-&;3Q+xN@E5nu;*7K^RNFt<&A#*;?W_Sm0JFj zzMeXg`+nyJZ)s6k>%DGp?W|K1sQK!zbYamNI=<{Y*}EnZ#2Y(#vF;Z-8s5-_&&Jh8 z2hHY}+*AAW-+^XIcJ9Idqsh*w6bj462jZIn4tRf=3)wbsHkMZHB5zc_az?|)QhUag zd|ePn-@aBMwTE@FURQ^%nH@z%-xtDxFM<!~tP;Kd;*F^Mrz#mz5H9q<d*g!LZlLMn z2`A4yfY4fDL28JCTXjq5knn!4xzdcdZI+@He&hbrWXBJ>AU*c*IVbVIdpY>#b{!dW zLxH}!s!8H4&Y)3;fa2UxMymY`xg|yMoY}o4q(8Wt*0~QQU+ZJ(vd2Rhv1vSAajz9B zR3m8V;DhwliW2f=r44!N;lgck9mx(T`XL-nBI%6fw?O5$KaD)BMm!6=X@`dr|EIy6 zESxlzyl);TIHqf}h=!gzHf2BLoZ1xFbF=esXJ!NqA5{aP69=+R#vehpQHID=hS9j_ zmk{ZeNt?eeqwh)+$QU^hiSidDST2;D-dKw3S;B`Gg^~XF7v$6T5LzK+jKTW5$*d!P zaNhNGB)9x1{rg0S5bGIG{nhg9^iFXv+&d#;RS*Aw!l?1EZKEvOTvH?=MpH>>pc;F> zCY&96`Y4^FW6y7%yA~wR#*&t(TI%f?PK-i$h)|!v|2~_+{e1l!j%GSB8&+RIW==S1 zEIdx)*PO<MGd~jL4O7VKzF2nE2pQ@zR7{Pw9iy*(pJSBW0s2k6Xd{UTd=7W~r@-oi zC+Hu?8jx8Q;y5i*hpbD_=GJUn3v;JfftvPL@V?lDXE*mz{}To*bM>yEmb9W}+HUf8 zL=;i)&?I9uLb=l~%(>S5Eu4Q!2Y&M~ARj+WAuz=ira45??zjV-#~=&I<KxnFg%G5S zcrccV8(VCsqvJ99dv7&S-~S3vE)arDN&(c-)tuZ^nolQJ9E6qMhLAM}hm-F+R?yUi zI%KK&D#xCbY)T>;AX3(nGz->VqcaA?OKmh)@3R`yMFr?^B#S!lQe+oed=eF`ROOp0 zRH@&eM0#>x1^u>qAe-^PgVd8IF~dB{Ab0snVz$te?4A%q54kF{r$VRG8B5~n1u~6p zd3~K`OnXQjS8pMg-3q8UQJvhKe2ch#(<g^V{3XfyPW<(NBwR6|m2>-kf_vR4;@wIQ za0y>Bx!(<;qN@wz8H=?`7;dZ)b1*1M^yAuc$zAvRHAZWiL`BoYY0MtYX2}))fM~@0 zE>Y9pa%N3%2<8vX;T}HeaQv`rfaLhr3dxtsN8GOQ*-Y5N_oB+i84Qd`l?--_W(K7F z6J-yV!8u!rWNN;R=&mSGG$Uskm#?@+Qkq>aG2U~EDRxT`b$tFMn)g~(RN`kZ3iD){ z93k7L{_u^xw|K<Kiw_c&xz#?Q+~JQ~xp%WRihgjXn7;uFIo<E4YSa|HBm?bhBpaMP zL`Q@G<odG%Bu(GSMVWOG+?-d*+?FSXT>gR)oRZ8il4?;v&Y!=7nfHR|jo<@R&%qsH zw_4ybWgq5rzaor?9Zl!Gd5D^kmqe>JJ9CHXmvUm?0ACXI@GoS%|BhsiDRzcO!Fi>9 zLXLC`k<*ICE?W+V=bB=U$8@Gk=x3dK708+JC{i_cI`@+3Ffx5v+^89~<iSQWDwZ7p zs#C%Ro*hSg1qxRa7c5EW=8^Wbl1;z+(9}|q9GQQbcG>39emiGcF}PezrS3?Rqkd!Q z=D!DsOv+^Nkc)t?rCp@pnj=|)<y?`0;P;#OgKpcZLWVBfNZS?^<B0$TBD-6YxCR{{ zk&Ss|&QB$B#I}-@_{ftXiUO*2Z7TgGR6mZ@TP6ry-E?fX7df5oOP2(@vbQgurI<dF z&G<Z=pRrvW0?%|tl6xC&ku38jeEf%_g`cCLTv2ede&X?|Z3oFMJV?xEO{f39`|@YP z4^hX-!iIg*o>jK_4Ii7|(uhy5Y1fpuxb^r=I%QoF8n(Qq{Q;NA0o%>g=(0ZF|E83> zB`LG#9{SKDDM=tXGJ<OG_aO0W5uO*1il^=u2Jp=vwvjV33G~;V!!&7(DIc*Qi)>#N zM)o%?g36_pBzk8gDX4V8*EYtuHz^MOsSRVa!c@7L57&c(t1C8$U2sO48mt`9&zy)e zf=ru6?w4M%qqt}`?rttb(}sT-@=_wvnVk-s2gKsVfyRtQawdph`A&v0xd%m|zID(N zPcS9;hUj^6EtJ*XMun1ONc@({{eHKUcw4k^OQpUu38ghS@MD_9X2J*vs6NGwF6=<# zK1c8gUeA5;K7+emvqW{5@8j}@!;E{1aD0zdr7l}!;f3O1xLdsdMr+z4d!!WCZB=Ks zoPRFHwfEn_;BjlXo<ToEr{xAv%@aQO;ot!#$u*l9l0ToyJ}yF)q3U>~(FU01ZOryT zGNLC76yVmMW?~zD7)>Q_Vfj&a^0@SeXp?YKZk>|FP4;rc{HM<`*vEpF)QktcHSXB= zTaK&|n#^s@PS7QKD{#k@S#&Fi{Y6=7W<+`2F>aH9>X1v-CBwZ1DRt;ND4%|eyiM}s zN*fx%TE2&zEA#`=oL;<tHx8VC|6#6d&w!4I!6YwRhF#P431@%O;JVfwgYGmS6>EFA z0i%UF7@<+kWzi;-`jN=(b{Gp~7xQrM5eKs1A&{6$Es{F%u4qU#m80GBi_y}w09BV> zgBs44KK+zK-8l(NI5ZF<3ilIj!4=gm2_%(sO38+OJX+?P(<!YYx@7Y#a^~9x>X`8e zt~V5tXWwLSr|^X}!uSNVDu;;lcUCdKcHAM+xluH4qz?P`;yRLfV3I`FGm6=k^9#g} zRt8X;P4i$;-54^=$bnp_n!qOHBw*jzVm$1yiq1Y_#4euqm8Nu=@!bYZSf*40Pm0vZ z^j|H+!b_Tse<({=FID1ejpRu9V<|S^_D9@(-_lVBcOiAw<F9ZxNz{#L{4P-EleR7- zE&VnmF`^r5H`WR{l)(a)JHDDzp{kNICA%SH>jqI}dbvcLHU^%4n2y?Qhw;aU*G$6^ zXGV6HCH&T3$K6kUCNiBr7{4r$q9l*wl=IA(X_=GYOtLBNI;RTlBn0Nq)PmyL0Lj)X zeXw_cG%lN0#l1FJ19|^iIhQy4;n&WcqMZCHM&YEmh7llx-1i7iEZL_I)8w9``Yl=b zyQmG91)dTe@Q&x^O?-zN%r}cH)B|zYmEAD;kRr)djf0@txp?<?8QiY0fg7hji%Nqg zqsuExNPd$C8|}K_>P~HlogR!E%}#?=kuDiu?ZK6p%w;AWnuKnflF|0EK8VHR4>7T= zUvQ{nJT!(RW8w!S&x49Y?`{>sbgD^irz%M#HASL_XH;<b<e~63Qb0aVb;l7YUED<N z0eJpMGjnnDIP%qGB~9wh#398;X@M$>?Y$8sq2MR<4H->;1T;h8bYX|wTLt3elN=*2 z@QDMqg30S7MqHn%LrnuFVD0&37-}^Nb_fygT}|^bz5gT^agAqY@0G<r`Hs|Z(^3%W zl*7*l>WovxO;O#PUdHIdL5OkJy{MOTh57t$CT9DM$Byx9Vf3Ai(7*aT=WCIObMy|0 zj+sR<PYo_&{Hfneyyrj0>+S=_^7AlU*CH$C91prfO!`zN=v@kCzc9f`V|I!fFIS^> zY&2)kG7@(AhoE<a8gsn#xx}hE7dje;;GX#^FmcOuiOi`C@LjhU=6gGElJan-?vIba z9(aKJJtr~0+hRpM35Xd3E<>uUF(#)xz@9fV@ec0B&ZZfR#r`aKu{yw?E7<5l6qVDN zAr<YS@70O$+o&48SxzL`H{8KHP?c^ov>=*8WvFwUHkoY^%E>Hxi#HCr;^c`YH0@eH z)0e7B8qYhE0p~Nw*Ny<toS*a`mRqNFZO{LaWKZ$l;tx0LnV0d!l0RRi$*++ec<0dw z^q84PaIXVi>HQ}$3>?i*kDo=#lfCKHpW)=x>D%0lxOBV|aSipf48Z%EGpTYj0I9l5 zoPOIhq1eF#QjaxC+?Up)N8n=kU-G=};H@?PKS}mrt*!W@xF3`Zd%;1<n9=cdVw2=w z<EwMSak#M}Gp&6mop%1E=(}W;$nUBgDeitP>{?DZ-=dg*9*qPZ$MeAnGPSz-=V|Rj zU)E3LMhEYB1ftj}{2H+ud&p7BY30fUqIK#soz!E@I~B&$(Ze25`Je0P^CPKzM*U1N zbsMWdPF+`L4X)jwqT(p>Kv$@;Q#pxam(C>33;RjCoE&)cRHObC12W@wDP!@e8bUwC zg3HF~gr1I};li115NRVaHG7GXP9M2?`7AqOfEq2_ss!V7<Jf(=fJdvd$N{<8EH_sh zo(z0S=OiAciy0}%IVPUUXm69D&-d&i?z7d%1cSYtl3W(<n>dB%mkp=K89Q=j@(k*> zcpM2OyWo$}Gya%QH@8}T8@Nq70wFuc(c3SEQM9%oOQx<6M%qCl@ycc2Kki^B#*b$$ z9S`$QCG%K)Q{7tScSqUrSAEzn*J3vN?{7LIy_t0uD<7zx__Bm=YdXm8Q#eKp4!G8e zABouIdAfA-*Kholwzu@Y5b$l9Rz+M2*YbxO6zD^0$-Fe#j!*U!3*WbPWbCr{Ky&oS zt>1I;$B-tG%)GlG>92qrJ2Yr`payMNa)fHJBiVG9a{hQbW$TZou%g;6?AF34UL5sO zhSuG>M-5{(V8^fVbU*)>_tU;c7Av^o@v-`J$(p|y?`}&I{tP4Ta!EwD=q09&nM?Oa z$<kw^7|2q5P4?*v?$n@iMst@BRdMJQ*q^K6XSY0PZFzuN30nvXBQ-58hRG0DqwIq? zs`*r%4RhJUTbEUcsi~t(?R~2Q{A_atUVZ8g{+Z=y{`++){$BB1K4IT-Qa7`n)j6ic zzUi;VoJq?eJv<#Nx!3UOdJ+*GNCM)kPmaFu#}$70P+(>V8pTcY;kbq5_^-pzD*p_F z?ReNXA_=EyzXLV99Q^!K7Oi6CAn)W<+-5D_&pr5&hNe@p=$*oDqB4ntR|mS;!n?-A zGR%~ZJXg#o&w0#F5c1qx+Skwv!!Gef?uYsK!>Ob@@)c&ymuKrIy(MOUm2t~}D7d*b zl0E1;lQ_qOqv4NmvUmSPR?qMZ=?neBy+70f+%bJBvoM_>wmpcfn4W~Y#X-gxY3IpR zjXDo`is2XoIb1-n6wM+JY4mDCK5o7WEiNtQ?WRt3+USzVU%pdCy0*sBlwGxCWc_qj z?MNQ4`K+6*Zh6KIF5N*)xzn^)HjEu#w~0S>^bTg#U1HNmH1a`j^msiwlyvIG(e877 zFu$owWTRb!VmVhOQufo5oO_glZzCJviH$tDrj_b&H(?z`rM+m~_zR3*$77hI2mLxm ziFLg@l)SnsyfBnTI@Qh?>vZ9=H@zLslU`R#xT4uZEQ{}9CXB9KT4zh-em?;9=i%UK zmrwj!s_=zPA^KaMA*b?niR7UHZ7}Q<)9=;?Nv+ibT=0g&m9I^3?r~Xa^!o?(yxcDO zYQ~cMJRjP0C<Vl+F5tV*iafcS#>HNL%V|HF$_Gx(XKe1pvQfIt?0B`6yoyXed-F&R zKjmsVh>lNW9rg@n8|U})pA}We-~(gWOHDFlo9kz?p(%{bQ1!;~9b#X)U1dIdr9lqF z8X06+nHhWXT07~|o=);3BXMfNO3Cr~yU1Esa8@-t85!=JDBw*1o%gC(RH!_WCUk7( zToy0I>GCT<&0d;M`Lu?d+#tp0Y#qlY$Rs)4bh9CW3$h@xtAm_<xm4gm7r>mKF?=s7 zk(SCv@<p8Q2O38-h}`KB?Ao<a+~2;*bnSLv$0a<3hHX4HEqq;*EZWCqv%M&@tO(3E z)`R)#a_-doVSKAy9H^d(z>~8S$m&3O`o%m3>9b3ssq?Dvv*5*&{3u}4B?9U*TmT4u z7+4#y;s;x@w2oF=-bUG`T_n9mAmdg}_n>(bSFqn)#Oz7Ok1*MFGfJXN2@E?$bldMj zdAl3&zpYFgr#~U&P7}vgjD$gY|48DedQQ{hGkSc`qc@^t$f}?AB>OU=+vQ^1@#!iC zmK?&%sqtWyNAde|VP6-nQvnbBS;4kQl@<rEw%?XhncaF~eu&*#*3xe-n{c#{u8l5( zeR(2QJVBLxDd}ZNc_})@5!AGQgiWS@KrzgUOc*3Xy<@FtM2aS{+_M3uU6!WfLzJn; zEisj?QsvfE=+W`HPqC`km|Xvv4=RVW7{1#bZgs4H?oH~%EhbFTs#PPB8k$4H?8QO| z{PSeqBx^lyzovjsuOC{Q;Psvzka(Z6-*b2rlFNajdQ{R6#J#(pvnq}w*|N}){6YV5 zRC8>nsB@e=U%x?(^*tg42cIlv(i1KC_0?bazOl>rW?M^ewHZin?_kKo$^hv6SIIvM zmcnoQZ-Ho-B^jr-5}$~#v|`1Q<C2ZU2c&lIVeM`1(+_CHa(pw`-wI-FpFMKQ|M!qR zIs2`s-3%q8XT*R{)?ilQx&e_ZoXtOumxT*|WZ3a0RhZPS&&nx05bal)kAFTpu^(*T zGYZ#A>91Y;s8`Q0a(mQD+CIb|XDIVH$0H9$kN!u*eb0YFhix}Jb$6o2Y{tQuMZRdH zdjzuIwsEb`q*(c@4n)iNEPHt6DDw4OAA2)am53HZvbzktSmTa1A)S1Pw%K0A!*Oji zy}_N&9BIx@?)gP{dXgUgY(lONR3+-uhtNLnb`tmeGPP5Z<wum~(#=uM^z>_QF}vQ? zh#V<)V(O2lalYex$XSKC@O187>|v|%!ioX-d_f%g8YM8g=2OUqj%?WW`#9V6elx3| z9>5o6Ud8nGc0N{go<((4r|JVytPGzANnYdF6%S951^g_&<xDD{B%u3zhQ*SESA%)8 z5o*Nxpf`!lIZL-^h(B=s_B}WORd~6Jw}qMR1Ag_^#OtXLY29rulDc|{Eeo_n%fEZL zqhmcmvR@gb9&dzt%gOZljBL^Ej#{eU&al$QR`W02*21m%akZhZj<L&fNAe}dz1aO+ ztdRHQ$hSkUXyu4|tfZih{m?Uw_5AXk^gVh(0>sYy$-K@y*6Q;V8WgaW%zLtw4Y(%9 zo_0xLoulN~?sXw_MQIsIC4IsTqf$9>N32i|uYg8Vrh#YU8aVGblzzGz$*gd<#f*u^ zQ9fLkKY((qhQkZ?o?rt1Ft~uX3r}<!bLb-*b>tFz$jXNuyzdDJH)_1H&K1S$&akzw zmmDFtUgnU3jy~v;mEs@0ZA6D$PtsIxO&@%i3^45x?*G(C>`r^r$k2&oLB&7F*02}i zbZX2Hp#$X22qhd}CMRl7xybw(Xi5ezyNKVD=R<mZ9ccLGFncRh*i#xs)OxrRzePwg zxqoS)p${lOT6M0NiS$o_mKpW5SvQ!NrPj0C|8lHXoi_K5JxgX~%JA0DW9T(SEx{vZ zO*`82sDAG%+WQNL#kuwD(U2y3=3zW%pEaLu>g&TtmiF-7FojJx;YIrcN8`QE`Xut| za)>M)O3F^kf&i=}1`mX@{H6%@l}N_PAVMr6MJFGVC`WDfD%Zy^b4`GgKWte`-iU1w z`Y{)(2%N^zI<<?(4yJC~d&v4tJK06dVp1nbV&hJm5ChwntaM2w`KyyfL$>tO8B-(J zShYi_dEOj9+>Zy^C{2#tD1mi*24IMeHxVGEWRhAHw%oV{uG5x+g7{{w#PESRy)y1N zNh)5(59l@HUkzN$TRpx*Woz}ERz6ar+}$TcC@H0j<`}cy!EWs2jqBLINhhh}{=vLz zS_!>0tPrA0HE0g8C3de&`4WL{*g8;ybWAp3&)bQp?5sC1DOHc|%DDmMLvL~}P8RSv zdJaw3@j`J_;!8=A%NCTb@j_FrKvA6LS6C&OjZSvH#0sq;>`476Rx@f6A67inX~C!| z#D450!mie2$1R@5UPxL;*kcDs;ds5;=$%UZ(q*MYZc;wEwRJN8+a!?8yzWI)zwaTh zJKJgY>PpV(_6}m{-iGsTpC?{LwRm86l^7oriZXWx^CL#ekn3g%AT`s9N<T;-Hc9?) zZN)PDc|wU(JQ>V}+J>`QLyCFvYIWMVF}v3O>}%$M$xWV)I!6v@hk^$8k_LP$;unwf zV4r)tIeD^{?8Se%>?h5wB<t25dQ-oYBu56)EY)1vaaw_WY5taODi;T`%nl+1|7F>~ zn~HFvsx?Yd?da=u`ULJDMumqb90G?-fOCn-5~;&8@Zd@*+_FZvP{wP~UHW;X<%<^R z+<Q#*-dyH4_B7GLRjIu7(8DCYESvopol9~Yvx&-u0i<Wgh}t_7l=(%FhMhXSM5*x! z?bb9TE#h;Lq)NJkz9?#i?5uwv{+><bZ<(?e`N8la<11)(4yE!LXYu*dGt|&W0=?5$ zF-@P>qgBr@=3GW8Bpy(t4KgmY)cpgbzY$WU*;zDn+8O5R(?9e;kcht)r@_k`y`quk zr}(gy!1K;Bba&u<$>L>lTus<Q5FdLZO>B>?r^c_-=#;V3@J-lyygo;RAA4#E3|%#o z+NHgNt{I!j!{k%Uzwu5m+xRNem}^WO%FYS*)`8HpDN7`~G!Qap&c?O2znK@NBL(d4 zBKZ8&4=VZzNuTzGai2eppT5+JAK6gJ=GG*#pZ7TMht}AWvHwEERCT%(N&B>zj82H) zcUi>p|IVEf&Hn2}{yvc9-(OLumc4ICs~_RBXDkMX&0Y9U`wuNBEhKy1G?Cd3I>a?X z1}A^K%WN4GOn;SMU@CmxL*gt&kd0~L#*4pWugh@K{_X)|)K^TXOfMaCJe*euR$&K~ z2G<TBD!#zqd9s@y*AP#n9%S<KMl58@I~NikOC$QjZw$Y8ycM32Gh|cc!^z;*m9)64 z7XC&)plQxS`0nl-I3YZVEEGJzSKR)hobn0kSv!za?JdP$Mejka>@x9R??j`0K4I@J z53*Zcn*QFvpt1EcoNYaaT*}Xe?_%30%#u_e=1#<EGP0Ynldo(fp)nrpI^9Y9J%16= z2q?oDf!oMb+XL*+Fg+)whw0?b%Y{s*#~Yzmy9Ce7RUo>>4P1TuKsspNc5uo=h=~bB zQBWIcunfQzuHIm=zZIHqMq|9XnP|ztHVlt*BA&~Zb6_IGq&2HoL+7wVl3k{8%#>GE z^hxPay6kl{d;R=ye&vmEf|tmT4>Ugkj`vhp_;8bT^0C8jZ&QhV=V20f<{^!8Jp&DA zXOc#j_1vW~3~7oSPQqGLuy)vL(uhj%w<ncE<c?&kFAQVPyidgGIusery`rv(+oAVS zqnIpSIvK2bPvf;*Et-F63cVL>g2|)OVR4TXLtfTUEun4x-(0QQthv%;<wwKXTl=#3 zc<m0}%}7Of8)nkGUdPzZ*3tZ()w%qIktR+)QAAKNF3`Y;MeLdj_m~oc4!R{r6Sq!p zgeGAv`!!@f?%H=v2*wBU!vobi>HjVzPhU4F{vS#9hS1@>XVx1O4OoEwJ9yky_yNVy zr(oKoi%hcZO_bYaji*z+iB#uiXdiGLhsr*Jc|F-Mryz=2D>%_t1))?&N<er!JA;<= zC_2448(oGs!~7qf^o?`~*=2bSuC;6f>)t%hcCRkE(y{?s-G@_qn+^Z{I+a)FNXsUN zG&xNu)N^1np2Tp5?irl^rj<;{I%BNwx1#@28bId2i~oN5kVpI|11Tvf1F8SuXNva) zqx}sLeMLP;hEf^MPu)l)Gq0ohSb<AvdJy-jKIW{p<Ux-p4pz>r!rR$%sq5+(G*4SZ zo-X6C%txDV*&R=m0-xHit^S6)qBf#LKAx=4okH#kf!BEoGwA{24E%nyi!19fp@;tH zkyEw0<oSXy%pF-wF6jk`;dlQmEK)CYEEHuDC4XrcbhL(uun1o*P~h$JPs4_Hdr1A< z`P5;>b<w7`wOn<_KDuz#Ju2(9l{tJwp1KN{&;d<b$*nE=^uF#cGS1M3(n%@g*A9Wa zwYv-AUw$PXVT)-@Nhp0Mx=j^&^T@sSUNZiq9P8pLjv>!MmS%tP1v3phe%OR#<jp-1 zZ#-cb?p&ElrySLn446{oXk2T>zIWIT3**j^^BLn<^J|LiQC$xriVGu{{}2X+Q+Re< zjye`RCjNId>7(G+WcBsq^vBO~yrWx6R9h{m@{(NWR$NMa%VV+AG61sMok*MbUKYt% z`d%c`pM?{BJL#tU7;^IGdb)1yS-N<`CE7M(1~H1Yq~#BM_(_s*>}!m~n=!)ZxbX!J zt=P*g@EOG2mOD!GEQ6@|Idz(l$aC#aZ{v#EDl%`jndGgg4ZZC3$?1vB7*g4efAv%G zUg=$E88;c-N6NEe7r%|TbEY*f5#0UB+jGg!zAsewSvT@WUtD~#DwmchG|~8;1LS>& zqA*}%=$?K((j!Wu_s%^7>jTzUWqW|MP1=UX43)s_z$P;GVLct8(nYt8T1>@-9L!o8 zMwM=C;RjV`;HZF2WOeaC{)_fJcC)t$Z<Qh_+b3ea(SpQo9DBl!-d)VH<7doazwWD~ z9ZrU1!fYX5)E$Q3cUZ7mgHMpgkB>;7(FMB1)QmL^R-nJS&yp=)Z(-cy$;2qK6Zbby zAamnZk{SPU&|pv`DO5g&Vs097p7IcqpK42vyl=sgBbC|g6Ok8{KAs_OcZ-YZ?aXh; zo>@fw28|$Ep4;e$*MDheHw)=bZ*X{RC^^;F!i>{yCGyb~Wc##sda$8}+P5na@s3>B z`0%}Cx&D2U9(w|Uq6g8a*m-nLRXi<qiN+Cyxg>Y!YFZ{OENXrYWa@-i_T=lmM9(Ik znr&G|TtzLgQEauJ%v#roZ%03+>jNW*vw=LT^wFITj#^6R{8Gj5aYI<!TpRYuvwYg} zI$R?9HJ|)3DJPRID6qeTc*0qwJ+$O<26f9-;vWrdqa9JVnLIg;d`jL=+*Mo1xf?<| z*j!U^y%Yd%?oJ{ZJ%6aF%q%|jNpa1_Ol4jy*4;@)&(Onbdl=p!{tIm${{Tv=H2CWR zk#$zjNj$x3BdvF>B6M*a@tGV=7Ke+er{o9?dLbc~wB-4mjh9753UiqsCr>!`p3vbe zBK4Tsm>bNRGhU+e28yCnUo^Nnt6I_YqT`Y=ap6$2tei24yeA4Ue<_NbDj01$KObVM zlhrt1oxh@HCauQ&t%w_OwurG={9UxIx{zym>;&-_gJ2<z7Nv!SqXtQWh<z!JU#C=w zn!40s%(~bbzX8*kXPH}=%b5jS&Cpkp<(rR69B1BQ)B^`Q%#zwK`V?AOV<=<6ERm#% zZjNRpeq$Di-lym?FBQaZB~6ja%$?2dH9dZ%5{E|y%+~433|;?BM9%JIB$ux{mYnMo z{R+3?KGnSu4fb{C_9Pon$+ROB7EZ;#dt<ngykWF0Fqv!>cYt_OJ#=4hpr+k}Y1HOu z(EC1&8eAVvABW|j^wtvQNni|Ex%|K{Q#DDxa7`{3mrsNVg;_*DS&X+DENO1rQTo7S zJdMv)qs5KuiI|HfzQS4g%Ad6`LTQR<+8b}su-L;4`;dick7wgVgJc*oW*TU0szAT) zBY30gCi75UjaxLMgWEPUk|}s=4i}B6qlvsRMp>R?_8+Ze9;+F1CDSSy{m+X*tX27q zduu%l$CZ4ArfWMv{?tj%**OWWFTcb%9_LW`Zvtm+U(09-4ugaDj77IYh4fCc2pYDo zLJ9pT`jO&~1MOyT2hKEt(F<jG<6i+;XEV`RuUGWTgyVdra+!(+ySX>rCNSn{2HdL1 zW{#IlK)22}xG8(77%MmD!#sS8j~68}kIv5GeELHjFUtyvvb!UZcioMQR5KLwT@G;{ z&1kY-0uE6uAvIxxm^FQdaIN(jH^<{AwbH$V32W+Mv4#o^==~{~Gjs}E|EB`>kwVwe zd2c)xxE&i-4a65CmNHH$qi~da2I%Haq+W}}Es$>8gQ>Ya@XqiX7S=aIiCiRnw%HB^ znWs@kfD>p+<f(0485~Q~!Q?O1Op;;|fc6(gr}PQ4)A9s!XRZm2d$tSuE2ct?IGStz zWCI8F*Fu$?7dJ&A89zUjh5mk)oS8G6tbTPwGIri=T>4WTr|N}(rP#op(U6-BQ*5R& z(~915ulM*kUNs&K5!vdZ)&6Z<-ro$yVayrMWRgD9{5u9NxX*&slB2jwA&S}X14yfk zDXu>5&s|!iz_ccvM)`;$Q8!S@)TBia^Q@k;FYm-VvsW|e7e;{7XbGG)8pISiMQ}G} z7}CZ)!mD0+P8<&OQG#_Ub2*Q7#&mhRA*Veg3BvrFF-W9EvJ;o1!ibl+S8oD7t(K>! z8Y%=hnhm{v;}}ewAP=i@i))Iuh)CXpQ8-aY9x~3}2lepvLgYIM@(e@i_bc}?D`pzD z`VOW(J<eR1N+{6|I>=0aH%}BQNXcj%>j9NnlZaf9n0bBJ1rp{=1d~+*8Gp_P5}b$7 zT~)8R_Q9uNv9>F0JJKrYRkG){PV|Mwt1(>KqezZT{=}H-YID}9faT2w@S`&UmMqs4 zEs~GnPQ^z-Y*+|LJv+-O?s^BfQwE<@UgHMcWbj_-B+gq#Y(_g9FL2P=&pdo3oCG@k zpd5yCvd0D9^3^4{$ike|c}a6^!T_4fO}yydw+cseggI0k&V+{3>#*$8MMmr4H*h=o zR<zBzOQd*I5x2NYVej<@E<!yOa~CB*dE!21<FExx*0ot!UpyR&ei$)D$wNuX@n%8i zIO&bqt$t{>Vj7p@r-a+x13<yUj{K46@(p@R8M&1T^loJaW2j1D(!?O<R;Mm#7TyLX zxs3a!aS=NQAFuIStOEgyj&q_6Wf0x3#u0ZXp+mzYnzKOx$6i+CPM@`7X7&E%{_QKL zji=Ug8k6-PKj$dOy=_3TciTM4sj1b>?s3l;-^KZOYSCfrz@L&YgSEixj|m1>{lJxn z6CDG&pYVO_7SesY5H<64>FAzxYEgKQ7PYH^^#W-=h*##=$5vdx?@GAwcnY`koTn&d zjv>u=`b3{jEF*(gZ6iIQl_)hS0jmB4Q9fCoeog);hT%KCiIZ(FGcaA9U8(#POhkjw zdRMsUq>(B=&3OVn6m$m7KcvBeYhh%)^dw?yxPWBcSP84QhZ9k^CnFb|Ky!xbgOPS2 zYWrm2`G4E!AIotx>Aowx&6vnL?J1)ZD-MwlU4QVlz$h{-oX0&)&7u=dQEHSD(9OgY zsq@dnHAz86FfQvgC*|fY>|^6TGRL_UTy74hnmML?Z-`)wZv8-wPgOwp*i`zsGnOv* zyG`0}PNNxhIrO;WNU}=Mij8D?$vi7N(f?iwp=$&}kCK#>lhS|7_NnIr&D+?3ZirZb z$5!3vzE#-a4Rr|w{qE;9%-`d-FcT8844K%sR;ahn0zY|Z@rkoe0FM2`EO;M-S^{Bs zhah*K&$ER3>PV)$Qv)XVKjex%uA)lNZ77s__n(5Ss3czU|CMrI*ov1g`U1-3j^wMt zJ*b`D#2I}Z00|}yu*N!%Nt=_5TVoC~0bw_>J4lD)&b`OzkHUktCX17HH6UQyf_q*J zg%!^Q-}x;!@EI|WT$*?u9c@%aE02cY?}h=;ldgb=evTrye_N8Oh?zKb>oVN4UqGxt z7&^YbAO_jCFK~M4ADr7Fyb2B-=A1{nlcBLcxSpD;7_vZ{PK6YZ7@LrF@;~rNca=~e z9|Fn47Lt2wZLwfo4fy-IIC|IaAc<!NQol5HG>e)=JD1!R6)zFW64QP&BX`V$Dyu^{ z>x3t@o&ON$%NWDi>|c<-awzQDAx?&*^+V|1HW|9d!5QBy&OxO*C!*@<NAKQLr0*P7 z;@*W5Xz;93L1SBs$K%hTO_B^%so4XQtPGj5#}h!lxgFM)&VtWfZFoX;0b0w<qP8ms zlc!(BTzi-j+2h}UX^TwAEx~p*+suGu%XgrT+!z`^uo*PPK81L4v@Egw_kd~D?T6KC z%9!Mxhaf1d9TKINi`)w`VW`JgIwMV>9QIuR{o_iY?=Tg1hsYDpiE&(ir96FFJdQT) zTSDqKNYltI>h$w^1yugWVa$Vza5!x+4ZA8!H@e?N#V8frZhr`W-%X^Z5{m1R(mC;o z74wB0aW;%U(hkpbpW|CE3#z}c5o>;xqoU$#T(C`w-gfTA;yt-Si)Ih*TP;Nvw|~dS zl}*fm*|ppVb6J#aI{~6!${h2a!lS<wo6oF4Isf;J?KT_weR3E~tuw>u@-J{@eJ~cc z4kJfL$<d}6+llo66R}XUvlO>oJVZSLWzn=`Dm11k!G)qYCaE<T;ykQr-HQl3#;j)! zRCYqE<SAGVKZjFS%p*P8Gw2kZ3y@>-8>!4*cxt<zdDJ@>v#+tDLmRyz*})j|7axc3 zX|rM4#<O6nw;7j>6cXCpC0yjik_}UZj?qBzKRD4dh`#Ww!9(qd0KYQe>U>MuqvS$H zCZ^yavW#YX3}=Rw?}B&71Ho;TP*`m~iObPAhGxG@V26q|-LAKdk#4yQho}3X{s~9O z7nOs9?PR8Trwg5283peo`8Z*r3o+NQ2Pyw4OmDU!_1HL@c%+RJkUA$uP}WX^lmvM| z-Xvu@gjGlX6g}dnU_c)~DPh*zxp8(m(lpyfA6}Wab5pMe(Scw97oPUR^YzDosY`b} z_5zSx%3vh#PongNmBJV`N2&XB;dkW_u66ww?tQ8>@jk+#bF(^Kc&Qkj2k+rJFHFU$ zYgU8!w9hC~SZ_;g)f{Os=wN4<6?FK@lLbxjs2*}u5_-v+)CFgXR!&aiq+XsTx=+Ng z-2W&VN?SuVNHA~a_nI`vVB)nQ1}a+T5TZ95Om%H&{NgZf%fBnk{a_iA+%^=~rk7$_ zNCURESc0YV2%@)n9(Bpm5KejZw@~|L1k62|2UgE4$#9`Jdfu>f*fZx3jK5q3d+!XQ zMz)2_$;%a}bx@b|&-)I=y9ImIn(5T*(H$(-b_aHuH7&9hP<v$~>Fc5v^tU;~Sy(Mc z&z+-4mFjwWYSdr!{8oh<3m1a|JcVr|d$@tMF2tZoEQ|ehGIZUna!g6M$E^9)g9%gB z>8|D9&~IiD<ZS#2J1x(H+ou~sLGc9YR?iT2%n!-?zn>srOS|pMQdhz@U4+FlLbl{< zEKK=(om(Rs2>tIz(}hA&YV5KJRN>AENsaVi^eZao9C;Jc_G1!_IvPr{%`_bUh|2_+ zNU#(+so5*~v+NZYz1@oh3&SIHvIYq?8O%i=-h#^4gMdDI0b}&9OEw=<CkbdOJYlav z-%AQ{5;-zX&mB~YYtU!%2WCoBBaHi(1zxsJkRYE7HjFmW>G{J+w?Dy(=V!SL`N_DW zZX8}2FCuJbn?yXXQ;Gzi3S#Qxtihf9<9=lA$NM2MkQ?zARyR6etPUXwqVwFb<N@?} z_-n@@iW;;fNtVi3j-kdachK5M7pxc6VR`vf(BBY_{Q)xYuAm&9g;6`qn$X^r!-&+# z<z&^cd7!DkfxMXGMw-fWY0Dr7@O3*X7A@WT63_WB0GUbhIIip$&M}*VB_SemIJW}6 zjMPBoMeg{1V;e@z9YA+zE6}}4{#?wSTIib@hI_w9pr%(iOnDjwGP(^g`|v%usF%oK z-wsE^P$^u#s|yMejx(-qvrzZIRnZ&4dbb%5f|CHZGz(sb3Y}s_(w6xJ(?(h0;Noid zI64A8?7IqPaSMs-7-7VYuYs5ikzCiY9+1-S#S<0A7@OJy5+S_yE`2y$IeZUaUpWrP zlSkuHwJ>~<^c|wFwZIU&7POB?NynC-khCQQPghLA1a3Z#^B4!4-W}vp+NKKy(v6TN z-m461D1~iO`!I7wGj0u<1k9a%SaxtOsay0LH>ll%xC<_%Z&)tKPjDnNhb1t37muZU zy*8)Wv>Tc=?n2UcHPP;xhc#ZyZeW|K9;&8ZhW%-(q-v5Zty{SW?Z@en;M)RZW|J5A z{uFqyJ3PR!@haRj-vi>fTdj=pu<6v*)rQLbSV6qMU4h`r4AJ*Z8bSr5kf1%|3!&ZH zz_orN=QD2{E<7ns((eqUZ!a$++G|;I-(n!SRev7R)xR@qr=;K;rN<J<5f^wd>LK&7 zOSr#<nh@i0eUhKW)9FPm3;Ntmn^uT&V8X5oQ1~@OEQ-%3^z)8&oZg#l+{JQ7s+S$h zsIJ(IXRW;<;*l)Pe5g$`0w$0$_04#$U@ar}wF@_UEydrnFM(m(LHxFXgDv`ov{^xi zWMoD&slVRg(HE8^aa0lKJ}?tDzW<E+t14h^zZO}qUXOuiu0Z9h3zE*$?f9)xd=9&o z7ocOa0jXQzhJL@UqK5Aj+;&unwl7&lOP9|De%y6Nv+Ne$NR5NMPoH7wy)16zTzlg3 zN|yZVsmEa#a|N)J6_wkTO9~VGX@N(EB&3DI-Hk@{{PPra6zT@0-4nT$n;)Ufhg<Nb z+6LkdUP5W_e~vH22bM8qJEbLeq|+EZ+Zsp;noZ6$kEIEZN5PU^J))3Tn=!g60N%gc zEUbc>bbhgm#Q9be)R~y#@$E_Y@%el3Og)T0Gt+Q-!VlmC_moZ2WytvP2O?f+U{IG5 zb^CT7dPhZqr?e3)IwVWbOow!ZYg10Qju9*F8b-}N#Dhl5V8<K(+e&JFz}SsP(5w3> zz)E{s?)nc^JwC(s&@{-2`-KM`#*z~s8kk8zuGDSbA{z5)E-C(f4r_->kx*qdYV4tb zGhI}Ob}PU-yZzkNkj)_Su7P{ggfocVdAy)In}V7&z3w`UtZSxXxKOqmY#Nlv_S4s~ z>&tM57qxDZ^{WHnxttvd;il5a_wuB%eL8%oDS^d$<LUD)wlwnJ43d-bS2V#%nHeA% zfI~}c$iQ-jF&bnFV*@Lg(v&zjb!8un9qvwVWrT2&4Vq;3nNgTCcR1DB7{ydwT26Ph zXW{poVix$dcHHscew>py5!YuXq4Q4}a<)I5c@({f&KMibMXQFQ&KY@T{ys}`p=AqN zY&XKwcT>6Xn~n&=91pXTTcPcS3OQVqgFB9U(mjU`qT=m7m}XXuKOWfQwB3QA#Z@`Z zaZ#iLip^-7${_OafiM~sVsW7O*AHCaI2r#a%F-)+2VqlVA2-Cgn#;Nw!`K}!hSdcn zaDL5PIxNML-1q*6^ZRpPMME9Tq((#qCWB+iI~-;489ZZJA+6{W{ISY~`_vs7;iIeY z`ZoAC>(fcLbBJAlJ?=kXQDfU9kMmF2kxM4dkUh|biWSelhEYZZaM5rsEciH%TDu2> z*7h$r+s~5>pEVCwIi5%J*|Xq5$ZA0Ik>m(Z;Y`APOz2MMm`m+&CUZFP^vr`-Dy^K% z{v1q-uEWvSzM>AkWW1!da5k%QQMP6kSQ*S<+%oQBKxqovrPsssPv`NA#dI;buDc18 z?iHXxu>^nVPrv|&weZNF<@RlP&#1Zv;)*RxX`b-(jUBg*8GmUVQMk(iNDZPtj1%dY zNGm)sCk3Y%zhaWy+qfFT;drUsfQ&5|NQWh=pmMiJ*zsOMY4B*tm6A8;|Eq*CI8p<! z#*}Mx^~BG1VpkgH`rc8;y3nC5_$>3crWNEOv%z<d9-X>fXwkGQ$8%YmxySaIcpzaU zZrfQ5Vvzw=E?}XnDu_<De9E*w$bjvSt*A>E4?ozSocE!LXv(gH(+>yHHDWXL+9FFQ zo|2|Py3Lq%Rhq0F7sp)Hu7rHGB=8lR`oSK*Um`oZRop632SiDI#Va>v(J6!9K<&U} z$v~e@a3A6V1MHtkA|huoEeh&HK0_D1=DdSfm08SagNbCsyt(*o;BqLtxfEIFy%3SE zL3gTKlPw*^f*h^Q!Ot}iw!r~(LS*rnkGiBORD~|OIs>zu22=6t<O9&%(v3&_V{w_1 z8{YCd0Iw{K$j-CRv32oI_*7>`^Wp{*dlx&=!_Wc&Ol(Q>Pd|bc+lErzFFkne*+FL4 zswJfFz-Y>eN7G;QAotQc0;&^=xt*`Q$dn5sBsqaTv_18cXhuah&Y3-$t}vVfFBdh! z#l4|oZr_S{^#7<t{{6NkDt5vx#kO4FJC7sp)IUMw%Q4V3e*@mQehRlLtB{t#o6)`? z9i*-{GN*sa(G$xik<x&-sGAXr-}gvi^ode<e8Z2d-|I|`8aScm&jnW|+LJAR-QdLO zVptUwMHTNKgTqHd=##DaqRL0&JkG@bC+Y=_rM@Fi;+gL8xN&<B1Q<=HnYWxV`ehs% zO`U-aJ5<p<v>v0~qS3oDjr-s@3~biqq1ojpOu-x}crs-?o&4!I=3TTS_wr{$+SVh` zmNSWDxp{%-I!oGLZ2?AI%Q(IG!6aBpinFoY3eye82=JsC>(Q%G1oK@_h@y)`q}!?n zBPOrsVqcsE9gPFnch8Ar?jBDrrp_e!o&r#K@_BB!aRi*)JeU?aB*Tis88G{yGM%hZ z!#S(GhUKg6$?Uj8+=vhf4m>fG`cV<yylG7f{o-L|!6)v=?2W?vQHC}yGNh_+>rg!I zViG7T_2ciUJ?OgvNW;s^%poN`a#pqgml<xLwTcmBmuxWYbvuExN=HL|?*|-Mb3hVp z?n@qzh=eo7yYNp+Ip(%KV~W9oZi<z{_t%<4Qm!m@(@esGZ~+}5iV!`D9E#>c1d_hC z1B7*Lg56!V)cF1aF;VVUqc$=1aKuUj+crJJ7Z#m3z;hH_@?6PnQ@H~B%>}14yC1G6 ze1x~cpZKoDLNaz^4S2LZ!FR5@SR&^@>VNuTzi~PaDG&l371l^&s^PC`A^xrihwZ{g zEyHUscWG)2iXv@jPkSTWHN6Cy+7%b_EYwQ=Grha|z5VfjrQ9xCvG?uIaLh@I-p**^ zbbe@(kI{Bq?Zhp3Lnj706P}`~V<u$2dxJZ2^{Bd87L(TJ!aV%<5n>lJ)V8?_t1E+W zMZhaud(zhabiXk%-<J$kJ>f9&kU6=vr<++;^|<Etk!mKed>-yojEAhxW}>=bIsXBe zUyhzE9iJT1?Ia$%Y7uz&go0Y_K9EwfB|T>ML?={-lZuXHc>U8tG_LpuXeZYRaVs@g z@bneTJ6#4cv;4SYm-;br+H~3-)Xs^1zeDz43A_>h%~~JEkT>$$%)Xs&LbTQe5+$Z^ zvn?J6iQ6P{(OIJ4a50>4C}cP_dFnpHQ!F$iHsVX?eQ^JSa1j|}3T}xls69v?PVWB6 z^gRe-t}KYfPtRJJS$CwFyANIIPPas1!_9*(RU<O|#sG5d<Q;Ce^HaRwHHmI|`xLJf zdve)hWN63!`{=2D50>mX%UONc&P~vs0ampRbnTqQ@GgH6ZF6}JGpC5PNJ>;A{<#-O zzdo_1kL9+Jhv8Cm<{*F>CJH3#xxkE~xzJ>;MH6P7ht^nMZZ>}cC$(RKJ%s{(uEw1f zyK2*HpPiz!qst-kyFNX8-GUoq>`O{+-9Uq~(e$u@=Hs)RVQBs!!o8nCmPDRr8d4Ks z#kp8e66?Q$F=a_OOQ0#VJ0-)Kl6E+xIh6XpA4n1wTM^G)FX3hQMBKFZ3d{=Dqbs)! zq1R6Kqv6^DXrG{o=N65JAbDGux@|cLb5|l)P4bz<r)gZGfa~?{2!?x~jY!h09<V*S ziWb`!pwppPTyteK9sgXPM$~45*eS}KvH~0EMuR!6`Wl6UZE`Vde-bcrSk8TFAy$t} z1M^HT#$i(k9=iJw7AY%}@Q!LcdZC%wTc=I-32y47H5VY$asVj}-6(*Lo-uB|cHFSH zH(<y$Eqbgf7nggirCmoFQElM>=DOc3RJV@D_&^?YCYp$8^YhP~mrEXAo!JQ0*L_I9 zrP<_~PCk_XOvSDG_s~Ua2AR9LmHT!%7`7boA*Oq#L-XJt7?+xX>hn3=KBfSgkEURK z*(9<w@1Zb1!ePA17cM^NBN{7xgLjvo;q9$vuyR2(<iFD--hM%7dc~D=gvnCDigOE$ zYudQJQ=_1!%z)NyNP>rvVYt&!mP8+UkD_O>;MQI!I0!U}g{(9Q&)4OWBicdH?ru%P zMxYfLF=V~D8f-C~1;Gm~$qn-^M!WDRtS|MT|F5q%569yB<Nxiu?Aa2LLfMn$p7WYJ zElAQrX(1}5m5NH+Eo&l6lu(vvBMC*h=bWLkL`oYf(Sp(*sg(4cKJV-LUf+LyzyHp; zX3m+zJ=e^f*X#9sJhvCzKw>d78R(UPLRDip%SA@UJk&-ldRiPb%NDD>KEhSn-YjG0 zjLgK*CLvfyJPKLLD3Z4s+4$;|M5OY}l$g9|M)y_q5Ocl*<A!1M;DIjeA;zRT`~s>7 z+krZaQt^(@4{+5IV{~)!D%f{F70sEVih5NlQSZ1AD%)!cn7lA>l4zU^{<8JB)}@-N za$ie!CCK9#UmZrEIgpCNXkX&(a|)l?G7;vdUqR9%rVyKufam*$GNYTmprD8+h-BYH zJ+rrw^9_GcZNp>iCI={Jdo$BMH5O+bP{PcjS>(C-2iy=M1s#eMgdLP82c3Q+^JVEc zV_OZH>l2IQkv{y5{DH!u1c{qFBSG9G0i#lsiIgwO!P7`BOtngbL4p(fdi51u_}quS z%^Z&nwsIH{bQ5bftO8MU5mI@|l`6Za18aB&U_DQQtju#K-_Cr;@n-I%v|EilfA<?L zS~3A!v!>|NVpn{R$z`&*c(7i@C47Yo|M3=c8y3$z)Ul`@P1X`flf_+&aazShl5lSv z5vZtv+gA;Mu3gy0rwG;L_@IYzgUHuL1=2O6u(Ri6A{C~OH;lc6-kf21_Gar*Tg46B z5q}e}Edo4#&I;lujK=&=N6@TJ1-w@IF0*M_2X36PnV2i=BafyxqwQ~+@StiL?$hXD z)(Wg5Fr5>G6}8pL)b0oz_IV}sgSm_>z6QZ9pRJH>bP#Widxt-cz0SOue*qWnR)C2! zqwzY`A)I6qg;q^fBAw(X{+n<drLMIm+1!6XYvmnk&&^UaUnmC&{wBotMk+e@ya>y> z-lDee&7lS!-o;-;Pg8%#|6&A9OLa(q?-#_kyFeXoy2tYg%wob1a3b?Jh!l*anS^p_ zTzS8m87c^)vKLlT&;QmS35jgf<d?=<6Cpz4VjI!)KPvEpQ`1D}&t)17bx8BIv5<A? z32rbBK+PF<cm*4;;IOPpOlr!|qwSK==3Gdjx(|%)eK7%Zh5CzIilj+*vLktLOcPx< z6@$8Ody!FGJ^no3kZhaCy^N+OQ=T!>P^)UmEPFpf$<)rnM?#)4HC0n!?gmLTV(<a4 za{7tACfdQBTh2sbXcUVN(L{Zg4>fbw6wZ~q8(&;ijuz(BF`KwY^!Y7MkmSoDWV=zI zO5SV8lSlDZ@XYTe_FMBB-I*MPCcn9bzkbfZ3-(GtOBd()<$eSgS7qR?Bl_Sy*_oPM ze+T!K`JojO-RQLm;yLT9kvJV|(6Q7bLDtnwe!d<u=OSut&Bu}4Qx4thqmA|6O2Nga zjnu*)mxZfSC&8jR3Aij!mmv$6Paz}HHpD3K4)XUfA@(9;!0&7l{_dmzDanpP!WRLT z&K$Jt))%A~*ui_U(H7SQ+mo97pVWI}O)Q#v0xNu%fqm<w$mEzz?ENYLD}+u*;kS&5 z<K1k=2h-@a*aRZ8AIOcOL)5w#Dd@O)9<I2XzzDvU^5NUB>uBcVS>$fRT}swyBbJXH z58HSAq<SQ3csX0A33u5~AZeWQwsE2)Qc{)$pAQ@;MQRl3&gsV8UY+>NmDMO%aTOQl z<$+<gG)4QLqkQkE!e_Vhc+;g1=<nbjD)Q4Za=vIGC`8GU<^~x!)~8JPJKhWMKEubT zv%4DGoovG=6(8bFT`E{wd@E}8s9_wwU%^)lULubUEl#Qt%)C7=LxR`Iz<M(=*dLjQ zbN#tz{gJguY3^RMI{pK4<UUTSHD{yXCIjRvW(3z(W}>tj1(GgXfrIQOLrc_2?BQ*I zR$5%;w7=oVI8HDFJ;{BEtIV!o=XFYul<1D!@-?vYcP*09b&!G$1vpFI16}2kQO)b- zply}2$h)FA;T_v#3a57Br;lw&SHT!?wkX0+!=!P#%5+$<F&8z(=@R>OXVCHfWGwPp zoJ>O^@J&ku(#D-c@*89!`0-p4w!4cl72H&X?m}H0dg2D&`EwYDce<k6z6w}YaUzNE zDx+F!^QbG413dFLD%hq!7q_<E#RW5OVCTm>vF4uNxc$5wi5t-;8U<dI)(uDCKbZzQ z3qPRPt@YG+R|m4fNEe!p8j_jUGMOKAzcB3z59SvS;fkP2Mmksq2~=gJiN=fphVRZ& ziw_;*wP_hb(~e3UG!|25K2AjKM~8%d?w_eG-EqjO`4NuunF*GQHY1ZAiezw+G&#On zn^d(v!EAySSun{NPZWGc-Y-v~C+p|I%}<W_&ZkNEiI^Tae{Bp}oAU^@8HqxvelF^5 zxrpEW5fi|eB}Ne2QH{~`8Aw-2hU71C!O`E0K++=_|5{T_nQLuAmk(EBn;m;_Me+og z^uP(8%r$_$esP%Eu7R#fUauZM=92IPjzlZ$KO+s9nM7mFTReu*A{`oC==De;m6Fec zmWt`HF!4Cr^>qT)nLif70($X@O>0dd?DQTy;ABX?dadEf8u~g={W@5vUc~%)`UG|R z%fs7yhp_JDhuCHDA#B&Kh;BqrAeY0r(28FYn&hoS6pRwlX$>!U6j6ay`0ud8ZSVgj zM?+Vy{rvyrWX=985$N2W28}a6q3`!*l8Wdx@NItyYU}c3lo}Ml$e<XT-7jYn<2ImQ zC&xj^U^ottib9(_?%=Auy5#Zvxp=IlFO_a!Njj5SguV8YAj`3xTHO<a2iv8f=YZz_ z>MrH#?@#;x&dDmKDU41NJY0GeeKVGUscoS|xcw4}f8h)M|8r2ePt`D|sPDb<$lSUG zzqR3@Li0}J*Sloc@lNO2yqZ1q_QNsc;-B?wzrjJ!*WX5Fl)JM&SIjw8lsq}fvtxDh zj?<bwPr1mpC);cG11$PgY2yn~u<PeLa@MpC+_yg#2vzs%kwdqxgYD4<@~g5OZ{5;| zhKHY1gW2QZuaf}ARrnFp5+gFhs)Di-84oihw8>@ewqE&RHd?4Z8%ODEqwhzH!<k4` z)-2eJFDM$%Z>xXC+6P>tXXpu_Qo)JMZL1|-Y#iO+?ZfH@i6E3}MlZBTAPO3SyXY+` zhS1hLtSx6nIsTbI>v3`T7_XlW#9N29x6C7J<pEq>r;;LtHj2Yy5n16Jxc=ITOp`i` z?+;GKvo3x|A8(tG8?i#_=+kIQ&dh^+sU0EWsgv0Qvg!QDS<P%zbqC3EGh)x}mtseG zCRpO(SL({yIzj|*KY{cfOR}<pQz5pM!|oDYVl90i>oQG{Z*T}7ID3Os%^k*dBa-Nh ziUPYRc@lh_rwTO_t#He}2|#nGWqT!S^0_Ppt883=KlaQb6S@k}GVuszhifL@*{R4> znN(v3lP}PesmyLZa=yl(FpiUFwXgwV`5>@Wm*YP)yUoV?=Q9U|4~cHG6s>#sBXo_C zsY$rJob5N{(bvX0)7}Gf*|(GeedFXJHgp~jn!n4?qw)Fl_~5hb>(lC3yQ32CKbVU@ zG~UNLQzS`?^lbR*^0s=5+7>+Jaxqf|3-RM8d+{dWOT4Jli_QGKm0h}UL;xS<HTY?T zgw7N%Wg~oRh}cVOHdf*QGV3}^E?kvnb*G%Aef=Eaq4zNo{j3|(tk$D^&h?`iA0yrY z*^oRd4gxg}6U|Fi=)|RB>}~iEzDlYi19?wuW3GyOca>M0i(Eu0WA0L!ufHIz5@pbT zuM2aiGSpHa5K<7yMU!8DfJuIl^bN@@VzA|PjsJCL{($&R7J9AdpWjAc<zE?M%6Tcd zlXzHfa~?XHcEHEJxjdr{G|5pv0EGjt#E_$ggvqyaK;cy8y)i?s*zF;&%sfC^MFpnM zRpOoIqS(#rL`huHQO0IqFVc4Z>LevdQ)IFm_}FtW8+HA?gEN(M@!kpzIB(w!!8=4~ z=bujq(Xy}EBqmj3-#>*Ee&={ft0d?&Ra3TzD6)Mk9oUlJTi|n+C|h9r7!4htfUnFj zL2EQOk!Q~{AYxe`+2?+m++SA@gNECneYgsS6QiK}{Wa8?`v?oJHJ`;E8zpf^Mhe=# z$ek=tD#Sm2JA*}YHKpogfbwfhu)U5uGHGAL8YfR@QK2N8RGz{QA9sUZEt1DJZwg>b zOsBD7!Rz7m_b=c+R*t=|F_BI<ejEM1HI3|96bP@M8Nrv^nfRT06uv;$<MGRrh}2C@ zka9UG;Pp+*!Rm7i!D^NZ6QE~JdeJeoa6vo>!p~B9xxPf;uYr4Un(zkCj-2ky#DRf{ zcy&7mW=ZP+tzuESD`Xoh)}B*y`{)Gzeo4g7DxheAgDT{2-^5<dRit<4c0;M22{eY4 zlatm*NN!3Qp?0bQ9TE$^P68#^?B`ECd}Ryv+7HQR^$Be9asf2!y+va@YMB-n1$16A z9fRCsDmLvXV-+h)Jl=l5zM3BJYqvD0C*>hG{hyc~>x_dMy5Vu!O}c)t2;8<tvRSu3 zu_6W4^sZVFdaU^>I&sQ&kd%>vCB->-TR;)}w(>Hn6D;aME}grH#r|nTce5L2GaNX# z(Ny7Kb`HreaRsOMMR?ML!RmX_v5duC8L&|tL!PIMBe}iGB=2S<>ekbM?MhFucd!B6 zy3v5`I9+hdw%2$_y#u?>{!YqD_~hNtZuUFnR%7TdUz4fg#(t^JAwk>UBLk_iM6l9h zA!JU_X18sxU~jc4F)}A+fdU7dv@lg88@v_}z0#c^!k$3Fo~LAPHV>}FnvtlMlUUYJ z4DU<3Pc3JZaQZ!S@JgDEj?bDxbPHloR7y4a^zsZI?ZVV%<v64@z+!hfXSAz310G9+ zvP1b>!1Uo!zE)qdfW^NsD4j8&zqrJ((urEcH{&n3L?w|@i*&kEl~1c4kt0WRjUZ`J zAbsjU7u+mxAV=p)(z!9x^ehi4_OJ(+^jqag?Ce#Dt6U2@m!kvuk11q!YB{71@5H)Z z(lFzl9Gn|-5+Bg-#+Q3T(b>5)k%`U2dpZP@P*rOht#QbPy?vmGy>??N|7_=VTCVpI zE>uvZcbrHk+sjjkx7Jaj`_Pm%X(_>1%AdoBx7$g^8AaARcs_e%o<4m>-k2EMJtD(S z6BwTMF_K*!hTfT6LP~#Xk-hn3>?bZm-aR!2>!m`pXj}&_*{8$HFYOd+3hdWGuq5ZJ zG0u^!Z1RJUQ8yA-{2fNG^r7WXBx<f_c=M;3tic_e@9MUtw)FZbitHKLgCOT1N{`NU zqf<uISqGE9peE~yP?8+&`CE+Mqql)9i4$QJI99C3g`IHhMFX@~9mFOlnwjV2C(r?( zapc4>#}P0xqy(86BE)8nCOnH4#oldyg=Pz;qG00}c=N6XrZOU*x3BaIxo}g2PPAIU zF1>q*e`v#0^wRPwlo$WO@A+C}<#0PXqF%`1m(P-dqWyI2ya;$=&_p(Vb07|JGSCxr zjeOM2!8-*fp!-KGUI~Mw#CRRPRWuEbmzfKQSGFdRl1-${4h5r#ozZB^dnrgQaRO}p z3XQZc!?j~N@v7ur>M&JD=~q=S%T`BHUbBlK8R`%jcH>uO8`X$p_|lGzvx#Pw2-~Qk zUb8&bibxzk0e|Jbkk%6u*=ZF!P~{6JcAi!mt={&H{qTGV+&<HWQhsqcnO|X9^lytK zT0>Ap%*BUDkXItUP_>_`TW7{dJ<7s~kK{;Sfg@6I8AS8V_>}co9pMG@E@ax8fLeYX z!1=2}ApF8oT5_o?{o~KBn%}#hkjDeXbll$|cqlWGj(U}a!k>GxuSXnU>X~fzcj-nP z@7Mt^Ca}aspq~!+97FJ&;5htCwGrp<wIPGjaiH~cG9;9ogy#XjNZ{9tOnHnh(GgJ~ zc5VSg_#lhd?>tOVZ=d7NW=+(0%?1Un974Vis*tZvIG5^J$|i~AfTvRon;RilQ(2eK znm&`N8F%_9TeGPWM_uFcyQa&CGSX%RucP0<_uwGV3ymPPJ|}VD*M;P7d;^K8=|zn? zTtPLjk!U@BNan8J&nT{SW?N_pI?Ikzx<>MG*)ln@Wa2G6nxKluSZYIIQX*3w_8IZ@ z&tbniGElJb4z82w!nXD;u+;V)dBIhO#;&+e@9j%wy+5?m1H=0SaG)ZD9XM5w-<&|~ z9F+#Bsp_v035+DhQD^C!1tsL=(<N|lV*-1+OqTO#uZ5n7GxWX~Lt2s*p*3!(krYou zmfl;7wrtRYU=>+vDDgJsGPD3^J(GayL=D(;<R}uPTOm{XDEy%1Dt<8UGQMtMPNyrU z(>?`)9PVqUv1Z#jEq=3aFw5SOqCckd>9^ADOk=bM+kVp@>JRp^7gg>M;gd16cW4F) zS$zh_@04YKyyWo8y;3Or8AD#1yl3=E;y8BNSeTq%OJ>eRBx*q|-e{%@WqC7%XF`Xu zZ&3|azW=q_`9lTTdN!Lfdcr4T1v(FqgTqC%{}P`TR{zHFU(bPfjL3}8MW5-wL!a1g zhdeq@W;46u{ZFV^SwwQyjw4B$t0DB(0<1o}98M=)Xa3&KfOVGfH1GUza=XU}|H?i> zZ$+|fjs75UP>#kYvu3mX#V1i42ev+ej^YeX->dn~8;ec=5X3(*gna=VJYxH7)X+bH zxtpm(HkV&O&7xDVPS|zgCh`kKro82Ey)RDleHWq8)^_sAa*z~Ao5NJ4Vr*lj0Ck)h zqrtqd%#Twugyv$2;Nox%B~J4}iJO#>Z%q%{*FS&^#_5AArv(hEkH^)b=h1HdWVF=w zxBy%DB%$_!$H?cZ327eY<2_RjqUuaz68}mEYj(PG(&_I^bCxx+YgeIf2ug|UvIts% z%HxL?f2GI0T!7|wIncV(;)rqg29Quwq-TV_2X*yKIz@C2DgApDecFEnvb&c<%$J$8 z=JZbDw{Q<x$$0@RZI4E+p9MoOPf?nDd@&h*bLr4u3X#<83XGoIy^7m^+`~Ro8-y<e z9@Lyj4Qyn*m;_0&*tFOX=Kra~QR~KX+FeW9^zsFgzx@_1Icmv2I#!JTVo@3LHV|bW zDfN^4rt{(U6lePC=Qy@BmSQ8@hls;h5qdN=6mN_Qp*sbk3gplBa<o*Wlb$qBmVJC? z2Rmzu0h^<uO54_rgS;31`271PsN||X%q_pnq!wb-vndl@6y->B8h|~lOHp9kGqmfx z9ZC7K50o9B(6yY~^nja9O~l>9EN`s}+j6D^M}0m>pIn=Qc-Hdt!p2r+f>$NmYxtH3 zWKkG)2#+Ufjx26PwZx$*4*0?mwC=ejyRvUDq7@DkcSkQ6ja4A!kzH6W;sVam;&E)X zXLwiND5a!pj85BbW!|6lBAd6_60zS==+@poW^ZmW68ebKDk~lny4{ksbTZ<<`W``x zYB;buE~Ox%G?^_rjySx+4UROBPzEnY>**a%1ajW{KvI1_H27Xe{^DYEn?sr7w^w&y z?y?lPw@r!oasrEMu_Yw?;ci$~Z$@spIpQ&=w88w`B$7Vlh13nrLG|57bYi8Dx$xsH zMZ0KoW$S^+H~KOjetnfHcYedVR3B&M<M{ljTtO*)<g`6|X1Xpszz5j?uwcX19VCY} z)Ij;bS5kV#l}vI9fzcOvc$iZP-ZuUV!PD1}2l9pJSnv-ZtCF$p0vlSRc^dSNpGKrD zZZa*65AdnvVKifG4yS!nf?JXki1O!8)UTE%%EfjzoNrA-Ig|zn1DBF-0@Z^^Quzhp zC0!({B?WxlQW^HD`5iQn?aKP=mJ!o^O93~phrt(9+0xKLT0TM)_UKP%Z#1TW=ocw? z#F>o9wTzIqM0Gg2Z9I88s7ku2T%b}{lf}gi;5sXva(HzXjeOD~Vc~&jMRz-1YvoU# zMNT4u-LpX;ndKwgC02+Vt{UQ9jVV|TFNBtR5-_<Zj*OfPVb5Eq^F<%!vT<2{^k)kl zdadJmLTP0YJ(;Pr61vOkmQN<uAKjsRNhp~Z_l|OsoPrI;O@r911w<<1J^GP+5@y`7 zM^80vQP)=;w)_Pb{GV$;R?!dfomVpiWZdOZ?D44#+0i|`E%P3uhpmg?)YF;x&;cWI zez6r!dKph``TY&QdNT;>(m&~ZKT$HXcr}a*Zy^^$R?_PuH?iZi$FNQF8tJ63=ZM`? zZFZvdPqNKG55!WGSiK506kOfG+@k8qE{A8tcG_C{<#ACm;Bf;SdzDk*U(x)P(*5}V z=48ib%u^DbCfKQe8@ZXELxUBz<k}GjT5ZV;vd3~BnXfX3q}Dy*wf}L%nHPZCvnCjP z3Jyal7wXnkb_RzIDH51uj?y3!o@$tKwA!8Msh2fgCi?=W-*>^+IBDa7O(Lkq`W%{Q zI|GUisuB9UId;BSM+I%}`d{Rx;@6O9$Z=&+zTn5*dDwF31+I72#Z`-B@qrpZ3kJXA zJcW7eic~ik7V^PR#*}OZKs%o$l5JDJqaw*Vn4}_r$e#Dqj{OcWPmfUUVUrnW-9aqZ zkwW&l4x_8LYtbu{dgkN_ZG3$fA^El=O!kQgMz>fNX`YheoTe{A-RU*~I#Ha9wyJ02 zk0ILV&WcFT+9L&_*WU?i@9igNGiQ^8hD8uoo5?sd=8(;Yy2<!qO?FFg0y-FB4%sb7 zVDs^85?EIRcHh2GOElMzo0Bt$U!5Mr&C?;5_9vmWSuK!0XBB>Z{2;98A7Mg#FOm*> z1kWcV63J148m*e}7@M9=qffh^hsk*>N%7sKbcfhvQrY_#xy|yyT5lsj0*S#E!&+<+ zLa~(<^CA5~EflI8rsESmp=X;BTR+fF@GVhtq}iNR=W1>3TR*|mfMBv=YcRRou>?%V zh6B&S35ELU5~(x`;+@~hOsh16MS_}iwDg!N>qQPh%(sQ)?NUeLYUKnEytPTCqd$7( z%84E`BcS?$BHHe~6)vT<g4(lJ_`22r8oquXRjt^I_r6<0e%{`MdM1wIx*;oa@3RIk zBk>V_oVpUu?n&mLa37JG-XVzSM0oCoaVXWl6`WL>p})wH2=qLU5li1MWI!gIOvwC( zG__<|&)7gR_nQp+EN=|^!&DIpGtV)Hw|L{$n_I}Zs}xLst4G?>bI{W~Ny0Nx1ua|d zB@-=$V&-YlGyPjo_fm7R6xI_Rt?6VcV@f+KyhZH53^wY#23fOM5qlPfk~hYVV7rrZ z{<b}ngLV2pVp;h$IC$j?>?e8}EADuUMJ8C$Ih)FG+T96k#=T?g+@UXI$M6bzUE3?v znyY}eF67foMdaB<oO|2>i%9zV(GOHfyATc!jpZDqiz%zbRgOQ!v+x}kQ+z5?x;pNx zAMP5O#Ttik*t~G=0m5pL)Z2ngRCekF*)1Uj%SIXa)-emNcv~RFP;vNL5lEGLN#kQ_ zq9lG_5}6&}hq{ZJP&;cbG!;+9rzD;ei4}jC&)OL8oi`s}oWzx<S)Yb&RjK6F=K&PC zma7=$*O1VzUzk60Hpx@nh3{*W!2B3hva?MJp6ywSCJ5TOV6u-UA&)t4d$Wh=&vtzj zkpG!1RP-SM@heDKxe429J%iNDW01{$N6=oh3kQ#@CUQ^+$4_s>g?kIYea{4TWPvEP z!%CG^HG74uW|`9ic?@y#*QIAU>(ITC4QTxm8di!Y!nax(+Te3G+IL_%EM4INg3pV* zNx!BXD?48khWeuD8tZuaQGPdGTRM@|cC=wVzs+PXff>7jb3v?pRK^&8bYV48B}uoY z8oa(BNw>U>W!C0OkY^V2@Ztx%nXIX)FdQ_1MMkyR6DN)#TMu`#(d7!UYP*Ia_kUo1 zsu(~}=og3%PX}`~4*@S`=6>)`Yy~5U9e9^tJWBl#k3Po9k>70zgvZ$;$>n6AYgX5> z!{KjC_!L*Hv*$2)RUUy9x1H$i0~-1I93je8vx&SS4~vUgqs<Rw+33J;Xw}Fo*ewvl zRy(rM2GLrw?`}AuuND%sk0V5xGJ-h|tPy@An9r^oQ76M%rlk1zLRKq53a|5807K!! zNZ7cabSPo6_`*k+Qe_F#Bj%Bbx~}Zv%7vJ|K@;M=0)obMk+z;RWVj=qz52Nj#vhvj z<F1_~YHNODHLE5t8(at{Mkm0ADVbzjc{0k<_oIiduOi1S_;B-n82ems@)Oa`7p0Y` z1K>JShpdpDK~Hg5$B1fQpyHZm(7uVsP&?X4F5TNhWL+BYD~nnzf0h$Kgl*+*<m_-f zM_f_fZUrRJnt)<k&tq*5cier;4lm)H;FmuaBf)(ecyASt<LQ%99RgyK@VJJ1%*vVv zyqmZ4kk5EcO5mEGMx2*d;oqx5gy&?Xu<WfEWHk0Km6&2AG&+_*S<O=5SOwRZu!3r4 zcX%On#?OnYX;CAo`O5IQK^nOUMwkR@5gr#Jf;MiFMK9OIW9?!){PcY)(k_b=b`*Ov z?a}fiLOmAGv}xi^(hEUTJPoL*o(GJ+%@YCMkhqfyPF5lZjV7Ykf7kP_yBfipjb5ne zsXYE{k%e3JUZBcvDNN=y7iO;AG~TaKW72S99dAc#3{Kl2%WZomlUY|Hka@)fNQu)S zZwz#aCdn2S{Juba*S?EdPBbE+U@IeLbBj7}szl8{aU2<bG6Bn|bIj4boFiT9kQ&kN zX~j9~eq)XPP(oGjLk9aDQP)!~GP1XdajMJZ?XDkXuBuWfc5xl!5p)V`9y*AwH*CQH z*UwT;Qd-Qa>^QVk&y{ND?ZEv9#F(9BWmHS`8LT+Gmb$S!o(c){VvcP~!iy`nI&O%Z zz}#x;#KV?W)J1`qAv&c~!8ouI_#HS<7k<pZlamgkn>$7+&nf**&)pn&Q;f8Qo2Ro( zgUw1rmW;<!Gj`$5XM6C1cp>Loe}dZAdl1JZT*On30xwnBmeaI);J2qQ3nPv7z;am; zZ#XlO8U4*e(h<JGhEFq@lIVI|w)Ze@e;@~fE8BhwWfEg}R==hr`8<2d{OdhLFI1o= ztX)oe*kTlr_6@n5nnGSSncz#Fa_Cvs9Yk$4AeSF5Co)0-%DvNuwz&Kdem!!CF-|^& z#FrhSF5Ox~_LRg^iZgfPeLp!alfMZy(L@rtj}CI>D+x#?M~lptRp35I6$A|L6i4*; zZe*;7ALGgczLfalbS%)hj6B!Kqi`un<Wrb{HGT4N$4xoPGx05RCUyZMzod!UXmEim zQI=qClxvYn{|xH!*A3J=)z#?IUP*Lsk1^?YT#9R~MH$IPb=+hjk8)Mj(c6Ve*l@Wn z{t#M5-6}DpjCurs{1)yOu5lURJz<XH4785P&EzBZ#v|xXybN-4ScK-=KBP{I9A-|; z?LpI<UGdF5#?1NL^lJBW{#2P;Om%XhI5i#|QPlxG6d-NoC>1p-v|c+ZRHy=6e76Uc zi@Tv9k=dX|sUhuZ5wzO!81vbW!hf%ez%{|G2qx^971qBaPx%#y;Xu_`va%xsA6MCc z>rdq3g5dRNdr%_r%%6wyjJ7al3hKP;YyiHnJ_m<Plw?fwC9qg+EY=-uVotm$!0M}# zsBFF>Qp=;rwj)=uyYfn6xNQ?@-VE3dYa<YflYIFlc%APt-d^5VEXbO2h|#O+uikBy zgvAsNqhDLYDZQhf)kS9~F%B!a-%@-5s(n$)th^(^2>p%l>Q!lYfp$4HLyL2OU>-3+ zPwTPMj~m!{$xcSXMx2~$iAP~m0@35|r<tko5%@yrG3u-JRiw1m3R^fNp&O?wg*_JK z)N$tm6c;KXz;e0D*l4x^wV`YQa%~kMQcq-rf0yN;8A%OHmfSe9U28n0c*uq~@G1_g zYbP-iKi=g~iba&t>mppY;uN(a-htU<Fi6=r++-R#b?>%<HAqCFoOd}_Oo+RpiLh20 z<^0V<PPxOp(<^6?)n6{-F$33-a^4~Q-eT<-oSwk367yEUr<D%K_-`CJ7(E8hw&q~B zYnp@#zY5XI+RxQ*%Elv!@6&M$Uj$1Y-G?$A&*8g4oz%OFL%4(|i^keDV5yEUboomU z_Pmuy{rlz@|M`99|4qt{&$y^7sxENYugeP<8Kg{lbTH><jn8F8!<nOM#BXA(ljep( zM(5*pqIY}>`Bbz3-mBceC;O&Svg1-1pJo@-eLI2az5apPzxN4q5N%>idkpZyvw#*| zGQcsa<y7;nzW*hIXC0JY`~Q=b&6qci+kAMGoPc`=qOnGt7*Xr2CqCu9Sdp@VA!$uw zxMeLAHy=O^FI?d44j(l4UOI|BERRRDXV9BGvf!kn533q21EvLv^m6$kRy+SB%)E0B z9C_!^-fwAS>ZwvTK`)8rA4(us9nWCX!;{f!nIgtC#fTKlR3s7XSOJKHJ3vO~d-D8Y zDu}B+#us_D<f_at?zs`j2xzXZ#&!ZOICmMXD|dsw>r$Y$A_eykpQbLHWAOVkhj95X zb#__(J3M5e&N89a@Fq-~pZ}9XEk%3)mnrU`r?inA=sPOR?K=*8tQ%-W-c(ZMu7d&> zxMHVC0z>-R#3tZn`*0eI8!*8#9cmUf2n%x$pd*s!N!o8AHDlKv*ttXkwv;Ty8F~xo zL_s0hvYWysYt3MNY#A<{>&bRF{zZ=O^3a3GoivUgg@ndFB6?Pu!xb2?E8avx!FOF+ zqmW0x5I;xOoREj-;>oZ-+6#sRN3YT)$JFT5VGVL`cQ$k_51?u?Ka&d;@<2}K!PQ4M zu|t3r3j3;0-=bQ0c>*!;Q(M4#Iz~}Dr2MF~-`4DtMWs;TVj^t+dk4jAUIPzqW`KG7 zLS*mOz-$;ih%P$?5UETPRwB6(UjdFTBz#8r*NxfFEA(l>#Ece_@ah(`Y35p@G5#fd zmXhIbxKv4J{8+{g4PGJ5F9q~aNGs+!pMjYS7o-TehlC+#V2;8!T0K=1UfHI>JN%gV ze+&S{EIVZN5yQOV2K2nY2|t`L0q!ZAksa;hkxZ^Wx&8be?iam*_nxw$C4Nq4oeL}l z#Qfr7_EU%j|J(F7c5K{5PTm|u=I=Klxe8`%QBouA`&OK<JbNvDF2xX@Xt+~z#C+(A zmPM$3@&>qh_Yif?VG}lXs--N@0eZqXKX~K39lJ*Rq7@0+XlX5{QgE07?-$6EYAXk_ zaet3vZlM`$?$&|6jjNb}PQi5YW@|Qb?#@9HJ}cN$p&{(_?2{DJ<6k50{*%r*YC^Y{ zPbEIOdq_^HKelL9W3{xz`H=^;=-czt@z$Q>Fghie-uCf0MyBhbn=7FVkLgEupA^8( zG4HA$JUC6IfB%Y|7t6q<Jzg;3rwzD{PG=>T6fjadWMR8t*>B3pJ&F3F<jpp7V))xB zT!HTCSC0Bs=JY=P5jx@Yci8Ckmlm;?VxMc~u^YPL>8mZ5=v=-!oY0E_D=uezvagYK zi+w`UvKO%Y<YH2{IExK$c?aIE8)!ZL8P@UQ6W|5C!||tI!vT>2<a$a1cZKS+wzb1Z zaDw4LkQdgmW*1bj?T;$z9#0yTx7*{ZKTbhZmL62D^nsdOGuoKj2426dNzeSCO6_yL zOMmJWf%o(6*h8D#SlweZ-4RnphC(KiS&pLYk+FVk5EtaWd#sY&^4BNH+_vEDK6hX| zm*WxLiDdAg5JbKg<BQc<0=9lrE;D<w2=NQ7B&+4Cu(4SbWrh@R|EmfV84yA?PSYZR z$!QcnMV36g7=YFHYr%J&i+DBXf+q3M6P}7?lbl*F_ND4O2zzq~2ku-$zu&IJ#_Tx= zU)uE8o?Exc$V=Iphsk<uQ%N_x(YHtOCQ>ZNFoJ6*1y`}})ntNA2uZn+j^ExI%Q|1M z!EW|>5Z1pKcqJj|r=YXCaN8u9d$WhrYs`ZM-@S17=AF#7yqz$rEd{6Z<)QR)CokGQ zk-aBlNbfwl7c>^qWbC>E`d#=B+U{f>=~SLTZ?<oxrcIv+&rWS&{nOnEqaVq72n?Q+ zrjf})i$@tG(lH3#`C33edpRPzR(<fw--k0wT}i<DL|~SMP|FOA*vEshOnHwY>Th`o z-qnU^ZkZu$Ehxb!#7FUYst<os)F9f2p5acNbL5Fi1e?#rg+-2hAx-zpYf8d$N$s{( z^bEHMR^NCLJ*uY13d(lt66Ft(WJWv3w30o_K6-1$${w^~H(c!y{@v|MtRxc9e3f1j zmB`sJ4zD9MJ_jM<djvkYV-uK6Jx9L2*aFQEz#(^w*{-4-=FJ}+^kY{R3NEif*=C&V zX53Z~EH%W(2GihFR2tD*kpMH^SAb8OHGi0AA*hM*oXQ?46p}=rA-rVRiQe|JkMzkJ zvUPt<*ky+jA$Y)$wyLa$l;_v+C6!#F*Xhment1HBSut$=-YcYJvj~dfO4}c<<q9?2 zQb^!xaX9(OjM%>}K+QA1BKiI?col6zoCmg}Bio5E+|v#=&7VmU&V}JOsT}oiU9Krs zaryx&Jxlmw&3ecN=h<vwZVbEJ{3)G3=@z{1l4S1<Ut-6b_psybQtAA?J-Dq)j5Y{O zVXJh<&|_~??3_VcI9sp7ie9Wo8y~i?w@s7S^FDg8ICLS&@KJ)QIx~<x^98%6I+Cl5 z4B5+k!i$CBxK;4S7J7QOAf<w{C?a7DJ;*s`2A$WZKRp`{iW0``nHXjM^Qb4Zd%!iU zseBj7F73rVzczrt_%<E3Vp>gKL>Dx^Fah7g{$$aZBdmW>GJNoj!%egqM9P0h+4j3h zvPLKz(^-!_o_G?=`%bWt^OW9oQy(OoN-2S$aUPLMC`BJSB_Vw4CML0|3%*mX5Zs<i zN%-fZ(qc*0{g*K*ui(=g>~7L`?^)6|dC~Owf<@$bjRM<lIET!UHYKYZbJ$mz3+edq zmw1}(J$$E)U}ZQ+dn<7S)Qm%r6T`W!%O(R~@|n|=Mfs>Gd@4zB?h|0!SJvcrcP}>Y z^C06Ntw5ov(j=>N5Sy#Dp-qh+(H-SV(guCZ`u>|#@~2EvBs)Taew|@w=AVa`UNh)k zhq0tfvJV=Lx&l<hv0uv~z%yQ&K9DR**Z-B~@2fsZJAA8$dspkR;bB8`!BP*`bCv9N zdlhICRlyk|ajKt~e;7kk-Vt1IS_w9k1e5izyvewMxg;{KfVbeiI%K_GMh;!u!lY<# zg|(%RK{|64a_!uR4|s^sMehbt$zCTqq@#~?N57+g2N%$~%E`pAD+O*JM)W0#^F(Z9 zC8iD&y7Ph>>!EZUc{(Z4J+pg=j=*IlIb8Vw)|Cs%=C|L7_qR3-ePxKM5MgVvDeD@r z41U*mk%og&Na@mb#Ftq?Y+lym4wVCF#i1Dd=Ml&Hm${3s?cji0)7InF=fV*)`vhE= zC{F8h&IY?vWNP5#1NN7dUQK=BJlc@=o7%qf5V^wPSz|08fFNzm)Ea3vij|aZB&WWV zk!$<`j!6823BEIt%<I!;i$hfD6sc6Ie%3MK)apa}zkfu<HOC2$btYbqt9WyycN6sz z6X-nq6dx8W<;iifx>iGu?)st(|2?`FD`&n&_m}(9Rl_Fq&fI$D&YujrszH)pA`?I_ z{545H3)(k;`Ii9JZJjen$LG*Nuhdzm`8((`-CwZC{~a0_N3)i*ir{a?RlNB|6gdCX z$2mHj)b-62_V1rIruvN>u<@p>O|2fhb+Lfen;uc*PZOG_9Dwc!o+BTP8DRaXkWy%z z4(r7=2wIknnuG7d)Wsvz&)tHR5NK<|K5DFCy<VTC-wwPddHVt}ds&2Buzc>cJkNu6 z(3YV+=dEJvcG|$h-Vx*~o=87W7bjVk&(I3@=S2VhT{ya+n!CGQNZ55JdV75ejQwIt zKf0{~5O?^xQo|%d^#fOP_y&zTQ6L<1s+0ry*a4H>j_wOit5Id*LD=40maP{?v%hz$ z(Ql5fsCksUi>&t=PwQ&DAk`u%#MYPFL?*UF=E0Th<XQLW#b>{RTgq3+{;&{+XY`W5 zNy%hg-Yi@@p99O<WRbl)^w_8(IeJc(BC`%xqFKIW$gJrT669r21}k;g8*$DsUbh?z z7Pn}EwYU}WV+@FYPb1pA_7WqyMFW=49VSERyV+8`F!V%&doS!xp<Cw0kfp;fVOl~s zdu0A3(!AUsN1piz>Qj!<Z{j~eGPk|^7U+v+8A{PG;}nd_#zNS27ko&{1IhlfK#%U^ z6P2CS@M}1dJWroT+jQR(;HCj@)U<OqZLlbuRQIZqTNAgyp;r&c`t%Ums^S=L@#zHo zV7d}L>(1Bey3I*=*AYVqo_Cd8D~ch$)rx5Txz$Y2(XW(U>o=ybOGh}EqeHB-|3ILt zKg=iXxJ+^;XsHDuyZrm4XZaN@^WZ3~UZaCvFYUtoXM${0WF`qllO@S<wM^Epaw7BV z%PXF)d>I*UGn*HxF&D27j>P$H8f5SDyD%>x8c&Kb;q)3;z;iT$mcuISgJV~4zT-sx zpXZHmZe0iAImyr&#s;it_INT?<1W?PTt?SAR?}0zyV0NT9mIpn|Dq9yhW!PZ@J6si znV5ZXq~DccBEjEHWd0yx{<;E<Gnl}16<`wHs|zl_4PiomE3s_WLhh>KuzR2rt+tA0 z)*M@kyzkk=`^;nLbX_cxHGTs-(#Mke?@wqc3A>t<d>81^x8bK3u`o2N8eMK1gzqY2 zXvdR6`e>^<edACA5jZ^!CP7b45fif>R#o_e%gt7zuyYnWZSfTtp6&xjlf#)U5ymjk z;0igl4Z@|hT5xB$2Ay<Cf`C0+$c{yxSoc#Oik&hQDWw*I*mq^<wj7TS`t+k`Q}ghn zS6SFBb1EIK?*gXEZ-gIS9c9m5RHt?CCy|0pWdfSEKT5ne$g)mew`ji?KG0PhN`}V_ z!%c;!Fik`R=KVAzyL5`dzqSy)YmuUhJ6F?QEpBw6Su1b!&{jIoAdXY8{6Wdf&yeR& zE+XI2n;fX~EndFQ6O`txWoylwd12?{m_4_(>5PQ&bf^9vS~|FuX#U#6_wf`&&}yk? zv7yc_*!P>;+s)@navj^r(A}#v{FbSiG$x&WoSsayzcn(8>wpy&9|OrfYQ(>$0e=jw z0K*a|eCm!J-NbgIttM>{cw`-k;>-pXZdQd=rXIvur3amx5`~t3Qi3gujfwO5IS`eC zpp~N=T*`P!WCW+R=r6C+SmX0mD17ytnwLM*X@PwPJ^QN-t+ZeTTcVhcy~jDw16T9d z(C#F*KkyN9Ez&@)-{u4Vls<i>HIMXpv_R1Nm*mH>AmTe|6KwCkiFU|J&>uv?!2a%J zR)~J1t#wIwr3OnTxl(w(r8_vbFUFN~pJBn6T3@(8dqdOt7+jcdNH;Z?5Fhb)HvXF@ zne(opCVJCn5+wbIEEH}d3WIti^hGfdP0E31i(aC;ZvC`N2cJIbnFvZRLm=VD2qn?6 zkG(Xd3D{***_~6bp+A2`S;<LRbjGm;nD5???8MiwTBWwU+TCI#P|Qw1qTRiTgt``a z!`161&n!a^&(?vAyf(4;-G}AYZh}+Wci^6#v2>u@0_dEZMgj($YR>wX(2fB&QI*zY zq7ZJ$R>X_a0+l9m>Gq_W2GNB?;o%5mS8Jg8V{_oZsZvhA8%^f+1ha~l4B7019z?dy zfHjhlBy!u85ByImE5!k2{-3mLqB$2Cxq?n$x5O7{_x?jTwA>M9QO@Ag`v)twT*o0} zFIUg#Jc^yt5}`BjEbqtqL*&ix!&qm@WDu3Of`3hYi~|>3MwXR{$mPyjbdpSjG3T@K zy{*&9#a{*F+iMBvtviJZbmX9}`X1S8v<5_@0`TlTSN^A!9UZY2mCxW?h~`Q7<?m9= z=&=xWkXT^vUXWp6$+e0$J&w;<U?n;>W4ooOk;I=*$yFH#twl96E?bHkN-U_m=AZG& zhCBM<-%;`Ug-#jz)3~mw71#B7UmWyrtfR!>m?`J~4I7(L@b7?Bng7%OhQ0XLjchB8 z|8JO{HP_90r0enDC71u}%APC!`){nHgkSf~`x(TVTh*&L!-D@NP`2St;Z^Mv{+mEE zW4;Zys?9r&?EP<o!~c#-DoC!#D6`=vn8}#{{~P8g!TIl+{ddV3w%jTIyu$t)ao}I~ zpNH3f%N73X{sUG2ceS7Yx*RB4{NGqdiGL92jFr>41^pw`{Wrm-Y1}FQ$Z`Ko@cv)- zpFH}%2~6y`E+<b;%J8(~Cio|!_;1+he+T}F8~&|SGvkFF_jmlu;Acop=cea!=OG!+ l|62bdd>IG+wf@E2GOGWz{@qd;-~Y9^A5%WV#$L4H{{Uua6Pf@3 From 99f98803aef0ae7c70b1cb2652dee8b3e544d219 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 25 Oct 2021 16:03:14 +0200 Subject: [PATCH 057/512] update policy for ballbot --- .../ocs2_ballbot_mpcnet/policy/ballbot.onnx | Bin 580 -> 580 bytes .../ocs2_ballbot_mpcnet/policy/ballbot.pt | Bin 1831 -> 1831 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.onnx b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.onnx index 22a85b67e9e0b3de400ef6fa629c2bae077b7161..e1caeb8a1f488100bacc89e59fb5489e93e2b193 100644 GIT binary patch delta 161 zcmV;S0AByZ1jGceE&+cJZks)0#%sRkd9gs!9PU6=ha14I0|dfsy#qf8{{caaD-S>8 z8u>mc*9*ZJ*xfyNJdwa(uJgah@CZQ{RP(`)`F%mpT)Dtjw=uvPF;+jZ>(jsjQiDK` zNu51|zJfsIz~;Y8r}{wBP}D)>af3nUzTiM(8!^CE04_hA&jkcQ5p;)>u>lqgrY6Ha Pf2wJ}h?HqOlhXllz)VXw delta 161 zcmV;S0AByZ1jGceE&+d}2f)0wPk=sWbF4rYZ~j07jgh}45Dmi210g@$F8V-Dwu(Oh z{T@9k4FbUfB}BZF$cw;3HV?qpfdN5F1@*y@aeP5i55d4=@MOT|@ufdfmFmD%l%YVt z1wFV5PKQ8{P}{$9n*%}TN#8-55rIL07TG{E<2%5))1yC?3-|;;%hi^Xu>lqgplV7# Pf?5>6x_78PlhXllHVQ}e diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.pt b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.pt index 6a1088d07117cdeb88cd6e35866121a19494e098..b5e2b6cc938e081c6481b092f21193dba4402f0a 100644 GIT binary patch delta 205 zcmV;;05bol4yO*VKm>meZks)0#%sRkd9gs!9PU6=ha14I0|dfsy#qf8{{caaD-S>8 z8u>mc*9*ZJ*xfyNJdwa(uJgah@CZQ{RP(`)`F%mpT)Dtjw=uvPF;+jZ>(jsjQiDK` zNu51|zJfsIz~;Y8r}{wBP}D)>af3nUzTiM(8!^CE04_hA&jk%Z5p;(@P)i30Y7`F2 zlgtEk8Kx$~K7XodzlfA+JWxvq2s_X$l?(s?01T7S1Raxh1*!yU6b{OhBnCJHJJ2kZ HlVAqNfoNCC delta 205 zcmV;;05bol4yO*VKm>oJ2f)0wPk=sWbF4rYZ~j07jgh}45Dmi210g@$F8V-Dwu(Oh z{T@9k4FbUfB}BZF$cw;3HV?qpfdN5F1@*y@aeP5i55d4=@MOT|@ufdfmFmD%l%YVt z1wFV5PKQ8{P}{$9n*%}TN#8-55rIL07TG{E<2%5))1yC?3-}E{%hi@ZP)i30TA5UX zlgtEk8K7!PKZ05mzq)s*K2S>s2-Y<1(F_0p01T7S1Raxh1*!yEnN)+5BnCJH)->(W HlVAqNwgFdY From ff58f0521fc5e2ed3e8415761c053d96ab2ccbc8 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 25 Oct 2021 16:05:06 +0200 Subject: [PATCH 058/512] fix relative state for ballbot --- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp index 9116651bb..e746e0ba0 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp @@ -8,7 +8,11 @@ vector_t BallbotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeSched } vector_t BallbotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { - return x - targetTrajectories.getDesiredState(t); + vector_t relativeState = x - targetTrajectories.getDesiredState(t); + Eigen::Matrix<scalar_t, 2, 2> R = (Eigen::Matrix<scalar_t, 2, 2>() << cos(x(2)), -sin(x(2)), sin(x(2)), cos(x(2))).finished().transpose(); + relativeState.segment<2>(0) = R * relativeState.segment<2>(0); + relativeState.segment<2>(5) = R * relativeState.segment<2>(5); + return relativeState; } matrix_t BallbotMpcnetDefinition::getInputTransformation(scalar_t t, const vector_t& x) { From 4103bbe9e157daa0dc249bdebde711753e4d980f Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 25 Oct 2021 16:05:34 +0200 Subject: [PATCH 059/512] use feedback policy for ballbot --- ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info index 2b0f938b5..f5e206792 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info @@ -53,7 +53,7 @@ ddp preComputeRiccatiTerms true useNominalTimeForBackwardPass false - useFeedbackPolicy false + useFeedbackPolicy true strategy LINE_SEARCH lineSearch From 28685a5264f609732f42d06c21538f1143678ee3 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 25 Oct 2021 16:06:29 +0200 Subject: [PATCH 060/512] tune initial and target states --- .../ocs2_ballbot_mpcnet/ballbot_helper.py | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py index 101e0fc78..cefbbb998 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py @@ -13,20 +13,28 @@ def get_default_event_times_and_mode_sequence(duration): def get_random_initial_state(): + max_linear_velocity_x = 0.5 + max_linear_velocity_y = 0.5 + max_euler_angle_derivative_z = 45.0 * np.pi / 180.0 + max_euler_angle_derivative_y = 45.0 * np.pi / 180.0 + max_euler_angle_derivative_x = 45.0 * np.pi / 180.0 random_state = np.zeros(config.STATE_DIM) - random_state[0] = np.random.uniform(-0.5, 0.5) # base x - random_state[1] = np.random.uniform(-0.5, 0.5) # base y - random_state[2] = np.random.uniform(-0.5, 0.5) # base yaw - random_state[3] = np.random.uniform(-0.1, 0.1) # base pitch - random_state[4] = np.random.uniform(-0.1, 0.1) # base roll + random_state[5] = np.random.uniform(-max_linear_velocity_x, max_linear_velocity_x) + random_state[6] = np.random.uniform(-max_linear_velocity_y, max_linear_velocity_y) + random_state[7] = np.random.uniform(-max_euler_angle_derivative_z, max_euler_angle_derivative_z) + random_state[8] = np.random.uniform(-max_euler_angle_derivative_y, max_euler_angle_derivative_y) + random_state[9] = np.random.uniform(-max_euler_angle_derivative_x, max_euler_angle_derivative_x) return random_state def get_random_target_state(): + max_position_x = 1.0 + max_position_y = 1.0 + max_orientation_z = 45.0 * np.pi / 180.0 random_state = np.zeros(config.STATE_DIM) - random_state[0] = np.random.uniform(-0.5, 0.5) # base x - random_state[1] = np.random.uniform(-0.5, 0.5) # base y - random_state[2] = np.random.uniform(-0.5, 0.5) # base yaw + random_state[0] = np.random.uniform(-max_position_x, max_position_x) + random_state[1] = np.random.uniform(-max_position_y, max_position_y) + random_state[2] = np.random.uniform(-max_orientation_z, max_orientation_z) return random_state From 97dd9fc02be8a6350eea5bc85062d60490c99c6b Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 9 Nov 2021 10:12:07 +0100 Subject: [PATCH 061/512] doc: add onnxruntime --- ocs2_doc/docs/installation.rst | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ocs2_doc/docs/installation.rst b/ocs2_doc/docs/installation.rst index a438bd218..6ae41593a 100644 --- a/ocs2_doc/docs/installation.rst +++ b/ocs2_doc/docs/installation.rst @@ -89,6 +89,19 @@ Optional Dependencies * `RaisimOgre <https://github.com/leggedrobotics/raisimOgre>`__ Visualizer for Raisim. Can be used for debugging purposes to see if conversions between OCS2 and Raisim are correct. +* `ONNX Runtime <https://github.com/microsoft/onnxruntime>`__ is an inferencing and training accelerator. Here, it is used for deploying learned MPC-Net policies in C++ code. To locally install it, do the following: + + .. code-block:: bash + + cd ~/Downloads + wget https://github.com/microsoft/onnxruntime/releases/download/v1.4.0/onnxruntime-linux-x64-1.4.0.tgz + tar xf onnxruntime-linux-x64-1.4.0.tgz + mkdir -p ~/.local/bin ~/.local/include/onnxruntime ~/.local/lib ~/.local/share/cmake/onnxruntime + rsync -a ~/Downloads/onnxruntime-linux-x64-1.4.0/include/ ~/.local/include/onnxruntime + rsync -a ~/Downloads/onnxruntime-linux-x64-1.4.0/lib/ ~/.local/lib + rsync -a ~/git/ocs2/ocs2_mpcnet/misc/onnxruntime/cmake/ ~/.local/share/cmake/onnxruntime + + We provide custom cmake config and version files to enable ``find_package(onnxruntime)`` without modifying ``LIBRARY_PATH`` and ``LD_LIBRARY_PATH``. Note that the last command above assumes that you cloned OCS2 into the folder ``git`` in your user's home directory. .. _doxid-ocs2_doc_installation_ocs2_doc_install: From ae2e2cac4df0b635b29cdb3607904d6e5b67d7df Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 9 Nov 2021 13:15:16 +0100 Subject: [PATCH 062/512] upgrade to onnxruntime v1.7.0 --- ocs2_doc/docs/installation.rst | 8 ++++---- .../misc/onnxruntime/cmake/onnxruntimeVersion.cmake | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ocs2_doc/docs/installation.rst b/ocs2_doc/docs/installation.rst index 6ae41593a..0bfc4766e 100644 --- a/ocs2_doc/docs/installation.rst +++ b/ocs2_doc/docs/installation.rst @@ -94,11 +94,11 @@ Optional Dependencies .. code-block:: bash cd ~/Downloads - wget https://github.com/microsoft/onnxruntime/releases/download/v1.4.0/onnxruntime-linux-x64-1.4.0.tgz - tar xf onnxruntime-linux-x64-1.4.0.tgz + wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz + tar xf onnxruntime-linux-x64-1.7.0.tgz mkdir -p ~/.local/bin ~/.local/include/onnxruntime ~/.local/lib ~/.local/share/cmake/onnxruntime - rsync -a ~/Downloads/onnxruntime-linux-x64-1.4.0/include/ ~/.local/include/onnxruntime - rsync -a ~/Downloads/onnxruntime-linux-x64-1.4.0/lib/ ~/.local/lib + rsync -a ~/Downloads/onnxruntime-linux-x64-1.7.0/include/ ~/.local/include/onnxruntime + rsync -a ~/Downloads/onnxruntime-linux-x64-1.7.0/lib/ ~/.local/lib rsync -a ~/git/ocs2/ocs2_mpcnet/misc/onnxruntime/cmake/ ~/.local/share/cmake/onnxruntime We provide custom cmake config and version files to enable ``find_package(onnxruntime)`` without modifying ``LIBRARY_PATH`` and ``LD_LIBRARY_PATH``. Note that the last command above assumes that you cloned OCS2 into the folder ``git`` in your user's home directory. diff --git a/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake b/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake index d45badeba..8dbbb0498 100644 --- a/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake +++ b/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake @@ -1,6 +1,6 @@ # Custom cmake version file -set(PACKAGE_VERSION "1.4.0") +set(PACKAGE_VERSION "1.7.0") # Check whether the requested PACKAGE_FIND_VERSION is compatible if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") From 9713ee152b1f77c27bcee1adf120880a191d19de Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 11 Nov 2021 09:09:44 +0100 Subject: [PATCH 063/512] adapt legged robot mpcnet to changes in raisim conversions --- .../src/LeggedRobotMpcnetDummyNode.cpp | 3 +-- .../src/LeggedRobotMpcnetInterface.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 73060803b..f2b9d2652 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -68,8 +68,7 @@ int main(int argc, char** argv) { std::unique_ptr<LeggedRobotRaisimConversions> conversions; if (raisim) { conversions.reset(new LeggedRobotRaisimConversions(leggedRobotInterface.getPinocchioInterface(), - leggedRobotInterface.getCentroidalModelInfo(), leggedRobotInterface.modelSettings(), - false)); + leggedRobotInterface.getCentroidalModelInfo(), false)); RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout", true); conversions->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); rolloutPtr.reset( diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 1ce90ea43..c6eedb542 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -46,8 +46,7 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout"); raisimRolloutSettings.portNumber_ += i; leggedRobotRaisimConversionsPtrs_.push_back(std::unique_ptr<LeggedRobotRaisimConversions>(new LeggedRobotRaisimConversions( - leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(), - leggedRobotInterfacePtrs_[i]->modelSettings(), false))); + leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(), false))); leggedRobotRaisimConversionsPtrs_[i]->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(new RaisimRollout( ros::package::getPath("anymal_c_simple_description") + "/urdf/anymal.urdf", From ca12691796a8b55778af3018e6645021168b47a0 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 11 Nov 2021 09:48:43 +0100 Subject: [PATCH 064/512] adapt ocs2_legged_robot_mpcnet to ocs2_robotic_assets --- .../src/LeggedRobotMpcnetDummyNode.cpp | 4 ++-- .../src/LeggedRobotMpcnetInterface.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index f2b9d2652..6abb6d6d7 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -72,8 +72,8 @@ int main(int argc, char** argv) { RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout", true); conversions->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); rolloutPtr.reset( - new RaisimRollout(ros::package::getPath("anymal_c_simple_description") + "/urdf/anymal.urdf", - ros::package::getPath("anymal_c_simple_description") + "/meshes", + new RaisimRollout(ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", + ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes", std::bind(&LeggedRobotRaisimConversions::stateToRaisimGenCoordGenVel, conversions.get(), std::placeholders::_1, std::placeholders::_2), std::bind(&LeggedRobotRaisimConversions::raisimGenCoordGenVelToState, conversions.get(), std::placeholders::_1, diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index c6eedb542..f03f607f1 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -22,7 +22,7 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr std::string taskFileFolderName = "mpc"; std::string targetCommandFile = ros::package::getPath("ocs2_legged_robot") + "/config/command/targetTrajectories.info"; // path to urdf file - std::string urdfFile = ros::package::getPath("anymal_c_simple_description") + "/urdf/anymal.urdf"; + std::string urdfFile = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf"; // set up MPC-Net rollout manager for data generation and policy evaluation std::vector<std::unique_ptr<MPC_BASE>> mpcPtrs; std::vector<std::unique_ptr<MpcnetControllerBase>> mpcnetPtrs; @@ -49,8 +49,8 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(), false))); leggedRobotRaisimConversionsPtrs_[i]->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(new RaisimRollout( - ros::package::getPath("anymal_c_simple_description") + "/urdf/anymal.urdf", - ros::package::getPath("anymal_c_simple_description") + "/meshes", + ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", + ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes", std::bind(&LeggedRobotRaisimConversions::stateToRaisimGenCoordGenVel, leggedRobotRaisimConversionsPtrs_[i].get(), std::placeholders::_1, std::placeholders::_2), std::bind(&LeggedRobotRaisimConversions::raisimGenCoordGenVelToState, leggedRobotRaisimConversionsPtrs_[i].get(), From b52c532a257d938cff377f50a40502ea3f60c246 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 18 Nov 2021 15:59:56 +0100 Subject: [PATCH 065/512] adapt to removal of anymal_c_simple_description and use lambda instead of std::bind --- .../launch/legged_robot_mpcnet.launch | 12 +++--- .../src/LeggedRobotMpcnetDummyNode.cpp | 43 ++++++++----------- .../src/LeggedRobotMpcnetInterface.cpp | 15 ++++--- 3 files changed, 33 insertions(+), 37 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch index 3e7c5652e..f7d17a578 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch @@ -6,25 +6,23 @@ <arg name="rviz" default="true" /> <arg name="multiplot" default="false"/> <arg name="description_name" default="legged_robot_description"/> - <arg name="description_file" default="$(find anymal_c_simple_description)/urdf/anymal.urdf"/> + <arg name="description_file" default="$(find ocs2_robotic_assets)/resources/anymal_c/urdf/anymal.urdf"/> <arg name="policy_file_path" default="$(find ocs2_legged_robot_mpcnet)/policy/legged_robot.onnx"/> <arg name="raisim" default="true"/> <group if="$(arg rviz)"> - <include file="$(find ocs2_legged_robot)/launch/visualize.launch"/> - </group> - - <include file="$(find anymal_c_simple_description)/launch/load.launch" pass_all_args="true"> + <include file="$(find ocs2_legged_robot)/launch/visualize.launch"> <arg name="description_name" value="$(arg description_name)"/> <arg name="description_file" value="$(arg description_file)"/> - </include> + </include> + </group> <group if="$(arg multiplot)"> <include file="$(find ocs2_legged_robot)/launch/multiplot.launch"/> </group> <node pkg="ocs2_legged_robot_mpcnet" type="legged_robot_mpcnet_dummy" name="legged_robot_mpcnet_dummy" - output="screen" args="$(arg robot_name) $(arg config_name) $(arg target_command) $(arg description_name) $(arg policy_file_path) $(arg raisim)" launch-prefix=""/> + output="screen" args="$(arg robot_name) $(arg config_name) $(arg target_command) $(arg description_file) $(arg policy_file_path) $(arg raisim)" launch-prefix=""/> <node pkg="ocs2_legged_robot" type="legged_robot_target" name="legged_robot_target" output="screen" args="$(arg robot_name) $(arg target_command)" launch-prefix="gnome-terminal --"/> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 6abb6d6d7..7ee26f03c 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -26,12 +26,12 @@ int main(int argc, char** argv) { ::ros::removeROSArgs(argc, argv, programArgs); if (programArgs.size() < 7) { throw std::runtime_error( - "No robot name, config folder, target command file, description name, policy file path, or rollout type specified. Aborting."); + "No robot name, config folder, target command file, description file, policy file path, or rollout type specified. Aborting."); } const std::string robotName(programArgs[1]); const std::string configName(programArgs[2]); const std::string targetCommandFile(programArgs[3]); - const std::string descriptionName("/" + programArgs[4]); + const std::string descriptionFile(programArgs[4]); const std::string policyFilePath(programArgs[5]); const bool raisim = (programArgs[6] == "true") ? true : false; @@ -40,11 +40,7 @@ int main(int argc, char** argv) { ros::NodeHandle nodeHandle; // legged robot interface - std::string urdfString; - if (!ros::param::get(descriptionName, urdfString)) { - std::cerr << "Param " << descriptionName << " not found; unable to generate urdf" << std::endl; - } - LeggedRobotInterface leggedRobotInterface(configName, targetCommandFile, urdf::parseURDF(urdfString)); + LeggedRobotInterface leggedRobotInterface(configName, targetCommandFile, urdf::parseURDFFile(descriptionFile)); // gait receiver auto gaitReceiverPtr = @@ -63,7 +59,7 @@ int main(int argc, char** argv) { // rollout std::unique_ptr<RolloutBase> rolloutPtr; - raisim::HeightMap* terrain = nullptr; + raisim::HeightMap* terrainPtr = nullptr; std::unique_ptr<RaisimHeightmapRosConverter> heightmapPub; std::unique_ptr<LeggedRobotRaisimConversions> conversions; if (raisim) { @@ -71,25 +67,24 @@ int main(int argc, char** argv) { leggedRobotInterface.getCentroidalModelInfo(), false)); RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout", true); conversions->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); - rolloutPtr.reset( - new RaisimRollout(ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", - ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes", - std::bind(&LeggedRobotRaisimConversions::stateToRaisimGenCoordGenVel, conversions.get(), std::placeholders::_1, - std::placeholders::_2), - std::bind(&LeggedRobotRaisimConversions::raisimGenCoordGenVelToState, conversions.get(), std::placeholders::_1, - std::placeholders::_2), - std::bind(&LeggedRobotRaisimConversions::inputToRaisimGeneralizedForce, conversions.get(), std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5), - nullptr, raisimRolloutSettings, nullptr)); + rolloutPtr.reset(new RaisimRollout( + ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", + ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes", + [&](const vector_t& state, const vector_t& input) { return conversions->stateToRaisimGenCoordGenVel(state, input); }, + [&](const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { return conversions->raisimGenCoordGenVelToState(q, dq); }, + [&](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return conversions->inputToRaisimGeneralizedForce(time, input, state, q, dq); + }, + nullptr, raisimRolloutSettings, nullptr)); // terrain if (raisimRolloutSettings.generateTerrain_) { raisim::TerrainProperties terrainProperties; terrainProperties.zScale = raisimRolloutSettings.terrainRoughness_; terrainProperties.seed = raisimRolloutSettings.terrainSeed_; - terrain = static_cast<RaisimRollout*>(rolloutPtr.get())->generateTerrain(terrainProperties); - conversions->terrain_ = terrain; + terrainPtr = static_cast<RaisimRollout*>(rolloutPtr.get())->generateTerrain(terrainProperties); + conversions->setTerrain(*terrainPtr); heightmapPub.reset(new ocs2::RaisimHeightmapRosConverter()); - heightmapPub->publishGridmap(*terrain, "odom"); + heightmapPub->publishGridmap(*terrainPtr, "odom"); } } else { rolloutPtr.reset(leggedRobotInterface.getRollout().clone()); @@ -104,9 +99,9 @@ int main(int argc, char** argv) { leggedRobotInterface.modelSettings().contactNames3DoF); std::shared_ptr<LeggedRobotVisualizer> leggedRobotVisualizerPtr; if (raisim) { - leggedRobotVisualizerPtr.reset(new LeggedRobotRaisimVisualizer(leggedRobotInterface.getPinocchioInterface(), - leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, - nodeHandle, 100.0, terrain)); + leggedRobotVisualizerPtr.reset(new LeggedRobotRaisimVisualizer( + leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + static_cast<LeggedRobotRaisimVisualizer*>(leggedRobotVisualizerPtr.get())->updateTerrain(); } else { leggedRobotVisualizerPtr.reset(new LeggedRobotVisualizer( leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index f03f607f1..3012d45b0 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -51,12 +51,15 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(new RaisimRollout( ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes", - std::bind(&LeggedRobotRaisimConversions::stateToRaisimGenCoordGenVel, leggedRobotRaisimConversionsPtrs_[i].get(), - std::placeholders::_1, std::placeholders::_2), - std::bind(&LeggedRobotRaisimConversions::raisimGenCoordGenVelToState, leggedRobotRaisimConversionsPtrs_[i].get(), - std::placeholders::_1, std::placeholders::_2), - std::bind(&LeggedRobotRaisimConversions::inputToRaisimGeneralizedForce, leggedRobotRaisimConversionsPtrs_[i].get(), - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5), + [&, i](const vector_t& state, const vector_t& input) { + return leggedRobotRaisimConversionsPtrs_[i]->stateToRaisimGenCoordGenVel(state, input); + }, + [&, i](const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return leggedRobotRaisimConversionsPtrs_[i]->raisimGenCoordGenVelToState(q, dq); + }, + [&, i](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return leggedRobotRaisimConversionsPtrs_[i]->inputToRaisimGeneralizedForce(time, input, state, q, dq); + }, nullptr, raisimRolloutSettings, nullptr))); } else { rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(leggedRobotInterfacePtrs_[i]->getRollout().clone())); From 3e566f7e3cf54c7283c7f13064b4e03a2f4674b8 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@student.ethz.ch> Date: Thu, 18 Nov 2021 18:50:52 +0100 Subject: [PATCH 066/512] one thread per onnx runtime session --- ocs2_mpcnet/src/control/MpcnetOnnxController.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp index 1d0ee87b8..debc4ae01 100644 --- a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp @@ -9,6 +9,8 @@ void MpcnetOnnxController::loadPolicyModel(const std::string& policyFilePath) { policyFilePath_ = policyFilePath; // create session Ort::SessionOptions sessionOptions; + sessionOptions.SetIntraOpNumThreads(1); + sessionOptions.SetInterOpNumThreads(1); sessionPtr_.reset(new Ort::Session(*onnxEnvironmentPtr_, policyFilePath_.c_str(), sessionOptions)); // get input and output info inputNames_.clear(); From 6dfc744ed960ca9c8c313875d6656f69c72feb1a Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@student.ethz.ch> Date: Thu, 18 Nov 2021 18:53:11 +0100 Subject: [PATCH 067/512] settings from pc for legged robot --- ocs2_mpcnet/python/ocs2_mpcnet/config.py | 2 +- .../legged_robot_mpcnet.py | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/config.py b/ocs2_mpcnet/python/ocs2_mpcnet/config.py index 6f925d44b..9622c9a9c 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/config.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/config.py @@ -4,4 +4,4 @@ dtype = torch.float # device on which tensors will be allocated -device = torch.device("cpu") +device = torch.device("cuda") diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index dc150b0a3..e18dec0ed 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -21,9 +21,9 @@ data_generation_time_step = 0.0025 data_generation_duration = 4.0 data_generation_data_decimation = 4 -data_generation_n_threads = 5 -data_generation_n_tasks = 10 -data_generation_n_samples = 1 +data_generation_n_threads = 12 +data_generation_n_tasks = 12 +data_generation_n_samples = 2 data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order='F') for i in range(0, 3): data_generation_sampling_covariance[i, i] = 0.05 ** 2 # normalized linear momentum @@ -39,8 +39,8 @@ # settings for computing metrics by applying learned policy policy_evaluation_time_step = 0.0025 policy_evaluation_duration = 4.0 -policy_evaluation_n_threads = 1 -policy_evaluation_n_tasks = 2 +policy_evaluation_n_threads = 3 +policy_evaluation_n_tasks = 3 # rollout settings for data generation and policy evaluation raisim = True @@ -61,7 +61,7 @@ gating_loss = GatingLoss(torch.tensor(epsilon, device=config.device, dtype=config.dtype)) # memory -memory_capacity = 1000000 +memory_capacity = 500000 memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) # policy @@ -77,7 +77,7 @@ torch.save(obj=policy, f=save_path + ".pt") # optimizer -batch_size = 2 ** 5 +batch_size = 2 ** 7 learning_iterations = 100000 learning_rate_default = 1e-3 learning_rate_gating_net = learning_rate_default From a46b65b40be76fc0d3f24a580ac548556385ea13 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 19 Nov 2021 17:48:59 +0100 Subject: [PATCH 068/512] adapt to checks disabled by default --- .../src/LeggedRobotMpcnetDummyNode.cpp | 4 ++-- .../src/LeggedRobotMpcnetInterface.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 7ee26f03c..8ad08e023 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -63,8 +63,8 @@ int main(int argc, char** argv) { std::unique_ptr<RaisimHeightmapRosConverter> heightmapPub; std::unique_ptr<LeggedRobotRaisimConversions> conversions; if (raisim) { - conversions.reset(new LeggedRobotRaisimConversions(leggedRobotInterface.getPinocchioInterface(), - leggedRobotInterface.getCentroidalModelInfo(), false)); + conversions.reset( + new LeggedRobotRaisimConversions(leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo())); RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout", true); conversions->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); rolloutPtr.reset(new RaisimRollout( diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 3012d45b0..f60522c2c 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -46,7 +46,7 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout"); raisimRolloutSettings.portNumber_ += i; leggedRobotRaisimConversionsPtrs_.push_back(std::unique_ptr<LeggedRobotRaisimConversions>(new LeggedRobotRaisimConversions( - leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(), false))); + leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo()))); leggedRobotRaisimConversionsPtrs_[i]->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(new RaisimRollout( ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", From 2101dfdc1db1f647708c3d12524a65279c30bb68 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 26 Nov 2021 16:58:43 +0100 Subject: [PATCH 069/512] only trot for now --- .../legged_robot_config.py | 6 +- .../legged_robot_helper.py | 140 ------------------ .../legged_robot_mpcnet.py | 13 +- 3 files changed, 6 insertions(+), 153 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py index 047130b49..a8ff93fb7 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py @@ -27,7 +27,7 @@ INPUT_DIM = 24 # expert number -EXPERT_NUM = 8 +EXPERT_NUM = 3 # default state default_state = [0.0, 0.0, 0.0, @@ -77,7 +77,3 @@ expert_for_mode[15] = 0 expert_for_mode[6] = 1 expert_for_mode[9] = 2 -expert_for_mode[7] = 3 -expert_for_mode[11] = 4 -expert_for_mode[13] = 5 -expert_for_mode[14] = 6 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py index 99abb6622..305e930d5 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -86,110 +86,6 @@ def get_random_target_state_trot(): return np.array(config.default_state) + random_deviation -def get_dynamic_diagonal_walk_1(duration): - # contact schedule: RF_LH_RH, RF_LH, LF_RF_LH, LF_LH_RH, LF_RH, LF_RF_RH - # swing schedule: LF, LF_RH, RH, RF, RF_LH, LH - event_times_template = np.array([0.15, 0.3, 0.45, 0.6, 0.75, 0.9], dtype=np.float64) - mode_sequence_template = np.array([7, 6, 14, 11, 9, 13], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) - - -def get_dynamic_diagonal_walk_2(duration): - # contact schedule: LF_LH_RH, LF_RH LF_RF_RH, RF_LH_RH, RF_LH, LF_RF_LH - # swing schedule: RF, RF_LH, LH, LF, LF_RH, RH - event_times_template = np.array([0.15, 0.3, 0.45, 0.6, 0.75, 0.9], dtype=np.float64) - mode_sequence_template = np.array([11, 9, 13, 7, 6, 14], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) - - -def get_random_initial_state_dynamic_diagonal_walk(): - max_normalized_linear_momentum_x = 0.4 - max_normalized_linear_momentum_y = 0.2 - max_normalized_linear_momentum_z = 0.2 - max_normalized_angular_momentum_x = config.normalized_inertia[0] * 52.5 * np.pi / 180.0 - max_normalized_angular_momentum_y = config.normalized_inertia[1] * 52.5 * np.pi / 180.0 - max_normalized_angular_momentum_z = config.normalized_inertia[2] * 30.0 * np.pi / 180.0 - random_deviation = np.zeros(config.STATE_DIM) - random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x / 2.0, max_normalized_linear_momentum_x) - random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) - random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) - random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) - random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) - random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) - return np.array(config.default_state) + random_deviation - - -def get_random_target_state_dynamic_diagonal_walk(): - max_position_x = 0.275 - max_position_y = 0.1375 - max_orientation_z = 25.0 * np.pi / 180.0 - random_deviation = np.zeros(config.STATE_DIM) - random_deviation[6] = np.random.uniform(-max_position_x / 2.0, max_position_x) - random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) - random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) - return np.array(config.default_state) + random_deviation - - -def get_static_walk_1(duration): - # contact schedule: LF_RF_RH, RF_LH_RH, LF_RF_LH, LF_LH_RH - # swing schedule: LH, LF, RH, RF - event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) - mode_sequence_template = np.array([13, 7, 14, 11], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) - - -def get_static_walk_2(duration): - # contact schedule: RF_LH_RH, LF_RF_LH, LF_LH_RH, LF_RF_RH - # swing schedule: LF, RH, RF, LH - event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) - mode_sequence_template = np.array([7, 14, 11, 13], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) - - -def get_static_walk_3(duration): - # contact schedule: LF_RF_LH, LF_LH_RH, LF_RF_RH, RF_LH_RH - # swing schedule: RH, RF, LH, LF - event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) - mode_sequence_template = np.array([14, 11, 13, 7], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) - - -def get_static_walk_4(duration): - # contact schedule: LF_LH_RH, LF_RF_RH, RF_LH_RH, LF_RF_LH - # swing schedule: RF, LH, LF, RH - event_times_template = np.array([0.3, 0.6, 0.9, 1.2], dtype=np.float64) - mode_sequence_template = np.array([11, 13, 7, 14], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) - - -def get_random_initial_state_static_walk(): - max_normalized_linear_momentum_x = 0.25 - max_normalized_linear_momentum_y = 0.125 - max_normalized_linear_momentum_z = 0.125 - max_normalized_angular_momentum_x = config.normalized_inertia[0] * 45.0 * np.pi / 180.0 - max_normalized_angular_momentum_y = config.normalized_inertia[1] * 45.0 * np.pi / 180.0 - max_normalized_angular_momentum_z = config.normalized_inertia[2] * 25.0 * np.pi / 180.0 - random_deviation = np.zeros(config.STATE_DIM) - random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x / 2.0, max_normalized_linear_momentum_x) - random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) - random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) - random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) - random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) - random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) - return np.array(config.default_state) + random_deviation - - -def get_random_target_state_static_walk(): - max_position_x = 0.25 - max_position_y = 0.125 - max_orientation_z = 20.0 * np.pi / 180.0 - random_deviation = np.zeros(config.STATE_DIM) - random_deviation[6] = np.random.uniform(-max_position_x / 2.0, max_position_x) - random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) - random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) - return np.array(config.default_state) + random_deviation - - def get_tasks(n_tasks, duration, choices): initial_observations = helper.get_system_observation_array(n_tasks) mode_schedules = helper.get_mode_schedule_array(n_tasks) @@ -213,42 +109,6 @@ def get_tasks(n_tasks, duration, choices): target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), get_random_target_state_trot().reshape((1, config.STATE_DIM)), np.zeros((1, config.INPUT_DIM))) - elif choices[i] == "dynamic_diagonal_walk_1": - initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_dynamic_diagonal_walk(), np.zeros(config.INPUT_DIM)) - mode_schedules[i] = helper.get_mode_schedule(*get_dynamic_diagonal_walk_1(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_dynamic_diagonal_walk().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) - elif choices[i] == "dynamic_diagonal_walk_2": - initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_dynamic_diagonal_walk(), np.zeros(config.INPUT_DIM)) - mode_schedules[i] = helper.get_mode_schedule(*get_dynamic_diagonal_walk_2(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_dynamic_diagonal_walk().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) - elif choices[i] == "static_walk_1": - initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_static_walk(), np.zeros(config.INPUT_DIM)) - mode_schedules[i] = helper.get_mode_schedule(*get_static_walk_1(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_static_walk().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) - elif choices[i] == "static_walk_2": - initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_static_walk(), np.zeros(config.INPUT_DIM)) - mode_schedules[i] = helper.get_mode_schedule(*get_static_walk_2(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_static_walk().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) - elif choices[i] == "static_walk_3": - initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_static_walk(), np.zeros(config.INPUT_DIM)) - mode_schedules[i] = helper.get_mode_schedule(*get_static_walk_3(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_static_walk().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) - elif choices[i] == "static_walk_4": - initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_static_walk(), np.zeros(config.INPUT_DIM)) - mode_schedules[i] = helper.get_mode_schedule(*get_static_walk_4(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_static_walk().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index e18dec0ed..fdadb2581 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -78,7 +78,7 @@ # optimizer batch_size = 2 ** 7 -learning_iterations = 100000 +learning_iterations = 50000 learning_rate_default = 1e-3 learning_rate_gating_net = learning_rate_default learning_rate_expert_nets = learning_rate_default @@ -86,16 +86,14 @@ {'params': policy.expert_nets.parameters(), 'lr': learning_rate_expert_nets}], lr=learning_rate_default, amsgrad=True) -# weights for ["stance", "trot_1", "trot_2", "dynamic_diagonal_walk_1", "dynamic_diagonal_walk_2", -# "static_walk_1", "static_walk_2", "static_walk_3", "static_walk_4"] -weights = [1, 2, 2, 2, 2, 1, 1, 1, 1] +# weights for ["stance", "trot_1", "trot_2"] +weights = [1, 2, 2] def start_data_generation(alpha, policy): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - choices = random.choices(["stance", "trot_1", "trot_2", "dynamic_diagonal_walk_1", "dynamic_diagonal_walk_2", - "static_walk_1", "static_walk_2", "static_walk_3", "static_walk_4"], k=data_generation_n_tasks, weights=weights) + choices = random.choices(["stance", "trot_1", "trot_2"], k=data_generation_n_tasks, weights=weights) initial_observations, mode_schedules, target_trajectories = helper.get_tasks(data_generation_n_tasks, data_generation_duration, choices) mpcnet_interface.startDataGeneration(alpha, policy_file_path, data_generation_time_step, data_generation_data_decimation, data_generation_n_samples, data_generation_sampling_covariance, @@ -105,8 +103,7 @@ def start_data_generation(alpha, policy): def start_policy_evaluation(policy): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - choices = random.choices(["stance", "trot_1", "trot_2", "dynamic_diagonal_walk_1", "dynamic_diagonal_walk_2", - "static_walk_1", "static_walk_2", "static_walk_3", "static_walk_4"], k=policy_evaluation_n_tasks, weights=weights) + choices = random.choices(["stance", "trot_1", "trot_2"], k=policy_evaluation_n_tasks, weights=weights) initial_observations, mode_schedules, target_trajectories = helper.get_tasks(policy_evaluation_n_tasks, policy_evaluation_duration, choices) mpcnet_interface.startPolicyEvaluation(policy_file_path, policy_evaluation_time_step, initial_observations, mode_schedules, target_trajectories) From 862ed1c7986982d2f37246fa5265e975aa741bec Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 26 Nov 2021 17:25:34 +0100 Subject: [PATCH 070/512] learning iterations back to default --- .../python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index fdadb2581..2ae06686e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -78,7 +78,7 @@ # optimizer batch_size = 2 ** 7 -learning_iterations = 50000 +learning_iterations = 100000 learning_rate_default = 1e-3 learning_rate_gating_net = learning_rate_default learning_rate_expert_nets = learning_rate_default From a08e04590f9080a3c73e0d5728a996d8245d2dc8 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 26 Nov 2021 17:51:17 +0100 Subject: [PATCH 071/512] add trot only policy --- .../policy/legged_robot.onnx | Bin 71976 -> 30761 bytes .../policy/legged_robot.pt | Bin 79865 -> 33205 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx index 6e2ef510dceaa114c6897aa40b15aa8e7c48c940..21dda591c641a58f7b2327bcb59d985e999185d3 100644 GIT binary patch literal 30761 zcmeFZ2{cz-`#&5qix3$rG$3Rwe9yklH#E>3Q5vLDsSK4gE6I?lh!TYisU#^yoP8Zh zQW`Z%8Wl;3N<{U3@8`MiXLx`2^LwB5to47_yWaI!>#TFmckgrd=lYzz_xW7=x(*pB z73sjuK>_QQuGEsSFtf8y_#`H0BqtUmC$>pN+`?8%#xcNusdtc<xxUn2kBsHz`g?Ek zU9TaQDJCxGFDI%br@F#B$k%^`m%mStnYr0UAKw)#gXEO|>Bl9$-s?p-w9}HB<m2b( zWuf2yx4p5FoTT@fz?I%={rc@>GsVP=73KOb@d^6-`2abopmpB<>mB~up1P<#I~8$D zb1f-1Z~v8EmiqmF+y92c()^!rSpFFfOS8Y?u(bFcht+?G!${Pgg$ReGmaJ>Q@*qF& zO<vae(!U@6Gev)6)6(j1Y}))en>)osx3!Yf@!1sUvo7d2I@gQ%T;uES<Gs$}4|}x! zy!3a&tbaFb?0;yOh+s==kzqE!6FlyBg8u@|=Fg%1jp{#z_BYPQ{tnIdKZN#oBFFuW zNV`8LGW0hh$Ne)}i~p6#znA__q^*|pUwf+ge+YN52!gGMNjoj+zjRd#QJ?=yU;PU> zyFUl_=T!ay?hh)h%>Mu<>i_@II5PfYh+A1`$^JgXEv)`EnN}A6jMwsiCG+p4e<jn( z@(;Y${~=xxnO2r6;#O9FkZJP|WLo{-hRh$$|2dg|FZ~^y^&jB=(=lTvdeUP>;B2%M z=J>B)yTQk2i;tJZIDOf_o&9x2@L4Yu&lHoE6V<me7u6s8PxWp8*ZQI{X*KSj>f8Ox z`sSkh+laQYqHUb$I@`ZJV$1(Ijf)V8|Axr&H+t><u?0(uf7!xN(VcBYn`kPwHrJA! z<{dO`!x}G3OMR)o9?BWZi3&u87Fu$yzY}6<C7K}rdi+1=uolgw)|UU&xb?qm{DO+8 zvXz#+sN1d&^7aq%vb53v?KSn66XO&WImuw}H5+`4V^n0sWHe+n28y}tRO#2R#-ZQ; z@xTAJ@$XaoU#|Yk)xXcczhCqJFa2uR_<!H#|Fq_R+xYjH`!84j<?7#O;NP$L|4zTk z6zeZ1Qf<~&qD`dqtgZj9+Q$BqY7+@fYny*iZR7rhYD*9kRUG?|D$MpDR9Mg-B5dVv z8cd|X7XLxWgVJAr6Jr0)@gLXyKka`rP*Y@_NcdWd#JKf1EvfO#mU&s)=}U<Af7os% z^2n@3diZz6Y9-Rczbn><;-V6}f3U~O;veiO75~GaeDS}iN(qrond1Kn)Nh9UvHgFc z&cDL&fBN@79*W3Uuon3WHlk&l-yVvU<zF6(%D}%YJejWY4KfuiY6PqcGQOoIqbf6e zpqRs-Eh)L2Q5!eCfEbO~KsEYD(YVr+aP!I<7*oVTP*50DpL_s&G|I``_EBZS?kr_? zyHO}<s%C1}7n4Q59)sb7B0_6CFt|w`vn#jb)%fY)SUZl_&YVbxYwN>=S?lo_*F~3a zGsJP<XW$A)eWtMIAe^YZPP_~kF%I#c@X*#a!cG{1=ln0xpHp<<`?0-{Cb5s+bZMlD ziTRKxoCjIG=2UgfAoMrY1I1bAU{uy!>UH}qEuS|XenfpECQCHQ%IHCu)47t7GCxdj zg_<)1TXo3|brX7O<}_@NH^g53ER<Q83&Ah}x|KJ;lFA_%dc%Qw$Y#TCMZncX-6Uyf z40_UCaJOinV5sUaOzFHw@@E&({YE?Rb7%<eZ<$3#rW{6vnCF<kBMrY@7o(@AsiOGP zJoK8{N~f+HMAH4#!2E+I?!4QIOSTy?O(#-dcWynIBj``cW74p<UYZ@YqmfxSt%ttd zWdr2aHs+>tEj+Z2#mG7b`f0*Wyx2Dfvjzl`r|S=Z_tO|~4A8=Zkch;sKWY@EU`;|l z_*ItAJTV=IIYFuTX+H-|XOiGcNItqhIED|S)giWH1bBJ@8W+gWYV|3YpB{=IyBvY( z>mdtEm6%DVQ^BRn9OVv(v3}Ku$WCWRQrYK6S%*H1^UtC3EG0JUmF@1vEkN^?Cm>$q z2vM9b%f9QaLY*;2&|(`yK76pjk{6Af%Hb3ebW<J_`TJD*NEW6^3E{5%4<^<2I*yCm zgJ<?kgA;ZDDK%U0a9<4lcJm-z_rn%Hw;2+@TkpwDKA#qhA4jh0sp6}-7OYo%AQ&(3 zM2O$V6u&WpP>U$yv0@d}SZ@P`>&vjdDi91S+DNCqI%GVHqEh2E%k}9(h*Nt)y#~mF zW#@Qwsb<l6(R;F_Vj8HQy@;pU@8D0&B0qoDG1ujnVN+5jHrYhM7dHhc^1DbL_Q;Xf z7j();3;RQFUMy|qZBV-~n#g3H0<lVWblX3Wj9_<BhC538RXULxBSTzV;zP^ZVj!Tb zhECNh#q~zFnY5zAaQ;UadRD0st;D^cc;pl~b+ys#sSfz)i82_>PlTCnKRC^wK`6O+ z5g2<810f7S`z{Om6wi=V`y_DN=s_U4${nf$R+aZTR}zwMMjk&j#Z|%mz|h2u??@bm z(W*SWFTKcgb(|(^@=Re<t}69DTT0GvKL!mAR%j-sgUUsp@tplBDCv;EzT8~yWkwJ4 zjQHZG^f>sqs1jbDTZhs;YS>kt2`8+c!J<LQR2aSrdt^t!ulq)@D=!|$ocqQcRLh_V z5AVVJg}Y#&%6S}N?ux2`1DN?*g-}@U%g4T3LQE6wq02R$q?}%Y`&4e>cC!R9+VPF* zG?&qw#6mDyy&UR1#-e)8bbQIJg6Ql5pdqW6>Ywc--FPdW4IGN_)(hHZo4|pZt7yG> z5zaH53zOf>faWe=YM;Iz?@dynWk=SNdmoy)HQzU4XSE*rzH~n1-e@8=d&;P+{wQql zHD){QRG_J485)icr%#nnkw*s?gIn8C%*c<X-m5#xBX8v5j|Vx-qjMWb!Hg5IahnbH z9<{)E71=Z~-UDA=c*y-sn1<^m0`YzK3;OHXeax%K#X+~CaOMV2^k2||cJE)|<1qtJ z-2P6P!<w0x^=2{R()Yy9L56gThr{>k{pGJamf#0&94q&0Bi!vhgT-O<iB6pqR4uh; z0?y2)OE)cq_p7$zcBLvooa1%kK1rLEO*(|WHR@;z^~^-n#N~F8_~i0!oRe}6m0Zi| zWZ@f>h)%=snJ2LHSSk)(zZdh~lp<;NLEX^37#O8S`ppPq9Fm`--C7y2TpbDD7lkli z&PKwcm}B^3p$0p)r<&mW8dTk|f_o7As9gDaA>_m>;(@nuT$BG2WL>6Vqp**tO1iKE zCQc&hYDIM3-2w2;bU1n+`arvqV=<$+90Q64nDF!!9!N~Zh?1q47xfIU{5*;#NA{u5 zFcqw7;xTCXMGj+CaH3QNHq07E3P<zsLQV=^43{JqzUJb`v&L*#o*i7?8;lqEv*hgc z8Stt_5q1q_X|zECT<wa)dG_&``*bH+niT^XCR>Qddo|RV9FGkPJE>+p149iAAu-Vo z#7D)#kDKox<Ng&$dPrfNmpCi_BL!}{a1eS>97>mWl8VNIjKL6ZGF3yDy;RmL*ml7l z=Shyh=MU!5imYmo^&821w<@8=qutPXK#%wg3d17<J?KQk(V$oThSEMys2b=4hj+Tt z>CY0d^^y%KdNu~M&GPB_VFjc(I0}j$G{ZgnFHp3lAH;k%g%$w^Lod0(R&OU5B`yK> zQ+mk2XYO!+Mg}|<Hj$C+Oej;^4+|y@0f$E+=xla^>W`AfwAy4afFF3z+!FI9CqnJg zLDauN95<=&qvy{Rk*p){xRb|@Lwf5~Ao0#Pyh4+|b8RM!UDQA~E}n+`w0SUmFyOSC zuF$s85f<zI;7WWeF=<&6TrL!X@-S_@GuRM|K2L{F!);;C_7FN+Hi_zHD?q4v6Dcge zPu9HGAa9*iaN4kNDs@$z+GVBSn8I!n87j?iJK~@_;xk!1YC4MLWIze$0S@(6*jW`z z&WSU`p=B=EpYNtmHX$9Gu^X}-m*G~YWL%}_3y&6U1<q6lE#_WA&moIo6j==UMtjh4 z)^RHR+K4>TkN~spU9{kQEH&C`iao_ym^H}{N}Uy%%1O~o(1#Rq@o^3<Q7~ie1b$?} z$ZAx|NG5|*hJqhRaq=En+~iCR81^LsH6<gd;U!-<syhps)JEXoLT$R-VK+Q}l7ej+ z8T9FSSNK)Q!T59iG2r0=deY`Gyt0dg1?>}YdhJ1)TYLdJ4NAy-y##tX{Smzo6-;hd zSA(W(0%0P?<J^W$y18Tk?EmQ^u;9ITMKet}mv0F(3<p8c33+<5a1o9$N&qLHxAfC& z2_hF{0q;+x!jN}y%=w5U^j67(f-^rEUxRpjG5QEe**1&W77;@SeklWO7gtz0Ul{|H zvrtml4oaEP#M?lZF0#EzgL-CzvBfr0v@g5NQ|2-Fb5k(WRv%YSt|p1GH@TmO(#Yes zW&B6s8@fmG26-BlPY1Y0!Tk%4q~*;K)DkLEC%*t7AdmJ(!?-)r7jXYzUD!MFCoQ>Y z%=sAY1M#Om)OWZenGl@^)>FNxx`GoLsZ_#;4W%&lK|0lR5MWq+4s_&PfTOSEFm|6f zr+hL-5UbY!Zna|2-%1)in^%$2pc`C<-cc}FC*ZHP>R@@o1jv(nMN@V;;Lcz1xMMPb z7Pnb=V^|2zGN>f&yFL<UDKD}(Egauo9RQ83_sNr<Bh=VOMpdL{|7R@JW$OB%1-#a> zqj)W01<Um5vkd}wwjtbxz2arYd*D1?<D4ezwZ@EH*fp2!x$e%cO%3GbldSk?O&6TH z%bGo6JD*>|`toGUB;I(JxQxVKTKY%b{73x5rAcg=+z#HUC>{5Y9nZR*-9Q!A3}o-x zrh~A15}&g!j=v~#7&?=8vaP+F*vsX1tk_T;-W1Gu;%|mS#D%O)!We$ei4A-#4(3}9 zPi2!OieQQG2fcP_18?J}!Uv8#1nJYJvd1R9$Df8jiT&%R*rh=+B>xPGt>^%WfxX~1 zU?9KiK?lB2k>jfuh_NqfRCxRC`XJ*c(mN7H{6vjmpq_mS+6z?p(N%9TDuZQR)@kuh zU26PC-~0F@&=058mV(<RJyw083R`n615`gNvzn&Q@aWmAU_3%b<dyvs*MAL|xhQPj zt^qcy_3`M8nP?oTFEivXKF0qVXLWg@m(Wy9ha|Z`(-0j{y~S~!sU>uF*>=*f{WYiQ ztwCxEUek5{8Xyxs1V6i1l4Gah*yStRVeI3BY|O&_LZ6X6EV&oMt1dTSr;b^PVv1iO zD*Y5Oc%H?o%$-ZWZNCa*n>xv`@_2YX<vDJTKFyw=MX>l|B(dQhgWc|WycYBX4dQn~ zcjsiV9jyolw=-yGZBA?63?>hr*O0Tj!>GyS6_|SLVA<fgBcb9#C3Ae;CFa<~epF&t zIDbMag`Zlv8efiA82@QpFI#q_lFy%f4U5jHie6vYkQwXEpE$aQe^D_``0GGC9PDYt zrm4N8^X+$dXEu#^7Vje4Zby(%)&v4;W3Ve=2UNFkba-qQj2!l!$T}1fpOZ*csuD?~ z@^;FM`otv%jHg%HK5-5HIxt|)GjhF87e=;@wwr1<ik(Mp;;rMMY~o!5VZ{R};pvkO zY~c!BHdnA4wLB-V6K<Y{tFj4jwDSwhGTDZKrycOp@?=m>I11|dlB|lC3p%#yVGe5p ztBtoZ^GsjBl9rngFTIyE#crf$4JGkjz)1Y!Vo!_nSJ0(9LrIe9M7r(JOmf~%T{Ka? z;>^1Ph`ZhZ&^NVb&t1%6jk0`@^v<i;EtV;)<Yd@U1)*%ePD56qzb$`FFA<vJ3t4gJ zGXC^bGYpDY%8t%XLK&0CDE59i1pYclS3J$ZE#U)Ll}}YfBKi!D+qIC@8~O!Tem+T> z9*l+jam8fYq%L|?WiE+4vVb^km8N%3DA5at3+M&3!b0Bys(V@iN(36bPS0|F@T_7! z>THg1(hmuKUP=jAaK~vz+$(IX{>onoK$84Pjt!e3&$n_p&~{vw&z(AsZF?~n9NHQ7 z;ky-l;=Svzwy_oNnOwkCdo*EEr!>EMqyc0OPo(LIgfuyhBb!%CL*Pp%;$10DWn0F< zoc9h~g@up`gErBF6HP$u&}aI=GKR%Q1->Yt1Jj*8z_lY<!l^mO*%04oHrdJ;PtwP5 zqty)aa2u1m=K=6axv;QC5~R;%Q3<1IFy?j_RGqTGi&2N*T=I5sl2@nKb0eWnqL(b0 zqeml*;%MRXW-hV#G2Onpj7!KlD2PdJqTkBuI4)2Rl27H4jjs%7)Y>VGv7CU5Pq;?M z_6G71E&Jf1wrT~QiDzq%J)k2F^ntfd4yM!>fz?ZKk|1XS35vh)(HSSOHSXbDL(b9; zb4h44-U$0^cc9zY;V{g54}4bbZ-2`9Fua<7pnS;QRN5QUACBBeCL8bR!e`F`czb9V zvHSRz@RJpwL*XVpJ-3N)s;{{-Q#jiD!vyV|EP2=MV#0`NTlnDRgDdul4d!1Tkm7ZV z9B_g{Fx$Rj0O0y%c&<W+H*h>hjmH}Bt5f#CGZ`s9@jxCoeEojD{@E$`*?txeHBaLg z?l5BS9`m8QU*^NhdV790Q%a`T<dLagJ?Yh3yO=&7CwjfFn)^0Rg9fzEBl6`B==d^y zy29-<i8^$O*(ljTlh-E@xmn4q(D4BuF)XB_?v)Okr@9-C?kgf)Bj&>4EOWukt*R&y zsm^Z*d0MV}&;rFR>PW?TZLUeHoXp&+kM-Nvl1WsbdCeXt4|?Ru^`Z)r0M1x)`x<Q_ zS82PUH}zn}ATy<&gtd3mq4^>%RQ6MQ32XSC)h8I(_=!$@@{%@h$+zD-Uy&5G58*Sc zu%hqPLe^-X7eBAD4xcpcE7$2RBobcpaC++~R$+YxlDP?Z@a-V-KDd}Uf4G=d@*QMg z(p1h+;si36q)3phC9Y77LR+Qpv^P%&R^7`Yr|0WnorMxM+r1*Cxm~3FiVd2tT}^YN zJs7q!lv>`Cgh%=+R9R3*CC1#tg&U*T84LG-`jwj%OD5Oyo%7E^=&xFA*rLtW2TjJ@ ztZlHTpB8Vkb}DWe_J-TNWh_x#{gn6^x?x1#7yLFT8$7RBGX3NR@=cXPpyS|EEI6A) zgzXa8-55@K&1CR?+X`}h>{I$S>jt^xd6+Ee6+@3;1A2PAI2L{v=e9+xB)4~cD{ma~ z3e6f;vlSJGcqyqZ!mNaubVTq*cB5A_rVnTVTy!7D2{h<{`SEaiLMzngtOc+ANw7V| z0d*H<)6s`h@aO9)uwHT)+Fc`w>rX>+%jqfi#_SPo`OGtRA7n_gNe0d7)1n=7)X8hx zY^qjtpPOy)o*T5Yo2*Ogz8ErOA_%f?lcx78iHh4Ad~Fzt{pFqz@jgzN@THGGvt=d? zUJ-y7Qf6XT%_XiY{Vn_&yn+O$DS>>}Kp^XPbAI8|$q2UqSaV5-(|Df5Rc7skr*U`4 zqq9ml|A`LNj$K2-{d<^jeafZ2XF+PG3&=Eh(7W%VX~>F@@->~oFw;vw-nuQI+2aR6 zD(495MGNWmdTWw<!HW-)*vr4w_uwygC-JK?a$sPO1bb5I2I^Zq!p)KvV39hVTWTuF z=0r2FTi*tjy)?((vwNZNtuIM>H-)l=zeq5b2&+E)pudWwL3vyl<wOod=u0RUzvf8C zpWZ{dq${X+Qy;B9XhOD?e<a3c9rWTR7cRra5^Uyt<7z(2(4xy`jIXaVx>q>z;U}IE z+mdK@)*27~gZC$@FR_N|MpojSd;uIT(Z^rmBiIRHJBgMCL(ED{(QN4=I5Z`e98`KB zP~M<}0h1z#&!#x?%Wx#zzhyxiqjQ<LTH?6Mpp&>{DpNfbXH*t9$L6X2P^Er|)*0@h zK?4MM-7t!_-P7m9zh{xqtTnuQVJf@kmn_?F+_Z`#J6hPWfiJP)rx9;u(h9trF06Yh z!T-|JfH}(Hs5NsQ=Jrj%S2nvq?1&^E5gvtBbC)xES^LmbeI&luc*nhIxqv2quSk9U zX1u{UL)+PB)LilyJ!~?A#G0Bz;bcwpXfPwbi~Iy~nlDImgcZIo`@y{Y97FDQTHzYy zhuqRJ6*zg$GM?jG=;EEHd82Qw_)aaJ2vm3Dik4JNxM_(mG*<GXw$Gr##sl>1x=fm* zMd<v;a(vm^oA4@V94y)0OT8**!;|@oh*e1rew26S627iwCN4u}%;6{^rJTfhr&!}N z-8E$6wq4AP9SK}v@kcUSp&zC-Q%=eOn3{q!;5|Z~zi>w%up*ORx>xibOt4|!4Lt$k z&kv(dtv1+ngrJm3HBQ~9&R<X&$y<fZhJq>9<Yc4@UXWZu-!~}Ii)20TEMVBkp(_08 zqkF;0yaqMj=&{di^f1ovELq$riDkiZ_|;D2RJ|5M?&KlT6L6XKhP@#h?h3$l_&yTX z+d_7Ywc=;aIKZwyzMR;}ud4`+vacvF_{Fy0t;P*Q)Y!4>D2b`g!a<uo`2KRL?CG~5 zc*Rkg=RSX?!!{RFGneK3<Qkroc;|8J?4F{<y&Ta3Knm0Jppa~L72~UV@|bit0WCBr zVIJhwa61O5&?grZ&}VcKmC<=7klBzxZz#tw>xMMa)0<^5w0JF~d^E?lg*(}lkEz1d zCpFpmaE`AW83hgQX&67VndupSo%&=R!u7SMV9T-ZBv{@J%N0(LHq3^XYrm3$CB7JA zvz{ml-_gg{TBzD)OE@Pc4vVBqX&2SQdKCqlT3t#ylt*K#`$Kxo@Hlh2w~(53sN%|J zW9gT>+2p}-dl)}l91V7?Mg8H{{MKw)c5~N7zE6#^OHA(&lN@QX)6W=#dW-4($zve4 zF_M#9F(0-aNg%S?$H<e?YqaCu4p{IY8rshe#T{-L5E!+WI7zgWxBO}$33jr~tnXH2 z;&o{}aAq*bK2|BecySu8{B)0mZBT<x4yJI(S6QGIf0hecVpHy;c!f#0c8!|O*W<12 z=J2O(ltE`&Ft78dnZJ{k$@-6<%YRATkH%N`Q*-H=I62>*sIOM$i`%#2FeP`~(^JT- z>?aKazDv+(nUf^&+;%!rZeMw+$s%$+`x#NmOGMM2c`#n=2Qy}I0ryQX4z6sIguOe2 zC^POfeY(IFnnz5;H~C9Q-R@d4<xME_QLT)Ibh2!OtP`Krt-_D3-O73gy=284Kk;K` z+q37-=VNA412j4B=k2;)LSoQP*7-#=ggE;{RA&>#V_HB*K7_5~M$&=lmmx_^j&-~f zkIQZ!0v|&saPj*_<hQ>j3gi)eJ82f+qf6x4`7LDctpI2aS`AamdZ~0sG99qQi(dCG zrt3L1dhMw=uPio?m)f(J)#<#%I>l-W-_`G9XFOPkO3W?XFnTz=`EU_-7-_)$sH1el zXier_dMa-7vnPAax{3RabIiQMaX5_|LOu+7Nq#I|Oz+esz}_R;IHG<y)T$_AX^9g3 zaBDsHX~tpVU|q{RXpI&O_HiREPW@r*w6b#RjN9bdyecwZP{J{;&it*byJ@{Jhu^U~ zn;n>_U(vAIgsu4^&09DgCfj12$N>{W_U)Du%u(#YUq_Tt;#U%WJfR1JOHPsQ)8*Vp zZGB?f(~m~?*Fmv)3Lr3=$t>;4#-K@B@bIM={bnYQWAD_H=H@sOZS#V;o?wqdUR|O` zno5}n=N96q(n{|zr>XmOA+Zj2<WFb@QQ4up`J7~TeD^I7Hpdph+1O6B+cyHcZz*7S z?{F%+Qwp-a*DxdO(i!z7?-*Ejhi+-tVy3^EfrE$olY^5N(raZ$NxXOvN>8ug?sf%} z>3zjis%Ip0dF|txgD(mu3Jk#Y%rB~~z`%0>A^S$~REOC?k7--L=ar_Mo&5+%p}M?D zvI$+?tIekA`c@>GZegz!US?;0ipMQ0%sHdMvqf6U6T+?!L;se=ynp3YhD=VvU%PFg zE&CYxX)%^G+6wUF8!4<43Q6Fw{g|CUoGZDKL-I;b(Ce$t5UGvbG*!nKO`k`Qtfk?! z@|_%~qV$Abnji^N?oA?Z_PdiSGOy?q&K-K$F&Mk{036mGLkCFC76xt=XL<KzcIA7a z{fqId$c^H`_)cyqTsl=lHz_2+zyL#>r#u8Y9p%vR#XWB4m3p#HQxP|>G)BL8h7QnM z2mz@BQDtx%tqYDL)!PSvW6mA2e&ILzUP1}p?$!mVRrAm(vy9x`dq<F{(oBy1)W%WA zJxPXdB~gCF(R7U%zBT_Kd#rPU@XKx+;RTDI&{2IF9h|lKYPq4Tyi6LFIW>UA0ckd2 zf)nYqOu*VK7tYIbAw-^%VxzX*#;GoG+&s?>5bhC<`-bO3faG9gMB{6I{#Od?&1kp& zF?wBQ71_iz(r&*6=sWuh7uZ`v2c#Q7-gTB7+^vbiI0CLE3)nXnJNflZHvF2CuTZS^ z5nH^Yj=gQW2yR5ip=<9FzRq_iJ7uI895nS~Td#eG-JWGYYvbwelAq85KZ(QkeCFt| zJMcN+IK0i@jSV(=z&df5EN#RZr;5o85v`c~Y1+2MUsB~=Hf`CUz~6r|9{11O4}#QB zbm!!u?75U?`rOBZ**$GMd*Q@>QscIb3g-96mv209Ur{?X?z6$^7j(&sl1zef{b+C7 z6<Cs;3GXz*$go$-$wSZG#P{75lGiOq-|JkX-jQ9L$HzJnn50aV$H!CgLk?)%tS(C4 z{0-ipF4O#NZL324!0`ce)QGKRq0jG;fy3oROA`&u%V9_HM9&_08+jViM{mJQ>l~Om z{}@TOcmZGawBh)y$7DwT0l4F3G_+<nl6$^;aC-e#F7mw`*1BtgrnD4zoSBHXTJy+D z`KchMog;8P9|PWfd7$rX!~Q%ciuDX>h3Z><AUQn^f4u!jH8pR;q1grG&gm<V$H}uT z+Mmd!eU)&&%ovtQ#PYu`Skt;S+7R=+5_MOuW1KQA*l)Zv)|sc_Cffj<Fk}Jsz@y+U zb{wXhiKOQb&O@D@;Y2Xt5J*fO!)NT>gC@?~;Niwe$j9fSg;)j3zfECZOnithBN8ES zu^alh_hSz|??%D)21Y$}cj?M$Eu8IsQ?9nKm!yvO1C7G*knLtlpEPGd!<|g>(8msQ zWT%nLQMTBst^pYnuX8Vxd+F7>m&`!Dc<%L2Rn*Cgz`5iWqjyOHkFaW>CcMQwuj!91 zF{eo3fv*_%X1Lw_DK2o~!6{1Be8H-9>BW=+Rj~8gDX2MLhTn$XB0m06VDPz*{Lnj& z2i$UTvZE@#(dlKHM->v?CK>n|77y{>EWYfJVRhX5!?vU0sPnj&+!|~^Z`^9dHIL3S zE~C3>ifTL>S+$a~gfL87-j5BsW6du?8SuY+4s_dH;rgd)Q7mr(D(y<35-A2in$K}f z?=oTRI%7PdF3&HM9YynM6Uq%Q|D>DDlvtk?src&Kbo906xdRVwfy}2|9Bprl@@r1u znAx$Qe5Mw%4408~lLusf=yKLK{3v8AY=@_t%~4#^58Di?$!d;4<84te(RVnVAc~n5 zIKL$=dYw2|rkDsU3So@NMm*!Nib&?RlKp-$_;ANG=o=PAQ|`*)>4(zrqiz!3+E__q zza9W%!<+DKi5gQlypYqstVFcO>Y}}v7s)_RoHb`Olvmw^FJE<V!;7!5E9E(3lGy|B zE`Zv0>0(@kEnPfGmP|S&O^w=*plw+Or7yi`-k?MhTkQxM`~vLPBgyvJ)G!qo4@WKq zK#5c;RP~iWxYh`GxalZV-967-bPJ*BbqO@;yc-BEeFVFk7Vy<eogF;JnwqIblcX8p z_H^Vz-Z?#o^gmDp5;D?!+I9t;XfU6AzjTr-N*n}F)yMP0GVYPZ2X8<^X94<zM1jeK z1hB17V>X8zht$xS`0MFrFkEiTP8lW1_t^-rSD?iozHbcfnR!@${4N$x1DbE`0|zbL z=-~3%Fn#kOu!&z!(nBQh`0`;$7RaLgrd5oG^-*vdyacA-y#`ee^l{Iw7#t~=N3Wb1 zj}l8#aEa+ENcx})hjhYex0N!~b!OpQ-DAvvJVgv&Fdw$Q9L<zeUZlI*;)srNF4k<g zOd67wLdV$eT(L|coFAA;Oczgr0}n00Xx3-?wXBOAm?Oc;OKpb<Cb_squK?9H9HiZj z&&v6`ijaFviB?DqA^c1ktlYm6#zi!fmJSzm4J-n`#x^FE|Aon?f8vF$(}_{wDtwHu zsaJIhlrPPIo2TUXjPEkU|By&B?TDg=-|SI#g&I0f*-z7zrjX^6t?4tGhg-UGY3-5} zIzM4MjvT52x=nLoOL`vpv2-eEBwhqhSvw-^7|e#OMB)(E&dpzV5H?GSgU0y)IMbAd zr{-_vt@Yg5lb36$f%7_SE#HOPFT~Ikbp~&*+lqJYZGz)dOrdw?X_#?-3%q*dNlV>) z$@g3P!6l=EYm+shjrUDpzgQep$($jJtM8Bn%XdRh#Y1L|UJ?!3sg2$G3<mmw$lbPO zo^`b|=kjU=dNI4GoVa{hcIGIYuV4W#Wh0nfifIDJ8PCWZs~j3(GYE5+hH^u`eBtCf z639o_4#7#Cmhx?@uW}DOTbcP@`smwh2~YI0Xy6z}x>V^Qt(g9XT1*&5GxKY?!c)(w z=7K6ZA=ZKDSH0qP?lYkVrXR|tuS{ZWF1;x?^>?PeWEQCin@gX=3e5TH$z7L;p&2{m z(a%Mf{<>ShJsfhi?8Q^7atU)2Qfobw`Y%?8qOisENcLqaxA-<S_|abesB;-c@4ZP* z9v(+Oiro0auWr*%LUs7rT14JV&}NjLxDv5>i>N`vAW_cEL1tF$O1zZtmKt}M5Gj{R zxb<C*^ytM?vO1TjJXM3EDH%*Zhj-Mlb}`NtUqLtE1Lle-7eU~$o$i^sf$(N!+|^p3 zheJOyN)=OZ)yk*T>*8JNopYE8`mDh9Q@=^|Z-tdFttv0y6`@1U+%jS|?G9$<D&&*y zf<j`xOOcMwzQ`>5_K=45>mtmmNZR+AMdiW6=!0vM$@6d5n41GX($KPO`<HJuN#tBf zd@0mstjZ_AB>x~9Rv*bZOh~6;qsM{bRgR_{SHhpa7^XG$IB~mPB(RV6A?7s?=rO-q z(l|HLE-pw1NAy?<bX_F4uqAT1ca|bvA#2E>=oWfJ`61)nJQx?;ai@(zhUllGE^u0w zS)OT{&73s#<rX(6qOtNhf#j<u=C!9Z&DuAYiz*$9<8qs92L(Ri)>n;zv+8oBV5kMX zGxaMSPj7Sc&ds2*=To?OpDUU0fhpvwa5Z)I)`5!4D<C^|K5dE{LT6e(A={IOLvq7< z@}%uOnVx8gZ#-+bh}8o@ZO(nNc*kD4@kbQ7_C1sYEY`tqPjn&R(o0fgsf&A6uh4Bd zZ>YhMMP!$2m_4KC%Vj`++LM*bT)d$zSYuF3GqS>{ti`EvE44(@-M*28WDWt&{}l}j zdO$;LB1i`ZF!RYWdN$F5l)P2pKHa*=T?{@&y`@w^+PQ_=%y>nHdP~5Q(33=yYvu}W zMbL3|BXF(R7IIgYr7u?!dVR1wiMuzE&QVfjvOjUmDBI1jdDa`sKhmJ_?+?+H9qPoT z@)Ly#4YcKX6^Z!TNN*?}WiB}AQ-e_p$)pVh+{IHi+>z3!^m9}qm5_|!rY<}~b)Ty< z-_JI3KX$Aj3x>(icd6@0?usxPls$@GFFHYNrBdj~ww&_d-JW#XdL`PhITO3Mcw(R_ zNgf$Tfcv5;+<R?JA|EJA$dYh!@E2ulo06G2w|Ue#$r^7SAIR)U9t6jaj->jF(z(c? zqFCSK7v$`79eh8lhYq|dhL@wG$@zzS=!lac#NwM0{IV|Ob`&Yn(5wAO?z09ia9bj? zW0@$^<jY2;F2a#2;Crg8T1V>Jg%Fo0L6g!Sk}wxHm{8utsHnc@c3~wq@zgHP#Wst? zexHpCY6_S^9g;93WHLPw9myC@)<T=_74%l^Uea&QYr)R1+T><N6@9XB5V0{hD!6sM zgjsy}t>D#!%bfOy^UR56J$g3Qn5=Xf1R;wjQ^}Im<k-w4TC(>7)jrgpG1{(4*7ZL{ zlKak+-coaBzw>)R^zGAJwXH9;%IQ}|bu4I>WB`qG&!qf4%3V0m3$$}HxLp=1u%U4b zB&()UzYzneNBU&0@30c*e=LGH$}gsg?MF%XR5eVly+zlC%2D-*e7gC|D=xy&3nolz z=ccyR(4jMfac$gv`tY$56`rdh+@4lypFwbN?g3)`BZHgwI1ksSPbHDd`ho4Qa{7MG zTQWgw3;7bNg2uc4lA`<PZouym)Wt_4HxlDtJ27{k9=g75r>{@$C3jyfqjAAQ$^#>8 zkxnJJ`jaseSF(_73!jX`YLn=I*fHF*d+uC(%M0$l$!8`edrSG~ci#o}wojO~JCf)L zaT8*3)zp5|i8SV4L}mZ@^6`(aUqw+_%QS0NN?1z2uir%4!H`|_?HPAufDF|Mk;Qns zM<is^X#VQSrHqRHYo_w~D1OM^N4Rxf2%Fk13nqPj(6CU9mtAp1upzGylO89sB~dkK zk!OvWIV!yN`8Iy1LJB`Leh4qjlj7Ul4ft`J>tVX=Xed1M7zf|E1REp{GBuIu{93QO zxNg)aB2}`Rt-Y*HGoFnlQ_pQ-_ilKO6MN-IP@4>vWnRWasn^IK?<BGp3OK(5?{H9k z4D(ohK!w;QQJO}Os&IwIS+-m6DAT~m(9*?g_|0qk@h7XySc9e>$g^J|{It<n==OR5 zY0}ijpe4$zw#QybtV-m#3{yN15QJ$Bi7+WxRcLi&36wn!geNm)*bl4r(<h?WVAsjf ztSnW+V?lxNWg<^!2Ct$G1CEdtV_6VC)mvUPXfr6R+zLKbFVQ|Ij@_qtnKv7x&!4iL zNUpnBvC0w?dF@mKQLJYaf3|ZJXx2F3Os8sE7VpZx@yvzk#~1S6f<gR}>|M+&+aL7A zj8Uxrg4N(OL=xViEIYi-g#W2G70phrpray10S&zxRGc6OQ3rR?;U1c()EYttn{L3N zCv!-*qZl4CP{N!927Gm9l2ryLh%oLYeo@Hf$?gfP>CiLm`*?ePv1<kD94~`0#mnG* zd?aliTY}F$eL?P+E4O7rEPQa+MUnH0+b0Kd1s*ps+&vuqUFQ++QdxFeSAVv7el$#u z&16*RB69g+C{&NNA!7?iVnRf`Aer+jf9#%89)9l$bKFyi^sp6nD`kV%$3rmVs1ZNq z(`2E?s(2ys|IFUG;|sT(zruweF}_4;JbTl|8Yaz8;d|Q8(8=GWaH^Omri)S_N)(%k zc(w@<_7CU#o~f}N##1>u;SN<!$wN0?f;CrKiSC};<o?w`O#FmPWcPC=433{jQp$#c znAAhAYUM?;yYCB;iA^B)KZ(Q4LASv)-IWixtizI557{wiw(`%fI`iyfJC>`9;ZIE4 z!=@`pvBJ6%wr{AMaLDH2Y)JGw$f64%ePADvbQ_7I<LZb(V<2oyXoaIM8lZSJ$66+E zFrZ@(CA=Yov2*Ylo*@-dk>Gi09F=ME!Dx^Bg5Z<4sZ+5gt$kEZVaQ}+XJ(Do)upU= zpD%j#j^+1#GGc|@@7P%J1|~_elKoM?h<}|tlebCvfMon3cKC5sDBkh_dLFey^r3W| z8K}+vm>|aqW$VffPwm4ZIUT%XzZ?gwo644jyRio!+pr-g8q1WwZzDfP_lM-2t}tjr zJvlWki5hM{%6*YrM~9YJU`xMdy6-VZx8$#(1+{WCvF#0fn*eM-iJ8JLZ5#PPGRIjr z<5xJ^ZZMm$C>BEoEn^3toC<3WIk861BQP)PEe?x51~01vp~^9byrMP4q-GTBmwb&_ z%?&3D%BJv*jT)@4zdbAzi(noJPSA?(8o}O-Pn^ZnnY88c2~MeP4Q1l;$TQqUEt#i+ zW;Tjm+PRl0TD^qsE<J|V#|hbx_kI<X=CZ<9DRHnu+YFW+J5Q%>x(yGjd~j%hJbOS* zojA1Xl7_M$1SXoGSL-smVcUGT<9dspZw<z4GxMO>b0`_N;5f95xlYY%*V3rZt9Y@_ zEc(St7ZrBJ^0iyjX@K2B`#nw9NJemfkUkv;-*eZJVJl<l&Z(E+UgSV_O7}^;UgeA* zPc0C>-qXO=3q&(}N)wby`H<u<@@V`u6PLca!ZP`x?BWeJ7%nfx8#LQuKt&4N)fmBs zZywApZ73I|Q0KB^v^qeqb|{Wc>O%4E8l)wLXrOtI6q;*;RG2;ux5=iB5iVS*PB9ab z*v37VdqCpu+ChQG1X22|962l<2SqOOY|+YF?D*SLSa;___QDHI;a%2+4XL$fpJi@h z4NI28vB3p6%Q%<6ZTOU$9bQX9G!?*diWpyauRq_P*@(Nfy71fU5GKo_kkrr8pbzh* zBFeeYY#mGdSuvB0acdH-IgY~_o_le`{9GzM`YiEY9g64f%VBzzE<IK%M@ERXa{E%{ zdB^9P?5(6E_}TY>ZCM2TzWRKa5YR^Zsl8!k2J5nJ;|{`J>oUl17qG=DCeZXDlN1+L z;gLzJcsDK-_)d<>DSGlXv4_cmA5*v|x%QZ(djwps?}WWu&oe*lw+Mz_tKyDq$)gMJ zKP8prQFQK>QCR!ShK%Ys!HjG@K$afX!H*s*VOfp}yZwj_TUk<9R`UKGxEr6rW}g^# zM~DVHeflPLka{dQ9o)$qDn_zT)+UJNf~kCGg9|==_>;<vvBhum$Fu4!W5MIGA^)+n z5JKI@f%-*tOmzyyjq@^z^OzE{ZSNpVy*!2vtQtskqq<3N^DkOFQX5Ys)Km4jugEc( zH;n3=>vTV8z>{Ta{EV~e{Ogs&_;7<w6}wJcV(TZx3XR7E;Ees@{H(hfu<4yD>-c&( zJElB|?NmHU-IMNv+y@1CI`k;4TR8+wMW0rDC-<Ow_-pcOx+U4Xa5#Hdrx+&X4o2g> zKqSpXGI_ugqI`4@jA<N4dxkxrMGMZ++3^z?`{ldoJoRc)(0YybYp!6D7UjUHWf8FL zqZO|pJwRA4(}?A_gZa!yvFy4Nr|`yiIexAnlFd3Zg)Q;x!bWcicG;RQKx4j>xuTW! zugQl&Z1gl3vO$KI9#IN@8@#z+(O>bog&LgNpN5*7t4Z9fDKvWWFs9Z03dh|HAgN4$ zFyA>0mI;^9jQwFKOb(@0)q1ddzYpH;If++Ksj!ch=>ch5ToF<2RiSoixUlMM8-6bz zz&gJ3rL(wP_QJ21e8Z736=|`%_$!Ns@?|g7`N?W_e7oQtw`=wn%rJN4XS=6y=jT4- z(j3<D`Etv7&t+P8!?guVlIywi>4q5G@*Y*@AAx53!8oI?g4=Sh7<5h!=NE_i+m})e z?)Xw?zGY%1$j#MfZPzKm&JL*xO^pjU+~5MK3shjP_xQ41ty3Y-h=;{nH?zktOyNPf zko3LmA;VWEqLG$8U$uWc%V)e1EpM6dX^kV;{4!toa$q67qxw=5{naJ&9=c+1=}+2r zyNV`i8DrZIeJH#gNzVw@GCz#0!0Fm^ELAQbpQ}YF{@EjW?mSYdxPusd;tMW5c~JQE zxg6j3R)>^nca=+w6XQqR^=HEd3}L*lNuZi}40~hQGJLa92#3xW(8(`0fnj$lF%OL* zor9d%AEjcn;G_moY#vUw4$Ywp7hBWREry^tU>&!APa5frssIbYF1*yI20s?2QuVQW zang|Ebn9`RI9-gUdv-i$vI>()+sj4lpd-%&XH6s75k=w{)IWy5TQh-L)`YMV<ZanE z=}XbH_XyiCXF8u<X2csF&WD8F8+4LYJ&wQT3Jcu(QR5eRyu!RJSj-HDc>(KKFOe&< zGRl%ftCtdQG$6q@Qs~toyUJ$QZYGMNXl_~ECnh~{C4IQz4Wsext0-+fj(Dv9L5x=W zv5Gr*-f*TQ=VIi?nkcPe(N<CTBW(|B3I+VvgA)bAl5c>^qC_@j!7w2awy_D{T4B(~ zYmm7{iG6Msk8>|B0`sYn;E*Z>>kryPvwI<1OB_M`c7G5{tHR9{!StI-E-f?cMz!F4 z=4sj$>hxwiBVEUEiu<pD`NTdhqVWeeYvWyl$!YLe`XwX>`l5_OyfC`60+dbE__YdQ zAYN(Bddt+K*DYySx=fY7bpIn3O3Uy!o^{cUa)-#?&oyu)cN!+uL}9d85*W`grI**Y z;OMY<{K}5y_$tkeDQfsY6*8`ox<v|{x3wIlnz3Y4cYg@bRl{ffJ-B<4+GIo4aq9d_ z8>W{=k=g2(*dFIOxc{s<`(xJ5ili@9e1XzwS}Q!vn)kh>`H$q;Y}IU{qBe&eFs*~x zF)SOjw@zo@i=v|a_bB4|L|q(uS%GX?{E*lQ7ZR`R@#NAmCA!AbnH<0C%GlQ@VI%7V z+$<ysEjjqhbSPH#&V}JxbzJlbOGYqu1pjEC3-P#llrFN6<m+QhurYNtTUe>j`p<P? zv*ySO=Ukf14jSAHb4xAR7L5w%-)Toj+)gAH=iK5ejs=2s@G?4Zvn=XAT8no#M}lI% z1(fqF<Ky~L!F6&Mb3S7Y3~)P4r2G%z>r^#<K=TEvzUV5GzGn#EzrhHzbryhEpc-A< zn@a|-Dg|H53=%3eo-==13q2D;`7g&!*{2_p>4e-=K6~zTsMK`lm45BwU&R}->jy={ zD<4O6IT+2yhJ4_?vN!Pa_Df(JV~CEIEim}$Uijes4nIlV#lu4;pwa0hdQ^S{|1ur1 zY|bi-%Id~N7oB0?qgc9a*9Tl#K}boO2dJq{r0*9^rwUI7^66JIxjAEIfyr5UJ~Ts& zkIZ)G7p)%0o{Mm;`11Z2EKb&8PZd7E^;4zzmWOuq$?yhJ7oUbY%QvuD+H!2tb!Gn9 zDsgNVtYkw%CNlo6XTVS}8m=ZEL)PXgEqYr^nHT|E=e7V|#8g1_CTWZ}Im)?qAsA?# zp>tlWC(7n?xaMvba%Ite$cPvN-#8s2oES!yPF@IUhM|1%^af_d$wufm%S$Le?+tIU zz77h)-}Bzb)YzeI&zPAPGS~ytzq3tK%;BukIXZ{$gC*}LV)?>IkPucdt`GYOOBZi} zXY-E2j^p|Ci&;3dT%S&q#x|fzKpeT3FY+lRzG0uFAyXq}k9F5S5!UD#-LrZ!XyxZJ z2VC>$_cK1wR~ODddr^RwTPgag_*b}1W-2m9wbPm(-n^HqF*FW3k71X>aCOFWJ~#L= zG3go3o=^V3Fpdfky!s9A@u7g9;eHM-eN*S_n?32^+C8Wf=4n6gB1I*YnaKKugWDl3 zVs}&rawH^Z|1pQT($iNMZ=W!xVqzsHwe=c}oYcW2#~kM#-7*2~h=(Nd*DY`;U&4xe zRSOHvbBXP<54hbViH<IA<Su31gy?`2T+!VFA4Tt>8=tbu+n7XnL1yFL01w=u+5p1P z!RY0xh7<Qw*nj3ClT{K!oj-<QN2>`Mho*x@WeLr%@~1xz7J_q-D?0etLHpyYRC$;T z=H?$q(~4)*W7<b@*P{wG9TBB3rLmT`9Ka(zts>jp0Zi7m;)Py)o|_fH){(DV?p14O zldQ+eW1akyVYPf^doEXgE`^3V`EX~``+;D32Hup@<1-tQxb;cz=)!Y3yyl!F47&XY zA{0x={gwUcA~u4)o^zYP!ga{ax<pP-e^Y*!pN4@8+c0B^RM~6ad$e8UIXaHKk4cYa z^7=n2;OaSna7p+XFn@Xn4&Jzi4F@v$L0{C_^82D|=0C-)QLovPVJ67?kH8F{<K(Kp z8#}2iQIK_hG~z5Va-88X+dCLq42JPL_H;tTXEpfKsO;a@L4NzEMJ}6`Iwljl+Xt}O zp;|Df`Ua^jnnAk4&ymf_G2E_}V)DpK6Jm@_QQ+eXf%3=5RWT<RpBr12BEJDoylJKJ zOA_JWAw{qmbewL>ct-DB455Sf+$Fl^=V-AUL+012z=Y}^dS}TM`qfbfr*3VciE7g1 zUqog9fcFnk*=-|k+e-+`FS?ia7Zmqp+g}}@Q<`C+!OirS78sw{TAuSjoGZ}2W|y{3 zvV210e6q^OfYUiN+rGW!QhDd85%x2)zS&2=m~NluGeFQOdrGi$S!J1I{Stxc7el*2 znok6{rpJD${~5uVbFF0^E{THR#bX3diZ7IRZp#$tAJ|%6uHR99ahp_`*V@psgY>rG z+N%p?)*I&7FZ#NeG1h-mKEzj5Fv8>vO;}i1?#_QK-+EX^VD8e73$gqn*cYc;o-nOY zuxnIDd5+=Tvb?ni%PkM{_U|_f1b*Sl<ujMXm;2opV;)`^Uq0nRcDe76UP0>uNjiID zGTAYTkY&{~s3O-*y;Uk{a9~r}^LSUfpwk2weT^z#D-|jbKThGt>g&u~)k`8b$%K~e zh$UAirxV+`#q`aUaM<*76Y(${2m8H8Q^_ZWuzOM!9WMQvDf1Wz&Y_0{>t=ZiW}R8f zJTW-T>@W);d+Lj+a_mHA(G6uj)tH0%-7a{nUIM)?THzf_Ig*dJ(eLdwjN2QA$paF2 zKi!qEw#}G-l+lG2QX5zwk4AL;_?|r53n*7L0*tjMW7zjc<tiuYK*g;dS}Yd`_AcvX ztVHg!wwELXg^S~opt-o=O)Z}LUJ4okvtWzgb{z5G5PBYRCb3t~@Y7}1!Tg``@L<k; zDhe5+)8k>Hq>Tbp^>&66>GP;t_HcHYx;XzvA|3Wfi1S6`f1%a6LhvY<&EJ)nfleo# z*{WF|L0LbE^DLCZ;O|4(MbjREvtkSzl{1FbGneN-38dMFS?S<;L<@?x`Lc|AE@n%g zgvIN^*b})Iu`2c|1`eMDtJmCuJExjqOt2ja-^IYb>*+Ab$_%X>^jXg!JvOf2M7#u# zv7?<Oi(C&2?hG5qdMQ04-wI;!{=@0`LgzFl>#ZlT{<&B;r;qj>-M~*vz5~f8I?1_? z(I9;959|Ci1Pe~T#gzg_x~)|PJ9N_dEjz}uT^{}UZ_X0vg1gw5b(itAqbo{?hcoB- zG>BBmMmMc>@J>mIoqHh%9$0Flt^8Z2Y~M4OFi<4d%vTb-%`b4{G-bZtA)ELH9D&(8 zUxHS9A&URnO%MNHb)9KAR#Dit4VkCRLr4ly2^pWg*V=|?P*MsdV<kf=6-ANEGLK~n zjVPgkJkMThQzT79Qle2ZMuVi3@A;<VeZTK`-yi$W`mv6+f9!p%`#!Jhx)V0zP2x|% zbNL)-q`Vx)em8^%ip|Imt1YPQy;*SO<Be3e|4%3<xq)#g`3`v&>0(j7C5+zDIe47d z%)H%|jwhZNQyn8kQ1X2UGG3H{RjzKQSJ_KY#qK(cUiepJXez}hS}ewUKO}*VH-BLD z2Z?Z*swm#%HH+8h8ZeAN6_$E&9T)4bg&uEw5pVJy-1+hWH2lC%efc*CdEJGWnd4T> za?L2**CoLqm)BhJ;Z~;5pc7ZTS7D|zk|{IkVO&}%Or__wWB0`}tmjVwEBTErQCZl; zI(~B{ac*Qk%P8QJ1<%PwR%~G+=Z;Vr>wupe$C7=HRBkdPHpD-+h;46a=t{q15$yGs zt#)!VJAYvkN9xXE;y(3=YdqOPTz{s-l9d%?#mKm`p^Y}X`T#FyAW@Kb_qmvLX3sbF zY0gs4o4u~Y(_t}kWKN!lTfKxG^7;~I$mcKX*4RSM8jl_Hm6nr)+&cwMXF~#*Ki*D= z&Zw}m-}SM7#=RyuhpRam*HSEIyxQ4w$wh?Qt0P4J-ykB~+n8lCxt?PjBf#qA$tSeZ z5$E5qD=|NDmR0$jkDgt<6!8g+l1ZDn|AS8{DEv|#TB?$Y_#Q!UI(QG-#507@^~-FB zt)3w3#~ZlW+@5^!RRSckJZR~Ii6B%$0d&otA%0DN2SN{$$wXEtIP1fMuDC4*AW{~o z$7_Q%p)bI@xLW#%*J0YzWjTj;^$2lbA`gcRPT_58cQ}`$YQYb$U*O-(bqFsxOp-0G zh-XDMxOKK1ylyfFRW|3*-^qU9bGeDO4<*TaDg)p-U5v8sCxJ(CPl2-1E$pAN7W;~M z&^`4mvW)9A`&AZ;9KwZ(E9XssLhlBU=bQ**mn6Y-8$PDT*BfRBZ-%Sxg@6v(2WVS# zD(0<Arq5h?LH@K?g4V+_Fi30+B&1TT3}ZeZO`Jr=*B*mcCm}G=D1om}+8}-)j+Fv+ zP<p=tT&LcP8;5z3>d9uLzvL(49r6N;H*G}6LM~vJ{7TR)_!7+;zk(tD94uBTkNcH> z&=1(sG>h*QJbP0Y+hvyFr6ck<JCu8laDJ>3VhlfR+W_}Is3sR2sz<gbC|H=F$jF9O zpx-V>Kwa!^d~TCDC{GlI*`dejQDJ4qbZIkuW8Q`o<GWF`1|M~JbR6|BT*Cc+KM<m} zo}?$MpOorXM<17OLO0iTp;rwm#L%Ks=B?{Ip@FO&DYE1yy-rsSW`}39sv;kNrZW;q zrD!*ZFf65?wcVk4GG*!B4hn3t3nCslatYzp6#Cx5a{5WrGQ!$wkkb-8#V*tm#|PY= zlkt=_M<&Y(t-7a=9K#lqX8i@A883i!J80T)u`qgbS03K_unI2uw~dZ6IE{qlg3-p+ zW$0#w7@lYp!94AY=&zbPkk;oU(z`hmY|!l@6^t9n`PGks=L%yaBxpmYur%m@?CqQl z_$0aNvplRH)j}OhvcVxQB~bM*99;Ql1&+Sb<CcE8_^O>cR61AyL{qrQ6o8lExEGN| z7at<ym_9IXj}6qBT?)<e^kKMyBCNRd7NqFxMB7uA;}pRw$WtVrj(hc(R7}eRy`$eb zdCMh1?xIqJkFP>0M~$)ny-sp&RuqTLA0uK*b?8M;qBvHrjwm3ci8cy}2WWFDee>@; zw0SNAKHL&T*Is&rhD2(Ci=`HFd7p*iQ>8$?ivw9&zl$wzsEz6-vykQ+XT+15N!Ax6 z0AGdItje}JqGruz+PJfpJaj9SPN@mSx+MZY(BF;ZIky(=G~?$8mfRu*a^jJmoCTBv zf$T1BLACexY4mK-Zp@B7LWe$Hjf9fAfbZxd@aM`R626H-`^vTH_h+AQ0&>|Pb=d^# ze5oi|;9Cm5TbcqqE{e8h(ws*Tdq6<r9NC!eLfiCdqnP)pq+<9!!o+q37j*@ZVItk+ zt++<;>OKco^HPZYb}Rj5;S#v^!3tRTFA6ZtQRqan0Dd?)jz(te!IOYM+C$wEiUjnr zH8WN5ufl8~Q2T={_&N{ArwGAPK2fkZ?jWjVw;>g-XuB=`1Nqr-AKcM;2VGE>=Il#o z$9Ufp>=MW=@55ujipVFxb-_VgzBm%OJ{YEJ)MF8U#sKhX;o~B!82Bx<2HgDIK_^VF zg4wBCfl4=`@BGe2mOZK1n7)iN)YI{ToN02)cX_&NIuqaeev!4!ubZ6Cu_7OaO@eHL z3b-tf5As;-gkjGMQKEl5w+$hK>^|H?)tl#mM+)0<$0r6O!U-mJ8{ilXPx$BPSL8HM zK<{Gs(MHW;`ttIfptbb?4xL?#5?1^HO6qZRa?vIr<)O?)blKo|!cT0wNDaO(;)5<* zmGE#<G5vn%Ai%9TDAd-L@k26D`^_Ub(CJF*dauAc_0qAAP&DumSOzDUUi3Do5T?ma zqZC13I_Qli>^?hAKK_^uP2So<sc$c!{>E$=zDE(;l#Ak;;SJC}*9G4dGU1w6@_=FN z6%5yIz;pX`p+Lo1&;|0z{Ego5zofGNanJr=Wq?<dmm7G`bzY5h4B*`TlPI)x6P%oE zhS8)doN3X*#p*wa@LOxh7rK66;F~U!>{r5#y19g&+2(=wTm`e`coVAG{*$YwRws%? z-;o2h^TEtNMYK4~AD!Qs0{$B+`+p-;{}YupsP8~8%n+4hdy_Iakcm_r5P13KDQRKQ z1bP&_kdwa=oLBZ2Yphzxctm7_T_}Wc)38C;9kt-YnU$9FVwN|Kt25YC=O?c8Xn~`B z`zWFNRk-Z&G@}?3NnL1if#E=%IcWD2r(+eyDo2>=T2~0)4jg5Uhlo>wp)sUNw-&V{ zB#Y_eQYrPfqTHa8WH^XhfmEOe5qJAH=u?|UW}7ESZ#gsg{b(La@2&@lj;Fv*pat8j z4^fe&MpW;eEjUu!tI^Z67xp#<<3n4%Qhzqxro^=OG7FF7pxVfD)W7Uz=B#-po+*w5 z9JOkWO0E@CWN$)=7x7cODrK1q%IVN+-y+C^xLW-{b@;bM3`7RVz!z<MVSQc-jGOz3 ziq;*b2Yl^GuRX@VHn<Nkt5Z?I4j*`;H;?Tru@Du1@?o;@DT))HhS&c%Z+YjUB&C+V z3eP(N7`+|0sPRlM#yhv1S|}FDWWOk9%KCztAfa<OHQ^H;PTd0ER_(%b0x3|kT$r@^ zz~FulC0ra)z$jMDXEyZ@;i0NuAmy1nV`MMFXs?u^T}DrlVY|#w5&H*eZ!7?tGoyj! zw$D&7kDrpNeMZLzm@~h1>zHTK+RR=3N6d|;v$!`zgOYYT#he&!WX$J^;hBdvQ0TTG zQ+H0(a!w-_wpDh*ruEyg^QCrbxp)h<4}6M6BpK?^zZm?Wp&l<d@*QeymZE~)3vpP> za<ntWlCtdRhxsR@p`L9SDBH{pF6nm0<BAA<>#U_y*k+V&j}2uaJ`LiQnd0?Zb*Tec z?$|OOPzF}T_>yTil6@k9k8AK!QHB|~^Jx<PhaTf8U3cb_$7Uu!Ef#w(Oa)QC*GOY= zW$gK59{zZ#o?Ub#1n5c?z!HrboB`FC_`58e4u^@1AXB{7Z#^=ad4lNUXK3ryzft(@ zzu@}K4`{NZ3sp>U1&*QFpl@9aIg)W4-OBa^TTX9d?iH-F?3^{nF{L_<J)Wnbv0)E% zF}TR^#pE&XY*rvA@+9^6MKio4Yyv+kJwk(pYpAUi-Eix!cl55;8kC66e(WRcg|DX= zLT49OoLUqC+vY2wQ?rM$#=}IcJ3u4h+XduclrpX(xdNE{li=CQT+muG0FE}b5iPnm z!0ALSEOgBIjzRlY{L{tHa>CiMk@TCPj?J&YNzRc3LI1|7?a$F{djJ!ZeS(UJD2Ecm zG5C&5Ct6|vsDW9UE@um2t!N$SK5`0Nj-5gsi`G+S*3<YdiiWAI6ToX(6?j+j1n6;v zLXr{9sMXUFid$}?c^kBF?#s1E_tjZ^iZ=sG8jO+k!yRDCZx9+S31KX|mok1*fy~2g z*QwN!d*G&{6(#&9nE8I*3@<2C#@WewjG1!~iXA3EY)c4|S`>gIS7C7O;0(~7rv+<U zN!nJc2EPxV#T#NzVB4o1F!)#weS2LEI+R+%x|R?@8@(sM*0>Ge;!-j4@5Kf*^`#dG zT;Rn;trEB_>oSTD-p0gbQp|3@E0kIpUt>?KE=)gEOGnK$Gf@YPnQIsK;8lO);mrB1 zjADBcrEgpV^<X1CHQt6r&2Mt4XC~NkPZO4OWm9$Z1-kowI8NPulHz#R!RLDVXq&=Q zpt-pZ>qs2L+kd#CcTp>GVoMD@JuL<fJk4OQto=m1zrGg6tDU0f_?F-SxdOoNW&kf$ zmoW=8JR4i*GAOaWHH>*d5d37RMx8k8z-ZYdpx^s<Ku{HhUN1NT=e4iL9S;)Vm-1db z@47$COFPP8ofl{FqcHRt5T%VXuLH+rNx0$CBWShl3@}v@W|^EHMk0cb$a9w?K|Y;D zyZjObcAuXBZ1)BU%NN3*&mEv{^-eUPYRm8^y<%?X{6Q=~7E?2(#Vp;s95<N7;td-r znf2%8DYx<wDA4_zxxa0v<&dEcwf?m){2FMEvng5210QeV<oOiy%Ha_uddZUczFipK zFj9g}4bM5=J3r!)%yG0Z591(_BqYsih@b7Ea4ofwOxB4<)2NBao2o>9cN0iwz4fT3 z=`IS`qrjvpYcbm&Q_QoL{ziWBcT`cUJfOLOPW?Y`F?TuK?slC3wREV3o>`ZM>$85r zbpJ54^W{dox}7V~g>u;5_YBVJDZxUdAbRFjjBoKfF!xLEqus16U>-JrXM%jFp%tg8 zx$<OGZ2Je83e$L3Z!?bRlZMd;_+gHM0<Lk-#ph`bN3to0Dz?3Xw<t)XrHdC>a#0MW z`$HNmD!l_W41b~V8;#INq6B<Q*0j_Dl`!5l30uG00XYK<?Jed*`P?g@Zmeffj~D7t z=ellDL2s8*k$UTKP0R-LPCx`b+<O}tKHG>gH?604&xqk=YvgHR+e~zGBSXGcv4-zf z@q^=4Mz~dB1o%{$!NLvapyIt})VJJVDrQiEQmT5$Y*&_OR6b>m4;t4~wU5)O)kpQ9 zGHRlAEB`S$ClwfnE9IzOtq}ShOJfAZ60m~#Wz4h7nbEi)N6m4)RvPMY*o&&cBQsf0 zk$s<@I^>8$)>@O&Q^Op#lsL8~C&1X9t>|GWmnNs{!VvRHpzR(8?(h{MXRgfzH}z3! z=VRePflQz}xr=HzHiw-nPg7qe4^yU=2oB`|EOfzwDwKcD9R4ng>u1kmSI>=T#n?+m z=y@}2xLpZ{t817LyK`{B`XW?X@dfYDypPBCKcVLiYeLP8rI4?VAMWT}0q-cCMOvCq zz^j^ezzj8W`dl1nJ-<L$memdegt^Kkd<ie@Nd@IwuOf2?%SPvQd{{U27Idq1rxVAz zV0oZC@a?dtzO#*(CA=m~*xJ3g@ShIK{df};D%N1Bqms;piCHAHnj7k7)&s|K_Cd*& zTTn!p2f9)^iTw+20^ed+s^FzL<w6U?yY3`_3<EwMD*}C`Z;+G3aikXT3x9snj_cz1 ziQx~+kmkbWuswjAO7b|8A)u_Kw{iyjbM81iC$|***pLga<xZftddbjJ%z#;}l+Em5 zrBLQw9k?lZC)Fx3#`W-AMm=1wwEPYp=FQo7Y{wquzUNYSWUmJ0dPoW%Ib{$2E%t#I z@>Stzpd?<f`X*Ak`~}=8NCiFJvLIu^9MxyYaoc#JSbX9l7CR$K)i4@Vg8vv&e_+&j zjOqc>)!QhZT|(5*XCb7TVnXdW_!L;H`Z42a+=9~aDR!U#67?Luj&I8osHw{gd@M;~ z+T3f23!a&@kaReHzTySud1OS+8&SsbjvO*_;2Juvlu9o()dQEybYb{UJ~$x755s3X z(4=HHYUkwOPZxSXDN2Hi7O9f2-KDY7P=e)^wKP;dTg?1kB?u>bw3+#t@=SA~5!Oyr zq!t&gVdTg{@|?pQ%ADj!nQb|k)1-@!4&H=o*SRt)&$YqMk|pp^a4L!wIt7)(=D>*a zVIl?2qCwRpyiULgtypUXGbWBA?zIPu4sZh>6BS`tpgcYQauob_a5;>AQ-)0+7em7% zdQ8-n2FqOatyFK-B;~rQ75Bu(;F|R9==j$~l=r?;E;c*?w+mN7f9oB1`WP>?ok_=v z@k1bZ9=8>`Uk)Z5f~fBMEAlGOd5~d!1~n;00E?|w*uOgg2YcIKZGm#6b-)MQn6QLz z95tX#@M@eoV~jfoa%nc7DZwOW0=KwHJaoDYwU!sa5NaO&Ez8Fo7cZh}vcl-A543Ot zMDT%LKlghvXFf0VrRrry;LC6`+<s~YjvRBNPSzyhOD+VS_E)5Qj>SUNb~RG$;t;qx z+m7s(jiEiGve;dpm!Y*3nCU?w_|o|p_@&MdmOd{>ZeMPLzU|7;Mu!bePR_wMn&aqh zni{n(<TCY9P?nLbE2X^uOhJbm`DE$7Rn!f!1nQ8O0)BsNh^U#josumTY}|HfF|$Sc zAU<ZKhyu;}nPw$%^6=;)I5>8Ld1>+lj(lLjOHx%>S)8kcXo!WMpLmk<GDN7a(NdIG zybio4qX>7lUB;ptZsS86W$BUow$Loig7UUtQ9+5eP$@SR#y`|xBs4FeXttT<?#Iii zt{O)aGE@PZJ_|7WY!);0nh#h#E)D9>@27gc&ZkzM8wG(sbV(|IEzFtCL48xVQA}+W zSYO8n_s4I7`qTlc;hsNv`;-{Il&6F<;#<+GO;boZ&4q|qa0WfFd5-=XN`OBTdw|S< z3>@5@jK>#yU@!JtP#wuY-&grq=bICrw%me$+AB8xGLeTcBbZw8as_-0F>Dx1rebq* zA=45=eJQtqi@3u1648Dv<<Ji+BJN`IQeJA0Ydp2`nl{z;CmlPRw_|OK)3D;iVXQW$ z3RSHmp~db`^nt6J5YP5kz|B1tY#CO=t~&mx&{hjd8Lh-G_!l8vsXP>9FO8=k=@MPd zvv{@S=0;_!ZtB2;Lk#bQcz~1L@Zj1a*xa&&aqyo&pO<Aa96yW|{R5ByRRk_C6Jtzv zOENjrZrpO$k@<8d8b;i-h6bh8_?GWy((|VrSehV>&yHQdW2|yKWi0|!9hbv}-sK?q zg$KBQjYRglyHQ7LDs20*2x^-$Tpu|%W9K~w`Ma_3IxR~aSd1uB8!1aua+o?DyqkJu zqRB*hR}pI2Bh;wL5(X^J178hfsc))nu)_Bic6h&!3fUKht#ksJyDuZ5@r*tYllh55 zO>$wZsUcPMod?cx^9aVZo6umT7^DwvB8QUsNZZgybigiYu&Op4EVxihDuk_ojw3BV z;_4-2Vfu<Wt6+_jP6juAxEb3xxc>s9do&M^E&0U!*jff#4;3M|mNg80DwLYln1bqO z%IPuwZZub22v6nqpd9rQ5S}1J3G6Pw70LBrRA38;Hwc2kh5PX3`C2exIfXr>((o$h z1fXgs3Pr|5VdhpPRDHA+ln=(xLT{ysuN}|Og|kmlxKAbuC;1p5>2B(@%sy&-e6eL- zUIrt>8U_O5EbJ6jN%if^2T~8WQROldDCmMc?ly9RzW;uL`)`*p5qIX$m*hIAe$Ntm zolSv%vZ9gvhafb@UjUn)yhY_k=IF<Y#gud7O6oVq9ItTH!)1I3>}0Y~`qx>~wUWdQ z`gPdLv;@m*wvxMbj)83(w^LDvXP}Gxsm4*GAj>k3WsK=53GC<}5BA?CsDRVipxkqU zx_Ij&^XW?*I5Kn_R$dpTLM2L>;AS~Exuy)eOQbR%KA%CpPx)}hLlb5|IEw23P>2gE z?tpJ(5MhBN$vp>)0BXKPHudP^fAkD`_~IN8a4Myt<bAA_P>aicD?&aN57RcALv^}K z!2w$V%OiLjKC+UR*<a+t7>`#`Vx3}?FnxpB3O$((uJ@UFGYhfb>(kiFvjQJpMc{e= z<f)$%iQsk$#prwOfm^K2U|j!MW+{<{XJuc4|B}l7M<3*WQ`shEC3Nzb6aM@~64oew zLxS(FVetkLj%@dPP?feB><K$VUjL#7%Ok_U?3qX;aJ2&Szh4If9iI?m>37kwlM7(c z=yo)6@f7LorGO5aW|LIc0c0T?jiaFnJj8WJxo@|J+mH0pPjhQfnEWqRo*ys#Z>a45 zJ>Gw&vWxD)CYA>y+Mz=2Txbd>tncHc3%23t*Bz)e+Jy9K=B1tt1KObF1WwWBp%P|e z2wBT;EU`<Sj4INn(oEB7O1u<*++U8bUP^(^y3N?#wHX-A?T6l{M6p=>I{3N03ihc~ z;6Jls<ioe(@c5uHMc9P_{m#{RrN0_p(s>Wwf1FGGt^EysZ>(i}H(m$2D=x#sX)3t0 zxg8I^{=wZ#5@+r11DT&TP*<vWuvl$9Ci~W-4;N~{@IxP_?bAgN6J^RQVj>yS(>Ykm z#)CRJG6za#<f&C#xg9~)H@w<l8ppfJG3D2eQ)#e*vukA%r7XOHnU=ET`X2e2sOc`a zI#83jE38Jf&o^SKuRI4oH_GEh6O|V7Z^wYiF$bW!ERYfk%t6l{$WX+b5_Xzy0wI@` znJQl!<fY#MJCdE@gCF)Ft=Not&Gqq$A9Lienw(JV#-sJ71CSgR|8wWL_;R+cQxWIc zR#}eF=_<kxD6sXiq{vTY8l0>vsYG>O6Z?g(2>Vw4Ct~~h64up@Pn>g8SIie=&g0y$ z*+OUM#gKE$o^hfhR5&`(k6BM52U)LbZ;*p_GDQCsUm`jzn|0=C3A<HQjb%byXway- z!%nrH#|dpSV;w2JO1M2NAx`p#(j%{{EKbk(u$)9<IVpdAIX5;wVq2Ndu`iCwavWa! zbB>;JV7Exrn6on+iS4S6B>Qe>gGc=uvP(jW(8#K>5Fc$~r^ha*3p4sHj`!*iLVKq; z$whYTp4oU#+4wX)x9~5R3;alm%Vg8%7D$i^8$*G|wQRU<&ob;`k^!Vs8c?o-0od|3 zj;!>S0$akG$VU?zv;d65KYSz+aXyqbbt}Sly=rhmq6PoeFD6!`WOA}1dO=?-h6(a2 zWNq<rT7=*QBWtb!r>g=it1LG{WrEwO{{uM^^V->K+#Ybsh+)D#^9`NVs>JH|Dj{E) zf8=N;9_M7McXKvhTS%-n-a}H}J)9CJ6N2*cCJtu3U^%28C8ew9gITs92=sDf%>~{h zVyojg=XnLlmN6kR_|sZajj^Dmy2m-*yAE;EvP)Q{$d+w-gW*J;Wg6&3CLAmOhn$!D z4e3CkP=eR-5X&yYjCgT!6XE%<jy=60i8FU4fJnBgYp_$vA$&VSiN+IY?DXOWHg9tt z3r-7@2__v4zr}vB#$>!XGSTv!m3-R>shu3wkM89h`G;+sDQ{Q8DlQ4_efJX_O^_xt z-<Hty%4}k=wFYqhxxyRzlPnr>MRHyU{WA0fer@_3yx=QX+pz`gm#-vW<=sVCF$UdT zE<`?b*T*^qyP<PN0&eWQK;Q5q!IF}Tp#EzY{MMrlod0H%_0C5@vUfJ<+$Tf7rEbIB zT3*OlMVRWbFMy8}BpCJlVmK341*=+@;?&(`;73mkt^Yy+XGR%NxlRhSv&(KcWxN0{ zoT`FnX9nnXB5KUT9~)tnj4&|bnU6yj^rEXe2Vvxo_c(Tm4@^rthn=>KpoWr{@R+s_ z-26wAS`4(|_c{cd%Odb(=W}Qn5J@SYZe?D^rr?r!i{bMOO_=oV0eMxU13St?YR|QN zZ2#jN9$6m%vr8-KkT;jW<k}`q_T5&Tpecogv)w_(gcuX>Ae{-Eq40SnVXWGI9ef@? zhjzu$7%KrLTA7V^Cc9Cpf2Xki+caopOECiq%b0y{v!LEVQ%daj6KK5F0VsH7!^S`@ zkR&z?BiVAezfFW`6duQF)G+;a<`g#iD99K)&!Z&nyFyLD*I=;3k;?0d!OABRV4tTo zINBV^@K^It)u(xwcF`j^{i-T6IeL<#yJJ5|>P-`GVlrvFoj2(7qBeAbe?8iiQ$)|j zlmnKWGHhOzMaZ6eMmBVh5iPGagZyd>;@4h5<S69J8TIKP)832WtaX3Uw?AU2!`6|m zE7(Xro>L};MJ>RFNnWC0<^>R!OQzk%Ea;x%$Fy_)Rq}H_Ce~Y*aAp!)+500W>8>?H zsH>$G5d&3dbh#`F9_NGm-s;f#9AmOqV>Q|wew}_|w1x}|_Cixhf5_aRZMe<+G4X?Q z0rf_hqhJY15FYZ5zLc8@j$}n}+e_NWe%&hMxYHQx{=LsBV%_09c)t~Ov_zvR6O)GD z>jLT1JVD^~dT!;sGYy6lda#6WqLU1g@q3m1<lO{GG$ZH-AZra=AaII~{?i0xts2lN zu>`nu><zGE*P*DBQdnI@7e$TQgQJCZ$bp(bg>TP-imcsmOmhvareuNT>k2|eL6{VF zGzQa0rAWiR`|z5|J<{{vHI&h*i^D3!VNAUlUZT1isn3<-Te6ej+lkG1&mcEmGp7YA z_8te0y<a$1af2Yj+6*1edk@ap#$okS%1n@p8*LSaP?T&t5PaZ5FUvPUU*0=|#|0@k zChZ8gxW*gY>D-MUihZX87MA0WM^3}MJVy)8GH#YVH!d^AS_lo=3gW{G-Qd8ddt|nu zIAiB|5j~VKMKX~cNVNr#VC4}G>yk9te=MHU<-iaUX&cal{XST=Ab~6FjDtBJ4AEv+ zZM-B~5<11_f@V$tG3~Ju%f+yP(TX;7VP6qU6IsH{OPK&apDd==pAJC|@7+O}AeRLD z?FZGL+L7hDHps(k2c%S;v6o>3^j>X+1Eiec(bu_{?Y97LT_6Kf4@y&)ZOd??nGcqe zl_CdBWl>JvX%J!0#vxjlQAl1U(v|F_<F}m$Y4?7D@?E{KDViHv=q-d%t~VA5c0vJ< zuA*y(c8K|$gR4@L0sna~T;5{<4aJJ#CWA%zeDf|8onQbb<oTc;C!4<JQiybCM40_s zSCC1A+{|-c3fj8g0J$UY5T(Jw7Yol}f9+|aRY(yhGx@X)H;$ffbt{=?Q-wW)(m{l2 z4-Qejh?LC~&{)wD{P5*hu-oV`>T#FD93eX#p>2sY5*ty7#~k9jW(tpP(8heOS-`Ee zh~9a)4))2`q0D!&I73XGm=l<fPl~U{pE?#}?W`nB=gmib8Ji*f%AiiGk&Pa$&A_+5 z{Dz0clTd2l8TjGk6pR%yfwQOg(T5(V0j(lluqpdBR<<&S20QAI8^1li?=AyD<|IA2 zZ6)gD2Y4?b1!WiiK-GhYD|EdI$?bp8P)-c6yT^vNKD_}#vwz`>1ZgyG%i^@H=_ae% zQX8swxf6Z2r0D>EO^p9CV54#;7#-S1W;VtEpZHWdK~^3JYhB`eSal86rjVq^CJpe+ zJC80~)<Y{+I$^p%6R#TLfE!)+*|r@{q`=k>wDY%IAl^Sp%4+d~`%amJ<Ni(Lmf%VD zrf3=D`C<#ukE=!^@#XZ(iU;IQMSz3GPk@!n4g+JCY%rTEinIfSVMXr+Aa>?7+3@=p z`CVu}q<aIwsJki|e^ZWYlyd~hMhCd>pM?I5z98eC*OM_P>`_6CCU)|;Mqd=`MnziP zr14Y<-S@nd)OQU9{VDrUZkZ93QHTY$XOE!7N^$siRusCYOtORZH{<M{MQE}z2A{B5 zi*=k*(M&}m$+>g@oBgoE%Z#kiTgeen6utv#`0asvE+&Eyfn&6e{XO~@^$Fyk?FA_j zK_H(SmvaWJ#H%C<fT^Az`Y);Mf84YGH<eY{Hy;b`Hb-vNQIJ>qB}s=i&`ie_d~b^g zDtWz!9vBj3zSUNP?LXIHvMmg9T3-RKcY>DxE)MR!d`)KjQ^D4P-YDc7Mi$*?k$^)S z_7nL^?o+x<Ki#iF?~vtA@05`g6&L1}5f<gnqv1Z{|8w!!|F@{*BgY-QV)kFHa>qUX gzZ>xTD2Z|hw8+Sa&gX6`{(rXRt_ky7^UW9eAFn@eH2?qr literal 71976 zcmeFZ2T)bX7B))GS(0P{K|qn5&hE7l6cCIUKrw)d3ZfVQ!3Y8d3<OaLq5@*XtVlS$ z8q8Tl5Cb5HSuvuR{m;xD$2<3*JNMR9y;t>KU8;8Nea>2E_t)R<uY2|C-pI*l$j(lP ziJm)ox}K!Ht-D9YVJQVG1@RaK@pughM>jn=@90^R!(t}dnacd*%34uDDl9%?p04;d zaS4T43L+<kZqveIB4$mSI4eBH*3Nc*c*L~nF$(H`_;ON2*gVk<-SuRA!)MN%Xm2X@ z%h_60K{_mI_Vh3<F|kB`o4B~Oih|Un@R*;Ul@(-S=7!Ch=k<^Bv_<8)Ye+cR>B)=? zn>Br+gQ?Un=U;F*+5G{B!=K@Bu>A)PC;Q)UIQ~r>R-*FkMK~Pv<VQqLjhPu1Kheol z_P5JFQuI5UPL98`>HOzxCW?!0>!@H59zQ#LZp<%q&J*z&6)`J3Y_9!}`sn?+^|y*S z{Z=uTzgaO6!A?%1iaGyAu<LIG{|e3d&!PQJ^`AofopYDppt=1`Xg`T`{hdhnKPNKj z7b0E%h}QluiTr8(jYu~=*?+WDyT1unM+Cu5#H72P?5|DLUexA)ZL7b6bN_R2e@^8O za6hPYw)+82)c*gLj;w!3+}U1F{x^x+JN|nzo$ddK*WoY8{Av9Mna&PB@H+iXydpB4 z9W*4I9e<GN{7+;$|BGaP6#wUB{<QuE&gloZzpI&_WIF#O)AcXN{1+WL{w0||t-s-Q z`GNQE3h5UzT|{KM{vgx+pUM0e+59N}&&m90{SDj?**N@N+59Ba{dY1Q?Eiwye^HXt zUy}LL`VYJ=Ka}L~ca<a})5T6h!o~hKG95%i%I`9F`4`#zDE`mM{Av9S+z;6}{9W1n zPNs|FPcr|_;PXqj<nk{{a{fy)e_DUT`$I_%e^*JrkSQ8{T%3Q9`D6I`72Lnb=11{= zPUcVRZ{U8&#^LYE<|mo1Kgs-y0m$WFl;rZ4Wd5}NhWCe(9RIG8ej!sd0J;1afE@pM z0CM>k+59N}&&m90{RcSLAF^@$yR!M6OxK?SkmFwrK(7CyB-g(r^QZMUyg!uW_;-~g zBGXkg0J;7cfE@pM0CN2o+59N}&&m90{SDj?**N}P+1QFg4N-^g>a3?UYSz3tvEkth z!Y4Yqn#%wF=^r6!_&iUEZQ`;DB7bL5pXKWE2Y<J}^cTs})%6el?tkeoYI&}1|LpH% z_gDTdzmV(x2Y>s&^cU6F&F&BW4u9qEDypxW{U7`t|I%MnUpI$8_&fbu|Nf%J;Ve2_ zM2D;Ba1$Nw3L<=>!(McV>gM*d19tkqJ7CdMk@)4QIQ`=3^oLSi{%xtoqB}c^j$e?9 z0*}FAF@s~HCOU}%kAGY$Sc{C#BBP6*!ie7*g_E0TmhzA5|Eo#3{X*#<9<kHEf5c@P zB4;-}MN#ZHFD7hO%tU89(_iL_zkaaJ(NK_%3yX>kw_dFwCoZQer>iFJpQs@wcFs%e z|M9>7mGR$G{6DV#$JKw&z<=-g|Ce@U((k{o^WXLSUm5>BbN}P&e_Z|d4E*<=|L?S` zZQ@c2zeH`K_|VN&6hDbF9KS_v&Y}#*KSgb#6t}zG&#2AW;on4Unc^Zx`+ttY9RDc_ zi}?|RP5&hZ6GdR5KL}|k|NeU*_TOCp_}zax|5Cy33L@;HLzEtOchHmRH)YC1XD3rh z(fRkfJBm!Ac=$)%Me*=&buW_<S)Bg4J}&=MpB$MV6<RIxYg8%sOH?WIpFsU5`!59i z)$yNx|DWuCb?<*B|3BU5-`W4ae*b@0um64j-!bq*bfUSSyJ#-xE?T<zRdlYuicUlA z*VVl~kr6R{L@P(pb7QQ(>B?!#nX8F={n@&mf2;1NTLl>M5x_RB8rpiRGZkM+=G&q~ zd-m%GGY-nJ3z{!6<lt%2FYqv=Z&`?a&9@Sx)cg2S*8s<?d;<AWiu|aOSKMcDLhtl4 z;IkGDhnpj$h|;S_+@<$Oc%^KIuxb547!gtk5;47azFmXW)?VS9nj>)4L2>?~-%C2S zs}+}YPKA4Z_pz|(F$_7>ok_en26Z7jaK33EU*A`irLE}#!)iI)bJ>y)E|*|3H`c%? z{Xuy4Q5JD|Y|aj+15E8E$;V!vP_e?~GEVeMBqp{#Y~tx}xIL_h9({fboCht3*kA+P zXs5$;eDC4J?3-9%@dgxX(;!DF58h%e&KC}Yh3`(lINLeG@-Nb~q)VM;XUa4FOf~Et zehySOo3g#G!K~K&DXCrck(;qXjg2+nm_wEs)D6vooL*T(+wmZoXLA_$=D9$~_B=9S zdkr|z&!8IH31!DO!FS_8Jov~14$hl^EP6c|_gRsjdVfE3_P)%uUo&L&@j)1zup1|E zEim}?11^uv#G;93Fs9uB+usWKDf$&;f@v1KR4>N6vn_ZDOFf*w(w;96v!}fRI^olb zqgBEOYJAI;5VX2~k=r`_G;&}AGrrV7uWjz+&h^cBzo<=ElB2{*>ze83$r3PBVJH}% zUILEqYsu)q9!yTZ7FES^smsdea7UsT2F~69*ThxX+C}Bme-Z=VF-M>&Zv<5@J&I{6 zSBd@5dQja!ps!Xc)~f2GPozE{>m&)|axP&=ojzLRK7>m?BWXZ4DOk``pU;nLs*<>G zAn?Ec8t$EPr4AQz@J7dNylFcX?LtnH{qwE(4X&GD+L$BciSHYHyR(+IM%82IliOAE z#j_x=_9G!j=hC^2i|947N*JI#6gEfY(D9i&p|<y7eB%3^x_L@6d|}LY<DHn|FdeF@ z`x$iyO7d6AyD(6)l3sAxj1&8vBc*lSSxv4wjK43=+kUbV%F9&ahLBlw;PHKUUaJP4 z7PM2%DJE=}bS`}?)<J#D_4vM<((sk79M2Uc2@3B!U|47&9&l@d04H%?OXd=4N^3!D zhc&eh(!r&(Mnk}xC8VL*f_?v}09&t(B9FTFh5MxgnD^@dP%k=3u5*bbb-yN~Ss#TV zU9VtA=UZ;Ugrmas_hPXAMk;*}X@#kLHtyVb7_e(3XxB6BZ;%EZHGAP$rXd`Q*M@q9 zEo4mdV$|6g0B7D6V1;J^RE=&W^H&W+_uF!n>pk=7-DO2oHq{wgAH9T(iyC;;avNrv z*3deifxP^_B5Y|>#<|o7pRBUw$$}#K-BucI8?J<l_hWFnV=>&nC&%|lHm3Wh%tY-s zH}JCGFzgHs<YFI`!>+voxV@wh7mbQwF~=5jt2`FryR)V6aq?;yXEvNX3qL@~x^QeU z%cU#M_U03BO+Z1`KKC;pzC-zv7Ssr6f^mi~>D68w@9bSlCpzfz6SOCwnS?q&uOki> z7WUxxOPdqBXn&Y9Ka|=XlV*c2orHPry&y>}6U)C|gGmRLV}FgA5W7-I^qn2sr^4vQ z*W=JB_A}L)WQ|Up$)Mu0N*L-B2(QkGvzw`%7-%>M*L^q#35!c1p)(9Lmp#CH$#T4m z>SuDI?hy=aQDG-Het^gegWzqh4U}BV#qDj8AZC&dFJ68n+t%O2siPi|{Svdm;@Ehi za6z86=On`FNiwWbV<p~r*oCHnJ@{!3)<o`O3-#C|dJd-Pcy{P_D%&B&%g1*U$ZVPl zrTl!1c)15uELK5a@C?k2TMxT7za^{NqHqJR#lDSuM9s2J!(GddsQE~TS*Gj3r;u~p zyZCI}<hc+>anc}@u8yrT^)U2MKBi~Lu~E)Wuw#xcm=1XXE2b_)tM>PxdAw1O`t_@@ zQdScM4;Ij+wWa*-$zSOVM^pad&_SquJ{6CSnT8L>y@2!6pTp>```9AbhsQG+wGlTb z8{#;)9^eSN_F<^KSC_9-*5D_UOY^c8gPE3h8YHy5hsd->aK5U;!hGb&){GnUL;fz< zM3+HWtpZE*enAg?QNpB#flMu~7;VhzgppGNp=Ej@1lfuSFe-_?nJ^EhI_NU@&QZ`m zDHaYrDTJi(0Q5QEh?X;i5OH?{Tpj?J;3S|yI_50?$p_dwrVx%E|3os^o<_mBr=aYw zzz;B&fE?+o7^!_3oDZypT^qVXcjL!cw#O7J*EfN1v=$#;`hh<A(g&`dzmA>F3oy2y z6h&(}{(!tU*LvWR;LDcNSXP86J@-7-y!QxPcJ8KbI}M;y^E4La4<?g$AAyw$eev#c z9%NpY<C>WD$P|je^O!a)FcV|vYH#4sDX#pc4OfV!b^}gtIgDHS8S*FhUnfg<?x0e- zJg@sQ1HYeLii%lnFl0s-Bu}%2-sZ`8Xv1h&^sJd$M|MDG^Ly}X?29(#IdsvyBoH*7 z<Rnv9BU!3T!re9K6zOZw@>UlY%@M=I3nMY+v;haT*XR?^haebGff-YD_)9t?acY%4 z->K(IAMTjXEAC#6d!Lqq_x=hxHP4KXFzp6iRi|N9YYVYXWBBCSYjC$eMP4r6fQlxn zeEW4B=4lfQcy$Q&(LG9@+7{5R>yhlK)plXls!CL!Ispnohr&$p63AF_0@sy4#$n&Q z!8ufg7f!H%^@sG(*HRnL-T8_E9u24~z7iC#4FiichOGODk<8Q>V4HzCoVYE?Y>E_F z>|S+Dx7bA{`ANdf5LpsEq7z4N4JWDJmcxmN9x&N25nR{Iz>EWr(f^t)&Z*7=Ys`f+ zu9k4YXc2a=Z=>DrrC^KE6&m(cNOBToKrKZB>PO$j6yqT@^)S#y1C5w+?~70x`x!n? z5@%|%E=>5oj_67m^5?qm#5s|2EY%^OboCF1{dwDkWg#-`WYlY#?|1|H5f}b^#}^vC zb`{KWPK4x98(^bV2kqyQ0!PnW#K@(Eq~M4>FEc=m>v`xjJZV~oE&3|h-S3@nX^1Xd zIo=;m9;?Q60cVhR8wP<=dc0!yePpqV1kBsM5ntYL=O0Cs!o2L+q*(DGdfe!Ox?U5& zz3U0d=$nc2R~g~kCmq;zKpGBL-3AFs3-V^JHcEu=p$9z<<5$y#(D#cQZeJ?JzId$@ zTs|~c)CY+3)1NS6|1=BIx^~gqyHmkyW;@+&co@z|#e(2uqVVjU<8ZKjJ4(;~L?^q{ zb4e{FxVK7*PkSGS7Q!uX<ikF+n`z8foiW0q6|ab~n>|jqJ_Hj48RX~}Q}UW+k)A2> zNR{)kq^}yt2A`pWl#O^#{cAWeLI5xCXV7sGy)f<AwW@tdr}0W=1Fd<#7dK5VBa+(U zLT%F>@TGYgzFnKg?dZP@gicnR-I)zgKdT+mKhK4gE6(BC#ZK6~yB?-5c>{6P16Zc< z75X_sj(_Q!0e5aa!mmT+u{oo!aQwmz=<c!&wipTplG{eJ%tDT4>?lJLE)M!3(}i1> z-J?fLgYb0GRvf!d8=dX8;P)5a)IfC`#?KxMfuH6I@2s1MYP~|Se62g`Y#9k9pFLnq zf;}l2_5}4!hmzC@$H6H+7glx|Fxjt?_%)~s4w|1Knit+<*0e`dx^NV%Q{4_^fimhX zmgFU0?Zh5;kK)or-n@ayZ7|G!LEfFc3|ICagp8?cq37{i;C+Dyi(&3?UP+uc&07j} z?<M%&RcnxP8;Dbn4cM*c5%_jT7q=b1jds!+ICR)0j2@o>Di3!<U5YL{@O2%uhNa_! z&KFSk&6fq#`SUVMG;rF&-C*6O&6G4hL*;!9h|3W5hZdzouJ3B$Q^8HV`@)%o)tCv3 zqZRq;eP#HuNCnb&7h+s^ceZB!YC2bAGl^Y!it>RbwD?dmxPRG2%kLFJ;hd}J^IQpM zM(u;J=uVKe*#_2g<g`VzkN<mvpnvf8G(Uc`v^mSt>d&9*W5?1~-a@OjL4265AOEE} znI%5cU{*FhthL;UpQ6!^DOdWjeWNbH{v8%<hHg82*7M<CawfdE#7)%etHE~lxdF+% zJhN_*mXrIn)PJ7y{8*p$ua@qy#E*Z~FOnHLc=Pqs7qFBpZT9d`46hyG&u=)M#L7$^ z*!4v@?D2<8epPWS`(PBo<Y!ES=J*+`yTVma@CoEAf**q88UeeMAj3weOYm)<8?frY zJAARd7vFt>Ijhu-<HICA;nVC`-liMRl9l?PSr0$HXP5;avmy(0Kj^R_AkUZ9@vLjR z86Q8aA2UHa-hHz(FDE<zMyGc{<3lB8wcrH=CFKJyG-8`B+OZGGCOqk5%-8hp!=5NV zfY~aRy!3$(zRua1Nh#&xm7qA#dQMR|xfza58^NE*)o0J+C7E5&0WxEm4l6!#6JNG{ zh8-{7;-IW#*5a+j7a#OvYaZM3>-)XNcc$k+`fwj!P||^`Z^W>{pG<h!W6jWe(n9|I zrAu^`ybyIH;@Pn|zM}cYOm=3<F|bfeVeapo`Df-{%xc(hc8og$@n+&|+`d}0lw8TI zZricoNqt%K2uJYqSjM(ZRp3huyD|BAU}8T0e2VJ`Ha>qN`z+Ce-`8gzuPC9z#CCLp z1><M%JxWXYA_*sKvJc{yMjeK$$B*zsG`#t;t-k!e4~qQbH%sBUsPBy*tj^EZNoBTC z%lKitj-g4sKEFtNB_AFp#jEsnWp8)6vBUWZJlr-SlV+&1GJSLA>#_noW*xzhYcA~g zoirwRlg6*;xq_LwCy~xU%h{php=_$pIeZi7&A+c1Ks<x@g0!MP4itQc$MZ(<dpT3S z>5VcgdT|~OJC9)>Ck^BScj;qH!WA%9G~%<r<l*~08L)Z%RkCE46uRCXPSZ+4P-@OA zye_B7C)HYF$Ju+-UP6_Go_Ir?OP`XioD%x=!c~+znhs{K-{K0J%h;S!06y!IxlOOT zXo}8Guy&UdWyAiUsekMi^fy_+Dq*W&3FZ5=lOdbpaC}S~%?vt>=fIb2zFv#gD@^5# ze$5B`<;GM0)*kAcYDur|Db#+HEcLx)N#9`*ofF{;`$p){GtU&zZ0&QBb<YS^xK;^o z+%-Vo!(l|Xn+ALLR3Eawo@Xu7uA_RN$LKID5ob@hfYJLSVZ}8quGxM+oRsV!T}LL9 zdG!w~)|qR-C#`&L#qBG?!3J;KCj|Mx!oI^GB6KJ=oYfE(?`srzK6i!Ptpjo6yL|e% zWIT65Z!L*kl_7lF>IdqktAw3_D`}XD0nLzgfK-)MlKxNzw$Qcw48cMYS8a)b)3)%B z9U8E-uZ-t6{pI`^{*>_01ZyB-hvAz>B19VA1@$;v)O~e}d{@mChN$Yp;H6cdy)F%} zxt7q6Sp|^1K%7imn+mxh1EIZf6PY||6?JrUCW#t#!h=yZg3ymQh+6NR!r9&v@%EL= zbb!ZQvMfK58X0_~Z%^w&_$qB?ks^fAS2OrYcluSk)LvzgngifPhbJRpmary8lt|g* zh1a!`(8ZttR3a|X^o}e%U$TxsXCA&%7bou;dtu?U`8crj8@l;O^Dq4x1zHnl;`<jy zc%Wxym0#K<8gG^DvG?nATHmNjb7ov6v-ehVspq_?-{I-xwU-1c@}Eg?#1nGj(He*l z31w`^bF3%wp1KzmJlF5JfsUu=3p=EeXxA_#u^QEwnV|{PC>=E$c9LUf#&bJ1y(L~# zIr?SxcA|M?w;-%R07ZjtaM1(y(=)q$LEE~9Tbf~8RoW_v4&K#VzQhW0=e-TyKfIX) z2rd#`uhZn%{V<fcGk|m?dqYK?Ims2spkKNwj;!e=l*kp`x9B84ajZX^e71?7YZQb} zawp;KmnpRNu`1s)stnKe$j7?{nPfq^111P8;nvMWI5s)}BW;7xY5QxkWaw_}eW8#V zzcL|bE3&XRDF^Y<iV$bkO;8agL599u=b<WgicC=zM|G#`WU$N*a#hU(z8bwF>ykdu z@nP$@g--SOwK0bUP2Y|)+*3VuD)aE@#Aon+&O6dluD}+IKEr7!o`SheJ=mnBQhcrX z5;$PI5RX^~lg$C~K<4eC#Y6M(NM#bVFZ8GRSqYWix`m{vxD*czU|_k+4hEtn8Z0%y zPP1xm@ce0X@5=)6MMsl<r;kWka6gFbTTKeH18DEBi>ZuEFD`EPAbwl%Fg`ohfk}wB zu)q_A>`>4av=J<ad>uPJTF*z+^E<PIW@T>V%|gVT4^Vq)Bn0ku#<<)an3M6H4B9mh zuZ%Td8y))aLWMXS=T=4q9m8;cdtY3m`Hn<v7!N7?{ZUQr2)WmOlx{Ywr+qh{<Cd>? zqpH_+z)RhSGYV9s<ITg^MXjS~RGG?}WkdLYaz(WC&tk*!-PzU+8Fbg|418SuhMGOI zf!r+x*o|KS%^rKI=BbsDEe>DF){0$pvCJ`=Fr*NQoV4J|#f#_?mJicYyW=2jQ+Qqe zfQZl5q4Qf!;cc2Jw9!*kFv%9SZG1}4&pt>ioi~!|Gsfic9bMwFUQ^iW=Z86JSNNHi zZTRLn0o6koG8QdIvkuguui7+z+Q1w%S~D1*SLEX<|4A^R_Y0g<E<xXSO0p*BXxeLN zFLth*4lkj!2_4s~u&rBOL)Hp$>{T7c8R>4r;nA~6YI!xCcc+qOl^)=}%zsYQ$F!0a zb+#z?#tS|P3%Lnfdg9QrHpEX!lO#KKz|FWVe3#(6V3?dX)7xB!sqHbGd!-6r`NExV zDwvITw>x0voE;?Unj)Vt<vd6q?afcLIYg&zy^R4<I?(6&clZux$wkRwR3YOcl$3bW zH#NQaq%vtX?t&Y>9aKX5fA<hf@N*+AwQGpf_IBaOjy-hKlT5ND*amxWhUEH`rYg(R z<utN!G(3{MhVgH&qv7O!o=XxaOqg|&9lgH~h9AE}oqD__**U(LlJ$VJo!tcWC-Uj( zogc~Q5j(i?Y8$vIL*>9CcqcVBPvlCs$btM;DOz`{gm$^if%WTUXpip8gb%YTiN}-+ z#QgbnlCD`zPY;u)wtcdw$FiF=NlJ+fzqp0GditI^HLane4))>Q7b~y<QK6Uw6|8M? zHcKp94c|uz_*oHaXj}6#R9T--u392Smz7{b$1~a<dQG@>^;Mb;o>1v3B$+1%V{OrU znynWCF}E#A;2wR@jonOpIz_<2NoR!1B<-PoggA_FE3L|KtRrzF;^@4L4%#B<MyEM1 zr{#k+Nq}uKxfy$!)NHs1sn-VMo49!p-de*K`&F?x9TOI#qzWN!%P|K;2|;}yTzEr^ zJc<xPL!BpNdS{Y}{fCpaHc~J}(h{C7dnBxT5D&KZ4WMD@0G!aEMLvt`aqXcegeMY< zgz4voQGd5e!NSmSRMGUEuugL{$}1V7#Q;^T%CsR*XH{2uDc+<`R@;c!8g0gf%x2IU zz=wWJ#E6BZ>_NZTJoQv%--@#Fz-Jx4lc@1KpB%vE&Aq@McEHWT{z&5sfbTaI<iy)a z{fRqxxbP9Y9bJVfC0_8fON(q>xt6n=D@6~zwuFS2i^zh6quhP{@509U_lVBLEMbt_ zI6C*BDXL#TF1R~t0BO2B1RT~_;TC}oub#7k9huGX>r(7Ir`UDydiUOefA{;K_%?z) zRqr5TY3exQ<7z&yQr~mOn^W*?{af--$p>tA^y9TX@}T*c6l+qG<g2oiV0q3b6s{h^ zX9eCQDJ$9_`qM#DZl;c&ALA(L{f;<}cY-dJ3R>24FQ>Xi1v!n2L@sMMd8*R`M-Nwq zd&Y5W<lxcFwaAN&H2g%b`I@o|?<DvugC{f9OAhStRbA>eT#n_eE?`Ds%XlTN1c);+ zV%lA6A>cqZs5({S^-fC?XO;#U{T`rU_w7_7L>{g$ChXGsQ{Xp34D4kSXyZ+Hcs5TT zH;`)L{dhdJ3TYx=21N_CY;O|FhU3&^;!Aq$P6Iuq^^x@2rqB1y3S%EA=UHxGTP^i+ zAG!%sIi(p*a4+;cg?Zt`Cf%MH?^fhpZ>r&KosHB~MS%P7?k0PqC@%aq4D!svuw<S% z7Ou+y73)G07;A_LW{>Hzn;M`rW(|q|)JpVHqpO~;>>%s+c*E4a$}oPXJ{~MNNLNnM zAb0BD3eKPEEyO7=uw}&^&=9WUooeTLY9AHz++>x_8GlGabJwYW3d>+bdl1Oi_DAK4 zEa-i{mQGGsMbie?&`l>*NOIUMT6}9P7k)R0+LwK!-^0&B`a~_r-ZX;@kUC6LqRR!2 zg{#OIi|c|-=PuDD?e5U8YY*9aq?W#@wu6JU2dSF19QDg`L<6U0(ioJApSsOtZo}V! zr|m?~*W>$C8_!8&p)X&e&u(3IG@(Bn*}eznitnbOYevENXT@;yt1P}P?@i-9mVqyR zBEh}RQwyhoFx6sq#j*=+)IGTj%6n|XMCK3U_BYYI2V>A{M-0y0`<WbBr;gfV%}J#G z3wmSAr7HV_S#<r<E2PKA-Q2xt?<%z_o#+fX!dItlV#?DWuxsi=c?I1^Bx)V+!7}~% zyu|kyUf={b6FP*wUmAg4t2V9;EQaTMY*@>j+vM#Ubv9MJ9X_5{#?5&d7~DFCZ`A7v zCm!s9v2#sW=ocB>E8~XpLynW2CGWZW)6#_BN_&A}paRS(pNNh*_N3#992)x=fd2PS zG_tG&R*RiM^s=E<JsLeDQv`gV-*|Gvrjc%qxyBuJmjQL(SUO<(E>2!YlRx!f3yh>@ zU}XE5HV!;N@}hmgS??R&o$Lk|Uh2S&u5@zg^GYb$aD(f$ZMTri{l*z*IfMQTWjg#? zLgmMiQ_!YD2Xb%nQ1kvdow7s|^B?JO^;eQfrKSf2+t1`Ry02tg9|wcgt2Nc_KI=V= z`rc#`UYq$o+m&#EY&R(C@<)wZ8)2fFBnv(|3|>oZg~Y9$RZkMzNNDT;er)|0Y&_vW zPWqjI#z>^wzw9J+8w)@`Wk2e^9St69y#>}iT1d>eN@AAF(^KM}sCmo_pGNg0QQcd} zKnH7lS6Cw)FwKsT(NTO;m^E)6%b=iD$#dAgvHS$dVSJ|81b%n0IyI17fnP%+=-x6% zh(4PJ_4BslnbBcT{Czt<j~ztkCGQ1Si94i!e*={4*9^V%Rfxx(M09NaPBQjRqpHIV zx!L`yh}ifVYHH?Adiwj4;LGPpT0c=jf6@lQQ^~;~_8^gjK5e32w`@S~(S8h^JOsWR zC_$A5h3XPq2B%CEJx7%;V8bg%kw!Cpu-c@|UpuSBJC{mh!jrl5p~NP7X_cd(wq%9C zFf^5LGo9$naaO2XG#nS3uO(J(<G^id3w^yghHw|kN&BasByd*_chz_&oy=>)pq3|O z#qvI!Nr5bC8P$@!f?Q$sECoEj-k$DWXu`NIEB2+S7UaK`GE1Fp)lr2D>8q7pIO|;s zmg^Y7TopSsDw1NI$M&PUUKi<cVLA34uL_qGq97;V2^W3O;yl}CLiPF_9QA5Gv7=+4 zW3(s<{ctl`Q$CDNdoK;P_ug<-M|dck)l43RMM0<RF7i!fqww+j@tn9}3in9H5_fbp z(hr+wgYCrYuzRRA>ut5I`eSN;vRg;O^Myt=Y_hxqH_mIaPwHCy)SD{UD!&e&H6t36 z)gUwfEo#5qi_h3gI-Cyymm^si)i9mZUt15^>(^kxB4s@M=^=@3Yvqp3SjMePJ|tS$ zQ^Ct)b_yr$R|B8)K>D>~3-xkw!qOp)RL`ziaA0N(O*8GoPHddR^EGyiOGsfeLWfkR zXymeSi^j6KqFCr#R3Fw=kW)3?<UZaTl}MEbYhZxhTRPJ^5GR~ljq)#~*t_D3P%SqR zx36r+-JAQ7&#p@`yX7$kOWS~MKql4xa)a2-@T|K0Yy_9=l1ysq14#KI6Uc0?pcZ~6 zc;Mt4Ze!a-VVSBnk@CKb*`h__2;BnMvC`7B{(3ueRxaY5QJI-PR6_R$-N^xOZJbxH zKui2ip{(CWa(}uUX^Tn|c6QwH&^$W<Z!a4_w)M&<LkDPKa-TKyqOA)pe>+IH{ALXm z$jD&d3&mAL>N%}Dcv~1@zLol%Zzek)zaSy*FKJbL1GTE}APqN*=!r)K<bbakGvmhc z#}1moWPexBcMDFy#^oXWrtB!xNgDy4&dWirn=E`uQGxRfI{Yig(^R?kFfGISs+sB& z@wIs(Y;HS5k}eI!NaK}Q-Q@)%Ei>`d_Iup&QE7NDsE$e+Hgb)dnrUmDD)bIqB}`ta z1;cFya0BfJ!riBbiDJJ~f|5c_`0854y=E!IVqPxq8*+-j*uSFM%&CDNCR!YMk$s%* zi8SG#oJ!;MhEK=+(_e$mU|^p;j|yGQb@?HeGTHbLE&RM)6YI3+@x3RjqF6u!HP<kM zoC;(7G~Su7Z%|>@dE#<LqAbdvzQ=0+>$}7L@lpHbDK(B2T)of}rpa3g*UgZiWqn6; z)7r%#ah49f{^|h8CD+2;K2h}2C{@zxw2v4}N+*xgqoFWHpDtYzMjz~ZK`d97(lmP+ zzGv-Gp}9#YF`ILS3u^GI>^!VO?50N$n<;iQ+q<31x3A^2MjJEhN7|x|^uNIS(=^S` zT&DK^XhJ<}7Gc_w17uJ^CMQ?6iM&0b!BwBH!QnS$U`63U(K<vQ_WXo3k?s2(>pN0$ z$g*UZc6uduZIubpySEL8T~UKjITu>7bSET5oFXd{nrN5}&z&wh4X|e}cAu;SbHkLe zMn0VjZcc*B_x-3v>OPn|sR19<EhSItOUamcz|bi&IRBgxSG!u2?s!lOZklr-|9ekx zI}e~eez{1tJrQ=V7Zwf4ht7?P?C5Sy)*>AP4Vq;XKFhPsZ_a^JQ74^me1hg5HvyHf zySUwdE}rpgq;3(W>~&}!=8ro`BK*d}nwk<+JCz_L_M1r`x!qVEu@QA@-Uxd<t`<tZ z=n1^k5iWr(6xIw*qZZv{*@AF&<`Ytab-~7XE4K$uICu*CC<gLP@jI!^u}n~o9LS}w zX%ZAf)Z@a?Qdm1=I4m3`h6ZCElgP0z;jMu;bShrJtwaHzpT0u&4Ox%1qWu*WAJsAN zj6S$s-U?dXtk|YAqo|{AZ`RR{bmPrH;n;2yh{8pADBl-=c>%p)NZE0^t1}ipZgrp` z$~8p#t36IQdsOJNS_+LbCZocH9B!?LI!SCvhcO$Ja9(X8x<!XEg)zo(Qb!teZdPIV z+07yxVtj&_BaENp&W5j+phMb49Hr*MhrK>fR(Y6uG)VDY$1KUgB?IyFmAk04c0Y!m z>Y|eubfDym4KTeR9M`vwLFb$ca7wU;Tv=@djV`vtytoIOHuMZxqIQR#X}TepC@Bh= zRa{|7q&uE|G#(Tsb=jQ@k-&;~;;8xhU^wJHd6`#BZoPdh>~!yeTYCBMljW^ocjX>> zDqI{3MD{nX=E#DckgIGM>CFv<?%^W{`*t1uMJWKaxk|X9=_wtxm*Cw}OBRpegzs|4 zW2VW%9tHP>Z&Jg_^r6e>)U3m}blXWXaPe#~9%#o}-hZUF>YK4e-3`xuB48ToOS-i< zli_(M(0I-!7!@go>05<ha@rYIS`DB&8eUi;N<A*Mn}wTvTS-TbDVG=Zg+%nY4jH=! zQ8w^2oZQp~C)yrR&&PHc>2(oqty>SKPtq`b<t4JJQl9TLKgvy>vko<T9)tYwRd`l$ z8`K4vqq503n8}$k{hT1o*=+|Flg^SX19f(PK_)cGf1z=fhWtufCHOpA0187d;c~YL z#J&4<e0tjj&xO3F`vgW<UiVeFzNMHNF7&18s`WIpv0dmVeu69t9tyjHc9Q4qO|-x2 zWpXN65zjVS;R)|ZcKw(GWRBcJc0L}5^A2c}3_Cmi*{#Jmd)!X+tt}*9UlDjW(g3uv z4~%P)<)`1!rriVf(&clK;r2~s$jCc}6%XWb`L5BVZQcQ_bE>2}4$cv-w#|mUQfEQV z&5G$8ZlGg(7_lBUR$TtbB)Yq316jTB99i3cH%2}BKzihVq25YM;pFfJVIR>t@%Jf# zxcuZcE@;hobbL4nYrCcjodc!7_iHW;^q0k%)9i5EGk<PSeLBgyP)eEEb~yL?Iejhp zmR@iYLuZNasHstc{WPBA%lEzDxltM(iFpsvX2)P!@wbXy`+Cyp!vVi|^ulArid5@9 z#r@`UK>p@LT=J<B;&0?a^+!)$ZJIqMc#8I9#O+6o8berlH5*pE_$K_6b{^*qP^6V@ z8E}5}LU`Y~581+{a3}j232F>RKe3af?18c{+*%Rdnrsvf4EqEJbw%rzBY4P`-Aqjc z+8~56m{!;mbPV6q^!Yonr6ZZPTAhT^(<`eaL*K!MLJ4RoG!*#vAI8EJc9Gilb;1wc zp2AOC8$@YYOPo=;oUA|9uWC<^G4zH<F?nZ@T{$9Kn^Sknp#s-7Ve{ZAMBhq^8{u1A z6+A^%5ay{#S2>4rS87^>)2@fpeO}+mvwfk|xGjgQ9y*+O-#I5Jo_>Vvxh{d`0(IQ~ z!BTkhizMl;v4Dh5Q4&fI-$_dM_Y?F<FQU>mp~5@odkK&0T@!lubEsS%vsT#s@oS-6 zt_G2IDJRA*Q>j6>^W>S~F|tDL0!^JeyNVnLAhO$2X+gj_?)t!9!tC%8k{6<jr+ueU zKi5hwZl5Jy^t?&WGX<hDb32_Col3iWQ%Rhlg+{#85%zx)#MuQWS4ob0P462YB6efU zsTI3KbX(2HwB4IYMUyeDcq|IH%~xOsO{3d19O;2LbGR74jgBPhFsWV^?=CUoEZZ$G z?|L|x)@4@8sC052qCIzuCC_jsz00WKNflvT$7P|4n+;c2E`^OQl9(-HPaGLX?=H=x zn?>mbhXn^X|KyGIc~4W$_x=Ks5@K65scoom{LTwxj$8>{IBF|RKiELeiJ8zwXCH3m zD1)kH9yh4Z*hj=|u>pZ)&xlICD=rXI5IEf{rmX`<RLuz<&ka5mU1cPlOaol~h^JXB z_fqo=Idtq1*Kpbh=SnDmYgfA9`*K;La_GEJSumSyee}L6l++3(xA&p^;XQQN(nln6 zRWs*Gx6+2D0&=cVlB{T8g0-66iSmdu^xC7nRM0Prv^=>^D&A-yzA>f?W494mF>m;A zY6_{-w85qK+JqH(NtpEFJMq7@5+2$Ej!>!)cATu`_N+NfF5Ept#+T-D-{(+HsrU>x zU{5=#F5HKoENw{kDh2THS?ob4$O@td?4$$sEu{T2onWwQ9XWj92+?kl!8dk8L8j*t zqFYo<Y*w75N1~f6pQ`235tey0t*w9#&RouQ3tugqVJb&ndad9ROxKX)gKvn3XBIc+ z-gvTXjTG6VX;12Qtrz%I9}?p1xn$bZ#X{AC$uwB!5V>urfxTAMb4v$IqhAw`lB);S zaSwuD)4S)yY2wLZ?&O9@+Cq1e!sTMvXZ33`VB`#;hFv{98PiG<+P2Z1&+pTBvlmg# zGu>#N`FJwU?gLFfz!6o$5nS%PNHTDjp0My)3!U_0wTIG<NTMk31Kqf*!oh=bs9CZX z4YriVSg{1bohcv4!Kz}i#LIz{lnvmJM^aZM3EK~8fz0SpczR|Po$pme^@3$^jjkT| zLfs9HwC*D@-!f^?l|0hxS_wI9DcX-$s#eu;H-tV|F_fnGjiMtqe<WQe_t0Gd{ZXZG zAax3TOYW}~qbWT)NrTHg+AT_lllI<Bj()D8iciF8apNA6vqP3#e`8+NFzN{HZY++s z+%vf)mOE&B>_xg$5I`0VR^sUJ^K{7R1Kgg85*Vd<g6Lch<I>NF_LOqEbeFaa&c<#i zyRy9MUR)U$|M?@Qv^ZDTvuPtS)2k)3gsMb)>_TDUE=^(m#?f>_>R8UjAcb5GeoT+` z*vWM$KdRb#Igc#pI7`<AjwPP@J?U_1IdCy~#5p~)$Dq~b<nH2H&h<u+NBekBPEw@# z%O1{@f3#Itv*#E&lCYj`x^jUyHYJg=rI}=^_C&62)eLHV>^!OaCXSKwUkb-97)94? zX%gPZaHa9E3YMPpr0<KjlIv3K^u#bnl0HQs+<0lP&>~_qdUY<NjgLKHS>#CS&{jfb z@h1cd7BhwE5{fwLu`f(M=7cL|CexH|jb!fDm9%AJD}C3#pFUwxf*D$4K__4hwGpNB zmJD+RZjb_W^8TFlvR_}9`15JNZ>y@I=T2M|6c)ze+?_J?hT~4Iw#NhFloE#*@~>14 zo|cK`ItMw&WHTDwXb!m+Jl<HJAXF&!B)!x;X}XvdEsy>nR7?sXiP0acE=Q-6Qx?bR zv_oRTlPO{JqGKufH><Kg{uRuRe+ML5l?|1?Mx01F*I;?HD(^@D<P4uD3>(o(4eN{O znPKmRHDPB+xS9;Do_>>f-HeCnFU)vzt7NQ+>A?yDO{$}xC{}O!G>}C!UIHU~36>A# zSktf(<7TbIsGyBRYF;kB_TG#dPvbFprUkCEGZU@$NkdA4yeJc|3FB0x>DBL{@L~Qk zT5+%v`vq9Qj`$Z`NY-#F+%LwPEa`=ds%(ke0~KHq<)mZpE#dlC>Ugenjc};PRyqkU zgXO+W&?jsRfBR!MQoFud=o6bnT1M}I&2M_K*`2Ss$as0?9F&ZEs@^gC;}@~;NH1JA z;{fTsFPk|GIE!3a1fD<KohA0xV3D&E$(_3WpnJ=OJf%J1VOs+VD>A9to>t=4sLz)y za;66>XjMw{UT&GP6PNKSOVIyoKjF$XyoV|7BEBz0tIuD@@x*5WE55Li-)Au#^e*MH z2S!imyMBZ}9gvB`>>Z$dK&5adA48w)J%NH?3tXyqn94Mn!?;;OE_D4Wx@T6ou+qJ} zN~$PP*q&`oudO<WThoMOgO?Nk)$tp-ZZ8k&<0JX`@&>R`VLDlK+W;@G`5@RORAWyD zUJz0xCo=2OD-21yhw7^?p^?vJ_}*Q$`rFZ6?DMKzetFM5sFd&wKfZV7>z4H5kL2IQ z&YPm=^I|d|9Xy?P5p9VzJflSScw4edVrI<CED3F&4`*vyeBk7cT9Qz|9IRty_<(2Y zsjpWdIhoQM2Yyp#4-6}-?l~CYk|p!#nUWwV-8dea8lp&TloduwL}NkPDSD`)2~G^s zVfOI@`RG<8JxtX2Z#T5q9Qk>C`QtR4_|gV?p0#G>!`HEa8TQb%*$U%VrBT_MQA{a- zCjrtSxY5s+jeL0uU*&8hS7vmILSP}Td~OY67P|1`25ZwgF;!$CMUX9Xh$c+-LS;dB zT4Ux-XQsrHRpl};r=UZ)@0BI)cAdqHK1nl~uP2y}p+a@SM5u25pjB-<!jJiwPhzyr z6CG4MnMYB#>gGj${Pg5c5LNzJFsf^auxQad{?@7sxNON*a(QfTzE-O*Q=fVeCbXHL zkL4Q9s7xEYmaYJKEhBg~MSz(r`mqasCA7G(lK84hW0j#YbZqm1!ig*BhqVR5b5DdM z@nwZ*E59|rCrzFAn4rl@%4DhyX0C?7%5hBEJBhbgQ7Qz*^I*NSfO+jUW`U~>Sy-Yr z!zOXjv{)QZFW-bA)6H?gvL?J4VS=~kCjgU9N3XGKVBmx^__TNxDYY?#T>n<OcTWqg zZdwW^ueJ*J+V`OTvnS#C00*LZ=q~xVX`@iOC04}gC_eL6I5g%-^2=Ut#OIBH)%KEW z`A)kN&|Bsk-QMjo+@7L{wzfU^Fc`|t4)w<6zAC(+^emWcPs94{H>s@V5^~^*NOOIR zF(*v4!ZoKApYAAw6Mc=L{QU}kQkXu;o^1rOJC@+Ji|5HeFIzY=Xd4Y19xA+A<<G91 z4Wi$j?$eNlbY7DclelvhykSrV>_4}}Q#<GYb6r0do;z-%?~k{`_(>p275GBg#~M<0 z_66=t*oI33K7f2$ER4(@3GX68A=+>{Zs{<_53(`v{NoY9V$F8gU+2v(DSQ>)e4>W2 zenIrv;yWZ<;VSpA%Z7|qtf2KlXUWGqL+P+9_h{?tS7g?;Opi_(2O8Cs#8R^(@K$~z z_qZSpl1E#!{JyJr_el>~`K!JJC-=s!Cw=*i=hK*;j}brR{&n!tpMrhFLNG8g2RiBx z3t|o)qqlW4$)i!-uw&j(YX9gM^yuY*9YQ&%xn+lOkH^F9<d~`#jo0Ae&8KAKtU&H` ztO_+RS4MZ!7sPj(H_U#!iyjUf1dU@>v5@F5{PeR*II%f|j!vn7ruTjLhmCP8@Ocsc zHhwppcvefFy;EjOW;V0dpwaAX$s3`(@he#1kWA-{84Y3mwvoJ~Ok8a@h_`>5h}N?w zu$u#o*{m6FaLdO9%x~Qzq2kSMWOAzvtBq14=HtY0Nkk_dJ6jzmjrHJ1c%}%WPdy_$ z-^_sRl~#O3bPZ@~THuDtbY`x8hiQqsvFp}IbVOT}W}a85^nUOG_473Nuc9rc#dmLE zX_*Fyg{tz$Gg8Q@H@?j2xE<o_TChFdhe>H|!Gu1YIPsY=e+*(t$tFj5woL(ES&G4q zb45hn>K%<>&16nCM<!Uz6P`S#h2iV%pm4_taw|y|wc|tJwxkDiS3QN&OWLas8asIG z&zAJeJSW4uJzNDzBQK!aw-A2i`@P`hox~1(mciu;)x<t}D{bq!4(~ksOl$5*;E=Pj zY_!Kwp&^OnVh0Q-4GB4zwZ9zNJkJp6(gS2u9@6(pGJMnO#U#6JrBL?idf~^yvCvyx zo!9Q&6L);>hV$)~k&8BdtZd&$lvpzbm!9m&tAFbv>am4<QgLtQeL0``Xx>2ed3LOc zTg|9^CO&GsPQDc>^1d@u;LuVd-g#h#Xsc##TpZ!SOUt*?{obeOoq_^fI=efWS{F`t zZPP~wX%`y&;wUY(F~cE!&R6OvEGNkiZjqYsFk(KwFOf*UNSC?xr`nKD(ihzn<tNqn zS>q+x!0xG@(@s|KiAhU56)P%HZ1MnJmkROB&Lfa@JcJI9Y9>p9PvF=mF4+6ke0oX2 z6lPA7gvo1P5XYSs7%4v<vI5IF*XPAl)o%y6|Fw)BaTvgi`m~al`;=h8)hEQlQXGRn zsepg1XxGh0BWxNym7g`@E^RqCjJT9_hk}YYY!e&B8eHy@JOjqsu3uzj21ZQIeG9ry zabeYY(tHI!7JVcAS@hdC{A-0^-Z!`(ZOl&LJ{(xcSEMxyw+)rS?A<GQn>J-Ov@I9j z-!{a1E!!a1>>Sm&o<qjeN038V$LV$bJuqu+9#!8U+Ld7wNTw#A7S6vppPtxcMrS<r z#@Rzm@O+P4VVs-|I>$(|>$|?N$bQfHhL;-bb5s?}H}B1i&8qO-+;(U_^;LLe_fQyd zI1b%UkL4S7_k^i^htn*lN^&c^57b7kg<;AXxWuy$Tszu|d$LUUF%P66o%d$FRrEos zwv|4#4PkhB5g4n8rfMl`$hwo;K_=rqoX|@qQv1WWqwh0#<xOsEeRc%zY#WAt@5=f3 zSYtN+`Z+GFD~0V)T*|+_7Rppt_|TRLJ9f!$J8L-a&d8Ke_;%8IE_dBijItbv5~>B{ z^Fsr?eftfp5o<$@+gjCn4+7Zr4O(~)uTVvQ2SN3o<yALc<>K7(>q1u>ad@rkNKa1h zjqzHRykXlgYBl09tqIJ+d%6=LYIr8j>0bfisezt@*WM$&T>G&&v;M5Ur+~+e14#ek zzNk32nr-#Tz&FzOh5or$X*Uu{g*_h%lG67IwayL0=AhGf;K~hrt7eSL3diB&ZhE+` zWdSkVy^giTS5dzDFqw1eA=+D9ghyG5FtS@KU9w;k1kT*ZW|j!JjRviPkIGUk#Vk{l znk#_jnN>s%a`;U%ucGk+Sw2QZmXv={;@xgE;pI_kI3YU~q|~BW=@vk6-$u$8gki>M zW9*Jy#3*lT#rgzow##58*w|;&j<{{O->R!>;+PjOw!9OAUOR$q(jJo33vqySUtm2} zkyNdE^6=seR9@u(@5Va{Z@sOcVp~%{E>(>ckd0*ghK&SO_f<c7w}l^8-isxU9>`iN zQhAjjQ^DCOoDTV%h3y)p^i|&1s)y0iY*5Tp()ZF?+T?Yf%3fTF*LIx1(Xm5uz@m{f z-Dn*Y%!@+P<C$FP>>hmgey=z^OKUJJE@FrJ^IZ9dATXNJ12zON7gp@_$7ApPV6JB= zJ(O;b^E-4P%O{Li*f$wWK1h4snSYp+Ja?%s^3(%U>s9<`Ce4G33QHK0&Ko{Gz~<kx zWd32Ya5erEQ5luO&a+KWDcUVLXkR}TYhuDT*VRGM%t#z=9R_2R_KQ-^rF5`#rEt!8 zHO^_qYYYxehuk=2(W_s5@ayCx&^r8rjx+4eU3}v~FOD8Tw&#@Nw>3jqN~|L`B&t;_ z<a>)=o6^8f<`o#H7)W%ce!?XyX7HLreVEaqXE<q_2K#i@9TWZTV8+36l9e+YlcmGq z=-D?^VNES2+ho(7BT{j3;}ZV$)j6PdPKBKdyUo?)7jU<ph*sImHi5Un0gAz*mpn3~ z;Dne53-CNi#p2J9>AN~`&P-Vrsoae%&MyUf$zGn;4^A<s?2j-tX$7VR`LNZ89^#VF zS@5=^ADfVLgnux4HO;;EfqE$iLBPl`K5|_j_G$i0%ICZCbL98qfa$kTxlamAJd{q< zE{+8&lT)aq+CV#Ml;Dbm0~@&`6{^ebaR(YV(99Gqcv*9-%FNybolQKc?1BtP&wm3i z`8)8<SAdbqs_gTbE*KIb$#012#w+ZR;|(+8*f0}$o?HHo2Rmgl>ZmR6IbkHp4_F9> z6>(&|q7M018;)OgX|inwGr{`Z3^;mCoy|Njm+#!F&8K;VV%O(wG%+Q}qhI0!GV_KE zUU=sO3p`&}oqcKxvj=41msxk{7_&8`_tVwbw0a9Jn(4@2)|cc}3)W!Pg+uJgyAXCA zvRTBK)4cnreHbjem2GLWVI!m0vTBhg-?nYU*Q@ey?;%IrEVB&8E?WmEdL?Z^cTL#y z`5T_=@WYYb6hjLX;C9#yJ~`4$II&0t{Vp%%ofKL)Nv}6_<fF&*+!uW)SsRa<K}YGv z@5;gr@fV4O^<z3fn9O_5?19{;vy`0Dsjdk*gT*;we2M*faGFH8*tIKYYv>ksC+jLZ za_241DBcg@B}TA!?HxuR<l$>G2{`^pg(WYO<Z=wA<H2b>jJc+YACgyK;=V1cd$BBb zOg~FPL^}a)%9}v%f#zJHVn3>Sc@s=FPUMXZOsFzwVupq?EDE<`4&zezGjlJ|b$-&G z3tw7SKkZ}0zV-2Em(RR|xKG1q9B0q+d`ek!crH8PSH`}q9?QB_y~57}??RkdFD&RD z%F130V`E24FbgL~Y|Ougf(%irE%7iewAjmrM2#g;EsC&dzyvl?UDScS)5H5JsiY~X z1qSZ0;74w4r2gMjASkg1pK&~iPid`ZD-_3JrDZryx|E8A9}bFM0rTKz>91#&RG*!2 z?hRpn8lLfI6g@pM-|!Q@dhz~y)cFzLocMJPQTTeJDyd05LFSH2f`%XoK6#Ru=ghBT z_=7{rK}GT;CcOGe7cIEVd3(FCC*8zIh^hd%p7)7${Ap4Ae2r|pvY0%*Tu072pMi%> zTQKhHh5tj>oB!n$ef|FpXcm=<MyW_NXi!~e?~^h#kS0_JnPtpO(VWtpO43Y&q*6-P zb@tjNL?SYzh$a~$q0GM5{rx=d$9@0szW;;sIKw_`t=H@ITyR-cgDSYJg6$qN>A8=} zTu|r<IP)<Ns^aER`=xrQgg$_IXf65m^(r>go5~9v+Cx;GKY-b<skH6-W&B?05!57J zfyM`8sja*UJAGv|o+YLQ|526wpE<~XLG+vs`B|Ps+jfd!ZIfgs<6s8aS8R)`^rhfK z>JH}Vh0kPavMo6J1aq|+f008<H?iH)#3_1NLeH5ZVi&XkU6C{--a9gJ+4vT+WJMw7 z=Tda?PMdJN6TqlQPs54#t+}HX`xq;}7`pXDf<17t7$?71g#Sa8{qM{BU#e{0?C*S$ zjGH#-rU(DmSS`NNV{e|}*~K=0=2r`rHOBECeGInt&z)eCJy2uwPhLe}kgLXjkBbFQ zW*)G)dDoY>aGQvAMP?d5;AZR9K=jroGiVxb&K}lg%jb{0$+>aXKX;rETu!jD8JVld zbKVlmOF0y4V-$6spL=_`;8;<W&2#0q0+EyI0vYuN{?0mOo|COE7nUH;L|vZ56R9z@ z`gD=c>-j4ukoXtIdzLK2*<aw>^d%qUm3Y|N+}13$e%eyNJGxr@YVL|-ybG<FyuR7P zJQ-erP3Lb(&Nsi3|C%io)Fo%~2I9SKxM)9KY`UUNY2qZ}xlj}|E4sL?FHC?mWT4E& za(HdoMA)Aki=MgXa;1NcF@lvM_}}ywXtzlO`Esy@OWl-1PTgI^1q67YQmqT<mvRru z4{0YC4rZW>t;wM9+K>R6ZqrqMh!b!dz;==(TsMvuTp1&Z?NS^-JE8{NcykxU2^HyM zwOVkADIv30oFV_p<mjxFOAy-s9amP@qGN(_D5!Q7)!BQT9skLgh)FboNsuNxxFU+2 zHZ%wE+^<+uIP$U96Js1A&cON<mWr*;go-`oI7lIldsUQ1Wd0=Jh@sO&S;U6@^jMPp z8}J5NigythUk1m&p1^$L8*qIjl3AK8P3Ib(gSXpk@QWG;s3vkSdORJ!kqxGkf1U;H zO^fl1W)Cd$^dIc}IDy^PY{|M>oagh!!mwD(Hd>_j1_@1n=&+wNZJRxX^~)Lo(<m`q zw#Ew2-#CkwJqyNl8!G4u#Yb4JJObm*SLlk@cVSe*40P{JAR*hw)AKr|C~xvwhz%%3 zH(qGt7xs}f&2%SrQ3|6btMg&;;z@YXF;iN)Nr`HHQAUA(LhxDZ5O#M$6J%_806)Ao zVnKi|n_?(Rn|%_&Th@y_Y6xdmEUm$^E(oj^=R@-=AH00REhrY2_XZJj*|MycNX)*L zglg>osi8gWh8}{iHPph{9ct{&2XRFIrW|bEQ;C1>6l2*`0n9*)VMNZ1dao6lF;&!Y zxPJ$6(|b-XGvRoa$ziVFREr%Qvlzc@m!b<CFEU^J!=Qgc0oXkd1|*h^V>c%b!Ou1N z_)}UUmT370Dv-@yyw-vt%K|nR`+#YAH=~&N8OI1d!x!z_xOwXweCfbinu=7|%;c4{ z)2|c^rRC_5crtBwE5Ta(r0Lf=A^7%rAx)w@7fko8U}Z0Avdb>Tu~ilGSikdU@T@uS zxZxk3pkkSXC7(MWtBvE>zxFfPI~V-1go`%TFpPt%k4I2Xwgg`6wgS>_<>KM_Ze&r^ z5!e~qi)Y+$!pm}3K;hEmY=vk%Yy74I@2HZexyPQdPG#P_xyjl5{nyqBd^B76rQzv< z<vm)w;h)Vm>anH*opeur{KC+>>hw&T)%&z~iH<k;URM_Jeono|f6!Ya@U`1)WtN9| z!=JTywL8xU200U+(&1>Ep0U#0IjdHiTZ_AS8Oy)(wRJ>m)-5U(<hCXVUMZOIo~ah{ zCW}uN-1EG^zgc#L@82&ah!GRzHBM>ZWzYh_x;-|6b?Yx%dHWXdBPxw~zwk-{&Xr?U zFH#kZZ<OX{8m*{PIk}!!FYG@HAHNgSo=N1%kr{$jYA#%uw;%6rf<3eA<9+_<r%Swn z)!JP39Zs-xvn}uK+Ofze)__dvo&^k>K+N6uLEN!-=zPK~VQRktsr@*J5_>(-l*_&7 z+x1dpcGVsQH%LPQuHqdER)(ZinxuT(Mbxou86NSvO=L2^p{GAZape1juuyQ7bI^5T z_TSfm%5*7+pD6<O(-KMJv~(oZse*M5`e1&KA?Ce0h@2hn6T5#mh}Se9BCjKf^S{P$ znX=kYp>2+n*G0gl_}3_Dn@|bQY!Z&w*I@7aDagN34ij^6ZnEPflrZZe$McBC->;Qo zy<t@}q@V)-Vj8%OH!!RkD{No1B&pGaBe+m=6w%-*_;6eeX+Kj=ir3l^t348^^4Aj* zq^v;aPn-<DnQ^$d@EN*T_7@jFa>f;fNu(zvni$D+p&}m)8xp=Uop)a%DscyQswFUW zS{h(s-GZiXn#u*_q%vM28c<7~ktM4`$Y;}uAn9ob-bYnX<y0PU8!Clmc{-k}YlH7? zG-D>5X~4O^rLg-z2wt`EFxT_zKDjnN0&*(HK-yPD_>-lG6hyDXhBuX@ZR#TAud4`q zKdz$QKAQB+_RaY2nMuU`@@cZB(T6Na&cPZhML_1%2v(gE4u;eF(0Ac;GcWqi?VWoJ zaQ85?%1Df9PHM1O^_eA6cViivS}ypSQcM))ck#CzMuKtcEpR~j51y%&&>54YgsNPl z(3}hzMzc$TbZ);P&^fgjcgaMOg<<Cyx>MK|9MvaH{3tZq(ZKxrx|0c!@8ln{Yb5*T zogi-$6VZ~ZhnV0;*O~q$4umWiBr6ubWa18GkYtfOv|{!R&gkrX68zqf)Ni{$Y`<lY zM{=u)l$R2+o)UygV}${wvnKe1#u=pF{gO0Xy3I9N+!MspK~DU#Bj<a`8BJ?3B!`aN z;K}y4GWRM@66HtN@X|gWtg_R`dvBfPYmbda(Rm0f_Un?H1)W&5O$0j6dtndvZOA$K zI6?s#Op0|Knxk(_;)Uwu{L>~d5*bL;1-l^Fq>^mk>x7@}@kA2;q>*6$TjuQWS)}6C zLAJT<MlMH|l9emxlVj;uQI=Z?DoOzIch(7FeW9B9q)K^qgL>R5ZB;Z?aF!TV8*`bD z-jGLGrJRgQC4Ynw!@kDl{Ez~57+%mN2ykjb=hnX$3?0uB#M#(#fAuHe^!1bzAE{tm z%kDEb{xosecRD(p=g2#}Yb+C$(`mD@cM^HM*BS@y>*e%bg`!5sMd;e`f9T5;9W=Ob z9DLRo!$q{6WDaB>Cb23~+*X4g#%IPL33N*#+rlQ{ypay#J4Fq3cJ=e>t~a2v#9ZFy zID2$eGl6NUTZ$?!Oh=j1uke@p#}bduYGQWWi=6S!<y>RTID>R$qHFS;DbTba0bMTS zX7pBw9L*;wZFaaIvJo?RI|RG$reK97xt!k}A@O8jHkMF)jtqOuz-h?}{Iz-lP`f?M zaVJffcmF=~c~%TbU-OXD<|z65q=d}Z@nhQ6mGCj)Xmea~82Y<Q8L9tR&BaX1N7=c} z$h*;(=vf7jldsB9TILMy^yy^g%TYgM!8^g-ILN@pf1Y@sm=dJy@+3+#T)>(?4xY1* zm_-@gh-%-!rB|)c(KE++v5Y9!QDg{6@gutT;S_)8_1#$1IGzMe^dzeHU!f57X>_a0 zSF}NYJeZV!N5;o}8T07v_}BI#QdAPbO_~`FN+KJe$?+2WnyQ13t_(-kzdxXHS@*Gc zhCKY{BFUh+FG(<WgC&DH_;k&5+?FOBfBSEMY40=8(#HX4;qV?bqH`8jcWUCX!}hT4 z%@#aaR2%Ji)B(u_@i=!;280Y{f%N?rP^%VWe~T<c_l^|e()Za&;ow}>)lU*?dD)Ek z=gC;DrU!at`;oJ688Q4EigY)eKobv6Cyke);8TMb>pO=*+2gXg$rkP8QD_o*t2dpw zx#%WX|Lj4p17_Il5z-~HtgYGqsLK9No&Eo}0c*?tToatEJivT<eIJfrcn%Ssrd*MK z8S~*-Dvaq!Aa52I;BVbN41!2r+3j1fN1+KVwVsN-9Zr#im*2THlf;SI`Yi6w@Il6K z_$qhdQ4Z5}yq@rPdtjR@)Bi73_P=*Z{4Z5D^G^b@|Ax`X*)Oo5MI3*Ayb@(6^a!5s zv_h+-eDN2Rg*c$Z7MC}>F@p5P*lpcI@DKE54^C`iAAa1(zIx(B?aklQ_|OS-;9ox5 z`(YfLvD$_9bUz~fWEV}EuoCLMDcU>fDgJh+NSFsxAf?+yVb+QicryPSj4pgbUc8<| zr@7>y>`TdT@0KHUzMTuHF9H~rXadK`2(u+B<fDTU`cZ*k*iVLxdD%v88Epj7p|McJ z#zD8N1g<zRjpn(;(-S*BunF2XX^&e3erE6mc>hk)C&U?>PTGnigU^9mpEP|fJC&7c zFr^}!eCXb;&yZQFkIlwL(cu$9^*HPvNX<S<Z%JoB-31fWSL=)4ZrzU0%yoh)Q+M<* zO$O`NkHb&<i@Aw?S!7C~4-TEXiE&=8hP2}l8TeTZ6^=6Wwm|~??R2vp$PA+j-s&{w zMIPPi`i_Q5Y@nasn&Fji;^>^HP&VM}7<MC;!L99G$ZEC<%XW#-4QdTg6Fm=CgvwE8 zp*f(XU<=#(z#QiGG-352MclgN6ntI%16P!bV;jFB(nu$fY+VL@y?L5czv>_fQ5TTx zfG3j`qK=n^6~S-V%`WNKLY*8lZRI<*g3s78x>QS=YPBV@^|f-;!ng%*+_@P3sJc+q z>P<AiYaIO{SAtF_4-)I^7=lM$k|USI=_joZIPM`PIVOK0FFGF7!e_9_JI8{|8&f#_ zKnGt=JxufrgocaEHe}JK3$}f+<T09#7k#0`V4xKJ>Bt~9X2I;)ql(D)>2Wf1+>&~~ zhFJJkJ-6Px(;F4EjbVGtPU2<mo9U#O!`MM86tcEmL=hjJL)GSu_}OSNvhxZizoRE0 zc5e#N#p7Ak-gs<teFGjpIuVvVX2G=e0otMh;AGqhP5G1XpKG)5qU<r~<xL}4B<_ih ziOS%gyT-FFcB|<lH;ft$YRRTe(%|8fM;yL<f`6Tr>>cJLJ7vrpZhUwQ+BxKe|2$fb zUrnl@S7(TTXy;WtEH?zABAcOSb1AO!+K)4i<%5=cKGGKnBGJ0%!S{Y79=WRnD$6fp zXDor&>@8tC&;J0ynlpmr>P2|_jRYv4mJi~>lG|yv4)(pdmA$Jzm(~2)MV88DW1XrX zS~+|II+pp+!%c~7O3O{`uOz^u$p!TJs;{88su$g`Yr?5(_p>J9o7u4HIjr*dB{=*@ zAetn9h0L;OgthyH(6_5bY{1Ael>8%uy;fwx4*IRe%4G#4DfKeeEh+_eZxX3g)&iHt z2`K7aBp0+(SYDP$VeO&`c*)pCj@UV)x${F=hxN(qzEzgksbVc9n^kCvfeZU#Y7E<T z{4{=?{1{)B$DBMCunSr$>9t=OP?)uZ@mM|wvc;9?I<69Li#CPjK1K9&TLXT3T8uT# z2!)_0RRs0Bla#-0oS;_tPV7C(8qf$dV;~5L9qUBhkIKm`**#=hd@Wi~R*Ky`;vnqF zKVs~ZL1(p_qdjr1M5V>u_VYRi+@UU7zvqiQJ?GVji=tP8@`4z|_!-yNp;R_ztP`Gj zE{4_$&zdWTMDYBcad?x>MQria7+m8o5!F67?nKdAe5J{iaxz<KVSzbB?_7YLV_le8 z13%G^RoTS;_fH0Yo6GHZ*3CcExtL5_?9HkAh(a7vL>IR$058ujIB?NZSRL2bmvufS z{SLwGvZ>cmhf)E)^KuI9ZY;tvi|vqKi98dVUPG+6zs6thsbe?ESQwvpfVlnDBq#1= zk%!+FF|wNRFu83yOrG-v%T4=-g&|<%y1<T>d72^C>>HAh`GoxLg=3Z3$AsVFMN~R> z5DoWlp%we1q4~xj$y*+QZ?%4bxkplw#(P<6;WLGvH2*J^zy;q98VghE_1XJ1*)}Q_ zm`$ED%+B%>x@=D6aNY@>u>Hd*iaFzmrsw_Q40BS+$ZspE^*{;7RtO<JeKVn=XNV}y zvn07ad34VDF8EZaiFB?_hN6)ZxcG1;4sE)PWL)K7=c@s1W8H+j?Fq6jn87Z2cm<}} zC!%cKa(1?~2-xjyKrc=C_*_E=IIZ6S6Sb$YUv%EGTT&C)_jfNcl1naf|LV>Wu4O-) zJCMt|KDB2p)|cT0n<Y?`_G{cZ+X|YeY~*g2b%J(JAo7_XhHMJQ<0RGXc-M&s#J*R9 zyX8$e!M8hP`3_wW4EB=gQ`F(XetCS=xfbqd#KEoa#;`@fr~YRC4|YwLJaE48$obCz z^k`_%^E0EV-sn~c*?*e6c;bkx#<rm0!1b_ey+5G1G0?DXB{F}sgbZ|^fy=QmaIrrd z%*2yG!~G{|KA?&hoQtIuS1CAcnhCob<)C+(CJxTa!;eniBZoYqYOl`{C9Y;6xV791 z9pSZudz=>QUOs~qg}AcSMU(Ie?Odu=tc_kgPC^QfrD(!CaTaac4ht-$>G9hw=#8;8 zG)KhY_e+1kk>Voq@5F6Prp=*SDz3o2>8D`7VJP18+JarCyB<awdyv|RD)vp21KaRR zji^{yqMZ#(P`psxUwk-Jm>o96&=^d|3{^png&o`rF^7(i7a)7w5c&{UhqLR))w@nl zW{>BehKY-PIIaEitjpd4yr3u+c25WPc2NS;U%ic1Zjz-ZuAc_~^UBPk*8^B*^gS7~ z$&}6Admer3J%PVZd<MQakXNHPjSG_-!eqx()@E%f!7Eo2=bSXqpP>oYpD%^sxo$)) zQx5m66OL`to}mG29qOzoNqOCFct%7Som^H(WrI)hudLuvyQdoTnf^7rzMSADW{{>| z8Dy75m6CARLSj(A7;4TXv;Bc0^uo+N;98a|RBZL3bG-$P-jISlLVjR()l0;xFo5ZJ zHkXbhi{e`4Gu)-M*0|43$Ov=&#cbGol6)9FPd>?H;uvNS2L7z&*@u}>a|a3b&C@{e zTi?uHdwv#IZ%bnHG}Ng4Yg^nrHVFTV$$?>I71}j$iT>SDf>x!Ap%Jo(xU30>r>W}n zo5DrB`C$ZHJ0Za=tI3A6>2KkgYZc_0#sP1v6iIoJhyxZH;jx1c$xpW<MEAxaSR%~Z zHp}OrfmN00{;U&F6WatgC!HkYf9tSv@hjOLnd8t??`+%UAIu&F9vgajE<4LNl)c#b z4>sy$z!d{&w(Iv}>Rnj~lMgL|dEK|sAK$gq<wFfl-nbFZ=#8Tz>q_wR$Xw=v*?VyH zp9sQZ1TA<!4vT%bjCrQd1o>xAkrJgSrm^z}SGQ+7rZ!iQU;0k;bb>zBZ?R=o6ibt= z$7zhC^%Cm+P=;CxGFj18ne5TpDmrFMHklgRMaA0>)6ddZArt$tY4tJetN9`9?Iruz z=+;Hxh|gl5v$I(ySXKyuy1@kr&(gw>wS>z`WL*aC;3-DkxM>fII#QybL^%gv-`mA3 zO$_5^cZi^%Z8>CQycD$GIl@)_^@R2k4UBhq!x*7cYT3mc9H4WByPwc4bOP{Ea%?yI z@#gq?kN5u|LOcqsSJ_4!19Y(UO?jGiLWEBGIEaonUBbO91EFp4R{YL91o(F`q)Ph1 zkk?`qHcN~idA1Apf37D2|7$4r`%=6{_7h%EKgwCQ&B59B<3VllPN+Y2iil=4qwVL^ z;Y(gRr1@o&A+!~ac1~r#UrNTU`z7mhts;T>-9t~h*}(A+_4q>A8633cHLAP#2HTvM z1zfs>O2sE(2|5QaRj{Nv+g~zBS(^Q1be6PTt-xe>CyXZfQ8jXm46PcE3^q&PR~yff zU+VjqS)SSezpQXf)?Kb-nH|!)BMp@o*5VlceWoK$7DU%-fXnA~)a6MyjTl3)%lZTL zZKY!MmnY9=qs-RP(Mgm(qJh8_xUk!0Oz81%1-N|eKU75iSl`s$50%l1?Bk=Q@TF=j zV}AV;3i^}<%I^Lsc3~V|`sE(Xe<+W87c}w`L!{9B{a?^R-A`mkyad)c-B0#qy+Cf( zbAhjIL`271!HWIyoKfKq#J$i%5AR#hnP>w|g!^oa>b&~ih36@gKZb37BLpo;{sXDz zO;kVrA-1zLViT5zV~aT-@!O77Ah-1&Ndp9bD&L^E<ni!g{|93AJ0H3BeIc7|*9)?0 zmEp&+nb7!j3qCr13j5pT2hq=zh3p4~yaV_)qxkd_(Jp?Aw*F$VXw^K>UHKll98QEd zsRH(bOg-(LwiLqVHqfe-6KtIy9|HYblr?$K2xLkds(fEYD;5^eO;eWABwu6t^3D$Y zTT-p==+l+pP&<r_^7oRR!V$No^9<rXph^Qz4#0W;dQy7y7dcpINMpKg!P=vmuuf%= z;O{R;`k)&*ZZ3+gBbBh~YdJQ;@*&>mtc6#-)ug*){AqQ^J&;>aSHEU3lLjmt!!*t| zr0!9U^!bmkc=ca%EcK|GGGUUGt(ea`3I7-K1190rj+rbvHHKDo@Py8u6RhRz7MMNg z2<defvC0@lIJxOD>CQJ|zJ2mT2I(^N(Yt>F^^Xq`U)@;fz(Zu{`xXecN@W&>IpTpW zrm*`|1qw{+1g<I@R8t+P?V}U*+h3)yub$tcQb!ijiHA!`%${U4QRrOoUZzbuwbVc+ zLYnn&yuzqEt;R(L+33yrDBN*lHbj~%XN6)VT>54X!C$sw{dt<$>|q9y+EvE<8@WSH zf7hnn;?uBR&TrCJ7*EzTpAv%UYSGf2kJ0Q2stnqYiQkET<jU$Nu?Lji!8EmH^sbyf zZF9|M2hw`kwL1-2yX|gN=J8?rcalF#J=eg6Gh6Y!vIrXA`X1ZwSx4KaUu8ArJ*a+G zB3!UCV!i*~!E%Rtpre|=q4SxTW>sRBnF3~ERTYS+WzoNvKQk#VHu!DPFJ{t^9g~&p zidw9XptAZn+^RZ{omXPU2~O-K?Pq22mQE}9bKKCj`t?idBKH@&eko@*m$cx`Io)JY zNCDBaGotbh!8lHC7{6+2BPOTUfyHY(n(m>;zU3K0{A4NmV&xc?IVy%9UOa;r-cX=D zbxz1wtDBezM=c>iRbZwf2|V3iB2{>SG<n@8GYxu4Q}<<*xlfKQ9_29aj0U7kQDDiD z18nuD3uL}VFFT;UmzK>^VWVHGvyb~d+2wARu%v7vCi_2QlM|x#{74p(Wbc8(%4?`j zBMF*j<-&$}T8v)<Lyw%jf(yGMaDqM`x{9AdD`yXmo8tr>(&w3iZ2~4Ksh9K}?&FmA zx{!go$wH7~CvnfdiGB&$H(sCe;Mt@FkoEHj4l9trw-i5fE=@^n)pcI|oFr9<3rQ#E zg`OC$`6YC}a7T+iPoUQhZ3m|vTk!c}H`2ABfXI%@!~QsD-t7~?$d((2z0@y5+OjMd zHCM+PoqR}lHN^i>mHnSN$p2GiQ!mW{i3j0m%V4`;p3pMA{Ms$l>zB^_df5oLBLEg| z*^G>JJ}||uLD)=KJS?g>g?=2niG3_gNzBMAZnv!xp7QO9!1uls?mpv&_dDD}U$Z`O zt9J^UKyZVv3dXpzOcyr9pG3d7cnDv?M~@?fWYqthmHqF_`(LVT?XGc9c$|-4K4ih6 z^t|Bnyu~=6)&S2}Q)ItgxeT&a671vgO8nT+R=lTl1g!g=AZe46@OS!?`_vsSNI2-q zrokz=$~%ko-4C;BcXFU;Sw3Fkxt2y$Ng#W<b@1`+8)lnE3X;!lMH(WDK;?NVoKMch zew&l<2ICO~ep2vt-9A<<RSY(sO`*4?cd|QY?!wkvjd&Jq6?AgtFy#3vu*YwN;(%+1 z@u0NOn4haf_eEyF3-ea)$67vO8oQWJ^L}BGa($LYB|&Se1N*spEc<SaI?f9yLM4ai z(*5dvP;1;lR9oY)VXQNq7bwHpz0$JTHSim<reN5=U7X$P90fz$o!DDa>#+`xk8AE^ z<AsV&cspN`-d0;qLpJ&|&n-jQ+@HJIDR1>b`#p!gsVtzCjptxwTOZEe{|9@h_~Lyh zUD;3CtEr!SFihJ*&_Q!w!MXfDHmUEg+8p0n%71UMohKh}CU~*bQZUK?*xVCg#{5NZ zwXA~WV)?(fPZj)GY9NR^mSAI=EW>Lkoy0S9e8fAKmcu_l90j4)5rRYOX`PHv>5l$; zr>>ySo*&dH!oUB0B7cH?zs*y-7@o?h7QwLPFRKURqIiXug9YflmSEw{$ht$#mjn%$ zY<RknwmjR8V!qq2?Y!MvWq9hp1pIf;n|Z9qbKX1c?>37T=n5W42MeaPEaj$3mDzNq zUgSNnn!(^#cGd}fvHZeQiURR#R@N5xr1=w{Pq5Ks{R9UM8U(e!ESQUB8+g~X_0dta zSb<s68I;w*5@+8jLQF{ns*CqU?28wIOwl5;$?iM%va5zM7(GR1thz<ajWwXDzLjy2 zwj`$mrI75qS4@h{ck3lC<B9U|+1T`;8##B#9KG#VKr&|2(d-)^dG_1(^ZIrxqhH&V zdA0?U;HdW${?ztug3i6+{IRM1HuL#^`G*@~_-9*8_!EM<`G=p2^PcQ2<jtNZZgcOk zGUGB%l=09H<pnA!3#`s-+N6EDFJQ{Y3Z@=h%nQ8s%%*wgIeta|A?wK%3k7>U>+2$e zLIi#8B7$<L<kyP(2rjqv@bqq8;l0qQvuRjU#@p!VZ_})@P>}c{nrE4#z`vWPV7;{O zuFa_F4#CeS5}bUd17BB|<L#^2&Y7n?6sQEb@nRbuT8}F{#57&A<+VP`5Qs@;3cfm| zS<V)HTKC+dkr&h?W3zmD9M59?S>BQ#dw8x>%6P{%F5xCCP2_SXN}?zI{!EZo4a$pK zEKK5ilGa59NcC?Sn%c9A@viwpGy@-#ea4fJW<3L=#vjPz9tRlJ&_jvyP7`gLPs{^7 zcjk3_7uRmRfv6^UV^{A(#PT-}dF(Jj{wH%$noyBc4Yy>jDw(4xA?LU^qVr&;)^&VB zFAoRCrV)kTQ`oR&k8xj&JpL*z!fyRA0|t1;>?z-wkYarYhs-g6FMNOe+y6N-G5Uha zby|_ms5yJ`xEao|@5T2#JXp))orH#|qLrm*;9aDUdEYZv$mFiUCNn)zQ|A?up}ZPa z6g`FX&~em#XeTnZ9Ea6z2hzt6B8mHKJ)$rzhR*ZDY+f;CVE;@ucxD6*|4|G|eUh-b zvIzZ2lw?;$55RZ1b_goDipGeg0YB|89`bhMebt-Aj=gaUrflAXUS6BUj!}{%N%dto z-tGz7>6r*TdK|9dKA5UC1zW!nVPA*&;#-C#c)8<2eCFU(+H&*+Z7#cqJ&Mv%dbNOA zE&QFY`5uEtd7e1rvme{FXczmvsQ`P3I-yyLv)J3)197&EDn0)EHFi8_!@}5Me6;^8 zR0s9qn!z&by(E)ucXno{|M`f$3M&~^yEw#um_n4KZgayG>iG4&XM*j^@_6}LvzeDB zRyIkU#b{YTG!J{{bIx_)+{XEh%o6Qq+@b7F-jy4b<cQQ9^2@E9_tQ+Acqy(y?@#LC z<MS?a^Tv;5?yt*ZCY_jpb`+?Pew8OS7pBB;WI;bCkgX@16yBmM#WPWceGb`ED+D4s zhYGxx&fz{uw=iC*8NA@XQs~@AOWwe%M=0raCue`Gn9R=?XL8<GGxe9Ab9OC(%sti( zP5xqr=KK}D|94N3p}-XKLl)45*OAOlzhTrF^PK6`l_P5#3Q@D`W)k+x3xC8W@b|}C zqILWSGTM~N3s4%*(KL<;h&s!89$}dJfdQUg-UET>es##@sX~|fc>K8WGnb?<LH0)# zan-wfiSq61WMtoU!C}pArhIKL68{#4i;q}B@Zu}HeV3!ba$_~;KK2OGes2jmYo6hp z=))vJ-kaEMJkK<X#h?XOHV{jvB_#2lfVal$6gHkOj>|_la=Rv$Ii|c8WUM(sRFw#u z(Y_ERDipzAPRKPre+`#}F9dPzDAcBshpp1n$+}Gqyg72U#CmTMRz97^Xw){NOU`M; zSm>?#QZ9|7J}E$MRXaLt6hN~2vxsw@H_~dV#4C-qql~y9@Z@3gqEZrk2HXjeUIFCo z;<^{?S$yT^Sgf-?7#|zBgoC^@L9TEGIc9s4T+~^>-OQIDxn7ZcrBC79_y(bod5@5y zyPg66?|k^J>x%ac9w*MnLvd!p7&xb<hIidxLD+fs$hVAz^!}POV&IvLw<ueX9O<dJ zzd9M;wm%I+nhxkYKMG<T{K40_h2%%w2en>n=H5ydGCru32r^fZ_Mv>dOZx)D-*O1s zzP-q(4y&+K=yo~Nwt#i5?&lfaI7AFb7@U$LiyKd;bEl)Vu&2EdVibf%)#iAzyyq5{ z4sJsB7bKxyX@m@DM*z9EiW|rX$4z6Cnb}%p@PIcHu5@SN?=?@k;f!C%UtJ5@C5HKN zIlkz7`4g0;`x4Km8=-7!J!)+);-WJ}=tL)F(q7R;tm|~KMUF7*Ywkyc<a&50;g7|A z<UzdhA{38&N0-GMAjsnw7HFS@%I(Ln=(9{*Bbg3XokeK)a{+9<I1a7SEg+p61nA)B zOzwBDBGgF=m^9mVH0*zikvc98MKeN~7Y~QgT~WaQ)+rE6Au}Lug$`qus189lX2M<e z6W94c*qU=!vE`sPR(rw0?cM_PJm?C*(_vzgF&VrzClTu>)7kz0gXry-Hd1io9_*j> z8;!lY5EiVk0{`82@D&vvujYV0{v~!9z8ihVT{<_(%Bo}VYVI8rf5{!^332U(E7p*C zPh&`bK{3gAbOvn;`@pk`*u%(tH$=}=Z*!}r&&7LFDwvXntIX#kl34Fm8V@(S@~Z8} zatiga+zR#@_x+R>sZe%Af1~FStuKF2iHQYy^3Vx+a4eH~%ARQSnlW6!AamhNGr!E@ zHQ%MyOAz5WMR2D!9Q{fjCa=DW2}3BZT$I8ju3};X>XCXPAYK8eu(*Mj>|<;7qfY{R zSBRmZE_H5A?sLX`jyzLb(n<9E?r;%{`j|G=%gC$j4>xvM0GzY%Ca$w3k<=M^c$9gE zSrrt?{QTihEc%*IPrDxat&PY8=V_q6MgdP1A{C83TcAfTGMI_ie-q8a6S#lN^O$^v za)HgOEvU%v6tp~PBX_>eMX%M&$UV<2qMF{0{M)WE8&2Ovk7`aq!^Iry?%;peV52ym zB|n~7?=SRN_N=x!p>ECjFXN*!5oi1#RoVZkv;UteyMMU{@UO20n5e+^j^Xh>*6reM zO0J}?Kk~S)wKAw&*MO?7jUbA%y~(_#6<Fq01>fQHdrqz3HrKI4mP<V+j|<>Bx~l(@ z^gZ-K7Im{>rFad;FMP+frd%ZdM^^T~m(%|*RkmYMGkbkNpSWnG5%*J;a7ML|^jgir zSz}Ie*<u`dyQ>O+-n0uHnqI~zl`DYs=hHOl*EDwAsr7jJA8EE;KB4|g_IWnDY7Q-5 zI<~%3V<lexN}V0<(4{|qJK8E3rn1|vrm-=`o0)S_BdpDa5SlP~FO}OYq&-zGptCF% zkcxB}TjkUZFnzNKPQ8(c-;^se+0TMd)}AOvW=SWLH7}Y;b9sQq_sD>M%sk?|vH;l* zb)pu9>w@14U*R<u&1vD}sVH}n9Gh-82piVMvZ2%6@ZNzRBy`OUytX+Qw|R4T`>rbX z=cOFHclrrZxZ*OFw4Bd^rW{+%F(m%@a=fG^hBP86_RvvDxEp#MU%G2aG<Q#;b;6_M z{fpn|+TVHPzevNwTgJlsDIyR)Q53)63y{qOcbgjCV@^$4=+2qCncX%q3N2Z11-@R- z!?TZ{Vtq<u*cDbXwl7zhk*h~U*tc6Jvbmz2>@v$g(2OKmU0ER!^tS>bZRV`ZwR8AV z=r(-iWGbj1)~2rebg3hjK}Sayu?=I2Aic<vUD$Mpnc(A&PX+HL5t9?3=fPY!_(Tq$ z|0{}jZe>|<?k#gCO9v-E&>|<YPU5Q>71(BVBKSnbfR+3{Hg;tezVUoKKJisZDSdf{ zOt}A)S+P7AzgqSS@0b>XYr7Y+sax}i$Ru%k-g*enGXI4dt5uj(kA2u&TS!C<d<cgu zEkLrt6rbpf$2%8D;S1lw$dPyh=9%LO9JEOiuQ-$_DB1lHsjaj`8+S<I{g+fo?n6V! zy09Im`82US-!6Pkq>|PZ+0~0(;IT&@yVHp0sjPQs3DwHDg70iy!@6VyL&^w&zQHCz zfzZ%7;bkfPx#bmGIa0+w8uYBs2)4s9mN9fs+H_iQV5jX}0yI`Jj#lP~ft|BF_Piht z3vTTdQj6=j?EVEHb>|oANY%%^lA*-p`bmZp=EFYB90r{rTef~e2s~I8Z5z7a1l_w^ z!uFhA4|p49v)?=n>9w9Eta6z$O%d$}9oKVoBvOvHZTk$r9Jb-^^{Q0;TMU=uwGe+e z`4YE&?g1668f-tSix~B9!b0dEZ5DY(P8P{SqPY*QOOQu1yti=Yj>&@Tt4~P6H36Lx z*Mc>UdZgAl9T^DAX7;!!dpP?XdUnsEel$|bwymm(O&82$ca&$c>O5)s@s%oTI5U%- zd}24WyH6%{4()>SgevqweF#2`+eIW5hp>XK7ri}yDja>Ih~lrEgOvJV>?d;_x;ty} z1fwp-zS<Y;PYt3}zBXDUR)Z3!4s%wbr-<^=TF#<WAGD&rkxO5{qg85+SRR_l;$7FF zw%}xa+m|lfQ*xO_>r=@3mC0PmK?e|sVRrMRJh*>n3{BmX1sgj~3fDpXCRtq#5a;j; z?RqZ@PlL7yRj>*iQT>LbKR=DhRpG?2ItG2q8Vf6jS@7Zyqw^bDNPzZu{M<_uCL}SS zaH*U;UR_6;We1RH)(F|ZO&JN>bZ{w~MngxUsb%*V+pP)3pgJ<b&f7MTy_g_e4HWeX zt_y<je2L%iVJpMlxSqo^O?!`)EN_7K9dGahUs*bnS0&i%P>XH1JSF>gd;-I!8shvl z1Vmpn!#yKqdT<J#JWO}Me>>lzeU)Z7F7lEPeNl((3#Q{qGVSErY87t!Mi!ZEQDYQ+ zTG_#I!K~fJD%N1bU-s_H)inB9DXV!~!FCLL4o|!;M+I)-Y_Cj^t&c-D{ViPQR(G&j z=<EMYtl|{lW8YbDa`eS}bNe`*o>aDaUodSsmdE108L)oX2x;ce7Oo5X$oNmYO!5x? zCAVa^6B(0QvZ!x>dHzrf=kmNsZcZrAbMTuW=)rXO(XC5ools}bzK^1T*3arUh`qpJ z)<+1b+e3@YLfDCEy0ph5A1}T&N>83qK=saDpq1gu#+vWPDxratnIS@N%4b5`%<a_8 zeJ<^}{|R5;CREftvS@2TF8#Jov3|#`&CKIFyNR_33%w$biKog(a&R*bn;L4t!O<r6 zQJo9+eV!?h)Srfa&-}t9TT4*h&j8GyaGL!kcN4D*m`)!UUB--rA5`6m#L;`BKvgUU zKHACRL4Gm>&iDj_cD>m4=xMTFtsEctbBI%L-iVJrPJtT})1YL>HTeBa2M<0+c=;J~ zcG-&-GAG#xtu;tT(l*;*>*3YdacdhIh!BV2pEJm^H4o9D{3Y0C+-)v*g(m$FB+Kfj ziQ-*rm27|fs%PIAiGp!zAY5C$m8C{T_{yqevZx}3Mn8(8ee0BI>FsdHJ4w)lepzbk zdJ`R%)u&lTgCHkfj)SKcLW|-CTo$qwYCbl>*tNMJ6|n%sCPX2Jpc{<8oIWxrc!^e~ z=tE=I8M5rkR&=>u3GN*o1CRDRC8$XUrk&lxUf358wUr*Wld=oxks2G?8pwmW2V3y? z-bgSA>BSX4BymEeDb?W%zklgS{IKXN+*OsNUZ*?oeJe*i%^{4wJJbLV-ZqjCMIrEY z*#&I+#fg+hL}JHHh<H4`!(Bc89lZ_kC$n#C#nzU7WbLQ{geS=%vGMN2<$xJJIK2w# zKiv=O1M@ImnZYt|4%jkZHEb2<K4s)?7gF1waX8Q;8JE{f(SGJMTH;rN(rmiWNWyCL zYRE&l8t4c-*|r&PzorHSPmJ;Vu3(&g=@ss9d&K#D?dLYkn2Em&N&5cGHpHpTC%NPw zFJ)m4m+w~2eLiuH^u$IY(=+-wYjrE{m|QW@I-iNe-(R=cFtmd;t=o?W-*!;3aFKe? z7ZtS9O_ScX*v4iW1>=3y^Mv=-jU9Vs4mI5v21Z(;IQ!^Rvb&}b%%jS%x^*PiuxT}Y znOF=*gl?kTTki;YEyI?R3~;eLfF4z*a!brIPzf^;R+q@(oLd>>#IChAn?AfCo{KH; zj~UivZC)|SX^;ff#SZLMUJx}}^_lr<7D0OqLT%Bo28euZWLti0q&19yD||Kr8Kpnj z(p}!R=Zkkz^Wz&xW_KwmH8!V<gClL#xh6I$`!&t%&I9+c*8pFBh?^__;-B1EsvI;1 zCwjd@ZzO7vy|M}@%UdwUIcJI8z#Q~#s}7dXNJi!FB$)|)C&=guZ9MTw0*xI_!LNFw zK|?;N{`Ko1Xq=qI2KN1h!1`v^?dlyS%RdyWuDwG=pAXO#+L=gkM>e=NEn{qd8q(Y) z^|&?WA0GGj3jN+XiF(o5$aRu34!1jiPo>Gja8NHZN6iEjon^30?H^KoqzSn^(+9_K zVr0XyW$4Af4e-X+9meP=K$Oo9B678f7+eac-}J(%$MkUanV5lX_HvW@Cwf)TqPu{4 z+Pl*3^TM?*-m28+SvFf0{FqF9odavyRiOT@KHFy53D?zU3Tv1n?Awb&xJS~Lbje*r zKMmwCJy?R(ckX7-{Luwtm0C0}Ruw1TnTrXfsBC8t@hF}vMB0s|N~Z-Rf8iU>KhFsV z1~|}RXAKfCj8UZ506?6Ctx41*GVkws_H}L^oOQZ@&gtue_?=sD++Bgb%WDCCS2LWs zk|gNv-HfLnX~Emi>tTMt1;|r>#+AR!#wRY9kj!Pru=a*TZUvK0UZ)5#UBPeB@{%Pu zw9W{<k$#8lTL197lim^KToIUCK~aT?2%7se4kcZ#!n<e3Ky!v8n|t>bn_uohMV&OL z=+R{~%E^hn?fx6z7%9RAY$<F%8I3Q;>#|Z8byzRdjwJcv%sEwS#P0sf$r^5e2zz-d zcDWAk74ZUNbuV)LP8Dch{DhlaW&l;FL1XSNCI!h0(4#C(p-V>=R;W7TWlCOf&Q2Ac zn_0q*eo5r1>CXn#{9vd2xd~b~C2SwWH;}}fGxTNHJ*-i#!?t=9(OGk@LAisBt+!kn zZEL)W56GLc@3(w}jRy9dXjBu#i1y(JH+SKAZ5*3Cb`_PGP{P*QoP@Ni0(^et1beq+ zJzDIt9f|b6C#u3Vgcq!>$kmr(;8o?07RtD=g9qorMsWw!@k1AXY^wx?Q-iqm`~_H@ zaI-$I{h95>SY3)XIe{Sh6Nox>f=JK<2y6My_W4*cx%%z++(0!}{agv>UklxPGFw>N zp+p#St%w@<Fszj@xN@hkl<e6Z&6bV$!R{?eY~ZK?zPwmlh#FHOk3DL*@a-Fs;GiNt zzoCp<np{JEq^`j~Z%oE-^F7GDFXq%Ut_LYNtFi0e3{&IYFYLc^X?pnEHri8@1})j* z@a5+^X7#ptR7moMJ3S`s_?p?+_Q@-BcBUVDUiAR=aGVa;M+dN7M+hFf!H|x8m!b-z z8Srz~FQ{2x%$xV|4ewu|P{~i1CxZc(QEpWxTBYzA_4f{t@O>+&{Jb(06?%#EjQc>k z>~v7^-d>#4FHJ)%PuWT}C(|bv?bvvsqCE9}7gp1`4>e;#aCgZJdQH%abK@20{-0GK z{^&BEzL5`Gi|xru%iC!Fvp#70qE5!m>4)@I3({n*4hPr7qDp%ya=aFSu73=&_$Uy> z#Hu2h`ZDs=shH?4JcO5g7a@H@C+IQ@2Z+=9hn_2K#YPSv@B>{!zprXxgD!aB{#l1< zlyx0gcDz7qhHPl3eIS(Fd4nc=QK1TsPlf7#E<Sid6R$nvi(ZB8qI;P}-0MFFORW%R zeXRWGNtqn{`jiVmrf^+afdy=tSWZSJ>!QUs-Ql=;0C_J|s>eSWkHhjWA^SN)SYlN? zxmzQ|c}^V<O4~M*Be_jjYUv8rD$mgNtN#}6$Ng*cd$A$wIa{24`tbucIdC6+pRGXC zSOsVwTLEvbHi6CU$ru;Z5V<F{FtA0P_H-1W$jCx?(moDe-4>x@I$?0cA_@FMR<dt4 zNAi*v>f^_2{)?X^bkQx_kCv~|z~U|^d0!@K(IKHLV6KNF9^HI~l<GO7cK;VN#d!+u zUuRqIe$m`^qlY9cYFN)c`6j`BlDDN(&Z|QHw%JgmU}-yfzXZEl>o(3jIupv{@*qfV zCOdgj8#WWjBRzpJ+fuHL@7@-pE>9zoY=$^|l>duOtZoqRp$llWsXcbJcSEOpMPUEV zx#0A<M4-N09~)a2lM7dJ(3sRzM#3x<8~gHXmq$0jtGp_DHHozy&D#Y>r%U2ubq%&^ zel^X@6|$kihS|vqO*HWqus3x2u>S@=_REi^Z{~f&uMXeDk=NGHc|xAj#}s`!(P0L@ z%)5__M4eg7`!C6_Zy!;}_atHxRL>|B=%eI7JJg}Lln7&2SW@CXx3*y^Sf?!p??hpb zCT)UWEY@U=jB+r?MbwACx2k{TF2&BA+KgLE{OT5(z9Pp?UWAqGLy|m|(rHVj>6cd< zfT%wLpU;Lc=XwmLKk~8pP#Y{L5UxMH#F0hODsaIx9e<mn#{EZC_J8Ie|4)_8e()Hf zDVxY~%RzL+=N!ZyF~Uub+E9J5M~LvB4CX~g(42n*WY3Lo=ChV5x3oTpuocVTo*){X z4jPMNy7Y*3`xZ2?BNvG-ze-%viiy)|PwYk2iHw3PR<i3RS3Z2?EYf0$jH3vo#5_le zIk*2WRrbFx?|-SX1F}v6Nps5k=Q&Q$o8MX|aMcv3KffrLKB{9a+HT2YHcVz-8-3<g zrg`yFl{0uRx6a}IYD(F}uldKj?-a*4oy`^We@L~?d~M4!%?M%SzH19ohstam)|%Sb zn6DS4dO8SFm|^~#_EUl}Y1ag$4v{wN7M&OTUiMj#G%}CT^^4$%nrH}yLt_MKzv2at zZ?_AQGenpzQ5C#tQR|t}<r)kzeZbSGSjz2JY~U?Rn8lo(n8^735#hS;@8?ZbO5p9g zdXllc*D5F;+s*qrs>YizF-wq9F^O9#J%uxE+QKRQoyfg^9L!&MOplW(-_7h<SxQ!R zx1km1HPJ(-{oJK7al(h>iBxOCg-T5r?7Mh{)D~-TU%d5T^y^);Nem;E+#5vMI1p-W zPBVjTr-Tb{R3P*4bts$~069N($g6u<OzRMjXP27Bs5Ly|HrYO6kYX^techQ@S+6E5 zH6D|Y_6i6=3CKEX3nV>>L>YSFI5}`UUGcFB29MolwAM!;nfPsZ(5DU7i&>%8J?{9I zi5i=0D8|-4{)X7_nfRu}d2CJ&le6cJ!@M7=bgGOw_VL_>Co)e!ZQ)w{ujmn3q^SYP z8(%V$D^=;O72k=~N=v#x;vZ*IlL_-nN|9~ZT6*Pr7)a^tg=^<tgNx!fWLy}D&2<Br zk_#B@2K(ULk#zXDJp(I5s^RDEi(q-_09Gn|gB;tB!Z=4Ycx>mzgh@?>bZt3$w4wl; z=&t5$xp(C0qn|KjEyW(JlB5Q4yYMmLhPzO!7D30cEC>#kX7_!6%;?5g!OaXscG7MQ z{&X1;kW(%JLw7!m7|4N|)phvst`!;wci^x?hO|wHfVy+nk)Nv(MBJ^yu>U7r_MgTW zy7!G9dAO5@6|&Po(E6MCnLCXAQzfZD_A8_2Xb3z0PJ~zOO3;(rfIdz!W!Ekf#g(HO z%)Ct>(XMuRSU97QY}z{l#}52KtM?`038J02Z_^z#QT+)T{?&q3k8XsB&_VKiwgRmG zw~6MC`GO7_jN(gqkDwxdC7#v(0`8s8Bac!R;B!tUbkJ508~+JGJDv$)|K>_iHeinz ze0YNv2F+r{93|Ptf0E$5IEa{jZzNaUUK5__dRSV188)GBpxc=aYps4Dho^GzrTQj3 zO?KkBaC{P>phC5LC!+HOdH7`5SemQchOD}bY4&>wzzY__x2?{?tYj0+cApP#QZA4+ z#u{|%WCa*;e<{pXeI&I9+whk~=Wx$*6HrfY#=&C1MazbgJ<UZZ-^PF%8@1uPZhACb zdmDSpr3&p^kqBG$G>K7mD7H4c#-uy%1+!aLc-XlBeU|?~lI9r0rJJYO<>frq`OzW> zY;gk*oldwBehji5;!)2eIjo#^8%ci7Lyv2caHm~2_{?|=(R&VnVB-wj-R^<z+hlNE z<7eQkox1pXbr#Vpdka2l*HL-34W}pm8pgD%F&qAt;{BOt;pXsHyn7+WF(O4|I7Jo< zH7xL&F$SAc10fzu5BvT|Bx<<<INRe7^L&#c-Y?C^?9-jFeqSGoob?)Y=$k<DEDd<t zN+H53pG+B;0zspNXu`0NY@L5cAR|&tZYYUB>3MB>?CuZ~7b_%<K6wU(**WO*IvMJI zQ4H>$OoSV^Y%!5bU;|fk=uPTE*tFdio&}b&E`hm3_lW?O7S1Qh28sCFF)MoSV<L90 zG-9)#Wa3_}Tws-J7|y_zKG$$#S6#|RC%vbzL+Z64Xe30bdOEa}B+<(r_c6ZMiqgYN zvD26LNaWKeTsA?6dX#>Ip>|)=>wT9bDFU&{eZjnyZX<^){b2D<J6s!c85!&E=b{JG znYZ(2ks@zV^zOY7l@t+%!x<~`<%0~-$eu`!P1J!|q6dh*%O2$ZERpQ+F6Ml%)*;of zB1XaNE1J`6%<1qxlPjrX>C3u5M4G#VbR2t7@|P%*r?LpI&ZvbgPwy~fIG;<&{)R>$ zjxzo;r8&p)Q1axIGXAw~2wyRsf_v?)fZbw61al_C&*O27&i_Z(nRrtb{r%sNF*46& z$WThA2>0y0uS_LTC=Em@N)nnh8$+TpW(Z{{q$nxw+56lE2^EnhG^bFJG!gyIZ++Kq zJ<nRd=lKuL;+%W-`Mf{x*UL{IE<~?b>MVQQD2XBd-j{$;y(jRH-vP20QaA>UV7chm zph{E*rVaIhPu~&8m|fo9cgYifSF|VVG_;V4x)?BlD@k<aCQ|+P8F9TN2#0n10aDK) zoJ;A<*iS*YVT1=o%C*Qj@6&`)PNsu4mI3e62H?=neI#Y35)hEwfj#Ek#G?-pHf1)! z<i<MEWV9L<Eh#5icqwU5VQXp0@9Z2{n#IQ+9qPr!7nHt#AuskuIn~>I6O_L^0A?Iz z%TW`pw4%Hwr&9c0L&eu#DsL=`-1b_9^Ou%`tTht!@2YKf(^cH)FQuB;WcwAGDo&!k z`<GK{(mhnN{CB$NuMD{xVo2`q(V!l_zrpD!I0lw2?czzjFvS15IpFn{8rmbEhG(Et zN)K2dO8#XNb<ALxZWGl6n{~~x!r6WFSla+~&7c7&+p58mF-3Cgog@|M#-~kWSVYpm z7~SMMA3j)V#@qkyJoRXAG2Px2LI;c2HwKocfNptJiYs!E9+YYTj|IEz7JV;<d2W8h z{dEl`zb}Rwk9Vha|45P*YU=c3iEF%K4-vcF+K0fa<U9&bN|0y0H!+Rci1)X%@QK$U zdSCQ7r8BDoi*|`oC)O__hi_wB=TH^QNu7yzT8aU$%Z5-ye}Xy__kd#`+d)N*nsHP* zH0f)8cd&AJ0ZqQq)a<fSp1RyOTA*NndYy0^9Q^$WXFZkR&HpY>8M()C-nw(CuH$pb z(p{mXQfWC9JN$rW;JAh|t`mpf;TK-a8EZ;bB#0vm4^sE#IUsavDbIqZNe*Ovr7cd5 zQWMiFI7NvWSldOC)}4756t?{YJw?V;y|o3swkw;Sc~PC(C4CG08qWY(%Axp3&Qacd zUqo9e>H&$xOMni`H#qq+2KOJ(r(Dxzs17w>Aa?R9Ryng9TR&gLc{Q|~_mg)Oe|@(J z+%J`dX45l(st~&fjN3V{-=ClZv(8b&wfDh}eU;?l5+f`pd6`<I`>^5cTnBvnMFplS z?=|*}io&Zi6!E<UX4G)H4p}^vgQwj05_PFou<<gV?mnV``=Z^5<!~L)+NlT2&%5I6 z$$z|4#xWpk#|<noZvj5K&J8~sum`bIdORJ?LRfWPfD-spML%m*!#lQ~qC`G4-jEFL zY~+vDfpY;PKyk|h5Nof11&4BJDTAWM?=!=2S?NB)%rU|<j|;%o{A+fBo5v~B+_m`m zCrsaJu)>}9t!Nc0oRZNyLfIWPpnBeI!7Iux;A6Shsq3X~^zs`QfZo*!ir4*v(hSMO zPptH?K|?bwoxc!+N*_4w=m?mj8%#Y@u%_jcBk=I4mmK`#DDPVmmlk-)2MwBO_=0E- zWl_o+2WVLk({O~^w)zLy94F0_Qug5-+nNokx-DoVeVXb!=M3Hq5AhZWZ>6;F#X$R` zM);<JFT^abq;G>PNSc-h#NOG0AESiTJc^JxVV^-#K_9iXZZ;6Ft)qK0`FK}Ne4}63 zEY?_%3KFby=t_}hAaI0BuI3fuE8Cvq2PZr@>0RwSIpK9=`WkPbKg$_j6qW;vyEExE zWiohGz9RV_Q`!GIXa6Ua)m?LuZdmY$9!M}?bob<tZ8CF6lgDeybAA;__$p1RLf_FF zeI5h9c{5m^Q!u&Q9fz-6QiR>dSKy;<v#Al`)zrq{a`@HBaw=-UB&B{J7w<^efL+Fw zaNN92|63}%V1DS<u+1AH|63}nr>8<h&mU(NK5C~w^wxk)SC*3Tv>aM>d@1~`uZPXf z>5&!OWAKY(HpWDmJSkCQvZs6E7=Azbx?G7n^|g@H?AgvtEi^;9A$=U6ql^?JRiTTc z1?n682Fo5wFo#y(hPUlA@yw|(=2F=xnbB#4VjaK2*kxu6{UL*F%yVVJ+;rj9saiZw zOAdZ=y+N7>RLISei|p-ehl8eRkXDq7rPHlg82D_qW$^)SJ-Zf<Mj3+Wh!afrgBWyW zC>eP<c{9u0#&C4o3HZ$MICC;o5q8NRM1G;s@NefsW})Rprt|m{^5RJ?^CoyR8olL( zW?5LV9kMDyZHhu+SDw;aM)c7EH!1Qsl!7{TGK`dJJvDFJOV06fQ>=2S0<XWg6qX2y zW8S{6pria9$Y#^W)c#u9#P=5Eb!|S3Q>-B;Ryi|^=H4SB*A1A1lm>FAW^vQKI7D>8 z6#jll2_5<*3_qnSFy+owC`@TH&i)<@#p?9H_Xn$3h`%k<AiSRxZZ3yLWzAS8^el8v zmqY#ne3J8eHPckX;7>OtnETyMFz}=?bQbX>brI{JaO+Vphzh}VIFqPZ9b=!OyWsup zHXwE*5%KfQkp0Dz441=4_ZQW0?dR@cYW@<G;WJ1+x|=Zb1Lwn!=L=Dwmk73dCde(> zwI9~Z4FSmt#l%>y9u2PjNnU0}qo=~XWaT)!@9!Ur0<wRiJMIk1diN4oHou3wkj3PU z*g-rfH-_cYHObWzncy?)^qu`$kNo3rAQg&LVC(Xy*jUh%E#5n#tc?wHt=203ub<0N z?e1Eby)hHMX%>NHZ)mc5`UJV%av0_JN$}0?tzdpe&Osu}UD2x5u5jRmG%Vj=0<zgp z-ZdwdppcL>sBx_t92&hzH+U<9H$VOY<FmUNA>~hC`R#bTzxx5ce5xEaW$puN{T^iM zbqwe&@MMzrR$&Wf3?IGhKu%;l#g0mv^m=L~@iopRKf4;3bzc+FwL3hrzT_k1NBrMz z&Sm(ge;d*~t4Z=L`^Y$GB2RMl!7Wt=ggbZRV=r^b<~`bs>yua-0WqeET_CD<j)MXQ z4}f66F6O;C7jMj4h`+D2hyE6O@mC8c5VVL7^q*bBd2@rvi{IAJ!8--tuw4p6Di1JX z9TMp7;eFiI;oV4J|4Uf$H-@1!Uo)SY<e2Mc2Ek~A7n=Eqhjty<&waGd2(s!u6q;Xz z=wKP{+2n_0OW0MkN$eOLNM235f7rmy*l5CCq9@69|9Sz_{Uc07RW4vB+t}@!JanCM zA-3iAB=*y9V3$+}4s3daHY&B^b4wzr)raEXm#@mO&VVJnN$n@oREK!8w$!4Kc^%|! z@M(DZpAgDoWhN^_^GMF@aN5-^3RLSYA{%!~kq1?c;1Cky9r0Sq)L2YIa<f@V-@_oh zXoD)8pEw6?JNk-zj+2E8Rb}D&zR&p6STkOQuX8H5mtdvGy<oyS5%|oR3#F1<zzOk1 zI3*$&k38B=BHH4K`wK@l)q2bbnFdfFPYp5wLN=(vvJU59bE-#V1@oZApES(zL$<F3 zQUBJ1K(W#k%7_eOeb(LkGh-D{HS5F7nKo+5u^zOG2$0b53*?k^6}iyj4!GmfnV9yK z@LuFq%5dC-T(*e=`&xce)4ryHTKV%}S;q~k=lv>b&tYL`VAGFw4Q^+CB)vteofP>$ z#bo&TeiqE+oU>@;xFwUH(GA<jm6#~yzi3Ro260Mlq2<PT1bz!<eiRIl<BrYbR{aAw zqpy_tWTJ)m`<_uF^NupZTYZ`BD{F}&=O29oTx70V6Wq7AoZ7H`oVMDUhX3iX1E9X? zM1R*CT3k|`cnfEO7jN%#sE=W2gZvxf8*d8TzpUWu-jGG8OBgucK8w87N}1w$V<6sD z1g^Tfft@mIa-Sv+!@8y$Fs3;kFPoE(UXQsm@nuQG<-7(`yCTJ$;)nppEAre$yK_j{ z5<(tVG-BtfIAjp91}kzy$WCoBsCh091O>7pn{``(`{HjvXbO|^-j{g&-BEBiHxDaE zsPG#uNI^|79U0Y>k*>E!C|ydAc<xkUdLP=MmBQ@jXs9TII#zIBdG*2zOT~$@?K@JU zm_#Cl4TxvmerV<T1|FytBHSQ*czpj7*kLUKL)ncKe@36o@>L|`8|RarF(Fb|p9>y# zNrU5CXTmz?-+0$DcWl2*4|`hIQHpoYa9p#N!`bVO;-X9iW_fKH*ECoOwd|1R3&}zz zaEmMR`uZZ`YRD1=rNfA~iZWSiEeXHdyAki%A>=o>L-p}>@!OzbY9vyU2|1^V8jp`q zH`j%dzdLHkrH&5lbM+>0@>d{D+j6P$-zlK6aTR$fun=#$TFskP-wp)cT?Lz6lAxkn zE-uP7!~TB4@W-bfveo??tbv+jV&fM?vG>6v(+<|ZeFO=DdQx@n3f!Br5_x%4!p|Hx z=H*%mbak2%lVPAoTvAoY-)wmVA01lgm2F72|E+?ezw%*w=W5bra{wyo&ceN>Ma<Na zexUBeV&q~4!0##_yU5e^ph`RioEPZBLzhmFUe!Ll)c}Iv9Cy?r9L<;q)syi*!~FUI zJHENZ0jfwKl9)!HAi0E1rjsw@C9n+k`6w{jH>NRtg`SA+*vky1Ni(~)Q_yKm4m#uK z44r}=5Veq_@atkpWX2NMpJqM+)&W-Js(&-{F?J1fi9JD2d!a+Bq}2%j_#hBJ=>mDr zGjJ4@f~6W#=sz{qaKdjsO8qH}UWY3)l7CC#t$KH^rY{FcH1!kb>35hji!{&^-USf& z?gv`%xDeX4&0tJ_jG^kTP_$F%0KDodgd{n$$<FPOsLMNn!EJ(M;ZAY%_xKx>Yobi% zkH>>~)w>vYK>$u;U3z6V?Wu}a<2e3AFg|+z2~hJfqH}aC7#Ei6{U_!_L;fZO@;dM< z5^=FaS?81Zakql`AJ?B{I-TB-<G**oU%P72p7{5ay>BgZ`N?kPT8cmOeB*59inKJE zib%k7?;A5Emoga9yT<4ed+yLIH(+9&G@07-{Y2o`Gt#o~EfD|ii&t9kaHXOR+-9FZ z9ndiV=Z&@lORu|NfyOBN`?V9)9Ek%LR@~&7FN}lH{P)n<(hd18vE>&(w&n&l=At`Q zswmNSC9EsY06E%!z#bJ3^!ig8(|aeI-04e$+h6yP&b!e>Cs`0l3mk^8Z>U1<-ySmi z+J085Qbdd&IRNg;O!$IbGS<~<rMQcf$R^Nb=U5yL#!~0vT|#BN;8<bc`cMD_Z4P6& z3-m#2dn;B^j)b!|u!5=!K|uLr4AM>w<m<m;$TveFX8k<{#_(V|^ZceT^JefO40#+* zgnQ<bc|Eqs#;}Pj>lz~7m)P^gqFtzalQ4QPU_st)&4$iym+_(Lt0Cuy3(C|;qWNpb zA-#Ssi4aTR6@ByO+1+cz6KWjtHBS^a_h^H%C(c0YzBVxoPy|A{?c|j?AAV@EL@(BF z<@PAoqS205bhvFLx_N&!(hk3XJ~K(A!RI*oV5QCPKCH}*5K98wpM7w`)B{<v$?t{W z0H*k<DBO~#%y@GzfSfT=RPisCd-d>faNGYJPK=O4mVN`^VXqrl2IO&i#5?>?(v8<P z)dOxg*Ww7HHuBTZmM$~C0tQNR$?KCJ(8`pxWH_i6h2APd8@lDtyf=|VS@j<IR^!E} zTr{FDzicO>Tc-2>%~n9`@2EgCK}%%0(Ge<6E&_g&&WyE)C$nq@k6zZ9O``XTqTk#u zIC<C&Wz|~aC#}Zh=lUXiDe@-nD$;@vj9=oXUQcl3)Ft45P?YWvKMRH`WU<fsnb7Iy zTITz!pD1Z$K6;&RiiSJh5F77R;ME&t`1zYQ`;JIJNGOvS*=z6@c$F|`|0NO*9mRA7 z4<X%+k5SUs)4)08G!v|?jsBv$L`LWy^fKhb&VdQY6$>G1JR4#$%32)~cf%z1`LD^! zrlSpW@nxS;e4!+jp8ej6+;^`8g^MqMw>`q3|8Wo8C)faMk1Zfh$D_GVPZl$-m#1;7 zd@dlSuo4Ot9EF;H$KjHSXt=6UkbTG2(uPfm<Ym<>xKO$UB)}Ma#oCKJD}6%@HXnqt zKL0@CojF8yu_HV=nhbu-K%l8$I=Suoot1i3gXGpXG^l65Ki|JpdTA=yxs%<v<W1mj z4iD(VmR(?PZ34WreHprz?a5^Bp!nuacH}`uIdM3#7p>_3gkELj!Je^NVzd#U;;1Gj z&ul&OXr?q1{`V4yXUCb@DTqWHTq5H^C!x(tK72HOjihY8$Vf+dBBCM+u0A(`+lT5& z#Fp1|LTWFV7WEdK`u3R;W|3SLA4Q11-4^OGYie2a)f{`AwxN2z#}Mtv&#>jPD=K|h z$epuIin;aLfa$%i%cRA~ptR%!ur#a%7+g?60_UHD=4dZ0@wpnfT&f@e8;Uq>mGN+o z!YGyOFA87DOTZVO`S{3V8h+_3Ch9vQalv(Y66V#8-CMiCpyDCCpDiSaHvQq5CGG^# zUmftPv!*b)H=ew74F$~wVwA_4HB4WH8`QPRhnuUF__a;}NbMNg)eSmFev8I3IoIRh z%G6#M#Tg_nkr9MrW&^Lyl41P6Dx!H`#^KYkO)zxoA>n(RBZuBpfy#~@#C%f>TszVa z56=Aq#>Shdps6G{(G*7&xNN0La{=t0{EBU!s*@*Bm~<sKQeBzGFeADHxUIiQMReOR z3Nb72(7jfw-CU6y8vl_=TItUu><L5kFzX-xejh4n$TB-#+A#w);wUQI09p<elBe5k zkfT=>xWUcA3+&TyTVXSu8)d^>lT0Naf@;a<CuYdtVG-Vxs|GiEeW#1xn~+k)HF(@! zn4}moypT#~e1qM*_qYPc)9)97%H?BJ`S2`W8Mg%XFAjr7z2Yc#RFSF56k>*??l8QZ z^9ZrMY@2j%G{BA%PW0v>eenh0aLNuwav%Vq%mK*Q_>lE6zlfM)8kk?j3LGD4F#RrW z(B5bUGClPf9CMaqz~mBm%B2r}&Ax+u=X6n7$LfG5>mc^B%*OP22v?0;V{$E@McnOf z2<NCljy#JC%~5Awnw23J4K9EAUnS=L!(WVUKt33;?1d$xb?DrXE8*;8x6_=H(F>g* z_BomZoH7TA-Hr;9le>jn9qcCWtyuT3d<MzcpNsGOIEK4T2)vUyi#g5iI-S-~WwuRB z;5HpdqEBamwQJ6Ua=&Z1q&5qO|J1~mlL|n1SQ#ttc>r?O@`=N<NCq~oAZukU_;(Ie zG5Ru92u#an#8U2)X}ceQ{4y2XpK1%P>m{NaF`8&hcM&Pn^k5REr;_&Vt6)Te3Tr%Z z!N&)s;PLAXIPBdY;GV=_nOHOMKc=$(H3#`mD%-f!AC%8-13`wbXydpH*wHmkJ@NE_ zXS;X9m|tzAwnz?M^ooS1uRI0o9@{}*UJ8Bek^uZLG!0fdFgU7H5|-U~O$pxAg3GT( zV)DR*oQbW*uP5c<?`?B&sE{Ps9Y?{B2?D@+rX)48c_nMcQu*If+5dNW|1FhusFcMX z3p+WSltSnuA^}I|1h819ar)!Wtz>WVAF?9)6)FDp7E?h3xceq6*m)IA`d;Nh**WdR zX44sH^xzDB<qTN|-zW*Gokxibn8K3oz3^e=0>;1SB~D*98^%nJh5|3j@!a20WShwp z=2Y0Aw-NzhdCM8-P3x2J!z_9s<1g%sbs%>*Yk=C>0(i?}4RV-f26rA`2v5okzynhy zP@n&gboZQx8|!xy-=90ku1&J=$BYzc)Hp_u<^)0IoO+@qc$t}T$^r^L>Lv*z0!Zfb zTG+|Af}PGfP_2c-_F!dDYm^pKd2)y>TfGDJo|8a*FV4aM7B}NGznRQ#FU1bxhQ#K^ z9|(dt_*efgyKTyRpz)#%-fXf#%u$wNYuO5AH>omDzbKJJ)17e9G*MEmBuARdve5JP zv+RFJjdZ$-GnD8D&Uc3}^7gC(-t;5SZZd9&Gw^vEFT+iN6W({(?xxD-#!m(t?b<I~ zZphb@redq-a8e)rY+TfXI7)A&=#{SrIVJ+ybj&FWN?UxqG3d4}-IjfsqhDFgTU5E9 zWAwPUQINZs6Z*!~ZjKGxbMe#Qh4r6o5W1<wStTLNGaoPK@Z1L*xBXZ}XOTY6f{3rY zmL&s?XC^szMZaWt{6?e3UFmUjQ^6!J?Oi6PducK6W5O=V;bjrMJt2eIMU~MfWKY@U zHPTdz*)Xpt_8({Qm>j+Sfg<H**~Qa-<jlJlH^{5#L>x<zqjr(cr0GwIlbl^bqdaAx z0jap#ps3*?5OB7l9vv-$v*%cWPxdh!1CAT#IQxq7|B#I9{bisK9>L3s^>MB1LHbT; zJBW7MO<i%N!0d{-*kh{;{ier<_w)J*&goDI;&ek0EVe<o=-@9ZussUjd1nUiGPenR zw*+42i;*o}!+5VmDimjq6Tu)vJEtut3K~9*BHTRkZe=TR)k+4%ozIC*UOY5$JqR{{ zW^njI63`glN!nfvf_K$vH08P+?DH)n4PP#S&5IGd<vR?TU-8LddjZ0nq-p<Y0z}Us zo$Pc8#|u6?!TLiS^e$YBM81ATzuXWAe$Ko{{zXmzxYZWd(<LBqZW`S5c?BqVv70{s zG8{I)N`(14K9PlWi-_6ueNeV=8u9TdqAkNB;orllI9X&r<uzeTR;6{2DVKR<N0Aq2 zXv+X;TH6S-H+#~KwM)Q<-N(qB?3;A4>I~*!?h{ybxeuG|+=}ikeM|pNS0fj=c?=N@ zK}&!+3EJYzoUV*v4u5N+<#(NdE9(TX%NKjJVfZj(^-c}79<iiS9}mLFbZsQf$e=q~ z_Q**265e<C7O8=c@Ct``@bG*WnK`eSJc}+OgPU2&kx?;TdVUaC&P&9%bIo9N$SRuI z8H~e!oF+>y4v>-JPhj&A5mF*{3Y!m!g1k#y*uvsfUa+{e!C-Be#lL_Lg{YIs(R1WU z(M@2^QUO}pg5l9KXPA~FrX(V90o)n507g!E<D$@i_@`qX&S$X;VxRv~a&PA2_qxOI zqULGBU9%d<?4L=FvFHuGvK6p|?b=$FE5Kf{C2VRH0EbNHGrL@G;RnjP@Ke<w9?rRq z!)E5f;2mr~O>a7BasLIRwPVP?Mn(MC%m`A>i<pg7wkRfJ1(|!d6zhF?P0z^JhXGPW zRNhA;MprEc)aXky{hbS$i7T5S_v$IQCruCb{UJd9G$8_cNoWU4nC^C<p)P(x77mSZ zwD*OOb>%ym!5VKy?@t;`V;9tWOuV4p0|x?sapC>VDLA628|;vM2Ar*?qYuGV<dWn` zvTtkvrkI`NDe-lvEo|S|?ua{mIA#_udiB(H@=F`1_VsGYVH&$^{Hcd$I-o*t*|?9= z{V<I<sQ;vT=53?W|GLn7N3tl1t~YqASWV;fim$v6hrZD#wjZRY((3Ti6<aAQgN0=L zP(R4fH75p(GlA})9{}~S)aE}I=!|t$^tfdyy~EHH_>SJ7eCCZ(-&W3}=S-RN=I_0T z?-o?x57%c>4{1?+O|uE?$@@!3?F$9%b%4JAt%Z^~cak!!iN{&PTj|7Tab8#PZ+hay zG@QIo9ISTSK&n!w=oLkdcu&6&^*q=FlvNZ^UAykvNoec^5-VB8fNdGJ&hw+8$rL>? z_bRn{Rt5C~WZ`0GWq3K$oHlqK2fkM45uG1~#D8lh=E)~;2J?CJvspRB;7dDrntq=8 zGh&G!FStSth~K8JMo(ZJ>vDM84ie9d-Qa}DdvMTLAIr(ffV9ySSf@RmDj_*wVqGRL zpzbb+iYnxx>O@{ivJX|=;R7}o3xXp4Y+zdj8qd9xChOL*$ACYxX_M=|4aCQuI-rmU zL=InXyea**(XD0?XWIr-+s8#KF&unNy(ktVp0$rD9f5L+$2p7({zUR7!xf=mvWBhM zX$njQ@Tt5_DLhWYMI3W3rSY#r7xmqVqSh@KZS;#u1*yD7>PA}#FL-7k=;$#4D?ew@ zZTU~A4~akMU8)*ji$XL|j;rFeOhi%g-t&RB#ZoG=-xNzPh@<Xhx#GKa$8gWX3ViFu zFYst=5kZk^c=ntPxchw~+4lJs6|}n^e2(DolFRI{vDggkEw!H9Ig*OszTJq8?<Uc~ zA-jm$SRUE>bscbZHpBV-TPO?FYoycb9v-VF^wK{$wA_~U__4`m+|`MI@iC0Q*{Tys zj|i|_C=a^rlqbRuA3}8dD46}v68!rYPi|RF1BQ1s;Hc^|s^_vb35n&v;YM+S(vDJV z@e=I1iw}#U--D7wE4($w4DNQ#rxQ%|@#QO7Ku@L``?}kL9gD8O!m>!<^u(6FU~mc7 z>y?6GVGO!_^+3q4ZQyT?0ZAH?1^nr{L|M`vF6<M;5>I`BoR0=^T&aL|Z~uYgZ%s$c zLKA3lX$r@b^wS?#^kDC|%VCP{Zg|a9oLbWUgw~SwBaR+(p`@u4e%dPqtS)~Cp<yl9 zJM1`EQ)9(Cwa?(DIh%;ZLVfrt@*;Tay^pLS`JD5^3&^juo8(IAJP@}$51+j0OybPr znQ-MG@GmKfQRNzufbu|4P$dq<N*lqm2Mr|YOgs6~xgK78@tab7^bE?awgn4+M1uIj zgLvSF4e4Fv3I22_&~Gbm!2Op`K*d>~;oPOm;Kc53yh)=Cu6)>mxAczF&FQ(koGD+h z%H?^Zx5y|EFO{Yj%v)wl<xS(P4&G0FK6i_HZZ$$ZaS5f19reks0b{#$(y>%+-#J?I z<3Y~qk|^r>vNN>1v??9F^EdB(^jd1c-lKRZ<`d-}E=%SvFXK2#C;~~tRrsQy5*6@F zhkY(Zuxgz?s1{2Bh1&&yQ&SJ+=haDDYL-)rQ%8AsmwRFhPa!C*&_Kx>kJ262n(?=U z1K_qRk6wGb+U|tD5*@xc1{B2ZqeN7@X!Wd};7so&s$lRp9-s5Pan5ujJVo4Sx}l5W z|6YUD7OVvqf|If3D_73*@ng9Bpa;=?5{$p~w1elx0Kh{Ec*E%eN;~}!d7o=T90Ofw zzmK1(v-Ahv#sk&Fcy}_8fn54g*#c@$UNqTOCyZx!JfV&o2oeVeH@fn`c4*e@f|oU> z0GC3Ro;-FBh(~<{v$fA-rK!CCxhwl$=j{KavImo0$@6+k81g$A&KbPKaan<RPp7#u zR!kXazGp}x_U#3nn|q0Q{~X>0-TN>mOb|akIuGmpDF)|453|OY`(U{-A~$0d>5UVT z_(ksszIuB-_{Z9QLdv7*|D`Ky_5Y``hvl<?z7)G4rs2TMwRuX7SR0Zz#19TfJK+7p zy5zQ5Dcf1$!nb4=@(DC1S9!OI<lLj^RXytgotVS#zM{yuD$U^L<4V#$7(kMS&w_+7 zfHt;2Bn2BkG9zGuijY4?ippZhPPJ5m9c`eMVku?3DvWGcT8xiYtc2AWTS>*vRAPLu z3X9(l1jC<hQsti-fToHxG>lmSOob46zIqt=?GLBy_<k_8N|M-x{DV}y2iBOGMa<>y zk$PSu>{mX44l^>`UR4YuUiOf{yK6~9>Iw2;vJcK~X(7MzwjuwpOv=M|HkdGsV&pr= z@mO*m&KtBs*FMf;zIm_1*Ke1R8Ke4yS}B1zA~KB3EG=TPAsY+SWHVQt4KUsQocevy z3HpS-fESXRsaseRx76;#8?Oc+*%e1YooybvXZr>{>BzzkQ9HODt_%^cSd0Q5&qEr4 zYvGOw75w_NFcSSO#PwT@83Tu{aOM_IwD(L5(a7>(9u&$TWbg{klCCE!_C_Hyp%r-d zuSPgiOB8vqe~7VQKswW(VZX3a@|r!${SIrf3z}OFWE3NzW<p&fFjXcdf!TQ1L@aO_ zis5M}L=f-exy*>hO1sXK8T`P{#@v75YW%OF??EFweWkz0z_5r2rl~cPu@l$kzTWYg z9Ej+E-XYEmd25Yin`B|SY9~)5XCCv8p8~H4IuN-&mb{i5h=zxXN!F}NYH0Rs>~c7l zTpoFejma$d;lN!G`)vitGgXDgQaLQk;sTZk6orye>nQWzi<m_%jbx91CL{hlg6mkP z&mZ5OkCc`LGM|gW;JM*@<m?hf#v$!1GoEr9g=YG|MU%}?PrHj;^_5^QTycZr-Fgt2 z7QmC&q)`dXgg!I;$&D*cOgMiGK9EL0*7ppKI~)&fr?Fu6=p^uar4zZnsF1#Jv6z0l zeI4BU=nSy^HXq*`am9PfS+ZW34D-c)332Yc&7cztnFf~x@^PIT3P@eY+#gSeURr%X z^zlX58ZwMDf2QM|>E1*-U@LyTSOe`p5Qc^=B;c^E9@-$=00XTjp|V;#w9(OoLl5F1 zZ<`UDWbUISUd%+|0crT2$^fout;1~%?8MD+DSox)9!|CXN~<Q8;UmY+Qe*AC)Lki7 zw2*L#Ir~M8f8=5kval#%UIa#<&hSpguzeMtXznDMy)n4!&V6)BQ;@k8{ss#4EGCm< z6Brk6BCDL|lbpFN@J%KU`Yp3Svm@jg(Z%t+lb&aA)O#87VA%~UNVUQ4ZPGB-aTYwb zbQ!tQlfWw(XaiPHQo;7rCZJ^SoR&%YM;k>Z!iE73zH7b}{rq@|`BA=(d8uiIw00gM z5&HMw4y1@){Z&LWJInFZv8zyPSOd-yX@-$y+0b+Eed7P8i^Q<E(&39D<QkC!)-TP# zr&aRsfSV2s&W|KpUX6hJ$>OBr;YMcn3=`Or;17fBmtc)Q(@5vbFsiC&1q{wDp|24Y z(s7{z*Hnn2!x|F&yRxTX>wz@xu~j<UWRHs|a!muQep1cs8@x<(*WZHjB5%;Ux7)df zE1w|~r7FlD%cjJay1{(Wg>bp!0Q?c3hqha9LHB=nK;0m=tG~<(ep-AA8nrdygjOly z5wRORSltPvW;au|6Ra%YV>Q*-{}U{H{ThSm*4XHPF@tufkRFNmFwoABG2yaQNtb5) zE{$CdJidhqlM+Uazv>u!Hh^{XY2{sZ|H<BO<!~84gdpz`5_BaLT|Ir03|FopA6e{- zv_>H*O25EFERDw}RXsrGX}06;DTUSVzM+nq?q`$#0@CWVuTgXN70wf!i+xxxw=NwA z)FnHC%gjVt^0FZDeUyls4^Ky8xou?Bz><lun8!D`Wy;TeHiw*9z6L5!6F|oPXPEE# z?}@=<PpF-Ej2VxZ0}>2)nA6il^b1#_!KM@{!G0&Sp6Lb1%mmUU`UVf3ae|+^weS{f z4>c!QkJ`^hYUe}tw<op_gy?9)(DjFDvm@c4#nu_m_dW$GcR=2S8>yJX#Nr<@)6v{3 z6HsU7d~6jG%)PLa_0kd*u0od%USX#KV_N-~vpY=TDwBK^<;?}9-RZO`JBl$^4+GkL z#n3T}z0RB@@MysvJa5TXqUPwrY+J32@>??DjBW!sJ`e>2>J`bz{Ah++ejkT>_X0|6 zF)>t-BMI8`;Z`BG%XwBD(C&wkNV6N<^SYCy#l2*j7h0na2`l++W5V{nzl{01rBkFn zi&atA1;ZDE(}{iADE?(PNk!}xA`_8C<iIRU>NK*+1D`aaa;=T{7aTx$Ek6?*MuRbv z`#|ch&VbJjak2iTM_`@MI8DvC4)=Xjhc;@$aK2kC_T)ar@jPF8HaqD)THQ=N`E-<Q zla+*R9h0O*P?FK#Fh~w%Y~j968b<nYcZkZr8s_<mN$eh$NuETmfSj&-r1X*?+H9_h z?RT0(d(ATZ<)S;;@7xA@3YJ4_i8Ca)@ET5858)ZXIi$Ra#U+^}Lz6u%_)2yK7G=}x zVLw;$zFrN_SbGK7=4F7&k~l25LWv0e5`x!G_v4*gPhj4U8~C>+LlR#^F&UgtK7ZbC zbU|VOHBW1W%E^CVXqp#NOTlQy%#DoJgU{s0dwINRyai4f=My-$lPs{WBh9O&VS%hI zQExp*zxX!`-cV>Geacnj1e=IPxIG4<$CpE=us!gp;Q+=g;%sSvI6QoSOC(bVu*$eN zT%K)1Hqi%(8*>>y)01J;M+?wUgbfaT;KW3<&f)t`97Puek{O+mDVo-^MxGT2I=AWq z&BYl^LYg9I?CYn`wx;8dq!Y~YLQ#bN9D^GrIvI^-B}P26g7yS1#Q*&l=%44$m}paE zD${|k5;g^Yf~Udxn*#8hmL7bh{1O#lbp`txTaf)FZkVboCoiVb8u!)<p>UB-u#k>n zz6w_}o~g^B!YL2(4|HRv=SPWiUL^B0OM_fhy@8Hi+)11sr-Rf?amJo?6PkvX-~vfy zNWPUpX^ENes$4!6JLAS~5Jf`uYwqO!w+xtmY8AX3C`6pgRY{&!A-)mv4*V#cz>*Vb z)Y2_4z!dj7{w}OaUP;H{z|R(NO3DC*bPvD=uTJ>ls~7jX>q15|HjBAz=Zud3`bn-l ztA=RU2ELxAg`_&Hu+O(+<lk34vLrqUZ1Z17x=m(7Sur6PyGjU#+-rbHE{XSDJcK;b z2_f?f<l(8Td}?=GA-MStvCh9*ka=r9aj2Y4>n_^^F6>=}cZNxkmLIu*-`&TX?QcNJ zb|AQA(-5p?&wsmi@wu@>$C-`a*Pv6+p1_Cd2eFi!KE1J`1Lz(&4CY?eVU*XT;%#i7 z{pQ^3u#xTFXLp~Y^fz<x{^`kN-=TK$%f^a$*qh=P7Jg8weFX2Qn*cd3_wij@2(KSM z2VP-8cusK|Z_~t0dQqh*Xc-R2lXogG$9pktD1971;zX1ALI2=#r6lxncM+QAv=6MC zC&_=Q`Gp+f`7ljh{%FgSBw~>~7fwdjkm#KX#N_2;VxAgCxo;p~DcuBx%Zh3HeF>1B zcZV8`)?z96=H%hQWBAb$bu8DZ4MZ0w(nGUi(fdDRK%vADzPuGeBo+vRm&^ZvHCB+y zOxlfC7blSCA|-f_@M&_^_B-S7m_VU(u6RM97c6+J$rT#h%Q%ESWU5?*k^S{Z+^&|w z++V29eR6&Y6Yc*HrtF(TN+x_^%eGihS8k4V=beMasSc=m{Ryb=S`Y5Cm?N%J5^$wg zlX;dASlF|c(Q<nN##GMW!1IrRoX&6Rw$Lmb6(5Jq3hvWVHj2#Rq8@OU<qn+nu|%2g z_L0_3DXe#>7B%e(Vf%O)@X_Z%5|kuNmP9WDt>W6ugTP$I!y<**RISNu>s`u-jAcN- z5*_4zNfC7_w-F`n5?&vh>^G*CF%x29Ov~>lU~8}%crCCJgq*L#+b;Kk4v#u~=ac|B zpY(+mF%cssmv>{4#Qi`}{XEn@)(^BLCWwRfLuy9DO4RpKmM?W=4cFfD6#Nh^N(8-+ z!R;1b(4vbz%!BNi{I&D{U}<v=Cg9_7)LuGFf}%#rolsA>-tZmSQhJ|w_)NmA6)(}& z?}wQ6HWj3F%Sj@lItO1|e;;I66d=cqDZp^#IZm0+(EcgkNQ{FxNhp(}re)v8Z%j_1 zewMy+MEWs#dd(Il^~SLGzZze|>^$?=<2K1`%7UmXmxM%VLBFa@^7+qlAZq!FSV_j> z$$)F5tWkgjS;kY33Om8%;2-*1>p0NFf2csVq*9Z(gzP*jP8RFTB}cbU@>~!ibDb}e zceo8}np~r<?|skmHg`iE&RN<sQWdrzSVb+kZi)q1v&0Os1SIr1o-s4s2A_HVMz6|h z(5{&>$ol9)#^a6@sXOe%xRzv+8af9d%UpC(xR1QdUj@Yl50TMV$B;|oUv$U24jm9T zBDzvn;Pv+`3#RKOpz>xQ&#`5M*N<2>g)wnZ)`H4@;k3**Nm%>Hm8e!uCsW8CPTR2s zbiG(Z)Kpi(b*mj9{UH;j4-C?)JTgg8{7!EAgFn<<jq^zOaVhe!>4JyS4j}D|ud!#9 z3tC%ijiR-V!VZ&PAfZ`}%$QdRb53KH5HOvbs-K1m21SvKXD!qSmF2X$%Tk4@@+8Dd zok&$F!8<yID0;vb0@<6iYKJ&k@Ff{!^v8njs|^UYKT7Kf9wTDMSqlN#j%q)@MYr_K znM>L<|FU>F_bob(c(-M_&koMv`q`gBh4&jthG;X2vJ&T?n+jpF<Yl-qimRE_dUfKx z@-?r1M=Uudc7y3DokEAJCArDd>zJf>3TQ_75INAX7%oblPHdedsK)Dt@Y{P8*q)+7 z-ZfS7N-Ho3inM@*za@#SjT=4bkq=}p9Aa4r3gn+#G<tWSh#_9`pjl3qJO4OBK_yp7 zi9!O@oUaEz29M#kryrrS)pOkRc@r%C{RltPo&_y#bz>RMR(#L*GMwZIAUx~|TlAH2 zz;R*Ljp<1Jld~nsJ|;L@=qNmPs|~EEwj;3gG!8jr02?l};VVJM04|FIU3b;-wykY+ ziE1;ms`4~C#+oBe`#)e}ABr-es`<dsO@?pBCadeC<(YIFHAY-zI^Xn_G~Yk|AL-XU z!#dl&NXZ5hwDzzPUXyhg8mx6?8otFd_i_RWzB)<fh|FVxY@|uOixTuz`+=W)(M3yc zzXGA@`$5M1V|cvKoA@XPQqcz&!$#2zoc>k?9$dZx8(K_KahD3f@)4FwUig&DnQ-Dd zCFUTBS**39T#t-Qo4`$Ddr-W;5sIAu2bH{Ph6{BbLt(pFqzUwbdFF`Z8TOGl6Gt>R z-3WbjzYE`dF-Nnx8|nM~GWdT?W&djq@}E?8P2fkIHZlfy>oTC6T@F6eaTO~CX%nf7 z%H-Dljd+7)B@sU70uOBt0QH}iQ7}La^gFYf9>t&3be}f7A~qMCQ}3o_QZ{45@_S&i zU^?7zHxa0=Du)wA*+jBtBb?lrf$!>_#Ukfk(;<(fVWiWp|1FjMf0y^)Qd!rdS#TGK zBom(t$>1+rvN|%FZ0$TpGFeoKJ+}rFeHDj0IK?D<{Yx_T+XD)<Zvq(;ccE~a1<bi% z4b9cQ(Lq)cOkZFMiyz{Hf^z~W@pLLIHJl5p)zop)`~$FOS}_@0RD>TmY5|4WF;MZj zC`6V4B>27<$6@##kma2L4WlQClE^X`GW8i!b>j42@j!T-lSAr~&%?bF;mlKMHE`@- z3b|ObjyBr12!865LnbbkWLTmC&(GQp!xEhuGUM+No2pp$m2e#&Uq0B7{n?v*I533Y zp5kJ+lR~5>Lx2&vK>=`n78A8&Et)4kNkozsz}lm7B*-U|%y7*hPYWo<HEIj)O0R+I z#G7Eg`2ysqiiq&i0!lSP8RondLTAH*Nyb1Q@Le87>>sjI-{OZbEI*z(9M?-)o}VZ4 zroF%~>kAR5@DTBvJq@L;4<OAVX(amSb+YwW28jr`!ZZ8*lKyVoikIlNfu_JrAUxv| zP$>~2GCHToMn_%j>@EhM5FR+P>;VX5kx(7`i-_ApU4r)Iz_RZ*@m7B)qBi3iutHW~ z%YyCTadR$w`uQozTk?gP3P{1THa~?u%ciJM*#f-la|~1~9|y57^vU`^b4jq=9hf@D z7;XL(1|Pp=5j8vG$-i`UxLAf=dNpsSLce5TneO?l81e#`YO{lro7~6=>0>19p%|>l zQX(@&%*cf_j2}C^#6iLHafh)Td(3%5|G4LkckmSPa&spJt2eQ<Noi>A7*E>xQ~2GM z&F~^%kya%D6(ld`)&7h^`@b@<rfmXm2@^!t$$c<T{098HLyt7ti4aYrF6eMM2C6(g z0Lxo=Af_$@zAl|)kz4}IkLbnlRDuEe;d399Mt@?LJYLZk#}1IZRz)<cgiE)aJkAl4 z3b!RkhHq$Tm2t`xuX3zq#W~^`Upf71PMmK>alHMHZt!w{9^jmsGU5C-`pElzOUPF0 zt`eQwaD(GLB*gO^m!_^N9O4*lisLQaqsl8-qQzsdw5?uqBBw~EgSV|Yl=Jt{DV}Ff z2IugzK%Uutb4v5qRHLNq0iIvZ9^NkP^Sl?g>Uj<xN}P(zpLte6-kdFkX&mO+vc}Di zD2`E`IFH}DgwtJ9VVCIC&YADNjGkQ}Yj>(9koJgw+E~`|&yKm^!@0g7uaTD%(3o5? zjrV+SCMP4dw9(pN2JdZIZ{y!>>p9C!ia?KRI!#{h#W~yG<C;(Q<Py6mw=-K7d|hZn z)Rn$sHHW!4K;a6wRF{tr?wNyg?q|S%XcIZCwHDg%h`{>NSAgTpvt(s+3K3A>gAdxg z18>*=0r^@S5OJa$4>Jqlh5;!uqeBp8Wf+maexh(VUx-nikxrbiTqmoZ7<0^1wMduR z6M~&6IB+wGdOlPKJ{Slx7HeOU8&+B*r+WlQ`1XTk-h8@rT@!c`|9>$dlcf9M4oXL) z8v6eg1Z9cl@Vd@YC~w6F=zI=r|5Ql28*k#58}f*wPZ*rORFYMHpGCsE!r<2n4}h%C zJ+f$r0<-2EhR*%xpy!AML@#n+$stFud+$xy`}`_6^o!GY?@lF{L&>8RK^3rUZ#G0B zQ`pKck!S|&A)#kt(dt+|^6hyf6&P7ejwNam)r=0Ze|a_K-1`c4wqK{OeidfQvy)+g z^jVPHe-gyotDr5{&C&h3T*~~YGD^9&lEt!B5LtmUM9^y;cp<05R6s7XCpd@|Ol3ji z)Wvj&^G?vR-iFLJlxlpXUO~#FPUG(NPpN~30r34-H73a>8%}t7laY;4Od{h>!bcsT z^Ph1r%CE9p_biLIePRu#+<uDER;i$cH{Jrnj>Vkxlp1hcadV^MAiuF)as%~ZFOC1+ z-Ea4&>MeHISx0T%a?Nf`X+KrEx1Hl6iAb(x7)F0}>DKM0oCV6lbWdOgr|iyK-oC#T zc!uU%USsVzCY#UGA{Wi@BhLm9u-<_7&es4R+1}&Wi)obA%|1Hwz#u3-Sp(WTdMIHu zLitOm07->2p#4D-CA8%OeN9&t9D3`<iw$l9rC1be&OCv$)NH}L%^T^;ZDv4bcmc?{ z{)t{&TtG+uXa@2HwUl>@G8vEw1rvwEsf8vD_~iR~Dm?cDUNt@oOE>!h-6O5wdY&2y zoNWXJ7lq)f2iM_W+qL1ylsQ=@_>s=|`G<PdJdJ35zfZN;tRWXK7r;64Lm+d)4~yK^ z1ohv};;NKC*i~T<I9qs{lBU&3)HsFzSrmemjjymy$x$4yUXESn=HR)$Gtlx~&ZO`~ z8Z{QILQ1s{Q<WAwi14Ykpbzb*K21*18%u@p^+n=v^|6N-xK@MxR`syWMIAIRih)|Z zLqNa!IIr>6Vd#-{i$40`6Sh1x$rFq(CNs~U#<zZs0KK}Ow2to?@O=JKvLbMT*e$Do zi`vqGl3oG0y@oAN8Fr8n18s_DC_w&7eF5G-&EYMY#U5Uk#T#~qlZesHR6xoM_$j9e zd#Rg)(|2`X$51qg`XfweX+IFcZG{JPb)n5)71Y@GntWPkP35hL!&+XAV87@n@H|rm zs>3x<%BKW;Bgur()KY|Va)jU!*UxaYct7la+rfHRRY;7=0O`wjf-0LngUj;H@R4~P zakH$U0+##0&i-Qj<<vBM@+fN)yB3cl1cS)TLK|i(?HaxI`bkFZ?|gv122o#A70507 zeW2z3L!3ULPn`J@9OaQmjkm4Use<WusDl%yaM`U&kh@Zt&d;?3+3nS|kzfP;>EtFV zqDF-%%}wDbnUr7&Eqki>hynHZSRvS4@|7o&IG0prU&GC(1t@x!3-$TZIgXX7UL#vB zq8oIJut#JxU323-7QZ5ar>+;$ksX>Kb!R1J7i6f3<n7d{*4g+JHxMWJM}b+}J#cua zD%kYL)z)fv9=*LJ6AW1HrHvkT0$Zn5)Ex79cv@8jFB4w3Yud~O_WK-h(b!e0DTHr# z%~BRL1~9e}at+j~3K`gWt&r}zrA+vhyQ$;pCDg)YLf{<#49MO)3-1^_iH}qm!a3pg zz%gYy*o#lW$Qj%5wLKrGw@?m}>=QuyoEUw~BZ7zyisH*bKS61_K1q;k1lu+2hz9i- z{C@I+jfpg9%fK)Ao_jAH^mc#>mU)B^ZN}80p_$Oa^ASBHCQJNTMuhI>$GjWIe5i!9 zTa>Tk0M9X-1_3RbIFFi#fl5{k?ee~cvXC^SZ-1YOZ&oKYN@EGUUZ$64c4{Rx5$^#% zm@Xx4#;Lfs<{rLiu$_8-eht-@W(7XXImYW6)u7AOJwV+`U0VD1B;b{AqJs<W;8_V? z;EF^l@HaGTJP{R$58B7zq-s%WXvWRPf1UPl{D=zsTxx-w)vLhkdv}0Rm<|YKq``i> zi}=A3W9-DrA1AGIK&{kO{P&L>UZ^F3g?-*q?y)quYkiLMV|<u8%w@$%%U4kyGpvF7 z!JE{Y&yx64fD_p7qfRVi_EI<droi|YFDP>H67_Z69(LTU!0TCDg#YFk<0f$)5Dyy0 zi|^&(t#LE(8HYRIl3y#3Ytq9(vRm-_md)_s`8KdGuN7n{?8UaJi0u7sOM{68`1c4) zeD3;0N2`Xh9cN=q`Ksdou`Byu=j{KavS|lS!ZUrkSOF<>TAv-pMk|Ye#Kt4g<U|$y zCZU1)S0ToX78t|n>R&nHkO2ZMZ|Q$s1`Y8WJiwW-KKy8xHZ&363jf%t)1Kki!IQgJ z@m{k-^iuDoxLdpZfAh*(|Np71<eGW#Z+R)$W9v?i?4Qf%{tf^mcP+6{M?O_wS4FGb zTZ9+qU2P2b2ni=S1KKRLXU?fPA^o>^N$IJB+~xDs_yetJ<eS7cX4Bds*uQ2ESU$N5 zzYYD#WZwQwW{zc%Aomkw@t-GT2#S!1n^y>Amx4}gSxr>s))LBOEwEl755?>Mavlk` z;|0a>V7(vfF{(dJi)MTQvp)~8jyNvdS6YoNffo4aXAKlb7r=~&8`Pr1%4m1V6|#Hh z3siXJI2H_WK@#tbxf6cw2rZ(4XulA!yf+NfHoF6z3wZ?IOl1<6>>#seD6;LdkL0b( z7+8C*0!zrLGtYcHiTpP+;`(SNV>ui~&TVsH{#^2fLuySx@=6L`DK>(WOVmjFl?ghr ze;J+^Cq&3omQ+m4VBbQPz+ZBfo_Lr=g^+i|-7^w7kpN^Xdx&fHyMQqrTunw*@|et} z?nuI*n}o%T(dDN%Fe!eSBzmSM5&IZMmS0n4<t_hcQz1Xj2V-lySEoJ63m^+--89Fl zH8!xYG8=4vSqyvYT*#0;7tXro53#~!N?kIJHvXXs8#e=3`cas~unK4SDI2IR9fTwL zmEkhi(`XO-DK+a%4z!tfoUig2aThJs;y*Lkjn?eD4QAg}N4@`+F}?a+L`zyz$4zhH zhqA2mELjwSw}q5=aW(94UW0`a^WhQ865yY<gX(`Y6GqZ&IQar+vAj72Qicj~#x*Zo z(yD=S8=p7Ej#l9Jt9RpP+5e}mGY_XK4BI_Y=2?c2OqD4a8rbW3mr|)T2qi<5G|{9v z&BmxuXf8#iNTvvTJ?}yyi6*HO&67|PP2{Za`p&t&bDi^jfB0uzuDv(5wV(I7@B4Qj zp<h0Ekh=vr=(OQ7V;wVxKd$V<YfZk&ZufL1b0zJ`py4%S+MGz<I3tD6yK{`J{OHOL zS-F-}h(7Sr;rhJ)QxpDL#!h~Hf-P~cNQHS?Q|Z?Hvmkk+hFD)@c<B;tve>6mIHHyk z>Hg2uc-RYwc?b|XOo@!uwukEj4E5b#Au18_PX_bPamUV%qix<{V18ExM~@FAzC8n2 zpwdM6Z5P=Oxw_6SI}h_g^6q@s=XTOOTb<pTznr~ubv-!}zmhkWd_&jS=Lm|?6uRtf zBFG50Xgp>`W3-~_N@)jFn?z{sC^d30S&4r#Xg}#od4&tMeq~ytFNx++9cr`nFZuVu zO&B2ym?gigpheiQoHBU>HV+OkIi<!lIek2F)==Wp<)83xLeBE)s~XwypE8C1V>z~f zQQ${q%a$KF`jZ+aIYOD_8~%sAjI+|vS?tURmba-9j*@|8Y+T|4nvp(*v8!X)lak{I zxfqK#jGxfG6(LT;l%<G9_(l>o?HF{teZi<7FlNsw+=eY}`o#8HK1>)mO@HpQA<p`v z>D|?qIB&#LBBhqdV}v;?H?^2Z=J~RTyHt6dXe0{{E@Xd(`k>6bG3>PqQ%L541N=JW zA?0fwR#Wq#uJp~V46LfHqpmX#5+9TA^!2+68Z|18W^6SeaV1^U;Giz=sdtE88Lv*7 z$wS<#sZT%C7kJ~uBP<Gkh7LzW^g^c4MVhc3%qP9WC$5TggU)K|cKsYanSF^gw11=N zZB4w~_7y~OuoG*%)sa2E+>JGv_Jt06Y)%KCeTHVQ{ovl8Ll_nuk9zN4(%j5isQegD z3U*(IrHMz$Ud?#=;vLVtACU*u6Sd*(o8QE{?l4}~iR0=gcEjB(Tgj{-Yuvo_F;{PM z7iJgR<MMwVG-T^Y*r5Ill2>(cC)~vJ?Si9hwXlhst~av$poFEfCEr2rhfk-O$5UA4 z=Wg`m-OaRM#C_(?Q={_Y(X)B6`ZRW$haTJQQXy=XhM=q^0V(pHPTGBqfW}hl*z$+u zuStb%PCB%tcRKSob1(C0<Xw2X)D$Kb&n5+J1>hBN7+$N6!EcHw@G*a%NU=3S)W0?m zj9WYL)haK#(p;9G;lGD9_E+XNcPFq1d|z?<RF=_$7K1R*{1`Rdbbwv*xrlAq8b-7x zjKKBwQAEL0iGGVnCQ2%TA<{x12j=wRr#EtRz*&n;iR&R7Cbp8T%Nt-rfhAnOFoK+} zRO8Fvh%sQ9EtQ`afJf{S(9mWuN$-?ko(=fJ9)+D`K*;9Jd%Ti{_bz~E*`Cf%hL<{% zm}GYQJ`Xbd_(iIqX2P4)<%&E;YOucVzp|Pq?t-(MEp?9Xr5jIbv90fqkj0nI)2_4( zLfeHrrIZmrZKWNp`n{N1$1Np1ZRUQd`-)^f3?rl7n^O<1=`_it1OA)^yfgPDH@%)= z!lzp^xA+E3|2URp7hIu5MH%cZt+TMiJGOkJ@i}&uUktl@&rv#RY$iK@e?DqG(qP5? zDg1-GKl#XA!}!rZM$liAQt8021R5~A4^KV&%}n3L(*+LNG)K2q@K&wh{Dq9)^43t4 z+fW3vlMiyizC-Zi7omfqKM)K3mkCGhD{#;zn>iTRB2pL~0gbcX;16~^NwOVZ7G!O~ zS`}Nc(`Kacc|kMT7auLz$^c7#Om;ab7$3=o$>dX=9oKpJqnT(wCIo!6>u7TQO;ouj z&Hj~*rwjIpc%xxD)F8s08z-+o>cw%afa>9UeF|u`Oa;n@zhbJ+nZWqFlR;}*511Hz z!E3gz^bl9YdDRRdKF$B|tdSDN^{UW+>UQ+{un6|b-~qm6cO{#Uoxxs<_zpAEREY7T zCU*KRp`2xq7agm&g_i!3EEgk7?Pd#9*VHm1DPK<azfq>@!m7{YM-sKL@f1fF#M8!} z^TIkQpR`)`Lw2quo)7%UtsTArIwsB~4)g|>9T)*MTG9;r{v|X>_k(rNRQlzg60<Yo zpEyEtCNH6r%kQg8Am%3N&bbOP6y}uDaybS5!60|u?Rg&ih<Qe=zqMmt)lc@wf^+Qc zafUQY^&WLo<;gpV&mf&6%WJF^lZ90Qv`kpxU9_);60tKr3O|GqZ$9Gj+|eRUFVnKr z%+I{O*+OE`VhGfKBpuVD!^E%AA(<LooQ%^5T3ICI(4m>cwLK8J33KV88D8b<MYiQ< z)Lig2-$<St>F{p_TwO=h7dq!h5!t#*)%n8cO=MH!2*~&*0;N0dB)IK5EH31rA~^%& zZXKdcdFSZk0}d>g8A%u0e!{GU0W`qLf@t?Q!Sb_r_=@v+7@;wOtf$kQT)znYq|x5o z&g0tXle7un2PCtfI>h8;1F%-s3M6WZ606ruo%7vxu)`LH5c{P~q(UwQm#Qd}ni1|S zE*1#yH6!@^!Iz06(Ps8+ETavcF=TO|4Y%L7ica~c%3gEn#<4#qkl1~LsEiunz0ceR z?J*kErF9$_AGykVyznQ>E*3)3%3yTda306ysWRIghvU>M5zrCPNz&|e`EUJ)NmytQ zo&0&Kb8Vx6vqfS8|8<-if4Ihk)4cPQ!q4%<&&jm>=PX@z+I?d_t9&x@dOsoCEsq^E zFM^+SREu`Xd$SiWj^%B&_wgzkm2{M7CJnQEOb6}UkFn2;s8q=my5@`zJrVkjyKpR( zu|J|oSL8*2#jjDMOyekw{<Hx<tan89rTrN3<peRQOC{?pw>Ss+gtCKmPw@)3B1u|+ z6pQ1B6ZLaK=1ck!?dvb4kK5fy<ID!;@Hi)GH1s}pD}G=y{5Em#zW0FdAwPOeECGR2 zW|5jP!-@6cB)HaK#GSuugg@*p@Y<6|;(v;vle&}P%-ATVKgJw;=C)u`)_cg%%;Ii- zx&?Q?TCfvs?Xbi%p1s+e%lbXI$Cu}LJAcZW#C|$Ag>ph?`?FOl<U-6r@<3|{ee*|& zehrHT_eUqOU3V6FA7VkebFVU2zaJ#3e;3dNwOT}5EKTczBFJV4=azW=W!^3yNmCSu zkPprO$daYgspocix@M#v`LVte+mtpyM%Dvx`4$hWG^FUt{&pcdxPy(Y+sa?Jr2?#M zIQh#TAiavEbhB3}_oaRwO=3cbv4lFAarq(*I&+8aH1?#KWm5D>P&J;oyNVQl*h_5J zZNUNwGkRog1F@dC9}KpQr7!gz;PTgM=)C-pY5sSe15{y6OpU3x*)|M|jsWjZ7wOG+ zhoRQ0)G6|q1RefsBzvYbf_C#q_<t5*#9-Mo_I}@-@^4d)lSAntWdEx^lH#JruCR*3 zfk$5K-+67U_IM?BTAV9$pG_fYP718wvk?TIw^GUB<z$TIO}eS4nK`yLgD76m#IDF& zMD$97`ionb-{a1q_SzycP1Y2b&JH1WKOKXmg}a&6cRt|cYi6WjLb^CHHkY{@EX6x? z684#AH0odVF0cG7&1-ktl=~~q=0oL!S%1kfq}u)<e=swMzrI|Czx1INpF%7ug+2zG z@9MB@yazx@Bu?AnK+ekEgg<dFz+deuYz~Y<=guHJcB_DEx2^!WA{lCbx{G$bW+6DN z4r&MPp=PflHSg7Bykd>G_v1|%_3Mcw!KjkodsR{7CGc||Wu=w}hco=5wp`-5^eMl| zc7STlt|YD#N3po8osXJ#pMRxmNYhfsP^AS<{HUXMY2=g{^iG&2UDULlmdDESm*-qZ z@6Q^X`~4ea+2F?*)vrT*EM9WE)=SVugC_FFwtfb+p397Fbrthv_e8>=3V9Lk1tHnR zu*lz&oLc#oujwQFg#38cNN*N9-Pn_rc4@-R?rP=ti^BNc?r-GX)Ho6ou2;T%#8-Cb z^VRIYc6+`rq!AvR4JT5|T<OM{=5)uNWqAC<VCd7FNxRg(k?$49;BlJ-^>$DeC04C~ z*Tw=mFa0`ATjxzipPx&WwT$Wbt&Pljom8xzKb0$Pa;9^pUgE}XGGV#1pQux(J2&x_ zE${JQFZ+4UJ9fKMK4eUE=I8EQ3*)}YvWmgRDF5g-`>QWiXrnO(Wo{u^wOO9k&a9;q z{HEY*{W^&G;y^^c0%r5u0|B`wMT`DqQze-Mf#Y<UslDldYrm$6tDTxa{GcD#S;gRJ zO?f74q!iS7|A4eFl3?^km-_WoldE(pA4MYg0O2I7c=<X%^JO<_%d%zb-o2tnot<cz zNa(BWzRZ593}hWto)F*P8;IAi^>pKq!{XdM`^d3fzqnJM{h-t15RTou1fOaCC062G zF7<5)xg@2{X~*1x!_O~p#c!lpn?E+p5TRJfR=ysOOg>4?wol;Rn$N@&+xp>j=6E`I zc^p6G$x(iDTb8Kcz)3b~RthhEwVyt(+sz1QBN`Rt%`S^%sq5>-{A!Obwmc#VJwvWg zaA+sjCP}j{?-%hMK4;M9;|ltCd!@imk))?%2RpB{lPIswPUm$a<f!l8Z#a>6#iZ)_ z?6f7<aD0Ip_|~RihkY<@J1wBu4mb&J5mRUyw4n5K+8#Qh{sBMvq6OdhO=41{X$yNh ze+@ge{XR*}lHiNHB%A}{pOU(WNM1A8$XWZXn00aL!w0LA+0iSj*#;*d<C2~EZ8x8@ zdAHB7D|*xDD*ryXZoZuEbh}6YTUGYI_8|YM%6{+Gr8z>IL}p+OPXG8G@3&jhk}fG) z^F*3<Ncz*)rvjNG+fkH69xPip;jj4Mi%9%AVGy|(s6e|))wx&sgK_qjX>{GfH)yt$ z;kp!Gaa(6U=kA%Vhwm?@)8<J^OpEG2xN<d!e(|MZNofT<`E3gQA9`i~&&&J2RN3+w ziuCkqU#esfLg)5xrb|{-lVMUzV8`_ixaeL2;*}H0Pi0Bgc3C`)^xHuq*Lac0vOVzm zOCE0YR3aTVrbMIq9Zq$*$Ry<}(mSovWVB`(>C%kCD!F8G<D4uVWM<9|R9i9n7ffMo z@haNKD3J%UUEJ-X=eR#bJaq}0%i8RACFWipbk>^VBsWzKKWv{wxxPYpWPK4AzbVGl zWdqE;!Rxv9$qbd0*aIi$7vj#Y6zb0D!!6GSG&u!yf#@2I{k>XbGiL$$da#z{)R>Z= zoi1EjPcUiU;D(ld*)&r@h2NCA8x&Ver+W=&pn2jxGJ^^&p=a(<BlnFsW=<;k3@s$0 zb3UDFd=}R!<`cOC(e%Ta&E$FCZ~DznmVdrz9eZ!tIzDJt7_F)K#4SypLazO}L|k9* zC6|u{kY5AN{A;1seK4a&G%7#QfYHyu**$~aZiuIHUIyf6K$2)`a5|9~KbTHzJ&dA? z7@BG}i=KA4Oww(Y>C>U#NRm?`H8)Nmmx{HC^PaEd<ED$8`^ukqTsE2vuDpy@QUbYl zvph*BMX+PIAzPZ%MylPDAW25}=nJ??4P8%j?T_X`p4||>qez1G<4bD&bPOx=a}I{w zC)6-!IGIrY1^g>)X>9LYYSlOv+eggDotBqTvGpOe*FV7r>Z|GW@AqKjvRgDH^%und zhY5Ef(#V<;y7IF!y2}n>Pu6`QdhTD~K}IsB3ZwUn^u_S$$W<D<;}`w(!jhkG!IX|z z9YYK4ZTO6X$~@C-%MNH=#SJCMT#HrX+jgiDxzL$x`fhdJOnANv`BAXsmLq#I<``Ah zJ4|L6`m)s)SsXsNiFZCyV)OYl=IS4%Up)u$S)YW+$g~nztD($3c93V+&ko{Sm*omE zcOft_Vkj{gbC_o5{2>PV#dPXGAjC``K_AVUPfs@gCiZ^k@PeWy>$^6W@R8AEqnSF~ z4AWp~-6=G@`~nYeNCf3>7y4oGDIsT90s0fqiavT)LwJM)JN%CiE@dwX`MuTjy_qy! zCGAMsW=rt+&78IjK1rkG$B>fEdx?Q_0M&4MK^h)c6RmwOxJfZN<lf~pjyoIx6ORS6 zjweQwKxZv#UQxhxZ%KmDXNu_gPe}XL=<%Z_T_&vqk<{VRVp@OSiI01#%KmH8fqrWp zwmb_-oK*ll_PG&d;<vN&eqJVBjvgdt^c!lvT?s1d#?b^HBR0v2(x@jvB)!y-T%jwW zpiz?7yPHaKR;t38ieA#U`Z`_vR)_VM_U5%E_mhWr^oi+}W_tBiF6#G<=BMP`1ED{d zZK!M$cPUk1$b>ZU_|<t-ck~ycocCRvLmfos&cV#F)wZG=wHl&dTeHR9rQ4j!jb4ft zud8zMo!H@|;yzt`rX{azn_05>il158ysL*r+`Kv`W#j!$2aUFfRBTnn8V44MzCR8S zB@Wn%oA-u0g;w7ZS(JYiS$a!}KmXXxSPy9yEjg9wcyinKvW7N8X3O_3N5$nj+|t~n zGSdt>rg7*uan*BKQTd|<Wh?d<aMsbWqHd>tadhukW~-ALvp(Xecs;YAbgfujtm_r+ z`14hS_)lD@c-+s`OjlVnv)>?D>_7ONQ`2pK=E-@6D|0ntM&7p*-<&;#arj&-_D)_d z&X0c#KVx5@bzn2M|7s5;Zp#M0c1;+&^eL{2@PyBXtLVPhd+_k)7<hZnh9qAa2ls>R zAl@tq=6ab4%GH6ghOTyOU3~+di_+-N^Ea7zCLDK2&B08OKev6UB4@b{x#v3_X}^;a z<W%ovjQ0IxYBCim8>WMnlZ8IkPCLl)+s=iXXpy>O<3wXM0&wlvN6Z!dAg=1qJ}h<W z;kLZI#nkvrVVc$t#;5&>xcX~1Gv76uGwfN)-73q#`oZed?^9hFcl$LHc`5~-<h%fb z1+uX0h`gZA*Mr3FTaFUtR@|FIarjGqX_=Sd3cU9w7dnI{_RHct=H$J7%zAHS+<3DT zeOfYcRJ{q-7pMc%p#~#vE{3N+)QRcTE9mh(nzM7N7i)?4bGy0|m`L9y(YgU?V!K|Q zdHie(7$4XJMOzZV<A({9o7#~!KT|aGAIyD;XUN`~0Pg0=VAAMmLu352ne`Lu!Jt(K z4b96-tryQh^`TZEiXBVWZV}_HwG7QVU4}_^OK2)x#%$av$OOe7MA93^Fz=Iez<zor z^cR1@kzO;IR=vHjBlZCnia+D!t|IhpZ3c@UH<(MY!?}C%qv?a&Qt0#MDw@As$zAWb z$K4LOkF^i-x#{;Rxy37#iFx>BwAGjdugW!$pHK#ap9RB$*IKYai!fK--(qgcgJ{jd zIo$AvLG;G6y#glZD`w_tGKmu=!=`1~ut3^^`F*yE>5x~U{LDqn=b~9$;jezKLW^MV zYYkF7s0B{Ed%~%1J<I)aSd1frqq!@Sit%d&Cw4rZhF`~Q7J?>XP9rdtlRF}d&$bmX zAKUGjo}~kvwd^)7`2J>i`%DHtrj7#1kr|@)3zfnupqh)f)GM2MIv9iBpTWk^dXcrW zWEu3xkv*^0!hOROyzy{2y|8&Ad~`P^HNyRLcJ(lkW^V+B+MGqld`DcJB2RyGO+b0y zPG)sZ3rrQv+Y0tkqWOhm!QkUZ=3BO~&f8)M4PA9y)}K@2e%-S~>*F}ln~5u!2*qn~ z*-e3cT%d>=%L=*KH|1#k+FY)xS%jxYCBm4K=A5i64?$P{h!m#(=61<^68|Z($M?;( zB8MfeoZ~17GF$N)Cw1x>=TtWc2i=&#?7q2=yR~6Hb87zr%s<kCiz0=9nNE?@ad#79 z5|;q~PTyg=H-*5=Y(w(4%bl6ob6m`h)+4u#Lzu!xa=37u3{j`&%f_W%#mQ>TOkLYd zJZ)Wq_eHaqduxTY+RPrt+&7QQjq>2eF8T_7E%8iV!e^%q5;7P#BwJ*7@*<OP+7d*+ zi(ypZAh2ATiD{2R@dPIeGmc#1;=bmHX;+Y=VR<iO@OK35t(V5V!*>h)y`eN?V<cme zHXELdT8|t0vf#Gj3Z}GXD0Q}q<${jZ<HHr*qLS_*utGr1c?_*Xspw|W&q=#MX53&L zweK0)Tn#3<LJ42@(>YjiX9rn*dKdiNv=^=Zrook`2Z(6jZ^6Zrf<qt-?AEL&b|*sc znAS1yn5M*bT@_p^_cMfC^a8>+m{2eA4AN<uhJlYFK+$S5%2&wIi*E~=jE-ogdwv|r zF0`SJMf+&B<1^e}tprK)x?$D7T{I<G3dy3CAi2>SCp31#;azj-QkmOAcrFx13T%Z@ zhv$%|?&DB@W+X|ljsl;$O8VL83BC}elYO>EB*<2mZg81L=T9^wyIvTOV{v;4`>&rn z@HU1ndd$(8TeJlurXn>yc8~}ZN4mo56jgk=fw(SE6*_<)<G=dzfI*kYLaBW)_BtgW zpRa(+XU0R;zrRqsb`-IlD8pNu{D4c&#_ZYB4dm=JOX}sB4iN{IK)-%9{5kxN2H7T4 z=uD+s5_L)SrRU_}+jXp^?jDw{se}DLexZ8?OF{*S>cCrPUUZ`kGSUj6RBJv>DSkvZ zi4-XbWT~Hl&3~Vj{jWLuKULY&Zy(^&lP&n_sS!CmHJUu?c#T0@Uqbz+Gi0NY1u7~l zkbqH5qMcJy#a5Qfd1E~buE}C39Qk%0Eq1NNvOQYNo$iawvaL^<gliYTa&i?e_%)bx zMBavJ{tN#nRd&MvugXSVlmplESzwji!XzAO!jXse<3Sin`@Q6ljqU-%WHa)$$(n9B z+KX-GlOb0ofIhN};#R~+)5tP4dN2F{PIE})_O3dEXA(<5Bls}PjJSm5ccxLZjl;+= z*935~QzVNbbaB>#KM-pAiJQ@P2rudDVCK_{@K4==_&dswO+j~H_RV<a)Qx|zZc74K zn!Q39%b}z*{Rlq!s!L7iWNMKtjQHWBNy5y%==nLAdtXyjicvPSeuM(d9To!B5AOqI zN7Gr18>u+B9qr!FCQp1@xeKQ2Xw}Q%wC`&tQ`gi3D%v;Up#MMa$yEyy6!{+KySQTd zb7kCaIgPGY^8>3vzp-td4ee;QCu*jn$j#_;(62L+-Y=d)24{{ZKUH6F5<5)kL6@1L zOO*#<=7VIcsCfr<yA_FKcP%V9RVd0lHjg}8;789*os8*AY{}@UIhb+zh4{XH4Bojm zT-X-bz`&QO^xu$8@b7mj_!kVKcM7lJ$l2pz)7ia34`DNU-<BY=u2WH~!91KIP@*Cy zS;4~(Y22ux1~~LnBdmIK+0p)OE%vP|fzsG(%;Px0L}7FWhgTSqEyb(hoqjtkc~6;( z(?<{oIYrcx@*%4dBgiBffNm{0>b=RHvvA5{sxC-V)yODvGV~-9KUoEDu4Cxst>a0; zemNLzxdN7$sglLqd3f0s1v0}Dxc2ZxFvO=27I+(q$sb8<%v*$oPuxiS<v7qE<qF-O z?=dMmjc}7kK4ZGtNBn4~f++EJE8N;H4_W*LT)i+2U-ZdP<y1xTBFz{)lYc<}JySUI zF&#hOx`3mljA2&NHZsmC9B-fa2t_v|@ZUyx((h9aBmFMJ%6n&U=7nT<=KKY#>*a9x zX+>tlza;F-O@dp?7}8Yh1l#>ufL^Z^sGPCPaCJu-TGPpCZH{NYbd=%7HM@b2{lN{J z-Cj0BBMjQ4t6;S6FMK2QO#C#&ftqfqLN{Y7W@EZAp?U*uPnV+i_!czUJ(grjFXU=j z9!eAv(8nhS@?Xf3=STa*nw$$6EtC-CPK`sZpa;u$38Pf!VJ>uXE2jQaq^p$e=$-Sf z^keraJQ_Hk-U{EuBs|Q7)_Q$d|N0q@UH=6F6PJN}i!q)Ux|x33(~GSs-vKA9(94fF zX6gA=)c@RYm^1SdO!zkfYt^UIqj7~0-V;a*CahxcwlcgsS^$gv)1ktn8*NsuhC{O2 zMDN8Mh&hvl^~*JgX{QtvK7Wkr@<)Kz7L2SXQUv4CQ3%#zsH3(eO1!BAjh&rvV%#@$ z-aP}q+7H2n6{f_nY#;7=4um`DD|EA7g)v#C_%KM7sBV4CH6PQ)`-{rZdfa$?Ggt}> zb*ylkg${GgOr0LqiDQmD(1Sr`rdT^Gnl9v?!}DjE@H*H9msuxc!YmasadsD6YT5^C zNv1SO-wcjNs?hs4vY^l*8S1CTa_Xr**j^~@1g}(4epDEk^bRA>GTnsQ^yySBQHlvD zHYCAb$OJ7^rK+dTV%DCqWbXGKw2igKw2R#s6zhcV@(1vZW;_f!D->NE_Q!oH^_V~Z zpJ?XQ0nyphz1$sd9^0q2L7)AbQr#nRcy7TtD9Y(U*MwQ5%}$D5^sg0-c{_vlR;iH# zsYb;1sUux-qYNFFp8~sihFrgmGFEOG#9Uaxb4>wBoQiW1T#E64%{lks%wU$A<vEmo z&Hn{%DPpGOu`sGDzJn57Yp~s_L*2U$!2^S@P=2zBk#@F$JAQ+Nt#}BW-1L*#_G$(8 z9Y}{odh)~~FdgGhc0=OgCc&^B4cB(=74nY_f`cWFYu}*;TgNNXMIjfswP)v2J@sZT zJH;4}hJ>L{_#LQuH=VZs@T8~DMo{m{uUrKqeA2y%fyLGG^lkM5`hH<Dc=-B2${B%d z>G}YQrrOif3cjN5Y4+%qnhYb#&B<IvU0OVLC0KhNhARF5_iK$lZH-o-v*{Dz{$fQ^ zpFP3o;3v4WGY}RUC}7h{4r-^&hX$7jOml0&EvM_T-AkV;B%WmsTLPYOHKd(3%5<Sd z306P22v&2`#n-;y6(#;rCUfiy&~4mwQokmJIp$x4w%?vXn74os%2tJe$b2+ZI0Ex+ z{c*|EZTMt}6rF8#OYA3afZ_Kdz*o(JcqvbyGQV56?8E7d*Rpf)_Phh_k6}1HyI9E1 z&&H~?Ga+ivANbrUPlO%{s5+Pdw+wx_ZNaCox~Lc85+vwIw{Ey;)`?BGKVh77B>Z&J zVD7Yi7u|TSPQ*t2sJVO*tZS-)$Zc`>D)1|8-YpQ*J%h2r)Sd*~HitV0-D$L*0-f+z zM26{`(&WGsc=e<fIW$HW_&0qRDKElZDY9s>QlJj|?1d>0w}XC9C)|{r2P)0}r2d5> zX+GhGDnhpHt9T*p(Nv(-!-O&|#nYl8hQq0*dn1}w{^a)6SHaC@OAIxhKwC;bL4R)) z*lP)H5uMkNFtZlhR%y|rt6p<E&iv$loph&DyYGYAI!$5aRLH!V>rZcf98FVypBFE` zFq|k|DS^+5r!coF6WfjqhY({&`1E`RJ!a%Vb}DQa`zB>^Zd(!O^`>L~_WhXfE*sy? zHl|O1jKfnII%La-T!9?j4f4AUMDcP@VM|9grdQmDNArVm<GZ0`XmbX}sHVUa`CM37 zH<UcT(2mctw!pjTt&o-WsjM-`fv%5UD>_urEe>3E7`I&82;=e%z`972UJ<S2W~xje zg>oU-w)Q!?w*SF|#2DIcW>5A_+*uZD6HM>Lhd^tnI(430g70i5k#FJ0P}R(d8gDQV zejizIk98rdr6$uMawAE#st1V=I?P$*4#jIdlj!M5apGHnceqtQ6v)xGFsymb(%{=d zKc4+&Yzhr^Qf&W?gU-E$uJjKu=A#Is@;1?Ir#pf{R)!XT<<O#JBtB7k1(SuJPvw0* z;~~F_iq`~@!X-bT^sNU;d;S)S-Yg`SjE_Ks;$V7v&IqCqFoQGyIF0P;nTHn(G9a_N z37`JSWF%JVk_5LjNS6;`9u-RAHP^xTfF8$yKa#|&ayUJ3{t0~aQz15EQ*eJ;30T=Z zfY4L5;zT`9%s%x2A2fOsmq%IL)57IM`n?Q2`&*N~ZOlM4lZBOn+A(2ODN2oXA|B0# zv~l_j%3t{bjp$A$Iq1^$^Et4x(1oQk58>Y9#aJi(9qe=)@PP4jtab>-u5I%m7`(8e zI1h3<PceyWdzl>Hxm0^~FT9zSh@QnE^wZaeu;J=foDynDzkNMOo1~V)DNe|}nO39w zfmCo__W-5kgK@QaDbCj!SJt+K#S?S;@aa<*6esM!gIZmfw0kN&-jx9@cOKx~MNP1k z3I_Zs{otP-27+q_=6%Z)8F;o~&qhU%nKcv-T&}`NOJqrkhAk{GnnQOF6VWwMmN<HP z4|6Q#C%7%GhUQ@th=Q<y4|d)K$%Y=Z+FCfWeMrD3>OOGxcRDw3i*US|Q-;gtn~<G( zh62J-c>eQgsGlJQx1t8H_+-iac{G~Ld3YAieCdGA*Y0CPgCbrHdxDFX%hDxL3&<XC zVMXq`7Bn<R;nFH=u$ou_FV`W2D@DSIo?J|e6lTt)%Q#oTi@9n=8LoLe7&|wwq8(Rf z6U~Jc-2Aaq;oFi;IQ*yx@&_FS@%1UFz5f%=TA_pnTs1IBMsyI|0E_5$xG8ghLDLG% zb;yL=vOl<fNDCGpawF%=_Tm^l9Z;K4%IObVN1Xi%n7{}ldU$ywHq{;gzWM{?ZRm&3 z(WNl==rwMzlMX6;e}mVW+i=90NBHOR6)0+VB;f)w%w@x9awp~mYFm_GsPtY~tHRP1 z4F%Zcc^o>VucPZTS?oA^0v|P~P#Milc<GZ1Y1x_%|L&L&3tvf+{B|px>@9+G`YgBm z<Zx(75s@LAKB9kcEYrH?JG_$r2?ag+wDj9<NWxZ7_iTd`*$HrrM8o}F6}ayjhnku3 zaQA&K?p<6Dk&_gtyh<b1j@6^FL!<HPv_tUJ|082)Y)6fP1G(j)Ur{A{1{fU(W31Y~ zz&`^G;=Z^NT!i^h?ZPjpQ<_A_b!3XfxEVFq?SO}l=dm(Dfrbn&f#NL;^%UmW1-`R! zz)DcirPrdO{!aSy&vJTvc>!~5@(?QD6h&IXN70LC7Q>{(jdX-7Lnc4(#vcFWB=pG{ z0oi9uy$459J7Yc4cj6)B^(){)i!>bgE_C~jJIowhE={$R9fWXEJmmQ`LTT6##(d^k zCN9Q|geZ5T>IwxRT&+#dhpdFZYIz`8(8atPd4k&**bZQR28cp5mK{sN%Hj{uDp?GN z{C|sOW7g6~+2+JXAjJMmpGvMVceyq1>%i~ZM{vztMQ?Yi(cAA9qDR3aeEjN;@P4z$ zyuUxOIOZ<iQV$1F{xq^lQc%U0KNX#kyo#UxwZNEnYG`76AG_8Hk9m`W*eDr#RWcM8 z>;@<+yjk+MO`HC>SOx25#?r*OYGCr)0vD!z7tjB&7hD)kqA*I8*2vnyN=FSEEs~%M zx2n=Bfo@E&!1NisD+ZdV4_7kN7tL;J2w>MHuG;A_=nhhav8*9+-o66jL@|)HT^cpr z^H2l)sEpr3xLILDH_cKcmKh#&s_i{4L}w5B_s@V6vPZdqu|K(UeoJB4YXfrp^&Bcc z%8G_1SqSTFLt1m)fsVRA6Q3JuW09KzmmT^GXL-*iidM&9$8JqjZuW-mjt5LbKa2fG zQgGUzJ2)+J2sKstF7{ibgn3RC5Pon3J(?mQ5*@z5uyggyuM3*o5*G<7l#9TlX-#m` z_7$g96bCffg_$&^3|iNzk^1ryaYJt(^LWH<?uKI>D)S>r{`Jk|^CETX=~E9SZs)mO zrloKod;)ncR^*r`gUQ%C#~JYkAKbsG3HrV!K*K8qYLzpR^v?Z<(^G>uHng6J3oZlM zo*S_9=}=0$MpIv57TLM(32rJ%Wj<dprn$vlWa1Aw;*M3&DQzn>=@jCkb;Yog$%V0d zH_(aS)o5O7G1R23BU^ILLuknorhU~puH))atX|WMi2^P$tZoVYaiN0ywptNZ4a$Zc zqDxL^^aW;((?{@^D8$0scI4^Z6dabEfOgN8GLr;(ujRI*=vvz`Sag+xZ%WrV@oFE~ z)T&8Y*+`I*(S%3p2~6O;6OdE+S{$kEOOg*4fb<Co`f$J<`=>;~(vNn6d*}pga2!Nb zZIZA@QUbzu7?Yuy>zSU1awLnZ<SrRs5@+7xAgb7aa4uRzHA<3ZFbU{opT(6J29O=y zA28ig9^8chPf~x|f1j28uRX|rs<LLnpsw!Z4M%@j6St;6@U){ACdM3r@RgB_{X1bN z{B;HvoLbI>Ip;B@bQj0(988)cPQg3l-?%C67A(bMAfeTOirxqD4ZY9!1$HoA+T&>8 zSsjp@{DrGYk^>U4AAeLd3%Iihs3VjmuP-~p{12+^|9N@;mnu6ev5ncT*Z?Ev4#R+T zlkk*@JW<{<h1w;27flX2gzAfO@Qr&vcQ~gT&o=MFM3<lVee`Qsu||)KT%$y^FNQ*N zo&e*Qk|fgaSbWnP4t+14L!y!$-F5T`D7p1<nrjrv=A4PJ>8%gwd43RhWBbwjLOJ{} zUI4m(gmwQNHPW?McndL4MFT}sh|Ufhz~yVuO7SrUSRZwi9GiqS=PXIqU%|LR2k_lS zLy*aHptk*Hw5!IMOzH8aK{soiezXr2pLHG21q`|hXO$T;PhNpaxJr^~tKI15d`-c) zd<83BPoZ0+_t3DaPF$${2b3~?fimu<m4?IU7eRHrt^5pzZ@MFpJeT8M-vVya{2H|P z-a>8C7BD|-y-|=9X{xUpRP7u^Z7y%2Zti=rkX=RxvxFvxE`+fo4RQW7G0K07#+^P5 zP%LD<)`dD_esUws<a-#ep}Sy!e8;@731rB2DJps+OKOYbVQTh2G}PenU%4S&T|_Wx ze-JtHeF3R_@(AyQ%2SVRs&wRv!(xM5c4U}+rN~oCglpI4VoFvqjI>vw?{p>-mEPsJ zMJpNtvQL232UkWr>jTuBXBms!7??jCIZI1DNXc(ulmwXm#aeav`1S&G;sz(O-~Jvh zrfnmGtxw?EI4|;5XDis``!MD?Cz$7x)oGat$NZVq4bxqMsLQJ?&NAMMShJ7OZ%hYx z<X(ms14cAKQx2kBu7g*JFwT9pK{xHU@St7=FO>;@gYmA^@^2UXk~>JYRkXm*Q5}#b zbh&Y3Y)H-K@65AqM_PM$4GGgc4}M$fP;I9!ZE6iA@%pkft^O^zX`L>OPB;cx7))Xp z8$n?3P-5fL0eRuK8PB@KL{CMAelcG_A3U5$&*fL)%_+i>TYn9=V^1G6$i7CKho2y{ zX(ai_%tOibW5mN%m(#!XEkX&71D5Y&xP*y)=r1~j6Qg3ml^ahN_N&shN_H4k5X&6Q zv8BfSZ?LJk4;L8PlMG>O>|T8n?dcx4F{>ALHG~R?GvPQe`wM24O{88PF5Hz576PtI zo0Hs9fNM?}kVE`z@VlXkoU0#h+-XjXFMP!<*E_JRVKVV(Nr1~%tAH_)qS?z_sQbey zFgH1p&VST_hla05FEd_zT2YS7%5ftdRr4VBoEK?n`G;Xs2#s$fRO*)x`E_V9iThe6 zYAsTt>FdjIb_T+SE6*U;Q}BPBJPck(+{yWxSD?1BmAiCN0v)dpCdVE-&?-G8+BgtO zdNSrhn9g&kDY^!6P4`g0R1pqT-UL(UPJHblOZ6t$k*&uwIPS!Lw9<Vfstn&kZyyj! zvO=Dt+0`Prr;!Jy33^2Iwwif)-H>#9jHFE~T~YRtHuhx)5SLX9(TvZ4Gp?&JQm+d= z9gT>-ZZ$@nK8fb)#)QxG!5h;i!6Eaj$Xm7Gg$!qMb5R(s*`rAIT9kr<5DMu$Y(_LL zq~c$Va*#QvN)L^3M{z(9JvVYNsrs!*_b~OK7xS7MwdDd#UrxZu@Dc>``cyvF3XKof zL7~+ROma~Km5>kkfm5a5?kb43$6SKmWHAoB7(yQV%hHk>58@$|=*ckKalFwk(F|=K z9bf7bH@g}bqR+$J;37Pyy@bA;G=xfpnV`GqoIp6JgJr|Dscu@QXr!ACJ=OdeUN0Bw z0u!HMZ&)lwshE?bRT1Q0&N8Rx&8FD@*PJM1Jz+ep;<59qEU_v%fd@?q*zH^e(w7{G z|H)-&Fi4JsXHOyxHbY5%*L#f8K7yLxIVUA?KeNew6g^pW5x33kb-Xx3nfN_S!SZn) z)NjZ#a5s@6a!ZwQ%~Vr5V7s%7zq1Hj2fDdQjnirGaYK<L?LjG@O!&QRA}M^WM(ym& z;CYG^+;Be49EugfEFZ6<f6HflJ+2gooO*}SRrS!=5r78$FX3&o60><uj!;;V!wpqY z;pFc9#0Qc~&@|~9dM<Q8j4FncCZ>2|;0117R=~|wK7ohtjDu;m_r&uzT;SF;N{}D! z!n$)C-~`c6)H^#4h6#;SH#otl_(}nH_l>5DU-WUe#CCMOK_U2R>C>G{zA_UwJ5sp^ zMs)wj{me%{EgbdoCYD1N%#~0fs!ye<^B-5bUQn86SDBEvI&t{0ZJY4_Z@|1X`VIj~ zV~ApcEInQ^2a*pMlg`ys8Ov5x(ti3kNG561Mu!+^2{k7g3ct9`zqN_@ogI4hKgM+h z@>K1rV0b)q5KB$Z!9Qrmc&#?rP+r3*#TVe^cNWB6LxOCcXiBF0iRkpIc;;T;5d0MV z5N?+=GyScjXx*?@WCR>birE$Ly`@f;?oy}2l{hrB+asEP*@zAsF_JcJ7)pLP4kepD z??&sajm!i_fo_>O3&(x>2WD-<NN8d$6qZh)va<r1JB9o4`xFy8s6dMDw(`ecckSu= z&C_7}O(PQF!NcBuJ%sjCOtg_5?pe74rj{<^Hb1?Cz3JxUqR)I-6Kh5mUdk2+zC8-5 zi*#{uqbz-`Do0jd9Yu=9_2b#2vY1rWjam8gn5avqIV+z=%=S_u2djtCX!AW_{L75I z4~=4)UOd5_LS|m}z5!96{+0`=F=AX5^l9pZNkns)B{?uui*E4R28-4xLC9GJT!jko zY7yf0QNGNo)<RIPR3ddEbKLa)CnUZ2j<s8LnDYf^F*I@pJj={sWDjhCP1?N>yQ32l za+ML^wPDH7eIT{piuR-%l8Z`(xWmYw>s@jd+V-i_i6_QG?XW61+BaNaUF<<Qoj#b; zB1x{y{eVpo8Z>HVHwLI{(7yOYrv;xwop#(Zq;qSsL9!>ETd*${gJh0ioTWYezPJj* z#;MbklX~=`j3zOdX-g~X_Az_(hluy+lt4qC7VX<z0(^KUXLZnkj(3%ydyh22u+<MS zbC5OB;HKh*p}Hj5RD$si7Krq(3`lkp#cq9R{KyZ0(%Z3g&QBFMF5n8EHdiy%Dbhrz zWjPV;{DWo7M`GBtJ`7iTg-7eYLJx{akVuZqUmeGsSJkFF!xLd_M<4iis-attD!ROP zA+4U-_($Ce<T@6Tx)uiR$xI_TPd;$hrnh3D%U)2(*pBz?7Bk02A=H(f#L6OF(l)~z zCwetux8SxfNs7cz$sw3}D+On-j|3~xH4LlM!43&c=KZIO_&a<Xj@f?$_RpVzXX~7V zPU_X*{309A*H{V&3WT{9g8QOnI~q0%=p(gjoObC_bUW?}vx8!=<jp^*Ow%S$_#2pA zqd`|T=#w(nS56=|f-HES52mX(Ggl<dVfX!cT<({C?t{?N(JAl4=qe=QE7f;6^0Wh4 z>UtaN{+<`s^~Pjmq&Li1tVhZYwcyYnM?iV`3q;9N%;0yec)gj&<Chw6*567rI<JZP ztJ-m3S_+)o^&Qo&w_$s+h%Bsa;UvDd!}Q~y;FH5$oYJO89&p8FCl-$;k*VI~J!?s- z1avF+#+m-dy>YMfb*>{>m2z7dqB}|mjRu~BJ<{vY=d~`K{Nfa&J5`TbTh9?S?$iA5 zv$Fp+XaA=vi>sYrzlkk%DZGbi13u6Yqs(dEEfCHt`eeL?C8O4}UcdtF=RV&0i{BdG zfn=>RMlPzvN0v>Hk<F0w<NwgqOQ`dBJDRj7q%v2|?*{Hm9i+@Eg5Sn(z*j?Fa+A8U zyrQh6x~#mEtfcUz_#YRy{XZ9Vf;8o&9PR$Qs}BF~RoNgN;d{ratIMkj53BgUA6B>~ LD?3?ARqlTQmylYp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.pt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.pt index 277990a2d695715d0ec8365c44aedfd78aa0681c..4835f989be4e98d9fe4373d135ad632f6e53e63e 100644 GIT binary patch delta 26764 zcmZU)c{EmE_&#jNkRc&L2&HI{d3c_)?<3MYsDzTKG)R-8IgiYl=Tsu1S%{S9Ir~18 z(mYFwMoDNMmGb(0-#>o8@B5y$?my06d!Mt<z1BYay07cny&ngii<YSfkySb^6BcSe zf0ooN&8%?Q`GYmXLS6pfyF0R3(wbSz2i+Lz6c)Pnzc$%fnsK4)20DV=<iWCGp<Dl_ zF)KlCy`1K1&snlnf8>^^W^FQal4+LCDl;=4`B++NV3fDIl$2CZ@H+q1!9hVQ*88v6 zwsFO#uuUz}S?|q841e-pAI<P(7VfhY6j9g}SFI@}Rjrvd(%euqR4Q=O^3}mxR!j;C z3=5oOIn_l)YRUg~Im|61=BD9PXHWu7xo{G0U0w^5Dp?2%i-s#Fn_#bwgm`}#D>k~j zoQd(EaK8Ntb7ONg@#}vKW=)lZUiHE)?aFxM;%;nAoeQov9QYB3r_#|QOu%jaW)U9c ze$bV>%+TTYJY3~!!j$*!gA+Hd6Mr*5#wGO=?%&x%ST{qgT3=89%`$-BN7LY-Y&yL) zvyBePJO#&`7enzMORBkc7_Oga3`6Eu!Pw$^)c;Ngl`NhMe-nO^3CncJ>cnAK@?|w6 zZ<$YT@3CZtb{UYH+M)^cwEG<VkZOj1Oo~y_w-mO38~joahh-NHanDT`>NWTX#0&up zsr*H<M<(GC8U^<%hYCk(8e!g-2jtX(N}6RZ!moQGFspMu8IzZfDoM}rl;|M-ye>`8 z%+W-d)??^DyNk}=G>jCi(*n!)x)^z{3zvy@nKSJt@*t+Pg)9;dB9f$o_@_mIH4?Qk zzH@qMN0c3q+q;-sGj6~m+hmNt??OMhMPhB=A}m(jNLn}NLSSnWxNgwHeUOR7V-V_8 z=Hb;0Dd-oUVxCNNz>=^7_$iBn_KIxy7I6wan~vh6L~Tg^Yzm?!{($4k6zLW1S$L{o z4}SdN3QS)w@x7qNOgnP`X8y25rCe!t-Ie_$a)v9p*td?dE`6A?zJw~Xl-O-nce<Cd z6fIYufK;7BWQfmT_T8Tb)SqMyosLQ5{d+q+|Dui4$j>8Tx0GQB|BxyiD#nBI&Tvop zuZTI|cpV*5_F~1}IdH-WAn)o9%<oI0KX2`$oBlfD*B&#n?shl1#h;>OlO0H-u_nIq zwqpHLH^R83OAu1ind&zdu*WKac&%CkS8aEJ%Jm>@Y1jy6bv@*Zi8d5IOQ7<TbtNX$ z7gDsIP=D3IVEtt>&b%UGafV+vSynd(w9nS!=?{1D9~P5;|L!x_m4mQ7`!KfKCBQci z6{uWSOCI$qk=Hf)lJU-i;LovS`j)rD5#@<Q@$hMozUYY_Swo2_8$}teh)P|YLav&d zVZiwnRML|K8^l-XY~u^K+58T3ureRc{f)*Yq6RIZmzf4b4xNT6KYHkq*)G`pL>)%? zWP-cLUrzU57|I3s!MG(x;0%T+_+dp`v4X5gm&IM<hk@K0Pq?ySjihhJMM6$lkjIZE z;+ic|U^c;m|D0)riJClgU#Mk%d_F_g9-9c;N;PTV*$d>{?xXPZsSS!Or1ep~@+(#e zPQ&@nve;Kz%DpV?WuB2>{8W$vAN?-E%c@PN(5r<%B!}UI%`@;DmP4Im*I@79vC#j} z9HNe;;-spd%s#C`n(^oX`1nS_P>pkFYPk?KH>xr|dgV~w63i#RTSg{k3gE}W0+M%T z8K!I87U6D-3@{h{r222gv?Q|}%tKbfeJ^{|E}4rjxiye@qzq`p8s^Hs52Rq+PCUDD zBtnNj^emVFxmO#}cDo-gHuHw*Z|1?<AHh^mkcAJXX;AT@&E&!Rx7^y_Tk*>kWAb~s z50u_)Cw6<qbg;=-d>TBC{o*9jfcDNHG@BetTh&jK=6wO+(NlzlrxIyk$Y)9X%~Jf^ zRKhe@g_E**Ct&L?JN#2*g^TNs(9BdXd|C5|`<F2XH_L9s?q4rx|Feg9tgaM?-A+LF z@Flo@X(u{$zrx3pR8dB7SM0LZ9gE)tATIAFPA-b%mq;cSeqYIwy#Bll-*XPEQvX)C z_oo7@qkV|}eR*hDZp&<_SU{I=^M&p;J8`#KgD}PQI`Nz~f*qW_AA_%I<3wm-rlKyc zbc)9(m+s)Ayed>%D52Ax-=J*bL5y`jfftS*z>%BN@YtIRNZzhMgFR`uF+q!nq~=94 zE;-N9DO3@xL*n7LUj*~*Y&<k49mT)CI;?%~6@orjQ8RoM*Oc5WQNLaeC8<L&w<CpX zU%w35nRBqsxsPbd&16-lP9p_cm2~kvRron^GzRW}Pk-bjV_~%fH&hETqxBW$W*)$} z^ULvA!ZW=5uSkRw4yEG?BMoe5=P_($Er-b(I90w5pUyWT<>PsHp(GD4M$3_!@1^+h z>^L_1m=jz|+k&<HS#tLJJb2YP1foW=G;vf0H2#Rk#e!5UZH*+$i<6*m!Vcoqt%ds2 zQ}L<q7pmLBz{pW%keTTOGGmkB?=8_gD13MsvL8{{<S)a@{LO<~Gdb9^PX;cm{6gy5 z_A#Rj1IcV11GZlLR=BH1fQ#i!@p;o?T337p2Co~#2X?8UPIC-=$u%Y`hDGC{p<Z;V z*?2I%@`lpBCD1T*1>{FAq;sETU{}2zseCpGMp&Gp=ZwlowP;HMR5rbZ2ZC=<xkCz) zzD|TrAqOMtJz!_x6c{Tb3xZj_Wau+bcsQ>R9y_;_F|0d?wX$I8G(&J{j=&ifC#cC- z1w42o2S&kPd|+vf$EIh(jpf7W`lm9uO*@^QtEwc$hu(1~j~<7Du0|lKGjMdBE-$)! z%^mFhp3<!WbC9327)B2VoO5d-^lWv70E55W`QVF~9h421%AG;oXawFJZiba#=fbDa zj<9HV1RXy(n;INZfj!#oq+IfltnJnz9aA)Lj!`U?Z`7tv#d$cX{1=Jeqrh;Y6!;bQ zl?05Pi_#^9aGvuLflG@GerZT1RWb~5>GTG{xnK0jHl+52F>u5+2zO4&!8JpIq1kUI za1-^>%DWzy82Z6j5&);n_oD0k<5c0bIce6B1&d!%wCr3mHIJN#z1787JZ&9Zm@$O8 zI4zM0d!I*YAD7VcDi*Aha2;7X<_fA6igL*CypgaD<T+)pVs83j9WeS9hq`j{)T}-j ziVWsMyOt>qFCRfyy2QZaCwbUYSV&vXErkAy989hngc}~^(vx<N;gwT7Ed4MQ=ib;y zORH<(%c%3j$2fzYDQKoO30ugWD_1~wa0X%GCZqS$FLe8PRmc+kn<=#7{rMplx=?k> z8s?b|gUS=i^klgonwn?8locKH(*jwd6lMk8rw@SPyA<YJTs8)39D}loe@yVGRD3c1 z5Xsv$pV<|cM2CJ8!-$y+VYQDsZd5NuIp+_cb~up)jxwNrj<;x7?*bTSwTo1y9}zE6 zd<>%X+$=ooXo4ZruaL~-Tin0>2g&1}ApWEC8@gBSCTUGLMO7Cjz{46>()s2P>NyXg zQ`T(&0?H^TistSr)L_<d14tY5kDk9Zj$2`#4l=DPXz*xP;+A*}Y-jsZZIvl#u5l6G zhhG5urUI(#BE;yH68KzFBZ8t=N|>B3!>ON45+)lz1&<rjFvvy$m%Lp=E`;6W3XO|k z!X_c#*rkt>3^zEY^or(1xnN{}DvG8P==7M6H;p22{-}%OL)1qyL*AbR9E`<|MpbC* zdPtu19-{x@-l|8PA1aijr2Y%{S+awqq!bmUq_W1FYlw#ZZwzcT)n(}a!odH*;mu)7 zdA*<_yq2+wW%^9mr$SHmX{;T4+24Zq!o|E!l`iYQ)`Io@;m!75_hds4Y~+=*ZTLjp znK(PjmObR?!!Kild9q^~|KD^*S}ksxJpkGVBFo5pgtnTL!~bU%15n&1E7HYQw9E94 zK-O7O>nRx|tnND^Xq;Sfq0mZ)b6>9@9Cu=;q@+oPD;sgm>EI?giCdcwSz|tm)8D^9 z@S(F_^5wLtz`gjVAo0aq!NC=(!Y_kQ3zr976w9?N6HfeQ<}^(Ai4fQJ3P!H45LQ%m zi9gTG6mAKaBz#g`Bl)uHu+SuTr${0(`7Ej3B`@|5-6P&d?+CBGsuA0UFB15D4`9Zb zypb3NYYI&#RL~6Ha)~GZQL-~%QD`|+ii@!RCQMH;kYvm$7e<Z!EGaR&Cq5RsPhy?V z3%a)ph3jI~CGJ6~l64QInMao=OJ>y^kpvt55q2$=qYJj?5Ybpdg06_>(IMO~8mMuR zZrRu_exACJF8wkA{k|tiLgn`eWsXz$8*-iL(5xrrmJ_I0luR0@7Z6A9YWn7KENuI? zjd+<kKvv**D)+<;Vx~3F(F(5_vDZ+Tv1h+<)BHf;{EATK$*6orWU+zlZK<Z}$x|7> zo9g_5aUA%Fe$B+AEwboeYlC;KmB=Z)gX=o3VM<yw=BQ@y>kL*yXwNvlx$p;C$%nHm zyxMT#$8Pd04N$4U6vmC1j?urHB^oF0gN8>7bXqSJrUm_BY@CL(BmCtcELH}Wg?VH6 zn;Tg5`vT}}m=8PF?MBn4{kUZR3?fQytl;M=ZUUcwsnE3OAq6dGobuQRU0;-;X5b7s zQLvbL92w09Y0L0$WD8)gtPEc{xgTw+%E7B_0e??+9!@zqgKe1q5!6kxxh3UFxaIdq z)^AQT%oviyCX`HKjV+b=PeKLuQE>q*Iiv@byMkH9vlNdgoD@O8rfBv=X)QJ+H{!<8 z(;#H+ZMb{-Elk?tgwF4hApLp)OtY~-8y6FHNtiL4A~hB3;W2*xz!JZO`NF$KLs@^d zXXIyDGCq7X7hmX~!5rhwBzb))-e1&5`-;N(IXQPB=foFM^?5uvcdv&{>vV)m&vf8w zp~#i)>Qcnd`UU(B(PZ|A*C77q3|X9sQEbwtOZeJ#A<D|cGUxb%5U+6rJ@hugJ2f@d zyQT!1tVf`uat9+$e+F(tT|vfjHF4Vh0=Ldl=UZHkkl+o6U_s<d(ECu1GW{_$KYl*W zQ@KqZYGs49?gWVWY6Tlaha8w!Q=HhvPcmWB%{gpS@DCU^WHvvy@GD3eo1>D<Xnx$* ze%L9T&p%(Dh97U)vUj`kVaUsHYHOU1dZ!k1HnUXOL%w7AaXUU!D|<D5xYH<H^=dD9 zUG)tOuO`5FeI=Y1(1$Z~EO}n84AmZ;!9y0)!SC5hDw0lGf%hL@1*=!G?1z6HAnmKb z_wJm`PcVtZ)(5IQ^?FLrZ1>;~THePKFZKAJ=}D}E`a3*)Qjtx|y@S4^Gz1TC6G755 zH{r>H^@5#MlY|9b>jc(o-Z)8#>I5+d6C@WDiUb>050f~H3mDycE8*<8n@%xzF4sOt zyC8B34frcI5X~3wAGueec43t83VXw8w$lRPnR_|{O-*@0w1%%3T*rvZH%Lp`6Xb<2 z-X0R{Tk%DlC^3^fTjedh{Z5(b>emv+P8lr@e_AT(T=rK`-7``$&2I^J?6OEW?1i@E zer-G%`u>hksaH>s^`ce$Blf9KvbkK6ep;05)EjU|+@F*$^l1qbw*6f%+!1Iiu=_Mq zVjC?dcp|l5XqrYP|K52EhbHt1PCk_3`lgss8M$s|@BD%P!kc8eqqK&a>7`Ja8$c36 zSI~J<os^zADxT}%PcpwfgZYlLn5&;vNrJ$SQ{R|Cwy0{82Yvg5zkYrt3Zkn?OoCtw z$y+8xkNJ!uBtnxK#*HD<wmc#)Vk^0@fX$r0*93|5lrG_hkGZ(L<2x=gxF9(iSxLSH z{38FVW>6fxnPDz_Q>lqrq&n|7d0OvC%3O=--%o91+0lA#))t1jq}NU!a))T<mA&Lf z>}{f}Q;osN(^0fi*^g^JFJOu)3-DJ_44u0}QFyG_j%YubO>#XGAZGMlNOP6pn^y)x zR_J`NxfD+BYF?#_qEb-0CW+g3>=E;0mJYbQ(}4BLJtRJb6{OqB5Otx5+Md2iT0}tX zj0)lDz)F)PDyS1OmZr67!wkbGSocm^MD;}t)ME4xD%}}CM$MZ;w}l@;ul*-UgZyLK zXWIhd!4gzHsfBI2-?*z{bxt7D0(n*DI5ndP&APNOYs<hycF3Y$xGlU{G#gf3EoVk- zI#1npv2ZYcIIp?=1pVz5MrvY$uwb4eIi8>hSzC5;-HN)ry_u*1o;hBl!{eH0l#vX( zx%)kB8##Jl9lsJPx%o5xf;L92&5*vHFppMEe?VJm^@N?qdmI~Q_=Bb9RA%t#DsG1P zFv!}GB`AxyO6vEiQoZ~jvfJt~ckkK-PAWr_dvcd0^QNvB-f%xjbWbI7mo^^fZq<($ zx=iknTo#3X7aufL!3{nSnK;%(qLDe7+FY`r?%PK(4s8cW1CD^2B^+lpN|8Rhs0G!p zY+&@iMO>t1B2^d`O6N=|qE#o9@nhX!l)7WgeKuJ_P2cWi0vj^OZ1V?<wrw3VwEQOV zpJ+=J<XyRBff4slyjU_DMU2f`EfAgWHl=q*XOT?-I;8C14sz_@WU{qo9A;n5!BbNW zL1*IuqLe&9KoMzP;+xM{mt3c|(XC|A3Rf80X9f<r7O+EmIGiYbPLjtirHhj%V6yx% z>OXit7u#}^8J?O!o^*ee<W3kua*PjC+-XCTx7uRxrTa{OpOOf-5A6}g93IOV--?t> z_I9Uh!s|Kf@Hj%}r*Kt&2hsWc>2Q5PBv)B_gLV$CBwo&@)azv?jZ0A@=e_1KhtDq; zYgvt<HJ>u6$umzXm6E}nKM+q=YCjd6yjCNum^PoYy<f>}s!rjOE4H9{p&XG9_F<$7 zrqiVkB3X%iVKpO{9Y@CvbAn+cMEn4Im{sQz={@5h6vu>dTW(IF3VR=rmEAYUpJT=h zJd31jkB{MA=G~I4$q|#3@gD`nhn1KED-V;e&h`Y~D^ZUOPI6=S3bLlIpQ%gp;#^zD z(CC*b%<vs+g?4TeNyN7G%=W=eOtmPsj<j5nV7W9)#m_ys4<kpz^s5u$;J--1H$>81 zNpg6-<2~)_okeb~3E}(<oni2rR<TKjKK?qGMdT{KF$X>m!nkAwI4q+?M#XNVmEvnu zPklUIi+jbqwOR{H8ZXczy6Td(@pll{-a@aC{UmKiG?^H2i-^2OY{cWEBB=M(cU*;G z43$l9C(AC&kapc@_>xjVs@~q^;(yveR*DDFYogqR-&xeTIR$OGqnK`(h9k0nG7G+H zaSwiGVD;A$!J^eo%+Kt}%=PV`NS5UZ7@sQxQcg=@`@@4YAvg{dT{P&_S5>rp{vdKg zd$9=bzTuGy7eYdlB}N<h!=J66>B9B{+;U!)&M`T}9i6b0G&XL)Eq&8z{KP**$1s*l z%AZHn{B%)#IE(Cz|AB7C2JkXp2E06U@ZH`++{@051RJyI7B@G3HPwJI&u&2beQ(A* za3U@pmxjv}qKKc|c=*T%J)zInAB0rRpEOy1CAa>W2{h%sXKudE0=wsKp!VevSj@?S z9V>>T>v1KlcsCnn<#^#m1v}g>mrJZ-jv-8+js5G)LGDBzc|i6v`{xA0e;{0Te@%?D zvXqpw@&FgEpS8xtQxqBCLtbiR(XPAgn6vH^-O@M@K7DF{C`KQ8FPq{a!ym#O)zg?q z=Br8j7jr&ob>RS)T}tn{<&u|gQ;75XdRnpg2kD$*D9l%S!L+*#CB6TK(^1=lY4Orz z^51JaZ*7<!Fo6C4i~m!K4BQOV0+!~=Su<VjMV9C9(nk)I&33C|EjMIPy}6V;es-I2 zvaBb~+5vQ7ur&-S`in+3BYD5wS!6j4=Y5P^=^1xZxZXR-c~JC(y7z`W+K>H#C;cu% zck5bK;Yt}6-TcW9kB(rE)O*1WV#sft`UBHYkDr{a$Uc~H5T3Vh<#&dwh}e)V(TrY` zDZ3;*lW(=KMZ@Px0}NymbYLS<3-J@ip8HK&4SrIG`5&3UVGi(hYc5S|I!_YZ_mHK; z6z-I7Vj~V)vrjH8z=$ycb^i8G;7NTbZd&k}{WI?zt2|~EKQb(vRz?)C|FRnRJjV>| zJrqGC2IUgH9MNPxf0iAqk}u0HKdH$d(M^MZwZ<Str|`ughVb{YGKpBD0gtY&g7dk_ z5ZnKQ=Fiy7wXdAT1gx+nZlSG&pOQikELjG-p5%&Gs*a?G-YnxYaStnrOU0Srik&Z% z3}Fq@Y;aH*;m0khX5VK7@PRqU*^$Z-e3t0ZalWWElwYq<fGP2B@LkFRcwV*~`{j~h z$Z<u+^%ak8emZz)_W^!**-(C7TPJpw{UXWtw0P@TgZVL&G&ryBJ<Rsy4m4l<jhSUD z2Mrlf#Cg$Mkk6H6)hh3Cacdm;-{v*^J@qmCMT;B!+4?+u5^lt*`|ROGyWZ9Dj{S#V z-*s0|I49576ev0O8^yr2llP&1=3?|Ly~9pWxs0<yZsXt~JiF;%G+wPek0Zjqg3)|6 zHq`eZZofQ%E=_i3o$t26{$1)Y&aH?P%^zSSn>_LTa7w@2ujG=&4y<{zD{H6nlf;g< z$C)1H?1oHV5jw{a)^hS8EVXZ<nzvMOr;#)pX_b!mZ|}u_^d^2c_vPRC&FA;0#$e#c z6cV}eG-Ioxi~ipR;p@`#;{331Vy<=o3XLi-9Sr!mivmo8cf$1b_Beg@OlsYGi*h^n zaV}GS(;esjk~3AW=%>36=!x$SNWd0R7HORk&2**jq}5seWI^I0{?dUN&iDHqG5YY> zx@P}Gu(fIiFUt~MCOVgY;X08nWJK)EM-5P_Xa{d~ZqSZ{(^!uaP2jQo1-JaE5j%M7 zI$Wk0fM=4ez|+ecQ}TDiwV^t6Pv2%Vx}JdM?HpA+cYx`L)WsSRbDy;A7m<69b4X)F zJK0))O?cV-EJ;i-MTKpi7cB31;193W&L2J9>zLKO?6#pNaIa^CP{{qplsgY;-<>u5 z`m9~-?%l^h^<6Yx(6~=WTN1XtkK>Mu70@(lEckZqAxC4r)4RqqSqGP&c#%dyieMKB z7+*$2FA8swajT-~klhWm(cc+Voab@UwWgT!csez2$-_O;={Ur)hdKZ5F8RK?1FT1f z^Uh6X{A#rj{`#UbY)auJQsq9GRs0jme=T;v5k<O~l{Aib@LW%0-U&&}<#4KIyapp| zkQ8j}C1VDeLS-YvxtUhr%N>0poE@_Z-EQB7&~4e=xfvC7Q%a%Wbo_44`lXO~#Lgxq zX3EUpl3M!x!xJKRL>luORk0}ZD2)qU#K&f^e9-D+tU-}XU2}{%q;0C?BKsTo$c?uA z>5>&_^EVE9i#_<^ck)>a+X@&5b=>#&*HFo^ik+mFK^9yRnLy!#EVhO_!Zlslfhmhc ztR%1o9*(o1i?nYOlliSUR&^sT{^m_zL{7ql%N5+upUPyz?R4>^$~VH7XQo4(!5*$( zW;C`BJ3wT8ETOc#h#z6(U)R{5&MLP~;~fvIhd0g!?5@1Iys2wE{k?7pkg`bnbVL{o z65W}Jcdy37hvQFh(3xP!P2DOH6sz$2BN3LhD{;0NXNddwy;xg%112xpN9^?!1$M>n z=wSI9Ou^9zvY*T3ynZQ>sc&x)ocfF^Y8`|h59fk;`BK`h@5amSZQ;*l|Dl4_0=}Zh zls8*70c#y%aQ5tz{LEr4R^)TM3*?%9^H&xvb?&qp%g%hd5<Z7GViv3Eyy0~fCJklz zmbo`rrBY}9>taPbYpnwdYadDim%hfXjQ4b8F5>#Zd#Sp#72aFUVkJ9=Ng5kRf6{tk z?)Q^)^~HF`bKFc?QGb!HS)t9R=$i72Z?gQo%WZYCDlbHAexnxQ21J{-AJzOtNfiI8 zMvgV>yv+5^NX7G+zaTAmJ6-yC4o<mKhFUNTXRX|anazc$z{t~kK8LVcb}oPA@D&;) zSU?7$CG1<jjP0D5$o3ykqKDl65PL-qmp^I1=vH-z+8_(r+S*v*n}fxiL^7mal+7M; zJBACi)v4L25zcJhQr7&NIx#+c0gSAE(f4QTz*@DCyiPK4HYFz^&U-JqyjTK~cAg7V zUdArFbbviOQ^4LFIhHMWP|dD?ZpKE8n~4?Cv*`=D!Swp7bJXhI9GWq2CKuGJjN_+i zaf)skv}z8|Jk@i77a|*3va`$@8@0R0vN8uaIJ*FbU%JPB$q8knJ5*Vnvg`a}UDZ0> zJubM>_B>m8GmV|HbsXr@dd~dhKR#Qe&Cfk{oSrv02&=cH^76{@sO@+ZrIvg0Mn{IR z{kAt4Bg0q>U@NezHxq`7uW;WtxnuZr7e@X2JBdiFrh+caN78e_gI?dFjR(@qA>8pK z8RNU1T#(79p2vX2`c~GUI0iP#Wf1*O%h}p({pfizk^S&#Gi&cmp);4D!VxF-pw>fv z^H)tg-<OBp{&VQWp2xhx!v?55cM{%}SMcFe3!vSl1au~Tz$GSE@cp`5BCdb437Djt zflQ4oEV(}sF6iV@Q<GbyrQ#0ZI~ydeUURu|t3#kD^A4<091FTwie}9z<haKv>NwZA z&U1zgnx|BQPo*!H(DML}hiH+NcW1F*#n$|2X*+)V^i_E9-&mUSx{4eeUV&;`hww)} z_ECi?1NyCls2O^)*TRrV3+V3ce)QPkPZ)f#imW{3%^rB{$a-;#aM70`!1LtwwtUih z_!(WOx|15L`Gs$9-NBkzS>d}^<EhEW32<kP$pGq!;Dw;+9H^TPe+qU&!7wxOZFUZv z&iP26k4pl7WlMgPP8PpJkjy$hxQq3oq@`@5Y7gcp9Hq^iD`+i|;-BTk;Z$+=z?Ygq z*eWB|dy^W5?U_aXjamjr_Ul7;$PgScrHbkt{Xi}pNFmKlnk4<BBR!u!Y=GQTLY0pt zsJu^!t>BH=_~0IDc-6XY8{16O%NMay%N5wpw+d80*^XVZ@iq~;=&$DArw-JX?zho* z=wsTv`3#=Z64Lqy4&-&=UjCYIrSOP<2B)CD10PO&gi<%GnL%B;80RiwBHB;WojNI; znf*9&w8$KG{E&fmHCfov>qkEgX`*)|+4$y2GdWE6g0Zna^VC-ztvlnLk4@)5H}4St z+eTglpPI+;Lo>AahJ)5PCSf=`Dt{V3j5){@%<ZQcpJZvqwQQ8so8#7wDwsaQo1av0 z4el3?hE1U<G)7?$=x*;PU7nkT$<RkT^!MTnxrKD%^vRI^aVs4FLZrKWz~hlH9JYsO zaYK(r!k3K`AnI8W+TT0`R$-!XeB`lO=Nv;1_DR_%*4w5Ln`5G}B5g6<`PrBaTzeQv z#x7W_coKqLmf+8A(%{ychQs4JN$8*figevDh~EUX=4%UcN~)NoyX>R&!*>%WkI5L^ z6px{St~f^SI5pj{jGX=G4A0z+z%_IVru5q4{q`JAERwMo@(CHlC-xI|CKl1g;|Cy| z9fZF%W%!*c`D{hzcJ9<wQ>+CFSI4ytEE`Au?Z}nvd5tc3yu$(S>{)^lJwB|cVlS3@ z3Grv}aCX_Y7|_3Cz$lk=l2d(m=+yB&bVau&`dUcyoT)bdvqJ$Mdu}7Y3}uPP?BQ|h z^Whw6U91bPW5r-6>W610@9D)<19nFEQTDaGCO@R+FdO*iJIp=1pE<nNhCQns&u&uI z#+TbVg%yJqvzmwH>lT%c;unnBh}*1((-4O?zClNY+1zam9X)6H$9A`%>y-dX)yhy; z<r@~(#=zTK{>-5C!6No^lo}flHx@2w42PxHj-t}+bGT`aCf9Yv4IEOP*gz)%yFS4U zbaGN4?z$1LYI1}|i5;ASZcbnyRJhad&J$4oR*qlmI*R9}y+Xs-RInJ@#x{Q*%1$ci zCL!O<8Fv44$nMLgt>4el=*luOvqlEi#mxf?R<wbwy%fxx+oO!7xjL90*GO&VeW&WF zUc%iY_R*`Z59wbkRr2TK3Zl`j0Ubd}_<och28f@N@(3PQw(Q5TUl!s|=LPs<*6_Mt zc3J@Gq3p!R6X7Nx)b=E?F*!NFUyf!!9CrfaWlC76)P`zv+u+3Ri|8miEX}U)j$<dC z9>ZSylZKv-cW{hTBAnQ@84Y^$LEj|;oPyqP8&1umQj1%Nk8ccF@Xi3e#|G1bZl<7S zJqaJl8dGz%T)KXiI{v(2E_~3?hf{{kuhX5}#BR8{iI+YSM=;3;JErGD!{yQZ+~AM& z?f48{vKmn|Ja`S2WAn+;@yfhi&=5ZRr_Vs?>(0Nq5CyxdT)^^hIaaTH%lQ8oM$F>X zF|X$c_6UyScbCCL-+cm%3_MPf9{G_grx`jcsEOWfOo3}3jA4vDPs)~+vobFwG}uRg zGfbT^<Pc@;UDcfJnRjerXb{_CXCmSw0?UL3SzT<m-DsYS$|avIHQ6uv*Wkp;YMlFW zEgQZz5+{!h;V(Xp0NY**qO9=)x7g)CjJ*|G_Eie{_)hwMOg-&5sSMnvc}!=L4CA)t z2DfIpI<cutBO{JfGTPfGf_v9xqI#;7I@!1IdD<?xR}@<Js!9}7*Rk#hZ@x7bdq%(E zzj+iv<EDJ-b9oxi?b*Wi8GQ%CeaE>T*(Taweh~KLG}COuLb4-Xft3q7fG3j9lWw^M zB+ha@gdSXrM~9k1`~(*L)KbyLGoI*ARf54iN|529L(8`|lH(oGoWgT;;pe;e=#jkJ zbcbkJ2HnBP@CxcpY@)_m_Wk=&&bhhiyoTT%ky8=i!pM_s>)QQ9?fN42xW-4i{>UtB zvi5<M|9+4w&qwpSFZ9z7Ni|@2$r%FjlHpHg6xDjQp7zKdfcjg{>2YgE`fcJU*0XLB z`&;6O6WzyQkqjkE`AnMjxsMTfpJcGsq6P!(3sK9YkqH{RjVzk8n2p@r3tn1#>bkAh zI~Vzl=k52XqI+;0S$9szu1U-y$NfLDCDpI_Hy>h2Sm!x7c}9`lqI#GQZ5RfhrWK*D zY6}1A?LNBlwhX3Ux8vItBiXiB2l2p(3*-y4Ug$)JFe^6ZiwJF~X6l<Q@E_MpuRkgv zatjZ0FysoF##ds|@8KXLkm9fPWwZBv)u7!?&N&PhVb~;TeqH`D-u8VNt9)OXRpid{ z9^lW<_P)Xo>K%!zpC)2}{|Ve|Bg8@fwAddX6Uez_mbVC40SjClAhs=!Hxp)JpXSiV z|HG7|IkQ18hDu4j82Ud1O`?ars9slxincApw;zT;#qcjw{>5ojsU0lQY<fw`Qs<Ku z+xIbNJ{Z99i0!0rUj&sqbpmBy&VUg2Tf&~Si*%c41mt%wrd=g_7|#H0y3sz1VIORu zPMT2|1$MB>ZW#J5o&}4;o^ZEwD(H5tUxM7#(*MI3HMJ>7Ps?&%<Sg1{d<p6We!S9M zJ$C6xd-&*b1<gh*!l<WrX=9Wf6VM>d-g-wk%gehkd5jbr-xn>^bl!og%MF>xd<!<! zK8<5l4&&=}$MIBYGI*Ldps#lWvF=|7fqRrtIc^5Ly;BCQdMEHtpE7g(xeDy;uw{i) zw-bx|Q$#o^*Z@c0zXVrq=CFS&f5XbN)A^Ni&Jgp7M`3fS9^P-bgPl*m4LocJW(KvA zj5o8{V`WmPTzMXu)|vFxkqYwe`ZE67n-UToY0n$;5xjk3HmbS$v7)YiQrN4-+ITF+ zyMiw`#quY{c@N`{pWex)!U@UpNus^1uHr=gr<yaImzU)ue?EXIAtwAqMFaNEP;0*Y z*hBJTjusk!Jn5wMyocCrn@jY^hp-AE+4SC34OZB%gA0GwlklUueA!A@8enl3?k0J{ z)o-&%>LCaIspUab+2$@0)GwqlbGDwhZwKa`%AX4lRVIkd7v@Xui99qV3W;UH)kIr7 zE>n$pQ)DE`JeDFXZ><+UG9N6i-v36pcxIvC)ZI6dg73#1M`R3=oONBmW#vXQ{p0UR zqIT;^#zx%~+=}QBv{asDI;Ls}+m5XiMs3d$?7LkkZqzpr*a?r+8kJoTr??D~Y`Nwj z2s?C2=ySbLD3aa6iMpPaIVJWk6D%AYBT4?dQgU|A4e?~hesM{+reyBZV9C}!bH$fc zD;&k??!v|T?u_{2{aU~C)0hXUYC@yT3MZBBdU0CJ1nyvZo72uGV}%N<zDttwr;3~V z;v_}ye{%gJ|C0WY*Nlor7FRGrmC>HFh4`J$f*C8uqn};6h^Qym(ww=LWWn=T=47B6 zS+KpHx$)76lY?0NwrmI$7H{F~ee!YY69f3DdKv#(91>1U&X8p8enMJf5aPA;n94&t zxxqqd(lzZgS$IlLFgeplsP}PzoBsn6)j@Z}(|oQ<iiGckz8TNB%tjqSTR<Vx;`mx( zk|5eC$uew`%s)L+IK_4a!v;1>3K!Z5*?>Ud#>_{8xoKM&_41*lPb^PD0^9}tAyvYd z@>of+v>bD}M}Y}_Go3NuojA3o_maTnn<S}Og@VJ>O>BRbmqg_8wVbh?WODFz$>Via zT!_LJp|txZ!PMOj!bhTcLjQj?;-8|~dnNtH)(Dd(*VInc%NDM@zeQNLD^;9!s8%f9 zkSl<nicGxS-P+&EzXUxRff9`<Ey*OAMMAZu62Z5o2@<XA*CgKqy@iuw_tI4_evqy4 z>P*J-LXMl1CG2RdAd-LHaMt3JfTJT|ZUCjfto+I9YZek5@tI7&yMTz+X`N(RaxYRG z9!)P!P++e6THx3NLExDlk9GHtaA#LDWOQK(Isf?qd}$s-JpX1f=RLQQq`)k4f2{`h zoIM9YrUBGePmyh&bpUQ?tMZ2X4?*wtGAL^_!<3*R@~t_Vvv{P98IhK3&O&X@(<=zR z+m67I-^)ah*W1p`7;L~_|27B8G!%)o)KCl`@r0flyAdM3y~LQ&%OEwi02eOmqP2yO zVcVExF#nGUJBo~fuQe1Jigx3t`wzitO$4i(*vLPQ8Sv>18U+v2O<?bftIR2*yXdY3 z?26O-an`p2?3%d-vJRi*!k?9rPt!&9lB|o37;mD6idnwo#7AX*&DAtMq?g5F9Yxf? zbB4TqUqF|~a)>&FkJ1(6(j*^N|L=FSc%BNA-B`X|dpy7Pc_xh8XwNGDz6G|^=MwFJ zET{`HC3}_ML4<f1wp|;{*D1b71NI&Fymt>;zn151JqNL>qAT8DBL9?h6uPsy&C#g4 zD;`??UC7pk2wt{aiY-r+;_oPhVcIEu{!_O|V!mV@!;JeWd={O-O<j7HD^_yl;)BoA zdD;0~fAn!87^VviHkm@rf_qGDQ;+a+%Y3rG+)4Orl{|G<SSjgVc9%(gsf?L3{?adh zl&R>hn>$x?U=DM$Uzbr-aw4-oNed73J|Zf^k~p6pC$9O>P0n-wDdz2d6wY)hl=LPv ziq}Pa;vP)vqz^7vQek@;?Vg}XL*L85+UH}r{SsT|iP01~f5#c_mi07dduRatzV{E4 zvwjg?bG#}1COJZ%>~^G~szb<*@E0PkG$(_EW$qpzmd4OoGi<2)Qd>0tdqt8jxFEUu z(u3Z;97Vs|+137@5yB-(ttU@s4iw8vQ(=d2925!{a(gY4@TJ~5=3@L1+AF`BfM6Po zklVvW{i!FKlWXZ7<#;gbc}Av+YiOiM4Gs0oX=L{-vi0CpI+qoFqz9kpkrSCg&|@+U z%2`d~{PcuSPg{{ubhjlxx2iE#tykc*-X+HW-)WkD-yFA}P=V<44mesrh#K}E#%j$^ z<jb!4xT0eW)Ma0W;ZJrF_a`4DlVdx`ZWjl-Irk+gaEnF5J-YmQFCT95c1j~P?-2Q` zUfg)memnZ%r6;+0AQ_`m!$`@rKyu-J5MEdQ%B>lB9AAeeLT;|RlVtpWTYbGgM7t=^ z4mWw+tldO5yt%|=S*h?-{Y&U|4ST8)ahK{}rVN=BCJ~gXGi}@ABoF5DLe<pS^y93x zP&OhSUag3Q>{nKFzV{d$oizkR3*&M~gJg~Hr{5$T7A+>$6R*)DYx5y>@MwNe@<;OH z)+lafVmO`q(w7v;4+z1l*OBr!cc}A>Yak^(m8j`^Vt`dG1WuWZYt%ep>(d+*uO5LO zBQzjoqdI%ktq2nwmf<i>HKyHOlV;~8lHIe!7;bu$hUaF8sQHllT->5!l6vU}IllY} z)JG{xngSJ&<^<y4(1mo(jZ^fr)l|yA&Bn5nBqCcJfXACH!AkiM%(FDc;)dlkD&7)4 zYRSMjNfvk7>mW7n9n7!un8@ty7;rR8v*6}?mgIE3psbMqOAZ!b@R*;%Mup*+#EbTG zt^<a1nJJA-u4@_kuTLYp?VB-Nw}k3CXw#nj(RlswXA)$+nKt_lLy5vv+&#vb8YR@x zaKC;kbJ`xZ&K`p@-kHRwF`rwyxdvJ_YiP!c7))0-6!yyvMUl!(d~<gcj>+7MT<%cX znm!-67RwsbIx)Q=nx2l;AAZATmAy12WFNc|eTSIAcF>o&mfLhQm6+yBlX+QBQFpQ< zST3odKC-j$imwKcj8ELBMU&`#S%RyCYM?pl8!hjk@FM9HFpK}t&g^J1^^zE>-#jA< zeShImygGgFCXig4*2I)uOQ|hi?kjAqR_8=(f=v+r@??(gebU{zh{>pnCd=YdxOh!1 zqG(zwd1Z5&Rwgry-#jC7FEE!Y8sE$fKe-UO114zGDIsScToJq7UC79Jyy85+<Pep% zZbs8onp|0!A#`6ik694<Njxt~gZe*OKrCX*>EO8I+~X5hnWe)Cu7AHvL?(^jOl-Zf zNMDW;9kWIePCPk6l=mevwZDHcUloP|_auaL`|2}sRl~4A$DJfuZy3nHUizo|5fl6H zJQKZZ7CjJcf(!jlb0x}6G~cv|vHf1iwLUz|Sa@$CZOLnCPLVZeXvYw@yfB(@QU(6@ zDS>bDCvm7n(0t5lHm09WM&mBm={R;_3hh0Sz(`6rpu@MRINsWYJ|EIW@^>tuMyprA zijo8pF1L*vJL?kni+w}(=RG0GyVsNbcE?~JnS?f~2Z;T+)&JwuT)Al2%l}K0?F#S| z>8%}#@<EQ&r#up*)gLq5mRgR#dkilv7)%SFuHf1`mH01}<z(@X8OU7Q4wA+e(m*pg zt*<KN(&ML0`aeB%kq@NdUywRA<x#o0vABBhXJ)O=QSSCSJ?=kEHuuuepnw4Z_P_Z5 zYqF|Yvm<RqK}sTi%76(c%YHKJaW<T)tR2eUb1VSoU(@)KO(}e>Vm^GyiDbL}Y-2A; zoLK3R`us$&;K_OmG?Z~>?J_3ui%x{|$vB+v%%9EX$X3EK=fCt?eK>EoPJ`b#W<M0n znav)Z){Xzn{t?0JR{WtuG2&DOO0W7X0@<N|z(aK?AJz03UuY=tSC&e%FRp6vg54&d zxNhM1imW+5RmTXlkDP`NWg7hWh7L?9WZ9XU^!O=1wD^y~5ApBDbvWn71@PEr%xe2; zuvf1Yg63CsR(IkvEIQlxKXuO7%7Yu*2NLmL{GVj}=atno`rj^9o2f4PS?=ZrdfRvF zfL+J{(>l)`|I6o@ye;`E1H)CM{yS!->G;2g*-mw_9q_i=i89)&sbTg^XgAac&D$Ke z<iL5lK)jng-Tj)=4b&l5%U;t>>vcde))2pXUL;3Pr?4wme*pW(``9GkEaw$tdRg)y ziPu~?ik&@aHA)Zp4haRP$*AYWtcJHY{kgjl?AyN(BS|W}p7k7eC!S%?%_ms>F`n3Q zk44}V(}LH+p5Um|Nci<-IyjCW0{eC|=wxe2uf7>hnx0=JXJex2giEXNz|no;;obw= zRn0}__@;X1=u{~x8x_l+kk8|1Ukt&QlT{{va`?lFZ(ih2O}~bfRhk2b6pp~*<UsyJ z(O&*Vor80KZYu2SZNv82f5?{((QkNXF^4Rvjv~A6#F0Jh1lV{Z34d%5!Ie&qj!rIy zF-F~FuuC~vaT2LULndic-%XjZpSYY2lj-H2Pu$ai?M-#jGjhGp0LFBUA28;PWfzlM zc>DMsHuK&n=ej0&=QAf=*z#2dY^g8?^_IA?Znw@r<KPS^5`Fmw^C#@Wjb~i2eq|1* zXB2_<DLGcde<r$i8Dj}+2O;BjGK(j^fMuPxAXOoav?p(+XU*jB!G<yTZKi-$pISwi zNA4ln6Q|N$``yVoC+z_*{437#*9PKgtO_O*1#DGq32R=w0?D7nburS1oiB2V?AWqB ztVrsM8LKkLk-uh~3GJx^wA2hSe`dA?hQ%#s#~;Z?#R-p5x_c#T?60D$T1#+8tSYPV zse#BQR-i+aFKayV8?OF(lC(G3!zqVqvTNE8TBPAk;twq)Q+6uQdneRrO@0}zK^rU& zE~5r#RN%Z&hu802$q%1jE#ec-mN-xQE6Xp=I}cXeaax%23fr!H=Pz$SlJiN4jh?5> zcX1`qb9^vgI@^KmdEpH%9~k!0yH$MVgX<95)&&nH)Zm)Ex-jjF0v|GF6dWF%NeeOw zX?JxX+d~v!<I5=|@S+SI-01*|x?Q+BD`)B)wvFzaDw+V&`@hon)=4b3sqmE>K4Za@ z_i*iyp7ZRI<7`B5BAa6~4o}j@aI?z-kKrz+bZ-;zYNg<NRSp!Yim9ym9GG<H2Q-|v z!rFxWP?fVArYLLE>!tB<U-l1Kw#b;qnWxb5=Wn^p>c@0<h?vVL*(XfOX{SF$;`<!8 z(HL@0A0t~|jiL#mvzT#8LM}Dq8nyqkk&o+4hesnc>#!n~y>YaOn&$RFpneJFwN!%5 zOBs@(GyyV(^kZ|y6mT5Z%PovJOFvu6LEE^kkaa_Z9`>WbC~z-))f^-^JtH4p`Q%Cr z(+<!-NrT|f%>jq^)&~agb%`q885vESK6Vg(x(a+&xkb--w-ZkDHCHi<qksNRK&L6z z{KDPR&T(^g@LN_6uS=I6&cDu;=M5@d&`o6v`(c$T;N~E#s?+C3xmM9}_M`ZayuI*D zQJ&AtJ;sgRoW-|1I}QInoW=cb=kUHFbN1fR6(Sn*@)W#m5%3F`3uKnvF*5u6654n> zis@T1g<kKw!u@p6p&LFdCIje-P8OTcRUThS!v53DR=LkKXLANonxDfuyEgH0MiF)Q zU+J^QG-IGBy^{Pe^@jXnOQHKtO_Yt-=EEaeB?kMfP{!&$sXI4<YuA$y_njgWY}p-3 zrco2-HG7;i^(vF=m31ToX5jfd*Jvkcq#w)zsTV5^hx1xU^oL(`<f(yd3>cIIvbONM zxKB8=?Gv5)<RyK(<CGxHX9%hMV8|ERU|ruUU)DU`pI_W|AD^_POZ0!06IuVoIJavo ztFpNeiFXF>6LkzD-CL@ebNSWuBLA5T&7RGf$(}%_UY>+GTH`9s1awsUP5&Izhcyq1 z$r&Ggyl<t3Z=GI|3#C8Ehs$<o85%-M6TKMr;vQ=KKn|KsG^o1pK9!yH0DZS6u=9NP zg7)QGb<3vT;J^5sg+2W@@ac{bY)jaHx3#o*7wnbN<LyFc;|`-YT+9x8G9;vxtT6My zxMSb&=ddHN<eD`jr8Japzi0@b_qAf#*=*wcK^A|t#gac3iukZ+6}fKTN;`^glKLh2 zWZ55S^x86to|!Cz<-cXPU2&_)ov5FZHp5qF@ic_3tJ}}Z%kOY5&Ttn|(=A)st^RMZ zK(!Ol?;$z}b*QRODx7iaf|im{@XyMI-FYr(;CqCQ&(FhuuN%O2Sw4JN7*7`dGb6XB zv~q7OnrY`(o^g7wNZw8;q$PcN^z$NZ^4jqT)vA2REg03!4U7CmHXZy`8(}yVgh%d> z_U_e0!$Y(dUz_d0K}t`EOdsc*@vV=q*x^pMtlEGzdG7e*YCZR(pac4cuOeFxs)2Ix zP#~LQxOK5}iK)j1SX-~p={(QoE*3{ZYsy{Hd{zy8p6J63`?Vx?eJ>MhLb(IoEXYUB z1jVOb^xnHf8nG%uvi8dsaQ7FI4w1)FdSvo2I4~e_YFGNw>n*mVw8ozglTG70OuYC@ zzq0u?g(WbwSC%~~e-lk?ng>jER$z5tF1LK599xpeK#YkU1iiGxKWEdRyd#)oznewb z@_w>~%Y-%W|I+?S1yFa0rrf}Zu{|$AB6H1^PCm1j{7|T)BANC+dS%}PvP<%jjI;Pm zYq!nh3TIk_-J+k|)sKp_@{$D;96SR(>s<NR6VHg_`9ya9S}*>6;3sM#yOtWnU&KYH zgphyU1p8x6S-0p&qNl?Ui}MrFV!0pepOsAZsWl1J!}W2)v^cV2TMFqn8v_q-ThX?} zQW4{=CxdH7eIYXst5ai*8K^E}iEn4GhX(Ea^uF0%8m215>t+eG=Ya_)^ShYrDPGHa zmLFi(_77&I9Ol#=5_Pin8(-qnf9AZ+gf8Gc3}92MEZ=Xe1B=vSQO|ubmiD>fE4wI= zJ|xG-#U`MQ_e#dNI2|Wyi^gEL&O7c+XAMqR_lmT%Y{#424Cp!gj9SVarTG))k>rV% zP(EE3y`EZ-V83-jCEXX~ZJZ5ui~lk&zb27;Uu<x#`Xg@nq&l3wD2V3<beDk0GralF zE_|nzN`#s*xT^C2X56yI7dor?vAgF{=eAsWcGF>6BGMz&=dltm4!s4h!W>{(%pdB1 zaREH>@gp|pOYo!e3@+n)C^Iz(nMwHxL|#3c3(T{{X9jD@)?HD|O;H9{Uj30QP?5re zZz(750?gI23J5e+=4<Yn0M;Gmm!}QviWzq7yOAeA=6OD@xG@6kK1ZPZgexeTovzK- zXpG@)q8C8fEL(CiUIT07{-3tK1e}Vf|KH8NM9GruQAvcNaOZsImXs|iQfZ-5(W-^g z;wD*=r7RI;-wJ6VV$K;PSxTt12&qJpHWg9*Ztwg2-sSiI&wb|3o%5WTInUgCXU_R- zvkAX%m&Lcxe%6*t(V?31?3MIrSY%vBRC=`OPBU#1x$8Re=ny64ep2L{g}_MnnnRe& z=h4WX+jumn2l=*e!CChViX81jC+5v%otB-W_vdXz7Sj7F1H&vUD+>S6gDtgu;;W%Z z&)bL5>DmmU;J=C$k(xwbc^g3PSc$XD*RNR1zZlQ9+sfM1v8Z%oF0;?#1u=P;C2)Qw zQg0p?p<~V*wq_)kN^#-hBArs|ac&*MA1{xe-;^O+X2oF%wN9>t?^%3r;%RE1Mu)(t zAwdF*_d?=FV=}Owf1FPIn8e$0S%qE;H`wawVbJcHOro56sgVT@cuU%OvcIta4(I$t ze$p<aLgo@0Aeqp;_nUyk^CqXw_9I!|JN&f14=ei5fg7B0utB^Gf5+P7nY;{6sx3pG z1v29#*EU?Qmq%S0EyA-uPa<BO^YBnhCVI@zvxEh@<A@G_FPW-q${xv-r2W6&V#gFQ zy~(f{8Dxp0<Gb|9fze|8$i@gFIzk!IZENA^#j{9KH3vN}tH+-o@?qWMa2UL<N%$_Q z;1d>&776!N^!@2WXDuYD<v-`5#SP-*+*M_ed@5gY>()}@_30rB@)c;?KP@zb^WGD= zic!~@1Dnh$w#eS0&eqps!?oJ1sl`gR;9fa=N%mvap7gTK$!YZN1*_Siq*$VVHx?U< zI}w|FOQf`8B3nFoglNgRl2ao^l$Ve=jQ=T&!zC^wpBu;Ubg45HWd<8iLuMzE&y695 zBWqv*=NDz<QOIY0aOcCFqoNSa=MjndSMZB<=FmHBG3m+Qgj!EFBKw{|>Z4*g4){XT zA(8_1>99OIuki@I@jy4tvHHXst+1p^O7clsTsyq6jb$yqcSFp9<FxI|a0sy74Pjs2 zV3IQh@boq~GJiUrka8R1I8wA#a}@D>z&{UL^cI2Lt{+JH*lQ$%p5V7jmIHZGh3ZQV zqv)nR(0gD9*q4uD@ql<dev><H*jSABGm5zW#W;2%XALWMDw<aNQbjL{Q02XQc7|T| zcps6YnuzZ#UFi963-~i9!=tct>^n<^dY6(!j_$HV(X)q<EB^+yhJPWFEM+v%2Ze6* z%fkaVx1NRQi>hSWGhJwumnCJTa`;2je&*A%3uvK0vh=t=oU6RWMWAjLfq6^ID@;=# zpw2ZlXf3ytp`2~mrn@KcGhP<UKbc8Sh?!d1zQcg78xm(ttS+FV5sT0{13mif;Zl+% zJ3{_koJjb>f8xl;OWL4ZT7ZVHR4^Y^ry}zaAsjBEMmTF^fIHKP+Wb9}99S|1+PXRT z$82dbulX736-cea&0bOsXDx}wt15i)O&Jwp+lQ>=`*AaM1-mxzkg1;)dr9R0mef4S zX2rXbcRzf<KjIo(kN83?&P*f2O)`8EJgSQ&kBdRZ&pK+FMGB>~=^X|8n(^VmDU{=@ zWkgwXH%hQskL%0RQPj8tMBK5GY5DGl9LI{W*vNGF?tX^p^}EGg%+&$stADVn3<X_W zgw9N3u^Pq4IjSb`)yt5vu$%^oSe-S9H^4haRq14P@5&fM{$ct~(QVr4QxrM8&6t^~ zyh6~ua)Y1-Ewa1MgWX+ymqIpi<j+ZS7|6^)zfI<$4s$N~*ds<-c|7E!6-zSnb(zvT zStz&c5^mUj6^R`j#z|`W#IP#_Wo!<{)$gPjdAaAf%32icA1*;Xv99Qj#4BvixWXu( zHX;#w&%p(CBRpPo1<&WmIGS~hr@h|uEMG3zj_wsJlXp^^p{k$`ACifK348R&nu!|l z#Y#$GKYYj>zw-?3Q;{Y9UixHL6otoYuZKNJ6NtQWGH&&YM777pgH={D+Q0q>elIKs zZ%?X&*!DGKQCc}_iEifdW8`~L&TmyRBhL+`dRL=~PZ*ps`83;~pFrn)vE~h(G~?Yg z`3;|IuaJeds%)*4CM_+IOv)FvgULB@`mFUL^kvRj(wJe#xVx=~&;l_!?C1mHU>C`( zar1@XRl($pZa(Z0RVI|+U$!>?8;1R}@$l3f+#s=?j}B2CczD-3;=N*s@foed<5P4X zw}D0pCshb95`lB+I=aW?IJ^Ii8N2iHE5d1fLKpK}=?CT;;9h7XaUR{owt63@?Wc1f z!EhJdU;h(Mx|IWNjKU{Ne?uSqMhlPSQ|VgG@O4ifyv;vJ+6Ax|y@(Nr%O=dE|AHl_ zzFn;N7tO7a;2_5_<sbHyVIMtTKw_O@ft&ORAGgt@ZzT5Ot}UymlS>!SH!sDaI+vrE zyH<pB_pBmkt_@=SF*D+LQysl5O+!RV2#*fjflZld@NRMt(t5QOwYi-{-tX?9++ivF zUacN)4E@fm`q+wm;wIvW3!?D2^9%n0-e^_t<#e8#z~S-F<&DQPrX48{?0SeM=t>I^ ziFT@6E1g^#IR$S+uRzMI!z9f#3mn$wpm>v)@J(A4@|HhE%S6T#es?(ZXLg{6-lvG; zvm;FCdnwZBssbwFVzBD!V$#%~i@K#9KuR@>>s)dgHjd@OR9iFp_YFashekitHjVK? z)G?X-dixQps62r4D+*Ebl{=8jNYj0)pHS7AYA7k!2T$P$_RmdI+`3a0PIpxkb+3KY zqBIlw2P;lm1yu4O^F72`V;x>a(!rIJ2liJ(aY@1&qINtOamSwrVH+bhHTo1WussTG z2bT~wDxa8eDv9)4ej@#HaU1Cli2)xE7qZ1wh(6ynOt{C|DW$-ZWnN4B81q;|rm<)g zB`w$mlZzHWri&qd-kSmK&1tA@iv`J&T#C|Wn3H~`$&k9Zf$5GP#dlk~sR`Oq%<JEi zh+1w4S&f<~?J8k%kyaF#dz+}PIuX)$x&RgN&wV42J-QZa?d{;^;{uEoy<u+u=39w{ zlfc6LIJd5(ocz#iLR)r+fzH=4^h-OBoO8)0Hdd2JkJ>2JJEI7xzmb4%K~WI3ktW@r zC1^EQ5jdJ2Ow^u^q9$b>e6OjW?0ix}+0FWn6DLKHnREM5`Pm?nyj6(iA80mZHxUWg zeftKe4?07`r&>W`%Q_-=;w%<U)B)6cgL(5V4d(6BCl{5ZSx?CsIJfa^h2HJo_|R-Q zdds#X^6G~p@it|dbB~)q;!`%6Wob^NcU~e!D<WXx)ketB^F%2Ik5O#kR@yr_9WrH( z!3%$5GEQ_CpA6{Kq8$uH^pA$YVsBk+EeOXcw0(>Ew7-zm62*vXQUpc@2g%ii+mUE? zKZ@OTnzZqk!kAVVPHd4PSK7qkSL+hebg&vld^-pFdiUYoCPk`9w}_c~TMnttQzw=j zca%!p$nur5prYnJ41H50zAwMQi9~)EWso)k@NN$_|E^9VE6uUT5=peAKpf8;yhzN; zQ!(y#$GHkID5BO1CbR2^(1<8KW>!a4k|?-XwMU?jNP?QNQV5<h4cZQ+LrqHwb;~6H zr?j5MVI?lWt@;QS_f6oNyArKzWQu1`3P*9vf-Uj%^{g#FB@2n1y9UA%;%xFU8M0Vs zE&5q?nYk9D056mluv)1CvqZu@IQylLYzYVh1M9P3{w$gD56FY0KqvC&g+J(R)u-)e zh_Yj5Tr$d?!d`f!53Xss<XK(|DP9UV-*gKk%yGfW6)V8e|2&vQ?MEpA!X$627U83H zlEm`Rc50PrIxJG&1dc8BQ1f^yId$SRnJ$%!?_63ygf}IUO@`Yc?t?m<R}03&b0<RU zmkhF6J%<{fD@%gct%W1qv#8SQTlnNaBvPB0P3nAaqxQJX@Oj=(rdXl~N+zTsLyskJ zuFV8yF8_+Zm48R)RtocJX|ZEqZIDfNY71oMz6p5Ps<VP^k%jDfIb12MfmkOAQXT6B z^Fw-3-)B4G>~jrvbqr8R>>m<;<u|!`#1YN(*-oC4*Vw%_5h^yP!u<j%Hua|j+I?Qo zHR6Y1y&slDa+@NtwU5Oqa`tGejVbQLx#aM7el~90l!(`!Jw~Q$%7glw)o?f^7ya7o z0Fz^Gft#cS;(b=81H1@Y7&OSNU7rB{qT^t4$sV}+CYcnhJ;Iu5yV93$H)0*zeWbtQ z1UYu|G)`2a$b)@HNb|!(kY{fQqfS>~S;=8|^~4RAxp<?WO|f8?`k5KvOB&#gM+Okf ziG&)7tH`6a8LiuT5=JW9sGZtz_`q>hGCY+cKHeZ0W1CZ*-v_B1xs6=y(<iXhIO+1t zv>9Zrj0xD4PoqxACUdQpb)uDXvv7z&<dnTRkkJ?#Vx&KxMIW6%b1$p)RUF-MmwD{g zPpx&IO1wws0ROpm2KF(s!kgvVaHV4pHnG;iY59#zQ9&10SyzLtBNn2mHLsZCXAH28 z;fHcZuQ<xAs;9zmw=MQY%TZ;}YTO0eNY*zura|H~PUTCJU3TjDPfH=wrg69Y<%_u$ z!o~)u(Nq)f_E3UrK_2*G=4~wH@c`@m8st|z`Qk~!qwk~37v|%Sg2DTRR}b(fo)Y}- zzlM6ORVlgW&WN*S1J-F*5G1W7P|G8{NY&Z5SpTyD60@s@rk_%1L^}$j9ob0!g(9RU zrcy!+-(kH*53*w1Htb6tQ+EW>Q`}X@@F@ph#Lg~f?luCx5crXjtK{30?Ordi`>htd zG3x?#;HwNHq;wxoZ3?Q`TvJhTB18>cZJJ3PI_XEPmdQuMg+<8tge;zwd5iM=(S`$s zz9VXTC?5MtlZnb&_;I}r>iSVn-JkFg2bO1AcE43Yp{qqnH&2zCTVV}Lb|1h&&qA4n z)+sn>)_joV-(_%Oo*eo8hobr;@{mizHLhj&7Gzxa80YM2L>;R`Eg}!7k!d4yxaxMo zOprjw8ogYW+(A2$LU<p(II)eg?Nui0nq6_n0X?!yO^Lh6Gp!=cFq67$=*@Vv%M$&G zH@Kp&-cYaI#Bs)%)l68~JTgD~jk$u)bB4da#t5z}Nufeb6Wr|Z4KKhCm^C++Vabw2 zX3f`XDtJO7y35;vZ8xex<?U^d8L<|>iPXSOrq9u_cwLBZ-;bUTyhn~Pb4ZU{9TT!+ z0w}J0ggp4s_~5TFRR1#&?eS0}KLj$RJyqT4+8lKfJ?RcUn$?4KE^gqX6V5@Fl(sjM z3L<zUBb&N)PnEk<rx>SZ1Yt>&f{M9{F=%-3APPv+0A}|q9CY9@4loNrpBVtB=brd_ zj0q}zE6;pty2ae`E5IAYCV{wZA2wU|3TbW>hUb9-%Qw@@6gGw6`K{B)-r0vyi#m<F zy%26t=1Zf<hl}w_xk*&!Cx)6~?hpRUdocTCGLCwG9(#ROLUz@kFj%+azOEV+@~s2k zlTD{?E}V*WW~@g`d<&Ud1!l~}vKRPkSPT{x4PhMCU&ZQOO4QHm9n3HOHndJl0>4Yz zhq47`uLGGgaKp7r$XqNDPanvt@Z+C!!%O$e;dcKt@|}r7Ix3>*iGB#UZm?(GtEwPr zA4!BZ1*3#Nm@<D8Pqn(N!M1Ux<bK`+>QuY}<fTu?Q#YhAp_&3x@aao*y-SU}Up|5- z+~ttl;o+#H?G&DNIRKgbkb^&_MGXI%EDpRYgt9x^8K0vu6rb-YNEaJANVSGoVHxrs zt4|X6;s$vT86%A2Qrb|EoeNl3yrJYLy=P94YG!f43C7Mm14aB?LDtn3QVO3%VOf9; zz7!rx>Df#nW<M)&Q)4s|TKSrL{F^GepIU>TuU9~3I_cb|hEmGo!dvbu>)VX#hZ5>i zuQnfFkI+Y6ixeQh!v>3%?m#(Cakw=4CRRN!Ld`s;g7%3Np!l&8G+Jg%#oE5-hCjH% z)S7$axmiNxSj_}yi0;9Wu4$Nkh?$!uELSx<l{sM|5565n5I-pi@0vCNuS&6D#xBS) zyK_R2m9z(r8B9mR4vHkc(X$Ef4V1!4A^F&U=oJ&9=ML6O1{sHeI;`pBNA^ZO!fj9G zFz-ekVovpA%Tz>^v(F*ZU#ZNRr@3T*iUSJu6aw=<75M$ix5#?RVKfvd|BpU)UVmZe zzj$tqgaTEL0{>#@L8SlZILSVvO`P8j;@6j>QOhe&9O<V~;S*v`umd7HKIv1DrR&kr zU>l;<7>CD47%`m>U74u9m&_xBuhi+x!xgjM{p4DjKd1Kc<M5?%2FT>Dq2-}V$<*Jm z#$sdFat{G9`*;2~w<}orhxvGXf`gi1@G&RZlosQa;h*~tp+V52H~i>iE{>PLY5|fY z%HjzMI5dmBdwDY@KlL?L-8F;Nh<-wjtO=l#h9$vZY!|ez=dhC7?r?o`i%8tl7`im9 zj+o?{lC&&&R<&e+JuZ{TYDQ_Wyj(GMz*UEx@Ba)OC1*j=)u%+cxrz_I!U<GeXbQX6 zy@l+XF$0N}o}?RZE8*16dC1|$VLIBki!2_MLI(yU1Qzq#Bu4BtVe`Hq$(x1Du5<5* z!n4!VQ>F2hoI`>jt^<>J+a_P9hqcqGc1i-5dF*8U_X@F>Yi83rZ$=>3avSf{L4BUf z>+u3ly(&4di9eB6T@?*6H8Bj6YDmuQIY5%zV_=EjB;MSMo1pxu4?K60pg(Mn#m@yd z!S9!6(UMq><Q(vUp~WnA^4pHv$6rL-=Fu>&V6@_zf<MT39f2)1-Nf=hBz;EqHalBk zDqCQ_7&X|<r6&q6W>u4P1SHQ4_WBorm5N^{7>6yY#pO}XY>!(uIOeTqH*yu&O_?XC zSLVO)rDZed-RpM1A`MY^M<i+80R#58wgZ`cc^jS)D&R4+>xiti6oe(5z`Cnch+KaF zQa1D@nwPWCuoZ`#*O4PxXDRSjb3)s7E+JlIHyM)2X3<G&+EDW<&3_+d$$B_f61BW? zFe>(h_fes^cU~#!a`OhM9B1aRbp(8HRVOC1H<4pDK1|`N`y|*knCy05gEp2)(nr6G z(7kKJ!6qV&nuIr?+qVLtcAgoUS2UfR4T<8$Gwu~nT@x#UA3mq@+;{|Em`jG`GQs`h zd03V{lh4|Jvf-`T9>qhuztYXk-q2+G4Q?Lbu%&Vf==)}-uw-o_J2H3`+x!$G4jeau z#08U5*<Lg*(*W^Ag4wZ7Mf$V81B0!b@x;Vj;-Zd7-JO1<e(C{wbXS3jvaUiWyX1&p z)MAuat_d8mHm1hw7CJdLgd`&PXVIfi<G@Mb0T`w@vwLo<(dbnhZFKbr+jZBLrJq{R zOzUa(($Z6Oii{Y|Yb~Y6G^KbN{<?HP_&dnJ>mX&q7!q}vPG&{6BApH&IC!=n(qFbi z@eYQbbC!YepHE@L>Ol~_l5~=*s8TEx+^Xhdi8otFIDge6uHWSccu}zmZhTUKLBj@F z2t4?vwPp0iF>m5NI*&c`X(r7Zen&@)Yp3EwtLb0QHn6YbomjKP4+JeZPwVDQg5txE zVdTjmgr83#PClyiFKa1=C)rw|S8#@0lTs6iySI|@`yA-<U>7>!sTmz`siS=2&!dR{ zdzJ{qA9n@?-)E>`X&lx&md*@G?ZcX-CZtcO7oT~`;KTVlabcqrjv45IAJ#w%2|MwI z1`e_c5_z<X{wp#|kZ*N%Lj(y>@T8S5JHXEKi|CnMAtX2BEzt_kf$mx#sIkgIuW%hQ zsGC9Wimyj=R|lhY<@S8Gqhm7dz1tGjb3&*m+)KD}xQ-j0`iU`daKe30FEMfhJ24fR zi#o{(JcoM0?WMzT)$wTR+Kx^5WLXYrn9rjF-tVfcHkRbQN{obUs<Xi}rvy73dH`)T zTZrZ!Y5JU^5?VN@j@rwAAy{lc-1|MT@6ol;?96Y%CH;P+-YFM~-89ksb$QTd)PRi} z_u{ax+gZ++3_LVfoyeSsU>lF5;5`;?mZ#p-qf|c;5Wf-$KeP8DEw2cC+@T5{hEAaE zhcA<c8e8(QU>)!EsdoAqS8$0>d;?`-TTuLvG|~T-MmE2?LsR*Iw1=-53Fb?Su{yox zWKU%xv`n5x2m342o7*b{;knrYfAnY2Zqy{R;=Yq{!*v9g>Jc54hl1=gRS*lBii6EE zaYu+9Q>Io-1;h+6T~d!xWQzq9uCf+nrb?j;;*oI8PMW^v)kH6NU{AZ+7ST6fs_<H9 zJ364zlI~19MDz7Zw?d9`Az7}U%|6h3foEUXivm<+V2(Y9ZG9-hickm1$tmB-kJkZI zhDi~6wtO;fdzeIslpW4gn?rtAIw2#MHv))pK3V1#O{T5Q#^STCqm4TP$&E)+B&9|j z=afmIX`FuMOp-Kf)ulo=#l^wzvBz}Z2H>-2p5=q}o&hYR*h5PwtJ5y?6Cm2O9P$Ub zbg{evy!nuZii>K<#U<NW7bXzcFASEFbz|!yE}(V4?3w4;mLyL7A~-i3hv*|E)Gy1! zT+R9#=HlU8y#CP(R9z8<S0A218vmG~8J{mv)BDe%%@@?j$5me7nZ=i<k6kpQt4mwU zOW(f(SN*G`cgty-A26AAbUZ{WC`G`cgyXE9Y$*MF?^(e$!GZnKZbx3U{l*eT=H$oP z1+-G%JXrNqkNx<i2m)Q_gVHU5hj&pRIk+Yb*&3CiqtOZ^>9!G`P%{CkhYh3A-aoi_ zx+=MJ_8C^<uYQGcBzmYxJq<V(Cc|@2MRwVBCHA$K78|T{sPe?6D*D-y2%f&t9<nSp zm|fnI3WwfJqOD$UrHv}$=r6MA*fs7ENPUoj7n<p?&r1Ug1=-p^(L<=!eU1J&&O!d` zb?FOg#jqq>ndnCY5}hsRERTPVCZ?YPzEQ_~Jfig&Ut4zruZXgyEVrJ-Ym{nHVShaq z>aC>WHe^A8X9yhqIG2?XAJ40h=pYpj{MfW75%j)G1?1jODRwnCl+L(nPnYicPC7OU z)1Etr0H6MeRtwPeZ}As^Gixbm_)4(i)5>6%??&cN_&3sJq6h`C$$X;XUyCA_+v9K> zEvn!34#V8vgOVr_Fg~sYo;**S8XH7-@qxIeRvS*nZXu6GE|a?j^7Io=Z9oGal_9n6 zm5NomyqfC+<Y&cr+UlJ*Ue09GH~)0A?H7$IlOs;BcQ$CU<u8?38$}Ctko%B1u|i;h zG`3<_xbl;klGU9|^1^*=zSLIM&2tL5=iEn1<DW4lDSE`O?>&)Udl7mqmC3T!O6Ksx zVo<xR%X$Rvwk*SwnY_)mY~SK)kXk*JHs2=)$3Kfzs!YB~baif`Rv#IvVZ@vM-tPdp zGg<IB;!o$@v}a*r5gO|rLApC)$V~neOSUF<0nMiN2tY3bHo0ROonP(^L+94x=1JWG z{#PBXX>%riWxw&j1A)|Wiar_mH5G~;gyO5*z0|Lnb74_^7b%-qh`!be0-iIcvrGxW zVvz|X{L&EdxSYWI)+NP`y;Vcys^2Sw=X2O;ExYNU@fy^|dSSv>G(Jt=^YkP=2YGP5 zq!8P@JOp~fNys=b41G~pME@$|;KIw3k!-InI-;3{*L#@aq{Dh3JANM%dny@y39AGX z?g>&grU<{*Ct;;|(PW869zK%CqD8mD@hJhYmQfUs2D&%U3Ku)M*9}AIY1hV)10tu{ zmO5)bo>Lb<TT7eMJt>=s;pj!WeWfFtSw551yO0lONAKY!bDxm~_0F))RS4_9%w=WP zWPk^y3~To6qum8#60fj1C|s!wZ6rF#?_MIlt8t=yMWa8G70|imt)Hlr7%$xB+e1zM z@l6m}9En!#|Al7m*hS0oSys=9FUr`>+(jG6ZKsL3EbmwHDcTSU*>4Gpxmxk}z-~i~ zAdo<dhj;_@*&qF&@Ub4!cFNIRv!lrBTN}XGAruxSiNXE^OXzhiq8pJQdhme=aFT0? zf2AM(A)k%Q^@fR}Uq1CB`3_#xbBq#ir5M@RdN5u*#)R-Yelg1rwjdIp3}3~&A>PMZ zkjfOr3;$9H6AcvEy)r>CuG*B|DDjNAH;Kb$&q-|6qmQIWT!OvV`5hmW62wP+t%Hl% zO9d%mVI-Ur2l~ql@$LP6WLD4`*6Z_D@+x^Yb*=pamPxHgts7*Rjiyo<t3;qf!y>@n zqpnCgMOHBnMOBe+Mjp2PBk;kLg`pKnRrHAMN)mhBnEti=cxBvB4O=L81vm08(8gnL zasCr&I&)Gcl2=?wk6-$k;%jAs>Jdl!y?_-JIVDR<V$_M|Z5edPqYYW`)+6`K1*j@V z4)1icMR_gGl;yKH(m^lc1G8M<@9xVYe+)HA_2_EQozlvLUz$U4=S^duoUucz?x*7o zCZg=K(*~p?X$M_YJ(b?Q+K$dxDal(|WkV|{_rmJ3IdtFTN)Y*Cfu}u)LAO>mv6VSK zVCv_IC-_T}sZaKj7XMI?6<UWGw{kXeED4-#zEdTs{GBk~<pL7heV)8dQe?;X-o#29 z?ouhIG+2@LnIuzf9k}}_;=QBUNO^l1c+W{ifnp07;}?xEVjaj1<r&g1KEz||>?AgG zwIfukxUzD81Q8lhGwJ;b;Q}{-6|qYQXCneWu<n_@?3!3%L7<r)vAS(S{9Z)E2mZ!) z<dawnxu9W9W?qTI>C)5K?i51GS8gX^8N+15EnArIBmy5j@qu_%B2=2Z3KSI=<M$gJ zvCQ)cY|7m<W~I?`Ft{$w2Bvb@&`ei$!;bm%jS%O`q4$5lBVLUzD0)oxJBYD;Z5H^s zZaZp?N+xPsed!EUDViVGFp=%tK8_4>z36~|#nf)+tDwi71$X0f2yONPUwhk#sncA# z)n%PPJ6{R4hr~&gK|167oq*1it9a$h{b-``N~U+%4&B)h3#lPS@Pko9yv0Fiv(0)) z)(d2d9owmGmpedcxjS#%njY3<e=8IQzh^h*DDr8|flkWlW-5Ko@hAPp-WaaS-M}l^ zG1&BeF{xM|3c|cf%DGL5SLSgTI@hEFKQA8-%?^gX21g_}ubs&6i9`?c1p`juA7o5a zkE-KXlGcV#h@RPrPwlXQDf!vdIp<vb^Xe8DYYk>QUlx+v{g`;m@2+&Q;Zv2VGX`<p zuZ^s`vp#evl#rmRAhIL1i_P|XiVQ|{>5}*l6lEm?emi>DRUZo3Wv(}%>W32htk(@I zH=ZJDL2j07Zeb!P??mWb!QgU!3bII7gDhcTEMjzlDZ6rq+PEc%s$5*nh#jfNp-Vnf z@u%~cCrt*R$`5Hnp?{iSVZ|nToO>;=$T%CBcYYwp4C3&t;tr-N<35D%NhI5bN8qF2 zrgZO9M#TUX1254E61`^?;ZJG@UZ65@cUB~eV==^Dy+vh|p2oHxgUIK81EL?80+Xvt zaemEi{41ddY!5h-g<C9O@abJVQA<J~%gfIrhLxRo)zXisWmOGPu_8pgDw&?sv=COM zBv)n{F9d_V{p9B8RF+vDLbsxCO!i$<7!Z9%s&l@u&$Sxa>cMQL;zlA4T(pI`o+1QX z$5e7(N}Ek<k7M@7y~FEoWU(qM<H&&rPas6L6g~11!5ioh{CedBK7#f82(!EjU2*KG zXknKUpY;PIb(2{6Yww46P`-;;O@Bn<o;a~ne^tWW8(iL|;HzN#q8SqI)syyfX{^GK z5?%2~u+7>R<Ve_S`f`u~VRug>sax{U-Q6zqlJXdCM#(HfmUB=Z#gNR6e$c0*#qv*m zfsn5g6yYyaQpbiB{s%1^VJJXGHe1CbiwEaO@4{N{%G!IV@!B%<J@^LlpLm)%(N~O~ zxU0ZveM7?C;tf91Ip{8D5iH1#C{L93C6{{oanz<5NH{MGW(s-uXlf^Jz7>F#PqiR* z;~ThGib87}<-xjk1UGNGgTGm+5r-phaEzk(Kitr%J4HD|3B1`H0m}QYr??~(%;uP? z{b#zh>_5Pq|9ultNEn;V(Ng=5z-giXt|6qEU~a_G68W2km*8Z~Q5ttaGeKbG))x62 zRF@EF!cpeTO1Sp7;IAJ$VQ`M1im^e$;NKE~C3;DMrm3LB2PMoh7L@!oaQxkkvVaix z`djeVTJd);O5>Vs6S4&*YX9jk`0ut{wM%F-5w!N-_5q>))7l~Xgaf968vo|Z|8I?T z4hb6bI9log7Q6G1;57W5|Ibm@3ni?b$I%q|3&JG?%;P8thiwo_NDvgQ`A>Z8zqgvO zUFg4E4D+4rIphAG=f6qff2V){VE&&YZWl_Bo6pfy7kr09|NlmPIwF){Kc8bETz6C` zA#^@R&rbfIdlmZM<pM!y`QJ(Y*guy}_G03HKRE~fb6xP59|5E%D)x6xUhw)`|DSdL E2fyLvumAu6 literal 79865 zcmb6B2|QKZ`~Qy{Qii0IA%rB&X2RKP?NVt_=rqwlW+FP<(JUHHBy*`GLW4?*(&U`A zHfbI;m!g#BL9^!mYo9H*_ucRF{=WbB<8iM2yso|Mb*<O6&b80f?d7iAK|w)PRpGDy zx+-WZ_%nfF;q!y7g8aq)mh&Pa-CVmUO#hGn5|jd?17mFcB7;LigM<8-=zwUk-`shD zQNd!%|A@_tjtmc6q&{9gPRI?L5WY|x%LIFdctl4<hDQbaGou#H3ueSIUi?sz!hq!> z#W;~te{U{5G2<T|6&@8TQcm*LAim!`f5v}qusE2B5p@U*jE;;94itw+N5!anGC{#i zaL~x`K(V?=Wq?rBQGNlxfY^`_Zgi(GE~x5l$Xz8WI?9jC8WSEP4vrH48RHidEEcH+ z`Vl`oQXC%T=ci60>LT@7A`OYCvrwezt;PLyfPY}b0)HmRFDyDbB1Y6jeq}9hUG7|X zj9*A>WaNL25q0(O?!ui7iDugUVK@07YI|$9KQ)gDZoO`I`RN|s-MG^+Vt;Y4Ul3Vc z>scN7SzT`f?rdbVe^C1&|6D{*K1`q2Vs3O$Y-DhZsHZ$=(BE5~{Bfv1S!$>ssh!9$ zP#jH)Zy6P3**erRFgi+1N)XJbPYM>r60ySlBh^Jlq)5HuM8^HYl#{|b$ggG+#w%<Z zrWQs7>n#zP2}OOvcyY|bx_X51%k3M+ugfBgUqQbxKE?iFd{zVcdn=NxtVIL=PKMtw z>f`16MqM<BWHC5SG^AY?mi@gulD}RM93C1b7Fo4k!Y@`F9vP$V#{_eg4f<yjsf(=t zk0E4V#6&ZG^KI2dHvRj1cOn;!A>#Ol28(R@KT&rhs*5M_L3NQ`Yufhm1Oj7&{9UX? z4iog1#UjUv?h5X@?&hmSL*32Y6<3R#*2pDu7K>bjB0-Yep%O(vD1zSPiK!3}?jIwf z!>q%|)M2(^{H1L~!`feZxVx@c<SM_iL^Q%(Q6d`2Uv-pN<R%o2=C3*?Q8ZR48W-DI z9CyAr<0YaALXk&naXkMij#q1OCbkww*jk)Pt;Lxve-e^sMo6$f_cV?XO>sB(?o3jS zjurnCoa(L{8z9%iMl|j3GWwIJ?0mB8qNCI&`$vVTi@Zr8ed0vZ+ZA#~YsNGAjAuzi zzCw{-YsRzx$=JU&<ABzT16wl=YRx#fH4$4;NEo+YLgPeX?WPIuA7=G0ooz*P+Up$Q zUM3brayqNJ>#i2fC50pvjN-M97K`QyMIv5nCQ%e46p6$BT{t^YZ2Jq&cUKjQ7RWCs z5iR5jw}`*kVzFq6P_&f4SX`oLnNSq}r*H|p49g{=6++R<RvA|PBZH(>hSjYyB(}<s z)GEW8uqp1kVN=~zTjj75CI8pDOo|Q>&-GuZE?P@8ONkSuw$m)FwKVB`P8kwWrcjjC zn$x;}a$4V-Q+8`k8(MSP*qYNOZ)LJ^g2LyDa@-YT17b;PT#FBu-w(Kc6eHUFcar}! zqzUqFHlFlwbx|%!b4#2kuU(q?t!ZxM(=3pP3WcJg)-<>Mljio;G<US7xwAFR;?^{G z{b@+M`I}zreXjM^_OIKRx@Zr{q$EzXw_PTst(olOGubZ@l?g@Vt(hG7CzFG%nN+l9 za;P<v!>ySd;rkHZ1#Lt}<s14~oT#$hT++Wcw7p2ye*d8fP94M|%xyqpt|zg4PpaZK zVYOIvTqvsHH{ppy(Mh4`)L%Q=L3FzP70<XEi$!PU7nF$3aXb1vf3XW<(M6%?5`VGF ziK1Ge=*piReU%sEnnZM6D7w)q#?61kxYa7g?N%}Fw2D#JD#l&1qs_^VHYVGKyN&(R zR7Cf}xQ0?6Cu(RX%>C9PJmBYTl!zV*MUPtNZTe^4$F1`|X`T0J>%7le=Y3A*<L(B0 z6>LQ><W+bXCwkRxve$oC!BO<4y=Ki5YL&#Ix14DD?#8P{??^F7UwO|9_dzWBC=`9- zh5MW+Y7vUQ{8fdaqOa|*_|08kEc!0LphWb8tHMwIV!y<q-$F)#+*Re*V-yJ`GD^e> z8D(NyE5dXjEEk7SAtYfs%13nKqVl^qqsrl*8#$xKdGdQXqt2oHcFt&UD8HjKojH`> z)EP|<r??xFdg+sAy8I>=MPA1-UC0UVu0+m%J&&vzE$$3=FDDJcFFcCrN_=v!;Y!U# z!oq`sf}@yjT+~=z5H3pYLX0*SBD3)KAErBT!^o48=|Lcl(INJqLNU7BS@{bWMvp`8 zwF{$9NW%0aR>&A|QGWS`9JVdri1Ya6_u`OWzA=aV@=ZA8mv2h2f0+FF%Je22$C$|@ ze_lK>edO-H??g_FIf=KcG1Hg$+|!q_An@lI%=9CU{7KC8CnPUAGk}no8Az;<8AL2! z`oV+}nIZD1C9!|)DQCutgxgPPO?>kFW^Blje~L4<Tu4#E*pVYYn>`^h<3OyCaU_<X zZ788c#z`J^Cic%pb0N$tEg&ReDESD$MR}zmhiyvJoX0CYj6+`O;T-ZxyK=}YJ%Yow z#t<g&1k6YhjAKR-+ph9%+(^FiqdAmUeheWAGnQB(GmeY$`M7h~md|+3<MWxoA)k*2 zhkQPs9P;^i5sVE8lQ#)wB5~pvA+hc9m?U@GmtKi%aT{x9GC9${5L1ZHH4SDefj@nN znMNFW>tMM3&R4^SkeHcHtdN;OEMJY8gc6xq@~AJd|51DSoHmRfiMO9~Ht|VYVf@LF z?<`CJ7b3+EBu9R_AVOj$m{=hbLM%UBD4|3qOdbs<_D}KW5az{>AS7WT<s;^DQC{pQ z4%@_z<~&~Pc^vX$i#X)PW;o=<j^Xg1E;Hx<YA!a6n2emmca%AN3z_pDZNrv{<*voG zjrj!Pm<7bPE80TtEWddcaVXzBiwQ}XCBzDurCgL>PaKDB>siKm{CeU!<kyqHA-|sG z9P;Z~LGVx8SV=gJStXCOwGD~f{cG>oGONjn_H|4ozPxWF5oqfhYly@5jbuW6G1n3j zGbzLhnN(u=Vx|#FWYXo)3}XM@J9bPaiMO9Ki}+mQSVxYnjbl9*B6}yB9Qo-s5E3&R zi4`)Nh~=ltA(Y5$mPd1m{j+zr5azYcBP3z+<s-IoQC{l;4%@UY<UC&MA`W@2w{gg8 zy`4i|>m3~ayK(F!!8oRv*mkww#f{`^znepO?e`FpFeStanY~<;&!?2bwtV(+9-q&C z4*7h_IOOvw=aA3m0KvA#agaE1Oa-y+@;D@S|0=#cbC{fHUx*{bmp6{11lk(MG2-xz zqmmF`4JjcpBO_MGAY%DyU_yxuE00zY`*-mjm}(MlKj(4cbB&{h99tX52`)s6f07*e z=}r+6GpC6aGG~b8r#nk1kvS)io+tKC@h=eO#lA>L!d#M%xXeX)v1>VO6Z;D1@nT=) zkQe(JhrHO=IpoE@!QubVIBc1lWaNKq9Cpkt?pj>qxJ@9AxkGHbqSbL{`OR~eL;2>p zM@Yid6DwpIxG2A#`y95d=K<&O>uKbWU(Z7h`Sm>FkY7&|!9R`TG2uAoi9FKQIG)Pg zzxIwJ^NgHmU&rUfmp6_V1lk(MOXBd2;}s#kn6C+mnK#4=nPy`7V!kDm$h?zB-xK@y z-Wke#Ao2Edek4BEI6jeMYvcILg~;A%AxD0?FNDO*S7L?CH)8qez7tAhe#oOgiT$&8 zei7!i{!K{2D3JS|kWnPIRcj><+q72ZJYMS#9P(PLaL8-jkwaeVP8|Nbaj24D9HT~T zyV|RBBl+5Ea44^RXF?K2lUO0sg^TjF*W$1(pRSz8=hKZtJ|Ark`Fy%_$mi38U|ZwR zAx<2lOKiJ5^yKbe#dl)#$%*!b=t+Ee<1ir5);J7_!#55iLVPuP5fU@T#0nV`V)<&A z5=vxx%cEw*{#|@$rVoj?pVOT9T;u3Vj;)Quf(w!2_ajGsy8eX3%m8AA%s^uK=>`!> zWCqKlLx}xTd`rT-*j9ujjJ14(4HxCbw&k!*Y&*{5#kS{=7u$hDUTjAWd9jCb_&+ob zJI0BO{BMoJo^j@`#WfBW0&$Fh*mgyuxU>A`0UXLV4<saEXkvxTFfPikXE=v#>v82g zemx^N<kvHjLw-G@IONykM(|JL7)>~i86%IhHIA`z_uqTR)|wedPPDJ1JMra>V?2Sj z#xa37eB<yS#23?(keKlzR>({wmM^A|P$Dx)9-U0=Uwg;ahM7X*?dP0Ie6Dd!BgfXp z;mw7}-ti$ve!A&|#LNt0h0IK1`RQg6N@RTHQ9ok;?48+!d9D2kNtginh(IpNYaPU4 zo7Tac$7>zJA+L2PhrHHd9P(O+bNKJZF^2@>m<VFq)jpCN$=7}^hw|D-5t1;`#0r^t zT$Im8#9>=L4CnFr#Bj*xBj%9LCzeA#pZNsa8pi_S#4!trZI{O)x%*e~ZJEX7MEgQ4 zA-=qEEG5v^IO2%IH;!e5_-e!x5;F<J3Yq1^^3_;DD3MtykFFy2@8a7r5)yAe=W60} zjU$m9TN_6b7b3-9Lyr7($%MqrT4IGv3bFijse}@lG<h_g*gwV3Ak2%MNl3zE$w#c? zqP*DaIcyU<oAY?FH*m;{y^%v+>`ff<V&`!9KQs<|W-}T2-x`MllgnL;YaCk$#4&lq zwkujbcb4BgTRD_(o&rJ=rjS@6Q^ZC2^=#v?Z9UsLk6+IY4*B)$<d9!aF^Bwmb`kv3 zICc|`WA?}+ZH=Qu?*6rR?3umfMEg3H5?|gp_7P}n9Q%pGH;ytwd@;)jiJ1e$3Ymk% z^2Mwml*k;CM-LPG_ug?}j*xi!Igb*bYaGYOv9)njav`#Jq~yp?CnF?g5V1lA6U$G> z5=vyM<k4zk|LmRPgn6xN2uYX|@)0MwD6jP?4%@Uo&3U}mXE@}wKFcAm^*IiCt<Q7# z@5XU~1ml>C#I~#bC2k~N`^y~4YhO!9!dxL%$Xw;3d_LDWY|H05=kfX6;E>PfCWm}J zw>aeUxlORGaoizJ98*VZyFBj7-M@<O$lN0*+83gp`0~clK%lL0+$Rp-I35t<tI<eE z%seDk$UGvJuSOH0MCP$P`h?iOi$9ckO5*M3d`5h(aXcr-*2eLI3z6c#Bu9R_SA@jO zYhs1W8)EtCnh7N`Z{^W<#QrJ%d&0cf9|%d9kMa?pxF|37XAaxMZs9y$>@OVhVt?h3 z7yBEByx8A4{2v;J1M`E7{BMoJk@?A8i)$Re2*feJiEURj1@i4o>*i79P`-JT2uT=a zVuefxF3PV*g~PV>bmTmKJ)Jn@*Q3fIzaBLX`Sqw1{L?rz2*)v<<&n0=p(%I&+B;56 z7jmL~9kqxrZya3-v^9=y#Niu<HX**4-3f`A9>fY69b)-n>Jmz1^yE=}V*lPd&P-1d zZ$GC2@wvufNRF+I!-xx!z0->v`RR-ai5U}Og^Ve&{B*quB{F96Xdhz#>>YE$yw-gQ zNf-<Hh<;p@*SbH4ZCVfDJYMU89P(NZ;*i&RFo(R>Lpc0*<FF*bIL3<DcD1+WM)Hlr zhC_MnZ3&SNeTfw^_FR<D$AQDPd>lEC&u1uyd_GPb^7%M($mioiu&r?jh!e+9#J0-= z$lZSz-_DwW<V5>I(8QNFj$s7a8pm+r@QuTj5MPZEgv889Vuj2oV)<&g5lUo6%cEn6 z{j2zPHq2NOZ$IZa;&Y9|og7;m$9OJ8ia&uI`RP0ei5X8~g^U-m{B#ouB{D*JbP}<D zia(h!FZL8d5@xD=#569-i_Lvy`j6N?oX3kjokL#i865Ir&*YF7dlrZPL*sB{e96fF z);NYTe%!UV#xa{f9OF-HyP^efXZg(&$f11m1QC)j!NigeiMc4ho=^_kHcuGm@#_ia zkYCRn4*B&&aLBJGlHi}lF_&;06D5zdHI8Vx``6yFW#*9+?dvEazPxcT1lk%$3~~6z zAtuBZGnSB;nNO^cSwJjb%!Py!nMLyGVq*W^J9f+x5^q1}QsQ%sBaR$f8^<y(MD|WR zIr7sb5E3)Xi4`&{h~=kSNhpz7C67vo{j+yg6Xvx}BqU*y<RjK_QC{n04%@U|%Xz%k zDID@zr*g<^oyH-rbvlRtZX6jT7{_E1+phLm+(^Fm>o}Cxemx=bp)avQW&;=H^V!H@ zTRxjOkIyHELq4C)9P;_(a>(big<xCb$Rkc1lTU2BJhsZ+zlv|q6p$0`3sFdXdE+P| z(AGG%5r=Oa+X?a2*g;6l>?Bsm6cfu=V;7-BX16@LhuFW1@4%Fhc>6i`5}#`vrR3P! zIQDTNQvCho$WK>BNX(QID`XB3%TITZP$E+yj~*iSPw@{E=EXijNWvVIk2uCfd9f=w zY!h3`dA!&%4tcQ=hrHOBLtbo_!~dai3}vdw$p52pkRNx%^54e{B;RiR@4p9Rs@q*t zn|yZXt}p+QNL1`xKmLazG0br?#9dYXGr51h8E!qx)Nm(_|9z7G%^P!qJ4t@3Wa;JJ zN!8}~4)-hl6%@FK*Z=tg$u0_Z|M3G!D{D7btAG7G@;`nn`Tym7yOW1!e1*_5xe7K8 z?#6Z*t;~*Y=#2Uf9}XkRRM~Sqn(>UXlVG^;08Gzcg00NA0%J)NZq_%#K1t7EZU+r| z%8s|xcSV5i_BW!lmQIAXCv^aw-_F6i44%p^9o{Lsx2_CMno$Uq#Qkadkv3afdx^5I z565fD6zTJ0n$h%@2YA`X5cqIR6D}-z20hAqvPy3b!K*WN;ss_x`nr`Co1D@DdsV68 zJs11Y-ba<#jyF@_6vOfO^wTWh_{^L=kPcx;cV#;EqVKUp(~H<|Oad^qbYuNa{=(b+ zi%`X@JJ4bLY8dNngg030v3jE)V!!pbah}C{s9u{4H+9|&Kj2!tK;{LPd^!$iT8d;x zf2g1xE#27lnQAP3stWF&cm`^1F=I=Hdb73WFF@^@uT(^0S9ZD)#oA`|fmc1V;HLgr zK-aDe#0)xsOEx>g8QV7l-|f}V9({*eu^-{#BOBpw6Co~p>H^DRd@&on4$S<nL5DQ$ zgC7T9q#j-A#a>@H4SO%zjeV&GIN{x6YBM_*7x|sS;zzdl(FXw?WOxktnq|S}Zrkwv zc^0%%KLflV$%f8TutELZKf-T`6;j#bu5`nt8F)a`d1~v#lbC{oV8oAV*uTIT+`YC1 zHx)I?c5Ld*?!8)%z6UBnPjydda&j58`&<jA3VXAvhP7BrAqP1oy@Gd@w!yLUvf&j) zEjD%OQ8X@qg`<5A!h4%1p>BICaB`Q+z{c}B)XE0XN=JfgwG6S_978(RUK!5ZbOFz} zYKSdzp1=!klaYJ(4sdZ_Lppc<J*iTYkzicYJNWR#P-J^<6TbPn4&Sy6!PYZsz`g|o z=<J~zVW`hR@O<=p{GqrOJ&3%HKR&OME>O&Z!rHF@R4|CSwG>?eYh>=4o^VU#CNwLv z7}gFxfS-^4jhu!lv-ph(-JP~)HN5nYmi~9FKUSH(bhHHvl`GLX$1T`z_!+SGYEQO0 zryHErq)1zS8z58bScS7^M4_=qO7U5pYWO1W5$X|S%I;FhLC+LkBR6vc+G=Ameru^p zQ$@=Kg-y2De|90>?{p8k+bhyK9WUS>Dmw7N>w#$CG(8+2H5IzQUj}Z}Td=>ss>7{U zrhunCtzgsMQS69!?yy@?4Y)=n0Li`{EXw*So6+(X?)><HTI^dPTlY|muiuoQ$8!c? z3B4W{Z#V#P%Vel~oy8-KlHu#>5_l-H7d*637hYG-2R`+2Sg+U}p8AxBj}6O%(y0%? zf;C>)xlXlm-LPDAe?<}MByoTbo;Jgb^V+zgUjfcEt43Gd#?oq~MYy3+6EnySKVM@> zgT+PYx1|cK>y-r0H;HkW-8R_tP?hez(gf`bnu~Sc-^3Tkc;Sz;g;eb0qi|P=0M;!l z#7n1yv*JT>)Ebwi_|xgV@N3{&IJ3`0@G^Km0%^gxp-&D<JUy6BxZ{fjS*6aWzWjzq zmo;E*_j_<=uV!?)KSeu?*o*vZ^=V&SU))Ej8y)j{K2~4So8G5l4y>ccLD7QQ$oh~9 zJK;hNjB)M{mn&rAqd%{}fc>lSNbR{WHmNg-S+k{GShV5YOl%+f9q9!O#P%Oo!Y+<$ zWV79b@a-8z_O|3B7WNvC)4rU6i{kdeMIZfPj}?#c!<DLZN3HMR_|>P-v!M%Hv*8Pz zb8bBRkTVGGxRQgnH_m|yrs?oa^G{H)?lumY@)Yb-ng=Zo%>wG@)YwOx65!f^j%=lN z62AGQ1)B+b)1kHlf$G-=<g$m<!7Lr0_WX@Hz3xD(E$lAnxG@Cor5E7v<~^{B#TqE| zj=(wd*TG#|K7h52kvN;yVSmkhiuz=og!lV>#XX+tvHj8w;I|oPs80*m<Bh|X;3<>} z?3msSKj?TJdY0$n^bA#Yii17eDbj~#9&cb`$Pzr@(P!A>$Sr~7=TBK>ryf}Fcrl8v z-AnHd{D~s$%;@u;<FW2p39j%7#gAvcfoH>B!Kp1xxIs{gk7TlFkfJ%ro=?GR?sib$ z#vkjJ=+jp<wQ1j@Ds(4{39QbDWVoo|Gn|ur3p!lZWBuJ!!PbnM=u7S{xDl;@{<Z3C z!iYDh{6}ZJ{Ki<e>-=qaP@k)^IUz#W5LO7MSt<x{<Z|}CZwwBx)n}bQPJttr$HMaG zg>ZSWJ9ayJ3-^nV!SMUp@Zu<l7ugHYG(B^6;qx!B#HSEe9Qg(^Q%_>SnHNxVoH{+q zTnTPcxs2!NUW5+&Q{k@cp0KCMGkkcD8LnJ+56Y(M(7}7Zpyxk^z{_W^;g9u;aqRF8 z2oF@H_p6Pd9_+s$_>q4SA1=aJg*l6QJbVfri+3ZZVk7vm$4Ok2I{^glJ_wW4t?>QT zH0;=X6sL&SVOG5e4m+d^7xz(M&(z+;o<T$DjoFt#58WF$tl<F8AKr_u*>?@Z7w<$J z^l4hZIRpPb9gj7#8lgu-3tSm$2?v|6#O2vj;nJ7&XyBaJ@MHaFIOdiW9&~gQS{kz) z3U1X<%96Dh#Os4#XKfUuas@Vg(8o(f3OM21WGp^uM8VoC==rcGP%!Ek&Ir<@FX&Ci zAyOOqqrqtOWak1}WA|EI@?tL>vF{iP+1!T?H|q{tq$lB;2Mu6gGK-&Ic?X?sPJrgP zY^-6bML)Wx#||6h4e@0UJVd_&ys*qeE!XC-F9vLvWv!{i-9mg}-fT}eS8)f-NIZ_y zjy}U)zeYfZ*<EOvuLWFJZh%Mk)5T}*{>1JsH?Y2964bcj1uar~u{|eEX3b0>EHE;M z$Lo~YK}8yDY)LnqZm|mlj8TTSXLJJ5lRo09TZ4h**J^k?yf+LSlK_XNMBt45&+xb_ zmRMA^84kob@YK+L@SO2d-1B-P>i%#QZZN)t{C~>8ri6~L>nd${ed>L@%ESXn4nVYY ztTC%O_&nSj`yGA_P-MGya%5$nuLAuJz34MNi?L{qDl4(g1uY|k;l9n=Wrt^UWNRYd zp<KJ0a5!+J&%XYFqEpwvD2D{Na!NMbFyJ*B?zjq8oH~!^#213RgEn->QC+FN<tO3u zduh1AunX=v=94UbhCaM>WF)LPRE5*rPhr~03ko|J&>B5UL7bx!jM=^cH{W!opN8*+ zG3)1nZ5mIo%gq*ewZAWPZg~zetTOR}HOBbE^Vhg#zY07ct%FL+7T`Uji<N@+pfZ;O z_@~(tX!XMhZ;$W5{%}nbTr6iu4^X7Tp0mK_MHWnM*@fzMOQ7rAM`(Ai1MpObSSYAT zke$AJ1eQJ8j#cJ;LxGOhspSnjaEY`7o&0$|wvgq+gI`Lq^;{ELdde6VCB6kFPBu7f zU^(;^WPpkvX5by01^TX9h>>P4-eJ`hcJe-j#%mhW!wj!rzi<I;ZpuJ2!~5gpLsz7w z%TMA<nKw}N=Muaz@Gwx;Rg~$P?Swz-L-B{y&D743E1=AN0A+nD8(xok1k=AWFe&j2 zPK~q2^}Dabux0Pz{HjrGrpYDrJzSM;9-RU2-g%0DdaB|23@h2JCE3{7u>j`xk_nUx zrm~rZ6w26n7=vI%XgDKGmcQa5I%qZxpDfynr>E&+2kU(N`^^Ysq*Z_y&YJ**-x%55 zG(X(6|7?6T)fwyMPlh|byFi~sHeiR>b8Kkl2_(Kpp#8!enABp#cKWG|e@>IaGV@cQ z$GOiqEA%N+DVze+w6;UASQ8t>Dbvbti*fJ!6*zwB2-?WB4)$9A27Eew5nkF?1~WoZ zVc#Qn;D~cHwD59<XFDs>W}D;T)z3=wU}*|QR5q~hosGNqJqUl@)yD-#>aewnHum(o zfTL$+z%EaA!>g<G+5JD$-~<13{P^P=c=gw4*8S=@y5ll!9J*vT9N4JKcJA>VRyJwF z`5B}iTI>a?R%>N11h?`1Hx9tRx{qvIv<6*OdKiB#>H^bu7vlNBJ=v56YZ0Tp1;i$u zK(x>lZ7W|1oqy~?M;{i#LeXXH_Np_U8(9kdqd&q<g9;$`cBb2phcjoBf0@BO{@dFb z^Z$H1W8>y(`u}`8vpsnXy+y^G&C(f3pBQ4zCMVs&15&5a{`zC+AN4EQgqPavfI)8T zgQNCzkoIs^vvLeuI^_b~x6^`+(0>HK8@SQUlqs#KbQ>F3X|ubA+=MG>HI~b}r|N5U zZ}J|Id;H&dYbgx+SKj1(Sik>g;;nDS3Wt2DRLD8y0!~e;{H|hB+2e+fV9=KFE<Z<B zNT<}K3a-CNaE>VG>XQHDu*+{PJ?Ve~!^)R%m-Obq`7Wm}`U>3UC^(npZm48VT|T-L zJa@^B>m#rg(Jo={-UxaYq&k0_n<PDu?&9)!Xm^2oSc+iX!W0+F#2u9dXQxV+6(4lD zr~6#0u+~VbVN_iibxc<<ZkV}jWx5uXctA&>aCneY^WI89<1bBVr{60Dce6BPBlc9f zJjz-i*ga{O%UR<R=i3*y36{)KK3XtsnPAW5T*0Fu?*$ryZ7y{`RAs(J<&{tA66vw5 zT)~qx9~ar0071&e?k*)6I%u++A~r6omxVW2V^p03a>r}J*`?iZY*q@mD=d(e{9Hy! zXDGnm{TjeLYYF<W;DRhWWGh;KakPwKCV>)@J>a`;BPv>c3+-8u1NL6d!rD&<Aq@Jt z)bC#?lgfhdFdbEV!fK85PzOcmx^4_Ml^g~qpIrp0l2W+rhzZ_D?M6eUZAQOKHQB-I z_Td#xUtoE~5wJ|!5yTyt$(qH)(_NaakW#19*gDRbemyM_Z5m{cl?y&X<KcjI?yE$N zk!;3+>u6SKRxU0R?T2yN8)T1*Hz19l=}_`^6Vg?1p_^~0(!ZEz_`<Gegu9gD_>W1j z(&{8W@p(1nnWfGS9kd-kpW^}>4v)bVNE5fjZ-mb}C$K%gZNa7?<6*<ONl@eVZyfce zD?R6&Bkkp|qq0(IB~(hD!xr~_1{U=pj{tY}>JS4uAn!A_NmPQReok;?@L;y|ZUQ_O zRK`y0ejOU_mp~YLh@JNIB5vtqhs`f_Mav_*ush63Kw;0>IE5(zCmT#*!-&=F2Ae48 z(PJfRJ*x<hAFl&PFSB7wLVB>qA9TUepUdGE=jHUg^wT&e><a$s9So(6IlXR>B71In z2KMRfjjmU(qNaHshMheCb{b!V&pn<Fr*=JqcRA<d0g|C~Y2JOHG~x(aVKNu1y%o_x zjR+pEK7zN*HKb2nNktZ?G;ye?9Da*ZqUmfY?hnS}&zg3u&+ICEP0t9finxO&_PvJ= zP^;iz>qW9A8xy*v!+3cAmKr;1>|W|a#7f-MbsKiQk_Wp?=}3oWyv5)AEMW774A|+y zZ>)#&>AlA<K%D1*Lw8NbHv1n?-80_7Wa&Hn!SpOV7j6sp&7aL?13fx7YX(~vP=W`k zYqD>Zv)Ee`cS94gI{VRfIXt_AeD1Kn0F!@RMt9z8Oi$UDN*^p6P6zDR3<ul3kiGvp z8S6P_Le+a?fKzZs`qzko^!Yszu#<-=>@_GAAHDGzH0F1L<0nqT8_pEK_ai5w(TR(3 zRLVox|KvD0rC=J~={c1yQ%s|+p51|S4{EUm%kI+SN__-Fv+^rrkNZof8(*$0S+!9* zwb4ZI{@XbhqZAvd*~ZD0X>KcyRcy?4nI$$6WQ;vo>3wLl;9KuYl~*1fmioGeI@uLM z!TWb6f+JCzrLSeyf*y<3xHPJ$%eFgRb~!Wtfgoq<mr7GJ#l!xiOQZ#t)1{BK?F4uA zcM5tc_mp0myr=S1>7mMqCN*iYlA@r-pjwc_Zj<_pT%`Vi2b_F-w^d5YtpwlU3@I$o zq-KrQmv*U9mkqR>c1&+=pr9(W=Ge{~FQi8{X9zS=f9Xs^57|ne0Kvud5mfY>+RB#Z zeS#;mOl1}4Wm3=3VS?wgRe)v60HpI^FsA5qWG@us)MYQgj`YFg{arOM{JH>SJe&*+ z4m<>(PLu$<qa#2<wK`6R2L%fgbaCcPW3<2HUT|m16!_WuEYisR1a5y*gsWe=VK?bf z*%<SQRBWvoF5jqz(*`Qw+6@_~rq4#8R;LHe7EFhgjf0@z@dDsJrWU#WK8d{hOh;N~ zs<7x|vMjf=DK0a$hgtp-9Fq1FWX{P(({oRQ;I4kqr*<8PsL_PTURl<2tPV&YyjLce zlm@>XFM)mE>w~x2diZy8wJi7~#4}Yo(P9%-*0SqjxYM`=un7imQR-oIYx91zYql$L z5_JOQ-*2KgU2S$`x1RV1)e-L6c^B+0{RMYjcZX#=Gg0I6HONw<9u!Z9I4J!iRd?|| zU^|_Mb%yEGF_T`{!TAE{7t&kCY|W;;6?)+#=q{QtYdLyn(+#UmcEvtR^gwxU0hR@o z<M|pJ;ZSoIcq!P9>bkia7W`0y!Y9k&%-}_`#_zT0cozxYTHXO~_}Cr)%<B%c6;I%x zXXWT>@6jN_ygQD0Gn4h1Zp=Q5422gr>mcEQO~|iiI+~ER7518;fHl^ChWZAp@SuK= zz!y?C`{FOMn4!xseDI!{X{kh=%dB>p`Hn`37gH!Ud%yJKx?M<nWPN4WA|UM;=m44h zUj;T!)j%b)2RP`J2yAmSDC7E0s4nuP)NK8DSg)}fxvktzu~BtY^%4tox-t>8+^MF% ze~hA*Yt>aQbge<+;YsLuMh2L0bRm^+{RGuCVGKf}UZZK_?^CG@b5NE-A(%Ggq|9>5 zNR;q$5UQH92Mzm_gRX1NLTcVUfU`jyC`lOwUT?96uX=3;77y;D>V0QrryVXy)7aNC z<pX17zWdxkp9_P~!o?>AotrLGm&(>6-RsAp=OY20>1qLE&TOePRapbp6av`2$sC>9 zRtFWYD&V>u-f)s|4sg$k2Y|_;);S*owiZ?>?PUWRxyc%TUcD3<NuzOsbvcTR83*r* zCWB7D)q!;6b85@`EkMuv4w~aJ4|pv0L^EcLM9VfF1$h&9gW`0Iehp4S&U-4TW_?!R z`ns=drl~&YE!~1FE39O>*Po&5c_lIpkMhdTloIr{+F!YRn-P9Ls$R;BI}Nr6zLdU= z&y%LQ43qt`=n6LmvNGk*Wt3NGEp_teX&LnG2No5M6)cKYp%S;&xdcDdK~H0xVVwA( ztncF$pl0l7a6JAu_+VfLUb}U~?|OBRNv^J?=I1X$DSB$M@Bxj~^!~5W(uwQPoRvDT z@bewyYhVcK>YD_|PE>=^i~>Pu>IiVuIGs9u%oCLD=?8NA9jf$<NI{e8Dv({gH`?q| zAoEJLlMUFYi_ER>QQM3i5L54gPOS;Yt6PfDx~r~m+v*xf70#8;ySNT&PbiQDoF|`+ zJsAQ!b-xD&HQM2E6Q;qB6<smwDx%`Y8ROx#wbZ-8$!MeBHJPc5MZa$DMkCDvs9Q!o z;IhNou;Z?k;8(ORF#0-6mfWWZ<QJR+J~h6muM>mTJ}w0ta{J3RZOWoPEC~P(f+X3= z1r!ecJsFCXdf;`@lTnZU9@x3EBfdvpr$*;I0IcarSaQ?}EZH0{NTC#EcZvsL(ESa# z^lE)&)QNde(JBqab(@UzYafH<Mt#_DkB=b8q6@a({{>jZ`%?C6BH{PQVpP0aBGVbT z3inV5!l%dX!{2+G!6h?R0p}mDK*zjVsGOsPf5=v&*Y>_B-F_mT@Y)P+I@}Ll-4KrN zL<HeJFLQwB4F<Tq7lF@aTkxzpW2o|e1fKIO4E9ts1)}SBaMrdoSTH&VFMped)oU+c z!wMz(hk_fpw0I{hd6^Hi7YwDn0#xx4K|ZDYt|v4++=v@HHvxCwQZ(q@3Sb_T1iCHg zhidjE;^t~4+Sir>`5p6RJsobL>nk$R^S=G4Q=?B|=WmVRDbwFYL_UYeb9Sb=*XfbJ zGb4wQf3M3u{@d&H5&wFfPTpY9|Lg1Y|F7>(xLE1W<I=U|^Qq>iwK#syJuI1QBP)(5 zrCu$|#vSgYqi5r{!A}pSQvk0Pl%73<McSu<r*m)UGiE(XzyC$%r=yGv1M_6(-!GsB zy+113bA2mSA76zk=S_kxhx!3-eV0A1-Iz_@gK&@kw!S|9TA!_(tI7XseYro=!H7=~ zeBSZ_kGh}?-`$u2^3xlo_oAG@Of_HlLC*~`yNAL3=O$9pjpN}&|7$p6sV}{t+iCjR zn_&9!&2j7q`<HCmimvRF-$nGpR~_k`SsrZTgX^dXMYEY*XW%Ly7R2b>hM&$CgY(MT zs3cMm51zIT-yFFex41n+4Nq;^J|0^^{=O`H>C9MM_k1YMZeS=H8DklxftUQ!LvP0P z0AI@hejlKLI^4gC&R7Ox#kVTBm`=qHI(LF)^ZT%c9%*dS+*fqE=_$5xq6FR@@Bs^c zuVrr{cW9#%4p%2^#}gl^vrjwsrq!x#ScQ=3Y)t(-oLgc6?Nk!k_eoo@_R33GZO9V# zjCu|}w#OPgI^qkTheyKAL&xERHbQW1g9fyy>IiQ)?UHqSl!pv<PKPUohEVQP4S{JY zKu^9^;IgqA?AZb7_*dP;VNY^bvf4gIY;r>(8}9XjUC}9sZGLVCXFN+~Z4+0}%*PIN zFw}sTZ`A{*A$l}jufPTwR^!8KhQqQInymY?(eT2yF#6#Ydpxx9G&FkK9bTTW9)Fzm z6_)K+hAsicsD{-+`Q{Y(cxn@>czg$?C+-2ApG>CmmK(t-D~s_DJdd7mCyX68Hg}lT zop3x|rIhtFQD;rAX3$kfG+76$3otlpJpQTg!Rl9putoJ9*{_<r!KSR&$oT}s37_wy z#ru@mW|LPi^%_K5t$*RdHEGyzRew4wN(E~?v%#CLn85?ti%{PIr%_7oRp9W*91nYx zf^L9*aP$Wj4R}%le%{GJE_Mm@mL=VR@9lUru;YZPfa(<dsbXlAeVq^3cC`cDXtx$l z5r(ol$&26^wG}vT&R!sSbq^m54Tg7Hb^%xK1oUG~S3t+CL*}pxt^Y6$x||4tU0S-~ zDK}_rbNLDg)5Cb2RUJNEqyv8*9}Gw5cL4WKS>n;klfg1Y4frj(3+>@Li@iP(f|>zG zP)LY6o;1A>jrq`wf7g}M=c%={L5F9uE~}D3)Z20J=k=-Zu}&F#w7&vYtUC(dYrZA_ z5GfQlhL*sC-mx%eSrIl776A)|IJCxm2llO94L@Hr!+KK>KzG;)`o-*~>vnv_Qoqg8 ztcuYv@?<*R-=_#GllK7QhL}O$Q{nVQqoK6%w|eB+IUkxGjAP5+C*eC&rn8GqXVB{| zoPrTOq_8Dx8+&i&N8ESjLvYgdG|ZkIOIxoBrB_zi(z;zHz*UQv0v)YGXs|;Ko*hfR zuRLl=GoPn`tgkur@nUQGb-*mBTe=NpW*>m&#U+@I$wcM4CfK8<D@c5?S{4^YUhnQ! zgQmq@;RKZ$8FF<8Lr1Ql#{_24;+c+cT-j_Eh3c{E26)h~dMDHM@tfd{tQ+uv7L;j0 zDLv{!IeYwj4&IqJftobc7UwJXVEtv~aLyVVJau|8yXk5*e7;GEw#iw6<8B^Aph<|< z{kkfX9wB=oW(hrjm4N<F;(*e!I`H87el)nV2=z%j0!EdVz=@Mm@yeUOk=3{ycJM8G zAWHQ@dKZMl-uaJ#cZ?LPL?5)+?cR@I@tPS}cT_T<0<5ZzfowWiWgHy1J()d1dd;DQ z3UFj&M;PL=7dm{j!d_|nkp82IvZUhK@X%>5R;Cfo?%ZaN*F=qi?kOJB;3wa}*O~cf z#E)+j{4`WH_wIwrg>~ampYc92{ppH06?6xC=ZwPMlk4&Py_3ny^{T4Ux*MozOaeWn z_X%*P$2NHWz5)B7rWhuVcLf2vwWyShhmmvSQ~2?c5uB)+g1cnQM-zV;qohlD=-Q{z zRA=Kf-1BNb+|%|0)a>&cqOASEYm_TnI@u1;cAr2ejb;#VX%*BPvWz?z_kxn4ufhAK zFt#jq4L*1BH7cAcfoCp%z(W^jgI+H?vkub@*tPa+fJDawo{dw%22~bx?csbEy)sB= z*}kU-d-tKu*Kd{iq}SodS1llU^H|WY@Vji#)@=0ohZAdZr3XwYBcDG%8i>mp-=glr z9Z^AJA!{30kDGTI1GD2jaq;IQxNA`^Tygp=(D2g4QIDTM7w6N!X9NPy+xpWJt{uXC zMr43|^ZoP?bp`AiQw{FhRKo4mcksBtxwxBYANqsYb2=<Loql<7FQqzRuk81+?MQYZ zmM(ZwKzrRDK|2JN!cn1}K%(hWSU1E8pEC%Soh_}yrj1L%^pPup%g!z^Q$G?$CtX1! z9`=%*@nL1sPv_Cpx#n2<`XTCPV1%#4YQdxKNAUSxsrbwnD;%ahz3NoaSK6;$3(I`d zfcwuUxUp9+cE`XqY~Pk}ygYUjYPdNTIH_C!@0SMR=)edJQaj*k{~5sk`ULc(ZZkfR zl8pB@<zqYLOx#QO4V{~>4@YfJVatxPcwERpJg-I*KkQ=+6ABCA^-Y)1!byooP7GE= zUUtjj<^A4ZvEUXKrkc>g{ryq#axc1~SO+GV7O*C}OhLnqOrSls1ay6&OoKU*c$A|$ z8-Ml!cxL5{&q-3@OV6)(@vdU@JLxP$eQepVvP1Y%zx6nF&<Ysx)PbI29*95JGy=n< zgY>i0W9aJdhDguB5kyr_0BN^hfbok~fa>f)5Z5HI9o`<qTOC~SrRDbc&YL|rzvEl* zYUweUU)8b7t6vr!U$hB#8$VrU601df#B773ic|2sewaR6oK7`W%wfwzI<rY9HsOdJ zy42{WPoP=LOVlC6hAxQN0X{uUf?vAb#lCQ<;BfaovXz=|A)4Epc9~s*;EY+wed`8n z(cc)KxaW!A51oh%b2VY3KlvPK!(H&i*^G7XuF47?OoaU<rEJgAoowfXwUvja30T+L zz1X`J$6?@p1W!}1*^P%@(^C>l&?>K;Xh79?e0X~n-LzDJ-7`>xy-Ew{cj^|nF3^Ep z6SNLaTK*LZ_4gsCoeXv7?ojq~mLfc&yIHnxwljR>O1|Fm`c4Ifu0^j}cA#dBT$oI~ z#!r6E7K~VF&DxLYL_fQ|6bA&JqmSR)0xRZZ(uKVYS*@qT;5n5z_&a$key^*?)<4<D z{+hcR%-pC1KBLjd!*3P7oo&Q^(%uV0uSxLnq)yb7!})kazvuX_*FjuhlZpi@YG_?U z24vhUp~~xP=-b4_$o%9&JV9>*I;XW2JegSzY6mCb!zrinDV?>b%MUYJGi?Uls1c7F ztK5gxM<mcouz+51U?@G<cLlw-?l%tZn}ZJxP^asE++cmmcjBH4N8{lS&Vrx5vssT< zhhbK5Fzo*@mHq6$8{S)8K<%-6iM=AaVbVsxsFxj~(yIedU~^Ymv}HZo-6N5zsrxEB zCW?ft%OMc3F$&!7YQb7u7)DLorH=A$Y@o(EPhfqnX|RscTv~BvF1_T)LAFC!KI)xP z&nn+q#J*EMigRHAy`d_ZemrtHeRhJFUUPXg9t*d?>05@-R6=L+{l!UH9O<Pym(NDB zybRjo$$4mC`2e04(csRyM7&#fD?AZXPkCmnlnuF~0OICsMW4H<;aley%MSjUjBoAk z1>syD+(E~Row9c;WXul9YSSOU#g{8VR>}kV&8aR`lV1MD66Hh?s5b|VWz3-SDJ?cH zNrBaQ^BTmT-UlDfSc<QX4~H-8m*dKd5NE3f;J4o6!OFo(?Bcu8u<2bDl13Z{DPKIH zU*~2xs;Wiic-0o>kLZF8$4B9+W$Te*-Z>Dt-3Wgu+=w>><fFGB951QsO@G;!1uw^{ zRuwp{#?+5ScI`wL9RI2c?pe7R#`!%3$M!yhE;~A7STccCOUs0vSX=0+?Z|G8yiWmL zb-LMd3%Yu=45If@xFs`yHAKtM+nHU!fY46xaqxEZ-AGIgo@|QocPE&fcTu)`iYqWV zua3+2%!bL8wbY%|&RB7FFYNKopY^!8ij{Og&?9hu)zuQEsslZT(1~{bY>N)dUT2qL z*)|V4Qp1{!|FjM6pZyyYqo2;F%wuu+n(p+CB_;U7K^4mWL^FtM&cnLG2$15I3Ozqu z!XvL~!H1)21R2ZKz{uDSz|FiF%}whB%{Db5ab5$M=sXlxnpz^oE>3t_Y?{n+=T{(W z=nJmZI<Nyl5SxK(>16%kRS(^EuvAe8`rNY(?041QSnXU0Yms&hx;k3Y>7J{ggY6sm z{LW0Q8U7n>zySX&e+E*sy5NS`SIFr{5%7BS0fi0=l;$1L#b1{V#5K3W;F5j@^be1( z$Rf8h&cCu#Fdv?!y5DX_rn{bl@b5HKJUASi&v*$u7G>a6wQcksjVktGA5Xk;Xf=Cq zM%Q8PHx^=xGc0X=r3NE|tDyX4DO={YjSVsIWHWuO*!$<_!XK)J$ClilfyW$q4=jsf zP!##h-PpZ95<byqm#%$+cSKa7k|p2Kg7QIZa{U=Rdx<gj*L#iNmj<-)^+XhJuLzx2 z_kj9OHED_CH7IsBfis^Qv-6T8*or%su;!>^RerB?S;nmcRWoD|D@+{A-uwCy&iZ8! z)vi~t)Jj#BE*nXYBlnAuj1J7cGmr-BJFo}u2*9tRB-(Mv1w7>SSiJGrUZ~fhJ6;=d z13f6Rq&_tVfB_pd*y}HTOO4)K1C>TraBxR}-hK(g2~OG6=#^vPlQ0`RZ+#h9npuZs z2lKIh_E>h<^`xrE$Lr|F_s+0ti{036i%L+kC<}Bu`Vsp~F=gva46%kposOtEL>Y~n z1&asdgJ(Mu;hmF1@M`O+^blhf@B3tr6F!7Pi{Zx5?ph8|i!P;pe?E^keKBPpDEEPV zxBftncBUb}bL+`>sz-ol)D1ACt3CySa^VZ*H?q<y9eRF`7r2k%6!xN~1$))2h<>u+ zAw4^45bYW{k=3}di2bD#L9>(n@Se@#@KULSO}qROju82?xB4BWjkPAR7I_(XkCP?s z^Xoj+T-1o~R3N-?M=oUZ%Av<VDdl$XAXYHUV}BiZN3HX4fzOM-Q#x;5sk|&NaKU*o zD6L9`m-UC!!*|=sq)9R8)|SpNtj-Dlj2|?t;^}?XL-QB(`mmo0-F*RuZhe48FW-jx zx>~YY)d?_F^F4fg`YN*C<c}Smy0RN5^`)N+2H~`xYHY)d4m7ny30~W~8M>X+W*d)< z16C#vkTv=2XL;N~Y^SG+1?CTt+Ri=bw0A8UIN%{V{onw|6>HMFT4YeLxffn%piQI2 z^XZD_J!oXFhx8NE7`Ak<9=+zN5q+a+GCg(TKB(F`1ESb>&^k%6s&X}rGdo|x+B1%W zW4$u*>A?j!Xt)U#P))Imw;X~y>m@MVq7v8dx{WW(M&PlbsnR>@JE(1Qq*P|+L-c6T zBbjcD2YPa>C;1IP9TMiB0^d)W0Pp5Pd{<`_&il3)uH4oMp6UKh=5accK6pY<Wt*vw zQ<rZ<J8(}dJ9i&HXb^(Y@4B+b7e?Z7bHm_{T@z9LsBNfoix!Sebr+mXN&v%T9ig|; z0lZ;K9&WKWg1zc0@kXyf^6#Hl?ixDw0r@{WagYChK0mShA3i_H-e-$DU0DUfUf+@q zx3Ps&kDmb#12$6M@7G{>hrw=Pp}@-Q6}8JN4%(5|9iz+EgRjd@!RZb*DEaea*}P#r zpuwk`Qr}uN_+axy7(3<?_?Xu$n-xXgKjMk_qqY^SD>cVKX=}lESsGq7trFaj)FbY5 zp%F6!D;AQ^gt*6lTbIVaJ{PigbM5r!bD<;A9r4ciN_hVojmMPikiHu}9x_J;z>$XC z=}(6a;Lc8+=o?*nRHm%B3`HfMv2)WnoEb8X+^4_Enjfr^rZ4cKH{kX7s9+1U5H6w( z&u_)WQ;Oh($+KC>!A@X=rayl3{24W;*E*n8a2fPc7>)JrW#b)L1u!5q69!p*1~@<s zfAkmAO4&*{c*{EWta=n3H82`Fhg%99u9mSq%irTdUu`=6<O;|fUj$#PC*jBf6IQ%B z2RGPXmVKRF38<QSs(JW#sIcFHW)m~<<;!E}cNHr13qK=R$P|O!i-xkXMvw3jt2;>l zaw;5@;?53VszJLxHgSo5@&o4?KpY#XOvkt<;<u6G=rd}8&`eMX51-G6Zr#Vh$VyfA ztl?C4d2j@E&v6A^@NFJ#@Z18MzLbGadZXC#n(g@WoJTM}_9vXA=L^Maz367sS!{q- z0`3!rzyf<;>Gq<ZF4-@Sy2OW<RK9eG6lkT{NgF&Jr8*JIh9<4FsvP~?#3?~DrSeB) zZ|P6Z0n*fE=`J=|8iMK)9f950>w@hYwpPwZW2GycCDMg~>@f}Uo&K6%=Z|fBG@>%D zPNA~)OSj6dBbr=pyCw_t)?bjmcl_>jrDLLC=Ya$%cxfVaJGJ`Q!gKqi)%#oo=BtMZ zhTYj!Iq`d>U|zU}!05ZQ^2NP#0(#Ot!3)zbE~7`8ORuOWNc&vyl=W6Cb*azZE4bp+ zpMsBFozowsRPJ2gU8;QC$=TtOdS$nJU0san0O^7O)zTy19jLvfL4p&e7GQ~CiqtN1 zGswF`BX?f|WMMA>$I^TO-OwP-RV+p!u3u#L>km@{TGpffGtVG<t6unY)n&><-4ShC zss=j0cucKx`QkjmqYKiF9|CO_Ohnrk+Jom!+Camu9~g4-jbOx_Siz%ty5M`Hu3*?U z9lXTHptAR^InuhARh25)O)euVe^oB3POjW?!Md_*+=I$R_ml-UV|EIL3|Dr!bVHZ& z=%`3dvRENl+Cx|Bw8PkCLvyW^+OHz*y<ogx>G8WR=c2Y(mNhMO?pfw0jhS3^Y<1jn z=_8?nbU!YyJfb{Zdf;lKpzo<ef(El=F4caeg5a?cF6Z>zq!|rs1ddy^D=!vmJ9|F5 z=+a^{SNiQ{Cz)36m`ZcAm9j^NBW3pMu1WRcCJIujuQ_+zxsW=2e3;<!-5jZsYOeI- zm<^6Y6mK89=TIYvJFVd|b!w`>A#jUe!dH>N%b--SEO>&fXOC{Of^MqdW>W+eXL1-6 zrjAF4ttO+(qqhP5U!|aTV>IP+_$M-6dIO2AbbxUcg<Gs%p&N~3a7(YgAY=F@Wa`pP zUFj>Np5CgL-Es~>`sqH<%V!~S{2>67=30Y@wFO`U`R-MJl_PbuhdnS@zFqcAaX21m zasnpxErd%`HX!XE2K34)H{hdWE%;GgfewGwA3qUT(d&H&;&smF;d0vn_(Np`{1I^v zSX+Jo`^_!`vle@LZM+@aI^rR`G-(p;7+;6jmHJ>t$!7dwHTfNQ<52QD)x*$w;AC*R z?hwk+orR|r-^LqPbYz8ZqkxrTM`(C<DSP9}Y9t)e7isrNW`_qrx^Ndu;n;z6!axbT z>gz7t<B=*3EiVQ?GgRrBYo6dQnzwM=?xUcC(gs|);TL@CGg0ud?_gTx<QZ%b8UpSg zA53@Xp^7rAN@1GoO%OFX0}I%Ad>lT)y-f_D^D_nd=}KRCX3%apb?gGTc|mXX!jdHR zT<Ik^sdyvUSRtimk=F@+Uy?zKU@~0(E`Y8d9Zi2Zy$w!M90vw>A55Q(Tnh7D^x61( zPvO|@E;Lqo50^A;!4+{2;o;Y%&M_#LzUA&t_xt$<dhaZ!^j%Xy<+XLFhuT@$`!XZ= z^wM2v<di}|k;xG1zO|D}X5B6@g;^tjK1DM3W2<DrBWtJ$rgvov^XmkMPL`v^YPRV6 z#QlPAcFM@RyB~PDwl9nyen2+7iwafiUr6aB^#^me>7gdQn=X3{l4WRAlT6yV3WaDt z2ZweI1UVzNBGHlSK+S!H)W_3S)~tSk^3Ki?B>YkX+ut|}o;<z|GN0DTMl9QfMiwbk zTVGaCRr~JATrVu8F3}S~&kuIM_7~az7uTb=OV^>VoiW(+bTt(f@E+79-=iLyYogiJ zJHa`xP_**9H+%!F@vpDXkxBelU>TAvV0v_su^VI*lek4Tc`-#*J$WMNTX;n}Io1dl z2=sBiQ5Sfl=AA6lq7#ZuES6Qwdx&(;o<N_)C!~vvA5i;eKLpC3R>ED29dW|=Ljv)E zHP|t@LMBvM3`}1-;;nvn;np>akVMM|xd!i`&M75>QHO$%<G2YZ<C0Y1=e-_UjZ}vF zKg-bB!zt7<-Pu^fStd<9s6gl3aswIK#rT(u{MK*BakzVx8&)<=1XuM6q0`2V$Umf7 zV5@lqImcu|-Ax;)UPsP>eeN5O)%mOV!+v#`*sP5U4&DNbEE$y7l!x4p`2dsC<#2{& zB*;mP!;=LNHI%F3=}&|RsZYb``S@cE^cHyNn+h}wOn}Rt?1OPWxma`OG_-8kDYVyY zl<ZVdCsg3Qy0S;}Dp{B6Oe_+aV(UN(M|`QoKg_+L_;oyTk6!_E(>vhphK4Y@b{e9G zUqYX9+}K*b4QRmRd>E$t|6}dV->Hn=zi(saWJp9AGNg%2VP9*ljiOX4lIB6B&^%Ad z6iFi?lp#Zg3Ms|D*10HBsT83?p%M*}O49V~@8^f-c#ivr`+FbX`}tx21GZ~l>pb7D z_iNeGR7FGl=2{HC=W-T$M^8rsWsz`ndLS&fct9^iJ_g+m2ky}l4>~j`pNf)~(x%=E zc(XwsSGIl+7It6awEIR7CgysQ>ZXyo*WL)mSM8zW`#Bt&s*G=+P2kT)>Ern>lMttV z2d%$<f-Y#kixq>bkxQODywU8Zod$=2K3d9mCWhl`*%)r3{$+R~7zfwe&f|fSXMA7c zFBGV!4^47?Wye#Oqk)1(6mR?rPi1T2vSB%@ZMw)uB}*_hcP-jf*h(Erjj?Si<&<rF zshH-5dbvO>y;v2bi!MP<{{XrwH64Py4q}l(G!$((h$Wkoafy5a*uS`l`o3g<|D_>l zsc{B<u||Y;e@W(lcW6MVyoiezHle=2yPU!yX}CBxlzUm<haO4-{<lh<+R0So<32{5 z^(j3Fsu~9m$!Gq>NAb+h-^6yG46yD?9`1Eypq8NP0MGiUWugu&Tz8r}G@6l}fuB(K z*E*U}^$2!O_>E*Ay23OcdkEZ8gRhSe2ugOD;$Ko%VPMh#ZZ*0?mlPj_w^M4+i7TEs zU5qei`>dc&&yLbJ896lZX%Y$u`zWwKyp>ZL7>}B@@9|5`rr_<dg<NjMP43G+d2I4F zUV!h<6<l+X<<-lN@jm1hKX68$7HZ8vf1{kJ{?|V!*V2|Y*1IDwo^Z)$T<E9{YmVRc ziOWm6Uv}B{U75#?g`&eVMv7`~gri?Eee~^s6n1u*%SWmY=L^*;P`g5-h%VfQvU4f~ zF^h%vKVQq?MTJtRw^ff{k=DZ5SgCS3xi6@RZw-Ig?KM}YeHAUd{D+tI-UgYri|E{m z@<<^`6`m&7a7%+CxWS)+)b@2XYHu<@zYP!_HggQ<tx(5?@B8VbFSh9E%S2AC@;4p5 zcNqU~K{|Iqy+Gvnc0Ic2dj=jf*3p`8Q_wqIYx-#Zd8(bzgaYesacj;#L{Cf3K*gn0 zhqmB<*nF)to}fCETOAmU0@{~39?^5)1HH@8Wr>;pnJ=DCAA0qFl0V!2|ByfLT;K&| zl`8?%)JcboK+sdVnZF~ygw6e#&bO{qLIuX=Onc>Fsxfg9b(&L%mF^anO+WjA*Uh-c zKcB74$7QPG45&mmO<&R1^$U@0=|or}UBZ`TzvpXXFa2lE*mnV&t`yH0|MmIb%^Ck| z{%rSuFh4zayH6@RO{vGIc<On^4wAI9X@~s;d|oD+Pm$tj_vT{!#cwm(V|JO-EKmo< zFK5~5Ut`FSGpn)LA4O8Gdb0d$N)|~ewqgZyWXoTST7nn6)gyh+joHuNGlc5n<4C~G zcyiQY9hVu|PaM~Tu#-C5naa9SYFadnO|YFt3lo%tT5)T@Y@Gy-t2%|d3beSC<{)%_ zYb2*M`vrI2DT<5tc!GwuD?#8<C%Sw|1`_tZKo8U_MZaC&;uV){ShkKKN*k_15}ZH5 znw7^$sM$Qcz4IpxT~UQs-Vergi+H?Yb1@mbl8U#R9iiDiSFya^R05+_$Tgm$Ck`#Z zvmYF#w~+$bvtJ$_hF0P$5AEpaEyG!<__yuD%irkMUnhDm>jdgsFAE<=N<g@pBz{>Y zLXN{c9ZLiaysn}cj51tD0@NbW>}l8GTV*<)c<2mSoPU(~*eeNN`B>AN`y@!WzZyxC zd_lbJ{=j`CPmGnzkmGN4O6zQh(ydH<B{TqEkB$Sqy#{RV4r4Y0E1~^A+(?DYMM$`4 zM_jA-aKjdR;xoZp=wY3c(Eel!>~2)SS$`$*CVxVt`EIV}yb+FhqEC;UkH$9>3$f#} zQ?NMlDA=p+Ajg)R$5kyu@sV%A+@)7ZblBr(oX>(_{MP#y-Z&-%-)M6sasKI4Vz@NR za_GeqY<{8J*G6!0UOTXjK`8b*P!D_TY(c(aGCuO+1l}}F0q1=Wqx(*nbImi3;2=MF z?6c>TD0fQ_(p_SQ)^1e5JFkqOY4zjbeBK5ezqpzRmbc<ei6T~d(YahIPeArHc(TJS zab!_wF4IrEj%)l^5Rb%Qi0!BF`ct(iL(I|+dzH@y*S{r2{l(<zr}^cH!Or-o-BGqR z-i&4J+9Z5PfgRI0&Wci{z<H)Bo}VWT)9!9ZwZlsJlsD5rq2?EQ9%qU><U^@tWi-c6 znMyy7>jR@8At@gg0#Cf7grU=pu<cvqgqbGouxNY=`R+BI-D;mrv@UD0SjjhFG&hs= zN2sv6fG_ZCdH`-)t<ChlALUaQy5f(~uW;>`b{Jt_f?X!G(n)Xp@QhoC-Ir*l(HB+W zl+9vXdQugQU9_IhJg5xHZ$BfsxhK&XX?<8R!-U?LnSjj2=Xi2RlI%^%M9q(E%YQ^D z2<wWgNrGq`*;tTF^aP5m=dCswKQ5W*9N7X*o;tL2dXuQ&WHI`v*9#wqY^L%Wy;$9N zA-gx#5cYRzpcA(;A-22^`zlpJ+lw1`*rZm@<=S#^Ir9m{l^Gy6sS<R`u#dNwJVUki z-{5WYO+i2MJH7I404>$Mja8wV&e~iFH!`Bj>%O+)_S~xkt&XLum&EY7yQhOl3X^ri z)8TQA42$zS4{M)Ci#waY>G{?QI6nO?+WbKoo&~KJ=Y56v@U<?Q&~g^jo8sN*wWH|! zd0AM}M_^%DAIe(ufNnDwidz<rhGC~UP`^?@8<v&Q`^ueY^7($cGe8T8_Xgl`c?=8f zk79OhGD828IiTI&Pn-hO$fc9w{$Au;s1ya^sdB&Jqd!NgDpLiM<3FI;3o78l^Dg{k zxiT9kC>Cv>egg~FKchQ0eunYYC3NPu5RiO%A0AE8V!KC{(fWkx`0tBuw4=xxACI^~ z7n+qKmkcvJT&aoPS~h|=TT77jdR<OqaV_~YB$zm_Ehgq`{*s5Uma(Yjd@}l;x=@B> zVzo*YCYl#cI+TKhi>J4--{Q`7>F)bba_Tp=Kdufvug`(|jOBQH+H2mZJ&s)45zHPO zOegsDSXkXR35~umQQYb6;R458rRlr>(!0tVsFLLk>h`*mYpK`AX@W&GEj3gy|I>F- z&=WKG*=Ecp9ML1^K18wu4$b9jq+a4MhkcZmZe<s(Lx@_uF>Cj_fM?zP!J?DYQTfbP z&`+F8j@j(QBSH@_Zma~mqnZqL<2JB)o>N%+<IlJ<K+Fz!ooBTfY3%zBjq;6m*KrLs zTd0EsfewiVI)6kD-MvnLCy&>M-9M_y(^3z-yd_yAZ#oA39`}`tagbwz!ELx~*je&R z<qlr8&5S*nbQN=QzEE5ffupuZg0@sD^f)WyPh~N1VC-l3<lKRU`_Iyyx&?UGpFO<# z%(ZxbLo8IO#Y67KTk!k45&qPI@PZ^8;{EagwThX9R+=XuMaKZ}-@6RY@UKIihozx! za4hv+QIGaqn2jBW+~d=HMzfDW%EUBY5^r9qDg61XoODf+1dF%>aBG=AVUs4|>q}#( zTVX7VdK$@IuhL@q_rf7PnxbKEl$mhu9kf^3l%1dS2~?yDaIjf6JkVH!FNgR;Nl!J% zu1o`k!_z=&SR|SrRK*3Vm?HCxS7=GBDco*NqTbj2(baNIc(h*zo^E|cQMD0_Ik%PM z?KlB9ioArwQ?l8<5=T~hKmb#AKfpsfBEUSP0~h|3$0v&>Gov!`^RF0z>o0zThuZRN z;o0Z-vHc7@W_lQVzo!D8bl;{QFNVN3?>s#Dt2-??9D!%}A?o$4hQE1e0CjH*q!X+B zv4fp2UHQWt!cVIpsiB_KW0y7FZB~p-pY4Rz2huTKl1R9&T|(~LD51ubXPnBtY$hB$ zjt_Xn;DT}m_J%u)X8Y!%c*j=Me{vam+v_Fn@9l%efOUAoEnUcHw7?%)gK@%@xA^(I zr@ZgCH~gBh<M6<ej}XWOAYOMWO{4z=v9787g?R=1mm`_9{a6H=oMej6FRK+CRLP<G zS;<KHL#5-I-i>5(=}!Eq`#F;emnfhAvXB+c8_n+729V@Q!Fb0tC-L8Q9+ACn#U`%} zgGu_KIA#Acx}_u=Y$7jXJ%<Rs!fzRSbt(t;iMgq?yYDG|r$h>9B6!&CLQjk0_}SKp zD3?=%Ww|Oi^==|PvU#PW-^Z79{w!PkbF2ehnVv&aE960Y)^u`H5X2@e{la~-KFr$9 zLxres6i9r#O&<JQ%St#ApWWOKiTQ&hfAb<?R?a47b7&1sZp)|n7B*~FaD-5guO^Wx z?^tqMI(W+70(`X|-!J-$2l;bMD@X>PTKFDy$(0}%tr4K5YRg%qo}<p4R_MFG5tbVj zg9_fubHiRAp+9^Kuv+6ucI-zie%lcRqf}3qzk3%1w{^~w1F!$Wf%5xg-pv~Bd|)Wn zURlE=TRNGKK{C?Vm;!UFy*c6Fc$PN19M>NGhll*V&IW3SvxRITnmb$zhdb}WXW~_% zFQ|jF(zOJQnMzpc#vgiZUp4Y*Hia2Or0AN1-st7OHP9vW1Q{cBh+O=WO5Cib=2ybm zcaw1DWfo4FrObsX3oOeUO^V@x@iaEyWiD&W8o@jkX|u)6DWo{KfvUYrg%wRBpuF3Z z)Y-j&O1*L7Gt55HeW@3>%P*&`Dwoipxe8{xbFtp0EhOoWF<6Yaft-$M<CvN$m@<Yg zZwjJbIa9EAfGpEIE20-%yZFF#cYI*mbk;X>6y4T`QN+Sdfa7vP%gEu>>2DT!mzECa z-1AVTsVPX;+=WA)>g;{`11M{~4@uWgi`qKYVY7V?@P;fCT(&I_()F78f>$Z{$kkk$ z?0pa$tU1N|a0&EX>;t47+>I9G&c>molTer9d*odEr>yPtd#aTt0aFSYDzub9Q=T11 zr>_>{EfbHz{lpn0?crT=p}>ntx{qR#`@LDDyF0n(`5RaDU&Q7lA2vis;j1T%iNYl# zvJf>P`Lb{>Q`-TNEq{6C@oV6)iz<`4T8g(zECdU^g|xD!7z{3b#?>BUfvJ}unTNAz zM$9zy^!#WskE9Gf+B31Y=0eDH)`k{qJE(dikLHgKB-=`Ul97Mzfc_mh;gb^;^i*mR zdlmKwk18-CwO$w51gl$6FkMNwNF|=t-M)!;sZJ&z*7v|#a~EDRvKo#`zQ#}PY{pJ? zJc*HA%9MuXk{gcE5PwsIv-*#ahq<fKERPLH;>`!DEp`RuIoQ*iucTmMu_tm>@*tmf zPl2`4)6w&v#<-`h2-MGf!nIj>u<YcW^7N)=;iY574Eec(DC#pvy1xL4peGRaV1T?{ zY{#XUHsQ?9YgqeB5oEm+bBapqiLmz+$lSWf%olUSUfda~$<C)+w?vW4{l2hey(T&E z!yI3oWq^hjY0?I-5<YyxS|r-7fwR_J=I6we(4TQD@L-h=?!MqfAAPl9^N+V9^_jY4 zRaYOg==e(h6)3X3-vd~CNjyAAk%q5>tGHzWPHfmXd8lc(BtuIkVqxQ3bZ(q4$<p4% zyk?j|<&RG6{5%B9t{Knz2Nan4k3<-p@C!;-=Lnp7x&;3Yh?#{1Rr+b$Rg_kojFzf@ zL2o)bY4{Ewrs{MVMTTCX?L$7&R%auWv%LeKext}j?9K=k?#HmkOU~qkI6F6d+=_LL z9z%&t2yV+A%WjD}aM}rVwsWu;q@P~JW^2p9KgWeGvAc(+Hou1IuX=Qd)f-5twWZY# zda!%NF;wKDKo8wOU>tZ9+4hJ;M~`VErSi-4nR^a3cHM(#4@l70V&>J`b~+r_|A$&M z{qdyfAMq1o$_AD`AVGOv_|1g9EYhJA?4G|wD|#K-3zq|sThoPxeI3EnXFL;g4rzGz zk<oZ%(sJ}RY%|-=-Nqe(R#?GDnk=^WWzkBh_}v)~fMju}Il~s#s}<0G9b+`>jwc+_ z+eSZ#Gx?#7Lvh%JE6ByF7t1X@K_8Y}rH+O}K{H?--IrF473TO5`}Fa`Z-MLipO0^` zft>MV{zPf=tmh-P-1Qg@OjKv_L>-!B3!&>~H8|eW!8oIYsx;n!&h>h%{doq8h{%S< zrXle5o&=LJ3WI&Nry($83F%rFA&7A`#SJUkkbRSw7xdnV7OWVBr9GkrU)A(kub6+B z;xz;RSeHceO=hB|z?Up`=1BZzm9X6Nl8tb!mpr&ttR{`$<;Z7MAsd;c4Hp6?!bNpE zq0UY@vP}OTPToHb3XZ2kkjglsGrSI4i&T+`$bvj5Fu)J*NimOS5lA^v8hTX!VoUof zx~Mk~Et~9u=eo>8XFDWd=cXy({v}tWx4;xzIONd0>#0a4E{>D44#gJB1;PbU)$lgG znB6>0gg??Z!+tY)oTE326i>az($h}UVPSnlN4=Vzx(lSr=rs;pQ-*ynM6oWX@A&QB zJ2>Lj3g#sCfb_(gGPUVr@m0ZNG)ZzMv3vZA{`%g7LIzG#%b;>jJ;M~m9B@X@HRe!p z;|<HpJ?2+d%mIh^S+MAoct%sS#4l%!CX*(mVxB)-9{$0;{H><~8E1GO*XH_`x=wye z4@O^tC8VCl7&10yjw1W|b`4Oyr?B|Tc(AHGirLQ#*rvA*re%m7C0BUr7BvF$CMV$U zR=WIu<+PsvK79M1<j+(75B4W1PZ|&!=|>OQeM0*dXTq_4lW_G61GsjnorZ7L0h^2a zkk!9Vy0t2t`=URYpHm)0Nuf795=Ei2L9+O0s|j^zT8}z6rXk4%H>pQ_4s~BPA1`Ej zR7rg<)^u*8*FW~~w(-ZP(hLcRJ=%gaQt$m|FEpUdUj0w<XZ!z8{@khTE|Rxlf`9Xe zh&nFRmWt+%7U{KI5}Exla*%AY<B}_MxObDj2#Vqt3gWa91+V<A_`jnS98awHCwT0B zoO3^yCVKNR&LR1oP%t?$gi{$X5XJRgcAUO)vZJHTYEj($>7rP!udJ)-j7TQ_mMDLE zgySl=EYWZ8FQU`^PMoptVS%LOC{bVNQBnM_6QYKDO`@1Y32uF4p<qnpYVOB^Q5>E8 zL@=sw4!={QLg0OJ0(VaB6zBU#f^U1gQ(&lhQn2G@G-vmyR+J;#CiwP4S1?TNyeP47 zIKM=3BtN-&J+Jv!jsMUPT;_VvgjXuq!fjoWPnWdSA)l<#sNQ`ie?{iF_$!=`v`fOp znN%3;xOAP~$kFG&E;4~1-yR}ADU3#>RZ%UA18~FfEcdDI4Bs|=1SIdRglxlYkUD5Y z-#$9e)%FSm&T;XaZpBmHPxzEW8o~Hp<xFbtu#7Gl)j&g<3LylYL=KVb;dEmJN;C<_ zF$ab+pPpj)bnqUhzxpszIuU?BEv|#rQue5Jt0(?tsY}wvOOYE5-w_EPhwsQ`iCtEE z>AB29;Pg|Q87kS}#q&2~HLekKU033N7oSqM(W4+{?JG{FNSodD8KCw{?ASE9f4pN! zGEB|QN5ac1+4agWP%zpKw=&;>hsJkgksX0;j1O?Rc^I5Oy@t$v3DC145vxb&;ucRg zSdia|HM6_WjHdlCWQHy@I4|VF6bvE3K!xou%)pk$%XlIGo<4gz2)zypWOuPVGe5o= zA9NW`LhT=jo*z69!NH1T$3O#TeAFK9Bx;c1TSk?cDM^5eRxXV9EQ5Y?70|V>grDzg z;r6GEIBd^&R`;eJ*F2n2mNqJgdfJEKz|Y3y-zXWjy~~8wZxUejlmrmf{^kbL`fy;J zJQFE@<8)_?hmC*L;BAv8w5L^|o{^KuN^ePA^dpgT^6NpHn^eJdY&P}V-VX<N{Xxri zoW{c>U*Ok%HAqdb5%v9gfR_DO3x`8L(Uytou=<}LOOyGEcANjeSJIzC;e{o5Leool zbT*wnjh%)w-7VQCp$WG56M{B2hr>-9O}O0Yf~S4#Lasp*h|~;ua{He=%$yZOClB1F zH|M>hg2}64PQg|1L*K#pMFOm}|B0qQQ-QD7?!dDccYz0AMh~lxVEW6|P*z4djt-M$ zX<Bv2zSV-Ie2@b?%@w};&lLAxt6`$&ROpJ$qbn>%F@GI(==Xdj?#uPi8@uc9SGP>u zzQ7Xn67J(*Dd3}&L+RH07tsYrb7nEA4nLe{!V(Mu$X$<Ow8Q5V_?wKTlTt#lgY_*g zVdi$QzH5*BW@ex-svqfTD+{=C=PX%JARsfJy1{`5^T5mK1yqF}gp}zgQ2THdtQCI` z$$v>l4JD`X3+FaiJhlO%w(bJa+OfE;$qPMpOypaKj>YFU8RN=p=c&o%Zdj~ai3+Yc z@+Q*nK&DBTTk|&`?@T@icly5JEv^_JmAFXzVwJHtt%HSQWv~r1hqy8my!_`Ws++bA zr+EG0TKqKdPQ@}zo^68FJ6@xR3GdKzQ%i`MFbba4GB|91fsX7P2|+)y(XhTZeBXr{ zk&;9Xt<sc${44`@@L?}^{FpS(Y;1<?lvMO(l@jy3Bn1znPeIjPA*L!P$$@1&>WXs( zzYRiYK9Em54x~}zMiI=(o=RiPPvP$e?b)NAQ+Q_4B$CpYj63wxfM}I)y!l+#GHM=K zdL;!#FB(aD^=^RZHl>=^%-})pX?E4?F~*l_Q9^h=cK`YTNqqi{FAp<fUim%H+q9f^ zEP6;!YXEgjd&zYx*3rF1zA$T(Grn>3DzY%$$wz%k;JT+ypcfZOqW2$;<GF{!a5!gA zzkXDrqf*r9K{X?oAi0aWc!=+nZ9YXeF3RDT-z-JiVHY`d>u<>Fz6Eb2_(HG8$+B0a zf2bmV1sTm~M=@U`Y5E8^ye#nstbbO+(Y_0OY|3}^qy7gMI8KqDQ4mTS&uHOa0loP8 z<dL|;#U99ddn&Thfx$z^IiqF9cnW!i>T<;An2ZouG5-oOEAd2g)HkE}DI5(Uy;Lo@ z4do9}#V0<rqOPw*Fiy<B@4ezlziK+c65UZ`#7H?b5qm>$-Ws_4`x&@jk;I=3I}sU~ z0D>zq>|4JiUeznYve~2H!u+$qw4!)F-z+r$tO*L*z89iL4@VM;o9Ubh6}0aWp%!cn zjw-tbRb~ru>WplNqcfp4THFVWdhO`4JCVL?HsD@ddchg5ND+nJ%;s9ED^T{!{b*=} zxL@|`KCh{+C&-h(TbkR`!X<s%1Gl{v(BzrfC~lDgKad~js5D|0|0z?CPTY8v=hBYw z^E>BqIx5Xvl=@e``L`<E2{45RyL7onZ*B@6pF4_X&#V_Izc8i$8U(0qeIY+*WueH# zAd~O5BAoimD(<MsXTD~L9$ITSnQEl(<-gT*bJtBuk(PrF&iJMYw_Yo9+h&#X6IJ68 zbbsTkmQKPCy)8xiUSH&%>`vors{;6S@+D=fv!hXix;7`2J;1+LE=5lz>m8lHrs1Sn z%fO?pkW=3q!VQLd@P<DXVV=%N{;9%sQQ90?$DR5?=v7n_NB=0mvz7|VlNy-+B^@_< z1@U`>2RVZ=kLi$lIqu|gXE=D9I=F5aK?TQ0)9u!B$g98<%NqaU&WAk|IE6mu_Vif_ zMm*NzuP>{lT3gQX@P+5bW@U;-s(s-l&UJHbhi{_;16?$(NkKH}t2$@q5hm#H5OVd$ z#>31V8z67^Tr79+p~z(FB5vF@dHfZB5`~<%<A%xl35MYV+yiw1+TfolvJ&aR{<t3A z>Qo>1ODR{78W~IV-4uDl(RWZvO+RW*9mkc}S@DbO<N48-Mshn;ZlNE8u_#V!13i=w zA$qWs@V1&pNMZU6WFU4{oO&5TJNFxN?lG#|W1Xc)?o=@yaegPYd%i&M>cdV^zo?k@ zyk3JIWDdiYO0h^=O3YHzT0z^Jll<zq3*6_T2Wa!&Ja{z2jH)RXaL$I0O4G-?(AzI^ zDW7+@thsLpE*`2$@7h>$pJNPQ`ricl*JC$~RKAaV3(EP1LmIR-cotZHzJ^9^H^SK$ z-D&)vf1=alLQvf1n^a+f4UJebi$3diLZN?+L<V{(IRByqC-Ette|BGoZuURT$-XVS zskpALti10Uy0EerX|8{WLY*|I<cCCF*(9~>>*!51D|0U}b2B>nm;}C`eBE*N+Ck1D zaWQ?~MfqE$wzTenEkA<W!l@b^;v6GPxaQaE>Ab8QdNlC{cO!EaKlf%1GAjPXi5k9h zdI4u>qpcA&DXr#JlBZCV=YwS;dXc%|I_{Z<9j_j>m3}_`Qb4~)h`t;V@)D2AQK{Yu znlmJUv&syme*7>LQhJCBT=*TW4O0;*YxxL{`p2XE1}mPZoaI_CxT5yYA4Ja5{+#~Z z5bP9TMk_RyVk&mfw5}S4j>se-xz`Tpdmn(Am^Cop^bs97*UI@{8;j(NuJJ8r%IS{6 z@Umr_#(@6T<LIzm0-q;ajU)~Up*SgpUJZOsAD)~eh^em?sYx#ZrA70R@fcTpNm>m} zZ#cs*%2K5Zk~QJK=8Fc|>h}Lh{_ODoL;h^I=n`LQ^Mvm{Y{Cq8B|xC6IaJMQ<2)zj zqr*KakiX$I@9XmvEt@bD?tWSa1r1^J>J?4gaBLops2R)kN-yMm2h`}RQ`y`en?Kyh z{fTt*;Z@Xaa5xQ{u;xE=MtEbH`G22350&^|{88fi^MCJ;GCVk)^g0EwQC2SG>H2*v zJVuLk7VRKf{_Z61MhGi0vSRfI&a$q7(<CZmCmR^;&lCdJ;nqEYOh>U6E4q4;(uJMa z`j~({*gJ%IsY#LcUkxBC_zpdpV~Nf_V^*fUi>#6wg!gB5lJOdZg(**jF^1EL$tqK_ zBQgeS4~X}XSb<!sBkV`!7_w)s6B83V#NnJRk>?BW=&Bst)TzwO_w`_(kYo(|N3(<) zOEwT@K+r^eQZZp7>sEP-w+uBSvIR>>ovkgCQBH(MKD+Snj}-XTt@zqHFLEnUmwntL z%`ANiP+;f?mQnl+KDYnE**#xjR!kUsDLyM_6i#Qyx+alWr!Vl`=q{GMI*|ynK0$Qj z4mNwxfXH2M#S?t@ll}(}Xp{mEBc%4Q>s#H#?%9p(&YJ7kbXYiZ_-;!+7&|lbx%1d{ zt{CqbBgvLtehg;PN0|8wOSaJ0g0*^C<LQo}EX_}mWR22b3VSe<aCIl)Q@q%+WO2_! z%8*>1xSc3TjbIYl8hGEbKw^0561gB{11*z%$e|5aaqW#_;-T(BN>bg(<pCwq)prPg z6npsg%vK}2M;vC8HiVM7IoH8pk1jc&b%d;4B}0Z<OkrPh?Ag`iy#&7)jeG;uSc$GN zbF+)Yj+=^M$rC$vqxmQk^c^LUCXsB6LkRjdD}oi>Sjqfc??Rua3+b<zhJ*|Av8<9i z%n<y;UE94$K4(N)`c&A3o_qMJ?IQNmcLwpy(S;p*A7Oo^(d5kUB<RmOh0n#-qJudy zFy+NOdNgYZ$ZYL}dU;I}^4JVMRd>+IQmSa>tv+OX={@>!HjDnbUkmcrPU10NzCz^q zhtL{+9=o0h;}X97py4Af{)a!=xcX=7e_i|XU!VUSpYgx=53IzVvj1!UfsxS&7;^C# zoARWVf7?=s)?A$lgC`Pr?ZKINz}SddUNC}rLb0cIYCNT&1&tXxEMCczhLm?g&)ng{ zzdb2XxN9T(JH?VD2DA#0ffmtF)W&X8tw`(pFF5Ow0t;Gr8{c+1Lr4GJ#I9uZ!O%K0 z5<0a9ht9TS{M%UYO>$?OW*Oq*zeRMyC^g*WeiN#@M?gi!CGq>ok^0H#!hMoRRbp&W zp=hkQdG;3jr!S^`drVRA)|0H^VF<bUA&PiSo6ly?`bL9mPU2@%kFir5G;zK90kUjE zF#cWlh)uD+#OjVU!i&Zt*1m2n>AU4c##q^kd$sw%t=U61U2Wpm_ZpM^vy|cK1`ZoI zsxoEm5^h4^OTn>h3p(OTE?s_UCeDzOr=q<*=yCRI6ffo!j&~OE6PMoNysl5eVVZ?- za)B#z9)A~PZ<w$HoG$U;#t37)1Te(rFZ~)coCI}A<E|JDmhGBPHVt1(<G-%Ma@UN| z*M|$B%FTh5O7DY|wb|G#tC|{YNXM=*YGj2(IV7|#WL1TXc2y{_2MyEk>Qm#etE?wn z+qwix-;Y4=Nea4wM}v;-QSoc>4tjID2FcxwB<0DL#Ocy0CKQyD2hN2;r}4X3;cp<Z zKJVe3$3!-1^(6f6VhUO9B}*NjNeVM|?8AlQ15lJk8jMpbA@3LW!^^l}(j?si-h<)} z)QV8DGQOWwdN30A`X#cieuG5;)1h7N0DZ6ajjG4!LGj5m=;QKncx;;y{3~AtxtjUN ze{K^UCut$Jqf8}nzNLK8s0HOe`sb3Ookcj__Y7{Ymc?1^JS<fD1-I`VB+0D`<(7Bn zvHor5M0Tz_S+LL@cfU}<+50k3ym-U4=;RC%5O4zPUKjf|`zrX-`C6#``)@QZeJ7LB z>OymGhtqux4{5>aY+Q9_FVg9p1Ap5>P|G<_7PUK{TCs05qQC`C#x~KZ!}a*(oHr~T zmk9m!rEE!0B)MKGg5??SaQW8%>MIuDpPhk3FI^9kty^IbRY7B-F}kJA&=%JQdh}%? ztlg#0+#5rAg5+4fn0>tL8iplI9wNz=JJ_4aLh73|g?{yR!YizH(;lm7$j`YP89%#D zlg9hOivc_AGC!K$beM?)^7b>i#|q@m!M(zTTN;SOzLz-ncL?M3+E`bW8oP1+J?h)) zMMggnksbT@37_mW!(k<uY)DQe{5n-(dekFWzp0q4kvodJqZaZWZ&wLpeJ2WM7%2)p zdU7b=*~_-(Cn9kMMrVCd$L@dKz#-cSLc0c#<B@A<|C%SncldpJVa7IYVNe+U)T4#3 znT*Ey%KJb@`-5oA`XUlA;W2cqJB!c$lOk~~a>RQ>5+sb>!n@DfgDx96gYR}_c$i;? zf{2{xkk?FBXeC3`#wz1SkNl|fDs4O|(wqcFyn>Hm!|)XCVR(7#N80tRnl7L>1bG`X z=<ug4=-2#6<YPV_D@WZ!C*_@K^wxE>_sK@sS`!W)FQ$s~@25=4VkP(P^n12a%AVv} zU!w_hGS@6Sk3GD%0!q!75r<cjq|^TZ(#*5Ks<NM{@t+{tAG-i)TeebmzJ~idwFK45 zO2CFKIdEDfA9C(_AmN}A3#s+ScefRDrh^m3Uhpuq_uc>}({mgZsb561AK&Df-z?yE z9hAl<_MK$M`;F}Tkq)wOnr3;woN9UUGAs7SJe~9&vu4S$4Y+o2INPK3TkPyDB!Wx_ zZ0$P%@RxP$`?+p7HnkdVl{~~lTQgbL#8ISt?=!A<LInHlzm#qCE&@%#KmI1V#ER{J zw(id6R&5;QZT(Nse+J@xZ>tg*?`Y@c73E>R^cnP`<AH#Cw~4G$Z-=Gf7TDv{JfY#u zVT9C6BiGyMWWG)&OPlZwg}ck*1$S17^C&%GQ{-oSt?DKYsSc;J&67#nHxCw`bp+fl z>Jpu+%Ivg27EQgXE_B|R09i8tp5~TO*ZeSIvUL&F6b8U{eL1XmAq@Gg&KJ)R{>Wqc z7bNwULUzkby8KQbx|5hhwYH8ZFU?WLdPs?w6=p$whZ%`cHUiJ>!&%ECN8&9l-Y;z! z!pP%!!dG4`IA^9jXgR!w49z33P1*!JukFLO?(KMgkrW91oba)IGw@?OS-e5auB~9k zFlMPH4Ej!j=5JDvQj&-s)vKUm{-g0V*8#d?wg+{ZZA3lou5p@`=LPO@bMe?E5j6FT z2Af-yCA3&KoZQ>2UM@8Zv(@X}S=$X~a5tUIl2kUq{1IBP*iI3Db(#h9#|FRvs^nVB z4QYqpXRddfA`7^nP0Eh-aurK9!0*k4aOLr1>Qh{SrmfI`s=!1pdmtK>l`Vjm5>x1! z;>)5jCACQ6bun7&b_8qAN~EcYmUP83Y5cvb8T>uI;6khizkELtPW*rH#Nsi`uQ)^` zQ3>Q<xQcg2dlRoYdHADX7JIo^ffUOOXR#(m;C6fj{Ek;A_({+duXt0~_&XmD`H_rk z>lQ-2{eC>$a13p+NM(O#bfS^d#IDa!2{e%J<GAf?3Cfp`Mi(Vo>4z&Pp+&ou`kP?1 zF2RG`lMZH+{Yqf)=jZa0ZpZS;3j4WKiEUsJd=e5t%&<fi&>1)jxB6%>{hKnZHN}(g zk9V^VCsf#ujU1k~D1n?`=8C8JJp`SA2;4JWkyw^3Wld+EAiI^eP`sj=y$fB0-9k_D zGA|4uUquJXkG)6or`)jUc`V(-MN{R{X#QuR9sad!5;@*4P1?3-F~#4R_*RLBP;aS# zC{%TVt5PL9@2pE2MLB5o>+i(oX$p3%8Okiae<PRcH<0a8`*E?m6j2n6h3y-+k^1?E z8Lg3oDcj}A?_=#GaiSJX8VpAhF7IGCM*_=)55-v(PF(J*K^lH?9gVoyh;)3+_yhwh z=H`@!eulg)O<tn`ZL52TteZ87yLhBL?AE&ScgxdR-L!T%Hn0Q#*il4wg}>pPmKL#s z#+~eX^a}RecPzWAqC);|Jxs?x7{@ZM#IhlG#*r)HbBA8G2@9R3$BHg?g2az!aBoTn zlK;AtdRvKTo~AtxbUMuKH!wjL%{C%yuRF*_w@>{0wH*~63PU;bDnyf~gyG=wH+Y=& zEV6WlLwVX$JK^fGL{cgCx<@Yc#@DiAQG)(Yv}?p1($;l?wN%DKW$OvNv8@^E?gWED zlq6A+IEdSBYGdK=W*B>YA5?8jg>g?@kkI=Kej)Bh7>>Hn37v<-8dUE%HEj#}c6>bD zA(bUs7b=b1A4wp;wVRmG#u(kNy-!DIZNp<$jb?^9en{(72+=>jy4?5`gD<91Z24Uc zW_looJ+F{v?e8z)fTvqPx_J^zXm%j>rd2Sz{sYXvB0gU@?;s6pq{+i>E9mf#$F8#q zXpquEEcot5&gdTD%NGw~e))LVDtB0v`enYz@opLYr6Yizq#?MvSs!IJx}s4J^uctc zCXzC&g;$fy@!KkE@?yEauvx2!^gVvXZ*j;Y6%Q5?{Vh4<BRc}6KF7#gTmABegIdC^ zaz~J`zZL(om_w|^j7ZM9l`O4k2wtD0#pVlhP{Ow%B==vau=wCHbbG}G8o5=CSTE~F zk6LD-*+`wnY<*4tDb5nr{B1@zU5n^evl{3(b>OqcT}9oQiO_cHE%A<C44?gq$c9^4 zWL1M2nb5usw6yQSmqIT#;*uF(@Uj+$tXC@kH&%lzuN;9bC9R3I?^LY$#~CgA<I3!0 zJ=yG`B7SyVJOu9^LI#BO_|L&vB(BJgHr^ix{mWD7m2DNYK6Mm+IPN8F@@k~p{$4>V z4h-QR%cr9cxx=W>^3ix&|6=y_RX;h>JBhR<TaeF>+rfVR0`#g~3xE8gFMdZHCPeBC zm^tZ|+jwQL^nZ~c;P<flbsvbK?^AN5=PYs!ILp@Q>yzK)4ya1q#a^c6xUTyb7Rm*H zj>sOY`U>&#$es9z`1vo4i|2z)6KR1@AI-@)&X0X#3lBW<P|EZi)X^-BI-fS<y^^K4 z=%@`$I~FW#I+e!U3uJ`(J~@P?<YB3E5m@i{AfAyMj2Gldir=wCylGV=yv%=vr>NXR zhj9qKYUc&dGTXsq?EyT@=O2o!G>2i+r{Yt6QRw?nf~w9b!EN`iNZ0%_in`y<qY{Sx zE&a{KWFAM`w~N_q(J%VN<spC2x*qK=I*cnf&L-F6J=vMfT>0c_j_@!x8(dEACi6PG z$g9{S-2ANw%zP0^+f&7oESIw<qgB|J-&at$IG>1*CJ=0L1qS_2Vf&Zm_{rdPh+cb% zsqFD2Fk%QQem)Uz{7?d0*SGP9kGCM1Jss%umyevZ*qdneP8N(E*K<!DQ>k;$WIE@p zJ=gLz1oXFk#P<r^N#>&zp?RP(yVYjGT5cG!6CtYPMATt4bJIO!k~4xxTzrnIgT1K2 z$IHm=N-nHil`5#o3&*=O`naeSL+~qg1^nV;IX(20$Ddl$VC42~^xO?~*yL49J?=N4 z_nJX;pSVjkr0S>0GIBc#7CY5nrCZ>r7O{`teFLgKC&$fMw1~Cl&ccSa$$0JM;pIis zRuY|~Vt;|(1sE6-$`WpbVejKDc#q&cxNX}C0!w>*eT*tw(W6Nwd>X_}-`3y_e;+~l zoC^@to{#b#Z-&WhLh$0=PJCeePxNiDiu3z>1plfE<IcJHbD4<<H~i_L_Dv(95lcgT zR2f%)W*m+UevD=<ui&;e*fWigdGy2G`&{j0P2q;{ckGDw3U+wcCc=M~XJ@}Yz{7Qi zvCS_XS$Cm4*|Wt2TYpG_rj7Pws#iX`DNLX?PA6zhN;RLj$DUnRJPvRDir{0TB{6xF zO4lUnU|+AVeA=6dkg2(d4mwFgv?&t><hjzD;?mHZs|cFDUP2=Zj&j+b$B43o8Mt%$ zCT!LsPeS`NS^gO*_CdLli4ranB6d~S$IK_);{D*smLy^<Z-Wj-Z)S?!D+xK%jhVd< zED!ksa#K&BN%?uS@`*0%ben~p%!U$+(;v}MS2c$I%)qDJT5(T&C0%M>&&3_RhCDZk zeQ4J4l)s4af<Ze9*OSE_(w(JS1UgutE_OO6jASn@vxu9nuzc?C;q1YqAIxxNGU~N% z!5Mwm$b}E?AlNJBSY4yY3j;s#b7YRDop}$An{y!{aXl2jZ-6(p$KbqrEF|npq?O-~ z(uRqEE6<E!XT?<HSzT=w`0E#~F~AUfHV!RbbP;7QyDoOk$I&hQder)l29o})MYVQ4 zL<x(_!R6UDhO6em;$c?hmHYFVv1&d+GVx3<`T@x7e29{>M$pdV4(NtaB)J))N4^<4 zLyF!Uc39~+)NWjWw;mpWi)XpfWABymu^Xjy)9ate;|Qawp_b^s_N!&5E-m{X@EcZQ zzvO@GUo4xs0%eb_L4KyM_;F#e_;LLp*XTJ1r#I}xAwO!MC{+z#^4f;aUTs24o;u>C zqG<l;6$$+IgAC4dVRTQOBF?(m#z|I;!gH^0qwsJdoDaQB+y1EIfxz)}gOnoL8OGsv zhb55RXhp7ftv5t&9PyvMs^M|%kN*RH!%DnZ{eS%dE_uV~oGEnzL39fCkyXHb<}1Yx zyg~k5zrXlyik~nq_!Xr6=%AcmH*Ki+CGdU~46Uz{@G$dQuwQc?n>{>FU%6s9Go%j! ziY9Qf-4-~bVK;uX&4#T=eMw_xi|_VP3dRyIvg!DNJrFqYFBRn4lMaQIXzso9cs_3o zTMmjnH?hBQYp4rU3Kk)q^mF)@)gs~|V~MvPn}ScNcH{kjGq7>_KWJ#ah<!_T!qWcD zuw%_I{C#LNHY@wa_a*pYt%MR7C0W3Rp0>i0Pa5EGuLM#3xER-!+u}M`1FUmTAa(<& zlKXo`vAk0sVD`ezxaEQZX?>B7SBl+U(<W8J*xF3$B5w-zH-BQ}C!jw%e>etel_T93 zS-7Igp0Eh9!`%8l9=1lCHGLWmkrvyrv&;~<JX{T`v*O6}+H~>vrvr8F@{AkuR`Atj z6Lh3&&^6za9RGxU5OjYG6vfWc5Nv5JaI6@ywyev<*ReLIpfuS?g$uoGE;#<Azs$Lr z2!?kk^WJUm1rsIo`H<6AoWA^Encr;(z9zmvV4QbZ<eaxpVD_}7Oj0;qu%X?;(cE6# z{adCh+SGZfRH|Z>V1a_PX!2mTK;-ehEb#jRJ`P$1Hd}i{_hxjLo&O_nO#PuMDlaoD z+YuASSDpJKI`R69pkZd3=-uHRoXg8pe&gX-ZU>jepB#4DF{zB_?pb~orH1|!O#i0F zFMp`X&9bf+=|6E5-3@y$%B>>;YuO0LZO>HruE;-v9a4QFEu@RNu-hoL^bwM9wdI~f zq~fvWwy4V~L|`JAB{(L2_pNvvMN3wwVkz27XQvs{BKHG)<%U`mJZmR+)ty6QbH~#; z{%(AGvyZ6%#!1214GJ*rrX-qfPiX3aAKdENJ+$(*CBDOM1Aiw2-zb-Z^<JOpZiVAm zo*e^8Kf=47m<t-ZK4r4PBzWz8AKXVpp|rZ^V2~7!C%PX%t5CIgpL+!9e%=l>FW#fq zmrwAV`&_hlX%>`zx`NhDC-~OV&#3xUIUID70CtM!SI9_!kx2|}ciTd3K2F0WK?3r6 z%P82^_KbhIYBlN~eHZ?1`-QN-11;e*(CYCg@S2bF(76{o`HL^N;ObYAIC*myOu6O^ zmP&i^uoM~a@k!;aH*Lee4<4scvimr%Uk<R~#AEpDHUT!LdI>(P?}n<yWjKDVCqK1l z270^mD454r@M+pZ*@47HoL|sNEw}rVyE8lZffyag5hgK^3?MVmWbj+Rl%35BVF$ld z@#;IyWAAGc)a{cKS@roKvwf{Y?jN$|jz4{mx5ela6{bomM>!EQtt)iz!CO#>pU`<O z;poxDdKf*S8lDBG!uz#Dsj*oaoq6#+vYrr0Zzo#f%K-~`wtXGl^8GB#xYP~3X<caT zAz8?fJ548l7=n_n2=P6!%j$*LZTWtkK8`ETp+Ny7;ZNTMXiTj@lf@pDdo}BD#QF2= z-XROv8fk;KueQP4{?4bV8~)M$sn=+-*l{EG@i(W|K8e0D{ERQ@odw~dg-CVZXgDhN z92sTJ!x>`#fVDVpXpx&C&bL?M4;GWy4)<I1p_U=;%70HkC)}o+MknHRo5h`7BPF=! z@dK&ohrquwP5RW*40Eo|%s1bGgv8E+@pm$*(Wf?kXtFV0shr9sy)$EmIw7dgScP@g zO<}*TuEoOQ(|FejBi#BEkos8wiKHWBv-p021{WS1(ngr_;hR8zZvZUG-p<|^itk7I zc><phcVwC;dSRo7E`Was@q@L|bZcq@+C1zTa<x?=Z`b9+6~$Aq_ggoPwmc;oUT(mx z7dsdn4|(thL&nh5S4|FoKGg_{+7@yyGUC2NSF^~Xdj!AUcQ0r7Rt8)~_H)e>0=byq zZv5`vI8LFyo%+iamMP`-h~5T$;ZJTn!2dmQjn16s&)J$xfx)0o6l*vcOs1bfhVPdl zR1(Uq{h7nZF3INytuy(}rWR;v-%ZYELLc|VdjfC%cd}^G?o0H}xm^18#%S&lKZIV_ zt3tbye)D_wZa}ry5dYxIJx=w)DbBPooW_0j=OcsVMfGU|{I8QTG-|IrS~ztT<RAab z&r6+3cXdi}&(}>vS-I!9`W+7(6?Ata1#hu;#UYE@B`xFe#J~Kn@x|QQF}d7#6i3rs zwQ#|i$-K$)Fw}E72@JlQiti*kLq+O`1@DtZ{If9$VDhOJHN{-ye)d|^r#4r)Zu#3> zaqur{V3&<=J7VyR-HA?4e1i^*Gp1^4s^~=DJZexI!(~7M`nBYYXyvs#XwRM$5xE>G z%82seE<g4`YttlAYWY~?kdMkPyjFoFODxg2pJVxnH<p%yj}y0FBNE9Tyir!6(or_6 z&{+_;%EIAksyD^&+qf5La^P9?lrxaX=0t*n^xV&FqCZ<Sv1F94gXLKc{asnkC9R1T z2}&=~kPFdezg_CNuhTegiA`VGvOUMqaZwp}vnD{aZuDyOxOpP-{us;GBsX$zBfs-I zv~|&XjbNk|mM^;ZYY(SBe-hHSn#pbJw4f?BVcgv~cY4S1C~ba}OK-jSfu4MG2C_|u zjx}FJ8{R}h;Ky5>-_8>BajQTSmE}ms$ql9Rm6t>1q2sip!<UY`bA(?Pumg0yC4qm> z66Ee`Ns~L*b5`2dq0Z|r{Z<0}%%2Io+WO`6>BP0PzK)3R9;5V&!$?q^vlY#iO2V_Y ztAq5TN0{7>Kx6+|qksRx;g+ooGQFdV`?Q~N%>{N45GuY`s!Sfpi3n~nok87ql;hOk zHz*_0mii}H;+^iv{9y}YT5vTE8L3{TOFbOWX6LIoC2Jd+*66_Jm|USHMw#fdG)47G zjZnakK=eDo1de<dhRT%;K}*pIPid8;3QbFqnvX6__0}LeH-4w#x0DE*G7(!{`Ab7G zI{A0=n(6$Gxj5QzC%$eW&&{Z9<VUG2gQ;`IV?_&P+SH<qYzw}k4V&)K`J0ZRMTNFd zXLFubnXdt>DaN>K+a=U7e=jV6WWmMHHt^#_1zgRXfWqb`(Nh(!5H>lSZPD^W|Bmco z+Cno}nY|jF%a_M;nPuqN!%{eMz7{^!EytH$3~-uHo?*3x4rt2vZ74kD0PX&64=v7~ z=;vb%z9a7@-dAuEYmWJd$IqONf9-6bYjkU{_oK&jeaj$U9g`?Z_`4J>aC=@hU$zg) zXR7cv6J|SbNiu?k>-KRUFWlmu+xBveZX5Wtsm8FQdz|AEl~Asz^#ZT=?toxn#vbm* z?DM>biZ&m-eL(akcrj<QJA!@)>Eb-L41@7=vjo!=G?Aj|0(wbuIJfedf%v(UrP|kw z(Pg=4l(JC*O{;3=mU-3j)_U38^y7V^J99m$m8TSz)+puF$Mx}#uUFGAhxen~?jnBi z?aPiQjfeAFriY+&p?f)5?RtJ>+;()n<qCK1{Qw;_e_m#;WJdqOES@i|=gJ2bQ5~Db zC}&+1wSMI;cs_WPW*?XXhK=j!m*!gZJPjcnq(N7mJ;&+C1i_m`dziY~jbHZeBbUy< z75VPJ4C8i2Ayq8opJdr^yOM$-@R~FoI;W94W+DkLF0=T&{Ts1mwHuvX7LD9e#P0lW zcai*_cWA8sMLPU%(tq+>g=t@h{tx&KE3u#K|2n@t5akZfORRCgKomBAe?{OnkBXXP z+?g%QLe+OoVe8)ANKmmGCU=^Patt5fkWG@bDPjUO`k96<Y&eKoLLQ*G;|NrQYVy9n z6zPkWURr#6Ir=x|7z)S^{ty4I;xZuh-}4))690=oz)I{X`)}u`gX(d}SXs=r>bkJ; z_Dx)`ohh`#GW<E%h3@-o2)8XW#XUYD?tn4GXZ1KJ7TpHL@e$-z$#Ayymw9=^RZZqT ze5m+l)I8{XzY>moPDh6~A>v#62+sMwW4-7Xw^jWDq-KS{cAeuur`ls%%}j3Gf=#e$ zW*Uvi^~V1XX>T4)Rrvq$nnPw~o`*6fiE#GwY$}vel86+cNhLHWjphs~Q%a~zsfa|y z+0SQdKuBm%lm?BWB+a6`f8Vw4UF+UI?r+`iwbuFjIA`s%_wyOvuUAFpCOE(S2$<Zt zNW||3q2UiVsL~I0NJCi~Pm5lNEQDz2SvibWM}<-L4Xg2yi;`d;@()vSONqKxHdxBv zfvfy_{7&g8eSnp9c&0*d`17X_)Z`0wM>v1mcpo-uX@Q@)n`!^hENZEr5gMDen^m~~ zhkQ%QCAlB1>Du>Z?1<+oa_v?L$b2;bYJ~*N6P0CU^)<nK?QtSdb)3E8Y)F`n9_m-Q z6ZYQHi_4OlshdQDv|Qdp0<Q$na?27>ja@E%$L<x~-I-11?cV0l>B@rm`2}>qLo-@E z$QN%LQzkD@2-9M}gdA2cAgtlMO;~HA2OXIo4eHrT+4cfinl^la^`)=EvdG=^Ord3D z=g)eqr71=)<-Uk-8xY*jctTc(7Q;*KZ22p+#eThMDUzKMfi>c5>XC&Km<JsvJH}#= z%V0ELT`?RyQ%|yA)K}QwPmyT|8ZvSC7pB_qS?mp}=laadXfzHD4`&-&vsim^ZHJfJ zUc%n+PV5=t%tF5nE!QZAGgR*LMRUy9*9|GST5ul7_i^0KlR@<G;2Fr)zd#Kd84;HQ zC!zAob7BJe`0d^%6f?36<yxp<6R8{yI8;U?g2b@o?p2iKuet2pmU`IbpT&yzggeZy zF=+U+^%OnTEr=aD9f}Kw@4(rGQ`mWFpV>btC+IC%-gxeKGuG3709X7Z*s|)y_|GFf zOk3pPW3|$B5zfNiGX8MA+KCNo_=el1X(Z>DPht<mVLK7d6LRP<`nAFduFWlA%F53$ z{aaVz$gX^3H!_3V`Ql0<OF6D;s4V-*aUnS0zs1r==dg7y@$i1tVmjc+D)#Q547^;k z4~ac2$E_j5w8oDNvOU8SqysjQmkZSCsJ)@|@GJ>DY^O)Bm8-)+Hse@HwFBGgXyC#2 zILzNXotxS2VI+FBXz_qFaz}Z9RJGQSwmPo2IDZj&;d6%^vH8rXB$kka$!Do=9nYvH zDbD*7e~CT&Nwwi%`C)q2tUR_iD4f0@cAuTr;YG%p?}Ns(X!79pT{>Muki8l93JW}4 z0OQ}r2q_2#FXtJMW7>jWW%2Q9w^_7NxB@G-AdY{`BcJSkBMa?r*NGt2h97N~#xe8t zaq=QJsD2vHFB)h=*4;<Y)+3E*s$mZ!d-xwSJt7g;4e&^l<tF;a`%CQi(pBtp4QpC+ z`+f*FxP!ORQ|K3er_fsWOUXp?6|6a|j`c;GaYV^+>=Ahv{C_`yXzo%vP%a9!AdhUG z&qN=*6!6}~I(WmW2-x`I3%Z*m4xRS`*_|@xxFy~nuXkKX)PIY>{pX?7#iz^ghLc51 zEhs~0*?CfRUW`7VF452=myTQara2^g={O`UEvF-V>TpGO1-s{CCFriXi4{a&(Xacr zI!s&fgf^dg5jT80PKhsCj8BQp!S3@1@b|b}daKPw`tJ9oSa&_gFL7IrKP*Vc)7u(J ze5({J4c|$(ue^_>jG8ICG0t7{zJjWM_XEv&`I4Z?He~u<6PDhl3{NHA;2`^Hthocn z6n1GQuhY1_=G2XBsFW~W|Fed5<VMZ&y<7Q}OMY<ITPb#H2m#vj3#_l+LSH#?42CcG zz<Z7jCaqoor!&gf@I`Utn95Rg{{+YV@Q@;EO|Pf~izsf#ArD%e_S9?atmbu-lf-+E z1kz<<k(%Uv<f4_xNLC7hUsob&J}{Y{bg~V;8d|a8v&<R{Z(1~*d}0jw?mk#aM1VH& z&u2$Zy#d3A9#}gunf()PjN%RXg!i-&3<_4z9~)Dsc*pJ7Mr%0&t$27K_KFPVJK+zH zG|5Kdh&9G_u-T7#YWqFz-V@V@LUgq8mNf^MnFqsAi=8u>;hByuY{UGr>qiKWjUnHo zC)1|YV^~LP2C)v=;83<bid_K84vG(S$TE9n9NoH_J-f{Udzqi2cY8XZ;zt>b1=kz0 zR0~DgeP{6eZ0`TeIgGky?;>UkH-YMW7k2YXCHhoL7M6Kri2n@iMgmu-a4zvfEaiTe zgn2$g)T9M4O;H}=wP)Z>LR`N6tT<wp9H2#;7vo(o??YPbbGCVo4gEHLMMK*+VMo88 zCJnm96L2*f*i|(faPP;-;8^mN{InmZ!Xt%XEaEim)hAG+ejM7p(?Gel4gB-=(oI%F zV9TnrGv(hx%@rB^WWNJ3xYUJK3H@OxnQM5@do^sUDvW0=jv*cnk4YTgk1^tU`Uw@y zRQHDj*eoZB+d9XgMNpD8So;z7XKr+Od3cyMh`kNU|Ek!YW#eQ?XclxwEW^A9ccA!^ zAid5~g*a}v#Eu#z<Wu<)h;nX2PxIWdjYK|dD5xbVYcS3iG=|bfj%jC}gw1!gkm}>- zi5NGhA71SWZ?38m8Q*GTmz#+$6vYz3Wm7@$rx30^@s4cYbd>PFUnjq;SV-*M&1Ukp zG&Gp~qRS)(=w^{ttd#T{Z%JEDtELcIMk|ojY#)N}Z#ZxApB6k}atg5NeVFZ71I;U? zah{wVsI?X{z5n#_b;UO5Q@RL8x!G^{;)h5q)g3#9?!u3!4G_X{C>I5Y;{$sgK=Q}{ zQT`*2-H+QsFtZO9vz6qDo-C{OHIE()w<TNJo!IbJ;|9O61iD-xiPiZs!7zF@w8wcG zJGbg0jRl!(eA*OL-}jC=+nPZ_4j*OR3&d#pcQOu?xX-FLPi4ik&NCj!1^nN9!v4Ab zthqJ?6It_^i^3M@_XZI>BRGH<w>%{WOD|CY*44z%WEMmfEhbb=DfCXH)kj_xqQgY* zqd818`&qbx^*G{=71Ni(KlF$=^(26EZUp-{TOF>bT&EMtw}aEe40I$*oORqlV~emN zk|(KzVWb30OK9OM@~6n8{Ked^R0LM5T>^JUGI2(_7p@Ev0_Rc{$ki+$*F#>T@5N(8 zax9Hnw6PaWI9wy6!Yc4WI+g?t&B7B>hIGiI0bIBIKJNXz++oyp4yzH9%~skw)2Tmy zK=qRfOb^@Qmz;}Ps?(Zyk0f($UOiYCcNlH<Uj>iMjj-G#Aspi+ghTGsVOst$e^fjK zx^zNdMxFvr&pt)%j4ePnUemB;`DK)Ka|X=2V8rOU?LuXdUSxZy6tsLli5edD@s0cq zp=2A4HwF*l3hw;3V@HES%wQ@TIO;>EKk3Hz)b<gn#Rg2^`A(#}_W&}j)M1r;j*!h9 z<Ku?uHC)f}NRB@$qzu;aNYvyc*t5R_e%e~YQb!BYJ8LzT>i9yo)r_GWm%F6N4&!U7 zh3Ewl#D!Bt_`zd0n7J1$P|I)_8NYp=@H`hV)1*^L7w-_vSpN^ZPd!XO-+7uAaoU4c znMpQ0*Z2hc`QB{fa({Yb_hFcoWQxZls^HLeMKFK<5G;>`QcKnXTEsMB;gU0q<DPiT znBAs69@69-D3)+<Uoz=hs7B=PYa_7*iOisW4E^T!H>6lJA3wht0ur-@(R26T$j2H} zS%-I$m1p9iN3@9S5<UTE?MB&o4*?4mx{}#J%W>XA4F{o*k?g!J_t=Xr!nEVH2-2aN z!rq;u=Foj^A$!RG9!}X~3`JvpxMgz;swuT3x@Lv=%#nF?#hRno!1XG+%dzMjoDL&b zW+j+eg%e?q%dF<&ZuCt#p9GzIh~#yCQMZKj$?mvVGBfWkBV{{<U2ys-YT{TdXT7cH ztk-*>^}ZC*+kcsE+!4ZMlrwSH&_`H*SQr)_azm}++H8B!Np|V16gId*gWddW5i9yF z6R$4Pp_g2mLf=<vgQ?m@{621nq5en-J2q(&+w!X$ZQ7uUUJ9&0A?Iqy=E^?QxwMAd zP8Wc4hd(i*=99p@awic@j6#BH=dgD2JESc!2J^J<Q8INa=)UK24N?bv92`B;@!LaU zAn2Klx6b-R&n@?6+mCBC_|EuEq%GCifcL3%NAWPM-~APCZ}GruroD!Z#dl$;_c+d8 z_MF}{x}RNRdmf569s^MoV^Y56F3OyhN6!yTLDRnUkdzrL<DW7D(euP1zC@l9Iev@0 zGEb-9aYU+v(huQrtsOr6ESkIiRU6c2o@4(my#-l~*_eKC5<+%s;?)<kVCc6y60>>% z){-$~JfIdz>IGoERUFk-a376-{LPHC{y`e#Hx<O?>#7nL!uAAlSfFDH30uecE;J3M z&gJl$v=I&STIyQl8xD%R6YKEKG9D2sxMQyuHT#+c5#YQ+GLzzIp`kc-rp0Fb#Pb*Z zqNIx6p(RV(B+Ox#-j;%z15T`KQ5IA&IW%o`k}enSgXgEbu(;rU`1&H5cB%hM-}bDb z_li#kU8!n(?G4Ardhi@kxiYlJH#gwFqd8uW3CvT{#7e8f7}*g?e7VaNR4z=03EB~h zY}<$)^!k9R$_l({<vh&1&7v~~J~CcQvtWJPc884i-;}BPIa>H(F}>9G0p6dsm)0(S zNjxsP(7u;#=tG(bxYPV6if>j08M6yG=LF%XDU%`nstBF;QH+-LxQx}e$njd2$WaAH z6d+`|8c1E7if`)_(1!;6Fp|5$sC0_M>`zH3^IZ(uy3!DcV*;Zmm<*FrIR_1Fr7sWl z(>L`j*-P3?L#24BLqDBL^KZ#HJlSXLu-Y-7F1T9{nPScKZfo&|!if+zTS3+#ddf=n z$W=A)T=9~Bbz2OiPrA-NEuNqcR7g4`O|D@NzgDDW!Ukb)=K?%8X)@S3Nl^9Irs0t{ z%D5v%8D2MD;1{1KXnn*iT<}X0>}(e^<4aE=*|Pl{A4U=WEj~oQ-g}yb<qD`-Ud~}g zDow91x&lRt@mOPq9)7>!8)<v|9y?q2kj9~4T>PtxJki$2vu-{jvb;^?j$b7n=L^te z*aNp1D3O3vVNTUNpZX_n2T9)M<hW1*PQKZOmQ~mTE<Qm*_8a25vNlq^J{ggcSoEMt zjcnf3#uTYEvtAcY(8-)P=!Abe8*@*L-J)^|O<OG6V9(8v*Bnw{Gi+5^aplPk7B8e5 z{Nw(?JKcPC+vsvAT5C@G9+*mevJYTGUstwnB#yn469nYSI2enXvFmN6;i}72?5Fyj zbbr#N7v6e-wq!)1%o)k#Pk|?RD+N)9_AS8mVwohPUm5RnUq+_Q8mD3}<stVkoRgy9 zu>)_+$-yZxhnCRioJFO2@I_>dG=AGf$N5jEBWC=ji(WM2IXVxqu)RJsqG!m=l7`%A zeGqFtpEk{yPQPE$gkOEKq>UT`nY-^~|1+mO{4#suznMRCe98Z{f8rDLo}_*GhWM*8 zvAlf_$?v>Ermoipsd6Q_c{h-(wYmVpg)Vsix&U<bgByhdRM9(U&hIwm2Q}HdjVy~f zi3-&oF|sM^$h6WsXgqH+UfYz2RJ=;@*y-aSSrv%K12ajJ-dQ4A_>u|flEx8EJO1;z zUJw29{cq;a99Q!Hnm@ZHWaAwu0>*|4;Nwp_SQ&8$Hr+3TERGB1=um}De-_8vcxNDN z&2#wnYbh4$2u7J>O;|W>7S1WN!Ir8c%zA4Hwl64!W4$$?yg~sw@x&2aJk1nWsH%~} zGxp-AB4^;++|#6ez9v#MipEoV#4v3Y02}U3;>{a=jpX?GsP5}Am@4XqLnekWRU^*) z6%WFxyd0=WI)@|2!q~^sswnwi3Y1r^Vy17Ni$C<q)8;N#Ff4JN%*ft~Lld3qvf}Q5 z?Zp`GS#yn~x__)YKI92+_YRW&bO*Bdm=IKD3b3NrDTL1Hv%9zX(q;<dAbNN<zMLQr z>%Fr;#x)Zj=TWTd?v3O@MipKq-iS|G&Zg(9&>*}hk5UO&!a4my^x4o2kU7wY{M>hg z<2|mwedZnxJr&0uh<yevJ?Fqoq?bIuT0rv(_QPr;5jt&605prH!J&j}u<2(ega=gf zXa0K5jGDBPg}QC1F(?ZO%UnXrMM5B}lMaFNb&2zmNw^#M=%8CW3gY;Col&P@@jYFj z_vGM`(HmrwzZ0m+)FNxz8f~1t6+LV|i60L=hTMgpsEL3SqQCAje(E+sZIR0(JBFgM zYUv*o(`x{0ew)Gu`P=x2u?fBILnwaO&+)*v$HBi0HM~HU+Z(p*ptgL<CbExaa028q zG|^^{$AcHcQR!p|y*CM;&z=feqBEf^jgW`)o|E+(W{^%3d+z-7iur!WlWgNpA?}t= zEKzIZSi{oTa(*1NHB6A#8`t4-#PI`*5S^Fg&cFO)Hy!nv#Z_%%WMilxZIjf8gT$}n zzuWYn-d+?mra!>*Dx<OT<Gr}Fg^!|ZGV#meagKi|z<xip0H?<r(%-%BqT)jzxILK{ zO!>FHklQ+i)-Q5kT8^digrvgk;Nb9eP0bQs$&@QR8#!^Fc;;u`J5?v%$n;o#RM&O> z$sc=p=@aI>U(?_7f87+alWLmEoUFUf^Bff7d;F26t|;#3O%IOcFWRNT&s(U;XNk0( zUUMSvv}`ATbMqG7-~H))kEfZu15bkZGovgijh_?sl5%_bt8;eocW9sE_ujn9pSN@> z?|kJD-+H|#Z(~6kkA31+zpjhoO|KE>H?%I~J*qlypXk)Vo3X@=G0Ky(Pp=AMmc~7< zFKPK_&z5=fuFcM^=jQ~}C!H7J_e5s#GGmJCZ471j{Uy)p|88Exb2mSYp1Ni*@G_F* zY<)wjJ~+Z9Zr5=8aXIvP&U8?l`kAQCGbI6v)#y^qDY9>uG0C}`iT}~Ta6r=+J8lan z2GZ4NzSdb-(VPMTYP-lj+t;Xn&2Mx{lZV2OmXcw14qiJT1u~t2I6HGX{9P@EhffKy zDl!@1Tzw6^x=nbNM>OGqYBvxk3J=^kO!W-bptpvC>@45saNSxHavps_5`OQHn`Z-4 zys8m($NdkJX&fHi+eYb#R$%|Xf~X|X5?|9<gcYnC5PdEOcYG*-NA)+z^R>A!-#Zjf zUL=XF4A0WSJ3{g2vUVireFx@lQ)GP#33h&0h&{f{!gOyAF4{jI?ToyEpY>cp`+xH4 z@7%tCj41_r+4}RiB=R_>Lnesz>O{~8*achiW9XGJdN9%xK?Oyef#gIDP|56sDEA7= z`PmD6zvCKn<+CtbdOQi|NuNbY?~b83M`e2BHB0(#%}L5KL5WVOUBNLh&x4#mJ_s&f zg?i<6*z?$d-L+vo=Pk^}CPx-9A<o-T%NkoSnkH5MLhU@1NSz>$);y;6O$)$pKC7~a zZI9zI4^Q|KxSLI6mq6IpdD!{)AM~~1qW!8T+5D|zKD<)L2})b}JR=-<6AjNl!^=pi zLa9^M)ld1@P~RcBmg<dU$ls<Y``;J)$-M0~)TWKK_TQ#PQN@uRJQqnCPFjT$`mZk2 zy48X=TS=IC8kET^x!up-^Y=WF(dg&bU;aa2-8n|Id?xAgs6zp33>nW;>gYX}1^?D7 zLRsJFW3u*sL}!jwp^na{lra5;@|RFXl8X7Lqx~=?wDB!dtE+<c_b=whY-mKqM2u)? z9VOYScF1g9Aah~!Oe8xz8|7U4!1$iYV<NscBZa)nlxK?)49IRlV+X>hIp%fb*qf_V z*vX^B>yJK>ZuUdE2V2p#TvZ4%nvMnMhL9`!R*|1uwegn;OK=l>&t(4iO}%Iq0nO37 zREw<-lvn0qV}(JKHMW|F-qJu<N6wOqDZh!U;x2Ty-~=VjsKM?(6!|x+0IjHhL3D}| zNSs<JaWTyyrhYQC`wnL)=uM-(ZBT|{?E}<>S=&H3-52%IQPhX=2_~>um|UAHj#nn% zBgnM^MOk0PZ7yo4d2Te;<nKoY6{-CCn+NdH?3+wN`v+o`KF$}6I|EwhPLP{Fz97At zAB>J)KI)mV2$ltnfxX*#Jhv?aP1Vaow|uw^+O$siVyI2=rwPDcsZYrBhb6wraE#eX zIkI+V7=(XaM+KzF;14;CWVxCpI?<$qI|mP;-M@u_kzS2L99r>SU0rPZSDCKwdkG&_ z*-*JYu|#uuJ&F?hiahc!qKYteI^{z=xqjH3)zF-RjdO(XLDwPt^-L6g*Wbzc#+4yj zc>wxOIbr4CAyld0jJqsrV6jyd72xiT@4q`kKBbG0V+ow3vNn!{3$6#P0$X+>t(Nh< zc8pd1I|I?5*HfR5D8fz0J*egGJ(4kI0L~2(Jf$yP^|x%)sJzLyseNPVq~zuWbaI6- zbLylOI^I#iOc$(UJ{$|C!mE^Fs%Z*us(BHS&~&7p9W<mKCKsS}MW6YiiKcMjcr9r@ zAwV(uF4WMaLY}pSUOku3V(N5Hlcf=dn5ye<h<LRGnYdQKM09GPBik<!ZWV|cOWI1M zw;GXjhahs;e>c+Kx|D=%Q9;4KUG1!Q<}zE0ve1B4Bs2ZqePrk4MHyR~5s{1M`C0gy zed9U@<hW-(IsNSl)fm!XUuz|Y>H}E2aQQmQ>%1(!Ut7RDxTypU7j{yq8Aa3_Hz8Em zkdKZ>>XU6BkCB7tr(xqTM>IcWGKwVWI6`JCsonLK>c{dJjvqzZg_D@%rQsm<QH)fs z|AC4#3?N><9&J^(2X*Qp`qkab&7ag6tDsNh&XQ-$`u+iGgKQVszm8D*2et4lk1l3# zk{tMR+#KC?5Bb-Vy{Y)Lo0Q-D0sj2s3<_uo=5;j>BjxO9#^p^FHA`|Db8A$K+^9HQ zFHI!K8rf(3ndvL2vACu9t;Hf}GdV(@Rox-whFhtgb3RmCnl*ZBoXmglRh=nSTZ(E{ z=rY>B#u2|XnAwnbo9M?cN7WKXkpHxq^+$IHk$sNQ<Zy);H7IkV{@;B^{O6!D_q)_Y zIV-);%R9Hx)KDF?g_TB8_T{8~p$T!~oWkQaIq0&~74rADJei{@L4>{gsU<NCYO*Qh zeg88|9dO`$eC}RUr;H6!+joQV8ImL)0-R8kw;EVQM^ZQXCeWWx%du$rCF=94U0feq zk^gkTY4SJ6gfxotk@)&yvfxfG*%T{7^5@+~msYnT`9?jmUTz~<)3OflJJ*Kx<hG(r z#Ykdzgoempb_^PuO@4jhc>fPRFo#q^IqriAq5M?-a}F;8`^o>!{F&pE{r{dnr|mt4 z^ZRs(BCW)0eR6<IUvU~q1RliZM=vt3;_Ik?=O?jW^Gxt$wa+|p%p!r7e&*i;!@9V& zOHqDkAL-hmjm^b3;otUZj7Qit)ZKK2M9$pLEb?4L9%*-=|JmPNDF5{Ie=~pPIA#B9 z{UqsQhX0lpqg{4O;9!&~tNSYeeQB~HLY=3mt2GxH<vVl9g4`?h0q-&Jk}|RFB1g7R z)rmIfZ-V0VeGcwsstp6JX)q$OnGN<G#P57|A@^}F(!b>gn|13KXno6u^-GSzg5TXR zh(#g%Mm1n=AM5DGm7pT;3zWGpvYD-b#jpP5bqRKm*=OR=n$?`I@9GIgEb|jG8XDkS zs1A5faRsqLn&|y%8#LwXY@8W>otk?<iQZXM4Li5@(goG2L@>aGmUwOAFt&OLP0wYJ z*gGL)b!Ql-ty_Y0%5nkUIKn0_+y+K6Q@BLod+2xhhI|Xp6A5`W_KCL#D2&Vm*DfvA zYB&@MH@mREFZtm?)kY*)okCVj`a+V5RH37Kj7fawM$BS`03LIkdYFm(=5RuoqO;7{ zy=*E3Uc(ZP2-*n(Xgj(64l{q{u@)a!!dK;7HfzxmTEg%Vghqd3N>8k1Q&wleAuSD< z^ga~aYgIUx*gwWXXf^MxiH-e>6CTiu<dFUiOQKR`iwiCsM_Zqt!Ov=3V9?P4>(}~Y zqF70(NyaiJ-&Jt^I)sbg3qy3}FQhPGi`Asplkj&+*v<6>y^E{u^z(DDtyyY=@<ZBT z?jp^GCx$y|pFOvbQIi_|?4KL^%)o(WByFfvi<{)09Ow2-62qv!fD%7bfjgajh*07w ze9)>0`KN88-gRl=2*!tZO5iL}u%uARU;)XjT~3Nx)#;P<J@qkP&yzPRcakT^Ghpz8 z2fkZfLF^;SDD#9R><JkkR%vcIy=S=tUMgsXg|(Wo^O9IruPBSHYR$nL-@CA48$<C8 z-aA$#N`v)#qR(C`+Q){aSz`Aa*=U)P6GUDvK!Vqrv3Uu_id<L50X{dmnz;^(j1PdG z+B1}JAEC`^(s-t_6{`71f$!lPyz5-1O>@N=Ca1s}K6-~E<GZrN$i^T0e*Z_RWxFxE zyM%sMsp>FqUpgBo>CTP}4B=ik1^UqC^>k}_7|uxE$m$99!#1l5ZZ<X_*1bN8CUMUU zl4Ax5O7XB!WHym^#LzNb9;aqXvyX)j<G)!iiC5$>^*p|mw+vLkB618*yj#SbX{)KV zzsyh{*GoF3-;XT1QmBeLJ;*Gy!48TXgF*5!+aFTEDr|a4+k8033apo)tEtKC^o!yR zDaU_;_AxtDFWAq1vzp`}J#{hd8bh-d&0KBkUr#3;?Si7h`P8g7ip~|Z#<(PrT+@3D zhi-(}tH}sq#i;E#+&Kq*di{)2NYSIuOumD5ebm5~moB3@|MK8xv;}t1FoL^VOvy5x zCs;`SC`)3DX$hyRSg^{MKDu9yRf#8fW$H@$=Vl)=Y1s_=Qt^CzE+vKCCL`7mI(rKk zPj!L*#v*dNr43wMQ?ZZ!S9ta22E<KIf}%)GoOFEzG*eaC<?2V^yp00x#rH|%6b%@F zXXM(+2c$Nths@64LGd}xxt+EL89Tlqk6ol7LS+jqsyRa*yOrX5LtmiqV>c_YX9E@# zwWswW?dTKh7t#98gP``v7(@$th~cYMsN>HO5+0mP)Ze~<%5yE~=KEw^eV_&f9X*Z@ zO-Y7lZ&>QBP8DjhRYtG-e`D{qbW*00#I)OvqPyoK@!~*pvNPxr({6DWxm~p)>nA)Q zByt*xQ0PIKn?{(Ei}>)`>p0!S^&A(dPisgOFm*6xKjF@(1#s>}7A^C15#-+83Dr8C zRR0s*hO0(yEMLKycJ@%GN9Wz(dT3%q+!P}r{1zM!T*62(2<-a);LBUH(QbPcxc*}S zHFoY0^<mmw^g74@*<N+S)gP;oXG}VJB{ze7k;+2vFGusFp2zUUL;aE7^UvhvCQsOC zEY2?UJ4oyK$*?;|)94i6mrS(mI!HAUCjQ1bpuHo7UOP}r_eF+dr8znz%qk8~UM>w^ zVluI`EVqkl!f|6Oevl9S67bJKiOx#;jw5WJ<H+^*P(-yUswvjNc{k<RhJHR-v(6GE zm#rZgv(kvRg(xokEI{@A^Fs$G@5BGNyydb-8zJh47wWmV+~KiSodZtDr1wO7V677+ zFj-!o)o-ihc}!EJecuk#Q%>GR4vQ?oA^8Vv&sCzIzs<k_rDtK}bP<LjE+Z?X%Q|nI z1-E|(fO%37W}%n)rQpk(^iB;=e`^dLN(<na{wMUO0Fl<EFPH`G6cx3=oN8q6k;3;g z@x|)%P+MC>H!2mNwcd#h)AY{Ji&rJk2M!*G=`+vKD-K^KN)Hri{&*JKb@wM5yI+kp z`lbWF9JAryuQXWW_LrRM`Asd@&w|%%WvEbX;dak9Fn(NabN%zpL?WUVxn-s@!M<YT z{UE1A8uuqx{MK=`|9O;Zagj>(@8eB2ia`$-_mgjQ7(QlcQy*w<LYrMRp`90=W~%~S z>1Xdv>6>dz*%=obaJ5Y=9X{zYsO+s_C6AvYRx?77k8&Giw%;PM9U}CYcrtiJ^H^Oq z70`@vXRIYB<92=$&GFROA3oL4H0cHrk9tYnKBJFp?#@L@&fk%~?jX5j=>kWX+l*(k z820I%AO*V8B<Y7NOeoBP8nqbuyy!o+@4!tu?P3vqDdsD3JuQp%9(2<S_H(W<VNaN; zz6<Jp2{!Nv4YS-hrgwHd7L;s&!~HU#!1W&Wza0Y;i{<?I>STEM{Vdntx{RNjj-!i} zrsS;udnQyX0)4Vwif6+$=AwTLYE}}V=(jJ>J&|!_9_R#v6VlYaq6vPCperk&QprZQ zrD0?JLWj!934oT=L4(9(woBNZUG%hyen9nL^DjfB>h@1M!|M!v$66aM$aTOXITpVW z7(gNw;;dpQAFsT<2I{$<Vu@7~y3ThX52B8cnEv<VM5PgLil;$+_PGI8!*C@w>C;Bw zHw|X=sZhyVRq#2*5oVIT4&1Ee*aD~*Cw=VV9K4lqWT9t67|*gHUw$5W#XiJObye8c z98>gD+#oFZR*NGy$vG4o?Z7)8>Y$=8JS5%fj)Onepnxk3x{+B#k{XXdchwnqlro!U z&c#B2<p*+M<r-LHZ-SM_yV3fBR`$l(DiWipgTuht-er*UaL0Qy`%WkmpJO}7+clZ= zhfjQ*dk@iO=96*Ud}&&J6dW!u+Dogg48c}G-S~z?773D-!Oc4EGzsAR9?d%JWzjNh zhn1;=+w0-p@&p{<Z^0b)y$$o<%h8wSjgpx^=itO>VVES3S?~NiNO^`L%zJK)^fJm> zk7s^(UC9+xyD^y9MVuwpRdUoGJ1ydLJ_dbS^BJF>rOJL8Psib#17YrflS9ixO$U>s zY3#7IJe%IE&rE3@2K3Vgud+92__<h>cJ9<;FEq?0tol!MaZwd5yex)Yd|U}WOM25K zB{Nw|<!Dw`@g_{?xk9+<BM{zqm?ZY-g3$H(ur=QYPHuj~6z619RvA-ZLsbki`86Ha zD;`HiA0o)RFgv0UG)`g$Ph$PHY`o2MmqVb>W?EGB6g#;w7N1@tL=zh=tZ;_Q@rgWu zzvFfAXlN0B=z5Pzx3&k}shwn0>K#?IY6tV?>v!aPWEEWE3n2gbi*fS|Eo>fe3|+dX z%bdNdOTJl|kV}tavEL~Q97i)z{>(UPJi(ZJU)o2GU3iO%rd(ideP~2?hfQc(%UR_5 z@?`o}Zza8|tAlN*@OJoc!IA!OW<D^+OX!|WvbZ=Q6?Z9#LH{3V7!Hp|?hkUwkm_Rm zHpB#vR+dxcUsJK%m=}1pC}Cy32($&p;GHOnS?f7Qy<R^JvZTcDyWR=BHfRAX-y;cI zr>Wy_VV}uI=?GMGp$pCXl8iPf3c<$lAubQPmriVpWNR#eBT8!FF*XJNkg9{7o^{M% z`!YC2g<w4a1-!7V1OoG0VV~Y|I9D$Oj{}>?$-A5I)pv)m<+fd<TEGx8LhoU7+rvn6 z_e^-9F&mW)H=)mE_o?2A8U_(rTHinqybX7g@c0<y{h<VIy-7zcW_9+lIRc>dYZ{$j z7Xzbg20LLAjy2cy(4BvmG<=zV0v{<1!G~Y|#aZ*z=?!K{<lh5NdTiN8TG>XLc207k zI_WHY+I}*<sz(Q-r_Vu9s{zk2y#+hI_fk2bMOdnM3K@xQ#Jrb^;K%Qyep{a*%AvK` zS=@jGxrN}nA97I8l>^k4)^}v?B}05~P9gtjVkLDqScskd8Ph$><B3MOcf-vA5mtHB zqQOtvjomC6O#2DWz)e=EZ0fl{wr2e#w)9;Kd4dv&^yWv%;;RB$XXAm;^;qJ(Yc?(r zzlHuJJwtx-<!GmW9C7#@NOBshnIZEVNTPNUSmlkt$SWEJhqs}Ye;s7X4=FJIp-Oos z>N0O_^eKg!qc~0XCVQw{is#8Okse&gZU~N|*wr5^@$#T2><-I+pyYNFyVy>r$^IcW zZdoV$QdJvHXU_m>FMD?S@w*T^e<8GnPl46ld!QjvoGn{YL%atR8TZa>c%A4Y5;v}b zeN0|3`@;lawXiLl6FGq7zn4*#O}DAR1GbnVviMn)CknZE6|MGLj!$iT%{KqV?3~NV zw66MMdV$_@T4Y`~+37Cd&{-SK{uup&-#8`Vgedif^*Y1!zNcI0e|xOhzabA%S3wjO zTIT}WU5#Py!FA-sJ5ls^iYtuBf5BgG<e*0%1;Bf@4Daae4d|5~$2Tpkfz#W(vC-M3 zAfu!QHjxjhFqLf5w8DwG+U)>KoJyIQJM?L$-~-ryc4ur~TCyHphv<PNZ|FVtmr;?e z1G{uzD6;+{PD=&r5y=O4=wE-cITx88l3`ZjO*<uN<#R1CXVrZ2N}~-W49><pUyg<R zrHkXD3PJ6kiy%EIjbn3_Q7yM-lhEPQ{3iQu#P1p>+sqP((G*E4e3}qy^ZtfT4+<jP zepOiYy$P3t6B~zP*fm@wD^*s*y1p32A1_$aZEs$}aR+;-=W$;4(K34QrawJf_A&PT z9f3X7!eF~tI=}K@G|t)oi#av03VrrCLS`OXOM0e^VKaUull?jbmkKE}$_b4q{b@0C zwO@p`_+vqdah^X*$##-4Hx~@|%wb*|yONW;$5Gxn8xUQe#Lj<woZb2H0<StHmp-;Q zi{-yO3{Tq*P#mrb;sU+tb+I(Kyb55qc#O~uF>z#h$az4shwvpw5qjR+)$AvqeB$$d z13cPuljGqCLSCY%!^T+x4NVsdS=AT`@E!X?Y*`m_tZ4=9y!H~Ysg_5+Em`D~RWN+a z<M^2=_S{~K0qPd^s>?fl5OmtR*tsPp?87etj<E)P^qtFFX{Vu1oP9xnt@RXeSd;t& zx5dP=Q-XCJlwb4ddG>!v*OpA$Xk!z7&mLjxOb2%Nt*3O=oqT%3kJGTp?=Pw`UJv^g zb^M1fc`~Hr-G4KG=J=BTYyafys47%&8lrRl&1Av*x1@8(6t0g5LGxn~_$25DuTJ?> zwU*NX$EMb=oHNExeHKf8&Jo5X{*z&(PJwxOS(Lc#a)xay`-x!?#f(V3WFp<3G93nC z=<Blu(Ca8o^~p`3^W}js=nH&7k;x?Y%l!X*F5zLv`2T(WEF&QGfA^<zu?zA4Q=je| z#n$7~Tpo)ye~p&p?1I%hUXgU~YPf@4P~L;i|GZCW>%rAJ0s;d6^ZI||^Zh^a4^|wX z?|=RN8Wu`H-WFew)(nBA<2zyPh9;~gv=;5H`GiW`uOt3OTl`Z-khWZx46&>B;@GX8 zIJW*E8W^l1+m}n@PZkDPvFQzQnpZ*{yDSB*&qc7&lzKceC63&d$i&yqh=Z`9G5xQ} zjM6AJK%rMR!Cy)mcZrWMcaEQ7{?xKCFK{Vsali!|dwRg)ttW70wgh>%#}SynSI`6V z5)#mVm1M8`M|FsXF++1H5EM9wa#vg-`$n?BozXyz%kL5WQwY3xmmu-?7M{ftFFc&u zf-9O0@XyclnA6{b@leDfV!G-goSQ7m?#MoXq&6&oL)r_8@zH3!5I7rmeh28fZznUB zWa9zUhhsjk04KcyvQ6qTmPm<*clkT<)4#vr%PevB>FRBC$GUB7;Noy-ZvMaoWzNT! z{*+>uSBG#}&Kmsdp9A~KHUUH_d8~Nz1FSLXK@RRkaOYk!NO)@EpKFfsoPrCnfQ=~F zK2ImS8wrqYxES(gm*GN78F(`F3qEFl6pZ!KaOqWL>~L@xzu!^9xNrPPPKd{2(VJ!D zwh+fn-zkX;aV^@brA^lz`-q#|kD+6exM%vBa?p0kV}>3qLshfH*iW?rFiu{8`I8y+ zq@PPjNGAsE3N1XReGvKGu!O`POTp~nOfsaig6uObBT~=rqoMZ4q)TB7Ecn`irmbs) zknCS5`G5APR}zSAz787)WQe=C7@gZTh}GQ(QCCqW$>wT`XN3XiLq<6y?)?QHo|&?9 ziVZ+#O9EW6vS5o+Wmu}$lK!VuP9m-o>QbUS`*E)vme}k{7amYx4Y~I#z8r_vHrmm- z2{|C6o{kr4`_fG&7Z}oYi?qH6Y%!2WDm9M7@N!}H!UrxZ<a`~4D$3B0W=qmxZh`Fc zb(I{Gfy=z;OvU;$(&1voAFQcy6`cP0ql5)I@L=%@$nE`&tyY~O#Zpsf-_T0T#>V6A zh6?CbxFQX0r-*jhGm;*06v>RvgLeU^xa{T)q+wgYd%wI1Ma2lvT7P^<5M9b;Lbt$M zLlM{{VuwGv39#ggG4zS%LY(9be0}F3tm&`@6z!kkdyks1QuH&%F`)u?l$~an^fkyf zCz!T7X@vb9l)(5#H8Z;F7&6MQg|i<B{N1X~PIoNB&;P~3?9u>e@3d!=p2*P?-709@ zT!n79KyZ@T8ps)VNG2ukp_lzE!y|ScIKikNjQ2>Rn{C#R=A%m=vj>QK9Ec0+wDEb^ zh^ik7vg&uUam7YClz-y~{=20HLSL)Uej?tivfyESzf}VpobQG5mz6~0uMs=Hq63v? zDbn|De&mlx-yk7#PV;THRDr6|AeO26%CCUgJY$DoDrbu&?^=r@?^onSzIWYj`v%<? zynt=D?R{-O*~_{w;OF;M)$caU<ey(<Sih`1oyRO|vzO63Y@e#TizjO-$5%{Q&HMUj z4e#hbOMdU6DErM#jXaZv_dHW?A^yO(1C+T~FK_LsqjtHwzt-RTs7>wqI$|faUWEy& zJXUW|Btbo#`h|b{sW`9Ufmi*8!_|y=d?Ihuew-iwV<r`8FHeQV9Os8oUUi{-1-`0h zyxq^2G5kMCoB7s1w@@SX@zi0>OunD!8T;-#e$?Z$6jSeFNKNaU#lPh?pPD_;!uQTx z&%d1f2>ncaM$G+tnZxDZ(b3%((W;>-XlBq8vMFXc8qnSZ(XS4Y^qmRlb%zDcEVV|R z!Lv}Z;W4z-(~z4HrqtgX86wZOTtiQJr{U+>TU0U?MfM6UA?J90%$^`A#&jEDp6;`Q zaeHZ0(R7H?js8hBpOXSQT!om<<-F&gXQ7H!dzdJFCES){&6}yXhJ?<1K%LhJWN!b7 zCUy4TnO!d$sb-(~RCkytc`|;KY#AP<R=C77+TVki#`+@CE~)^lKD5;{cV1Dkr?SxF zif2gEOB}7skmU9t+L6G4MmvE9Gp7GY68R+=RPU+1fpqj&qED*8l<}*m+>U4}%v*+R zzg0(k`p%K*?fRs>S^-g?<dM#;0QBUW0yc0uPdvWHGqdd5`AYo5%>L0dD%Q7~x9y(@ zwhU9C9`)=(dMO7{?XIK9<C{KeFqnlut}-Boexl4^GKCK{uVHTG2IGgzEg-?~A{92L z9ceySA=<_bb>;y}h{9Ae#7mqBp}Y8`F_eM}dG+MjthJC0>!|JfxcSG`cRZ1Z8PwZj zD#&WVIW&HCkWBM*rJk!FLVFXt$QAwoDI2LJzR!D+$+v4%X`&X>A!!6%cZ7&fe>pLJ zv5~3y)WO^d=_D;(mzf0}H<^G9GT1n3F0oW}L@yf@2|K4AiS`5|uUASaLJ3pn-!@XW zBoS}x$|a1}{Xn?ZbBJR)4U=<KQ>deJ=As?zE+Q`x6Y6)tZR(SxEU>Ptse#(X%#~l` z%nc<>f?p}(tHOQg<eSHgTx0?BYjyz9365vZJ6<KjHyFO%iPL0w#!fCP#Ag)!vl)pD zaniH9ntDHEMSTzY$C!)nW`a9+qSrl>(EIG^NN`#aZ>abt*W+knl1<g?o$`W7@Y{Uy zaC1A)+(ED&eV4!oUxuPi?JRQbz7`bkv_<dT^>8zHKXq$T<4ySyLpEC!5WCBEWJ{JL zd>ffVBz-?qTPpgH6Ssdq*(#2=;>t{<`Tjli<s#Ro++~XHjkGZr{+!~Es}^9T_tw0A z+YMBV)Fo85Xfpk%T8b#HyTZ8Ll7RNmO6GPikK|21ie}^*GvY2R3OxUZH+jKtX8)uQ z{6Do;<ZW*YZ}wUj#%{U*c9Xiq2%YL-?AwHi@U?~1fm_i`W5f#T)L}1jIiruPj^#3f zDz)|}-1V`3QW~1bYo$hagdo?8+W7T|JLUTQ1fMoi$9MEXs4EX7$V%%;SOLz~TW6P( zx$?bK+ecTDXMUY@@)lDap<KVv^*d$kTg6nyc`!3q52ICm$y8O^fPI9(B;qf2k*A$o zLZ#)IBHr(-X!;dlWEymioPM;KoMgn&!i-WTX}E$9BY}3>4L>N&F&+5PE<z4z9l+Z< zH$&0(SW5r28+trFj70psfbK|bpz4~Zf`eHi6L`Fx+}|+DyFMz0HgH@-kEv}$D885X z)A0bBWGzajNB0nm@?c!axn)P6EFm{q_u?&i`_b5rL&R+CG&=tz1@oeRbNf<RL=1(a zSzE*Ktdk)mM=1w+I7`zb<=j3{XAzgR_rmNweemQj#GegL6aNP>NXl#{k-Q-RC9khg zMW5oS(G^Mf;uQ<9tBr<>c0J^9lQcTEY!q#p*biBmLIkhghy=HLlQ|DRqxAhtA!yPa zF8{b0P2+eS)6<vWC+^ln!!;JCna3fYwwo}Z`<OiA72;@1T^wks3K8>`!3tY#y#JXd z&Ph6i>4|YB<#htAe#C(5E@f_)RtofTQZa`dg$-t>K<Y&VcJYz};meQ6MEhAp0!#5q zp=dO-2Jri*8&Fxk4Z1KfhFU_WV@umfthxR-RO+Bd7t}@I0%ue3TwaJ`Qr4nzjVAOb z{S5?KW&--04ZDu2;-=E4IQ8{5+En!*O*glp!{2@p_aYi^=4NjxuN_$4wU4Oi^c7U6 zv;wlOK7buODZu_TtkSgjPySkddA9g}G5@yW_)hfyRS5Y0cYk@WyGT%OA9?vi7pFVL z;|HH!k-*3osC`F1-mYsxq+}-JHPgF!`<$}*W~S>|J#`bN+hi)r_;Qw*><=aN2bHMS z(GqH1<P$3GQZX`}dz*Ou62+fl?;vNtmH(N42k8y>1aP&-|GfUc)E^w@>3^+1VoM~D z%h?OaEVGYFJJL<29XU)=(KHzMlpu8ccch(Zh=;q)A>#NC^3ixMs+_b29+<{48xlky zwq71OqEd+S?4!(~P5C7M=yjwRoQ_;$N=ZYjGZ=1H!)h*R$bOa-ULB)K7JL0cn+-lN z3;!M=r5Y;a+>;VCp)ec!*-gSb0$Y*Wtz_!dwF$IsR~j-kd`TvmPQ{-KGsxp%RnUjI zV3Ntz98pF%&GisjJ`l{jZLX~&aTd_7GZ`&a3qeiyI}y-Eu$WqeZ=~)av);Pl$G*>* zVuNjP`-K+#9sW$Ub$>^)%C}Ie-vslx+yn>4z9lQ>xsbxAGGvdbGla>nLT19h$;WLL z@Tu1d%NtC`x8l#Bag}M%d38P(J!gY|$~|KQ_8LIyJXc=n%~a&tl}T<izd>yWq_E&< z3-UU3g?BDz8Se2~1!tV*lESr?*vP4Z6qP;WcWNY%)=OGkU(N#k8+3vRu^njQcQ*2? z7KYX<m&i0X8?>X~5a;{tCEj-g@ZuWaJ=a`D=5vg-SVuE-|J`Y3`czFa^}|E7=|P#D z)$11WciVMTmw1VKl*H}N=;jlx8`^l+)h+0a#t>Tj7O0X1I(W8(6j2iL!JCf8V8=-a zjVeii_YNz@#Qp+xyI2I|V&ib`=3FXyt}MB=je@dB8=Q7n0vVZZKx+-<Z~$`_y%>o@ zlho3fp{UhJ%;yU7^48|#KZ4|8)oOC(@gkgDmV`8>yP(m54k~M(F4^I6nKIbo!+)@E zGVkb}=csXyB)Y&BlPxPxlV^V?flRg(es)?9EzkUh#ybp9{`*2Q&{#~2g!ItjW4p1n zSroZ*@;$1(6+<SrOX6{#1~hF|8QR#9Ph5*LQIEqQX=;}sT6t2G&creDxAGWjTu0&V z7JIa3RUd+y7LI9~NNFk9!RF@AjMC0zYVcD%*}nAvViUhHYHmaI3l+oBN0Hme$oCiN z7wX|Z37HKByKa+3dcda>Mo3yy1ldz41RZQ2(LFE|pA%Whw9qWNJ~@r}_*9_F&&2W5 z<A3>6n0eTUbMjO=B@w3jJ89U@o$WrSGn?l=C)pEHut{bXw4QZ=_oJuCasL(27`1~+ zyMGQnZ`VL!uX@PLutDU1bRClH(<3LR?t~8qe~{-{UlExr3uO-&D(LJc@H?Z0mbjLp zITJdhMPUIPPr8DlzWYP<oJ|zDBZJ-?uSNlWh3JOID6!bM1sxGr#_G?OpoIKmq<y_2 zHux-rt~`B26eKectIX{%pUmQR&W@vCB?|16O^HDNO{BQ*GdgMgg*Y5oNQSM%$jTcA zSi3%&?0<?dlk3Yl(96+`3kKwVpd6Nqe8lwTD3i|B^~BuThV+XHkt-@@WVeY5b;(cx z(p8eEj4pK~TyH>H7RSR%<|%sGa}K=<o=4W1XOgtVve?#b1eJD2Bl%+na7@Dxorsl% z&TALYmD!o7-6@e#$o3&aS48a5OF1GrJsj!(P{Tdv7U4d<1t5P^h+1=18wYz5DsZJ7 z$mJD~3kPT7rC+}j%S3Z>x@43DCfbuXm;aIeDalB<fb%}3`;lnbc5->e1kbhnAFm+q z2h-}!k|F1h=&x0Oooa>zIpcK()mD5bE@_ML$5}#9;@85P@p>WrxGj%UvURcL6FXRY zt)AGeKZRy3(`LpkWXR23!c_4Fmg!z|jFEMyMWqQIXlF$y$`_@X#mlF{@a11<Q5K)- zd&JcaQg6_8RdZw+sRHgJM^KmMFlxx{rbHYpQ0ppTZvGI0a(DcscE8*}{-zY7)#{Si z#J`Xv=Z>PI0o~lLemuIg?+}-%y~pheB{4&L<x!-K6s!&@W<m>=g1SO4b1_Sg91jU6 zK2fcx`ON|t`nDYM3Sz+f<}h=E;vT*I2`HdR5?(iX!P}LY$ivqMW#w~RSeGtT>tqFa zlYM!k&Q`=TI}_<N7~`c<s&IAYMr6J`9o=SAm|t5p;CcLHaD&I({lyGt_dF)?!H-GM zXMeO(b291P$e@<_E6}}pG34~3KC&yXoeX(uz~rL^RJti5`7YY<*+K?ZDqbf|T_wnD zX(9j8*Sox<e`N3yt7@{ydI4_VnnmUK)e_4uJt*9p<78cwL;qqg6Ya?vXoaO8S?jc$ zJQfoIH?v0mDoITe)e(by<xQ}s%p92XyN|h;UPyVaJA+=Ioekp&6r(;X5na4|k=zb- zMR5oJpn=blnA0+$+o?sUQQL>v9ej#3)&4+9X#y~9(I~oQ_?dLy`9P9HV$n}~MXL4V zSKhU!3Yf1uPNuA1jka|+quAX^<fZ>G+IfKU4J;2PHw>)snmfj*HPs#B)hEN8F&<Xa zFn~<|lcYRX2_KoEirD_YBvz6~_GgI`lZ_m++vgCPe}50s`2HE)5?qF4d;M_xGb!AA zauJc`@{+^+mGFJaWN1?3+>%mxJTYx8nBxAB7~K5%e~|X((O8A=+qW{$L#B+0NK~dK z_dc&}D3k^?P*OCTk_M$giYCdNNRl*C(LjcMUK=S%gA7q2(x^0$=HcDn-yiS0-sf5C z{jT-AfB4H<H}~H6zSnu3$8mfPZr8mUxbegSgGWxH9c6Fe&(~P6)f9X=+TD;aqYirm zHEDWaH@7YO8~6RF2c0(10IFf?!Vc^z(=}%?z43Z9J^UlLV$~@FqIkX(-YR5aVa-|W zO)-F=kyGH!)9Lhxkr&x6A6c;|DW7xSjOh9GBwmi(j|ndd@Wrf=wC&3T%*xXy5w8jb z?)m`8Md^#;WZNL3uK-V0H9*t+6}aJr2GMwuhkI4hV5(dp_|$8Vr>EZI<NOGC;noTH zId96Fm)X&Ed)A5$T^XoYx-uCfE^UAbm-S(6u{u33TEopyo<y$724U~or|A0rCnhBB zrSC`Cl3kOxm+u?Df<A}~g3e$y>flt0FUC8PPg{?m$|!p}a=pHAJ@VlJ>r6t1I#DTE zQ*ukii^MHU=Ef9i;H5!Fdd%@a#m%L6xWF&+B)vBrYoD@o#ciR7)^-y<3J$hcc>e>% zPxQd&ldoXhYZ1m4ZKMVEcLZ;=G%fka;h0iWY*p+6C*l1mzr4qI$pzAi&}HOmz!xa% z@gg}-d$9Pq4>>b31$HY)(%Z8~5cwt3IrG;pWXGT<p1zU?X9pf(+t0I%*qY%a!952~ z$^|h^SBK&yS4n(GGjPezA!J^)0X>-83a@>Y$#|<Y+@DhlmNpL|IIFHAQFksDWWB<N z&ECYhDW7Y*x{64=l%~gjsMDV2Jj78lutw08CIyz^P%C@l^~8`iyG^J3`7h9n9>mdZ zI9)fN1IsbPVYzf8JZSO9dWn9p8GauRj&#FYb}R7n7Ef3K^RTL<2rhoeViMPWWiBq7 zL$yM_!gH5IoLdq^-+XL@^%pkd)L;wx>0>;7G;}#+aYB~T<Q94yJPZzD4^cvH1%{ZH z;e720<-GwcX3qJIZEen2k+2QpH9uog)HIs$IS)GSJjA<xk6<$uvKLeTz~YnPAo!EO z^V3<8{@h+1+@Ju`Gd1wwxf*l~kRfTqtYLxCY#OB_qM@-CINEKHIg<7b+?U^iCpwde zyr5UCaM%H<hF<j6Sm6x$Dgj&77Q*o#Cppgu;k-4w99PaaCfkb)1x~5(_j7YlFRudJ zi|@mjHx|s#rqN_}<8jD-*9V&}HQ?_13V0#B75!Jq(16$lWT&^Vvv*w!!_>`idCgd` zoLmLZ!VtD9#z5v^A?CygGw1S^oU7oK4P0H0p)Hd5VN)ROyD*EW`&4oBt){`JfQ@L7 zE`rPA=}>WPDr)V2gELnvqCR&Em?R@A4%cDKp7(G=`XGZQRaj_u77ELM;yS4gEIH&( zPK=7iak|=|I;o7)(+MLEzE_x~yNzh_s%CstcM$knub^oCA9%Z`4CbU?;w0_0QNI5< zUV74tBeI+D=ehGx{C*1ADsb_f*N-N5_O_$em{JUuh=#SwEM0y73VxoO0euqJ(Dktl z_N8ZH(|u(st-cY@ym2NSn@_^uJH}+pq9G);XEPlAS_~)jST5?Q0d%B^h}6c{xOl}r zrZco3y5zpWl|emP_9+UIuoKkg_CjVs0vsWGpy8`BG`Jo>^|Nts_hlhQ``?2YM|moz z+>CWrx^$n$9=zak2-+6EW(-H#P@@$~xmCd*QMq6`7#$2}EPLO<U;Sak!@nAwh51nR z)OV;?bfgpd&Wb8<6RL-8gT^VjSe+nGgCt9#B!Z!Hg?V<tqFMOQQt&EBtVIRA?eyEv zRWxJO73PSO6qS1vOFFii(bL)f;F!39j&Nm&)6)SQT)c_|w`L1mN^9yZ8AEMG>XP4? zjZpMQ9(~5-;J<zYI%z^O6TeD=YAV?Yxw1GY@@<B)a4E)o#&PDr-cck-X#iDL%L{>h zEt(s&27aj)!H_GTnHQ#++=iv^0nD?3$nU}OBS~0Y@(MbKl)$0IKPqJQuBA-{=48CU zSN?W#8o9*W<w9T9gYT!;;CeQY-u|FUZ@=(CuPaU1(sf7pyxC&WuWwkg_b%R4+X|w~ zE@a~n!GEx-O_V+40>1g%0pnh%qOo-YeqJlQ<_!+^#Y)o)LxOQZ6hQga8>PQ`wdj}A zH4rvqA5EO23dTRipifSJ#r#*%;LNBKc{3GSD`Nv|rVOKdL}JuuvkE=G)SX!&aH1r4 z?1e{kAy+zM5stbsOkf*7;%?cuz;JODuwo5~L*!~WAleK0krJrxQG~<5mrDCK!i_2; zx^bogvB>kH)2ttGLE1ZU@t^6CDU;4EvHHfH@Ldi%-TEY>dp4Cbv!vlkV}$*>A+5b; zN6i{$;8Q~_EOwXY3WC4mOz&Al!SV=fi&96WC*Cm7_mH{&hs8fBY3TCv4!XohQ4{6< z3SU1(EV8eHt??shdYZriwfh7*C+;!dPpNYO&SF%k=7A=cM{vWsi_<JV05sK^ahzHX zonflvUS(;;{ja~7mJzqP>r?7ci8m#euWcf4{nY5(h4-M;J(t^IQU(XNP9je$6gZ|; zl33lzU@F!x#Qhr|!S9a=aKB5QT3$3IU+4Tqx5LXgHuxTMU`08|3|@!rZ5ot*9!(bs zv&i<aR@_*8n0b3@BrPnNM<#!fB_3D<A0(`W9;&P87ghq>nL@CNUQZ|YtJ0#wB~Y6a zMj|feLU3sS^FHtd*LNWuZ-qX=M1ct!ULQceoT}nJg(yIvcmZq^ow3i>6F5lruVJy+ zRlIuJhP2&HL!HzFw0XRoaTN5%j@wg2mwJzY-vtgnDPH0#LKectPIb!4#K2H#b!bvc zV3vAj!o}+DiWsd$BsKX8NMwr9#(y68XKE}ge{Cc9v@&7+6mg<5J_!ehh(Y+akwoL{ zI%cp@mgIBQ+?kPQD$d^IAhtxGaL$@UC3XnSV-j$lZ9Z3OxP)vQc!eh|<iJD7ZYBNc z{hxfq?xttI{}1$UTR|xPultix!i=wG?+xkS#}fBPKcTI!4kqtSfvsy|7~2=Ze*5Ee zyppww3wJ1D%IFS`-!4g>?9PH0BY)t=12<tg9sx1U`>5a@kI!iX<GZwvnWr^@E<LUd zvQF=~+9X*ZyZ7UlswV=oZxU+fO`z+_vzh<oB6PMZZ~yQ5x3ZZ0|Dt~jT)zK1e?2p? zmx)xk52kZ;a7maWW*N&7rHHB2CZS*CwCoV7`CY{49)GyxivxK4$u3NE{)YXdyJ2;x zE-?*NBwD9~VNa3Jkuh`#k$A!4^RHXscl%RFRJ5Tx(o;au{Wqr`sz5efoD3U#7J}~F zR{|VwKYE|4gfAl(!0?}fuyIF~eD)VUn@pSNU-49;y=^?;s!+64Xu&08)29rvO2XO` z79{_d;Ju;$@Wlp0kS?;L)_+FP&$SL@>Yz7WcB9Vz%X^KA<E|6ACE^$0xDrD=<>aZD z>k#4+;!fXQRu{bI=dr4LDvglXNyBSCppVv1P|W)dN*G0}4Rz={L0`SC^cV~_-Vr#s zt1x=e6>j7FTD0|!pyP8EFkh^_Q4mk*;YF%Yv)zo2KNmsWJ)-d{yOK(>gr)}jfR(8s z&UdLmxlem=`@;KBBIF0df*tU3YBS8>2bp;qJK!Jb$D;5_L@IJ96+M?BbtQ2yt>7;j z4&(7}r6CO|CYZE;8A<71K&o4t@J_HC_1dCBO;;yZ=-;#<I=0oKxkE*`c5NZ1<(Gh| ztulR~J((zfU4;>vdtgaHCRo05Wwi2NL2WL}j49j;^9_)*u+W9H%N>lO&~b6PP7Pl7 zoMJMsb0XWwmvA~~3y~a~iE9tcBOkRl!}!Y!8S{&o%u^>dT5ilSKW7eroAWa2+?CH+ z#LXjP*%tI2*9TsO=b-(c5lv8+g;?inFt1dY6W@+UcdZ_HcuyJ6lneJ@qARud^%=g) z#*-~o9q`Sp4|0UgRBqgOQoE_2c|0(M)+L9MaP?g9ji^V}?ZfG#&R`O!Cqr}Y^?<wP zv9di0M<5?pkbV9}uyln68NaX(iniWn=GOZYU1e$d&U^uV*f^P<xLkubrV3|yy-;r3 z&fjofrW?mMzJcIJrsOZ<i9^<nt1wVmMStDv5W1=CuyPl}B~1Q}i$z&DId&hoaucb~ z9~HV*(FS9$>|^3DTGNq#p5vn@zj1+~Ey)w+(SeW~XiIm(^_gE`$NgY|$0wXKX1&9+ z<&)_=FK6!jt1$vwPKz57aRo!O^vNN97WiIQLC)0|H*7a2BTs$Ah--aVe&30BbtJ$! z%Rpd^hth(T&eWrED$GfZq4S&i@Q}eeoHvTEIHn*=W?pnBeKnr2@5DUP(eW3<rxF_1 zOz6<>3(5CG{^Y>Na#3foB0agT9B1VrygL6F3g-$woTJGwFU5o8)^>sFhEDFxX)&B~ zO_CgGv7<G*inRG(Fd5971L4|Fp|<!E9C-8q^~x0BVD$|!arl7UUNTg7k`39Mk;ieF z`_XcElc;)Y1igJw=&lQTilZ(R!-HW(V3MFqL_N2dXV(nLfR`zKw8j->nzZnD!4l#e z$Po3oJjiwp#2DSrICqK>(HnjXcON^7=4vAee|8~WcX5P6<`<B+?7&lb4&;VkIEL<2 zAkkyWKwil8d`KQehMhW$zlK$U^a&MuXq*RDELlcRm`akG9}09Qa}RX)c5`MCr@(C$ z0eizUu!7g4a{DZCWO6-RwY-i=&I+I$^a@{bD)iG`c~RuvGw?OF0{^v3k(R|Ww6xZX zcnRH>(o7^yG}<AWuEpb&XL`ilrWU01c$l-I7*A*g&}WWPbZEFSdWcR4OpSV2X`n@i z=X?;Ex@*&{CoRyuO6bl_e2ibi_hGEEIY|oKP5xe7Y5(+z3I6$IPUQ1jnYos6_~D}r zu`JERcoPCP+XF%3%oMWt=t|TVmnB;Z9LfFh8szflmqJfy3aa;W_KFpMn2jE0^k~g# z+%n_ql+)9dh;L&WR!;DuzEUf}!+0o>U9N<o(@f|;>+R+I9Y1jWH^4bIyV0*1hN2;K z5Qi>23qQ6@CRe*vsf}$pJWU%4*By>AhxQ41q}SJQamQQio=}ETSuaqc<{mWnEkXT1 z&!FdtBC~1sMWILNBB!CO%*j6Zh7X4Xph?mtoa<wU7+V5IjZHA~UpsDId4-#!l!?iA zCV-3egNpg<PjR8mV&sd5ut(hjI7##kb&pQ~9ib@hIwyEdyW}zI_h{<h{+qj5VMEvH zUxh`QdUSihM`qHdDOC2M5#9fKKl9pG6V0C8z)JWGbHtR1N}B|A_~}a537Yz>8e`I< zeE=JKw+PR_KJ(0|AC@SNBMJ#JG^1)Zq#hheK7>qTEIL)l`(r;~NRk$9w%ZFG!RBO` z{C95C4=qyh!UpI4X~D27a#Zz#;8{Hsk7Xt&;4eJEIL%&IUs=m2#$Ca4FUAntVPa&{ zWE0}%E23^Sam<6?Qut<1Bit^1!u;tpqxCwS$O!D9v{C0_(M>h7e1{q}P~>ow%}&w$ zb4FBWgeh%auR*>{(I6Y&M&a1}4a_7)o<_`=i4)%Zg;BjaBsj4Su9i)rGBcMjcdqWo z{;9@P{K`-oWw{u?-?gRdHn~9L4I{GKi-+hxx(M&Hm_0@|xO2^Fm{#V;ZECxNUr(Bo z(+lTA=)O_J=S)Gx(w=lU>^B_!n`LOXiYy7aU`C23{K4btGMH2|fccj_nb<SOILn32 zSTIkK#NX1Pd(3yj$nT@b%ivh%QF|+H7dQ+u4f;gQt%nP$HDX-l_2}VAjznF@f*jP) zr0eHx0l!d12s$p0fhZ4Meu$A~i<rR9tDsh`Na{uAxbfvTNNVrLy3N{5?v>*h95Wpr zpS{S)9E^aCT3=z`whxd{sD${U7fUsE!O;Ddbnv7hIjwjVw;3(wz6KnJ-d$>Ra^^&+ z)2V^<-v$CdWhcsN|AyHeL&*6#ukg|CVKjEe04`A*Mt{d8+Anw;Y`^WMA)QlO07C{( zatn6t!)4Mbc)-Gz_WRdh_yjeYc2t+1mR2YFGpuQK{Vrywo>aw7?NYd3q)C5Al>)!@ z17{hpPba#H(dd+B&<Sb8v*KgPFm4*2(il!sO~e@Q6+)jwmp&<YL~%e*0$=n0K(WV) z&i<wh83J3g?a3|XR+<FS?pQ@c+kayDDpL%1`HfpuyD+`}BMhR5EECC+`5_0GToo<4 zeQP3E_5Fs$A5_tOPz9Y|I+M=11^83V5@h>)NPPzb52Rhl#nxBcCAUtz>KqO7d6D?Q z#-BN2hEQLA6swDeliumxIC<V991sE%rAaaPCN&5T-%P_<>teuCbP2=jwXsi3oq74@ zH2&DS1;_2b4*Ta%$K&<(=sY_F9NG&ox7I?ynjp*>Blt`@BGK@P!0A%G#A%f+NB0a@ zn6+##mOlRr)j3+EmA{S!wZrI|`+B6@waXr4N00?CFM~<QCg%JQbBJp2<O<*Y;a&+H zI3MH|GQ;H)u}kFznjW(w%Uy3{{jXd>2N+3AW4vLyzb+|1)PWjbQb1`{JK~TmM)E}` zUVFl0#+my#^H((*<*K7z;CuY%k_IPs^rPyvUVL97B0hB;oLK*RaLafDZ|v^k)Lvck zkSi(A^dC)P4ttZAtOcnNIK$j?2l}7y#%PIaTwkgR<u)^9xS5dqU3vm`N`&FU?%~v_ zJ&PGWO_z=xJ6qJeOJL0ibGTN8;J}p^6Z_B4|Cc`dzw|fT3Vf&k-}*C#*u#EfYwCRU z0p|Q$2>16Yaq4%k2xlxkGI5LrqdK@wz+>&_Uf=wMpPFC5kUAxd@vFuri${=Gz>t#} zf6-)~(6iGsn!Haq%$(1S0`6Tsq|GdbA0watkN&Jbx=QoE>(BpNf4gwZ`0xGgsz(}N z^zc};o-d8Acg=8+dSiH?E4esV3va!XqsE8cL&|dlvTsra_vo1(b-fY*+QZcNLE#>z z{HfzRS2t1Bv2Uo2N3_6*x<kV*1(AIZHJA?TOXP-lAAGyI95yt+DoZd?BR@5=n0-&~ zbF=jNr!4VyA=}1#kU;-Aw53*!%e&Yra(F+9obQ@W(+9Kg&1pZTTIUdKI*`N-bh#1L zkq5XhUi&dXNe`2xZO9&_E=YVOMULYkHdwR`A}Y<O*XrZ!8=Dq-VZ4;XU){azLbisx zESSnc><am-7EOZmpAprFaa3E-GyW-Oa=yyCWY*3KqL~m!A5IdUxs+@Yvvmk8J+y~p z`c5Z3twJVanZUNUodD5lja*TPg~<QMBT%tC%dK->N}t@nhtuqz!S1YRG|>Bj12?tF z$^)9bX)H$;JV;`fJ$0;{*zkZ4QlCaX^f~YlU`FC&<=Of9PP9p57oDh=O_T!fVq#wk ztvj6nMCf<;q&fr!TSwE})tiMLm%ntfizNHetyQG4G?ae*U_c8;C0Dq`EyFG5Y4!zw z)?jn1GG+unfOQ4s%%1;b<*%%P&rU<BJo^iL0^bUG(|8gnbht0_eNUS~)<L^o&LK7b z5w*R!nd_6-h2K0VY*MSF$w}%24HBt(%ULMC<;R>kG5}7i82r8N6sTW4FA8W8k&`nY zF=5j#;jQzoL~~3PvomR2#f2_$YU5nVWDVU1Prq8w7gvsfyQm(too+($ivT+G=``qz zaVBMrCXgwTqHc-GLa*v*ZfK_Px+ib2O9hVH^4fNGy@5A<o4JfW`5220Z<N_lAtkid zD2qPJPKK?eHZ)RXMjp3Cli~&LG-#X;wLA40qUM~ZmUnW|@Y4vWElZ)6P)dd@kS7tw z!$f5PLt)Ox1bbz%8d#w`gsR##!7QmW@Ib|${4p4Wgk9g!FCc;0X4g#rv}W+$YfjPN zDSI5W%8$}(OW%>N;e*gwD#veKaEnosuOaK(jre6dCD;a&C{k#+jb0t&1IOIA5D=Im zd2^1^tL3{$&$bzul@eL*tep$(c^7EmG@h89vl9H(W>jyd9{plm$;{fk8Vf#V!*4Bh z{D-fh#K(~Yjjx2<G<UQVx{;-%Ml%uTXR;^qJlM2NHvG^boxE3dE`QnkIJFS%C0SZl zY?zJ<Qf>kt*`dhHYs;nd>`SV-Gl+PdpFkrr&(Msde=zgh26}&y9-nSAR>;*y(8ZHW zP}Ju^FZGP4@#=#Rc-W7`UYbEwRIbAFo@;o_xEaSEyUpxPos7y&TEt1!g)#7wM?aGQ zzFy-RH7MW1cS!rP?xph7Y(@(2ku`<SIh=&&)+Nz5mHlY^&Vpnf&!)rJeWb%azhZ+* z2^_ciBlJ%RJwsB(7&$waoV3#*_v`EF#DFZaX3uasQ*$KwT>27*tkc5HT_edr+(^=m zH7Hs(j+{vE#kzGxSUw>gDsLGII+!-tr>b*ZZf-O~<vtsF&w}j;cdwkyQ=Si#!IZ*A z>Z-DuT|GU68pO|{@5{1i&5UJaiRA~ntaK=T{UXk{PYA=&b4K&GhiS1x6^>BbR3$#= z_-B%`ZwMV-8Ne85pP-&$>tIi5C2n|Hjwu%kncth=gX+RA*w;9Y%Je&tpIk1p<oGB$ zXORWCDX2q?Z69fiIL>|({S$e}X!1J88tI;%jm(sCCAR#-6t+Ek9esAnhL(q)fslvt zY~+eMB6-b{U21U|SLZyT?h;yL?E8P@AE||UaSxPBswbyUJ7a&9CA+Icl3#phG98$C z8mIoV7cFs{44n<}AaSaPo7b0*N8TpGaUTn6#OOoQiuMY#o23}kx`4cven__rG*SKK zjt)_gNS3U<!C!lEk$7Hzigrc=ke1;}V^dy2ckL0<T%CnC&;Eb~bI&k-Du<aBb7Y9A z&sj7yiDoVwmm#t_64?0oG=7^HPEu2)&}jH>?p0bj*sr()ChwadQN0pxddT9qu_<W3 zyA5|qD1c}Earo5x746#NaemQQcFGPpep-k>-9^gy?&WEGbjd;TZ@!3K8yJt>9lNPg zY8E^&qcp7KG>z<ghdurextxO!FpW5n@<|*d-<U-kPJYES9bdBXi5Yn1>k{ov$8eNg zASqgQi`y-3EqG;zkhzmDR3zCpLd4t%+>q3VouXm5dV(+y%~A*VaWT-g=_XVieopp0 zoJIR1HjtHFRcxMH1s|a`g5Rj1OnimRLIzoj({)|wwnrN9I*=nRjSeK)Ia$bDdcq+K z39>@mjP&k)%{9K<LdLz&BQ0~L(IqV!@N0+;)8k*wRY#xX5^sCpjLGGqZT^c<e&it6 zsJ?*8D(F+wY09)h5Z`;(R#rI4x1pW+2@s3d<Qd;}Jn3?0{eKAT#cdb(myYXLbWrC1 zo=l^KzqHsdpu(PgTSz;OjV3e58Pb+JmEx9c!aA-JxUN0WT>X?@$$d=*7F5vK(@v!K zn+D|UKg3wAm%z)P%}C_O?XWfS8uLQ;AJ@A1IcU|VaNd&_<NBhJRJG~4=-K>f(0*?= zv57aQ$3<GKYQ|yy>N<u^h_!N9Vb#a#JRc-8hQAQByFk8IwGYJNRO#Fw2ic?Lx(;Xh zYskCQ0eGe0LdKnOWHs%Nl8#~tzFkF}tw`HN_GbK~+`-vwidP%N?&~IDKZ~H$Sd}{b zh(NF$1Y18lAzxF5C8G)$<>N|}QL6`;6i?{YGNKDS70GkM2!7tI1^lFwPW(LmpZL&q zB!6dcD0^S%i%`C6!(VxzjZU6Iujs*S-XLH%tDq4{B1RbSn%@o)_rf%yY*$H}zL-IT zaU4-|d`b0(pTeQOvZQGT@OM*dh}&E-Vl5qst!-1tyA8VZFjRu`8$UGnZHM18!#Is` zZD7`N9Y-wvh{b|;r$*xkj6R{uj!y~Tza!&NsyD7u;^Rd+ncKrC1h<pt{&h$;tOSch zYu@m@JUgjPg+9?r$B{}Rdg<ADC<sBi?XL$pY8*gMZx}*z6EcX>m|XDMq)#J_-{9^x zHKMRE9=81K0-Zf!74P@=L2AA;SyiA&{LbpqqSHmVf0-IQZ5|NS)mU<L#RuBC?<rB^ z5?H&2^$wcX#2k*8r!j`#<EY7`RfNjzCUbkdiEP7Es#umnEbAI@dE^0%n^lEJZYV)a zz+=pNyog!(%p0vs{^GxtwIp$=21z>-4AUg8U~E{a$Ts%?EHrHr9l3oMqk5(g$8Y(N zbF~3KR9cauh9XoMBZF=!wp7oq16sZJ(4WIX`N^Jx#9`c0htGavD-FZrc>j+dsmpn7 z{#xWza`jX`SwG~wz_6W9{NCk}wm;Hzpj7ZJ+3zN<^e6a?u0vD1>137Zy0YDOx^YTO z2`M!?L8JK@WbvhTJo<7Wbvm<=t}plnR})mJ<{}da(iOZS$M05H7o}k8&ii2W<2>_x z^<cS1xg7?}09zS%gjZbil7FZ=hn3TQ4Iv4vJ)b;-JsSO$uFSS0ZIOMP<wpae)1^rd zdF7G!`4)U<_!Ah2SLIg?=^;Pr6zQ>}Nz|t+oNd(^MXF!slSS)C@czH0Xo1vZDm(i+ zWJG;sUaXGe{$3bO^u6Rrc<EAVn_&%oZ)K>ViyqPa_Y;FkPLqRTw<vY8zzU;QhoD#y z>*eMLk1Sd-XX8WW+7v0G>beQ1tvScYYN@j|FOQRXXiN;o{leDi)o?V-l}yn2i|1n| zlRF=^$fIwGaQD}Ka{BNiX7q{k9Fr+90#has-C#xZd>C2&W8MmCQKm&Q+gMWd^*yeL zQm0w3wV3AnF;K2<PkgLHS+(K&`J6XC#QalyWsgg$gTeSVeyGzicI+tyx>b4@Ir(h{ zRePLHma2&JKG!_RXNepVo%5yQZFD#IZ<@v~YW_`It8L(hTQzA7qIl}}S!hhpCc3eg zsP@1DVt>e4G{&eCHZCp)<4hK7hB#35Vkg=gG73V5cfxd=F?2Auikr6D3SdD9+a556 zHQB_I>@EcdkBf`gCE^}zve**#ypJmCiSMI-d;_sS=v)e`jU&w)PSINn0!ZG!Q}q3& znYbaQfJ_?t6sAtqqvDQ2?~kq$*guV?wjKW<sbDoKd+Iam94kP~uL?&RPk~W0T*2pF z9mF~Mf!wmgqF(V?MC@fW`1iJ>(_;&w^ZF9?T0WcnE);r!TI4EE)7=7#THay)g{{12 z`Fv<K))m-!itNK$1$KhaaT57f$o&sJf_D$tiW*Mu6Y2Z!0VdQALl>J<?USB#hshx@ z?_NwMuj<6l$2J0UrxbdAjsmZ98O#Hr(`q@ZNoIDwg?)R+G9$93sfIxV9L>(;Duqtr zx>RdCzikB1d^6{Nw>J>kzZZBjtrL|Yx!ds5{%>^cU@R@w64*#eR@C661plS@5}l&+ z4UF#WrQ`jS$z8b+l96RcxBp9F9J)hEWoibU|7kN=;X=~4AckIkbqwN5J#h6`Niyzv zKT~m)B_(S+;B`O<`64Z_V3pFjH(&jjA^Ne*Ybi5&=36U%KempHTiPV-d&cmV<|itD z?3oJZwT3!;P^%<I%$^EfP<8&Nss_8NO^J5NCeU{slp4Z8BDHydYJM!B@A!}C$@-Fs zS5s(6%Nl5Ym`c)8g>K{RiuB6QR}j|S#S{ncX7&rdGi7ZhdT-%bZrLRj;*#ivfBKH2 z(?mOZVRkF(SapaBLpNgF$gzBNdN|8gS@BF{EFbJYyE0ZSlV7}j5x-uTh5m+&<-cWT zRIC~Cf<B)gjf%6>sJqSphK})~OKu;evL7V)!Mu7>DYKNG+TTOZA9I9XlXlXy&NtLY z(t>EaC!^-?M_?7~P;u|wTqb5>3{*9{L+SPrB)OvuO@)kI;f;PKy?ZHFqO1uL&i81V z#T6Q;olVZ{H*;ui>ft9So@6IbMc(9<0-f@5I21Z-(hbdWc-pOoO1u4l7i(moJ0y<# z()ZL}y><zGvU?hw7@Y-krfCSA>v*gmHxWw*W^#Mms!${)MaSO}3^Ee$v9ah07bxTz zT<SXD%$pD3JLMx*Y-vIB=00d?JBih=v!T#c;N38b*y18%vV6uQhrzAYBz><hdn7G{ zYQ@bZ4iokgnPJl8cdQbrYtdpq+1^COhAUV?n=3+9m(tHB(d1b7W!QCh4h=HgPb<GU zk$GmxwC2=vX7Bts`rNw_#r0d6)+2%^zfqZ3dL7_m_G=JNi)qYs>*?f~zz&jktP!2g zRVRNYRWP6VSTNm?$-4U1u=P{RDvj-0SdWx$@*(Xy<_C>n-`2#jI-YCjr8S?4)-1yR za=6A#GSOyd-%aNId^PB=Q|h!)a|3I+T$zfwx1fodG07-1q(A*8u+1$>{D1aCat7xm z{=5GCzwJ+i_0WIcpM+eJMKs(bdT?hHSuJbMB?J$}lJN_e)jeV)daV{VeJUg}F%9I| z*buxsUm3dWE`r{&M0k@JMshdm;?Af5e0lK$m>s-;an@4osD^8t$q0Wi4!^~Cx44ym zxuOJCYXZSyg%zeb_b^%34UEPD!~c0-wb7$v|GWPDzvZt5{P+KpzrGX(=uowt#zhsv z%<N=FrsN0=RI4$Sbyd`}O^WQxEh5)$jpg50j{)iN|7dgH9y)t>3|W11KlAXw2+(<c zf_mH+{OK|iv25pAvMaC#_C>a1fCbCkJb9Cl{PlGBas{$JK#^9-CNe%9yU4v4ZfLsa zB3Zwzg}!VQveeBNVBr=*{Z~lQ&9@DhhJ(T$<z)kztR7CX{*5A&>j=^G+bet@jH2Xx zDtB^r7WtAc&tE&Q&UZ>~BrWPCNPfxk$NFy*yOUpVvtc!6T^~V|0-n)RGuG2vZml>u za3ufP|0vB`d;<dA7LoX>(^RD<k^}2wFjnR~Ee%YkT2=j=(VI$6{KF{1N?c_k`E6X) ztT;3sCe3eMsmi<fo~DgHhV*f!5nWPLL&wT{vF%&VqEvA*Q4E^SB*wRkvICpxHvbFc z&}>h#ZN3=QTlfZo7JVcGdd}pF{2iJDa`gSp`*3k~s?d4mOUr(!QZJ!{bMn0$qA|>z zKXPk6+PYfueLWP@+q}3%!<K+ty(}rcC~&;oEy?VX>v-<VCh{Z427MK)K=O|@T~d3E zb2%s>aM_kqxg{CQA$wJb?o1>L4=d0O4Y_o3SO71#(2(5Fk|bJf6?A3oG2#CZV<W|E ziC_2>-t*v4oZTZ_qdl4Adw~lnDZhgDEfTC#u^ALaO{X{SKcfnVE>ZuQZ@7GG9~J*_ zn5@ZONmIKP(g_)NNR22T?jJNDtrN$ANuCkEdd@9~QhAEE+8>FQiVL&2(j*cUG=<)L z?MLLrwfU!af(V~?md@X-OY~>IfR9HT;PJp4?#mP-dVI7CyIj_soG;JEnw3Lnw($G@ zNhXx{ehUwH53ppWli@4p0{^#(&JcDBD(e;K;r3pfUjX#k1v7pNT?wqfu`eDO!x?42 z;QIHhgf(+^<EoS^bmxg1Fnz~5VmRH3@BI1$$EhBpovM@R?VmtK`n$p~fidKHw3-@* zA0hLD#Asp;M@HP7K=zwYLoGEYdRo|N?zCD<kGOV0U&cu0Xvl8}G-@JA=Vl^5{U*6_ zq?=TCzeI;ORy4?|o;*%SB_rR)(Zv0C;Xt`8`^DrMvphV3s*frrSt}3FTKN;C(c6S7 zj#x}WnUTD1hBwVPZ$(U()k2D%D*s|@GHI9njS*)0?EY~I<ktcbk(+au?wz~@rVMYQ zy-z06+rD4%qR4=jHvZvKJM&O~n=2*?{=DSY9?oq@HEj2pL(X}hh4($}I92%`)WpaO zYzT8&?Ht5672A;HdHHbm&0@NtP!p1@tk`#tchGf<&r;WhT=??|$lyFZA~Xt;#qH8U zPmU%IcQ3%b;W6Y%n<7a%T1?Ac%F?~(7C`rgLZO4T9M2Sma|g$zkphWYA~V^X*VR9a zi;N6-BMWmTYu+wAfATOK+;$rdO+8OTUVjIptl#LYu#?>IY~jXQ*TBCOUUct`6O4Df zAGLiolQw)?#ZB;%Ag+Hh$@Cf0G<3BUUHonaGqX7nQtk+Pwecx(TgVlE79YSnc4BnG z(0^22?KE{%drv=p6?W<darElOuO!U4n5@qGTXyc^C|u)7=x_Vcv>42xQoEO4G6^TL zZA)p?&vLTmQ6{PU;lQe_wx*E|ui;X}C8}1XPxe1ZBl|x5<$lK1(GAn&vAjEp)E(SL zzJ9q#`E5JN)3kTs-ReW##BM;zOGR$w7<n=<BAuHa@RJm28?X*@S&}Aw3`dAG2}c&v zxZF`hOaCh-Zazyp`(m)m{03RDro2Mje~=u`9ZEWL^+hwLdhjdd&Ot*@0{7k7f%}=$ zBJ6w3XmI&nNUd?K$TwPukL>ecP%o`~Zki^eYL|hcN!?t>tQDYZF2T%o&8zTPp)3k; zP{#uk{F(bzo!siCm3YzVAH2KhkA~eDaBz+%I6u8D%3E_4@|%WI6Ok&t^xceW`z;Q` z1#hDN3I$Hm^DJDr<R}`OcoHQo{JE!fqq*xk4>@N?oASLI4{^iaeCA{_)j)D$DHu*% zg?hv4;GKRk?31~Jd)BY3fI@eWKD7t4-ET8Z(?@e@D^J5wUu}BRbv3$8DrX`t3VN+W z8`kl1pcHxv*M{xEZ?1bFLez<YAGNrtZ@n2SpO^~q#h>wo;bpK|XoBYaUC{0_hSlef zL0P*Ymc0??c$0lJ3FGjDnk^Pan2`D{CvYC9l4Z@(^jVYvW7cCzk2b9&BO8;;rIfxf zeJRFdhxjdKgk=fp-%#Qj`|fc{lP#FWQVH5RQJkhpS%WRl;Io~{cud$^*laCiX2hi9 z`%xnq*B4tM)^}XRvhF#Y-`P75E^`{U&CkKaq87X@HUe8GxG?+Y>s9Qwe}pcJUc=-a zdV-Ja9Vlf@qFcq}M0U^fuxt9<if|u4W>!sDg@I%Yx=(Tg2jfl5NA+89x%f5Ha?^mW zA1X&CeM=Pm+ba!9m+Lr1(K^U^{k6g$8aVM&W0Ad*j~+W;L(qW^W)kLLOM5olZWV`p zExhQE`fyO3dkY`FE<llE3Ut105_&e&DD5}IZJSPjw3svbUb6xk)dfxQc{f*fbQj(A z;UCO+yq~<XCUmYs8P|8Cfys}*0(YKW7JP-7%)fAC6!LB{)AD<uGWR0=X=VXw2jqyo z%MN>7A}tD=b{3~y+=h<Hc4XG1Mz~UV6*N1gXus7QA~h-sv`^-N#lBj+8rD(XtCERx z&5mMRcQ(#S-pdSId5{YpDFYv!_A!wo;~}P~AM71cn1#>%V0XL(<f~gl<GECkOXX#b zu3HbQSMA`Gi(&*H*=2a5uSQ27Xl8a!TaACBufc=D1m>mBXMA>h2u9z?V{RM{!cII7 zxqHRv*n^*8+Pq*+&8iu1Z0v%_?h|<S{R<piw;k1Q4Z}tgKUi$_9TN)~P}ZNzWNrw8 z>F0E~+;^S0?88BOg)>1QFY7{vF%P&|Gc(XQ#tD7QB<UuxNYT?3-=U}?52BoG;B?6} zhO!hIE5ymE%Nj&#fsi{3t-#Gr6{zDQMdP(~m=CIg7o_VVZ2X&y-uI8fXd#Pp#Y~?R zUr?#&d*+KT_szjrxA{0%@JxKWk&oxxr&6Wd>1gLa051-SVXV;?XqmVHhlOY{lFrBA z+OH~<e>(*8TJs^}j5IX$n^d$2teD}3L+Ilv$xM{l8SL3qFEAI}VcRSP20iO=_RT^j zf9X&fqFxPJ_X3#2TdA;^(Z+KE|7{%|My2<cRy>a=VYdAG!6@v=<VLlpgRxEntmTwJ zbI~>~`kXq~oW1~;>{-N2)Qg3CK5wwt=q%Hx__`wJ-ci`vSBvppi@-s56naX^kclH+ zGj{I;pY}l$c($W~ne@opzQ@mj5f`5Md-fBMeVxNq<rl-%$W%OX{|?x;?}Cz@$*@Xu zDbsx*7{?UXLF3;c0tfXYw|MJ(j6dGaJxZE{TgU;j^R@$i&C7u%i5{%>u!Y1GA};-I z0cRSxfI5BIjjeAS$nKzdXw!Wf*0R+iInz)sai~0<|HhRpFSeumLSry?SSzg0*^iy+ zT{zft3E%P|qF{|hM9V!MErdN`l*c5(%#<Tv*ct!h15Mqg_2s|o&q`tv|8F0tfd3W~ zyIuW2l$#qt*PoTbN495~2BVi?7aKwEWZkcrwK|!aXcaNGF#<cj)r4f4vh-1EBqx{W z0HakLFj34LOT)f%^1FN?I_yWqy|6^6F};qfFN<+EVgs<=_QL<@$2Uwf9sUpWV*%g& z-_Jk(k`KWSN|_e3YZXVYx|0mg4P3z7F4S*6iMKqya#aDhV5N!_R<3CSr?xF*%?D%F z#5{&pZ8YMuy+%}qy_K&#@^d;L*m{>3SP$j1NGYvqNv9EO_tOyXbdcDPNk2OuqiVfd zXiTUnO|UW+bV*4PEA*c}FjFUsl_c@OKY#Ll^KL9FDyNR_rsT|)4~%b$Cvum>*bz~q z>Gq0oAoEg*@PVb!SMZoi{UrGDFT`_m>~nA#y+_P09wB1`7P3!%41<Q$O3r1|F6dm4 zPmcAE=GT4s%mi(b<tKQ@(EN%)-uiky&AmFB?hY;l%Zq8e&9qv|2u!ECE5rF{OEo@d zT{JvxyhOAgPlR3^MP7BcP_8T)Rr0%Fa;q+Tdiw+{D#MD{jsj-4q8*d;DMd8(k0ZA~ zp0yuI&w=YlK_~ySm<5+5eBPaO_M)jL(Yc$+zchG@gO0%7bWf%p);6ScS~(ZWZp60* z)l}qTN_Xm9L8*2VvUn}W_@^Gg{I#WA`IOQMiIdS>PuduKc%X>p#Bp%g$&USF`xlz5 zWr?a^5W87ckEF}3f$dN9=)L&wqH~-I|91Kv4i)TR?cGn*H|{xAJ#d#AxZETEhAUV8 zy>^cObs&@7J1U<lM82axzD{5pqein=v!2l}ZNmNeu$&F^S;I~Yn?dw%DPX>{8Gl#I zn0GSXMaRAO<l{SC$c-}%5ZSz!jM*f`y1z?BSEpRK5o<}O|5fB)>X%nMw=tkmQ5*2q zX>W2N-H)`lgg`@xISm>bMziB;@N!u@sh+9DTW^`p3U$?BG~z#OAfb<3b^}}bCXO!s zXhBBRj^Rr^6Zq*#*5unUbGqd~97<Qs=M~&paF_I@>2Blrc^_-&r;K#CAN)m_sX4m; z{TQ-v+eCKpEKO__6Bs4FCrO&rWsF?zL={EDvC7yPLu0qVfl?_Fp54b?{A5PYPg=_x z{FLOS{#5f?`f`<#OG#zNca6$%bKQ6slVv<MI#3%W2j2eVu*#0@ZtR+vpCqL8muUXC z+1$zP8`#GO?$F&)IdE^0CEK7eo>yH}M3!`qpe|<dj6sPeaoV|$$Z8mncPm6Rd7mSH z$L%!c<(7l1vLvn07x-f*TuAQHefa%QHh23i2hktPXwmC2Y<`?7YrjODKV2eKsTX>X zc$F{aC7pM%ru!~%M81xU*_q8doj2sY4(js((V9GMA0nKyhtQh>cglB-3EjH8owfyz zpieeO5?(TqIxUJP)0f<$y?F=Vf`vZGoY92^`JGtVzLSjjl*1KR8{v#~%V?du4X9s! z20xCZbCR8#gljsVO@6$Rv>p{_cYjW&?_0ept;G+qU#zN$rPN<MHS8XFvO=DY8)w7@ zkU4zq9A~=MRf!c{s3jv#3GCieZ78iC1%>y8XKt(^%?J>*xbQC8d!~d`k2fTxU-z-g z0t8?6Is+noCW=0+uY>7M<H*&SCosU%pL<X-gTG(vjsNUkpl?eetIp>^#BEbn-#dw1 zx*g@9>0QWAN?lLh+n&I$*L#THG9v6ATuI4~D#1Jcfu4;#L3g@;C$e#y$h^#XWH8X5 zgz2xL$NLQFcj=Af{g11n9qK*gQlm3}SMCqj_Ev>%a`VP_LSTEP+ymy-Hw#!KUxv-z zweaKV9Q3&V9J>yFg0&Bm?Y~IbU`YEeeotB;eVipQwzK0%%z`m|*7yVLlx45@(of@o zF1MsPH(c5Dx;S3P#eiM$qKUZZuApPZe5qGZ2I*_QBHCC~j8C+a;r0Asv~R;4w0>Po zj7HnjK2C;IJ+`6|Z~VxUn2i-5S|5^EZM`sWtrv51lM<SgD$*$<KY;6MXR>bK9A5F7 zNm>^k;C;h>vukP<=+X{fTo7AE+P{uvU$sW?Uhhw`16$6M>URzJZcver3hm&#ych7b zr~5fs!%t+ZO$>%FTtEUGPr%V#$@HMrOxC(LnvPkwgm0T}$gd6V7qXB-|8v4?PQGmz zEbo%y8$wjTWU&~H3jBhL)~V8Ei|pCC4zXNV%{w^TA52b_o3mwMRYYCglpZcm<V{qc z@)|=X^G#zYXjS(^Xq{ZS^ULp4_oy2CXQkl9c=niHC{ZI~{>tq2q*$owcjfJ_TM0Rx z1~TsYSYAToIE@_pg)V(($QBF4+S5mD$-5JB<deYJIdl6Y$eIsgAm0JuX$&ke-N4-_ z)}Sj>tw`>fYIwX$nrd$GB~Qfd$#CTwDjC&NS!8Hqe<@AeA^ElxJNeZCvTNQQI{B|J zyZ>teadO_pU;ZUU_sUg*byyB|k4m6V-~Ga>=R@i2T4{cP{WVS>f|yOyJfS5rgQi?6 zCEX6UK=MK%963tytAZ5UesBk*b?@h-dsDd|xr>OUsw%5#If|b7HH>by+70y<ZhXnb zA9QH^3cB;gC|33FH&ES_%kIjv<el$j@h<9*sOknQzMVP9qiiyL-P#0yPs+2dp|Rxh zP6Kwr^dxE}X-Rhk+Ov|fU3kg42A^hU)1B*v!>Y!Wc<zKQwUL~NJ|C{(1q)+3du&~~ zmfT*5dHEQsRtA8H-*_0BSdY6WO+`(T1&Q0+gmtJ2yVh?gKYjQfht)UA*yvqR4)SH? zRBZV)RvS5b>+DsMa@`j_LpmVJr<yK$JCRy`+KhMQMv~Cg;$-=u4`6%Nlm^NAkrb~| zX43mSRCYT9FaDI^RhwzN!PqYNcu|3Deef1c&4y4PA+tVXli&~fVL;m#tYX*BeTJR4 zJz!$VaFSgXLA%A~^DPsf!BIV)?{2E+OY{tQnJLHVq!knS%A=BO8M}zO2F(yUF8kTf zaz3o9k0Z9G#WLRux3Oh$9o&gIQZ()Se%7K}k)P9@NxnYOr_Y5hj7;O(sMeGL3!4Ms za>{jV(#<Dp4;@9-!-9Xr!V6Z#+~hX5ZN};&#u(h|OxMjGLF<e%xd<5xI$@(Y-*oOb zALRI+ZTYCi{|c$#vrH^`L*oiMxW0#U)coPDo}WYJUWuSnZZ2Y5&W|Fi#(QFlT{%2X z8%r934iOJUH5%nGmOQ-HMe|ceunS*Gl0?>-w^Y(45)EDWYMd`m?`<cBO2XMMHXagg zoFY<5FG#gc3`krGV6J^lVik`}=2O!G*$LwUsM}yEyJeFh@7Hvj3HTPvpON3m4m|Yd zmG`+|XPFg$*X<PFQa6Q%74zx9vQ#EBp_hi3O{YVZv*Fh(J^JKHKZzIXrfN?#Ds^7E z^G!lm;B$H(<!9K4D)aYNJo=PL*OxYNlPrdi&)T+lV~r)<qG87BcYC1u+&5U|l|r9u zFC`(K$uxXw8Cki<%VE}`=P-JbBOhTrm2Vy;Vrlv`n3^}9%CE2Fb6k>WzvK&UM&<({ zqZowTs8^z0Lhp^nZ4cVveUld6e?$jV4C(IN#q`ZE9h%U&74*+1@ZDPkYJBAt2(NiX ztxfC6>lAr1Z&(*bZ9PJ~Leu%s(;_BauS@hpQG$;(P8N23*`y;>=nWzn?2*t1)Nrdb zyHQCRN`EV`lOMIyd-GN3lC(WULM4p9aGX#w<ph*&4WLPa*LgVo1_noS%2FdW`Ez>v ziG{Vmp^Z2}FPVR<Si0~7SycLkdVjVh<96l4uF;fElN?WYqXV!<qZwY+2UEoZHe}Gx zmU}!<hGIFfL}rf)FSJR4-{Ewi$`>nN4<2VdN=Nh23#Rj3WqVkq*{jF|yOlWmR|@S> zyMUjL{;7BsCdtp-xC+MKt;KeyI+U*8PamGCrVBRBq0_d{!$gAwlD#2>j=Y}CTv%ts z4tM;-=$MTm`gte$%TrmV^t(4PSYbpC`|Rb)&d#94U){)hhYNT)(VA}V(;_J@0j%7` z<z&QnNr$JKufXZ|6Dv<T=#Y_P4zRy?A(=H%iI1F}$m;hN@|&NV@iPLX$%8F5pfo?0 zuj7x9a>3U*^P(faX~YP&qp^{k3=N{5V*<!Rg-gN?`vT6AEa$@gR2aM9&(z01kz__F z(lxeY>7V7hh{lx<xLAKUQ{Qio^$X_0sf<$kH+~KuyUCWeM5|QFWjV7_%4+ndNg0ih z_X4d|KWWsyU{-yO3vY1w9bI-pjsICYg+{wQrAbAlkdom^V<cCSYqk9-7vDf*EYk4o z+&y$hYZUwWK{(O5t;F9Bc*0a=WiyZ83VNXN5#p>@h}35c%}fp<)nfL%yTc6>+j0xm zoa>|Eq0)Si;xK+k)&*iMKH6c-%NpJ;?FU)4YaiX??ZO|t{E9~TuO$Ozj{K5cSJ{^f z4r1o>@93oHP2A@Nut5o9`JbCVBAYdd4VS${r>%KR701SsrI!;yrG61HA5lXUlv}W` zN`c%rwc+QT*+VKzo->85hcP);gM6$it}wP9K_`rGK<TYXBr&U>d}N=Jzpf%0q^QjQ zy7i6B_7!IjhYVxo@?}{4<Otqlge=SK9b}1>BFw)wj&)cv53=015&g0V@RQeqzYQzt z?{n(>3B6D<W-ypsd#K8X7OrQ%<Y=;SPX6@UuM-#@>uv8C{SrbSNzprlc4Vu==Zf0i zab(@J6#84>M=Ugs2g}}rwEf_5x;@mEy{9YADrd*jlslLCw}ZZX6G`I(7v5y2%)dx| zq;vS=-4^`3utR*M@JtSLr_;{|j?#k50w+akH(9hhfza*{x^=j^kWu_gZ}+*;dCo}v zv*pN>fM7Nz$cbBeQi;0V+sWF=bu!{k{W$OS8@&Bnmz+Mdg{pgB!}NcO+~F<tU^?av zPUB)&hfpKR{H#T&(W<QSy+!jf#Msl;sl;v>Fq;nT!!G~h{L_>N{MDxeG&t{)&`DrG z3JyKx@#RtaS;zrhf33vF>=tJ-^w!X#)ht=~P?>&@*+-)<9_NSWNz=YHwcsmw7TN?) zzU6ciCRg4OmG2!P%MGJhL%k8GNYrVPnj+c0(ww(h9LwHXe-{(nBptSW98=jl)`0&z zb_Rd%)*y-a>46c9HGkCQ0^hMR^Z)AXd|;Y7;yC`S0SjyeWKkUM4_Pe!K`oXRmA?0? z4N0+C*K9fzJI9}q=~fs+(zzKj5Y4&}mx`i6GdC9Jm~%-3iln1=4mUBII?6;{;s7_Z zWP_2ixr7D5-CH5M``t@S*5=-Oy}VC<?|1jTm&@IIzr)@34t%en5T|?*K@<6dl2=%a z-d|9JJ4Ri&@Ocf^XB*LtV_}r*Cu4WDZD_atGwk#fik^W4=}6u-{9-b>f&KL;x<g{z z+^QRp@=xmJjUU_yZJ5$Y1+^rm>bW|3@6a{;PJ$Z+Gs;lO<$b7SJb;#Uu9Yj&4&sbZ z8tyh_O2ziM)?F79tZNVaA#a;pFK;-NEI&7uEx)(QgT@XgieK*Q7OUKKQh$L)t}hX+ z<&#_FfU{H5&Ax!%j7*X^V^=ITn;HL_B8XcPb0y27JL1w^Uz7fGSUh~SM*QtcpV(u% zDE)HlIPy+@gW6Ib!*iW(vPSz-d}4H7SIr(DIy%~d+V`)+wr3WJpW-lDCl-Y!?sk$j zMALKkL|zZ2I!2IjA|3aA)g}!;at(F(&!P+A7;H_PFK29hSIX2pft2~<-mCL&{*U=% z62AMv<0mEN2Jw{&jr63U(bD`zbEvx2Dz)q5&`49c<>sleP<o>k<rmg=ook*F?FWZK z*6lrADVxpc_R&v5>ud7Fv)W~$E#=3g&vlnWPZyk&f*)eB;oCkko>XO-AODo(?_JZi z+f;2?9n^@IuFsLZr(31Q;go;<T4;2hc<}r&{r<AItZL_u*D9#@myPjC35Acpyh%f% z<0;AIn5FCltyb9zLRR<3j__hZP*PGfi`o`fm2ThREnE3gadq*E%2z#*PQLe(yXYYj zdyga?#R)?C5`i60GeCBM_V@VBgi#X9MofUsf~@EM0nf~sULs?+)eKqA@ORGd`-=#q zBo=m&AwSyXm06tS`qrHM8MU#n!3N<~@Um_0(lawv%EFZ!A#8;34|ZLfy3h7a5YC41 zTOUb!b)S1lHtVwq!q46xzN_wYpcukt2#=@YL3N)aRS?dBaDPnCZFQe7?S$}Z2=Bcf zT%zu?>um_HfpB^0-#?xwEk#MJIt{e|=Bhd9KmCGwdAaH#%c(Pcf9i;Oc?AwZm{Vt5 z-k&=4^71u7n7izUZQ~mC@=9ufFsDx6P}7vU&k+vh)cNzsc%iz_hIVL~QzyN?C)=pB zXCTa}lU_WNh5Z=9oI2^1BUrfdJcK!Q(#tcja3Bm}PM!3c3#>YQeE{ZaijD`%8jf%* zr%pQ9DhvAupkYp(bfgg$t{j9gr%pPY2MY%x5axWQ-$7(y-x!2Bb<!{Rv2fBPggJH6 zPq(o(6@f5kn0|tcWeth5*qb(Im!9=vS#1(vxkf?HNU^Lx4X|9BpnGF1Ygh<au1U~6 z9+s_K4p^>5(0v@XlI$z~W2bvaE=O!^rV#x+K70A}ks#=&k7o8UanX!M@)yd7aC4OX Wk(}snW$SXp#434u@>R}{p8F4`F8J2~ From fc20c5af8e1a4807b9e7f93819cd35d5987d77e1 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 2 Dec 2021 12:55:06 +0100 Subject: [PATCH 072/512] kill raisim server in destructor (joins joinable threads) --- .../ocs2_raisim/include/ocs2_raisim/RaisimRollout.h | 3 +++ ocs2_raisim/ocs2_raisim/src/RaisimRollout.cpp | 9 +++++++++ 2 files changed, 12 insertions(+) diff --git a/ocs2_raisim/ocs2_raisim/include/ocs2_raisim/RaisimRollout.h b/ocs2_raisim/ocs2_raisim/include/ocs2_raisim/RaisimRollout.h index e4cb0149a..1a5219699 100644 --- a/ocs2_raisim/ocs2_raisim/include/ocs2_raisim/RaisimRollout.h +++ b/ocs2_raisim/ocs2_raisim/include/ocs2_raisim/RaisimRollout.h @@ -84,6 +84,9 @@ class RaisimRollout final : public RolloutBase { //! Copy constructor RaisimRollout(const RaisimRollout& other); + //! Destructor + ~RaisimRollout(); + void resetRollout() override { raisimRolloutSettings_.setSimulatorStateOnRolloutRunOnce_ = true; } RaisimRollout* clone() const override { return new RaisimRollout(*this); } diff --git a/ocs2_raisim/ocs2_raisim/src/RaisimRollout.cpp b/ocs2_raisim/ocs2_raisim/src/RaisimRollout.cpp index 6d20d139e..dd6e08827 100644 --- a/ocs2_raisim/ocs2_raisim/src/RaisimRollout.cpp +++ b/ocs2_raisim/ocs2_raisim/src/RaisimRollout.cpp @@ -94,6 +94,15 @@ RaisimRollout::RaisimRollout(const RaisimRollout& other) } } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +RaisimRollout::~RaisimRollout() { + if (raisimRolloutSettings_.raisimServer_) { + serverPtr_->killServer(); + } +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ From 60b59aa6a0e29ffda92f43f298a1a953a9a438c9 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 25 Jan 2022 15:53:51 +0100 Subject: [PATCH 073/512] adapt to changes in ocs2_legged_robot_raisim --- .../src/LeggedRobotMpcnetDummyNode.cpp | 7 ++++--- .../src/LeggedRobotMpcnetInterface.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 8ad08e023..b6e19c5b4 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -63,10 +63,11 @@ int main(int argc, char** argv) { std::unique_ptr<RaisimHeightmapRosConverter> heightmapPub; std::unique_ptr<LeggedRobotRaisimConversions> conversions; if (raisim) { - conversions.reset( - new LeggedRobotRaisimConversions(leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo())); + conversions.reset(new LeggedRobotRaisimConversions(leggedRobotInterface.getPinocchioInterface(), + leggedRobotInterface.getCentroidalModelInfo(), + leggedRobotInterface.getInitialState())); RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout", true); - conversions->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); + conversions->loadSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout", true); rolloutPtr.reset(new RaisimRollout( ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes", diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index f60522c2c..b00d02524 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -46,8 +46,10 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout"); raisimRolloutSettings.portNumber_ += i; leggedRobotRaisimConversionsPtrs_.push_back(std::unique_ptr<LeggedRobotRaisimConversions>(new LeggedRobotRaisimConversions( - leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo()))); - leggedRobotRaisimConversionsPtrs_[i]->setGains(raisimRolloutSettings.pGains_, raisimRolloutSettings.dGains_); + leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(), + leggedRobotInterfacePtrs_[i]->getInitialState()))); + leggedRobotRaisimConversionsPtrs_[i]->loadSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", + "rollout", true); rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(new RaisimRollout( ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes", From 49fb4b5c549f621b192a02bc1a86fc8e5d11bcc2 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 8 Feb 2022 16:47:54 +0100 Subject: [PATCH 074/512] adapt to changes in main --- .../ocs2_legged_robot_mpcnet/CMakeLists.txt | 1 + .../launch/legged_robot_mpcnet.launch | 64 +++++++++++++------ .../ocs2_legged_robot_mpcnet/package.xml | 1 + .../src/LeggedRobotMpcnetDummyNode.cpp | 43 ++++++------- .../src/LeggedRobotMpcnetInterface.cpp | 21 +++--- 5 files changed, 75 insertions(+), 55 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt index c2515d4d6..ef000ebdb 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt @@ -4,6 +4,7 @@ project(ocs2_legged_robot_mpcnet) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_legged_robot ocs2_legged_robot_raisim + ocs2_legged_robot_ros ocs2_mpcnet ) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch index f7d17a578..59311840a 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch @@ -1,32 +1,56 @@ +<?xml version="1.0" ?> + <launch> - <arg name="robot_name" default="legged_robot"/> - <arg name="config_name" default="mpc"/> - <arg name="target_command" default="$(find ocs2_legged_robot)/config/command/targetTrajectories.info"/> - <arg name="gait_command" default="$(find ocs2_legged_robot)/config/command/gait.info"/> + <!-- visualization config --> <arg name="rviz" default="true" /> - <arg name="multiplot" default="false"/> <arg name="description_name" default="legged_robot_description"/> - <arg name="description_file" default="$(find ocs2_robotic_assets)/resources/anymal_c/urdf/anymal.urdf"/> - <arg name="policy_file_path" default="$(find ocs2_legged_robot_mpcnet)/policy/legged_robot.onnx"/> - <arg name="raisim" default="true"/> - + <arg name="multiplot" default="false"/> + + <!-- The task file for the mpc. --> + <arg name="taskFile" default="$(find ocs2_legged_robot)/config/mpc/task.info"/> + <!-- The reference related config file of the robot --> + <arg name="referenceFile" default="$(find ocs2_legged_robot)/config/command/reference.info"/> + <!-- The URDF model of the robot --> + <arg name="urdfFile" default="$(find ocs2_robotic_assets)/resources/anymal_c/urdf/anymal.urdf"/> + <!-- The file defining gait definition --> + <arg name="gaitCommandFile" default="$(find ocs2_legged_robot)/config/command/gait.info"/> + <!-- The file containing the RaiSim settings --> + <arg name="raisimFile" default="$(find ocs2_legged_robot_raisim)/config/raisim.info"/> + <!-- The path to the resource directory (meshes etc.) --> + <arg name="resourcePath" default="$(find ocs2_robotic_assets)/resources/anymal_c/meshes"/> + <!-- The path to the MPC-Net policy --> + <arg name="policyFile" default="$(find ocs2_legged_robot_mpcnet)/policy/legged_robot.onnx"/> + <!-- Whether to use RaiSim or not --> + <arg name="useRaisim" default="true"/> + + <!-- rviz --> <group if="$(arg rviz)"> - <include file="$(find ocs2_legged_robot)/launch/visualize.launch"> - <arg name="description_name" value="$(arg description_name)"/> - <arg name="description_file" value="$(arg description_file)"/> - </include> + <param name="$(arg description_name)" textfile="$(arg urdfFile)"/> + <arg name="rvizconfig" default="$(find ocs2_legged_robot_ros)/rviz/legged_robot.rviz" /> + <node pkg="rviz" type="rviz" name="rviz" args="-d $(arg rvizconfig)" output="screen" /> </group> - + + <!-- multiplot --> <group if="$(arg multiplot)"> - <include file="$(find ocs2_legged_robot)/launch/multiplot.launch"/> + <include file="$(find ocs2_legged_robot_ros)/launch/multiplot.launch"/> </group> + <!-- make the files into global parameters --> + <param name="taskFile" value="$(arg taskFile)" /> + <param name="referenceFile" value="$(arg referenceFile)" /> + <param name="urdfFile" value="$(arg urdfFile)" /> + <param name="gaitCommandFile" value="$(arg gaitCommandFile)"/> + <param name="raisimFile" value="$(arg raisimFile)" /> + <param name="resourcePath" value="$(arg resourcePath)"/> + <param name="policyFile" value="$(arg policyFile)" /> + <param name="useRaisim" value="$(arg useRaisim)"/> + <node pkg="ocs2_legged_robot_mpcnet" type="legged_robot_mpcnet_dummy" name="legged_robot_mpcnet_dummy" - output="screen" args="$(arg robot_name) $(arg config_name) $(arg target_command) $(arg description_file) $(arg policy_file_path) $(arg raisim)" launch-prefix=""/> + output="screen" launch-prefix=""/> - <node pkg="ocs2_legged_robot" type="legged_robot_target" name="legged_robot_target" - output="screen" args="$(arg robot_name) $(arg target_command)" launch-prefix="gnome-terminal --"/> + <node pkg="ocs2_legged_robot_ros" type="legged_robot_target" name="legged_robot_target" + output="screen" launch-prefix="gnome-terminal --"/> - <node pkg="ocs2_legged_robot" type="legged_robot_gait_command" name="legged_robot_gait_command" - output="screen" args="$(arg robot_name) $(arg gait_command)" launch-prefix="gnome-terminal --"/> + <node pkg="ocs2_legged_robot_ros" type="legged_robot_gait_command" name="legged_robot_gait_command" + output="screen" launch-prefix="gnome-terminal --"/> </launch> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml index e340a2d41..b23c2720c 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml @@ -15,6 +15,7 @@ <depend>ocs2_legged_robot</depend> <depend>ocs2_legged_robot_raisim</depend> + <depend>ocs2_legged_robot_ros</depend> <depend>ocs2_mpcnet</depend> </package> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index b6e19c5b4..91c4bf29a 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -1,13 +1,12 @@ #include <ros/init.h> #include <ros/package.h> -#include <urdf_parser/urdf_parser.h> #include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h> #include <ocs2_legged_robot/LeggedRobotInterface.h> -#include <ocs2_legged_robot/gait/GaitReceiver.h> -#include <ocs2_legged_robot/visualization/LeggedRobotVisualizer.h> #include <ocs2_legged_robot_raisim/LeggedRobotRaisimConversions.h> #include <ocs2_legged_robot_raisim/LeggedRobotRaisimVisualizer.h> +#include <ocs2_legged_robot_ros/gait/GaitReceiver.h> +#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h> #include <ocs2_mpcnet/control/MpcnetOnnxController.h> #include <ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h> #include <ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h> @@ -22,25 +21,24 @@ using namespace ocs2; using namespace legged_robot; int main(int argc, char** argv) { - std::vector<std::string> programArgs{}; - ::ros::removeROSArgs(argc, argv, programArgs); - if (programArgs.size() < 7) { - throw std::runtime_error( - "No robot name, config folder, target command file, description file, policy file path, or rollout type specified. Aborting."); - } - const std::string robotName(programArgs[1]); - const std::string configName(programArgs[2]); - const std::string targetCommandFile(programArgs[3]); - const std::string descriptionFile(programArgs[4]); - const std::string policyFilePath(programArgs[5]); - const bool raisim = (programArgs[6] == "true") ? true : false; + const std::string robotName = "legged_robot"; // initialize ros node ros::init(argc, argv, robotName + "_mpcnet_dummy"); ros::NodeHandle nodeHandle; + // Get node parameters + bool useRaisim; + std::string taskFile, urdfFile, referenceFile, raisimFile, resourcePath, policyFile; + nodeHandle.getParam("/taskFile", taskFile); + nodeHandle.getParam("/urdfFile", urdfFile); + nodeHandle.getParam("/referenceFile", referenceFile); + nodeHandle.getParam("/raisimFile", raisimFile); + nodeHandle.getParam("/resourcePath", resourcePath); + nodeHandle.getParam("/policyFile", policyFile); + nodeHandle.getParam("/useRaisim", useRaisim); // legged robot interface - LeggedRobotInterface leggedRobotInterface(configName, targetCommandFile, urdf::parseURDFFile(descriptionFile)); + LeggedRobotInterface leggedRobotInterface(taskFile, urdfFile, referenceFile); // gait receiver auto gaitReceiverPtr = @@ -55,22 +53,21 @@ int main(int argc, char** argv) { std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr(new LeggedRobotMpcnetDefinition(leggedRobotInterface.getInitialState())); std::unique_ptr<MpcnetControllerBase> mpcnetControllerPtr( new MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); - mpcnetControllerPtr->loadPolicyModel(policyFilePath); + mpcnetControllerPtr->loadPolicyModel(policyFile); // rollout std::unique_ptr<RolloutBase> rolloutPtr; raisim::HeightMap* terrainPtr = nullptr; std::unique_ptr<RaisimHeightmapRosConverter> heightmapPub; std::unique_ptr<LeggedRobotRaisimConversions> conversions; - if (raisim) { + if (useRaisim) { conversions.reset(new LeggedRobotRaisimConversions(leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo(), leggedRobotInterface.getInitialState())); - RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout", true); - conversions->loadSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout", true); + RaisimRolloutSettings raisimRolloutSettings(raisimFile, "rollout", true); + conversions->loadSettings(raisimFile, "rollout", true); rolloutPtr.reset(new RaisimRollout( - ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", - ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes", + urdfFile, resourcePath, [&](const vector_t& state, const vector_t& input) { return conversions->stateToRaisimGenCoordGenVel(state, input); }, [&](const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { return conversions->raisimGenCoordGenVelToState(q, dq); }, [&](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { @@ -99,7 +96,7 @@ int main(int argc, char** argv) { PinocchioEndEffectorKinematics endEffectorKinematics(leggedRobotInterface.getPinocchioInterface(), pinocchioMapping, leggedRobotInterface.modelSettings().contactNames3DoF); std::shared_ptr<LeggedRobotVisualizer> leggedRobotVisualizerPtr; - if (raisim) { + if (useRaisim) { leggedRobotVisualizerPtr.reset(new LeggedRobotRaisimVisualizer( leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); static_cast<LeggedRobotRaisimVisualizer*>(leggedRobotVisualizerPtr.get())->updateTerrain(); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index b00d02524..6e2f63444 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -1,7 +1,6 @@ #include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h" #include <ros/package.h> -#include <urdf_parser/urdf_parser.h> #include <ocs2_mpc/MPC_DDP.h> #include <ocs2_mpcnet/control/MpcnetOnnxController.h> @@ -18,11 +17,12 @@ namespace legged_robot { LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim) { // create ONNX environment auto onnxEnvironmentPtr = createOnnxEnvironment(); - // path to config files - std::string taskFileFolderName = "mpc"; - std::string targetCommandFile = ros::package::getPath("ocs2_legged_robot") + "/config/command/targetTrajectories.info"; - // path to urdf file + // paths to files + std::string taskFile = ros::package::getPath("ocs2_legged_robot") + "/config/mpc/task.info"; std::string urdfFile = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf"; + std::string referenceFile = ros::package::getPath("ocs2_legged_robot") + "/config/command/reference.info"; + std::string raisimFile = ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info"; + std::string resourcePath = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes"; // set up MPC-Net rollout manager for data generation and policy evaluation std::vector<std::unique_ptr<MPC_BASE>> mpcPtrs; std::vector<std::unique_ptr<MpcnetControllerBase>> mpcnetPtrs; @@ -35,24 +35,21 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr mpcnetDefinitionPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { - leggedRobotInterfacePtrs_.push_back(std::unique_ptr<LeggedRobotInterface>( - new LeggedRobotInterface(taskFileFolderName, targetCommandFile, urdf::parseURDFFile(urdfFile)))); + leggedRobotInterfacePtrs_.push_back(std::unique_ptr<LeggedRobotInterface>(new LeggedRobotInterface(taskFile, urdfFile, referenceFile))); std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr( new LeggedRobotMpcnetDefinition(leggedRobotInterfacePtrs_[i]->getInitialState())); mpcPtrs.push_back(getMpc(*leggedRobotInterfacePtrs_[i])); mpcnetPtrs.push_back(std::unique_ptr<MpcnetControllerBase>( new MpcnetOnnxController(mpcnetDefinitionPtr, leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr(), onnxEnvironmentPtr))); if (raisim) { - RaisimRolloutSettings raisimRolloutSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", "rollout"); + RaisimRolloutSettings raisimRolloutSettings(raisimFile, "rollout"); raisimRolloutSettings.portNumber_ += i; leggedRobotRaisimConversionsPtrs_.push_back(std::unique_ptr<LeggedRobotRaisimConversions>(new LeggedRobotRaisimConversions( leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(), leggedRobotInterfacePtrs_[i]->getInitialState()))); - leggedRobotRaisimConversionsPtrs_[i]->loadSettings(ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info", - "rollout", true); + leggedRobotRaisimConversionsPtrs_[i]->loadSettings(raisimFile, "rollout", true); rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(new RaisimRollout( - ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf", - ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes", + urdfFile, resourcePath, [&, i](const vector_t& state, const vector_t& input) { return leggedRobotRaisimConversionsPtrs_[i]->stateToRaisimGenCoordGenVel(state, input); }, From 9c2cbd8bd9b12b36b81004c1267795846737844d Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Sun, 13 Feb 2022 10:42:55 +0100 Subject: [PATCH 075/512] update package.xml --- ocs2_mpcnet/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_mpcnet/package.xml b/ocs2_mpcnet/package.xml index e6b0e9e4e..b2120f7fc 100644 --- a/ocs2_mpcnet/package.xml +++ b/ocs2_mpcnet/package.xml @@ -4,10 +4,12 @@ <version>0.0.0</version> <description>The ocs2_mpcnet package</description> + <author email="areske@ethz.ch">Alexander Reske</author> + <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer> <maintainer email="areske@ethz.ch">Alexander Reske</maintainer> - <license>TODO</license> + <license>BSD-3</license> <buildtool_depend>catkin</buildtool_depend> From b1a39df82ccc5c66a4546fa65080e1c52c8041d3 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Sun, 13 Feb 2022 10:44:54 +0100 Subject: [PATCH 076/512] update package.xml --- ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml index 56162164c..1a792362e 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml @@ -4,10 +4,12 @@ <version>0.0.0</version> <description>The ocs2_ballbot_mpcnet package</description> + <author email="areske@ethz.ch">Alexander Reske</author> + <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer> <maintainer email="areske@ethz.ch">Alexander Reske</maintainer> - <license>TODO</license> + <license>BSD-3</license> <buildtool_depend>catkin</buildtool_depend> From 3c9d1fd4c09fcd696712898f15d96836c9ae609e Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Sun, 13 Feb 2022 10:47:26 +0100 Subject: [PATCH 077/512] update package.xml --- ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml index b23c2720c..4b0973fa0 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml @@ -4,10 +4,12 @@ <version>0.0.0</version> <description>The ocs2_legged_robot_mpcnet package</description> + <author email="areske@ethz.ch">Alexander Reske</author> + <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer> <maintainer email="areske@ethz.ch">Alexander Reske</maintainer> - <license>TODO</license> + <license>BSD-3</license> <buildtool_depend>catkin</buildtool_depend> From 7ba09d5d08884c43e3f4786765aa4b56c61ac4dd Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Sun, 20 Feb 2022 17:33:04 +0100 Subject: [PATCH 078/512] add venv to installation docs --- ocs2_doc/docs/installation.rst | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/ocs2_doc/docs/installation.rst b/ocs2_doc/docs/installation.rst index e74c6bab7..15506ea9b 100644 --- a/ocs2_doc/docs/installation.rst +++ b/ocs2_doc/docs/installation.rst @@ -103,6 +103,40 @@ Optional Dependencies We provide custom cmake config and version files to enable ``find_package(onnxruntime)`` without modifying ``LIBRARY_PATH`` and ``LD_LIBRARY_PATH``. Note that the last command above assumes that you cloned OCS2 into the folder ``git`` in your user's home directory. +* `Virtual environments <https://docs.python.org/3/library/venv.html>`__ are recommended when training MPC-Net policies: + + .. code-block:: bash + + sudo apt-get install python3-venv + + Create an environment and give it access to the system site packages: + + .. code-block:: bash + + mkdir venvs && cd venvs + python3 -m venv mpcnet --system-site-packages + + Activate the environment and install the requirements: + + .. code-block:: bash + + source ~/venvs/mpcnet/bin/activate + pip3 install torch tensorboard + + Always activate the environment when running and monitoring the training, i.e. in one terminal: + + .. code-block:: bash + + source ~/venvs/mpcnet/bin/activate + pip3 <robot_name>_mpcnet.py + + And in another terminal: + + .. code-block:: bash + + source ~/venvs/mpcnet/bin/activate + tensorboard --logdir=runs + .. _doxid-ocs2_doc_installation_ocs2_doc_install: Installation From 90d1e6545372b709a8c56ee651acb193af30bec3 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Feb 2022 19:00:03 +0100 Subject: [PATCH 079/512] update mpcnet install instructions --- ocs2_doc/docs/installation.rst | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/ocs2_doc/docs/installation.rst b/ocs2_doc/docs/installation.rst index 15506ea9b..50d0d9127 100644 --- a/ocs2_doc/docs/installation.rst +++ b/ocs2_doc/docs/installation.rst @@ -114,14 +114,21 @@ Optional Dependencies .. code-block:: bash mkdir venvs && cd venvs - python3 -m venv mpcnet --system-site-packages + python3 -m venv mpcnet Activate the environment and install the requirements: .. code-block:: bash source ~/venvs/mpcnet/bin/activate - pip3 install torch tensorboard + python3 -m pip install -r ~/git/ocs2_dev/ocs2_mpcnet/requirements.txt + + Newer graphics cards might require a CUDA capability which is currently not supported by the standard PyTorch install. + In that case check `PyTorch Start Locally <https://pytorch.org/get-started/locally/>`__ for a compatible version and, e.g., run: + + .. code-block:: bash + + pip3 install torch==1.10.2+cu113 -f https://download.pytorch.org/whl/cu113/torch_stable.html Always activate the environment when running and monitoring the training, i.e. in one terminal: From 0d55538faea2e5c95dde1c01cdfed1a4230d8950 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Feb 2022 09:48:36 +0100 Subject: [PATCH 080/512] make controllers final --- .../include/ocs2_mpcnet/control/MpcnetBehavioralController.h | 2 +- ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h index 082627492..4ccf2d1ac 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -13,7 +13,7 @@ namespace ocs2 { * and a learned policy (e.g. explicitly represented by a neural network). * The behavioral policy is pi_behavioral = alpha * pi_optimal + (1 - alpha) * pi_learned with alpha in [0, 1]. */ -class MpcnetBehavioralController : public ControllerBase { +class MpcnetBehavioralController final : public ControllerBase { public: using Base = ControllerBase; using Optimal = ControllerBase; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index 2f93cb655..4799527f3 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -25,7 +25,7 @@ inline std::shared_ptr<Ort::Env> createOnnxEnvironment() { * U: predicted expert inputs (1 x dimensionOfInput x numberOfExperts). * @note The additional first dimension with size 1 for the variables of the model comes from batch processing during training. */ -class MpcnetOnnxController : public MpcnetControllerBase { +class MpcnetOnnxController final : public MpcnetControllerBase { public: using Base = MpcnetControllerBase; using tensor_element_t = float; From 1b506e2efd08089a598d102dc176f38e6b900723 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Feb 2022 10:39:08 +0100 Subject: [PATCH 081/512] remove unnecessary using declarations --- .../control/MpcnetBehavioralController.h | 20 ++++++++----------- .../control/MpcnetControllerBase.h | 2 -- .../control/MpcnetOnnxController.h | 5 ++--- .../control/MpcnetBehavioralController.cpp | 2 +- 4 files changed, 11 insertions(+), 18 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h index 4ccf2d1ac..37c0bee4e 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -15,10 +15,6 @@ namespace ocs2 { */ class MpcnetBehavioralController final : public ControllerBase { public: - using Base = ControllerBase; - using Optimal = ControllerBase; - using Learned = MpcnetControllerBase; - /** * Default constructor, leaves object uninitialized. */ @@ -30,10 +26,10 @@ class MpcnetBehavioralController final : public ControllerBase { * @param [in] optimalControllerPtr : Pointer to the optimal controller. * @param [in] learnedControllerPtr : Pointer to the learned controller. */ - MpcnetBehavioralController(scalar_t alpha, Optimal* optimalControllerPtr, Learned* learnedControllerPtr) + MpcnetBehavioralController(scalar_t alpha, ControllerBase* optimalControllerPtr, MpcnetControllerBase* learnedControllerPtr) : alpha_(alpha), - optimalControllerPtr_(std::unique_ptr<Optimal>(optimalControllerPtr)), - learnedControllerPtr_(std::unique_ptr<Learned>(learnedControllerPtr)) {} + optimalControllerPtr_(std::unique_ptr<ControllerBase>(optimalControllerPtr)), + learnedControllerPtr_(std::unique_ptr<MpcnetControllerBase>(learnedControllerPtr)) {} /** * Copy constructor. @@ -56,17 +52,17 @@ class MpcnetBehavioralController final : public ControllerBase { * Set the optimal controller. * @param [in] optimalControllerPtr : Pointer to the optimal controller. */ - void setOptimalController(Optimal* optimalControllerPtr) { optimalControllerPtr_.reset(optimalControllerPtr); } + void setOptimalController(ControllerBase* optimalControllerPtr) { optimalControllerPtr_.reset(optimalControllerPtr); } /** * Set the learned controller. * @param [in] learnedControllerPtr : Pointer to the learned controller. */ - void setLearnedController(Learned* learnedControllerPtr) { learnedControllerPtr_.reset(learnedControllerPtr); } + void setLearnedController(MpcnetControllerBase* learnedControllerPtr) { learnedControllerPtr_.reset(learnedControllerPtr); } vector_t computeInput(scalar_t t, const vector_t& x) override; - void concatenate(const Base* otherController, int index, int length) override; + void concatenate(const ControllerBase* otherController, int index, int length) override; int size() const override; @@ -80,8 +76,8 @@ class MpcnetBehavioralController final : public ControllerBase { private: scalar_t alpha_; - std::unique_ptr<Optimal> optimalControllerPtr_; - std::unique_ptr<Learned> learnedControllerPtr_; + std::unique_ptr<ControllerBase> optimalControllerPtr_; + std::unique_ptr<MpcnetControllerBase> learnedControllerPtr_; }; } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h index 97d1070d7..9a0411e21 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h @@ -14,8 +14,6 @@ namespace ocs2 { */ class MpcnetControllerBase : public ControllerBase { public: - using Base = ControllerBase; - /** * Constructor. * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions. diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index 4799527f3..e226bc2f0 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -27,7 +27,6 @@ inline std::shared_ptr<Ort::Env> createOnnxEnvironment() { */ class MpcnetOnnxController final : public MpcnetControllerBase { public: - using Base = MpcnetControllerBase; using tensor_element_t = float; /** @@ -38,7 +37,7 @@ class MpcnetOnnxController final : public MpcnetControllerBase { */ MpcnetOnnxController(std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr, std::shared_ptr<Ort::Env> onnxEnvironmentPtr) - : Base(mpcnetDefinitionPtr, referenceManagerPtr), onnxEnvironmentPtr_(onnxEnvironmentPtr) {} + : MpcnetControllerBase(mpcnetDefinitionPtr, referenceManagerPtr), onnxEnvironmentPtr_(onnxEnvironmentPtr) {} /** * Constructor, initializes all members of the controller. @@ -69,7 +68,7 @@ class MpcnetOnnxController final : public MpcnetControllerBase { vector_t computeInput(const scalar_t t, const vector_t& x) override; - void concatenate(const typename Base::Base* otherController, int index, int length) override { + void concatenate(const ControllerBase* otherController, int index, int length) override { throw std::runtime_error("MpcnetOnnxController::concatenate not implemented."); } diff --git a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp index e8840696f..bd066edb3 100644 --- a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp @@ -17,7 +17,7 @@ vector_t MpcnetBehavioralController::computeInput(scalar_t t, const vector_t& x) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MpcnetBehavioralController::concatenate(const Base* otherController, int index, int length) { +void MpcnetBehavioralController::concatenate(const ControllerBase* otherController, int index, int length) { if (optimalControllerPtr_) { optimalControllerPtr_->concatenate(otherController, index, length); } From b21a5d976f8a17601775f74c32ddab71108b07bc Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Feb 2022 10:57:36 +0100 Subject: [PATCH 082/512] fix asset ownership --- .../control/MpcnetBehavioralController.h | 20 +++++++++---------- .../src/rollout/MpcnetDataGeneration.cpp | 4 ++-- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h index 37c0bee4e..a1ce025bb 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -23,19 +23,19 @@ class MpcnetBehavioralController final : public ControllerBase { /** * Constructor, initializes all required members of the controller. * @param [in] alpha : The mixture parameter. - * @param [in] optimalControllerPtr : Pointer to the optimal controller. - * @param [in] learnedControllerPtr : Pointer to the learned controller. + * @param [in] optimalController : The optimal controller (this class takes ownership of a clone). + * @param [in] learnedController : The learned controller (this class takes ownership of a clone). */ - MpcnetBehavioralController(scalar_t alpha, ControllerBase* optimalControllerPtr, MpcnetControllerBase* learnedControllerPtr) + MpcnetBehavioralController(scalar_t alpha, const ControllerBase& optimalController, const MpcnetControllerBase& learnedController) : alpha_(alpha), - optimalControllerPtr_(std::unique_ptr<ControllerBase>(optimalControllerPtr)), - learnedControllerPtr_(std::unique_ptr<MpcnetControllerBase>(learnedControllerPtr)) {} + optimalControllerPtr_(std::unique_ptr<ControllerBase>(optimalController.clone())), + learnedControllerPtr_(std::unique_ptr<MpcnetControllerBase>(learnedController.clone())) {} /** * Copy constructor. */ MpcnetBehavioralController(const MpcnetBehavioralController& other) - : MpcnetBehavioralController(other.alpha_, other.optimalControllerPtr_->clone(), other.learnedControllerPtr_->clone()) {} + : MpcnetBehavioralController(other.alpha_, *other.optimalControllerPtr_, *other.learnedControllerPtr_) {} /** * Default destructor. @@ -50,15 +50,15 @@ class MpcnetBehavioralController final : public ControllerBase { /** * Set the optimal controller. - * @param [in] optimalControllerPtr : Pointer to the optimal controller. + * @param [in] optimalController : The optimal controller (this class takes ownership of a clone). */ - void setOptimalController(ControllerBase* optimalControllerPtr) { optimalControllerPtr_.reset(optimalControllerPtr); } + void setOptimalController(const ControllerBase& optimalController) { optimalControllerPtr_.reset(optimalController.clone()); } /** * Set the learned controller. - * @param [in] learnedControllerPtr : Pointer to the learned controller. + * @param [in] learnedController : The learned controller (this class takes ownership of a clone). */ - void setLearnedController(MpcnetControllerBase* learnedControllerPtr) { learnedControllerPtr_.reset(learnedControllerPtr); } + void setLearnedController(const MpcnetControllerBase& learnedController) { learnedControllerPtr_.reset(learnedController.clone()); } vector_t computeInput(scalar_t t, const vector_t& x) override; diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index fd3ad307c..bfedb3b71 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -36,7 +36,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st // set up behavioral controller with mixture parameter alpha and learned controller MpcnetBehavioralController behavioralController; behavioralController.setAlpha(alpha); - behavioralController.setLearnedController(mpcnetPtr_->clone()); + behavioralController.setLearnedController(*mpcnetPtr_); // set up scalar standard normal generator and compute Cholesky decomposition of covariance matrix std::random_device randomDevice; @@ -91,7 +91,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st } // update behavioral controller with MPC controller - behavioralController.setOptimalController(primalSolution.controllerPtr_->clone()); + behavioralController.setOptimalController(*primalSolution.controllerPtr_); // forward simulate system with behavioral controller scalar_array_t timeTrajectory; From 88ef49900c2f0a1afc724105a2343093695219dd Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Feb 2022 12:26:37 +0100 Subject: [PATCH 083/512] make copy constructors protected/private --- .../control/MpcnetBehavioralController.h | 12 ++++++------ .../ocs2_mpcnet/control/MpcnetControllerBase.h | 5 +++++ .../ocs2_mpcnet/control/MpcnetOnnxController.h | 16 +++++++--------- 3 files changed, 18 insertions(+), 15 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h index a1ce025bb..7d305ce20 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -31,12 +31,6 @@ class MpcnetBehavioralController final : public ControllerBase { optimalControllerPtr_(std::unique_ptr<ControllerBase>(optimalController.clone())), learnedControllerPtr_(std::unique_ptr<MpcnetControllerBase>(learnedController.clone())) {} - /** - * Copy constructor. - */ - MpcnetBehavioralController(const MpcnetBehavioralController& other) - : MpcnetBehavioralController(other.alpha_, *other.optimalControllerPtr_, *other.learnedControllerPtr_) {} - /** * Default destructor. */ @@ -75,6 +69,12 @@ class MpcnetBehavioralController final : public ControllerBase { MpcnetBehavioralController* clone() const override { return new MpcnetBehavioralController(*this); } private: + /** + * Copy constructor. + */ + MpcnetBehavioralController(const MpcnetBehavioralController& other) + : MpcnetBehavioralController(other.alpha_, *other.optimalControllerPtr_, *other.learnedControllerPtr_) {} + scalar_t alpha_; std::unique_ptr<ControllerBase> optimalControllerPtr_; std::unique_ptr<MpcnetControllerBase> learnedControllerPtr_; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h index 9a0411e21..a1f7364d7 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h @@ -64,6 +64,11 @@ class MpcnetControllerBase : public ControllerBase { MpcnetControllerBase* clone() const override = 0; protected: + /** + * Copy constructor. + */ + MpcnetControllerBase(const MpcnetControllerBase& other) : MpcnetControllerBase(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_) {} + std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; }; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index e226bc2f0..871699a46 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -53,12 +53,6 @@ class MpcnetOnnxController final : public MpcnetControllerBase { loadPolicyModel(policyFilePath); } - /** - * Copy constructor. - */ - MpcnetOnnxController(const MpcnetOnnxController& other) - : MpcnetOnnxController(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_, other.onnxEnvironmentPtr_, other.policyFilePath_) {} - /** * Default destructor. */ @@ -80,11 +74,15 @@ class MpcnetOnnxController final : public MpcnetControllerBase { MpcnetOnnxController* clone() const override { return new MpcnetOnnxController(*this); } - protected: + private: + /** + * Copy constructor. + */ + MpcnetOnnxController(const MpcnetOnnxController& other) + : MpcnetOnnxController(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_, other.onnxEnvironmentPtr_, other.policyFilePath_) {} + std::shared_ptr<Ort::Env> onnxEnvironmentPtr_; std::string policyFilePath_; - - private: std::unique_ptr<Ort::Session> sessionPtr_; std::vector<const char*> inputNames_; std::vector<const char*> outputNames_; From edc12db44674a6305dfc7c691f43e5f6cf651e3e Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Feb 2022 16:27:39 +0100 Subject: [PATCH 084/512] adapt legged robot mpcnet to raisim metapackage --- .../src/LeggedRobotMpcnetDummyNode.cpp | 2 +- .../src/LeggedRobotMpcnetInterface.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 91c4bf29a..1fc131c5c 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -11,7 +11,7 @@ #include <ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h> #include <ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h> #include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h> -#include <ocs2_raisim/RaisimRollout.h> +#include <ocs2_raisim_core/RaisimRollout.h> #include <ocs2_raisim_ros/RaisimHeightmapRosConverter.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 6e2f63444..e9bc12f50 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -6,8 +6,8 @@ #include <ocs2_mpcnet/control/MpcnetOnnxController.h> #include <ocs2_oc/rollout/TimeTriggeredRollout.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> -#include <ocs2_raisim/RaisimRollout.h> -#include <ocs2_raisim/RaisimRolloutSettings.h> +#include <ocs2_raisim_core/RaisimRollout.h> +#include <ocs2_raisim_core/RaisimRolloutSettings.h> #include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" From d1030f3220969f9a11685fddf7af24cc30dd97d6 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Feb 2022 16:35:11 +0100 Subject: [PATCH 085/512] add raisim pd option for legged robot mpcnet --- .../src/LeggedRobotMpcnetDummyNode.cpp | 5 ++++- .../src/LeggedRobotMpcnetInterface.cpp | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 1fc131c5c..66198070b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -73,7 +73,10 @@ int main(int argc, char** argv) { [&](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { return conversions->inputToRaisimGeneralizedForce(time, input, state, q, dq); }, - nullptr, raisimRolloutSettings, nullptr)); + nullptr, raisimRolloutSettings, + [&](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return conversions->inputToRaisimPdTargets(time, input, state, q, dq); + })); // terrain if (raisimRolloutSettings.generateTerrain_) { raisim::TerrainProperties terrainProperties; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index e9bc12f50..9939b6dd2 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -59,7 +59,10 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr [&, i](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { return leggedRobotRaisimConversionsPtrs_[i]->inputToRaisimGeneralizedForce(time, input, state, q, dq); }, - nullptr, raisimRolloutSettings, nullptr))); + nullptr, raisimRolloutSettings, + [&, i](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return leggedRobotRaisimConversionsPtrs_[i]->inputToRaisimPdTargets(time, input, state, q, dq); + }))); } else { rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(leggedRobotInterfacePtrs_[i]->getRollout().clone())); } From b9319f0c16e2be7f7458e61d3c6360ea375f9077 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 3 Mar 2022 15:28:38 +0100 Subject: [PATCH 086/512] no eps for inital time --- ocs2_oc/src/rollout/RolloutBase.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ocs2_oc/src/rollout/RolloutBase.cpp b/ocs2_oc/src/rollout/RolloutBase.cpp index 5ead7dcbb..816b5fa28 100644 --- a/ocs2_oc/src/rollout/RolloutBase.cpp +++ b/ocs2_oc/src/rollout/RolloutBase.cpp @@ -61,10 +61,13 @@ vector_t RolloutBase::run(scalar_t initTime, const vector_t& initState, scalar_t for (int i = 0; i < numSubsystems; i++) { const auto& beginTime = switchingTimes[i]; const auto& endTime = switchingTimes[i + 1]; + timeIntervalArray[i] = std::make_pair(beginTime, endTime); // adjusting the start time to correct for subsystem recognition - constexpr auto eps = numeric_traits::weakEpsilon<scalar_t>(); - timeIntervalArray[i] = std::make_pair(std::min(beginTime + eps, endTime), endTime); + if (i > 0) { + constexpr auto eps = numeric_traits::weakEpsilon<scalar_t>(); + timeIntervalArray[i].first = std::min(beginTime + eps, endTime); + } } // end of for loop return runImpl(timeIntervalArray, initState, controller, timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); From 5e6a82d3429010bb0c4da2f686b859e264210d7f Mon Sep 17 00:00:00 2001 From: FU Zhengyu <zhengfuaj@gmail.com> Date: Thu, 3 Mar 2022 17:53:27 +0100 Subject: [PATCH 087/512] new convention for event time --- .../TrajectorySpreadingTest.cpp | 47 +++++++++++++++++-- 1 file changed, 44 insertions(+), 3 deletions(-) diff --git a/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp b/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp index 5cf46e5d5..4ca91c495 100644 --- a/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp +++ b/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp @@ -128,6 +128,11 @@ class TrajectorySpreadingTest : public testing::Test { /******************************************************************************************************/ ocs2::TrajectorySpreading::Status checkResults(const ocs2::ModeSchedule& modeSchedule, const ocs2::ModeSchedule& updatedModeSchedule, const std::pair<ocs2::scalar_t, ocs2::scalar_t>& period, bool display = true) { + // helper function + auto upperBoundIndex = [](const ocs2::scalar_array_t& timeTrajectory, ocs2::scalar_t queryTime) -> int { + return std::distance(timeTrajectory.begin(), std::upper_bound(timeTrajectory.begin(), timeTrajectory.end(), queryTime)); + }; + // rollout const auto result = rollout(modeSchedule, period); @@ -217,7 +222,7 @@ class TrajectorySpreadingTest : public testing::Test { } auto eventIndexActualItr = spreadResult.postEventsIndeces.begin(); - auto eventTimeReferenceInd = ocs2::lookup::findIndexInTimeArray(updatedModeSchedule.eventTimes, period.first); + auto eventTimeReferenceInd = upperBoundIndex(updatedModeSchedule.eventTimes, period.first); for (size_t k = 0; k < spreadResult.timeTrajectory.size(); k++) { // time should be monotonic sequence if (k > 0) { @@ -236,8 +241,16 @@ class TrajectorySpreadingTest : public testing::Test { eventTimeReferenceInd++; eventIndexActualItr++; } - // mode should match the given modeSchedule - EXPECT_TRUE(updatedModeSchedule.modeAtTime(spreadResult.timeTrajectory[k]) == spreadResult.modeTrajectory[k]); + // Mode should match the given modeSchedule + // The current convention is, the exact event times belong to pre-mode except for the first time point. For the first time point, if + // it is the same as an event-time, then it belongs to the post-event. + int modeAtTime = + k == 0 ? updatedModeSchedule.modeSequence[upperBoundIndex(updatedModeSchedule.eventTimes, spreadResult.timeTrajectory[k])] + : updatedModeSchedule.modeAtTime(spreadResult.timeTrajectory[k]); + EXPECT_TRUE(modeAtTime == spreadResult.modeTrajectory[k]) + << "Time Index: " << k << "\nThe mode should be \033[0;33m(" << modeAtTime << ")\033[0m but it is \033[0;33m(" + << spreadResult.modeTrajectory[k] << ")\033[0m"; + } // end of k loop // test postEventsIndeces @@ -332,6 +345,34 @@ TEST_F(TrajectorySpreadingTest, fully_matched_modes) { EXPECT_TRUE(status.willPerformTrajectorySpreading); } +TEST_F(TrajectorySpreadingTest, init_time_is_the_same_as_event_time) { + const ocs2::scalar_array_t eventTimes{1.1, 1.3}; + const ocs2::size_array_t modeSequence{0, 1, 2}; + + const ocs2::scalar_array_t updatedEventTimes{0.5, 2.1}; + const ocs2::size_array_t updatedModeSequence{0, 1, 2}; + + const std::pair<ocs2::scalar_t, ocs2::scalar_t> period{0.5, 2.5}; + const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period); + + EXPECT_FALSE(status.willTruncate); + EXPECT_TRUE(status.willPerformTrajectorySpreading); +} + +TEST_F(TrajectorySpreadingTest, final_time_is_the_same_as_event_time) { + const ocs2::scalar_array_t eventTimes{1.1, 1.3}; + const ocs2::size_array_t modeSequence{0, 1, 2}; + + const ocs2::scalar_array_t updatedEventTimes{0.5, 2.1}; + const ocs2::size_array_t updatedModeSequence{0, 1, 2}; + + const std::pair<ocs2::scalar_t, ocs2::scalar_t> period{0.2, 2.1}; + const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period); + + EXPECT_FALSE(status.willTruncate); + EXPECT_TRUE(status.willPerformTrajectorySpreading); +} + TEST_F(TrajectorySpreadingTest, out_range_event_to_in_range_at_back_1) { const ocs2::scalar_array_t eventTimes{0.6, 1.7}; const ocs2::size_array_t modeSequence{0, 1, 2}; From 97decd81f8697f2c5f48941fd66a9c009036f215 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 4 Mar 2022 10:46:26 +0100 Subject: [PATCH 088/512] switch to raisim control mode 1 for legged robot --- ocs2_raisim/ocs2_legged_robot_raisim/config/raisim.info | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_raisim/ocs2_legged_robot_raisim/config/raisim.info b/ocs2_raisim/ocs2_legged_robot_raisim/config/raisim.info index b4c9212ea..404503643 100644 --- a/ocs2_raisim/ocs2_legged_robot_raisim/config/raisim.info +++ b/ocs2_raisim/ocs2_legged_robot_raisim/config/raisim.info @@ -30,7 +30,7 @@ rollout [11] RH_KFE } - controlMode 0 ; 0: FORCE_AND_TORQUE, 1: PD_PLUS_FEEDFORWARD_TORQUE + controlMode 1 ; 0: FORCE_AND_TORQUE, 1: PD_PLUS_FEEDFORWARD_TORQUE ; PD control on torque level (if controlMode = 1) pGains From 378fc008a67373bb0c1d6eb8afb5b6ff0a2cef9e Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 4 Mar 2022 15:30:07 +0100 Subject: [PATCH 089/512] adapt to changes from dev/main --- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index 1da459497..0a4731f8b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -2,7 +2,7 @@ #include <ros/package.h> -#include <ocs2_mpc/MPC_DDP.h> +#include <ocs2_ddp/GaussNewtonDDP_MPC.h> #include <ocs2_mpcnet/control/MpcnetOnnxController.h> #include <ocs2_oc/rollout/TimeTriggeredRollout.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> @@ -53,9 +53,9 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr<MPC_BASE> BallbotMpcnetInterface::getMpc(BallbotInterface& ballbotInterface) { - std::unique_ptr<MPC_BASE> mpcPtr(new MPC_DDP(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), - ballbotInterface.getRollout(), ballbotInterface.getOptimalControlProblem(), - ballbotInterface.getInitializer())); + std::unique_ptr<MPC_BASE> mpcPtr(new GaussNewtonDDP_MPC(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), + ballbotInterface.getRollout(), ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer())); mpcPtr->getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); return mpcPtr; } From a9e26490edeeaa0eb2a0cf3a209b10adb19473ae Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 4 Mar 2022 15:30:39 +0100 Subject: [PATCH 090/512] fix drift issue --- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp index e746e0ba0..7459b09b9 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp @@ -4,7 +4,7 @@ namespace ocs2 { namespace ballbot { vector_t BallbotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) { - return t * vector_t::Ones(1); + return vector_t::Zero(1); } vector_t BallbotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { From d6c684f810554b8d1d7b11c626308a4fa4693d3a Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 4 Mar 2022 15:54:32 +0100 Subject: [PATCH 091/512] adapt to changes from dev/amin --- .../src/LeggedRobotMpcnetInterface.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 9939b6dd2..fcb9158b8 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -2,7 +2,7 @@ #include <ros/package.h> -#include <ocs2_mpc/MPC_DDP.h> +#include <ocs2_ddp/GaussNewtonDDP_MPC.h> #include <ocs2_mpcnet/control/MpcnetOnnxController.h> #include <ocs2_oc/rollout/TimeTriggeredRollout.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> @@ -78,9 +78,9 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr<MPC_BASE> LeggedRobotMpcnetInterface::getMpc(LeggedRobotInterface& leggedRobotInterface) { - std::unique_ptr<MPC_BASE> mpcPtr(new MPC_DDP(leggedRobotInterface.mpcSettings(), leggedRobotInterface.ddpSettings(), - leggedRobotInterface.getRollout(), leggedRobotInterface.getOptimalControlProblem(), - leggedRobotInterface.getInitializer())); + std::unique_ptr<MPC_BASE> mpcPtr( + new GaussNewtonDDP_MPC(leggedRobotInterface.mpcSettings(), leggedRobotInterface.ddpSettings(), leggedRobotInterface.getRollout(), + leggedRobotInterface.getOptimalControlProblem(), leggedRobotInterface.getInitializer())); mpcPtr->getSolverPtr()->setReferenceManager(leggedRobotInterface.getReferenceManagerPtr()); return mpcPtr; } From 34cc7e6edf8fc390302c76daaa7a292bacb7f345 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 7 Mar 2022 18:45:59 +0100 Subject: [PATCH 092/512] reduce memory capacity --- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index e3886cb95..821d33e9e 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -48,7 +48,7 @@ loss = Loss() # memory -memory_capacity = 1000000 +memory_capacity = 100000 memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) # policy From 7c52fe79ea2bba34bab41bb24aa2d50ddf0bd5f6 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 7 Mar 2022 18:52:51 +0100 Subject: [PATCH 093/512] normalize loss with batch size --- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 821d33e9e..c54dfa936 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -145,11 +145,11 @@ def closure(): u_predicted = bmv(input_transformation, u_predicted) U_predicted = bmm(input_transformation, U_predicted) # compute the empirical loss - empirical_loss = loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() + empirical_loss = loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size # compute the gradients empirical_loss.backward() # logging - writer.add_scalar('objective/empirical_loss', empirical_loss.item() / batch_size, iteration) + writer.add_scalar('objective/empirical_loss', empirical_loss.item(), iteration) # return empirical loss return empirical_loss optimizer.step(closure) From 475dff084081c68d18f7bc62e3fa35cc0f365468 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 7 Mar 2022 19:22:54 +0100 Subject: [PATCH 094/512] normalize loss with batch size --- .../ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 2ae06686e..8bdeca676 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -167,15 +167,15 @@ def closure(): u_predicted = bmv(input_transformation, u_predicted) U_predicted = bmm(input_transformation, U_predicted) # compute the empirical loss - empirical_experts_loss = experts_loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() - empirical_gating_loss = gating_loss.compute_batch(p, p_predicted).sum() + empirical_experts_loss = experts_loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size + empirical_gating_loss = gating_loss.compute_batch(p, p_predicted).sum() / batch_size empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss # compute the gradients empirical_loss.backward() # logging - writer.add_scalar('objective/empirical_experts_loss', empirical_experts_loss.item() / batch_size, iteration) - writer.add_scalar('objective/empirical_gating_loss', empirical_gating_loss.item() / batch_size, iteration) - writer.add_scalar('objective/empirical_loss', empirical_loss.item() / batch_size, iteration) + writer.add_scalar('objective/empirical_experts_loss', empirical_experts_loss.item(), iteration) + writer.add_scalar('objective/empirical_gating_loss', empirical_gating_loss.item(), iteration) + writer.add_scalar('objective/empirical_loss', empirical_loss.item(), iteration) # return empirical loss return empirical_loss optimizer.step(closure) From e0bfe362e6db23bf6e7c4fbba645c5152fa7d374 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 8 Mar 2022 12:41:48 +0100 Subject: [PATCH 095/512] remove amsgrad for now --- .../python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 8bdeca676..dca51b53b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -84,7 +84,7 @@ learning_rate_expert_nets = learning_rate_default optimizer = torch.optim.Adam([{'params': policy.gating_net.parameters(), 'lr': learning_rate_gating_net}, {'params': policy.expert_nets.parameters(), 'lr': learning_rate_expert_nets}], - lr=learning_rate_default, amsgrad=True) + lr=learning_rate_default) # weights for ["stance", "trot_1", "trot_2"] weights = [1, 2, 2] From cafc40841c932408fde42b01caea26dd6ce4cd9f Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 11 Mar 2022 12:02:24 +0100 Subject: [PATCH 096/512] accounting for the final event at dynamics rollout of GaussNewtonDDP::rolloutInitialTrajectory --- ocs2_ddp/src/GaussNewtonDDP.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 593126795..a33b4dbf7 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -463,7 +463,7 @@ scalar_t GaussNewtonDDP::rolloutInitialTrajectory(PrimalDataContainer& primalDat // Divide the rollout segment in controller rollout and operating points const std::pair<scalar_t, scalar_t> controllerRolloutFromTo{t0, std::max(t0, std::min(controllerAvailableTill, tf))}; - const std::pair<scalar_t, scalar_t> operatingPointsFromTo{controllerRolloutFromTo.second, tf}; + std::pair<scalar_t, scalar_t> operatingPointsFromTo{controllerRolloutFromTo.second, tf}; if (ddpSettings_.debugPrintRollout_) { std::cerr << "[GaussNewtonDDP::rolloutInitialTrajectory] for t = [" << t0 << ", " << tf << "]\n"; @@ -492,6 +492,10 @@ scalar_t GaussNewtonDDP::rolloutInitialTrajectory(PrimalDataContainer& primalDat stateTrajectory.pop_back(); inputTrajectory.pop_back(); // eventsPastTheEndIndeces is not removed because we need to mark the start of the operatingPointTrajectory as being after an event. + + // adjusting the start time to correct for subsystem recognition + constexpr auto eps = numeric_traits::weakEpsilon<scalar_t>(); + operatingPointsFromTo.first = std::min(operatingPointsFromTo.first + eps, operatingPointsFromTo.second); } scalar_array_t timeTrajectoryTail; From 601619779d302b783fb1aaca34e7ef4f2b499de8 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 11 Mar 2022 16:34:29 +0100 Subject: [PATCH 097/512] cleanup ort inference --- .../src/control/MpcnetOnnxController.cpp | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp index 1d0ee87b8..0dbd0114f 100644 --- a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp @@ -31,23 +31,21 @@ void MpcnetOnnxController::loadPolicyModel(const std::string& policyFilePath) { /******************************************************************************************************/ vector_t MpcnetOnnxController::computeInput(const scalar_t t, const vector_t& x) { // create input tensor objects - Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> tEigenData = getGeneralizedTime(t).cast<tensor_element_t>(); - Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> xEigenData = getRelativeState(t, x).cast<tensor_element_t>(); - std::vector<tensor_element_t> tData(tEigenData.data(), tEigenData.data() + tEigenData.size()); - std::vector<tensor_element_t> xData(xEigenData.data(), xEigenData.data() + xEigenData.size()); + Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> time = getGeneralizedTime(t).cast<tensor_element_t>(); + Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> state = getRelativeState(t, x).cast<tensor_element_t>(); Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, OrtMemType::OrtMemTypeDefault); std::vector<Ort::Value> inputValues; inputValues.push_back( - Ort::Value::CreateTensor<tensor_element_t>(memoryInfo, tData.data(), tData.size(), inputShapes_[0].data(), inputShapes_[0].size())); + Ort::Value::CreateTensor<tensor_element_t>(memoryInfo, time.data(), time.size(), inputShapes_[0].data(), inputShapes_[0].size())); inputValues.push_back( - Ort::Value::CreateTensor<tensor_element_t>(memoryInfo, xData.data(), xData.size(), inputShapes_[1].data(), inputShapes_[1].size())); + Ort::Value::CreateTensor<tensor_element_t>(memoryInfo, state.data(), state.size(), inputShapes_[1].data(), inputShapes_[1].size())); // run inference Ort::RunOptions runOptions; - std::vector<Ort::Value> outputValues = sessionPtr_->Run(runOptions, inputNames_.data(), inputValues.data(), 2, outputNames_.data(), 2); - // evaluate output tensor objects (note that from u, p, U we only need u = U * p which is already evaluated by the model) - Eigen::Map<Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1>> u(outputValues[0].GetTensorMutableData<tensor_element_t>(), - outputShapes_[0][1], outputShapes_[0][0]); - return getInputTransformation(t, x) * u.cast<scalar_t>(); + std::vector<Ort::Value> outputValues = sessionPtr_->Run(runOptions, inputNames_.data(), inputValues.data(), 2, outputNames_.data(), 1); + // evaluate output tensor objects + Eigen::Map<Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1>> input(outputValues[0].GetTensorMutableData<tensor_element_t>(), + outputShapes_[0][1], outputShapes_[0][0]); + return getInputTransformation(t, x) * input.cast<scalar_t>(); } } // namespace ocs2 From aad844cd64fc29715b175b9c498e86fa9f2fca51 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 15 Mar 2022 13:20:17 +0100 Subject: [PATCH 098/512] address some comments --- .../ocs2_mpcnet/MpcnetDefinitionBase.h | 13 +++-- .../include/ocs2_mpcnet/MpcnetInterfaceBase.h | 11 ++--- .../include/ocs2_mpcnet/MpcnetPybindMacros.h | 2 +- .../control/MpcnetBehavioralController.h | 11 +---- .../control/MpcnetControllerBase.h | 20 ++++---- .../control/MpcnetOnnxController.h | 12 ++--- .../rollout/MpcnetDataGeneration.h | 33 +++++++------ .../rollout/MpcnetPolicyEvaluation.h | 31 +++++++----- .../control/MpcnetBehavioralController.cpp | 24 +++++----- .../src/rollout/MpcnetDataGeneration.cpp | 47 +++++++++---------- .../src/rollout/MpcnetPolicyEvaluation.cpp | 8 ++-- .../src/rollout/MpcnetRolloutManager.cpp | 32 ++++++------- 12 files changed, 125 insertions(+), 119 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h index 8119b4ef0..eff9249aa 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h @@ -11,14 +11,19 @@ namespace ocs2 { class MpcnetDefinitionBase { public: /** - * Default constructor. + * Default destructor. */ - MpcnetDefinitionBase() = default; + virtual ~MpcnetDefinitionBase() = default; /** - * Default destructor. + * Deleted copy constructor. */ - virtual ~MpcnetDefinitionBase() = default; + MpcnetDefinitionBase(const MpcnetDefinitionBase&) = delete; + + /** + * Deleted copy assignment. + */ + MpcnetDefinitionBase& operator=(const MpcnetDefinitionBase&) = delete; /** * Get the generalized time. diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h index 8190daa87..54ccac543 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h @@ -18,12 +18,6 @@ class MpcnetInterfaceBase { using metrics_array_t = MpcnetRolloutManager::metrics_array_t; using metrics_ptr_t = MpcnetRolloutManager::metrics_ptr_t; - protected: - /** - * Default constructor. - */ - MpcnetInterfaceBase() = default; - public: /** * Default destructor. @@ -65,6 +59,11 @@ class MpcnetInterfaceBase { metrics_array_t getComputedMetrics(); protected: + /** + * Default constructor. + */ + MpcnetInterfaceBase() = default; + std::unique_ptr<MpcnetRolloutManager> mpcnetRolloutManagerPtr_; }; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index 15a158b67..2d56ee64b 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -64,10 +64,10 @@ using namespace pybind11::literals; /* bind data point struct */ \ pybind11::class_<MPCNET_INTERFACE::data_point_t>(m, "DataPoint") \ .def(pybind11::init<>()) \ + .def_readwrite("mode", &MPCNET_INTERFACE::data_point_t::mode) \ .def_readwrite("t", &MPCNET_INTERFACE::data_point_t::t) \ .def_readwrite("x", &MPCNET_INTERFACE::data_point_t::x) \ .def_readwrite("u", &MPCNET_INTERFACE::data_point_t::u) \ - .def_readwrite("mode", &MPCNET_INTERFACE::data_point_t::mode) \ .def_readwrite("generalized_time", &MPCNET_INTERFACE::data_point_t::generalizedTime) \ .def_readwrite("relative_state", &MPCNET_INTERFACE::data_point_t::relativeState) \ .def_readwrite("input_transformation", &MPCNET_INTERFACE::data_point_t::inputTransformation) \ diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h index 7d305ce20..54fa469c5 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -16,20 +16,13 @@ namespace ocs2 { class MpcnetBehavioralController final : public ControllerBase { public: /** - * Default constructor, leaves object uninitialized. - */ - MpcnetBehavioralController() = default; - - /** - * Constructor, initializes all required members of the controller. + * Constructor. * @param [in] alpha : The mixture parameter. * @param [in] optimalController : The optimal controller (this class takes ownership of a clone). * @param [in] learnedController : The learned controller (this class takes ownership of a clone). */ MpcnetBehavioralController(scalar_t alpha, const ControllerBase& optimalController, const MpcnetControllerBase& learnedController) - : alpha_(alpha), - optimalControllerPtr_(std::unique_ptr<ControllerBase>(optimalController.clone())), - learnedControllerPtr_(std::unique_ptr<MpcnetControllerBase>(learnedController.clone())) {} + : alpha_(alpha), optimalControllerPtr_(optimalController.clone()), learnedControllerPtr_(learnedController.clone()) {} /** * Default destructor. diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h index a1f7364d7..6006a01b1 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h @@ -34,6 +34,16 @@ class MpcnetControllerBase : public ControllerBase { */ virtual void loadPolicyModel(const std::string& policyFilePath) = 0; + ControllerType getType() const override { return ControllerType::MPCNET; } + + MpcnetControllerBase* clone() const override = 0; + + protected: + /** + * Copy constructor. + */ + MpcnetControllerBase(const MpcnetControllerBase& other) : MpcnetControllerBase(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_) {} + /** * Get the generalized time. * @param [in] t : Absolute time. @@ -59,16 +69,6 @@ class MpcnetControllerBase : public ControllerBase { */ matrix_t getInputTransformation(scalar_t t, const vector_t& x) { return mpcnetDefinitionPtr_->getInputTransformation(t, x); } - ControllerType getType() const override { return ControllerType::MPCNET; } - - MpcnetControllerBase* clone() const override = 0; - - protected: - /** - * Copy constructor. - */ - MpcnetControllerBase(const MpcnetControllerBase& other) : MpcnetControllerBase(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_) {} - std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; }; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index 871699a46..c751e5ac5 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -27,8 +27,6 @@ inline std::shared_ptr<Ort::Env> createOnnxEnvironment() { */ class MpcnetOnnxController final : public MpcnetControllerBase { public: - using tensor_element_t = float; - /** * Constructor, does not load the model of the policy. * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions. @@ -63,18 +61,20 @@ class MpcnetOnnxController final : public MpcnetControllerBase { vector_t computeInput(const scalar_t t, const vector_t& x) override; void concatenate(const ControllerBase* otherController, int index, int length) override { - throw std::runtime_error("MpcnetOnnxController::concatenate not implemented."); + throw std::runtime_error("[MpcnetOnnxController::concatenate] not implemented."); } - int size() const override { throw std::runtime_error("MpcnetOnnxController::size not implemented."); } + int size() const override { throw std::runtime_error("[MpcnetOnnxController::size] not implemented."); } - void clear() override { throw std::runtime_error("MpcnetOnnxController::clear not implemented."); } + void clear() override { throw std::runtime_error("[MpcnetOnnxController::clear] not implemented."); } - bool empty() const override { throw std::runtime_error("MpcnetOnnxController::empty not implemented."); } + bool empty() const override { throw std::runtime_error("[MpcnetOnnxController::empty] not implemented."); } MpcnetOnnxController* clone() const override { return new MpcnetOnnxController(*this); } private: + using tensor_element_t = float; + /** * Copy constructor. */ diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index f1a536ca6..a4722aa91 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -18,17 +18,11 @@ namespace ocs2 { */ class MpcnetDataGeneration { public: - using mpc_t = MPC_BASE; - using mpcnet_t = MpcnetControllerBase; - using rollout_t = RolloutBase; - using mpcnet_definition_t = MpcnetDefinitionBase; - using reference_manager_t = ReferenceManagerInterface; - struct DataPoint { + size_t mode; scalar_t t; vector_t x; vector_t u; - size_t mode; vector_t generalizedTime; vector_t relativeState; matrix_t inputTransformation; @@ -45,8 +39,9 @@ class MpcnetDataGeneration { * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions to be used (shared ownership). * @param [in] referenceManagerPtr : Pointer to the reference manager to be used (shared ownership). */ - MpcnetDataGeneration(std::unique_ptr<mpc_t> mpcPtr, std::unique_ptr<mpcnet_t> mpcnetPtr, std::unique_ptr<rollout_t> rolloutPtr, - std::shared_ptr<mpcnet_definition_t> mpcnetDefinitionPtr, std::shared_ptr<reference_manager_t> referenceManagerPtr) + MpcnetDataGeneration(std::unique_ptr<MPC_BASE> mpcPtr, std::unique_ptr<MpcnetControllerBase> mpcnetPtr, + std::unique_ptr<RolloutBase> rolloutPtr, std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, + std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr) : mpcPtr_(std::move(mpcPtr)), mpcnetPtr_(std::move(mpcnetPtr)), rolloutPtr_(std::move(rolloutPtr)), @@ -58,6 +53,16 @@ class MpcnetDataGeneration { */ virtual ~MpcnetDataGeneration() = default; + /** + * Deleted copy constructor. + */ + MpcnetDataGeneration(const MpcnetDataGeneration&) = delete; + + /** + * Deleted copy assignment. + */ + MpcnetDataGeneration& operator=(const MpcnetDataGeneration&) = delete; + /** * Run the data generation. * @param [in] alpha : The mixture parameter for the behavioral controller. @@ -76,11 +81,11 @@ class MpcnetDataGeneration { const TargetTrajectories& targetTrajectories); private: - std::unique_ptr<mpc_t> mpcPtr_; - std::unique_ptr<mpcnet_t> mpcnetPtr_; - std::unique_ptr<rollout_t> rolloutPtr_; - std::shared_ptr<mpcnet_definition_t> mpcnetDefinitionPtr_; - std::shared_ptr<reference_manager_t> referenceManagerPtr_; + std::unique_ptr<MPC_BASE> mpcPtr_; + std::unique_ptr<MpcnetControllerBase> mpcnetPtr_; + std::unique_ptr<RolloutBase> rolloutPtr_; + std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; + std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; }; } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h index ca4751843..5a14d3ebf 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -17,12 +17,6 @@ namespace ocs2 { */ class MpcnetPolicyEvaluation { public: - using mpc_t = MPC_BASE; - using mpcnet_t = MpcnetControllerBase; - using rollout_t = RolloutBase; - using mpcnet_definition_t = MpcnetDefinitionBase; - using reference_manager_t = ReferenceManagerInterface; - struct Metrics { scalar_t survivalTime = 0.0; scalar_t incurredHamiltonian = 0.0; @@ -38,8 +32,9 @@ class MpcnetPolicyEvaluation { * @param [in] mpcnetDefinitionPtr: Pointer to the MPC-Net definitions to be used (shared ownership). * @param [in] referenceManagerPtr: Pointer to the reference manager to be used (shared ownership). */ - MpcnetPolicyEvaluation(std::unique_ptr<mpc_t> mpcPtr, std::unique_ptr<mpcnet_t> mpcnetPtr, std::unique_ptr<rollout_t> rolloutPtr, - std::shared_ptr<mpcnet_definition_t> mpcnetDefinitionPtr, std::shared_ptr<reference_manager_t> referenceManagerPtr) + MpcnetPolicyEvaluation(std::unique_ptr<MPC_BASE> mpcPtr, std::unique_ptr<MpcnetControllerBase> mpcnetPtr, + std::unique_ptr<RolloutBase> rolloutPtr, std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, + std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr) : mpcPtr_(std::move(mpcPtr)), mpcnetPtr_(std::move(mpcnetPtr)), rolloutPtr_(std::move(rolloutPtr)), @@ -51,6 +46,16 @@ class MpcnetPolicyEvaluation { */ virtual ~MpcnetPolicyEvaluation() = default; + /** + * Deleted copy constructor. + */ + MpcnetPolicyEvaluation(const MpcnetPolicyEvaluation&) = delete; + + /** + * Deleted copy assignment. + */ + MpcnetPolicyEvaluation& operator=(const MpcnetPolicyEvaluation&) = delete; + /** * Run the policy evaluation. * @param [in] policyFilePath : The path to the file with the learned policy for the controller. @@ -64,11 +69,11 @@ class MpcnetPolicyEvaluation { const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); private: - std::unique_ptr<mpc_t> mpcPtr_; - std::unique_ptr<mpcnet_t> mpcnetPtr_; - std::unique_ptr<rollout_t> rolloutPtr_; - std::shared_ptr<mpcnet_definition_t> mpcnetDefinitionPtr_; - std::shared_ptr<reference_manager_t> referenceManagerPtr_; + std::unique_ptr<MPC_BASE> mpcPtr_; + std::unique_ptr<MpcnetControllerBase> mpcnetPtr_; + std::unique_ptr<RolloutBase> rolloutPtr_; + std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; + std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; }; } // namespace ocs2 diff --git a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp index bd066edb3..c98834e9f 100644 --- a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp @@ -6,11 +6,11 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ vector_t MpcnetBehavioralController::computeInput(scalar_t t, const vector_t& x) { - if (optimalControllerPtr_ && learnedControllerPtr_) { + if (optimalControllerPtr_ != nullptr && learnedControllerPtr_ != nullptr) { return alpha_ * optimalControllerPtr_->computeInput(t, x) + (1 - alpha_) * learnedControllerPtr_->computeInput(t, x); } else { throw std::runtime_error( - "MpcnetBehavioralController::computeInput cannot return input, since optimal and/or learned controller not set."); + "[MpcnetBehavioralController::computeInput] cannot return input, since optimal and/or learned controller not set."); } } @@ -18,10 +18,10 @@ vector_t MpcnetBehavioralController::computeInput(scalar_t t, const vector_t& x) /******************************************************************************************************/ /******************************************************************************************************/ void MpcnetBehavioralController::concatenate(const ControllerBase* otherController, int index, int length) { - if (optimalControllerPtr_) { + if (optimalControllerPtr_ != nullptr) { optimalControllerPtr_->concatenate(otherController, index, length); } - if (learnedControllerPtr_) { + if (learnedControllerPtr_ != nullptr) { learnedControllerPtr_->concatenate(otherController, index, length); } } @@ -30,11 +30,11 @@ void MpcnetBehavioralController::concatenate(const ControllerBase* otherControll /******************************************************************************************************/ /******************************************************************************************************/ int MpcnetBehavioralController::size() const { - if (optimalControllerPtr_ && learnedControllerPtr_) { + if (optimalControllerPtr_ != nullptr && learnedControllerPtr_ != nullptr) { return std::max(optimalControllerPtr_->size(), learnedControllerPtr_->size()); - } else if (optimalControllerPtr_) { + } else if (optimalControllerPtr_ != nullptr) { return optimalControllerPtr_->size(); - } else if (learnedControllerPtr_) { + } else if (learnedControllerPtr_ != nullptr) { return learnedControllerPtr_->size(); } else { return 0; @@ -45,10 +45,10 @@ int MpcnetBehavioralController::size() const { /******************************************************************************************************/ /******************************************************************************************************/ void MpcnetBehavioralController::clear() { - if (optimalControllerPtr_) { + if (optimalControllerPtr_ != nullptr) { optimalControllerPtr_->clear(); } - if (learnedControllerPtr_) { + if (learnedControllerPtr_ != nullptr) { learnedControllerPtr_->clear(); } } @@ -57,11 +57,11 @@ void MpcnetBehavioralController::clear() { /******************************************************************************************************/ /******************************************************************************************************/ bool MpcnetBehavioralController::empty() const { - if (optimalControllerPtr_ && learnedControllerPtr_) { + if (optimalControllerPtr_ != nullptr && learnedControllerPtr_ != nullptr) { return optimalControllerPtr_->empty() && learnedControllerPtr_->empty(); - } else if (optimalControllerPtr_) { + } else if (optimalControllerPtr_ != nullptr) { return optimalControllerPtr_->empty(); - } else if (learnedControllerPtr_) { + } else if (learnedControllerPtr_ != nullptr) { return learnedControllerPtr_->empty(); } else { return true; diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index bfedb3b71..c38db26fa 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -34,19 +34,16 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st referenceManagerPtr_->setTargetTrajectories(targetTrajectories); // set up behavioral controller with mixture parameter alpha and learned controller - MpcnetBehavioralController behavioralController; - behavioralController.setAlpha(alpha); - behavioralController.setLearnedController(*mpcnetPtr_); + std::unique_ptr<MpcnetBehavioralController> behavioralControllerPtr; + behavioralControllerPtr->setAlpha(alpha); + behavioralControllerPtr->setLearnedController(*mpcnetPtr_); // set up scalar standard normal generator and compute Cholesky decomposition of covariance matrix std::random_device randomDevice; std::default_random_engine pseudoRandomNumberGenerator(randomDevice()); std::normal_distribution<scalar_t> standardNormalDistribution(scalar_t(0.0), scalar_t(1.0)); - std::function<scalar_t(scalar_t)> standardNormalNullaryOp = [&](scalar_t) -> scalar_t { - return standardNormalDistribution(pseudoRandomNumberGenerator); - }; - matrix_t S = samplingCovariance; - matrix_t L = S.llt().matrixL(); + auto standardNormalNullaryOp = [&](scalar_t) -> scalar_t { return standardNormalDistribution(pseudoRandomNumberGenerator); }; + const matrix_t L = samplingCovariance.llt().matrixL(); // run data generation int iteration = 0; @@ -54,9 +51,9 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st while (time <= targetTrajectories.timeTrajectory.back()) { // run mpc and get solution if (!mpcPtr_->run(time, state)) { - throw std::runtime_error("MpcnetDataGeneration::run Main routine of MPC returned false."); + throw std::runtime_error("[MpcnetDataGeneration::run] main routine of MPC returned false."); } - PrimalSolution primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); + const auto primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); // downsample the data signal by an integer factor if (iteration % dataDecimation == 0) { @@ -67,9 +64,10 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st dataPoint.x = primalSolution.stateTrajectory_.front(); dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); - dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); - dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); - dataPoint.inputTransformation = mpcnetPtr_->getInputTransformation(dataPoint.t, dataPoint.x); + dataPoint.generalizedTime = mpcnetDefinitionPtr_->getGeneralizedTime(dataPoint.t, referenceManagerPtr_->getModeSchedule()); + dataPoint.relativeState = + mpcnetDefinitionPtr_->getRelativeState(dataPoint.t, dataPoint.x, referenceManagerPtr_->getTargetTrajectories()); + dataPoint.inputTransformation = mpcnetDefinitionPtr_->getInputTransformation(dataPoint.t, dataPoint.x); dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); dataPtr->push_back(std::move(dataPoint)); } @@ -78,20 +76,21 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st for (int i = 0; i < nSamples; i++) { DataPoint dataPoint; dataPoint.t = primalSolution.timeTrajectory_.front(); - dataPoint.x = primalSolution.stateTrajectory_.front() + - L * vector_t::NullaryExpr(primalSolution.stateTrajectory_.front().size(), standardNormalNullaryOp); + dataPoint.x = primalSolution.stateTrajectory_.front(); + dataPoint.x.noalias() += L * vector_t::NullaryExpr(primalSolution.stateTrajectory_.front().size(), standardNormalNullaryOp); dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); - dataPoint.generalizedTime = mpcnetPtr_->getGeneralizedTime(dataPoint.t); - dataPoint.relativeState = mpcnetPtr_->getRelativeState(dataPoint.t, dataPoint.x); - dataPoint.inputTransformation = mpcnetPtr_->getInputTransformation(dataPoint.t, dataPoint.x); + dataPoint.generalizedTime = mpcnetDefinitionPtr_->getGeneralizedTime(dataPoint.t, referenceManagerPtr_->getModeSchedule()); + dataPoint.relativeState = + mpcnetDefinitionPtr_->getRelativeState(dataPoint.t, dataPoint.x, referenceManagerPtr_->getTargetTrajectories()); + dataPoint.inputTransformation = mpcnetDefinitionPtr_->getInputTransformation(dataPoint.t, dataPoint.x); dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); dataPtr->push_back(std::move(dataPoint)); } } // update behavioral controller with MPC controller - behavioralController.setOptimalController(*primalSolution.controllerPtr_); + behavioralControllerPtr->setOptimalController(*primalSolution.controllerPtr_); // forward simulate system with behavioral controller scalar_array_t timeTrajectory; @@ -99,22 +98,22 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st vector_array_t stateTrajectory; vector_array_t inputTrajectory; rolloutPtr_->run(primalSolution.timeTrajectory_.front(), primalSolution.stateTrajectory_.front(), - primalSolution.timeTrajectory_.front() + timeStep, &behavioralController, primalSolution.modeSchedule_.eventTimes, - timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); + primalSolution.timeTrajectory_.front() + timeStep, behavioralControllerPtr.get(), + primalSolution.modeSchedule_.eventTimes, timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); // update time, state and iteration time = timeTrajectory.back(); state = stateTrajectory.back(); - iteration++; + ++iteration; // check if forward simulated system diverged if (!mpcnetDefinitionPtr_->validState(state)) { - throw std::runtime_error("MpcnetDataGeneration::run State is not valid."); + throw std::runtime_error("[MpcnetDataGeneration::run] state is not valid."); } } } catch (const std::exception& e) { // print error for exceptions - std::cerr << "MpcnetDataGeneration::run A standard exception was caught, with message: " << e.what() << std::endl; + std::cerr << "[MpcnetDataGeneration::run] a standard exception was caught, with message: " << e.what() << "\n"; // this data generation run failed, clear data dataPtr->clear(); } diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index 9bb9fb62c..4723e8064 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -35,7 +35,7 @@ MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string while (time <= targetTrajectories.timeTrajectory.back()) { // run mpc and get solution if (!mpcPtr_->run(time, state)) { - throw std::runtime_error("MpcnetPolicyEvaluation::run Main routine of MPC returned false."); + throw std::runtime_error("[MpcnetPolicyEvaluation::run] main routine of MPC returned false."); } PrimalSolution primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); @@ -55,16 +55,16 @@ MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string // update time, state and iteration time = timeTrajectory.back(); state = stateTrajectory.back(); - iteration++; + ++iteration; // check if forward simulated system diverged if (!mpcnetDefinitionPtr_->validState(state)) { - throw std::runtime_error("MpcnetPolicyEvaluation::run State is not valid."); + throw std::runtime_error("[MpcnetPolicyEvaluation::run] state is not valid."); } } } catch (const std::exception& e) { // print error for exceptions - std::cerr << "MpcnetPolicyEvaluation::run A standard exception was caught, with message: " << e.what() << std::endl; + std::cerr << "[MpcnetPolicyEvaluation::run] a standard exception was caught, with message: " << e.what() << "\n"; // this policy evaluation run failed, incurred quantities are not reported metricsPtr->incurredHamiltonian = std::numeric_limits<scalar_t>::quiet_NaN(); } diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp index e449746a3..79e5b43e8 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp @@ -45,7 +45,7 @@ void MpcnetRolloutManager::startDataGeneration(scalar_t alpha, const std::string const std::vector<ModeSchedule>& modeSchedules, const std::vector<TargetTrajectories>& targetTrajectories) { if (nDataGenerationThreads_ <= 0) { - throw std::runtime_error("MpcnetRolloutManager::startDataGeneration cannot work without at least one data generation thread."); + throw std::runtime_error("[MpcnetRolloutManager::startDataGeneration] cannot work without at least one data generation thread."); } // reset variables @@ -60,7 +60,7 @@ void MpcnetRolloutManager::startDataGeneration(scalar_t alpha, const std::string initialObservations.at(i), modeSchedules.at(i), targetTrajectories.at(i)); nDataGenerationTasksDone_++; // print thread and task number - std::cerr << "Data generation thread " << threadNumber << " finished task " << nDataGenerationTasksDone_ << std::endl; + std::cerr << "Data generation thread " << threadNumber << " finished task " << nDataGenerationTasksDone_ << "\n"; return result; })); } @@ -71,11 +71,11 @@ void MpcnetRolloutManager::startDataGeneration(scalar_t alpha, const std::string /******************************************************************************************************/ bool MpcnetRolloutManager::isDataGenerationDone() { if (nDataGenerationThreads_ <= 0) { - throw std::runtime_error("MpcnetRolloutManager::isDataGenerationDone cannot work without at least one data generation thread."); + throw std::runtime_error("[MpcnetRolloutManager::isDataGenerationDone] cannot work without at least one data generation thread."); } if (dataGenerationFtrs_.size() <= 0) { throw std::runtime_error( - "MpcnetRolloutManager::isDataGenerationDone cannot return if startDataGeneration has not been triggered once."); + "[MpcnetRolloutManager::isDataGenerationDone] cannot return if startDataGeneration has not been triggered once."); } // check if done @@ -84,7 +84,7 @@ bool MpcnetRolloutManager::isDataGenerationDone() { } else if (nDataGenerationTasksDone_ == dataGenerationFtrs_.size()) { return true; } else { - throw std::runtime_error("MpcnetRolloutManager::isDataGenerationDone error since more tasks done than futures available."); + throw std::runtime_error("[MpcnetRolloutManager::isDataGenerationDone] error since more tasks done than futures available."); } } @@ -93,10 +93,10 @@ bool MpcnetRolloutManager::isDataGenerationDone() { /******************************************************************************************************/ MpcnetRolloutManager::data_array_t MpcnetRolloutManager::getGeneratedData() { if (nDataGenerationThreads_ <= 0) { - throw std::runtime_error("MpcnetRolloutManager::getGeneratedData cannot work without at least one data generation thread."); + throw std::runtime_error("[MpcnetRolloutManager::getGeneratedData] cannot work without at least one data generation thread."); } if (!isDataGenerationDone()) { - throw std::runtime_error("MpcnetRolloutManager::getGeneratedData cannot get data when data generation is not done."); + throw std::runtime_error("[MpcnetRolloutManager::getGeneratedData] cannot get data when data generation is not done."); } // get pointers to data @@ -107,7 +107,7 @@ MpcnetRolloutManager::data_array_t MpcnetRolloutManager::getGeneratedData() { dataPtrs.push_back(dataGenerationFtrs_[i].get()); } catch (const std::exception& e) { // print error for exceptions - std::cerr << "MpcnetRolloutManager::getGeneratedData A standard exception was caught, with message: " << e.what() << std::endl; + std::cerr << "[MpcnetRolloutManager::getGeneratedData] a standard exception was caught, with message: " << e.what() << "\n"; } } @@ -138,7 +138,7 @@ void MpcnetRolloutManager::startPolicyEvaluation(const std::string& policyFilePa const std::vector<ModeSchedule>& modeSchedules, const std::vector<TargetTrajectories>& targetTrajectories) { if (nPolicyEvaluationThreads_ <= 0) { - throw std::runtime_error("MpcnetRolloutManager::startPolicyEvaluation cannot work without at least one policy evaluation thread."); + throw std::runtime_error("[MpcnetRolloutManager::startPolicyEvaluation] cannot work without at least one policy evaluation thread."); } // reset variables @@ -153,7 +153,7 @@ void MpcnetRolloutManager::startPolicyEvaluation(const std::string& policyFilePa targetTrajectories.at(i)); nPolicyEvaluationTasksDone_++; // print thread and task number - std::cerr << "Policy evaluation thread " << threadNumber << " finished task " << nPolicyEvaluationTasksDone_ << std::endl; + std::cerr << "Policy evaluation thread " << threadNumber << " finished task " << nPolicyEvaluationTasksDone_ << "\n"; return result; })); } @@ -164,11 +164,11 @@ void MpcnetRolloutManager::startPolicyEvaluation(const std::string& policyFilePa /******************************************************************************************************/ bool MpcnetRolloutManager::isPolicyEvaluationDone() { if (nPolicyEvaluationThreads_ <= 0) { - throw std::runtime_error("MpcnetRolloutManager::isPolicyEvaluationDone cannot work without at least one policy evaluation thread."); + throw std::runtime_error("[MpcnetRolloutManager::isPolicyEvaluationDone] cannot work without at least one policy evaluation thread."); } if (policyEvaluationFtrs_.size() <= 0) { throw std::runtime_error( - "MpcnetRolloutManager::isPolicyEvaluationDone cannot return if startPolicyEvaluation has not been triggered once."); + "[MpcnetRolloutManager::isPolicyEvaluationDone] cannot return if startPolicyEvaluation has not been triggered once."); } // check if done @@ -177,7 +177,7 @@ bool MpcnetRolloutManager::isPolicyEvaluationDone() { } else if (nPolicyEvaluationTasksDone_ == policyEvaluationFtrs_.size()) { return true; } else { - throw std::runtime_error("MpcnetRolloutManager::isPolicyEvaluationDone error since more tasks done than futures available."); + throw std::runtime_error("[MpcnetRolloutManager::isPolicyEvaluationDone] error since more tasks done than futures available."); } } @@ -186,10 +186,10 @@ bool MpcnetRolloutManager::isPolicyEvaluationDone() { /******************************************************************************************************/ MpcnetRolloutManager::metrics_array_t MpcnetRolloutManager::getComputedMetrics() { if (nPolicyEvaluationThreads_ <= 0) { - throw std::runtime_error("MpcnetRolloutManager::getComputedMetrics cannot work without at least one policy evaluation thread."); + throw std::runtime_error("[MpcnetRolloutManager::getComputedMetrics] cannot work without at least one policy evaluation thread."); } if (!isPolicyEvaluationDone()) { - throw std::runtime_error("MpcnetRolloutManager::getComputedMetrics cannot get metrics when policy evaluation is not done."); + throw std::runtime_error("[MpcnetRolloutManager::getComputedMetrics] cannot get metrics when policy evaluation is not done."); } // get pointers to metrics @@ -200,7 +200,7 @@ MpcnetRolloutManager::metrics_array_t MpcnetRolloutManager::getComputedMetrics() metricsPtrs.push_back(policyEvaluationFtrs_[i].get()); } catch (const std::exception& e) { // print error for exceptions - std::cerr << "MpcnetRolloutManager::getComputedMetrics A standard exception was caught, with message: " << e.what() << std::endl; + std::cerr << "[MpcnetRolloutManager::getComputedMetrics] a standard exception was caught, with message: " << e.what() << "\n"; } } From 60f29973724e87ebbeefec07ed77160f617f461a Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 15 Mar 2022 13:46:33 +0100 Subject: [PATCH 099/512] add license to ocs2_mpcnet --- .../ocs2_mpcnet/MpcnetDefinitionBase.h | 29 ++++++++++++++++++ .../include/ocs2_mpcnet/MpcnetInterfaceBase.h | 29 ++++++++++++++++++ .../include/ocs2_mpcnet/MpcnetPybindMacros.h | 29 ++++++++++++++++++ .../control/MpcnetBehavioralController.h | 29 ++++++++++++++++++ .../control/MpcnetControllerBase.h | 29 ++++++++++++++++++ .../control/MpcnetOnnxController.h | 29 ++++++++++++++++++ .../ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h | 29 ++++++++++++++++++ .../dummy/MpcnetDummyObserverRos.h | 29 ++++++++++++++++++ .../rollout/MpcnetDataGeneration.h | 29 ++++++++++++++++++ .../rollout/MpcnetPolicyEvaluation.h | 29 ++++++++++++++++++ .../rollout/MpcnetRolloutManager.h | 29 ++++++++++++++++++ ocs2_mpcnet/python/ocs2_mpcnet/config.py | 29 ++++++++++++++++++ ocs2_mpcnet/python/ocs2_mpcnet/helper.py | 29 ++++++++++++++++++ ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 29 ++++++++++++++++++ ocs2_mpcnet/python/ocs2_mpcnet/memory.py | 30 +++++++++++++++++++ ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 29 ++++++++++++++++++ ocs2_mpcnet/src/MpcnetInterfaceBase.cpp | 29 ++++++++++++++++++ ocs2_mpcnet/src/MpcnetPybindings.cpp | 29 ++++++++++++++++++ .../control/MpcnetBehavioralController.cpp | 29 ++++++++++++++++++ .../src/control/MpcnetOnnxController.cpp | 29 ++++++++++++++++++ ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp | 29 ++++++++++++++++++ .../src/dummy/MpcnetDummyObserverRos.cpp | 29 ++++++++++++++++++ .../src/rollout/MpcnetDataGeneration.cpp | 29 ++++++++++++++++++ .../src/rollout/MpcnetPolicyEvaluation.cpp | 29 ++++++++++++++++++ .../src/rollout/MpcnetRolloutManager.cpp | 29 ++++++++++++++++++ 25 files changed, 726 insertions(+) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h index eff9249aa..33a78ff89 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_core/reference/ModeSchedule.h> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h index 54ccac543..e084af594 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_core/thread_support/ThreadPool.h> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index 2d56ee64b..9a20f98b2 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <pybind11/eigen.h> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h index 54fa469c5..00c1dd476 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <memory> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h index 6006a01b1..6ea188472 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <memory> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index c751e5ac5..fc8d3be99 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <onnxruntime/onnxruntime_cxx_api.h> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h index fd4e721f4..c0d0da475 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_mpc/SystemObservation.h> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h index 345ad4403..0e7239113 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ros/ros.h> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index a4722aa91..e586c935f 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_core/reference/ModeSchedule.h> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h index 5a14d3ebf..a8750b2e3 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_core/reference/ModeSchedule.h> diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h index 1cf4b9fe9..aa53292ab 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_core/thread_support/ThreadPool.h> diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/config.py b/ocs2_mpcnet/python/ocs2_mpcnet/config.py index 6f925d44b..e1c946e12 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/config.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/config.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import torch # data type for tensor elements diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py index 7ff833ce2..c58d74bbf 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import torch import numpy as np diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index ca3c1f207..64e47781b 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import torch from ocs2_mpcnet.helper import bdot, bmv diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py index 29b08b943..5263db760 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py @@ -1,4 +1,34 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import torch + from ocs2_mpcnet import config diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index b10aeef56..36f6778cf 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import torch from ocs2_mpcnet import config diff --git a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp index e4af5e686..fb32c6956 100644 --- a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp +++ b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_mpcnet/MpcnetInterfaceBase.h" namespace ocs2 { diff --git a/ocs2_mpcnet/src/MpcnetPybindings.cpp b/ocs2_mpcnet/src/MpcnetPybindings.cpp index 4fe2bddf0..8180c82b1 100644 --- a/ocs2_mpcnet/src/MpcnetPybindings.cpp +++ b/ocs2_mpcnet/src/MpcnetPybindings.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <ocs2_mpcnet/MpcnetPybindMacros.h> #include "ocs2_mpcnet/MpcnetInterfaceBase.h" diff --git a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp index c98834e9f..db4b53d31 100644 --- a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_mpcnet/control/MpcnetBehavioralController.h" namespace ocs2 { diff --git a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp index 0dbd0114f..ec3f536af 100644 --- a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_mpcnet/control/MpcnetOnnxController.h" namespace ocs2 { diff --git a/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp b/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp index be82d7679..8e1c9c593 100644 --- a/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp +++ b/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h" #include <ros/ros.h> diff --git a/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp b/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp index dcd544a6a..516573e15 100644 --- a/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp +++ b/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h" #include <ros/ros.h> diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index c38db26fa..9c72463d7 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_mpcnet/rollout/MpcnetDataGeneration.h" #include <random> diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index 4723e8064..d83ee72f6 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h" namespace ocs2 { diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp index 79e5b43e8..1e638d321 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_mpcnet/rollout/MpcnetRolloutManager.h" namespace ocs2 { From c179e1b70a82342dfa6660d93b22b4f1ae9a799e Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 15 Mar 2022 17:21:36 +0100 Subject: [PATCH 100/512] small fix --- ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index d83ee72f6..5d48d2733 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -66,7 +66,7 @@ MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string if (!mpcPtr_->run(time, state)) { throw std::runtime_error("[MpcnetPolicyEvaluation::run] main routine of MPC returned false."); } - PrimalSolution primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); + const auto primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); // incurred quantities vector_t input = mpcnetPtr_->computeInput(time, state); From 8fd784c444c9bcc3aa6a37652175780d85db848b Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 15 Mar 2022 17:44:39 +0100 Subject: [PATCH 101/512] add license to ocs2_ballbot_mpcnet --- .../BallbotMpcnetDefinition.h | 29 +++++++++++++++++++ .../BallbotMpcnetInterface.h | 29 +++++++++++++++++++ .../ocs2_ballbot_mpcnet/ballbot_config.py | 29 +++++++++++++++++++ .../ocs2_ballbot_mpcnet/ballbot_helper.py | 29 +++++++++++++++++++ .../ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 29 +++++++++++++++++++ .../src/BallbotMpcnetDefinition.cpp | 29 +++++++++++++++++++ .../src/BallbotMpcnetDummyNode.cpp | 29 +++++++++++++++++++ .../src/BallbotMpcnetInterface.cpp | 29 +++++++++++++++++++ .../src/BallbotMpcnetPybindings.cpp | 29 +++++++++++++++++++ 9 files changed, 261 insertions(+) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h index 21bb01b72..23947b470 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_mpcnet/MpcnetDefinitionBase.h> diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h index 26be26212..f9e959f15 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_ballbot/BallbotInterface.h> diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py index a9926d59f..7ffcfc489 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + from ocs2_mpcnet import config # diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py index cefbbb998..d2fba8da3 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import numpy as np from ocs2_mpcnet import helper diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index c54dfa936..b80155644 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import os import time import datetime diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp index 7459b09b9..656335f7e 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h" namespace ocs2 { diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp index c51b8af46..3edef99ae 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <ros/package.h> #include <ros/ros.h> #include <urdf_parser/urdf_parser.h> diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index 0a4731f8b..6cc21d6df 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h" #include <ros/package.h> diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp index 567af03a7..9508b85f4 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <ocs2_mpcnet/MpcnetPybindMacros.h> #include "ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h" From 6676677af23ad4969432286c80229856fff8fd57 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 15 Mar 2022 17:48:34 +0100 Subject: [PATCH 102/512] small fix --- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp index 3edef99ae..d455ff5c6 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp @@ -27,9 +27,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ +#include <ros/init.h> #include <ros/package.h> -#include <ros/ros.h> -#include <urdf_parser/urdf_parser.h> #include <ocs2_ballbot/BallbotInterface.h> #include <ocs2_ballbot_ros/BallbotDummyVisualization.h> From 3b4df012125c5eeeb04d053c2ed659a8c53b2349 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 15 Mar 2022 17:53:23 +0100 Subject: [PATCH 103/512] add license to ocs2_legged_robot_mpcnet --- .../LeggedRobotMpcnetDefinition.h | 29 +++++++++++++++++++ .../LeggedRobotMpcnetInterface.h | 29 +++++++++++++++++++ .../legged_robot_config.py | 29 +++++++++++++++++++ .../legged_robot_helper.py | 29 +++++++++++++++++++ .../legged_robot_mpcnet.py | 29 +++++++++++++++++++ .../legged_robot_policy.py | 29 +++++++++++++++++++ .../src/LeggedRobotMpcnetDefinition.cpp | 29 +++++++++++++++++++ .../src/LeggedRobotMpcnetDummyNode.cpp | 29 +++++++++++++++++++ .../src/LeggedRobotMpcnetInterface.cpp | 29 +++++++++++++++++++ .../src/LeggedRobotMpcnetPybindings.cpp | 29 +++++++++++++++++++ 10 files changed, 290 insertions(+) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h index f96cad2b5..2edd68b42 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_mpcnet/MpcnetDefinitionBase.h> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h index 885ed3967..904bb5065 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_legged_robot/LeggedRobotInterface.h> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py index a8ff93fb7..68660e5ca 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + from ocs2_mpcnet import config # diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py index 305e930d5..0becf1715 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import numpy as np from ocs2_mpcnet import helper diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index dca51b53b..03287b1fa 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import os import time import datetime diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index 08c7dc85b..8a78b9803 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -1,3 +1,32 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + import torch from ocs2_mpcnet import policy diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index b08b702de..748235087 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" #include <iostream> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 66198070b..1a459a0b9 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <ros/init.h> #include <ros/package.h> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index fcb9158b8..2918d80b8 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h" #include <ros/package.h> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp index 64f847634..6645e01a2 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <ocs2_mpcnet/MpcnetPybindMacros.h> #include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h" From 86eacc82d38a83ef834574e578d002f873ca2116 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 12:47:25 +0100 Subject: [PATCH 104/512] cleanup mpcnet controllers --- .../ocs2_core/control/ControllerType.h | 2 +- .../control/MpcnetControllerBase.h | 56 +------------------ .../control/MpcnetOnnxController.h | 50 +++++++---------- .../src/control/MpcnetOnnxController.cpp | 11 +++- 4 files changed, 31 insertions(+), 88 deletions(-) diff --git a/ocs2_core/include/ocs2_core/control/ControllerType.h b/ocs2_core/include/ocs2_core/control/ControllerType.h index 299bccd60..bbae196ee 100644 --- a/ocs2_core/include/ocs2_core/control/ControllerType.h +++ b/ocs2_core/include/ocs2_core/control/ControllerType.h @@ -34,6 +34,6 @@ namespace ocs2 { /** * Enum class for specifying controller type */ -enum class ControllerType { UNKNOWN, FEEDFORWARD, LINEAR, MPCNET, BEHAVIORAL }; +enum class ControllerType { UNKNOWN, FEEDFORWARD, LINEAR, ONNX, BEHAVIORAL }; } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h index 6ea188472..70d3663df 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h @@ -29,12 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include <memory> - #include <ocs2_core/control/ControllerBase.h> -#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> - -#include "ocs2_mpcnet/MpcnetDefinitionBase.h" namespace ocs2 { @@ -43,19 +38,9 @@ namespace ocs2 { */ class MpcnetControllerBase : public ControllerBase { public: - /** - * Constructor. - * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions. - * @param [in] referenceManagerPtr : Pointer to the reference manager. - */ - MpcnetControllerBase(std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, - std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr) - : mpcnetDefinitionPtr_(mpcnetDefinitionPtr), referenceManagerPtr_(referenceManagerPtr) {} - - /** - * Default destructor. - */ + MpcnetControllerBase() = default; ~MpcnetControllerBase() override = default; + MpcnetControllerBase* clone() const override = 0; /** * Load the model of the policy. @@ -63,43 +48,8 @@ class MpcnetControllerBase : public ControllerBase { */ virtual void loadPolicyModel(const std::string& policyFilePath) = 0; - ControllerType getType() const override { return ControllerType::MPCNET; } - - MpcnetControllerBase* clone() const override = 0; - protected: - /** - * Copy constructor. - */ - MpcnetControllerBase(const MpcnetControllerBase& other) : MpcnetControllerBase(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_) {} - - /** - * Get the generalized time. - * @param [in] t : Absolute time. - * @return Generalized time. - */ - vector_t getGeneralizedTime(scalar_t t) { return mpcnetDefinitionPtr_->getGeneralizedTime(t, referenceManagerPtr_->getModeSchedule()); } - - /** - * Get the relative state. - * @param [in] t : Absolute time. - * @param [in] x : Robot state. - * @return Relative state. - */ - vector_t getRelativeState(scalar_t t, const vector_t& x) { - return mpcnetDefinitionPtr_->getRelativeState(t, x, referenceManagerPtr_->getTargetTrajectories()); - } - - /** - * Get the input transformation. - * @param[in] t : Absolute time. - * @param[in] x : Robot state. - * @return The input transformation. - */ - matrix_t getInputTransformation(scalar_t t, const vector_t& x) { return mpcnetDefinitionPtr_->getInputTransformation(t, x); } - - std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; - std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; + MpcnetControllerBase(const MpcnetControllerBase& other) = default; }; } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index fc8d3be99..cad1701fd 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -31,6 +31,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <onnxruntime/onnxruntime_cxx_api.h> +#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> + +#include "ocs2_mpcnet/MpcnetDefinitionBase.h" #include "ocs2_mpcnet/control/MpcnetControllerBase.h" namespace ocs2 { @@ -57,59 +60,44 @@ inline std::shared_ptr<Ort::Env> createOnnxEnvironment() { class MpcnetOnnxController final : public MpcnetControllerBase { public: /** - * Constructor, does not load the model of the policy. + * Constructor. + * @note The class is not fully instantiated until calling loadPolicyModel(). * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions. * @param [in] referenceManagerPtr : Pointer to the reference manager. * @param [in] onnxEnvironmentPtr : Pointer to the environment for ONNX Runtime. */ MpcnetOnnxController(std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr, std::shared_ptr<Ort::Env> onnxEnvironmentPtr) - : MpcnetControllerBase(mpcnetDefinitionPtr, referenceManagerPtr), onnxEnvironmentPtr_(onnxEnvironmentPtr) {} + : mpcnetDefinitionPtr_(mpcnetDefinitionPtr), referenceManagerPtr_(referenceManagerPtr), onnxEnvironmentPtr_(onnxEnvironmentPtr) {} - /** - * Constructor, initializes all members of the controller. - * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions. - * @param [in] referenceManagerPtr : Pointer to the reference manager. - * @param [in] environmentPtr : Pointer to the environment for ONNX Runtime. - * @param [in] policyFilePath : Path to the ONNX file with the model of the policy. - */ - MpcnetOnnxController(std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, - std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr, std::shared_ptr<Ort::Env> onnxEnvironmentPtr, - const std::string& policyFilePath) - : MpcnetOnnxController(mpcnetDefinitionPtr, referenceManagerPtr, onnxEnvironmentPtr) { - loadPolicyModel(policyFilePath); - } - - /** - * Default destructor. - */ ~MpcnetOnnxController() override = default; + MpcnetOnnxController* clone() const override { return new MpcnetOnnxController(*this); } void loadPolicyModel(const std::string& policyFilePath) override; + ControllerType getType() const override { return ControllerType::ONNX; } vector_t computeInput(const scalar_t t, const vector_t& x) override; - void concatenate(const ControllerBase* otherController, int index, int length) override { - throw std::runtime_error("[MpcnetOnnxController::concatenate] not implemented."); - } - int size() const override { throw std::runtime_error("[MpcnetOnnxController::size] not implemented."); } - void clear() override { throw std::runtime_error("[MpcnetOnnxController::clear] not implemented."); } - bool empty() const override { throw std::runtime_error("[MpcnetOnnxController::empty] not implemented."); } - - MpcnetOnnxController* clone() const override { return new MpcnetOnnxController(*this); } + void concatenate(const ControllerBase* otherController, int index, int length) override { + throw std::runtime_error("[MpcnetOnnxController::concatenate] not implemented."); + } private: using tensor_element_t = float; - /** - * Copy constructor. - */ + /** Copy constructor. */ MpcnetOnnxController(const MpcnetOnnxController& other) - : MpcnetOnnxController(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_, other.onnxEnvironmentPtr_, other.policyFilePath_) {} + : MpcnetOnnxController(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_, other.onnxEnvironmentPtr_) { + if (!other.policyFilePath_.empty()) { + loadPolicyModel(other.policyFilePath_); + } + } + std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; + std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; std::shared_ptr<Ort::Env> onnxEnvironmentPtr_; std::string policyFilePath_; std::unique_ptr<Ort::Session> sessionPtr_; diff --git a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp index ec3f536af..627737135 100644 --- a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp @@ -59,9 +59,14 @@ void MpcnetOnnxController::loadPolicyModel(const std::string& policyFilePath) { /******************************************************************************************************/ /******************************************************************************************************/ vector_t MpcnetOnnxController::computeInput(const scalar_t t, const vector_t& x) { + if (sessionPtr_ == nullptr) { + throw std::runtime_error("[MpcnetOnnxController::computeInput] cannot compute input, since policy model is not loaded."); + } // create input tensor objects - Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> time = getGeneralizedTime(t).cast<tensor_element_t>(); - Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> state = getRelativeState(t, x).cast<tensor_element_t>(); + Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> time = + mpcnetDefinitionPtr_->getGeneralizedTime(t, referenceManagerPtr_->getModeSchedule()).cast<tensor_element_t>(); + Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> state = + mpcnetDefinitionPtr_->getRelativeState(t, x, referenceManagerPtr_->getTargetTrajectories()).cast<tensor_element_t>(); Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, OrtMemType::OrtMemTypeDefault); std::vector<Ort::Value> inputValues; inputValues.push_back( @@ -74,7 +79,7 @@ vector_t MpcnetOnnxController::computeInput(const scalar_t t, const vector_t& x) // evaluate output tensor objects Eigen::Map<Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1>> input(outputValues[0].GetTensorMutableData<tensor_element_t>(), outputShapes_[0][1], outputShapes_[0][0]); - return getInputTransformation(t, x) * input.cast<scalar_t>(); + return mpcnetDefinitionPtr_->getInputTransformation(t, x) * input.cast<scalar_t>(); } } // namespace ocs2 From d51c6bb3c5d787a5028d3a7a750f3d3d3d72fadd Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 13:01:30 +0100 Subject: [PATCH 105/512] additional cleanup for mpcnet controllers --- .../control/MpcnetBehavioralController.h | 17 +++--------- .../control/MpcnetOnnxController.h | 7 ++--- .../control/MpcnetBehavioralController.cpp | 26 +++++++++---------- 3 files changed, 18 insertions(+), 32 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h index 00c1dd476..732396ad5 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -53,10 +53,8 @@ class MpcnetBehavioralController final : public ControllerBase { MpcnetBehavioralController(scalar_t alpha, const ControllerBase& optimalController, const MpcnetControllerBase& learnedController) : alpha_(alpha), optimalControllerPtr_(optimalController.clone()), learnedControllerPtr_(learnedController.clone()) {} - /** - * Default destructor. - */ ~MpcnetBehavioralController() override = default; + MpcnetBehavioralController* clone() const override { return new MpcnetBehavioralController(*this); } /** * Set the mixture parameter. @@ -77,23 +75,14 @@ class MpcnetBehavioralController final : public ControllerBase { void setLearnedController(const MpcnetControllerBase& learnedController) { learnedControllerPtr_.reset(learnedController.clone()); } vector_t computeInput(scalar_t t, const vector_t& x) override; - - void concatenate(const ControllerBase* otherController, int index, int length) override; - - int size() const override; - ControllerType getType() const override { return ControllerType::BEHAVIORAL; } + int size() const override; void clear() override; - bool empty() const override; - - MpcnetBehavioralController* clone() const override { return new MpcnetBehavioralController(*this); } + void concatenate(const ControllerBase* otherController, int index, int length) override; private: - /** - * Copy constructor. - */ MpcnetBehavioralController(const MpcnetBehavioralController& other) : MpcnetBehavioralController(other.alpha_, *other.optimalControllerPtr_, *other.learnedControllerPtr_) {} diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index cad1701fd..874caca77 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -49,12 +49,10 @@ inline std::shared_ptr<Ort::Env> createOnnxEnvironment() { /** * A neural network controller using ONNX Runtime based on the Open Neural Network Exchange (ONNX) format. - * The model of the policy computes u, p, U = model(t, x) with + * The model of the policy computes u = model(t, x) with * t: generalized time (1 x dimensionOfTime), * x: relative state (1 x dimensionOfState), * u: predicted input (1 x dimensionOfInput), - * p: predicted expert weights (1 x numberOfExperts), - * U: predicted expert inputs (1 x dimensionOfInput x numberOfExperts). * @note The additional first dimension with size 1 for the variables of the model comes from batch processing during training. */ class MpcnetOnnxController final : public MpcnetControllerBase { @@ -75,8 +73,8 @@ class MpcnetOnnxController final : public MpcnetControllerBase { void loadPolicyModel(const std::string& policyFilePath) override; - ControllerType getType() const override { return ControllerType::ONNX; } vector_t computeInput(const scalar_t t, const vector_t& x) override; + ControllerType getType() const override { return ControllerType::ONNX; } int size() const override { throw std::runtime_error("[MpcnetOnnxController::size] not implemented."); } void clear() override { throw std::runtime_error("[MpcnetOnnxController::clear] not implemented."); } @@ -88,7 +86,6 @@ class MpcnetOnnxController final : public MpcnetControllerBase { private: using tensor_element_t = float; - /** Copy constructor. */ MpcnetOnnxController(const MpcnetOnnxController& other) : MpcnetOnnxController(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_, other.onnxEnvironmentPtr_) { if (!other.policyFilePath_.empty()) { diff --git a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp index db4b53d31..7d8d13da1 100644 --- a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp @@ -39,19 +39,7 @@ vector_t MpcnetBehavioralController::computeInput(scalar_t t, const vector_t& x) return alpha_ * optimalControllerPtr_->computeInput(t, x) + (1 - alpha_) * learnedControllerPtr_->computeInput(t, x); } else { throw std::runtime_error( - "[MpcnetBehavioralController::computeInput] cannot return input, since optimal and/or learned controller not set."); - } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void MpcnetBehavioralController::concatenate(const ControllerBase* otherController, int index, int length) { - if (optimalControllerPtr_ != nullptr) { - optimalControllerPtr_->concatenate(otherController, index, length); - } - if (learnedControllerPtr_ != nullptr) { - learnedControllerPtr_->concatenate(otherController, index, length); + "[MpcnetBehavioralController::computeInput] cannot compute input, since optimal and/or learned controller not set."); } } @@ -97,4 +85,16 @@ bool MpcnetBehavioralController::empty() const { } } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetBehavioralController::concatenate(const ControllerBase* otherController, int index, int length) { + if (optimalControllerPtr_ != nullptr) { + optimalControllerPtr_->concatenate(otherController, index, length); + } + if (learnedControllerPtr_ != nullptr) { + learnedControllerPtr_->concatenate(otherController, index, length); + } +} + } // namespace ocs2 From 5f3c72dda5df7db876388d27b52a2874315f9503 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 13:12:11 +0100 Subject: [PATCH 106/512] also move shared pointers --- .../include/ocs2_mpcnet/control/MpcnetOnnxController.h | 4 +++- .../include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h | 4 ++-- .../include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h | 4 ++-- ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp | 2 +- 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index 874caca77..0d3077f24 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -66,7 +66,9 @@ class MpcnetOnnxController final : public MpcnetControllerBase { */ MpcnetOnnxController(std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr, std::shared_ptr<Ort::Env> onnxEnvironmentPtr) - : mpcnetDefinitionPtr_(mpcnetDefinitionPtr), referenceManagerPtr_(referenceManagerPtr), onnxEnvironmentPtr_(onnxEnvironmentPtr) {} + : mpcnetDefinitionPtr_(std::move(mpcnetDefinitionPtr)), + referenceManagerPtr_(std::move(referenceManagerPtr)), + onnxEnvironmentPtr_(std::move(onnxEnvironmentPtr)) {} ~MpcnetOnnxController() override = default; MpcnetOnnxController* clone() const override { return new MpcnetOnnxController(*this); } diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index e586c935f..3ca867d41 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -74,8 +74,8 @@ class MpcnetDataGeneration { : mpcPtr_(std::move(mpcPtr)), mpcnetPtr_(std::move(mpcnetPtr)), rolloutPtr_(std::move(rolloutPtr)), - mpcnetDefinitionPtr_(mpcnetDefinitionPtr), - referenceManagerPtr_(referenceManagerPtr) {} + mpcnetDefinitionPtr_(std::move(mpcnetDefinitionPtr)), + referenceManagerPtr_(std::move(referenceManagerPtr)) {} /** * Default destructor. diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h index a8750b2e3..0b9e23838 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -67,8 +67,8 @@ class MpcnetPolicyEvaluation { : mpcPtr_(std::move(mpcPtr)), mpcnetPtr_(std::move(mpcnetPtr)), rolloutPtr_(std::move(rolloutPtr)), - mpcnetDefinitionPtr_(mpcnetDefinitionPtr), - referenceManagerPtr_(referenceManagerPtr) {} + mpcnetDefinitionPtr_(std::move(mpcnetDefinitionPtr)), + referenceManagerPtr_(std::move(referenceManagerPtr)) {} /** * Default destructor. diff --git a/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp b/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp index 8e1c9c593..fa1b9a82b 100644 --- a/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp +++ b/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp @@ -42,7 +42,7 @@ MpcnetDummyLoopRos::MpcnetDummyLoopRos(scalar_t controlFrequency, scalar_t rosFr rosFrequency_(rosFrequency), mpcnetPtr_(std::move(mpcnetPtr)), rolloutPtr_(std::move(rolloutPtr)), - rosReferenceManagerPtr_(rosReferenceManagerPtr) {} + rosReferenceManagerPtr_(std::move(rosReferenceManagerPtr)) {} /******************************************************************************************************/ /******************************************************************************************************/ From 172497a0711f6447fb7110c083e1a929732e213c Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 15:01:19 +0100 Subject: [PATCH 107/512] separate data and metrics headers --- .../include/ocs2_mpcnet/MpcnetInterfaceBase.h | 10 ---- .../include/ocs2_mpcnet/MpcnetPybindMacros.h | 37 +++++++------- .../include/ocs2_mpcnet/rollout/MpcnetData.h | 50 +++++++++++++++++++ .../rollout/MpcnetDataGeneration.h | 20 ++------ .../ocs2_mpcnet/rollout/MpcnetMetrics.h | 44 ++++++++++++++++ .../rollout/MpcnetPolicyEvaluation.h | 12 ++--- .../rollout/MpcnetRolloutManager.h | 7 --- ocs2_mpcnet/src/MpcnetInterfaceBase.cpp | 4 +- ocs2_mpcnet/src/MpcnetPybindings.cpp | 2 +- .../src/rollout/MpcnetDataGeneration.cpp | 13 +++-- .../src/rollout/MpcnetPolicyEvaluation.cpp | 8 ++- .../src/rollout/MpcnetRolloutManager.cpp | 4 +- 12 files changed, 135 insertions(+), 76 deletions(-) create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h index e084af594..ced704cb8 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h @@ -29,8 +29,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include <ocs2_core/thread_support/ThreadPool.h> - #include "ocs2_mpcnet/rollout/MpcnetRolloutManager.h" namespace ocs2 { @@ -39,14 +37,6 @@ namespace ocs2 { * Base class for all MPC-Net interfaces between C++ and Python. */ class MpcnetInterfaceBase { - public: - using data_point_t = MpcnetRolloutManager::data_point_t; - using data_array_t = MpcnetRolloutManager::data_array_t; - using data_ptr_t = MpcnetRolloutManager::data_ptr_t; - using metrics_t = MpcnetRolloutManager::metrics_t; - using metrics_array_t = MpcnetRolloutManager::metrics_array_t; - using metrics_ptr_t = MpcnetRolloutManager::metrics_ptr_t; - public: /** * Default destructor. diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index 9a20f98b2..77b262b07 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -36,12 +36,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_python_interface/PybindMacros.h> +#include "ocs2_mpcnet/rollout/MpcnetData.h" +#include "ocs2_mpcnet/rollout/MpcnetMetrics.h" + using namespace pybind11::literals; /** * Convenience macro to bind general MPC-Net functionalities and other classes with all required vectors. */ -#define CREATE_MPCNET_PYTHON_BINDINGS(MPCNET_INTERFACE, LIB_NAME) \ +#define CREATE_MPCNET_PYTHON_BINDINGS(LIB_NAME) \ /* make vector types opaque so they are not converted to python lists */ \ PYBIND11_MAKE_OPAQUE(ocs2::size_array_t) \ PYBIND11_MAKE_OPAQUE(ocs2::scalar_array_t) \ @@ -50,8 +53,8 @@ using namespace pybind11::literals; PYBIND11_MAKE_OPAQUE(std::vector<ocs2::SystemObservation>) \ PYBIND11_MAKE_OPAQUE(std::vector<ocs2::ModeSchedule>) \ PYBIND11_MAKE_OPAQUE(std::vector<ocs2::TargetTrajectories>) \ - PYBIND11_MAKE_OPAQUE(MPCNET_INTERFACE::data_array_t) \ - PYBIND11_MAKE_OPAQUE(MPCNET_INTERFACE::metrics_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::data_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::metrics_array_t) \ /* create a python module */ \ PYBIND11_MODULE(LIB_NAME, m) { \ /* bind vector types so they can be used natively in python */ \ @@ -62,8 +65,8 @@ using namespace pybind11::literals; VECTOR_TYPE_BINDING(std::vector<ocs2::SystemObservation>, "SystemObservationArray") \ VECTOR_TYPE_BINDING(std::vector<ocs2::ModeSchedule>, "ModeScheduleArray") \ VECTOR_TYPE_BINDING(std::vector<ocs2::TargetTrajectories>, "TargetTrajectoriesArray") \ - VECTOR_TYPE_BINDING(MPCNET_INTERFACE::data_array_t, "DataArray") \ - VECTOR_TYPE_BINDING(MPCNET_INTERFACE::metrics_array_t, "MetricsArray") \ + VECTOR_TYPE_BINDING(ocs2::data_array_t, "DataArray") \ + VECTOR_TYPE_BINDING(ocs2::metrics_array_t, "MetricsArray") \ /* bind approximation classes */ \ pybind11::class_<ocs2::ScalarFunctionQuadraticApproximation>(m, "ScalarFunctionQuadraticApproximation") \ .def_readwrite("f", &ocs2::ScalarFunctionQuadraticApproximation::f) \ @@ -91,21 +94,21 @@ using namespace pybind11::literals; .def_readwrite("state_trajectory", &ocs2::TargetTrajectories::stateTrajectory) \ .def_readwrite("input_trajectory", &ocs2::TargetTrajectories::inputTrajectory); \ /* bind data point struct */ \ - pybind11::class_<MPCNET_INTERFACE::data_point_t>(m, "DataPoint") \ + pybind11::class_<ocs2::data_point_t>(m, "DataPoint") \ .def(pybind11::init<>()) \ - .def_readwrite("mode", &MPCNET_INTERFACE::data_point_t::mode) \ - .def_readwrite("t", &MPCNET_INTERFACE::data_point_t::t) \ - .def_readwrite("x", &MPCNET_INTERFACE::data_point_t::x) \ - .def_readwrite("u", &MPCNET_INTERFACE::data_point_t::u) \ - .def_readwrite("generalized_time", &MPCNET_INTERFACE::data_point_t::generalizedTime) \ - .def_readwrite("relative_state", &MPCNET_INTERFACE::data_point_t::relativeState) \ - .def_readwrite("input_transformation", &MPCNET_INTERFACE::data_point_t::inputTransformation) \ - .def_readwrite("hamiltonian", &MPCNET_INTERFACE::data_point_t::hamiltonian); \ + .def_readwrite("mode", &ocs2::data_point_t::mode) \ + .def_readwrite("t", &ocs2::data_point_t::t) \ + .def_readwrite("x", &ocs2::data_point_t::x) \ + .def_readwrite("u", &ocs2::data_point_t::u) \ + .def_readwrite("generalized_time", &ocs2::data_point_t::generalizedTime) \ + .def_readwrite("relative_state", &ocs2::data_point_t::relativeState) \ + .def_readwrite("input_transformation", &ocs2::data_point_t::inputTransformation) \ + .def_readwrite("hamiltonian", &ocs2::data_point_t::hamiltonian); \ /* bind metrics struct */ \ - pybind11::class_<MPCNET_INTERFACE::metrics_t>(m, "Metrics") \ + pybind11::class_<ocs2::metrics_t>(m, "Metrics") \ .def(pybind11::init<>()) \ - .def_readwrite("survival_time", &MPCNET_INTERFACE::metrics_t::survivalTime) \ - .def_readwrite("incurred_hamiltonian", &MPCNET_INTERFACE::metrics_t::incurredHamiltonian); \ + .def_readwrite("survival_time", &ocs2::metrics_t::survivalTime) \ + .def_readwrite("incurred_hamiltonian", &ocs2::metrics_t::incurredHamiltonian); \ } /** diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h new file mode 100644 index 000000000..a51360f57 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h @@ -0,0 +1,50 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> + +namespace ocs2 { + +struct DataPoint { + size_t mode; + scalar_t t; + vector_t x; + vector_t u; + vector_t generalizedTime; + vector_t relativeState; + matrix_t inputTransformation; + ScalarFunctionQuadraticApproximation hamiltonian; +}; +using data_point_t = DataPoint; +using data_array_t = std::vector<data_point_t>; +using data_ptr_t = std::unique_ptr<data_array_t>; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index 3ca867d41..61548cbb7 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -38,6 +38,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/MpcnetDefinitionBase.h" #include "ocs2_mpcnet/control/MpcnetControllerBase.h" +#include "ocs2_mpcnet/rollout/MpcnetData.h" namespace ocs2 { @@ -47,19 +48,6 @@ namespace ocs2 { */ class MpcnetDataGeneration { public: - struct DataPoint { - size_t mode; - scalar_t t; - vector_t x; - vector_t u; - vector_t generalizedTime; - vector_t relativeState; - matrix_t inputTransformation; - ScalarFunctionQuadraticApproximation hamiltonian; - }; - using DataArray = std::vector<DataPoint>; - using DataPtr = std::unique_ptr<DataArray>; - /** * Constructor. * @param [in] mpcPtr : Pointer to the MPC solver to be used (this class takes ownership). @@ -105,9 +93,9 @@ class MpcnetDataGeneration { * @param [in] targetTrajectories : The target trajectories to be tracked. * @return Pointer to the generated data. */ - DataPtr run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, - const matrix_t& samplingCovariance, const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, - const TargetTrajectories& targetTrajectories); + data_ptr_t run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, + const matrix_t& samplingCovariance, const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories); private: std::unique_ptr<MPC_BASE> mpcPtr_; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h new file mode 100644 index 000000000..bbd7cb933 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h @@ -0,0 +1,44 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> + +namespace ocs2 { + +struct Metrics { + scalar_t survivalTime = 0.0; + scalar_t incurredHamiltonian = 0.0; +}; +using metrics_t = Metrics; +using metrics_array_t = std::vector<metrics_t>; +using metrics_ptr_t = std::unique_ptr<metrics_t>; + +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h index 0b9e23838..e920ad04b 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -38,6 +38,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/MpcnetDefinitionBase.h" #include "ocs2_mpcnet/control/MpcnetControllerBase.h" +#include "ocs2_mpcnet/rollout/MpcnetMetrics.h" namespace ocs2 { @@ -46,13 +47,6 @@ namespace ocs2 { */ class MpcnetPolicyEvaluation { public: - struct Metrics { - scalar_t survivalTime = 0.0; - scalar_t incurredHamiltonian = 0.0; - }; - using MetricsArray = std::vector<Metrics>; - using MetricsPtr = std::unique_ptr<Metrics>; - /** * Constructor. * @param [in] mpcPtr: Pointer to the MPC solver to be used (this class takes ownership). @@ -94,8 +88,8 @@ class MpcnetPolicyEvaluation { * @param [in] targetTrajectories : The target trajectories to be tracked. * @return Pointer to the computed metrics. */ - MetricsPtr run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, - const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); + metrics_ptr_t run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); private: std::unique_ptr<MPC_BASE> mpcPtr_; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h index aa53292ab..b0ed00258 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h @@ -41,13 +41,6 @@ namespace ocs2 { */ class MpcnetRolloutManager { public: - using data_point_t = MpcnetDataGeneration::DataPoint; - using data_array_t = MpcnetDataGeneration::DataArray; - using data_ptr_t = MpcnetDataGeneration::DataPtr; - using metrics_t = MpcnetPolicyEvaluation::Metrics; - using metrics_array_t = MpcnetPolicyEvaluation::MetricsArray; - using metrics_ptr_t = MpcnetPolicyEvaluation::MetricsPtr; - /** * Constructor. * @note The first nDataGenerationThreads pointers will be used for the data generation and the next nPolicyEvaluationThreads pointers for diff --git a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp index fb32c6956..3f45dbefb 100644 --- a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp +++ b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp @@ -53,7 +53,7 @@ bool MpcnetInterfaceBase::isDataGenerationDone() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MpcnetInterfaceBase::data_array_t MpcnetInterfaceBase::getGeneratedData() { +data_array_t MpcnetInterfaceBase::getGeneratedData() { return mpcnetRolloutManagerPtr_->getGeneratedData(); } @@ -77,7 +77,7 @@ bool MpcnetInterfaceBase::isPolicyEvaluationDone() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MpcnetInterfaceBase::metrics_array_t MpcnetInterfaceBase::getComputedMetrics() { +metrics_array_t MpcnetInterfaceBase::getComputedMetrics() { return mpcnetRolloutManagerPtr_->getComputedMetrics(); } diff --git a/ocs2_mpcnet/src/MpcnetPybindings.cpp b/ocs2_mpcnet/src/MpcnetPybindings.cpp index 8180c82b1..c0a3694f1 100644 --- a/ocs2_mpcnet/src/MpcnetPybindings.cpp +++ b/ocs2_mpcnet/src/MpcnetPybindings.cpp @@ -31,4 +31,4 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/MpcnetInterfaceBase.h" -CREATE_MPCNET_PYTHON_BINDINGS(ocs2::MpcnetInterfaceBase, MpcnetPybindings) +CREATE_MPCNET_PYTHON_BINDINGS(MpcnetPybindings) diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index 9c72463d7..a726960d0 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -38,12 +38,11 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, - size_t dataDecimation, size_t nSamples, const matrix_t& samplingCovariance, - const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, - const TargetTrajectories& targetTrajectories) { +data_ptr_t MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, + size_t nSamples, const matrix_t& samplingCovariance, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { // declare data pointer - DataPtr dataPtr(new DataArray); + data_ptr_t dataPtr(new data_array_t); // init time and state scalar_t time = initialObservation.time; @@ -88,7 +87,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st if (iteration % dataDecimation == 0) { // get nominal data point { - DataPoint dataPoint; + data_point_t dataPoint; dataPoint.t = primalSolution.timeTrajectory_.front(); dataPoint.x = primalSolution.stateTrajectory_.front(); dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); @@ -103,7 +102,7 @@ MpcnetDataGeneration::DataPtr MpcnetDataGeneration::run(scalar_t alpha, const st // get samples around nominal data point for (int i = 0; i < nSamples; i++) { - DataPoint dataPoint; + data_point_t dataPoint; dataPoint.t = primalSolution.timeTrajectory_.front(); dataPoint.x = primalSolution.stateTrajectory_.front(); dataPoint.x.noalias() += L * vector_t::NullaryExpr(primalSolution.stateTrajectory_.front().size(), standardNormalNullaryOp); diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index 5d48d2733..5a59cecad 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -34,12 +34,10 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MpcnetPolicyEvaluation::MetricsPtr MpcnetPolicyEvaluation::run(const std::string& policyFilePath, scalar_t timeStep, - const SystemObservation& initialObservation, - const ModeSchedule& modeSchedule, - const TargetTrajectories& targetTrajectories) { +metrics_ptr_t MpcnetPolicyEvaluation::run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { // declare metrics pointer - MetricsPtr metricsPtr(new Metrics); + metrics_ptr_t metricsPtr(new metrics_t); // init time and state scalar_t time = initialObservation.time; diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp index 1e638d321..127ccbd92 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp @@ -120,7 +120,7 @@ bool MpcnetRolloutManager::isDataGenerationDone() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MpcnetRolloutManager::data_array_t MpcnetRolloutManager::getGeneratedData() { +data_array_t MpcnetRolloutManager::getGeneratedData() { if (nDataGenerationThreads_ <= 0) { throw std::runtime_error("[MpcnetRolloutManager::getGeneratedData] cannot work without at least one data generation thread."); } @@ -213,7 +213,7 @@ bool MpcnetRolloutManager::isPolicyEvaluationDone() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MpcnetRolloutManager::metrics_array_t MpcnetRolloutManager::getComputedMetrics() { +metrics_array_t MpcnetRolloutManager::getComputedMetrics() { if (nPolicyEvaluationThreads_ <= 0) { throw std::runtime_error("[MpcnetRolloutManager::getComputedMetrics] cannot work without at least one policy evaluation thread."); } From 3b061bb92a64ca7cff2086df2bf48876f17eb05c Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 15:27:48 +0100 Subject: [PATCH 108/512] update computeInput of MpcnetBehavioralController --- ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp index 7d8d13da1..448579023 100644 --- a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/control/MpcnetBehavioralController.h" +#include <ocs2_core/misc/Numerics.h> + namespace ocs2 { /******************************************************************************************************/ @@ -36,7 +38,13 @@ namespace ocs2 { /******************************************************************************************************/ vector_t MpcnetBehavioralController::computeInput(scalar_t t, const vector_t& x) { if (optimalControllerPtr_ != nullptr && learnedControllerPtr_ != nullptr) { - return alpha_ * optimalControllerPtr_->computeInput(t, x) + (1 - alpha_) * learnedControllerPtr_->computeInput(t, x); + if (numerics::almost_eq(alpha_, 0.0)) { + return learnedControllerPtr_->computeInput(t, x); + } else if (numerics::almost_eq(alpha_, 1.0)) { + return optimalControllerPtr_->computeInput(t, x); + } else { + return alpha_ * optimalControllerPtr_->computeInput(t, x) + (1 - alpha_) * learnedControllerPtr_->computeInput(t, x); + } } else { throw std::runtime_error( "[MpcnetBehavioralController::computeInput] cannot compute input, since optimal and/or learned controller not set."); From e8a24833116aa6cda9f5a346f2c0f1d3774d3a9a Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 15:29:18 +0100 Subject: [PATCH 109/512] add getDataPoint function --- .../include/ocs2_mpcnet/rollout/MpcnetData.h | 19 ++++++++++++ .../src/rollout/MpcnetDataGeneration.cpp | 29 +++---------------- 2 files changed, 23 insertions(+), 25 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h index a51360f57..6cb398aec 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h @@ -30,6 +30,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include <ocs2_core/Types.h> +#include <ocs2_mpc/MPC_BASE.h> +#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> + +#include "ocs2_mpcnet/MpcnetDefinitionBase.h" namespace ocs2 { @@ -47,4 +51,19 @@ using data_point_t = DataPoint; using data_array_t = std::vector<data_point_t>; using data_ptr_t = std::unique_ptr<data_array_t>; +inline data_point_t getDataPoint(MPC_BASE* mpcPtr, MpcnetDefinitionBase* mpcnetDefinitionPtr, + ReferenceManagerInterface* referenceManagerPtr, const vector_t& deviation) { + data_point_t dataPoint; + const auto primalSolution = mpcPtr->getSolverPtr()->primalSolution(mpcPtr->getSolverPtr()->getFinalTime()); + dataPoint.t = primalSolution.timeTrajectory_.front(); + dataPoint.x = primalSolution.stateTrajectory_.front() + deviation; + dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); + dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); + dataPoint.generalizedTime = mpcnetDefinitionPtr->getGeneralizedTime(dataPoint.t, referenceManagerPtr->getModeSchedule()); + dataPoint.relativeState = mpcnetDefinitionPtr->getRelativeState(dataPoint.t, dataPoint.x, referenceManagerPtr->getTargetTrajectories()); + dataPoint.inputTransformation = mpcnetDefinitionPtr->getInputTransformation(dataPoint.t, dataPoint.x); + dataPoint.hamiltonian = mpcPtr->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); + return dataPoint; +} + } // namespace ocs2 diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index a726960d0..4bd8a6ce9 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -86,34 +86,13 @@ data_ptr_t MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFi // downsample the data signal by an integer factor if (iteration % dataDecimation == 0) { // get nominal data point - { - data_point_t dataPoint; - dataPoint.t = primalSolution.timeTrajectory_.front(); - dataPoint.x = primalSolution.stateTrajectory_.front(); - dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); - dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); - dataPoint.generalizedTime = mpcnetDefinitionPtr_->getGeneralizedTime(dataPoint.t, referenceManagerPtr_->getModeSchedule()); - dataPoint.relativeState = - mpcnetDefinitionPtr_->getRelativeState(dataPoint.t, dataPoint.x, referenceManagerPtr_->getTargetTrajectories()); - dataPoint.inputTransformation = mpcnetDefinitionPtr_->getInputTransformation(dataPoint.t, dataPoint.x); - dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); - dataPtr->push_back(std::move(dataPoint)); - } + const vector_t deviation = vector_t::Zero(primalSolution.stateTrajectory_.front().size()); + dataPtr->push_back(getDataPoint(mpcPtr_.get(), mpcnetDefinitionPtr_.get(), referenceManagerPtr_.get(), deviation)); // get samples around nominal data point for (int i = 0; i < nSamples; i++) { - data_point_t dataPoint; - dataPoint.t = primalSolution.timeTrajectory_.front(); - dataPoint.x = primalSolution.stateTrajectory_.front(); - dataPoint.x.noalias() += L * vector_t::NullaryExpr(primalSolution.stateTrajectory_.front().size(), standardNormalNullaryOp); - dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); - dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); - dataPoint.generalizedTime = mpcnetDefinitionPtr_->getGeneralizedTime(dataPoint.t, referenceManagerPtr_->getModeSchedule()); - dataPoint.relativeState = - mpcnetDefinitionPtr_->getRelativeState(dataPoint.t, dataPoint.x, referenceManagerPtr_->getTargetTrajectories()); - dataPoint.inputTransformation = mpcnetDefinitionPtr_->getInputTransformation(dataPoint.t, dataPoint.x); - dataPoint.hamiltonian = mpcPtr_->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); - dataPtr->push_back(std::move(dataPoint)); + const vector_t deviation = L * vector_t::NullaryExpr(primalSolution.stateTrajectory_.front().size(), standardNormalNullaryOp); + dataPtr->push_back(getDataPoint(mpcPtr_.get(), mpcnetDefinitionPtr_.get(), referenceManagerPtr_.get(), deviation)); } } From a89be041fd134c74504c4d97676c437ebac46ae0 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 15:43:05 +0100 Subject: [PATCH 110/512] add namespace mpcnet --- .../ocs2_mpcnet/MpcnetDefinitionBase.h | 2 ++ .../include/ocs2_mpcnet/MpcnetInterfaceBase.h | 2 ++ .../include/ocs2_mpcnet/MpcnetPybindMacros.h | 32 +++++++++---------- .../control/MpcnetBehavioralController.h | 2 ++ .../control/MpcnetControllerBase.h | 2 ++ .../control/MpcnetOnnxController.h | 2 ++ .../ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h | 2 ++ .../dummy/MpcnetDummyObserverRos.h | 2 ++ .../include/ocs2_mpcnet/rollout/MpcnetData.h | 2 ++ .../rollout/MpcnetDataGeneration.h | 2 ++ .../ocs2_mpcnet/rollout/MpcnetMetrics.h | 2 ++ .../rollout/MpcnetPolicyEvaluation.h | 2 ++ .../rollout/MpcnetRolloutManager.h | 2 ++ ocs2_mpcnet/src/MpcnetInterfaceBase.cpp | 2 ++ .../control/MpcnetBehavioralController.cpp | 2 ++ .../src/control/MpcnetOnnxController.cpp | 2 ++ ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp | 2 ++ .../src/dummy/MpcnetDummyObserverRos.cpp | 2 ++ .../src/rollout/MpcnetDataGeneration.cpp | 2 ++ .../src/rollout/MpcnetPolicyEvaluation.cpp | 2 ++ .../src/rollout/MpcnetRolloutManager.cpp | 2 ++ 21 files changed, 56 insertions(+), 16 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h index 33a78ff89..760e05228 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h @@ -33,6 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/reference/TargetTrajectories.h> namespace ocs2 { +namespace mpcnet { /** * Base class for MPC-Net definitions. @@ -87,4 +88,5 @@ class MpcnetDefinitionBase { virtual bool validState(const vector_t& x) = 0; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h index ced704cb8..6bef2d977 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/rollout/MpcnetRolloutManager.h" namespace ocs2 { +namespace mpcnet { /** * Base class for all MPC-Net interfaces between C++ and Python. @@ -86,4 +87,5 @@ class MpcnetInterfaceBase { std::unique_ptr<MpcnetRolloutManager> mpcnetRolloutManagerPtr_; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index 77b262b07..f8cdc7962 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -53,8 +53,8 @@ using namespace pybind11::literals; PYBIND11_MAKE_OPAQUE(std::vector<ocs2::SystemObservation>) \ PYBIND11_MAKE_OPAQUE(std::vector<ocs2::ModeSchedule>) \ PYBIND11_MAKE_OPAQUE(std::vector<ocs2::TargetTrajectories>) \ - PYBIND11_MAKE_OPAQUE(ocs2::data_array_t) \ - PYBIND11_MAKE_OPAQUE(ocs2::metrics_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::mpcnet::data_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::mpcnet::metrics_array_t) \ /* create a python module */ \ PYBIND11_MODULE(LIB_NAME, m) { \ /* bind vector types so they can be used natively in python */ \ @@ -65,8 +65,8 @@ using namespace pybind11::literals; VECTOR_TYPE_BINDING(std::vector<ocs2::SystemObservation>, "SystemObservationArray") \ VECTOR_TYPE_BINDING(std::vector<ocs2::ModeSchedule>, "ModeScheduleArray") \ VECTOR_TYPE_BINDING(std::vector<ocs2::TargetTrajectories>, "TargetTrajectoriesArray") \ - VECTOR_TYPE_BINDING(ocs2::data_array_t, "DataArray") \ - VECTOR_TYPE_BINDING(ocs2::metrics_array_t, "MetricsArray") \ + VECTOR_TYPE_BINDING(ocs2::mpcnet::data_array_t, "DataArray") \ + VECTOR_TYPE_BINDING(ocs2::mpcnet::metrics_array_t, "MetricsArray") \ /* bind approximation classes */ \ pybind11::class_<ocs2::ScalarFunctionQuadraticApproximation>(m, "ScalarFunctionQuadraticApproximation") \ .def_readwrite("f", &ocs2::ScalarFunctionQuadraticApproximation::f) \ @@ -94,21 +94,21 @@ using namespace pybind11::literals; .def_readwrite("state_trajectory", &ocs2::TargetTrajectories::stateTrajectory) \ .def_readwrite("input_trajectory", &ocs2::TargetTrajectories::inputTrajectory); \ /* bind data point struct */ \ - pybind11::class_<ocs2::data_point_t>(m, "DataPoint") \ + pybind11::class_<ocs2::mpcnet::data_point_t>(m, "DataPoint") \ .def(pybind11::init<>()) \ - .def_readwrite("mode", &ocs2::data_point_t::mode) \ - .def_readwrite("t", &ocs2::data_point_t::t) \ - .def_readwrite("x", &ocs2::data_point_t::x) \ - .def_readwrite("u", &ocs2::data_point_t::u) \ - .def_readwrite("generalized_time", &ocs2::data_point_t::generalizedTime) \ - .def_readwrite("relative_state", &ocs2::data_point_t::relativeState) \ - .def_readwrite("input_transformation", &ocs2::data_point_t::inputTransformation) \ - .def_readwrite("hamiltonian", &ocs2::data_point_t::hamiltonian); \ + .def_readwrite("mode", &ocs2::mpcnet::data_point_t::mode) \ + .def_readwrite("t", &ocs2::mpcnet::data_point_t::t) \ + .def_readwrite("x", &ocs2::mpcnet::data_point_t::x) \ + .def_readwrite("u", &ocs2::mpcnet::data_point_t::u) \ + .def_readwrite("generalized_time", &ocs2::mpcnet::data_point_t::generalizedTime) \ + .def_readwrite("relative_state", &ocs2::mpcnet::data_point_t::relativeState) \ + .def_readwrite("input_transformation", &ocs2::mpcnet::data_point_t::inputTransformation) \ + .def_readwrite("hamiltonian", &ocs2::mpcnet::data_point_t::hamiltonian); \ /* bind metrics struct */ \ - pybind11::class_<ocs2::metrics_t>(m, "Metrics") \ + pybind11::class_<ocs2::mpcnet::metrics_t>(m, "Metrics") \ .def(pybind11::init<>()) \ - .def_readwrite("survival_time", &ocs2::metrics_t::survivalTime) \ - .def_readwrite("incurred_hamiltonian", &ocs2::metrics_t::incurredHamiltonian); \ + .def_readwrite("survival_time", &ocs2::mpcnet::metrics_t::survivalTime) \ + .def_readwrite("incurred_hamiltonian", &ocs2::mpcnet::metrics_t::incurredHamiltonian); \ } /** diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h index 732396ad5..87d9837c4 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/control/MpcnetControllerBase.h" namespace ocs2 { +namespace mpcnet { /** * A behavioral controller that computes the input based on a mixture of an optimal policy (e.g. implicitly found via MPC) @@ -91,4 +92,5 @@ class MpcnetBehavioralController final : public ControllerBase { std::unique_ptr<MpcnetControllerBase> learnedControllerPtr_; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h index 70d3663df..8a669cf79 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/control/ControllerBase.h> namespace ocs2 { +namespace mpcnet { /** * The base class for all controllers that use a MPC-Net policy. @@ -52,4 +53,5 @@ class MpcnetControllerBase : public ControllerBase { MpcnetControllerBase(const MpcnetControllerBase& other) = default; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h index 0d3077f24..351facd05 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h @@ -37,6 +37,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/control/MpcnetControllerBase.h" namespace ocs2 { +namespace mpcnet { /** * Convenience function for creating an environment for ONNX Runtime and getting a corresponding shared pointer. @@ -106,4 +107,5 @@ class MpcnetOnnxController final : public MpcnetControllerBase { std::vector<std::vector<int64_t>> outputShapes_; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h index c0d0da475..80a09910d 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h @@ -38,6 +38,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/control/MpcnetControllerBase.h" namespace ocs2 { +namespace mpcnet { /** * Dummy loop to test a robot controlled by an MPC-Net policy. @@ -107,4 +108,5 @@ class MpcnetDummyLoopRos { std::vector<std::shared_ptr<SolverSynchronizedModule>> synchronizedModulePtrs_; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h index 0e7239113..a03661bb4 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_ros_interfaces/mrt/DummyObserver.h> namespace ocs2 { +namespace mpcnet { /** * Dummy observer that publishes the current system observation that is required for some target command nodes. @@ -64,4 +65,5 @@ class MpcnetDummyObserverRos : public DummyObserver { ros::Publisher observationPublisher_; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h index 6cb398aec..fac120288 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h @@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/MpcnetDefinitionBase.h" namespace ocs2 { +namespace mpcnet { struct DataPoint { size_t mode; @@ -66,4 +67,5 @@ inline data_point_t getDataPoint(MPC_BASE* mpcPtr, MpcnetDefinitionBase* mpcnetD return dataPoint; } +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index 61548cbb7..689912cfe 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -41,6 +41,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/rollout/MpcnetData.h" namespace ocs2 { +namespace mpcnet { /** * A class for generating MPC data from a system that is forward simulated with a behavioral controller. @@ -105,4 +106,5 @@ class MpcnetDataGeneration { std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h index bbd7cb933..fe3c85643 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> namespace ocs2 { +namespace mpcnet { struct Metrics { scalar_t survivalTime = 0.0; @@ -41,4 +42,5 @@ using metrics_t = Metrics; using metrics_array_t = std::vector<metrics_t>; using metrics_ptr_t = std::unique_ptr<metrics_t>; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h index e920ad04b..dbe939360 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -41,6 +41,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/rollout/MpcnetMetrics.h" namespace ocs2 { +namespace mpcnet { /** * A class for evaluating a policy for a system that is forward simulated with a learned controller. @@ -99,4 +100,5 @@ class MpcnetPolicyEvaluation { std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h index b0ed00258..c39994255 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h" namespace ocs2 { +namespace mpcnet { /** * A class to manage the data generation and policy evaluation rollouts for MPC-Net. @@ -130,4 +131,5 @@ class MpcnetRolloutManager { std::vector<std::future<metrics_ptr_t>> policyEvaluationFtrs_; }; +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp index 3f45dbefb..cd527d3cb 100644 --- a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp +++ b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/MpcnetInterfaceBase.h" namespace ocs2 { +namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ @@ -81,4 +82,5 @@ metrics_array_t MpcnetInterfaceBase::getComputedMetrics() { return mpcnetRolloutManagerPtr_->getComputedMetrics(); } +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp index 448579023..b641d3cbe 100644 --- a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/misc/Numerics.h> namespace ocs2 { +namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ @@ -105,4 +106,5 @@ void MpcnetBehavioralController::concatenate(const ControllerBase* otherControll } } +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp index 627737135..f15167805 100644 --- a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp +++ b/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/control/MpcnetOnnxController.h" namespace ocs2 { +namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ @@ -82,4 +83,5 @@ vector_t MpcnetOnnxController::computeInput(const scalar_t t, const vector_t& x) return mpcnetDefinitionPtr_->getInputTransformation(t, x) * input.cast<scalar_t>(); } +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp b/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp index fa1b9a82b..af825ef68 100644 --- a/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp +++ b/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ros/ros.h> namespace ocs2 { +namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ @@ -145,4 +146,5 @@ void MpcnetDummyLoopRos::preSolverRun(scalar_t time, const vector_t& state) { } } +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp b/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp index 516573e15..850f607d2 100644 --- a/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp +++ b/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_ros_interfaces/common/RosMsgConversions.h> namespace ocs2 { +namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ @@ -51,4 +52,5 @@ void MpcnetDummyObserverRos::update(const SystemObservation& observation, const observationPublisher_.publish(observationMsg); } +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index 4bd8a6ce9..8395b04b8 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/control/MpcnetBehavioralController.h" namespace ocs2 { +namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ @@ -129,4 +130,5 @@ data_ptr_t MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFi return dataPtr; } +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index 5a59cecad..1f82b4180 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h" namespace ocs2 { +namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ @@ -103,4 +104,5 @@ metrics_ptr_t MpcnetPolicyEvaluation::run(const std::string& policyFilePath, sca return metricsPtr; } +} // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp index 127ccbd92..6b0a40567 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_mpcnet/rollout/MpcnetRolloutManager.h" namespace ocs2 { +namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ @@ -244,4 +245,5 @@ metrics_array_t MpcnetRolloutManager::getComputedMetrics() { return metricsArray; } +} // namespace mpcnet } // namespace ocs2 From 68058818e024224a2092c98e519118a890a7c6aa Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 16:46:26 +0100 Subject: [PATCH 111/512] fix getDataPoint() --- .../include/ocs2_mpcnet/rollout/MpcnetData.h | 21 ++++++++++++------- .../src/rollout/MpcnetDataGeneration.cpp | 4 ++-- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h index fac120288..f8e105822 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h @@ -52,18 +52,25 @@ using data_point_t = DataPoint; using data_array_t = std::vector<data_point_t>; using data_ptr_t = std::unique_ptr<data_array_t>; -inline data_point_t getDataPoint(MPC_BASE* mpcPtr, MpcnetDefinitionBase* mpcnetDefinitionPtr, - ReferenceManagerInterface* referenceManagerPtr, const vector_t& deviation) { +/** + * Get a data point. + * @param [in] mpc : The MPC with a pointer to the underlying solver. + * @param [in] mpcnetDefinition : The MPC-Net definitions. + * @param [in] deviation : The state deviation from the nominal state where to get the data point from. + * @return A data point. + */ +inline data_point_t getDataPoint(MPC_BASE& mpc, MpcnetDefinitionBase& mpcnetDefinition, const vector_t& deviation) { data_point_t dataPoint; - const auto primalSolution = mpcPtr->getSolverPtr()->primalSolution(mpcPtr->getSolverPtr()->getFinalTime()); + const auto& referenceManager = mpc.getSolverPtr()->getReferenceManager(); + const auto primalSolution = mpc.getSolverPtr()->primalSolution(mpc.getSolverPtr()->getFinalTime()); dataPoint.t = primalSolution.timeTrajectory_.front(); dataPoint.x = primalSolution.stateTrajectory_.front() + deviation; dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); - dataPoint.generalizedTime = mpcnetDefinitionPtr->getGeneralizedTime(dataPoint.t, referenceManagerPtr->getModeSchedule()); - dataPoint.relativeState = mpcnetDefinitionPtr->getRelativeState(dataPoint.t, dataPoint.x, referenceManagerPtr->getTargetTrajectories()); - dataPoint.inputTransformation = mpcnetDefinitionPtr->getInputTransformation(dataPoint.t, dataPoint.x); - dataPoint.hamiltonian = mpcPtr->getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); + dataPoint.generalizedTime = mpcnetDefinition.getGeneralizedTime(dataPoint.t, referenceManager.getModeSchedule()); + dataPoint.relativeState = mpcnetDefinition.getRelativeState(dataPoint.t, dataPoint.x, referenceManager.getTargetTrajectories()); + dataPoint.inputTransformation = mpcnetDefinition.getInputTransformation(dataPoint.t, dataPoint.x); + dataPoint.hamiltonian = mpc.getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); return dataPoint; } diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index 8395b04b8..c9f3dd873 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -88,12 +88,12 @@ data_ptr_t MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFi if (iteration % dataDecimation == 0) { // get nominal data point const vector_t deviation = vector_t::Zero(primalSolution.stateTrajectory_.front().size()); - dataPtr->push_back(getDataPoint(mpcPtr_.get(), mpcnetDefinitionPtr_.get(), referenceManagerPtr_.get(), deviation)); + dataPtr->push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); // get samples around nominal data point for (int i = 0; i < nSamples; i++) { const vector_t deviation = L * vector_t::NullaryExpr(primalSolution.stateTrajectory_.front().size(), standardNormalNullaryOp); - dataPtr->push_back(getDataPoint(mpcPtr_.get(), mpcnetDefinitionPtr_.get(), referenceManagerPtr_.get(), deviation)); + dataPtr->push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); } } From c572d67c873561335cddb0d79c7f99d4edcbae40 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 16:51:12 +0100 Subject: [PATCH 112/512] small fix --- ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h | 1 - 1 file changed, 1 deletion(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h index f8e105822..a8b32278e 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h @@ -31,7 +31,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_mpc/MPC_BASE.h> -#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> #include "ocs2_mpcnet/MpcnetDefinitionBase.h" From dd970d95e82da372dce55ca3c7a3aaa20b6fc7ec Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 17 Mar 2022 17:49:01 +0100 Subject: [PATCH 113/512] optimize memory allocation a bit --- .../include/ocs2_mpcnet/rollout/MpcnetData.h | 1 - .../rollout/MpcnetDataGeneration.h | 9 ++++--- .../ocs2_mpcnet/rollout/MpcnetMetrics.h | 1 - .../rollout/MpcnetPolicyEvaluation.h | 6 ++--- .../rollout/MpcnetRolloutManager.h | 4 +-- .../src/rollout/MpcnetDataGeneration.cpp | 21 ++++++++------- .../src/rollout/MpcnetPolicyEvaluation.cpp | 18 ++++++------- .../src/rollout/MpcnetRolloutManager.cpp | 26 +++++++------------ 8 files changed, 39 insertions(+), 47 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h index a8b32278e..b8c5b27e2 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h @@ -49,7 +49,6 @@ struct DataPoint { }; using data_point_t = DataPoint; using data_array_t = std::vector<data_point_t>; -using data_ptr_t = std::unique_ptr<data_array_t>; /** * Get a data point. diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index 689912cfe..1e4a8fb0f 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -92,11 +92,11 @@ class MpcnetDataGeneration { * @param [in] initialObservation : The initial system observation to start from (time and state required). * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. * @param [in] targetTrajectories : The target trajectories to be tracked. - * @return Pointer to the generated data. + * @return Pointer to the data array with the generated data. */ - data_ptr_t run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, - const matrix_t& samplingCovariance, const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, - const TargetTrajectories& targetTrajectories); + const data_array_t* run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, + const matrix_t& samplingCovariance, const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories); private: std::unique_ptr<MPC_BASE> mpcPtr_; @@ -104,6 +104,7 @@ class MpcnetDataGeneration { std::unique_ptr<RolloutBase> rolloutPtr_; std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; + data_array_t dataArray_; }; } // namespace mpcnet diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h index fe3c85643..b9ecd71b0 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h @@ -40,7 +40,6 @@ struct Metrics { }; using metrics_t = Metrics; using metrics_array_t = std::vector<metrics_t>; -using metrics_ptr_t = std::unique_ptr<metrics_t>; } // namespace mpcnet } // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h index dbe939360..f50217f09 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -87,10 +87,10 @@ class MpcnetPolicyEvaluation { * @param [in] initialObservation : The initial system observation to start from (time and state required). * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. * @param [in] targetTrajectories : The target trajectories to be tracked. - * @return Pointer to the computed metrics. + * @return The computed metrics. */ - metrics_ptr_t run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, - const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); + metrics_t run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); private: std::unique_ptr<MPC_BASE> mpcPtr_; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h index c39994255..df85bde5d 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h @@ -122,13 +122,13 @@ class MpcnetRolloutManager { std::atomic_int nDataGenerationTasksDone_; std::unique_ptr<ThreadPool> dataGenerationThreadPoolPtr_; std::vector<std::unique_ptr<MpcnetDataGeneration>> dataGenerationPtrs_; - std::vector<std::future<data_ptr_t>> dataGenerationFtrs_; + std::vector<std::future<const data_array_t*>> dataGenerationFtrs_; // policy evaluation variables size_t nPolicyEvaluationThreads_; std::atomic_int nPolicyEvaluationTasksDone_; std::unique_ptr<ThreadPool> policyEvaluationThreadPoolPtr_; std::vector<std::unique_ptr<MpcnetPolicyEvaluation>> policyEvaluationPtrs_; - std::vector<std::future<metrics_ptr_t>> policyEvaluationFtrs_; + std::vector<std::future<metrics_t>> policyEvaluationFtrs_; }; } // namespace mpcnet diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index c9f3dd873..325e6badc 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -39,11 +39,12 @@ namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -data_ptr_t MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, - size_t nSamples, const matrix_t& samplingCovariance, const SystemObservation& initialObservation, - const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { - // declare data pointer - data_ptr_t dataPtr(new data_array_t); +const data_array_t* MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, + size_t nSamples, const matrix_t& samplingCovariance, + const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + // clear data array + dataArray_.clear(); // init time and state scalar_t time = initialObservation.time; @@ -88,12 +89,12 @@ data_ptr_t MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFi if (iteration % dataDecimation == 0) { // get nominal data point const vector_t deviation = vector_t::Zero(primalSolution.stateTrajectory_.front().size()); - dataPtr->push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); + dataArray_.push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); // get samples around nominal data point for (int i = 0; i < nSamples; i++) { const vector_t deviation = L * vector_t::NullaryExpr(primalSolution.stateTrajectory_.front().size(), standardNormalNullaryOp); - dataPtr->push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); + dataArray_.push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); } } @@ -123,11 +124,11 @@ data_ptr_t MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFi // print error for exceptions std::cerr << "[MpcnetDataGeneration::run] a standard exception was caught, with message: " << e.what() << "\n"; // this data generation run failed, clear data - dataPtr->clear(); + dataArray_.clear(); } - // return data pointer - return dataPtr; + // return pointer to the data array + return &dataArray_; } } // namespace mpcnet diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index 1f82b4180..f45177389 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -35,10 +35,10 @@ namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -metrics_ptr_t MpcnetPolicyEvaluation::run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, - const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { - // declare metrics pointer - metrics_ptr_t metricsPtr(new metrics_t); +metrics_t MpcnetPolicyEvaluation::run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { + // declare metrics + metrics_t metrics; // init time and state scalar_t time = initialObservation.time; @@ -69,7 +69,7 @@ metrics_ptr_t MpcnetPolicyEvaluation::run(const std::string& policyFilePath, sca // incurred quantities vector_t input = mpcnetPtr_->computeInput(time, state); - metricsPtr->incurredHamiltonian += mpcPtr_->getSolverPtr()->getHamiltonian(time, state, input).f * timeStep; + metrics.incurredHamiltonian += mpcPtr_->getSolverPtr()->getHamiltonian(time, state, input).f * timeStep; // forward simulate system with learned controller scalar_array_t timeTrajectory; @@ -94,14 +94,14 @@ metrics_ptr_t MpcnetPolicyEvaluation::run(const std::string& policyFilePath, sca // print error for exceptions std::cerr << "[MpcnetPolicyEvaluation::run] a standard exception was caught, with message: " << e.what() << "\n"; // this policy evaluation run failed, incurred quantities are not reported - metricsPtr->incurredHamiltonian = std::numeric_limits<scalar_t>::quiet_NaN(); + metrics.incurredHamiltonian = std::numeric_limits<scalar_t>::quiet_NaN(); } // report survival time - metricsPtr->survivalTime = time; + metrics.survivalTime = time; - // return metrics pointer - return metricsPtr; + // return metrics + return metrics; } } // namespace mpcnet diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp index 6b0a40567..fa888bb73 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp @@ -85,9 +85,9 @@ void MpcnetRolloutManager::startDataGeneration(scalar_t alpha, const std::string // push tasks into pool for (int i = 0; i < initialObservations.size(); i++) { dataGenerationFtrs_.push_back(dataGenerationThreadPoolPtr_->run([=](int threadNumber) { - data_ptr_t result; - result = dataGenerationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, dataDecimation, nSamples, samplingCovariance, - initialObservations.at(i), modeSchedules.at(i), targetTrajectories.at(i)); + const auto* result = + dataGenerationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, dataDecimation, nSamples, samplingCovariance, + initialObservations.at(i), modeSchedules.at(i), targetTrajectories.at(i)); nDataGenerationTasksDone_++; // print thread and task number std::cerr << "Data generation thread " << threadNumber << " finished task " << nDataGenerationTasksDone_ << "\n"; @@ -130,7 +130,7 @@ data_array_t MpcnetRolloutManager::getGeneratedData() { } // get pointers to data - std::vector<data_ptr_t> dataPtrs; + std::vector<const data_array_t*> dataPtrs; for (int i = 0; i < dataGenerationFtrs_.size(); i++) { try { // get results from futures of the tasks @@ -178,9 +178,8 @@ void MpcnetRolloutManager::startPolicyEvaluation(const std::string& policyFilePa // push tasks into pool for (int i = 0; i < initialObservations.size(); i++) { policyEvaluationFtrs_.push_back(policyEvaluationThreadPoolPtr_->run([=](int threadNumber) { - metrics_ptr_t result; - result = policyEvaluationPtrs_[threadNumber]->run(policyFilePath, timeStep, initialObservations.at(i), modeSchedules.at(i), - targetTrajectories.at(i)); + const auto result = policyEvaluationPtrs_[threadNumber]->run(policyFilePath, timeStep, initialObservations.at(i), modeSchedules.at(i), + targetTrajectories.at(i)); nPolicyEvaluationTasksDone_++; // print thread and task number std::cerr << "Policy evaluation thread " << threadNumber << " finished task " << nPolicyEvaluationTasksDone_ << "\n"; @@ -222,25 +221,18 @@ metrics_array_t MpcnetRolloutManager::getComputedMetrics() { throw std::runtime_error("[MpcnetRolloutManager::getComputedMetrics] cannot get metrics when policy evaluation is not done."); } - // get pointers to metrics - std::vector<metrics_ptr_t> metricsPtrs; + // get metrics and fill metrics array + metrics_array_t metricsArray; for (int i = 0; i < policyEvaluationFtrs_.size(); i++) { try { // get results from futures of the tasks - metricsPtrs.push_back(policyEvaluationFtrs_[i].get()); + metricsArray.push_back(policyEvaluationFtrs_[i].get()); } catch (const std::exception& e) { // print error for exceptions std::cerr << "[MpcnetRolloutManager::getComputedMetrics] a standard exception was caught, with message: " << e.what() << "\n"; } } - // fill metrics array - metrics_array_t metricsArray; - metricsArray.reserve(metricsPtrs.size()); - for (int i = 0; i < metricsPtrs.size(); i++) { - metricsArray.push_back((*metricsPtrs[i])); - } - // return metrics return metricsArray; } From ef4880394156edec98f34e46fe02f2fddd6fdfa0 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 18 Mar 2022 12:54:36 +0100 Subject: [PATCH 114/512] add general step method for mpcnet rollouts --- ocs2_mpcnet/CMakeLists.txt | 1 + .../include/ocs2_mpcnet/MpcnetInterfaceBase.h | 2 +- .../include/ocs2_mpcnet/MpcnetPybindMacros.h | 34 +++--- .../rollout/MpcnetDataGeneration.h | 37 ++---- .../rollout/MpcnetPolicyEvaluation.h | 45 ++----- .../ocs2_mpcnet/rollout/MpcnetRolloutBase.h | 114 ++++++++++++++++++ .../rollout/MpcnetRolloutManager.h | 9 +- ocs2_mpcnet/src/MpcnetInterfaceBase.cpp | 4 +- .../src/rollout/MpcnetDataGeneration.cpp | 57 ++------- .../src/rollout/MpcnetPolicyEvaluation.cpp | 58 ++------- ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp | 100 +++++++++++++++ .../src/rollout/MpcnetRolloutManager.cpp | 6 +- 12 files changed, 283 insertions(+), 184 deletions(-) create mode 100644 ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h create mode 100644 ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp diff --git a/ocs2_mpcnet/CMakeLists.txt b/ocs2_mpcnet/CMakeLists.txt index 9bae9aca5..6708884f9 100644 --- a/ocs2_mpcnet/CMakeLists.txt +++ b/ocs2_mpcnet/CMakeLists.txt @@ -49,6 +49,7 @@ add_library(${PROJECT_NAME} src/dummy/MpcnetDummyObserverRos.cpp src/rollout/MpcnetDataGeneration.cpp src/rollout/MpcnetPolicyEvaluation.cpp + src/rollout/MpcnetRolloutBase.cpp src/rollout/MpcnetRolloutManager.cpp src/MpcnetInterfaceBase.cpp ) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h index 6bef2d977..0d55bcc37 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h @@ -64,7 +64,7 @@ class MpcnetInterfaceBase { /** * @see MpcnetRolloutManager::startPolicyEvaluation() */ - void startPolicyEvaluation(const std::string& policyFilePath, scalar_t timeStep, + void startPolicyEvaluation(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, const std::vector<SystemObservation>& initialObservations, const std::vector<ModeSchedule>& modeSchedules, const std::vector<TargetTrajectories>& targetTrajectories); diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index f8cdc7962..6bc80ec7a 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -114,21 +114,21 @@ using namespace pybind11::literals; /** * Convenience macro to bind robot MPC-Net interface. */ -#define CREATE_ROBOT_MPCNET_PYTHON_BINDINGS(MPCNET_INTERFACE, LIB_NAME) \ - /* create a python module */ \ - PYBIND11_MODULE(LIB_NAME, m) { \ - /* import the general MPC-Net module */ \ - pybind11::module::import("ocs2_mpcnet.MpcnetPybindings"); \ - /* bind actual MPC-Net interface for specific robot */ \ - pybind11::class_<MPCNET_INTERFACE>(m, "MpcnetInterface") \ - .def(pybind11::init<size_t, size_t, bool>()) \ - .def("startDataGeneration", &MPCNET_INTERFACE::startDataGeneration, "alpha"_a, "policyFilePath"_a, "timeStep"_a, \ - "dataDecimation"_a, "nSamples"_a, "samplingCovariance"_a.noconvert(), "initialObservations"_a, "modeSchedules"_a, \ - "targetTrajectories"_a) \ - .def("isDataGenerationDone", &MPCNET_INTERFACE::isDataGenerationDone) \ - .def("getGeneratedData", &MPCNET_INTERFACE::getGeneratedData) \ - .def("startPolicyEvaluation", &MPCNET_INTERFACE::startPolicyEvaluation, "policyFilePath"_a, "timeStep"_a, "initialObservations"_a, \ - "modeSchedules"_a, "targetTrajectories"_a) \ - .def("isPolicyEvaluationDone", &MPCNET_INTERFACE::isPolicyEvaluationDone) \ - .def("getComputedMetrics", &MPCNET_INTERFACE::getComputedMetrics); \ +#define CREATE_ROBOT_MPCNET_PYTHON_BINDINGS(MPCNET_INTERFACE, LIB_NAME) \ + /* create a python module */ \ + PYBIND11_MODULE(LIB_NAME, m) { \ + /* import the general MPC-Net module */ \ + pybind11::module::import("ocs2_mpcnet.MpcnetPybindings"); \ + /* bind actual MPC-Net interface for specific robot */ \ + pybind11::class_<MPCNET_INTERFACE>(m, "MpcnetInterface") \ + .def(pybind11::init<size_t, size_t, bool>()) \ + .def("startDataGeneration", &MPCNET_INTERFACE::startDataGeneration, "alpha"_a, "policyFilePath"_a, "timeStep"_a, \ + "dataDecimation"_a, "nSamples"_a, "samplingCovariance"_a.noconvert(), "initialObservations"_a, "modeSchedules"_a, \ + "targetTrajectories"_a) \ + .def("isDataGenerationDone", &MPCNET_INTERFACE::isDataGenerationDone) \ + .def("getGeneratedData", &MPCNET_INTERFACE::getGeneratedData) \ + .def("startPolicyEvaluation", &MPCNET_INTERFACE::startPolicyEvaluation, "alpha"_a, "policyFilePath"_a, "timeStep"_a, \ + "initialObservations"_a, "modeSchedules"_a, "targetTrajectories"_a) \ + .def("isPolicyEvaluationDone", &MPCNET_INTERFACE::isPolicyEvaluationDone) \ + .def("getComputedMetrics", &MPCNET_INTERFACE::getComputedMetrics); \ } diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index 1e4a8fb0f..57c3b0a30 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -29,47 +29,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include <ocs2_core/reference/ModeSchedule.h> -#include <ocs2_core/reference/TargetTrajectories.h> -#include <ocs2_mpc/MPC_BASE.h> -#include <ocs2_mpc/SystemObservation.h> -#include <ocs2_oc/rollout/RolloutBase.h> -#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> - -#include "ocs2_mpcnet/MpcnetDefinitionBase.h" -#include "ocs2_mpcnet/control/MpcnetControllerBase.h" #include "ocs2_mpcnet/rollout/MpcnetData.h" +#include "ocs2_mpcnet/rollout/MpcnetRolloutBase.h" namespace ocs2 { namespace mpcnet { /** - * A class for generating MPC data from a system that is forward simulated with a behavioral controller. - * The behavioral policy is a mixture of an MPC policy and an MPC-Net policy (e.g. a neural network). + * A class for generating data from a system that is forward simulated with a behavioral controller. + * @note Usually the behavioral controller moves from the MPC policy to the MPC-Net policy throughout the training process. */ -class MpcnetDataGeneration { +class MpcnetDataGeneration final : public MpcnetRolloutBase { public: /** - * Constructor. - * @param [in] mpcPtr : Pointer to the MPC solver to be used (this class takes ownership). - * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). - * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). - * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions to be used (shared ownership). - * @param [in] referenceManagerPtr : Pointer to the reference manager to be used (shared ownership). + * @see MpcnetRolloutBase::MpcnetRolloutBase() */ MpcnetDataGeneration(std::unique_ptr<MPC_BASE> mpcPtr, std::unique_ptr<MpcnetControllerBase> mpcnetPtr, std::unique_ptr<RolloutBase> rolloutPtr, std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr) - : mpcPtr_(std::move(mpcPtr)), - mpcnetPtr_(std::move(mpcnetPtr)), - rolloutPtr_(std::move(rolloutPtr)), - mpcnetDefinitionPtr_(std::move(mpcnetDefinitionPtr)), - referenceManagerPtr_(std::move(referenceManagerPtr)) {} + : MpcnetRolloutBase(std::move(mpcPtr), std::move(mpcnetPtr), std::move(rolloutPtr), std::move(mpcnetDefinitionPtr), + std::move(referenceManagerPtr)) {} /** * Default destructor. */ - virtual ~MpcnetDataGeneration() = default; + ~MpcnetDataGeneration() override = default; /** * Deleted copy constructor. @@ -99,11 +83,6 @@ class MpcnetDataGeneration { const TargetTrajectories& targetTrajectories); private: - std::unique_ptr<MPC_BASE> mpcPtr_; - std::unique_ptr<MpcnetControllerBase> mpcnetPtr_; - std::unique_ptr<RolloutBase> rolloutPtr_; - std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; - std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; data_array_t dataArray_; }; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h index f50217f09..d617e81f9 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -29,46 +29,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include <ocs2_core/reference/ModeSchedule.h> -#include <ocs2_core/reference/TargetTrajectories.h> -#include <ocs2_mpc/MPC_BASE.h> -#include <ocs2_mpc/SystemObservation.h> -#include <ocs2_oc/rollout/RolloutBase.h> -#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> - -#include "ocs2_mpcnet/MpcnetDefinitionBase.h" -#include "ocs2_mpcnet/control/MpcnetControllerBase.h" #include "ocs2_mpcnet/rollout/MpcnetMetrics.h" +#include "ocs2_mpcnet/rollout/MpcnetRolloutBase.h" namespace ocs2 { namespace mpcnet { /** - * A class for evaluating a policy for a system that is forward simulated with a learned controller. + * A class for evaluating a policy for a system that is forward simulated with a behavioral controller. + * @note Usually the behavioral controller is evaluated for the MPC-Net policy (alpha = 0). */ -class MpcnetPolicyEvaluation { +class MpcnetPolicyEvaluation final : public MpcnetRolloutBase { public: /** - * Constructor. - * @param [in] mpcPtr: Pointer to the MPC solver to be used (this class takes ownership). - * @param [in] mpcnetPtr: Pointer to the MPC-Net policy to be used (this class takes ownership). - * @param [in] rolloutPtr: Pointer to the rollout to be used (this class takes ownership). - * @param [in] mpcnetDefinitionPtr: Pointer to the MPC-Net definitions to be used (shared ownership). - * @param [in] referenceManagerPtr: Pointer to the reference manager to be used (shared ownership). + * @see MpcnetRolloutBase::MpcnetRolloutBase() */ MpcnetPolicyEvaluation(std::unique_ptr<MPC_BASE> mpcPtr, std::unique_ptr<MpcnetControllerBase> mpcnetPtr, std::unique_ptr<RolloutBase> rolloutPtr, std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr) - : mpcPtr_(std::move(mpcPtr)), - mpcnetPtr_(std::move(mpcnetPtr)), - rolloutPtr_(std::move(rolloutPtr)), - mpcnetDefinitionPtr_(std::move(mpcnetDefinitionPtr)), - referenceManagerPtr_(std::move(referenceManagerPtr)) {} + : MpcnetRolloutBase(std::move(mpcPtr), std::move(mpcnetPtr), std::move(rolloutPtr), std::move(mpcnetDefinitionPtr), + std::move(referenceManagerPtr)) {} /** * Default destructor. */ - virtual ~MpcnetPolicyEvaluation() = default; + ~MpcnetPolicyEvaluation() override = default; /** * Deleted copy constructor. @@ -82,22 +67,16 @@ class MpcnetPolicyEvaluation { /** * Run the policy evaluation. - * @param [in] policyFilePath : The path to the file with the learned policy for the controller. - * @param [in] timeStep : The time step for the forward simulation of the system with the controller. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the behavioral controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. * @param [in] initialObservation : The initial system observation to start from (time and state required). * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. * @param [in] targetTrajectories : The target trajectories to be tracked. * @return The computed metrics. */ - metrics_t run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, + metrics_t run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); - - private: - std::unique_ptr<MPC_BASE> mpcPtr_; - std::unique_ptr<MpcnetControllerBase> mpcnetPtr_; - std::unique_ptr<RolloutBase> rolloutPtr_; - std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; - std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; }; } // namespace mpcnet diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h new file mode 100644 index 000000000..6b2a528d6 --- /dev/null +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h @@ -0,0 +1,114 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/reference/ModeSchedule.h> +#include <ocs2_core/reference/TargetTrajectories.h> +#include <ocs2_mpc/MPC_BASE.h> +#include <ocs2_mpc/SystemObservation.h> +#include <ocs2_oc/oc_data/PrimalSolution.h> +#include <ocs2_oc/rollout/RolloutBase.h> +#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> + +#include "ocs2_mpcnet/MpcnetDefinitionBase.h" +#include "ocs2_mpcnet/control/MpcnetBehavioralController.h" +#include "ocs2_mpcnet/control/MpcnetControllerBase.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * The base class for doing rollouts for a system that is forward simulated with a behavioral controller. + * The behavioral policy is a mixture of an MPC policy and an MPC-Net policy (e.g. a neural network). + */ +class MpcnetRolloutBase { + public: + /** + * Constructor. + * @param [in] mpcPtr : Pointer to the MPC solver to be used (this class takes ownership). + * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions to be used (shared ownership). + * @param [in] referenceManagerPtr : Pointer to the reference manager to be used (shared ownership). + */ + MpcnetRolloutBase(std::unique_ptr<MPC_BASE> mpcPtr, std::unique_ptr<MpcnetControllerBase> mpcnetPtr, + std::unique_ptr<RolloutBase> rolloutPtr, std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, + std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr) + : mpcPtr_(std::move(mpcPtr)), + mpcnetPtr_(std::move(mpcnetPtr)), + rolloutPtr_(std::move(rolloutPtr)), + mpcnetDefinitionPtr_(std::move(mpcnetDefinitionPtr)), + referenceManagerPtr_(std::move(referenceManagerPtr)) {} + + /** + * Default destructor. + */ + virtual ~MpcnetRolloutBase() = default; + + /** + * Deleted copy constructor. + */ + MpcnetRolloutBase(const MpcnetRolloutBase&) = delete; + + /** + * Deleted copy assignment. + */ + MpcnetRolloutBase& operator=(const MpcnetRolloutBase&) = delete; + + /** + * Initialize the system. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the controller. + * @param [in] initialObservation : The initial system observation to start from (time and state required). + * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + */ + void init(scalar_t alpha, const std::string& policyFilePath, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); + + /** + * Simulate the system one step forward. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. + */ + void step(scalar_t timeStep); + + protected: + std::unique_ptr<MPC_BASE> mpcPtr_; + std::unique_ptr<MpcnetControllerBase> mpcnetPtr_; + std::unique_ptr<RolloutBase> rolloutPtr_; + std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; + std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; + std::unique_ptr<MpcnetBehavioralController> behavioralControllerPtr_; + SystemObservation systemObservation_; + PrimalSolution primalSolution_; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h index df85bde5d..092ee05f5 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h @@ -93,14 +93,15 @@ class MpcnetRolloutManager { data_array_t getGeneratedData(); /** - * Starts the policy evaluation forward simulated by a learned controller. - * @param [in] policyFilePath : The path to the file with the learned policy for the learned controller. - * @param [in] timeStep : The time step for the forward simulation of the system with the learned controller. + * Starts the policy evaluation forward simulated by a behavioral controller. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the behavioral controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. * @param [in] initialObservations : The initial system observations to start from (time and state required). * @param [in] modeSchedules : The mode schedules providing the event times and mode sequence. * @param [in] targetTrajectories : The target trajectories to be tracked. */ - void startPolicyEvaluation(const std::string& policyFilePath, scalar_t timeStep, + void startPolicyEvaluation(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, const std::vector<SystemObservation>& initialObservations, const std::vector<ModeSchedule>& modeSchedules, const std::vector<TargetTrajectories>& targetTrajectories); diff --git a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp index cd527d3cb..f21749070 100644 --- a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp +++ b/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp @@ -61,11 +61,11 @@ data_array_t MpcnetInterfaceBase::getGeneratedData() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MpcnetInterfaceBase::startPolicyEvaluation(const std::string& policyFilePath, scalar_t timeStep, +void MpcnetInterfaceBase::startPolicyEvaluation(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, const std::vector<SystemObservation>& initialObservations, const std::vector<ModeSchedule>& modeSchedules, const std::vector<TargetTrajectories>& targetTrajectories) { - mpcnetRolloutManagerPtr_->startPolicyEvaluation(policyFilePath, timeStep, initialObservations, modeSchedules, targetTrajectories); + mpcnetRolloutManagerPtr_->startPolicyEvaluation(alpha, policyFilePath, timeStep, initialObservations, modeSchedules, targetTrajectories); } /******************************************************************************************************/ diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index 325e6badc..ebc4acb0f 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -46,27 +46,8 @@ const data_array_t* MpcnetDataGeneration::run(scalar_t alpha, const std::string& // clear data array dataArray_.clear(); - // init time and state - scalar_t time = initialObservation.time; - vector_t state = initialObservation.state; - - // reset mpc - mpcPtr_->reset(); - - // reset rollout, i.e. reset the internal simulator state (e.g. relevant for RaiSim) - rolloutPtr_->resetRollout(); - - // prepare learned controller - mpcnetPtr_->loadPolicyModel(policyFilePath); - - // update the reference manager - referenceManagerPtr_->setModeSchedule(modeSchedule); - referenceManagerPtr_->setTargetTrajectories(targetTrajectories); - - // set up behavioral controller with mixture parameter alpha and learned controller - std::unique_ptr<MpcnetBehavioralController> behavioralControllerPtr; - behavioralControllerPtr->setAlpha(alpha); - behavioralControllerPtr->setLearnedController(*mpcnetPtr_); + // init system + init(alpha, policyFilePath, initialObservation, modeSchedule, targetTrajectories); // set up scalar standard normal generator and compute Cholesky decomposition of covariance matrix std::random_device randomDevice; @@ -78,47 +59,25 @@ const data_array_t* MpcnetDataGeneration::run(scalar_t alpha, const std::string& // run data generation int iteration = 0; try { - while (time <= targetTrajectories.timeTrajectory.back()) { - // run mpc and get solution - if (!mpcPtr_->run(time, state)) { - throw std::runtime_error("[MpcnetDataGeneration::run] main routine of MPC returned false."); - } - const auto primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); + while (systemObservation_.time <= targetTrajectories.timeTrajectory.back()) { + // step system + step(timeStep); // downsample the data signal by an integer factor if (iteration % dataDecimation == 0) { // get nominal data point - const vector_t deviation = vector_t::Zero(primalSolution.stateTrajectory_.front().size()); + const vector_t deviation = vector_t::Zero(primalSolution_.stateTrajectory_.front().size()); dataArray_.push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); // get samples around nominal data point for (int i = 0; i < nSamples; i++) { - const vector_t deviation = L * vector_t::NullaryExpr(primalSolution.stateTrajectory_.front().size(), standardNormalNullaryOp); + const vector_t deviation = L * vector_t::NullaryExpr(primalSolution_.stateTrajectory_.front().size(), standardNormalNullaryOp); dataArray_.push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); } } - // update behavioral controller with MPC controller - behavioralControllerPtr->setOptimalController(*primalSolution.controllerPtr_); - - // forward simulate system with behavioral controller - scalar_array_t timeTrajectory; - size_array_t postEventIndicesStock; - vector_array_t stateTrajectory; - vector_array_t inputTrajectory; - rolloutPtr_->run(primalSolution.timeTrajectory_.front(), primalSolution.stateTrajectory_.front(), - primalSolution.timeTrajectory_.front() + timeStep, behavioralControllerPtr.get(), - primalSolution.modeSchedule_.eventTimes, timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); - - // update time, state and iteration - time = timeTrajectory.back(); - state = stateTrajectory.back(); + // update iteration ++iteration; - - // check if forward simulated system diverged - if (!mpcnetDefinitionPtr_->validState(state)) { - throw std::runtime_error("[MpcnetDataGeneration::run] state is not valid."); - } } } catch (const std::exception& e) { // print error for exceptions diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index f45177389..44b58a9d4 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -35,60 +35,26 @@ namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -metrics_t MpcnetPolicyEvaluation::run(const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, - const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { +metrics_t MpcnetPolicyEvaluation::run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, + const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { // declare metrics metrics_t metrics; - // init time and state - scalar_t time = initialObservation.time; - vector_t state = initialObservation.state; - - // reset mpc - mpcPtr_->reset(); - - // reset rollout, i.e. reset the internal simulator state (e.g. relevant for RaiSim) - rolloutPtr_->resetRollout(); - - // prepare learned controller - mpcnetPtr_->loadPolicyModel(policyFilePath); - - // update the reference manager - referenceManagerPtr_->setModeSchedule(modeSchedule); - referenceManagerPtr_->setTargetTrajectories(targetTrajectories); + // init system + init(alpha, policyFilePath, initialObservation, modeSchedule, targetTrajectories); // run policy evaluation - int iteration = 0; try { - while (time <= targetTrajectories.timeTrajectory.back()) { - // run mpc and get solution - if (!mpcPtr_->run(time, state)) { - throw std::runtime_error("[MpcnetPolicyEvaluation::run] main routine of MPC returned false."); - } - const auto primalSolution = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); + while (systemObservation_.time <= targetTrajectories.timeTrajectory.back()) { + // step system + step(timeStep); // incurred quantities - vector_t input = mpcnetPtr_->computeInput(time, state); + const scalar_t time = primalSolution_.timeTrajectory_.front(); + const vector_t state = primalSolution_.stateTrajectory_.front(); + const vector_t input = behavioralControllerPtr_->computeInput(time, state); metrics.incurredHamiltonian += mpcPtr_->getSolverPtr()->getHamiltonian(time, state, input).f * timeStep; - - // forward simulate system with learned controller - scalar_array_t timeTrajectory; - size_array_t postEventIndicesStock; - vector_array_t stateTrajectory; - vector_array_t inputTrajectory; - rolloutPtr_->run(primalSolution.timeTrajectory_.front(), primalSolution.stateTrajectory_.front(), - primalSolution.timeTrajectory_.front() + timeStep, mpcnetPtr_.get(), primalSolution.modeSchedule_.eventTimes, - timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); - - // update time, state and iteration - time = timeTrajectory.back(); - state = stateTrajectory.back(); - ++iteration; - - // check if forward simulated system diverged - if (!mpcnetDefinitionPtr_->validState(state)) { - throw std::runtime_error("[MpcnetPolicyEvaluation::run] state is not valid."); - } } } catch (const std::exception& e) { // print error for exceptions @@ -98,7 +64,7 @@ metrics_t MpcnetPolicyEvaluation::run(const std::string& policyFilePath, scalar_ } // report survival time - metrics.survivalTime = time; + metrics.survivalTime = systemObservation_.time; // return metrics return metrics; diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp new file mode 100644 index 000000000..6b6106132 --- /dev/null +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp @@ -0,0 +1,100 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_mpcnet/rollout/MpcnetRolloutBase.h" + +#include "ocs2_mpcnet/control/MpcnetBehavioralController.h" + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetRolloutBase::init(scalar_t alpha, const std::string& policyFilePath, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { + // init system observation + systemObservation_ = initialObservation; + + // reset mpc + mpcPtr_->reset(); + + // prepare learned controller + mpcnetPtr_->loadPolicyModel(policyFilePath); + + // reset rollout, i.e. reset the internal simulator state (e.g. relevant for RaiSim) + rolloutPtr_->resetRollout(); + + // update the reference manager + referenceManagerPtr_->setModeSchedule(modeSchedule); + referenceManagerPtr_->setTargetTrajectories(targetTrajectories); + + // set up behavioral controller with mixture parameter alpha and learned controller + behavioralControllerPtr_->setAlpha(alpha); + behavioralControllerPtr_->setLearnedController(*mpcnetPtr_); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetRolloutBase::step(scalar_t timeStep) { + // run mpc + if (!mpcPtr_->run(systemObservation_.time, systemObservation_.state)) { + throw std::runtime_error("[MpcnetRolloutBase::step] main routine of MPC returned false."); + } + + // update primal solution + primalSolution_ = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); + + // update behavioral controller with MPC controller + behavioralControllerPtr_->setOptimalController(*primalSolution_.controllerPtr_); + + // forward simulate system with behavioral controller + scalar_array_t timeTrajectory; + size_array_t postEventIndicesStock; + vector_array_t stateTrajectory; + vector_array_t inputTrajectory; + rolloutPtr_->run(primalSolution_.timeTrajectory_.front(), primalSolution_.stateTrajectory_.front(), + primalSolution_.timeTrajectory_.front() + timeStep, behavioralControllerPtr_.get(), + primalSolution_.modeSchedule_.eventTimes, timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); + + // update system observation + systemObservation_.time = timeTrajectory.back(); + systemObservation_.state = stateTrajectory.back(); + systemObservation_.input = inputTrajectory.back(); + systemObservation_.mode = primalSolution_.modeSchedule_.modeAtTime(systemObservation_.time); + + // check if forward simulated system diverged + if (!mpcnetDefinitionPtr_->validState(systemObservation_.state)) { + throw std::runtime_error("[MpcnetRolloutBase::step] state is not valid."); + } +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp index fa888bb73..493cd026a 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp @@ -163,7 +163,7 @@ data_array_t MpcnetRolloutManager::getGeneratedData() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MpcnetRolloutManager::startPolicyEvaluation(const std::string& policyFilePath, scalar_t timeStep, +void MpcnetRolloutManager::startPolicyEvaluation(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, const std::vector<SystemObservation>& initialObservations, const std::vector<ModeSchedule>& modeSchedules, const std::vector<TargetTrajectories>& targetTrajectories) { @@ -178,8 +178,8 @@ void MpcnetRolloutManager::startPolicyEvaluation(const std::string& policyFilePa // push tasks into pool for (int i = 0; i < initialObservations.size(); i++) { policyEvaluationFtrs_.push_back(policyEvaluationThreadPoolPtr_->run([=](int threadNumber) { - const auto result = policyEvaluationPtrs_[threadNumber]->run(policyFilePath, timeStep, initialObservations.at(i), modeSchedules.at(i), - targetTrajectories.at(i)); + const auto result = policyEvaluationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, initialObservations.at(i), + modeSchedules.at(i), targetTrajectories.at(i)); nPolicyEvaluationTasksDone_++; // print thread and task number std::cerr << "Policy evaluation thread " << threadNumber << " finished task " << nPolicyEvaluationTasksDone_ << "\n"; From 748cf41ec4e8836bfc6c34c4e18e3cde497d0260 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 18 Mar 2022 15:48:31 +0100 Subject: [PATCH 115/512] adress comments for step method --- .../ocs2_mpcnet/rollout/MpcnetDataGeneration.h | 7 ++++++- .../ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h | 7 ++++++- .../ocs2_mpcnet/rollout/MpcnetRolloutBase.h | 16 +++++++++------- ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp | 4 ++-- .../src/rollout/MpcnetPolicyEvaluation.cpp | 4 ++-- ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp | 4 ++-- 6 files changed, 27 insertions(+), 15 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h index 57c3b0a30..f916b32fc 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h @@ -42,7 +42,12 @@ namespace mpcnet { class MpcnetDataGeneration final : public MpcnetRolloutBase { public: /** - * @see MpcnetRolloutBase::MpcnetRolloutBase() + * Constructor. + * @param [in] mpcPtr : Pointer to the MPC solver to be used (this class takes ownership). + * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions to be used (shared ownership). + * @param [in] referenceManagerPtr : Pointer to the reference manager to be used (shared ownership). */ MpcnetDataGeneration(std::unique_ptr<MPC_BASE> mpcPtr, std::unique_ptr<MpcnetControllerBase> mpcnetPtr, std::unique_ptr<RolloutBase> rolloutPtr, std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h index d617e81f9..188475b69 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h @@ -42,7 +42,12 @@ namespace mpcnet { class MpcnetPolicyEvaluation final : public MpcnetRolloutBase { public: /** - * @see MpcnetRolloutBase::MpcnetRolloutBase() + * Constructor. + * @param [in] mpcPtr : Pointer to the MPC solver to be used (this class takes ownership). + * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions to be used (shared ownership). + * @param [in] referenceManagerPtr : Pointer to the reference manager to be used (shared ownership). */ MpcnetPolicyEvaluation(std::unique_ptr<MPC_BASE> mpcPtr, std::unique_ptr<MpcnetControllerBase> mpcnetPtr, std::unique_ptr<RolloutBase> rolloutPtr, std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr, diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h index 6b2a528d6..51f623dfb 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h @@ -82,16 +82,17 @@ class MpcnetRolloutBase { */ MpcnetRolloutBase& operator=(const MpcnetRolloutBase&) = delete; + protected: /** - * Initialize the system. + * (Re)set system components. * @param [in] alpha : The mixture parameter for the behavioral controller. * @param [in] policyFilePath : The path to the file with the learned policy for the controller. * @param [in] initialObservation : The initial system observation to start from (time and state required). * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. * @param [in] targetTrajectories : The target trajectories to be tracked. */ - void init(scalar_t alpha, const std::string& policyFilePath, const SystemObservation& initialObservation, - const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); + void set(scalar_t alpha, const std::string& policyFilePath, const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories); /** * Simulate the system one step forward. @@ -99,15 +100,16 @@ class MpcnetRolloutBase { */ void step(scalar_t timeStep); - protected: std::unique_ptr<MPC_BASE> mpcPtr_; - std::unique_ptr<MpcnetControllerBase> mpcnetPtr_; - std::unique_ptr<RolloutBase> rolloutPtr_; std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr_; - std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; std::unique_ptr<MpcnetBehavioralController> behavioralControllerPtr_; SystemObservation systemObservation_; PrimalSolution primalSolution_; + + private: + std::unique_ptr<MpcnetControllerBase> mpcnetPtr_; + std::unique_ptr<RolloutBase> rolloutPtr_; + std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; }; } // namespace mpcnet diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp index ebc4acb0f..39aba49c7 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp @@ -46,8 +46,8 @@ const data_array_t* MpcnetDataGeneration::run(scalar_t alpha, const std::string& // clear data array dataArray_.clear(); - // init system - init(alpha, policyFilePath, initialObservation, modeSchedule, targetTrajectories); + // set system + set(alpha, policyFilePath, initialObservation, modeSchedule, targetTrajectories); // set up scalar standard normal generator and compute Cholesky decomposition of covariance matrix std::random_device randomDevice; diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp index 44b58a9d4..9affeb6d0 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp @@ -41,8 +41,8 @@ metrics_t MpcnetPolicyEvaluation::run(scalar_t alpha, const std::string& policyF // declare metrics metrics_t metrics; - // init system - init(alpha, policyFilePath, initialObservation, modeSchedule, targetTrajectories); + // set system + set(alpha, policyFilePath, initialObservation, modeSchedule, targetTrajectories); // run policy evaluation try { diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp index 6b6106132..afa7e1011 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp @@ -37,8 +37,8 @@ namespace mpcnet { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void MpcnetRolloutBase::init(scalar_t alpha, const std::string& policyFilePath, const SystemObservation& initialObservation, - const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { +void MpcnetRolloutBase::set(scalar_t alpha, const std::string& policyFilePath, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { // init system observation systemObservation_ = initialObservation; From 0d2b34c6fa392a4e06df13fcc7ac2f27fb8c2392 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 18 Mar 2022 17:00:50 +0100 Subject: [PATCH 116/512] address further comments --- .../rollout/MpcnetRolloutManager.h | 3 +- .../src/rollout/MpcnetRolloutManager.cpp | 34 ++++++++++--------- 2 files changed, 20 insertions(+), 17 deletions(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h index 092ee05f5..24b2d91ad 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h @@ -90,7 +90,7 @@ class MpcnetRolloutManager { * Get the data generated from the data generation rollout. * @return The generated data. */ - data_array_t getGeneratedData(); + const data_array_t& getGeneratedData(); /** * Starts the policy evaluation forward simulated by a behavioral controller. @@ -124,6 +124,7 @@ class MpcnetRolloutManager { std::unique_ptr<ThreadPool> dataGenerationThreadPoolPtr_; std::vector<std::unique_ptr<MpcnetDataGeneration>> dataGenerationPtrs_; std::vector<std::future<const data_array_t*>> dataGenerationFtrs_; + data_array_t dataArray_; // policy evaluation variables size_t nPolicyEvaluationThreads_; std::atomic_int nPolicyEvaluationTasksDone_; diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp index 493cd026a..5013e9411 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp @@ -84,7 +84,7 @@ void MpcnetRolloutManager::startDataGeneration(scalar_t alpha, const std::string // push tasks into pool for (int i = 0; i < initialObservations.size(); i++) { - dataGenerationFtrs_.push_back(dataGenerationThreadPoolPtr_->run([=](int threadNumber) { + dataGenerationFtrs_.push_back(dataGenerationThreadPoolPtr_->run([&](int threadNumber) { const auto* result = dataGenerationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, dataDecimation, nSamples, samplingCovariance, initialObservations.at(i), modeSchedules.at(i), targetTrajectories.at(i)); @@ -121,7 +121,7 @@ bool MpcnetRolloutManager::isDataGenerationDone() { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -data_array_t MpcnetRolloutManager::getGeneratedData() { +const data_array_t& MpcnetRolloutManager::getGeneratedData() { if (nDataGenerationThreads_ <= 0) { throw std::runtime_error("[MpcnetRolloutManager::getGeneratedData] cannot work without at least one data generation thread."); } @@ -129,12 +129,16 @@ data_array_t MpcnetRolloutManager::getGeneratedData() { throw std::runtime_error("[MpcnetRolloutManager::getGeneratedData] cannot get data when data generation is not done."); } + // clear data array + dataArray_.clear(); + // get pointers to data std::vector<const data_array_t*> dataPtrs; - for (int i = 0; i < dataGenerationFtrs_.size(); i++) { + dataPtrs.reserve(dataGenerationFtrs_.size()); + for (auto& dataGenerationFtr : dataGenerationFtrs_) { try { // get results from futures of the tasks - dataPtrs.push_back(dataGenerationFtrs_[i].get()); + dataPtrs.push_back(dataGenerationFtr.get()); } catch (const std::exception& e) { // print error for exceptions std::cerr << "[MpcnetRolloutManager::getGeneratedData] a standard exception was caught, with message: " << e.what() << "\n"; @@ -148,16 +152,13 @@ data_array_t MpcnetRolloutManager::getGeneratedData() { } // fill data array - data_array_t dataArray; - dataArray.reserve(nDataPoints); - for (int i = 0; i < dataPtrs.size(); i++) { - for (int j = 0; j < dataPtrs[i]->size(); j++) { - dataArray.push_back((*dataPtrs[i])[j]); - } + dataArray_.reserve(nDataPoints); + for (const auto dataPtr : dataPtrs) { + dataArray_.insert(dataArray_.end(), dataPtr->begin(), dataPtr->end()); } - // return data - return dataArray; + // return data array + return dataArray_; } /******************************************************************************************************/ @@ -177,7 +178,7 @@ void MpcnetRolloutManager::startPolicyEvaluation(scalar_t alpha, const std::stri // push tasks into pool for (int i = 0; i < initialObservations.size(); i++) { - policyEvaluationFtrs_.push_back(policyEvaluationThreadPoolPtr_->run([=](int threadNumber) { + policyEvaluationFtrs_.push_back(policyEvaluationThreadPoolPtr_->run([&](int threadNumber) { const auto result = policyEvaluationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, initialObservations.at(i), modeSchedules.at(i), targetTrajectories.at(i)); nPolicyEvaluationTasksDone_++; @@ -223,17 +224,18 @@ metrics_array_t MpcnetRolloutManager::getComputedMetrics() { // get metrics and fill metrics array metrics_array_t metricsArray; - for (int i = 0; i < policyEvaluationFtrs_.size(); i++) { + metricsArray.reserve(policyEvaluationFtrs_.size()); + for (auto& policyEvaluationFtr : policyEvaluationFtrs_) { try { // get results from futures of the tasks - metricsArray.push_back(policyEvaluationFtrs_[i].get()); + metricsArray.push_back(policyEvaluationFtr.get()); } catch (const std::exception& e) { // print error for exceptions std::cerr << "[MpcnetRolloutManager::getComputedMetrics] a standard exception was caught, with message: " << e.what() << "\n"; } } - // return metrics + // return metrics array return metricsArray; } From f170b50e746bb9bf460eeb5d59eed161723cb08d Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 21 Mar 2022 10:57:43 +0100 Subject: [PATCH 117/512] adapt to changes in ocs2 mpcnet core package --- .../BallbotMpcnetDefinition.h | 2 +- .../BallbotMpcnetInterface.h | 2 +- .../ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 14 +++++++------- .../src/BallbotMpcnetDummyNode.cpp | 15 ++++++++------- .../src/BallbotMpcnetInterface.cpp | 18 +++++++++--------- 5 files changed, 26 insertions(+), 25 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h index 23947b470..55d5738d7 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h @@ -37,7 +37,7 @@ namespace ballbot { /** * MPC-Net definitions for ballbot. */ -class BallbotMpcnetDefinition : public MpcnetDefinitionBase { +class BallbotMpcnetDefinition : public ocs2::mpcnet::MpcnetDefinitionBase { public: /** * Default constructor. diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h index f9e959f15..f7e7bc8f4 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h @@ -38,7 +38,7 @@ namespace ballbot { /** * Ballbot MPC-Net interface between C++ and Python. */ -class BallbotMpcnetInterface : public MpcnetInterfaceBase { +class BallbotMpcnetInterface : public ocs2::mpcnet::MpcnetInterfaceBase { public: /** * Constructor. diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index b80155644..20b386ada 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -99,7 +99,7 @@ optimizer = torch.optim.Adam(policy.parameters(), lr=learning_rate) -def start_data_generation(alpha, policy): +def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks(data_generation_n_tasks, data_generation_duration) @@ -108,18 +108,18 @@ def start_data_generation(alpha, policy): initial_observations, mode_schedules, target_trajectories) -def start_policy_evaluation(policy): +def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks(policy_evaluation_n_tasks, policy_evaluation_duration) - mpcnet_interface.startPolicyEvaluation(policy_file_path, policy_evaluation_time_step, + mpcnet_interface.startPolicyEvaluation(alpha, policy_file_path, policy_evaluation_time_step, initial_observations, mode_schedules, target_trajectories) try: print("==============\nWaiting for first data.\n==============") - start_data_generation(alpha=1.0, policy=policy) - start_policy_evaluation(policy=policy) + start_data_generation(policy) + start_policy_evaluation(policy) while not mpcnet_interface.isDataGenerationDone(): time.sleep(1.0) @@ -140,7 +140,7 @@ def start_policy_evaluation(policy): writer.add_scalar('data/total_data_points', len(memory), iteration) print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) # start new data generation - start_data_generation(alpha=alpha, policy=policy) + start_data_generation(policy, alpha) # policy evaluation if mpcnet_interface.isPolicyEvaluationDone(): @@ -153,7 +153,7 @@ def start_policy_evaluation(policy): writer.add_scalar('metric/incurred_hamiltonian', incurred_hamiltonian, iteration) print("iteration", iteration, "received metrics:", "incurred_hamiltonian", incurred_hamiltonian, "survival_time", survival_time) # start new policy evaluation - start_policy_evaluation(policy=policy) + start_policy_evaluation(policy) # intermediate policies if (iteration % 1000 == 0) and (iteration > 0): diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp index d455ff5c6..f8efb7c1d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp @@ -68,17 +68,18 @@ int main(int argc, char** argv) { rosReferenceManagerPtr->subscribe(nodeHandle); // policy (MPC-Net controller) - auto onnxEnvironmentPtr = createOnnxEnvironment(); - std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition()); - std::unique_ptr<MpcnetControllerBase> mpcnetControllerPtr( - new MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); + auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); + std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition()); + std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase> mpcnetControllerPtr( + new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); mpcnetControllerPtr->loadPolicyModel(policyFilePath); // rollout std::unique_ptr<RolloutBase> rolloutPtr(ballbotInterface.getRollout().clone()); // observer - std::shared_ptr<MpcnetDummyObserverRos> mpcnetDummyObserverRosPtr(new MpcnetDummyObserverRos(nodeHandle, robotName)); + std::shared_ptr<ocs2::mpcnet::MpcnetDummyObserverRos> mpcnetDummyObserverRosPtr( + new ocs2::mpcnet::MpcnetDummyObserverRos(nodeHandle, robotName)); // visualization std::shared_ptr<BallbotDummyVisualization> ballbotDummyVisualization(new BallbotDummyVisualization(nodeHandle)); @@ -86,8 +87,8 @@ int main(int argc, char** argv) { // MPC-Net dummy loop ROS scalar_t controlFrequency = ballbotInterface.mpcSettings().mrtDesiredFrequency_; scalar_t rosFrequency = ballbotInterface.mpcSettings().mpcDesiredFrequency_; - MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), - rosReferenceManagerPtr); + ocs2::mpcnet::MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), + rosReferenceManagerPtr); mpcnetDummyLoopRos.addObserver(mpcnetDummyObserverRosPtr); mpcnetDummyLoopRos.addObserver(ballbotDummyVisualization); diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index 6cc21d6df..720a56dfd 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -43,16 +43,16 @@ namespace ballbot { BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim) { // create ONNX environment - auto onnxEnvironmentPtr = createOnnxEnvironment(); + auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); // path to config file std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/mpc/task.info"; // path to save auto-generated libraries std::string libraryFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; // set up MPC-Net rollout manager for data generation and policy evaluation std::vector<std::unique_ptr<MPC_BASE>> mpcPtrs; - std::vector<std::unique_ptr<MpcnetControllerBase>> mpcnetPtrs; + std::vector<std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>> mpcnetPtrs; std::vector<std::unique_ptr<RolloutBase>> rolloutPtrs; - std::vector<std::shared_ptr<MpcnetDefinitionBase>> mpcnetDefinitionPtrs; + std::vector<std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase>> mpcnetDefinitionPtrs; std::vector<std::shared_ptr<ReferenceManagerInterface>> referenceManagerPtrs; mpcPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); mpcnetPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); @@ -61,10 +61,10 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { BallbotInterface ballbotInterface(taskFile, libraryFolder); - std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition()); + std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition()); mpcPtrs.push_back(getMpc(ballbotInterface)); - mpcnetPtrs.push_back(std::unique_ptr<MpcnetControllerBase>( - new MpcnetOnnxController(mpcnetDefinitionPtr, ballbotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr))); + mpcnetPtrs.push_back(std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>( + new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, ballbotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr))); if (raisim) { throw std::runtime_error("BallbotMpcnetInterface::BallbotMpcnetInterface RaiSim rollout not yet implemented for ballbot."); } else { @@ -73,9 +73,9 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si mpcnetDefinitionPtrs.push_back(mpcnetDefinitionPtr); referenceManagerPtrs.push_back(ballbotInterface.getReferenceManagerPtr()); } - mpcnetRolloutManagerPtr_.reset(new MpcnetRolloutManager(nDataGenerationThreads, nPolicyEvaluationThreads, std::move(mpcPtrs), - std::move(mpcnetPtrs), std::move(rolloutPtrs), mpcnetDefinitionPtrs, - referenceManagerPtrs)); + mpcnetRolloutManagerPtr_.reset(new ocs2::mpcnet::MpcnetRolloutManager(nDataGenerationThreads, nPolicyEvaluationThreads, + std::move(mpcPtrs), std::move(mpcnetPtrs), std::move(rolloutPtrs), + mpcnetDefinitionPtrs, referenceManagerPtrs)); } /******************************************************************************************************/ From 1e39603c58c469db42af1f1301bdd7709bc7de7d Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 21 Mar 2022 11:37:11 +0100 Subject: [PATCH 118/512] fix: error when building robot-specific mpcnet definition --- ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h index 760e05228..cbc971b66 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h @@ -40,6 +40,11 @@ namespace mpcnet { */ class MpcnetDefinitionBase { public: + /** + * Default constructor. + */ + MpcnetDefinitionBase() = default; + /** * Default destructor. */ From 48f303c415760721b39e68d4735fc55c93bb92e2 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 21 Mar 2022 11:39:25 +0100 Subject: [PATCH 119/512] fix: error when python defined rollout variables go out of scope --- ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp index 5013e9411..af8e119e1 100644 --- a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp @@ -84,7 +84,7 @@ void MpcnetRolloutManager::startDataGeneration(scalar_t alpha, const std::string // push tasks into pool for (int i = 0; i < initialObservations.size(); i++) { - dataGenerationFtrs_.push_back(dataGenerationThreadPoolPtr_->run([&](int threadNumber) { + dataGenerationFtrs_.push_back(dataGenerationThreadPoolPtr_->run([=](int threadNumber) { const auto* result = dataGenerationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, dataDecimation, nSamples, samplingCovariance, initialObservations.at(i), modeSchedules.at(i), targetTrajectories.at(i)); @@ -178,7 +178,7 @@ void MpcnetRolloutManager::startPolicyEvaluation(scalar_t alpha, const std::stri // push tasks into pool for (int i = 0; i < initialObservations.size(); i++) { - policyEvaluationFtrs_.push_back(policyEvaluationThreadPoolPtr_->run([&](int threadNumber) { + policyEvaluationFtrs_.push_back(policyEvaluationThreadPoolPtr_->run([=](int threadNumber) { const auto result = policyEvaluationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, initialObservations.at(i), modeSchedules.at(i), targetTrajectories.at(i)); nPolicyEvaluationTasksDone_++; From 993f10bc9996c54314e303bd31e0bd2db51efec4 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 21 Mar 2022 11:41:28 +0100 Subject: [PATCH 120/512] fix: mpcnet behavioral controller uninitialized, owns nothing, return nullptr --- .../include/ocs2_mpcnet/control/MpcnetBehavioralController.h | 1 + ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h index 87d9837c4..787f6edfb 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h @@ -54,6 +54,7 @@ class MpcnetBehavioralController final : public ControllerBase { MpcnetBehavioralController(scalar_t alpha, const ControllerBase& optimalController, const MpcnetControllerBase& learnedController) : alpha_(alpha), optimalControllerPtr_(optimalController.clone()), learnedControllerPtr_(learnedController.clone()) {} + MpcnetBehavioralController() = default; ~MpcnetBehavioralController() override = default; MpcnetBehavioralController* clone() const override { return new MpcnetBehavioralController(*this); } diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h index 51f623dfb..475b50581 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h @@ -65,7 +65,8 @@ class MpcnetRolloutBase { mpcnetPtr_(std::move(mpcnetPtr)), rolloutPtr_(std::move(rolloutPtr)), mpcnetDefinitionPtr_(std::move(mpcnetDefinitionPtr)), - referenceManagerPtr_(std::move(referenceManagerPtr)) {} + referenceManagerPtr_(std::move(referenceManagerPtr)), + behavioralControllerPtr_(new MpcnetBehavioralController()) {} /** * Default destructor. From c3762ba9a9e4b4eb2e81fcab14adbe40f4391625 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 21 Mar 2022 12:02:58 +0100 Subject: [PATCH 121/512] tune ballbot sampling around nominal state a bit --- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 20b386ada..f68eb2b47 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -52,8 +52,14 @@ data_generation_n_tasks = 10 data_generation_n_samples = 2 data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order='F') -for i in range(config.STATE_DIM): - data_generation_sampling_covariance[i, i] = 0.01 +for i in range(0, 2): + data_generation_sampling_covariance[i, i] = 0.01 ** 2 # position +for i in range(2, 5): + data_generation_sampling_covariance[i, i] = (1.0 * np.pi / 180.0) ** 2 # orientation +for i in range(5, 7): + data_generation_sampling_covariance[i, i] = 0.05 ** 2 # linear velocity +for i in range(7, 10): + data_generation_sampling_covariance[i, i] = (5.0 * np.pi / 180.0) ** 2 # angular velocity # settings for computing metrics by applying learned policy policy_evaluation_time_step = 0.1 From df804a822c3fc4c194bcb0b35378fca83a128c68 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 21 Mar 2022 14:35:09 +0100 Subject: [PATCH 122/512] adapt to changes in ocs2 mpcnet core package --- .../LeggedRobotMpcnetDefinition.h | 2 +- .../LeggedRobotMpcnetInterface.h | 2 +- .../legged_robot_mpcnet.py | 14 +++++++------- .../src/LeggedRobotMpcnetDummyNode.cpp | 16 +++++++++------- .../src/LeggedRobotMpcnetInterface.cpp | 18 +++++++++--------- 5 files changed, 27 insertions(+), 25 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h index 2edd68b42..92158e474 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h @@ -37,7 +37,7 @@ namespace legged_robot { /** * MPC-Net definitions for legged robot. */ -class LeggedRobotMpcnetDefinition : public MpcnetDefinitionBase { +class LeggedRobotMpcnetDefinition : public ocs2::mpcnet::MpcnetDefinitionBase { public: /** * Constructor. diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h index 904bb5065..6a5d6ab42 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h @@ -39,7 +39,7 @@ namespace legged_robot { /** * Legged robot MPC-Net interface between C++ and Python. */ -class LeggedRobotMpcnetInterface : public MpcnetInterfaceBase { +class LeggedRobotMpcnetInterface : public ocs2::mpcnet::MpcnetInterfaceBase { public: /** * Constructor. diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 03287b1fa..2b2f154b3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -119,7 +119,7 @@ weights = [1, 2, 2] -def start_data_generation(alpha, policy): +def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) choices = random.choices(["stance", "trot_1", "trot_2"], k=data_generation_n_tasks, weights=weights) @@ -129,19 +129,19 @@ def start_data_generation(alpha, policy): initial_observations, mode_schedules, target_trajectories) -def start_policy_evaluation(policy): +def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) choices = random.choices(["stance", "trot_1", "trot_2"], k=policy_evaluation_n_tasks, weights=weights) initial_observations, mode_schedules, target_trajectories = helper.get_tasks(policy_evaluation_n_tasks, policy_evaluation_duration, choices) - mpcnet_interface.startPolicyEvaluation(policy_file_path, policy_evaluation_time_step, + mpcnet_interface.startPolicyEvaluation(alpha, policy_file_path, policy_evaluation_time_step, initial_observations, mode_schedules, target_trajectories) try: print("==============\nWaiting for first data.\n==============") - start_data_generation(alpha=1.0, policy=policy) - start_policy_evaluation(policy=policy) + start_data_generation(policy) + start_policy_evaluation(policy) while not mpcnet_interface.isDataGenerationDone(): time.sleep(1.0) @@ -162,7 +162,7 @@ def start_policy_evaluation(policy): writer.add_scalar('data/total_data_points', len(memory), iteration) print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) # start new data generation - start_data_generation(alpha=alpha, policy=policy) + start_data_generation(policy, alpha) # policy evaluation if mpcnet_interface.isPolicyEvaluationDone(): @@ -175,7 +175,7 @@ def start_policy_evaluation(policy): writer.add_scalar('metric/incurred_hamiltonian', incurred_hamiltonian, iteration) print("iteration", iteration, "received metrics:", "incurred_hamiltonian", incurred_hamiltonian, "survival_time", survival_time) # start new policy evaluation - start_policy_evaluation(policy=policy) + start_policy_evaluation(policy) # intermediate policies if (iteration % 10000 == 0) and (iteration > 0): diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 1a459a0b9..45824a4da 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -78,10 +78,11 @@ int main(int argc, char** argv) { rosReferenceManagerPtr->subscribe(nodeHandle); // policy (MPC-Net controller) - auto onnxEnvironmentPtr = createOnnxEnvironment(); - std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr(new LeggedRobotMpcnetDefinition(leggedRobotInterface.getInitialState())); - std::unique_ptr<MpcnetControllerBase> mpcnetControllerPtr( - new MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); + auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); + std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr( + new LeggedRobotMpcnetDefinition(leggedRobotInterface.getInitialState())); + std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase> mpcnetControllerPtr( + new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); mpcnetControllerPtr->loadPolicyModel(policyFile); // rollout @@ -121,7 +122,8 @@ int main(int argc, char** argv) { } // observer - std::shared_ptr<MpcnetDummyObserverRos> mpcnetDummyObserverRosPtr(new MpcnetDummyObserverRos(nodeHandle, robotName)); + std::shared_ptr<ocs2::mpcnet::MpcnetDummyObserverRos> mpcnetDummyObserverRosPtr( + new ocs2::mpcnet::MpcnetDummyObserverRos(nodeHandle, robotName)); // visualization CentroidalModelPinocchioMapping pinocchioMapping(leggedRobotInterface.getCentroidalModelInfo()); @@ -140,8 +142,8 @@ int main(int argc, char** argv) { // MPC-Net dummy loop ROS scalar_t controlFrequency = leggedRobotInterface.mpcSettings().mrtDesiredFrequency_; scalar_t rosFrequency = leggedRobotInterface.mpcSettings().mpcDesiredFrequency_; - MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), - rosReferenceManagerPtr); + ocs2::mpcnet::MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), + rosReferenceManagerPtr); mpcnetDummyLoopRos.addObserver(mpcnetDummyObserverRosPtr); mpcnetDummyLoopRos.addObserver(leggedRobotVisualizerPtr); mpcnetDummyLoopRos.addSynchronizedModule(gaitReceiverPtr); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 2918d80b8..5da1c37c0 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -45,7 +45,7 @@ namespace legged_robot { LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim) { // create ONNX environment - auto onnxEnvironmentPtr = createOnnxEnvironment(); + auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); // paths to files std::string taskFile = ros::package::getPath("ocs2_legged_robot") + "/config/mpc/task.info"; std::string urdfFile = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf"; @@ -54,9 +54,9 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr std::string resourcePath = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes"; // set up MPC-Net rollout manager for data generation and policy evaluation std::vector<std::unique_ptr<MPC_BASE>> mpcPtrs; - std::vector<std::unique_ptr<MpcnetControllerBase>> mpcnetPtrs; + std::vector<std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>> mpcnetPtrs; std::vector<std::unique_ptr<RolloutBase>> rolloutPtrs; - std::vector<std::shared_ptr<MpcnetDefinitionBase>> mpcnetDefinitionPtrs; + std::vector<std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase>> mpcnetDefinitionPtrs; std::vector<std::shared_ptr<ReferenceManagerInterface>> referenceManagerPtrs; mpcPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); mpcnetPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); @@ -65,11 +65,11 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { leggedRobotInterfacePtrs_.push_back(std::unique_ptr<LeggedRobotInterface>(new LeggedRobotInterface(taskFile, urdfFile, referenceFile))); - std::shared_ptr<MpcnetDefinitionBase> mpcnetDefinitionPtr( + std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr( new LeggedRobotMpcnetDefinition(leggedRobotInterfacePtrs_[i]->getInitialState())); mpcPtrs.push_back(getMpc(*leggedRobotInterfacePtrs_[i])); - mpcnetPtrs.push_back(std::unique_ptr<MpcnetControllerBase>( - new MpcnetOnnxController(mpcnetDefinitionPtr, leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr(), onnxEnvironmentPtr))); + mpcnetPtrs.push_back(std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>(new ocs2::mpcnet::MpcnetOnnxController( + mpcnetDefinitionPtr, leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr(), onnxEnvironmentPtr))); if (raisim) { RaisimRolloutSettings raisimRolloutSettings(raisimFile, "rollout"); raisimRolloutSettings.portNumber_ += i; @@ -98,9 +98,9 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr mpcnetDefinitionPtrs.push_back(mpcnetDefinitionPtr); referenceManagerPtrs.push_back(leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr()); } - mpcnetRolloutManagerPtr_.reset(new MpcnetRolloutManager(nDataGenerationThreads, nPolicyEvaluationThreads, std::move(mpcPtrs), - std::move(mpcnetPtrs), std::move(rolloutPtrs), mpcnetDefinitionPtrs, - referenceManagerPtrs)); + mpcnetRolloutManagerPtr_.reset(new ocs2::mpcnet::MpcnetRolloutManager(nDataGenerationThreads, nPolicyEvaluationThreads, + std::move(mpcPtrs), std::move(mpcnetPtrs), std::move(rolloutPtrs), + mpcnetDefinitionPtrs, referenceManagerPtrs)); } /******************************************************************************************************/ From d21b9a52ede46f6a259263c16e3004e9b2a4ca8a Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 21 Mar 2022 16:05:20 +0100 Subject: [PATCH 123/512] run black --- ocs2_mpcnet/python/ocs2_mpcnet/helper.py | 17 +++++++++--- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 22 ++++++++++++--- ocs2_mpcnet/python/ocs2_mpcnet/memory.py | 34 +++++++++++++++++++----- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 28 +++++++------------ ocs2_mpcnet/setup.py | 5 +--- 5 files changed, 69 insertions(+), 37 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py index c58d74bbf..54db924ce 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py @@ -30,8 +30,17 @@ import torch import numpy as np -from ocs2_mpcnet import size_array, scalar_array, vector_array, SystemObservation, SystemObservationArray,\ - ModeSchedule, ModeScheduleArray, TargetTrajectories, TargetTrajectoriesArray +from ocs2_mpcnet import ( + size_array, + scalar_array, + vector_array, + SystemObservation, + SystemObservationArray, + ModeSchedule, + ModeScheduleArray, + TargetTrajectories, + TargetTrajectoriesArray, +) def bdot(bv1, bv2): @@ -119,7 +128,9 @@ def get_event_times_and_mode_sequence(default_mode, duration, event_times_templa event_times = np.array([0.0], dtype=np.float64) mode_sequence = np.array([default_mode], dtype=np.uintp) for _ in range(num_gait_cycles): - event_times = np.append(event_times, event_times[-1] * np.ones(len(event_times_template)) + event_times_template) + event_times = np.append( + event_times, event_times[-1] * np.ones(len(event_times_template)) + event_times_template + ) mode_sequence = np.append(mode_sequence, mode_sequence_template) mode_sequence = np.append(mode_sequence, np.array([default_mode], dtype=np.uintp)) return event_times, mode_sequence diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index 64e47781b..ff90c7cce 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -48,7 +48,14 @@ def compute_sample(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHd else: dx = torch.sub(x_inquiry, x_nominal) du = torch.sub(u_inquiry, u_nominal) - return 0.5 * torch.dot(dx, torch.mv(dHdxx, dx)) + torch.dot(du, torch.mv(dHdux, dx)) + 0.5 * torch.dot(du, torch.mv(dHduu, du)) + torch.dot(dHdx, dx) + torch.dot(dHdu, du) + H + return ( + 0.5 * torch.dot(dx, torch.mv(dHdxx, dx)) + + torch.dot(du, torch.mv(dHdux, dx)) + + 0.5 * torch.dot(du, torch.mv(dHduu, du)) + + torch.dot(dHdx, dx) + + torch.dot(dHdu, du) + + H + ) @staticmethod def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): @@ -61,7 +68,14 @@ def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHdu else: dx = torch.sub(x_inquiry, x_nominal) du = torch.sub(u_inquiry, u_nominal) - return 0.5 * bdot(dx, bmv(dHdxx, dx)) + bdot(du, bmv(dHdux, dx)) + 0.5 * bdot(du, bmv(dHduu, du)) + bdot(dHdx, dx) + bdot(dHdu, du) + H + return ( + 0.5 * bdot(dx, bmv(dHdxx, dx)) + + bdot(du, bmv(dHdux, dx)) + + 0.5 * bdot(du, bmv(dHduu, du)) + + bdot(dHdx, dx) + + bdot(dHdu, du) + + H + ) class BehavioralCloning: @@ -91,7 +105,7 @@ def __init__(self, epsilon): self.epsilon = epsilon def compute_sample(self, p_target, p_predicted): - return - torch.dot(p_target, torch.log(p_predicted + self.epsilon)) + return -torch.dot(p_target, torch.log(p_predicted + self.epsilon)) def compute_batch(self, p_target, p_predicted): - return - bdot(p_target, torch.log(p_predicted + self.epsilon)) + return -bdot(p_target, torch.log(p_predicted + self.epsilon)) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py index 5263db760..abd9dcc28 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py @@ -33,7 +33,6 @@ class CircularMemory: - def __init__(self, capacity, time_dimension, state_dimension, input_dimension, expert_number=1): # init variables self.capacity = capacity @@ -46,7 +45,9 @@ def __init__(self, capacity, time_dimension, state_dimension, input_dimension, e self.p = torch.zeros(capacity, expert_number, device=config.device, dtype=config.dtype) self.generalized_time = torch.zeros(capacity, time_dimension, device=config.device, dtype=config.dtype) self.relative_state = torch.zeros(capacity, state_dimension, device=config.device, dtype=config.dtype) - self.input_transformation = torch.zeros(capacity, input_dimension, input_dimension, device=config.device, dtype=config.dtype) + self.input_transformation = torch.zeros( + capacity, input_dimension, input_dimension, device=config.device, dtype=config.dtype + ) self.dHdxx = torch.zeros(capacity, state_dimension, state_dimension, device=config.device, dtype=config.dtype) self.dHdux = torch.zeros(capacity, input_dimension, state_dimension, device=config.device, dtype=config.dtype) self.dHduu = torch.zeros(capacity, input_dimension, input_dimension, device=config.device, dtype=config.dtype) @@ -62,9 +63,15 @@ def push(self, t, x, u, p, generalized_time, relative_state, input_transformatio self.x[self.position].copy_(torch.as_tensor(x, dtype=None, device=torch.device("cpu"))) self.u[self.position].copy_(torch.as_tensor(u, dtype=None, device=torch.device("cpu"))) self.p[self.position].copy_(torch.as_tensor(p, dtype=None, device=torch.device("cpu"))) - self.generalized_time[self.position].copy_(torch.as_tensor(generalized_time, dtype=None, device=torch.device("cpu"))) - self.relative_state[self.position].copy_(torch.as_tensor(relative_state, dtype=None, device=torch.device("cpu"))) - self.input_transformation[self.position].copy_(torch.as_tensor(input_transformation, dtype=None, device=torch.device("cpu"))) + self.generalized_time[self.position].copy_( + torch.as_tensor(generalized_time, dtype=None, device=torch.device("cpu")) + ) + self.relative_state[self.position].copy_( + torch.as_tensor(relative_state, dtype=None, device=torch.device("cpu")) + ) + self.input_transformation[self.position].copy_( + torch.as_tensor(input_transformation, dtype=None, device=torch.device("cpu")) + ) self.dHdxx[self.position].copy_(torch.as_tensor(hamiltonian.dfdxx, dtype=None, device=torch.device("cpu"))) self.dHdux[self.position].copy_(torch.as_tensor(hamiltonian.dfdux, dtype=None, device=torch.device("cpu"))) self.dHduu[self.position].copy_(torch.as_tensor(hamiltonian.dfduu, dtype=None, device=torch.device("cpu"))) @@ -90,8 +97,21 @@ def sample(self, batch_size): dHdx_batch = self.dHdx[indices] dHdu_batch = self.dHdu[indices] H_batch = self.H[indices] - return t_batch, x_batch, u_batch, p_batch, generalized_time_batch, relative_state_batch,\ - input_transformation_batch, dHdxx_batch, dHdux_batch, dHduu_batch, dHdx_batch, dHdu_batch, H_batch + return ( + t_batch, + x_batch, + u_batch, + p_batch, + generalized_time_batch, + relative_state_batch, + input_transformation_batch, + dHdxx_batch, + dHdux_batch, + dHduu_batch, + dHdx_batch, + dHdu_batch, + H_batch, + ) def __len__(self): return self.size diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index 36f6778cf..46ac45b37 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -34,19 +34,17 @@ class Policy(torch.nn.Module): - def __init__(self, dim_in, dim_out): super().__init__() - self.name = 'Policy' + self.name = "Policy" self.dim_in = dim_in self.dim_out = dim_out class LinearPolicy(Policy): - def __init__(self, dim_t, dim_x, dim_u): super().__init__(dim_t + dim_x, dim_u) - self.name = 'LinearPolicy' + self.name = "LinearPolicy" self.linear = torch.nn.Linear(self.dim_in, self.dim_out) def forward(self, t, x): @@ -55,10 +53,9 @@ def forward(self, t, x): class NonlinearPolicy(Policy): - def __init__(self, dim_t, dim_x, dim_u): super().__init__(dim_t + dim_x, dim_u) - self.name = 'NonlinearPolicy' + self.name = "NonlinearPolicy" self.dim_hidden = int((self.dim_in + dim_u) / 2) self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) self.activation = torch.nn.Tanh() @@ -71,16 +68,12 @@ def forward(self, t, x): class MixtureOfLinearExpertsPolicy(Policy): - def __init__(self, dim_t, dim_x, dim_u, num_experts): super().__init__(dim_t + dim_x, dim_u) - self.name = 'MixtureOfLinearExpertsPolicy' + self.name = "MixtureOfLinearExpertsPolicy" self.num_experts = num_experts # gating - self.gating_net = torch.nn.Sequential( - torch.nn.Linear(self.dim_in, self.num_experts), - torch.nn.Softmax(dim=1) - ) + self.gating_net = torch.nn.Sequential(torch.nn.Linear(self.dim_in, self.num_experts), torch.nn.Softmax(dim=1)) # experts self.expert_nets = torch.nn.ModuleList( [LinearExpert(i, self.dim_in, self.dim_out) for i in range(self.num_experts)] @@ -94,10 +87,9 @@ def forward(self, t, x): class MixtureOfNonlinearExpertsPolicy(Policy): - def __init__(self, dim_t, dim_x, dim_u, num_experts): super().__init__(dim_t + dim_x, dim_u) - self.name = 'MixtureOfNonlinearExpertsPolicy' + self.name = "MixtureOfNonlinearExpertsPolicy" self.num_experts = num_experts self.dim_hidden_expert = int((self.dim_in + dim_u) / 2) self.dim_hidden_gating = int((self.dim_in + num_experts) / 2) @@ -106,7 +98,7 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): torch.nn.Linear(self.dim_in, self.dim_hidden_gating), torch.nn.Tanh(), torch.nn.Linear(self.dim_hidden_gating, self.num_experts), - torch.nn.Softmax(dim=1) + torch.nn.Softmax(dim=1), ) # experts self.expert_nets = torch.nn.ModuleList( @@ -121,10 +113,9 @@ def forward(self, t, x): class LinearExpert(torch.nn.Module): - def __init__(self, id, dim_in, dim_out): super().__init__() - self.name = 'LinearExpert' + str(id) + self.name = "LinearExpert" + str(id) self.dim_in = dim_in self.dim_out = dim_out self.linear = torch.nn.Linear(self.dim_in, self.dim_out) @@ -134,10 +125,9 @@ def forward(self, input): class NonlinearExpert(torch.nn.Module): - def __init__(self, id, dim_in, dim_hidden, dim_out): super().__init__() - self.name = 'NonlinearExpert' + str(id) + self.name = "NonlinearExpert" + str(id) self.dim_in = dim_in self.dim_hidden = dim_hidden self.dim_out = dim_out diff --git a/ocs2_mpcnet/setup.py b/ocs2_mpcnet/setup.py index 55a253e33..c4235d359 100644 --- a/ocs2_mpcnet/setup.py +++ b/ocs2_mpcnet/setup.py @@ -3,9 +3,6 @@ from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup -setup_args = generate_distutils_setup( - packages=['ocs2_mpcnet'], - package_dir={'': 'python'} -) +setup_args = generate_distutils_setup(packages=["ocs2_mpcnet"], package_dir={"": "python"}) setup(**setup_args) From 9bed20173d1098a5883d670e9f9778284661fac4 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 21 Mar 2022 16:08:23 +0100 Subject: [PATCH 124/512] add black as requirement for mpcnet --- ocs2_mpcnet/requirements.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/ocs2_mpcnet/requirements.txt b/ocs2_mpcnet/requirements.txt index 92e16611d..535112152 100644 --- a/ocs2_mpcnet/requirements.txt +++ b/ocs2_mpcnet/requirements.txt @@ -2,6 +2,7 @@ ####### requirements.txt ####### # ###### Requirements without version specifiers ###### +black numpy tensorboard torch From 8bb47fb87060639e0c8b8f626877a4f55c8ea531 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 10:21:42 +0100 Subject: [PATCH 125/512] small change for loss functions --- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index ff90c7cce..3948c1ebb 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -29,6 +29,7 @@ import torch +from ocs2_mpcnet_core import config from ocs2_mpcnet.helper import bdot, bmv @@ -83,13 +84,13 @@ class BehavioralCloning: # Uses a simple quadratic function as loss # BC(u) = du' R du - def __init__(self, R, batch_size): - self.R = R - self.R_batch = torch.stack([R for i in range(batch_size)]) + def __init__(self, R): + self.R_sample = torch.tensor(R, device=config.device, dtype=config.dtype) + self.R_batch = self.R_sample.unsqueeze(dim=0) def compute_sample(self, u_predicted, u_target): du = torch.sub(u_predicted, u_target) - return torch.dot(du, torch.mv(self.R, du)) + return torch.dot(du, torch.mv(self.R_sample, du)) def compute_batch(self, u_predicted, u_target): du = torch.sub(u_predicted, u_target) @@ -102,7 +103,7 @@ class CrossEntropy: # CE(p_target, p_predicted) = - sum(p_target * log(p_predicted)) def __init__(self, epsilon): - self.epsilon = epsilon + self.epsilon = torch.tensor(epsilon, device=config.device, dtype=config.dtype) def compute_sample(self, p_target, p_predicted): return -torch.dot(p_target, torch.log(p_predicted + self.epsilon)) From 899bea4b9d5618f49ca8bd9ee4bb7d8e4f8ea35e Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 15:12:51 +0100 Subject: [PATCH 126/512] small change for policies --- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index 46ac45b37..fd0100104 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -48,8 +48,7 @@ def __init__(self, dim_t, dim_x, dim_u): self.linear = torch.nn.Linear(self.dim_in, self.dim_out) def forward(self, t, x): - u = self.linear(torch.cat((t, x), dim=1)) - return u, torch.ones(len(u), 1, device=config.device, dtype=config.dtype), u.unsqueeze(dim=2) + return self.linear(torch.cat((t, x), dim=1)) class NonlinearPolicy(Policy): @@ -62,9 +61,7 @@ def __init__(self, dim_t, dim_x, dim_u): self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) def forward(self, t, x): - z = self.activation(self.linear1(torch.cat((t, x), dim=1))) - u = self.linear2(z) - return u, torch.ones(len(u), 1, device=config.device, dtype=config.dtype), u.unsqueeze(dim=2) + return self.linear2(self.activation(self.linear1(torch.cat((t, x), dim=1)))) class MixtureOfLinearExpertsPolicy(Policy): @@ -83,7 +80,7 @@ def forward(self, t, x): p = self.gating_net(torch.cat((t, x), dim=1)) U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) u = bmv(U, p) - return u, p, U + return u, p class MixtureOfNonlinearExpertsPolicy(Policy): @@ -109,7 +106,7 @@ def forward(self, t, x): p = self.gating_net(torch.cat((t, x), dim=1)) U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) u = bmv(U, p) - return u, p, U + return u, p class LinearExpert(torch.nn.Module): From 36e94efedca9d667b163d34994e3796a5a4e0b0d Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 15:53:38 +0100 Subject: [PATCH 127/512] document mpcnet python code with docstrings --- ocs2_mpcnet/python/ocs2_mpcnet/config.py | 5 + ocs2_mpcnet/python/ocs2_mpcnet/helper.py | 153 ++++++++++++++++- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 130 +++++++++++++- ocs2_mpcnet/python/ocs2_mpcnet/memory.py | 87 +++++++++- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 210 +++++++++++++++++++++++ 5 files changed, 575 insertions(+), 10 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/config.py b/ocs2_mpcnet/python/ocs2_mpcnet/config.py index e1c946e12..1b5b5805b 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/config.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/config.py @@ -27,6 +27,11 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Configuration variables. + +Sets general configuration variables. +""" + import torch # data type for tensor elements diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py index 54db924ce..23d17274f 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py @@ -27,6 +27,11 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Helper functions. + +Provides helper functions, such as convenience functions for batch-wise operations or access to OCC2 types. +""" + import torch import numpy as np @@ -44,21 +49,64 @@ def bdot(bv1, bv2): - # batched dot product + """Batch-wise dot product. + + Performs a batch-wise dot product between two batches of vectors with batch size B and dimension N. Supports + broadcasting for the batch dimension. + + Args: + bv1: A (B,N) tensor containing a batch of vectors. + bv2: A (B,N) tensor containing a batch of vectors. + + Returns: + A (B,N) tensor containing the batch-wise dot product. + """ return torch.sum(torch.mul(bv1, bv2), dim=1) def bmv(bm, bv): - # batched matrix-vector product + """Batch-wise matrix-vector product. + + Performs a batch-wise matrix-vector product between a batch of MxN matrices and a batch of vectors of dimension N, + each with batch size B. Supports broadcasting for the batch dimension. + + Args: + bm: A (B,M,N) tensor containing a batch of matrices. + bv: A (B,N) tensor containing a batch of vectors. + + Returns: + A (B,M) tensor containing the batch-wise matrix-vector product. + """ return torch.matmul(bm, bv.unsqueeze(dim=2)).squeeze(dim=2) def bmm(bm1, bm2): - # batched matrix-matrix product + """Batch-wise matrix-matrix product. + + Performs a batch-wise matrix-matrix product between a batch of MxK matrices and a batch of KxN matrices, each with + batch size B. Supports broadcasting for the batch dimension (unlike torch.bmm). + + Args: + bm1: A (B,M,K) tensor containing a batch of matrices. + bm2: A (B,K,N) tensor containing a batch of matrices. + + Returns: + A (B,M,N) tensor containing the batch-wise matrix-matrix product. + """ return torch.matmul(bm1, bm2) def get_size_array(data): + """Get an OCS2 size array. + + Creates an OCS2 size array and fills it with integer data from a NumPy array. + + Args: + data: A NumPy array of shape (N) containing integers. + + Returns: + An OCS2 size array of length N. + """ my_size_array = size_array() my_size_array.resize(len(data)) for i in range(len(data)): @@ -67,6 +115,16 @@ def get_size_array(data): def get_scalar_array(data): + """Get an OCS2 scalar array. + + Creates an OCS2 scalar array and fills it with float data from a NumPy array. + + Args: + data: A NumPy array of shape (N) containing floats. + + Returns: + An OCS2 scalar array of length N. + """ my_scalar_array = scalar_array() my_scalar_array.resize(len(data)) for i in range(len(data)): @@ -75,6 +133,16 @@ def get_scalar_array(data): def get_vector_array(data): + """Get an OCS2 vector array. + + Creates an OCS2 vector array and fills it with float data from a NumPy array. + + Args: + data: A NumPy array of shape (M,N) containing floats. + + Returns: + An OCS2 vector array of length M with vectors of dimension N. + """ my_vector_array = vector_array() my_vector_array.resize(len(data)) for i in range(len(data)): @@ -83,6 +151,19 @@ def get_vector_array(data): def get_system_observation(mode, time, state, input): + """Get an OCS2 system observation object. + + Creates an OCS2 system observation object and fills it with data. + + Args: + mode: The observed mode given by an integer. + time: The observed time given by a float. + state: The observed state given by a NumPy array of shape (M) containing floats. + input: The observed input given by a NumPy array of shape (N) containing floats. + + Returns: + An OCS2 system observation object. + """ system_observation = SystemObservation() system_observation.mode = mode system_observation.time = time @@ -92,12 +173,34 @@ def get_system_observation(mode, time, state, input): def get_system_observation_array(length): + """Get an OCS2 system observation array. + + Creates an OCS2 system observation array but does not fill it with data. + + Args: + length: The length that the array should have given by an integer. + + Returns: + An OCS2 system observation array of the desired length. + """ system_observation_array = SystemObservationArray() system_observation_array.resize(length) return system_observation_array def get_target_trajectories(time_trajectory, state_trajectory, input_trajectory): + """Get an OCS2 target trajectories object. + + Creates an OCS2 target trajectories object and fills it with data. + + Args: + time_trajectory: The target time trajectory given by a NumPy array of shape (K) containing floats. + state_trajectory: The target state trajectory given by a NumPy array of shape (K,M) containing floats. + input_trajectory: The target input trajectory given by a NumPy array of shape (K,N) containing floats. + + Returns: + An OCS2 target trajectories object. + """ time_trajectory_array = get_scalar_array(time_trajectory) state_trajectory_array = get_vector_array(state_trajectory) input_trajectory_array = get_vector_array(input_trajectory) @@ -105,24 +208,68 @@ def get_target_trajectories(time_trajectory, state_trajectory, input_trajectory) def get_target_trajectories_array(length): + """Get an OCS2 target trajectories array. + + Creates an OCS2 target trajectories array but does not fill it with data. + + Args: + length: The length that the array should have given by an integer. + + Returns: + An OCS2 target trajectories array of the desired length. + """ target_trajectories_array = TargetTrajectoriesArray() target_trajectories_array.resize(length) return target_trajectories_array def get_mode_schedule(event_times, mode_sequence): + """Get an OCS2 mode schedule object. + + Creates an OCS2 mode schedule object and fills it with data. + + Args: + event_times: The event times given by a NumPy array of shape (K-1) containing floats. + mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + + Returns: + An OCS2 mode schedule object. + """ event_times_array = get_scalar_array(event_times) mode_sequence_array = get_size_array(mode_sequence) return ModeSchedule(event_times_array, mode_sequence_array) def get_mode_schedule_array(length): + """Get an OCS2 mode schedule array. + + Creates an OCS2 mode schedule array but does not fill it with data. + + Args: + length: The length that the array should have given by an integer. + + Returns: + An OCS2 mode schedule array of the desired length. + """ mode_schedule_array = ModeScheduleArray() mode_schedule_array.resize(length) return mode_schedule_array def get_event_times_and_mode_sequence(default_mode, duration, event_times_template, mode_sequence_template): + """Get the event times and mode sequence describing a mode schedule. + + Creates the event times and mode sequence for a certain time duration from a template (e.g. a gait). + + Args: + default_mode: The default mode prepended and appended to the mode schedule and given by an integer. + duration: The duration of the mode schedule given by a float. + event_times_template: The event times template given by a NumPy array of shape (T) containing floats. + mode_sequence_template: The mode sequence template given by a NumPy array of shape (T) containing integers. + + Returns: + The event times and mode sequence given by NumPy arrays. + """ gait_cycle_duration = event_times_template[-1] num_gait_cycles = int(np.floor(duration / gait_cycle_duration)) event_times = np.array([0.0], dtype=np.float64) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index 3948c1ebb..63b40b623 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -27,6 +27,12 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Loss classes. + +Provides classes with loss functions for MPC-Net, such as the Hamiltonian loss and the cross entropy loss for training +the gating network of a mixture of experts network. Additionally, a simple behavioral cloning loss is implemented. +""" + import torch from ocs2_mpcnet_core import config @@ -34,12 +40,34 @@ class Hamiltonian: + """Hamiltonian loss. - # Uses the linear quadratic approximation of the Hamiltonian as loss - # H(x,u) = 1/2 dx' dHdxx dx + du' dHdux dx + 1/2 du' dHduu du + dHdx' dx + dHdu' du + H + Uses the linear quadratic approximation of the Hamiltonian as loss: + H(x,u) = 1/2 dx' dHdxx dx + du' dHdux dx + 1/2 du' dHduu du + dHdx' dx + dHdu' du + H, + where the state x is of dimension X and the input u is of dimension U. + """ @staticmethod def compute_sample(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): + """Computes the Hamiltonian for one sample. + + Computes the Hamiltonian for one sample using the provided linear quadratic approximation. + + Args: + x_inquiry: A (X) tensor with the state the Hamiltonian should be computed for. + x_nominal: A (X) tensor with the state that was used as development/expansion point. + u_inquiry: A (U) tensor with the input the Hamiltonian should be computed for. + u_nominal: A (U) tensor with the input that was used as development/expansion point. + dHdxx: A (X,X) tensor with the state-state Hessian of the approximation. + dHdux: A (U,X) tensor with the input-state Hessian of the approximation. + dHduu: A (U,U) tensor with the input-input Hessian of the approximation. + dHdx: A (X) tensor with the state gradient of the approximation. + dHdu: A (U) tensor with the input gradient of the approximation. + H: A (1) tensor with the Hamiltonian at the development/expansion point. + + Returns: + A (1) tensor containing the computed Hamiltonian. + """ if torch.equal(x_inquiry, x_nominal): du = torch.sub(u_inquiry, u_nominal) return 0.5 * torch.dot(du, torch.mv(dHduu, du)) + torch.dot(dHdu, du) + H @@ -60,6 +88,25 @@ def compute_sample(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHd @staticmethod def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): + """Computes the Hamiltonians for a batch. + + Computes the Hamiltonians for a batch of size B using the provided linear quadratic approximations. + + Args: + x_inquiry: A (B,X) tensor with the states the Hamiltonians should be computed for. + x_nominal: A (B,X) tensor with the states that were used as development/expansion points. + u_inquiry: A (B,U) tensor with the inputs the Hamiltonians should be computed for. + u_nominal: A (B,U) tensor with the inputs that were used as development/expansion point. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the approximations. + dHdx: A (B,X) tensor with the state gradients of the approximations. + dHdu: A (B,U) tensor with the input gradients of the approximations. + H: A (B) tensor with the Hamiltonians at the development/expansion points. + + Returns: + A (B) tensor containing the computed Hamiltonians. + """ if torch.equal(x_inquiry, x_nominal): du = torch.sub(u_inquiry, u_nominal) return 0.5 * bdot(du, bmv(dHduu, du)) + bdot(dHdu, du) + H @@ -80,33 +127,104 @@ def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHdu class BehavioralCloning: + """Behavioral cloning loss. + + Uses a simple quadratic function as loss: + BC(u) = du' R du, + where the input u is of dimension U. - # Uses a simple quadratic function as loss - # BC(u) = du' R du + Attributes: + R_sample: A (U,U) tensor with the input cost matrix R for one sample. + R_batch: A (1,U,U) tensor with the input cost matrix for a batch. + """ def __init__(self, R): + """Initializes the BehavioralCloning class. + + Initializes the BehavioralCloning class by setting fixed attributes. + + Args: + R: A NumPy array of shape (U, U) with the input cost matrix. + """ self.R_sample = torch.tensor(R, device=config.device, dtype=config.dtype) self.R_batch = self.R_sample.unsqueeze(dim=0) def compute_sample(self, u_predicted, u_target): + """Computes the behavioral cloning loss for one sample. + + Computes the behavioral cloning loss for one sample using the cost matrix. + + Args: + u_predicted: A (U) tensor with the predicted input. + u_target: A (U) tensor with the target input. + + Returns: + A (1) tensor containing the behavioral cloning loss. + """ du = torch.sub(u_predicted, u_target) return torch.dot(du, torch.mv(self.R_sample, du)) def compute_batch(self, u_predicted, u_target): + """Computes the behavioral cloning loss for a batch. + + Computes the behavioral cloning loss for a batch of size B using the cost matrix. + + Args: + u_predicted: A (B, U) tensor with the predicted inputs. + u_target: A (B, U) tensor with the target inputs. + + Returns: + A (B) tensor containing the behavioral cloning losses. + """ du = torch.sub(u_predicted, u_target) return bdot(du, bmv(self.R_batch, du)) class CrossEntropy: + """Cross entropy loss. + + Uses the cross entropy between two discrete probability distributions as loss: + CE(p_target, p_predicted) = - sum(p_target * log(p_predicted)), + where the sample space is the set of P individually identified items. - # Uses the cross entropy between two probability distributions as loss - # CE(p_target, p_predicted) = - sum(p_target * log(p_predicted)) + Attributes: + epsilon: A (1) tensor with a small epsilon used to stabilize the logarithm. + """ def __init__(self, epsilon): + """Initializes the CrossEntropy class. + + Initializes the CrossEntropy class by setting fixed attributes. + + Args: + epsilon: A float used to stabilize the logarithm. + """ self.epsilon = torch.tensor(epsilon, device=config.device, dtype=config.dtype) def compute_sample(self, p_target, p_predicted): + """Computes the cross entropy loss for one sample. + + Computes the cross entropy loss for one sample, where the logarithm is stabilized by a small epsilon. + + Args: + p_target: A (P) tensor with the target discrete probability distribution. + p_predicted: A (P) tensor with the predicted discrete probability distribution. + + Returns: + A (1) tensor containing the cross entropy loss. + """ return -torch.dot(p_target, torch.log(p_predicted + self.epsilon)) def compute_batch(self, p_target, p_predicted): + """Computes the cross entropy loss for a batch. + + Computes the cross entropy loss for a batch, where the logarithm is stabilized by a small epsilon. + + Args: + p_target: A (B,P) tensor with the target discrete probability distributions. + p_predicted: A (B,P) tensor with the predicted discrete probability distributions. + + Returns: + A (B) tensor containing the cross entropy losses. + """ return -bdot(p_target, torch.log(p_predicted + self.epsilon)) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py index abd9dcc28..d7208da53 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py @@ -27,13 +27,54 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Memory classes. + +Provides classes for storing data in memory. +""" + import torch from ocs2_mpcnet import config class CircularMemory: + """Circular memory. + + Stores data in a circular memory that overwrites old data if the size of the memory reaches its capacity. + + Attributes: + capacity: An integer defining the capacity of the memory. + size: An integer giving the current size of the memory. + position: An integer giving the current position in the memory. + t: A (C) tensor for the times. + x: A (C,X) tensor for the observed states. + u: A (C,U) tensor for the optimal inputs. + p: A (C,P) tensor for the observed discrete probability distributions of the modes. + generalized_time: A (C,T) tensor for the generalized times. + relative_state: A (C,X) tensor for the relative states. + input_transformation: A (C,U,U) tensor for the input transformations. + dHdxx: A (C,X,X) tensor for the state-state Hessians of the Hamiltonian approximations. + dHdux: A (C,U,X) tensor for the input-state Hessians of the Hamiltonian approximations. + dHduu: A (C,U,U) tensor for the input-input Hessians of the Hamiltonian approximations. + dHdx: A (C,X) tensor for the state gradients of the Hamiltonian approximations. + dHdu: A (C,U) tensor for the input gradients of the Hamiltonian approximations. + H: A (C) tensor for the Hamiltonians at the development/expansion points. + """ + def __init__(self, capacity, time_dimension, state_dimension, input_dimension, expert_number=1): + """Initializes the CircularMemory class. + + Initializes the BehavioralCloning class by setting fixed attributes, initializing variable attributes and + pre-allocating memory. + + Args: + capacity: An integer defining the capacity, i.e. maximum size, C of the memory. + time_dimension: An integer defining the dimension T of the generalized time. + state_dimension: An integer defining the dimension X of the state and relative state. + input_dimension: An integer defining the dimension U of the input. + expert_number: An integer defining the number of experts E equal to the number of individually identifiable + items P in the sample space of the discrete probability distributions of the modes. + """ # init variables self.capacity = capacity self.size = 0 @@ -56,8 +97,22 @@ def __init__(self, capacity, time_dimension, state_dimension, input_dimension, e self.H = torch.zeros(capacity, device=config.device, dtype=config.dtype) def push(self, t, x, u, p, generalized_time, relative_state, input_transformation, hamiltonian): + """Pushes data into the circular memory. + + Pushes one data sample into the circular memory. + + Args: + t: A float with the time. + x: A NumPy array of shape (X) with the observed state. + u: A NumPy array of shape (U) with the optimal input. + p: A NumPy array of shape (P) tensor for the observed discrete probability distributions of the modes. + generalized_time: A NumPy array of shape (T) with the generalized times. + relative_state: A NumPy array of shape (X) with the relative states. + input_transformation: A NumPy array of shape (U,U) with the input transformations. + hamiltonian: An OCS2 scalar function quadratic approximation representing the Hamiltonian around x and u. + """ # push data into memory - # note: - torch.as_tensor: no copy as data is an ndarray of the corresponding dtype and the device is the cpu + # note: - torch.as_tensor: no copy as data is a ndarray of the corresponding dtype and the device is the cpu # - torch.Tensor.copy_: copy performed together with potential dtype and device change self.t[self.position].copy_(torch.as_tensor(t, dtype=None, device=torch.device("cpu"))) self.x[self.position].copy_(torch.as_tensor(x, dtype=None, device=torch.device("cpu"))) @@ -83,6 +138,29 @@ def push(self, t, x, u, p, generalized_time, relative_state, input_transformatio self.position = (self.position + 1) % self.capacity def sample(self, batch_size): + """Samples data from the circular memory. + + Samples a batch of data from the circular memory. + + Args: + batch_size: An integer defining the batch size B. + + Returns: + A tuple containing the sampled batch of data. + - t_batch: A (B) tensor with the times. + - x_batch: A (B,X) tensor with the observed states. + - u_batch: A (B,U) tensor with the optimal inputs. + - p_batch: A (B,P) tensor with the observed discrete probability distributions of the modes. + - generalized_time_batch: A (B,T) tensor with the generalized times. + - relative_state_batch: A (B,X) tensor with the relative states. + - input_transformation_batch: A (B,U,U) tensor with the input transformation matrices. + - dHdxx_batch: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + - dHdux_batch: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + - dHduu_batch: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + - dHdx_batch: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + - dHdu_batch: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + - H_batch: A (B) tensor with the Hamiltonians at the development/expansion points. + """ indices = torch.randint(0, self.size, (batch_size,), device=config.device) t_batch = self.t[indices] x_batch = self.x[indices] @@ -114,4 +192,11 @@ def sample(self, batch_size): ) def __len__(self): + """The length of the memory. + + Return the length of the memory given by the current size. + + Returns: + An integer describing the length of the memory. + """ return self.size diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index fd0100104..a39ffe35f 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -27,6 +27,11 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Policy classes. + +Provides classes for different neural network policies. +""" + import torch from ocs2_mpcnet import config @@ -34,7 +39,24 @@ class Policy(torch.nn.Module): + """Policy. + + Base class for all neural network policies. + + Attributes: + name: A string with the name of the policy. + dim_in: An integer defining the input dimension of the policy. + dim_out: An integer defining the output dimension of the policy. + """ def __init__(self, dim_in, dim_out): + """Initializes the Policy class. + + Initializes the Policy class by setting fixed attributes. + + Args: + dim_in: An integer defining the input dimension of the policy. + dim_out: An integer defining the output dimension of the policy. + """ super().__init__() self.name = "Policy" self.dim_in = dim_in @@ -42,17 +64,65 @@ def __init__(self, dim_in, dim_out): class LinearPolicy(Policy): + """Linear policy. + + Class for a simple linear neural network policy. + + Attributes: + name: A string with the name of the policy. + linear: The linear neural network layer. + """ def __init__(self, dim_t, dim_x, dim_u): + """Initializes the LinearPolicy class. + + Initializes the LinearPolicy class by setting fixed and variable attributes. + + Args: + dim_t: An integer defining the generalized time dimension. + dim_x: An integer defining the relative state dimension. + dim_u: An integer defining the control input dimension. + """ super().__init__(dim_t + dim_x, dim_u) self.name = "LinearPolicy" self.linear = torch.nn.Linear(self.dim_in, self.dim_out) def forward(self, t, x): + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + t: A (B,T) tensor with the generalized times. + x: A (B,X) tensor with the relative states. + + Returns: + u: A (B,U) tensor with the predicted control inputs. + """ return self.linear(torch.cat((t, x), dim=1)) class NonlinearPolicy(Policy): + """Nonlinear policy. + + Class for a simple nonlinear neural network policy, where the hidden layer is the mean of the input and output layer. + + Attributes: + name: A string with the name of the policy. + dim_hidden: An integer defining the dimension of the hidden layer. + linear1: The first linear neural network layer. + activation: The activation to get the hidden layer. + linear2: The second linear neural network layer. + """ def __init__(self, dim_t, dim_x, dim_u): + """Initializes the NonlinearPolicy class. + + Initializes the NonlinearPolicy class by setting fixed and variable attributes. + + Args: + dim_t: An integer defining the generalized time dimension. + dim_x: An integer defining the relative state dimension. + dim_u: An integer defining the control input dimension. + """ super().__init__(dim_t + dim_x, dim_u) self.name = "NonlinearPolicy" self.dim_hidden = int((self.dim_in + dim_u) / 2) @@ -61,11 +131,42 @@ def __init__(self, dim_t, dim_x, dim_u): self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) def forward(self, t, x): + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + t: A (B,T) tensor with the generalized times. + x: A (B,X) tensor with the relative states. + + Returns: + u: A (B,U) tensor with the predicted control inputs. + """ return self.linear2(self.activation(self.linear1(torch.cat((t, x), dim=1)))) class MixtureOfLinearExpertsPolicy(Policy): + """Mixture of linear experts policy. + + Class for a mixture of experts neural network with linear experts. + + Attributes: + name: A string with the name of the policy. + num_experts: An integer defining the number of experts. + gating_net: The gating network. + expert_nets: The expert networks. + """ def __init__(self, dim_t, dim_x, dim_u, num_experts): + """Initializes the MixtureOfLinearExpertsPolicy class. + + Initializes the MixtureOfLinearExpertsPolicy class by setting fixed and variable attributes. + + Args: + dim_t: An integer defining the generalized time dimension. + dim_x: An integer defining the relative state dimension. + dim_u: An integer defining the control input dimension. + num_experts: An integer defining the number of experts. + """ super().__init__(dim_t + dim_x, dim_u) self.name = "MixtureOfLinearExpertsPolicy" self.num_experts = num_experts @@ -77,6 +178,18 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): ) def forward(self, t, x): + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + t: A (B,T) tensor with the generalized times. + x: A (B,X) tensor with the relative states. + + Returns: + u: A (B,U) tensor with the predicted control inputs. + p: A (B,E) tensor with the predicted expert weights. + """ p = self.gating_net(torch.cat((t, x), dim=1)) U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) u = bmv(U, p) @@ -84,7 +197,30 @@ def forward(self, t, x): class MixtureOfNonlinearExpertsPolicy(Policy): + """Mixture of nonlinear experts policy. + + Class for a mixture of experts neural network with nonlinear experts, where the hidden layer is the mean of the + input and output layer. + + Attributes: + name: A string with the name of the policy. + num_experts: An integer defining the number of experts. + dim_hidden_gating: An integer defining the dimension of the hidden layer for the gating network. + dim_hidden_expert: An integer defining the dimension of the hidden layer for the expert networks. + gating_net: The gating network. + expert_nets: The expert networks. + """ def __init__(self, dim_t, dim_x, dim_u, num_experts): + """Initializes the MixtureOfNonlinearExpertsPolicy class. + + Initializes the MixtureOfNonlinearExpertsPolicy class by setting fixed and variable attributes. + + Args: + dim_t: An integer defining the generalized time dimension. + dim_x: An integer defining the relative state dimension. + dim_u: An integer defining the control input dimension. + num_experts: An integer defining the number of experts. + """ super().__init__(dim_t + dim_x, dim_u) self.name = "MixtureOfNonlinearExpertsPolicy" self.num_experts = num_experts @@ -103,6 +239,18 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): ) def forward(self, t, x): + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + t: A (B,T) tensor with the generalized times. + x: A (B,X) tensor with the relative states. + + Returns: + u: A (B,U) tensor with the predicted control inputs. + p: A (B,E) tensor with the predicted expert weights. + """ p = self.gating_net(torch.cat((t, x), dim=1)) U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) u = bmv(U, p) @@ -110,7 +258,26 @@ def forward(self, t, x): class LinearExpert(torch.nn.Module): + """Linear expert. + + Class for a simple linear neural network expert. + + Attributes: + name: A string with the name of the expert. + dim_in: An integer defining the input dimension of the expert. + dim_out: An integer defining the output dimension of the expert. + linear: The linear neural network layer. + """ def __init__(self, id, dim_in, dim_out): + """Initializes the LinearExpert class. + + Initializes the LinearExpert class by setting fixed and variable attributes. + + Args: + id: An integer with the index of the expert. + dim_in: An integer defining the input dimension of the expert. + dim_out: An integer defining the output dimension of the expert. + """ super().__init__() self.name = "LinearExpert" + str(id) self.dim_in = dim_in @@ -118,11 +285,44 @@ def __init__(self, id, dim_in, dim_out): self.linear = torch.nn.Linear(self.dim_in, self.dim_out) def forward(self, input): + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + input: A (B,I) tensor with the inputs. + + Returns: + output: A (B,O) tensor with the outputs. + """ return self.linear(input) class NonlinearExpert(torch.nn.Module): + """Nonlinear expert. + + Class for a simple nonlinear neural network expert, where the hidden layer is the mean of the input and output layer. + + Attributes: + name: A string with the name of the expert. + dim_in: An integer defining the input dimension of the expert. + dim_hidden: An integer defining the dimension of the hidden layer. + dim_out: An integer defining the output dimension of the expert. + linear1: The first linear neural network layer. + activation: The activation to get the hidden layer. + linear2: The second linear neural network layer. + """ def __init__(self, id, dim_in, dim_hidden, dim_out): + """Initializes the NonlinearExpert class. + + Initializes the NonlinearExpert class by setting fixed and variable attributes. + + Args: + id: An integer with the index of the expert. + dim_in: An integer defining the input dimension of the expert. + dim_hidden: An integer defining the dimension of the hidden layer. + dim_out: An integer defining the output dimension of the expert. + """ super().__init__() self.name = "NonlinearExpert" + str(id) self.dim_in = dim_in @@ -133,4 +333,14 @@ def __init__(self, id, dim_in, dim_hidden, dim_out): self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) def forward(self, input): + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + input: A (B,I) tensor with the inputs. + + Returns: + output: A (B,O) tensor with the outputs. + """ return self.linear2(self.activation(self.linear1(input))) From 9da4d9955a46a4c9df217fba7cb774bd6a2dbb8f Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 15:55:49 +0100 Subject: [PATCH 128/512] small cleanup for policies --- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index a39ffe35f..82319e5ff 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -224,8 +224,8 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): super().__init__(dim_t + dim_x, dim_u) self.name = "MixtureOfNonlinearExpertsPolicy" self.num_experts = num_experts - self.dim_hidden_expert = int((self.dim_in + dim_u) / 2) self.dim_hidden_gating = int((self.dim_in + num_experts) / 2) + self.dim_hidden_expert = int((self.dim_in + dim_u) / 2) # gating self.gating_net = torch.nn.Sequential( torch.nn.Linear(self.dim_in, self.dim_hidden_gating), @@ -268,18 +268,18 @@ class LinearExpert(torch.nn.Module): dim_out: An integer defining the output dimension of the expert. linear: The linear neural network layer. """ - def __init__(self, id, dim_in, dim_out): + def __init__(self, index, dim_in, dim_out): """Initializes the LinearExpert class. Initializes the LinearExpert class by setting fixed and variable attributes. Args: - id: An integer with the index of the expert. + index: An integer with the index of the expert. dim_in: An integer defining the input dimension of the expert. dim_out: An integer defining the output dimension of the expert. """ super().__init__() - self.name = "LinearExpert" + str(id) + self.name = "LinearExpert" + str(index) self.dim_in = dim_in self.dim_out = dim_out self.linear = torch.nn.Linear(self.dim_in, self.dim_out) @@ -312,19 +312,19 @@ class NonlinearExpert(torch.nn.Module): activation: The activation to get the hidden layer. linear2: The second linear neural network layer. """ - def __init__(self, id, dim_in, dim_hidden, dim_out): + def __init__(self, index, dim_in, dim_hidden, dim_out): """Initializes the NonlinearExpert class. Initializes the NonlinearExpert class by setting fixed and variable attributes. Args: - id: An integer with the index of the expert. + index: An integer with the index of the expert. dim_in: An integer defining the input dimension of the expert. dim_hidden: An integer defining the dimension of the hidden layer. dim_out: An integer defining the output dimension of the expert. """ super().__init__() - self.name = "NonlinearExpert" + str(id) + self.name = "NonlinearExpert" + str(index) self.dim_in = dim_in self.dim_hidden = dim_hidden self.dim_out = dim_out From 48ac7169f94e107c27a04ed2b7885cd0b0f4e441 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 16:07:33 +0100 Subject: [PATCH 129/512] remove unnecesary policy base class --- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 67 ++++++++++-------------- 1 file changed, 29 insertions(+), 38 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index 82319e5ff..c57121f97 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -34,42 +34,18 @@ import torch -from ocs2_mpcnet import config from ocs2_mpcnet.helper import bmv -class Policy(torch.nn.Module): - """Policy. - - Base class for all neural network policies. - - Attributes: - name: A string with the name of the policy. - dim_in: An integer defining the input dimension of the policy. - dim_out: An integer defining the output dimension of the policy. - """ - def __init__(self, dim_in, dim_out): - """Initializes the Policy class. - - Initializes the Policy class by setting fixed attributes. - - Args: - dim_in: An integer defining the input dimension of the policy. - dim_out: An integer defining the output dimension of the policy. - """ - super().__init__() - self.name = "Policy" - self.dim_in = dim_in - self.dim_out = dim_out - - -class LinearPolicy(Policy): +class LinearPolicy(torch.nn.Module): """Linear policy. Class for a simple linear neural network policy. Attributes: name: A string with the name of the policy. + dim_in: An integer defining the input dimension of the policy. + dim_out: An integer defining the output dimension of the policy. linear: The linear neural network layer. """ def __init__(self, dim_t, dim_x, dim_u): @@ -82,8 +58,10 @@ def __init__(self, dim_t, dim_x, dim_u): dim_x: An integer defining the relative state dimension. dim_u: An integer defining the control input dimension. """ - super().__init__(dim_t + dim_x, dim_u) + super().__init__() self.name = "LinearPolicy" + self.dim_in = dim_t + dim_x + self.dim_out = dim_u self.linear = torch.nn.Linear(self.dim_in, self.dim_out) def forward(self, t, x): @@ -101,14 +79,16 @@ def forward(self, t, x): return self.linear(torch.cat((t, x), dim=1)) -class NonlinearPolicy(Policy): +class NonlinearPolicy(torch.nn.Module): """Nonlinear policy. Class for a simple nonlinear neural network policy, where the hidden layer is the mean of the input and output layer. Attributes: name: A string with the name of the policy. + dim_in: An integer defining the input dimension of the policy. dim_hidden: An integer defining the dimension of the hidden layer. + dim_out: An integer defining the output dimension of the policy. linear1: The first linear neural network layer. activation: The activation to get the hidden layer. linear2: The second linear neural network layer. @@ -123,9 +103,11 @@ def __init__(self, dim_t, dim_x, dim_u): dim_x: An integer defining the relative state dimension. dim_u: An integer defining the control input dimension. """ - super().__init__(dim_t + dim_x, dim_u) + super().__init__() self.name = "NonlinearPolicy" - self.dim_hidden = int((self.dim_in + dim_u) / 2) + self.dim_in = dim_t + dim_x + self.dim_hidden = int((dim_t + dim_x + dim_u) / 2) + self.dim_out = dim_u self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) self.activation = torch.nn.Tanh() self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) @@ -145,13 +127,15 @@ def forward(self, t, x): return self.linear2(self.activation(self.linear1(torch.cat((t, x), dim=1)))) -class MixtureOfLinearExpertsPolicy(Policy): +class MixtureOfLinearExpertsPolicy(torch.nn.Module): """Mixture of linear experts policy. Class for a mixture of experts neural network with linear experts. Attributes: name: A string with the name of the policy. + dim_in: An integer defining the input dimension of the policy. + dim_out: An integer defining the output dimension of the policy. num_experts: An integer defining the number of experts. gating_net: The gating network. expert_nets: The expert networks. @@ -167,8 +151,10 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): dim_u: An integer defining the control input dimension. num_experts: An integer defining the number of experts. """ - super().__init__(dim_t + dim_x, dim_u) + super().__init__() self.name = "MixtureOfLinearExpertsPolicy" + self.dim_in = dim_t + dim_x + self.dim_out = dim_u self.num_experts = num_experts # gating self.gating_net = torch.nn.Sequential(torch.nn.Linear(self.dim_in, self.num_experts), torch.nn.Softmax(dim=1)) @@ -196,7 +182,7 @@ def forward(self, t, x): return u, p -class MixtureOfNonlinearExpertsPolicy(Policy): +class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): """Mixture of nonlinear experts policy. Class for a mixture of experts neural network with nonlinear experts, where the hidden layer is the mean of the @@ -204,9 +190,11 @@ class MixtureOfNonlinearExpertsPolicy(Policy): Attributes: name: A string with the name of the policy. - num_experts: An integer defining the number of experts. + dim_in: An integer defining the input dimension of the policy. dim_hidden_gating: An integer defining the dimension of the hidden layer for the gating network. dim_hidden_expert: An integer defining the dimension of the hidden layer for the expert networks. + dim_out: An integer defining the output dimension of the policy. + num_experts: An integer defining the number of experts. gating_net: The gating network. expert_nets: The expert networks. """ @@ -221,11 +209,14 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): dim_u: An integer defining the control input dimension. num_experts: An integer defining the number of experts. """ - super().__init__(dim_t + dim_x, dim_u) + super().__init__() self.name = "MixtureOfNonlinearExpertsPolicy" + self.dim_in = dim_t + dim_x + self.dim_hidden_gating = int((dim_t + dim_x + num_experts) / 2) + self.dim_hidden_expert = int((dim_t + dim_x + dim_u) / 2) + self.dim_out = dim_u self.num_experts = num_experts - self.dim_hidden_gating = int((self.dim_in + num_experts) / 2) - self.dim_hidden_expert = int((self.dim_in + dim_u) / 2) + # gating self.gating_net = torch.nn.Sequential( torch.nn.Linear(self.dim_in, self.dim_hidden_gating), From 4d0d69aff911540271ad45a24c732ed6d2742371 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 16:16:14 +0100 Subject: [PATCH 130/512] run black --- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index c57121f97..2490c82ce 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -48,6 +48,7 @@ class LinearPolicy(torch.nn.Module): dim_out: An integer defining the output dimension of the policy. linear: The linear neural network layer. """ + def __init__(self, dim_t, dim_x, dim_u): """Initializes the LinearPolicy class. @@ -93,6 +94,7 @@ class NonlinearPolicy(torch.nn.Module): activation: The activation to get the hidden layer. linear2: The second linear neural network layer. """ + def __init__(self, dim_t, dim_x, dim_u): """Initializes the NonlinearPolicy class. @@ -140,6 +142,7 @@ class MixtureOfLinearExpertsPolicy(torch.nn.Module): gating_net: The gating network. expert_nets: The expert networks. """ + def __init__(self, dim_t, dim_x, dim_u, num_experts): """Initializes the MixtureOfLinearExpertsPolicy class. @@ -198,6 +201,7 @@ class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): gating_net: The gating network. expert_nets: The expert networks. """ + def __init__(self, dim_t, dim_x, dim_u, num_experts): """Initializes the MixtureOfNonlinearExpertsPolicy class. @@ -259,6 +263,7 @@ class LinearExpert(torch.nn.Module): dim_out: An integer defining the output dimension of the expert. linear: The linear neural network layer. """ + def __init__(self, index, dim_in, dim_out): """Initializes the LinearExpert class. @@ -303,6 +308,7 @@ class NonlinearExpert(torch.nn.Module): activation: The activation to get the hidden layer. linear2: The second linear neural network layer. """ + def __init__(self, index, dim_in, dim_hidden, dim_out): """Initializes the NonlinearExpert class. From 8fa6949c95831cfd3ba235c967c55402103ac3ae Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 16:24:06 +0100 Subject: [PATCH 131/512] bugfix --- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index 63b40b623..cc0db32b4 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -35,7 +35,7 @@ import torch -from ocs2_mpcnet_core import config +from ocs2_mpcnet import config from ocs2_mpcnet.helper import bdot, bmv From 7b1805ca0cc91c22a5770d307c06264f02642f76 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 16:26:56 +0100 Subject: [PATCH 132/512] adapt to small changes in ocs2 mpcnet core --- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index f68eb2b47..b6bd5a29d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -176,9 +176,8 @@ def closure(): # clear the gradients optimizer.zero_grad() # prediction - u_predicted, p_predicted, U_predicted = policy(generalized_time, relative_state) + u_predicted = policy(generalized_time, relative_state) u_predicted = bmv(input_transformation, u_predicted) - U_predicted = bmm(input_transformation, U_predicted) # compute the empirical loss empirical_loss = loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size # compute the gradients From 2c28ccb1a0ba633445b577ba7d7c52bbb4579f24 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 16:29:53 +0100 Subject: [PATCH 133/512] adapt to small changes in ocs2 mpcnet core --- .../python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 2b2f154b3..2a655b907 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -87,7 +87,7 @@ epsilon = 1e-8 # epsilon to improve numerical stability of logs and denominators my_lambda = 10.0 # parameter to control the relative importance of both loss types experts_loss = ExpertsLoss() -gating_loss = GatingLoss(torch.tensor(epsilon, device=config.device, dtype=config.dtype)) +gating_loss = GatingLoss(epsilon) # memory memory_capacity = 500000 @@ -192,9 +192,8 @@ def closure(): # clear the gradients optimizer.zero_grad() # prediction - u_predicted, p_predicted, U_predicted = policy(generalized_time, relative_state) + u_predicted, p_predicted = policy(generalized_time, relative_state) u_predicted = bmv(input_transformation, u_predicted) - U_predicted = bmm(input_transformation, U_predicted) # compute the empirical loss empirical_experts_loss = experts_loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size empirical_gating_loss = gating_loss.compute_batch(p, p_predicted).sum() / batch_size From fac7b3fdda555eb6d2b4b77934eeb7200fb06841 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 16:39:06 +0100 Subject: [PATCH 134/512] also fix legged robot policies --- .../legged_robot_policy.py | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index 8a78b9803..111e6f39b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -44,10 +44,6 @@ def u_transform(u): return bmv(input_scaling, u) + input_bias -def U_transform(U): - return bmm(input_scaling, U) + input_bias_stacked - - class LeggedRobotLinearPolicy(policy.LinearPolicy): def __init__(self, dim_t, dim_x, dim_u): @@ -55,8 +51,8 @@ def __init__(self, dim_t, dim_x, dim_u): self.name = 'LeggedRobotLinearPolicy' def forward(self, t, x): - u, p, U = super().forward(t, x) - return u_transform(u), p, U_transform(U) + u = super().forward(t, x) + return u_transform(u) class LeggedRobotNonlinearPolicy(policy.NonlinearPolicy): @@ -66,8 +62,8 @@ def __init__(self, dim_t, dim_x, dim_u): self.name = 'LeggedRobotNonlinearPolicy' def forward(self, t, x): - u, p, U = super().forward(t, x) - return u_transform(u), p, U_transform(U) + u = super().forward(t, x) + return u_transform(u) class LeggedRobotMixtureOfLinearExpertsPolicy(policy.MixtureOfLinearExpertsPolicy): @@ -77,8 +73,8 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): self.name = 'LeggedRobotMixtureOfLinearExpertsPolicy' def forward(self, t, x): - u, p, U = super().forward(t, x) - return u_transform(u), p, U_transform(U) + u, p = super().forward(t, x) + return u_transform(u), p class LeggedRobotMixtureOfNonlinearExpertsPolicy(policy.MixtureOfNonlinearExpertsPolicy): @@ -88,5 +84,5 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): self.name = 'LeggedRobotMixtureOfNonlinearExpertsPolicy' def forward(self, t, x): - u, p, U = super().forward(t, x) - return u_transform(u), p, U_transform(U) + u, p = super().forward(t, x) + return u_transform(u), p From 2ccf1205469d0d5b9b4cbc8cb1143e77b6224344 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 22 Mar 2022 16:45:16 +0100 Subject: [PATCH 135/512] remove unnecessary definition --- .../python/ocs2_legged_robot_mpcnet/legged_robot_policy.py | 1 - 1 file changed, 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index 111e6f39b..2a85a8800 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -37,7 +37,6 @@ input_scaling = torch.tensor(config.input_scaling, device=config.device, dtype=config.dtype).diag().unsqueeze(dim=0) input_bias = torch.tensor(config.input_bias, device=config.device, dtype=config.dtype).unsqueeze(dim=0) -input_bias_stacked = torch.stack([input_bias for i in range(config.EXPERT_NUM)], dim=2) def u_transform(u): From 5354c15271d39bbfe7559aa69e630fa7a1a0f2e8 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 23 Mar 2022 14:11:51 +0100 Subject: [PATCH 136/512] fix error messages --- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index 720a56dfd..877732faa 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -66,7 +66,7 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si mpcnetPtrs.push_back(std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>( new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, ballbotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr))); if (raisim) { - throw std::runtime_error("BallbotMpcnetInterface::BallbotMpcnetInterface RaiSim rollout not yet implemented for ballbot."); + throw std::runtime_error("[BallbotMpcnetInterface::BallbotMpcnetInterface] raisim rollout not yet implemented for ballbot."); } else { rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(ballbotInterface.getRollout().clone())); } From b965fb6eb0511c64ec553d5f9e2e05e2261cdfa4 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 23 Mar 2022 14:17:43 +0100 Subject: [PATCH 137/512] run black --- .../ocs2_ballbot_mpcnet/ballbot_helper.py | 12 ++- .../ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 94 ++++++++++++++----- .../ocs2_ballbot_mpcnet/setup.py | 5 +- 3 files changed, 80 insertions(+), 31 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py index d2fba8da3..0dbbfed21 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py @@ -72,9 +72,13 @@ def get_tasks(n_tasks, duration): mode_schedules = helper.get_mode_schedule_array(n_tasks) target_trajectories = helper.get_target_trajectories_array(n_tasks) for i in range(n_tasks): - initial_observations[i] = helper.get_system_observation(0, 0.0, get_random_initial_state(), np.zeros(config.INPUT_DIM)) + initial_observations[i] = helper.get_system_observation( + 0, 0.0, get_random_initial_state(), np.zeros(config.INPUT_DIM) + ) mode_schedules[i] = helper.get_mode_schedule(*get_default_event_times_and_mode_sequence(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + get_random_target_state().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM)), + ) return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index b6bd5a29d..483a3e0da 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -51,13 +51,13 @@ data_generation_n_threads = 2 data_generation_n_tasks = 10 data_generation_n_samples = 2 -data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order='F') +data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order="F") for i in range(0, 2): - data_generation_sampling_covariance[i, i] = 0.01 ** 2 # position + data_generation_sampling_covariance[i, i] = 0.01**2 # position for i in range(2, 5): data_generation_sampling_covariance[i, i] = (1.0 * np.pi / 180.0) ** 2 # orientation for i in range(5, 7): - data_generation_sampling_covariance[i, i] = 0.05 ** 2 # linear velocity + data_generation_sampling_covariance[i, i] = 0.05**2 # linear velocity for i in range(7, 10): data_generation_sampling_covariance[i, i] = (5.0 * np.pi / 180.0) ** 2 # angular velocity @@ -91,15 +91,17 @@ policy.to(config.device) print("Initial policy parameters:") print(list(policy.named_parameters())) -dummy_input = (torch.randn(1, config.TIME_DIM, device=config.device, dtype=config.dtype), - torch.randn(1, config.STATE_DIM, device=config.device, dtype=config.dtype)) +dummy_input = ( + torch.randn(1, config.TIME_DIM, device=config.device, dtype=config.dtype), + torch.randn(1, config.STATE_DIM, device=config.device, dtype=config.dtype), +) print("Saving initial policy.") save_path = "policies/" + folder + "/initial_policy" torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") # optimizer -batch_size = 2 ** 5 +batch_size = 2**5 learning_rate = 1e-2 learning_iterations = 10000 optimizer = torch.optim.Adam(policy.parameters(), lr=learning_rate) @@ -108,18 +110,31 @@ def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks(data_generation_n_tasks, data_generation_duration) - mpcnet_interface.startDataGeneration(alpha, policy_file_path, data_generation_time_step, data_generation_data_decimation, - data_generation_n_samples, data_generation_sampling_covariance, - initial_observations, mode_schedules, target_trajectories) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks( + data_generation_n_tasks, data_generation_duration + ) + mpcnet_interface.startDataGeneration( + alpha, + policy_file_path, + data_generation_time_step, + data_generation_data_decimation, + data_generation_n_samples, + data_generation_sampling_covariance, + initial_observations, + mode_schedules, + target_trajectories, + ) def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks(policy_evaluation_n_tasks, policy_evaluation_duration) - mpcnet_interface.startPolicyEvaluation(alpha, policy_file_path, policy_evaluation_time_step, - initial_observations, mode_schedules, target_trajectories) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks( + policy_evaluation_n_tasks, policy_evaluation_duration + ) + mpcnet_interface.startPolicyEvaluation( + alpha, policy_file_path, policy_evaluation_time_step, initial_observations, mode_schedules, target_trajectories + ) try: @@ -139,11 +154,19 @@ def start_policy_evaluation(policy, alpha=0.0): data = mpcnet_interface.getGeneratedData() for i in range(len(data)): # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory - memory.push(data[i].t, data[i].x, data[i].u, torch.ones(1, device=config.device, dtype=config.dtype), - data[i].generalized_time, data[i].relative_state, data[i].input_transformation, data[i].hamiltonian) + memory.push( + data[i].t, + data[i].x, + data[i].u, + torch.ones(1, device=config.device, dtype=config.dtype), + data[i].generalized_time, + data[i].relative_state, + data[i].input_transformation, + data[i].hamiltonian, + ) # logging - writer.add_scalar('data/new_data_points', len(data), iteration) - writer.add_scalar('data/total_data_points', len(memory), iteration) + writer.add_scalar("data/new_data_points", len(data), iteration) + writer.add_scalar("data/total_data_points", len(memory), iteration) print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) # start new data generation start_data_generation(policy, alpha) @@ -155,9 +178,17 @@ def start_policy_evaluation(policy, alpha=0.0): survival_time = np.mean([metrics[i].survival_time for i in range(len(metrics))]) incurred_hamiltonian = np.mean([metrics[i].incurred_hamiltonian for i in range(len(metrics))]) # logging - writer.add_scalar('metric/survival_time', survival_time, iteration) - writer.add_scalar('metric/incurred_hamiltonian', incurred_hamiltonian, iteration) - print("iteration", iteration, "received metrics:", "incurred_hamiltonian", incurred_hamiltonian, "survival_time", survival_time) + writer.add_scalar("metric/survival_time", survival_time, iteration) + writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) + print( + "iteration", + iteration, + "received metrics:", + "incurred_hamiltonian", + incurred_hamiltonian, + "survival_time", + survival_time, + ) # start new policy evaluation start_policy_evaluation(policy) @@ -169,7 +200,21 @@ def start_policy_evaluation(policy, alpha=0.0): torch.save(obj=policy, f=save_path + ".pt") # extract batch from memory - t, x, u, p, generalized_time, relative_state, input_transformation, dHdxx, dHdux, dHduu, dHdx, dHdu, H = memory.sample(batch_size) + ( + t, + x, + u, + p, + generalized_time, + relative_state, + input_transformation, + dHdxx, + dHdux, + dHduu, + dHdx, + dHdu, + H, + ) = memory.sample(batch_size) # take an optimization step def closure(): @@ -179,13 +224,16 @@ def closure(): u_predicted = policy(generalized_time, relative_state) u_predicted = bmv(input_transformation, u_predicted) # compute the empirical loss - empirical_loss = loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size + empirical_loss = ( + loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size + ) # compute the gradients empirical_loss.backward() # logging - writer.add_scalar('objective/empirical_loss', empirical_loss.item(), iteration) + writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) # return empirical loss return empirical_loss + optimizer.step(closure) # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py index c33f1ef78..959f0cad9 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py @@ -3,9 +3,6 @@ from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup -setup_args = generate_distutils_setup( - packages=['ocs2_ballbot_mpcnet'], - package_dir={'': 'python'} -) +setup_args = generate_distutils_setup(packages=["ocs2_ballbot_mpcnet"], package_dir={"": "python"}) setup(**setup_args) From a3479c8b37e6f63c5d3870340c6cc2957fe2b0d9 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 23 Mar 2022 14:32:13 +0100 Subject: [PATCH 138/512] fix error messages --- .../src/LeggedRobotMpcnetDefinition.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index 748235087..f2a3fa548 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -91,13 +91,13 @@ matrix_t LeggedRobotMpcnetDefinition::getInputTransformation(scalar_t t, const v bool LeggedRobotMpcnetDefinition::validState(const vector_t& x) { vector_t deviation = x - defaultState_; if (std::abs(deviation[8]) > 0.2) { - std::cerr << "LeggedRobotMpcnetDefinition::validState Height diverged: " << x[8] << std::endl; + std::cerr << "[LeggedRobotMpcnetDefinition::validState] height diverged: " << x[8] << "\n"; return false; } else if (std::abs(deviation[10]) > 30.0 * M_PI / 180.0) { - std::cerr << "LeggedRobotMpcnetDefinition::validState Pitch diverged: " << x[10] << std::endl; + std::cerr << "[LeggedRobotMpcnetDefinition::validState] pitch diverged: " << x[10] << "\n"; return false; } else if (std::abs(deviation[11]) > 30.0 * M_PI / 180.0) { - std::cerr << "LeggedRobotMpcnetDefinition::validState Roll diverged: " << x[11] << std::endl; + std::cerr << "[LeggedRobotMpcnetDefinition::validState] roll diverged: " << x[11] << "\n"; return false; } else { return true; From b9fc53d5edbf14b3a79a4986d951aad189422bc5 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 23 Mar 2022 15:00:31 +0100 Subject: [PATCH 139/512] run black --- .../legged_robot_config.py | 136 +++++++++++++----- .../legged_robot_helper.py | 36 +++-- .../legged_robot_mpcnet.py | 112 +++++++++++---- .../legged_robot_policy.py | 12 +- .../ocs2_legged_robot_mpcnet/setup.py | 5 +- 5 files changed, 216 insertions(+), 85 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py index 68660e5ca..f980ec7a2 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py @@ -59,47 +59,119 @@ EXPERT_NUM = 3 # default state -default_state = [0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.575, - 0.0, 0.0, 0.0, - -0.25, 0.6, -0.85, - -0.25, -0.6, 0.85, - 0.25, 0.6, -0.85, - 0.25, -0.6, 0.85] +default_state = [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.575, + 0.0, + 0.0, + 0.0, + -0.25, + 0.6, + -0.85, + -0.25, + -0.6, + 0.85, + 0.25, + 0.6, + -0.85, + 0.25, + -0.6, + 0.85, +] # input bias -input_bias = [0.0, 0.0, 127.861, - 0.0, 0.0, 127.861, - 0.0, 0.0, 127.861, - 0.0, 0.0, 127.861, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0] +input_bias = [ + 0.0, + 0.0, + 127.861, + 0.0, + 0.0, + 127.861, + 0.0, + 0.0, + 127.861, + 0.0, + 0.0, + 127.861, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, +] # input scaling -input_scaling = [100.0, 100.0, 100.0, - 100.0, 100.0, 100.0, - 100.0, 100.0, 100.0, - 100.0, 100.0, 100.0, - 10.0, 10.0, 10.0, - 10.0, 10.0, 10.0, - 10.0, 10.0, 10.0, - 10.0, 10.0, 10.0] +input_scaling = [ + 100.0, + 100.0, + 100.0, + 100.0, + 100.0, + 100.0, + 100.0, + 100.0, + 100.0, + 100.0, + 100.0, + 100.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, +] # (diagonally dominant) nominal centroidal inertia normalized by robot mass normalized_inertia = [1.62079 / 52.1348, 4.83559 / 52.1348, 4.72382 / 52.1348] # input cost for behavioral cloning -R = [0.001, 0.001, 0.001, - 0.001, 0.001, 0.001, - 0.001, 0.001, 0.001, - 0.001, 0.001, 0.001, - 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0, - 5.0, 5.0, 5.0] +R = [ + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 0.001, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, +] # dictionary for cheating expert_for_mode = dict([(i, None) for i in range(16)]) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py index 0becf1715..d7f41b445 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -121,23 +121,35 @@ def get_tasks(n_tasks, duration, choices): target_trajectories = helper.get_target_trajectories_array(n_tasks) for i in range(n_tasks): if choices[i] == "stance": - initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_stance(), np.zeros(config.INPUT_DIM)) + initial_observations[i] = helper.get_system_observation( + 15, 0.0, get_random_initial_state_stance(), np.zeros(config.INPUT_DIM) + ) mode_schedules[i] = helper.get_mode_schedule(*get_stance(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_stance().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + get_random_target_state_stance().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM)), + ) elif choices[i] == "trot_1": - initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_trot(), np.zeros(config.INPUT_DIM)) + initial_observations[i] = helper.get_system_observation( + 15, 0.0, get_random_initial_state_trot(), np.zeros(config.INPUT_DIM) + ) mode_schedules[i] = helper.get_mode_schedule(*get_trot_1(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_trot().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + get_random_target_state_trot().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM)), + ) elif choices[i] == "trot_2": - initial_observations[i] = helper.get_system_observation(15, 0.0, get_random_initial_state_trot(), np.zeros(config.INPUT_DIM)) + initial_observations[i] = helper.get_system_observation( + 15, 0.0, get_random_initial_state_trot(), np.zeros(config.INPUT_DIM) + ) mode_schedules[i] = helper.get_mode_schedule(*get_trot_2(duration)) - target_trajectories[i] = helper.get_target_trajectories(duration * np.ones((1, 1)), - get_random_target_state_trot().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM))) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + get_random_target_state_trot().reshape((1, config.STATE_DIM)), + np.zeros((1, config.INPUT_DIM)), + ) return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 2a655b907..4f749dc5d 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -53,13 +53,15 @@ data_generation_n_threads = 12 data_generation_n_tasks = 12 data_generation_n_samples = 2 -data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order='F') +data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order="F") for i in range(0, 3): - data_generation_sampling_covariance[i, i] = 0.05 ** 2 # normalized linear momentum + data_generation_sampling_covariance[i, i] = 0.05**2 # normalized linear momentum for i in range(3, 6): - data_generation_sampling_covariance[i, i] = (config.normalized_inertia[i - 3] * 2.5 * np.pi / 180.0) ** 2 # normalized angular momentum + data_generation_sampling_covariance[i, i] = ( + config.normalized_inertia[i - 3] * 2.5 * np.pi / 180.0 + ) ** 2 # normalized angular momentum for i in range(6, 9): - data_generation_sampling_covariance[i, i] = 0.01 ** 2 # position + data_generation_sampling_covariance[i, i] = 0.01**2 # position for i in range(9, 12): data_generation_sampling_covariance[i, i] = (0.5 * np.pi / 180.0) ** 2 # orientation for i in range(12, 24): @@ -98,22 +100,28 @@ policy.to(config.device) print("Initial policy parameters:") print(list(policy.named_parameters())) -dummy_input = (torch.randn(1, config.TIME_DIM, device=config.device, dtype=config.dtype), - torch.randn(1, config.STATE_DIM, device=config.device, dtype=config.dtype)) +dummy_input = ( + torch.randn(1, config.TIME_DIM, device=config.device, dtype=config.dtype), + torch.randn(1, config.STATE_DIM, device=config.device, dtype=config.dtype), +) print("Saving initial policy.") save_path = "policies/" + folder + "/initial_policy" torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") # optimizer -batch_size = 2 ** 7 +batch_size = 2**7 learning_iterations = 100000 learning_rate_default = 1e-3 learning_rate_gating_net = learning_rate_default learning_rate_expert_nets = learning_rate_default -optimizer = torch.optim.Adam([{'params': policy.gating_net.parameters(), 'lr': learning_rate_gating_net}, - {'params': policy.expert_nets.parameters(), 'lr': learning_rate_expert_nets}], - lr=learning_rate_default) +optimizer = torch.optim.Adam( + [ + {"params": policy.gating_net.parameters(), "lr": learning_rate_gating_net}, + {"params": policy.expert_nets.parameters(), "lr": learning_rate_expert_nets}, + ], + lr=learning_rate_default, +) # weights for ["stance", "trot_1", "trot_2"] weights = [1, 2, 2] @@ -123,19 +131,32 @@ def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) choices = random.choices(["stance", "trot_1", "trot_2"], k=data_generation_n_tasks, weights=weights) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks(data_generation_n_tasks, data_generation_duration, choices) - mpcnet_interface.startDataGeneration(alpha, policy_file_path, data_generation_time_step, data_generation_data_decimation, - data_generation_n_samples, data_generation_sampling_covariance, - initial_observations, mode_schedules, target_trajectories) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks( + data_generation_n_tasks, data_generation_duration, choices + ) + mpcnet_interface.startDataGeneration( + alpha, + policy_file_path, + data_generation_time_step, + data_generation_data_decimation, + data_generation_n_samples, + data_generation_sampling_covariance, + initial_observations, + mode_schedules, + target_trajectories, + ) def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) choices = random.choices(["stance", "trot_1", "trot_2"], k=policy_evaluation_n_tasks, weights=weights) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks(policy_evaluation_n_tasks, policy_evaluation_duration, choices) - mpcnet_interface.startPolicyEvaluation(alpha, policy_file_path, policy_evaluation_time_step, - initial_observations, mode_schedules, target_trajectories) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks( + policy_evaluation_n_tasks, policy_evaluation_duration, choices + ) + mpcnet_interface.startPolicyEvaluation( + alpha, policy_file_path, policy_evaluation_time_step, initial_observations, mode_schedules, target_trajectories + ) try: @@ -155,11 +176,19 @@ def start_policy_evaluation(policy, alpha=0.0): data = mpcnet_interface.getGeneratedData() for i in range(len(data)): # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory - memory.push(data[i].t, data[i].x, data[i].u, helper.get_one_hot(data[i].mode), data[i].generalized_time, - data[i].relative_state, data[i].input_transformation, data[i].hamiltonian) + memory.push( + data[i].t, + data[i].x, + data[i].u, + helper.get_one_hot(data[i].mode), + data[i].generalized_time, + data[i].relative_state, + data[i].input_transformation, + data[i].hamiltonian, + ) # logging - writer.add_scalar('data/new_data_points', len(data), iteration) - writer.add_scalar('data/total_data_points', len(memory), iteration) + writer.add_scalar("data/new_data_points", len(data), iteration) + writer.add_scalar("data/total_data_points", len(memory), iteration) print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) # start new data generation start_data_generation(policy, alpha) @@ -171,9 +200,17 @@ def start_policy_evaluation(policy, alpha=0.0): survival_time = np.mean([metrics[i].survival_time for i in range(len(metrics))]) incurred_hamiltonian = np.mean([metrics[i].incurred_hamiltonian for i in range(len(metrics))]) # logging - writer.add_scalar('metric/survival_time', survival_time, iteration) - writer.add_scalar('metric/incurred_hamiltonian', incurred_hamiltonian, iteration) - print("iteration", iteration, "received metrics:", "incurred_hamiltonian", incurred_hamiltonian, "survival_time", survival_time) + writer.add_scalar("metric/survival_time", survival_time, iteration) + writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) + print( + "iteration", + iteration, + "received metrics:", + "incurred_hamiltonian", + incurred_hamiltonian, + "survival_time", + survival_time, + ) # start new policy evaluation start_policy_evaluation(policy) @@ -185,7 +222,21 @@ def start_policy_evaluation(policy, alpha=0.0): torch.save(obj=policy, f=save_path + ".pt") # extract batch from memory - t, x, u, p, generalized_time, relative_state, input_transformation, dHdxx, dHdux, dHduu, dHdx, dHdu, H = memory.sample(batch_size) + ( + t, + x, + u, + p, + generalized_time, + relative_state, + input_transformation, + dHdxx, + dHdux, + dHduu, + dHdx, + dHdu, + H, + ) = memory.sample(batch_size) # take an optimization step def closure(): @@ -195,17 +246,20 @@ def closure(): u_predicted, p_predicted = policy(generalized_time, relative_state) u_predicted = bmv(input_transformation, u_predicted) # compute the empirical loss - empirical_experts_loss = experts_loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size + empirical_experts_loss = ( + experts_loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size + ) empirical_gating_loss = gating_loss.compute_batch(p, p_predicted).sum() / batch_size empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss # compute the gradients empirical_loss.backward() # logging - writer.add_scalar('objective/empirical_experts_loss', empirical_experts_loss.item(), iteration) - writer.add_scalar('objective/empirical_gating_loss', empirical_gating_loss.item(), iteration) - writer.add_scalar('objective/empirical_loss', empirical_loss.item(), iteration) + writer.add_scalar("objective/empirical_experts_loss", empirical_experts_loss.item(), iteration) + writer.add_scalar("objective/empirical_gating_loss", empirical_gating_loss.item(), iteration) + writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) # return empirical loss return empirical_loss + optimizer.step(closure) # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index 2a85a8800..cb2824275 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -44,10 +44,9 @@ def u_transform(u): class LeggedRobotLinearPolicy(policy.LinearPolicy): - def __init__(self, dim_t, dim_x, dim_u): super().__init__(dim_t, dim_x, dim_u) - self.name = 'LeggedRobotLinearPolicy' + self.name = "LeggedRobotLinearPolicy" def forward(self, t, x): u = super().forward(t, x) @@ -55,10 +54,9 @@ def forward(self, t, x): class LeggedRobotNonlinearPolicy(policy.NonlinearPolicy): - def __init__(self, dim_t, dim_x, dim_u): super().__init__(dim_t, dim_x, dim_u) - self.name = 'LeggedRobotNonlinearPolicy' + self.name = "LeggedRobotNonlinearPolicy" def forward(self, t, x): u = super().forward(t, x) @@ -66,10 +64,9 @@ def forward(self, t, x): class LeggedRobotMixtureOfLinearExpertsPolicy(policy.MixtureOfLinearExpertsPolicy): - def __init__(self, dim_t, dim_x, dim_u, num_experts): super().__init__(dim_t, dim_x, dim_u, num_experts) - self.name = 'LeggedRobotMixtureOfLinearExpertsPolicy' + self.name = "LeggedRobotMixtureOfLinearExpertsPolicy" def forward(self, t, x): u, p = super().forward(t, x) @@ -77,10 +74,9 @@ def forward(self, t, x): class LeggedRobotMixtureOfNonlinearExpertsPolicy(policy.MixtureOfNonlinearExpertsPolicy): - def __init__(self, dim_t, dim_x, dim_u, num_experts): super().__init__(dim_t, dim_x, dim_u, num_experts) - self.name = 'LeggedRobotMixtureOfNonlinearExpertsPolicy' + self.name = "LeggedRobotMixtureOfNonlinearExpertsPolicy" def forward(self, t, x): u, p = super().forward(t, x) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py index 0a7b41b85..2227298c6 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py @@ -3,9 +3,6 @@ from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup -setup_args = generate_distutils_setup( - packages=['ocs2_legged_robot_mpcnet'], - package_dir={'': 'python'} -) +setup_args = generate_distutils_setup(packages=["ocs2_legged_robot_mpcnet"], package_dir={"": "python"}) setup(**setup_args) From bfe57396cc65c27fade1ac5e316f9a900e795e56 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 24 Mar 2022 09:29:16 +0100 Subject: [PATCH 140/512] document ballbot mpcnet python code with docstrings --- .../ocs2_ballbot_mpcnet/ballbot_config.py | 5 ++ .../ocs2_ballbot_mpcnet/ballbot_helper.py | 47 ++++++++++++++++++- .../ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 5 ++ 3 files changed, 55 insertions(+), 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py index 7ffcfc489..1819907a2 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py @@ -27,6 +27,11 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Ballbot configuration variables. + +Sets robot-specific configuration variables for ballbot. +""" + from ocs2_mpcnet import config # diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py index 0dbbfed21..16140310d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py @@ -27,6 +27,11 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Ballbot helper functions. + +Provides robot-specific helper functions for ballbot. +""" + import numpy as np from ocs2_mpcnet import helper @@ -34,14 +39,31 @@ def get_default_event_times_and_mode_sequence(duration): - # contact schedule: - - # swing schedule: - + """Get the event times and mode sequence describing the default mode schedule. + + Creates the default event times and mode sequence for a certain time duration. + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ event_times_template = np.array([1.0], dtype=np.float64) mode_sequence_template = np.array([0], dtype=np.uintp) return helper.get_event_times_and_mode_sequence(0, duration, event_times_template, mode_sequence_template) def get_random_initial_state(): + """Get a random initial state. + + Samples a random initial state for the robot. + + Returns: + x: A random initial state given by a NumPy array of shape (X) containing floats. + """ max_linear_velocity_x = 0.5 max_linear_velocity_y = 0.5 max_euler_angle_derivative_z = 45.0 * np.pi / 180.0 @@ -57,6 +79,13 @@ def get_random_initial_state(): def get_random_target_state(): + """Get a random target state. + + Samples a random target state for the robot. + + Returns: + x: A random target state given by a NumPy array of shape (X) containing floats. + """ max_position_x = 1.0 max_position_y = 1.0 max_orientation_z = 45.0 * np.pi / 180.0 @@ -68,6 +97,20 @@ def get_random_target_state(): def get_tasks(n_tasks, duration): + """Get tasks. + + Get a random set of task that should be executed by the data generation or policy evaluation. + + Args: + n_tasks: Number of tasks given by an integer. + duration: Duration of each task given by a float. + + Returns: + A tuple containing the components of the task. + - initial_observations: The initial observations given by an OCS2 system observation array. + - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. + - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. + """ initial_observations = helper.get_system_observation_array(n_tasks) mode_schedules = helper.get_mode_schedule_array(n_tasks) target_trajectories = helper.get_target_trajectories_array(n_tasks) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 483a3e0da..84d919e3a 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -27,6 +27,11 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Ballbot MPC-Net. + +Main script for training an MPC-Net policy for ballbot. +""" + import os import time import datetime From b987a39ec6a0ed55db90b4e00909f37578c7af3d Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 24 Mar 2022 09:40:08 +0100 Subject: [PATCH 141/512] add shebang line to main script --- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 84d919e3a..847cd9e2b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + ############################################################################### # Copyright (c) 2022, Farbod Farshidian. All rights reserved. # From ac061227b2866d25f5aaf4abba5ccda35369e17e Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 24 Mar 2022 11:40:03 +0100 Subject: [PATCH 142/512] document legged robot mpcnet python code with docstrings --- .../legged_robot_config.py | 5 + .../legged_robot_helper.py | 107 +++++++++++++++++- .../legged_robot_mpcnet.py | 5 + .../legged_robot_policy.py | 18 +++ 4 files changed, 129 insertions(+), 6 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py index f980ec7a2..214d5c031 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py @@ -27,6 +27,11 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Legged robot configuration variables. + +Sets robot-specific configuration variables for legged robot. +""" + from ocs2_mpcnet import config # diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py index d7f41b445..76adcab83 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -27,6 +27,11 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Legged robot helper functions. + +Provides robot-specific helper functions for legged robot. +""" + import numpy as np from ocs2_mpcnet import helper @@ -34,14 +39,33 @@ def get_stance(duration): - # contact schedule: STANCE - # swing schedule: - + """Get the stance gait. + + Creates the stance event times and mode sequence for a certain time duration: + - contact schedule: STANCE + - swing schedule: - + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ event_times_template = np.array([1.0], dtype=np.float64) mode_sequence_template = np.array([15], dtype=np.uintp) return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_random_initial_state_stance(): + """Get a random initial state for stance. + + Samples a random initial state for the robot in the stance gait. + + Returns: + x: A random initial state given by a NumPy array of shape (X) containing floats. + """ max_normalized_linear_momentum_x = 0.1 max_normalized_linear_momentum_y = 0.1 max_normalized_linear_momentum_z = 0.1 @@ -59,6 +83,13 @@ def get_random_initial_state_stance(): def get_random_target_state_stance(): + """Get a random target state for stance. + + Samples a random target state for the robot in the stance gait. + + Returns: + x: A random target state given by a NumPy array of shape (X) containing floats. + """ max_position_z = 0.075 max_orientation_z = 25.0 * np.pi / 180.0 max_orientation_y = 15.0 * np.pi / 180.0 @@ -72,22 +103,53 @@ def get_random_target_state_stance(): def get_trot_1(duration): - # contact schedule: LF_RH, RF_LH - # swing schedule: RF_LH, LF_RH + """Get the first trot gait. + + Creates the first trot event times and mode sequence for a certain time duration: + - contact schedule: LF_RH, RF_LH + - swing schedule: RF_LH, LF_RH + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ event_times_template = np.array([0.35, 0.7], dtype=np.float64) mode_sequence_template = np.array([9, 6], dtype=np.uintp) return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_trot_2(duration): - # contact schedule: RF_LH, LF_RH - # swing schedule: LF_RH, RF_LH + """Get the second trot gait. + + Creates the second trot event times and mode sequence for a certain time duration: + - contact schedule: RF_LH, LF_RH + - swing schedule: LF_RH, RF_LH + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ event_times_template = np.array([0.35, 0.7], dtype=np.float64) mode_sequence_template = np.array([6, 9], dtype=np.uintp) return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) def get_random_initial_state_trot(): + """Get a random initial state for trot. + + Samples a random initial state for the robot in a trot gait. + + Returns: + x: A random initial state given by a NumPy array of shape (X) containing floats. + """ max_normalized_linear_momentum_x = 0.5 max_normalized_linear_momentum_y = 0.25 max_normalized_linear_momentum_z = 0.25 @@ -105,6 +167,13 @@ def get_random_initial_state_trot(): def get_random_target_state_trot(): + """Get a random target state for trot. + + Samples a random target state for the robot in a trot gait. + + Returns: + x: A random target state given by a NumPy array of shape (X) containing floats. + """ max_position_x = 0.3 max_position_y = 0.15 max_orientation_z = 30.0 * np.pi / 180.0 @@ -116,6 +185,21 @@ def get_random_target_state_trot(): def get_tasks(n_tasks, duration, choices): + """Get tasks. + + Get a random set of task that should be executed by the data generation or policy evaluation. + + Args: + n_tasks: Number of tasks given by an integer. + duration: Duration of each task given by a float. + choices: Different gaits for the tasks given by a list containing strings with the gait names. + + Returns: + A tuple containing the components of the task. + - initial_observations: The initial observations given by an OCS2 system observation array. + - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. + - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. + """ initial_observations = helper.get_system_observation_array(n_tasks) mode_schedules = helper.get_mode_schedule_array(n_tasks) target_trajectories = helper.get_target_trajectories_array(n_tasks) @@ -154,6 +238,17 @@ def get_tasks(n_tasks, duration, choices): def get_one_hot(mode): + """Get one hot encoding of mode. + + Get a one hot encoding of a mode represented by a discrete probability distribution, where the sample space is the + set of P individually identified items given by the set of E individually identified experts. + + Args: + mode: The mode of the system given by an integer. + + Returns: + p: Discrete probability distribution given by a NumPy array of shape (P) containing floats. + """ one_hot = np.zeros(config.EXPERT_NUM) one_hot[config.expert_for_mode[mode]] = 1.0 return one_hot diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 4f749dc5d..841a82bd3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -27,6 +27,11 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Legged robot MPC-Net. + +Main script for training an MPC-Net policy for legged robot. +""" + import os import time import datetime diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index cb2824275..e0d55fc33 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -27,6 +27,14 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### +"""Legged robot policy classes. + +Provides robot-specific classes for different neural network policies for legged robot. + +Todo: + * Delete this file as part of refactoring, as it will be become obsolete. +""" + import torch from ocs2_mpcnet import policy @@ -40,6 +48,16 @@ def u_transform(u): + """Control input transformation. + + Transforms the predicted control input by scaling and adding a bias. + + Args: + u: A (B,U) tensor with the predicted control inputs. + + Returns: + u: A (B,U) tensor with the transformed control inputs. + """ return bmv(input_scaling, u) + input_bias From b18f62aa4d9139a968ea57d024c20f87c02d167e Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 24 Mar 2022 11:41:09 +0100 Subject: [PATCH 143/512] add shebang line to main script --- .../python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 841a82bd3..d84b7ad18 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + ############################################################################### # Copyright (c) 2022, Farbod Farshidian. All rights reserved. # From 2f297bbb06ea7b723fa11f8982be25dd3f0cda23 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 24 Mar 2022 11:51:42 +0100 Subject: [PATCH 144/512] small fix in doc --- ocs2_mpcnet/python/ocs2_mpcnet/helper.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py index 23d17274f..f41a0cc80 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py @@ -268,7 +268,9 @@ def get_event_times_and_mode_sequence(default_mode, duration, event_times_templa mode_sequence_template: The mode sequence template given by a NumPy array of shape (T) containing integers. Returns: - The event times and mode sequence given by NumPy arrays. + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. """ gait_cycle_duration = event_times_template[-1] num_gait_cycles = int(np.floor(duration / gait_cycle_duration)) From 4f33326f8904c3b80fc94113d1f3bf2e42eb69a1 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 25 Mar 2022 17:06:52 +0100 Subject: [PATCH 145/512] ensure only one thread per MPC instance --- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index 877732faa..3ea06dbd7 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -82,9 +82,12 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr<MPC_BASE> BallbotMpcnetInterface::getMpc(BallbotInterface& ballbotInterface) { - std::unique_ptr<MPC_BASE> mpcPtr(new GaussNewtonDDP_MPC(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), - ballbotInterface.getRollout(), ballbotInterface.getOptimalControlProblem(), - ballbotInterface.getInitializer())); + // ensure only one thread per MPC instance + auto ddpSettings = ballbotInterface.ddpSettings(); + ddpSettings.nThreads_ = 1; + // create one MPC instance + std::unique_ptr<MPC_BASE> mpcPtr(new GaussNewtonDDP_MPC(ballbotInterface.mpcSettings(), ddpSettings, ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer())); mpcPtr->getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); return mpcPtr; } From 8535cf69bc1a6132bbb7279b5e72a3873df95f30 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 25 Mar 2022 17:20:58 +0100 Subject: [PATCH 146/512] ensure only one thread per MPC instance --- .../ocs2_legged_robot/config/mpc/task.info | 2 +- .../src/LeggedRobotMpcnetInterface.cpp | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index a4d1736ff..2fc6e1d04 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -51,7 +51,7 @@ ddp { algorithm SLQ - nThreads 1 + nThreads 3 threadPriority 50 maxNumIterations 1 diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 5da1c37c0..73f0f625f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -107,8 +107,12 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr<MPC_BASE> LeggedRobotMpcnetInterface::getMpc(LeggedRobotInterface& leggedRobotInterface) { + // ensure only one thread per MPC instance + auto ddpSettings = leggedRobotInterface.ddpSettings(); + ddpSettings.nThreads_ = 1; + // create one MPC instance std::unique_ptr<MPC_BASE> mpcPtr( - new GaussNewtonDDP_MPC(leggedRobotInterface.mpcSettings(), leggedRobotInterface.ddpSettings(), leggedRobotInterface.getRollout(), + new GaussNewtonDDP_MPC(leggedRobotInterface.mpcSettings(), ddpSettings, leggedRobotInterface.getRollout(), leggedRobotInterface.getOptimalControlProblem(), leggedRobotInterface.getInitializer())); mpcPtr->getSolverPtr()->setReferenceManager(leggedRobotInterface.getReferenceManagerPtr()); return mpcPtr; From 37045a5750a73ce2c67c5c6b4c808785bb6bbb7b Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 25 Mar 2022 17:57:44 +0100 Subject: [PATCH 147/512] address comments --- .../ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h | 2 +- .../ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h | 4 ++-- .../src/BallbotMpcnetDefinition.cpp | 3 ++- .../src/BallbotMpcnetDummyNode.cpp | 12 ++++++------ .../src/BallbotMpcnetInterface.cpp | 6 +++--- 5 files changed, 14 insertions(+), 13 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h index 55d5738d7..55ee34551 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h @@ -37,7 +37,7 @@ namespace ballbot { /** * MPC-Net definitions for ballbot. */ -class BallbotMpcnetDefinition : public ocs2::mpcnet::MpcnetDefinitionBase { +class BallbotMpcnetDefinition final : public ocs2::mpcnet::MpcnetDefinitionBase { public: /** * Default constructor. diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h index f7e7bc8f4..cead0cfd3 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h @@ -38,7 +38,7 @@ namespace ballbot { /** * Ballbot MPC-Net interface between C++ and Python. */ -class BallbotMpcnetInterface : public ocs2::mpcnet::MpcnetInterfaceBase { +class BallbotMpcnetInterface final : public ocs2::mpcnet::MpcnetInterfaceBase { public: /** * Constructor. @@ -51,7 +51,7 @@ class BallbotMpcnetInterface : public ocs2::mpcnet::MpcnetInterfaceBase { /** * Default destructor. */ - virtual ~BallbotMpcnetInterface() = default; + ~BallbotMpcnetInterface() override = default; private: /** diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp index 656335f7e..87c63435a 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp @@ -38,7 +38,8 @@ vector_t BallbotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeSched vector_t BallbotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { vector_t relativeState = x - targetTrajectories.getDesiredState(t); - Eigen::Matrix<scalar_t, 2, 2> R = (Eigen::Matrix<scalar_t, 2, 2>() << cos(x(2)), -sin(x(2)), sin(x(2)), cos(x(2))).finished().transpose(); + const Eigen::Matrix<scalar_t, 2, 2> R = + (Eigen::Matrix<scalar_t, 2, 2>() << cos(x(2)), -sin(x(2)), sin(x(2)), cos(x(2))).finished().transpose(); relativeState.segment<2>(0) = R * relativeState.segment<2>(0); relativeState.segment<2>(5) = R * relativeState.segment<2>(5); return relativeState; diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp index f8efb7c1d..ac446370e 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp @@ -51,8 +51,8 @@ int main(int argc, char** argv) { if (programArgs.size() <= 2) { throw std::runtime_error("No task name and policy file path specified. Aborting."); } - std::string taskFileFolderName = std::string(programArgs[1]); - std::string policyFilePath = std::string(programArgs[2]); + const std::string taskFileFolderName = std::string(programArgs[1]); + const std::string policyFilePath = std::string(programArgs[2]); // initialize ros node ros::init(argc, argv, robotName + "_mpcnet_dummy"); @@ -69,7 +69,7 @@ int main(int argc, char** argv) { // policy (MPC-Net controller) auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); - std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition()); + std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition); std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase> mpcnetControllerPtr( new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); mpcnetControllerPtr->loadPolicyModel(policyFilePath); @@ -85,8 +85,8 @@ int main(int argc, char** argv) { std::shared_ptr<BallbotDummyVisualization> ballbotDummyVisualization(new BallbotDummyVisualization(nodeHandle)); // MPC-Net dummy loop ROS - scalar_t controlFrequency = ballbotInterface.mpcSettings().mrtDesiredFrequency_; - scalar_t rosFrequency = ballbotInterface.mpcSettings().mpcDesiredFrequency_; + const scalar_t controlFrequency = ballbotInterface.mpcSettings().mrtDesiredFrequency_; + const scalar_t rosFrequency = ballbotInterface.mpcSettings().mpcDesiredFrequency_; ocs2::mpcnet::MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), rosReferenceManagerPtr); mpcnetDummyLoopRos.addObserver(mpcnetDummyObserverRosPtr); @@ -100,7 +100,7 @@ int main(int argc, char** argv) { systemObservation.input = vector_t::Zero(ocs2::ballbot::INPUT_DIM); // initial target trajectories - TargetTrajectories targetTrajectories({systemObservation.time}, {systemObservation.state}, {systemObservation.input}); + const TargetTrajectories targetTrajectories({systemObservation.time}, {systemObservation.state}, {systemObservation.input}); // run MPC-Net dummy loop ROS mpcnetDummyLoopRos.run(systemObservation, targetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index 3ea06dbd7..835deae69 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -45,9 +45,9 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si // create ONNX environment auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); // path to config file - std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/mpc/task.info"; + const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/mpc/task.info"; // path to save auto-generated libraries - std::string libraryFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + const std::string libraryFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; // set up MPC-Net rollout manager for data generation and policy evaluation std::vector<std::unique_ptr<MPC_BASE>> mpcPtrs; std::vector<std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>> mpcnetPtrs; @@ -61,7 +61,7 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { BallbotInterface ballbotInterface(taskFile, libraryFolder); - std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition()); + std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition); mpcPtrs.push_back(getMpc(ballbotInterface)); mpcnetPtrs.push_back(std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>( new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, ballbotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr))); From bc810aa93c8cccf93283846e2599982941e91959 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 25 Mar 2022 18:25:45 +0100 Subject: [PATCH 148/512] ensure MPC and DDP settings are as needed for MPC-Net --- .../ocs2_ballbot/config/mpc/task.info | 2 +- .../src/BallbotMpcnetInterface.cpp | 24 +++++++++++++++---- 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info index 44df1e5e8..13865354a 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info @@ -52,7 +52,7 @@ ddp preComputeRiccatiTerms true - useFeedbackPolicy true + useFeedbackPolicy false strategy LINE_SEARCH lineSearch diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index 835deae69..1860cd9a8 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -82,11 +82,27 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr<MPC_BASE> BallbotMpcnetInterface::getMpc(BallbotInterface& ballbotInterface) { - // ensure only one thread per MPC instance - auto ddpSettings = ballbotInterface.ddpSettings(); - ddpSettings.nThreads_ = 1; + // ensure MPC and DDP settings are as needed for MPC-Net + const auto mpcSettings = [&]() { + auto settings = ballbotInterface.mpcSettings(); + settings.debugPrint_ = false; + settings.coldStart_ = false; + return settings; + }(); + const auto ddpSettings = [&]() { + auto settings = ballbotInterface.ddpSettings(); + settings.algorithm_ = ocs2::ddp::Algorithm::SLQ; + settings.nThreads_ = 1; + settings.displayInfo_ = false; + settings.displayShortSummary_ = false; + settings.checkNumericalStability_ = false; + settings.debugPrintRollout_ = false; + settings.debugCaching_ = false; + settings.useFeedbackPolicy_ = true; + return settings; + }(); // create one MPC instance - std::unique_ptr<MPC_BASE> mpcPtr(new GaussNewtonDDP_MPC(ballbotInterface.mpcSettings(), ddpSettings, ballbotInterface.getRollout(), + std::unique_ptr<MPC_BASE> mpcPtr(new GaussNewtonDDP_MPC(mpcSettings, ddpSettings, ballbotInterface.getRollout(), ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer())); mpcPtr->getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); return mpcPtr; From 628af4fb2be7aa119be9bb81751aa3d5c40bdbca Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 25 Mar 2022 18:44:57 +0100 Subject: [PATCH 149/512] ensure MPC and DDP settings are as needed for MPC-Net --- .../ocs2_legged_robot/config/mpc/task.info | 2 +- .../src/LeggedRobotMpcnetInterface.cpp | 28 +++++++++++++++---- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index 2fc6e1d04..0774030fd 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -77,7 +77,7 @@ ddp preComputeRiccatiTerms true - useFeedbackPolicy true + useFeedbackPolicy false strategy LINE_SEARCH lineSearch diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 73f0f625f..d8d821938 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -107,13 +107,29 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr<MPC_BASE> LeggedRobotMpcnetInterface::getMpc(LeggedRobotInterface& leggedRobotInterface) { - // ensure only one thread per MPC instance - auto ddpSettings = leggedRobotInterface.ddpSettings(); - ddpSettings.nThreads_ = 1; + // ensure MPC and DDP settings are as needed for MPC-Net + const auto mpcSettings = [&]() { + auto settings = leggedRobotInterface.mpcSettings(); + settings.debugPrint_ = false; + settings.coldStart_ = false; + return settings; + }(); + const auto ddpSettings = [&]() { + auto settings = leggedRobotInterface.ddpSettings(); + settings.algorithm_ = ocs2::ddp::Algorithm::SLQ; + settings.nThreads_ = 1; + settings.displayInfo_ = false; + settings.displayShortSummary_ = false; + settings.checkNumericalStability_ = false; + settings.debugPrintRollout_ = false; + settings.debugCaching_ = false; + settings.useFeedbackPolicy_ = true; + return settings; + }(); // create one MPC instance - std::unique_ptr<MPC_BASE> mpcPtr( - new GaussNewtonDDP_MPC(leggedRobotInterface.mpcSettings(), ddpSettings, leggedRobotInterface.getRollout(), - leggedRobotInterface.getOptimalControlProblem(), leggedRobotInterface.getInitializer())); + std::unique_ptr<MPC_BASE> mpcPtr(new GaussNewtonDDP_MPC(mpcSettings, ddpSettings, leggedRobotInterface.getRollout(), + leggedRobotInterface.getOptimalControlProblem(), + leggedRobotInterface.getInitializer())); mpcPtr->getSolverPtr()->setReferenceManager(leggedRobotInterface.getReferenceManagerPtr()); return mpcPtr; } From b4b52c10ac494ec2b95e5a9235ec52491108fc64 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Mar 2022 12:10:04 +0200 Subject: [PATCH 150/512] fix raisim rollout destructor --- .../ocs2_raisim_core/include/ocs2_raisim_core/RaisimRollout.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_raisim/ocs2_raisim_core/include/ocs2_raisim_core/RaisimRollout.h b/ocs2_raisim/ocs2_raisim_core/include/ocs2_raisim_core/RaisimRollout.h index 59e613159..a62a19dd0 100644 --- a/ocs2_raisim/ocs2_raisim_core/include/ocs2_raisim_core/RaisimRollout.h +++ b/ocs2_raisim/ocs2_raisim_core/include/ocs2_raisim_core/RaisimRollout.h @@ -85,7 +85,7 @@ class RaisimRollout final : public RolloutBase { RaisimRollout(const RaisimRollout& other); //! Destructor - ~RaisimRollout(); + ~RaisimRollout() override; void resetRollout() override { raisimRolloutSettings_.setSimulatorStateOnRolloutRunOnce_ = true; } From eb8c838d1e55f947df4915e0e8bc2d0597c55d77 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Mar 2022 12:12:02 +0200 Subject: [PATCH 151/512] fix ocs2_legged_robot for mpcnet --- .../include/ocs2_legged_robot/common/Types.h | 4 +- .../ocs2_legged_robot/gait/GaitSchedule.h | 4 + .../include/ocs2_legged_robot/gait/LegLogic.h | 89 ++++------- .../ocs2_legged_robot/src/gait/LegLogic.cpp | 148 ++++++++++++++++-- 4 files changed, 168 insertions(+), 77 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/common/Types.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/common/Types.h index e7643ca47..646a72cc8 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/common/Types.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/common/Types.h @@ -37,10 +37,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace legged_robot { -constexpr size_t NUM_CONTACT_POINTS = 4; - template <typename T> -using feet_array_t = std::array<T, NUM_CONTACT_POINTS>; +using feet_array_t = std::array<T, 4>; using contact_flag_t = feet_array_t<bool>; using vector3_t = Eigen::Matrix<scalar_t, 3, 1>; diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h index a7221e953..fe97199f4 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h @@ -44,11 +44,15 @@ class GaitSchedule { GaitSchedule(ModeSchedule initModeSchedule, ModeSequenceTemplate initModeSequenceTemplate, scalar_t phaseTransitionStanceTime); /** + * Sets the mode schedule. + * * @param [in] modeSchedule: The mode schedule to be used. */ void setModeSchedule(const ModeSchedule& modeSchedule) { modeSchedule_ = modeSchedule; } /** + * Gets the mode schedule. + * * @param [in] lowerBoundTime: The smallest time for which the ModeSchedule should be defined. * @param [in] upperBoundTime: The greatest time for which the ModeSchedule should be defined. */ diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h index 460b88037..6f105b980 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_core/reference/ModeSchedule.h> @@ -22,66 +51,6 @@ struct SwingTiming { scalar_t end; }; -inline scalar_t timingNaN() { - return std::numeric_limits<scalar_t>::quiet_NaN(); -} - -inline bool hasStartTime(const ContactTiming& timing) { - return !std::isnan(timing.start); -} -inline bool hasEndTime(const ContactTiming& timing) { - return !std::isnan(timing.end); -} - -inline bool hasStartTime(const SwingTiming& timing) { - return !std::isnan(timing.start); -} -inline bool hasEndTime(const SwingTiming& timing) { - return !std::isnan(timing.end); -} - -inline bool startsWithSwingPhase(const std::vector<ContactTiming>& timings) { - return timings.empty() || hasStartTime(timings.front()); -} -inline bool startsWithContactPhase(const std::vector<ContactTiming>& timings) { - return !startsWithSwingPhase(timings); -} -inline bool endsWithSwingPhase(const std::vector<ContactTiming>& timings) { - return timings.empty() || hasEndTime(timings.back()); -} -inline bool endsWithContactPhase(const std::vector<ContactTiming>& timings) { - return !endsWithSwingPhase(timings); -} - -inline bool startsWithContactPhase(const std::vector<SwingTiming>& timings) { - return timings.empty() || hasStartTime(timings.front()); -} -inline bool startsWithSwingPhase(const std::vector<SwingTiming>& timings) { - return !startsWithContactPhase(timings); -} -inline bool endsWithContactPhase(const std::vector<SwingTiming>& timings) { - return timings.empty() || hasEndTime(timings.back()); -} -inline bool endsWithSwingPhase(const std::vector<SwingTiming>& timings) { - return !endsWithContactPhase(timings); -} - -inline bool touchesDownAtLeastOnce(const std::vector<ContactTiming>& timings) { - return std::any_of(timings.begin(), timings.end(), [](const ContactTiming& timing) { return hasStartTime(timing); }); -} - -inline bool liftsOffAtLeastOnce(const std::vector<ContactTiming>& timings) { - return !timings.empty() && hasEndTime(timings.front()); -} - -inline bool touchesDownAtLeastOnce(const std::vector<SwingTiming>& timings) { - return !timings.empty() && hasEndTime(timings.front()); -} - -inline bool liftsOffAtLeastOnce(const std::vector<SwingTiming>& timings) { - return std::any_of(timings.begin(), timings.end(), [](const SwingTiming& timing) { return hasStartTime(timing); }); -} - /** * @brief Get the contact phase for all legs. * If leg in contact, returns a value between 0.0 (at start of contact phase) and 1.0 (at end of contact phase). diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp index 8e0cc32fc..d61ae5b71 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp @@ -1,10 +1,106 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_legged_robot/gait/LegLogic.h" #include "ocs2_legged_robot/gait/MotionPhaseDefinition.h" +namespace { + +inline ocs2::scalar_t timingNaN() { + return std::numeric_limits<ocs2::scalar_t>::quiet_NaN(); +} + +inline bool hasStartTime(const ocs2::legged_robot::ContactTiming& timing) { + return !std::isnan(timing.start); +} +inline bool hasEndTime(const ocs2::legged_robot::ContactTiming& timing) { + return !std::isnan(timing.end); +} + +inline bool hasStartTime(const ocs2::legged_robot::SwingTiming& timing) { + return !std::isnan(timing.start); +} +inline bool hasEndTime(const ocs2::legged_robot::SwingTiming& timing) { + return !std::isnan(timing.end); +} + +inline bool startsWithSwingPhase(const std::vector<ocs2::legged_robot::ContactTiming>& timings) { + return timings.empty() || hasStartTime(timings.front()); +} +inline bool startsWithContactPhase(const std::vector<ocs2::legged_robot::ContactTiming>& timings) { + return !startsWithSwingPhase(timings); +} +inline bool endsWithSwingPhase(const std::vector<ocs2::legged_robot::ContactTiming>& timings) { + return timings.empty() || hasEndTime(timings.back()); +} +inline bool endsWithContactPhase(const std::vector<ocs2::legged_robot::ContactTiming>& timings) { + return !endsWithSwingPhase(timings); +} + +inline bool startsWithContactPhase(const std::vector<ocs2::legged_robot::SwingTiming>& timings) { + return timings.empty() || hasStartTime(timings.front()); +} +inline bool startsWithSwingPhase(const std::vector<ocs2::legged_robot::SwingTiming>& timings) { + return !startsWithContactPhase(timings); +} +inline bool endsWithContactPhase(const std::vector<ocs2::legged_robot::SwingTiming>& timings) { + return timings.empty() || hasEndTime(timings.back()); +} +inline bool endsWithSwingPhase(const std::vector<ocs2::legged_robot::SwingTiming>& timings) { + return !endsWithContactPhase(timings); +} + +inline bool touchesDownAtLeastOnce(const std::vector<ocs2::legged_robot::ContactTiming>& timings) { + return std::any_of(timings.begin(), timings.end(), [](const ocs2::legged_robot::ContactTiming& timing) { return hasStartTime(timing); }); +} + +inline bool liftsOffAtLeastOnce(const std::vector<ocs2::legged_robot::ContactTiming>& timings) { + return !timings.empty() && hasEndTime(timings.front()); +} + +inline bool touchesDownAtLeastOnce(const std::vector<ocs2::legged_robot::SwingTiming>& timings) { + return !timings.empty() && hasEndTime(timings.front()); +} + +inline bool liftsOffAtLeastOnce(const std::vector<ocs2::legged_robot::SwingTiming>& timings) { + return std::any_of(timings.begin(), timings.end(), [](const ocs2::legged_robot::SwingTiming& timing) { return hasStartTime(timing); }); +} + +} // anonymous namespace + namespace ocs2 { namespace legged_robot { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ feet_array_t<LegPhase> getContactPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule) { feet_array_t<LegPhase> contactPhasePerLeg; @@ -12,18 +108,18 @@ feet_array_t<LegPhase> getContactPhasePerLeg(scalar_t time, const ocs2::ModeSche const auto contactTimingsPerLeg = extractContactTimingsPerLeg(modeSchedule); // Extract contact phases per leg - for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { + for (size_t leg = 0; leg < contactPhasePerLeg.size(); ++leg) { if (contactTimingsPerLeg[leg].empty()) { // Leg is always in swing phase - contactPhasePerLeg[leg].phase = scalar_t(-1.0); + contactPhasePerLeg[leg].phase = -1.0; contactPhasePerLeg[leg].duration = std::numeric_limits<scalar_t>::quiet_NaN(); } else if (startsWithContactPhase(contactTimingsPerLeg[leg]) && (time <= contactTimingsPerLeg[leg].front().end)) { // It is assumed that contact phase started at minus infinity, so current time will be always close to ContactTiming.end - contactPhasePerLeg[leg].phase = scalar_t(1.0); + contactPhasePerLeg[leg].phase = 1.0; contactPhasePerLeg[leg].duration = std::numeric_limits<scalar_t>::infinity(); } else if (endsWithContactPhase(contactTimingsPerLeg[leg]) && (time >= contactTimingsPerLeg[leg].back().start)) { // It is assumed that contact phase ends at infinity, so current time will be always close to ContactTiming.start - contactPhasePerLeg[leg].phase = scalar_t(0.0); + contactPhasePerLeg[leg].phase = 0.0; contactPhasePerLeg[leg].duration = std::numeric_limits<scalar_t>::infinity(); } else { // Check if leg is in contact interval at current time @@ -31,11 +127,11 @@ feet_array_t<LegPhase> getContactPhasePerLeg(scalar_t time, const ocs2::ModeSche [time](ContactTiming timing) { return (timing.start <= time) && (time <= timing.end); }); if (it == contactTimingsPerLeg[leg].end()) { // Leg is not in contact for current time - contactPhasePerLeg[leg].phase = scalar_t(-1.0); + contactPhasePerLeg[leg].phase = -1.0; contactPhasePerLeg[leg].duration = std::numeric_limits<scalar_t>::quiet_NaN(); } else { // Leg is in contact for current time - const auto currentContactTiming = *it; + const auto& currentContactTiming = *it; contactPhasePerLeg[leg].phase = (time - currentContactTiming.start) / (currentContactTiming.end - currentContactTiming.start); contactPhasePerLeg[leg].duration = currentContactTiming.end - currentContactTiming.start; } @@ -45,6 +141,9 @@ feet_array_t<LegPhase> getContactPhasePerLeg(scalar_t time, const ocs2::ModeSche return contactPhasePerLeg; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ feet_array_t<LegPhase> getSwingPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule) { feet_array_t<LegPhase> swingPhasePerLeg; @@ -52,18 +151,18 @@ feet_array_t<LegPhase> getSwingPhasePerLeg(scalar_t time, const ocs2::ModeSchedu const auto swingTimingsPerLeg = extractSwingTimingsPerLeg(modeSchedule); // Extract swing phases per leg - for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { + for (size_t leg = 0; leg < swingPhasePerLeg.size(); ++leg) { if (swingTimingsPerLeg[leg].empty()) { // Leg is always in contact phase - swingPhasePerLeg[leg].phase = scalar_t(-1.0); + swingPhasePerLeg[leg].phase = -1.0; swingPhasePerLeg[leg].duration = std::numeric_limits<scalar_t>::quiet_NaN(); } else if (startsWithSwingPhase(swingTimingsPerLeg[leg]) && (time <= swingTimingsPerLeg[leg].front().end)) { // It is assumed that swing phase started at minus infinity, so current time will be always close to SwingTiming.end - swingPhasePerLeg[leg].phase = scalar_t(1.0); + swingPhasePerLeg[leg].phase = 1.0; swingPhasePerLeg[leg].duration = std::numeric_limits<scalar_t>::infinity(); } else if (endsWithSwingPhase(swingTimingsPerLeg[leg]) && (time >= swingTimingsPerLeg[leg].back().start)) { // It is assumed that swing phase ends at infinity, so current time will be always close to SwingTiming.start - swingPhasePerLeg[leg].phase = scalar_t(0.0); + swingPhasePerLeg[leg].phase = 0.0; swingPhasePerLeg[leg].duration = std::numeric_limits<scalar_t>::infinity(); } else { // Check if leg is in swing interval at current time @@ -75,7 +174,7 @@ feet_array_t<LegPhase> getSwingPhasePerLeg(scalar_t time, const ocs2::ModeSchedu swingPhasePerLeg[leg].duration = std::numeric_limits<scalar_t>::quiet_NaN(); } else { // Leg is swinging for current time - const auto currentSwingTiming = *it; + const auto& currentSwingTiming = *it; swingPhasePerLeg[leg].phase = (time - currentSwingTiming.start) / (currentSwingTiming.end - currentSwingTiming.start); swingPhasePerLeg[leg].duration = currentSwingTiming.end - currentSwingTiming.start; } @@ -85,6 +184,9 @@ feet_array_t<LegPhase> getSwingPhasePerLeg(scalar_t time, const ocs2::ModeSchedu return swingPhasePerLeg; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ feet_array_t<std::vector<ContactTiming>> extractContactTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule) { feet_array_t<std::vector<ContactTiming>> contactTimingsPerLeg; @@ -92,13 +194,16 @@ feet_array_t<std::vector<ContactTiming>> extractContactTimingsPerLeg(const ocs2: const auto contactSequencePerLeg = extractContactFlags(modeSchedule.modeSequence); // Extract timings per leg - for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { + for (size_t leg = 0; leg < contactTimingsPerLeg.size(); ++leg) { contactTimingsPerLeg[leg] = extractContactTimings(modeSchedule.eventTimes, contactSequencePerLeg[leg]); } return contactTimingsPerLeg; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ feet_array_t<std::vector<SwingTiming>> extractSwingTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule) { feet_array_t<std::vector<SwingTiming>> swingTimingsPerLeg; @@ -106,13 +211,16 @@ feet_array_t<std::vector<SwingTiming>> extractSwingTimingsPerLeg(const ocs2::Mod const auto contactSequencePerLeg = extractContactFlags(modeSchedule.modeSequence); // Extract timings per leg - for (size_t leg = 0; leg < NUM_CONTACT_POINTS; ++leg) { + for (size_t leg = 0; leg < swingTimingsPerLeg.size(); ++leg) { swingTimingsPerLeg[leg] = extractSwingTimings(modeSchedule.eventTimes, contactSequencePerLeg[leg]); } return swingTimingsPerLeg; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector<ContactTiming>& contactTimings) { for (const auto& contactPhase : contactTimings) { if (hasEndTime(contactPhase) && contactPhase.end > currentTime) { @@ -122,6 +230,9 @@ scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector<ContactTim return timingNaN(); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ scalar_t getTimeOfNextTouchDown(scalar_t currentTime, const std::vector<ContactTiming>& contactTimings) { for (const auto& contactPhase : contactTimings) { if (hasStartTime(contactPhase) && contactPhase.start > currentTime) { @@ -131,6 +242,9 @@ scalar_t getTimeOfNextTouchDown(scalar_t currentTime, const std::vector<ContactT return timingNaN(); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ std::vector<ContactTiming> extractContactTimings(const std::vector<scalar_t>& eventTimes, const std::vector<bool>& contactFlags) { assert(eventTimes.size() + 1 == contactFlags.size()); const int numPhases = contactFlags.size(); @@ -166,6 +280,9 @@ std::vector<ContactTiming> extractContactTimings(const std::vector<scalar_t>& ev return contactTimings; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ std::vector<SwingTiming> extractSwingTimings(const std::vector<scalar_t>& eventTimes, const std::vector<bool>& contactFlags) { assert(eventTimes.size() + 1 == contactFlags.size()); const int numPhases = contactFlags.size(); @@ -201,6 +318,9 @@ std::vector<SwingTiming> extractSwingTimings(const std::vector<scalar_t>& eventT return swingTimings; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ feet_array_t<std::vector<bool>> extractContactFlags(const std::vector<size_t>& modeSequence) { const size_t numPhases = modeSequence.size(); @@ -209,7 +329,7 @@ feet_array_t<std::vector<bool>> extractContactFlags(const std::vector<size_t>& m for (size_t i = 0; i < numPhases; i++) { const auto contactFlag = modeNumber2StanceLeg(modeSequence[i]); - for (size_t j = 0; j < NUM_CONTACT_POINTS; j++) { + for (size_t j = 0; j < contactFlagStock.size(); j++) { contactFlagStock[j][i] = contactFlag[j]; } } From a52f55fc559322aedb072c1ae31fa485ed04d189 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Mar 2022 12:19:21 +0200 Subject: [PATCH 152/512] address comments --- .../ocs2_legged_robot_mpcnet/CMakeLists.txt | 2 +- .../LeggedRobotMpcnetDefinition.h | 7 +++- .../LeggedRobotMpcnetInterface.h | 4 +- .../src/LeggedRobotMpcnetDefinition.cpp | 39 +++++++++---------- .../src/LeggedRobotMpcnetDummyNode.cpp | 6 +-- .../src/LeggedRobotMpcnetInterface.cpp | 10 ++--- 6 files changed, 35 insertions(+), 33 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt index ef000ebdb..c213339f4 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt @@ -63,7 +63,7 @@ target_link_libraries(LeggedRobotMpcnetPybindings PRIVATE ${catkin_LIBRARIES} ) set_target_properties(LeggedRobotMpcnetPybindings - PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} + PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) # MPC-Net dummy node diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h index 92158e474..bfc630187 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h @@ -37,7 +37,7 @@ namespace legged_robot { /** * MPC-Net definitions for legged robot. */ -class LeggedRobotMpcnetDefinition : public ocs2::mpcnet::MpcnetDefinitionBase { +class LeggedRobotMpcnetDefinition final : public ocs2::mpcnet::MpcnetDefinitionBase { public: /** * Constructor. @@ -71,7 +71,10 @@ class LeggedRobotMpcnetDefinition : public ocs2::mpcnet::MpcnetDefinitionBase { bool validState(const vector_t& x) override; private: - vector_t defaultState_; + const scalar_t allowedHeightDeviation_ = 0.2; + const scalar_t allowedPitchDeviation_ = 30.0 * M_PI / 180.0; + const scalar_t allowedRollDeviation_ = 30.0 * M_PI / 180.0; + const vector_t defaultState_; }; } // namespace legged_robot diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h index 6a5d6ab42..90eae8b8a 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h @@ -39,7 +39,7 @@ namespace legged_robot { /** * Legged robot MPC-Net interface between C++ and Python. */ -class LeggedRobotMpcnetInterface : public ocs2::mpcnet::MpcnetInterfaceBase { +class LeggedRobotMpcnetInterface final : public ocs2::mpcnet::MpcnetInterfaceBase { public: /** * Constructor. @@ -52,7 +52,7 @@ class LeggedRobotMpcnetInterface : public ocs2::mpcnet::MpcnetInterfaceBase { /** * Default destructor. */ - virtual ~LeggedRobotMpcnetInterface() = default; + ~LeggedRobotMpcnetInterface() override = default; private: /** diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index f2a3fa548..cf5b2fdff 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -38,31 +38,30 @@ namespace ocs2 { namespace legged_robot { vector_t LeggedRobotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) { - feet_array_t<LegPhase> swingPhasePerLeg = getSwingPhasePerLeg(t, modeSchedule); - vector_t generalizedTime; - generalizedTime.resize(3 * NUM_CONTACT_POINTS, Eigen::NoChange); + const feet_array_t<LegPhase> swingPhasePerLeg = getSwingPhasePerLeg(t, modeSchedule); + vector_t generalizedTime(3 * swingPhasePerLeg.size()); // phase - for (int i = 0 * NUM_CONTACT_POINTS; i < 1 * NUM_CONTACT_POINTS; i++) { - if (swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase < 0.0) { + for (int i = 0; i < swingPhasePerLeg.size(); i++) { + if (swingPhasePerLeg[i].phase < 0.0) { generalizedTime[i] = 0.0; } else { - generalizedTime[i] = swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase; + generalizedTime[i] = swingPhasePerLeg[i].phase; } } // phase rate - for (int i = 1 * NUM_CONTACT_POINTS; i < 2 * NUM_CONTACT_POINTS; i++) { - if (swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase < 0.0) { - generalizedTime[i] = 0.0; + for (int i = 0; i < swingPhasePerLeg.size(); i++) { + if (swingPhasePerLeg[i].phase < 0.0) { + generalizedTime[i + swingPhasePerLeg.size()] = 0.0; } else { - generalizedTime[i] = 1.0 / swingPhasePerLeg[i % NUM_CONTACT_POINTS].duration; + generalizedTime[i + swingPhasePerLeg.size()] = 1.0 / swingPhasePerLeg[i].duration; } } // sin(pi * phase) - for (int i = 2 * NUM_CONTACT_POINTS; i < 3 * NUM_CONTACT_POINTS; i++) { - if (swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase < 0.0) { - generalizedTime[i] = 0.0; + for (int i = 0; i < swingPhasePerLeg.size(); i++) { + if (swingPhasePerLeg[i].phase < 0.0) { + generalizedTime[i + 2 * swingPhasePerLeg.size()] = 0.0; } else { - generalizedTime[i] = std::sin(M_PI * swingPhasePerLeg[i % NUM_CONTACT_POINTS].phase); + generalizedTime[i + 2 * swingPhasePerLeg.size()] = std::sin(M_PI * swingPhasePerLeg[i].phase); } } return generalizedTime; @@ -70,7 +69,7 @@ vector_t LeggedRobotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeS vector_t LeggedRobotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { vector_t relativeState = x - targetTrajectories.getDesiredState(t); - matrix3_t R = getRotationMatrixFromZyxEulerAngles<scalar_t>(x.segment<3>(9)).transpose(); + const matrix3_t R = getRotationMatrixFromZyxEulerAngles<scalar_t>(x.segment<3>(9)).transpose(); relativeState.segment<3>(0) = R * relativeState.segment<3>(0); relativeState.segment<3>(3) = R * relativeState.segment<3>(3); relativeState.segment<3>(6) = R * relativeState.segment<3>(6); @@ -79,7 +78,7 @@ vector_t LeggedRobotMpcnetDefinition::getRelativeState(scalar_t t, const vector_ } matrix_t LeggedRobotMpcnetDefinition::getInputTransformation(scalar_t t, const vector_t& x) { - matrix3_t R = getRotationMatrixFromZyxEulerAngles<scalar_t>(x.segment<3>(9)); + const matrix3_t R = getRotationMatrixFromZyxEulerAngles<scalar_t>(x.segment<3>(9)); matrix_t inputTransformation = matrix_t::Identity(24, 24); inputTransformation.block<3, 3>(0, 0) = R; inputTransformation.block<3, 3>(3, 3) = R; @@ -89,14 +88,14 @@ matrix_t LeggedRobotMpcnetDefinition::getInputTransformation(scalar_t t, const v } bool LeggedRobotMpcnetDefinition::validState(const vector_t& x) { - vector_t deviation = x - defaultState_; - if (std::abs(deviation[8]) > 0.2) { + const vector_t deviation = x - defaultState_; + if (std::abs(deviation[8]) > allowedHeightDeviation_) { std::cerr << "[LeggedRobotMpcnetDefinition::validState] height diverged: " << x[8] << "\n"; return false; - } else if (std::abs(deviation[10]) > 30.0 * M_PI / 180.0) { + } else if (std::abs(deviation[10]) > allowedPitchDeviation_) { std::cerr << "[LeggedRobotMpcnetDefinition::validState] pitch diverged: " << x[10] << "\n"; return false; - } else if (std::abs(deviation[11]) > 30.0 * M_PI / 180.0) { + } else if (std::abs(deviation[11]) > allowedRollDeviation_) { std::cerr << "[LeggedRobotMpcnetDefinition::validState] roll diverged: " << x[11] << "\n"; return false; } else { diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 45824a4da..469a1538f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -140,8 +140,8 @@ int main(int argc, char** argv) { } // MPC-Net dummy loop ROS - scalar_t controlFrequency = leggedRobotInterface.mpcSettings().mrtDesiredFrequency_; - scalar_t rosFrequency = leggedRobotInterface.mpcSettings().mpcDesiredFrequency_; + const scalar_t controlFrequency = leggedRobotInterface.mpcSettings().mrtDesiredFrequency_; + const scalar_t rosFrequency = leggedRobotInterface.mpcSettings().mpcDesiredFrequency_; ocs2::mpcnet::MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), rosReferenceManagerPtr); mpcnetDummyLoopRos.addObserver(mpcnetDummyObserverRosPtr); @@ -156,7 +156,7 @@ int main(int argc, char** argv) { systemObservation.input = vector_t::Zero(leggedRobotInterface.getCentroidalModelInfo().inputDim); // initial target trajectories - TargetTrajectories targetTrajectories({systemObservation.time}, {systemObservation.state}, {systemObservation.input}); + const TargetTrajectories targetTrajectories({systemObservation.time}, {systemObservation.state}, {systemObservation.input}); // run MPC-Net dummy loop ROS mpcnetDummyLoopRos.run(systemObservation, targetTrajectories); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index d8d821938..b20ca6a0f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -47,11 +47,11 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr // create ONNX environment auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); // paths to files - std::string taskFile = ros::package::getPath("ocs2_legged_robot") + "/config/mpc/task.info"; - std::string urdfFile = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf"; - std::string referenceFile = ros::package::getPath("ocs2_legged_robot") + "/config/command/reference.info"; - std::string raisimFile = ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info"; - std::string resourcePath = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes"; + const std::string taskFile = ros::package::getPath("ocs2_legged_robot") + "/config/mpc/task.info"; + const std::string urdfFile = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf"; + const std::string referenceFile = ros::package::getPath("ocs2_legged_robot") + "/config/command/reference.info"; + const std::string raisimFile = ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info"; + const std::string resourcePath = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes"; // set up MPC-Net rollout manager for data generation and policy evaluation std::vector<std::unique_ptr<MPC_BASE>> mpcPtrs; std::vector<std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>> mpcnetPtrs; From 365c421c89585678a6a8a1c91b41632db177dc3f Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Mar 2022 18:28:34 +0200 Subject: [PATCH 153/512] update developers --- ocs2_doc/docs/overview.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ocs2_doc/docs/overview.rst b/ocs2_doc/docs/overview.rst index 6c7e5f68c..6141cc135 100644 --- a/ocs2_doc/docs/overview.rst +++ b/ocs2_doc/docs/overview.rst @@ -60,7 +60,8 @@ Michael Spieler (ETHZ), Jan Carius (ETHZ), Jean-Pierre Sleiman (ETHZ). -**Other Developers**: +**Other Developers**: +Alexander Reske, Mayank Mittal, Johannes Pankert, Perry Franklin, From e40367c1ad29a9c31f3ea2d24152cadb344ad006 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Mar 2022 18:45:52 +0200 Subject: [PATCH 154/512] add mpcnet documentation --- ocs2_doc/docs/index.rst | 1 + ocs2_doc/docs/installation.rst | 18 +----- ocs2_doc/docs/mpcnet.rst | 105 +++++++++++++++++++++++++++++++++ 3 files changed, 108 insertions(+), 16 deletions(-) create mode 100644 ocs2_doc/docs/mpcnet.rst diff --git a/ocs2_doc/docs/index.rst b/ocs2_doc/docs/index.rst index 887be2291..9c98a75f5 100644 --- a/ocs2_doc/docs/index.rst +++ b/ocs2_doc/docs/index.rst @@ -11,6 +11,7 @@ Table of Contents robotic_examples.rst from_urdf_to_ocp.rst profiling.rst + mpcnet.rst .. rubric:: Reference and Index: diff --git a/ocs2_doc/docs/installation.rst b/ocs2_doc/docs/installation.rst index c8c00c05c..b22637cbc 100644 --- a/ocs2_doc/docs/installation.rst +++ b/ocs2_doc/docs/installation.rst @@ -89,7 +89,7 @@ Optional Dependencies * `Grid Map <https://github.com/ANYbotics/grid_map>`__ catkin package, which may be installed with ``sudo apt install ros-noetic-grid-map-msgs``. -* `ONNX Runtime <https://github.com/microsoft/onnxruntime>`__ is an inferencing and training accelerator. Here, it is used for deploying learned MPC-Net policies in C++ code. To locally install it, do the following: +* `ONNX Runtime <https://github.com/microsoft/onnxruntime>`__ is an inferencing and training accelerator. Here, it is used for deploying learned :ref:`MPC-Net <doxid-ocs2_doc_mpcnet>` policies in C++ code. To locally install it, do the following: .. code-block:: bash @@ -103,7 +103,7 @@ Optional Dependencies We provide custom cmake config and version files to enable ``find_package(onnxruntime)`` without modifying ``LIBRARY_PATH`` and ``LD_LIBRARY_PATH``. Note that the last command above assumes that you cloned OCS2 into the folder ``git`` in your user's home directory. -* `Virtual environments <https://docs.python.org/3/library/venv.html>`__ are recommended when training MPC-Net policies: +* `Virtual environments <https://docs.python.org/3/library/venv.html>`__ are recommended when training :ref:`MPC-Net <doxid-ocs2_doc_mpcnet>` policies: .. code-block:: bash @@ -130,20 +130,6 @@ Optional Dependencies pip3 install torch==1.10.2+cu113 -f https://download.pytorch.org/whl/cu113/torch_stable.html - Always activate the environment when running and monitoring the training, i.e. in one terminal: - - .. code-block:: bash - - source ~/venvs/mpcnet/bin/activate - pip3 <robot_name>_mpcnet.py - - And in another terminal: - - .. code-block:: bash - - source ~/venvs/mpcnet/bin/activate - tensorboard --logdir=runs - .. _doxid-ocs2_doc_installation_ocs2_doc_install: Installation diff --git a/ocs2_doc/docs/mpcnet.rst b/ocs2_doc/docs/mpcnet.rst new file mode 100644 index 000000000..f5def57fb --- /dev/null +++ b/ocs2_doc/docs/mpcnet.rst @@ -0,0 +1,105 @@ +.. index:: pair: page; MPC-Net + +.. _doxid-ocs2_doc_mpcnet: + +MPC-Net +======= + +MPC-Net is an imitation learning approach that uses solutions from MPC to guide the policy search. +The main idea is to imitate MPC by minimizing the control Hamiltonian while representing the corresponding control inputs by a parametrized policy. +MPC-Net can be used to clone a model predictive controller into a neural network policy, which can be evaluated much faster than MPC. +Therefore, MPC-Net is a useful proxy for MPC in computationally demanding applications that do not require the most exact solution. + +The multi-threaded data generation and policy evaluation run asynchronously with the policy training. +The data generation and policy evaluation are implemented in C++ and run on CPU, while the policy training is implemented in Python an runs on GPU. +The control Hamiltonian is represented by a linear quadratic approximation. +Therefore, the training can run on GPU without callbacks to OCS2 C++ code running on CPU to evaluate the Hamiltonian, and one can exploit batch processing on GPU. + +Robots +~~~~~~ + +MPC-Net has been implemented for the following :ref:`Robotic Examples <doxid-ocs2_doc_robotic_examples>`: + +============= ================ ============== ======== ============= +Robot Recom. CPU Cores Recom. GPU RAM RaiSim Training Time +============= ================ ============== ======== ============= +ballbot 4 2 GB No 1 min +legged_robot 12 8 GB Yes / No 8 min +============= ================ ============== ======== ============= + +Setup +~~~~~ + +Make sure to follow the :ref:`Installation <doxid-ocs2_doc_installation>` page. +Follow all the instructions for the dependencies. +Regarding the optional dependencies, make sure to follow the instruction for ONNX Runtime as well as the virtual environment, and optionally set up RaiSim. + +To build all MPC-Net packages, build the metapackage: + +.. code-block:: bash + + cd ~/catkin_ws + catkin_build ocs2_mpcnet + +To build a robot-specific package, replace :code:`<robot>` with the robot name: + +.. code-block:: bash + + cd ~/catkin_ws + catkin_build ocs2_<robot>_mpcnet + +Training +~~~~~~~~ + +To train an MPC-Net policy, run: + +.. code-block:: bash + + cd ~/git/ocs2/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet + source ~/catkin_ws/devel/setup.bash + source ~/venvs/mpcnet/bin/activate + python3 <robot>_mpcnet.py + +To monitor the training progress with Tensorboard, run: + +.. code-block:: bash + + cd ~/git/ocs2/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet + source ~/venvs/mpcnet/bin/activate + tensorboard --logdir=runs + +If you use RaiSim, you can visualize the data generation and policy evaluation runs with RaiSim Unity, where pre-built executables are provided in the :code:`~/git/raisimLib/raisimUnity` folder. For example, on Linux run: + +.. code-block:: bash + + ~/git/raisimLib/raisimUnity/linux/raisimUnity.x86_64 + +Deployment +~~~~~~~~~~ + +To deploy the default policy stored in the robot-specific package's :code:`policy` folder, run: + +.. code-block:: bash + + cd ~/catkin_ws + source devel/setup.bash + roslaunch ocs2_<robot>_mpcnet <robot>_mpcnet.launch + +To deploy a new policy stored in the robot-specific package's :code:`./python/ocs2_<robot>_mpcnet/policies` folder, replace :code:`<path>` with the absolute file path to the final policy and run: + +.. code-block:: bash + + cd ~/catkin_ws + source devel/setup.bash + roslaunch ocs2_<robot>_mpcnet <robot>_mpcnet.launch policyFile:=<path> + +References +~~~~~~~~~~ + +This part of the toolbox has been developed based on the following publications: + +.. bibliography:: + :list: enumerated + + carius2020mpcnet + reske2021imitation From 1701a196585bf85994530a16b7014389b861e462 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Mar 2022 19:27:51 +0200 Subject: [PATCH 155/512] add hot to set up a new robot section --- ocs2_doc/docs/mpcnet.rst | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/ocs2_doc/docs/mpcnet.rst b/ocs2_doc/docs/mpcnet.rst index f5def57fb..72e149f5a 100644 --- a/ocs2_doc/docs/mpcnet.rst +++ b/ocs2_doc/docs/mpcnet.rst @@ -93,6 +93,24 @@ To deploy a new policy stored in the robot-specific package's :code:`./python/oc source devel/setup.bash roslaunch ocs2_<robot>_mpcnet <robot>_mpcnet.launch policyFile:=<path> +How to Set Up a New Robot +~~~~~~~~~~~~~~~~~~~~~~~~~ + +Setting up MPC-Net for a new robot is relatively easy, as the **ocs2_mpcnet_core** package takes care of the data generation as well as policy evaluation rollouts and implements important learning components, such as the memory, policy and loss function. + +This section assumes that you already have the packages for the robot-specific MPC implementation: + +1. **ocs2_<robot>**: Provides the library with the robot-specific MPC implementation. +2. **ocs2_<robot>_ros**: Wraps around the MPC implementation with ROS to define ROS nodes. +3. **ocs2_<robot>_raisim**: (Optional) interface between the robot-specific MPC implementation and RaiSim. + +For the actual **ocs2_<robot>_mpcnet** package, follow the structure of existing robot-specific MPC-Net packages. +The most important classes/files that have to be implemented are: + +* **<Robot>MpcnetDefinition**: Defines how OCS2 state variables are transformed to the policy observations. and how policy actions are transformed to OCS2 control inputs. +* **<Robot>MpcnetInterface**: Provides the interface between C++ and Python allowing to exchange data and policies. +* **<robot>_mpcnet.py**: Implements the main training script. + References ~~~~~~~~~~ From 7c2b26ce10932bfe7ee4f41fd32ded8ad9c03f06 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Mar 2022 19:37:31 +0200 Subject: [PATCH 156/512] small fixes --- ocs2_doc/docs/mpcnet.rst | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/ocs2_doc/docs/mpcnet.rst b/ocs2_doc/docs/mpcnet.rst index 72e149f5a..dcea93762 100644 --- a/ocs2_doc/docs/mpcnet.rst +++ b/ocs2_doc/docs/mpcnet.rst @@ -11,8 +11,8 @@ MPC-Net can be used to clone a model predictive controller into a neural network Therefore, MPC-Net is a useful proxy for MPC in computationally demanding applications that do not require the most exact solution. The multi-threaded data generation and policy evaluation run asynchronously with the policy training. -The data generation and policy evaluation are implemented in C++ and run on CPU, while the policy training is implemented in Python an runs on GPU. -The control Hamiltonian is represented by a linear quadratic approximation. +The data generation and policy evaluation are implemented in C++ and run on CPU, while the policy training is implemented in Python and runs on GPU. +The control Hamiltonian is represented by a linear-quadratic approximation. Therefore, the training can run on GPU without callbacks to OCS2 C++ code running on CPU to evaluate the Hamiltonian, and one can exploit batch processing on GPU. Robots @@ -20,21 +20,21 @@ Robots MPC-Net has been implemented for the following :ref:`Robotic Examples <doxid-ocs2_doc_robotic_examples>`: -============= ================ ============== ======== ============= -Robot Recom. CPU Cores Recom. GPU RAM RaiSim Training Time -============= ================ ============== ======== ============= -ballbot 4 2 GB No 1 min -legged_robot 12 8 GB Yes / No 8 min -============= ================ ============== ======== ============= +============= ================ ================= ======== ============= +Robot Recom. CPU Cores Recom. GPU Memory RaiSim Training Time +============= ================ ================= ======== ============= +ballbot 4 2 GB No 1 min +legged_robot 12 8 GB Yes / No 8 min +============= ================ ================= ======== ============= Setup ~~~~~ Make sure to follow the :ref:`Installation <doxid-ocs2_doc_installation>` page. Follow all the instructions for the dependencies. -Regarding the optional dependencies, make sure to follow the instruction for ONNX Runtime as well as the virtual environment, and optionally set up RaiSim. +Regarding the optional dependencies, make sure to follow the instruction for ONNX Runtime and the virtual environment, optionally set up RaiSim. -To build all MPC-Net packages, build the metapackage: +To build all MPC-Net packages, build the meta package: .. code-block:: bash @@ -68,7 +68,7 @@ To monitor the training progress with Tensorboard, run: source ~/venvs/mpcnet/bin/activate tensorboard --logdir=runs -If you use RaiSim, you can visualize the data generation and policy evaluation runs with RaiSim Unity, where pre-built executables are provided in the :code:`~/git/raisimLib/raisimUnity` folder. For example, on Linux run: +If you use RaiSim, you can visualize the data generation and policy evaluation rollouts with RaiSim Unity, where pre-built executables are provided in the :code:`~/git/raisimLib/raisimUnity` folder. For example, on Linux run: .. code-block:: bash @@ -96,7 +96,7 @@ To deploy a new policy stored in the robot-specific package's :code:`./python/oc How to Set Up a New Robot ~~~~~~~~~~~~~~~~~~~~~~~~~ -Setting up MPC-Net for a new robot is relatively easy, as the **ocs2_mpcnet_core** package takes care of the data generation as well as policy evaluation rollouts and implements important learning components, such as the memory, policy and loss function. +Setting up MPC-Net for a new robot is relatively easy, as the **ocs2_mpcnet_core** package takes care of the data generation as well as policy evaluation rollouts and implements important learning components, such as the memory, policy, and loss function. This section assumes that you already have the packages for the robot-specific MPC implementation: @@ -108,7 +108,7 @@ For the actual **ocs2_<robot>_mpcnet** package, follow the structure of existing The most important classes/files that have to be implemented are: * **<Robot>MpcnetDefinition**: Defines how OCS2 state variables are transformed to the policy observations. and how policy actions are transformed to OCS2 control inputs. -* **<Robot>MpcnetInterface**: Provides the interface between C++ and Python allowing to exchange data and policies. +* **<Robot>MpcnetInterface**: Provides the interface between C++ and Python, allowing to exchange data and policies. * **<robot>_mpcnet.py**: Implements the main training script. References From 9645479cc272e23bb7bb08f4168a8da5579e0030 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Mar 2022 19:53:25 +0200 Subject: [PATCH 157/512] update training times --- ocs2_doc/docs/mpcnet.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_doc/docs/mpcnet.rst b/ocs2_doc/docs/mpcnet.rst index dcea93762..7cae89a4f 100644 --- a/ocs2_doc/docs/mpcnet.rst +++ b/ocs2_doc/docs/mpcnet.rst @@ -23,8 +23,8 @@ MPC-Net has been implemented for the following :ref:`Robotic Examples <doxid-ocs ============= ================ ================= ======== ============= Robot Recom. CPU Cores Recom. GPU Memory RaiSim Training Time ============= ================ ================= ======== ============= -ballbot 4 2 GB No 1 min -legged_robot 12 8 GB Yes / No 8 min +ballbot 4 2 GB No 0m 20s +legged_robot 12 8 GB Yes / No 7m 40s ============= ================ ================= ======== ============= Setup From 79f840da859e3062f1da12e1a88ce0e4e914911b Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 28 Mar 2022 20:01:28 +0200 Subject: [PATCH 158/512] reduce memory capacity to required amount --- .../python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index d84b7ad18..06b6ba15e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -99,7 +99,7 @@ gating_loss = GatingLoss(epsilon) # memory -memory_capacity = 500000 +memory_capacity = 400000 memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) # policy From e351e3fb80c37851f1a7a8d94e4064aaa0fa1867 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 30 Mar 2022 10:41:23 +0200 Subject: [PATCH 159/512] address some comments --- ocs2_doc/docs/installation.rst | 8 +-- .../include/ocs2_mpcnet/MpcnetPybindMacros.h | 20 +++--- .../include/ocs2_mpcnet/rollout/MpcnetData.h | 11 ++++ .../ocs2_mpcnet/rollout/MpcnetMetrics.h | 5 ++ ocs2_mpcnet/python/ocs2_mpcnet/config.py | 4 +- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 63 +++++++++++-------- ocs2_mpcnet/python/ocs2_mpcnet/memory.py | 28 ++++----- 7 files changed, 84 insertions(+), 55 deletions(-) diff --git a/ocs2_doc/docs/installation.rst b/ocs2_doc/docs/installation.rst index c8c00c05c..a383e796c 100644 --- a/ocs2_doc/docs/installation.rst +++ b/ocs2_doc/docs/installation.rst @@ -93,13 +93,13 @@ Optional Dependencies .. code-block:: bash - cd ~/Downloads + cd /tmp wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz tar xf onnxruntime-linux-x64-1.7.0.tgz mkdir -p ~/.local/bin ~/.local/include/onnxruntime ~/.local/lib ~/.local/share/cmake/onnxruntime - rsync -a ~/Downloads/onnxruntime-linux-x64-1.7.0/include/ ~/.local/include/onnxruntime - rsync -a ~/Downloads/onnxruntime-linux-x64-1.7.0/lib/ ~/.local/lib - rsync -a ~/git/ocs2/ocs2_mpcnet/misc/onnxruntime/cmake/ ~/.local/share/cmake/onnxruntime + rsync -a /tmp/onnxruntime-linux-x64-1.7.0/include/ ~/.local/include/onnxruntime + rsync -a /tmp/onnxruntime-linux-x64-1.7.0/lib/ ~/.local/lib + rsync -a ~/git/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ ~/.local/share/cmake/onnxruntime We provide custom cmake config and version files to enable ``find_package(onnxruntime)`` without modifying ``LIBRARY_PATH`` and ``LD_LIBRARY_PATH``. Note that the last command above assumes that you cloned OCS2 into the folder ``git`` in your user's home directory. diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h index 6bc80ec7a..b14c30ac0 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h @@ -85,14 +85,14 @@ using namespace pybind11::literals; /* bind mode schedule struct */ \ pybind11::class_<ocs2::ModeSchedule>(m, "ModeSchedule") \ .def(pybind11::init<ocs2::scalar_array_t, ocs2::size_array_t>()) \ - .def_readwrite("event_times", &ocs2::ModeSchedule::eventTimes) \ - .def_readwrite("mode_sequence", &ocs2::ModeSchedule::modeSequence); \ + .def_readwrite("eventTimes", &ocs2::ModeSchedule::eventTimes) \ + .def_readwrite("modeSequence", &ocs2::ModeSchedule::modeSequence); \ /* bind target trajectories class */ \ pybind11::class_<ocs2::TargetTrajectories>(m, "TargetTrajectories") \ .def(pybind11::init<ocs2::scalar_array_t, ocs2::vector_array_t, ocs2::vector_array_t>()) \ - .def_readwrite("time_trajectory", &ocs2::TargetTrajectories::timeTrajectory) \ - .def_readwrite("state_trajectory", &ocs2::TargetTrajectories::stateTrajectory) \ - .def_readwrite("input_trajectory", &ocs2::TargetTrajectories::inputTrajectory); \ + .def_readwrite("timeTrajectory", &ocs2::TargetTrajectories::timeTrajectory) \ + .def_readwrite("stateTrajectory", &ocs2::TargetTrajectories::stateTrajectory) \ + .def_readwrite("inputTrajectory", &ocs2::TargetTrajectories::inputTrajectory); \ /* bind data point struct */ \ pybind11::class_<ocs2::mpcnet::data_point_t>(m, "DataPoint") \ .def(pybind11::init<>()) \ @@ -100,15 +100,15 @@ using namespace pybind11::literals; .def_readwrite("t", &ocs2::mpcnet::data_point_t::t) \ .def_readwrite("x", &ocs2::mpcnet::data_point_t::x) \ .def_readwrite("u", &ocs2::mpcnet::data_point_t::u) \ - .def_readwrite("generalized_time", &ocs2::mpcnet::data_point_t::generalizedTime) \ - .def_readwrite("relative_state", &ocs2::mpcnet::data_point_t::relativeState) \ - .def_readwrite("input_transformation", &ocs2::mpcnet::data_point_t::inputTransformation) \ + .def_readwrite("generalizedTime", &ocs2::mpcnet::data_point_t::generalizedTime) \ + .def_readwrite("relativeState", &ocs2::mpcnet::data_point_t::relativeState) \ + .def_readwrite("inputTransformation", &ocs2::mpcnet::data_point_t::inputTransformation) \ .def_readwrite("hamiltonian", &ocs2::mpcnet::data_point_t::hamiltonian); \ /* bind metrics struct */ \ pybind11::class_<ocs2::mpcnet::metrics_t>(m, "Metrics") \ .def(pybind11::init<>()) \ - .def_readwrite("survival_time", &ocs2::mpcnet::metrics_t::survivalTime) \ - .def_readwrite("incurred_hamiltonian", &ocs2::mpcnet::metrics_t::incurredHamiltonian); \ + .def_readwrite("survivalTime", &ocs2::mpcnet::metrics_t::survivalTime) \ + .def_readwrite("incurredHamiltonian", &ocs2::mpcnet::metrics_t::incurredHamiltonian); \ } /** diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h index b8c5b27e2..75bb2339a 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h @@ -37,14 +37,25 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace mpcnet { +/** + * Data point collected during the data generation rollout. + */ struct DataPoint { + /** Mode of the system. */ size_t mode; + /** Absolute time. */ scalar_t t; + /** Observed state. */ vector_t x; + /** Optimal control input. */ vector_t u; + /** Generalized time as defined by the robot-specific MPC-Net definitions. */ vector_t generalizedTime; + /** Relative state as defined by the robot-specific MPC-Net definitions. */ vector_t relativeState; + /** Input transformation as defined by the robot-specific MPC-Net definitions. */ matrix_t inputTransformation; + /** Linear-quadratic approximation of the Hamiltonian, using x and u as development/expansion points. */ ScalarFunctionQuadraticApproximation hamiltonian; }; using data_point_t = DataPoint; diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h index b9ecd71b0..120da00c7 100644 --- a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h +++ b/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h @@ -34,8 +34,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace mpcnet { +/** + * Metrics computed during the policy evaluation rollout. + */ struct Metrics { + /** Survival time. */ scalar_t survivalTime = 0.0; + /** Hamiltonian incurred over time. */ scalar_t incurredHamiltonian = 0.0; }; using metrics_t = Metrics; diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/config.py b/ocs2_mpcnet/python/ocs2_mpcnet/config.py index 1b5b5805b..e97bf65da 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/config.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/config.py @@ -35,7 +35,7 @@ import torch # data type for tensor elements -dtype = torch.float +DTYPE = torch.float # device on which tensors will be allocated -device = torch.device("cpu") +DEVICE = torch.device("cpu") diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index cc0db32b4..52f4cc49b 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -39,24 +39,29 @@ from ocs2_mpcnet.helper import bdot, bmv -class Hamiltonian: - """Hamiltonian loss. +class HamiltonianLoss: + r"""Hamiltonian loss. Uses the linear quadratic approximation of the Hamiltonian as loss: - H(x,u) = 1/2 dx' dHdxx dx + du' dHdux dx + 1/2 du' dHduu du + dHdx' dx + dHdu' du + H, + + .. math:: + + H(x,u) = \frac{1}{2} \delta x^T \partial H_{xx} \delta x +\delta u^T \partial H_{ux} \delta x + \frac{1}{2} + \delta u^T \partial H_{uu} \delta u + \partial H_{x}^T \delta x + \partial H_{u}^T \delta u + H, + where the state x is of dimension X and the input u is of dimension U. """ @staticmethod def compute_sample(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): - """Computes the Hamiltonian for one sample. + """Computes the Hamiltonian loss for one sample. - Computes the Hamiltonian for one sample using the provided linear quadratic approximation. + Computes the Hamiltonian loss for one sample using the provided linear quadratic approximation. Args: - x_inquiry: A (X) tensor with the state the Hamiltonian should be computed for. + x_inquiry: A (X) tensor with the state the Hamiltonian loss should be computed for. x_nominal: A (X) tensor with the state that was used as development/expansion point. - u_inquiry: A (U) tensor with the input the Hamiltonian should be computed for. + u_inquiry: A (U) tensor with the input the Hamiltonian loss should be computed for. u_nominal: A (U) tensor with the input that was used as development/expansion point. dHdxx: A (X,X) tensor with the state-state Hessian of the approximation. dHdux: A (U,X) tensor with the input-state Hessian of the approximation. @@ -66,7 +71,7 @@ def compute_sample(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHd H: A (1) tensor with the Hamiltonian at the development/expansion point. Returns: - A (1) tensor containing the computed Hamiltonian. + A (1) tensor containing the computed Hamiltonian loss. """ if torch.equal(x_inquiry, x_nominal): du = torch.sub(u_inquiry, u_nominal) @@ -88,14 +93,14 @@ def compute_sample(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHd @staticmethod def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): - """Computes the Hamiltonians for a batch. + """Computes the Hamiltonian loss for a batch. - Computes the Hamiltonians for a batch of size B using the provided linear quadratic approximations. + Computes the Hamiltonian loss for a batch of size B using the provided linear quadratic approximations. Args: - x_inquiry: A (B,X) tensor with the states the Hamiltonians should be computed for. + x_inquiry: A (B,X) tensor with the states the Hamiltonian loss should be computed for. x_nominal: A (B,X) tensor with the states that were used as development/expansion points. - u_inquiry: A (B,U) tensor with the inputs the Hamiltonians should be computed for. + u_inquiry: A (B,U) tensor with the inputs the Hamiltonian loss should be computed for. u_nominal: A (B,U) tensor with the inputs that were used as development/expansion point. dHdxx: A (B,X,X) tensor with the state-state Hessians of the approximations. dHdux: A (B,U,X) tensor with the input-state Hessians of the approximations. @@ -105,7 +110,7 @@ def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHdu H: A (B) tensor with the Hamiltonians at the development/expansion points. Returns: - A (B) tensor containing the computed Hamiltonians. + A (B) tensor containing the computed Hamiltonian losses. """ if torch.equal(x_inquiry, x_nominal): du = torch.sub(u_inquiry, u_nominal) @@ -126,11 +131,15 @@ def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHdu ) -class BehavioralCloning: - """Behavioral cloning loss. +class BehavioralCloningLoss: + r"""Behavioral cloning loss. Uses a simple quadratic function as loss: - BC(u) = du' R du, + + .. math:: + + BC(u) = \delta u^T R \, \delta u, + where the input u is of dimension U. Attributes: @@ -139,14 +148,14 @@ class BehavioralCloning: """ def __init__(self, R): - """Initializes the BehavioralCloning class. + """Initializes the BehavioralCloningLoss class. - Initializes the BehavioralCloning class by setting fixed attributes. + Initializes the BehavioralCloningLoss class by setting fixed attributes. Args: R: A NumPy array of shape (U, U) with the input cost matrix. """ - self.R_sample = torch.tensor(R, device=config.device, dtype=config.dtype) + self.R_sample = torch.tensor(R, device=config.DEVICE, dtype=config.DTYPE) self.R_batch = self.R_sample.unsqueeze(dim=0) def compute_sample(self, u_predicted, u_target): @@ -180,11 +189,15 @@ def compute_batch(self, u_predicted, u_target): return bdot(du, bmv(self.R_batch, du)) -class CrossEntropy: - """Cross entropy loss. +class CrossEntropyLoss: + r"""Cross entropy loss. Uses the cross entropy between two discrete probability distributions as loss: - CE(p_target, p_predicted) = - sum(p_target * log(p_predicted)), + + .. math:: + + CE(p_{target}, p_{predicted}) = - \sum_{i=1}^{P} (p_{target,i} \log(p_{predicted,i} + \varepsilon)), + where the sample space is the set of P individually identified items. Attributes: @@ -192,14 +205,14 @@ class CrossEntropy: """ def __init__(self, epsilon): - """Initializes the CrossEntropy class. + """Initializes the CrossEntropyLoss class. - Initializes the CrossEntropy class by setting fixed attributes. + Initializes the CrossEntropyLoss class by setting fixed attributes. Args: epsilon: A float used to stabilize the logarithm. """ - self.epsilon = torch.tensor(epsilon, device=config.device, dtype=config.dtype) + self.epsilon = torch.tensor(epsilon, device=config.DEVICE, dtype=config.DTYPE) def compute_sample(self, p_target, p_predicted): """Computes the cross entropy loss for one sample. diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py index d7208da53..03760feea 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py @@ -80,21 +80,21 @@ def __init__(self, capacity, time_dimension, state_dimension, input_dimension, e self.size = 0 self.position = 0 # pre-allocate memory - self.t = torch.zeros(capacity, device=config.device, dtype=config.dtype) - self.x = torch.zeros(capacity, state_dimension, device=config.device, dtype=config.dtype) - self.u = torch.zeros(capacity, input_dimension, device=config.device, dtype=config.dtype) - self.p = torch.zeros(capacity, expert_number, device=config.device, dtype=config.dtype) - self.generalized_time = torch.zeros(capacity, time_dimension, device=config.device, dtype=config.dtype) - self.relative_state = torch.zeros(capacity, state_dimension, device=config.device, dtype=config.dtype) + self.t = torch.zeros(capacity, device=config.DEVICE, dtype=config.DTYPE) + self.x = torch.zeros(capacity, state_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.u = torch.zeros(capacity, input_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.p = torch.zeros(capacity, expert_number, device=config.DEVICE, dtype=config.DTYPE) + self.generalized_time = torch.zeros(capacity, time_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.relative_state = torch.zeros(capacity, state_dimension, device=config.DEVICE, dtype=config.DTYPE) self.input_transformation = torch.zeros( - capacity, input_dimension, input_dimension, device=config.device, dtype=config.dtype + capacity, input_dimension, input_dimension, device=config.DEVICE, dtype=config.DTYPE ) - self.dHdxx = torch.zeros(capacity, state_dimension, state_dimension, device=config.device, dtype=config.dtype) - self.dHdux = torch.zeros(capacity, input_dimension, state_dimension, device=config.device, dtype=config.dtype) - self.dHduu = torch.zeros(capacity, input_dimension, input_dimension, device=config.device, dtype=config.dtype) - self.dHdx = torch.zeros(capacity, state_dimension, device=config.device, dtype=config.dtype) - self.dHdu = torch.zeros(capacity, input_dimension, device=config.device, dtype=config.dtype) - self.H = torch.zeros(capacity, device=config.device, dtype=config.dtype) + self.dHdxx = torch.zeros(capacity, state_dimension, state_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.dHdux = torch.zeros(capacity, input_dimension, state_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.dHduu = torch.zeros(capacity, input_dimension, input_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.dHdx = torch.zeros(capacity, state_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.dHdu = torch.zeros(capacity, input_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.H = torch.zeros(capacity, device=config.DEVICE, dtype=config.DTYPE) def push(self, t, x, u, p, generalized_time, relative_state, input_transformation, hamiltonian): """Pushes data into the circular memory. @@ -161,7 +161,7 @@ def sample(self, batch_size): - dHdu_batch: A (B,U) tensor with the input gradients of the Hamiltonian approximations. - H_batch: A (B) tensor with the Hamiltonians at the development/expansion points. """ - indices = torch.randint(0, self.size, (batch_size,), device=config.device) + indices = torch.randint(0, self.size, (batch_size,), device=config.DEVICE) t_batch = self.t[indices] x_batch = self.x[indices] u_batch = self.u[indices] From da73efdf80799a45899ecd1d2a1809d5dfe0cc56 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 30 Mar 2022 11:26:42 +0200 Subject: [PATCH 160/512] remove code duplication and add call method for losses --- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 118 ++++++++++--------------- 1 file changed, 49 insertions(+), 69 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index 52f4cc49b..ad06b93dc 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -52,55 +52,38 @@ class HamiltonianLoss: where the state x is of dimension X and the input u is of dimension U. """ - @staticmethod - def compute_sample(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): - """Computes the Hamiltonian loss for one sample. + def __call__(self, x_query, x_nominal, u_query, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): + """Computes the mean Hamiltonian loss. - Computes the Hamiltonian loss for one sample using the provided linear quadratic approximation. + Computes the mean Hamiltonian loss for a batch of size B using the provided linear quadratic approximations. Args: - x_inquiry: A (X) tensor with the state the Hamiltonian loss should be computed for. - x_nominal: A (X) tensor with the state that was used as development/expansion point. - u_inquiry: A (U) tensor with the input the Hamiltonian loss should be computed for. - u_nominal: A (U) tensor with the input that was used as development/expansion point. - dHdxx: A (X,X) tensor with the state-state Hessian of the approximation. - dHdux: A (U,X) tensor with the input-state Hessian of the approximation. - dHduu: A (U,U) tensor with the input-input Hessian of the approximation. - dHdx: A (X) tensor with the state gradient of the approximation. - dHdu: A (U) tensor with the input gradient of the approximation. - H: A (1) tensor with the Hamiltonian at the development/expansion point. + x_query: A (B,X) tensor with the states the Hamiltonian loss should be computed for. + x_nominal: A (B,X) tensor with the states that were used as development/expansion points. + u_query: A (B,U) tensor with the inputs the Hamiltonian loss should be computed for. + u_nominal: A (B,U) tensor with the inputs that were used as development/expansion point. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the approximations. + dHdx: A (B,X) tensor with the state gradients of the approximations. + dHdu: A (B,U) tensor with the input gradients of the approximations. + H: A (B) tensor with the Hamiltonians at the development/expansion points. Returns: - A (1) tensor containing the computed Hamiltonian loss. + A (1) tensor containing the mean Hamiltonian loss. """ - if torch.equal(x_inquiry, x_nominal): - du = torch.sub(u_inquiry, u_nominal) - return 0.5 * torch.dot(du, torch.mv(dHduu, du)) + torch.dot(dHdu, du) + H - elif torch.equal(u_inquiry, u_nominal): - dx = torch.sub(x_inquiry, x_nominal) - return 0.5 * torch.dot(dx, torch.mv(dHdxx, dx)) + torch.dot(dHdx, dx) + H - else: - dx = torch.sub(x_inquiry, x_nominal) - du = torch.sub(u_inquiry, u_nominal) - return ( - 0.5 * torch.dot(dx, torch.mv(dHdxx, dx)) - + torch.dot(du, torch.mv(dHdux, dx)) - + 0.5 * torch.dot(du, torch.mv(dHduu, du)) - + torch.dot(dHdx, dx) - + torch.dot(dHdu, du) - + H - ) + return torch.mean(self.compute(x_query, x_nominal, u_query, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H)) @staticmethod - def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): - """Computes the Hamiltonian loss for a batch. + def compute(x_query, x_nominal, u_query, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): + """Computes the Hamiltonian losses for a batch. - Computes the Hamiltonian loss for a batch of size B using the provided linear quadratic approximations. + Computes the Hamiltonian losses for a batch of size B using the provided linear quadratic approximations. Args: - x_inquiry: A (B,X) tensor with the states the Hamiltonian loss should be computed for. + x_query: A (B,X) tensor with the states the Hamiltonian loss should be computed for. x_nominal: A (B,X) tensor with the states that were used as development/expansion points. - u_inquiry: A (B,U) tensor with the inputs the Hamiltonian loss should be computed for. + u_query: A (B,U) tensor with the inputs the Hamiltonian loss should be computed for. u_nominal: A (B,U) tensor with the inputs that were used as development/expansion point. dHdxx: A (B,X,X) tensor with the state-state Hessians of the approximations. dHdux: A (B,U,X) tensor with the input-state Hessians of the approximations. @@ -112,15 +95,15 @@ def compute_batch(x_inquiry, x_nominal, u_inquiry, u_nominal, dHdxx, dHdux, dHdu Returns: A (B) tensor containing the computed Hamiltonian losses. """ - if torch.equal(x_inquiry, x_nominal): - du = torch.sub(u_inquiry, u_nominal) + if torch.equal(x_query, x_nominal): + du = torch.sub(u_query, u_nominal) return 0.5 * bdot(du, bmv(dHduu, du)) + bdot(dHdu, du) + H - elif torch.equal(u_inquiry, u_nominal): - dx = torch.sub(x_inquiry, x_nominal) + elif torch.equal(u_query, u_nominal): + dx = torch.sub(x_query, x_nominal) return 0.5 * bdot(dx, bmv(dHdxx, dx)) + bdot(dHdx, dx) + H else: - dx = torch.sub(x_inquiry, x_nominal) - du = torch.sub(u_inquiry, u_nominal) + dx = torch.sub(x_query, x_nominal) + du = torch.sub(u_query, u_nominal) return ( 0.5 * bdot(dx, bmv(dHdxx, dx)) + bdot(du, bmv(dHdux, dx)) @@ -143,8 +126,7 @@ class BehavioralCloningLoss: where the input u is of dimension U. Attributes: - R_sample: A (U,U) tensor with the input cost matrix R for one sample. - R_batch: A (1,U,U) tensor with the input cost matrix for a batch. + R: A (1,U,U) tensor with the input cost matrix. """ def __init__(self, R): @@ -155,28 +137,26 @@ def __init__(self, R): Args: R: A NumPy array of shape (U, U) with the input cost matrix. """ - self.R_sample = torch.tensor(R, device=config.DEVICE, dtype=config.DTYPE) - self.R_batch = self.R_sample.unsqueeze(dim=0) + self.R = torch.tensor(R, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) - def compute_sample(self, u_predicted, u_target): - """Computes the behavioral cloning loss for one sample. + def __call__(self, u_predicted, u_target): + """Computes the mean behavioral cloning loss. - Computes the behavioral cloning loss for one sample using the cost matrix. + Computes the mean behavioral cloning loss for a batch of size B using the cost matrix. Args: - u_predicted: A (U) tensor with the predicted input. - u_target: A (U) tensor with the target input. + u_predicted: A (B, U) tensor with the predicted inputs. + u_target: A (B, U) tensor with the target inputs. Returns: - A (1) tensor containing the behavioral cloning loss. + A (1) tensor containing the mean behavioral cloning loss. """ - du = torch.sub(u_predicted, u_target) - return torch.dot(du, torch.mv(self.R_sample, du)) + return torch.mean(self.compute(u_predicted, u_target)) - def compute_batch(self, u_predicted, u_target): - """Computes the behavioral cloning loss for a batch. + def compute(self, u_predicted, u_target): + """Computes the behavioral cloning losses for a batch. - Computes the behavioral cloning loss for a batch of size B using the cost matrix. + Computes the behavioral cloning losses for a batch of size B using the cost matrix. Args: u_predicted: A (B, U) tensor with the predicted inputs. @@ -186,7 +166,7 @@ def compute_batch(self, u_predicted, u_target): A (B) tensor containing the behavioral cloning losses. """ du = torch.sub(u_predicted, u_target) - return bdot(du, bmv(self.R_batch, du)) + return bdot(du, bmv(self.R, du)) class CrossEntropyLoss: @@ -214,24 +194,24 @@ def __init__(self, epsilon): """ self.epsilon = torch.tensor(epsilon, device=config.DEVICE, dtype=config.DTYPE) - def compute_sample(self, p_target, p_predicted): - """Computes the cross entropy loss for one sample. + def __call__(self, p_target, p_predicted): + """Computes the mean cross entropy loss. - Computes the cross entropy loss for one sample, where the logarithm is stabilized by a small epsilon. + Computes the mean cross entropy loss for a batch, where the logarithm is stabilized by a small epsilon. Args: - p_target: A (P) tensor with the target discrete probability distribution. - p_predicted: A (P) tensor with the predicted discrete probability distribution. + p_target: A (B,P) tensor with the target discrete probability distributions. + p_predicted: A (B,P) tensor with the predicted discrete probability distributions. Returns: - A (1) tensor containing the cross entropy loss. + A (1) tensor containing the mean cross entropy loss. """ - return -torch.dot(p_target, torch.log(p_predicted + self.epsilon)) + return torch.mean(self.compute(p_target, p_predicted)) - def compute_batch(self, p_target, p_predicted): - """Computes the cross entropy loss for a batch. + def compute(self, p_target, p_predicted): + """Computes the cross entropy losses for a batch. - Computes the cross entropy loss for a batch, where the logarithm is stabilized by a small epsilon. + Computes the cross entropy losses for a batch, where the logarithm is stabilized by a small epsilon. Args: p_target: A (B,P) tensor with the target discrete probability distributions. From 3ae467a06d89d6c006adf1d0016a323e577b6c2e Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 30 Mar 2022 14:22:52 +0200 Subject: [PATCH 161/512] add type hints --- ocs2_mpcnet/python/ocs2_mpcnet/helper.py | 31 ++++++++++-------- ocs2_mpcnet/python/ocs2_mpcnet/loss.py | 40 +++++++++++++++++++----- ocs2_mpcnet/python/ocs2_mpcnet/memory.py | 23 +++++++++++--- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 25 ++++++++------- 4 files changed, 82 insertions(+), 37 deletions(-) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py index f41a0cc80..d135833a6 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/helper.py @@ -34,6 +34,7 @@ import torch import numpy as np +from typing import Tuple from ocs2_mpcnet import ( size_array, @@ -48,7 +49,7 @@ ) -def bdot(bv1, bv2): +def bdot(bv1: torch.Tensor, bv2: torch.Tensor) -> torch.Tensor: """Batch-wise dot product. Performs a batch-wise dot product between two batches of vectors with batch size B and dimension N. Supports @@ -64,7 +65,7 @@ def bdot(bv1, bv2): return torch.sum(torch.mul(bv1, bv2), dim=1) -def bmv(bm, bv): +def bmv(bm: torch.Tensor, bv: torch.Tensor) -> torch.Tensor: """Batch-wise matrix-vector product. Performs a batch-wise matrix-vector product between a batch of MxN matrices and a batch of vectors of dimension N, @@ -80,7 +81,7 @@ def bmv(bm, bv): return torch.matmul(bm, bv.unsqueeze(dim=2)).squeeze(dim=2) -def bmm(bm1, bm2): +def bmm(bm1: torch.Tensor, bm2: torch.Tensor) -> torch.Tensor: """Batch-wise matrix-matrix product. Performs a batch-wise matrix-matrix product between a batch of MxK matrices and a batch of KxN matrices, each with @@ -96,7 +97,7 @@ def bmm(bm1, bm2): return torch.matmul(bm1, bm2) -def get_size_array(data): +def get_size_array(data: np.ndarray) -> size_array: """Get an OCS2 size array. Creates an OCS2 size array and fills it with integer data from a NumPy array. @@ -114,7 +115,7 @@ def get_size_array(data): return my_size_array -def get_scalar_array(data): +def get_scalar_array(data: np.ndarray) -> scalar_array: """Get an OCS2 scalar array. Creates an OCS2 scalar array and fills it with float data from a NumPy array. @@ -132,7 +133,7 @@ def get_scalar_array(data): return my_scalar_array -def get_vector_array(data): +def get_vector_array(data: np.ndarray) -> vector_array: """Get an OCS2 vector array. Creates an OCS2 vector array and fills it with float data from a NumPy array. @@ -150,7 +151,7 @@ def get_vector_array(data): return my_vector_array -def get_system_observation(mode, time, state, input): +def get_system_observation(mode: int, time: float, state: np.ndarray, input: np.ndarray) -> SystemObservation: """Get an OCS2 system observation object. Creates an OCS2 system observation object and fills it with data. @@ -172,7 +173,7 @@ def get_system_observation(mode, time, state, input): return system_observation -def get_system_observation_array(length): +def get_system_observation_array(length: int) -> SystemObservationArray: """Get an OCS2 system observation array. Creates an OCS2 system observation array but does not fill it with data. @@ -188,7 +189,9 @@ def get_system_observation_array(length): return system_observation_array -def get_target_trajectories(time_trajectory, state_trajectory, input_trajectory): +def get_target_trajectories( + time_trajectory: np.ndarray, state_trajectory: np.ndarray, input_trajectory: np.ndarray +) -> TargetTrajectories: """Get an OCS2 target trajectories object. Creates an OCS2 target trajectories object and fills it with data. @@ -207,7 +210,7 @@ def get_target_trajectories(time_trajectory, state_trajectory, input_trajectory) return TargetTrajectories(time_trajectory_array, state_trajectory_array, input_trajectory_array) -def get_target_trajectories_array(length): +def get_target_trajectories_array(length: int) -> TargetTrajectoriesArray: """Get an OCS2 target trajectories array. Creates an OCS2 target trajectories array but does not fill it with data. @@ -223,7 +226,7 @@ def get_target_trajectories_array(length): return target_trajectories_array -def get_mode_schedule(event_times, mode_sequence): +def get_mode_schedule(event_times: np.ndarray, mode_sequence: np.ndarray) -> ModeSchedule: """Get an OCS2 mode schedule object. Creates an OCS2 mode schedule object and fills it with data. @@ -240,7 +243,7 @@ def get_mode_schedule(event_times, mode_sequence): return ModeSchedule(event_times_array, mode_sequence_array) -def get_mode_schedule_array(length): +def get_mode_schedule_array(length: int) -> ModeScheduleArray: """Get an OCS2 mode schedule array. Creates an OCS2 mode schedule array but does not fill it with data. @@ -256,7 +259,9 @@ def get_mode_schedule_array(length): return mode_schedule_array -def get_event_times_and_mode_sequence(default_mode, duration, event_times_template, mode_sequence_template): +def get_event_times_and_mode_sequence( + default_mode: int, duration: float, event_times_template: np.ndarray, mode_sequence_template: np.ndarray +) -> Tuple[np.ndarray, np.ndarray]: """Get the event times and mode sequence describing a mode schedule. Creates the event times and mode sequence for a certain time duration from a template (e.g. a gait). diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py index ad06b93dc..e8831ead4 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss.py @@ -34,6 +34,7 @@ """ import torch +import numpy as np from ocs2_mpcnet import config from ocs2_mpcnet.helper import bdot, bmv @@ -52,7 +53,19 @@ class HamiltonianLoss: where the state x is of dimension X and the input u is of dimension U. """ - def __call__(self, x_query, x_nominal, u_query, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): + def __call__( + self, + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: """Computes the mean Hamiltonian loss. Computes the mean Hamiltonian loss for a batch of size B using the provided linear quadratic approximations. @@ -75,7 +88,18 @@ def __call__(self, x_query, x_nominal, u_query, u_nominal, dHdxx, dHdux, dHduu, return torch.mean(self.compute(x_query, x_nominal, u_query, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H)) @staticmethod - def compute(x_query, x_nominal, u_query, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H): + def compute( + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: """Computes the Hamiltonian losses for a batch. Computes the Hamiltonian losses for a batch of size B using the provided linear quadratic approximations. @@ -129,7 +153,7 @@ class BehavioralCloningLoss: R: A (1,U,U) tensor with the input cost matrix. """ - def __init__(self, R): + def __init__(self, R: np.ndarray) -> None: """Initializes the BehavioralCloningLoss class. Initializes the BehavioralCloningLoss class by setting fixed attributes. @@ -139,7 +163,7 @@ def __init__(self, R): """ self.R = torch.tensor(R, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) - def __call__(self, u_predicted, u_target): + def __call__(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: """Computes the mean behavioral cloning loss. Computes the mean behavioral cloning loss for a batch of size B using the cost matrix. @@ -153,7 +177,7 @@ def __call__(self, u_predicted, u_target): """ return torch.mean(self.compute(u_predicted, u_target)) - def compute(self, u_predicted, u_target): + def compute(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: """Computes the behavioral cloning losses for a batch. Computes the behavioral cloning losses for a batch of size B using the cost matrix. @@ -184,7 +208,7 @@ class CrossEntropyLoss: epsilon: A (1) tensor with a small epsilon used to stabilize the logarithm. """ - def __init__(self, epsilon): + def __init__(self, epsilon: float) -> None: """Initializes the CrossEntropyLoss class. Initializes the CrossEntropyLoss class by setting fixed attributes. @@ -194,7 +218,7 @@ def __init__(self, epsilon): """ self.epsilon = torch.tensor(epsilon, device=config.DEVICE, dtype=config.DTYPE) - def __call__(self, p_target, p_predicted): + def __call__(self, p_target: torch.Tensor, p_predicted: torch.Tensor) -> torch.Tensor: """Computes the mean cross entropy loss. Computes the mean cross entropy loss for a batch, where the logarithm is stabilized by a small epsilon. @@ -208,7 +232,7 @@ def __call__(self, p_target, p_predicted): """ return torch.mean(self.compute(p_target, p_predicted)) - def compute(self, p_target, p_predicted): + def compute(self, p_target: torch.Tensor, p_predicted: torch.Tensor) -> torch.Tensor: """Computes the cross entropy losses for a batch. Computes the cross entropy losses for a batch, where the logarithm is stabilized by a small epsilon. diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py index 03760feea..25d58d926 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory.py @@ -33,8 +33,11 @@ """ import torch +import numpy as np +from typing import Tuple from ocs2_mpcnet import config +from ocs2_mpcnet import ScalarFunctionQuadraticApproximation class CircularMemory: @@ -61,7 +64,9 @@ class CircularMemory: H: A (C) tensor for the Hamiltonians at the development/expansion points. """ - def __init__(self, capacity, time_dimension, state_dimension, input_dimension, expert_number=1): + def __init__( + self, capacity: int, time_dimension: int, state_dimension: int, input_dimension: int, expert_number: int = 1 + ) -> None: """Initializes the CircularMemory class. Initializes the BehavioralCloning class by setting fixed attributes, initializing variable attributes and @@ -96,7 +101,17 @@ def __init__(self, capacity, time_dimension, state_dimension, input_dimension, e self.dHdu = torch.zeros(capacity, input_dimension, device=config.DEVICE, dtype=config.DTYPE) self.H = torch.zeros(capacity, device=config.DEVICE, dtype=config.DTYPE) - def push(self, t, x, u, p, generalized_time, relative_state, input_transformation, hamiltonian): + def push( + self, + t: float, + x: np.ndarray, + u: np.ndarray, + p: np.ndarray, + generalized_time: np.ndarray, + relative_state: np.ndarray, + input_transformation: np.ndarray, + hamiltonian: ScalarFunctionQuadraticApproximation, + ) -> None: """Pushes data into the circular memory. Pushes one data sample into the circular memory. @@ -137,7 +152,7 @@ def push(self, t, x, u, p, generalized_time, relative_state, input_transformatio self.size = min(self.size + 1, self.capacity) self.position = (self.position + 1) % self.capacity - def sample(self, batch_size): + def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: """Samples data from the circular memory. Samples a batch of data from the circular memory. @@ -191,7 +206,7 @@ def sample(self, batch_size): H_batch, ) - def __len__(self): + def __len__(self) -> int: """The length of the memory. Return the length of the memory given by the current size. diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py index 2490c82ce..ba12bdecc 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py @@ -33,6 +33,7 @@ """ import torch +from typing import Tuple from ocs2_mpcnet.helper import bmv @@ -49,7 +50,7 @@ class LinearPolicy(torch.nn.Module): linear: The linear neural network layer. """ - def __init__(self, dim_t, dim_x, dim_u): + def __init__(self, dim_t: int, dim_x: int, dim_u: int) -> None: """Initializes the LinearPolicy class. Initializes the LinearPolicy class by setting fixed and variable attributes. @@ -65,7 +66,7 @@ def __init__(self, dim_t, dim_x, dim_u): self.dim_out = dim_u self.linear = torch.nn.Linear(self.dim_in, self.dim_out) - def forward(self, t, x): + def forward(self, t: torch.Tensor, x: torch.Tensor) -> torch.Tensor: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. @@ -95,7 +96,7 @@ class NonlinearPolicy(torch.nn.Module): linear2: The second linear neural network layer. """ - def __init__(self, dim_t, dim_x, dim_u): + def __init__(self, dim_t: int, dim_x: int, dim_u: int) -> None: """Initializes the NonlinearPolicy class. Initializes the NonlinearPolicy class by setting fixed and variable attributes. @@ -114,7 +115,7 @@ def __init__(self, dim_t, dim_x, dim_u): self.activation = torch.nn.Tanh() self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) - def forward(self, t, x): + def forward(self, t: torch.Tensor, x: torch.Tensor) -> torch.Tensor: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. @@ -143,7 +144,7 @@ class MixtureOfLinearExpertsPolicy(torch.nn.Module): expert_nets: The expert networks. """ - def __init__(self, dim_t, dim_x, dim_u, num_experts): + def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None: """Initializes the MixtureOfLinearExpertsPolicy class. Initializes the MixtureOfLinearExpertsPolicy class by setting fixed and variable attributes. @@ -166,7 +167,7 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): [LinearExpert(i, self.dim_in, self.dim_out) for i in range(self.num_experts)] ) - def forward(self, t, x): + def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. @@ -202,7 +203,7 @@ class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): expert_nets: The expert networks. """ - def __init__(self, dim_t, dim_x, dim_u, num_experts): + def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None: """Initializes the MixtureOfNonlinearExpertsPolicy class. Initializes the MixtureOfNonlinearExpertsPolicy class by setting fixed and variable attributes. @@ -233,7 +234,7 @@ def __init__(self, dim_t, dim_x, dim_u, num_experts): [NonlinearExpert(i, self.dim_in, self.dim_hidden_expert, self.dim_out) for i in range(self.num_experts)] ) - def forward(self, t, x): + def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. @@ -264,7 +265,7 @@ class LinearExpert(torch.nn.Module): linear: The linear neural network layer. """ - def __init__(self, index, dim_in, dim_out): + def __init__(self, index: int, dim_in: int, dim_out: int) -> None: """Initializes the LinearExpert class. Initializes the LinearExpert class by setting fixed and variable attributes. @@ -280,7 +281,7 @@ def __init__(self, index, dim_in, dim_out): self.dim_out = dim_out self.linear = torch.nn.Linear(self.dim_in, self.dim_out) - def forward(self, input): + def forward(self, input: torch.Tensor) -> torch.Tensor: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. @@ -309,7 +310,7 @@ class NonlinearExpert(torch.nn.Module): linear2: The second linear neural network layer. """ - def __init__(self, index, dim_in, dim_hidden, dim_out): + def __init__(self, index: int, dim_in: int, dim_hidden: int, dim_out: int) -> None: """Initializes the NonlinearExpert class. Initializes the NonlinearExpert class by setting fixed and variable attributes. @@ -329,7 +330,7 @@ def __init__(self, index, dim_in, dim_hidden, dim_out): self.activation = torch.nn.Tanh() self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) - def forward(self, input): + def forward(self, input: torch.Tensor) -> torch.Tensor: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. From 9e4c39ade2d8171fb50cc7b5dbe70e7432e23db4 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 30 Mar 2022 14:58:18 +0200 Subject: [PATCH 162/512] adapt to changes in ocs2 mpcnet core --- .../python/ocs2_ballbot_mpcnet/ballbot_config.py | 4 ++-- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 16 +++++++--------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py index 1819907a2..7e814ca8e 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py @@ -39,10 +39,10 @@ # # data type for tensor elements -dtype = config.dtype +dtype = config.DTYPE # device on which tensors will be allocated -device = config.device +device = config.DEVICE # # ballbot_config diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 847cd9e2b..0a17351b0 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -43,7 +43,7 @@ from torch.utils.tensorboard import SummaryWriter from ocs2_mpcnet.helper import bmv, bmm -from ocs2_mpcnet.loss import Hamiltonian as Loss +from ocs2_mpcnet.loss import HamiltonianLoss as Loss from ocs2_mpcnet.memory import CircularMemory as Memory from ocs2_mpcnet.policy import LinearPolicy as Policy @@ -166,9 +166,9 @@ def start_policy_evaluation(policy, alpha=0.0): data[i].x, data[i].u, torch.ones(1, device=config.device, dtype=config.dtype), - data[i].generalized_time, - data[i].relative_state, - data[i].input_transformation, + data[i].generalizedTime, + data[i].relativeState, + data[i].inputTransformation, data[i].hamiltonian, ) # logging @@ -182,8 +182,8 @@ def start_policy_evaluation(policy, alpha=0.0): if mpcnet_interface.isPolicyEvaluationDone(): # get computed metrics metrics = mpcnet_interface.getComputedMetrics() - survival_time = np.mean([metrics[i].survival_time for i in range(len(metrics))]) - incurred_hamiltonian = np.mean([metrics[i].incurred_hamiltonian for i in range(len(metrics))]) + survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) + incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) # logging writer.add_scalar("metric/survival_time", survival_time, iteration) writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) @@ -231,9 +231,7 @@ def closure(): u_predicted = policy(generalized_time, relative_state) u_predicted = bmv(input_transformation, u_predicted) # compute the empirical loss - empirical_loss = ( - loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size - ) + empirical_loss = loss(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) # compute the gradients empirical_loss.backward() # logging From 66424213dae8c8c1290d2385eb06a078de4f94b7 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 30 Mar 2022 15:03:04 +0200 Subject: [PATCH 163/512] global variables --- .../python/ocs2_ballbot_mpcnet/ballbot_config.py | 6 +++--- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py index 7e814ca8e..6ffd0eced 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py @@ -39,17 +39,17 @@ # # data type for tensor elements -dtype = config.DTYPE +DTYPE = config.DTYPE # device on which tensors will be allocated -device = config.DEVICE +DEVICE = config.DEVICE # # ballbot_config # # name of the robot -name = "ballbot" +NAME = "ballbot" # (generalized) time dimension TIME_DIM = 1 diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 0a17351b0..95e64cf4d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -82,7 +82,7 @@ # logging description = "description" -folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.name + "_" + description +folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + description writer = SummaryWriter("runs/" + folder) os.makedirs(name="policies/" + folder) @@ -95,12 +95,12 @@ # policy policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) -policy.to(config.device) +policy.to(config.DEVICE) print("Initial policy parameters:") print(list(policy.named_parameters())) dummy_input = ( - torch.randn(1, config.TIME_DIM, device=config.device, dtype=config.dtype), - torch.randn(1, config.STATE_DIM, device=config.device, dtype=config.dtype), + torch.randn(1, config.TIME_DIM, device=config.DEVICE, dtype=config.DTYPE), + torch.randn(1, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE), ) print("Saving initial policy.") save_path = "policies/" + folder + "/initial_policy" @@ -165,7 +165,7 @@ def start_policy_evaluation(policy, alpha=0.0): data[i].t, data[i].x, data[i].u, - torch.ones(1, device=config.device, dtype=config.dtype), + torch.ones(1, device=config.DEVICE, dtype=config.DTYPE), data[i].generalizedTime, data[i].relativeState, data[i].inputTransformation, From 7db99ef5a3e4e4a5f41d90f1b3a484cb2dd81e36 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 30 Mar 2022 15:11:17 +0200 Subject: [PATCH 164/512] type hints --- .../python/ocs2_ballbot_mpcnet/ballbot_helper.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py index 16140310d..a665320de 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py @@ -33,12 +33,14 @@ """ import numpy as np +from typing import Tuple from ocs2_mpcnet import helper +from ocs2_mpcnet import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray from ocs2_ballbot_mpcnet import ballbot_config as config -def get_default_event_times_and_mode_sequence(duration): +def get_default_event_times_and_mode_sequence(duration: float) -> Tuple[np.ndarray, np.ndarray]: """Get the event times and mode sequence describing the default mode schedule. Creates the default event times and mode sequence for a certain time duration. @@ -56,7 +58,7 @@ def get_default_event_times_and_mode_sequence(duration): return helper.get_event_times_and_mode_sequence(0, duration, event_times_template, mode_sequence_template) -def get_random_initial_state(): +def get_random_initial_state() -> np.ndarray: """Get a random initial state. Samples a random initial state for the robot. @@ -78,7 +80,7 @@ def get_random_initial_state(): return random_state -def get_random_target_state(): +def get_random_target_state() -> np.ndarray: """Get a random target state. Samples a random target state for the robot. @@ -96,7 +98,9 @@ def get_random_target_state(): return random_state -def get_tasks(n_tasks, duration): +def get_tasks( + n_tasks: int, duration: float +) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: """Get tasks. Get a random set of task that should be executed by the data generation or policy evaluation. From 5bf71ab308385aee7df404f97accba9ab2bce718 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 30 Mar 2022 15:40:20 +0200 Subject: [PATCH 165/512] adapt to changes in ocs2 mpcnet core --- .../legged_robot_config.py | 4 ++-- .../legged_robot_mpcnet.py | 20 +++++++++---------- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py index 214d5c031..7797b6e57 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py @@ -39,10 +39,10 @@ # # data type for tensor elements -dtype = config.dtype +dtype = config.DTYPE # device on which tensors will be allocated -device = config.device +device = config.DEVICE # # legged_robot_config diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 06b6ba15e..320893299 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -44,8 +44,8 @@ from torch.utils.tensorboard import SummaryWriter from ocs2_mpcnet.helper import bmv, bmm -from ocs2_mpcnet.loss import Hamiltonian as ExpertsLoss -from ocs2_mpcnet.loss import CrossEntropy as GatingLoss +from ocs2_mpcnet.loss import HamiltonianLoss as ExpertsLoss +from ocs2_mpcnet.loss import CrossEntropyLoss as GatingLoss from ocs2_mpcnet.memory import CircularMemory as Memory from ocs2_legged_robot_mpcnet.legged_robot_policy import LeggedRobotMixtureOfNonlinearExpertsPolicy as Policy @@ -188,9 +188,9 @@ def start_policy_evaluation(policy, alpha=0.0): data[i].x, data[i].u, helper.get_one_hot(data[i].mode), - data[i].generalized_time, - data[i].relative_state, - data[i].input_transformation, + data[i].generalizedTime, + data[i].relativeState, + data[i].inputTransformation, data[i].hamiltonian, ) # logging @@ -204,8 +204,8 @@ def start_policy_evaluation(policy, alpha=0.0): if mpcnet_interface.isPolicyEvaluationDone(): # get computed metrics metrics = mpcnet_interface.getComputedMetrics() - survival_time = np.mean([metrics[i].survival_time for i in range(len(metrics))]) - incurred_hamiltonian = np.mean([metrics[i].incurred_hamiltonian for i in range(len(metrics))]) + survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) + incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) # logging writer.add_scalar("metric/survival_time", survival_time, iteration) writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) @@ -253,10 +253,8 @@ def closure(): u_predicted, p_predicted = policy(generalized_time, relative_state) u_predicted = bmv(input_transformation, u_predicted) # compute the empirical loss - empirical_experts_loss = ( - experts_loss.compute_batch(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H).sum() / batch_size - ) - empirical_gating_loss = gating_loss.compute_batch(p, p_predicted).sum() / batch_size + empirical_experts_loss = experts_loss(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + empirical_gating_loss = gating_loss(p, p_predicted) empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss # compute the gradients empirical_loss.backward() From bbb199e007477b32386e1cf365dbd28413d0456d Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 30 Mar 2022 15:58:43 +0200 Subject: [PATCH 166/512] global variables --- .../legged_robot_config.py | 22 +++++++++---------- .../legged_robot_helper.py | 22 +++++++++---------- .../legged_robot_mpcnet.py | 10 ++++----- .../legged_robot_policy.py | 4 ++-- 4 files changed, 29 insertions(+), 29 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py index 7797b6e57..be368b66e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py @@ -39,17 +39,17 @@ # # data type for tensor elements -dtype = config.DTYPE +DTYPE = config.DTYPE # device on which tensors will be allocated -device = config.DEVICE +DEVICE = config.DEVICE # # legged_robot_config # # name of the robot -name = "legged_robot" +NAME = "legged_robot" # (generalized) time dimension TIME_DIM = 12 @@ -64,7 +64,7 @@ EXPERT_NUM = 3 # default state -default_state = [ +DEFAULT_STATE = [ 0.0, 0.0, 0.0, @@ -92,7 +92,7 @@ ] # input bias -input_bias = [ +INPUT_BIAS = [ 0.0, 0.0, 127.861, @@ -120,7 +120,7 @@ ] # input scaling -input_scaling = [ +INPUT_SCALING = [ 100.0, 100.0, 100.0, @@ -148,7 +148,7 @@ ] # (diagonally dominant) nominal centroidal inertia normalized by robot mass -normalized_inertia = [1.62079 / 52.1348, 4.83559 / 52.1348, 4.72382 / 52.1348] +NORMALIZED_INERTIA = [1.62079 / 52.1348, 4.83559 / 52.1348, 4.72382 / 52.1348] # input cost for behavioral cloning R = [ @@ -179,7 +179,7 @@ ] # dictionary for cheating -expert_for_mode = dict([(i, None) for i in range(16)]) -expert_for_mode[15] = 0 -expert_for_mode[6] = 1 -expert_for_mode[9] = 2 +EXPERT_FOR_MODE = dict([(i, None) for i in range(16)]) +EXPERT_FOR_MODE[15] = 0 # stance +EXPERT_FOR_MODE[6] = 1 # trot +EXPERT_FOR_MODE[9] = 2 # trot diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py index 76adcab83..161b4472e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -69,9 +69,9 @@ def get_random_initial_state_stance(): max_normalized_linear_momentum_x = 0.1 max_normalized_linear_momentum_y = 0.1 max_normalized_linear_momentum_z = 0.1 - max_normalized_angular_momentum_x = config.normalized_inertia[0] * 30.0 * np.pi / 180.0 - max_normalized_angular_momentum_y = config.normalized_inertia[1] * 30.0 * np.pi / 180.0 - max_normalized_angular_momentum_z = config.normalized_inertia[2] * 30.0 * np.pi / 180.0 + max_normalized_angular_momentum_x = config.NORMALIZED_INERTIA[0] * 30.0 * np.pi / 180.0 + max_normalized_angular_momentum_y = config.NORMALIZED_INERTIA[1] * 30.0 * np.pi / 180.0 + max_normalized_angular_momentum_z = config.NORMALIZED_INERTIA[2] * 30.0 * np.pi / 180.0 random_deviation = np.zeros(config.STATE_DIM) random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) @@ -79,7 +79,7 @@ def get_random_initial_state_stance(): random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) - return np.array(config.default_state) + random_deviation + return np.array(config.DEFAULT_STATE) + random_deviation def get_random_target_state_stance(): @@ -99,7 +99,7 @@ def get_random_target_state_stance(): random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) random_deviation[10] = np.random.uniform(-max_orientation_y, max_orientation_y) random_deviation[11] = np.random.uniform(-max_orientation_x, max_orientation_x) - return np.array(config.default_state) + random_deviation + return np.array(config.DEFAULT_STATE) + random_deviation def get_trot_1(duration): @@ -153,9 +153,9 @@ def get_random_initial_state_trot(): max_normalized_linear_momentum_x = 0.5 max_normalized_linear_momentum_y = 0.25 max_normalized_linear_momentum_z = 0.25 - max_normalized_angular_momentum_x = config.normalized_inertia[0] * 60.0 * np.pi / 180.0 - max_normalized_angular_momentum_y = config.normalized_inertia[1] * 60.0 * np.pi / 180.0 - max_normalized_angular_momentum_z = config.normalized_inertia[2] * 35.0 * np.pi / 180.0 + max_normalized_angular_momentum_x = config.NORMALIZED_INERTIA[0] * 60.0 * np.pi / 180.0 + max_normalized_angular_momentum_y = config.NORMALIZED_INERTIA[1] * 60.0 * np.pi / 180.0 + max_normalized_angular_momentum_z = config.NORMALIZED_INERTIA[2] * 35.0 * np.pi / 180.0 random_deviation = np.zeros(config.STATE_DIM) random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) @@ -163,7 +163,7 @@ def get_random_initial_state_trot(): random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) - return np.array(config.default_state) + random_deviation + return np.array(config.DEFAULT_STATE) + random_deviation def get_random_target_state_trot(): @@ -181,7 +181,7 @@ def get_random_target_state_trot(): random_deviation[6] = np.random.uniform(-max_position_x, max_position_x) random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) - return np.array(config.default_state) + random_deviation + return np.array(config.DEFAULT_STATE) + random_deviation def get_tasks(n_tasks, duration, choices): @@ -250,5 +250,5 @@ def get_one_hot(mode): p: Discrete probability distribution given by a NumPy array of shape (P) containing floats. """ one_hot = np.zeros(config.EXPERT_NUM) - one_hot[config.expert_for_mode[mode]] = 1.0 + one_hot[config.EXPERT_FOR_MODE[mode]] = 1.0 return one_hot diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 320893299..649ab7b6c 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -65,7 +65,7 @@ data_generation_sampling_covariance[i, i] = 0.05**2 # normalized linear momentum for i in range(3, 6): data_generation_sampling_covariance[i, i] = ( - config.normalized_inertia[i - 3] * 2.5 * np.pi / 180.0 + config.NORMALIZED_INERTIA[i - 3] * 2.5 * np.pi / 180.0 ) ** 2 # normalized angular momentum for i in range(6, 9): data_generation_sampling_covariance[i, i] = 0.01**2 # position @@ -88,7 +88,7 @@ # logging description = "description" -folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.name + "_" + description +folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + description writer = SummaryWriter("runs/" + folder) os.makedirs(name="policies/" + folder) @@ -104,12 +104,12 @@ # policy policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) -policy.to(config.device) +policy.to(config.DEVICE) print("Initial policy parameters:") print(list(policy.named_parameters())) dummy_input = ( - torch.randn(1, config.TIME_DIM, device=config.device, dtype=config.dtype), - torch.randn(1, config.STATE_DIM, device=config.device, dtype=config.dtype), + torch.randn(1, config.TIME_DIM, device=config.DEVICE, dtype=config.DTYPE), + torch.randn(1, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE), ) print("Saving initial policy.") save_path = "policies/" + folder + "/initial_policy" diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index e0d55fc33..b72711e54 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -43,8 +43,8 @@ from ocs2_legged_robot_mpcnet import legged_robot_config as config -input_scaling = torch.tensor(config.input_scaling, device=config.device, dtype=config.dtype).diag().unsqueeze(dim=0) -input_bias = torch.tensor(config.input_bias, device=config.device, dtype=config.dtype).unsqueeze(dim=0) +input_scaling = torch.tensor(config.INPUT_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) +input_bias = torch.tensor(config.INPUT_BIAS, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) def u_transform(u): From 8e0d73a7f8111fcd9e8597b0a87dea9f43629313 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 30 Mar 2022 16:09:50 +0200 Subject: [PATCH 167/512] type hints --- .../legged_robot_helper.py | 22 +++++++++++-------- .../legged_robot_policy.py | 2 +- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py index 161b4472e..ed6985adc 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -33,12 +33,14 @@ """ import numpy as np +from typing import Tuple, List from ocs2_mpcnet import helper +from ocs2_mpcnet import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray from ocs2_legged_robot_mpcnet import legged_robot_config as config -def get_stance(duration): +def get_stance(duration: float) -> Tuple[np.ndarray, np.ndarray]: """Get the stance gait. Creates the stance event times and mode sequence for a certain time duration: @@ -58,7 +60,7 @@ def get_stance(duration): return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) -def get_random_initial_state_stance(): +def get_random_initial_state_stance() -> np.ndarray: """Get a random initial state for stance. Samples a random initial state for the robot in the stance gait. @@ -82,7 +84,7 @@ def get_random_initial_state_stance(): return np.array(config.DEFAULT_STATE) + random_deviation -def get_random_target_state_stance(): +def get_random_target_state_stance() -> np.ndarray: """Get a random target state for stance. Samples a random target state for the robot in the stance gait. @@ -102,7 +104,7 @@ def get_random_target_state_stance(): return np.array(config.DEFAULT_STATE) + random_deviation -def get_trot_1(duration): +def get_trot_1(duration: float) -> Tuple[np.ndarray, np.ndarray]: """Get the first trot gait. Creates the first trot event times and mode sequence for a certain time duration: @@ -122,7 +124,7 @@ def get_trot_1(duration): return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) -def get_trot_2(duration): +def get_trot_2(duration: float) -> Tuple[np.ndarray, np.ndarray]: """Get the second trot gait. Creates the second trot event times and mode sequence for a certain time duration: @@ -142,7 +144,7 @@ def get_trot_2(duration): return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) -def get_random_initial_state_trot(): +def get_random_initial_state_trot() -> np.ndarray: """Get a random initial state for trot. Samples a random initial state for the robot in a trot gait. @@ -166,7 +168,7 @@ def get_random_initial_state_trot(): return np.array(config.DEFAULT_STATE) + random_deviation -def get_random_target_state_trot(): +def get_random_target_state_trot() -> np.ndarray: """Get a random target state for trot. Samples a random target state for the robot in a trot gait. @@ -184,7 +186,9 @@ def get_random_target_state_trot(): return np.array(config.DEFAULT_STATE) + random_deviation -def get_tasks(n_tasks, duration, choices): +def get_tasks( + n_tasks: int, duration: float, choices: List[str] +) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: """Get tasks. Get a random set of task that should be executed by the data generation or policy evaluation. @@ -237,7 +241,7 @@ def get_tasks(n_tasks, duration, choices): return initial_observations, mode_schedules, target_trajectories -def get_one_hot(mode): +def get_one_hot(mode: int) -> np.ndarray: """Get one hot encoding of mode. Get a one hot encoding of a mode represented by a discrete probability distribution, where the sample space is the diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index b72711e54..01055bcfa 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -47,7 +47,7 @@ input_bias = torch.tensor(config.INPUT_BIAS, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) -def u_transform(u): +def u_transform(u: torch.Tensor) -> torch.Tensor: """Control input transformation. Transforms the predicted control input by scaling and adding a bias. From bc1b6f872ad83847ae938ae7333f247be2f927a9 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 09:37:26 +0200 Subject: [PATCH 168/512] small fix in doc --- ocs2_doc/docs/installation.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_doc/docs/installation.rst b/ocs2_doc/docs/installation.rst index a383e796c..3f6a6108f 100644 --- a/ocs2_doc/docs/installation.rst +++ b/ocs2_doc/docs/installation.rst @@ -121,9 +121,9 @@ Optional Dependencies .. code-block:: bash source ~/venvs/mpcnet/bin/activate - python3 -m pip install -r ~/git/ocs2_dev/ocs2_mpcnet/requirements.txt + python3 -m pip install -r ~/git/ocs2_dev/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt - Newer graphics cards might require a CUDA capability which is currently not supported by the standard PyTorch install. + Newer graphics cards might require a CUDA capability which is currently not supported by the standard PyTorch installation. In that case check `PyTorch Start Locally <https://pytorch.org/get-started/locally/>`__ for a compatible version and, e.g., run: .. code-block:: bash From 226ffd2256e5892226d002ff4f21f6b2ad278e46 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 09:41:50 +0200 Subject: [PATCH 169/512] small fix in documentation --- ocs2_doc/docs/installation.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_doc/docs/installation.rst b/ocs2_doc/docs/installation.rst index 3f6a6108f..8181e6d3a 100644 --- a/ocs2_doc/docs/installation.rst +++ b/ocs2_doc/docs/installation.rst @@ -121,7 +121,7 @@ Optional Dependencies .. code-block:: bash source ~/venvs/mpcnet/bin/activate - python3 -m pip install -r ~/git/ocs2_dev/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt + python3 -m pip install -r ~/git/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt Newer graphics cards might require a CUDA capability which is currently not supported by the standard PyTorch installation. In that case check `PyTorch Start Locally <https://pytorch.org/get-started/locally/>`__ for a compatible version and, e.g., run: From 9051a36129d2bd0b0205d6868060a38aa5ba68a4 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 11:04:30 +0200 Subject: [PATCH 170/512] add general commands and example --- ocs2_doc/docs/mpcnet.rst | 48 +++++++++++++++++++++++++++++++++------- 1 file changed, 40 insertions(+), 8 deletions(-) diff --git a/ocs2_doc/docs/mpcnet.rst b/ocs2_doc/docs/mpcnet.rst index 7cae89a4f..11976ea28 100644 --- a/ocs2_doc/docs/mpcnet.rst +++ b/ocs2_doc/docs/mpcnet.rst @@ -38,6 +38,10 @@ To build all MPC-Net packages, build the meta package: .. code-block:: bash + cd <path_to_catkin_ws> + catkin_build ocs2_mpcnet + + # Example: cd ~/catkin_ws catkin_build ocs2_mpcnet @@ -45,9 +49,13 @@ To build a robot-specific package, replace :code:`<robot>` with the robot name: .. code-block:: bash - cd ~/catkin_ws + cd <path_to_catkin_ws> catkin_build ocs2_<robot>_mpcnet + # Example: + cd ~/catkin_ws + catkin_build ocs2_ballbot_mpcnet + Training ~~~~~~~~ @@ -55,23 +63,37 @@ To train an MPC-Net policy, run: .. code-block:: bash - cd ~/git/ocs2/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet + cd <path_to_git_repos>/ocs2/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet + source <path_to_catkin_ws>/devel/setup.bash + source <path_to_venvs>/mpcnet/bin/activate + python3 <robot>_mpcnet.py + + # Example: + cd ~/git/ocs2/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet source ~/catkin_ws/devel/setup.bash source ~/venvs/mpcnet/bin/activate - python3 <robot>_mpcnet.py + python3 ballbot_mpcnet.py To monitor the training progress with Tensorboard, run: .. code-block:: bash - cd ~/git/ocs2/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet + cd <path_to_git_repos>/ocs2/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet + source <path_to_venvs>/mpcnet/bin/activate + tensorboard --logdir=runs + + # Example: + cd ~/git/ocs2/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet source ~/venvs/mpcnet/bin/activate tensorboard --logdir=runs -If you use RaiSim, you can visualize the data generation and policy evaluation rollouts with RaiSim Unity, where pre-built executables are provided in the :code:`~/git/raisimLib/raisimUnity` folder. For example, on Linux run: +If you use RaiSim, you can visualize the data generation and policy evaluation rollouts with RaiSim Unity, where pre-built executables are provided in the :code:`<path_to_git_repos>/raisimLib/raisimUnity` folder. For example, on Linux run: .. code-block:: bash + <path_to_git_repos>/raisimLib/raisimUnity/linux/raisimUnity.x86_64 + + # Example: ~/git/raisimLib/raisimUnity/linux/raisimUnity.x86_64 Deployment @@ -81,18 +103,28 @@ To deploy the default policy stored in the robot-specific package's :code:`polic .. code-block:: bash - cd ~/catkin_ws + cd <path_to_catkin_ws> source devel/setup.bash roslaunch ocs2_<robot>_mpcnet <robot>_mpcnet.launch -To deploy a new policy stored in the robot-specific package's :code:`./python/ocs2_<robot>_mpcnet/policies` folder, replace :code:`<path>` with the absolute file path to the final policy and run: + # Example: + cd ~/catkin_ws + source devel/setup.bash + roslaunch ocs2_ballbot_mpcnet ballbot_mpcnet.launch + +To deploy a new policy stored in the robot-specific package's :code:`python/ocs2_<robot>_mpcnet/policies` folder, replace :code:`<path>` with the absolute file path to the final policy and run: .. code-block:: bash - cd ~/catkin_ws + cd <path_to_catkin_ws> source devel/setup.bash roslaunch ocs2_<robot>_mpcnet <robot>_mpcnet.launch policyFile:=<path> + # Example: + cd ~/catkin_ws + source devel/setup.bash + roslaunch ocs2_ballbot_mpcnet ballbot_mpcnet.launch policyFile:='/home/user/git/ocs2/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/policies/2022-04-01_12-00-00_ballbot_description/final_policy.onnx' + How to Set Up a New Robot ~~~~~~~~~~~~~~~~~~~~~~~~~ From 43c4ec509d0466d83af0c825e309a910ba328cb3 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 11:16:20 +0200 Subject: [PATCH 171/512] small fix --- ocs2_doc/docs/mpcnet.rst | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ocs2_doc/docs/mpcnet.rst b/ocs2_doc/docs/mpcnet.rst index 11976ea28..2427c6a1a 100644 --- a/ocs2_doc/docs/mpcnet.rst +++ b/ocs2_doc/docs/mpcnet.rst @@ -63,7 +63,7 @@ To train an MPC-Net policy, run: .. code-block:: bash - cd <path_to_git_repos>/ocs2/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet + cd <path_to_ocs2_repo>/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet source <path_to_catkin_ws>/devel/setup.bash source <path_to_venvs>/mpcnet/bin/activate python3 <robot>_mpcnet.py @@ -78,7 +78,7 @@ To monitor the training progress with Tensorboard, run: .. code-block:: bash - cd <path_to_git_repos>/ocs2/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet + cd <path_to_ocs2_repo>/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet source <path_to_venvs>/mpcnet/bin/activate tensorboard --logdir=runs @@ -87,11 +87,11 @@ To monitor the training progress with Tensorboard, run: source ~/venvs/mpcnet/bin/activate tensorboard --logdir=runs -If you use RaiSim, you can visualize the data generation and policy evaluation rollouts with RaiSim Unity, where pre-built executables are provided in the :code:`<path_to_git_repos>/raisimLib/raisimUnity` folder. For example, on Linux run: +If you use RaiSim, you can visualize the data generation and policy evaluation rollouts with RaiSim Unity, where pre-built executables are provided in RaiSim's :code:`raisimUnity` folder. For example, on Linux run: .. code-block:: bash - <path_to_git_repos>/raisimLib/raisimUnity/linux/raisimUnity.x86_64 + <path_to_raisimLib_repo>/raisimUnity/linux/raisimUnity.x86_64 # Example: ~/git/raisimLib/raisimUnity/linux/raisimUnity.x86_64 From afc4b35656c7495894777b4e22c355a51d5e2100 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 12:32:47 +0200 Subject: [PATCH 172/512] split losses and policies into files --- .../python/ocs2_mpcnet/loss/__init__.py | 0 .../ocs2_mpcnet/loss/behavioral_cloning.py | 94 +++++ .../python/ocs2_mpcnet/loss/cross_entropy.py | 92 +++++ .../{loss.py => loss/hamiltonian.py} | 116 +----- ocs2_mpcnet/python/ocs2_mpcnet/policy.py | 344 ------------------ .../python/ocs2_mpcnet/policy/__init__.py | 0 .../python/ocs2_mpcnet/policy/linear.py | 78 ++++ .../policy/mixture_of_linear_experts.py | 136 +++++++ .../policy/mixture_of_nonlinear_experts.py | 154 ++++++++ .../python/ocs2_mpcnet/policy/nonlinear.py | 84 +++++ 10 files changed, 640 insertions(+), 458 deletions(-) create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/loss/__init__.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/loss/behavioral_cloning.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/loss/cross_entropy.py rename ocs2_mpcnet/python/ocs2_mpcnet/{loss.py => loss/hamiltonian.py} (59%) delete mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/policy.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/policy/__init__.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/policy/linear.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/policy/nonlinear.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss/__init__.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss/behavioral_cloning.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss/behavioral_cloning.py new file mode 100644 index 000000000..2bd25bb1e --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss/behavioral_cloning.py @@ -0,0 +1,94 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Behavioral cloning loss. + +Provides a class that implements a simple behavioral cloning loss for benchmarking MPC-Net. +""" + +import torch +import numpy as np + +from ocs2_mpcnet import config +from ocs2_mpcnet.helper import bdot, bmv + + +class BehavioralCloningLoss: + r"""Behavioral cloning loss. + + Uses a simple quadratic function as loss: + + .. math:: + + BC(u) = \delta u^T R \, \delta u, + + where the input u is of dimension U. + + Attributes: + R: A (1,U,U) tensor with the input cost matrix. + """ + + def __init__(self, R: np.ndarray) -> None: + """Initializes the BehavioralCloningLoss class. + + Initializes the BehavioralCloningLoss class by setting fixed attributes. + + Args: + R: A NumPy array of shape (U, U) with the input cost matrix. + """ + self.R = torch.tensor(R, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) + + def __call__(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: + """Computes the mean behavioral cloning loss. + + Computes the mean behavioral cloning loss for a batch of size B using the cost matrix. + + Args: + u_predicted: A (B, U) tensor with the predicted inputs. + u_target: A (B, U) tensor with the target inputs. + + Returns: + A (1) tensor containing the mean behavioral cloning loss. + """ + return torch.mean(self.compute(u_predicted, u_target)) + + def compute(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: + """Computes the behavioral cloning losses for a batch. + + Computes the behavioral cloning losses for a batch of size B using the cost matrix. + + Args: + u_predicted: A (B, U) tensor with the predicted inputs. + u_target: A (B, U) tensor with the target inputs. + + Returns: + A (B) tensor containing the behavioral cloning losses. + """ + du = torch.sub(u_predicted, u_target) + return bdot(du, bmv(self.R, du)) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss/cross_entropy.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss/cross_entropy.py new file mode 100644 index 000000000..ea3e116d3 --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss/cross_entropy.py @@ -0,0 +1,92 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Cross entropy loss. + +Provides a class that implements the cross entropy loss for training a gating network of a mixture of experts network. +""" + +import torch + +from ocs2_mpcnet import config +from ocs2_mpcnet.helper import bdot, bmv + + +class CrossEntropyLoss: + r"""Cross entropy loss. + + Uses the cross entropy between two discrete probability distributions as loss: + + .. math:: + + CE(p_{target}, p_{predicted}) = - \sum_{i=1}^{P} (p_{target,i} \log(p_{predicted,i} + \varepsilon)), + + where the sample space is the set of P individually identified items. + + Attributes: + epsilon: A (1) tensor with a small epsilon used to stabilize the logarithm. + """ + + def __init__(self, epsilon: float) -> None: + """Initializes the CrossEntropyLoss class. + + Initializes the CrossEntropyLoss class by setting fixed attributes. + + Args: + epsilon: A float used to stabilize the logarithm. + """ + self.epsilon = torch.tensor(epsilon, device=config.DEVICE, dtype=config.DTYPE) + + def __call__(self, p_target: torch.Tensor, p_predicted: torch.Tensor) -> torch.Tensor: + """Computes the mean cross entropy loss. + + Computes the mean cross entropy loss for a batch, where the logarithm is stabilized by a small epsilon. + + Args: + p_target: A (B,P) tensor with the target discrete probability distributions. + p_predicted: A (B,P) tensor with the predicted discrete probability distributions. + + Returns: + A (1) tensor containing the mean cross entropy loss. + """ + return torch.mean(self.compute(p_target, p_predicted)) + + def compute(self, p_target: torch.Tensor, p_predicted: torch.Tensor) -> torch.Tensor: + """Computes the cross entropy losses for a batch. + + Computes the cross entropy losses for a batch, where the logarithm is stabilized by a small epsilon. + + Args: + p_target: A (B,P) tensor with the target discrete probability distributions. + p_predicted: A (B,P) tensor with the predicted discrete probability distributions. + + Returns: + A (B) tensor containing the cross entropy losses. + """ + return -bdot(p_target, torch.log(p_predicted + self.epsilon)) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py b/ocs2_mpcnet/python/ocs2_mpcnet/loss/hamiltonian.py similarity index 59% rename from ocs2_mpcnet/python/ocs2_mpcnet/loss.py rename to ocs2_mpcnet/python/ocs2_mpcnet/loss/hamiltonian.py index e8831ead4..dabf637e8 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/loss.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/loss/hamiltonian.py @@ -27,16 +27,13 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### -"""Loss classes. +"""Hamiltonian loss. -Provides classes with loss functions for MPC-Net, such as the Hamiltonian loss and the cross entropy loss for training -the gating network of a mixture of experts network. Additionally, a simple behavioral cloning loss is implemented. +Provides a class that implements the Hamiltonian loss for MPC-Net. """ import torch -import numpy as np -from ocs2_mpcnet import config from ocs2_mpcnet.helper import bdot, bmv @@ -136,112 +133,3 @@ def compute( + bdot(dHdu, du) + H ) - - -class BehavioralCloningLoss: - r"""Behavioral cloning loss. - - Uses a simple quadratic function as loss: - - .. math:: - - BC(u) = \delta u^T R \, \delta u, - - where the input u is of dimension U. - - Attributes: - R: A (1,U,U) tensor with the input cost matrix. - """ - - def __init__(self, R: np.ndarray) -> None: - """Initializes the BehavioralCloningLoss class. - - Initializes the BehavioralCloningLoss class by setting fixed attributes. - - Args: - R: A NumPy array of shape (U, U) with the input cost matrix. - """ - self.R = torch.tensor(R, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) - - def __call__(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: - """Computes the mean behavioral cloning loss. - - Computes the mean behavioral cloning loss for a batch of size B using the cost matrix. - - Args: - u_predicted: A (B, U) tensor with the predicted inputs. - u_target: A (B, U) tensor with the target inputs. - - Returns: - A (1) tensor containing the mean behavioral cloning loss. - """ - return torch.mean(self.compute(u_predicted, u_target)) - - def compute(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: - """Computes the behavioral cloning losses for a batch. - - Computes the behavioral cloning losses for a batch of size B using the cost matrix. - - Args: - u_predicted: A (B, U) tensor with the predicted inputs. - u_target: A (B, U) tensor with the target inputs. - - Returns: - A (B) tensor containing the behavioral cloning losses. - """ - du = torch.sub(u_predicted, u_target) - return bdot(du, bmv(self.R, du)) - - -class CrossEntropyLoss: - r"""Cross entropy loss. - - Uses the cross entropy between two discrete probability distributions as loss: - - .. math:: - - CE(p_{target}, p_{predicted}) = - \sum_{i=1}^{P} (p_{target,i} \log(p_{predicted,i} + \varepsilon)), - - where the sample space is the set of P individually identified items. - - Attributes: - epsilon: A (1) tensor with a small epsilon used to stabilize the logarithm. - """ - - def __init__(self, epsilon: float) -> None: - """Initializes the CrossEntropyLoss class. - - Initializes the CrossEntropyLoss class by setting fixed attributes. - - Args: - epsilon: A float used to stabilize the logarithm. - """ - self.epsilon = torch.tensor(epsilon, device=config.DEVICE, dtype=config.DTYPE) - - def __call__(self, p_target: torch.Tensor, p_predicted: torch.Tensor) -> torch.Tensor: - """Computes the mean cross entropy loss. - - Computes the mean cross entropy loss for a batch, where the logarithm is stabilized by a small epsilon. - - Args: - p_target: A (B,P) tensor with the target discrete probability distributions. - p_predicted: A (B,P) tensor with the predicted discrete probability distributions. - - Returns: - A (1) tensor containing the mean cross entropy loss. - """ - return torch.mean(self.compute(p_target, p_predicted)) - - def compute(self, p_target: torch.Tensor, p_predicted: torch.Tensor) -> torch.Tensor: - """Computes the cross entropy losses for a batch. - - Computes the cross entropy losses for a batch, where the logarithm is stabilized by a small epsilon. - - Args: - p_target: A (B,P) tensor with the target discrete probability distributions. - p_predicted: A (B,P) tensor with the predicted discrete probability distributions. - - Returns: - A (B) tensor containing the cross entropy losses. - """ - return -bdot(p_target, torch.log(p_predicted + self.epsilon)) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy.py deleted file mode 100644 index ba12bdecc..000000000 --- a/ocs2_mpcnet/python/ocs2_mpcnet/policy.py +++ /dev/null @@ -1,344 +0,0 @@ -############################################################################### -# Copyright (c) 2022, Farbod Farshidian. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -############################################################################### - -"""Policy classes. - -Provides classes for different neural network policies. -""" - -import torch -from typing import Tuple - -from ocs2_mpcnet.helper import bmv - - -class LinearPolicy(torch.nn.Module): - """Linear policy. - - Class for a simple linear neural network policy. - - Attributes: - name: A string with the name of the policy. - dim_in: An integer defining the input dimension of the policy. - dim_out: An integer defining the output dimension of the policy. - linear: The linear neural network layer. - """ - - def __init__(self, dim_t: int, dim_x: int, dim_u: int) -> None: - """Initializes the LinearPolicy class. - - Initializes the LinearPolicy class by setting fixed and variable attributes. - - Args: - dim_t: An integer defining the generalized time dimension. - dim_x: An integer defining the relative state dimension. - dim_u: An integer defining the control input dimension. - """ - super().__init__() - self.name = "LinearPolicy" - self.dim_in = dim_t + dim_x - self.dim_out = dim_u - self.linear = torch.nn.Linear(self.dim_in, self.dim_out) - - def forward(self, t: torch.Tensor, x: torch.Tensor) -> torch.Tensor: - """Forward method. - - Defines the computation performed at every call. Computes the output tensors from the input tensors. - - Args: - t: A (B,T) tensor with the generalized times. - x: A (B,X) tensor with the relative states. - - Returns: - u: A (B,U) tensor with the predicted control inputs. - """ - return self.linear(torch.cat((t, x), dim=1)) - - -class NonlinearPolicy(torch.nn.Module): - """Nonlinear policy. - - Class for a simple nonlinear neural network policy, where the hidden layer is the mean of the input and output layer. - - Attributes: - name: A string with the name of the policy. - dim_in: An integer defining the input dimension of the policy. - dim_hidden: An integer defining the dimension of the hidden layer. - dim_out: An integer defining the output dimension of the policy. - linear1: The first linear neural network layer. - activation: The activation to get the hidden layer. - linear2: The second linear neural network layer. - """ - - def __init__(self, dim_t: int, dim_x: int, dim_u: int) -> None: - """Initializes the NonlinearPolicy class. - - Initializes the NonlinearPolicy class by setting fixed and variable attributes. - - Args: - dim_t: An integer defining the generalized time dimension. - dim_x: An integer defining the relative state dimension. - dim_u: An integer defining the control input dimension. - """ - super().__init__() - self.name = "NonlinearPolicy" - self.dim_in = dim_t + dim_x - self.dim_hidden = int((dim_t + dim_x + dim_u) / 2) - self.dim_out = dim_u - self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) - self.activation = torch.nn.Tanh() - self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) - - def forward(self, t: torch.Tensor, x: torch.Tensor) -> torch.Tensor: - """Forward method. - - Defines the computation performed at every call. Computes the output tensors from the input tensors. - - Args: - t: A (B,T) tensor with the generalized times. - x: A (B,X) tensor with the relative states. - - Returns: - u: A (B,U) tensor with the predicted control inputs. - """ - return self.linear2(self.activation(self.linear1(torch.cat((t, x), dim=1)))) - - -class MixtureOfLinearExpertsPolicy(torch.nn.Module): - """Mixture of linear experts policy. - - Class for a mixture of experts neural network with linear experts. - - Attributes: - name: A string with the name of the policy. - dim_in: An integer defining the input dimension of the policy. - dim_out: An integer defining the output dimension of the policy. - num_experts: An integer defining the number of experts. - gating_net: The gating network. - expert_nets: The expert networks. - """ - - def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None: - """Initializes the MixtureOfLinearExpertsPolicy class. - - Initializes the MixtureOfLinearExpertsPolicy class by setting fixed and variable attributes. - - Args: - dim_t: An integer defining the generalized time dimension. - dim_x: An integer defining the relative state dimension. - dim_u: An integer defining the control input dimension. - num_experts: An integer defining the number of experts. - """ - super().__init__() - self.name = "MixtureOfLinearExpertsPolicy" - self.dim_in = dim_t + dim_x - self.dim_out = dim_u - self.num_experts = num_experts - # gating - self.gating_net = torch.nn.Sequential(torch.nn.Linear(self.dim_in, self.num_experts), torch.nn.Softmax(dim=1)) - # experts - self.expert_nets = torch.nn.ModuleList( - [LinearExpert(i, self.dim_in, self.dim_out) for i in range(self.num_experts)] - ) - - def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: - """Forward method. - - Defines the computation performed at every call. Computes the output tensors from the input tensors. - - Args: - t: A (B,T) tensor with the generalized times. - x: A (B,X) tensor with the relative states. - - Returns: - u: A (B,U) tensor with the predicted control inputs. - p: A (B,E) tensor with the predicted expert weights. - """ - p = self.gating_net(torch.cat((t, x), dim=1)) - U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) - u = bmv(U, p) - return u, p - - -class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): - """Mixture of nonlinear experts policy. - - Class for a mixture of experts neural network with nonlinear experts, where the hidden layer is the mean of the - input and output layer. - - Attributes: - name: A string with the name of the policy. - dim_in: An integer defining the input dimension of the policy. - dim_hidden_gating: An integer defining the dimension of the hidden layer for the gating network. - dim_hidden_expert: An integer defining the dimension of the hidden layer for the expert networks. - dim_out: An integer defining the output dimension of the policy. - num_experts: An integer defining the number of experts. - gating_net: The gating network. - expert_nets: The expert networks. - """ - - def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None: - """Initializes the MixtureOfNonlinearExpertsPolicy class. - - Initializes the MixtureOfNonlinearExpertsPolicy class by setting fixed and variable attributes. - - Args: - dim_t: An integer defining the generalized time dimension. - dim_x: An integer defining the relative state dimension. - dim_u: An integer defining the control input dimension. - num_experts: An integer defining the number of experts. - """ - super().__init__() - self.name = "MixtureOfNonlinearExpertsPolicy" - self.dim_in = dim_t + dim_x - self.dim_hidden_gating = int((dim_t + dim_x + num_experts) / 2) - self.dim_hidden_expert = int((dim_t + dim_x + dim_u) / 2) - self.dim_out = dim_u - self.num_experts = num_experts - - # gating - self.gating_net = torch.nn.Sequential( - torch.nn.Linear(self.dim_in, self.dim_hidden_gating), - torch.nn.Tanh(), - torch.nn.Linear(self.dim_hidden_gating, self.num_experts), - torch.nn.Softmax(dim=1), - ) - # experts - self.expert_nets = torch.nn.ModuleList( - [NonlinearExpert(i, self.dim_in, self.dim_hidden_expert, self.dim_out) for i in range(self.num_experts)] - ) - - def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: - """Forward method. - - Defines the computation performed at every call. Computes the output tensors from the input tensors. - - Args: - t: A (B,T) tensor with the generalized times. - x: A (B,X) tensor with the relative states. - - Returns: - u: A (B,U) tensor with the predicted control inputs. - p: A (B,E) tensor with the predicted expert weights. - """ - p = self.gating_net(torch.cat((t, x), dim=1)) - U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) - u = bmv(U, p) - return u, p - - -class LinearExpert(torch.nn.Module): - """Linear expert. - - Class for a simple linear neural network expert. - - Attributes: - name: A string with the name of the expert. - dim_in: An integer defining the input dimension of the expert. - dim_out: An integer defining the output dimension of the expert. - linear: The linear neural network layer. - """ - - def __init__(self, index: int, dim_in: int, dim_out: int) -> None: - """Initializes the LinearExpert class. - - Initializes the LinearExpert class by setting fixed and variable attributes. - - Args: - index: An integer with the index of the expert. - dim_in: An integer defining the input dimension of the expert. - dim_out: An integer defining the output dimension of the expert. - """ - super().__init__() - self.name = "LinearExpert" + str(index) - self.dim_in = dim_in - self.dim_out = dim_out - self.linear = torch.nn.Linear(self.dim_in, self.dim_out) - - def forward(self, input: torch.Tensor) -> torch.Tensor: - """Forward method. - - Defines the computation performed at every call. Computes the output tensors from the input tensors. - - Args: - input: A (B,I) tensor with the inputs. - - Returns: - output: A (B,O) tensor with the outputs. - """ - return self.linear(input) - - -class NonlinearExpert(torch.nn.Module): - """Nonlinear expert. - - Class for a simple nonlinear neural network expert, where the hidden layer is the mean of the input and output layer. - - Attributes: - name: A string with the name of the expert. - dim_in: An integer defining the input dimension of the expert. - dim_hidden: An integer defining the dimension of the hidden layer. - dim_out: An integer defining the output dimension of the expert. - linear1: The first linear neural network layer. - activation: The activation to get the hidden layer. - linear2: The second linear neural network layer. - """ - - def __init__(self, index: int, dim_in: int, dim_hidden: int, dim_out: int) -> None: - """Initializes the NonlinearExpert class. - - Initializes the NonlinearExpert class by setting fixed and variable attributes. - - Args: - index: An integer with the index of the expert. - dim_in: An integer defining the input dimension of the expert. - dim_hidden: An integer defining the dimension of the hidden layer. - dim_out: An integer defining the output dimension of the expert. - """ - super().__init__() - self.name = "NonlinearExpert" + str(index) - self.dim_in = dim_in - self.dim_hidden = dim_hidden - self.dim_out = dim_out - self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) - self.activation = torch.nn.Tanh() - self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) - - def forward(self, input: torch.Tensor) -> torch.Tensor: - """Forward method. - - Defines the computation performed at every call. Computes the output tensors from the input tensors. - - Args: - input: A (B,I) tensor with the inputs. - - Returns: - output: A (B,O) tensor with the outputs. - """ - return self.linear2(self.activation(self.linear1(input))) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/__init__.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/linear.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy/linear.py new file mode 100644 index 000000000..f7f6df7a0 --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy/linear.py @@ -0,0 +1,78 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Linear policy. + +Provides a class that implements a linear policy. +""" + +import torch + + +class LinearPolicy(torch.nn.Module): + """Linear policy. + + Class for a simple linear neural network policy. + + Attributes: + name: A string with the name of the policy. + dim_in: An integer defining the input dimension of the policy. + dim_out: An integer defining the output dimension of the policy. + linear: The linear neural network layer. + """ + + def __init__(self, dim_t: int, dim_x: int, dim_u: int) -> None: + """Initializes the LinearPolicy class. + + Initializes the LinearPolicy class by setting fixed and variable attributes. + + Args: + dim_t: An integer defining the generalized time dimension. + dim_x: An integer defining the relative state dimension. + dim_u: An integer defining the control input dimension. + """ + super().__init__() + self.name = "LinearPolicy" + self.dim_in = dim_t + dim_x + self.dim_out = dim_u + self.linear = torch.nn.Linear(self.dim_in, self.dim_out) + + def forward(self, t: torch.Tensor, x: torch.Tensor) -> torch.Tensor: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + t: A (B,T) tensor with the generalized times. + x: A (B,X) tensor with the relative states. + + Returns: + u: A (B,U) tensor with the predicted control inputs. + """ + return self.linear(torch.cat((t, x), dim=1)) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py new file mode 100644 index 000000000..348965d34 --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py @@ -0,0 +1,136 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Mixture of linear experts policy. + +Provides classes that implement a mixture of linear experts policy. +""" + +import torch +from typing import Tuple + +from ocs2_mpcnet.helper import bmv + + +class MixtureOfLinearExpertsPolicy(torch.nn.Module): + """Mixture of linear experts policy. + + Class for a mixture of experts neural network with linear experts. + + Attributes: + name: A string with the name of the policy. + dim_in: An integer defining the input dimension of the policy. + dim_out: An integer defining the output dimension of the policy. + num_experts: An integer defining the number of experts. + gating_net: The gating network. + expert_nets: The expert networks. + """ + + def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None: + """Initializes the MixtureOfLinearExpertsPolicy class. + + Initializes the MixtureOfLinearExpertsPolicy class by setting fixed and variable attributes. + + Args: + dim_t: An integer defining the generalized time dimension. + dim_x: An integer defining the relative state dimension. + dim_u: An integer defining the control input dimension. + num_experts: An integer defining the number of experts. + """ + super().__init__() + self.name = "MixtureOfLinearExpertsPolicy" + self.dim_in = dim_t + dim_x + self.dim_out = dim_u + self.num_experts = num_experts + # gating + self.gating_net = torch.nn.Sequential(torch.nn.Linear(self.dim_in, self.num_experts), torch.nn.Softmax(dim=1)) + # experts + self.expert_nets = torch.nn.ModuleList( + [LinearExpert(i, self.dim_in, self.dim_out) for i in range(self.num_experts)] + ) + + def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + t: A (B,T) tensor with the generalized times. + x: A (B,X) tensor with the relative states. + + Returns: + u: A (B,U) tensor with the predicted control inputs. + p: A (B,E) tensor with the predicted expert weights. + """ + p = self.gating_net(torch.cat((t, x), dim=1)) + U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) + u = bmv(U, p) + return u, p + + +class LinearExpert(torch.nn.Module): + """Linear expert. + + Class for a simple linear neural network expert. + + Attributes: + name: A string with the name of the expert. + dim_in: An integer defining the input dimension of the expert. + dim_out: An integer defining the output dimension of the expert. + linear: The linear neural network layer. + """ + + def __init__(self, index: int, dim_in: int, dim_out: int) -> None: + """Initializes the LinearExpert class. + + Initializes the LinearExpert class by setting fixed and variable attributes. + + Args: + index: An integer with the index of the expert. + dim_in: An integer defining the input dimension of the expert. + dim_out: An integer defining the output dimension of the expert. + """ + super().__init__() + self.name = "LinearExpert" + str(index) + self.dim_in = dim_in + self.dim_out = dim_out + self.linear = torch.nn.Linear(self.dim_in, self.dim_out) + + def forward(self, input: torch.Tensor) -> torch.Tensor: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + input: A (B,I) tensor with the inputs. + + Returns: + output: A (B,O) tensor with the outputs. + """ + return self.linear(input) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py new file mode 100644 index 000000000..434446688 --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py @@ -0,0 +1,154 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Mixture of nonlinear experts policy. + +Provides classes that implement a mixture of nonlinear experts policy. +""" + +import torch +from typing import Tuple + +from ocs2_mpcnet.helper import bmv + + +class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): + """Mixture of nonlinear experts policy. + + Class for a mixture of experts neural network with nonlinear experts, where the hidden layer is the mean of the + input and output layer. + + Attributes: + name: A string with the name of the policy. + dim_in: An integer defining the input dimension of the policy. + dim_hidden_gating: An integer defining the dimension of the hidden layer for the gating network. + dim_hidden_expert: An integer defining the dimension of the hidden layer for the expert networks. + dim_out: An integer defining the output dimension of the policy. + num_experts: An integer defining the number of experts. + gating_net: The gating network. + expert_nets: The expert networks. + """ + + def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None: + """Initializes the MixtureOfNonlinearExpertsPolicy class. + + Initializes the MixtureOfNonlinearExpertsPolicy class by setting fixed and variable attributes. + + Args: + dim_t: An integer defining the generalized time dimension. + dim_x: An integer defining the relative state dimension. + dim_u: An integer defining the control input dimension. + num_experts: An integer defining the number of experts. + """ + super().__init__() + self.name = "MixtureOfNonlinearExpertsPolicy" + self.dim_in = dim_t + dim_x + self.dim_hidden_gating = int((dim_t + dim_x + num_experts) / 2) + self.dim_hidden_expert = int((dim_t + dim_x + dim_u) / 2) + self.dim_out = dim_u + self.num_experts = num_experts + + # gating + self.gating_net = torch.nn.Sequential( + torch.nn.Linear(self.dim_in, self.dim_hidden_gating), + torch.nn.Tanh(), + torch.nn.Linear(self.dim_hidden_gating, self.num_experts), + torch.nn.Softmax(dim=1), + ) + # experts + self.expert_nets = torch.nn.ModuleList( + [NonlinearExpert(i, self.dim_in, self.dim_hidden_expert, self.dim_out) for i in range(self.num_experts)] + ) + + def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + t: A (B,T) tensor with the generalized times. + x: A (B,X) tensor with the relative states. + + Returns: + u: A (B,U) tensor with the predicted control inputs. + p: A (B,E) tensor with the predicted expert weights. + """ + p = self.gating_net(torch.cat((t, x), dim=1)) + U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) + u = bmv(U, p) + return u, p + + +class NonlinearExpert(torch.nn.Module): + """Nonlinear expert. + + Class for a simple nonlinear neural network expert, where the hidden layer is the mean of the input and output layer. + + Attributes: + name: A string with the name of the expert. + dim_in: An integer defining the input dimension of the expert. + dim_hidden: An integer defining the dimension of the hidden layer. + dim_out: An integer defining the output dimension of the expert. + linear1: The first linear neural network layer. + activation: The activation to get the hidden layer. + linear2: The second linear neural network layer. + """ + + def __init__(self, index: int, dim_in: int, dim_hidden: int, dim_out: int) -> None: + """Initializes the NonlinearExpert class. + + Initializes the NonlinearExpert class by setting fixed and variable attributes. + + Args: + index: An integer with the index of the expert. + dim_in: An integer defining the input dimension of the expert. + dim_hidden: An integer defining the dimension of the hidden layer. + dim_out: An integer defining the output dimension of the expert. + """ + super().__init__() + self.name = "NonlinearExpert" + str(index) + self.dim_in = dim_in + self.dim_hidden = dim_hidden + self.dim_out = dim_out + self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) + self.activation = torch.nn.Tanh() + self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) + + def forward(self, input: torch.Tensor) -> torch.Tensor: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + input: A (B,I) tensor with the inputs. + + Returns: + output: A (B,O) tensor with the outputs. + """ + return self.linear2(self.activation(self.linear1(input))) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/nonlinear.py b/ocs2_mpcnet/python/ocs2_mpcnet/policy/nonlinear.py new file mode 100644 index 000000000..8276b1d74 --- /dev/null +++ b/ocs2_mpcnet/python/ocs2_mpcnet/policy/nonlinear.py @@ -0,0 +1,84 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Nonlinear policy. + +Provides a class that implements a nonlinear policy. +""" + +import torch + + +class NonlinearPolicy(torch.nn.Module): + """Nonlinear policy. + + Class for a simple nonlinear neural network policy, where the hidden layer is the mean of the input and output layer. + + Attributes: + name: A string with the name of the policy. + dim_in: An integer defining the input dimension of the policy. + dim_hidden: An integer defining the dimension of the hidden layer. + dim_out: An integer defining the output dimension of the policy. + linear1: The first linear neural network layer. + activation: The activation to get the hidden layer. + linear2: The second linear neural network layer. + """ + + def __init__(self, dim_t: int, dim_x: int, dim_u: int) -> None: + """Initializes the NonlinearPolicy class. + + Initializes the NonlinearPolicy class by setting fixed and variable attributes. + + Args: + dim_t: An integer defining the generalized time dimension. + dim_x: An integer defining the relative state dimension. + dim_u: An integer defining the control input dimension. + """ + super().__init__() + self.name = "NonlinearPolicy" + self.dim_in = dim_t + dim_x + self.dim_hidden = int((dim_t + dim_x + dim_u) / 2) + self.dim_out = dim_u + self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) + self.activation = torch.nn.Tanh() + self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) + + def forward(self, t: torch.Tensor, x: torch.Tensor) -> torch.Tensor: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + t: A (B,T) tensor with the generalized times. + x: A (B,X) tensor with the relative states. + + Returns: + u: A (B,U) tensor with the predicted control inputs. + """ + return self.linear2(self.activation(self.linear1(torch.cat((t, x), dim=1)))) From f62d0ebd43169bb326e19fafaaf9e05e56456ea8 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 12:50:37 +0200 Subject: [PATCH 173/512] create memory subpackage --- ocs2_mpcnet/python/ocs2_mpcnet/memory/__init__.py | 0 .../python/ocs2_mpcnet/{memory.py => memory/circular.py} | 4 ++-- 2 files changed, 2 insertions(+), 2 deletions(-) create mode 100644 ocs2_mpcnet/python/ocs2_mpcnet/memory/__init__.py rename ocs2_mpcnet/python/ocs2_mpcnet/{memory.py => memory/circular.py} (99%) diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory/__init__.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py b/ocs2_mpcnet/python/ocs2_mpcnet/memory/circular.py similarity index 99% rename from ocs2_mpcnet/python/ocs2_mpcnet/memory.py rename to ocs2_mpcnet/python/ocs2_mpcnet/memory/circular.py index 25d58d926..b9506bb02 100644 --- a/ocs2_mpcnet/python/ocs2_mpcnet/memory.py +++ b/ocs2_mpcnet/python/ocs2_mpcnet/memory/circular.py @@ -27,9 +27,9 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### -"""Memory classes. +"""Circular memory. -Provides classes for storing data in memory. +Provides a class that implements a circular memory. """ import torch From f4303bff94ff449d257a4cb9d9264194fc1a4350 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 12:54:34 +0200 Subject: [PATCH 174/512] adapt to subpackages in ocs2 mpcnet core --- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 95e64cf4d..603e60cf9 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -43,9 +43,9 @@ from torch.utils.tensorboard import SummaryWriter from ocs2_mpcnet.helper import bmv, bmm -from ocs2_mpcnet.loss import HamiltonianLoss as Loss -from ocs2_mpcnet.memory import CircularMemory as Memory -from ocs2_mpcnet.policy import LinearPolicy as Policy +from ocs2_mpcnet.loss.hamiltonian import HamiltonianLoss as Loss +from ocs2_mpcnet.memory.circular import CircularMemory as Memory +from ocs2_mpcnet.policy.linear import LinearPolicy as Policy from ocs2_ballbot_mpcnet import ballbot_config as config from ocs2_ballbot_mpcnet import ballbot_helper as helper From 31d937ee5392f5746ed6e6cbac2ca0bbece27822 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 13:23:59 +0200 Subject: [PATCH 175/512] adapt to subpackages in ocs2 mpcnet core --- .../ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 6 +++--- .../ocs2_legged_robot_mpcnet/legged_robot_policy.py | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 649ab7b6c..9890658f8 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -44,9 +44,9 @@ from torch.utils.tensorboard import SummaryWriter from ocs2_mpcnet.helper import bmv, bmm -from ocs2_mpcnet.loss import HamiltonianLoss as ExpertsLoss -from ocs2_mpcnet.loss import CrossEntropyLoss as GatingLoss -from ocs2_mpcnet.memory import CircularMemory as Memory +from ocs2_mpcnet.loss.hamiltonian import HamiltonianLoss as ExpertsLoss +from ocs2_mpcnet.loss.cross_entropy import CrossEntropyLoss as GatingLoss +from ocs2_mpcnet.memory.circular import CircularMemory as Memory from ocs2_legged_robot_mpcnet.legged_robot_policy import LeggedRobotMixtureOfNonlinearExpertsPolicy as Policy from ocs2_legged_robot_mpcnet import legged_robot_config as config diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index 01055bcfa..9ea67eb72 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -37,7 +37,7 @@ import torch -from ocs2_mpcnet import policy +from ocs2_mpcnet.policy import linear, mixture_of_linear_experts, mixture_of_nonlinear_experts, nonlinear from ocs2_mpcnet.helper import bmv, bmm from ocs2_legged_robot_mpcnet import legged_robot_config as config @@ -61,7 +61,7 @@ def u_transform(u: torch.Tensor) -> torch.Tensor: return bmv(input_scaling, u) + input_bias -class LeggedRobotLinearPolicy(policy.LinearPolicy): +class LeggedRobotLinearPolicy(linear.LinearPolicy): def __init__(self, dim_t, dim_x, dim_u): super().__init__(dim_t, dim_x, dim_u) self.name = "LeggedRobotLinearPolicy" @@ -71,7 +71,7 @@ def forward(self, t, x): return u_transform(u) -class LeggedRobotNonlinearPolicy(policy.NonlinearPolicy): +class LeggedRobotNonlinearPolicy(nonlinear.NonlinearPolicy): def __init__(self, dim_t, dim_x, dim_u): super().__init__(dim_t, dim_x, dim_u) self.name = "LeggedRobotNonlinearPolicy" @@ -81,7 +81,7 @@ def forward(self, t, x): return u_transform(u) -class LeggedRobotMixtureOfLinearExpertsPolicy(policy.MixtureOfLinearExpertsPolicy): +class LeggedRobotMixtureOfLinearExpertsPolicy(mixture_of_linear_experts.MixtureOfLinearExpertsPolicy): def __init__(self, dim_t, dim_x, dim_u, num_experts): super().__init__(dim_t, dim_x, dim_u, num_experts) self.name = "LeggedRobotMixtureOfLinearExpertsPolicy" @@ -91,7 +91,7 @@ def forward(self, t, x): return u_transform(u), p -class LeggedRobotMixtureOfNonlinearExpertsPolicy(policy.MixtureOfNonlinearExpertsPolicy): +class LeggedRobotMixtureOfNonlinearExpertsPolicy(mixture_of_nonlinear_experts.MixtureOfNonlinearExpertsPolicy): def __init__(self, dim_t, dim_x, dim_u, num_experts): super().__init__(dim_t, dim_x, dim_u, num_experts) self.name = "LeggedRobotMixtureOfNonlinearExpertsPolicy" From 6b0847ad8a5cb908c42ff4b00f8e4937509f708e Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 14:00:52 +0200 Subject: [PATCH 176/512] rename ocs2_mpcnet to ocs2_mpcnet_core --- ocs2_mpcnet/{ => ocs2_mpcnet_core}/CMakeLists.txt | 0 .../include/ocs2_mpcnet/MpcnetDefinitionBase.h | 0 .../include/ocs2_mpcnet/MpcnetInterfaceBase.h | 0 .../include/ocs2_mpcnet/MpcnetPybindMacros.h | 0 .../include/ocs2_mpcnet/control/MpcnetBehavioralController.h | 0 .../include/ocs2_mpcnet/control/MpcnetControllerBase.h | 0 .../include/ocs2_mpcnet/control/MpcnetOnnxController.h | 0 .../include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h | 0 .../include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h | 0 .../include/ocs2_mpcnet/rollout/MpcnetData.h | 0 .../include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h | 0 .../include/ocs2_mpcnet/rollout/MpcnetMetrics.h | 0 .../include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h | 0 .../include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h | 0 .../include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h | 0 .../misc/onnxruntime/cmake/onnxruntimeConfig.cmake | 0 .../misc/onnxruntime/cmake/onnxruntimeVersion.cmake | 0 ocs2_mpcnet/{ => ocs2_mpcnet_core}/package.xml | 0 ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/__init__.py | 0 ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/config.py | 0 ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/helper.py | 0 .../{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/loss/__init__.py | 0 .../python/ocs2_mpcnet/loss/behavioral_cloning.py | 0 .../python/ocs2_mpcnet/loss/cross_entropy.py | 0 .../{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/loss/hamiltonian.py | 0 .../{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/memory/__init__.py | 0 .../{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/memory/circular.py | 0 .../{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/policy/__init__.py | 0 .../{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/policy/linear.py | 0 .../python/ocs2_mpcnet/policy/mixture_of_linear_experts.py | 0 .../python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py | 0 .../{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/policy/nonlinear.py | 0 ocs2_mpcnet/{ => ocs2_mpcnet_core}/requirements.txt | 0 ocs2_mpcnet/{ => ocs2_mpcnet_core}/setup.py | 0 ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/MpcnetInterfaceBase.cpp | 0 ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/MpcnetPybindings.cpp | 0 .../src/control/MpcnetBehavioralController.cpp | 0 .../{ => ocs2_mpcnet_core}/src/control/MpcnetOnnxController.cpp | 0 .../{ => ocs2_mpcnet_core}/src/dummy/MpcnetDummyLoopRos.cpp | 0 .../{ => ocs2_mpcnet_core}/src/dummy/MpcnetDummyObserverRos.cpp | 0 .../{ => ocs2_mpcnet_core}/src/rollout/MpcnetDataGeneration.cpp | 0 .../{ => ocs2_mpcnet_core}/src/rollout/MpcnetPolicyEvaluation.cpp | 0 .../{ => ocs2_mpcnet_core}/src/rollout/MpcnetRolloutBase.cpp | 0 .../{ => ocs2_mpcnet_core}/src/rollout/MpcnetRolloutManager.cpp | 0 44 files changed, 0 insertions(+), 0 deletions(-) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/CMakeLists.txt (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/MpcnetDefinitionBase.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/MpcnetInterfaceBase.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/MpcnetPybindMacros.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/control/MpcnetBehavioralController.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/control/MpcnetControllerBase.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/control/MpcnetOnnxController.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/rollout/MpcnetData.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/rollout/MpcnetMetrics.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/misc/onnxruntime/cmake/onnxruntimeConfig.cmake (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/misc/onnxruntime/cmake/onnxruntimeVersion.cmake (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/package.xml (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/__init__.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/config.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/helper.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/loss/__init__.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/loss/behavioral_cloning.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/loss/cross_entropy.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/loss/hamiltonian.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/memory/__init__.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/memory/circular.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/policy/__init__.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/policy/linear.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/python/ocs2_mpcnet/policy/nonlinear.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/requirements.txt (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/setup.py (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/MpcnetInterfaceBase.cpp (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/MpcnetPybindings.cpp (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/control/MpcnetBehavioralController.cpp (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/control/MpcnetOnnxController.cpp (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/dummy/MpcnetDummyLoopRos.cpp (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/dummy/MpcnetDummyObserverRos.cpp (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/rollout/MpcnetDataGeneration.cpp (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/rollout/MpcnetPolicyEvaluation.cpp (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/rollout/MpcnetRolloutBase.cpp (100%) rename ocs2_mpcnet/{ => ocs2_mpcnet_core}/src/rollout/MpcnetRolloutManager.cpp (100%) diff --git a/ocs2_mpcnet/CMakeLists.txt b/ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt similarity index 100% rename from ocs2_mpcnet/CMakeLists.txt rename to ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetDefinitionBase.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/MpcnetDefinitionBase.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetDefinitionBase.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetInterfaceBase.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/MpcnetInterfaceBase.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetInterfaceBase.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetPybindMacros.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/MpcnetPybindMacros.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetPybindMacros.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetBehavioralController.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetBehavioralController.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetBehavioralController.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetControllerBase.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetControllerBase.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetControllerBase.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetOnnxController.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/control/MpcnetOnnxController.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetOnnxController.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetData.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetData.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetData.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetMetrics.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetMetrics.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetMetrics.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h diff --git a/ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h similarity index 100% rename from ocs2_mpcnet/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h diff --git a/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeConfig.cmake b/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeConfig.cmake similarity index 100% rename from ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeConfig.cmake rename to ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeConfig.cmake diff --git a/ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake b/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeVersion.cmake similarity index 100% rename from ocs2_mpcnet/misc/onnxruntime/cmake/onnxruntimeVersion.cmake rename to ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeVersion.cmake diff --git a/ocs2_mpcnet/package.xml b/ocs2_mpcnet/ocs2_mpcnet_core/package.xml similarity index 100% rename from ocs2_mpcnet/package.xml rename to ocs2_mpcnet/ocs2_mpcnet_core/package.xml diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/__init__.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/__init__.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/__init__.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/config.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/config.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/config.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/config.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/helper.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/helper.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/helper.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/__init__.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/loss/__init__.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/__init__.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss/behavioral_cloning.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/behavioral_cloning.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/loss/behavioral_cloning.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/behavioral_cloning.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss/cross_entropy.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/cross_entropy.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/loss/cross_entropy.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/cross_entropy.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/loss/hamiltonian.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/hamiltonian.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/loss/hamiltonian.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/hamiltonian.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/memory/__init__.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/memory/__init__.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/memory/__init__.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/memory/circular.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/memory/circular.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/memory/circular.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/memory/circular.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/__init__.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/policy/__init__.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/__init__.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/linear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/linear.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/policy/linear.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/linear.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py diff --git a/ocs2_mpcnet/python/ocs2_mpcnet/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/nonlinear.py similarity index 100% rename from ocs2_mpcnet/python/ocs2_mpcnet/policy/nonlinear.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/nonlinear.py diff --git a/ocs2_mpcnet/requirements.txt b/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt similarity index 100% rename from ocs2_mpcnet/requirements.txt rename to ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt diff --git a/ocs2_mpcnet/setup.py b/ocs2_mpcnet/ocs2_mpcnet_core/setup.py similarity index 100% rename from ocs2_mpcnet/setup.py rename to ocs2_mpcnet/ocs2_mpcnet_core/setup.py diff --git a/ocs2_mpcnet/src/MpcnetInterfaceBase.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp similarity index 100% rename from ocs2_mpcnet/src/MpcnetInterfaceBase.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp diff --git a/ocs2_mpcnet/src/MpcnetPybindings.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp similarity index 100% rename from ocs2_mpcnet/src/MpcnetPybindings.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp diff --git a/ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp similarity index 100% rename from ocs2_mpcnet/src/control/MpcnetBehavioralController.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp diff --git a/ocs2_mpcnet/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp similarity index 100% rename from ocs2_mpcnet/src/control/MpcnetOnnxController.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp diff --git a/ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp similarity index 100% rename from ocs2_mpcnet/src/dummy/MpcnetDummyLoopRos.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp diff --git a/ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp similarity index 100% rename from ocs2_mpcnet/src/dummy/MpcnetDummyObserverRos.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp diff --git a/ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp similarity index 100% rename from ocs2_mpcnet/src/rollout/MpcnetDataGeneration.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp diff --git a/ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetPolicyEvaluation.cpp similarity index 100% rename from ocs2_mpcnet/src/rollout/MpcnetPolicyEvaluation.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetPolicyEvaluation.cpp diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp similarity index 100% rename from ocs2_mpcnet/src/rollout/MpcnetRolloutBase.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp diff --git a/ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp similarity index 100% rename from ocs2_mpcnet/src/rollout/MpcnetRolloutManager.cpp rename to ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp From 9352081c7f98218f59e12d41a0a21bb4cb93b642 Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Thu, 31 Mar 2022 14:14:08 +0200 Subject: [PATCH 177/512] fix - final time can be post-event --- .../TrajectorySpreading.h | 9 +- .../TrajectorySpreading.cpp | 2 +- .../TrajectorySpreadingTest.cpp | 93 +++++++++++++++---- 3 files changed, 85 insertions(+), 19 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/trajectory_adjustment/TrajectorySpreading.h b/ocs2_oc/include/ocs2_oc/trajectory_adjustment/TrajectorySpreading.h index 476255e13..d9b1fc3c0 100644 --- a/ocs2_oc/include/ocs2_oc/trajectory_adjustment/TrajectorySpreading.h +++ b/ocs2_oc/include/ocs2_oc/trajectory_adjustment/TrajectorySpreading.h @@ -120,8 +120,13 @@ class TrajectorySpreading final { */ size_array_t findPostEventIndices(const scalar_array_t& eventTimes, const scalar_array_t& timeTrajectory) const { size_array_t postEventIndices(eventTimes.size()); - std::transform(eventTimes.cbegin(), eventTimes.cend(), postEventIndices.begin(), - [this, &timeTrajectory](scalar_t time) -> int { return upperBoundIndex(timeTrajectory, time); }); + for (std::size_t i = 0; i < eventTimes.size(); i++) { + if (i == eventTimes.size() - 1 && eventTimes[i] == timeTrajectory.back()) { + postEventIndices[i] = timeTrajectory.size() - 1; + } else { + postEventIndices[i] = upperBoundIndex(timeTrajectory, eventTimes[i]); + } + } return postEventIndices; } diff --git a/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp b/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp index 35c50cc6a..cd3ab6f8d 100644 --- a/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp +++ b/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp @@ -170,7 +170,7 @@ auto TrajectorySpreading::set(const ModeSchedule& oldModeSchedule, const ModeSch // if last mode of the new mode sequence is NOT matched else if (!isLastActiveModeOfNewModeSequenceMatched) { const auto mismatchEventTime = newModeSchedule.eventTimes[newStartIndexOfMatchedSequence + w - 1]; - eraseFromIndex_ = upperBoundIndex(oldTimeTrajectory, mismatchEventTime); + eraseFromIndex_ = lowerBoundIndex(oldTimeTrajectory, mismatchEventTime); } // computes the index of the spreading values and intervals diff --git a/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp b/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp index c2c0cc755..49202d81a 100644 --- a/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp +++ b/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp @@ -52,6 +52,16 @@ struct Result { ocs2::size_array_t preEventModeTrajectory; }; +std::size_t modeAtTime(const ocs2::ModeSchedule& modeSchedule, ocs2::scalar_t time, bool isFinalTime) { + if (isFinalTime) { + auto timeItr = std::upper_bound(modeSchedule.eventTimes.begin(), modeSchedule.eventTimes.end(), time); + int modeIndex = std::distance(modeSchedule.eventTimes.begin(), timeItr); + return modeSchedule.modeSequence[modeIndex]; + } else { + return modeSchedule.modeAtTime(time); + } +} + class TrajectorySpreadingTest : public testing::Test { protected: static constexpr size_t STATE_DIM = 2; @@ -99,8 +109,9 @@ class TrajectorySpreadingTest : public testing::Test { [&out](size_t eventIndex) { return out.stateTrajectory[eventIndex - 1]; }); out.modeTrajectory.resize(out.timeTrajectory.size()); - std::transform(out.timeTrajectory.begin(), out.timeTrajectory.end(), out.modeTrajectory.begin(), - [&modeSchedule](ocs2::scalar_t time) { return modeSchedule.modeAtTime(time); }); + for (int i = 0; i < out.modeTrajectory.size(); i++) { + out.modeTrajectory[i] = modeAtTime(modeSchedule, out.timeTrajectory[i], i == out.modeTrajectory.size() - 1 ? true : false); + } out.preEventModeTrajectory.resize(out.postEventsIndeces.size()); std::transform(out.postEventsIndeces.begin(), out.postEventsIndeces.end(), out.preEventModeTrajectory.begin(), @@ -194,17 +205,22 @@ class TrajectorySpreadingTest : public testing::Test { const ocs2::scalar_t finalTime = spreadResult.timeTrajectory.back(); const auto startEventItr = std::upper_bound(updatedModeSchedule.eventTimes.begin(), updatedModeSchedule.eventTimes.end(), initTime); - // If a time point is aligned with(the same as) an event time, it belongs to the pre-mode. - const auto endEventItr = std::lower_bound(updatedModeSchedule.eventTimes.begin(), updatedModeSchedule.eventTimes.end(), finalTime); - - ocs2::size_array_t postEventIndice(endEventItr - startEventItr); - std::transform(startEventItr, endEventItr, postEventIndice.begin(), [&spreadResult](ocs2::scalar_t time) { - auto timeItr = std::upper_bound(spreadResult.timeTrajectory.begin(), spreadResult.timeTrajectory.end(), time); - return std::distance(spreadResult.timeTrajectory.begin(), timeItr); - }); + // If the final time is aligned with(the same as) an event time, it belongs to the post-mode. + const auto endEventItr = std::upper_bound(updatedModeSchedule.eventTimes.begin(), updatedModeSchedule.eventTimes.end(), finalTime); + + ocs2::size_array_t postEventIndices(endEventItr - startEventItr); + for (auto itr = startEventItr; itr != endEventItr; itr++) { + int index = std::distance(startEventItr, itr); + if (itr == endEventItr - 1 && *itr == spreadResult.timeTrajectory.back()) { + postEventIndices[index] = spreadResult.timeTrajectory.size() - 1; + } else { + auto timeItr = std::upper_bound(spreadResult.timeTrajectory.begin(), spreadResult.timeTrajectory.end(), *itr); + postEventIndices[index] = std::distance(spreadResult.timeTrajectory.begin(), timeItr); + } + } - EXPECT_TRUE(spreadResult.postEventsIndeces.size() == postEventIndice.size()); - EXPECT_TRUE(std::equal(postEventIndice.begin(), postEventIndice.end(), spreadResult.postEventsIndeces.begin())); + EXPECT_TRUE(spreadResult.postEventsIndeces.size() == postEventIndices.size()); + EXPECT_TRUE(std::equal(postEventIndices.begin(), postEventIndices.end(), spreadResult.postEventsIndeces.begin())); } else { EXPECT_EQ(spreadResult.postEventsIndeces.size(), 0); @@ -220,9 +236,10 @@ class TrajectorySpreadingTest : public testing::Test { auto eventIndexActualItr = spreadResult.postEventsIndeces.begin(); auto eventTimeReferenceInd = ocs2::lookup::findIndexInTimeArray(updatedModeSchedule.eventTimes, period.first); for (size_t k = 0; k < spreadResult.timeTrajectory.size(); k++) { - // time should be monotonic sequence - if (k > 0) { - EXPECT_TRUE(spreadResult.timeTrajectory[k - 1] < spreadResult.timeTrajectory[k]); + // Time should be monotonic sequence except the final time. It is possible that the last two time points have the same time, but one + // stands for pre-mode and the other stands for post-mode. + if (k > 0 && k < spreadResult.timeTrajectory.size() - 1) { + EXPECT_TRUE(spreadResult.timeTrajectory[k - 1] < spreadResult.timeTrajectory[k]) << "TimeIndex: " << k; } // Pre-event time should be equal to the event time @@ -238,7 +255,9 @@ class TrajectorySpreadingTest : public testing::Test { eventIndexActualItr++; } // mode should match the given modeSchedule - EXPECT_TRUE(updatedModeSchedule.modeAtTime(spreadResult.timeTrajectory[k]) == spreadResult.modeTrajectory[k]); + auto updatedMode = + modeAtTime(updatedModeSchedule, spreadResult.timeTrajectory[k], k == spreadResult.timeTrajectory.size() - 1 ? true : false); + EXPECT_TRUE(updatedMode == spreadResult.modeTrajectory[k]); } // end of k loop // test postEventsIndeces @@ -319,6 +338,48 @@ TEST_F(TrajectorySpreadingTest, partially_matching_modes) { EXPECT_TRUE(status.willPerformTrajectorySpreading); } +TEST_F(TrajectorySpreadingTest, final_time_is_the_same_as_event_time_1) { + const ocs2::scalar_array_t eventTimes{1.1, 1.3}; + const ocs2::size_array_t modeSequence{0, 1, 2}; + + const ocs2::scalar_array_t updatedEventTimes{1.1, 2.1}; + const ocs2::size_array_t updatedModeSequence{0, 1, 2}; + + const std::pair<ocs2::scalar_t, ocs2::scalar_t> period{0.2, 2.1}; + const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period); + + EXPECT_FALSE(status.willTruncate); + EXPECT_TRUE(status.willPerformTrajectorySpreading); +} + +TEST_F(TrajectorySpreadingTest, final_time_is_the_same_as_event_time_2) { + const ocs2::scalar_array_t eventTimes{1.1, 2.1}; + const ocs2::size_array_t modeSequence{0, 1, 2}; + + const ocs2::scalar_array_t updatedEventTimes{1.1, 1.3}; + const ocs2::size_array_t updatedModeSequence{0, 1, 2}; + + const std::pair<ocs2::scalar_t, ocs2::scalar_t> period{0.2, 2.1}; + const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period); + + EXPECT_FALSE(status.willTruncate); + EXPECT_TRUE(status.willPerformTrajectorySpreading); +} + +TEST_F(TrajectorySpreadingTest, erase_trajectory) { + const ocs2::scalar_array_t eventTimes{1.1, 1.3}; + const ocs2::size_array_t modeSequence{0, 1, 2}; + + const ocs2::scalar_array_t updatedEventTimes{1.1, 1.3}; + const ocs2::size_array_t updatedModeSequence{0, 1, 3}; + + const std::pair<ocs2::scalar_t, ocs2::scalar_t> period{0.2, 2.1}; + const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period); + + EXPECT_TRUE(status.willTruncate); + EXPECT_FALSE(status.willPerformTrajectorySpreading); +} + TEST_F(TrajectorySpreadingTest, fully_matched_modes) { const ocs2::scalar_array_t eventTimes{1.1, 1.3}; const ocs2::size_array_t modeSequence{0, 1, 2}; From 1830c8e36f5e3116f06b2ad10604e6b673e16c2b Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 14:20:44 +0200 Subject: [PATCH 178/512] adapt ocs2 mpcnet core to name change --- ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt | 4 ++-- .../MpcnetDefinitionBase.h | 0 .../MpcnetInterfaceBase.h | 2 +- .../{ocs2_mpcnet => ocs2_mpcnet_core}/MpcnetPybindMacros.h | 4 ++-- .../control/MpcnetBehavioralController.h | 2 +- .../control/MpcnetControllerBase.h | 0 .../control/MpcnetOnnxController.h | 4 ++-- .../dummy/MpcnetDummyLoopRos.h | 2 +- .../dummy/MpcnetDummyObserverRos.h | 0 .../{ocs2_mpcnet => ocs2_mpcnet_core}/rollout/MpcnetData.h | 2 +- .../rollout/MpcnetDataGeneration.h | 4 ++-- .../rollout/MpcnetMetrics.h | 0 .../rollout/MpcnetPolicyEvaluation.h | 4 ++-- .../rollout/MpcnetRolloutBase.h | 6 +++--- .../rollout/MpcnetRolloutManager.h | 4 ++-- ocs2_mpcnet/ocs2_mpcnet_core/package.xml | 4 ++-- .../ocs2_mpcnet_core/python/ocs2_mpcnet/__init__.py | 7 ------- .../ocs2_mpcnet_core/python/ocs2_mpcnet_core/__init__.py | 7 +++++++ .../python/{ocs2_mpcnet => ocs2_mpcnet_core}/config.py | 0 .../python/{ocs2_mpcnet => ocs2_mpcnet_core}/helper.py | 2 +- .../{ocs2_mpcnet => ocs2_mpcnet_core}/loss/__init__.py | 0 .../loss/behavioral_cloning.py | 4 ++-- .../loss/cross_entropy.py | 4 ++-- .../{ocs2_mpcnet => ocs2_mpcnet_core}/loss/hamiltonian.py | 2 +- .../{ocs2_mpcnet => ocs2_mpcnet_core}/memory/__init__.py | 0 .../{ocs2_mpcnet => ocs2_mpcnet_core}/memory/circular.py | 4 ++-- .../{ocs2_mpcnet => ocs2_mpcnet_core}/policy/__init__.py | 0 .../{ocs2_mpcnet => ocs2_mpcnet_core}/policy/linear.py | 0 .../policy/mixture_of_linear_experts.py | 2 +- .../policy/mixture_of_nonlinear_experts.py | 2 +- .../{ocs2_mpcnet => ocs2_mpcnet_core}/policy/nonlinear.py | 0 ocs2_mpcnet/ocs2_mpcnet_core/setup.py | 2 +- ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp | 2 +- ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp | 4 ++-- .../src/control/MpcnetBehavioralController.cpp | 2 +- .../ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp | 2 +- .../ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp | 2 +- .../ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp | 2 +- .../ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp | 4 ++-- .../src/rollout/MpcnetPolicyEvaluation.cpp | 2 +- .../ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp | 4 ++-- .../ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp | 2 +- 42 files changed, 52 insertions(+), 52 deletions(-) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/MpcnetDefinitionBase.h (100%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/MpcnetInterfaceBase.h (98%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/MpcnetPybindMacros.h (99%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/control/MpcnetBehavioralController.h (98%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/control/MpcnetControllerBase.h (100%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/control/MpcnetOnnxController.h (97%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/dummy/MpcnetDummyLoopRos.h (98%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/dummy/MpcnetDummyObserverRos.h (100%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/rollout/MpcnetData.h (98%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/rollout/MpcnetDataGeneration.h (97%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/rollout/MpcnetMetrics.h (100%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/rollout/MpcnetPolicyEvaluation.h (97%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/rollout/MpcnetRolloutBase.h (96%) rename ocs2_mpcnet/ocs2_mpcnet_core/include/{ocs2_mpcnet => ocs2_mpcnet_core}/rollout/MpcnetRolloutManager.h (98%) delete mode 100644 ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/__init__.py create mode 100644 ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/__init__.py rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/config.py (100%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/helper.py (99%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/loss/__init__.py (100%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/loss/behavioral_cloning.py (97%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/loss/cross_entropy.py (97%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/loss/hamiltonian.py (99%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/memory/__init__.py (100%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/memory/circular.py (99%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/policy/__init__.py (100%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/policy/linear.py (100%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/policy/mixture_of_linear_experts.py (99%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/policy/mixture_of_nonlinear_experts.py (99%) rename ocs2_mpcnet/ocs2_mpcnet_core/python/{ocs2_mpcnet => ocs2_mpcnet_core}/policy/nonlinear.py (100%) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt b/ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt index 6708884f9..7b91fd000 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt +++ b/ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(ocs2_mpcnet) +project(ocs2_mpcnet_core) set(CATKIN_PACKAGE_DEPENDENCIES pybind11_catkin @@ -84,7 +84,7 @@ catkin_python_setup() ######################### find_package(cmake_clang_tools QUIET) if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target ocs2_mpcnet") + message(STATUS "Run clang tooling for target ocs2_mpcnet_core") add_clang_tooling( TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetDefinitionBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetDefinitionBase.h similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetDefinitionBase.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetDefinitionBase.h diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetInterfaceBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetInterfaceBase.h similarity index 98% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetInterfaceBase.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetInterfaceBase.h index 0d55bcc37..ec4b72d00 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetInterfaceBase.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetInterfaceBase.h @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "ocs2_mpcnet/rollout/MpcnetRolloutManager.h" +#include "ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetPybindMacros.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h similarity index 99% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetPybindMacros.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h index b14c30ac0..568d4f986 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h @@ -36,8 +36,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_python_interface/PybindMacros.h> -#include "ocs2_mpcnet/rollout/MpcnetData.h" -#include "ocs2_mpcnet/rollout/MpcnetMetrics.h" +#include "ocs2_mpcnet_core/rollout/MpcnetData.h" +#include "ocs2_mpcnet_core/rollout/MpcnetMetrics.h" using namespace pybind11::literals; diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetBehavioralController.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetBehavioralController.h similarity index 98% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetBehavioralController.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetBehavioralController.h index 787f6edfb..60ec8a6c6 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetBehavioralController.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetBehavioralController.h @@ -33,7 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/control/ControllerBase.h> -#include "ocs2_mpcnet/control/MpcnetControllerBase.h" +#include "ocs2_mpcnet_core/control/MpcnetControllerBase.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetControllerBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetControllerBase.h similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetControllerBase.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetControllerBase.h diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetOnnxController.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetOnnxController.h similarity index 97% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetOnnxController.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetOnnxController.h index 351facd05..89c102eb3 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/control/MpcnetOnnxController.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetOnnxController.h @@ -33,8 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> -#include "ocs2_mpcnet/MpcnetDefinitionBase.h" -#include "ocs2_mpcnet/control/MpcnetControllerBase.h" +#include "ocs2_mpcnet_core/MpcnetDefinitionBase.h" +#include "ocs2_mpcnet_core/control/MpcnetControllerBase.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h similarity index 98% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h index 80a09910d..b1e954e28 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h @@ -35,7 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_ros_interfaces/mrt/DummyObserver.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> -#include "ocs2_mpcnet/control/MpcnetControllerBase.h" +#include "ocs2_mpcnet_core/control/MpcnetControllerBase.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyObserverRos.h similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyObserverRos.h diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetData.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h similarity index 98% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetData.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h index 75bb2339a..83a566400 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetData.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_mpc/MPC_BASE.h> -#include "ocs2_mpcnet/MpcnetDefinitionBase.h" +#include "ocs2_mpcnet_core/MpcnetDefinitionBase.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h similarity index 97% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h index f916b32fc..6b0ee2a0e 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetDataGeneration.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h @@ -29,8 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "ocs2_mpcnet/rollout/MpcnetData.h" -#include "ocs2_mpcnet/rollout/MpcnetRolloutBase.h" +#include "ocs2_mpcnet_core/rollout/MpcnetData.h" +#include "ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetMetrics.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetMetrics.h similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetMetrics.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetMetrics.h diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h similarity index 97% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h index 188475b69..24d8ac57e 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h @@ -29,8 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "ocs2_mpcnet/rollout/MpcnetMetrics.h" -#include "ocs2_mpcnet/rollout/MpcnetRolloutBase.h" +#include "ocs2_mpcnet_core/rollout/MpcnetMetrics.h" +#include "ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h similarity index 96% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h index 475b50581..23c16f1d2 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutBase.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h @@ -37,9 +37,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/rollout/RolloutBase.h> #include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> -#include "ocs2_mpcnet/MpcnetDefinitionBase.h" -#include "ocs2_mpcnet/control/MpcnetBehavioralController.h" -#include "ocs2_mpcnet/control/MpcnetControllerBase.h" +#include "ocs2_mpcnet_core/MpcnetDefinitionBase.h" +#include "ocs2_mpcnet_core/control/MpcnetBehavioralController.h" +#include "ocs2_mpcnet_core/control/MpcnetControllerBase.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h similarity index 98% rename from ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h rename to ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h index 24b2d91ad..c1862e9bc 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet/rollout/MpcnetRolloutManager.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h @@ -31,8 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/thread_support/ThreadPool.h> -#include "ocs2_mpcnet/rollout/MpcnetDataGeneration.h" -#include "ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h" +#include "ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h" +#include "ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/package.xml b/ocs2_mpcnet/ocs2_mpcnet_core/package.xml index b2120f7fc..48eb0efea 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/package.xml +++ b/ocs2_mpcnet/ocs2_mpcnet_core/package.xml @@ -1,8 +1,8 @@ <?xml version="1.0"?> <package format="2"> - <name>ocs2_mpcnet</name> + <name>ocs2_mpcnet_core</name> <version>0.0.0</version> - <description>The ocs2_mpcnet package</description> + <description>The ocs2_mpcnet_core package</description> <author email="areske@ethz.ch">Alexander Reske</author> diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/__init__.py deleted file mode 100644 index 134e00bd3..000000000 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -from ocs2_mpcnet.MpcnetPybindings import size_array, scalar_array, vector_array, matrix_array -from ocs2_mpcnet.MpcnetPybindings import ScalarFunctionQuadraticApproximation -from ocs2_mpcnet.MpcnetPybindings import SystemObservation, SystemObservationArray -from ocs2_mpcnet.MpcnetPybindings import ModeSchedule, ModeScheduleArray -from ocs2_mpcnet.MpcnetPybindings import TargetTrajectories, TargetTrajectoriesArray -from ocs2_mpcnet.MpcnetPybindings import DataPoint, DataArray -from ocs2_mpcnet.MpcnetPybindings import Metrics, MetricsArray diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/__init__.py new file mode 100644 index 000000000..0b2bd554f --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/__init__.py @@ -0,0 +1,7 @@ +from ocs2_mpcnet_core.MpcnetPybindings import size_array, scalar_array, vector_array, matrix_array +from ocs2_mpcnet_core.MpcnetPybindings import ScalarFunctionQuadraticApproximation +from ocs2_mpcnet_core.MpcnetPybindings import SystemObservation, SystemObservationArray +from ocs2_mpcnet_core.MpcnetPybindings import ModeSchedule, ModeScheduleArray +from ocs2_mpcnet_core.MpcnetPybindings import TargetTrajectories, TargetTrajectoriesArray +from ocs2_mpcnet_core.MpcnetPybindings import DataPoint, DataArray +from ocs2_mpcnet_core.MpcnetPybindings import Metrics, MetricsArray diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/config.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/config.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/helper.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py similarity index 99% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/helper.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py index d135833a6..08da92d2a 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/helper.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py @@ -36,7 +36,7 @@ import numpy as np from typing import Tuple -from ocs2_mpcnet import ( +from ocs2_mpcnet_core import ( size_array, scalar_array, vector_array, diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/__init__.py similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/__init__.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/__init__.py diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/behavioral_cloning.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py similarity index 97% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/behavioral_cloning.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py index 2bd25bb1e..990971556 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/behavioral_cloning.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py @@ -35,8 +35,8 @@ import torch import numpy as np -from ocs2_mpcnet import config -from ocs2_mpcnet.helper import bdot, bmv +from ocs2_mpcnet_core import config +from ocs2_mpcnet_core.helper import bdot, bmv class BehavioralCloningLoss: diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/cross_entropy.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py similarity index 97% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/cross_entropy.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py index ea3e116d3..cf5702f27 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/cross_entropy.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py @@ -34,8 +34,8 @@ import torch -from ocs2_mpcnet import config -from ocs2_mpcnet.helper import bdot, bmv +from ocs2_mpcnet_core import config +from ocs2_mpcnet_core.helper import bdot, bmv class CrossEntropyLoss: diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/hamiltonian.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py similarity index 99% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/hamiltonian.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py index dabf637e8..cbeac66f1 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/loss/hamiltonian.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py @@ -34,7 +34,7 @@ import torch -from ocs2_mpcnet.helper import bdot, bmv +from ocs2_mpcnet_core.helper import bdot, bmv class HamiltonianLoss: diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/memory/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/__init__.py similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/memory/__init__.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/__init__.py diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/memory/circular.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py similarity index 99% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/memory/circular.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py index b9506bb02..95d350d04 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/memory/circular.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py @@ -36,8 +36,8 @@ import numpy as np from typing import Tuple -from ocs2_mpcnet import config -from ocs2_mpcnet import ScalarFunctionQuadraticApproximation +from ocs2_mpcnet_core import config +from ocs2_mpcnet_core import ScalarFunctionQuadraticApproximation class CircularMemory: diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/__init__.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/linear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/linear.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py similarity index 99% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py index 348965d34..70602c0d8 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_linear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py @@ -35,7 +35,7 @@ import torch from typing import Tuple -from ocs2_mpcnet.helper import bmv +from ocs2_mpcnet_core.helper import bmv class MixtureOfLinearExpertsPolicy(torch.nn.Module): diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py similarity index 99% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py index 434446688..0395d65b7 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/mixture_of_nonlinear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py @@ -35,7 +35,7 @@ import torch from typing import Tuple -from ocs2_mpcnet.helper import bmv +from ocs2_mpcnet_core.helper import bmv class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py similarity index 100% rename from ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet/policy/nonlinear.py rename to ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/setup.py b/ocs2_mpcnet/ocs2_mpcnet_core/setup.py index c4235d359..61478769f 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/setup.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/setup.py @@ -3,6 +3,6 @@ from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup -setup_args = generate_distutils_setup(packages=["ocs2_mpcnet"], package_dir={"": "python"}) +setup_args = generate_distutils_setup(packages=["ocs2_mpcnet_core"], package_dir={"": "python"}) setup(**setup_args) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp index f21749070..e02f861d9 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_mpcnet/MpcnetInterfaceBase.h" +#include "ocs2_mpcnet_core/MpcnetInterfaceBase.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp index c0a3694f1..38fec2cf0 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp @@ -27,8 +27,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include <ocs2_mpcnet/MpcnetPybindMacros.h> +#include "ocs2_mpcnet_core/MpcnetPybindMacros.h" -#include "ocs2_mpcnet/MpcnetInterfaceBase.h" +#include "ocs2_mpcnet_core/MpcnetInterfaceBase.h" CREATE_MPCNET_PYTHON_BINDINGS(MpcnetPybindings) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp index b641d3cbe..3c3ef8437 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_mpcnet/control/MpcnetBehavioralController.h" +#include "ocs2_mpcnet_core/control/MpcnetBehavioralController.h" #include <ocs2_core/misc/Numerics.h> diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp index f15167805..30d3e5537 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_mpcnet/control/MpcnetOnnxController.h" +#include "ocs2_mpcnet_core/control/MpcnetOnnxController.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp index af825ef68..96ab54ead 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h" +#include "ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h" #include <ros/ros.h> diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp index 850f607d2..6005b62c9 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h" +#include "ocs2_mpcnet_core/dummy/MpcnetDummyObserverRos.h" #include <ros/ros.h> diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp index 39aba49c7..0cfc884eb 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp @@ -27,11 +27,11 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_mpcnet/rollout/MpcnetDataGeneration.h" +#include "ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h" #include <random> -#include "ocs2_mpcnet/control/MpcnetBehavioralController.h" +#include "ocs2_mpcnet_core/control/MpcnetBehavioralController.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetPolicyEvaluation.cpp index 9affeb6d0..d1847fc04 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetPolicyEvaluation.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetPolicyEvaluation.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_mpcnet/rollout/MpcnetPolicyEvaluation.h" +#include "ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp index afa7e1011..1414dfdb9 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp @@ -27,9 +27,9 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_mpcnet/rollout/MpcnetRolloutBase.h" +#include "ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h" -#include "ocs2_mpcnet/control/MpcnetBehavioralController.h" +#include "ocs2_mpcnet_core/control/MpcnetBehavioralController.h" namespace ocs2 { namespace mpcnet { diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp index af8e119e1..9d3f43d2b 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_mpcnet/rollout/MpcnetRolloutManager.h" +#include "ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h" namespace ocs2 { namespace mpcnet { From ad59e458755509696b4f6cf61cf72961deffd37a Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 14:34:05 +0200 Subject: [PATCH 179/512] small fix in mpcnet pybindings --- .../include/ocs2_mpcnet_core/MpcnetPybindMacros.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h index 568d4f986..d3ea46b7e 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h @@ -118,7 +118,7 @@ using namespace pybind11::literals; /* create a python module */ \ PYBIND11_MODULE(LIB_NAME, m) { \ /* import the general MPC-Net module */ \ - pybind11::module::import("ocs2_mpcnet.MpcnetPybindings"); \ + pybind11::module::import("ocs2_mpcnet_core.MpcnetPybindings"); \ /* bind actual MPC-Net interface for specific robot */ \ pybind11::class_<MPCNET_INTERFACE>(m, "MpcnetInterface") \ .def(pybind11::init<size_t, size_t, bool>()) \ From ad55ae655d9cf2ede9f65858cea4e5c256722172 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 14:37:39 +0200 Subject: [PATCH 180/512] adapt ocs2 mpcnet ballbot to name change --- ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt | 2 +- .../include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h | 2 +- .../include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h | 2 +- ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml | 2 +- .../python/ocs2_ballbot_mpcnet/ballbot_config.py | 2 +- .../python/ocs2_ballbot_mpcnet/ballbot_helper.py | 4 ++-- .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 8 ++++---- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp | 6 +++--- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp | 2 +- .../ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp | 2 +- 10 files changed, 16 insertions(+), 16 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt index ee0ef0bc2..d95da9050 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt @@ -4,7 +4,7 @@ project(ocs2_ballbot_mpcnet) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ballbot ocs2_ballbot_ros - ocs2_mpcnet + ocs2_mpcnet_core ) find_package(catkin REQUIRED COMPONENTS diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h index 55ee34551..21a654c25 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include <ocs2_mpcnet/MpcnetDefinitionBase.h> +#include <ocs2_mpcnet_core/MpcnetDefinitionBase.h> namespace ocs2 { namespace ballbot { diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h index cead0cfd3..4456b7ab5 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h @@ -30,7 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include <ocs2_ballbot/BallbotInterface.h> -#include <ocs2_mpcnet/MpcnetInterfaceBase.h> +#include <ocs2_mpcnet_core/MpcnetInterfaceBase.h> namespace ocs2 { namespace ballbot { diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml index 1a792362e..c2448bd1b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml @@ -17,6 +17,6 @@ <depend>ocs2_ballbot</depend> <depend>ocs2_ballbot_ros</depend> - <depend>ocs2_mpcnet</depend> + <depend>ocs2_mpcnet_core</depend> </package> diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py index 6ffd0eced..794e872da 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py @@ -32,7 +32,7 @@ Sets robot-specific configuration variables for ballbot. """ -from ocs2_mpcnet import config +from ocs2_mpcnet_core import config # # config diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py index a665320de..6181d212a 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py @@ -35,8 +35,8 @@ import numpy as np from typing import Tuple -from ocs2_mpcnet import helper -from ocs2_mpcnet import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray +from ocs2_mpcnet_core import helper +from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray from ocs2_ballbot_mpcnet import ballbot_config as config diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py index 603e60cf9..22dd90c08 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py @@ -42,10 +42,10 @@ from torch.utils.tensorboard import SummaryWriter -from ocs2_mpcnet.helper import bmv, bmm -from ocs2_mpcnet.loss.hamiltonian import HamiltonianLoss as Loss -from ocs2_mpcnet.memory.circular import CircularMemory as Memory -from ocs2_mpcnet.policy.linear import LinearPolicy as Policy +from ocs2_mpcnet_core.helper import bmv, bmm +from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss as Loss +from ocs2_mpcnet_core.memory.circular import CircularMemory as Memory +from ocs2_mpcnet_core.policy.linear import LinearPolicy as Policy from ocs2_ballbot_mpcnet import ballbot_config as config from ocs2_ballbot_mpcnet import ballbot_helper as helper diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp index ac446370e..010fc2a8b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp @@ -32,9 +32,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_ballbot/BallbotInterface.h> #include <ocs2_ballbot_ros/BallbotDummyVisualization.h> -#include <ocs2_mpcnet/control/MpcnetOnnxController.h> -#include <ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h> -#include <ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h> +#include <ocs2_mpcnet_core/control/MpcnetOnnxController.h> +#include <ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h> +#include <ocs2_mpcnet_core/dummy/MpcnetDummyObserverRos.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> #include "ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h" diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index 1860cd9a8..f76f23a34 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ros/package.h> #include <ocs2_ddp/GaussNewtonDDP_MPC.h> -#include <ocs2_mpcnet/control/MpcnetOnnxController.h> +#include <ocs2_mpcnet_core/control/MpcnetOnnxController.h> #include <ocs2_oc/rollout/TimeTriggeredRollout.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp index 9508b85f4..c08e80a5d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include <ocs2_mpcnet/MpcnetPybindMacros.h> +#include <ocs2_mpcnet_core/MpcnetPybindMacros.h> #include "ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h" From fe5bdee68891d989ad27d6fcb6d89cf2c88162e6 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 14:39:17 +0200 Subject: [PATCH 181/512] move ocs2 ballbot mpcnet into ocs2 mpcnet --- .../ocs2_ballbot_mpcnet/CMakeLists.txt | 0 .../ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h | 0 .../ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h | 0 .../launch/ballbot_mpcnet.launch | 0 .../ocs2_ballbot_mpcnet/package.xml | 0 .../ocs2_ballbot_mpcnet/policy/ballbot.onnx | Bin .../ocs2_ballbot_mpcnet/policy/ballbot.pt | Bin .../python/ocs2_ballbot_mpcnet/__init__.py | 0 .../python/ocs2_ballbot_mpcnet/ballbot_config.py | 0 .../python/ocs2_ballbot_mpcnet/ballbot_helper.py | 0 .../python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py | 0 .../ocs2_ballbot_mpcnet/setup.py | 0 .../src/BallbotMpcnetDefinition.cpp | 0 .../src/BallbotMpcnetDummyNode.cpp | 0 .../src/BallbotMpcnetInterface.cpp | 0 .../src/BallbotMpcnetPybindings.cpp | 0 16 files changed, 0 insertions(+), 0 deletions(-) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/CMakeLists.txt (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/package.xml (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/policy/ballbot.onnx (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/policy/ballbot.pt (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/setup.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp (100%) diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt b/ocs2_mpcnet/ocs2_ballbot_mpcnet/CMakeLists.txt similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/CMakeLists.txt rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/CMakeLists.txt diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h b/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h b/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch b/ocs2_mpcnet/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml b/ocs2_mpcnet/ocs2_ballbot_mpcnet/package.xml similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/package.xml rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/package.xml diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.onnx b/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.onnx similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.onnx rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.onnx diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.pt b/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.pt similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/policy/ballbot.pt rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.pt diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/setup.py similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/setup.py rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/setup.py diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp diff --git a/ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp From d5fc5961f5068c11f726545b85005bbe9ac41051 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 14:50:13 +0200 Subject: [PATCH 182/512] adapt ocs2 mpcnet legged robot to name change --- .../ocs2_legged_robot_mpcnet/CMakeLists.txt | 2 +- .../LeggedRobotMpcnetDefinition.h | 2 +- .../ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h | 2 +- .../ocs2_legged_robot_mpcnet/package.xml | 2 +- .../ocs2_legged_robot_mpcnet/legged_robot_config.py | 2 +- .../ocs2_legged_robot_mpcnet/legged_robot_helper.py | 4 ++-- .../ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 8 ++++---- .../ocs2_legged_robot_mpcnet/legged_robot_policy.py | 4 ++-- .../src/LeggedRobotMpcnetDummyNode.cpp | 6 +++--- .../src/LeggedRobotMpcnetInterface.cpp | 2 +- .../src/LeggedRobotMpcnetPybindings.cpp | 2 +- 11 files changed, 18 insertions(+), 18 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt index c213339f4..6addac114 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt @@ -5,7 +5,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_legged_robot ocs2_legged_robot_raisim ocs2_legged_robot_ros - ocs2_mpcnet + ocs2_mpcnet_core ) find_package(catkin REQUIRED COMPONENTS diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h index bfc630187..aee27b718 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include <ocs2_mpcnet/MpcnetDefinitionBase.h> +#include <ocs2_mpcnet_core/MpcnetDefinitionBase.h> namespace ocs2 { namespace legged_robot { diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h index 90eae8b8a..3acb1d0ee 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h @@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_legged_robot/LeggedRobotInterface.h> #include <ocs2_legged_robot_raisim/LeggedRobotRaisimConversions.h> -#include <ocs2_mpcnet/MpcnetInterfaceBase.h> +#include <ocs2_mpcnet_core/MpcnetInterfaceBase.h> namespace ocs2 { namespace legged_robot { diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml index 4b0973fa0..c39765d14 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml @@ -18,6 +18,6 @@ <depend>ocs2_legged_robot</depend> <depend>ocs2_legged_robot_raisim</depend> <depend>ocs2_legged_robot_ros</depend> - <depend>ocs2_mpcnet</depend> + <depend>ocs2_mpcnet_core</depend> </package> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py index be368b66e..390437ed6 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py @@ -32,7 +32,7 @@ Sets robot-specific configuration variables for legged robot. """ -from ocs2_mpcnet import config +from ocs2_mpcnet_core import config # # config diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py index ed6985adc..958873ef6 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py @@ -35,8 +35,8 @@ import numpy as np from typing import Tuple, List -from ocs2_mpcnet import helper -from ocs2_mpcnet import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray +from ocs2_mpcnet_core import helper +from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray from ocs2_legged_robot_mpcnet import legged_robot_config as config diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py index 9890658f8..21a51cc59 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py @@ -43,10 +43,10 @@ from torch.utils.tensorboard import SummaryWriter -from ocs2_mpcnet.helper import bmv, bmm -from ocs2_mpcnet.loss.hamiltonian import HamiltonianLoss as ExpertsLoss -from ocs2_mpcnet.loss.cross_entropy import CrossEntropyLoss as GatingLoss -from ocs2_mpcnet.memory.circular import CircularMemory as Memory +from ocs2_mpcnet_core.helper import bmv, bmm +from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss as ExpertsLoss +from ocs2_mpcnet_core.loss.cross_entropy import CrossEntropyLoss as GatingLoss +from ocs2_mpcnet_core.memory.circular import CircularMemory as Memory from ocs2_legged_robot_mpcnet.legged_robot_policy import LeggedRobotMixtureOfNonlinearExpertsPolicy as Policy from ocs2_legged_robot_mpcnet import legged_robot_config as config diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py index 9ea67eb72..cad6f8801 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py @@ -37,8 +37,8 @@ import torch -from ocs2_mpcnet.policy import linear, mixture_of_linear_experts, mixture_of_nonlinear_experts, nonlinear -from ocs2_mpcnet.helper import bmv, bmm +from ocs2_mpcnet_core.policy import linear, mixture_of_linear_experts, mixture_of_nonlinear_experts, nonlinear +from ocs2_mpcnet_core.helper import bmv, bmm from ocs2_legged_robot_mpcnet import legged_robot_config as config diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 469a1538f..982ce5cf6 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -36,9 +36,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_legged_robot_raisim/LeggedRobotRaisimVisualizer.h> #include <ocs2_legged_robot_ros/gait/GaitReceiver.h> #include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h> -#include <ocs2_mpcnet/control/MpcnetOnnxController.h> -#include <ocs2_mpcnet/dummy/MpcnetDummyLoopRos.h> -#include <ocs2_mpcnet/dummy/MpcnetDummyObserverRos.h> +#include <ocs2_mpcnet_core/control/MpcnetOnnxController.h> +#include <ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h> +#include <ocs2_mpcnet_core/dummy/MpcnetDummyObserverRos.h> #include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h> #include <ocs2_raisim_core/RaisimRollout.h> #include <ocs2_raisim_ros/RaisimHeightmapRosConverter.h> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index b20ca6a0f..ac226d6d6 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ros/package.h> #include <ocs2_ddp/GaussNewtonDDP_MPC.h> -#include <ocs2_mpcnet/control/MpcnetOnnxController.h> +#include <ocs2_mpcnet_core/control/MpcnetOnnxController.h> #include <ocs2_oc/rollout/TimeTriggeredRollout.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> #include <ocs2_raisim_core/RaisimRollout.h> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp index 6645e01a2..f170bde7a 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include <ocs2_mpcnet/MpcnetPybindMacros.h> +#include <ocs2_mpcnet_core/MpcnetPybindMacros.h> #include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h" From fd53853079e772fbcc7f51160b294566f981a8ed Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 14:59:50 +0200 Subject: [PATCH 183/512] move ocs2 legged robot mpcnet into ocs2 mpcnet --- .../ocs2_legged_robot_mpcnet/CMakeLists.txt | 0 .../LeggedRobotMpcnetDefinition.h | 0 .../LeggedRobotMpcnetInterface.h | 0 .../launch/legged_robot_mpcnet.launch | 0 .../ocs2_legged_robot_mpcnet/package.xml | 0 .../policy/legged_robot.onnx | Bin .../ocs2_legged_robot_mpcnet/policy/legged_robot.pt | Bin .../python/ocs2_legged_robot_mpcnet/__init__.py | 0 .../ocs2_legged_robot_mpcnet/legged_robot_config.py | 0 .../ocs2_legged_robot_mpcnet/legged_robot_helper.py | 0 .../ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py | 0 .../ocs2_legged_robot_mpcnet/legged_robot_policy.py | 0 .../ocs2_legged_robot_mpcnet/setup.py | 0 .../src/LeggedRobotMpcnetDefinition.cpp | 0 .../src/LeggedRobotMpcnetDummyNode.cpp | 0 .../src/LeggedRobotMpcnetInterface.cpp | 0 .../src/LeggedRobotMpcnetPybindings.cpp | 0 17 files changed, 0 insertions(+), 0 deletions(-) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/CMakeLists.txt (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/package.xml (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/policy/legged_robot.pt (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/setup.py (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp (100%) rename {ocs2_robotic_examples => ocs2_mpcnet}/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp (100%) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/CMakeLists.txt similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/CMakeLists.txt rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/CMakeLists.txt diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/package.xml similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/package.xml rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/package.xml diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.pt b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/policy/legged_robot.pt rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/setup.py similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/setup.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/setup.py diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp From 0182a09a06e034fc85080ef0ef43788d5dc9d6c2 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 15:20:43 +0200 Subject: [PATCH 184/512] add mpcnet metapackage --- ocs2_mpcnet/ocs2_mpcnet/CMakeLists.txt | 4 ++++ ocs2_mpcnet/ocs2_mpcnet/package.xml | 24 ++++++++++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 ocs2_mpcnet/ocs2_mpcnet/CMakeLists.txt create mode 100644 ocs2_mpcnet/ocs2_mpcnet/package.xml diff --git a/ocs2_mpcnet/ocs2_mpcnet/CMakeLists.txt b/ocs2_mpcnet/ocs2_mpcnet/CMakeLists.txt new file mode 100644 index 000000000..a9dba76cb --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_mpcnet) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/ocs2_mpcnet/ocs2_mpcnet/package.xml b/ocs2_mpcnet/ocs2_mpcnet/package.xml new file mode 100644 index 000000000..d1402ef3c --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet/package.xml @@ -0,0 +1,24 @@ +<?xml version="1.0"?> +<package format="2"> + <name>ocs2_mpcnet</name> + <version>0.0.1</version> + <description>The ocs2_mpcnet metapackage</description> + + <author email="areske@ethz.ch">Alexander Reske</author> + + <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer> + <maintainer email="areske@ethz.ch">Alexander Reske</maintainer> + + <license>BSD-3</license> + + <buildtool_depend>catkin</buildtool_depend> + + <exec_depend>ocs2_ballbot_mpcnet</exec_depend> + <exec_depend>ocs2_legged_robot_mpcnet</exec_depend> + <exec_depend>ocs2_mpcnet_core</exec_depend> + + <export> + <metapackage /> + </export> + +</package> From 10d49104dd737353ae6e143b2470bfa0ab2afbd1 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 18:06:35 +0200 Subject: [PATCH 185/512] unify version info for mpcnet packages --- ocs2_mpcnet/ocs2_mpcnet/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet/package.xml b/ocs2_mpcnet/ocs2_mpcnet/package.xml index d1402ef3c..7cb3afff8 100644 --- a/ocs2_mpcnet/ocs2_mpcnet/package.xml +++ b/ocs2_mpcnet/ocs2_mpcnet/package.xml @@ -1,7 +1,7 @@ <?xml version="1.0"?> <package format="2"> <name>ocs2_mpcnet</name> - <version>0.0.1</version> + <version>0.0.0</version> <description>The ocs2_mpcnet metapackage</description> <author email="areske@ethz.ch">Alexander Reske</author> From 823ad1bb42d1a6021ce39e697a8f84a32b633f3f Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 18:34:59 +0200 Subject: [PATCH 186/512] make expert classes internal --- .../policy/mixture_of_linear_experts.py | 10 +++++----- .../policy/mixture_of_nonlinear_experts.py | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py index 70602c0d8..20441a27a 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py @@ -41,7 +41,7 @@ class MixtureOfLinearExpertsPolicy(torch.nn.Module): """Mixture of linear experts policy. - Class for a mixture of experts neural network with linear experts. + Class for a mixture of experts neural network policy with linear experts. Attributes: name: A string with the name of the policy. @@ -72,7 +72,7 @@ def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None self.gating_net = torch.nn.Sequential(torch.nn.Linear(self.dim_in, self.num_experts), torch.nn.Softmax(dim=1)) # experts self.expert_nets = torch.nn.ModuleList( - [LinearExpert(i, self.dim_in, self.dim_out) for i in range(self.num_experts)] + [_LinearExpert(i, self.dim_in, self.dim_out) for i in range(self.num_experts)] ) def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: @@ -94,7 +94,7 @@ def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch return u, p -class LinearExpert(torch.nn.Module): +class _LinearExpert(torch.nn.Module): """Linear expert. Class for a simple linear neural network expert. @@ -107,9 +107,9 @@ class LinearExpert(torch.nn.Module): """ def __init__(self, index: int, dim_in: int, dim_out: int) -> None: - """Initializes the LinearExpert class. + """Initializes the _LinearExpert class. - Initializes the LinearExpert class by setting fixed and variable attributes. + Initializes the _LinearExpert class by setting fixed and variable attributes. Args: index: An integer with the index of the expert. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py index 0395d65b7..e957cd4ee 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py @@ -41,7 +41,7 @@ class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): """Mixture of nonlinear experts policy. - Class for a mixture of experts neural network with nonlinear experts, where the hidden layer is the mean of the + Class for a mixture of experts neural network policy with nonlinear experts, where the hidden layer is the mean of the input and output layer. Attributes: @@ -83,7 +83,7 @@ def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None ) # experts self.expert_nets = torch.nn.ModuleList( - [NonlinearExpert(i, self.dim_in, self.dim_hidden_expert, self.dim_out) for i in range(self.num_experts)] + [_NonlinearExpert(i, self.dim_in, self.dim_hidden_expert, self.dim_out) for i in range(self.num_experts)] ) def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: @@ -105,7 +105,7 @@ def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch return u, p -class NonlinearExpert(torch.nn.Module): +class _NonlinearExpert(torch.nn.Module): """Nonlinear expert. Class for a simple nonlinear neural network expert, where the hidden layer is the mean of the input and output layer. @@ -121,9 +121,9 @@ class NonlinearExpert(torch.nn.Module): """ def __init__(self, index: int, dim_in: int, dim_hidden: int, dim_out: int) -> None: - """Initializes the NonlinearExpert class. + """Initializes the _NonlinearExpert class. - Initializes the NonlinearExpert class by setting fixed and variable attributes. + Initializes the _NonlinearExpert class by setting fixed and variable attributes. Args: index: An integer with the index of the expert. From a41f9471ac0c719a42b4c870fab0b45ea8559787 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 31 Mar 2022 18:41:01 +0200 Subject: [PATCH 187/512] fix docstrings line length --- .../ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py | 4 ++-- .../python/ocs2_mpcnet_core/policy/nonlinear.py | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py index e957cd4ee..2cbb5d8d3 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py @@ -41,8 +41,8 @@ class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): """Mixture of nonlinear experts policy. - Class for a mixture of experts neural network policy with nonlinear experts, where the hidden layer is the mean of the - input and output layer. + Class for a mixture of experts neural network policy with nonlinear experts, where the hidden layer is the mean of + the input and output layer. Attributes: name: A string with the name of the policy. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py index 8276b1d74..7e28d678c 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py @@ -38,7 +38,8 @@ class NonlinearPolicy(torch.nn.Module): """Nonlinear policy. - Class for a simple nonlinear neural network policy, where the hidden layer is the mean of the input and output layer. + Class for a simple nonlinear neural network policy, where the hidden layer is the mean of the input and output + layer. Attributes: name: A string with the name of the policy. From 5c185c6a5bfa35d7f1059b7a68ea908c82987da1 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Apr 2022 10:25:42 +0200 Subject: [PATCH 188/512] adapt ocs2 mpcnet core to changes in main --- ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp | 3 ++- .../ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp index 96ab54ead..80e138143 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp @@ -117,11 +117,12 @@ void MpcnetDummyLoopRos::rollout(scalar_t duration, const SystemObservation& ini // start of while loop while (time <= initialSystemObservation.time + duration) { // forward simulate system + ModeSchedule modeSchedule = rosReferenceManagerPtr_->getModeSchedule(); scalar_array_t timeTrajectory; size_array_t postEventIndicesStock; vector_array_t stateTrajectory; vector_array_t inputTrajectory; - rolloutPtr_->run(time, state, time + timeStep, mpcnetPtr_.get(), {}, timeTrajectory, postEventIndicesStock, stateTrajectory, + rolloutPtr_->run(time, state, time + timeStep, mpcnetPtr_.get(), modeSchedule, timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); // update time, state and input diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp index 1414dfdb9..85fbbaa4a 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp @@ -81,8 +81,8 @@ void MpcnetRolloutBase::step(scalar_t timeStep) { vector_array_t stateTrajectory; vector_array_t inputTrajectory; rolloutPtr_->run(primalSolution_.timeTrajectory_.front(), primalSolution_.stateTrajectory_.front(), - primalSolution_.timeTrajectory_.front() + timeStep, behavioralControllerPtr_.get(), - primalSolution_.modeSchedule_.eventTimes, timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); + primalSolution_.timeTrajectory_.front() + timeStep, behavioralControllerPtr_.get(), primalSolution_.modeSchedule_, + timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); // update system observation systemObservation_.time = timeTrajectory.back(); From 4d5ed38abe2e745e71c02bf1f3f8e6c933ca1fa8 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Apr 2022 11:03:18 +0200 Subject: [PATCH 189/512] rename ballbot python files --- .../ocs2_ballbot_mpcnet/{ballbot_config.py => config.py} | 0 .../ocs2_ballbot_mpcnet/{ballbot_helper.py => helper.py} | 3 ++- .../ocs2_ballbot_mpcnet/{ballbot_mpcnet.py => mpcnet.py} | 4 ++-- 3 files changed, 4 insertions(+), 3 deletions(-) rename ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/{ballbot_config.py => config.py} (100%) rename ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/{ballbot_helper.py => helper.py} (99%) rename ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/{ballbot_mpcnet.py => mpcnet.py} (98%) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py similarity index 100% rename from ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_config.py rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py similarity index 99% rename from ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py index 6181d212a..4caf53fa4 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_helper.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py @@ -37,7 +37,8 @@ from ocs2_mpcnet_core import helper from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray -from ocs2_ballbot_mpcnet import ballbot_config as config + +from ocs2_ballbot_mpcnet import config def get_default_event_times_and_mode_sequence(duration: float) -> Tuple[np.ndarray, np.ndarray]: diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py similarity index 98% rename from ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py rename to ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index 22dd90c08..822a28544 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/ballbot_mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -47,8 +47,8 @@ from ocs2_mpcnet_core.memory.circular import CircularMemory as Memory from ocs2_mpcnet_core.policy.linear import LinearPolicy as Policy -from ocs2_ballbot_mpcnet import ballbot_config as config -from ocs2_ballbot_mpcnet import ballbot_helper as helper +from ocs2_ballbot_mpcnet import config +from ocs2_ballbot_mpcnet import helper from ocs2_ballbot_mpcnet import MpcnetInterface # settings for data generation by applying behavioral policy From 812e7a051aa171b839872faec9e95648ce5eaf3f Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Apr 2022 11:10:44 +0200 Subject: [PATCH 190/512] rename legged robot python files --- .../{legged_robot_config.py => config.py} | 0 .../{legged_robot_helper.py => helper.py} | 3 ++- .../{legged_robot_mpcnet.py => mpcnet.py} | 6 +++--- .../{legged_robot_policy.py => policy.py} | 2 +- 4 files changed, 6 insertions(+), 5 deletions(-) rename ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/{legged_robot_config.py => config.py} (100%) rename ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/{legged_robot_helper.py => helper.py} (99%) rename ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/{legged_robot_mpcnet.py => mpcnet.py} (98%) rename ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/{legged_robot_policy.py => policy.py} (98%) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py similarity index 100% rename from ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_config.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py similarity index 99% rename from ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py index 958873ef6..9e7459b1a 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_helper.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py @@ -37,7 +37,8 @@ from ocs2_mpcnet_core import helper from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray -from ocs2_legged_robot_mpcnet import legged_robot_config as config + +from ocs2_legged_robot_mpcnet import config def get_stance(duration: float) -> Tuple[np.ndarray, np.ndarray]: diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py similarity index 98% rename from ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index 21a51cc59..bd37d94cb 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -48,9 +48,9 @@ from ocs2_mpcnet_core.loss.cross_entropy import CrossEntropyLoss as GatingLoss from ocs2_mpcnet_core.memory.circular import CircularMemory as Memory -from ocs2_legged_robot_mpcnet.legged_robot_policy import LeggedRobotMixtureOfNonlinearExpertsPolicy as Policy -from ocs2_legged_robot_mpcnet import legged_robot_config as config -from ocs2_legged_robot_mpcnet import legged_robot_helper as helper +from ocs2_legged_robot_mpcnet.policy import LeggedRobotMixtureOfNonlinearExpertsPolicy as Policy +from ocs2_legged_robot_mpcnet import config +from ocs2_legged_robot_mpcnet import helper from ocs2_legged_robot_mpcnet import MpcnetInterface # settings for data generation by applying behavioral policy diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py similarity index 98% rename from ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py rename to ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py index cad6f8801..eee3971ad 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/legged_robot_policy.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py @@ -40,7 +40,7 @@ from ocs2_mpcnet_core.policy import linear, mixture_of_linear_experts, mixture_of_nonlinear_experts, nonlinear from ocs2_mpcnet_core.helper import bmv, bmm -from ocs2_legged_robot_mpcnet import legged_robot_config as config +from ocs2_legged_robot_mpcnet import config input_scaling = torch.tensor(config.INPUT_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) From 3d81f0240f91ae3a2817b64c108050957d15f0b3 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Apr 2022 12:20:17 +0200 Subject: [PATCH 191/512] improve readability and documentation --- .../python/ocs2_legged_robot_mpcnet/config.py | 136 ++++++------------ 1 file changed, 40 insertions(+), 96 deletions(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py index 390437ed6..b0d36365b 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py @@ -64,119 +64,63 @@ EXPERT_NUM = 3 # default state +# fmt: off DEFAULT_STATE = [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.575, - 0.0, - 0.0, - 0.0, - -0.25, - 0.6, - -0.85, - -0.25, - -0.6, - 0.85, - 0.25, - 0.6, - -0.85, - 0.25, - -0.6, - 0.85, + 0.0, 0.0, 0.0, # normalized linear momentum + 0.0, 0.0, 0.0, # normalized angular momentum + 0.0, 0.0, 0.575, # position + 0.0, 0.0, 0.0, # orientation + -0.25, 0.6, -0.85, # joint positions LF + -0.25, -0.6, 0.85, # joint positions LH + 0.25, 0.6, -0.85, # joint positions RF + 0.25, -0.6, 0.85 # joint positions RH ] +# fmt: on # input bias +# fmt: off INPUT_BIAS = [ - 0.0, - 0.0, - 127.861, - 0.0, - 0.0, - 127.861, - 0.0, - 0.0, - 127.861, - 0.0, - 0.0, - 127.861, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, + 0.0, 0.0, 127.861, # contact forces LF + 0.0, 0.0, 127.861, # contact forces LH + 0.0, 0.0, 127.861, # contact forces RF + 0.0, 0.0, 127.861, # contact forces RH + 0.0, 0.0, 0.0, # joint velocities LF + 0.0, 0.0, 0.0, # joint velocities LH + 0.0, 0.0, 0.0, # joint velocities RF + 0.0, 0.0, 0.0 # joint velocities RH ] +# fmt: on # input scaling +# fmt: off INPUT_SCALING = [ - 100.0, - 100.0, - 100.0, - 100.0, - 100.0, - 100.0, - 100.0, - 100.0, - 100.0, - 100.0, - 100.0, - 100.0, - 10.0, - 10.0, - 10.0, - 10.0, - 10.0, - 10.0, - 10.0, - 10.0, - 10.0, - 10.0, - 10.0, - 10.0, + 100.0, 100.0, 100.0, # contact forces LF + 100.0, 100.0, 100.0, # contact forces LH + 100.0, 100.0, 100.0, # contact forces RF + 100.0, 100.0, 100.0, # contact forces RH + 10.0, 10.0, 10.0, # joint velocities LF + 10.0, 10.0, 10.0, # joint velocities LH + 10.0, 10.0, 10.0, # joint velocities RF + 10.0, 10.0, 10.0, # joint velocities RH ] +# fmt: on # (diagonally dominant) nominal centroidal inertia normalized by robot mass NORMALIZED_INERTIA = [1.62079 / 52.1348, 4.83559 / 52.1348, 4.72382 / 52.1348] # input cost for behavioral cloning +# fmt: off R = [ - 0.001, - 0.001, - 0.001, - 0.001, - 0.001, - 0.001, - 0.001, - 0.001, - 0.001, - 0.001, - 0.001, - 0.001, - 5.0, - 5.0, - 5.0, - 5.0, - 5.0, - 5.0, - 5.0, - 5.0, - 5.0, - 5.0, - 5.0, - 5.0, + 0.001, 0.001, 0.001, # contact forces LF + 0.001, 0.001, 0.001, # contact forces LH + 0.001, 0.001, 0.001, # contact forces RF + 0.001, 0.001, 0.001, # contact forces RH + 5.0, 5.0, 5.0, # joint velocities LF + 5.0, 5.0, 5.0, # joint velocities LH + 5.0, 5.0, 5.0, # joint velocities RF + 5.0, 5.0, 5.0, # joint velocities RH ] +# fmt: on # dictionary for cheating EXPERT_FOR_MODE = dict([(i, None) for i in range(16)]) From 9e404b6e681d6c80f8cea94accec2d62ec869d54 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Apr 2022 12:32:38 +0200 Subject: [PATCH 192/512] improve documentation for cheating --- .../python/ocs2_legged_robot_mpcnet/config.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py index b0d36365b..9e54f92a1 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py @@ -122,7 +122,8 @@ ] # fmt: on -# dictionary for cheating +# dictionary for cheating with the gating loss +# assigns each of the OCS2 modes to an expert that is responsible for covering the corresponding contact configuration EXPERT_FOR_MODE = dict([(i, None) for i in range(16)]) EXPERT_FOR_MODE[15] = 0 # stance EXPERT_FOR_MODE[6] = 1 # trot From 4d0e39a1269713f5943849c431381180231c5c09 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Apr 2022 13:42:03 +0200 Subject: [PATCH 193/512] add name guard to the main script --- .../python/ocs2_ballbot_mpcnet/mpcnet.py | 429 +++++++++--------- 1 file changed, 216 insertions(+), 213 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index 822a28544..3295bcbed 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -51,218 +51,221 @@ from ocs2_ballbot_mpcnet import helper from ocs2_ballbot_mpcnet import MpcnetInterface -# settings for data generation by applying behavioral policy -data_generation_time_step = 0.1 -data_generation_duration = 3.0 -data_generation_data_decimation = 1 -data_generation_n_threads = 2 -data_generation_n_tasks = 10 -data_generation_n_samples = 2 -data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order="F") -for i in range(0, 2): - data_generation_sampling_covariance[i, i] = 0.01**2 # position -for i in range(2, 5): - data_generation_sampling_covariance[i, i] = (1.0 * np.pi / 180.0) ** 2 # orientation -for i in range(5, 7): - data_generation_sampling_covariance[i, i] = 0.05**2 # linear velocity -for i in range(7, 10): - data_generation_sampling_covariance[i, i] = (5.0 * np.pi / 180.0) ** 2 # angular velocity - -# settings for computing metrics by applying learned policy -policy_evaluation_time_step = 0.1 -policy_evaluation_duration = 3.0 -policy_evaluation_n_threads = 1 -policy_evaluation_n_tasks = 5 - -# rollout settings for data generation and policy evaluation -raisim = False - -# mpcnet interface -mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads, raisim) - -# logging -description = "description" -folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + description -writer = SummaryWriter("runs/" + folder) -os.makedirs(name="policies/" + folder) - -# loss -loss = Loss() - -# memory -memory_capacity = 100000 -memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) - -# policy -policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) -policy.to(config.DEVICE) -print("Initial policy parameters:") -print(list(policy.named_parameters())) -dummy_input = ( - torch.randn(1, config.TIME_DIM, device=config.DEVICE, dtype=config.DTYPE), - torch.randn(1, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE), -) -print("Saving initial policy.") -save_path = "policies/" + folder + "/initial_policy" -torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") -torch.save(obj=policy, f=save_path + ".pt") - -# optimizer -batch_size = 2**5 -learning_rate = 1e-2 -learning_iterations = 10000 -optimizer = torch.optim.Adam(policy.parameters(), lr=learning_rate) - - -def start_data_generation(policy, alpha=1.0): - policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - data_generation_n_tasks, data_generation_duration +if __name__ == "__main__": + # settings for data generation by applying behavioral policy + data_generation_time_step = 0.1 + data_generation_duration = 3.0 + data_generation_data_decimation = 1 + data_generation_n_threads = 2 + data_generation_n_tasks = 10 + data_generation_n_samples = 2 + data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order="F") + for i in range(0, 2): + data_generation_sampling_covariance[i, i] = 0.01**2 # position + for i in range(2, 5): + data_generation_sampling_covariance[i, i] = (1.0 * np.pi / 180.0) ** 2 # orientation + for i in range(5, 7): + data_generation_sampling_covariance[i, i] = 0.05**2 # linear velocity + for i in range(7, 10): + data_generation_sampling_covariance[i, i] = (5.0 * np.pi / 180.0) ** 2 # angular velocity + + # settings for computing metrics by applying learned policy + policy_evaluation_time_step = 0.1 + policy_evaluation_duration = 3.0 + policy_evaluation_n_threads = 1 + policy_evaluation_n_tasks = 5 + + # rollout settings for data generation and policy evaluation + raisim = False + + # mpcnet interface + mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads, raisim) + + # logging + description = "description" + folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + description + writer = SummaryWriter("runs/" + folder) + os.makedirs(name="policies/" + folder) + + # loss + loss = Loss() + + # memory + memory_capacity = 100000 + memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) + + # policy + policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) + policy.to(config.DEVICE) + print("Initial policy parameters:") + print(list(policy.named_parameters())) + dummy_input = ( + torch.randn(1, config.TIME_DIM, device=config.DEVICE, dtype=config.DTYPE), + torch.randn(1, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE), ) - mpcnet_interface.startDataGeneration( - alpha, - policy_file_path, - data_generation_time_step, - data_generation_data_decimation, - data_generation_n_samples, - data_generation_sampling_covariance, - initial_observations, - mode_schedules, - target_trajectories, - ) - - -def start_policy_evaluation(policy, alpha=0.0): - policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - policy_evaluation_n_tasks, policy_evaluation_duration - ) - mpcnet_interface.startPolicyEvaluation( - alpha, policy_file_path, policy_evaluation_time_step, initial_observations, mode_schedules, target_trajectories - ) - - -try: - print("==============\nWaiting for first data.\n==============") - start_data_generation(policy) - start_policy_evaluation(policy) - while not mpcnet_interface.isDataGenerationDone(): - time.sleep(1.0) - - print("==============\nStarting training.\n==============") - for iteration in range(learning_iterations): - alpha = 1.0 - 1.0 * iteration / learning_iterations - - # data generation - if mpcnet_interface.isDataGenerationDone(): - # get generated data - data = mpcnet_interface.getGeneratedData() - for i in range(len(data)): - # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory - memory.push( - data[i].t, - data[i].x, - data[i].u, - torch.ones(1, device=config.DEVICE, dtype=config.DTYPE), - data[i].generalizedTime, - data[i].relativeState, - data[i].inputTransformation, - data[i].hamiltonian, + print("Saving initial policy.") + save_path = "policies/" + folder + "/initial_policy" + torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.save(obj=policy, f=save_path + ".pt") + + # optimizer + batch_size = 2**5 + learning_rate = 1e-2 + learning_iterations = 10000 + optimizer = torch.optim.Adam(policy.parameters(), lr=learning_rate) + + def start_data_generation(policy, alpha=1.0): + policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks( + data_generation_n_tasks, data_generation_duration + ) + mpcnet_interface.startDataGeneration( + alpha, + policy_file_path, + data_generation_time_step, + data_generation_data_decimation, + data_generation_n_samples, + data_generation_sampling_covariance, + initial_observations, + mode_schedules, + target_trajectories, + ) + + def start_policy_evaluation(policy, alpha=0.0): + policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks( + policy_evaluation_n_tasks, policy_evaluation_duration + ) + mpcnet_interface.startPolicyEvaluation( + alpha, + policy_file_path, + policy_evaluation_time_step, + initial_observations, + mode_schedules, + target_trajectories, + ) + + try: + print("==============\nWaiting for first data.\n==============") + start_data_generation(policy) + start_policy_evaluation(policy) + while not mpcnet_interface.isDataGenerationDone(): + time.sleep(1.0) + + print("==============\nStarting training.\n==============") + for iteration in range(learning_iterations): + alpha = 1.0 - 1.0 * iteration / learning_iterations + + # data generation + if mpcnet_interface.isDataGenerationDone(): + # get generated data + data = mpcnet_interface.getGeneratedData() + for i in range(len(data)): + # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory + memory.push( + data[i].t, + data[i].x, + data[i].u, + torch.ones(1, device=config.DEVICE, dtype=config.DTYPE), + data[i].generalizedTime, + data[i].relativeState, + data[i].inputTransformation, + data[i].hamiltonian, + ) + # logging + writer.add_scalar("data/new_data_points", len(data), iteration) + writer.add_scalar("data/total_data_points", len(memory), iteration) + print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) + # start new data generation + start_data_generation(policy, alpha) + + # policy evaluation + if mpcnet_interface.isPolicyEvaluationDone(): + # get computed metrics + metrics = mpcnet_interface.getComputedMetrics() + survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) + incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) + # logging + writer.add_scalar("metric/survival_time", survival_time, iteration) + writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) + print( + "iteration", + iteration, + "received metrics:", + "incurred_hamiltonian", + incurred_hamiltonian, + "survival_time", + survival_time, ) - # logging - writer.add_scalar("data/new_data_points", len(data), iteration) - writer.add_scalar("data/total_data_points", len(memory), iteration) - print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) - # start new data generation - start_data_generation(policy, alpha) - - # policy evaluation - if mpcnet_interface.isPolicyEvaluationDone(): - # get computed metrics - metrics = mpcnet_interface.getComputedMetrics() - survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) - incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) - # logging - writer.add_scalar("metric/survival_time", survival_time, iteration) - writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) - print( - "iteration", - iteration, - "received metrics:", - "incurred_hamiltonian", - incurred_hamiltonian, - "survival_time", - survival_time, - ) - # start new policy evaluation - start_policy_evaluation(policy) - - # intermediate policies - if (iteration % 1000 == 0) and (iteration > 0): - print("Saving intermediate policy for iteration", iteration) - save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) - torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") - torch.save(obj=policy, f=save_path + ".pt") - - # extract batch from memory - ( - t, - x, - u, - p, - generalized_time, - relative_state, - input_transformation, - dHdxx, - dHdux, - dHduu, - dHdx, - dHdu, - H, - ) = memory.sample(batch_size) - - # take an optimization step - def closure(): - # clear the gradients - optimizer.zero_grad() - # prediction - u_predicted = policy(generalized_time, relative_state) - u_predicted = bmv(input_transformation, u_predicted) - # compute the empirical loss - empirical_loss = loss(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) - # compute the gradients - empirical_loss.backward() - # logging - writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) - # return empirical loss - return empirical_loss - - optimizer.step(closure) - - # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) - if iteration == learning_iterations - 1: - while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): - time.sleep(1.0) - - print("==============\nTraining completed.\n==============") - -except KeyboardInterrupt: - # let data generation and policy evaluation finish (to avoid a segmentation fault) - while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): - time.sleep(1.0) - print("==============\nTraining interrupted.\n==============") - pass - -print("Final policy parameters:") -print(list(policy.named_parameters())) - -print("Saving final policy.") -save_path = "policies/" + folder + "/final_policy" -torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") -torch.save(obj=policy, f=save_path + ".pt") - -writer.close() - -print("Done. Exiting now.") + # start new policy evaluation + start_policy_evaluation(policy) + + # intermediate policies + if (iteration % 1000 == 0) and (iteration > 0): + print("Saving intermediate policy for iteration", iteration) + save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) + torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.save(obj=policy, f=save_path + ".pt") + + # extract batch from memory + ( + t, + x, + u, + p, + generalized_time, + relative_state, + input_transformation, + dHdxx, + dHdux, + dHduu, + dHdx, + dHdu, + H, + ) = memory.sample(batch_size) + + # take an optimization step + def closure(): + # clear the gradients + optimizer.zero_grad() + # prediction + u_predicted = policy(generalized_time, relative_state) + u_predicted = bmv(input_transformation, u_predicted) + # compute the empirical loss + empirical_loss = loss(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + # compute the gradients + empirical_loss.backward() + # logging + writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) + # return empirical loss + return empirical_loss + + optimizer.step(closure) + + # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) + if iteration == learning_iterations - 1: + while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): + time.sleep(1.0) + + print("==============\nTraining completed.\n==============") + + except KeyboardInterrupt: + # let data generation and policy evaluation finish (to avoid a segmentation fault) + while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): + time.sleep(1.0) + print("==============\nTraining interrupted.\n==============") + pass + + print("Final policy parameters:") + print(list(policy.named_parameters())) + + print("Saving final policy.") + save_path = "policies/" + folder + "/final_policy" + torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.save(obj=policy, f=save_path + ".pt") + + writer.close() + + print("Done. Exiting now.") From 42ed62c5f109eaf33d58b1fa5fbf65dd06fccbc0 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Apr 2022 13:45:37 +0200 Subject: [PATCH 194/512] add name guard to the main script --- .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 473 +++++++++--------- 1 file changed, 238 insertions(+), 235 deletions(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index bd37d94cb..bd62fa720 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -53,242 +53,245 @@ from ocs2_legged_robot_mpcnet import helper from ocs2_legged_robot_mpcnet import MpcnetInterface -# settings for data generation by applying behavioral policy -data_generation_time_step = 0.0025 -data_generation_duration = 4.0 -data_generation_data_decimation = 4 -data_generation_n_threads = 12 -data_generation_n_tasks = 12 -data_generation_n_samples = 2 -data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order="F") -for i in range(0, 3): - data_generation_sampling_covariance[i, i] = 0.05**2 # normalized linear momentum -for i in range(3, 6): - data_generation_sampling_covariance[i, i] = ( - config.NORMALIZED_INERTIA[i - 3] * 2.5 * np.pi / 180.0 - ) ** 2 # normalized angular momentum -for i in range(6, 9): - data_generation_sampling_covariance[i, i] = 0.01**2 # position -for i in range(9, 12): - data_generation_sampling_covariance[i, i] = (0.5 * np.pi / 180.0) ** 2 # orientation -for i in range(12, 24): - data_generation_sampling_covariance[i, i] = (0.5 * np.pi / 180.0) ** 2 # joint positions - -# settings for computing metrics by applying learned policy -policy_evaluation_time_step = 0.0025 -policy_evaluation_duration = 4.0 -policy_evaluation_n_threads = 3 -policy_evaluation_n_tasks = 3 - -# rollout settings for data generation and policy evaluation -raisim = True - -# mpcnet interface -mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads, raisim) - -# logging -description = "description" -folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + description -writer = SummaryWriter("runs/" + folder) -os.makedirs(name="policies/" + folder) - -# loss -epsilon = 1e-8 # epsilon to improve numerical stability of logs and denominators -my_lambda = 10.0 # parameter to control the relative importance of both loss types -experts_loss = ExpertsLoss() -gating_loss = GatingLoss(epsilon) - -# memory -memory_capacity = 400000 -memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) - -# policy -policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) -policy.to(config.DEVICE) -print("Initial policy parameters:") -print(list(policy.named_parameters())) -dummy_input = ( - torch.randn(1, config.TIME_DIM, device=config.DEVICE, dtype=config.DTYPE), - torch.randn(1, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE), -) -print("Saving initial policy.") -save_path = "policies/" + folder + "/initial_policy" -torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") -torch.save(obj=policy, f=save_path + ".pt") - -# optimizer -batch_size = 2**7 -learning_iterations = 100000 -learning_rate_default = 1e-3 -learning_rate_gating_net = learning_rate_default -learning_rate_expert_nets = learning_rate_default -optimizer = torch.optim.Adam( - [ - {"params": policy.gating_net.parameters(), "lr": learning_rate_gating_net}, - {"params": policy.expert_nets.parameters(), "lr": learning_rate_expert_nets}, - ], - lr=learning_rate_default, -) - -# weights for ["stance", "trot_1", "trot_2"] -weights = [1, 2, 2] - - -def start_data_generation(policy, alpha=1.0): - policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - choices = random.choices(["stance", "trot_1", "trot_2"], k=data_generation_n_tasks, weights=weights) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - data_generation_n_tasks, data_generation_duration, choices +if __name__ == "__main__": + # settings for data generation by applying behavioral policy + data_generation_time_step = 0.0025 + data_generation_duration = 4.0 + data_generation_data_decimation = 4 + data_generation_n_threads = 12 + data_generation_n_tasks = 12 + data_generation_n_samples = 2 + data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order="F") + for i in range(0, 3): + data_generation_sampling_covariance[i, i] = 0.05**2 # normalized linear momentum + for i in range(3, 6): + data_generation_sampling_covariance[i, i] = ( + config.NORMALIZED_INERTIA[i - 3] * 2.5 * np.pi / 180.0 + ) ** 2 # normalized angular momentum + for i in range(6, 9): + data_generation_sampling_covariance[i, i] = 0.01**2 # position + for i in range(9, 12): + data_generation_sampling_covariance[i, i] = (0.5 * np.pi / 180.0) ** 2 # orientation + for i in range(12, 24): + data_generation_sampling_covariance[i, i] = (0.5 * np.pi / 180.0) ** 2 # joint positions + + # settings for computing metrics by applying learned policy + policy_evaluation_time_step = 0.0025 + policy_evaluation_duration = 4.0 + policy_evaluation_n_threads = 3 + policy_evaluation_n_tasks = 3 + + # rollout settings for data generation and policy evaluation + raisim = True + + # mpcnet interface + mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads, raisim) + + # logging + description = "description" + folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + description + writer = SummaryWriter("runs/" + folder) + os.makedirs(name="policies/" + folder) + + # loss + epsilon = 1e-8 # epsilon to improve numerical stability of logs and denominators + my_lambda = 10.0 # parameter to control the relative importance of both loss types + experts_loss = ExpertsLoss() + gating_loss = GatingLoss(epsilon) + + # memory + memory_capacity = 400000 + memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) + + # policy + policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) + policy.to(config.DEVICE) + print("Initial policy parameters:") + print(list(policy.named_parameters())) + dummy_input = ( + torch.randn(1, config.TIME_DIM, device=config.DEVICE, dtype=config.DTYPE), + torch.randn(1, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE), ) - mpcnet_interface.startDataGeneration( - alpha, - policy_file_path, - data_generation_time_step, - data_generation_data_decimation, - data_generation_n_samples, - data_generation_sampling_covariance, - initial_observations, - mode_schedules, - target_trajectories, + print("Saving initial policy.") + save_path = "policies/" + folder + "/initial_policy" + torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.save(obj=policy, f=save_path + ".pt") + + # optimizer + batch_size = 2**7 + learning_iterations = 100000 + learning_rate_default = 1e-3 + learning_rate_gating_net = learning_rate_default + learning_rate_expert_nets = learning_rate_default + optimizer = torch.optim.Adam( + [ + {"params": policy.gating_net.parameters(), "lr": learning_rate_gating_net}, + {"params": policy.expert_nets.parameters(), "lr": learning_rate_expert_nets}, + ], + lr=learning_rate_default, ) - -def start_policy_evaluation(policy, alpha=0.0): - policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) - choices = random.choices(["stance", "trot_1", "trot_2"], k=policy_evaluation_n_tasks, weights=weights) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - policy_evaluation_n_tasks, policy_evaluation_duration, choices - ) - mpcnet_interface.startPolicyEvaluation( - alpha, policy_file_path, policy_evaluation_time_step, initial_observations, mode_schedules, target_trajectories - ) - - -try: - print("==============\nWaiting for first data.\n==============") - start_data_generation(policy) - start_policy_evaluation(policy) - while not mpcnet_interface.isDataGenerationDone(): - time.sleep(1.0) - - print("==============\nStarting training.\n==============") - for iteration in range(learning_iterations): - alpha = 1.0 - 1.0 * iteration / learning_iterations - - # data generation - if mpcnet_interface.isDataGenerationDone(): - # get generated data - data = mpcnet_interface.getGeneratedData() - for i in range(len(data)): - # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory - memory.push( - data[i].t, - data[i].x, - data[i].u, - helper.get_one_hot(data[i].mode), - data[i].generalizedTime, - data[i].relativeState, - data[i].inputTransformation, - data[i].hamiltonian, + # weights for ["stance", "trot_1", "trot_2"] + weights = [1, 2, 2] + + def start_data_generation(policy, alpha=1.0): + policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + choices = random.choices(["stance", "trot_1", "trot_2"], k=data_generation_n_tasks, weights=weights) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks( + data_generation_n_tasks, data_generation_duration, choices + ) + mpcnet_interface.startDataGeneration( + alpha, + policy_file_path, + data_generation_time_step, + data_generation_data_decimation, + data_generation_n_samples, + data_generation_sampling_covariance, + initial_observations, + mode_schedules, + target_trajectories, + ) + + def start_policy_evaluation(policy, alpha=0.0): + policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + choices = random.choices(["stance", "trot_1", "trot_2"], k=policy_evaluation_n_tasks, weights=weights) + initial_observations, mode_schedules, target_trajectories = helper.get_tasks( + policy_evaluation_n_tasks, policy_evaluation_duration, choices + ) + mpcnet_interface.startPolicyEvaluation( + alpha, + policy_file_path, + policy_evaluation_time_step, + initial_observations, + mode_schedules, + target_trajectories, + ) + + try: + print("==============\nWaiting for first data.\n==============") + start_data_generation(policy) + start_policy_evaluation(policy) + while not mpcnet_interface.isDataGenerationDone(): + time.sleep(1.0) + + print("==============\nStarting training.\n==============") + for iteration in range(learning_iterations): + alpha = 1.0 - 1.0 * iteration / learning_iterations + + # data generation + if mpcnet_interface.isDataGenerationDone(): + # get generated data + data = mpcnet_interface.getGeneratedData() + for i in range(len(data)): + # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory + memory.push( + data[i].t, + data[i].x, + data[i].u, + helper.get_one_hot(data[i].mode), + data[i].generalizedTime, + data[i].relativeState, + data[i].inputTransformation, + data[i].hamiltonian, + ) + # logging + writer.add_scalar("data/new_data_points", len(data), iteration) + writer.add_scalar("data/total_data_points", len(memory), iteration) + print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) + # start new data generation + start_data_generation(policy, alpha) + + # policy evaluation + if mpcnet_interface.isPolicyEvaluationDone(): + # get computed metrics + metrics = mpcnet_interface.getComputedMetrics() + survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) + incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) + # logging + writer.add_scalar("metric/survival_time", survival_time, iteration) + writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) + print( + "iteration", + iteration, + "received metrics:", + "incurred_hamiltonian", + incurred_hamiltonian, + "survival_time", + survival_time, ) - # logging - writer.add_scalar("data/new_data_points", len(data), iteration) - writer.add_scalar("data/total_data_points", len(memory), iteration) - print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) - # start new data generation - start_data_generation(policy, alpha) - - # policy evaluation - if mpcnet_interface.isPolicyEvaluationDone(): - # get computed metrics - metrics = mpcnet_interface.getComputedMetrics() - survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) - incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) - # logging - writer.add_scalar("metric/survival_time", survival_time, iteration) - writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) - print( - "iteration", - iteration, - "received metrics:", - "incurred_hamiltonian", - incurred_hamiltonian, - "survival_time", - survival_time, - ) - # start new policy evaluation - start_policy_evaluation(policy) - - # intermediate policies - if (iteration % 10000 == 0) and (iteration > 0): - print("Saving intermediate policy for iteration", iteration) - save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) - torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") - torch.save(obj=policy, f=save_path + ".pt") - - # extract batch from memory - ( - t, - x, - u, - p, - generalized_time, - relative_state, - input_transformation, - dHdxx, - dHdux, - dHduu, - dHdx, - dHdu, - H, - ) = memory.sample(batch_size) - - # take an optimization step - def closure(): - # clear the gradients - optimizer.zero_grad() - # prediction - u_predicted, p_predicted = policy(generalized_time, relative_state) - u_predicted = bmv(input_transformation, u_predicted) - # compute the empirical loss - empirical_experts_loss = experts_loss(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) - empirical_gating_loss = gating_loss(p, p_predicted) - empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss - # compute the gradients - empirical_loss.backward() - # logging - writer.add_scalar("objective/empirical_experts_loss", empirical_experts_loss.item(), iteration) - writer.add_scalar("objective/empirical_gating_loss", empirical_gating_loss.item(), iteration) - writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) - # return empirical loss - return empirical_loss - - optimizer.step(closure) - - # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) - if iteration == learning_iterations - 1: - while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): - time.sleep(1.0) - - print("==============\nTraining completed.\n==============") - -except KeyboardInterrupt: - # let data generation and policy evaluation finish (to avoid a segmentation fault) - while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): - time.sleep(1.0) - print("==============\nTraining interrupted.\n==============") - pass - -print("Final policy parameters:") -print(list(policy.named_parameters())) - -print("Saving final policy.") -save_path = "policies/" + folder + "/final_policy" -torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") -torch.save(obj=policy, f=save_path + ".pt") - -writer.close() - -print("Done. Exiting now.") + # start new policy evaluation + start_policy_evaluation(policy) + + # intermediate policies + if (iteration % 10000 == 0) and (iteration > 0): + print("Saving intermediate policy for iteration", iteration) + save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) + torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.save(obj=policy, f=save_path + ".pt") + + # extract batch from memory + ( + t, + x, + u, + p, + generalized_time, + relative_state, + input_transformation, + dHdxx, + dHdux, + dHduu, + dHdx, + dHdu, + H, + ) = memory.sample(batch_size) + + # take an optimization step + def closure(): + # clear the gradients + optimizer.zero_grad() + # prediction + u_predicted, p_predicted = policy(generalized_time, relative_state) + u_predicted = bmv(input_transformation, u_predicted) + # compute the empirical loss + empirical_experts_loss = experts_loss(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + empirical_gating_loss = gating_loss(p, p_predicted) + empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss + # compute the gradients + empirical_loss.backward() + # logging + writer.add_scalar("objective/empirical_experts_loss", empirical_experts_loss.item(), iteration) + writer.add_scalar("objective/empirical_gating_loss", empirical_gating_loss.item(), iteration) + writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) + # return empirical loss + return empirical_loss + + optimizer.step(closure) + + # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) + if iteration == learning_iterations - 1: + while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): + time.sleep(1.0) + + print("==============\nTraining completed.\n==============") + + except KeyboardInterrupt: + # let data generation and policy evaluation finish (to avoid a segmentation fault) + while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): + time.sleep(1.0) + print("==============\nTraining interrupted.\n==============") + pass + + print("Final policy parameters:") + print(list(policy.named_parameters())) + + print("Saving final policy.") + save_path = "policies/" + folder + "/final_policy" + torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.save(obj=policy, f=save_path + ".pt") + + writer.close() + + print("Done. Exiting now.") From 80447f1b87bf85f4d68c6344e7bc83f4e3d53be4 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Apr 2022 14:05:05 +0200 Subject: [PATCH 195/512] add main function --- .../python/ocs2_ballbot_mpcnet/mpcnet.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index 3295bcbed..f2d9c8803 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -51,7 +51,8 @@ from ocs2_ballbot_mpcnet import helper from ocs2_ballbot_mpcnet import MpcnetInterface -if __name__ == "__main__": + +def main(): # settings for data generation by applying behavioral policy data_generation_time_step = 0.1 data_generation_duration = 3.0 @@ -269,3 +270,7 @@ def closure(): writer.close() print("Done. Exiting now.") + + +if __name__ == '__main__': + main() From 8b5b258a19fae6df2a1367d37e53b4954e1fed48 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Mon, 4 Apr 2022 14:08:46 +0200 Subject: [PATCH 196/512] add main function --- .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index bd62fa720..90cd24340 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -53,7 +53,8 @@ from ocs2_legged_robot_mpcnet import helper from ocs2_legged_robot_mpcnet import MpcnetInterface -if __name__ == "__main__": + +def main(): # settings for data generation by applying behavioral policy data_generation_time_step = 0.0025 data_generation_duration = 4.0 @@ -295,3 +296,7 @@ def closure(): writer.close() print("Done. Exiting now.") + + +if __name__ == '__main__': + main() From 7f32e73de00d010bf1e215792f9966d72500ecd4 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 7 Apr 2022 12:04:38 +0200 Subject: [PATCH 197/512] fix small mistake in docstring --- .../ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py index 95d350d04..152415ad3 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py @@ -69,7 +69,7 @@ def __init__( ) -> None: """Initializes the CircularMemory class. - Initializes the BehavioralCloning class by setting fixed attributes, initializing variable attributes and + Initializes the CircularMemory class by setting fixed attributes, initializing variable attributes and pre-allocating memory. Args: From af3e8d6c7ff3763ce29fad4072845f5fb8587bbe Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 7 Apr 2022 18:12:35 +0200 Subject: [PATCH 198/512] small formating fix --- .../ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py | 2 +- .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index f2d9c8803..2cf0e5f36 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -272,5 +272,5 @@ def closure(): print("Done. Exiting now.") -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index 90cd24340..278a0d5b9 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -298,5 +298,5 @@ def closure(): print("Done. Exiting now.") -if __name__ == '__main__': +if __name__ == "__main__": main() From 913167389b6d01d0a398eb3d6f0128fdd9a5abad Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 7 Apr 2022 20:29:05 +0200 Subject: [PATCH 199/512] switch to observation and action transformation --- .../BallbotMpcnetDefinition.h | 19 ++-- .../python/ocs2_ballbot_mpcnet/config.py | 12 +++ .../python/ocs2_ballbot_mpcnet/helper.py | 6 +- .../python/ocs2_ballbot_mpcnet/mpcnet.py | 38 ++++---- .../src/BallbotMpcnetDefinition.cpp | 24 ++--- .../LeggedRobotMpcnetDefinition.h | 19 ++-- .../python/ocs2_legged_robot_mpcnet/config.py | 12 +++ .../python/ocs2_legged_robot_mpcnet/helper.py | 16 ++-- .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 47 +++++----- .../python/ocs2_legged_robot_mpcnet/policy.py | 48 +++++----- .../src/LeggedRobotMpcnetDefinition.cpp | 46 ++++++---- .../ocs2_mpcnet_core/MpcnetDefinitionBase.h | 33 +++---- .../ocs2_mpcnet_core/MpcnetPybindMacros.h | 5 +- .../ocs2_mpcnet_core/rollout/MpcnetData.h | 17 ++-- .../ocs2_mpcnet_core/memory/circular.py | 71 ++++++++------- .../python/ocs2_mpcnet_core/policy/linear.py | 26 +++--- .../policy/mixture_of_linear_experts.py | 60 ++++++------- .../policy/mixture_of_nonlinear_experts.py | 87 ++++++++++--------- .../ocs2_mpcnet_core/policy/nonlinear.py | 36 ++++---- .../src/control/MpcnetOnnxController.cpp | 28 +++--- .../src/rollout/MpcnetRolloutBase.cpp | 7 +- 21 files changed, 345 insertions(+), 312 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h b/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h index 21a654c25..e50cf779f 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h @@ -50,24 +50,21 @@ class BallbotMpcnetDefinition final : public ocs2::mpcnet::MpcnetDefinitionBase ~BallbotMpcnetDefinition() override = default; /** - * @see MpcnetDefinitionBase::getGeneralizedTime + * @see MpcnetDefinitionBase::getObservation */ - vector_t getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) override; + vector_t getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) override; /** - * @see MpcnetDefinitionBase::getRelativeState + * @see MpcnetDefinitionBase::getActionTransformation */ - vector_t getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) override; + std::pair<matrix_t, vector_t> getActionTransformation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) override; /** - * @see MpcnetDefinitionBase::getInputTransformation + * @see MpcnetDefinitionBase::isValid */ - matrix_t getInputTransformation(scalar_t t, const vector_t& x) override; - - /** - * @see MpcnetDefinitionBase::validState - */ - bool validState(const vector_t& x) override; + bool isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) override; }; } // namespace ballbot diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py index 794e872da..f2b7b94e2 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py @@ -60,5 +60,17 @@ # input dimension INPUT_DIM = 3 +# target trajectories state dimension +TARGET_STATE_DIM = STATE_DIM + +# target trajectories input dimension +TARGET_INPUT_DIM = INPUT_DIM + +# observation dimension +OBSERVATION_DIM = STATE_DIM + +# action dimension +ACTION_DIM = INPUT_DIM + # input cost for behavioral cloning R = [2.0, 2.0, 2.0] diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py index 4caf53fa4..42fe99bec 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py @@ -92,7 +92,7 @@ def get_random_target_state() -> np.ndarray: max_position_x = 1.0 max_position_y = 1.0 max_orientation_z = 45.0 * np.pi / 180.0 - random_state = np.zeros(config.STATE_DIM) + random_state = np.zeros(config.TARGET_STATE_DIM) random_state[0] = np.random.uniform(-max_position_x, max_position_x) random_state[1] = np.random.uniform(-max_position_y, max_position_y) random_state[2] = np.random.uniform(-max_orientation_z, max_orientation_z) @@ -126,7 +126,7 @@ def get_tasks( mode_schedules[i] = helper.get_mode_schedule(*get_default_event_times_and_mode_sequence(duration)) target_trajectories[i] = helper.get_target_trajectories( duration * np.ones((1, 1)), - get_random_target_state().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM)), + get_random_target_state().reshape((1, config.TARGET_STATE_DIM)), + np.zeros((1, config.TARGET_INPUT_DIM)), ) return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index 2cf0e5f36..ed2786f70 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -93,20 +93,17 @@ def main(): # memory memory_capacity = 100000 - memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) + memory = Memory(memory_capacity, config.STATE_DIM, config.INPUT_DIM, config.OBSERVATION_DIM, config.ACTION_DIM) # policy - policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM) + policy = Policy(config.OBSERVATION_DIM, config.ACTION_DIM) policy.to(config.DEVICE) print("Initial policy parameters:") print(list(policy.named_parameters())) - dummy_input = ( - torch.randn(1, config.TIME_DIM, device=config.DEVICE, dtype=config.DTYPE), - torch.randn(1, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE), - ) + dummy_observation = torch.randn(1, config.OBSERVATION_DIM, device=config.DEVICE, dtype=config.DTYPE) print("Saving initial policy.") save_path = "policies/" + folder + "/initial_policy" - torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") # optimizer @@ -117,7 +114,7 @@ def main(): def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( data_generation_n_tasks, data_generation_duration ) @@ -135,7 +132,7 @@ def start_data_generation(policy, alpha=1.0): def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( policy_evaluation_n_tasks, policy_evaluation_duration ) @@ -164,15 +161,14 @@ def start_policy_evaluation(policy, alpha=0.0): # get generated data data = mpcnet_interface.getGeneratedData() for i in range(len(data)): - # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory + # push t, x, u, p, observation, action transformation, Hamiltonian into memory memory.push( data[i].t, data[i].x, data[i].u, torch.ones(1, device=config.DEVICE, dtype=config.DTYPE), - data[i].generalizedTime, - data[i].relativeState, - data[i].inputTransformation, + data[i].observation, + data[i].actionTransformation, data[i].hamiltonian, ) # logging @@ -207,7 +203,7 @@ def start_policy_evaluation(policy, alpha=0.0): if (iteration % 1000 == 0) and (iteration > 0): print("Saving intermediate policy for iteration", iteration) save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) - torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") # extract batch from memory @@ -216,9 +212,9 @@ def start_policy_evaluation(policy, alpha=0.0): x, u, p, - generalized_time, - relative_state, - input_transformation, + observation, + action_transformation_matrix, + action_transformation_vector, dHdxx, dHdux, dHduu, @@ -232,10 +228,10 @@ def closure(): # clear the gradients optimizer.zero_grad() # prediction - u_predicted = policy(generalized_time, relative_state) - u_predicted = bmv(input_transformation, u_predicted) + action = policy(observation) + input = bmv(action_transformation_matrix, action) + action_transformation_vector # compute the empirical loss - empirical_loss = loss(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + empirical_loss = loss(x, x, input, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) # compute the gradients empirical_loss.backward() # logging @@ -264,7 +260,7 @@ def closure(): print("Saving final policy.") save_path = "policies/" + folder + "/final_policy" - torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") writer.close() diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp index 87c63435a..81ec0bc06 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp @@ -32,24 +32,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace ballbot { -vector_t BallbotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) { - return vector_t::Zero(1); -} - -vector_t BallbotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { - vector_t relativeState = x - targetTrajectories.getDesiredState(t); +vector_t BallbotMpcnetDefinition::getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + vector_t observation = x - targetTrajectories.getDesiredState(t); const Eigen::Matrix<scalar_t, 2, 2> R = (Eigen::Matrix<scalar_t, 2, 2>() << cos(x(2)), -sin(x(2)), sin(x(2)), cos(x(2))).finished().transpose(); - relativeState.segment<2>(0) = R * relativeState.segment<2>(0); - relativeState.segment<2>(5) = R * relativeState.segment<2>(5); - return relativeState; + observation.segment<2>(0) = R * observation.segment<2>(0); + observation.segment<2>(5) = R * observation.segment<2>(5); + return observation; } -matrix_t BallbotMpcnetDefinition::getInputTransformation(scalar_t t, const vector_t& x) { - return matrix_t::Identity(3, 3); +std::pair<matrix_t, vector_t> BallbotMpcnetDefinition::getActionTransformation(scalar_t t, const vector_t& x, + const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + return {matrix_t::Identity(3, 3), vector_t::Zero(3)}; } -bool BallbotMpcnetDefinition::validState(const vector_t& x) { +bool BallbotMpcnetDefinition::isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { return true; } diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h index aee27b718..6c141bd38 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h @@ -51,24 +51,21 @@ class LeggedRobotMpcnetDefinition final : public ocs2::mpcnet::MpcnetDefinitionB ~LeggedRobotMpcnetDefinition() override = default; /** - * @see MpcnetDefinitionBase::getGeneralizedTime + * @see MpcnetDefinitionBase::getObservation */ - vector_t getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) override; + vector_t getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) override; /** - * @see MpcnetDefinitionBase::getRelativeState + * @see MpcnetDefinitionBase::getActionTransformation */ - vector_t getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) override; + std::pair<matrix_t, vector_t> getActionTransformation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) override; /** - * @see MpcnetDefinitionBase::getInputTransformation + * @see MpcnetDefinitionBase::isValid */ - matrix_t getInputTransformation(scalar_t t, const vector_t& x) override; - - /** - * @see MpcnetDefinitionBase::validState - */ - bool validState(const vector_t& x) override; + bool isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) override; private: const scalar_t allowedHeightDeviation_ = 0.2; diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py index 9e54f92a1..274c8df95 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py @@ -60,6 +60,18 @@ # input dimension INPUT_DIM = 24 +# target trajectories state dimension +TARGET_STATE_DIM = STATE_DIM + +# target trajectories input dimension +TARGET_INPUT_DIM = INPUT_DIM + +# observation dimension +OBSERVATION_DIM = 12 + STATE_DIM + +# action dimension +ACTION_DIM = INPUT_DIM + # expert number EXPERT_NUM = 3 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py index 9e7459b1a..4ef244197 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py @@ -97,7 +97,7 @@ def get_random_target_state_stance() -> np.ndarray: max_orientation_z = 25.0 * np.pi / 180.0 max_orientation_y = 15.0 * np.pi / 180.0 max_orientation_x = 25.0 * np.pi / 180.0 - random_deviation = np.zeros(config.STATE_DIM) + random_deviation = np.zeros(config.TARGET_STATE_DIM) random_deviation[8] = np.random.uniform(-max_position_z, max_position_z) random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) random_deviation[10] = np.random.uniform(-max_orientation_y, max_orientation_y) @@ -180,7 +180,7 @@ def get_random_target_state_trot() -> np.ndarray: max_position_x = 0.3 max_position_y = 0.15 max_orientation_z = 30.0 * np.pi / 180.0 - random_deviation = np.zeros(config.STATE_DIM) + random_deviation = np.zeros(config.TARGET_STATE_DIM) random_deviation[6] = np.random.uniform(-max_position_x, max_position_x) random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) @@ -216,8 +216,8 @@ def get_tasks( mode_schedules[i] = helper.get_mode_schedule(*get_stance(duration)) target_trajectories[i] = helper.get_target_trajectories( duration * np.ones((1, 1)), - get_random_target_state_stance().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM)), + get_random_target_state_stance().reshape((1, config.TARGET_STATE_DIM)), + np.zeros((1, config.TARGET_INPUT_DIM)), ) elif choices[i] == "trot_1": initial_observations[i] = helper.get_system_observation( @@ -226,8 +226,8 @@ def get_tasks( mode_schedules[i] = helper.get_mode_schedule(*get_trot_1(duration)) target_trajectories[i] = helper.get_target_trajectories( duration * np.ones((1, 1)), - get_random_target_state_trot().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM)), + get_random_target_state_trot().reshape((1, config.TARGET_STATE_DIM)), + np.zeros((1, config.TARGET_INPUT_DIM)), ) elif choices[i] == "trot_2": initial_observations[i] = helper.get_system_observation( @@ -236,8 +236,8 @@ def get_tasks( mode_schedules[i] = helper.get_mode_schedule(*get_trot_2(duration)) target_trajectories[i] = helper.get_target_trajectories( duration * np.ones((1, 1)), - get_random_target_state_trot().reshape((1, config.STATE_DIM)), - np.zeros((1, config.INPUT_DIM)), + get_random_target_state_trot().reshape((1, config.TARGET_STATE_DIM)), + np.zeros((1, config.TARGET_INPUT_DIM)), ) return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index 278a0d5b9..f70ccd891 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -102,20 +102,24 @@ def main(): # memory memory_capacity = 400000 - memory = Memory(memory_capacity, config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) + memory = Memory( + memory_capacity, + config.STATE_DIM, + config.INPUT_DIM, + config.OBSERVATION_DIM, + config.ACTION_DIM, + config.EXPERT_NUM, + ) # policy - policy = Policy(config.TIME_DIM, config.STATE_DIM, config.INPUT_DIM, config.EXPERT_NUM) + policy = Policy(config.OBSERVATION_DIM, config.ACTION_DIM, config.EXPERT_NUM) policy.to(config.DEVICE) print("Initial policy parameters:") print(list(policy.named_parameters())) - dummy_input = ( - torch.randn(1, config.TIME_DIM, device=config.DEVICE, dtype=config.DTYPE), - torch.randn(1, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE), - ) + dummy_observation = torch.randn(1, config.OBSERVATION_DIM, device=config.DEVICE, dtype=config.DTYPE) print("Saving initial policy.") save_path = "policies/" + folder + "/initial_policy" - torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") # optimizer @@ -137,7 +141,7 @@ def main(): def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) choices = random.choices(["stance", "trot_1", "trot_2"], k=data_generation_n_tasks, weights=weights) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( data_generation_n_tasks, data_generation_duration, choices @@ -156,7 +160,7 @@ def start_data_generation(policy, alpha=1.0): def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_input, f=policy_file_path) + torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) choices = random.choices(["stance", "trot_1", "trot_2"], k=policy_evaluation_n_tasks, weights=weights) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( policy_evaluation_n_tasks, policy_evaluation_duration, choices @@ -186,15 +190,14 @@ def start_policy_evaluation(policy, alpha=0.0): # get generated data data = mpcnet_interface.getGeneratedData() for i in range(len(data)): - # push t, x, u, p, generalized time, relative state, input_transformation, Hamiltonian into memory + # push t, x, u, p, observation, action transformation, Hamiltonian into memory memory.push( data[i].t, data[i].x, data[i].u, helper.get_one_hot(data[i].mode), - data[i].generalizedTime, - data[i].relativeState, - data[i].inputTransformation, + data[i].observation, + data[i].actionTransformation, data[i].hamiltonian, ) # logging @@ -229,7 +232,7 @@ def start_policy_evaluation(policy, alpha=0.0): if (iteration % 10000 == 0) and (iteration > 0): print("Saving intermediate policy for iteration", iteration) save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) - torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") # extract batch from memory @@ -238,9 +241,9 @@ def start_policy_evaluation(policy, alpha=0.0): x, u, p, - generalized_time, - relative_state, - input_transformation, + observation, + action_transformation_matrix, + action_transformation_vector, dHdxx, dHdux, dHduu, @@ -254,11 +257,11 @@ def closure(): # clear the gradients optimizer.zero_grad() # prediction - u_predicted, p_predicted = policy(generalized_time, relative_state) - u_predicted = bmv(input_transformation, u_predicted) + action, expert_weights = policy(observation) + input = bmv(action_transformation_matrix, action) + action_transformation_vector # compute the empirical loss - empirical_experts_loss = experts_loss(x, x, u_predicted, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) - empirical_gating_loss = gating_loss(p, p_predicted) + empirical_experts_loss = experts_loss(x, x, input, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + empirical_gating_loss = gating_loss(p, expert_weights) empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss # compute the gradients empirical_loss.backward() @@ -290,7 +293,7 @@ def closure(): print("Saving final policy.") save_path = "policies/" + folder + "/final_policy" - torch.onnx.export(model=policy, args=dummy_input, f=save_path + ".onnx") + torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") torch.save(obj=policy, f=save_path + ".pt") writer.close() diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py index eee3971ad..546ecb599 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py @@ -47,55 +47,55 @@ input_bias = torch.tensor(config.INPUT_BIAS, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) -def u_transform(u: torch.Tensor) -> torch.Tensor: +def u_transform(a: torch.Tensor) -> torch.Tensor: """Control input transformation. - Transforms the predicted control input by scaling and adding a bias. + Transforms the predicted action by scaling and adding a bias. Args: - u: A (B,U) tensor with the predicted control inputs. + a: A (B,A) tensor with the predicted actions. Returns: u: A (B,U) tensor with the transformed control inputs. """ - return bmv(input_scaling, u) + input_bias + return bmv(input_scaling, a) + input_bias class LeggedRobotLinearPolicy(linear.LinearPolicy): - def __init__(self, dim_t, dim_x, dim_u): - super().__init__(dim_t, dim_x, dim_u) + def __init__(self, observation_dimension, action_dimension): + super().__init__(observation_dimension, action_dimension) self.name = "LeggedRobotLinearPolicy" - def forward(self, t, x): - u = super().forward(t, x) - return u_transform(u) + def forward(self, o): + a = super().forward(o) + return u_transform(a) class LeggedRobotNonlinearPolicy(nonlinear.NonlinearPolicy): - def __init__(self, dim_t, dim_x, dim_u): - super().__init__(dim_t, dim_x, dim_u) + def __init__(self, observation_dimension, action_dimension): + super().__init__(observation_dimension, action_dimension) self.name = "LeggedRobotNonlinearPolicy" - def forward(self, t, x): - u = super().forward(t, x) - return u_transform(u) + def forward(self, o): + a = super().forward(o) + return u_transform(a) class LeggedRobotMixtureOfLinearExpertsPolicy(mixture_of_linear_experts.MixtureOfLinearExpertsPolicy): - def __init__(self, dim_t, dim_x, dim_u, num_experts): - super().__init__(dim_t, dim_x, dim_u, num_experts) + def __init__(self, observation_dimension, action_dimension, expert_number): + super().__init__(observation_dimension, action_dimension, expert_number) self.name = "LeggedRobotMixtureOfLinearExpertsPolicy" - def forward(self, t, x): - u, p = super().forward(t, x) - return u_transform(u), p + def forward(self, o): + a, p = super().forward(o) + return u_transform(a), p class LeggedRobotMixtureOfNonlinearExpertsPolicy(mixture_of_nonlinear_experts.MixtureOfNonlinearExpertsPolicy): - def __init__(self, dim_t, dim_x, dim_u, num_experts): - super().__init__(dim_t, dim_x, dim_u, num_experts) + def __init__(self, observation_dimension, action_dimension, expert_number): + super().__init__(observation_dimension, action_dimension, expert_number) self.name = "LeggedRobotMixtureOfNonlinearExpertsPolicy" - def forward(self, t, x): - u, p = super().forward(t, x) - return u_transform(u), p + def forward(self, o): + a, p = super().forward(o) + return u_transform(a), p diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index cf5b2fdff..85018d8bd 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -37,7 +37,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace legged_robot { -vector_t LeggedRobotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) { +vector_t LeggedRobotMpcnetDefinition::getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + /** + * generalized time + */ const feet_array_t<LegPhase> swingPhasePerLeg = getSwingPhasePerLeg(t, modeSchedule); vector_t generalizedTime(3 * swingPhasePerLeg.size()); // phase @@ -64,39 +68,47 @@ vector_t LeggedRobotMpcnetDefinition::getGeneralizedTime(scalar_t t, const ModeS generalizedTime[i + 2 * swingPhasePerLeg.size()] = std::sin(M_PI * swingPhasePerLeg[i].phase); } } - return generalizedTime; -} - -vector_t LeggedRobotMpcnetDefinition::getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) { + /** + * relative state + */ vector_t relativeState = x - targetTrajectories.getDesiredState(t); const matrix3_t R = getRotationMatrixFromZyxEulerAngles<scalar_t>(x.segment<3>(9)).transpose(); relativeState.segment<3>(0) = R * relativeState.segment<3>(0); relativeState.segment<3>(3) = R * relativeState.segment<3>(3); relativeState.segment<3>(6) = R * relativeState.segment<3>(6); // TODO(areske): use quaternionDistance() for orientation error? - return relativeState; + /** + * observation + */ + vector_t observation(36); + observation << generalizedTime, relativeState; + return observation; } -matrix_t LeggedRobotMpcnetDefinition::getInputTransformation(scalar_t t, const vector_t& x) { +std::pair<matrix_t, vector_t> LeggedRobotMpcnetDefinition::getActionTransformation(scalar_t t, const vector_t& x, + const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { const matrix3_t R = getRotationMatrixFromZyxEulerAngles<scalar_t>(x.segment<3>(9)); - matrix_t inputTransformation = matrix_t::Identity(24, 24); - inputTransformation.block<3, 3>(0, 0) = R; - inputTransformation.block<3, 3>(3, 3) = R; - inputTransformation.block<3, 3>(6, 6) = R; - inputTransformation.block<3, 3>(9, 9) = R; - return inputTransformation; + matrix_t actionTransformationMatrix = matrix_t::Identity(24, 24); + actionTransformationMatrix.block<3, 3>(0, 0) = R; + actionTransformationMatrix.block<3, 3>(3, 3) = R; + actionTransformationMatrix.block<3, 3>(6, 6) = R; + actionTransformationMatrix.block<3, 3>(9, 9) = R; + // TODO(areske): weight compensating bias + return {actionTransformationMatrix, vector_t::Zero(24)}; } -bool LeggedRobotMpcnetDefinition::validState(const vector_t& x) { +bool LeggedRobotMpcnetDefinition::isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { const vector_t deviation = x - defaultState_; if (std::abs(deviation[8]) > allowedHeightDeviation_) { - std::cerr << "[LeggedRobotMpcnetDefinition::validState] height diverged: " << x[8] << "\n"; + std::cerr << "[LeggedRobotMpcnetDefinition::isValid] height diverged: " << x[8] << "\n"; return false; } else if (std::abs(deviation[10]) > allowedPitchDeviation_) { - std::cerr << "[LeggedRobotMpcnetDefinition::validState] pitch diverged: " << x[10] << "\n"; + std::cerr << "[LeggedRobotMpcnetDefinition::isValid] pitch diverged: " << x[10] << "\n"; return false; } else if (std::abs(deviation[11]) > allowedRollDeviation_) { - std::cerr << "[LeggedRobotMpcnetDefinition::validState] roll diverged: " << x[11] << "\n"; + std::cerr << "[LeggedRobotMpcnetDefinition::isValid] roll diverged: " << x[11] << "\n"; return false; } else { return true; diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetDefinitionBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetDefinitionBase.h index cbc971b66..de322bcc2 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetDefinitionBase.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetDefinitionBase.h @@ -61,36 +61,39 @@ class MpcnetDefinitionBase { MpcnetDefinitionBase& operator=(const MpcnetDefinitionBase&) = delete; /** - * Get the generalized time. + * Get the observation. + * @note The observation o is the input to the policy. * @param[in] t : Absolute time. + * @param[in] x : Robot state. * @param[in] modeSchedule : Mode schedule. - * @return The generalized time. + * @param[in] targetTrajectories : Target trajectories. + * @return The observation. */ - virtual vector_t getGeneralizedTime(scalar_t t, const ModeSchedule& modeSchedule) = 0; + virtual vector_t getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) = 0; /** - * Get the relative state. + * Get the action transformation. + * @note Used for computing the control input u = A * a + b from the action a predicted by the policy. * @param[in] t : Absolute time. * @param[in] x : Robot state. + * @param[in] modeSchedule : Mode schedule. * @param[in] targetTrajectories : Target trajectories. - * @return The relative state. + * @return The action transformation pair {A, b}. */ - virtual vector_t getRelativeState(scalar_t t, const vector_t& x, const TargetTrajectories& targetTrajectories) = 0; + virtual std::pair<matrix_t, vector_t> getActionTransformation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) = 0; /** - * Get the input transformation. + * Check if the tuple (t, x, modeSchedule, targetTrajectories) is valid. + * @note E.g., check if the state diverged or if tracking is poor. * @param[in] t : Absolute time. * @param[in] x : Robot state. - * @return The input transformation. - */ - virtual matrix_t getInputTransformation(scalar_t t, const vector_t& x) = 0; - - /** - * Check if a state is valid. - * @param [in] x : State. + * @param[in] modeSchedule : Mode schedule. + * @param[in] targetTrajectories : Target trajectories. * @return True if valid. */ - virtual bool validState(const vector_t& x) = 0; + virtual bool isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) = 0; }; } // namespace mpcnet diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h index d3ea46b7e..632fa4717 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h @@ -100,9 +100,8 @@ using namespace pybind11::literals; .def_readwrite("t", &ocs2::mpcnet::data_point_t::t) \ .def_readwrite("x", &ocs2::mpcnet::data_point_t::x) \ .def_readwrite("u", &ocs2::mpcnet::data_point_t::u) \ - .def_readwrite("generalizedTime", &ocs2::mpcnet::data_point_t::generalizedTime) \ - .def_readwrite("relativeState", &ocs2::mpcnet::data_point_t::relativeState) \ - .def_readwrite("inputTransformation", &ocs2::mpcnet::data_point_t::inputTransformation) \ + .def_readwrite("observation", &ocs2::mpcnet::data_point_t::observation) \ + .def_readwrite("actionTransformation", &ocs2::mpcnet::data_point_t::actionTransformation) \ .def_readwrite("hamiltonian", &ocs2::mpcnet::data_point_t::hamiltonian); \ /* bind metrics struct */ \ pybind11::class_<ocs2::mpcnet::metrics_t>(m, "Metrics") \ diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h index 83a566400..7fd63215e 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h @@ -49,12 +49,10 @@ struct DataPoint { vector_t x; /** Optimal control input. */ vector_t u; - /** Generalized time as defined by the robot-specific MPC-Net definitions. */ - vector_t generalizedTime; - /** Relative state as defined by the robot-specific MPC-Net definitions. */ - vector_t relativeState; - /** Input transformation as defined by the robot-specific MPC-Net definitions. */ - matrix_t inputTransformation; + /** Observation given as input to the policy. */ + vector_t observation; + /** Action transformation applied to the output of the policy. */ + std::pair<matrix_t, vector_t> actionTransformation; /** Linear-quadratic approximation of the Hamiltonian, using x and u as development/expansion points. */ ScalarFunctionQuadraticApproximation hamiltonian; }; @@ -76,9 +74,10 @@ inline data_point_t getDataPoint(MPC_BASE& mpc, MpcnetDefinitionBase& mpcnetDefi dataPoint.x = primalSolution.stateTrajectory_.front() + deviation; dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); - dataPoint.generalizedTime = mpcnetDefinition.getGeneralizedTime(dataPoint.t, referenceManager.getModeSchedule()); - dataPoint.relativeState = mpcnetDefinition.getRelativeState(dataPoint.t, dataPoint.x, referenceManager.getTargetTrajectories()); - dataPoint.inputTransformation = mpcnetDefinition.getInputTransformation(dataPoint.t, dataPoint.x); + dataPoint.observation = mpcnetDefinition.getObservation(dataPoint.t, dataPoint.x, referenceManager.getModeSchedule(), + referenceManager.getTargetTrajectories()); + dataPoint.actionTransformation = mpcnetDefinition.getActionTransformation(dataPoint.t, dataPoint.x, referenceManager.getModeSchedule(), + referenceManager.getTargetTrajectories()); dataPoint.hamiltonian = mpc.getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); return dataPoint; } diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py index 152415ad3..d0e583277 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py @@ -34,7 +34,7 @@ import torch import numpy as np -from typing import Tuple +from typing import Tuple, List from ocs2_mpcnet_core import config from ocs2_mpcnet_core import ScalarFunctionQuadraticApproximation @@ -53,9 +53,9 @@ class CircularMemory: x: A (C,X) tensor for the observed states. u: A (C,U) tensor for the optimal inputs. p: A (C,P) tensor for the observed discrete probability distributions of the modes. - generalized_time: A (C,T) tensor for the generalized times. - relative_state: A (C,X) tensor for the relative states. - input_transformation: A (C,U,U) tensor for the input transformations. + observation: A (C,O) tensor for the observations. + action_transformation_matrix: A (C,U,A) tensor for the action transformation matrices. + action_transformation_vector: A (C,U) tensor for the action transformation vectors. dHdxx: A (C,X,X) tensor for the state-state Hessians of the Hamiltonian approximations. dHdux: A (C,U,X) tensor for the input-state Hessians of the Hamiltonian approximations. dHduu: A (C,U,U) tensor for the input-input Hessians of the Hamiltonian approximations. @@ -65,7 +65,13 @@ class CircularMemory: """ def __init__( - self, capacity: int, time_dimension: int, state_dimension: int, input_dimension: int, expert_number: int = 1 + self, + capacity: int, + state_dimension: int, + input_dimension: int, + observation_dimension: int, + action_dimension: int, + expert_number: int = 1, ) -> None: """Initializes the CircularMemory class. @@ -74,9 +80,10 @@ def __init__( Args: capacity: An integer defining the capacity, i.e. maximum size, C of the memory. - time_dimension: An integer defining the dimension T of the generalized time. - state_dimension: An integer defining the dimension X of the state and relative state. + state_dimension: An integer defining the dimension X of the state. input_dimension: An integer defining the dimension U of the input. + observation_dimension: An integer defining the dimension O of the observation. + action_dimension: An integer defining the dimension A of the action. expert_number: An integer defining the number of experts E equal to the number of individually identifiable items P in the sample space of the discrete probability distributions of the modes. """ @@ -89,10 +96,12 @@ def __init__( self.x = torch.zeros(capacity, state_dimension, device=config.DEVICE, dtype=config.DTYPE) self.u = torch.zeros(capacity, input_dimension, device=config.DEVICE, dtype=config.DTYPE) self.p = torch.zeros(capacity, expert_number, device=config.DEVICE, dtype=config.DTYPE) - self.generalized_time = torch.zeros(capacity, time_dimension, device=config.DEVICE, dtype=config.DTYPE) - self.relative_state = torch.zeros(capacity, state_dimension, device=config.DEVICE, dtype=config.DTYPE) - self.input_transformation = torch.zeros( - capacity, input_dimension, input_dimension, device=config.DEVICE, dtype=config.DTYPE + self.observation = torch.zeros(capacity, observation_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.action_transformation_matrix = torch.zeros( + capacity, input_dimension, action_dimension, device=config.DEVICE, dtype=config.DTYPE + ) + self.action_transformation_vector = torch.zeros( + capacity, input_dimension, device=config.DEVICE, dtype=config.DTYPE ) self.dHdxx = torch.zeros(capacity, state_dimension, state_dimension, device=config.DEVICE, dtype=config.DTYPE) self.dHdux = torch.zeros(capacity, input_dimension, state_dimension, device=config.DEVICE, dtype=config.DTYPE) @@ -107,9 +116,8 @@ def push( x: np.ndarray, u: np.ndarray, p: np.ndarray, - generalized_time: np.ndarray, - relative_state: np.ndarray, - input_transformation: np.ndarray, + observation: np.ndarray, + action_transformation: List[np.ndarray], hamiltonian: ScalarFunctionQuadraticApproximation, ) -> None: """Pushes data into the circular memory. @@ -121,9 +129,8 @@ def push( x: A NumPy array of shape (X) with the observed state. u: A NumPy array of shape (U) with the optimal input. p: A NumPy array of shape (P) tensor for the observed discrete probability distributions of the modes. - generalized_time: A NumPy array of shape (T) with the generalized times. - relative_state: A NumPy array of shape (X) with the relative states. - input_transformation: A NumPy array of shape (U,U) with the input transformations. + observation: A NumPy array of shape (O) with the generalized times. + action_transformation: A list containing NumPy arrays of shape (U,A) and (U) with the action transformation. hamiltonian: An OCS2 scalar function quadratic approximation representing the Hamiltonian around x and u. """ # push data into memory @@ -133,14 +140,12 @@ def push( self.x[self.position].copy_(torch.as_tensor(x, dtype=None, device=torch.device("cpu"))) self.u[self.position].copy_(torch.as_tensor(u, dtype=None, device=torch.device("cpu"))) self.p[self.position].copy_(torch.as_tensor(p, dtype=None, device=torch.device("cpu"))) - self.generalized_time[self.position].copy_( - torch.as_tensor(generalized_time, dtype=None, device=torch.device("cpu")) - ) - self.relative_state[self.position].copy_( - torch.as_tensor(relative_state, dtype=None, device=torch.device("cpu")) + self.observation[self.position].copy_(torch.as_tensor(observation, dtype=None, device=torch.device("cpu"))) + self.action_transformation_matrix[self.position].copy_( + torch.as_tensor(action_transformation[0], dtype=None, device=torch.device("cpu")) ) - self.input_transformation[self.position].copy_( - torch.as_tensor(input_transformation, dtype=None, device=torch.device("cpu")) + self.action_transformation_vector[self.position].copy_( + torch.as_tensor(action_transformation[1], dtype=None, device=torch.device("cpu")) ) self.dHdxx[self.position].copy_(torch.as_tensor(hamiltonian.dfdxx, dtype=None, device=torch.device("cpu"))) self.dHdux[self.position].copy_(torch.as_tensor(hamiltonian.dfdux, dtype=None, device=torch.device("cpu"))) @@ -166,9 +171,9 @@ def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: - x_batch: A (B,X) tensor with the observed states. - u_batch: A (B,U) tensor with the optimal inputs. - p_batch: A (B,P) tensor with the observed discrete probability distributions of the modes. - - generalized_time_batch: A (B,T) tensor with the generalized times. - - relative_state_batch: A (B,X) tensor with the relative states. - - input_transformation_batch: A (B,U,U) tensor with the input transformation matrices. + - observation_batch: A (B,O) tensor with the observations. + - action_transformation_matrix_batch: A (B,U,A) tensor with the action transformation matrices. + - action_transformation_vector_batch: A (B,U) tensor with the action transformation vectors. - dHdxx_batch: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. - dHdux_batch: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. - dHduu_batch: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. @@ -181,9 +186,9 @@ def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: x_batch = self.x[indices] u_batch = self.u[indices] p_batch = self.p[indices] - generalized_time_batch = self.generalized_time[indices] - relative_state_batch = self.relative_state[indices] - input_transformation_batch = self.input_transformation[indices] + observation_batch = self.observation[indices] + action_transformation_matrix_batch = self.action_transformation_matrix[indices] + action_transformation_vector_batch = self.action_transformation_vector[indices] dHdxx_batch = self.dHdxx[indices] dHdux_batch = self.dHdux[indices] dHduu_batch = self.dHduu[indices] @@ -195,9 +200,9 @@ def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: x_batch, u_batch, p_batch, - generalized_time_batch, - relative_state_batch, - input_transformation_batch, + observation_batch, + action_transformation_matrix_batch, + action_transformation_vector_batch, dHdxx_batch, dHdux_batch, dHduu_batch, diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py index f7f6df7a0..10e875d6e 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py @@ -42,37 +42,35 @@ class LinearPolicy(torch.nn.Module): Attributes: name: A string with the name of the policy. - dim_in: An integer defining the input dimension of the policy. - dim_out: An integer defining the output dimension of the policy. + observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. + action_dimension: An integer defining the action (i.e. output) dimension of the policy. linear: The linear neural network layer. """ - def __init__(self, dim_t: int, dim_x: int, dim_u: int) -> None: + def __init__(self, observation_dimension: int, action_dimension: int) -> None: """Initializes the LinearPolicy class. Initializes the LinearPolicy class by setting fixed and variable attributes. Args: - dim_t: An integer defining the generalized time dimension. - dim_x: An integer defining the relative state dimension. - dim_u: An integer defining the control input dimension. + observation_dimension: An integer defining the observation dimension. + action_dimension: An integer defining the action dimension. """ super().__init__() self.name = "LinearPolicy" - self.dim_in = dim_t + dim_x - self.dim_out = dim_u - self.linear = torch.nn.Linear(self.dim_in, self.dim_out) + self.observation_dimension = observation_dimension + self.action_dimension = action_dimension + self.linear = torch.nn.Linear(self.observation_dimension, self.action_dimension) - def forward(self, t: torch.Tensor, x: torch.Tensor) -> torch.Tensor: + def forward(self, observation: torch.Tensor) -> torch.Tensor: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. Args: - t: A (B,T) tensor with the generalized times. - x: A (B,X) tensor with the relative states. + observation: A (B,O) tensor with the observations. Returns: - u: A (B,U) tensor with the predicted control inputs. + action: A (B,A) tensor with the predicted actions. """ - return self.linear(torch.cat((t, x), dim=1)) + return self.linear(observation) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py index 20441a27a..b1772402b 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py @@ -45,53 +45,53 @@ class MixtureOfLinearExpertsPolicy(torch.nn.Module): Attributes: name: A string with the name of the policy. - dim_in: An integer defining the input dimension of the policy. - dim_out: An integer defining the output dimension of the policy. - num_experts: An integer defining the number of experts. + observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. + action_dimension: An integer defining the action (i.e. output) dimension of the policy. + expert_number: An integer defining the number of experts. gating_net: The gating network. expert_nets: The expert networks. """ - def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None: + def __init__(self, observation_dimension: int, action_dimension: int, expert_number: int) -> None: """Initializes the MixtureOfLinearExpertsPolicy class. Initializes the MixtureOfLinearExpertsPolicy class by setting fixed and variable attributes. Args: - dim_t: An integer defining the generalized time dimension. - dim_x: An integer defining the relative state dimension. - dim_u: An integer defining the control input dimension. - num_experts: An integer defining the number of experts. + observation_dimension: An integer defining the observation dimension. + action_dimension: An integer defining the action dimension. + expert_number: An integer defining the number of experts. """ super().__init__() self.name = "MixtureOfLinearExpertsPolicy" - self.dim_in = dim_t + dim_x - self.dim_out = dim_u - self.num_experts = num_experts + self.observation_dimension = observation_dimension + self.action_dimension = action_dimension + self.expert_number = expert_number # gating - self.gating_net = torch.nn.Sequential(torch.nn.Linear(self.dim_in, self.num_experts), torch.nn.Softmax(dim=1)) + self.gating_net = torch.nn.Sequential( + torch.nn.Linear(self.observation_dimension, self.expert_number), torch.nn.Softmax(dim=1) + ) # experts self.expert_nets = torch.nn.ModuleList( - [_LinearExpert(i, self.dim_in, self.dim_out) for i in range(self.num_experts)] + [_LinearExpert(i, self.observation_dimension, self.action_dimension) for i in range(self.expert_number)] ) - def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. Args: - t: A (B,T) tensor with the generalized times. - x: A (B,X) tensor with the relative states. + observation: A (B,O) tensor with the observations. Returns: - u: A (B,U) tensor with the predicted control inputs. - p: A (B,E) tensor with the predicted expert weights. + action: A (B,A) tensor with the predicted actions. + expert_weights: A (B,E) tensor with the predicted expert weights. """ - p = self.gating_net(torch.cat((t, x), dim=1)) - U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) - u = bmv(U, p) - return u, p + expert_weights = self.gating_net(observation) + expert_actions = torch.stack([self.expert_nets[i](observation) for i in range(self.expert_number)], dim=2) + action = bmv(expert_actions, expert_weights) + return action, expert_weights class _LinearExpert(torch.nn.Module): @@ -101,26 +101,26 @@ class _LinearExpert(torch.nn.Module): Attributes: name: A string with the name of the expert. - dim_in: An integer defining the input dimension of the expert. - dim_out: An integer defining the output dimension of the expert. + input_dimension: An integer defining the input dimension of the expert. + output_dimension: An integer defining the output dimension of the expert. linear: The linear neural network layer. """ - def __init__(self, index: int, dim_in: int, dim_out: int) -> None: + def __init__(self, index: int, input_dimension: int, output_dimension: int) -> None: """Initializes the _LinearExpert class. Initializes the _LinearExpert class by setting fixed and variable attributes. Args: index: An integer with the index of the expert. - dim_in: An integer defining the input dimension of the expert. - dim_out: An integer defining the output dimension of the expert. + input_dimension: An integer defining the input dimension of the expert. + output_dimension: An integer defining the output dimension of the expert. """ super().__init__() self.name = "LinearExpert" + str(index) - self.dim_in = dim_in - self.dim_out = dim_out - self.linear = torch.nn.Linear(self.dim_in, self.dim_out) + self.input_dimension = input_dimension + self.output_dimension = output_dimension + self.linear = torch.nn.Linear(self.input_dimension, self.output_dimension) def forward(self, input: torch.Tensor) -> torch.Tensor: """Forward method. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py index 2cbb5d8d3..d6cd20477 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py @@ -41,104 +41,105 @@ class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): """Mixture of nonlinear experts policy. - Class for a mixture of experts neural network policy with nonlinear experts, where the hidden layer is the mean of - the input and output layer. + Class for a mixture of experts neural network policy with nonlinear experts, where the hidden layer dimension is the + mean of the input and output dimensions. Attributes: name: A string with the name of the policy. - dim_in: An integer defining the input dimension of the policy. - dim_hidden_gating: An integer defining the dimension of the hidden layer for the gating network. - dim_hidden_expert: An integer defining the dimension of the hidden layer for the expert networks. - dim_out: An integer defining the output dimension of the policy. - num_experts: An integer defining the number of experts. + observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. + gating_hidden_dimension: An integer defining the dimension of the hidden layer for the gating network. + expert_hidden_dimension: An integer defining the dimension of the hidden layer for the expert networks. + action_dimension: An integer defining the action (i.e. output) dimension of the policy. + expert_number: An integer defining the number of experts. gating_net: The gating network. expert_nets: The expert networks. """ - def __init__(self, dim_t: int, dim_x: int, dim_u: int, num_experts: int) -> None: + def __init__(self, observation_dimension: int, action_dimension: int, expert_number: int) -> None: """Initializes the MixtureOfNonlinearExpertsPolicy class. Initializes the MixtureOfNonlinearExpertsPolicy class by setting fixed and variable attributes. Args: - dim_t: An integer defining the generalized time dimension. - dim_x: An integer defining the relative state dimension. - dim_u: An integer defining the control input dimension. - num_experts: An integer defining the number of experts. + observation_dimension: An integer defining the observation dimension. + action_dimension: An integer defining the action dimension. + expert_number: An integer defining the number of experts. """ super().__init__() self.name = "MixtureOfNonlinearExpertsPolicy" - self.dim_in = dim_t + dim_x - self.dim_hidden_gating = int((dim_t + dim_x + num_experts) / 2) - self.dim_hidden_expert = int((dim_t + dim_x + dim_u) / 2) - self.dim_out = dim_u - self.num_experts = num_experts - + self.observation_dimension = observation_dimension + self.gating_hidden_dimension = int((observation_dimension + expert_number) / 2) + self.expert_hidden_dimension = int((observation_dimension + action_dimension) / 2) + self.action_dimension = action_dimension + self.expert_number = expert_number # gating self.gating_net = torch.nn.Sequential( - torch.nn.Linear(self.dim_in, self.dim_hidden_gating), + torch.nn.Linear(self.observation_dimension, self.gating_hidden_dimension), torch.nn.Tanh(), - torch.nn.Linear(self.dim_hidden_gating, self.num_experts), + torch.nn.Linear(self.gating_hidden_dimension, self.expert_number), torch.nn.Softmax(dim=1), ) # experts self.expert_nets = torch.nn.ModuleList( - [_NonlinearExpert(i, self.dim_in, self.dim_hidden_expert, self.dim_out) for i in range(self.num_experts)] + [ + _NonlinearExpert(i, self.observation_dimension, self.expert_hidden_dimension, self.action_dimension) + for i in range(self.expert_number) + ] ) - def forward(self, t: torch.Tensor, x: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. Args: - t: A (B,T) tensor with the generalized times. - x: A (B,X) tensor with the relative states. + observation: A (B,O) tensor with the observations. Returns: - u: A (B,U) tensor with the predicted control inputs. - p: A (B,E) tensor with the predicted expert weights. + action: A (B,A) tensor with the predicted actions. + expert_weights: A (B,E) tensor with the predicted expert weights. """ - p = self.gating_net(torch.cat((t, x), dim=1)) - U = torch.stack([self.expert_nets[i](torch.cat((t, x), dim=1)) for i in range(self.num_experts)], dim=2) - u = bmv(U, p) - return u, p + expert_weights = self.gating_net(observation) + expert_actions = torch.stack([self.expert_nets[i](observation) for i in range(self.expert_number)], dim=2) + action = bmv(expert_actions, expert_weights) + return action, expert_weights class _NonlinearExpert(torch.nn.Module): """Nonlinear expert. - Class for a simple nonlinear neural network expert, where the hidden layer is the mean of the input and output layer. + Class for a simple nonlinear neural network expert, where the hidden layer dimension is the mean of the input and + output dimensions. Attributes: name: A string with the name of the expert. - dim_in: An integer defining the input dimension of the expert. - dim_hidden: An integer defining the dimension of the hidden layer. - dim_out: An integer defining the output dimension of the expert. + input_dimension: An integer defining the input dimension of the expert. + hidden_dimension: An integer defining the dimension of the hidden layer. + output_dimension: An integer defining the output dimension of the expert. linear1: The first linear neural network layer. activation: The activation to get the hidden layer. linear2: The second linear neural network layer. """ - def __init__(self, index: int, dim_in: int, dim_hidden: int, dim_out: int) -> None: + def __init__(self, index: int, input_dimension: int, hidden_dimension: int, output_dimension: int) -> None: """Initializes the _NonlinearExpert class. Initializes the _NonlinearExpert class by setting fixed and variable attributes. Args: index: An integer with the index of the expert. - dim_in: An integer defining the input dimension of the expert. - dim_hidden: An integer defining the dimension of the hidden layer. - dim_out: An integer defining the output dimension of the expert. + input_dimension: An integer defining the input dimension of the expert. + hidden_dimension: An integer defining the dimension of the hidden layer. + output_dimension: An integer defining the output dimension of the expert. """ super().__init__() self.name = "NonlinearExpert" + str(index) - self.dim_in = dim_in - self.dim_hidden = dim_hidden - self.dim_out = dim_out - self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) + self.input_dimension = input_dimension + self.hidden_dimension = hidden_dimension + self.output_dimension = output_dimension + self.linear1 = torch.nn.Linear(self.input_dimension, self.hidden_dimension) self.activation = torch.nn.Tanh() - self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) + self.linear2 = torch.nn.Linear(self.hidden_dimension, self.output_dimension) def forward(self, input: torch.Tensor) -> torch.Tensor: """Forward method. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py index 7e28d678c..67bf6822d 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py @@ -38,48 +38,46 @@ class NonlinearPolicy(torch.nn.Module): """Nonlinear policy. - Class for a simple nonlinear neural network policy, where the hidden layer is the mean of the input and output - layer. + Class for a simple nonlinear neural network policy, where the hidden layer dimension is the mean of the input and + output dimensions. Attributes: name: A string with the name of the policy. - dim_in: An integer defining the input dimension of the policy. - dim_hidden: An integer defining the dimension of the hidden layer. - dim_out: An integer defining the output dimension of the policy. + observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. + hidden_dimension: An integer defining the dimension of the hidden layer. + action_dimension: An integer defining the action (i.e. output) dimension of the policy. linear1: The first linear neural network layer. activation: The activation to get the hidden layer. linear2: The second linear neural network layer. """ - def __init__(self, dim_t: int, dim_x: int, dim_u: int) -> None: + def __init__(self, observation_dimension: int, action_dimension: int) -> None: """Initializes the NonlinearPolicy class. Initializes the NonlinearPolicy class by setting fixed and variable attributes. Args: - dim_t: An integer defining the generalized time dimension. - dim_x: An integer defining the relative state dimension. - dim_u: An integer defining the control input dimension. + observation_dimension: An integer defining the observation dimension. + action_dimension: An integer defining the action dimension. """ super().__init__() self.name = "NonlinearPolicy" - self.dim_in = dim_t + dim_x - self.dim_hidden = int((dim_t + dim_x + dim_u) / 2) - self.dim_out = dim_u - self.linear1 = torch.nn.Linear(self.dim_in, self.dim_hidden) + self.observation_dimension = observation_dimension + self.hidden_dimension = int((observation_dimension + action_dimension) / 2) + self.action_dimension = action_dimension + self.linear1 = torch.nn.Linear(self.observation_dimension, self.hidden_dimension) self.activation = torch.nn.Tanh() - self.linear2 = torch.nn.Linear(self.dim_hidden, self.dim_out) + self.linear2 = torch.nn.Linear(self.hidden_dimension, self.action_dimension) - def forward(self, t: torch.Tensor, x: torch.Tensor) -> torch.Tensor: + def forward(self, observation: torch.Tensor) -> torch.Tensor: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. Args: - t: A (B,T) tensor with the generalized times. - x: A (B,X) tensor with the relative states. + observation: A (B,O) tensor with the observations. Returns: - u: A (B,U) tensor with the predicted control inputs. + action: A (B,A) tensor with the predicted actions. """ - return self.linear2(self.activation(self.linear1(torch.cat((t, x), dim=1)))) + return self.linear2(self.activation(self.linear1(observation))) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp index 8ba5a91a9..b77c73d2b 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp @@ -65,24 +65,24 @@ vector_t MpcnetOnnxController::computeInput(const scalar_t t, const vector_t& x) if (sessionPtr_ == nullptr) { throw std::runtime_error("[MpcnetOnnxController::computeInput] cannot compute input, since policy model is not loaded."); } - // create input tensor objects - Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> time = - mpcnetDefinitionPtr_->getGeneralizedTime(t, referenceManagerPtr_->getModeSchedule()).cast<tensor_element_t>(); - Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> state = - mpcnetDefinitionPtr_->getRelativeState(t, x, referenceManagerPtr_->getTargetTrajectories()).cast<tensor_element_t>(); + // create input tensor object + Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1> observation = + mpcnetDefinitionPtr_->getObservation(t, x, referenceManagerPtr_->getModeSchedule(), referenceManagerPtr_->getTargetTrajectories()) + .cast<tensor_element_t>(); Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, OrtMemType::OrtMemTypeDefault); std::vector<Ort::Value> inputValues; - inputValues.push_back( - Ort::Value::CreateTensor<tensor_element_t>(memoryInfo, time.data(), time.size(), inputShapes_[0].data(), inputShapes_[0].size())); - inputValues.push_back( - Ort::Value::CreateTensor<tensor_element_t>(memoryInfo, state.data(), state.size(), inputShapes_[1].data(), inputShapes_[1].size())); + inputValues.push_back(Ort::Value::CreateTensor<tensor_element_t>(memoryInfo, observation.data(), observation.size(), + inputShapes_[0].data(), inputShapes_[0].size())); // run inference Ort::RunOptions runOptions; - std::vector<Ort::Value> outputValues = sessionPtr_->Run(runOptions, inputNames_.data(), inputValues.data(), 2, outputNames_.data(), 1); - // evaluate output tensor objects - Eigen::Map<Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1>> input(outputValues[0].GetTensorMutableData<tensor_element_t>(), - outputShapes_[0][1], outputShapes_[0][0]); - return mpcnetDefinitionPtr_->getInputTransformation(t, x) * input.cast<scalar_t>(); + std::vector<Ort::Value> outputValues = sessionPtr_->Run(runOptions, inputNames_.data(), inputValues.data(), 1, outputNames_.data(), 1); + // evaluate output tensor object + Eigen::Map<Eigen::Matrix<tensor_element_t, Eigen::Dynamic, 1>> action(outputValues[0].GetTensorMutableData<tensor_element_t>(), + outputShapes_[0][1], outputShapes_[0][0]); + std::pair<matrix_t, vector_t> actionTransformation = mpcnetDefinitionPtr_->getActionTransformation( + t, x, referenceManagerPtr_->getModeSchedule(), referenceManagerPtr_->getTargetTrajectories()); + // transform action + return actionTransformation.first * action.cast<scalar_t>() + actionTransformation.second; } } // namespace mpcnet diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp index 85fbbaa4a..8037e5541 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp @@ -90,9 +90,10 @@ void MpcnetRolloutBase::step(scalar_t timeStep) { systemObservation_.input = inputTrajectory.back(); systemObservation_.mode = primalSolution_.modeSchedule_.modeAtTime(systemObservation_.time); - // check if forward simulated system diverged - if (!mpcnetDefinitionPtr_->validState(systemObservation_.state)) { - throw std::runtime_error("[MpcnetRolloutBase::step] state is not valid."); + // check forward simulated system + if (!mpcnetDefinitionPtr_->isValid(systemObservation_.time, systemObservation_.state, referenceManagerPtr_->getModeSchedule(), + referenceManagerPtr_->getTargetTrajectories())) { + throw std::runtime_error("MpcnetDataGeneration::run Tuple (time, state, modeSchedule, targetTrajectories) is not valid."); } } From 0acd7c6f6a497a40be8f180db190d35dbd14e98f Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 7 Apr 2022 20:41:17 +0200 Subject: [PATCH 200/512] weight compensating input as action bias --- .../LeggedRobotMpcnetDefinition.h | 7 +++++-- .../python/ocs2_legged_robot_mpcnet/config.py | 14 -------------- .../python/ocs2_legged_robot_mpcnet/policy.py | 5 ++--- .../src/LeggedRobotMpcnetDefinition.cpp | 7 +++++-- .../src/LeggedRobotMpcnetDummyNode.cpp | 3 +-- .../src/LeggedRobotMpcnetInterface.cpp | 3 +-- 6 files changed, 14 insertions(+), 25 deletions(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h index 6c141bd38..8a6b44f92 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h @@ -29,6 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include <ocs2_legged_robot/LeggedRobotInterface.h> #include <ocs2_mpcnet_core/MpcnetDefinitionBase.h> namespace ocs2 { @@ -41,9 +42,10 @@ class LeggedRobotMpcnetDefinition final : public ocs2::mpcnet::MpcnetDefinitionB public: /** * Constructor. - * @param [in] defaultState : Default state. + * @param [in] leggedRobotInterface : Legged robot interface. */ - LeggedRobotMpcnetDefinition(const vector_t& defaultState) : defaultState_(defaultState) {} + LeggedRobotMpcnetDefinition(const LeggedRobotInterface& leggedRobotInterface) + : defaultState_(leggedRobotInterface.getInitialState()), centroidalModelInfo_(leggedRobotInterface.getCentroidalModelInfo()) {} /** * Default destructor. @@ -72,6 +74,7 @@ class LeggedRobotMpcnetDefinition final : public ocs2::mpcnet::MpcnetDefinitionB const scalar_t allowedPitchDeviation_ = 30.0 * M_PI / 180.0; const scalar_t allowedRollDeviation_ = 30.0 * M_PI / 180.0; const vector_t defaultState_; + const CentroidalModelInfo centroidalModelInfo_; }; } // namespace legged_robot diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py index 274c8df95..e3b956288 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py @@ -89,20 +89,6 @@ ] # fmt: on -# input bias -# fmt: off -INPUT_BIAS = [ - 0.0, 0.0, 127.861, # contact forces LF - 0.0, 0.0, 127.861, # contact forces LH - 0.0, 0.0, 127.861, # contact forces RF - 0.0, 0.0, 127.861, # contact forces RH - 0.0, 0.0, 0.0, # joint velocities LF - 0.0, 0.0, 0.0, # joint velocities LH - 0.0, 0.0, 0.0, # joint velocities RF - 0.0, 0.0, 0.0 # joint velocities RH -] -# fmt: on - # input scaling # fmt: off INPUT_SCALING = [ diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py index 546ecb599..03a00f049 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py @@ -44,13 +44,12 @@ input_scaling = torch.tensor(config.INPUT_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) -input_bias = torch.tensor(config.INPUT_BIAS, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) def u_transform(a: torch.Tensor) -> torch.Tensor: """Control input transformation. - Transforms the predicted action by scaling and adding a bias. + Transforms the predicted action by scaling. Args: a: A (B,A) tensor with the predicted actions. @@ -58,7 +57,7 @@ def u_transform(a: torch.Tensor) -> torch.Tensor: Returns: u: A (B,U) tensor with the transformed control inputs. """ - return bmv(input_scaling, a) + input_bias + return bmv(input_scaling, a) class LeggedRobotLinearPolicy(linear.LinearPolicy): diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index 85018d8bd..b92a3bbeb 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -31,7 +31,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <iostream> +#include <ocs2_legged_robot/common/utils.h> #include <ocs2_legged_robot/gait/LegLogic.h> +#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h> #include <ocs2_robotic_tools/common/RotationTransforms.h> namespace ocs2 { @@ -94,8 +96,9 @@ std::pair<matrix_t, vector_t> LeggedRobotMpcnetDefinition::getActionTransformati actionTransformationMatrix.block<3, 3>(3, 3) = R; actionTransformationMatrix.block<3, 3>(6, 6) = R; actionTransformationMatrix.block<3, 3>(9, 9) = R; - // TODO(areske): weight compensating bias - return {actionTransformationMatrix, vector_t::Zero(24)}; + const auto contactFlags = modeNumber2StanceLeg(modeSchedule.modeAtTime(t)); + const vector_t actionTransformationVector = weightCompensatingInput(centroidalModelInfo_, contactFlags); + return {actionTransformationMatrix, actionTransformationVector}; } bool LeggedRobotMpcnetDefinition::isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index 982ce5cf6..e16898bac 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -79,8 +79,7 @@ int main(int argc, char** argv) { // policy (MPC-Net controller) auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); - std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr( - new LeggedRobotMpcnetDefinition(leggedRobotInterface.getInitialState())); + std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new LeggedRobotMpcnetDefinition(leggedRobotInterface)); std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase> mpcnetControllerPtr( new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); mpcnetControllerPtr->loadPolicyModel(policyFile); diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index ac226d6d6..c2cd925d6 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -65,8 +65,7 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { leggedRobotInterfacePtrs_.push_back(std::unique_ptr<LeggedRobotInterface>(new LeggedRobotInterface(taskFile, urdfFile, referenceFile))); - std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr( - new LeggedRobotMpcnetDefinition(leggedRobotInterfacePtrs_[i]->getInitialState())); + std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new LeggedRobotMpcnetDefinition(*leggedRobotInterfacePtrs_[i])); mpcPtrs.push_back(getMpc(*leggedRobotInterfacePtrs_[i])); mpcnetPtrs.push_back(std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>(new ocs2::mpcnet::MpcnetOnnxController( mpcnetDefinitionPtr, leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr(), onnxEnvironmentPtr))); From 205633f68aab890294d18819e21b1afb0a8e29fb Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 7 Apr 2022 21:48:29 +0200 Subject: [PATCH 201/512] add observation and action scaling --- .../python/ocs2_ballbot_mpcnet/config.py | 11 ++ .../python/ocs2_ballbot_mpcnet/mpcnet.py | 2 +- .../python/ocs2_legged_robot_mpcnet/config.py | 21 +++- .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 6 +- .../python/ocs2_legged_robot_mpcnet/policy.py | 100 ------------------ .../python/ocs2_mpcnet_core/policy/linear.py | 27 ++++- .../policy/mixture_of_linear_experts.py | 31 +++++- .../policy/mixture_of_nonlinear_experts.py | 31 +++++- .../ocs2_mpcnet_core/policy/nonlinear.py | 27 ++++- 9 files changed, 139 insertions(+), 117 deletions(-) delete mode 100644 ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py index f2b7b94e2..e61ffaea5 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py @@ -72,5 +72,16 @@ # action dimension ACTION_DIM = INPUT_DIM +# observation scaling +# fmt: off +OBSERVATION_SCALING = [ + 1.0, 1.0, 1.0, 1.0, 1.0, # pose + 1.0, 1.0, 1.0, 1.0, 1.0 # twist +] +# fmt: on + +# action scaling +ACTION_SCALING = [1.0, 1.0, 1.0] + # input cost for behavioral cloning R = [2.0, 2.0, 2.0] diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index ed2786f70..60c44014e 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -96,7 +96,7 @@ def main(): memory = Memory(memory_capacity, config.STATE_DIM, config.INPUT_DIM, config.OBSERVATION_DIM, config.ACTION_DIM) # policy - policy = Policy(config.OBSERVATION_DIM, config.ACTION_DIM) + policy = Policy(config.OBSERVATION_DIM, config.ACTION_DIM, config.OBSERVATION_SCALING, config.ACTION_SCALING) policy.to(config.DEVICE) print("Initial policy parameters:") print(list(policy.named_parameters())) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py index e3b956288..a99c10fbb 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py @@ -89,9 +89,26 @@ ] # fmt: on -# input scaling +# observation scaling # fmt: off -INPUT_SCALING = [ +OBSERVATION_SCALING = [ + 1.0, 1.0, 1.0, 1.0, # swing phases + 1.0, 1.0, 1.0, 1.0, # swing phase rates + 1.0, 1.0, 1.0, 1.0, # sinusoidal bumps + 1.0, 1.0, 1.0, # normalized linear momentum + 1.0, 1.0, 1.0, # normalized angular momentum + 1.0, 1.0, 1.0, # position + 1.0, 1.0, 1.0, # orientation + 1.0, 1.0, 1.0, # joint positions LF + 1.0, 1.0, 1.0, # joint positions LH + 1.0, 1.0, 1.0, # joint positions RF + 1.0, 1.0, 1.0 # joint positions RH +] +# fmt: on + +# action scaling +# fmt: off +ACTION_SCALING = [ 100.0, 100.0, 100.0, # contact forces LF 100.0, 100.0, 100.0, # contact forces LH 100.0, 100.0, 100.0, # contact forces RF diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index f70ccd891..b6fc98217 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -47,8 +47,8 @@ from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss as ExpertsLoss from ocs2_mpcnet_core.loss.cross_entropy import CrossEntropyLoss as GatingLoss from ocs2_mpcnet_core.memory.circular import CircularMemory as Memory +from ocs2_mpcnet_core.policy.mixture_of_nonlinear_experts import MixtureOfNonlinearExpertsPolicy as Policy -from ocs2_legged_robot_mpcnet.policy import LeggedRobotMixtureOfNonlinearExpertsPolicy as Policy from ocs2_legged_robot_mpcnet import config from ocs2_legged_robot_mpcnet import helper from ocs2_legged_robot_mpcnet import MpcnetInterface @@ -112,7 +112,9 @@ def main(): ) # policy - policy = Policy(config.OBSERVATION_DIM, config.ACTION_DIM, config.EXPERT_NUM) + policy = Policy( + config.OBSERVATION_DIM, config.ACTION_DIM, config.EXPERT_NUM, config.OBSERVATION_SCALING, config.ACTION_SCALING + ) policy.to(config.DEVICE) print("Initial policy parameters:") print(list(policy.named_parameters())) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py deleted file mode 100644 index 03a00f049..000000000 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/policy.py +++ /dev/null @@ -1,100 +0,0 @@ -############################################################################### -# Copyright (c) 2022, Farbod Farshidian. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -############################################################################### - -"""Legged robot policy classes. - -Provides robot-specific classes for different neural network policies for legged robot. - -Todo: - * Delete this file as part of refactoring, as it will be become obsolete. -""" - -import torch - -from ocs2_mpcnet_core.policy import linear, mixture_of_linear_experts, mixture_of_nonlinear_experts, nonlinear -from ocs2_mpcnet_core.helper import bmv, bmm - -from ocs2_legged_robot_mpcnet import config - - -input_scaling = torch.tensor(config.INPUT_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) - - -def u_transform(a: torch.Tensor) -> torch.Tensor: - """Control input transformation. - - Transforms the predicted action by scaling. - - Args: - a: A (B,A) tensor with the predicted actions. - - Returns: - u: A (B,U) tensor with the transformed control inputs. - """ - return bmv(input_scaling, a) - - -class LeggedRobotLinearPolicy(linear.LinearPolicy): - def __init__(self, observation_dimension, action_dimension): - super().__init__(observation_dimension, action_dimension) - self.name = "LeggedRobotLinearPolicy" - - def forward(self, o): - a = super().forward(o) - return u_transform(a) - - -class LeggedRobotNonlinearPolicy(nonlinear.NonlinearPolicy): - def __init__(self, observation_dimension, action_dimension): - super().__init__(observation_dimension, action_dimension) - self.name = "LeggedRobotNonlinearPolicy" - - def forward(self, o): - a = super().forward(o) - return u_transform(a) - - -class LeggedRobotMixtureOfLinearExpertsPolicy(mixture_of_linear_experts.MixtureOfLinearExpertsPolicy): - def __init__(self, observation_dimension, action_dimension, expert_number): - super().__init__(observation_dimension, action_dimension, expert_number) - self.name = "LeggedRobotMixtureOfLinearExpertsPolicy" - - def forward(self, o): - a, p = super().forward(o) - return u_transform(a), p - - -class LeggedRobotMixtureOfNonlinearExpertsPolicy(mixture_of_nonlinear_experts.MixtureOfNonlinearExpertsPolicy): - def __init__(self, observation_dimension, action_dimension, expert_number): - super().__init__(observation_dimension, action_dimension, expert_number) - self.name = "LeggedRobotMixtureOfNonlinearExpertsPolicy" - - def forward(self, o): - a, p = super().forward(o) - return u_transform(a), p diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py index 10e875d6e..42c321cce 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py @@ -33,6 +33,10 @@ """ import torch +import numpy as np + +from ocs2_mpcnet_core import config +from ocs2_mpcnet_core.helper import bmv class LinearPolicy(torch.nn.Module): @@ -44,10 +48,18 @@ class LinearPolicy(torch.nn.Module): name: A string with the name of the policy. observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. action_dimension: An integer defining the action (i.e. output) dimension of the policy. + observation_scaling: A (1,O,O) tensor for the observation scaling. + action_scaling: A (1,A,A) tensor for the action scaling. linear: The linear neural network layer. """ - def __init__(self, observation_dimension: int, action_dimension: int) -> None: + def __init__( + self, + observation_dimension: int, + action_dimension: int, + observation_scaling: np.ndarray, + action_scaling: np.ndarray, + ) -> None: """Initializes the LinearPolicy class. Initializes the LinearPolicy class by setting fixed and variable attributes. @@ -55,11 +67,19 @@ def __init__(self, observation_dimension: int, action_dimension: int) -> None: Args: observation_dimension: An integer defining the observation dimension. action_dimension: An integer defining the action dimension. + observation_scaling: A NumPy array of shape (O) defining the observation scaling. + action_scaling: A NumPy array of shape (A) defining the action scaling. """ super().__init__() self.name = "LinearPolicy" self.observation_dimension = observation_dimension self.action_dimension = action_dimension + self.observation_scaling = ( + torch.tensor(observation_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) + self.action_scaling = ( + torch.tensor(action_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) self.linear = torch.nn.Linear(self.observation_dimension, self.action_dimension) def forward(self, observation: torch.Tensor) -> torch.Tensor: @@ -73,4 +93,7 @@ def forward(self, observation: torch.Tensor) -> torch.Tensor: Returns: action: A (B,A) tensor with the predicted actions. """ - return self.linear(observation) + scaled_observation = bmv(self.observation_scaling, observation) + unscaled_action = self.linear(scaled_observation) + action = bmv(self.action_scaling, unscaled_action) + return action diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py index b1772402b..54ff093bb 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py @@ -33,8 +33,10 @@ """ import torch +import numpy as np from typing import Tuple +from ocs2_mpcnet_core import config from ocs2_mpcnet_core.helper import bmv @@ -48,11 +50,20 @@ class MixtureOfLinearExpertsPolicy(torch.nn.Module): observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. action_dimension: An integer defining the action (i.e. output) dimension of the policy. expert_number: An integer defining the number of experts. + observation_scaling: A (1,O,O) tensor for the observation scaling. + action_scaling: A (1,A,A) tensor for the action scaling. gating_net: The gating network. expert_nets: The expert networks. """ - def __init__(self, observation_dimension: int, action_dimension: int, expert_number: int) -> None: + def __init__( + self, + observation_dimension: int, + action_dimension: int, + expert_number: int, + observation_scaling: np.ndarray, + action_scaling: np.ndarray, + ) -> None: """Initializes the MixtureOfLinearExpertsPolicy class. Initializes the MixtureOfLinearExpertsPolicy class by setting fixed and variable attributes. @@ -61,12 +72,20 @@ def __init__(self, observation_dimension: int, action_dimension: int, expert_num observation_dimension: An integer defining the observation dimension. action_dimension: An integer defining the action dimension. expert_number: An integer defining the number of experts. + observation_scaling: A NumPy array of shape (O) defining the observation scaling. + action_scaling: A NumPy array of shape (A) defining the action scaling. """ super().__init__() self.name = "MixtureOfLinearExpertsPolicy" self.observation_dimension = observation_dimension self.action_dimension = action_dimension self.expert_number = expert_number + self.observation_scaling = ( + torch.tensor(observation_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) + self.action_scaling = ( + torch.tensor(action_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) # gating self.gating_net = torch.nn.Sequential( torch.nn.Linear(self.observation_dimension, self.expert_number), torch.nn.Softmax(dim=1) @@ -88,9 +107,13 @@ def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor action: A (B,A) tensor with the predicted actions. expert_weights: A (B,E) tensor with the predicted expert weights. """ - expert_weights = self.gating_net(observation) - expert_actions = torch.stack([self.expert_nets[i](observation) for i in range(self.expert_number)], dim=2) - action = bmv(expert_actions, expert_weights) + scaled_observation = bmv(self.observation_scaling, observation) + expert_weights = self.gating_net(scaled_observation) + expert_actions = torch.stack( + [self.expert_nets[i](scaled_observation) for i in range(self.expert_number)], dim=2 + ) + unscaled_action = bmv(expert_actions, expert_weights) + action = bmv(self.action_scaling, unscaled_action) return action, expert_weights diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py index d6cd20477..57ab846f5 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py @@ -33,8 +33,10 @@ """ import torch +import numpy as np from typing import Tuple +from ocs2_mpcnet_core import config from ocs2_mpcnet_core.helper import bmv @@ -51,11 +53,20 @@ class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): expert_hidden_dimension: An integer defining the dimension of the hidden layer for the expert networks. action_dimension: An integer defining the action (i.e. output) dimension of the policy. expert_number: An integer defining the number of experts. + observation_scaling: A (1,O,O) tensor for the observation scaling. + action_scaling: A (1,A,A) tensor for the action scaling. gating_net: The gating network. expert_nets: The expert networks. """ - def __init__(self, observation_dimension: int, action_dimension: int, expert_number: int) -> None: + def __init__( + self, + observation_dimension: int, + action_dimension: int, + expert_number: int, + observation_scaling: np.ndarray, + action_scaling: np.ndarray, + ) -> None: """Initializes the MixtureOfNonlinearExpertsPolicy class. Initializes the MixtureOfNonlinearExpertsPolicy class by setting fixed and variable attributes. @@ -64,6 +75,8 @@ def __init__(self, observation_dimension: int, action_dimension: int, expert_num observation_dimension: An integer defining the observation dimension. action_dimension: An integer defining the action dimension. expert_number: An integer defining the number of experts. + observation_scaling: A NumPy array of shape (O) defining the observation scaling. + action_scaling: A NumPy array of shape (A) defining the action scaling. """ super().__init__() self.name = "MixtureOfNonlinearExpertsPolicy" @@ -72,6 +85,12 @@ def __init__(self, observation_dimension: int, action_dimension: int, expert_num self.expert_hidden_dimension = int((observation_dimension + action_dimension) / 2) self.action_dimension = action_dimension self.expert_number = expert_number + self.observation_scaling = ( + torch.tensor(observation_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) + self.action_scaling = ( + torch.tensor(action_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) # gating self.gating_net = torch.nn.Sequential( torch.nn.Linear(self.observation_dimension, self.gating_hidden_dimension), @@ -99,9 +118,13 @@ def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor action: A (B,A) tensor with the predicted actions. expert_weights: A (B,E) tensor with the predicted expert weights. """ - expert_weights = self.gating_net(observation) - expert_actions = torch.stack([self.expert_nets[i](observation) for i in range(self.expert_number)], dim=2) - action = bmv(expert_actions, expert_weights) + scaled_observation = bmv(self.observation_scaling, observation) + expert_weights = self.gating_net(scaled_observation) + expert_actions = torch.stack( + [self.expert_nets[i](scaled_observation) for i in range(self.expert_number)], dim=2 + ) + unscaled_action = bmv(expert_actions, expert_weights) + action = bmv(self.action_scaling, unscaled_action) return action, expert_weights diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py index 67bf6822d..c80f7bc0b 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py @@ -33,6 +33,10 @@ """ import torch +import numpy as np + +from ocs2_mpcnet_core import config +from ocs2_mpcnet_core.helper import bmv class NonlinearPolicy(torch.nn.Module): @@ -46,12 +50,20 @@ class NonlinearPolicy(torch.nn.Module): observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. hidden_dimension: An integer defining the dimension of the hidden layer. action_dimension: An integer defining the action (i.e. output) dimension of the policy. + observation_scaling: A (1,O,O) tensor for the observation scaling. + action_scaling: A (1,A,A) tensor for the action scaling. linear1: The first linear neural network layer. activation: The activation to get the hidden layer. linear2: The second linear neural network layer. """ - def __init__(self, observation_dimension: int, action_dimension: int) -> None: + def __init__( + self, + observation_dimension: int, + action_dimension: int, + observation_scaling: np.ndarray, + action_scaling: np.ndarray, + ) -> None: """Initializes the NonlinearPolicy class. Initializes the NonlinearPolicy class by setting fixed and variable attributes. @@ -59,12 +71,20 @@ def __init__(self, observation_dimension: int, action_dimension: int) -> None: Args: observation_dimension: An integer defining the observation dimension. action_dimension: An integer defining the action dimension. + observation_scaling: A NumPy array of shape (O) defining the observation scaling. + action_scaling: A NumPy array of shape (A) defining the action scaling. """ super().__init__() self.name = "NonlinearPolicy" self.observation_dimension = observation_dimension self.hidden_dimension = int((observation_dimension + action_dimension) / 2) self.action_dimension = action_dimension + self.observation_scaling = ( + torch.tensor(observation_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) + self.action_scaling = ( + torch.tensor(action_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) self.linear1 = torch.nn.Linear(self.observation_dimension, self.hidden_dimension) self.activation = torch.nn.Tanh() self.linear2 = torch.nn.Linear(self.hidden_dimension, self.action_dimension) @@ -80,4 +100,7 @@ def forward(self, observation: torch.Tensor) -> torch.Tensor: Returns: action: A (B,A) tensor with the predicted actions. """ - return self.linear2(self.activation(self.linear1(observation))) + scaled_observation = bmv(self.observation_scaling, observation) + unscaled_action = self.linear2(self.activation(self.linear1(scaled_observation))) + action = bmv(self.action_scaling, unscaled_action) + return action From d2e5dbafb0b587a3cc2e2f8b80ea4a4f6ed61660 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 8 Apr 2022 18:04:49 +0200 Subject: [PATCH 202/512] update ballbot policy --- .../ocs2_ballbot_mpcnet/policy/ballbot.onnx | Bin 580 -> 1156 bytes .../ocs2_ballbot_mpcnet/policy/ballbot.pt | Bin 1831 -> 2719 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.onnx b/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.onnx index e1caeb8a1f488100bacc89e59fb5489e93e2b193..56c3a7a8e2b6f2a0973e66eafb966e68678830d2 100644 GIT binary patch literal 1156 zcmd;J6Jjr@EXglQ&X8g@)U&jD%fV&J#hss2oLW?tSdy8aC&Xwh#T}YgTv(c#T9q1a zpu`F1XmN9~BvzyrJ1{L^Wam1<EW~Ic#pRrzS6q^qR}yci!~tV#tzhP2ElbQPP1Tys z%)!XP#la=O=rw_vfuX^kfoL$Q77|pn3x@@aEL_rDj3!)+#zKsyQXIaCCBCIO@kUB) z5Ehp@7o({VqnQ+EFf=(BE3reFsEHwgi_wgWHzzYMHL*yqJT)^tqlAka%1z2lEEZxk zmtu2I&CQKBQDOnnTKrtBi8%!siK3uHv4D|Li;s&XDYXQ_=jCE6DN4*Mc9LLRz{td< z&BbUg#Atz>HcfG+%}^mmOQfV}hLkj|U@21%m@t_+n1KmX1v6cuhdIcJEL<{NjFw!C z7D7yh2JpZ#hX<A>7f>81XNU+c3+%xqBmmBVx>=bex~UZf`9&pK{UFBxbBPmbAbM3? z{T8vWGpEL3cJ)_>;40PqQ@_1FxcGDPzNCX&90LC-IrPt$*e51);XwOO#{<kIj0e`n zPj?8JzWv~u+-%2w&yEAX5)2RAYuRsasXFlh=b|8o;5pF_KV}IWcwDf-VdeWXjuXyj zIo_YLz~N7q(E&R-Mu(}9+Z<*anLB97gTlrMITF2izVtn}JMxoxzg2&vU6d?pxxvT9 d!@(%T!NtVE2*g}TBEaA<6hakcc489X1OP0c-pK#} literal 580 zcmd;J6Jjr@EXglQ&X8g@)U&i&%fh9>#c06AXeh*JBE{jHpO>6i5^tcy24QJ&bFm~= zWEM*>E?{KhO5kEN;o{B7%u7uy(koBROwTCc;)ZgQG82o17)_<v+*5ON;|-NqK(rP= z7i(fpK}Mn|14D!T0!BtHJ}#D|)Di@rmy4~WC^4_t31Wk>5TltC7trqFlEl1{cq1i{ zi@<Cx2`<*M#GKMpEglX=4n_e+FBXWIEL_@LjHW`2=2G0DdBugLsi{?|@y1G=a1O)| zsl^UV3mDmj1i+rx&B`p%O|2-%FDlWR!^pwR!7ae(gz7V|7DoQO*|y2YviCi&+~{yg z=AA=GyYzu|%q)j;_A=XZ{AYCR(dM^*EcMe)^E&rI$s2cVD{Lknh+p@4|A`MAj^ZJo z5BC48bvz%l^FY{k!vj)=VfGteUp~MT)a=mjIoGy%Uz5Yr1JCw*FaPCmDd39Zlfq`l z=ldQwBug6}2xHK*pL3qoQLv=lK}(*48R$0TVD{o!rgqq_epTlFjwzWoQNqCZ6XN3n z28$2}7ZV2~2!jL-(FD1YfP$uIg3KU6Gc-ZQBylcAb0Hxv0ic;YP&1i9v=fs6Cjgpg Bo@xL9 diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.pt b/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.pt index b5e2b6cc938e081c6481b092f21193dba4402f0a..06a0acb5970f59e781217b39ffba51021742a5ff 100644 GIT binary patch literal 2719 zcmeHJOKjX!6!paUx*<tZ8cNfq^b?psJDE%ZZ39i5p$vpq(uvbjrym)Q?TJksd#?Q^ z<yR@FB$IYQibPe3@)N9(Kw^RDE}00#3JItLD+EX^kU)Y3P*H>s@7a^Yo+<$r?0C`} zkKeuL_`ZAY`=k+D$uPCG%nhxHX=Y?c(Tz#1Q<bqC%Z;0<WCL?N)TSyFTX7T8L{70Z zEGf35#d5Z3C{M;r!_s7jJLEmm-WUf=$FmSQ0~Jw`eiA#fVHwsKu(RSi@-F3MN1o8I z=D1L)D7I;83N~!Z<qkWl=4k3cL%|$WbqG-H5lESQHcP#0bUIiowo(?$wxk>xHC(J& zc*#d{H4Jr%MBWV(8<r$-WP}6m6s((udI4?{8!0*?E90kSN0oHj9(SR^V{Q~%sLgPt zY~D1(KF~BIHc)HUb^^fl9<W(#4w`a~R^r{@nQs=?Q?rX@tVt@FUCFx9v$lwvsnxV) zHR!TJa*OA-$<M`vt>#V5g<HMB&0;md`XWH9f^DK@%(7x7udqBEY$3vIJp*k~ot@Py zJ%(*jkK(jujOiF|D^W;!Y?v-5IU3EQx*{0|+gEWRD!8^IO(r<l5siv9Brcc4BadmY z(}!>=GDke(4|1?ej1ZqVw0i+4c~$O>!|kXI!yUtjK`i2CAc{B?nSqXTUaWUwxJ!WD zv);>|0>lL96iH<>8AEm<u5Z`Lm-U^x&)fxFLFNQvF?4&()6jz=({Q)XdJl%Z0_^iy z_ZOg7fP3@0Uu*a2bvh}X1JlqaK)>#n39mN|>Avs-y036j_qDuV_r*G>`}#cKRXi!f ztR~~ULv%|aPOK*(+j+b)_#k5Q8P}VNMV1CU<hcYF6%2|SDJtV?&ZJD23n`=tl5+A7 z9wwYk#43C1C}2Q{6*rW@Ttz1JG6si2$x6T@!DI~~6~kegtO!wnD?l2lREh!&`$-cp zj0kYVTN0(uE)gFM5qHC*LE>Ym4uj|uvxqIgD5@h;6XN4O@d*qk1vr(@#L}pyHhZjN zaFQrYdwSJftbu6=?P8trRFVtqVV_?U=g*nX@Ry(dl^=Pkec|G@9~R&I`RsgV@jZU{ z&mH{SYn$d<wtQMFTpuX1I8uB^y2Ky3bY=1FiSdEC!P(;Pk98HldTybAfBOr?x;F%V z<mD6mZ!a|!zs<eNzxC6{0~bDdeBkRBU+4cgpD6aVMfi)O@AH=vdwDv8^H*oSc!Eek z`&u)o56_@0m8@N!!7tB$*Z1LdcA<A}w2yLLQNRC@))4J$aqbIqCQ`|sWzNtB|2HoT zGu2N%^$*+szJ8+qyFAbuh*!o=ULYGN?Q7LQPq+rUQ_1+hu7Ml%FQ3~~KaFYrgIBih zB9o_m^_Uu&ZObuD(gWPKQpwh}g6ifd`)99?9jzeDw5L&oK9=t^!F_`6gyB|x!zo7x z*C;*XSK3Ep-wC2S<K@`kS`1@ntIDul<=CKxVeFUX*hD!tnCme18*=o9V(u=-1~W-- zLn{rF3)>R5q)i%CSMM%)w6S}Zed)!6wx#DdJ<UnGTDoQu9K8cD2RQo6Fw~qzRi1k_ JK~aC-_b>J9UNHaw delta 734 zcmbO)x}0x<AG=;beokg`<;FyNCMjkH28QH<(n2+F#ganxKyL<bW^eAcLJe>3$$Ob> z>NC{5nM(?dz~Zfi#@>vrg(jUDteQcErX_`DL51cWV2(w5p=D5^RcQu0m}{NEnZXrX zXwzC~8&qhQ!3P$!&k*p-5Ck(FGK9gjV}>Z0cFGV3)6N-^VA>^u8|bXey!iUG)Wnj~ zqSWF-S8whJ9-v@;X-Pa>(9N5<G^sd(1t^}En44PY4p!omnU|Va6ae;Pp+^K8P&Oqq zH$F43&=Y6~JCqAFsL%_fUoXI$orC}Iw)9?LSb%VVH#5*g4i2Eg$*<W3Ma&rzi;^=k z%To1I5=#>G4cwfJ872#|=}%T*W2xuQn{AtXEPLPc%8d?}WZpT1v`ZgY$INmlXD_oo z$A3o09&LX6$5KD-G_P|Xl)Q1*w!&uOf%tWw_n-K{;V2&R`C$LgTF3J-I}e0yH#{I^ z7-qlm_2mOhLCp^Ro^x%R_cb{@J@9P5_wrv3mjbRhJ}GQ=e7^62L$b8tfiMO=`#I-X zg&YM-+8sbX$P(c@)dF;K%j7~1Sus8a2UJ%Zx;fdvT+J)FOzp5;{i@9U9aA!GK$@*D z=u8ELE)P(%yePuMWvNBQnfZChKAwDlLx~F!T$A5$n8?8G1{%Q!#0YSgd9n?s0t?9e z$qAfdOjlSYS8ys!7T}U%iea6s!=>p3iaZd2(Vrn)hD)pvMu0aPD9RX)g5nVf7=bvz Pn-$Ds0qO#&ho}VrGE>xA From df6aa161721e611da5e9e58b9f09bcfd4dc6daa9 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 8 Apr 2022 18:59:39 +0200 Subject: [PATCH 203/512] update legged robot policy --- .../policy/legged_robot.onnx | Bin 30761 -> 35344 bytes .../policy/legged_robot.pt | Bin 33205 -> 41391 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx index 21dda591c641a58f7b2327bcb59d985e999185d3..220d9843b9373e8adf31c2b7671b48f75927ed20 100644 GIT binary patch literal 35344 zcmeGE2T&Ew7XAwpB?pltA}AmTCUV--YY-I?3@GM=pa>=e1Vl^(1(BeL1POwQ3L+v& zkv+Wz44{Ij7*SL(fLSqMmb>3`&U?c5z2{%nxmCAr-IA)QJ$uh$`r)^Gdb-!M23Z+3 znZR`+tAiK&>quFQv@m!4lqw-_E3dHHXN_-gn0Lss)d6Y}7Ir!cGXmBGh5GtN_<ETe z$o=z;nS#8Ocew8wCy9J<8TpSxMX~le@?%y9tO@ZB2=THoko|Mt?DkN3=`in=p}uB~ zLuJKfjbx2f#ocO$iit(CV*juE`K{^KzLMXZeslUY0>2jZyFdNr^lJouE$VlF`pxOr z2>e>q@BZ|g)2|WuwW#0y={KieBk*fczx&f~PQOOr*P?#+r{A1@jli!({q9e{IsF=e zUyF)l^Tnm)_2flqN6}^{N<TX2$WHPOnH0Lx%hI6F-}mHAMaLY}BrMH!<UIe8#I!Py z`N!^mrZ5BKMPc&&{6v}006(t)-;j~!BiH&a^Yagp@B1Gg`YiKaBRZvpPM`6<t5$hg z8%X`}YNjGD?Y%P4-&^C4yk@?*xS6uNl#g%7zuqg!_X!F14p`%?EuJqfA+ITF&q8z_ zOPxN`y#xHcYz(CSc$NP{hvk3hu>30>mLvb6!|ES8Z2w0cB1D!}A|2KuM5~vEtnv=` zvJ)Zt`|jTnEnD+<$&uoJW2EaF9_Sk!@&}_eB1kKj1^9XgTl^_f=dX|cL#@p})Y|`# zYX6{SBT{So4{8qopceIqG+X(<l=e@4e<|&sAnkNy{`Q4hnE#L3{-(kFKQ&nVAMyO_ z=s)n-|Ea;^f7GBQ!eB4r<Det+w_aso`R{-n{#x8$1Nu|kpMb2){}gBSKZ^S+K$ibU zK>s@WcR*Gae`@$&FFO$+E74_VW%(zdKd-w##93MXC!jx@|7$@1I{FWBR)327U$4cH zqHj+$lv!EpDE`xE@v^Y{ueOUQ-pX7QZ}T7V_W#Ft(Y0=6`ycTR|25uRbbf2mW+U2c zMZagKBPVio7JG+yS(+R4`TOoahX|1oBq9IDUtleYwEy=OEG_<P3;Lon+le+&pS5z( zk^6H1|JMj^CORk@!L7}8<o_JOE&tmPE^6Fbgu~k6KN`3GuZ^Ej69rr9DE#SEd0E>0 zn{Ab;_NQ%?peC}dv}LtLwpFB>nAmyeU%N&9---I&#;?=+&FVL+UnlVEk^f)mSA$0X z`rrJ&M*eQ&*U9~6^_$hN6ZrMW|7ZHuAIomhtj*d|w23Ct)>i+Twb}l2-7T86S&Jr` z*4F<vYx`qW`S&&We=NM!RR30lG-CO(kP)J#<LclLvt|w1et#4RIsesa-0ir=wv-Hz z+wFn{<&n6~_B4c^y9D&rV>;-DE;e*3vyvVQ!PWH`-fN4ai8ph>y_KVn3*=zM)((1R z)E!bYtc!~;bA|&t>#1+;BDkKX%j%D~M9*|Arz*{q+P}U@`ftpFz=T#R*ky!Cjt9W} zxCFWkROVjy9>*bJ8BpcElxD6OiEE@sLrrosh*#>O`?WW8^!<a7cT0ioUKE2X!w=!9 zy?xldi5ZamF$i<#I-=u-6>vy7os+}_vLy5rnI4r(ZklXIKYJZi8y<?f_iW*WiYJu0 zg<-+hTjchuFjDVt$GkpY0uN=nQT&THj+)UQx96-w`Tpw!?TwoJbJM-T(YynU47B7; zR#Q;lR|5J0EbiSYK%W>jIJV^#ylsue!|6Ndyihf^YuXy<*z|^;SX@D;9a{@8P8p)U z(=h7M(@l1^rEoqrS$Jo~HfsFPA8$8Sl9L%1VWqVO&OiN#PQIN)JBM818lnz^O}`Rc zyCMjeoMs?+dp^!N`jL1HOT}-?WFeu+5T{DHqS_l5u<?(e*DjyHI}OQr%OesUtHk-9 z1*&}0B`rQ@&?8)W^(HlIFu=;R%XlE!9kroSn6d2ybv*QwnuHxDk=y`i{&t!~`pqLn z;`JDRVIqcIs0U2=Mt<5NEua68F)><&`?VS9G}uZ$l(Vp7%|uWxp3Gf%OQBrD3??i% zNV<lX!<%Yz{5-D$*_i8?JhKVwyVR*|W;05R73byOTk=M$RC%>$Dp>5%f#>$_#}T2b z=<4u}OgcqrK*>%<dTtbb@M8?5PaaBop9R9F+9SBlp$falD#D!NYh<SNBsAdX(-jvK zN#js0n3NewD~9B8ddl~RL27?^GDnKlvA;<I534|)u?;M9(7@v8t?>E6ec-EF!TPf^ z+<K|Xo*kyiKIv_O0F7=it_*^uFDu}EfDJqi-UYjK3^+ftM!MX~4RRL^g|vBLv}61u z3_VNn^4n7IeqIBfCJ)i`=>*J3yh6FzhiGZ{T=e*U8bcipWAckUJhLkX2Hp!KvZ33B z15=a`z6!9*MS$-cw~&~Ky-ZntC)Svc#an06;P&PnFw0n-x9_#&-8^*Q$Fb?e?Ccag z6>%4bUXB7`k{u(MQ6$XkGR3zQK{S2!ce-&-wlH<-3o>0|9^RN<i)_nAxPWuPV6HkG zh_7ej10~6iF>SOdS^#p}SAv9CBocc$Dt|Q*$(#+aV|*Z{l5gatSRZn2L^kS77N_H* z2ZOjxDkhhS^Y?$~L+ES*6T&L-OrA4_HN7DRqi2Fy$X-;ANT!o64FK_%nY6@Q9rqM^ z!`3gQq`O-eu4&1kQv5+2!kB?{QW$ZbeVF!LYfhE&wDGBoB0P#145RXzxbVoGkWgzx z?W~0GNqP<%d$P1+WeLpBTZHou>>&9wx8k!?Pw}k3CM?`^n(Ui5gy#lI^7ao>z|kxT zwDs4ZN?9llebPYgx*Jft@`rRt%m?Z?+y=jVZKt8D%LNKHWu#oy9;?qjK+jGqDD4;y z{VUbzrH&nNr1?1bBqcK*(L3Qo(Pikbum^`Jms5p|6l`1D7i#*ckm{`(WZmdUsOxKw zV{4M|X;Cx&DA|BhvcF*c#mngLq{BbBr^|Q0+KziOTJVzhF`Vshg%vAzaC3VGz@ob8 zv^rOx61#YkWvm2|ac5w>=M|L3+gP4b3)7PqLqYc_${4oLii@Rm$^8b9KQSEo$~1!Y zSq+@ql<g=N@|lDzNQ94{G-1!aUO4M`0Jp3NhWqm_J8pgYnMN#l0Ee@h!FhWh&hPL} zyskKaRr9|O@}=%*ZUi{!qX1rnE8>lm$5eaCC5L+JZ0hpN0_zIaGOvAV9X8a}fa!}$ z@SCcKA08c}`-h0p8C5or*L#(GjVr(>_m6QOChiBnBlqxqS{pUmau;v5Edo^wZSKP4 zGTf_@0|!4J!SgC3VX_MWPTCiT8YIyPSyJrJM^B+|SRY(+-~eVfZ-x5{ra*d*0;aV{ zFe}#06qxVm4>yKngS(D4?if{sVF$vf+-E0vd+!KbT%eC1nGLuxNrv1yY>yFf3cQgH zM^yGNL*~6cAOHCz`1EdJie0rxqG>!XxeCIAyLJfM<+kGIOWHW7r3vbsE`hu5LHwvr z@WdD;*3nXjop7DSS*?T6VD4G)xsU`2Ter~zU1P|awitnnrX-Yq&445617Wm@Dt7g3 zA@gR`!Pp^pV6W3uDj#Hxu_G^%v&K{KLg*=+GOdf;QBlQ@9;b1<%{Hi0DI<|5q#2X< zYPce}jk6l=P9k2<qDP;{;?8|fnZx7zK=RPVnEtL8r&}usJ}PL!+I_mb*2!@gp%@9W z`5B<S=N-CSdP`)NZet!B-K2RZ`#|DOS6I~eiFT_x5goNPm^G*kyA=u`wO|OSMmG_y z_|f!9-&)$+unFXh(@5^#TlBj8Hu$LC7qiTwp?ceKI@x>&nJ&8@KGtR<EIA6@@3pC2 zy8&$|Uk%KVF4#VGAP#ZY!&+ZOc7pR6==oYgXJu_iBli<<DJqmkxXmWDL)A!g<Swpy zAE5rUM<hzSKRPaVg!>K2VErzK3RJeDyG$kxt4t*)9}M8$H<zN<&?LHqF5yfzq|gmM zK2&3_=)Qe?KbfOiL#_9f;kbpFpnY!{8tt6{>CX?sig9*m%rv3=q}Qb3vLz&KkmP%g ztU|pcQ=qL;aG!5MSR6$L^vflu=W5bN=1!!T8iCK#9c0<X0@_wlPlK+mCVMgyi9@j# z*AsM#RNi)=6{nA2{>A06Ipqj}?!)xPju23|XNsE(qd?K^FuD$q#+%0iAY;QR3<{Zz zZ<VSs>efR{()QzWe@gRDXRGkGn`QX4ajp1d{#@Kv(~4$V-MIeS0Q{OAP0N1dGE-*D zP=*_XW%fRB?h3-a3CTnz@;MGl&cHU6n^0gJ4iSTta8%(^+Us%|?)J8jIL%?OGv*|0 zZWu%zmhL4NilX59{X>vbCJQoVyU3aB3?kX*09q!S!&bxNF!FIAdgg@TtBGRl^6~0y z`rIuzbH!O)ygVOV$vFrzyp2t-Be)|`ZUXn@$xt;}2AG9joLR9o?u(m=tEvR>E!YiQ zHa<h6g|qM*IZtI5f2YA;cj8>nr5J8hg~U{ur*ct{ke~wVm)SzgbxpkLE$U}fi9e9; zS#vmcDky!p3WIG5Anm6!M4yvk4;{J)rKNq@f*TSnJr)Vd{xQ%ncrP5>=?5Q-hhx6! z5VTSQlI44fNM)_0<+Bs<i+Tf&(_H|!7afGw9p}(;tO`~%o~M3Q4{7V{8EEb2ila_W zLkD&(o>=ArQM=|aZIL7C<--2>f)PiRT^6|2u#KAe%Jvh@X8-?fB)5sNt9I~*=kMf8 zy$bNFO(MH`+i^DU)lR;sqmtE*oxv8m9%ik+7qToD!+$z-h;P*5cw_&W?BiHVzH|Ca z_C!=ED|0-Azf%y!n?*~=O8%{-f1fx1nY(s-EN-XPhqt&@OvN7P@i+HZvR+5@oi3&v z;g?lMvq5J@u=Ty$*n-Q2Y_L)?FPp!Rue*}O?|S9JHV8fVRqCa%Hm#k#(O||esI2D4 zpLoj#E*{|IoEOJlR~^ZZJ><`~pB~IMyi;cT{mkKI!wuP%vfaE>Mk`w%DaK08RN!a5 zO5=CU3SrA9Uf~A}b>)>7X!8;JHy|@bhzSGoG5A0fU)Ute(<x2-I~gnXasNtwwN5fr z%RJ-{9+}Cjuan@Db#<JQUa31x>>0(X_5X<l_Xe`*-!k|~Gb-5=f&r}A5LwCpAJ%_2 zFx(XNeyGv2PfYlQ(P_L{oPn&~-_{s^X&L?LX1TQwe!Itq)_hRnsuPva-M5A7o7zkU zDw)!@okMZ=r#4z#*hX)~Kcj9Jb>Mlv98J=<U^4_NP@$Z_&p&>NFDi4#LlK4S%Dc7j zFme=~RHunwe(rz;WiIfsBMra2^u{M@6G@n<G_LlZf#E;(iF9MNpkMPDS}WN}2h^*= zP4!8b&=Cz%@io-=^J}^lBFSy1Q6ynPKh$zPLOe6;NWtr9t|G9UJlb`StX`K-7CpPo z{jl9ehOIH<$6U;1QgdcuX!aT>DT5JC65{Q=!GZOBvs*At)YyiS>RD*D*At$6up;(3 zt3cO#FZMe#3-a$@C+GY&l5g`r(S5;&@J6hRy67K7|2O(Du;(+a{`rW>Snz=kJbaL< z&J=^4^<sEe&5<O{e@;5?m~nlV55O{S9UQtq8ND{>f&+-doQa1}IIJ1$7EZ(y3CguE z<e%~vW4EH&=6QUb5)aFNYH`@I5eN47f{Wi%G2Ce^#>Z}8T<hm@bE;!;MdDp<qE8H* zU1~;c4+hgV-y*KwH=Z8#8ALNLpG3F)EzFP)4TRNdAgX0DaP)*UjuD$nUoO-{r_1X| z#)F}ZO4deNy`ztC-yUxoE_;T`y$t~4?c=Fx&@C3z`||8=b6i3O<Nc>OB%-|q_w15^ zRgwcRxa9`+r1XKI9TJ%Cl?%f^iot=34e0dx4N;uB5;ImdV{o+*`WEjN3<+_DY19KH zoTV9i1yyvHd_$vzeL>@~33d83432n6fUk@UMESSUblVYl{fHAIqaXpFdYV8_qk#Sp zFs$mO6}5%7;!cS-U*UN5Y*<t|l8c-C7FHb9qc?nh!l<GINIf|UX}bmS?BdCA9S>}{ zm<h%PJE~n2w=&u0Uzr2m3FQ9xTDmeP7q(n#z$%~JWM$D|YVznBxzO^OK2k3yE6l>l zGFeH`(97qP#H66G`x`Ajsz}GLD5Xui03VL-g6sQ=xV#spwKtdK@>ABm=2zbG#@>h_ zK$lGejT{*|;gu<Vk3Iv6snan&-3;BM^oWja87eNc$9{7az$1A&);4Xeb_tyU%q(~G zxq6ukT;PS|?mT*XY#{wOq=k-N_>EEZ)P<`pmq>7V2E}cCsqU2@#Nn+zlpa4x56{kG z6m<=#bV&x6^|}Nn?oo5P7=Icx#Mbca@~JrB@f2*(WbvYSC--8+9$XnA3CB*w(0P(0 zsbQOtZ2q7R86#(5uR<L3@0>|(wzQB?K^on+wv)Qq_Rz1lD!GL(TWHG6FwtjW^gBpF zm2@dBeszazt;%EU`o17D0?!dcG637e4M}F13E6wPKYPMmoLxNr8O9EM=dk!vFO2(? zhP4})u;0&(BTMsb$h-I)eE6lEWLem<O5Q8+>8MDs>|I1n9zLPpA1afzc49D7>pN8_ z+5kjBKrW5GkB--^pz-}Hdh(JnH7U#_BVs#=*-&v>KX3;b*=0|iDlgNYFSe4+0lJKy zYd0}$lSHRQLlAHaO^QvUf#c^8^LuXGboDwm$f5_-JF75ogEPuF?t!kOIZS7{AvhJ9 z2?LV~VNv2+vbIziF8b+_GbhIr^EN+hJ8+5?>TLnD&|vD?u@eTL9gUNeyy<h7&kP%J ziq0{W!;i&#sinhV8XeI}Q>xVQ`ID>6)499oH+C1(@hOs;@18~~O<#j=b1^=&k>s1> zuknlJOq{N}r=pJEC)hPUo_?(0u-;RVEict$17FO7$9xh@RO<jK$qYPZcONX?_(5bq zs!(NmBHSK66Jo5!piw|7d>JazuP+A}KaG4Z>!tF}#bkfT7oxS~4Y#&Df(XhLIBu>E zGrF!1W?xXl0pHIsO6NDzm#e*4FU1`^H`;}TaD&=?LB|ED%8Pmb_w(47=k}mcdNOPu zq{KT#C}AE(VxGSwbd|f2=jN6$J@61{>P!SD^H*GuP9!vi6@h10kSHNx4eCZIuxYO} zbaWVF@XW8Am*RF>6@8L=D`wE?N1xFk`hun#>>z&c?oz$5bL7VD3Cy?`lK6hCD|B9p z$B5p0w8S5ryhdH)YwNYx8^uNV>Bkwm<W3b$UD!y~EiIt>>;`&IX%2j!_lSlJpTwP8 zKZ;n_X+gQII4M-GBtyAK*xWRj+85}9-l}QXdMSbK%6Ubv<?bWe$q(qHHy5bMvJ=E< zmI-n9{w!FTcbYjH{gyG9tU)fe>?Sjdx6^kQE!fU6I&6-*JWlPdVMo2b%+D#3sjU+w z#P-{H!N@g+)VS6Ie&nf=>JT0N<A@UKaOePeT{Q)B7Tlt}?fsCKs3kW(6w+O@CSygv za?<Hi!hBInBE{chXz!Z~jK#9CxY)`WO<Rx*nL3qjSIHp)$$QNAPkQu&g?^2UdoLHm z#ZZ|p8JeA?1`)|0VO)_eJL{6GQ|}iIr#pQFtlGm0xH@q@bJt0Q{pnmo*Xk9)#O!#~ zimIg9odsmfuE$KXk{&5P@|>J#md3RB3ygvVPmXt3P<b&)D2sawqpdgN^>#P9;%EXn zF)NI|bZg;O?v=&jyQYjCnMyyH^~F6i#|Zsqw-OVX$Mp774QOdmWxIVIvBP$z@DGOU z=Z(%MvDM2n`9050g5JXMtXGIF|9DdwDawuooT9_piV|F<>$gMX_8E-w<4OD>?GAWr z*hR!9p2FRVTXFKULMZR_;ENmtpzb>yGlr(qgrr<*Ib9hGw3X0>c}(UeekNaEd?(#6 z8#ykcf~+;@4`TEBqVCCSw8Ho`)@?eE1`5i&UtvzI&J-oym~`-ATRt&yYemVG(iPYc z<IU&xXz|6T)%ixdr$ook8gGnBg=AkPJh8?UL#qygev&hzt7(Rb3r-7n&uODEdIIWn zxtn{Y^N~Et7>1T%lJw@NFrk&y54tF%i7+*fNzJ}bbmP4fM6&oAP498Wd9E)Rx=!0E zV{I$i_q$4uTYY7B8}{LQYLjsH@}=zP*^zkBd^n#ucn555d;>Ermf+-ReOWJ)u`uAG zHG8rA2&T-M%-ZG~bIK#c*y7nk@IaC{tUD*k`|T6wtCO?gnv4hG*b+L!BZ_99zd_Hr zCXv}{2B36X6@pWW$lY8av*Gnn8nGiwnEY)J<94<Nyk<2rt=VZf+18}iC4GGDoVIwF znpR3I?rMSWHaU2xx)d(lr1&!M46J!D7&DKar;|M9faU@T2s3Y_Gs9vWmP#fPcDX&) zB)5^L<GQF#p(F0C+C}WD9EtYvTzc3|7q5MY=K>DMf>!JZl%P^5Q`QG%_VytI7zdJ^ zCCfC8l7rCW27-Ra)2Q~|)8v8rDE=<5%5UiJ#RHY)2TF_P1=VZdsH7XM_-;V96z!*z z&kZ8}bvj`EK^gO>UBI49JsMFMPi;rf!unI@OjvFOWT*zwF|XWUwAvRssH}|H{H}$v zRx%_`W;;oea0Mp0hZ6B9&SU2frt0l??pUpmz$6RKX3H{Ro5e0z(UD3NL$1I(Q(<if zU(fC_GIdg(^qs!@dI=_)oWXS7l-**i$eyFSYag3gk(<gba4;+vD-uH)SCcwy8uDJW zkkqB4KGhLYbPxjV2ST`5Gud;I$E?O5bho<_e#Fbn!jRKMy~ve3w0<enwrM4CTdxws z`fL)vO9B)dG)3b_5AAODhiCO-Alvm3E}Y9_SMLxd)(UpAqmSjlnw@>A?}BDxnsy$B zyUoTh&n^(UA}PI8jdT3hlI)YC;Qj2^@KGug+bz{$AiYZutW5%!fgG0*zaEpHE&z`P zo@$s$pu{{)EbbP^x`~@eV_^jO`hF6T5cg&_xmA<6&Kk}=d?>tqxsz=CuEM4mi?dW+ z8{3YH)wVUK3l_??v!g9ulR&$P;C!0L>$l!ARhQ%O#rOWKa}a|eWBOxpO9|uqKpHR3 z9gWYVT8Y}@GHMhw7=0vj$Taa>Tygaj-FEB)@!Kr{fy2#k&JZzJJ9HH02qjT<Nxg8J zrxrbJ-%5fm<#1hV%t8EkjAMkjHpISDWhGFvcKef~{MDoKtc`9IY&>bg-n)^_!Qo5j z@MsC2y>mM$Rm_Bs2S=d6YZZ8**^OGK`mjfoJ!#4J5p0p;IUKqug((Sm3>EWvRIH4E zbwUf;t(*_Rt4e8<Z8tY`UVkzyelqEK?@5FEEuzn+>B5r49kf>c1@lRjA%=%l@L8B8 z?c3VJxgOQzl~=gKoBi>;@;zJj-8@TnV|o)EtGSQ9x{(Vrr__UQw>IjJ$!6{ixJ6P{ zt6*k<GJGGl1fHLifYC3vVPLThtScOW%fG~eqg@G!DTqg1h5cm1?PglDyHMEQ*&a?E z{6?PM{z_wfjZiN|1MV-&rSGS7(CeN#bdg;I37d9{?3*EviPb7>{r=VPC3!15`MC`< zes+Jp(&QPI>w3e5Q8h%TrkRnmXy!JFje=Im4$K(020qt^;sRwIetRB=G1+Uu<y8{O zoZZDdJyH&%rYEBN&puFc%NOsCm`z?B8i>AX(T;@|&yw_tL3HW)<6KdgDAN@5g*FVy zqbD0&=+Ogr>4fAcu9EJgsbg=#^C3IHVYW1c9V_8mCnv+g@h!M?awO0;Ifw{;O{L~F z(N<<Cd){pzzHS=|6$d@Znn*eRVN@(eTF-&e3TLqAPM%0(A$R}jc$yVc$|NPr;c&Zc zbaQJvy?#24C<Q(y+uLKw6{($c&D^a5pGG};R$vB-u?8UXT9pe(9Zm-6U4m_ICV;eo zHt!NXs&?%G1@>9q7Culigl=3vn%I6jPTvh`Mc2vYkR&6+u5WopZxrdn@?ll<ozX13 z=$ggMIhISt{@jeE@oPaMri>O_N8)E}W+K<D$2A}3Q*tAoJR7M=n|1qu*nlEBW~&Mq zj2;PnONK-E4J%I9J(XEpDuW$Q>IH*t81cJ3vryh48ct$D?f!`l?189a_VVc<exuH4 zYV>9fnM($+Vh=wNu5&n_WZyu3$SkIPRdevti1W03<9t+je2ErC|DbZFQaHy=0j~at zg=fa|(YHs3-tyT+o(HJlmXR8QVpDloXCM^jo0c*v^JK}YBoo@a`6qeN#*-u&3(VOk zj;jxA^6M|S)Si8|ogLwv1tw3d_$?yCaZ|!urvCtAzWUQ)2p#uDuzi*tn%t=)!TW^d zxcw0vbiEe7#Z^Ov8HJ?4k^I@Ya*&y%z$(o&<A=XF4?d>a{3nwc?DB|#WUCRu!4D>) zW{(XrzDi&rx_2#*=m&*1k}!VV0C2mzlOzR(Q0b(@RCm#Q{QhnN*_15Bjy^P@cFL^* zwfQ~~eBNhQewv{iUETPI{4||PEgI*-@a^5O=tz8xZSN8qT~SIlDca%K`ZyBb=O@jm z9Zuf`C)2L?qe)*$f8miEvNSE@IyK7@m(>$xO#ZSs*5Yqf#%|%o_D47tMlfqs+)mv| znde|oR#kK7RP3p7nk|CJ2R!E{Yb4aXVh=W3Q#r${N&?*x$xO4iIJup43>SxwC+tUC z-fX+Iz)j(bFi-s|lN!tmmQBnT===3$vs4+u=#}MyF{!heio{XeXdfwogrqG$xhR4& z>(Uez-u)r(FVi&t%x%?bcDym8)#II^x{o5UVjb|kYZ-B<Ge*8x3z-iISP-1!xQ^LR zDxVDCk6rje^RIuv!nONIP5Tm2`eYB4^y!0(zwJSDGY5#xvgY)AvZ>v;pVUQh4f*1; zn|8+WM7P18%P7`|nFhCE(YPqGz{QXh`z^ug*Jfir8wC#S{%EZq!1!Kh;VwVw5;m^u zM#*(%@N`3=po_$_eHv>x4<A_)=Qj@>*9XJ1xWO#QI>N030hqeBg*;J?LES06*pl}X zgC-q<0u5_Hq#J|F6eIDV>p^<xgf#2dT8~lUi^u@=?XY<<!KJ(v-Wzolr3M*6@R|tZ zvvP>vMKxCSZ6o>cVhlUxQ~^xivygZ`9)lAv)}ms2I&1QL710fM;2&HcMFJKg9jqD6 z_8C!&i=1siy-S_fF&&O;7fwc>G!JC;XRy~tYTM_^-@^5Y_n9xUKWUNdXVC6Qf*{#j zM0-{U7%CFDdncZoSNN103pM0wX&VWW=g=ucjQRdH6WO`eFym1Q{kCKr%xN4$H!nMf znhW>Q5qTw4{O1lls~Lxmjtoh&>xU2XZ;=IVJ<R?&!(d9|95Ai8L~h>rPF7`}gmvc| zp;9Fg8r?-X3I7V{$DSb3HGofIO|V_EfzCU66x@!!0k<A+axmB(ZkULnqxd=0m>$Va zQwjwK*%dV1HJu#ZpG1dWO|DU&GY(a?E6@>dkq;l=(K~j@7@PM9t~H9<4892Y4&fwK zZ7M8Chym%FYIwqb8+S1FG`f~_<FLoN@Tk-u&S=afHx;beIfrD)V3!8sS!GCF&hLU# zR|eyfe$TLSup+DMrog|>?Tho>#_>m#Utz{wSM1GN%7)A@#IXX7X4_xG&%=D+S>+en zY2O2uKKgv(w~<ui#}rt%ESX-^u|n0brDV3D2Z}4?)A>6Vao#`bxaEE^WXvQ}e#n%o zWcHeBY@R9&?sxsj*p^p<T0K*gEN?|Y77_yoMXuN`o%-ERpqKBbkuk$QkQ2!TWRAE# z88&e}nOG^yTI*C1<+YnY_tasM6J)@9OmoCtk+V==%!_WyEW-IWljzRxD^W&j7WtCT z2p-wKgoTlY0^Rnr$PO;U$Pi6v>Na<rta71dM4=6=zGFolCO_nyGoI6ab<45v%LCB- zlmN%ABI%1{RZL6Sftqg;*+mKkSiOEH6(lTz_e&m9E!{v+-w;O^`y22fgU`a|XMJJ# z=K*l-dptd|+=X)OC-G%}S-NhOBF>t)i@#=@4qv7#v)}d)<p=llW4A`%p_cX2*ai`| zu<mK_?u0cz*|0wiP#2{tdY3`0`XQLI_Z!hjQsz5#9e`C&rOI!_+31|fP`lTPge_hM zI;T{@K<O4Ug*uVFhYN|qpjOi9b^*7`?<4CLhTz=^+K{asNqY)IV9SRp>Ug~iVdZIR z*~8Fa_qFJ{#{<_)c*?L-Luq5D77^X?iC_2=VsdsTnC3MRwYkyo?mokA)nAIyi38wC z`9v^`J_{P_CsPSGk^d%Vi_WQon3WA>fV7e7K5FMiR{3G}PZ6s=&A4&S68P9u$jtj9 z2dUdz$ibp}WPX7TWcMqC)kEjtJCToeC~6>{Sk;WLGOp36@9&}fNjJ<te2<RXDGBpB zXMp?huSE9XG5GN^h!32ji^Zo3;l#-xbU9y06$_SPUa}{@Xml>QUp)>6-+hje&Rh8P zf?;rM?f~|}f>Pq{pn{?4v3z-cC8`=0Q0=>u*qE+Vod0YK8JZ}EC&cRT_|6VuRm(x) z%WAALaiZTkcXPK_sxxMfa)l4Bo+1`rdx+e{YQgmRw}rv3+H~O9$>dnrDY9Xd45k=L zleT*uROj&?CM{i+I3+IRrcQSy`bqDFFIP?yq+PJ1(;waAYWBG@`h`}4=@naQO5_S? z*7hWl=5d}Zf2PSi?kyt<Yd?~2NsYv==@@->s)`;6N@vhO7K+-Fh@uMR-t;*_Bpi~+ zEnjt}ud603ns<{ZUN0f@1AWLM+D=p|N6~AarQpi>3(SI8B}RAuY)Ym{z}78qxXqWv z;BLYrx_m<yvn%Nfd61q%a{YEO3Iprtvd_bC^-(WWsUHcZz(T~7r_2ktP)y!^o%-i_ zkmDHyr(YR|Qxl4a?!q|Qb7MC#GS0#3HAu^5CgHAPXA*Em3)hW3;+U|uiR;yvN^(=? z;ds`7lts<OT`NaIac~9oH*-O<)EpOYlcra$$3w&QP-e7R1J%wFCnGeqIUVN>WW=i! zF1N>&?mp1MWlhL(G`9QB#Mwxb>EeB9t+^`RnYL5#y1<_sHZGOC8=vkth<(jmk6s|) zz1jsoy34tnE)vj`bJj8FwYE@ip$|Q_-Gu4U*g?WCDu9vYN?}JwHq+;f8OA>eC1VFk z;LXn2BtW`~9B4U3=SM1Vo;#)Jq(?WnmF#{xu-+8TtF@9nCwj>k&n@(AO(d}mdO}y% z)(e^!ZJ~1`3dpyvNN!!pI_fa2iPWu$Wz>TYGX8!C$f;&OGG=)&-SKt?d7^cSb5ks$ z{?86WlcxsGik%BNCGvDby)k}HeM~AX<Veb)C^BnV0mGDJlSm5(+`eKKmAPNa1v=+* z1r^oQukIA-7CX)@GVp-%0|)4s_pyw9*f?@&WjK}?o6z|_GIV?Y;rQrm2X&b)3v=oO znCYQRVq$iJ*EnsO8+n!6eRC`w{>q*?_B@ezb=)Eb>y9%IzB)NZ%sRpJu=_~R5H(UU zQ5Jjh=9B%8wo-1_o0_1FSICR(CgCl2S#oF9cwx~}31(uzN^WNE?V4KaXlD7j7G`~o z7qwj5$r*o@hj?2lsGWX@Sv}H^%os5gtmHgMjp_?-r1yUA?W-6fBc+Iad=0=<Qj62- zog-kUi<WX1o>H}W_egGsG$_=klj3>ybXdW7GQU)odtD|)`fT4%`b3G-P@Ob7);@&% zxHMXrwABT~=MSW(byLZOJ=aOs!Kd8YD;JpjTpi3#)<bD&0e)WjiL}Ld6VqyWR9U=~ zoHg2qFQ@0yP_41d=JDTZvfgBZpxKwY7fImoQNz);Hj0dzZ-yI}^a{S@4<<J{<Z$e} zp=A4!bELG>keWYT#;s^9!HlK-xk<~5nR&-OG0er33lh~dg_iSh!d8it7kAK)-jQV7 zkx=HOekw8C>_Dd}2)S_|g2{e|Jnm!QbY}UdwOrpb14T=repu{(k1J0ZKwEsHNzwEa z;pD@+$XnyKnz~0mj7O|JCA*)HOpSg*n{Q5xzgYpz>2{+f&;5w@fzQNp#%;k9pOs{t zs}e??RiUd|q-fyRyWElw^^7F%MxA6*nPcM}@kN{#bXLictiAql@boBhe((%pmLJ6! zetbx5)#4eWL%X?cd_MO=@Q$)0jxcKJ$}qvw45k);5xV%LaZ8V8lcNjk$cld1r1AL` z0&R*^yR4Q;`H})XVRck1Q3g~^FOfX6OiE`f(xZYtR3Nf&(k_SL_-RVm^Q0V29@;=( zbuoJNViFA)?ui>q>S>79HfGD-pJbPsEo5%;=8iC9@W8c1%ow=dv3>bA^3`4+=RQ+H zHhLX5e$ZOnm@twA6)z=b(V|CO{@XI(9}emwee)O^JH;CZE+2`N@n%#n;5bzOw4&=? zWK)`P3>T<|qs;XAw8?)t+_tz*XRk>Wj$VyIp?E!0e$$ZCH%(%^7t{*XRxRbmYr6^k zY@3<sdv6Gnbcpcw00Hw~T-iULMEf^aw$(R+gleQQ`RB{%Dt9kV#X(3M4m{)5WekHu zBU!Mj_<?&j_>+OX-|5wr=g~?2A$@j9kGGC9L~EyG{Oybm{=wT&cD@+miUp(iqDj`I z&f+5+T9eDK>Q!_4;(i~3&(9EEoY@YpJIYz{8`k_N<?#^RJefV3>4-V(E_}cb#`dP& zurJON22QA^i^bYVR;DJ{ZY{)~fvMzdo-Y@Zbe_IdTu;`UI>Y&&4II?!kl3(%xH6{> z=F`*s*^T?K%{7OAu%#U=vUjkXf=+-lN#V^?yEyc0pp!?+Ir$tBIMwMLVqXr{VfzGc zhH+w7nd~+mtA}oZC$XkbmKaC3Z(0ED_bb`K0tMFb%{;7JDTD6Q2Eg}koZvxnyvV`q zCVX}zz5e4QscDWT7nZ-Lr{$EfeX<hX-m8H@BI}>ZIrB0z-Joc~cJ|dpHLz}cgnH$V z+1&-3_|#2qc)hoYk#SxPAzHWCtv8}s=?Eis%1}Kx{j3BgZ*idx2F_qR<1E-d*n^i> ziNTkJ-mH1SQfj$09Q>ozMQ*tguH1NpY;Jx-{I(AOZ~uOnEzuyzS8Jmi@-|ax;R()X z!W%M1#~+=<l-WZY=YqAvN;alt25V+f%x`+H0x!e7$al}%usStI^d0X<i3VNFh|FNG zFJ8!w4Vwvj^3>s2s1EBR=}khH9e|p5L)hT-oe(=!lD#xam4&cwQth4ul0WWYy#vyw zjh(^|y)JmAXaKyQ(McY(e_)IkX~Sfx&!pt(Tq-_83hqx?4NCo#S@jz=F!RWM@Yy`y zX<>=1lcjwF?5pbwty7g@kpFiG7TB|!e$Hd<)?UH4X^F73z?7{K8JwL<TA6CqP&lP` z8jj?QW7|d#q5j%yp`-H{XFl!~th(>S49wGpWBQg<ZKgUI%Y7uvo$k_EhxXFJPj8US z16u@<S*3z7hbxStAX|8Fdjlz4bOr_tSw_nJ{qXIYRI1ijv$mr72aH?Sg>sWRac6!M zj?Uf+@@l~94D#mh>bT>#k=LMi<6fH3l!(rvtl&ZoN!)bXnRlLXj?;Z22Tx6=@t;H` zPm5ZDxwJd_?AuLy=#t(8)b;~mm$rxD>8!Ej<2Ok<<WVFJRx!t8{nfBn!-1Q$MH@@B zhp=x-rP$fWIULtHzgB0QJNr&LpUs<n7#a*)$@;RRpdT8EZ@PA~+Coi!fYnz1YVmMZ zO-7lW>X<^EDztH0TRrxy6T_elEhxF72rcvD_z9QCv1DQ!R4(_YX6rZMEceAEqGB`M za9<OLja0(urZ-e0)JWj|$qK@#F(|&-Pi8+E3gr%t?6y8%saf1d469jE8!oEA@Yv(a ze)}xrba4G*ev-Tlj&ofD%wTKqUv-<GEZ2xbDmOuU*m+u0BhTA=<dK#aak%O6XoOex z{P`^#aH7wB%zkm33Jp(_zN1Y=bvGZVid_m_xA_~mz1x73^cK-k(z}U=#2B=T8xAwh z8q&FEWjWLI8pw?G!N8Dw=t|GW<b`#-tkAUfZJ$Z(lXb}?GNKu}Qh-fJnt(*ylDDa) zyh(Z_&RQD-d%lgwu?Nifg1&Kh)RE$}EDPStLx6XgG<tB{WNfGYyw~$t=o{$+-yBQH z9#0!=o+u4f<29H`k44oMd>!2oZ-f`b^N8`8eh^Zl#2oZ9V6F@wO%fh?;`8Mhp!9A{ zEj=)kz2Ey8(qlYu64wg7Hp;krWG2~mSb>jPx{)VURWw0M9$sD&gE{?I;1t>Oq7vsc z+*w$FW!H|8;s81Nz->3?J<Y|<XD*U+!xTXCcrktZbt&1>n+F|_v`K?YIW1Ygmo8dd zP7AYCA$M3RwTRV%W1HR+ZHZ*438`S~{Yz1Ip^cMnhf!_&%zTLLJrBO^@7UKD2l1Dy zCS&UiMSkPYL%e5B1>F0dMGAc6uy$!4Km49F+i>;@MqeGl+bM{_@{5P)xCQEX@p~2O z5A4Ako@E#my@FdiO9~UlDe()3j3MFC{m}i6CTKP&(}2bvs`j>*dM>yuIP5-Flpa|Q zlOH+p8y?o9{DG=k$-8HGVKM8Jp_mIFHJy1)*X{7)$V}L;9F1JV6!uD3A84J{2c?!K zae9mQfPCjdxTn4r))EU|T>B;jTN-mFO7F>_oXKc>@)@_(_$Rn`PJ&)*2e2)81r|;1 zwC95inE1ytx9&_KD-It3oyIS8isNnS;&BmT3o`l2RrC41i|wGes-)KC(muS^gisJ0 z#aBse;wO*<%#!cVr@iQ38#p$QZS2hjW1UT)-~S<8zjlIsZ)wG&cp83tzm!$jnuEIr zkEJr(lR<M&1}2?R;^X{nVb5w0?)p?++!rnhm4%+fXVY_LQimZ}+;YP=QBA09olj;+ zP~ox{hw<2vh3uC2EdGY;bXdQ>l^+$k9U@kzz-HHUcD?d?K6=J3)O2!$z;6|7TB5uY zX^LZWCs?uu>E5Ux3f$eHhsng#D!8!!N18J@3iB?W1?}h@w!LZ)yK9&Vd(T&gJv2j| zvyC_;2$-tI?pZOGmL3Qri3WX$q+9?=SCrvf-sxf4glu~1#wV<3HsXiqMYFs2?}PW@ zA82^b86108kvAS8&X3F<$3F3~L9aQYd)4rxypooFZRm|G{=z&DbbtSqIzJ7=){22} zAT<pWCQ9*O>VP?ic`RC-$=_Z@P*Qms*p=>sai$rZTCP8x{&+2Pl}%w}I<GJ$R!wB= z+*lfa#SC}yCCtIRL*$OpZt_+41f0q`0`EjqNQHBWwR?WV@vXZB?0A<*GTqOQO*k*h zUmoJb8W_)Krwe6j=N)&YH}n=_&GkX}Jw1kZ-S~#?x3xra)(|#-9L@`lh{okPBWc1o z9ac=3OzkBLIM>k)<ijR?Dn7CgXsswA$@yDpt5+Ya?SGKoTJVHX+-1+Hm2M=lOP#6g zq$oUV_YUG)WBJ^?7*=FHk|CWd*je4t{7n^aR*_WU@@+wE$YnMDl%+iHxS|2IZ?3@W zM>FuTz8Krqq5)~1jihmc4R7<xnDt*d8OHfnf<a|pUgzp3GABP188rrUN9~|0Q5m@D zj4V`yOeBY2>ci6smx)n>6pUCBL)rQosx6)k!%y3@=3|e-+EtRQO}Sp}R=M;1EPd75 zz?rtJh22PIZg3j?;cUf!YYWHTbt7vPRe4_U;20CRd<)ZfC5WG~<RLCH&O-+ibN+_K zR5I&$3+@bY<Ygzlf&j3E63oRA$=zf{;%+8u(i>EGbb%9;e-n7r%W|8y2(UQ+1DrlH z0BlFfVx-1Nu)g_{+<0xtuYYj`LiQcx@5E}=E>QJkx13&0#}p*<KT`7HP(>UKEgH@L zTy%xq{@#dxuCfN^AG<`P)RN!@n4;Ko2Q)GAWZNbd^MlW?hej$6i8^h}MDg7~Zkh>= z<Q{V?cXd&>%#B>c?G*AXCz>-mqlgo23TU8(1U0hWLR02wgQw#aE@;Y19xojfRS?~U zCBuaLtA{+>6ZVo#-{8VOZK#7Ti+gy@S&rRCT0y(Mj;~gT<Lgt8QI{Q6+_h61u_<5; z#0Be+wvsNoL0aTNwf9ga?L40_)QsjoZYPI6En$+BW2s}}9)hkLAgxrMsE3~+{f`>s z=&O2U`9>sLTO=TH(wkF73aT*oEFz;L&bQB$V8<+d3@KwRYulzPJ5@S5K*h{c_)=8$ zICaQjcIGh?zU+D;T^2Nj9i|sgr8oY7lY$&vH2WLgDSbzBlnnV~?OR~A?iG~GX(kb; zq*xE9dD!1^9yC`Mk^AjMWRrg*OiG^1IR%*EtIew%WgpiOWlJGm{L)3I%w536Nay3K zIB`CtHjXcTv;saXXW3)*6su%Kl|qhXIB0GPAGYWOUhzK3EJ+&A8|EuH)$~TQch=vA z6?a>S>Rxf^|H+sytQ&#1%ftA~lO*`K@_Arb&!ME-dmwr;to=bVwr<Ek;geiRRGIb~ zY`2V~m0Sw9%>M$t-r`DC$7I91d0Uvot1;y6`lB#m>n>K}^jLPAp*kI25QY0U9^&UL z|G_FBAH*IsH(;Z$e!`AJFTfz@CEM>&H_p4~$4<=3g_EDsx!E#{S+yq_WcTWy5ME;p zWlaM1j`lJ(Vn+$6m`K7)i3)UY{>0T+nA3SzpOZ3><vc}m>F^~lXk9`IbCvr^br+20 zuB5A@^{Q6VzgQh6Sj^_tqVDi=Ek`()94#lwz!|j?PY&_Nu8iiZFQoCam4q--r5nv| zc(dIKqn(1EZsuoQFM!jV)Oiy!hWAabhKIT8tev<En=@|{AF|{i44W9s#yO3Hl<FxE z6la9e=?}OTnGeDbeXo%Ex|LLo&*XM^YoO!63g*(zS~5dS^hiL>Y&!GsCMM|SBfLJ& z0Mpl2!~M$bwK}U*_|prA*Ge>|GbgvE66xJfLH!ew+luYDK|_-58<oLr3Ga)p>zp8c znI+lzVHKzTD2i?glE%#n4e)(Y8RO~Iiw$QsW8|_xYziCCJ9|FkZe7?zRY%$I->aHo zOr|8%-_XF1F1b*2UyVNyK<Th?9dzM!dtPl-GE+X;n)SQYpC0z^&oXXV{KS$jEU;36 zd~XZ(Ov+IhQ!)g{>Ui;fwlDdkV~UW~I>i_iTVa=tJm2lIfH^(No$If$ls8@9O!A{+ za6<Y(9PI3a5x&dGtcjuI`tTsKFhZW0a`ZH{{%l~EIDb9e|MoID^Qn|fc)E}q6RQBr zGz=kK?=h8Bu0+fIg&<g02r_D&?ETO8`7oJvtgDd~7_ImWD6&azuQXv{-(h}1kAUy{ zI-75vnvBBF$LZpSN3hS^Gz_MlC>MMIo1A`<_Js=k!GHu_{qt^Q6VE!lJo}z@c1od5 zY7BX{cn;1Oe3<FJ6iDk&>SEBFo1#icc{Hkg#Lyi#DEEF1b$hoRmRq)yv%6Qbb_M;N z7ImIxt=^hDeOM&PP6ZAOwW?S;yB}&;`LeKlGppo0f^B{(&1F6kIph0B@Dm$i!E=of zAFPtX-@K!Pv&A1!8RNI$H_Hz{U7CVNC&lvP_l_W^FDKH9j5vPcx1(I-ny*yxqbe|0 zjuEp47Tl!F!Rd?(k$c3zmE(!HFZ(Gtu8hNNUh%c%fz$bC-_-a%jf&7ac_3C8SwdX? zV?1zZ4}bmrZobR>7UUSrhPs+za4OClzY4<mMbF$pVu>0r_JrY=cT4deL-pCGCnrO6 z`h7-@lj6rtABdF;Z`1WF`tpoQBzGlJ9KXLbAOVKUX>6=4dueJNQ5v*_RIqZa`91}{ z`Q%o1j>B54ee{H^txx6I_;c*Av0XT*p9d?*)1vB=c7X7<BcD|Lh>x?_&rdYz!<UcK z23tuhUcttSuFlWLDOoan&g~PV%jp6BxOk7Z_7ly`!j<taUD<!-LH_uMGj8G6?&rXi z(f*L?Y|c4o<kHrY+PF<i!m-XF5$$enr|0$NqtO#tSZ!lOJ;utz2IGUI_MtA;cLp$< z?QWC#D>cyMWHULR^O7sA@g_?yW|8o;BC_^&A-OE&i$_0<0Y#Nj5VP?H9s2xvO}F(D zvbeNT@LycnKjr<KD|=^cl;GehTfyfsmG%ep@1L3xTvc=EYKGvFTC8x<yuLMWP5TS) zn03{>d~{x5+*VtCwS2HJ;+wf(cK>X_d|4gg`=CjVcK6N+XFNM3NFO>txaZqO$9RVy z4&FNh9Qf~EHC|Wrgykuc!j7*q1#<&N3UkC;9JVMP5y;zxJ5HLXz>H8=<H}0aYGzCO z2sDDUnYmtxf&{}GjtSf2gvOa;1f^dmFq5in1zzHj!op@F!L|(&%=_wL+|<b$!jPUc z;R_`z!Q?tUf!V{Ig3xYpVE~&iOrG`5am2@b;Z3gzH8s;c1eJ~Q9B28-As}tI;EMYe z!PLd?1f?3!Yi=`}1iQi_m@Qvt2*X!YIxg9GT2OoG<f*%dwg|Rlk8tc;Fi8;bR+(x3 z=`CnYt#|CEr)vtnbqN|$KRGsMN(%Ic|8NK|F&4(Z2^Vhvmg6{PYrN26{vE+)DN{j~ zx|pC{AzUzi?o`JYIGXEsc`!f9OB~#Wfbg!el<0}S0x+Ca548!yiPnr5-Zbtq-E>r& zyunHo&N+?R2_x8N*GJNM55w{1^ZU@3?I58IK4`M@1}?go4yo-@d_~v+($q(VFI&8k zik=%K%fy<9wPYbajwyf*OQj&MzLj`n&*nE7N%8cd4Igntij6Vt$3K}b!M<4%Mirc< zvT@_bLbW%Nk8Y>%+v|5QGPw+9UyZ}Q&UXAe<1>&qM3rAXUX^wAzK3P^RDnw?;Z|s+ zF{yqU{J<qD`1F|^duil2c1Nf)uVk*r(+MZ(P?Z4w#oePIacc%UZQyM7gWxvSJKiGk z=XIG4^IYMC?+mu2`a4>RrvFjjY*4xC5(Fk@lh-GYvco!6VB2#QUOjd%TsW%F$EqIV z3w+b)f^$*)iu;w=M{5w+XfA~B?=NDb*##IeOddV7ocYZk0(no9wQNqNBHw?I2Aivq z%7=@Vd7X})?5Ja3f&Of!b7rey!1W23WVM$}TYVSDv^e0_Oi@9PPY3k9oQIjZm%z?O zhMn`&j4w~PjSKVNz&f=lc+tO>sS=v;v+jPPnK?X7Tg||O_DV1gI|PfSj)3CYQ8?5% zh?kz9icy1if?d*X)ToUnPJ41;+Y@gn{b&gt3&LS$d@)=RE#-F3FJ|kc#n=o{0sPD% zI{fsaCG59sH?nK>K4SNO)ph34SVeKaH)c%8m@)HE<_ym{dmoh{DwPIJl87`Ak_ttL zGFK|IgbX2~c+T1TPzjMzG)tvOr6QH2+~>XTde?jJUGM$(tmluj&U((-zy1AwzTf@) zBU;<Kow{_+mU?@m0Vi)y#s~ceu)FI4yb^RlWGDe=4#(ieNk#0bm(IGD9;YnYP1vL* z2Us7cJ0L8*4(sXdgx~FXi9uopuFV>OdVvVmEI2_OaP36n+KV`?PW<e<fs<^}87C_1 zbu=|0V~OvY+v4KLJXjPg#7^g*q&Cg##21x~sdu5T@fJdc66#SV@0KrQO}ok{g-3U> z$3=5m+>M_|3tT`nHxv<n@8)whD(<DDtfM%WmiBVng?+i;-M^;2zbEO9sRi6k%_oTF zZb<?}UFCZJhQ#CjeViYid`)=`SLkDQ`v|kM)!gRwC7k&7=Z)88yEq9AT1}$*5}dC_ z4DHq9MqKm|<gUwl%W+!qioSj4qh04)DcU_zmGn6ClBnU#(89Xa#P*aBV)N^C&N)q4 zV*MSGxn%0cakMca&+-=&vbO{oYr}s;nD-U}eJLY+tQDE)Pj-YwlA-Oh7-zftvXyk7 zxiaUn+YPQ__i<v&9frQ)-OZ`@_2B9c613r+c4EGTGg)y(k_b4c&0!WelFRGoG3x(B z$)~`9w%v7tkQK8dJ0#laRKIh?Q>Q$_yv2yjGwr9_1P&72B?091h=rWnf4ABlbqye$ z<4ftW(RECg+H7*M(@|PU;v`WU<V3U#ZzbmhSd$4NL!1y3ijglrMe-+?;<42SalG9y zvbw5?mX&)mmP6Sv>HKYA;j$d4Pr9Qw{f1C`-xIKy`i??F>v7Ywf7s6E0vbUJKvFG& zhn+5igsF@0rS2uLR!0iVc`w57X_OJ#c7N#2?I*}nTY2ELp%TcsD+U})7P46~>p;*? zBfRmq87k*)#EMZ*@uAhL@b9nYxOta29NoDY*HtRP^9Jfz=BFe!p^OQC*?g2B`4Ia& z<;f&}Yw6nlgLuD4J0K-Z!NO=7+0=%D3h_<wZLK1@&4WVM4cvnBdGOhd+*#mOHA7@7 zXv0^-8rVnrJ@SfPg(t?Zz>H91Sktuvu1kx@T?OOJ=KKNp##@vfxvLAyzaEB>PfAeB zc~3m;x(&))S0+~0Z9@`=nqdK%!Aw5kIa1t0aIaAWW}pn-ccv9}+AqXQl?~W6XMe(% z{$9+dtaqr}+>wp!(WWPA&x563T+q)VVUS*TpGi@xL@N)UAi2ZO$;ExOOcqZvy87-A zvwYD8_~^GaDAzhl&U2IlPYdfo;+PcrHLStxGH)Pi64gNOcU#2wI-B<N-$51#$&#V= zB;zQeg^0sFjEZC>@#ePxott`|jGVU`S?vs@HTNk3w`Uw+*4<B@PZ5PtJN%J>Xdv3c zeSt<McEU^P$56hdBu+N=M+%R0k&$8q;pi5Q*2(Qac>x8)8l@FrQd0-1&%EK3jQwO_ z^$ce#A&)K`^Ft#34rGV7DxBGx59U{?5OrcL=+8twuvsXN?l<S7pN+3U&7%l(%3cyA z>0AYuwJ^AMS{1HU)M3}HoB&G%PC>-?6qL}5!1Ax_$mHF};5mbzKu{|etm^eb8lw}) z{ckIib@nLGwQi)r`wL*r-nB4R;|Zf-6^wiB3~~18UgpqITUcOyi(Xs$i_{vQplkd| zbWzWfR4cYY>b-*Cdz&YHiIQYgzs-Qmp<2dpV-dJCT1?;TmIdqP_>yx6TEQe&0-WC? z1N<f0$Z^>#4861vdG9DA-({8(uD9Hnx1S?mkZwG~A5)HE-R7bfMFFIIsTvc0-vsQh zIgKQg<I&Ac3b_6pfO#P+$j7!XKzO4cJ^lR!Gxw-CvoqeE>#HvWbRCwU;HIUFc8n2O z6Q+U$;!=t7?P18Sp^{Nf^}t%Xjz~J&1*|#g#dLaaMg_J;xK)d0E(xZfFKTM&t?(e@ z6BZA&W^F@jPD#L(!`Hy^wjo$^u?l<i`$An_2yOS8Lk!3=0Y~PHA?wMnWW+={*d_A; z)V%U#TFYOMqmN{v>K{{7azhFhDMf*V<RDm9`U<QbR>0Ap|B%0wr@`%EYe+<OlHcE~ zh2cw21CK;kFqHEUO^?0fE?oH-k6v1YC%<T6<$6IdujxE^!0!vip5_CA)%WNZyB30z zT0wAy;v{$&IgJ_x8u6EAIacP9BC{=|5fx@~;EzX(z(&V%o>U|cuFX}3tL}O-t&w8r zn`02@d~_U~dp8Q?p2na%I+ElOX*u?1avv}sBAEt0Idosl2D}$vhbE8zLO0DZHt4wv z>=HgeFq99rtK3EWx42+u92ealHo_X)Y=NzA9+-X9lzC}eg}w_k0nG*%$l0omogXh} z1XrFww~BwFr=PW9;G0c2{}cjRDh~jfuZ<?0e&F1s7-r4%Rh%$?HM99xIbQe11^;-R z2R-&3hl{WKAs25x)@02RWV6r+7HfK84dMOZu*og-cS4V~YZb=(kS`tx+K!jFrZGlt ze)!9CA6T$05`EQigGWSN;oZCLSagF4mAr}r`}gVLg*h`|_4Z=;Kf1F2J!k*#ZNT<T zi}Z+*!74(o9WWmiZLnxAKNQ4<xKQQ~IKv~+4aMKWS;fz}dp-nnZ|}WBd)7S%fqcm* z^ZgVv!O-BXB#U<Y_JELIpUA`yqC6}6GShQ+7Wu0+7I+Kv@H~|NF)RCD^_TyLD_dCg z8Epm!*mK#tSh8R#XiKOfR&L6K-4WN|ebWzMVzVAjxtmPoO;u7Rg#)Ov_c3maJ<lHJ z&^Wt$Keckw8D~;lGAhgtyNZdirCnqAePJzpy6Xq~?V+vxoDH+6xUU1Cp(qo-e^^GP zxjexFsc~#?dK_hyvzV0}QNojh|6qMzDiohHfzN2=Aa{i!u*ksxIvdxcnIBF_TWJTb z*R;h8p9-Pa%hlk5r8y=4&yo^N)CUDi9z)3oEH1OsM<I__HWvnmQ<K#^Cc3u}hPTaP z0~=GIa@z)2;j)ZspE^N_OE1JPB%U(1bsFf|iD;C!>nqc8Wj$`O5=QzP_QGP-hoETb zajI#79K|$zM6>7a1Uqg?vfHBcS;^nk%tw)r%qzPL6n;XCOn%wL%(kjRkD9e%ZRRf? z1AmX%Y!k(%Co7>Wa1H&F9%9YPW;JKNh=cyu)!D*89<-`&N={y8ki5S}vqNJkW&H6a zI3>>li*t?GuEYl}t5}AII9y!h6pJH7|KJq<-3Y9Zr_=}Z7+<mt*4Sr}OIPVZ)9Nay z7i|PaQ#;7S<}}oMDv`;``h+A_PLdjPpOfJZJP>S13U~bWz^Ab>CH$ib{tf20?<~@` zKR!ptUg)MV6}~)?YMRYO1*;{gmGzY{Q2a8h(d>vT8nWPnj=8+ibuoTd_#3LOFQ(7< z@5XgTz1Y60fb=#t#`3{=c$%Mpk;&8K-7VrcHpv_(1z#kM|MA1RELre~mucoRlEjIJ z>w$2E9}LW$0wtBzVA<R(z_F2~GIlFbsRpEdWTjN|pb^3PJbnWClOeUD#e|CGG23rL zo?z!2Mbx^arBwUMEPTu75!UP8Nl`CW!*S^%9-xyzu?x0AD(4n_Gb@y`9J!6howAWt z&Ps56wG`#5@d7?Qn$E1eB8A0{?FUkvY%n~>1RIRK0?K!fV;!Njn6mhZaODyBW!#v} zuKWUX5)a!~dR0^3&D7Z4%bT!MmmB-)oh0-S-^03SR#KeYYU*_E7reJD2x^S?0Ec(H zt-@G3evs9O^DcQ)H@6-|%_}pwt1IK#&&LEQt%<RwWD8xuXXZii#aU3=xD)Iq){$?X zUnXTXUP1-(QqXyy2kCwP08Wovi4!&)LUqDc)NH?VypVk+j=SR8e7IbYy5pTfEiQQn z4ew{M*RA+iV)8!IR9Zu2KlR5ppS(#+hf{FoXe28+6bp`e3BajpE2KLA6<l~tmg;^n zhD5~OsMqh`;*VW!c<ILwaP~?QdYjt~c1_+xcb}I6_p>FSBVU?K&|ic;f9a;LSR^o} zDMCz3Hby7noLKYzZ7?!U1MjiVZEm}IkPYh#z^;-*K)hC$iVk@L1qACs`SNLOm8Zo% z<U!e)Z*7ou>no(egJA;Re8V<_wfIA8GB$vQfX_e=jNiKg?2o#V!-1<|`RZx(kADzJ zEg8TUTz%2LRjR0VE&|$>Z-JNgG@1Kj33!tw4*DN{MUqno(VE4l*kc=~fHBd7tLvqj zbCRm5zI;onIWL#33q8z=JI$e#Wc8^84S*ZvPqW2JeXutt6-HzZ!PRz`*}7R{xHefA zi&;K^a&f2dMP~|!*R^1Uk#^>HdIfHNWX{A$#^NKd_F}p956LYXk`c4r0u{TFaMPd& z%Dh}ji|x}yKabrZX3h)XW<__Zpv8}+T&$=A50#p|rcbbUWO}IdHW#Yx#&K5peKI94 z;zg0QN$h}VHFa@QI{Xuu4DI9W*sjV+u+GXKr#>>phnHLd3u><5P@Pv`+jf%bkvfl( z&C`M57GpS}KtML;6P%mk3`I2+;0~D{6j&`wE}C19T7MLx{-1HA78%EIV)9Xyj4I4o zCWgn)ZKOU-Yc-EG|Dsm>il9z*$ik|F$uN$873`g;fQG-fQN~Y>!ei7GoKv$NYNbg- zl|x_P+)^{Bz@A47!XF?J0ZB@wAp-`C*kRB9<>=#uIIOQ6i)6aERQ&w&)B?Xpz|UP5 z_Uw6uht;2APZ2e^yiEjd3Cv_87ss$O>nKRXrLjJKU)aK~T=@NS5Uh#`f$B^dPi5T5 z5~fesSN<B5l-oNz5jLy&vgLaF3t1De@mxQ6qMwZ~WeD4M9Z+ZU@lTM@uWz5ya*jQ` zmtut$t)RMQj$vJ|A6Pf3osMju!Aa8(uw?Cirf8ily3_R-klGx)A$K1BW>pFr<7n7h zngeBeW0{xJ!uU#j9_zm6J!TY4s622RpRr#{-6san`d`uP4OtC%t4fthSRRG_D)jB& zRYTbSZY%uWufPf)7qeIV`4l_nC&KZkW7JC%VcccX2@71DVZpuYyivvx{>xIpj$hJ2 z?fa8RA*db5vR3e}!3Dx0t`B(jeL@|0F7~=E1Md6q;PM-X!Paak`1JG(Y`S9@SS{9R zP6$x6-_4z26V&)o8E@PRVhpgA&27-~_6Ak!a2+rD#Dm;2s&Hg{0$$_c0JZr{si6)o zZ0660!s{I2!R1Eqev<=iT^4}Pg{7fQtSQ{@c@}p{)}TQdEoMl#9oghM<D&f*yiD<I z*y-^c=<UdbO|xZSa@`zQoS}vVJQ|_yI&Ufz8L~J`p*d=IA#R9Vf$x`Zro?#Ry&u(4 z7;AR00&}C-33~<hNmVG@m*R@th197dUmwA(CuUP$UR{Ed8cV>jdktXIkMB^fzZa|L zpM{(5UIsH2rc~le32N`a9H`x-htC%IqNm|C$nUox9?n+B6?+X}V!9kO!e!`E?|y2u z)rmd({s7)<zo~h2gD>kfIL_9pCsN0M>|wKZ-NS-kKH$On2};ey1^Vqxp^B|Ssd=hA zvg+grSoX{YDWCa^#)VUO??pZcU(|q)HyXf{T63x|+XoB6VA4`2jn&;;&I*OyMH>e= z*jr42i5Uw+AIE<I$5s5u<6Jsb$G4fX>(2%%Ht#^uab>u|r-N8!C`+X73WKHRgh;KC zLp=Dq8}!Xi1TIenfZk*_j5yzi6K5XcYqMiuJsiV>MK0JTL!6r6Ww6((en8vC?%-2K zBo)2)6~}6w1gm1?0>6!41V?9CvOf>Yu(P-D!yoUBK?xZT4*fMoMaP4H`8NS__ti|8 zeP<txw@`<}OV=Ugfo;H>mxpfa5O1E^uV`<)A%@a*N<lNbj9EA;fy>sMhIX?b!|~}U zmKnT?M+bOqG;I?oKco~-_w8ln-MX-i@GiW*<P#q8zmFG`PXeysI!fY{Fo;x;hi`W+ zzym2>pziZ|vhXDzTsznX-2SachZav0n-B7~YEpURvzZzccH0>JSf-DSbGD;Q9_1of zIs<E-4O3J7mXu;6L5fe9;>ClrvHmOqmo~()&(vzEuwZ_;S;~x!*yDv|O1t6N5_vY} zM+&@0-hijuI$^JaJNTIY7W(kQAd-`FfssoIsCZBY7hMa4TD|&Mt=R{B?Dj%Zp>{~E zDIA&M9^@06g&jK-(9b_npyYZmwbs>#I&@HvjTF_T2J0m$k+>1C;E<&K@XNE*tS=Ao zkx(5RkvM=8uJJX~+;142U5HnS*h8h?ZmdEBMC)aRDA_-Xto+{3u(7`fh2^k#+SDB_ z+$w;dRTP6gbzNX-M>I?oc|$HW90vF1%A%|*{D}Q*j<0lT!S+4+z{=7B`P1)Fmps7M zT5WL0nQ>e#kz+6BaR$pBxX9jY$_L1~3Z5B-M18pn^f{@BDwFQPr90wK>VIr+zMKHT zzZBss(-YYD!d}|i#0>U4u7H=7&42+9`fu>Jg-=v_KrMeVbGO<QF6n#=WY3R)phOXz z>Zpbzi^o7&ITt0}eE^(w29V&CJBr(Wn>lo~2<{~qSfeD!RvuAjz2~s(=;1)>rzEfJ zkZ&s$)o9GVy2^!Tdz#r@yUtO67k0v~RqE`vkhdtrRuTO1>_le`X{eG7DCt~tytpkF z|DjI7q}!7C)}iNM=%Er`%S<7O-vQv{kqeyg`fLzX?1Ox{sxZ{{38$s!F(d!`4_crf zhzzympxDSP<Rj(H&QEus_K#%POXL@_1^!E@6!%sTpJ&7R2ie2>2B)ak8oVN}(gCVi zs2?`<N8pMXFRbSJiSa%#i;dCPjRYotp+mmT<k=Br9JDHsG9R)>8<(lF>Cdg979T`8 z3TF7PbOE}h{Rik==>q9tCA{qBF!<;r2gUz(A_vFyxX8Plb3V(IN*G)TohsbWi$ChD zYSt&}RdWxk(I{?TYDQ3*lfrDyu?*aunt?C0oo0W<t%8bKYE*ZLFur^tlJz+I2dvIt zMepPa0n4~K_>yTQ5V2msrrC{R&E{Qr-1P%mBQAurXYU5jK9m8^Jr(GU_j<HY|Glj{ z7RR!+ok%Uz0xx(`Pm8DLfXgjg;jun0^*jC_gn?m{bXXZMdu775I%Oi!nPXH*b~$9) zLU4?aI<;v28<v^Wf!7C*Vq+BpIQZ>8`>R0(ieWok{->S2zi>CytJA<zOZcfk`y1e~ zW(ZE^9JbrOG8wObIEajQ_M^8TnP)J%K-KtCh*g})!kQy!P|uiM!+RD|+cLn_!xx|p zWz7a|QHFPpM6k)v{Mm;2C#cCIvGAF?Ec^5)0feUrI92c$#QU^SIpf|`*Csw{n9Rcd zLl}HdxB$nCj8K{fLfQ$wc)3j)zB)S@YF|xYbeogmaPMVa2ZjSU#Vc5}2>k$FJ}*Qf zmsdl-*K+~5<cI{<1=0HUpHat?3?wlx2Y7}aXLHu9p`zm@sD^?sR6u}mvtibCcC>IG z8)>abHMW*ObDu+$uhb~}$GnD(kbeUI>KRg$zY?rn`w%>^n#=w^DvLu-2vX&t10cEA z4j$XD2qcPw*dLLinB1)d!oK?BDkp~Yv>8O}J5;dh2Mfs5wSg6K5_os82z+F;53Aj+ z#llhP$Zp#?WHGXe+8i7U{6cm#r-%x$gi;|ja6}W8^2)DvG`X{LT)x7-hYQ)5LH*_( z!6fVJb_P9EodA^wpTpV(MpRpdF${FSjuj>J(a<+r==vv|Rm)q7L>KeJ2j5$O-^Wze zw;_S`x$%TJb~O=sa_<uT+r~lTD*-h6NCaKj@B~aZ4}*W1pUH0)($t5=ew0847aSfC zuus%$!I}~2)Y<86aHQZfjJEZMKGYQa_sEj6nTTdb%^$Mgt4~s{yVLR3P2X`{RuXg& zU5MMpwOHMRG#pf!O1|Cs9WO{uAj1PADe)FdU}b%u*l_MC_#hy|SWj!>OrisoiERdx zMh+-0bPVNMCZoPgL$q&WAUabL#d4>w!<B!Z!<!|1&?L5zn*FbYow^y%wu&Ka9&3U3 zzf8tspo6`YX$+@7pJ4LRw0SacBd8kbM!iWUY#FcYQ)oXwtDH=-N_J!T*28hsx=|Tf zui5!OyR!eSgZ!r}EBgKf92dWcowmF47N|-jLB2s@8RwYs#}`0jaWp>Ps)C2@BG3vH zfX5~I*~XKtAh5{;FRSqckrlT{(fyMk$l4G~g^i%k1y=Z*$qr!K{S3tY{6rqV=?Bl0 znL>_u2iUk$7>;awz<EH+U>!M8{Qq%f|LgYt4_DS;l_q{-q(W7P+(%)jMW{2WvaIvQ z1I)Ecc5q%&5!UiM3pB-LSmVbhpzIMFyd}Z{c=p->wSp~JcgTaS%;yKhrblpga2@=v z8_VLiRVZTZB9PLZg@uxN5X9mTIJQ`X+Iv%%a{0Lry9xcpOYf!NyP~IXma#XRIHE#z zl@qL{&p0mmc^vj_v*cCs2vCoE<ax`70<YRdlg&`h#UY|Suyef##pkU;sV$pNN&ne^ z$CO{7oLjBTv6d^?!CICLri|Em96R<zS1-KTsZDkG=Cc2yPauVv5m?&v0;i6&gMF6k zus3H86yG96WnGGevV;~p6Y&YAtnURG7PsNhuk(0j^Fg+niRVa+S+XB4bW+dV_EM9? z5_X-t6t$^e8#_!cpmuK$hoi!=)EcXo;1blPx`G$7W+F7Kx4TZ!8tRmrXgFRFoDUai zc~bMo8}akEt7yff2;w8(C$4s+38D7Jg1(z{o8$7wl;#%L+Rc9wLErGH<C@K+(CvIy zbPU&rm~hVH7F2H|wuj4crIqC9@8RX#7Mb~UQIiU7%!YGP>XaG74k=pY%XMNCNpq$i z$q{q6PjRZ^#OWUoZRxYcagC;Vj|rjZeEQSwJbL<>8Qr9GgJ!g3h{n)X!bMG=oITH# zIP)!!C{rq=%~BT=TR&zIfg#_xJC-OC=H}I$iLMS}#%T%tukRK8SlgG_>X%O|betg7 ze4Y}%i>m03o=t@Ir9yg*aVGuSM4QyMY~vKq)g`}dKge;oyqo)Wi7j#L(`BxbK|S4I zm)&G^Xg=rVhWVs)zbaYTVn*!A4<H?yWO#W#L~nV5=!#jtIU$<<#Gh-?#L|myM0kug z6K?US$$4KC;mebRl29|T6sdDBr`XY}uVv8NIS!16`Eqgt-zK6yIizVrO*;46@I1O> z<N}8?;=wU+yGZNBn9{wIK7@7J3BrHbEkb7Eh25jX?;MMNVsv}MT4JQ}0cWIqJ-26j zGKX(7<Q_8M*twH=v_kw*j!4}oH}}mmLa<~p_w%-A^aj&Y#1&$idvW>_cb7u~(YM^0 zvq{;T_R$a`u6mjgA49LwZ4o!P3F9va)3g#|@r5Do*TuqgsA(>pgzf0GrX#d`N0Hrw zURQd<sp6*6#@?p+)EaurJ0H%iBN=x4D^y8oH(TOw<0Crl({FnFP&dcRzl*p&qhous zL!5YjXFD(E7(^e=4WO6RWfMZY{-ctBgY+h)-E?f>Y3`DM2-?<5f^m*^A{U7`(0&sJ zB-bo}xNwG|CGbz~kMJy7`#@Gx_LFWRsG22C@=J3^mX{L|@+M61n%%U?VSQ5IjuElp z(++y@rVSIYErdp%CkdBhR`i)FCyu9?0}&J6N-w!?2IZU@;2h31Fkj#y(vO2o#-{H; z=M_Wx+-Sj4CNhljp7rdrb9`j(umJSS5P?1h?$r9*d~BbW8+&H+gk7lMHOOzGh<EpU z0S|EnR@vMWzc#v#Tkp+d8>DzSy&)Y+=aCY16w9(+YbP+%IR?|a&yfW?9oY{@bg6uc zc$hkCHOpCT377dUpcWnc2H&DR)SQrYlyxqLmx0a0|GX@zv>&qoSF;_q+zf&jc@=^B zo6S(pOGW%jC>x4!ma;Qxs_=noEzr}Q&%V8?$^$xM+3s8Em~(m`b>nX@e!6=ut9!?T zIs|sGv2UiKWLOwW-1f#Q=9#R|{nOO+=tulWE1Kcxy0bIGsjU0$Q<RBk51wz{j$c-` z;KQMx@y_KFaMhIs?DNu>aL@gJ=*)RrczI7AJYO}Bavl+7pX`ldt>@G;@3iORLu;ad z;ND|Mrb&W&z<ozv6PhM9?@W=&52f+cP&5*$njs`|qnLgD06wc1BtM)t0$mzD;9j5? z*Z;~@qUy5^RFHp2t{3$KoY`FZ-sO$V{g-*j=hhIZ{ihHW->3kE=Dm#go*43Z)@eFG z<s4b2|DDLy&t#6wOC+onECKpm3znXMKx%4|Q&A9r3Qs$t?zrpBo6$6e&b-0c5OPQ& z1p(I#6JF)z9#UtX5i@Ea2d{W8#&4GPkho+qFj;FrqkS2`>6aqV*(r@KdkDe<PpoiW zM?QM|MhSG+4=^Lw<$>d}LU63;53{ay38S}78(YTA!oYehl78ca9}27nI%bpflMyrU zyX!Ui`oc4&+4>Fgy|M<hFI$2V&p8moGc`C-v=&ZnT#IrS<e_<CN??}pC*E>xgl4Q) z!EgHKQCNE{>?xx`p)y3Qh$OgqE*9ONt%~ROEe47$=18hqg6(iF1D8?~$k`?jNHt3r z+?>dhSQh4kvbD2-=%u4j?R`3V=tTi@*jR~;YHfi;aw&58>j;A+A<||YND;dwICw5U zUL#-#D@LM0?ztYOM)@JS92f<@Mm&PYcjf^l{lARig>X_JI;E+3h6TcpJ)yz-9pF{W zcl?rX1c(I80-e`2K!D0V#$Le+)@wV`V*?O8q#vLa!GA!r)eiu2Q;}$VJTTeu7`and zvDk|_IMwtoh!%{cO<qr->#EY&?^z4r`EwxEycAHIMRDefHOMP|59*E-fn2eA*j#%G z4hO5_y01e(Bf0=b#lAvkd6gjJXBxnO+a>4|(nHSpl_BrnGB9kg6FSbdVpcyphQCu~ zAY<+`a7o#ds;qT^&zA||vyLL<jjoF@`}i3+D8+{ZTtkt4zaCOR9WZP~5i%3ZBDY6I zVv~Rq*hfKyjrG%ktE+aPfYMK-7f(*gJ@F54iqk-FQ7Wi@V!>3BImln)8@ick$S%q+ zfnSH_<5eb-@Xdxq!YlF)I4R+bzqrJLQGRbAB$9^q9U4Xaqa=BST?zc%Z$sapTi`~> zZZP4+fk(c@G3{3K;lQ!O(Aw-ftut>8UaIc{`_epcu%QU_wjC#podw{Cv@wuTdyii4 zu*N5oQlO&aDfm(-6J=`-fTha0p!%^a#ZCW;elO)oV|mu#eLD@JMz#Rg?--mE8w3|p z{h-*!P4IyqPjco#|4((i(ZS9fxY{EX?mV9j4Z;}I)VUa|m^^^Cadk*`X)rCx?uJ30 z_n1TJH-TRC4kW%{0jztGj{n9ekz4kwW4YRT^a1T;?R3ph#PDORPO2cuec$j7p-P~y zmjb@XS0F!Lk9(|4F&^=d2MRVUkY2k4{*%gN^sZhf<D?H0rw>~LG3{lHk@7{<I<|*A zvT7Hp*JlnFen#YDM{~3)pazV7%_7zNRuJRq+9>-=1(A5WiPWjQ!A&n(#*E)s33N2` zIWb9fxZ9!;sQLENqB9?v3k5sroqGx1+#twAaqlxqv3t1tzy3y11Iy9y#vw+ZU5<!~ zcC^Yi7_6U+MBdwO5L<fqG51LtS^E4x{$u%2>rgCGzAS+Bp$Yn|bO<auJx=G7vyty@ z226z_ZWy*e<~OgQV&yKNW0`=`=J*47`El}b*){I?ay|4$?i2x4QVFfUE#RJeILNH{ zLHf%y0F7`9FeUjN?X$cI>iK7bE@KBc75xQu^CZq^Kb2Xxs$LKl56ORLtnm#Mb?kj* z5i>Bs1?{zZc)jQfyvwcxT;+WStcETy_cjiLU5%@;SD+s{@OmF|+SCU~#Uzq{NQ>Dy z-h}^0SN6Z>?EiFSy)u^Zx(^rPStT(r+UR)G@^6C7tGb`uvT{8_#IlyoiHxIvuS^9w z8J6%%$}Mn2x|=(v@+`{eyb3h0nStjSCRi(|6TEIKLVM4LFtg9N^7gpa;PxC}UM2Q3 zv{_ADB4|hG&X9w=6H9{vWu?R=Mfqh##RWup2N{a$?2zLf!D99QTvhsytGx61{_o!k JE)kFx`(IxQ=<NUi literal 30761 zcmeFZ2{cz-`#&5qix3$rG$3Rwe9yklH#E>3Q5vLDsSK4gE6I?lh!TYisU#^yoP8Zh zQW`Z%8Wl;3N<{U3@8`MiXLx`2^LwB5to47_yWaI!>#TFmckgrd=lYzz_xW7=x(*pB z73sjuK>_QQuGEsSFtf8y_#`H0BqtUmC$>pN+`?8%#xcNusdtc<xxUn2kBsHz`g?Ek zU9TaQDJCxGFDI%br@F#B$k%^`m%mStnYr0UAKw)#gXEO|>Bl9$-s?p-w9}HB<m2b( zWuf2yx4p5FoTT@fz?I%={rc@>GsVP=73KOb@d^6-`2abopmpB<>mB~up1P<#I~8$D zb1f-1Z~v8EmiqmF+y92c()^!rSpFFfOS8Y?u(bFcht+?G!${Pgg$ReGmaJ>Q@*qF& zO<vae(!U@6Gev)6)6(j1Y}))en>)osx3!Yf@!1sUvo7d2I@gQ%T;uES<Gs$}4|}x! zy!3a&tbaFb?0;yOh+s==kzqE!6FlyBg8u@|=Fg%1jp{#z_BYPQ{tnIdKZN#oBFFuW zNV`8LGW0hh$Ne)}i~p6#znA__q^*|pUwf+ge+YN52!gGMNjoj+zjRd#QJ?=yU;PU> zyFUl_=T!ay?hh)h%>Mu<>i_@II5PfYh+A1`$^JgXEv)`EnN}A6jMwsiCG+p4e<jn( z@(;Y${~=xxnO2r6;#O9FkZJP|WLo{-hRh$$|2dg|FZ~^y^&jB=(=lTvdeUP>;B2%M z=J>B)yTQk2i;tJZIDOf_o&9x2@L4Yu&lHoE6V<me7u6s8PxWp8*ZQI{X*KSj>f8Ox z`sSkh+laQYqHUb$I@`ZJV$1(Ijf)V8|Axr&H+t><u?0(uf7!xN(VcBYn`kPwHrJA! z<{dO`!x}G3OMR)o9?BWZi3&u87Fu$yzY}6<C7K}rdi+1=uolgw)|UU&xb?qm{DO+8 zvXz#+sN1d&^7aq%vb53v?KSn66XO&WImuw}H5+`4V^n0sWHe+n28y}tRO#2R#-ZQ; z@xTAJ@$XaoU#|Yk)xXcczhCqJFa2uR_<!H#|Fq_R+xYjH`!84j<?7#O;NP$L|4zTk z6zeZ1Qf<~&qD`dqtgZj9+Q$BqY7+@fYny*iZR7rhYD*9kRUG?|D$MpDR9Mg-B5dVv z8cd|X7XLxWgVJAr6Jr0)@gLXyKka`rP*Y@_NcdWd#JKf1EvfO#mU&s)=}U<Af7os% z^2n@3diZz6Y9-Rczbn><;-V6}f3U~O;veiO75~GaeDS}iN(qrond1Kn)Nh9UvHgFc z&cDL&fBN@79*W3Uuon3WHlk&l-yVvU<zF6(%D}%YJejWY4KfuiY6PqcGQOoIqbf6e zpqRs-Eh)L2Q5!eCfEbO~KsEYD(YVr+aP!I<7*oVTP*50DpL_s&G|I``_EBZS?kr_? zyHO}<s%C1}7n4Q59)sb7B0_6CFt|w`vn#jb)%fY)SUZl_&YVbxYwN>=S?lo_*F~3a zGsJP<XW$A)eWtMIAe^YZPP_~kF%I#c@X*#a!cG{1=ln0xpHp<<`?0-{Cb5s+bZMlD ziTRKxoCjIG=2UgfAoMrY1I1bAU{uy!>UH}qEuS|XenfpECQCHQ%IHCu)47t7GCxdj zg_<)1TXo3|brX7O<}_@NH^g53ER<Q83&Ah}x|KJ;lFA_%dc%Qw$Y#TCMZncX-6Uyf z40_UCaJOinV5sUaOzFHw@@E&({YE?Rb7%<eZ<$3#rW{6vnCF<kBMrY@7o(@AsiOGP zJoK8{N~f+HMAH4#!2E+I?!4QIOSTy?O(#-dcWynIBj``cW74p<UYZ@YqmfxSt%ttd zWdr2aHs+>tEj+Z2#mG7b`f0*Wyx2Dfvjzl`r|S=Z_tO|~4A8=Zkch;sKWY@EU`;|l z_*ItAJTV=IIYFuTX+H-|XOiGcNItqhIED|S)giWH1bBJ@8W+gWYV|3YpB{=IyBvY( z>mdtEm6%DVQ^BRn9OVv(v3}Ku$WCWRQrYK6S%*H1^UtC3EG0JUmF@1vEkN^?Cm>$q z2vM9b%f9QaLY*;2&|(`yK76pjk{6Af%Hb3ebW<J_`TJD*NEW6^3E{5%4<^<2I*yCm zgJ<?kgA;ZDDK%U0a9<4lcJm-z_rn%Hw;2+@TkpwDKA#qhA4jh0sp6}-7OYo%AQ&(3 zM2O$V6u&WpP>U$yv0@d}SZ@P`>&vjdDi91S+DNCqI%GVHqEh2E%k}9(h*Nt)y#~mF zW#@Qwsb<l6(R;F_Vj8HQy@;pU@8D0&B0qoDG1ujnVN+5jHrYhM7dHhc^1DbL_Q;Xf z7j();3;RQFUMy|qZBV-~n#g3H0<lVWblX3Wj9_<BhC538RXULxBSTzV;zP^ZVj!Tb zhECNh#q~zFnY5zAaQ;UadRD0st;D^cc;pl~b+ys#sSfz)i82_>PlTCnKRC^wK`6O+ z5g2<810f7S`z{Om6wi=V`y_DN=s_U4${nf$R+aZTR}zwMMjk&j#Z|%mz|h2u??@bm z(W*SWFTKcgb(|(^@=Re<t}69DTT0GvKL!mAR%j-sgUUsp@tplBDCv;EzT8~yWkwJ4 zjQHZG^f>sqs1jbDTZhs;YS>kt2`8+c!J<LQR2aSrdt^t!ulq)@D=!|$ocqQcRLh_V z5AVVJg}Y#&%6S}N?ux2`1DN?*g-}@U%g4T3LQE6wq02R$q?}%Y`&4e>cC!R9+VPF* zG?&qw#6mDyy&UR1#-e)8bbQIJg6Ql5pdqW6>Ywc--FPdW4IGN_)(hHZo4|pZt7yG> z5zaH53zOf>faWe=YM;Iz?@dynWk=SNdmoy)HQzU4XSE*rzH~n1-e@8=d&;P+{wQql zHD){QRG_J485)icr%#nnkw*s?gIn8C%*c<X-m5#xBX8v5j|Vx-qjMWb!Hg5IahnbH z9<{)E71=Z~-UDA=c*y-sn1<^m0`YzK3;OHXeax%K#X+~CaOMV2^k2||cJE)|<1qtJ z-2P6P!<w0x^=2{R()Yy9L56gThr{>k{pGJamf#0&94q&0Bi!vhgT-O<iB6pqR4uh; z0?y2)OE)cq_p7$zcBLvooa1%kK1rLEO*(|WHR@;z^~^-n#N~F8_~i0!oRe}6m0Zi| zWZ@f>h)%=snJ2LHSSk)(zZdh~lp<;NLEX^37#O8S`ppPq9Fm`--C7y2TpbDD7lkli z&PKwcm}B^3p$0p)r<&mW8dTk|f_o7As9gDaA>_m>;(@nuT$BG2WL>6Vqp**tO1iKE zCQc&hYDIM3-2w2;bU1n+`arvqV=<$+90Q64nDF!!9!N~Zh?1q47xfIU{5*;#NA{u5 zFcqw7;xTCXMGj+CaH3QNHq07E3P<zsLQV=^43{JqzUJb`v&L*#o*i7?8;lqEv*hgc z8Stt_5q1q_X|zECT<wa)dG_&``*bH+niT^XCR>Qddo|RV9FGkPJE>+p149iAAu-Vo z#7D)#kDKox<Ng&$dPrfNmpCi_BL!}{a1eS>97>mWl8VNIjKL6ZGF3yDy;RmL*ml7l z=Shyh=MU!5imYmo^&821w<@8=qutPXK#%wg3d17<J?KQk(V$oThSEMys2b=4hj+Tt z>CY0d^^y%KdNu~M&GPB_VFjc(I0}j$G{ZgnFHp3lAH;k%g%$w^Lod0(R&OU5B`yK> zQ+mk2XYO!+Mg}|<Hj$C+Oej;^4+|y@0f$E+=xla^>W`AfwAy4afFF3z+!FI9CqnJg zLDauN95<=&qvy{Rk*p){xRb|@Lwf5~Ao0#Pyh4+|b8RM!UDQA~E}n+`w0SUmFyOSC zuF$s85f<zI;7WWeF=<&6TrL!X@-S_@GuRM|K2L{F!);;C_7FN+Hi_zHD?q4v6Dcge zPu9HGAa9*iaN4kNDs@$z+GVBSn8I!n87j?iJK~@_;xk!1YC4MLWIze$0S@(6*jW`z z&WSU`p=B=EpYNtmHX$9Gu^X}-m*G~YWL%}_3y&6U1<q6lE#_WA&moIo6j==UMtjh4 z)^RHR+K4>TkN~spU9{kQEH&C`iao_ym^H}{N}Uy%%1O~o(1#Rq@o^3<Q7~ie1b$?} z$ZAx|NG5|*hJqhRaq=En+~iCR81^LsH6<gd;U!-<syhps)JEXoLT$R-VK+Q}l7ej+ z8T9FSSNK)Q!T59iG2r0=deY`Gyt0dg1?>}YdhJ1)TYLdJ4NAy-y##tX{Smzo6-;hd zSA(W(0%0P?<J^W$y18Tk?EmQ^u;9ITMKet}mv0F(3<p8c33+<5a1o9$N&qLHxAfC& z2_hF{0q;+x!jN}y%=w5U^j67(f-^rEUxRpjG5QEe**1&W77;@SeklWO7gtz0Ul{|H zvrtml4oaEP#M?lZF0#EzgL-CzvBfr0v@g5NQ|2-Fb5k(WRv%YSt|p1GH@TmO(#Yes zW&B6s8@fmG26-BlPY1Y0!Tk%4q~*;K)DkLEC%*t7AdmJ(!?-)r7jXYzUD!MFCoQ>Y z%=sAY1M#Om)OWZenGl@^)>FNxx`GoLsZ_#;4W%&lK|0lR5MWq+4s_&PfTOSEFm|6f zr+hL-5UbY!Zna|2-%1)in^%$2pc`C<-cc}FC*ZHP>R@@o1jv(nMN@V;;Lcz1xMMPb z7Pnb=V^|2zGN>f&yFL<UDKD}(Egauo9RQ83_sNr<Bh=VOMpdL{|7R@JW$OB%1-#a> zqj)W01<Um5vkd}wwjtbxz2arYd*D1?<D4ezwZ@EH*fp2!x$e%cO%3GbldSk?O&6TH z%bGo6JD*>|`toGUB;I(JxQxVKTKY%b{73x5rAcg=+z#HUC>{5Y9nZR*-9Q!A3}o-x zrh~A15}&g!j=v~#7&?=8vaP+F*vsX1tk_T;-W1Gu;%|mS#D%O)!We$ei4A-#4(3}9 zPi2!OieQQG2fcP_18?J}!Uv8#1nJYJvd1R9$Df8jiT&%R*rh=+B>xPGt>^%WfxX~1 zU?9KiK?lB2k>jfuh_NqfRCxRC`XJ*c(mN7H{6vjmpq_mS+6z?p(N%9TDuZQR)@kuh zU26PC-~0F@&=058mV(<RJyw083R`n615`gNvzn&Q@aWmAU_3%b<dyvs*MAL|xhQPj zt^qcy_3`M8nP?oTFEivXKF0qVXLWg@m(Wy9ha|Z`(-0j{y~S~!sU>uF*>=*f{WYiQ ztwCxEUek5{8Xyxs1V6i1l4Gah*yStRVeI3BY|O&_LZ6X6EV&oMt1dTSr;b^PVv1iO zD*Y5Oc%H?o%$-ZWZNCa*n>xv`@_2YX<vDJTKFyw=MX>l|B(dQhgWc|WycYBX4dQn~ zcjsiV9jyolw=-yGZBA?63?>hr*O0Tj!>GyS6_|SLVA<fgBcb9#C3Ae;CFa<~epF&t zIDbMag`Zlv8efiA82@QpFI#q_lFy%f4U5jHie6vYkQwXEpE$aQe^D_``0GGC9PDYt zrm4N8^X+$dXEu#^7Vje4Zby(%)&v4;W3Ve=2UNFkba-qQj2!l!$T}1fpOZ*csuD?~ z@^;FM`otv%jHg%HK5-5HIxt|)GjhF87e=;@wwr1<ik(Mp;;rMMY~o!5VZ{R};pvkO zY~c!BHdnA4wLB-V6K<Y{tFj4jwDSwhGTDZKrycOp@?=m>I11|dlB|lC3p%#yVGe5p ztBtoZ^GsjBl9rngFTIyE#crf$4JGkjz)1Y!Vo!_nSJ0(9LrIe9M7r(JOmf~%T{Ka? z;>^1Ph`ZhZ&^NVb&t1%6jk0`@^v<i;EtV;)<Yd@U1)*%ePD56qzb$`FFA<vJ3t4gJ zGXC^bGYpDY%8t%XLK&0CDE59i1pYclS3J$ZE#U)Ll}}YfBKi!D+qIC@8~O!Tem+T> z9*l+jam8fYq%L|?WiE+4vVb^km8N%3DA5at3+M&3!b0Bys(V@iN(36bPS0|F@T_7! z>THg1(hmuKUP=jAaK~vz+$(IX{>onoK$84Pjt!e3&$n_p&~{vw&z(AsZF?~n9NHQ7 z;ky-l;=Svzwy_oNnOwkCdo*EEr!>EMqyc0OPo(LIgfuyhBb!%CL*Pp%;$10DWn0F< zoc9h~g@up`gErBF6HP$u&}aI=GKR%Q1->Yt1Jj*8z_lY<!l^mO*%04oHrdJ;PtwP5 zqty)aa2u1m=K=6axv;QC5~R;%Q3<1IFy?j_RGqTGi&2N*T=I5sl2@nKb0eWnqL(b0 zqeml*;%MRXW-hV#G2Onpj7!KlD2PdJqTkBuI4)2Rl27H4jjs%7)Y>VGv7CU5Pq;?M z_6G71E&Jf1wrT~QiDzq%J)k2F^ntfd4yM!>fz?ZKk|1XS35vh)(HSSOHSXbDL(b9; zb4h44-U$0^cc9zY;V{g54}4bbZ-2`9Fua<7pnS;QRN5QUACBBeCL8bR!e`F`czb9V zvHSRz@RJpwL*XVpJ-3N)s;{{-Q#jiD!vyV|EP2=MV#0`NTlnDRgDdul4d!1Tkm7ZV z9B_g{Fx$Rj0O0y%c&<W+H*h>hjmH}Bt5f#CGZ`s9@jxCoeEojD{@E$`*?txeHBaLg z?l5BS9`m8QU*^NhdV790Q%a`T<dLagJ?Yh3yO=&7CwjfFn)^0Rg9fzEBl6`B==d^y zy29-<i8^$O*(ljTlh-E@xmn4q(D4BuF)XB_?v)Okr@9-C?kgf)Bj&>4EOWukt*R&y zsm^Z*d0MV}&;rFR>PW?TZLUeHoXp&+kM-Nvl1WsbdCeXt4|?Ru^`Z)r0M1x)`x<Q_ zS82PUH}zn}ATy<&gtd3mq4^>%RQ6MQ32XSC)h8I(_=!$@@{%@h$+zD-Uy&5G58*Sc zu%hqPLe^-X7eBAD4xcpcE7$2RBobcpaC++~R$+YxlDP?Z@a-V-KDd}Uf4G=d@*QMg z(p1h+;si36q)3phC9Y77LR+Qpv^P%&R^7`Yr|0WnorMxM+r1*Cxm~3FiVd2tT}^YN zJs7q!lv>`Cgh%=+R9R3*CC1#tg&U*T84LG-`jwj%OD5Oyo%7E^=&xFA*rLtW2TjJ@ ztZlHTpB8Vkb}DWe_J-TNWh_x#{gn6^x?x1#7yLFT8$7RBGX3NR@=cXPpyS|EEI6A) zgzXa8-55@K&1CR?+X`}h>{I$S>jt^xd6+Ee6+@3;1A2PAI2L{v=e9+xB)4~cD{ma~ z3e6f;vlSJGcqyqZ!mNaubVTq*cB5A_rVnTVTy!7D2{h<{`SEaiLMzngtOc+ANw7V| z0d*H<)6s`h@aO9)uwHT)+Fc`w>rX>+%jqfi#_SPo`OGtRA7n_gNe0d7)1n=7)X8hx zY^qjtpPOy)o*T5Yo2*Ogz8ErOA_%f?lcx78iHh4Ad~Fzt{pFqz@jgzN@THGGvt=d? zUJ-y7Qf6XT%_XiY{Vn_&yn+O$DS>>}Kp^XPbAI8|$q2UqSaV5-(|Df5Rc7skr*U`4 zqq9ml|A`LNj$K2-{d<^jeafZ2XF+PG3&=Eh(7W%VX~>F@@->~oFw;vw-nuQI+2aR6 zD(495MGNWmdTWw<!HW-)*vr4w_uwygC-JK?a$sPO1bb5I2I^Zq!p)KvV39hVTWTuF z=0r2FTi*tjy)?((vwNZNtuIM>H-)l=zeq5b2&+E)pudWwL3vyl<wOod=u0RUzvf8C zpWZ{dq${X+Qy;B9XhOD?e<a3c9rWTR7cRra5^Uyt<7z(2(4xy`jIXaVx>q>z;U}IE z+mdK@)*27~gZC$@FR_N|MpojSd;uIT(Z^rmBiIRHJBgMCL(ED{(QN4=I5Z`e98`KB zP~M<}0h1z#&!#x?%Wx#zzhyxiqjQ<LTH?6Mpp&>{DpNfbXH*t9$L6X2P^Er|)*0@h zK?4MM-7t!_-P7m9zh{xqtTnuQVJf@kmn_?F+_Z`#J6hPWfiJP)rx9;u(h9trF06Yh z!T-|JfH}(Hs5NsQ=Jrj%S2nvq?1&^E5gvtBbC)xES^LmbeI&luc*nhIxqv2quSk9U zX1u{UL)+PB)LilyJ!~?A#G0Bz;bcwpXfPwbi~Iy~nlDImgcZIo`@y{Y97FDQTHzYy zhuqRJ6*zg$GM?jG=;EEHd82Qw_)aaJ2vm3Dik4JNxM_(mG*<GXw$Gr##sl>1x=fm* zMd<v;a(vm^oA4@V94y)0OT8**!;|@oh*e1rew26S627iwCN4u}%;6{^rJTfhr&!}N z-8E$6wq4AP9SK}v@kcUSp&zC-Q%=eOn3{q!;5|Z~zi>w%up*ORx>xibOt4|!4Lt$k z&kv(dtv1+ngrJm3HBQ~9&R<X&$y<fZhJq>9<Yc4@UXWZu-!~}Ii)20TEMVBkp(_08 zqkF;0yaqMj=&{di^f1ovELq$riDkiZ_|;D2RJ|5M?&KlT6L6XKhP@#h?h3$l_&yTX z+d_7Ywc=;aIKZwyzMR;}ud4`+vacvF_{Fy0t;P*Q)Y!4>D2b`g!a<uo`2KRL?CG~5 zc*Rkg=RSX?!!{RFGneK3<Qkroc;|8J?4F{<y&Ta3Knm0Jppa~L72~UV@|bit0WCBr zVIJhwa61O5&?grZ&}VcKmC<=7klBzxZz#tw>xMMa)0<^5w0JF~d^E?lg*(}lkEz1d zCpFpmaE`AW83hgQX&67VndupSo%&=R!u7SMV9T-ZBv{@J%N0(LHq3^XYrm3$CB7JA zvz{ml-_gg{TBzD)OE@Pc4vVBqX&2SQdKCqlT3t#ylt*K#`$Kxo@Hlh2w~(53sN%|J zW9gT>+2p}-dl)}l91V7?Mg8H{{MKw)c5~N7zE6#^OHA(&lN@QX)6W=#dW-4($zve4 zF_M#9F(0-aNg%S?$H<e?YqaCu4p{IY8rshe#T{-L5E!+WI7zgWxBO}$33jr~tnXH2 z;&o{}aAq*bK2|BecySu8{B)0mZBT<x4yJI(S6QGIf0hecVpHy;c!f#0c8!|O*W<12 z=J2O(ltE`&Ft78dnZJ{k$@-6<%YRATkH%N`Q*-H=I62>*sIOM$i`%#2FeP`~(^JT- z>?aKazDv+(nUf^&+;%!rZeMw+$s%$+`x#NmOGMM2c`#n=2Qy}I0ryQX4z6sIguOe2 zC^POfeY(IFnnz5;H~C9Q-R@d4<xME_QLT)Ibh2!OtP`Krt-_D3-O73gy=284Kk;K` z+q37-=VNA412j4B=k2;)LSoQP*7-#=ggE;{RA&>#V_HB*K7_5~M$&=lmmx_^j&-~f zkIQZ!0v|&saPj*_<hQ>j3gi)eJ82f+qf6x4`7LDctpI2aS`AamdZ~0sG99qQi(dCG zrt3L1dhMw=uPio?m)f(J)#<#%I>l-W-_`G9XFOPkO3W?XFnTz=`EU_-7-_)$sH1el zXier_dMa-7vnPAax{3RabIiQMaX5_|LOu+7Nq#I|Oz+esz}_R;IHG<y)T$_AX^9g3 zaBDsHX~tpVU|q{RXpI&O_HiREPW@r*w6b#RjN9bdyecwZP{J{;&it*byJ@{Jhu^U~ zn;n>_U(vAIgsu4^&09DgCfj12$N>{W_U)Du%u(#YUq_Tt;#U%WJfR1JOHPsQ)8*Vp zZGB?f(~m~?*Fmv)3Lr3=$t>;4#-K@B@bIM={bnYQWAD_H=H@sOZS#V;o?wqdUR|O` zno5}n=N96q(n{|zr>XmOA+Zj2<WFb@QQ4up`J7~TeD^I7Hpdph+1O6B+cyHcZz*7S z?{F%+Qwp-a*DxdO(i!z7?-*Ejhi+-tVy3^EfrE$olY^5N(raZ$NxXOvN>8ug?sf%} z>3zjis%Ip0dF|txgD(mu3Jk#Y%rB~~z`%0>A^S$~REOC?k7--L=ar_Mo&5+%p}M?D zvI$+?tIekA`c@>GZegz!US?;0ipMQ0%sHdMvqf6U6T+?!L;se=ynp3YhD=VvU%PFg zE&CYxX)%^G+6wUF8!4<43Q6Fw{g|CUoGZDKL-I;b(Ce$t5UGvbG*!nKO`k`Qtfk?! z@|_%~qV$Abnji^N?oA?Z_PdiSGOy?q&K-K$F&Mk{036mGLkCFC76xt=XL<KzcIA7a z{fqId$c^H`_)cyqTsl=lHz_2+zyL#>r#u8Y9p%vR#XWB4m3p#HQxP|>G)BL8h7QnM z2mz@BQDtx%tqYDL)!PSvW6mA2e&ILzUP1}p?$!mVRrAm(vy9x`dq<F{(oBy1)W%WA zJxPXdB~gCF(R7U%zBT_Kd#rPU@XKx+;RTDI&{2IF9h|lKYPq4Tyi6LFIW>UA0ckd2 zf)nYqOu*VK7tYIbAw-^%VxzX*#;GoG+&s?>5bhC<`-bO3faG9gMB{6I{#Od?&1kp& zF?wBQ71_iz(r&*6=sWuh7uZ`v2c#Q7-gTB7+^vbiI0CLE3)nXnJNflZHvF2CuTZS^ z5nH^Yj=gQW2yR5ip=<9FzRq_iJ7uI895nS~Td#eG-JWGYYvbwelAq85KZ(QkeCFt| zJMcN+IK0i@jSV(=z&df5EN#RZr;5o85v`c~Y1+2MUsB~=Hf`CUz~6r|9{11O4}#QB zbm!!u?75U?`rOBZ**$GMd*Q@>QscIb3g-96mv209Ur{?X?z6$^7j(&sl1zef{b+C7 z6<Cs;3GXz*$go$-$wSZG#P{75lGiOq-|JkX-jQ9L$HzJnn50aV$H!CgLk?)%tS(C4 z{0-ipF4O#NZL324!0`ce)QGKRq0jG;fy3oROA`&u%V9_HM9&_08+jViM{mJQ>l~Om z{}@TOcmZGawBh)y$7DwT0l4F3G_+<nl6$^;aC-e#F7mw`*1BtgrnD4zoSBHXTJy+D z`KchMog;8P9|PWfd7$rX!~Q%ciuDX>h3Z><AUQn^f4u!jH8pR;q1grG&gm<V$H}uT z+Mmd!eU)&&%ovtQ#PYu`Skt;S+7R=+5_MOuW1KQA*l)Zv)|sc_Cffj<Fk}Jsz@y+U zb{wXhiKOQb&O@D@;Y2Xt5J*fO!)NT>gC@?~;Niwe$j9fSg;)j3zfECZOnithBN8ES zu^alh_hSz|??%D)21Y$}cj?M$Eu8IsQ?9nKm!yvO1C7G*knLtlpEPGd!<|g>(8msQ zWT%nLQMTBst^pYnuX8Vxd+F7>m&`!Dc<%L2Rn*Cgz`5iWqjyOHkFaW>CcMQwuj!91 zF{eo3fv*_%X1Lw_DK2o~!6{1Be8H-9>BW=+Rj~8gDX2MLhTn$XB0m06VDPz*{Lnj& z2i$UTvZE@#(dlKHM->v?CK>n|77y{>EWYfJVRhX5!?vU0sPnj&+!|~^Z`^9dHIL3S zE~C3>ifTL>S+$a~gfL87-j5BsW6du?8SuY+4s_dH;rgd)Q7mr(D(y<35-A2in$K}f z?=oTRI%7PdF3&HM9YynM6Uq%Q|D>DDlvtk?src&Kbo906xdRVwfy}2|9Bprl@@r1u znAx$Qe5Mw%4408~lLusf=yKLK{3v8AY=@_t%~4#^58Di?$!d;4<84te(RVnVAc~n5 zIKL$=dYw2|rkDsU3So@NMm*!Nib&?RlKp-$_;ANG=o=PAQ|`*)>4(zrqiz!3+E__q zza9W%!<+DKi5gQlypYqstVFcO>Y}}v7s)_RoHb`Olvmw^FJE<V!;7!5E9E(3lGy|B zE`Zv0>0(@kEnPfGmP|S&O^w=*plw+Or7yi`-k?MhTkQxM`~vLPBgyvJ)G!qo4@WKq zK#5c;RP~iWxYh`GxalZV-967-bPJ*BbqO@;yc-BEeFVFk7Vy<eogF;JnwqIblcX8p z_H^Vz-Z?#o^gmDp5;D?!+I9t;XfU6AzjTr-N*n}F)yMP0GVYPZ2X8<^X94<zM1jeK z1hB17V>X8zht$xS`0MFrFkEiTP8lW1_t^-rSD?iozHbcfnR!@${4N$x1DbE`0|zbL z=-~3%Fn#kOu!&z!(nBQh`0`;$7RaLgrd5oG^-*vdyacA-y#`ee^l{Iw7#t~=N3Wb1 zj}l8#aEa+ENcx})hjhYex0N!~b!OpQ-DAvvJVgv&Fdw$Q9L<zeUZlI*;)srNF4k<g zOd67wLdV$eT(L|coFAA;Oczgr0}n00Xx3-?wXBOAm?Oc;OKpb<Cb_squK?9H9HiZj z&&v6`ijaFviB?DqA^c1ktlYm6#zi!fmJSzm4J-n`#x^FE|Aon?f8vF$(}_{wDtwHu zsaJIhlrPPIo2TUXjPEkU|By&B?TDg=-|SI#g&I0f*-z7zrjX^6t?4tGhg-UGY3-5} zIzM4MjvT52x=nLoOL`vpv2-eEBwhqhSvw-^7|e#OMB)(E&dpzV5H?GSgU0y)IMbAd zr{-_vt@Yg5lb36$f%7_SE#HOPFT~Ikbp~&*+lqJYZGz)dOrdw?X_#?-3%q*dNlV>) z$@g3P!6l=EYm+shjrUDpzgQep$($jJtM8Bn%XdRh#Y1L|UJ?!3sg2$G3<mmw$lbPO zo^`b|=kjU=dNI4GoVa{hcIGIYuV4W#Wh0nfifIDJ8PCWZs~j3(GYE5+hH^u`eBtCf z639o_4#7#Cmhx?@uW}DOTbcP@`smwh2~YI0Xy6z}x>V^Qt(g9XT1*&5GxKY?!c)(w z=7K6ZA=ZKDSH0qP?lYkVrXR|tuS{ZWF1;x?^>?PeWEQCin@gX=3e5TH$z7L;p&2{m z(a%Mf{<>ShJsfhi?8Q^7atU)2Qfobw`Y%?8qOisENcLqaxA-<S_|abesB;-c@4ZP* z9v(+Oiro0auWr*%LUs7rT14JV&}NjLxDv5>i>N`vAW_cEL1tF$O1zZtmKt}M5Gj{R zxb<C*^ytM?vO1TjJXM3EDH%*Zhj-Mlb}`NtUqLtE1Lle-7eU~$o$i^sf$(N!+|^p3 zheJOyN)=OZ)yk*T>*8JNopYE8`mDh9Q@=^|Z-tdFttv0y6`@1U+%jS|?G9$<D&&*y zf<j`xOOcMwzQ`>5_K=45>mtmmNZR+AMdiW6=!0vM$@6d5n41GX($KPO`<HJuN#tBf zd@0mstjZ_AB>x~9Rv*bZOh~6;qsM{bRgR_{SHhpa7^XG$IB~mPB(RV6A?7s?=rO-q z(l|HLE-pw1NAy?<bX_F4uqAT1ca|bvA#2E>=oWfJ`61)nJQx?;ai@(zhUllGE^u0w zS)OT{&73s#<rX(6qOtNhf#j<u=C!9Z&DuAYiz*$9<8qs92L(Ri)>n;zv+8oBV5kMX zGxaMSPj7Sc&ds2*=To?OpDUU0fhpvwa5Z)I)`5!4D<C^|K5dE{LT6e(A={IOLvq7< z@}%uOnVx8gZ#-+bh}8o@ZO(nNc*kD4@kbQ7_C1sYEY`tqPjn&R(o0fgsf&A6uh4Bd zZ>YhMMP!$2m_4KC%Vj`++LM*bT)d$zSYuF3GqS>{ti`EvE44(@-M*28WDWt&{}l}j zdO$;LB1i`ZF!RYWdN$F5l)P2pKHa*=T?{@&y`@w^+PQ_=%y>nHdP~5Q(33=yYvu}W zMbL3|BXF(R7IIgYr7u?!dVR1wiMuzE&QVfjvOjUmDBI1jdDa`sKhmJ_?+?+H9qPoT z@)Ly#4YcKX6^Z!TNN*?}WiB}AQ-e_p$)pVh+{IHi+>z3!^m9}qm5_|!rY<}~b)Ty< z-_JI3KX$Aj3x>(icd6@0?usxPls$@GFFHYNrBdj~ww&_d-JW#XdL`PhITO3Mcw(R_ zNgf$Tfcv5;+<R?JA|EJA$dYh!@E2ulo06G2w|Ue#$r^7SAIR)U9t6jaj->jF(z(c? zqFCSK7v$`79eh8lhYq|dhL@wG$@zzS=!lac#NwM0{IV|Ob`&Yn(5wAO?z09ia9bj? zW0@$^<jY2;F2a#2;Crg8T1V>Jg%Fo0L6g!Sk}wxHm{8utsHnc@c3~wq@zgHP#Wst? zexHpCY6_S^9g;93WHLPw9myC@)<T=_74%l^Uea&QYr)R1+T><N6@9XB5V0{hD!6sM zgjsy}t>D#!%bfOy^UR56J$g3Qn5=Xf1R;wjQ^}Im<k-w4TC(>7)jrgpG1{(4*7ZL{ zlKak+-coaBzw>)R^zGAJwXH9;%IQ}|bu4I>WB`qG&!qf4%3V0m3$$}HxLp=1u%U4b zB&()UzYzneNBU&0@30c*e=LGH$}gsg?MF%XR5eVly+zlC%2D-*e7gC|D=xy&3nolz z=ccyR(4jMfac$gv`tY$56`rdh+@4lypFwbN?g3)`BZHgwI1ksSPbHDd`ho4Qa{7MG zTQWgw3;7bNg2uc4lA`<PZouym)Wt_4HxlDtJ27{k9=g75r>{@$C3jyfqjAAQ$^#>8 zkxnJJ`jaseSF(_73!jX`YLn=I*fHF*d+uC(%M0$l$!8`edrSG~ci#o}wojO~JCf)L zaT8*3)zp5|i8SV4L}mZ@^6`(aUqw+_%QS0NN?1z2uir%4!H`|_?HPAufDF|Mk;Qns zM<is^X#VQSrHqRHYo_w~D1OM^N4Rxf2%Fk13nqPj(6CU9mtAp1upzGylO89sB~dkK zk!OvWIV!yN`8Iy1LJB`Leh4qjlj7Ul4ft`J>tVX=Xed1M7zf|E1REp{GBuIu{93QO zxNg)aB2}`Rt-Y*HGoFnlQ_pQ-_ilKO6MN-IP@4>vWnRWasn^IK?<BGp3OK(5?{H9k z4D(ohK!w;QQJO}Os&IwIS+-m6DAT~m(9*?g_|0qk@h7XySc9e>$g^J|{It<n==OR5 zY0}ijpe4$zw#QybtV-m#3{yN15QJ$Bi7+WxRcLi&36wn!geNm)*bl4r(<h?WVAsjf ztSnW+V?lxNWg<^!2Ct$G1CEdtV_6VC)mvUPXfr6R+zLKbFVQ|Ij@_qtnKv7x&!4iL zNUpnBvC0w?dF@mKQLJYaf3|ZJXx2F3Os8sE7VpZx@yvzk#~1S6f<gR}>|M+&+aL7A zj8Uxrg4N(OL=xViEIYi-g#W2G70phrpray10S&zxRGc6OQ3rR?;U1c()EYttn{L3N zCv!-*qZl4CP{N!927Gm9l2ryLh%oLYeo@Hf$?gfP>CiLm`*?ePv1<kD94~`0#mnG* zd?aliTY}F$eL?P+E4O7rEPQa+MUnH0+b0Kd1s*ps+&vuqUFQ++QdxFeSAVv7el$#u z&16*RB69g+C{&NNA!7?iVnRf`Aer+jf9#%89)9l$bKFyi^sp6nD`kV%$3rmVs1ZNq z(`2E?s(2ys|IFUG;|sT(zruweF}_4;JbTl|8Yaz8;d|Q8(8=GWaH^Omri)S_N)(%k zc(w@<_7CU#o~f}N##1>u;SN<!$wN0?f;CrKiSC};<o?w`O#FmPWcPC=433{jQp$#c znAAhAYUM?;yYCB;iA^B)KZ(Q4LASv)-IWixtizI557{wiw(`%fI`iyfJC>`9;ZIE4 z!=@`pvBJ6%wr{AMaLDH2Y)JGw$f64%ePADvbQ_7I<LZb(V<2oyXoaIM8lZSJ$66+E zFrZ@(CA=Yov2*Ylo*@-dk>Gi09F=ME!Dx^Bg5Z<4sZ+5gt$kEZVaQ}+XJ(Do)upU= zpD%j#j^+1#GGc|@@7P%J1|~_elKoM?h<}|tlebCvfMon3cKC5sDBkh_dLFey^r3W| z8K}+vm>|aqW$VffPwm4ZIUT%XzZ?gwo644jyRio!+pr-g8q1WwZzDfP_lM-2t}tjr zJvlWki5hM{%6*YrM~9YJU`xMdy6-VZx8$#(1+{WCvF#0fn*eM-iJ8JLZ5#PPGRIjr z<5xJ^ZZMm$C>BEoEn^3toC<3WIk861BQP)PEe?x51~01vp~^9byrMP4q-GTBmwb&_ z%?&3D%BJv*jT)@4zdbAzi(noJPSA?(8o}O-Pn^ZnnY88c2~MeP4Q1l;$TQqUEt#i+ zW;Tjm+PRl0TD^qsE<J|V#|hbx_kI<X=CZ<9DRHnu+YFW+J5Q%>x(yGjd~j%hJbOS* zojA1Xl7_M$1SXoGSL-smVcUGT<9dspZw<z4GxMO>b0`_N;5f95xlYY%*V3rZt9Y@_ zEc(St7ZrBJ^0iyjX@K2B`#nw9NJemfkUkv;-*eZJVJl<l&Z(E+UgSV_O7}^;UgeA* zPc0C>-qXO=3q&(}N)wby`H<u<@@V`u6PLca!ZP`x?BWeJ7%nfx8#LQuKt&4N)fmBs zZywApZ73I|Q0KB^v^qeqb|{Wc>O%4E8l)wLXrOtI6q;*;RG2;ux5=iB5iVS*PB9ab z*v37VdqCpu+ChQG1X22|962l<2SqOOY|+YF?D*SLSa;___QDHI;a%2+4XL$fpJi@h z4NI28vB3p6%Q%<6ZTOU$9bQX9G!?*diWpyauRq_P*@(Nfy71fU5GKo_kkrr8pbzh* zBFeeYY#mGdSuvB0acdH-IgY~_o_le`{9GzM`YiEY9g64f%VBzzE<IK%M@ERXa{E%{ zdB^9P?5(6E_}TY>ZCM2TzWRKa5YR^Zsl8!k2J5nJ;|{`J>oUl17qG=DCeZXDlN1+L z;gLzJcsDK-_)d<>DSGlXv4_cmA5*v|x%QZ(djwps?}WWu&oe*lw+Mz_tKyDq$)gMJ zKP8prQFQK>QCR!ShK%Ys!HjG@K$afX!H*s*VOfp}yZwj_TUk<9R`UKGxEr6rW}g^# zM~DVHeflPLka{dQ9o)$qDn_zT)+UJNf~kCGg9|==_>;<vvBhum$Fu4!W5MIGA^)+n z5JKI@f%-*tOmzyyjq@^z^OzE{ZSNpVy*!2vtQtskqq<3N^DkOFQX5Ys)Km4jugEc( zH;n3=>vTV8z>{Ta{EV~e{Ogs&_;7<w6}wJcV(TZx3XR7E;Ees@{H(hfu<4yD>-c&( zJElB|?NmHU-IMNv+y@1CI`k;4TR8+wMW0rDC-<Ow_-pcOx+U4Xa5#Hdrx+&X4o2g> zKqSpXGI_ugqI`4@jA<N4dxkxrMGMZ++3^z?`{ldoJoRc)(0YybYp!6D7UjUHWf8FL zqZO|pJwRA4(}?A_gZa!yvFy4Nr|`yiIexAnlFd3Zg)Q;x!bWcicG;RQKx4j>xuTW! zugQl&Z1gl3vO$KI9#IN@8@#z+(O>bog&LgNpN5*7t4Z9fDKvWWFs9Z03dh|HAgN4$ zFyA>0mI;^9jQwFKOb(@0)q1ddzYpH;If++Ksj!ch=>ch5ToF<2RiSoixUlMM8-6bz zz&gJ3rL(wP_QJ21e8Z736=|`%_$!Ns@?|g7`N?W_e7oQtw`=wn%rJN4XS=6y=jT4- z(j3<D`Etv7&t+P8!?guVlIywi>4q5G@*Y*@AAx53!8oI?g4=Sh7<5h!=NE_i+m})e z?)Xw?zGY%1$j#MfZPzKm&JL*xO^pjU+~5MK3shjP_xQ41ty3Y-h=;{nH?zktOyNPf zko3LmA;VWEqLG$8U$uWc%V)e1EpM6dX^kV;{4!toa$q67qxw=5{naJ&9=c+1=}+2r zyNV`i8DrZIeJH#gNzVw@GCz#0!0Fm^ELAQbpQ}YF{@EjW?mSYdxPusd;tMW5c~JQE zxg6j3R)>^nca=+w6XQqR^=HEd3}L*lNuZi}40~hQGJLa92#3xW(8(`0fnj$lF%OL* zor9d%AEjcn;G_moY#vUw4$Ywp7hBWREry^tU>&!APa5frssIbYF1*yI20s?2QuVQW zang|Ebn9`RI9-gUdv-i$vI>()+sj4lpd-%&XH6s75k=w{)IWy5TQh-L)`YMV<ZanE z=}XbH_XyiCXF8u<X2csF&WD8F8+4LYJ&wQT3Jcu(QR5eRyu!RJSj-HDc>(KKFOe&< zGRl%ftCtdQG$6q@Qs~toyUJ$QZYGMNXl_~ECnh~{C4IQz4Wsext0-+fj(Dv9L5x=W zv5Gr*-f*TQ=VIi?nkcPe(N<CTBW(|B3I+VvgA)bAl5c>^qC_@j!7w2awy_D{T4B(~ zYmm7{iG6Msk8>|B0`sYn;E*Z>>kryPvwI<1OB_M`c7G5{tHR9{!StI-E-f?cMz!F4 z=4sj$>hxwiBVEUEiu<pD`NTdhqVWeeYvWyl$!YLe`XwX>`l5_OyfC`60+dbE__YdQ zAYN(Bddt+K*DYySx=fY7bpIn3O3Uy!o^{cUa)-#?&oyu)cN!+uL}9d85*W`grI**Y z;OMY<{K}5y_$tkeDQfsY6*8`ox<v|{x3wIlnz3Y4cYg@bRl{ffJ-B<4+GIo4aq9d_ z8>W{=k=g2(*dFIOxc{s<`(xJ5ili@9e1XzwS}Q!vn)kh>`H$q;Y}IU{qBe&eFs*~x zF)SOjw@zo@i=v|a_bB4|L|q(uS%GX?{E*lQ7ZR`R@#NAmCA!AbnH<0C%GlQ@VI%7V z+$<ysEjjqhbSPH#&V}JxbzJlbOGYqu1pjEC3-P#llrFN6<m+QhurYNtTUe>j`p<P? zv*ySO=Ukf14jSAHb4xAR7L5w%-)Toj+)gAH=iK5ejs=2s@G?4Zvn=XAT8no#M}lI% z1(fqF<Ky~L!F6&Mb3S7Y3~)P4r2G%z>r^#<K=TEvzUV5GzGn#EzrhHzbryhEpc-A< zn@a|-Dg|H53=%3eo-==13q2D;`7g&!*{2_p>4e-=K6~zTsMK`lm45BwU&R}->jy={ zD<4O6IT+2yhJ4_?vN!Pa_Df(JV~CEIEim}$Uijes4nIlV#lu4;pwa0hdQ^S{|1ur1 zY|bi-%Id~N7oB0?qgc9a*9Tl#K}boO2dJq{r0*9^rwUI7^66JIxjAEIfyr5UJ~Ts& zkIZ)G7p)%0o{Mm;`11Z2EKb&8PZd7E^;4zzmWOuq$?yhJ7oUbY%QvuD+H!2tb!Gn9 zDsgNVtYkw%CNlo6XTVS}8m=ZEL)PXgEqYr^nHT|E=e7V|#8g1_CTWZ}Im)?qAsA?# zp>tlWC(7n?xaMvba%Ite$cPvN-#8s2oES!yPF@IUhM|1%^af_d$wufm%S$Le?+tIU zz77h)-}Bzb)YzeI&zPAPGS~ytzq3tK%;BukIXZ{$gC*}LV)?>IkPucdt`GYOOBZi} zXY-E2j^p|Ci&;3dT%S&q#x|fzKpeT3FY+lRzG0uFAyXq}k9F5S5!UD#-LrZ!XyxZJ z2VC>$_cK1wR~ODddr^RwTPgag_*b}1W-2m9wbPm(-n^HqF*FW3k71X>aCOFWJ~#L= zG3go3o=^V3Fpdfky!s9A@u7g9;eHM-eN*S_n?32^+C8Wf=4n6gB1I*YnaKKugWDl3 zVs}&rawH^Z|1pQT($iNMZ=W!xVqzsHwe=c}oYcW2#~kM#-7*2~h=(Nd*DY`;U&4xe zRSOHvbBXP<54hbViH<IA<Su31gy?`2T+!VFA4Tt>8=tbu+n7XnL1yFL01w=u+5p1P z!RY0xh7<Qw*nj3ClT{K!oj-<QN2>`Mho*x@WeLr%@~1xz7J_q-D?0etLHpyYRC$;T z=H?$q(~4)*W7<b@*P{wG9TBB3rLmT`9Ka(zts>jp0Zi7m;)Py)o|_fH){(DV?p14O zldQ+eW1akyVYPf^doEXgE`^3V`EX~``+;D32Hup@<1-tQxb;cz=)!Y3yyl!F47&XY zA{0x={gwUcA~u4)o^zYP!ga{ax<pP-e^Y*!pN4@8+c0B^RM~6ad$e8UIXaHKk4cYa z^7=n2;OaSna7p+XFn@Xn4&Jzi4F@v$L0{C_^82D|=0C-)QLovPVJ67?kH8F{<K(Kp z8#}2iQIK_hG~z5Va-88X+dCLq42JPL_H;tTXEpfKsO;a@L4NzEMJ}6`Iwljl+Xt}O zp;|Df`Ua^jnnAk4&ymf_G2E_}V)DpK6Jm@_QQ+eXf%3=5RWT<RpBr12BEJDoylJKJ zOA_JWAw{qmbewL>ct-DB455Sf+$Fl^=V-AUL+012z=Y}^dS}TM`qfbfr*3VciE7g1 zUqog9fcFnk*=-|k+e-+`FS?ia7Zmqp+g}}@Q<`C+!OirS78sw{TAuSjoGZ}2W|y{3 zvV210e6q^OfYUiN+rGW!QhDd85%x2)zS&2=m~NluGeFQOdrGi$S!J1I{Stxc7el*2 znok6{rpJD${~5uVbFF0^E{THR#bX3diZ7IRZp#$tAJ|%6uHR99ahp_`*V@psgY>rG z+N%p?)*I&7FZ#NeG1h-mKEzj5Fv8>vO;}i1?#_QK-+EX^VD8e73$gqn*cYc;o-nOY zuxnIDd5+=Tvb?ni%PkM{_U|_f1b*Sl<ujMXm;2opV;)`^Uq0nRcDe76UP0>uNjiID zGTAYTkY&{~s3O-*y;Uk{a9~r}^LSUfpwk2weT^z#D-|jbKThGt>g&u~)k`8b$%K~e zh$UAirxV+`#q`aUaM<*76Y(${2m8H8Q^_ZWuzOM!9WMQvDf1Wz&Y_0{>t=ZiW}R8f zJTW-T>@W);d+Lj+a_mHA(G6uj)tH0%-7a{nUIM)?THzf_Ig*dJ(eLdwjN2QA$paF2 zKi!qEw#}G-l+lG2QX5zwk4AL;_?|r53n*7L0*tjMW7zjc<tiuYK*g;dS}Yd`_AcvX ztVHg!wwELXg^S~opt-o=O)Z}LUJ4okvtWzgb{z5G5PBYRCb3t~@Y7}1!Tg``@L<k; zDhe5+)8k>Hq>Tbp^>&66>GP;t_HcHYx;XzvA|3Wfi1S6`f1%a6LhvY<&EJ)nfleo# z*{WF|L0LbE^DLCZ;O|4(MbjREvtkSzl{1FbGneN-38dMFS?S<;L<@?x`Lc|AE@n%g zgvIN^*b})Iu`2c|1`eMDtJmCuJExjqOt2ja-^IYb>*+Ab$_%X>^jXg!JvOf2M7#u# zv7?<Oi(C&2?hG5qdMQ04-wI;!{=@0`LgzFl>#ZlT{<&B;r;qj>-M~*vz5~f8I?1_? z(I9;959|Ci1Pe~T#gzg_x~)|PJ9N_dEjz}uT^{}UZ_X0vg1gw5b(itAqbo{?hcoB- zG>BBmMmMc>@J>mIoqHh%9$0Flt^8Z2Y~M4OFi<4d%vTb-%`b4{G-bZtA)ELH9D&(8 zUxHS9A&URnO%MNHb)9KAR#Dit4VkCRLr4ly2^pWg*V=|?P*MsdV<kf=6-ANEGLK~n zjVPgkJkMThQzT79Qle2ZMuVi3@A;<VeZTK`-yi$W`mv6+f9!p%`#!Jhx)V0zP2x|% zbNL)-q`Vx)em8^%ip|Imt1YPQy;*SO<Be3e|4%3<xq)#g`3`v&>0(j7C5+zDIe47d z%)H%|jwhZNQyn8kQ1X2UGG3H{RjzKQSJ_KY#qK(cUiepJXez}hS}ewUKO}*VH-BLD z2Z?Z*swm#%HH+8h8ZeAN6_$E&9T)4bg&uEw5pVJy-1+hWH2lC%efc*CdEJGWnd4T> za?L2**CoLqm)BhJ;Z~;5pc7ZTS7D|zk|{IkVO&}%Or__wWB0`}tmjVwEBTErQCZl; zI(~B{ac*Qk%P8QJ1<%PwR%~G+=Z;Vr>wupe$C7=HRBkdPHpD-+h;46a=t{q15$yGs zt#)!VJAYvkN9xXE;y(3=YdqOPTz{s-l9d%?#mKm`p^Y}X`T#FyAW@Kb_qmvLX3sbF zY0gs4o4u~Y(_t}kWKN!lTfKxG^7;~I$mcKX*4RSM8jl_Hm6nr)+&cwMXF~#*Ki*D= z&Zw}m-}SM7#=RyuhpRam*HSEIyxQ4w$wh?Qt0P4J-ykB~+n8lCxt?PjBf#qA$tSeZ z5$E5qD=|NDmR0$jkDgt<6!8g+l1ZDn|AS8{DEv|#TB?$Y_#Q!UI(QG-#507@^~-FB zt)3w3#~ZlW+@5^!RRSckJZR~Ii6B%$0d&otA%0DN2SN{$$wXEtIP1fMuDC4*AW{~o z$7_Q%p)bI@xLW#%*J0YzWjTj;^$2lbA`gcRPT_58cQ}`$YQYb$U*O-(bqFsxOp-0G zh-XDMxOKK1ylyfFRW|3*-^qU9bGeDO4<*TaDg)p-U5v8sCxJ(CPl2-1E$pAN7W;~M z&^`4mvW)9A`&AZ;9KwZ(E9XssLhlBU=bQ**mn6Y-8$PDT*BfRBZ-%Sxg@6v(2WVS# zD(0<Arq5h?LH@K?g4V+_Fi30+B&1TT3}ZeZO`Jr=*B*mcCm}G=D1om}+8}-)j+Fv+ zP<p=tT&LcP8;5z3>d9uLzvL(49r6N;H*G}6LM~vJ{7TR)_!7+;zk(tD94uBTkNcH> z&=1(sG>h*QJbP0Y+hvyFr6ck<JCu8laDJ>3VhlfR+W_}Is3sR2sz<gbC|H=F$jF9O zpx-V>Kwa!^d~TCDC{GlI*`dejQDJ4qbZIkuW8Q`o<GWF`1|M~JbR6|BT*Cc+KM<m} zo}?$MpOorXM<17OLO0iTp;rwm#L%Ks=B?{Ip@FO&DYE1yy-rsSW`}39sv;kNrZW;q zrD!*ZFf65?wcVk4GG*!B4hn3t3nCslatYzp6#Cx5a{5WrGQ!$wkkb-8#V*tm#|PY= zlkt=_M<&Y(t-7a=9K#lqX8i@A883i!J80T)u`qgbS03K_unI2uw~dZ6IE{qlg3-p+ zW$0#w7@lYp!94AY=&zbPkk;oU(z`hmY|!l@6^t9n`PGks=L%yaBxpmYur%m@?CqQl z_$0aNvplRH)j}OhvcVxQB~bM*99;Ql1&+Sb<CcE8_^O>cR61AyL{qrQ6o8lExEGN| z7at<ym_9IXj}6qBT?)<e^kKMyBCNRd7NqFxMB7uA;}pRw$WtVrj(hc(R7}eRy`$eb zdCMh1?xIqJkFP>0M~$)ny-sp&RuqTLA0uK*b?8M;qBvHrjwm3ci8cy}2WWFDee>@; zw0SNAKHL&T*Is&rhD2(Ci=`HFd7p*iQ>8$?ivw9&zl$wzsEz6-vykQ+XT+15N!Ax6 z0AGdItje}JqGruz+PJfpJaj9SPN@mSx+MZY(BF;ZIky(=G~?$8mfRu*a^jJmoCTBv zf$T1BLACexY4mK-Zp@B7LWe$Hjf9fAfbZxd@aM`R626H-`^vTH_h+AQ0&>|Pb=d^# ze5oi|;9Cm5TbcqqE{e8h(ws*Tdq6<r9NC!eLfiCdqnP)pq+<9!!o+q37j*@ZVItk+ zt++<;>OKco^HPZYb}Rj5;S#v^!3tRTFA6ZtQRqan0Dd?)jz(te!IOYM+C$wEiUjnr zH8WN5ufl8~Q2T={_&N{ArwGAPK2fkZ?jWjVw;>g-XuB=`1Nqr-AKcM;2VGE>=Il#o z$9Ufp>=MW=@55ujipVFxb-_VgzBm%OJ{YEJ)MF8U#sKhX;o~B!82Bx<2HgDIK_^VF zg4wBCfl4=`@BGe2mOZK1n7)iN)YI{ToN02)cX_&NIuqaeev!4!ubZ6Cu_7OaO@eHL z3b-tf5As;-gkjGMQKEl5w+$hK>^|H?)tl#mM+)0<$0r6O!U-mJ8{ilXPx$BPSL8HM zK<{Gs(MHW;`ttIfptbb?4xL?#5?1^HO6qZRa?vIr<)O?)blKo|!cT0wNDaO(;)5<* zmGE#<G5vn%Ai%9TDAd-L@k26D`^_Ub(CJF*dauAc_0qAAP&DumSOzDUUi3Do5T?ma zqZC13I_Qli>^?hAKK_^uP2So<sc$c!{>E$=zDE(;l#Ak;;SJC}*9G4dGU1w6@_=FN z6%5yIz;pX`p+Lo1&;|0z{Ego5zofGNanJr=Wq?<dmm7G`bzY5h4B*`TlPI)x6P%oE zhS8)doN3X*#p*wa@LOxh7rK66;F~U!>{r5#y19g&+2(=wTm`e`coVAG{*$YwRws%? z-;o2h^TEtNMYK4~AD!Qs0{$B+`+p-;{}YupsP8~8%n+4hdy_Iakcm_r5P13KDQRKQ z1bP&_kdwa=oLBZ2Yphzxctm7_T_}Wc)38C;9kt-YnU$9FVwN|Kt25YC=O?c8Xn~`B z`zWFNRk-Z&G@}?3NnL1if#E=%IcWD2r(+eyDo2>=T2~0)4jg5Uhlo>wp)sUNw-&V{ zB#Y_eQYrPfqTHa8WH^XhfmEOe5qJAH=u?|UW}7ESZ#gsg{b(La@2&@lj;Fv*pat8j z4^fe&MpW;eEjUu!tI^Z67xp#<<3n4%Qhzqxro^=OG7FF7pxVfD)W7Uz=B#-po+*w5 z9JOkWO0E@CWN$)=7x7cODrK1q%IVN+-y+C^xLW-{b@;bM3`7RVz!z<MVSQc-jGOz3 ziq;*b2Yl^GuRX@VHn<Nkt5Z?I4j*`;H;?Tru@Du1@?o;@DT))HhS&c%Z+YjUB&C+V z3eP(N7`+|0sPRlM#yhv1S|}FDWWOk9%KCztAfa<OHQ^H;PTd0ER_(%b0x3|kT$r@^ zz~FulC0ra)z$jMDXEyZ@;i0NuAmy1nV`MMFXs?u^T}DrlVY|#w5&H*eZ!7?tGoyj! zw$D&7kDrpNeMZLzm@~h1>zHTK+RR=3N6d|;v$!`zgOYYT#he&!WX$J^;hBdvQ0TTG zQ+H0(a!w-_wpDh*ruEyg^QCrbxp)h<4}6M6BpK?^zZm?Wp&l<d@*QeymZE~)3vpP> za<ntWlCtdRhxsR@p`L9SDBH{pF6nm0<BAA<>#U_y*k+V&j}2uaJ`LiQnd0?Zb*Tec z?$|OOPzF}T_>yTil6@k9k8AK!QHB|~^Jx<PhaTf8U3cb_$7Uu!Ef#w(Oa)QC*GOY= zW$gK59{zZ#o?Ub#1n5c?z!HrboB`FC_`58e4u^@1AXB{7Z#^=ad4lNUXK3ryzft(@ zzu@}K4`{NZ3sp>U1&*QFpl@9aIg)W4-OBa^TTX9d?iH-F?3^{nF{L_<J)Wnbv0)E% zF}TR^#pE&XY*rvA@+9^6MKio4Yyv+kJwk(pYpAUi-Eix!cl55;8kC66e(WRcg|DX= zLT49OoLUqC+vY2wQ?rM$#=}IcJ3u4h+XduclrpX(xdNE{li=CQT+muG0FE}b5iPnm z!0ALSEOgBIjzRlY{L{tHa>CiMk@TCPj?J&YNzRc3LI1|7?a$F{djJ!ZeS(UJD2Ecm zG5C&5Ct6|vsDW9UE@um2t!N$SK5`0Nj-5gsi`G+S*3<YdiiWAI6ToX(6?j+j1n6;v zLXr{9sMXUFid$}?c^kBF?#s1E_tjZ^iZ=sG8jO+k!yRDCZx9+S31KX|mok1*fy~2g z*QwN!d*G&{6(#&9nE8I*3@<2C#@WewjG1!~iXA3EY)c4|S`>gIS7C7O;0(~7rv+<U zN!nJc2EPxV#T#NzVB4o1F!)#weS2LEI+R+%x|R?@8@(sM*0>Ge;!-j4@5Kf*^`#dG zT;Rn;trEB_>oSTD-p0gbQp|3@E0kIpUt>?KE=)gEOGnK$Gf@YPnQIsK;8lO);mrB1 zjADBcrEgpV^<X1CHQt6r&2Mt4XC~NkPZO4OWm9$Z1-kowI8NPulHz#R!RLDVXq&=Q zpt-pZ>qs2L+kd#CcTp>GVoMD@JuL<fJk4OQto=m1zrGg6tDU0f_?F-SxdOoNW&kf$ zmoW=8JR4i*GAOaWHH>*d5d37RMx8k8z-ZYdpx^s<Ku{HhUN1NT=e4iL9S;)Vm-1db z@47$COFPP8ofl{FqcHRt5T%VXuLH+rNx0$CBWShl3@}v@W|^EHMk0cb$a9w?K|Y;D zyZjObcAuXBZ1)BU%NN3*&mEv{^-eUPYRm8^y<%?X{6Q=~7E?2(#Vp;s95<N7;td-r znf2%8DYx<wDA4_zxxa0v<&dEcwf?m){2FMEvng5210QeV<oOiy%Ha_uddZUczFipK zFj9g}4bM5=J3r!)%yG0Z591(_BqYsih@b7Ea4ofwOxB4<)2NBao2o>9cN0iwz4fT3 z=`IS`qrjvpYcbm&Q_QoL{ziWBcT`cUJfOLOPW?Y`F?TuK?slC3wREV3o>`ZM>$85r zbpJ54^W{dox}7V~g>u;5_YBVJDZxUdAbRFjjBoKfF!xLEqus16U>-JrXM%jFp%tg8 zx$<OGZ2Je83e$L3Z!?bRlZMd;_+gHM0<Lk-#ph`bN3to0Dz?3Xw<t)XrHdC>a#0MW z`$HNmD!l_W41b~V8;#INq6B<Q*0j_Dl`!5l30uG00XYK<?Jed*`P?g@Zmeffj~D7t z=ellDL2s8*k$UTKP0R-LPCx`b+<O}tKHG>gH?604&xqk=YvgHR+e~zGBSXGcv4-zf z@q^=4Mz~dB1o%{$!NLvapyIt})VJJVDrQiEQmT5$Y*&_OR6b>m4;t4~wU5)O)kpQ9 zGHRlAEB`S$ClwfnE9IzOtq}ShOJfAZ60m~#Wz4h7nbEi)N6m4)RvPMY*o&&cBQsf0 zk$s<@I^>8$)>@O&Q^Op#lsL8~C&1X9t>|GWmnNs{!VvRHpzR(8?(h{MXRgfzH}z3! z=VRePflQz}xr=HzHiw-nPg7qe4^yU=2oB`|EOfzwDwKcD9R4ng>u1kmSI>=T#n?+m z=y@}2xLpZ{t817LyK`{B`XW?X@dfYDypPBCKcVLiYeLP8rI4?VAMWT}0q-cCMOvCq zz^j^ezzj8W`dl1nJ-<L$memdegt^Kkd<ie@Nd@IwuOf2?%SPvQd{{U27Idq1rxVAz zV0oZC@a?dtzO#*(CA=m~*xJ3g@ShIK{df};D%N1Bqms;piCHAHnj7k7)&s|K_Cd*& zTTn!p2f9)^iTw+20^ed+s^FzL<w6U?yY3`_3<EwMD*}C`Z;+G3aikXT3x9snj_cz1 ziQx~+kmkbWuswjAO7b|8A)u_Kw{iyjbM81iC$|***pLga<xZftddbjJ%z#;}l+Em5 zrBLQw9k?lZC)Fx3#`W-AMm=1wwEPYp=FQo7Y{wquzUNYSWUmJ0dPoW%Ib{$2E%t#I z@>Stzpd?<f`X*Ak`~}=8NCiFJvLIu^9MxyYaoc#JSbX9l7CR$K)i4@Vg8vv&e_+&j zjOqc>)!QhZT|(5*XCb7TVnXdW_!L;H`Z42a+=9~aDR!U#67?Luj&I8osHw{gd@M;~ z+T3f23!a&@kaReHzTySud1OS+8&SsbjvO*_;2Juvlu9o()dQEybYb{UJ~$x755s3X z(4=HHYUkwOPZxSXDN2Hi7O9f2-KDY7P=e)^wKP;dTg?1kB?u>bw3+#t@=SA~5!Oyr zq!t&gVdTg{@|?pQ%ADj!nQb|k)1-@!4&H=o*SRt)&$YqMk|pp^a4L!wIt7)(=D>*a zVIl?2qCwRpyiULgtypUXGbWBA?zIPu4sZh>6BS`tpgcYQauob_a5;>AQ-)0+7em7% zdQ8-n2FqOatyFK-B;~rQ75Bu(;F|R9==j$~l=r?;E;c*?w+mN7f9oB1`WP>?ok_=v z@k1bZ9=8>`Uk)Z5f~fBMEAlGOd5~d!1~n;00E?|w*uOgg2YcIKZGm#6b-)MQn6QLz z95tX#@M@eoV~jfoa%nc7DZwOW0=KwHJaoDYwU!sa5NaO&Ez8Fo7cZh}vcl-A543Ot zMDT%LKlghvXFf0VrRrry;LC6`+<s~YjvRBNPSzyhOD+VS_E)5Qj>SUNb~RG$;t;qx z+m7s(jiEiGve;dpm!Y*3nCU?w_|o|p_@&MdmOd{>ZeMPLzU|7;Mu!bePR_wMn&aqh zni{n(<TCY9P?nLbE2X^uOhJbm`DE$7Rn!f!1nQ8O0)BsNh^U#josumTY}|HfF|$Sc zAU<ZKhyu;}nPw$%^6=;)I5>8Ld1>+lj(lLjOHx%>S)8kcXo!WMpLmk<GDN7a(NdIG zybio4qX>7lUB;ptZsS86W$BUow$Loig7UUtQ9+5eP$@SR#y`|xBs4FeXttT<?#Iii zt{O)aGE@PZJ_|7WY!);0nh#h#E)D9>@27gc&ZkzM8wG(sbV(|IEzFtCL48xVQA}+W zSYO8n_s4I7`qTlc;hsNv`;-{Il&6F<;#<+GO;boZ&4q|qa0WfFd5-=XN`OBTdw|S< z3>@5@jK>#yU@!JtP#wuY-&grq=bICrw%me$+AB8xGLeTcBbZw8as_-0F>Dx1rebq* zA=45=eJQtqi@3u1648Dv<<Ji+BJN`IQeJA0Ydp2`nl{z;CmlPRw_|OK)3D;iVXQW$ z3RSHmp~db`^nt6J5YP5kz|B1tY#CO=t~&mx&{hjd8Lh-G_!l8vsXP>9FO8=k=@MPd zvv{@S=0;_!ZtB2;Lk#bQcz~1L@Zj1a*xa&&aqyo&pO<Aa96yW|{R5ByRRk_C6Jtzv zOENjrZrpO$k@<8d8b;i-h6bh8_?GWy((|VrSehV>&yHQdW2|yKWi0|!9hbv}-sK?q zg$KBQjYRglyHQ7LDs20*2x^-$Tpu|%W9K~w`Ma_3IxR~aSd1uB8!1aua+o?DyqkJu zqRB*hR}pI2Bh;wL5(X^J178hfsc))nu)_Bic6h&!3fUKht#ksJyDuZ5@r*tYllh55 zO>$wZsUcPMod?cx^9aVZo6umT7^DwvB8QUsNZZgybigiYu&Op4EVxihDuk_ojw3BV z;_4-2Vfu<Wt6+_jP6juAxEb3xxc>s9do&M^E&0U!*jff#4;3M|mNg80DwLYln1bqO z%IPuwZZub22v6nqpd9rQ5S}1J3G6Pw70LBrRA38;Hwc2kh5PX3`C2exIfXr>((o$h z1fXgs3Pr|5VdhpPRDHA+ln=(xLT{ysuN}|Og|kmlxKAbuC;1p5>2B(@%sy&-e6eL- zUIrt>8U_O5EbJ6jN%if^2T~8WQROldDCmMc?ly9RzW;uL`)`*p5qIX$m*hIAe$Ntm zolSv%vZ9gvhafb@UjUn)yhY_k=IF<Y#gud7O6oVq9ItTH!)1I3>}0Y~`qx>~wUWdQ z`gPdLv;@m*wvxMbj)83(w^LDvXP}Gxsm4*GAj>k3WsK=53GC<}5BA?CsDRVipxkqU zx_Ij&^XW?*I5Kn_R$dpTLM2L>;AS~Exuy)eOQbR%KA%CpPx)}hLlb5|IEw23P>2gE z?tpJ(5MhBN$vp>)0BXKPHudP^fAkD`_~IN8a4Myt<bAA_P>aicD?&aN57RcALv^}K z!2w$V%OiLjKC+UR*<a+t7>`#`Vx3}?FnxpB3O$((uJ@UFGYhfb>(kiFvjQJpMc{e= z<f)$%iQsk$#prwOfm^K2U|j!MW+{<{XJuc4|B}l7M<3*WQ`shEC3Nzb6aM@~64oew zLxS(FVetkLj%@dPP?feB><K$VUjL#7%Ok_U?3qX;aJ2&Szh4If9iI?m>37kwlM7(c z=yo)6@f7LorGO5aW|LIc0c0T?jiaFnJj8WJxo@|J+mH0pPjhQfnEWqRo*ys#Z>a45 zJ>Gw&vWxD)CYA>y+Mz=2Txbd>tncHc3%23t*Bz)e+Jy9K=B1tt1KObF1WwWBp%P|e z2wBT;EU`<Sj4INn(oEB7O1u<*++U8bUP^(^y3N?#wHX-A?T6l{M6p=>I{3N03ihc~ z;6Jls<ioe(@c5uHMc9P_{m#{RrN0_p(s>Wwf1FGGt^EysZ>(i}H(m$2D=x#sX)3t0 zxg8I^{=wZ#5@+r11DT&TP*<vWuvl$9Ci~W-4;N~{@IxP_?bAgN6J^RQVj>yS(>Ykm z#)CRJG6za#<f&C#xg9~)H@w<l8ppfJG3D2eQ)#e*vukA%r7XOHnU=ET`X2e2sOc`a zI#83jE38Jf&o^SKuRI4oH_GEh6O|V7Z^wYiF$bW!ERYfk%t6l{$WX+b5_Xzy0wI@` znJQl!<fY#MJCdE@gCF)Ft=Not&Gqq$A9Lienw(JV#-sJ71CSgR|8wWL_;R+cQxWIc zR#}eF=_<kxD6sXiq{vTY8l0>vsYG>O6Z?g(2>Vw4Ct~~h64up@Pn>g8SIie=&g0y$ z*+OUM#gKE$o^hfhR5&`(k6BM52U)LbZ;*p_GDQCsUm`jzn|0=C3A<HQjb%byXway- z!%nrH#|dpSV;w2JO1M2NAx`p#(j%{{EKbk(u$)9<IVpdAIX5;wVq2Ndu`iCwavWa! zbB>;JV7Exrn6on+iS4S6B>Qe>gGc=uvP(jW(8#K>5Fc$~r^ha*3p4sHj`!*iLVKq; z$whYTp4oU#+4wX)x9~5R3;alm%Vg8%7D$i^8$*G|wQRU<&ob;`k^!Vs8c?o-0od|3 zj;!>S0$akG$VU?zv;d65KYSz+aXyqbbt}Sly=rhmq6PoeFD6!`WOA}1dO=?-h6(a2 zWNq<rT7=*QBWtb!r>g=it1LG{WrEwO{{uM^^V->K+#Ybsh+)D#^9`NVs>JH|Dj{E) zf8=N;9_M7McXKvhTS%-n-a}H}J)9CJ6N2*cCJtu3U^%28C8ew9gITs92=sDf%>~{h zVyojg=XnLlmN6kR_|sZajj^Dmy2m-*yAE;EvP)Q{$d+w-gW*J;Wg6&3CLAmOhn$!D z4e3CkP=eR-5X&yYjCgT!6XE%<jy=60i8FU4fJnBgYp_$vA$&VSiN+IY?DXOWHg9tt z3r-7@2__v4zr}vB#$>!XGSTv!m3-R>shu3wkM89h`G;+sDQ{Q8DlQ4_efJX_O^_xt z-<Hty%4}k=wFYqhxxyRzlPnr>MRHyU{WA0fer@_3yx=QX+pz`gm#-vW<=sVCF$UdT zE<`?b*T*^qyP<PN0&eWQK;Q5q!IF}Tp#EzY{MMrlod0H%_0C5@vUfJ<+$Tf7rEbIB zT3*OlMVRWbFMy8}BpCJlVmK341*=+@;?&(`;73mkt^Yy+XGR%NxlRhSv&(KcWxN0{ zoT`FnX9nnXB5KUT9~)tnj4&|bnU6yj^rEXe2Vvxo_c(Tm4@^rthn=>KpoWr{@R+s_ z-26wAS`4(|_c{cd%Odb(=W}Qn5J@SYZe?D^rr?r!i{bMOO_=oV0eMxU13St?YR|QN zZ2#jN9$6m%vr8-KkT;jW<k}`q_T5&Tpecogv)w_(gcuX>Ae{-Eq40SnVXWGI9ef@? zhjzu$7%KrLTA7V^Cc9Cpf2Xki+caopOECiq%b0y{v!LEVQ%daj6KK5F0VsH7!^S`@ zkR&z?BiVAezfFW`6duQF)G+;a<`g#iD99K)&!Z&nyFyLD*I=;3k;?0d!OABRV4tTo zINBV^@K^It)u(xwcF`j^{i-T6IeL<#yJJ5|>P-`GVlrvFoj2(7qBeAbe?8iiQ$)|j zlmnKWGHhOzMaZ6eMmBVh5iPGagZyd>;@4h5<S69J8TIKP)832WtaX3Uw?AU2!`6|m zE7(Xro>L};MJ>RFNnWC0<^>R!OQzk%Ea;x%$Fy_)Rq}H_Ce~Y*aAp!)+500W>8>?H zsH>$G5d&3dbh#`F9_NGm-s;f#9AmOqV>Q|wew}_|w1x}|_Cixhf5_aRZMe<+G4X?Q z0rf_hqhJY15FYZ5zLc8@j$}n}+e_NWe%&hMxYHQx{=LsBV%_09c)t~Ov_zvR6O)GD z>jLT1JVD^~dT!;sGYy6lda#6WqLU1g@q3m1<lO{GG$ZH-AZra=AaII~{?i0xts2lN zu>`nu><zGE*P*DBQdnI@7e$TQgQJCZ$bp(bg>TP-imcsmOmhvareuNT>k2|eL6{VF zGzQa0rAWiR`|z5|J<{{vHI&h*i^D3!VNAUlUZT1isn3<-Te6ej+lkG1&mcEmGp7YA z_8te0y<a$1af2Yj+6*1edk@ap#$okS%1n@p8*LSaP?T&t5PaZ5FUvPUU*0=|#|0@k zChZ8gxW*gY>D-MUihZX87MA0WM^3}MJVy)8GH#YVH!d^AS_lo=3gW{G-Qd8ddt|nu zIAiB|5j~VKMKX~cNVNr#VC4}G>yk9te=MHU<-iaUX&cal{XST=Ab~6FjDtBJ4AEv+ zZM-B~5<11_f@V$tG3~Ju%f+yP(TX;7VP6qU6IsH{OPK&apDd==pAJC|@7+O}AeRLD z?FZGL+L7hDHps(k2c%S;v6o>3^j>X+1Eiec(bu_{?Y97LT_6Kf4@y&)ZOd??nGcqe zl_CdBWl>JvX%J!0#vxjlQAl1U(v|F_<F}m$Y4?7D@?E{KDViHv=q-d%t~VA5c0vJ< zuA*y(c8K|$gR4@L0sna~T;5{<4aJJ#CWA%zeDf|8onQbb<oTc;C!4<JQiybCM40_s zSCC1A+{|-c3fj8g0J$UY5T(Jw7Yol}f9+|aRY(yhGx@X)H;$ffbt{=?Q-wW)(m{l2 z4-Qejh?LC~&{)wD{P5*hu-oV`>T#FD93eX#p>2sY5*ty7#~k9jW(tpP(8heOS-`Ee zh~9a)4))2`q0D!&I73XGm=l<fPl~U{pE?#}?W`nB=gmib8Ji*f%AiiGk&Pa$&A_+5 z{Dz0clTd2l8TjGk6pR%yfwQOg(T5(V0j(lluqpdBR<<&S20QAI8^1li?=AyD<|IA2 zZ6)gD2Y4?b1!WiiK-GhYD|EdI$?bp8P)-c6yT^vNKD_}#vwz`>1ZgyG%i^@H=_ae% zQX8swxf6Z2r0D>EO^p9CV54#;7#-S1W;VtEpZHWdK~^3JYhB`eSal86rjVq^CJpe+ zJC80~)<Y{+I$^p%6R#TLfE!)+*|r@{q`=k>wDY%IAl^Sp%4+d~`%amJ<Ni(Lmf%VD zrf3=D`C<#ukE=!^@#XZ(iU;IQMSz3GPk@!n4g+JCY%rTEinIfSVMXr+Aa>?7+3@=p z`CVu}q<aIwsJki|e^ZWYlyd~hMhCd>pM?I5z98eC*OM_P>`_6CCU)|;Mqd=`MnziP zr14Y<-S@nd)OQU9{VDrUZkZ93QHTY$XOE!7N^$siRusCYOtORZH{<M{MQE}z2A{B5 zi*=k*(M&}m$+>g@oBgoE%Z#kiTgeen6utv#`0asvE+&Eyfn&6e{XO~@^$Fyk?FA_j zK_H(SmvaWJ#H%C<fT^Az`Y);Mf84YGH<eY{Hy;b`Hb-vNQIJ>qB}s=i&`ie_d~b^g zDtWz!9vBj3zSUNP?LXIHvMmg9T3-RKcY>DxE)MR!d`)KjQ^D4P-YDc7Mi$*?k$^)S z_7nL^?o+x<Ki#iF?~vtA@05`g6&L1}5f<gnqv1Z{|8w!!|F@{*BgY-QV)kFHa>qUX gzZ>xTD2Z|hw8+Sa&gX6`{(rXRt_ky7^UW9eAFn@eH2?qr diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt index 4835f989be4e98d9fe4373d135ad632f6e53e63e..6aa01ac3dcf466092a2232e3e8ac2499686979d2 100644 GIT binary patch literal 41391 zcmeFa2UHYG*EUKNB!ff|P;yYRgz2toOc)RYCX7f1MQ{dCR8WwpWEByKN)W*WqQXp9 zHAW1GiaF<iSwzLi?O{0Qc+UCX_xFDHUw7Sgm#o!$p4wGiwfot-cU9N)=oAl0DKRlw zS+RfpsfekG`SScjLYD>F1o%ezS}$E3<~~wMY|h{Q;wAhe{3GpsmMrxT4~+8hkKhGb zFO3Kb^<QDVBs3-}niuF35#$pd5gryA9_Y*S35;19$cu`U^AuH14x02w)fiFDl-`r& zxMCIwT=6)rgt@oSwkV!&Xn1INFjq3oTb?cZEcNC2E(weZ<VA9&{QV=s!UFxHLL<T> z<tFn20(pS}qeK0p<harnUfh1Y9r*Y~2L%aR%Y+D(vfjGFURb+)SbHKvBclStqyA{) z6B!uA?eFiymP5m$Lc@K0<XDXyS8hI6KAEfF#Z~lH7M|_r>%VxpFE79+BqCyQBv+}o zXJv0qVO?mXPf&DN*x%c5RVH~W32TEQct4$}+Iym!x7yEDOIhYb{SN4@R`*sFR!2tp zMg{r=uvgbttI=Dl={-<b8y4Xk@N<*DI??KFrY+)PNkl+&SYRYqr&l@9+*^)4IoOxI z)L<Xhi(Fm*s0h~c*5TpSeNC<XBf_It7X<R;rUi1NS+YWX!{oSwSSRVlarMnZB-28q zdV4bn5ivYCq<;uY*pOtdp%>REL`05ph{~i8(dC+ih_1^tM05paAtDpaLqt|tn0t$} zR@rhb|80yYmgFY(hLjxFinU^B9Cz4HR#=;R_hZjq9vB)N62-OY>%u2GDl{xo&W9H$ z^lZRiK_thu{SQspxQLA4`7E=S<Jy^<d&{t$MzZAi1_yHOMW@KQv#du=6Dj4m4t-`j z_8Q<H9pKBc<qr4UB@xASn(86uF~Gw(h3o8L>>-}Qby?R-nQIi6<HaS?dJAMK7kF`@ zH+xr#`GxvMa;Xs85O(Mg`w&src3iigyB^^&Ac{M(x94Q;C=c;u?r2f3W1_h3Ufi*w zUdN?!$9r)nME5zzL*$%^$y`q_?xa5FO#aI`Q~I1Uwa+<Tea@NI=bY)iHz9jx1O@sE z@9{|P3=d;(1=i$<=%~MxGd%`G`}Oi+$DQ?WH~O-->@qg$BEsdS`-X?ealKhb&W_{G z`N@%U`z)R(vUq+ncYznzr_bVre_8C?XR%+O#r}O32lQDS*k_16Hz-6HFTru#ke`ML zH4m}*2haB0ML+Yr*n@Rbn2={lj{zy%C9ES^4~C1lj)>we_2P0xT=P=7kzU-WkbgSP zfgAmEhs!)9qqxg^J51)r2pzXV)Y-}??kX>Ctf;fNRPJgo?wUUw7cZhAA(^|@i<{U- zL(*R~B=^yf(nmvT9}Q`JG^`7m;V~d&riWx79S+>}|J5(kB7&lp_{PX_(^<|k;<%YV zahBERnrx9xImz5yFYbmun>PMs)22R~^7?Gr+-FmMpG{l5CE36U2wlQ0@DPvoi)Kv| zrg+gzA<RdS+^zpM@~?#C*_+KKvN>FiTgaMO6vr+8$;^^IGfPEgmL+qyd2zS*nOXjq znH7CzR`!`$)o138J~ONTNJu;XXEUD9KV~{PZVhW}Z5(&kPsZ-<Gj@;2*uBZzeO}zU zK4a_uGIoETu?>C39_TanV4tyvM5#)YDDAk7y)oYu$L0Stmf+tp@5rTp4pHRkA{E8O z!Wh*TrY%O4wwgt8dN_)E#EW}W6sN~hxyQY@C;k=m!?`U#_juAnKZ<**x5H%aX<^Wx z5p~uY#XalAZ4-5NE|q)Ui+kaZpl=tEaWR>D$%}irkBlpSk#V(;jB9;lT<;^}MjshB z*`POOgI=GFEa6P|SKi^?3K8a<+i~1GKM`}c&k6TL<K9o^KJel`>>KydU*kUR8}~`y zxKI1WebzT_2Roi{!Vr1Ep8LGl3oqihFMk^B)xW*q#C`oUXKy^uvy;bLA!*tk`YGIZ ztTWiu(<vhEeH6FLi~B)D+{aYzCogXIzr5hg{rq!}Up%y<xL<oaOy+(Qdf~gMvma61 z9xt94JMs4R#}j8!Do=uyUOY)w_IZRS#o}Iac+xCN=Jo4sAtTiGPV_ukA^c;m=k*ts zdZ&AyoDlZT_&j+b?49&^3PRXB@ADLeaE6CI>o09K8T8JB;p|e6r^HrxtFUzb<36(G zDGO_aE3(k=rR?;>W)gPNW@i<i3R}U>OTv>vLIVN<!+)w%71rtZ*0FQe-_KJM*0H0D zrY_z9wiv?RtUPrV#PKv(`IiHEn!?)NH4jfq2!+cYo;Hh;c{;50;tdpPMYQM&;h(e& z5|%`?=n0{S7JVTU(PAKkB3cHs&^)B~KIRQ!aU9RESM$fZi)YkZ{O2U*!ZT*|Klz?# z!j^@%HP4g<f85zTGq%usbMwqu)Ef*u3l>H3ELrKrvtp$vB!;pml{c(cYt71k#hNS6 zhE@MOq%B)!?|Ysd`zgBhdG<n;crwp{{S=Mn$f79Ta8`QpoLDIu&6!21JeOXrD=Ys9 zKn{yVOcN|h=8?TEfKV%98Vcc`OjE*=h-o(=6fr$Q2t`bf6haZxqlEC!gb~u4A$X%% zWgKq|D}VC5yRfCm^J9gu*Yo38l*}8?N-y37p;ly%hY<d0&qQHKWRIs1itL#rgd%$; z3!%uKDJ+ck3+YWBys2y<j_1Y7pX`{{Tm0Ge5^TiV+482d6+b&-23r;;6W&Y~{E<(1 zv)Dp!O5q9PUF40~EQ;dIVWk&uE-OXen8%`2-uzze0#^Rb_q}7<@qAeQ&tooR%WS&g z`Lds)jKlL2s#xdyv!9~j0$3Eq3uL7iFNl?*;euI|$_we$hO+Vx=PzQhi0s8IO6G<2 zwpb$6ipUNZ!avE55SBz_FBL)&*<2wMk<AlA5!sPK_*eE>^q-QB9WRP)xk!{?7Kw7n zqQ9jLdtS7#FJanP#)3HBa#sH2v>0KnD0o%~VQ=uPWKlA26)U}Xu|lordg6re&+Az& zEQzjXjSz~iCte6e*OMTGqU%}9!aveRB8%gANxhms(?)V{@n5lH&r4w|e)eN3Tkg#p zX)O3NZ>(brqP(%5MIvXWvnYy}!AdV)CM!kG%wkb0FS}Qp!^(fhjsq{3)&D%^2DU6r z92?oszQnOfsA6L$kNp%4x0yvzynI%A@wTv1G+Y6TQh8f@wS}zwBX){dEaJMDMajI9 z-WH`ot%&O~A^el;ZNid>>+M1);<{W2MO;@1p@{2BA^d0JsA83IydA9k$@kU5mLlKp z6vAHL*RUv=SIbH--Y%h5WY2CP{L`L2!jj0Iy+SCmXP*#??5Pt%kv;V+{4;UvXA5z> z23G!L$ARABznt&LJIGf2?1)2bxi@h%vf$6e(Zm)+iG$A~kv9Y^isI3%^x`2_ioAhY zl*(gzwau*jxATYd4zv27$2`K8g^A-R``MQ`jtNz)^N+KiqTx=kD2ms@N-y3?R*Hr@ z#iCT+>0a#_R{r7qRu+rMKFgwHUR!UAb3(0%?DInSC)pQ-B@x-}LMS5pq7aJ6z9fVq zvM&qaFC-3o-W9gxe@GncqffLb<ruID{C~4W#=FXP<gLLrjPQ#L<cS`ne_G&NW2@Q4 zj~3gYZ)@jYYoP1GCIfn#^gV6;tH}*v6VWnBxNK(k%l=wk-4xbodn>Xd{PD0By~Hn& z$Gau0kn|q#58ERBec7dOFz+^7{pXfO@3i5>yTj^!_WoVA?5QFh#k<D>q5H+zG-I5~ zyD!oUb51Jn0b3Bo!9x~B@gA|#i}#q7z1wVk2LCba6ITECVV!wTg>8j9MWT%w-ZQrB z>Bi2G9V`$HtL>qk%6l%-vkP!`*cWU;H0(<jMe$y-(u?<+mC=6IQ#@p3H@4ZSs<NID z9#j6~2CI^o!{0VoZEW2~+WcdC^>2Hv|Eoe;Za(b4$sJeJ$Kw@FM_}~vvk<*{ga%(8 zgj=2{QIb;@z;WaD;M=!SQO1>G=y9D!k1G1Z#mV>4QJWipZ}pU3S2q%t4_t);n-{`M zrGqGgVQ10N`-_lr8$vFxuK=C6t#E1jbwq41#+jUQXuD4WyXz^@uY2}klV$nvfJYG8 z5^0SirAI)1RvQ#=7=%48zCk1IRKn7$^3=P9i8w5#3OAMXqe?RJVb%vOE}p}|oY=*% zN@+7KiPJ$qbQ74KPz<h^t;fMG1F_oBXguh)6FjIq4c1LqhAX~Y1=n6J11Cb9$=4@$ z!v`|&u=r<RY%@a#uP<7G<#bjM_s(cC&n-#>BN$g`z0{sDi$G9)+iqwOPT`Uq0{bPZ z!97W@;M?mdc*o{!G&fp}dg>Jk@2`G?4*Ks$UVE0q7fnXkz|9Ix`S=dx+{&f>hHu3; z7N;T82O;>{nFequ|1=D9)WGwOJVcYOWuhl0=joP&9dP)7-FW$8E)F<CLf-l^JbTv% zFvV&k{u(L^(_4-3RH<=T?TtGe9<mZ$Ja-V^XvxA?r^I8<0deNzd=;kktQJ!=_#qCu za0OYm7~+P!bGSUq18c(uL4Mjn#Hsp$%$Dr{@w6^%`+5Y#2hRmN#ZTaMrzYZMr%phe z{uTUi!l-`U1JcZR3Er+v!Y78wpsSvO*^v{WQtc%A)LR7WH7w!y`IX@5(0cf$(H3{l z-H)lnOE_z0D?ag59XV}j!xE##8M*iNjPVi`My*2`*G{>QkC$x6!=hF2IM+_#*@RH| z?i^BjP6E37eH7e0$sF`_EQOz%YjB$D0sL;X0-RlY5zKV-#D>f~wD^<)IAg8_J-5W8 z{U)WfzS3=AxKRf_o-IWUbh!eS?ofuMro&;Vs|K!Jn+&^8-GR)3>(H@#B)s}kg*s-X zNj>gqh2a|SplJga2EE)5--i!}&v+Z)hJl83u;m%F$Y%m9UT6;U<}O3`$3Dc-#}GdE zb}#gO&WF>?9^h%u#^d~q^N9AYLVMrM!Bf5+!O^ZeaMp`bd~`!1)VsYD$VRUh=;bP6 z_=Uhv-3k2uOcF?3Swhy8J;8k2(fI1oJa}zQHk@Uu&baj0GZUr^gx~i}2bRYs<EE82 zvH7_KD9Cgsi5WWurB5yJ+x=X$IpQ0Nn_Vc_IQ0dXE-@Ehp5Bb9_BeP7&w++>)M5F$ z6Xd$3lHmKOTc~v{0sF5HgA!u#7`XIDau+f%m>mnV$1cSi!B_B7tRJ{ItPl^JB#y?e z9RkINZ^T)3;>?}z1~A$i!12o(@X^wdcv<TkP`P#{w2UgjN-MLF=UH7S{&EZ2ZL5w8 zs(oSd=e^+FyFu`xR)4Ixt`eJ&mQXr#85rrk1F0^zMT(``_?e6Xe7JH5v?*<+W8!mQ zdb2Tdb`Zc%(zCJYGz#4h+YRTHF2wW7vq9O+WZcp83?B>8gbP+50o&%9FtnZ|<8n6_ zax62Uwm~FTu8YRzk6XY^4@2Z!{{WdJb|KEt;rR2Hdnh`ho{%422kKQ^aO1JNc-j*O zxcB~0sMDZ^&fd?4HEsK#UuG6LWo-`Z+IbG@$QNKMrFtZvpNnrTSB3lm%Ahe>1FRSk z4-c!l;L-d{{A_0%{=PdFPcHn7Pn<r7L)->3cW)13-o09nOY+<CS>HX_JH!F+56h<K zeAI;t4^Kyp#RdpCuLE066=8hpQ8;$mc`S{u;riTWI6ccBR=l%8q)|KCe|j$pxYGjV z4i1H?GH0OUF%3MYwUE<4svAVj&ww93X~KeSJ@6Q(949U2!8>!$agv{Pqm}dT!W~=N z;K=p;=-?eW_>zJyr517r%I)>Qw#E<-{y@MNF$(x{?jxifaMtyNV<B?yu)~KdmXojj znq6ZL^P$Cy1{ge5A9p?6gSMN9p&18;!_uA$;7e)+etc&S-8FGL46eD2-{;*z#z{Bv zm0Js;ik&unYEm68Q7(d&-8J~6vNfFK4j?TZh|LW%(fF-W)Q^YHpz5-IIH0^77q%tC zJM$;Q%|-G!uU&#%ykaI{o2>&cn-oHif!a9RW+z@&z6|y6c7tzk*TB>B4e$pt7RP1E zfU7%P@XAzq#&{qNl(&as^1T7GuKN)5>q#PO$7z8Ki*-2Q0u)qk$QIn|pNzZDYU9D} zt?;nhS?DpS5`Rz!_~0l-ieo>J8h?qxv#t-uhI5WVzf+koJvj}PKOF@kZzU4$nv$^o zOFpdGtOrM!so<v{lfc{=hv8_G8?eM}Dw5+m;uP!C;F#%Td@8yLPxg8WZYZnZ4^xid zvBT5gVdXjye^8n<d#{EU^KQ`&Lp{LC*R#;B=P5X6+cR><*nTj}+#hf5?7`C=<%tjS znsE8HL5$X+F?gjyJd`cVhuQ_5*!}EVARClMJ~F<7N)Ppe89C$N!ZV-HI~6xDP%RQ~ z9efMFldpgqD@>ru+E$>oZUlO)+KhTyR>S_Ld7!xDD!L?>20y5);;oizVPo1pG|4s_ zOqbmbKQtF&7_bYzd#{b0?-`<&`UpsxJcaA0>R}TPecT+VK#d=H6n^}&8_n9f9vgcc zgl7|?(aH(lpxImvw8d|r8@EAh;PntBXzO6kA`ZOMk_8<*ix8olj6Gzwpk)mk!J)gl z^!v8G*vC8*1)u=hEH)R#`uQP^IqbZ>Z#$T+!bgtV>hPEaTcGysP;6W>18#m^2^WuX z#-?N|mh*fKTF%+S%vee0W6ct*pJ@Tn^#pi_X~)n%0q71W21n*-qKCF_pcWZJzh~JX z^mGNfwf_X-UWfn%TQY!atrq=}+XNb}xuX3?YH-=<MQ}}S4S?@<pv&1&Q2w?BUQ?X_ z6(;P!<8-C*l|A7wKeh>TqrCB3#YUWP^#RV*4yKELNHfp8l^LfsGECl>>-h1!IXI1f z9b0O>!>hjP;xC13QQh}qa<aD!BI&`n&czQNKab(<@mWA7{y83;m5*;JUx5{-F>vKz zMQl?YgnHc1!J9qpAXU=}<|H11Ygz^)*Ps$`YG(qxbf*dy)yYB`%MIXYVLp)TSB~wo zY+<s|K4|@DDV|oe48NKvMlBkvPHmo(gl8^3hW!_n!ExX?<QiSWt*=+oH3<_4kE}`X zz$6(+F7Tl(YaQ{n)R}n60Rn#IO@Quk9oTrmEc_LmM6&+h5bsM4o--{7#~2^Lz(R>Z z{S#n%x-wi9>IB;_Y2pjMY@8uQrhM}>{*IKXP_gR*95TED=KUB6*B+Ols;W-Iy?a%u zipvrd+7l0zLK0!ikP=v#6AZgdhvG5|6YQV{!PdYgAhk6N)q7{)&+09B%%J)3+QLeB zJ^MJeAFYh{pE-$w4?IBEy=P#@;BnaIkQa8Pmg9q=?l57)Z1PsTH9A+VgI|#1Sb2jT zPByxQgv;YE=2<rT*o}DM@!yum>VIDz+qsWa`PcF|Wl1)(V_puk*QWx18J<B!r0t_h zU*#}6?>A7|DKn_*aXTo7Z`Bk<Co-R^s+cocG-DbvlX{e5&peqvlRB8Nmy+2R#oVY! zV1%|wdmJs9$UYAUkN<3&vY7KfY-88%4*xRlf3;x$hwjGm1fp_@6VW}Y!KK{bPSXtD z0e;nmeB!KHieTYfRsLHG9l;IDr~H=>PZFlLnj0_F4-u^VYD;+Q6cY1f2MXSEJvq*| zj|*mWR1urabp-`q<2dVFzq|V8gu61|eE2@+^#%30l7jnRW)gG4tp!Em?XF1*HH4gV z499b>JUL8Vjjr3P#`l)?BQ&_$<Q$(2BHidRCw+aYz;w$fV(*vnq-UcO;UgX|sBSYR z(qbjZ_l;Kc)JYnGsE>Jq7m5zVq{I4z<%1j|`klBSoGKG!&FbU~`%osh;xnGlpFV|X zI3q{X_McqC^M(@VJ(7s2{++~LjpzJp<Z5EWvXx}gml=YX#SNT*xFbaK*+We?tCEPM z!eJcM3Qr>ZtrFSx!<V?e@dW1`I>N8``jlwd_=(fDMUpTW`rS2Vx2a&=n;60RuSJ|u z$?F7m^KKApq%4T7>SDw_`50pCoT;1_cmzG*+z`gbM;uNtfr6V#QtUdQ0vgRa0h`l@ z0<9T|j791>w0f5|c!L|TVD=HLoj#1}xMYpyK8V3rp5KA0)O`@$;)l(0F5`u#H^Yth zq?rB7%0X*CWv0$Q4za6A5GvLR93`vqqr?gr8zcowPh1C63cZ=t#!?J=Fq~OgBSj@z z3}7D5lc3%NEJN~cQ>oOkqhX^j1|KFg;jgbdp><Xr^uCaaRYy8Aou)@&sfh{`F;;~d z=X)F1-By8g-fnuaRvx)AScB0EP{z+X`cr4E$57ePBN;_oeFlv`gv^!0nHM*AL5Zs~ zC@(#4s*AXWPjIe+btearv2(}4gMl-s-HqR{{Y?cX;p=d$bl@ypno$T|AKFD(JyC{f z&y^YVloEJqmjRQavWKY%%tP~!CoqfeG~j+(gW+(^1@PPZ)A)?#DLBkZ4o}e<$*k#G z%1kp`P8DrYU~~p+P{kS>nHc{9_=Gc!vf1+mq91K&wznD%zce0aI+Osfh?{s+yDLuK zVosg(yAM^*mEtXf&O+zmGSuv6mP~#6HN2qg4P2o%8J`YmCJzWKnOQeKp)ExW%8MZ3 z-FpqtbXgT#ICU7TZMMPYrd&pP-bS1-BnLWYZo(SPYk^xqF-&{x3-^Aohxg~lz?ti6 z;rXMAOwPPo>aesJl|QTi&omjxOy3zmeJz{-HbiU#&NrT;X~!2*<(plo2Zs*ejD;C^ zS@2!#H6aRnf>wwu<YCW>1l-uIh3A{*FcYfQQuZgUne>rSjKBK{5S>$xO-%#gdpC9+ zLG-|B1s%{d9KpRqQ>ds3XHcinaL#deapuw8b<FmS?o`3Oc&ba)0iU*W!4<K^a64Cu z`Bt)ynmzCg-lk(kJ&L@C=Mt)v)CC>#^v+?7O=~r!dGR!!warc-H%Xkx4j)Q1AJ|TO zKV8D{)(R8EImL0xM_uBdln&s7M?advK6eYevr751n^TD9HU$F2?c@7?hQyVnmpLEK zh&2@-*d<uwx`eRZw1?k3qmq++@<!u9wN_5*0mCL)GkMM%OIqM_VG^-zmLz|A!2^!_ z#5;mxC!V{Wd7vclN*hGZ+IWlD%jpqFo9rPLX7Y$R_i{L!2dfb?PLOoDO%P{{vn9Dn zyn;|WDoHz8{2-!z=Mw03HR121MaREzCG68JT&^dKbv>t6E4XZ@!`U(E5MQfpEiv~5 zEjZ-c#%Tzc#W#CM2rN#VB!<|JC2Mvm5TVPAIP}mlq({R*TK|VEc@2yfxP+t-YW-cw zQ}QPTSwWkLYwpE_-C;|z*ygsNMPeDzRvAhjTs4ey?E5^|)e}O=vB_0}cdw??1$v6) z2=~<jZTWRXU4%Pv_~|@SIn;?vm3hM9SyQye&h?~tMiqWHWf@L(eTp3S4MyX3`qB<h zw!-xMW59mA2hi{KLicZ5K%*sB!3gR-ii~W)P1k>5SLZFL0}Tb~bqKC--vLs;Y=gH< z%E2^aC7}FRh88oZCX8Ia3g(<lA=l5-fbACApzw5mFxq+;Q=mE>M0~Ww-k)vJPQEwR zio1qmrcB14-`L^i5IOiNa1O4o)rR@z`dIa&0=A~Ch+wr6l&a8<{jbSDv+s3+y4%a} zQkj#0RImZV;swaLE)vwp&4v%^w9v6}7)-x=6b{kdi4GO@14sAJM4qM*yz|rm`>Q-g zKJk-rSLZI68)*giwoZi8vy*XaX(v6W<SxALE6a48Hi0|etbnmsE79Tn`S{y}1yJ>% z4&hn90LjNR!%{Mr?!Foi#_-F)*+v<di*ngKa~z#<8-_>em@`v1eT27yedrejk5HT4 z7$)|Dk)W$CAB=iE9(~*{4RY$w(V2R+$a6&s$$xr-9C5jhE_kv4?t2tNdkmimFMf6c zI}KNp1IMU?Yh?`}?VS?(^wfY3u{%KQP16IH-n$^Nds_wbgZ;=-DK#?EjikrO7$RcD z1zK03mbm{}LQs^IPsR?Mf*b?G1%sDpfl1dnz_#r+nV%^O<NSh=xokL^%fE>_x&mQ& z&KgwWpnx;1f|2G$6J)8iiWoC#IhwBShl)c>iK*HXLHA%|q~CL&Q~B;A4fphL<`Eib z%bFl06Fizc<vR%W%qsyycIy)L{STwBT@Ao_m<Br6T!KC}-UE9tu0reG6hON1KCr_O zgR>h3!D(8?%yiE#Fj8VYL}J%KrC>Plcr%^MShNOiHvb4D4U53!OFqcpRTuL5ew;4Y zv>KQ=H44DvEnsTcG?-{`l{RqX;!Ca;c<ZY$I%c&CEOk06m{#?PH0<mW><uQ-Hq-f} zUWGH#za$CXx6BumQwsE;w>=>5NgZwBy&aUlst}xQQv=hL1IPh)kArT$Jjh?H3WDWZ z$WFCgv|v;j^7X4GALUgM6OK-zAG}-zBTSNM@r0cyanb;Eb9*SMQKd&OKW7b=?%jao zb&}DM*_yavGl0dsiR2ZRn_zikkl@?<oAiLya&%y_7eByE3Yd%@iMUOpXrly6a&NRQ zl1R!Tb}o!Yt_Nyqovc~d&}0l!**YFfUFSoe@tuQ8T`ci&LjhebnTcNO>7fVG4{87C zWMJ5D0h+pA9(q3A57xFkfqS>@#<Ok*K$Aveyl83+4GOHmO67ORsrwDNs%s|*QGEvX z-U*<O@4QLAx~K*ReYHWAhm_!U?KqH{5do{K?tm#zHF5mQujD74Z{XNdCrHGdA>ZGh z2A7Z80A{6408a|r(YJSx_`^J};8*3tarbLOtkWO~1~%n`^Wp)p|FsezG3Bh_X2>wG z&M*Q_)anN9vENXmL?eFPtj?&GYtajMji@Y-13z3G4!p<gg!`j3!2SU`aPsN-^zqpK z=<S#YaOUD#u=&v|pnfd@oiJ7)SE{HpA2TikyC)=lKujH->+cL6%S}h!Yd@hQb{Lyq zI1OA=pF!}X1T54&jl_@gL0}Re9eZkt4Hmcn7n5S3xY~xk<+2;SmuLcm4~&PLc}95b z6%Sg{GX)*3_=v8(G=kyxXXBFf2pH;~2WW^9>T>^pi_#P5so(bD)FD&oIcs*}>G#Lu z5BG}UtR-vVh=W0ByssEzJ#{2<9%czE2K!(G>7`(W^-=V_%an0FE{&I<0DL!MAs#s) zo3@-3gkRtAhouW*(HrAQaHZ@7c>1&#mYr!$WlZM4+e=LGu)-cNWnl#ruGv?Y*6`if zSH^_Le_OMU`p24`eW0NJCu{bfm+-TPn-U!lcN6L-0sUOd8Os)lLrH9b%T&LDjiYq1 zh1>(!ui^%O@iQ*}Sl9`{{Q4UpTr30SJ^n&>(E{*5fkBG`E&$%A7i8KqS#Wa54*J6B ze&na)iNIIlLhtj^Dy=i23)$xp;ql+D&*&f5XYW4J;D2|0WxHRZIUtJJyfuU&OGkm0 z)ZK*V>^#`EYCk+@^9*#&F~ymuGpOP(wUl+)T~vGN3T{lyXV!8Acx&5I%Cmbc&ZGEb zT(m2m(7!)Z)%p%UE~|srLOw8W+g;q0XZE9#-rNNTw&&r;?bTHF_^VhVD~Y+3lSDZd zj$jlzv~l;tAF$za7L-%&!W#_>k(cHZFnqK*9Bb8pdOo-#BW*w2FxUkTyC#JackBUM z9PB8K9}bjkni(h^c?Bw*XK=Nn8RA{>Y%b$2r@Hry08W?6;PRG!On74^)M=RsYsQbG zPJT(D<Wz>?oATFamwE$qJtZC$hrFQ=@0x)RJ4z$7nPISEP&?Q@YAw|?RGp#^JV%NH z0)gLA1!h5<8Kdxd5B*%`Ieo`97cEcePiEX|r4=1_ql?W(urBWt<T#(D=QzhPIT_lh z0PIIURGu)l)%}|DZYIItgZfNacma|$vmv{8&`2ZLpm}s-6=n7O7Fe&r0Q=32*tOCh zj;k4mpK$niyL%#DCHoa;iZ4Q7q6VdZ*OU$*Ti{-|0&>)3Q)shiH#Cj61h2AAk!j7@ z=+gQ$x}e|%lK1Q;4F=pGmya$6cb+KWQ=ezy4cLm3{;(T<=Zd?X*>2>vR@vB1>WCG! z+#`)@Qskr3DGHQlLoE!K+rbz#kHIwu3gG!u1K73n2>ht*GaNLdLa;G-5w5qqgxz+R zlD<|}Sc6-PzljqtHsc$4dafK!Ot-`7+-;=Q4{=yupaw2(xDCWC6>!>$1|VG%1jCEI zfXdoEVBCNLz;RZiau;b+S>~i$Y^_rBLra43zj76dXFzJ=VQVT;*OYm{yNbsi+D=VR zA4Q$?EWk(oFJjZSK#ICK1$L@z2m2?cQq0f=kSaV1@Ar$O96FBSPWP?IvCtE&ouWid zFt`b?t<Ir6cPZiiYnB2f&Q|bL*&3U_y90Diuf@hv(=cWK5#idE@O7sZv$gg$EKFPB zR_n8edT*=8Eb?f=?yZxUJC79LEV;$Z_`$Ulr)Uqgq3AUZtB!yMofp99N9?o9yBvJJ zpb;0B`%*{dtwznBx%?@$$;`_&l9XZByQU0#6Ch?gixNw+r;P9!u!xvW-oLSfRP`=L zr5Z|b?2=ie@3|<PljMn0XU3p<X-7&iXfw0YAP*<)n$WyrrzCa4H<KDs`3PE^D_{;f ziZMj@Il8H8FSYerFm`_7OFE2R4|`U}G73);!D=4~_+^hH8Z_h%9JXJLYP<Ok$;eHj z?md2hpSMoJqn`7?rd>_wK~WnB={}22->3p!n<~Mn5*0GlY&d%Px=palK9#n~l%fxB z#V94oow2*U0LCU6;Kgo5%`N+uG0~Sp@dSk@K(5Y&is#*j5|RyIr^h$!SZv6&`}M<l z51f(H@jJ-C;w%Wg{}wwxti#WaXJB(^0mRHrLFd_Bz-{#e@@e=KxO2)k^h5k1QW|*| zZ<!E)mP{Um>INWSRQmw<7=0s)K8ytS3*^A<_BTl3%Q7@|#Cm3p_ZMJAT)=x8l$s0E z_fVHh9H{2vBBnla1taIKOlhl`QK<$1H)?EPDzq=dONCi*Ro)Xg#dQZ$-|ro+%P_(H z9nM4br1f~)SPCz%Ka4dyPST%qYH;&KJ32uj5wE-xhSg`ZlXGWgAbO!as+dH=*$-t< z-i}&9|0RRb$2BL2o_q=1tmQ?O9u8ut@s3nfyLPkBw-n}t>IEvNWjxh#Xf31jID^uV z@uA4Nbmp$?9%|d{9QZXn1G*)-GOe}UV7g;4&bnxWSB%^RhVI>kBaQEX1q(^)f>J)p zu*(4!bFE;PCIOj*7jQu4SSUMK6Z)xMK;e7T$l(JT(D4st==R4X(vVD|ISD0bx9T8R zIIchL-0V#~`)1huuK5!+@zW}5-6=J=dszld5}yn&b=5$N&kHE4tE=G}Y8NitI|CYK zt3chD*Kk0UE!1T4(a`1Rk&J`_rF$S3hIY8(`L{jL^DRl(OeYbkw(_auA^FtMpo<{L z%LHCnd<Q?(zlP__=s}Mb88|mQkBJ?T!1PR~Ad!^K_y@ga%36!y`<D@LcLERU)71=5 z*_$D3t}=Im4Jf5ak8oFXzvdkdGu*Zmbiu~Wx4~7jt++f_+O0K8pDDo~LF#QYx6H$v znH6CaBQ<;?)!MTLoA`XdCg~>yu_t?Q`nU5~p>8R?eYy)e(Ru}tMjSk|Xdr&;SOprB z1n^Q-AymDTNZ<M<jdvv%GhT}yV_MUiDh9{!Mz?9yIpQvw@hP4;q-Fq*?jA&?dc@(N z8Z);?dmz01XdZljTa%Gq+uu#=<25|CBn@_6dq>@}md35tXJG00v9R>)L3V952L33} z#A9CPfV#))kY>b5pvE}D)8<=<(MgxV{L3%UDLerC98?A8{8jP1Lo2|%txE9PhMU;N z?<sH`VceV=s^zwb-@~NpiKA+E{TV@<V@Ky>;P8V(RNd%<c=!uNoRGU4$0n!Zsk25y zBQYE5$tgZ;7B7L))5pMN9+vQ2(`a~nTqxcgosDKQHgM_uP56w$Ui47akbWY45;+%* z#oL$K1Gia<@XV|mz|^k@HYuvYjCy5Qk*kL#W;H^S>AqAXvS4tuW^>%4GJGI*B0jfs z4%J^U4u9Aahw<Q3jKqLArprx}xw<=&xtuuxc}eM0E8kp%^HLP4*LTWcx4}rT=IjA5 z`@?&ve)|&EFWCfVpWXp_YHX-9PkAcrt}--gGR2#=2cT=q_oATBlKAOXeOwb}4%2ef zp(U<H<(HOHua3Jjn;u8uFt^#wuVw}?J`X#YI{h?i?T5uoLC9Gw`T7}t*w97kjUNw# z!ZN7}$4F}6payElx(+bzx--(*_#JghXR`N230OY-0AAZ@4m0cQsLNaZu_WY@4#wGx z$()^xRP<@&eV2oM`)kq(@1oK3&QD;>WN|cWa}HH6HivS(y%p#>KLXp=>cENqr-;cG zYD88@G_2YzMH+U*kYO!t;Id*G7=KLyn09Z4tMV`7w4QdnUoinTz<2oJ_VL&`SB~n6 z8puo=^b9R{cLJ}sB&qnYI~>R9@{F$Kc=)z+8(7`Xf%&*Xl~J544nI7$0+nPki1}`X zwy%u<c5fxfMf>vL))Px$vb{chI%+!7xw`=P?n(zOr{tQyEY)(enwdbExM!lC5Gw}0 zlE>9kH$Yd#E3ot17lwYg55Kz0?tiy*p%Pvd{&qQx(U{bVouxzYjLH|dBlsL1y0aVb zC8tyJ>!m@grUrZvG8Es<^a1rR^U1PXVsP5S7BK0@3=}is8!=}Y`|PMxOkVHVi=vNN zp%3HCuvOtgl!rnX$*LZ>_xe-n%WVfrtC1k(x@_=>hl<#&AAze5Br(_Z>ZoY0IGm$o z%dA@LgH@~A;HF9qCgDRSJWC#e8(Pl5OQXHO^O6V9UojdgbPoWQ<10bU`D!?Pe>gO} zWQO&c{lW7#AEXrNiu9V6BU^j{`9~JuF{d=q$FFgq@*tO*Ho>2YS!T+_%9>CQ8x$y+ zqz*7NM#1grtxZ(F*X?*^q%mHVb{D7a7i$*q-(s|B7@jQS2DLv=Vl)pxG($~_Qv0gK zXoS6ljkhnL=t2g6v++X1=1Jh|H5FiSeJdDsDjsIZ+$Tp_JOyV5sG)*g;)r=^hj*PZ zgeMo90Y?XW6fAg*S~UQ6GHii<8$0nH`9in;vo>P&sBO%VrV@b0?uHv*L84*jc<8@Q z3)QBdg`@nEP?qp{>vam?e$s+>Y*KK*mN0>nwJp4Gr3UWMu?6NMiot<k7kG8h1yCoR zL7(1Z14o{D0Mzn3Kt!4h&KjeKV=LZ)>YaR)e)>EZYkU_;e(^#{3y;w;`?kX{f`)sw zC7Ifl`i!qK!@OD%PJL8h-^mc0N5wT-F?aUy;ie1COi0LP>ie)WuywLNvw-&i@m#dP z*ZF7ACJO=7-3lm`B0D^yr3ine*2DB;3ixQu4e+F08&9LZAo<UsVBN|soaGH$K}3Z= z3g8cdkuFy`hc8^AH9mhuL(RgGg^@b!-_Z~GEBP`*az;~2J96FROUjti;E`0O*Kv?s z?92p5xWRMg>#2JN?DvgTcc}`g+py{ODqPd!gY_o7pnaqIF$o5XkVN+<6caF(+|;3i zBPNGac2C@p_qah!&J8DMC<alXrY-)iQi_fmeFet5T0u^9C5ViF3ZDC`L%Hu~(C9HU z@OIywocsbCD)pf!bg!9&ZhqBg1{J)Z?lfOu3>xL!s%!}=uUncaT$792vU2g3mJQ6O zq{&dLK#yvxl*T)@#4@useFalWCJO@jQotce8JFAC0vV^FOt$MQJh(XocTRYQrpien zBgIAF`m<^<e{l`E?>hqxGkfgfh2^kX-5I18X^)5AY!Jxh6oMUx=fO3X`PAp+9}tE| zQ!3Hb!1j(cbKE@-$@Z+FD!1;0bPEqB`0G={humlAZew`x;c9H9YYrd2J;!`HpbPtB zSG@D<N#@+JMbNa~04t3Yr^4M1fh&V~ID@mob)jblp3(jgSq0uk4?qUWzC0cdO0I%f zcPv@9cO`mgYQ;=txlme=3-+zp0-Y%*CVZ|AJh5^WlW{$mIWQ!J>Ry=$uj{KZ*FF+J z`U?TSlzs=v{w-8tr!UnyTa0>27U19~7`#v20z0=`qU>1+8Knkb59e&WPcZ`;?MtOi znls?jOFNL~o6%rB`yPmW<TG&VMj4XXF$D(Q8vsE07$iA8LSW|h5}mr5i{u9ug87ka znZl`4srY1h>OkphDl}BO*`nYe^QvqK6YHc!H6E{ocK$I`fYK}GtKD8^mBv;0-PD4j zg0*4Yw03abaRBpqwHoH7NK!i^?}Cg=u5is#Eg)YJ!F-67#pEJw5d9_??{=rj`OXi~ zj8nRJ&@+2T*SCO)>hgFIR|a0RT!QsZ*J0_n9OSxSGqUfPOwHjYf*_t>bEd2WLui*# zcUKNZRfI0(*W|@0kADL%w+~|y9-1}#aY-g%(ni!is0-9Cy8-KlT2d{!RxsS_Al8yM zLr>nizzJWMGkV3Nkn9L?c>eui5cE8Y2{@3-_#e7Ttl5`_=JQVzw-<DR#yb+|)kPV! zW#(1zt@$bVk@u2(Yp+5*8xcfF<nqCayAp0`ribz1RXNnAZwp{Y=}Q>z5)A#RFYw1j z2g<oCo_S^0&b;5Vj+(G22hW@R9@iJ7!_l(Ca7(8lW0IPUBWknA2Z8VL(2P`ad3Y=( zch~_qI-MhCZoUSdNvP6J-v;A6;uNaxKL>PMjz&q5?@*CL2D+SQftGlOqm7ku4FB6f z==uEyJW?qJtrHt5#UGW-mm|r{@%{+gCEDYqw=(cMaEdvaX9d5#OreXjjo5wZMzFi1 z4P8pNW~$kDlBAZ3GddY0qwV?*A8qeM$Gvrs)6_sL+#hm0t3T)-`y(LX@qcIk#NikA zPh=mbz)rbs*nOcFJGa#$dGbDr&fQFRUfBW~E8=nfab5h>brqV3LUE^pIMcZ9I0$c= zg~#ok4`OSMlCn#?L4=b9R*LRGFH0TqTWdey(smsreSAT#JrV>rR@*?1+$rGgDGfWk z&vVWTRI#zTZ0~g~N_cQJhTRhq9{=sS<o~fR<mf(9=8t_L^T~tpRZCrJ5APg`-XKG5 z%u-{<dPmXw%U$8X^zGO%XcHJLr^;AeNr7rBo$=gN_F(=cSD;rq7n?kp#nhIF17h|? zsK~8{?@baJ{9rd)HElS^Y%9Q08G3lg2p)VlLWT-EVnU7oxCBp<`i@7P&BUi=*W&^! zUnZ?XmulTfFoXR&aplLg@bUr&%11$hx^h8-otHEzwFiTlT%97!lf3}X%#fkPe03?k zaYHDTuQTyGotvoe=y7_@;azyNlN!UNESZ5ES0<(P5<GIoh&mNe#Qca)L7F`su&U`M z&gwV`mN-nuz8qyJH&=-&C{KiHgdx+j>IKZ4aS7zwAA?Um<>Q_?%a}cMGDrTM1M_Ul z8S2KQFshpv$xQcBqGsPV!lS!~Qi~QYhp(g)si}^)Kshv`TDij*TNwdta6L!~4D_i< zvdi&MZV4Q2IG-BQ*@$mEm@LprUqw6@d%-t~wI=lL+Y3&oALESwY9rv6y0{Lxx=L`! zzn*W~lPNeU<|s(u`x9Mbi}|H{77z=UtMgT~)dlaD@8lm=9U|D?q${vumUA-eb!dxI zN&?;22Z`CFfb->|Ix%437tZb^Il+f^7r~~Aq(+<KD}+>hiQvVeV!^lTwt^<@Lju}R zm1vASPK?(xBNYcuAU3`&CaSf|1h!cth<VQoh;ZIpzTZeK!p?3Fr>pf8(c?Z+@Z<6w z!4;zbVqQ>*K=V`zspo%<2pGOwaO%Qr!l=AVFx4tg@Y&jkG;(O+R17d7UoTw78NFi> z|G`KXV$F*kd~Ndv!2#E;O_ni3IJagFAysY<B5M!Z5{pYh$<a-!^wjA{F!w4F)b#tz z;SCNZzV44FMs1r!EKe|^m)l=#8oMNp2w?Y`(or)p3hDEAWV#Bb?9UY}<cy|g*?EvN z#by%?8N8;Mdvo}2pAHn9>e#~Jbj;$IPueChO|TJM>h>p`vQvoQaYqT&uA8nG)82FJ zfAkleJTQ&uXgts9*g1oLVPOVGY!2kdm~&jc$YOzJ@@kGu{VRUa{p*Be<p}=E1=j^L zZPpXJh;RID-^%$Rqf?2?9%DJPb$kW>22#Yn`L@LK$bEvARfqVgoi_=a>`G$9mM8o- zBcuh9HbsJT>?+7^S}D*twcYjnr3r$W>nob78ZR{sp{5GvKJw=rU772;v}O>gGRcMb z-gr@v{o=D=;gdFwPjD-7u*cZt$SFDE@ri}}ZQ~*YD~dt|<Lb8(QaAb$m7&W7v$Yoq z63aI5M~1EvxcJD^W8>Y);WDEIL0#q~-!_!kvXK(V<B$9g%L@cXQ3Xv~ueK2pdl+J! zxC+0+V<)jn!<yz!T_mtxVMa=vuq0-_@Dn^d;!KAw;0e(Db;S5Jj)IN5-8u7ZM-vIl zj|)Z~w1w*K2cR-%KNup>j?9uEojdzIFup^R{)Y}@C2Lh$XYmZ?`ere*=&1w@%9Vls z=3dl{V`9u@pGnNdIbE)ilKY{!wH97<+Xu{&(`0n)=Hq*o2l4T<1DOL#g&_WkF=c#F zn_7+47@uifm_G9k=Co}lO9RI+&sLgHCHBcMtKSrcGsOXp3m8fbU-lM0K#M75-gL^T zhyx1;6yqO04pjDset<vtBs_d10&WZ3Nt?~FMTNJt@ExhGP=+&#>B$}h&kw2trY1v} z2m1z5>)$6bZAWu3XTuWe(DzID+M)rB$%$E14De$T?|*{|(a{WX%opq0<uU%}Hc;PQ zJ;xUf<7tkG7t`}Ji}5<Pp0b{Q0S~b|iEq^&#w#LU;y{lsIC<Al=0?>mxcJ-;v@zcW z?pRz5^LG!V#&*auSHt2MC*=nEk<kzwGc^uKhOI%WP4d)v{v&d~)HibQi7#YEy9)mD zBp%7^?jhuh;^-x}0leNINj}TB1g!@C;B2@LKX}(ZV)sjDsHxFT&X5fP97Vq1><(}G z+^u5dfAk4y^tB9C9I63jc9&?m#R=q%f(?RD-OXgR*?XeMERS9}FpY51bO7jc9T=4Y zfzp?5PEBbjD%&sywIv;-@4w2X1$l>PXF?sxXCg2m*BV`2x0o~@Xi2{^SBJaikHGgw zT_ADg2w*+UT!5D30{2f^z&KC^?U*GAqpmvQ;!`E)%6)Cn)^L~ZIH&=}j4K0cwtuCk zA0J7ZE-=Cl3H>l|nub*FyW@6=DZtpaTX40*7JP2KN8a0Voo;rzj{<g01t-UiL}{Bx z6Hj~g;xySh_{Dn~DjHgh21aXxepWBob$})6aheR@n&qSDlZo&`wE&dqK*Y!>fFqj| z(K*FIc*x}uK<ls_QraWWoElpV$}>|*MeFmVp2K+DoW}0m3@ZWE)A|9~^3_o9aSj=C zvy@(8rOm`0KMaYCDm4E47#N`dkr6YRlyM!2xdX)UR0#`M(-9AfHeaCk>a?RB;c?*2 zs*7-KU@_1(`%YVISx!pCXErtWFhKgsd}#jI58O$3k8g=}0GZH!;LJe-5URU`cGGl( z4Mt-G@9sj-E;x@Sa=(IR#}5D$Wg*$*WMJ)g1$j~XaQ~aiILqcch?h(hSl|1C4h~Ym zLDvrh_M1qMWmg5LIkGtK=2Ya9yco5`%0Pbq2H0G;9zNyj<N7yGfI)mIj!V3QHu17h z=f(rz?xb?)&$~eO1XUy7&#Ex`;Tbq)fFnKS`WpP6ss_0Ou7h%&`BZJ)cz9!+6y7vO zhCI}|4Q^e#5k6EB!=V!*k=t!kq=`<!=!x5rtz-eYFg6xjho)eEO<g82$QVx9?T13E zUXVWQK2=f555TF&2HfpgV9!;1x|S?N!SZj>kvt1#cu6IE^JEB~Y^?zA&rBnHVo!i| z@?-Jq@rmG-xG#{B$wo_JULo;UB)N<61i@a%U_iuCaEP}Ebop@L%C|}MNyj1Z?wS?Q z$@aa#c;Hk#%FG{L&Yp$27BbM+rIWN8D*-!HtbnTCV|35Y39n1fgj!?P!&_2$XzSp+ zV3bY~*mFgV;^(|UpGUEKZpBXE@ks%Q>zE5ByvJZ&|A$~pRuJs(JsX}6V)wF*H-T%$ zzG&H*LO5ks77Wba3eBTw)O2P9*0nwlU6SgN+9<9-fmsA2=AWfwa*hDgct0dJbSSL9 znS;M4Xp?i7>SOh~2J{RCGOi}JXw}mzSfA8I3QOK%KdD+^W||3JYt*11_C5AQ)e77( zO9N;+GeBk9Nccl3k2c+RkW5lpL2Ovz1o|6|qb+r|q2uoslPf2OkfxXI;INm7yfVfP zO%B})UcD(G^)F8(I&+NB*4H&e+Oa0mxb_e~XZtw1^N=Sn9$dmnNUz6j_KiR<;F3VL z=Q+KlG*A#2MzHG!Nji>yj@C|G%wPKEGm5+Gfu4Flq0JZ%MAV!_lU=xAMt3aoU2uq) zdqEuYuV#}~H-s@(0u5slk<JbYWCpF#OYImid_$+8gj7TU$7t{+67i$4J+eEpA64kI z0%M0%l&u^LG&DNNE7kk?ogSv>zWRCs>Shs!-w%VcUdusV%?C1A^#CwfZV$dFyhlqM zj(`SnMbK(B8h(j?joR3~)9W8~m`S@Yf#_sN{@Cb*59#V--(AD$yIp*6vd$FGke!G_ zTr0soHV-&H*+QT7ehNYwr(mD(AQW|P338u(8IW4(r1%p<I<T{;cO5>?Y+b+q$@kC0 z{||is?2|hV?;kJ>_p3~R@s?|wJl;yucj`a#t9P0bG7fcu!q_CiXU{B9nCk#vXC4JB zRoeK<wVP1xnSEgJep_%O*BTo}oB{V*wxh6o9<8`>0{dL<1db^Oux}e*@BRL{UJXFu zchAD(zddLC^ZRF~UwD2J6N{(VZvQ{__uHmlbI5O=e)IHe4E$QF-|_UDr(a{>*INCK zr{6sN8Uw%9>UTW-=IPfM__bEQ<LNg~zsA6?wfY@Tzj^vK27ay8?|Ayn)2}h`Yps6A z({G-Bje%cl6;BD@?{I7X;^wB#{{DwO{@ecKKY#z@{C{BoRZQ&U$X~Pd|4FOgWBfXM zzq$I&)vq(~>n;EP5?6yq{Q5QjPh0*T<JXz{&DC$New~3|Z~6aDTnX1-n;fdSV*Odm z*yBI1zm&wJ{{9{+`}1TmF=w@YjNR2*Bz9Myxw5^1@~JU!JDpp@gf_0FxJQRkCwkJT zigVQzPce&;EnC1GKA*{Kc;!yD2&OPg)c3;WdH1NxEtbsuhDK)W!MD^>e_gkcrK!{< z6>Da6RS0wM$PlWfQ;8bzqll4>F{0Y*HZgAb*QpcnVwBWOd1ltDJZ8hJD5`$qc}CZK z9HTg2oBi$OWw<3*fYWu$Ft0p;scw~J(BxL8Q^tXMq|?Ae49tR!G7p%_nwgCH3JE4_ z&_K7$SL$vPKiW`gIzMp5Z9Qu9*L=ovMgw(_eaBnK(W$$p;~%jU36KBA(cr&x#J++& z=#N)*fBxC|pIHC98anpaj9IWYulL`t>fAZ7NBIBn2#^0Zj?3T2VP6S${IBEuIp9CQ z+X6qt?^2{2GZe8$U^}h4u?^@cTA*7w=6KVmTd21B7W;l!2byqtAbeidA7vWYQTc>2 z+^>|*%-eUC*;(g-t5#N1VK<xMgLoU{c~}#F{*evm*SW(F_w(@Qm%jM1+C;F-LK;W- z&cHE041n~RMq)tQQPeE?1nHhofmhT$ar*tWP--0?nRdTM$uJ&VbF%^I;|E}^aW!Dt zmcyXp^;&xW(t7Za{atdziZZaU;~M?lDGgXfS~8<f7n2){X5r|<NH;0NVQv!Q_ZY+S zRZQCi9?H;2!;<P-u|vr;__)gfxD+jcgM3TyfTOcu*_}(^cyJu}I`0$O#xsI%#Ojc{ zK_w1(V*vF&cB95056S%bT}W?7B~qCw26Ik`;hSn4kU8%;xPQZvR$Ziv>wE`d^Z82H zCw35Yh2n7b#40SXYJ<)TCgOwXO3g3io-wCWlCkBQxy)fj1}^%cMdS83tf%7xPk-Bp zW86mLbt$prxD#{e*^Mc9amG!0qF*9B7G#N>DtYJ@`@Z9ez;$Ss-(a-m+#x(+dpl{; z)dDE37NAlm19u&i#-qgMpqC3YvD>*7Apfp8sk}7~HD>n{Y%B0ZF|tQd|F_}Lbp2SQ z!o5o2&8iHw$rcBIA^6U-BCzsaJ1*ED1D8nZVqW`Y{4uv5G`}x_H~SRBp&!Ix`Tkh! z_WBJ_m>P!j!`d*9{dYG4Yc~-lQ6r%jnt~-pN|P?~D%eBv4N4HGLXAge$nBFAteGMK z17&1jLdbQr*=ZQQRO3d<$V<RaA6ucmMg{s#kd(^V#m&`D;%*sNUg5Fog>YemHJv)? zEnK`yA6@qQ0d01s!;Sw}XJ-OU)%X8>Qz2tQk|;!^j1l+j&p}8?N|Tf{k|r8xln@b0 zrZPmOB1w@loc%dh1C^4bqEt$e=24UK-0%PY4Apn7=l85<-*xV}?z-=@kA3gmcc0Jj z`kXUG+HOv~yLe)v>4kMy_kn@#?g|(A9ZZ(lcjoYdUF6|JAzhK31JT#(uzX=ESy6b5 z8g|?wSDN0?4%Je!+;{_7CL;-I+IgIUm=qMe|3QmR$kU0-OX#(|fKME{p!r}Sm;2I4 zcxQ1AKV{<^e#PAd*tcmU&}Gv=EnAvSdToTgkrzNdWje;C8>9PnZK7#?8s+`Q<DfZm z;FUBTh1YgexU8Q6%q(|Yc=HAqJa0ad`@XcrDVTm9*+d=uelW`3T5z-JI$2YiL2>6m zs@3?D*uK+&l2hmCvDul7yp}HQSDe9Rz9~kJ{VMiX<IaPc*jk<qnu<f8PQf~L7O#qT zaxX>fs}-9h;bdhL^_3h;_1db*wof{cF?JUA$;HBu&Y5&vbQ4)GNTUbCI;op=5B+}k z4CnW%i6+ks6+IV9dqE1y`<2k5*Z0Vd@?6Gd;7c+i_!7}0L$OU<kL){bNDiDI!k%>( zXBSQE#+XqbY!_Yc1LrSkDBQZ3?Y-nomgbEkAL6p{$+vcrX>QFbELefhO(Vdf&z~AT zc}9DmD3UN6F_<~Hm&z4x0U{?L*Bu_BU9%<Be|$~PT{obH1^dXDm`-9mN}SdX+fBxH zji>f!ZqQ#ZcaUwXv>0vI_e8Hv67Bs*B49L4j7g)x6Mcx;12=BEY7HA=-UF(g<ruuh z5vA?+L)VFHrn6KJ><f&mgA)tDKj9q-D^Y~20ovrkxrxNAEdbjNSJDFQXfR&ChPpo9 z10yav;ADjb^o7e;hTT+2eT-!BbI}26VS9{5Zfd2;<*NAN*-hs8oW1l1yO(+VC4!oz zP9tZG-oTQEB78DVl5dE+#V?XIv~PA#LCt_Kuy<k{{anUjt+zZ|TB6Mcznle6`9$zg zc??pL8F<p>A(+1nfQVHo)k@P7pv7b+L|INi{Z%ROZItNzbz}kKrjg#$eRP0h5jnK( z8yUR#Ef-e0i3m#NIBt$6<51Hdv#zM%(B2D-!sTuB)#~}8{EOW@=itJ^23_I7kW+#b z#YKGJM_=~UrTwU%o&@p36?pqi3Yd!#m>VbwU8Qd1g_#9R4?YU&njT<p_L>XPjDTyQ zh2Y&4B4SiqfvSEogdgY!j~^T0nwj6Z`SS6!Jn|e}AfG{}pXjC`^d(K#-Aw{M+^5>1 zm&omwNsRMLN&M*K3Z2*Ea8us{S{w-W^G$E@!rH;??V>{b^78^+e6O6Q_|;QY3v;Nr zxP?AY@PS_64!X`{GIwdSDY2><45ilMq(Joy8O24wwreBk_<SAEUO5e0ukWIJvtQF& zIR}YG(j!rx*%fNI?5rp++K{*}_$pYDd!D%%`Hs=`R3lfLQpwDsc>3Y0Iomlwlg(Bg zfK%UBv8Hcs@IHmo!Wt1H{gBOk7`s-F8VJ4MXRb1-5as!P9#c$hj~*s(%BNuVyt}lo zeGu{zLUQ|40o^;xQxv;bN;+MNnQtnIq^LKF_PxErm@jj}MV5|e)P!W@)TuOHDVqo+ zA27XNwCN{vohoVfJ}!!jqS9T`G%HgDHYI%q=Rz%Z)^%6=zHe&wqLctp-b@+X^q9-s zw^w3+Iabjy?LzR#io?O%&(N&Sd@^D0Q>H;do0J}ZLC!Yx!?d_7jGQ@7PCYiK1H>fZ zbnH8Du-b;r?QV3rD7X0RtWf&Ot%+N4Kn9EM8!<LyD*a?U5ckiVP#rM4l^9Atr7h1z z46`O>_Wi;RcJ!WP{?W)oy#D1xwjyXBzrXt&X!}iM=dZKopN5|%g;_CxQ#4s?5%auc zb38=E&tMdvPUeqlJcf6AT|~^I5>w@Opl5dhly-XYg|-4vU1EY6qtfZF#2jidT@mv& z6wrlvO6DYdCEs86lJ~FbIWD7&gy{|eG2elxb?z1|GkAkF;g?ZYPLU5N$QEi&QQ!^8 zV?H$c3lkeA;%%2K$GWHmd`{0`zUaIvUvKlAXa-o}ZPOG;TB3kw*Basa@*|*==*VcP z8)L$}^VO+7Z8S<-K<#h5=ejjNla7qhXb~z&@0f;GTT1<;{_Cz0rs^rFI{1ZdeQ=gY z7Tu!hJ&x$>`ih|&HS9COT5<fKoAi|BcQ#e8Ki?xv#MGdrti$XGylQ5`?;Eijw$;Cd zndXbpbJ{?5zM&Hgy=ui?Ej^COvpiYrJOfT~j2K%qdn6uC6o-wMB>8}YqTI5iEVw1@ zML4#Y&hXk!vo7DJms}IcY!zKlXi<hW$%W*8PBpXT%_zEQcV>0ckKv5l#VVLTtDb4i zN<&X;L!nFhM4?Yx9866sA?Ei7!;+n{@I-kjT)9K>RqzE^`)CC2J8_v#_VNMsc@hw6 zR!?V!M%gZvOdxE~c&tilBhQ_?=(qwqJW#%u*p%B5jZ-=Fn41>f`V_~lIxGW&W5%Ea zl|t#${ZaZre=?M@B}thw%r#S4Sbs`aFz8eo)i`jTJW@5~@AJz1mLc<bpfdchexh|j z#acKa=|;<XbxCyLA?kT)I0>xL1glSqm^bYT_UzN9n+oEnwZkl|tu$jobIKq?IfPDl z?FJ4i-{|nur<rXZnkZ{2O=6|vNuq=+FiAa>h;Qe-_WWeZ-}Q1Qh1CQmn{(r$msPi! z?}g=$Q;7V!M)+V<Equ(^vitRo>=h^X(huLSgNNY-Oy`Z*Xajln5=|98HMS&o6r12k zXbzSotY=&eYw+60k0Q3P7B&4+LrCEf2p&HSHi$Kl{a1O+tp7<<-4*aN-eCOJohPb= zuH=c;t7?sLtt58GO`=zuMdJ2KfP9_0X#VJ-?;8T4yH*Tjx<12|OS$ao-6Ga{{vOuh zWHzkbGmtKs*FcQYE`y2NYz+180uio(lw7YspTIDZb<Pw%&VB=*rS@UFg(?iA_vzuV zL~t3#al7I+W76|^;8n*{HDd{s@Kwj6_u^RN5l-q0Hj(ciCld+r1x&bG1&Qsf;@mfk zf_Ja>kgdH+Y_fqkOI0<n?Ub0Xts!0DC)>_Cn7<*xHXh)3p2z09@0ju%arm-#2<sTa z;5rc(tEj1%S@NhKUY+BB-BPVY<>_gvA2I?LN@kO3;yJkdW+mNu@)HS2m4ILqWAqs* z24SO2F}qq4l^54m@AMu_&yQ~<A=k6HuC-<$ek#gtleh-Nd{Aa3P+b`R>;!-F!~k}j z)^^x>&X9d@JBx#3*U`3PF`u<3o|MS%gU?6Cpza$bc&Yv#2UqrIk1Kl9;@&ZAq1`1M z<)6$HuX+k)b9t0Mvk5j<o74A-d9Y?>3Egh}o*U&mgp7{!Bt0L!>6$_Qw0oKsEKb-> zg{m)^FUkzjJEnx)p%!#tYY*poLY-G!?hbDc#qo*{tl1B~7VOsaYt%{oAbov12WC#G zg(dGbP-j9G^I+&*lDt|8_vI@>@94$w;+zCHyxNJuMdM&&!59qs79-+J6_cp^IMk9m zM7Fdv(5lpe>LHHfq4LNN^1S6cjas6Q+R1A0a9IxhIOQ>I_RgmMHk(N3w7cZsi~*QX zp~Ti6S`FWlcCel=#xWCT58=-kc4Mj50=QyYMKr4#7+LcME?mqMS|uN2hVxqZTDu<S zDQfcZxg17ig@McKM3la`mwA4?6ilZlp!=`>P<(d@-XAlYygWJ#m#9SA6<obY(#wX^ zrI$}}g`pz&c*r+eH!_!=t8<|z4&SGflD2ba=pLHlbO&CH+zqy~`$6c*V!qWg3H&BD z;Zn~Cpl!0SY0Vod<$H~`G9uoR+c137HVVp)c#HD&W%(!DV=%(X2OQ)sV9&i=(K!}y z51&t@nNcN7Vv;PH*zBa+TH9&!`81*s{FKDE$B;&;J#_7y9fF1R+VrBp802GgLHdm{ zw<^Vi4A;I6JKs)%e!3dG%LY?n*kL)gJ2#pSmRv`-ZWeLHew?BohPR@tXDK8~OS7As zy6Npg9S9m-PCw|+!mF;CjL*p&;`D19mc)gDMAT_oWEFv5v4M$LyBXJhnoG&;IMO{< zoi=Fo2Qd+UbHWZK&~+FK1B*>y!);4W%RPlzR3eR!pVbP6-`3|-y)$uuZ6ut-UBW{i zw(Q~UMeL39A^cWN2de+phs+^ES+OTy2-j)CCyuWpKcyGZfy&u<eavNAx^*tfJ-to~ zBY#p^BPsN8lY^T-W1!n$E-vZOq<0tYB`;PfVf0uvL6Ok_*r;1woo7_SDEZ2em5GM5 zVcReAvW+K+(&m_bP#jkuQ|C8daS>kZj%UX>W`f}}OFmjugC=~}J7$Q8+g0)97_4{x zCWxP9gNFBN$eM%I<ka}%IJ{X1KVmDO%$P!A@L2xh99h_xD90+yH0Dj-UWSE68vGZ- z8Env|VPuCs!4Z!<P`zgyGD{S|T=d&DPht=hjFW_k8;641{XHZxcpdGRc#LZK&&A#k zlSp`y6zgzwl5oo1p~Ad{oA}(XuKYATS-QHugZwg@O3mx%fJyv&@IM|`W!<-!MwXS3 zaCsYas*NRa{eRI6p$YvU%75zm=s*Ta238-xEkn~Xn*S^R!B^;b>Obd?D~jp=X;TrU zcMSiJHkG3B$2k_pFl&|ED(@xx+UlMzuew(mQ|YYUB#3y#b8a&F)#{DoVcd=sPVc6I zKx<4A)389Cv?QLyMH?m(_Omr_9B(CXlWVNbRlUiitl<UAJn{rO0R!1gWk%qzqEs*; zWj0fmV9GfxloCitTJxTTo49{$QpTUHF8=5IaYd0W>GwMDrlPbCng9KNJ$`FUD<(QZ zMSpo>$=c#a*VDwd#sK-E!N`2th52i;?KU!p$eCwD`IA?^(Y)qQSP*uQRJAV_*=0mL zJP|K!(U1LTW^4;FnO2-mPZqUt{zYBn*OG4wQ)y=`PqgX+xr`zmn5o+W{?6OUJQqDu z6tEbl-<pki>~^qi4@4`SRm_qrP27!+uIl=Y?@@B2F+AT=Am}1-Z2$Ty&TF9zi4E{Y zyUlB$J9Y#MGIntH@G49RYa-7Sqfl!~A2#Lw!jQ>FAz#f(5aGt)GWiHR;(CN0J=>2B zXsyNV;{If)YCLRPL~to@i4RO~qSSDGShIE$@|oEr;HnC%{H~sSdO3leP?-<Y_xlm= zrxVcQsu1Pd(^<n8D~Z+yTmDhADOu%*bcA{&+kcD@{T;1AwM&)PG%~?3KTlkk=7p@z z47Pc!#`v58cX4yVL*|>zFIp(`6*L|vLWs;=qA_b7=*bhfe=m;nEqKm3K^3`K(ndlC zaA?0yjOl&157{|ZFry=x{#fh`KJ^pmwq=)4-R~eBlUq#1f9=ML>al2N$B;CeLHH!^ zE}7@n!yNJ%4O8lUz^LpxxpTXhtlW1FHeRlWGfD|i?=CWS1eU=d_AH640(=%@i0zVf z)c3>*a69o9+<F#}BWv8@wxJl>iC;ps=@INS5x2)yW;soFO((|=B~p`{NmZ&o&Zw+W zhIV+DeER%>-m^)<nA{GyRWIsu%~i;=-9S=Qroy~kQPA&>3Z4zz$sLI~kFLe<ar9Fy z=qL$<3u<%79XTu3=co)B;ZjGu%k`+s<-JhZI06?B>c%r8<XJ^GIsQ$Kh_mJ9%pX^L zjT!e{u`hEeyKZg)Ite(MHU1WU9la2`&wQhu<9onjp$?z$V=PtsIR!Q@OQKgbEm1ji zDVeS3h2nB~bnb3{Zo$tQE+`<1OqguMkDPLo%wAi84O9Dp`~3jo)bv^))HXuN(pD5? zBGI*#=Zb97X~4r>^v1(9GGX*5ayBWS_=xL}(H@(L#~B&cO0%3OhJ}Mx<uQ^SqRV?t zv%|d+vrtEDJ`LYjh;#2G(mlN^P+Dph`Ig5BI;>xTUxc1OtNkLfBTi$)I(4}A-ptNZ z=}Og@f^o3=o+Y*Qe8M?qyr6?>g0SG*BT)ac3r<-^(3eTdn3lX7)!!zt{&M+Pv3U;_ z?DB_?i=WWJTEU>YC6+D<)aBQWxCq<22ZG7hp>V4=jvfzkp<Mepd^JRdZd@skvpn|l zx2)6Q+jK?t$DvXDh@L_0j>vn|qIMcvCmOfV_tW6RSu5UCZwOtbDzbs}ErS@<qcG*b z52BW+$aiYl0;`%r72k@pk=dRgJYY#e7cB$LN@dVhxXVnT_T<2^0wOoOm2|pY!T13O z$wt3*cz==xWNAduo`Q7{{i&SVHFqJbI8QBl7`nzi3|;qo;o3>h8FuP=THiUCh?Wv0 zV8b(FcySLH<z6Eyb0XoxLx$a<vlJr}hQhN_573Le2x^->sf3%T){Cq)I;IR4alB3g z(t4`Z(aw!655TNnqOt1VfLnbQ!{=)SjPEyDNQrMEM+zU1x%rx+Jo5rrJ<10^h-$VR z-98M@u57^98Mo;6-j7iF>^;mq_JBI?kp$n)8Q^~EJCQkZ5`Ml4;e&m&u&A;C&YlZF zm&*lIK7T3ZCVBJzBA(O33TGH`{{==kM)R8mqv7P7q3o4;CB)rU3D>8`@TGZYP+2dZ zYTTd9Ms=m&-0o;HDnS;{iq+t$J&%c{kb{C(6<BU)Pk(f#axE)V8RL$e>PI&#iTV8f zMD}WhVEWva>NTz!beNMTIT>0>wwOv|vR*&Z_TVwqe7c`WOIIfL34S6rgDcTV{8;^J z#biO+6&pIe;{hjv5HLCgmV)VJ(N)E=`7|>=k)(NDCPCfm%+tQpL@w+z`H@&pY_6T8 z-Ie9^a7a3Xx-wAMo=D`CDEGGiaUx-xNbW9CWd^#cgTL<`BHvt0<_0e${<NJaoiU}i zzDhyk<txm*7zIY_&}>SkNx+Wix7@ZHVsL*~2MyZN#q3S|MjoYSlbnFPjNGsqy6o#{ zTzz6bD%Fk!BVb|El;_M#xAmBm+Drp;y~wEyg3}wFaq6x@qU9G$dv2!^eS>VQSc~-Z z%tYK<<VaRs7>pa8j@#`DyT<jYO(i+WzBrN9C8xK~!M!WSLeZKs8fff-WT_c0+S!jb zHpf9-^LoZXrH*Q3ijy(w8l0x%7Bc2_GMCe1L{kqpaha1c?F?*snb>jt$aL|6RA{D* z_onR;yvYybMmwjF4-?bvhO=*&=E!*hetx^)=lfFbj*A3b%f4tA@<yXt+ixK~8E?q+ zsO=^juF8SF#fs|3kF%Km7mP9P*?QtMTmtWO&L*q+m6O9wm2_@|9Ou19icaph&8=V$ z(P6bla9O35>_6K_CU{5FcU2L@I^-E$EvywZ_(#(@oASwzt_W^p@kVMp`WmTO8^frs zIl=@6943_w0c1kZ8oK-44DxJnCFdqzNCUf%!8LC+oE0+%vWo}MEwu*tHRUNeV=haQ zk8UTkmgO@{v54VdZj15DXHn^gC0wv$9+zKMK?7<k$$PO=oWHIYlpa1zCwz=y#)mqS z>nk>3v4J6-yHJ|O4>7@xi;t<xbQ$oe5#T;AMG_UY2j)9#(42^yT<RSsYVvwKbMi$3 zng94M(cO58dGy`hZqux@Ob>gIgp5=XF=Az~CwDG6)Uktdd*4=tY;7bjv#wR&b(bOc zR!*!gTq?nMtXjd%%xS3-T17HJmztQ(Rr9GuSSM%jeE`H+OM!6uQD*hn05W6DD6o|E zB2~&Sxv>ilaqnJ75osxT?7u`8j3fthgZq30>~zsJ;+5xA#rFZpdE5`=YST%P?|3>o ze<GP%BE!8oEk*jrA0qv?i_`U*Y1C=_I`Z?nLv`W~7Z9I2jGos@Ay@V{lddDrxp$3M zn7kZK%u3S6e*Faab;TFb7PWvFRSZC-MN7#={e$>wdJbJb*ooOT@kdqW+kGHtSVG+k zCD6pw1g(YJiRoNp+`70=@GWlyx&2rcoj!~r@y9Qbl1@Ep_Iw$)yuKJSmJZ=22Nf~C zr@S%L#fS?L)z4dB%EMV}1yWk{n0{U$V)!3l&z#dqA$r?v=`^`&&iT_Ca>zE9`y4!- z3HlPo4ZJW+bd5C#ivk~TrO88S(~?M1I6b-A^VnYU&Y-QTreh)F6*Hca)MsR$+Mw!j zKkS)6<9wR^-i;Q&2p}4VzY>cXErMqYSCEaaBEIECCAzXniUxnb&n^B`%SiHW)LuG; zIXTe|U&aoG&T?6jc_0vuoHrHmvS$$EyzPwM=O@HkC63WQn#%3u^SGCS50o8qoKZ<v zgh>|0FtzAgwM#%6xAa67IpJ4BmJiAz^)DI;w8>MA(?TZsTQc;7*3iKT(x5Ej*5(@T zqja`BJt62%1tQ;2+Ko`0I86b2o|U5ElW{OmRgB)enn+ifc;nXMTDorVPA2-mFS1v~ z8uo=R;EppB@bIk!%ow)Wu03cc`959;=X5I|8@Z92I6Mru?ix!%ik6ap*5OhQ){pwn z`Q!icpNqm5#l&>XqG-&N1vo5dES`xorrN7cLB%giy76Ter5Pu2p7I8io<5ge3k-r5 z^JY4GZA!JnYOJmnuVqT_=y5tmiOhm|!fKV3OSy>}Zq)(S4b1cdx2qF1Np;Im0rSuK zt(Lst;V<$h|Fiwye9C`a|BU~Cx&CQgvWcu$OJnjbpQbC_=W|N7)x`F2H@7ikG#nkv zf@RrHJg_B@4D0KqH&<Ln`vFgA_jPUFDpn7z>`(G78ISo#@7A+(#SoXzGvx~>Tag;` z&v0~Y4!^Qb#r~W7Ls)ZpM)lR1?cln*loh{i#hWTlgvbU@_QXCr%x3rEBYp(7UrU99 zvEDFjQUzTk)<!b-se|>70_+)<LN4Ym;i3{R(|7Wl$z~%*xcsY*1ED5~3C)8>pBk7; z&+`|z9>g}+Z2nPnJD6wfX2V0yf+I=h%~HBJ^sb|xV`c3Z9v9fxXdh)?jnHKKuh|C9 zVmFzrHXbWRMZ>cgBRHK9OXI`mLHk1yyHp^@+P(F~Gb^OgecDjy{lN(yCB=z+t?vn+ z6+xSSo+DKak>pCyM|xgX5!*c#u;qXnhKS-NsH`I|J<|;eC&ja`ud0AmeFtinK4nw$ z!}*kOH*D^^#z;G^hINDQvO8`^vi&ycvr|TC!};!F@Qik$wz`gBJ>w!+KiZEsR*Jzl zzXhyW{!(hObOQuNs*3#j3b<nHak8!9EeVJp3JU@UVU|RlAWx-@Zpqz7`&FOi7EXFg zCTIqty_h0<bn6_jvR%POHO*j+&5QW(k4o?=bUx|zZh_S)*`oLO5K7c(VMas-+q}q+ zbqbvc`*T&{<a$kZq2vOxe%WED`Y@7RlfDOHrb@EcO_f;)eNQUf6G8Il1FW@0dTncG z^`|};Y%ClKA7^xuN9~^&1Ah(hl=@1FpU<J<Go;|*l+~axNRd^&T?I3bAA*J3=Gyxe z%h+3tuY-d%1EF=QA`B1gg*AflZ1^u<)+VeG-=!tM(tIPfO5}>|T-?f3D6fY~?elOv z+nH^17)b*)!r*b|NzTmqHLQGS&kW1efRj2FRAr_rapFFcAp84t*3koW#Pi!^-{ELM zL}rN~)V7h47i3i*iLWCC{uf~A$YrE7FaY1JO`$3S)rDmZKf!rp7s^iV#65Z2(IIOG z3{U}9bNB-OzNS0=7<&u)wjQ9nt|g$O$bRXkCW+xKj=bZHOPtm-S$J+ZjsGI5*XQ4~ zhD*Dr!@djOPZ#$crq-VbyR<zN&u2Q3&p#yT$c_jcp=5?9hp1qmnk_duS_6wUMzU{9 zq}bV~ICSouE7Wv$XFv4IV{>O8gF3xdvibB0&{-dWZ@W@ijcRp%sO1j+W|0Z2BCW_y zwM(Y<Wg0lGtrmMWiebo>CX`%Wh!%OV{G=PsEb(ZAGeLpWcyl<;a$iI?m2IP29;)N$ zu?iS@?JZSXuP<=_VhN$t0Oa2uBD0^3f>K*Mc4z<Z)HwDthE^>WZV<(ac<o=p{`e|w ze`NC_e)0fmbaq_}%m^z8T-m~V%GTq^GvUx4dYM*L4dBOn<&vhCu^9f;0pay{{&Msd z^jP>1vtG8)YQ6Jhpo5X9{@W+2WRpxcZu<dlAGY9RZGURoFO_&nOhB7h6PR&PkIuO$ z!x^Pl!M>P<7`!eIy3+G7$*+c&sWuY6>pz)&wlRrBY-)h6WMFqCPC_DX!H*MC-Y`7^ zXN5(<{vQ+3>98@MKQI<g*ioF8Y0l6065xF%jUI9K#C8!ccm9i6xFljB{IDw_`@P3u zgGWCopQy%6ekzI!;cMuYIDNb#o=Xfa41#r43e1rJU8d2*f$Zw=#uq^upzvX>kRG1N zKJ5Dn=}}%dnQMi<af+Bab|2Y!Opf2abSqEF%jvGc1K`zlG4L6(9H+=!7S-oX!#xH0 zc>2~!QnX5zK5|RN+~+yC?ZQ=ZX|x=upDLp7zAq)weYx<sLxa@0l+xnO2dMv|Qd*Fy z3^}7ysCmp_I2rztXh<YM)w(jaHn0S>{Kna9J=Pbt&&-3EzRR$r{R8{v>Tv!>xhJ;H zkmtAlI?8)zm%)SHOp?D)7KKZ5d6NhI*t(027<qFHZzCrLL06Ab=Xt7lwYMB~hV|fW z@6#9(xtt4|C55}36?ngq6Uc_hLFj%@9n|X->8ko3s`9Rnde6HdIOgsovbO|*XNNt% z<w-3LI9x82ynlhOE@JI7<a6M&x+Aad8V@gz&xAvYk;v6eVH-pHL+iBuD77?^(-ze) z8z8DV^guNX!iYI9u5ky}SQv1I3LnYvY)>>e*Uc?8_yz8rlcCSb7OeAMgZZ^~+Ve?N ze<?7IxqEL4S$^y=Xx4wDQ|wx(i`P|%$=}DHSvi+Ku*e3A%8P|A*AL>|YY6!<+xc>d zaDEcmg_#3}@M$lH2!ox1+4{a5FwhJKogq)4`PNzXqlG1p;%WHf<5E^`M>g&q;Y6k5 zlR$lc1}2JXo5coN!~WG?T=P^dJh(v;&J=i)h2bxl$&dBG{H`0m-F^)ft@6kW30l4E z<uN>Y+>ecp%j9pnPKV8#TY1xnc-XW$8Me8mvzrw+^N}<5qPo2u1pg>w(-H>QlWVbT z&Lj&~H+=!>tOxG?sAI(Ayb}5i`AoA%Y{%TI7eOO3n{6*2&h8zp#6DP}$sV1d%2{tZ zDp)mDh26j0iIyA=B?-FyiKOf*k}faJH+|5?)047j<?Sz6)}YUi)Q)6R4;_S$8$Qtu zJr~gFzC3R*Mw}m;<;*@?I1cCgh<>X~PVfqYb%g70XYyBkz0m#Rck1{&6kE%N!QqrN z+~py~gOM#}ALFrb(LTOqB|%BWX<$=w5S)!NIF+10I{j%Fbe*2UNOv|ehL+ce)0`L@ z*JzA;_+sWr?oo12#Butrbrvc!kHZJinpEymf^h%OSiUt?z)p0DAkzZ^*j<-p_!}eb zSzUv<?DT4Bq3<bIdRyBMtD1*nZ+aB(y7etRWNm@uq8@DfY{CnUi{@pYv2>TSCM#B* zM8`|!bFL0`<Wsl~6(8Fl1}`rrNqIYH>-_#G9CC!-o%f89-#eaDDcMS5mO4_I$=mUw z%?F5Ujp1{0qgaufl#J|L&dz!t$=^|0z{-<y4B8pOuDhYaS6U3<?UvV}#+~KZd?Ev% z>WHyzO=^(lT~F#KjpN6?Hedr+c!G1_8PGj5kk`EVh4|#{Lq>%GEz{jpX?q5SUyu>i z9`Yc^Ug^N|NjHdoofM2&97WmMDyku#1t#amvt~{wAZ(>1JFZk)xI^|bKTAhh7(COO zHMbed%vqC0e>z(7KiW26-^Q^*d1am#JUYol1VuCTjUoJu#ZS=RAQx>7&G_5qQ^~AT zO}J;B9WOKaHLL<_D8?LICz(o?C!{i&li#9T#}!Uc`a|GVE5mJz7GP1{Cpdp$C|Hk` z!3ec;V0Gsex&6k1-~6%>)*U>;--{V6oTu!~MxS3zC*&vbKa=y|Xjv>>U+BR9@^2Ke z{`C15N^4>6$?HT)B@teN5sJOAMMHgWw#}o6A8~mz)KhUt&}?Ho#8ZLXF|O8^eafxa z+eO{>ZRIw#B$Mv!NY40zJbH}Fr@`hDRNpF^Ci`fBw_PI_GGzsi*N=!|o$kZp(bfFx zCp_B|`if28;=(_#tAQ@_2YAa-mfcBOL8G>YuaJx7Yg0~Am)+&ut;(%<ZPi+cU870b zio58Rej;yKdk<yOF7vxa8PmL{?d0f}#Z01N47E$xPtbJ>q?HUHsv9nlAtwyb;ifhT z+KOaHlLRD8ep^{6rwns0A~KrdeEUoZcEZx9knCh3Y@4oVf5y%h%4SyLD^Yyr)RD*7 znI{eT)6EHVS;!Q2w00crxAiBS6J(?R>>qfq<O9i8(BqRd?t<mU*HG-!KsHrMv0nDR zIK<8u8Y&9O!}dZF9#{{Pljd;ttBmpWwv~1=Piu&xMKxai)<vhxna4%-%fpqi;`};c zEML^I96klH?8#b+<uam}CcD!(d`>bS>VFm+7o1}jCr;$`@)Yc=`XbqTn_FP{{Z^uU zKpcjAG2jbo#$ZcnD1T$J1Rq=K3wpI2O1ganqAksiKVr<*j2u?|EJqTRro92{XlHtc zOXijZUZKrRu2gwK7JTrHW)f~jk^7rZz@!~}S&8#b>`pyZYLdSl4{bfl`vm=D6;BOk zkC^GQkvG5K<D)M@H~SSksN+5QJ_ukvGIQYEmvnBn^deT}Sq4d6{R=i!8Nlgl0`{K9 zGIrDMVo)-agjW(}=-%*!t1UC5zBgZx(;&lni`LQ`7Qdu5yONok+%Kv%&w*=9S4FFp ztz<}%Doiq;&8uv`$ICVy=UlP}+e-$|5K25d%Aag>;47}A@v{}GVXV@7G`_uneJ|%= zzvlTiepYinoDWy!4ao$4Nm2zo$x&r(#9i2I-*A52;v+EHBZiH&cZTGODG(B?kNwgg zaZS>nsy_{EB(*gws0zQ2+r2;y?S_>x*Y^m?3^7qX>#Esw=CN=l<W2`RJL_V4SOq*h z6ED<Usl=c6GZ9MEr!(hvq>z58&q4JIk{0=P+@dDQ4&0u>MQ<30t{d$keVGN>^Jyig z+OeHRhxEg3a&^$_f12^0--mS<wqeAwV7wMOk$3d&=I&nEPnAu_@xA2@Fkzo0)ZSLZ z&n`Jo_)vvEyo%D%&X1{I^LSomWfD{BX~hQI9YT*S7{W4cnY>4F7v@_kLEZv$_CoRr zm{2?toiyk30oJeh6B7!N9bC!i7FlA~xB>iomwC*2Q+IBN+EU(Va|6kXjKWFjB5twc zLfo_@h|Ka>Pnu0ah~K6G%#;)7snu6qn}oTW>7jQw$b~N@WYTj#ZbFP4EK}2ibnT~9 zQt=F094Y|8#sZL5>0}>%eaMGOZ)9EdEkS?zS3r^9rDcU73kQ$!^LhmQz&BZZ!_*|K z{(6codU725ze~e4v=e35T)}Jhzeu~E9DiihE?)I(DzXU|ZC_pdNIN^Fa9m0h>0ac6 zGe#U^-d_)<wdb@j<n0|%%;o^pKhwd8pzW0VxR$zoh=(AHc5*RwHEWYU#NNO2JZt&R z%>I+VBs&#2&>LLN(%FMh&2k9~LEBgb$1!Zf^M2gE4v`Q4&=}sME(W~U>Wk{NB=dLf zY2s}0M^xJ29R$n@z%SRQ;EBmG{KNxe$oU%yv@9c*_xN#wi&*=e%70b{rtu^(u4BPX z$_(r;xDeS61{zN#;K8itV7DR`cg~L!mIhDfyML(g{p;nS&vO`->03Z--cvk$bU)wx zF_rH!y9?R6v!SMHG*ree!0&=k-oM)&Bo?diV$T>p=)Dy0HA;tle$Epj(;qUjoD}ag zeHfncYoVK$59Ar82(B?g9D85sl2v*^G$uxdy*{;uC=6dr%2-*}?4TUqaBc_dV;hFT zj%OsSHic*7F0rGXx^Va)FIJE{n5s_R4b?4nd}2ihA8USy_b}|wmpW^JwWK94H_no- z&db9onbLfA%URN8{|J9xeZX4<h}LEs6#t74XfpcH-v1B(vF-nh{p8leY?$H@2q}(c zoUK|8Z9S)fI|obH)z~JW&7F99S$i((Ka+vg<MgSQ(*W3FaD)h-Xkl&VDrTEa3z@q@ z4GqsVkjvSxxRR;`WbxHZvLUUIgtZiq8&XT~#HR@$uVe~QTW`}*FJ4r=w^~dVm7Ed$ zV;52wq3ZXa{l`jT|A#*wx@67TWvf^H#vivF@XwKM9tfBrA+j$0vw69T|FaX5k@@Fd zOf03reBeMeF|mI(5y1Vg_Pb|#Z1D0Gf2)75HLm!Fc7f=4zpG8n{?`6p3;a)Q#PGkg z=6`E{uSux%N55Jl{?`7h<KGMZ{V8rV`CI(2o`0v^|0zB-=5MjZ->3aMQvOfzJNR2{ z`M3CY4(6ZY%Ob|(A7gE9^|$zU$mE}5FZaL2<Ng-^o&o-+*i2N&{m-7Q{}%tAzVxTK z(f@C;&EMkR^LqaLjCck8CH~i7|DLP!r*_@)zqJ1v>)&Hi{?xYpt^L<f{~q`8r`Bxi zU;X}Tq<^<n|EZM;|4aL?f&Sf&{HJzL++W&%jq~qw=$~4%_`kH{|9+2tpQrxR250@H zwf$TB`^@y`hr>1d|7ic`yyoTJzrW}U`d>|3QuJ;6pH0Li_^<ZA_SI7V{r6tcA^vG1 ho%BEZ$p`<r7ZVc~ZC>vE`v3D-RdoC3_5c0a{{e{<Z<hc7 delta 26698 zcmZ6yc{o+k`#x;QkRc&L2&HI{c{pdUXNxosnou%EB~gjyoHCbrOc4>4N=PZrUeBhK z=2=oSN<#Cfl-K9;`@Yxv{k`Y9)<4gB&RTn~>sou=&wbyk;*!+4XsNWBR_g*+uMQ0G z30v>KKHMi{!^+UWaI^m_wqgCcpq1Mc=KeQt(SOo1^VCOI?Jtv+H~XKy>R>r<X)9R? z35kH95TBsXu;G#t5_86SDEv=kO;A8UU}%`?f1>&xa{m)u9}!+XLNQuiWx`<K{~ms< zXzl+QVy)VL{-fogIG7R|5#kfLbwglS_@=PO{|QTbsQzCQ{7-LK>;IYO|JS+NFFYu8 zb=Z^tbgD=Ao4L-BlSg59+*x%AiT^a$InqNUB;@4>hm?@W4AoE(4VUl>Te&7^bKsN! zzi_`P=F^>&B$ofLpWXa&Vrm>gwT2|n<O?U@*5!3DrHX~{@MySlq8avTiHXOD@pT4w zS28h6D4g%O!ra(&mUs_529xG0La(~x<_<+Xa&Z?nrOXHC8+QDt!_(>5QAXghaFYm+ zazE&*ohE4adjYO?HexFJ_Cwi?>%_;zn{i6{ga>x?64pf@t3w;;zd3sF`)DfUNvF|U zv)k#gjFWK8aTyf<F{kS5Mqub<Lm0NO8papjqds>!sd(9Z_?z&PO!C(zYxa%6k}qo* zS@V2)dyhFYyjzdl)D%skr(EaZhZGb1V^oatUZt=ZT;P{#1o&Un$2~WlsQb_(5Hk!g zxat?l8l8yCX%yV68ZI2IZh*O89*~oZs%WOE2*2)$#LTXRWL$1ODkVP0lcGHQd0mp8 zo~MpdZO71OZa1A9Hi8s{Xn^^9ZQOmY8~sH)O_`3eT!<-cB};`vh&VA1|Fp`n2BLPx zYhEAijIsf8dna>i)(v=MorLlCo#-c*-B{bd6pNKNkhV=Z;MbN2&g*q>KV%?TG6c1% za`9@q1Ps)jWS&g6!;<iW_$iZvj>;_f7I_lgnvde6eVUN;*%(C2eE=tv%hM~GbMR!r z9{l*j8JPY);&nlVnQ{6c%>H4H3OSN&$dv<R_bg{}u|I^ePW_l1T0#|BN^CZ%I^0WM zf#z$<AVupC8Rj{ZefOskb*Gp@mwh66|K0}Azi8*w@^eY}Ekzi{KcsSpiZM^t5$-Ae z6)^|xucKY^UaZ_Z56T<>a<6X3{QgAx^VWVE_SYW2_L`88+dbqKf0C9@wIfZ2>iEjT zg7rz+025X$M@UIy&b~2&Jr)VXef3(nYP}Pbt_NUi;|4IP?<HT1G@<ZW0+pSrEjFTF zkgV~9`X~<t%P&)L_7xF}v%GtVfBihrJX4FOKHSBBSWN!?yU$!#48V@8!`NYy0N<7< zK~+dCdDN#sUf1Y~Cpr#+KgW{jTiymoRqP}3hfjgzMK@fMIh+`?QIz3|sKmu-<f^F& z`koJ@;@(79Uw4(xHN1eEOz$vxRrzr4Z!|6!HEIx@j8qtQ=oC!*(Mylab;6b>sxZbg z16-H<<+T5Wql~XNOjvFJj-Zc%9~QI?E6Lh4Y1}z+1jwv)gDdOTiu-3>B;=$SdHiTH zuH7sFCX>wg&lv`|Po0OJ3$@IT&!@?{W0PTPsXFyLbAg=ObrhaHwL+1Zq%NvfeZ^|Q zDLDUG8v9F2xtE1~%rg>%p9+%UqxVI4SsjLQeH!>fd>G2Co`Ls>Z0Z=h7W;;dhk=Kt z5OpjCr&RxB_G=W<^hXcC(<=&wtDQq*^TnvXL7DN?sep>sAU^4xKbf2%fFFwsNbYHW zOjElp!d+(RU@H1ab>G&}l8g#44PFKJ-EC2`WIn#+*22Cc<v=6XGFSe6AO#b4;F%4h z5juUKchMxsx!Q!*+q`j^i3iMlvjE=y2%>_5Onfjyjn*C7L>|0<%dPvp1;1P|B)?aB zLg~#8Vzal74mBE&PlG0~UmQeg(9souCR1Z+o9Zdjvfme$^cG>^$$iu>__H|vW-0z{ zE@4`#BS`szGT5@y2LBXU;IjH7G$X|wU)DV0{-w{uP0|~%=hq85@a!QTt1rb7w-eAc zVmXGc=t75{SNM2}GD->V);X<n#o{-<h%0-DgOfb@C6bDT-&ZoluRr_ad(Mtk7}x^$ z{#4@GXiuVhUltlyS~Ke_7txhly`X394&0^EC`@*~PTXdUVuxlOz@V#|I2l@*>8OpX z9OCiGr8~GZw;ELzi|I_qHz>U?4`W@+@WRo9IC@hm9(!{E$=g8G+mnhL5;TZNVnH<H zl>HnXHp+u#a6J6>j%2=_iHDZNqxjcLi?!{$LeTRnsz<Emnv+_@s@E%^BxM-pbS85h zq5jCuo`>y@{X|`6Hmf{+1}V^}qRZ|n!_UcM(eJ=}`Xf6D3(tyi{aGQVx4pugjDr|= zekC4Dc!rn%6^U@tp)?FMP{YO!9>Z7Fa+sus(`D=N=|TfiF_DKC3bOEGtPH97UWy;j zOkksrIl!gV%~;EyA!n{HfLC3^AZj#A_l-%1rXTUROpt=5ZM(_J;zTH%w4J#3XrS)Q z6nyISg=)7lFnWv$WMnvi)c7R$drR~V3LjpEtVa~We56>ZzqxR0HV1q5OTmRzUr2rX zerAlmADOGA$2Qcx74EDN;4&FweBQi_))!xap&{dVzit)OYKeg_Iff)~L^K{6?oOwh zOa#L#Zz%0w4voVDA%FK`I{#TZb~o6Ns%KMRl-Wso&Y+x}6>Uy{s^+)wK=2K!wo5?b z*U8W&<Y08e64>E44aQ4JgJ4b{8UD--9xf<^$BrFj9P0{o8kw+ShCVp8MB*&7GHNtl z4)bng!x;FB56ms`*vt&Lv2p|reJX`pHPh(1>MBxv=pA?B=y53MZUT}r3&+-L^P;=g zT*21+Dc#~b5BYh^VC+c1dAAlr?-pn9)%(kx54wn10a<XV!Vy#rM&aF&CRp`#K71N$ z4@-AN(uqT}sNNAJ*rVA&D#Q=Tx*jdkIZX}c8N^cACQa&4oQqQ`ev$Y+attR*hF@`C ziSPLNC|Obn=Q(!~IJH{gm&PPgEyWP0E)Ni#`$eB@MQU3Z14o<#aL2T4TstfXTD*4v zH(3`gJQ{GhzBh~~zHriXFFG$gPUT*kk`^s#F#8on%g-fI)7_J??`$y^&j^7FvxYGj zXY6Cb-{+Fr$0hW<k{RnD3?VDVT|t#XQ8pQwI~qbjmQ!>u=4Kw&0)uaHs4WvuO&Wrr zNN*u@Xc*(jicxfxQw%(Ql8e2Cg|zM5Vi>r{!PM#@xc*TNJz?`0UOB|WiVxFq{*C># z^lS}$8FQX^8m80J1ue8DVKcdN<qBvIO(#s;RP=cIg>E~q44I;TvxOGC4?oOI8>&xQ z!UB^KP*tW#PgHoLv1vL?3+$wy7D*F@a0}=;brAI5B{S#Zvd~ZM7?fB3V}iz{;ERce zNbb&s%+9z(I{aH5jGDa|)_AJo2GwGear^)(hxZY`F?!V7{uT}ITLcp<c9N>JBX!H= zAA=~An}dh#jWBrT6_Sy3i~DyVk38-T;6FOPp?hU+lD34CRC#d%JgjjhU2hJdj^i*o zEo40qP((pdG<R371~W(MLF%}F^!%*}T%c(hNVNshps~)xW#2Kdp6f$3m8PMo+C_LD zaRF?b3#hh}5Tjd5;B!fh2#Q`QU{abCr+Ok$m}K}AmfVnpAy#s@{OwwDA^av+XjlZ3 z!i0QNw=Rm)UEr9)E1DbSgu4e)P&AW3*OG;J(;yNTj=4xaM13T)WPONlUMzMtDMNer zL-M5W5d9Afu71?@p>hzc|EK+bFgP<<LoGAkVC=F@;VXEZfFit>zM5tFjo7C`H}+|) z4SU(gjCaRnyjHa~>$A>`_4?t#_FZ>lHy+%;D`r{o`?P1{+$d}Iki94G&j#^i`wad+ z(~Y)R);xC*WY5jq7_66RYdBl9)A)`++EHBVCLSU@+kZsRG_~YHp@kOb8Y(B8P_{!{ z(k#W5kGkfN7bYWiY4;>+O~-J$2NnrFbTx>-oH7=;7XK9Ndof>-7pN@!GW3*iWx&Nc znO1+{<ZmVpBeb6gab2HabZDipvbwwO^Xv@aX5T5oCueKKUv?fA8s+Q|iN!{r#kD(S z>wGrusoPKQ2(P`Wsk4q)D)9d9%S<qOBi0X67aC8hr0HH2VmJPyct^gx(0sN87isxT zn3k+3PM=pHj2i!0Tw-#s?%2ltV#|D9(6db_42e}0y9T6)Lmo;pk1kIY&#5^g4$}W4 z>|P;57j4NVqVa?TToEmx!?<76PwgVzyrHA+dCFqC;>#rT{+=M-D7!}}b)3TA;Ok7M zdIPC2pG50KNu+6J0kQWuOW$0Mg{}X#5_c0j$n={?WuBNo%#21lR_--Z=RO=}?KvO} zTj(cTSh<mTGA5r9nXM;#ThCI}r0I<JO;!Hj1P(k!zh>jnR%!I9wZglW3gjf-!H~{t zn4B7o*~;mBh~665*gJu5Dg1#JvJq^cdpj=v*h8MB0xC2b!-P>YG5U9lSgq_ns4Z!Q zF3T0d)PO&XmBUDOl#dLA$4a4pxCchOxq;QcFM!tig|Izj7aBJoz~u*K5m8c8B|l$2 z3_Sm(K=aav6f_)h+G7KBeo>C<ezTygU>RL<WGow?DaF5$E`Yt#Qhe3a0ko>F0Qd4m z{5|OfIPJtNwsGM{P&LZpmRBg?=HH`P?|CgSYgi(iP%?!zG*{$53FX*F#RahZkPcMs z3}P9#QamDeLIl2H(QH|1EjA`K;fAp@Ab8zvxO?g?Oxf&!j_(p7?Ro*surfm{CnI)w zxFMS?F&!J=F@FBQ67R+N!n+2;Ss#^W<Y##jK72GEU+A93Y{N|?DYO*tFYTxOMG^eG z?7NU%_Jve`o(PUTp%50LC0ud36W0hu&U9zDJbu<K;J1sWvOnC1@IPls<7|v#6T>dy zYv;u%Efve0<MSY1?FcT>34?bkDy&CM2{c=dLVLwdrY`LnxD0m&Df2bNVcQGbGEbFn zbvi<V)*pgJyI+FNhYFM$h@tuM3vq$cZSqhf3oNxKLCjYRST8zc$Gn>6z%F}|0aI?y zW1EA1z=&aU`T2!kLBh}!6{N=U6SfS%4&g%n`N~xMc*~l-+no=?UPe%B!!*=6xs0=# zqs$)i8qZJI{+U|Xs_-Kn#^CB#d&%qSZ>WDY0Ve7y-~!)%oSkjX^D^bA^5`@kGMfqB z&sI^9WMUxRe|!}zUP-eb{&j++mmJ@>V=6z%Xg9V!Q0A%oQ+j&a5<buTK9;@I;eVzj zvUaNP@bC$FHZ|uCdW}&NJiJW=iO*bwC-NEuJF2G$3%WxDmTTWQNQmkMF?k8%3vxw* z^=n3m9qS4h?FI|s+_;+#F?TN4K1jVFa@gqmw@y#Au<pR<y<(LMV}w`O8xC_F770(^ z(-Nqw%L<~^yz0PtTwTR_NpVMltnkI#LxTN*U+VUWO~lVudkAm8Q)IdaG=#C!#@0nV zEfsh9{}r6=9W9>Wy_`FCStK0sLQ{OdHl7TBe@Cd$rz6OG(N^~(_Nh?3sY0A~N|fZ# z=X<AaATeL)*%~fv{~IdY?q@Bq`7~Q>9W5hxB5^=yoJz(2-gyXzC-e(WJe1=4r<qVG znI2~E!olytn<Touw1%4KBvYvyK=y46qzfdvC_R0&ZvGM<lJV^sEVQ4)T>Y#}5(M6y z>V^ceSy_`j=-)5=_46x{6J1SY5(Jw`uD=95<~fFtNOh_oH;&BM{D{1Wt>VIcH*r4h zlf;tKx`pdM=HRx@@3>U&g81m}D)P<u7x`B`i{jW#40G9oN=(iqXLFB}rw#U`+_{+k z{nSqUk2Y{~HZ#m6oeuJlJ47?C>?Jp1Zxd~;vlx^#6Gf{Oy}6e20;Z^{0Dl$5(D~ct zg~y6*h~|^IB*!fQV#e-;RA(u^WtAUfZd?dfmm<hr^{aGgR5D7|By#(YJz{>$(E_J; zY7nZ}OX8DRL7KG`Q5A})^{JbrRRqMwpb(x8?lftlgj&JlX==MB%+h~?_3tD_R9DnU z&Bp$ql3l)J%z}AzYs3+BKX8IH%08z3)~yf`Bu2#(8rZJ=jk{W>$_b=eA-CETr>7U8 zNw)@OZXSG)?b4_dVGVDV&V|)iE0|GX=c&t17V_dp^6J~l=x_IMQWFz^1q<xS@dSCu z+`NP9k=N#JO+=0G%>Ei38P`ms45ZjiJ@0Az=&^(Q_?1w>#fR|`v@<I0`t<dr1+;qR z1KL`vBkVHVW8XB(2h7!{GegH#bF)lGK<4&LL3!j=(y(8d>f{HIT^5JAd)F>-66xyP zle;WgFg;Xw!}SEwKAFT_+Hjn^)i6=$G__NFS+wzcU7oQLuJ?S%#Ia6dwT!9M>XI3C z-8P1?Yd=UDaTL@n=QxWo^7Pq74LJMC3da6h%I!AaN99Irr1Pc~(dsfq{8&E}CGHq< zpN*DN<F|Vmzs7Vj*Yp9SX<g3@ueeEkCR<ZES!XUuV8H#WTPB{1BF5^i28ho07}L9B zGf9}Q7AgO?ogDi&m27F9fLT|w@#J)U(Asd2C?pN?NJNqsd*w5hCD*BSbQ>8G=nUig zO~5Y447O{IgtF4-Bx(E#x-4lDCdnS7K0^<1v8^|mktylqNzYet&ZJ=^+wd^O9ac1H zi!}yay3Y*sD~NF0@Lple;qjc|t=;0O9<Fq4L<46T5l864WUl(}5V~+64X!WR%~h4& zpj|_&h`XaPb$^*b<C0a#dH4Cu;qxo&G%Uu^nok+j=$RXpNKR+YAB-ofG@lAiT&odQ z&RED<->+iA&L(q7m7CGDP=?3`c`_0OGwBLDk+fL0@GK*f6-OtGaDWjcxb6Y=GON$+ zqxTF0ur4N?+kA5xmD~G(tm?Tz{v0!8;Ms1v?)W(FW$rEU+Uz=#Jn^HT_^<+VaMfY* z)zOyVdj+~AofF^K6-d_B4>0wq?woVmI2!#jnHjl#ozTW*GKt(8%4{3j%$ya))|1vN zVyuv4Y2EWB+=tO)VdmAzkoRvl;Tw0;oryAdz4JZo?wdnytqtb9^&Mg8+O|5QbY1+F zmq}!*zA*<s55c%3IXEn(K*q#wpjCC(sE+DHycYM0d26u_mN#9XN3>PN>*DVquDgZq z!3Rj{_GmIW@)i+!jM{+5$3#+(tM9l<{TM2p)<OI)OOX!kX!w#`Nvhx8<>G%@L1yw2 zqSH*d3%@g|V@onxb4M{vKNUx1{bUw>)!-idOvkfdO9V^TG&4W5rZU&JeIl9WWiT;E z3M3p>z_y2ZG$AMs<(<^%^jFoiV&M>SLvxu3@4n%Y3Kv5{vpGf^_`shnpXuU`gWO79 zn$9yi#2uZqf;2U)$IbmSY5e3rL`y%GOUz$DRJ^sZ?r<jA5&r{S4E5k;z7)7G(ZY9o z4{<NMHV|yeqMKb@_%&1w#yz_M9rryLQ@_c$VnQnV%S91ynTha`4|qbKhvq?w`cIl9 zyNU~aW(3W-@0pvgGr{J$3#fc~1ZMLxVSC_6bUv<tmG9=loNRZzC})G)WO9f_%rS(S zb8#TV6lBVB$pdnLIWW%;{)4rp57fjs4zlNtikUtdx|!XR+(LHydXkrFnY8<E2WE$S zqMMr*z^6}*5XI<1-(_Pwr2j*>{p<|pk?9)J@x_!+TvIqmNte=lE;;1o+hpSSzJXRQ z`$4*<=?n7}UN9Xl!%5%2k#x+qAX>a4iTr2Uu3PJ8`VK;T-~U4V>yvFo=I8IyM|PCW za;av`*QZmR`IJ0<cAIf9Zy+t2zI1VrB@8M0iw0JsdGB4BWF?K@Jq?`cX;)*o-Z#Z@ zNc5!o_xe2Aj{kutye~sf+d5Y6N;wwY{K=1uj%1HCxWjg$&u^Ii15;6lpPD7lKA4pU z&pWp8J0g@sZ1Co2MyJ`BT^^Cax0zX^{&R&v+AtA1v5BYzdkbUF{U&XCKdIfqkBr|4 zJNUXKho&~4Ckd{5$O>W%cPhfz$itTGlM9P5a-46ykL?q9(y$T37JX*_EI7w1j$6%- z4$q=hkp=9(%tk)fJ{|iGMG~=Ig;*zBG?mYvW5X)tOS3CasPjj(Q^9wgAxO|^d~vWo z{JpG5BG;<Hqid_-d`=R?4*a0`vo>)ZtL8Aif!4%jV;kY8CDVh;{bA>moVr!Yqv@eH z{(J`RVa0JNIQv_%<Asu8tX`@W4hbjxgym=1_vya8U-of!v|=QmDSC9AFKXM!hsqUT za{L>7m%Ip`m#@SDnIsr?T%K`$#bdj-79QGlkRMq-oL|u1g<a*pNYXtG-g3@Re%ur_ z&b?<3vu&jv&9D2$%(0e%#`GxSxb!W^=18+DRrk2Kwf6jP(;EJs>Nx(Q*$w_oLoPmv zFkn?Z_wb^f@9KH`fkUwWx--a~ljUm)6dVT(V&K||`_M3Z8M>9;VJ9hF#yP>aap*9f z4f_|3S8LDXsPL~~uuz5F=#_`tE>EH>k{nsbyX|mbrz%WvDI!G+2WiA+H+(;m(l7U` zxTHEe*0jZ$wNd&>Vkg?->?Nk``V21-I>r&!eCi=AwQZ*Ax0G>*fh4=zA`S1~-i!a} zP5f@^#lP`h$R9|FLBG+-WcR95jJ1+B`g|LLuS?I@<%dTQQ<Z~IXi$l1pvT8u6ksa6 z6Q+gQ;><O(sb$|S%I(<CIZgXbx1ak<PFKI8pYA@OW#1nV-_4>-(l#ra=}y~0&u03N zMf;ZWmk!Qyyx(t+(TB&^xA^P>Yl{|eH!tC(qI38c&Xeh4M#SEH)Ci^WHt<&K2JOt7 z!7eFlh9xUsa4Vl0utUd(pufB?o=&m=H+K(A&ff*shHKG1{hQF>dIFkua8&->L8f!J zHr9xk`=s@Nh}?UeO`0k@$d-m{!po*-$i4()l-ufd!Tin={NcXF@uQn-J+r2d-8#Gs z_qs(2h1_pUzVndw-&xCtX6|Hn?K%$1@1pU7+I>3KoUk4J9Cy4<4vnM6gID(+ax~^U zy=yp|wR8H37iknE3w9FUiRDD}qVN`(usWI!+to;$d>lc^aRDb;YmC{CXHwJFT-+m> zhQrKzne*@NlJ9Fe!E$T_@7Qd@uTcr+uP;5#CKp~J)vi-n`9B-^uf=vasz@6%6DRO? zZlN^hosh&_j-V=rYcbLaNx_CbGH!@5R5dZ2i*Y5s+}<z3xiLG@<@R0JxHXGAH>;9{ zB^L@##qZ)QUkb^R*tw*{M3MPhQcJ&octT{3NMe4IG8SbVrEwdV^0DbGAF$>at5+md z-x6aAsbN*z?tw;r_XcbJR7oIO{f&dZ;wAjZJNc}cbtO!IdhYxCYp7sf%}&utCyOqL zjG*vACR@WD;hL{($K+)qR_s>;4=0$>rJA>i(ZV(yue<@5ee<9%c2B{C%az>EpNeGt z?X<cnRd0kZPtSxny*=E3)L85oaga!RnnP(t5kJbnr@m<*ja6)$!P_4Ug*T3R?9SZz zys>jU{T;F#NcnF1bW}JD5#5=Mcdy37hvQFh$mt-+N!cP66f5xub|d(AC~(&4r-|#t zy;xg%1EwzBPi%GM1vbU+=up`kOu^Aea)8U=+<z&M>2Gfloc@f;YvjR?hx5U-Vg>Ec zb>XG=w({q){!qah0bki`%$uy9gtc}tICt&|es-}2EAl+v4KmHY`729TICfc#XJ<cM z1)qcMF_TqyT>rWn6Nj^W>--z6La8JFb(uV#vDAXawGYL9D_&!F`g=M$2QhT$UaBf- zf%jIjSjEm`6355UpR_@k^Zf)}b1|NAn=qSJHe94@12y?%ZDW4fO_slRxxHRm>4k{R zZ_*&#pwiO*<1Bws9L2w?kzq}`E^~deQt*7nFGvmAMprzZhtuwqqXvw?Iji<#MoS^e zF|zcY=OH{RJ)ggF_zDdWEFweD9QKF$vt5(-u>;2w=^>Xt#8#fel}{Qmx=j_L)=NW{ zrY2T;Wn(cX77uF>WwD1`j^QFrRcbP3lp~wFf;Ii7N(>KQ00WC(^!=H7uv9K2uM>?N zjmZg!^Vo|{FP4M2gXjDd{aOD@2iY^T1?<hy<Jp1-XW7u_CT!${*;pApm%fl0O0Taz zM=kEnqv;D~a{+ydIB|vsC-0I@tLO2|QynLGA+nMtJIXDwNwb^ym)k+!+yWSR=^pzf zdm|g&smyAXU+0%;E7xo9al#GO=h>>8sqC~Z6F{3baHc2z@mV5Ge*UrJ^t@gktl65v z%PPjBru|WrSn0+a92vn5Sl?g_^kdPNt;FuW3>aB=h5H`niV-uN7}f9Z#G*PCC3Ipw zlHLnT==IH-creuzBJ59)abDZV1*v@Mb_`f-Xk+z?V_<_!I??^KlC9l3fNm%Du^&Ed zVr?BMbmb6~JL16RX*}dNeO1Tv{kiDjGmlR0eay=}Y=qi#C*WO0B_A=p06LsXKx^^` zTyAs)--p~1aRZx-z$ncGq-vyL`TfaoK`WOU8{HzUm3Ii=)hKRrpU+KL6AVQecVMmj zc+kdDG-*jD$CsR>_VXR<-DXLlY4Ta{tn%U#dLO{?U=6bB?i}`Ooh3h3(uUtQb2aAu z8&9)eSChPvm8i017=PqrKb4y{D7xB-TA(*;9SobYi0<0vO^+S^gh6@LWYr-L_TXcC z)}51wi(U)?o+q!j=99L=&*)<19aL}aFMNCJ4%Wm<3*Wt(NR38Mf;(%C22oEWF9dZ* zKkYR5Q?LUHMwpOqbF<-8_DA}BLL&Gmn)73{GWq3#B-Z}HU2G5~u3(##dof$?C~e`K zL1VcD|138Sr`PoizNra>uQp&k!c;JP&m8h^j6WPXpbI_0!*JBJYN~bg1G#W8nY1*k zleCZa^nBWgK`KuHl|GiB;(i6Tk~d)EgL<j{Rm=LVYztAXSjtMQlw-Tz%2C}U8+Q4I z+eGA~yM}+CGFVBv-bSzCk7>)M(|ArpNE;s5k=KQL`D<QP!XrNEoSf=*d^q_LO5Cty zhIDIVoU53L>^MbtXeDzdwiC$FB2(D@Lkc=nq+xrXH~loMncfj+;hQ5Z<S^X}hK9P# zQ!iDt?230hHj@MG+(Z0tD_IeIY8l55PuJiZ^DJ>(!bo;Z{tSKulgAXyAE4=<q-pxK zEEG4G;+D>8m^sUXpHgrQ?iY@Qu#L$yMs5#iZyO-pZkvQj&`&#c_u?#>#dPw_sgU+@ z3mpVPq^D!hn2|6Nwgqc&!;kKUFB>L7)UzVAy?F>M!bKDK-N$Mjv-Ov-Ps%^B9#&1* z5)+M;smth&&xWkuy2D7)cfvCH6A<LI9Di<=1ed;492wU|HVzpiL_<bEd>GK0udU2U ziDHuGw4XMN+(jIgOvRw)c--jcjN@dEQ{(mi<jhA$c;;#V&Ksv;a-TKc@5turL{hdw zJ|UfW#(u)CeMPkC_(6zZhv08@DSn4iK3kcwjXQbO7;AyT)d}r`+s2-MJ9-s+UacD* zZ@0rcdzNEluO}<2+>52|Li`ytlJ(yj1G;zg7{!t<a<cypoj$Rb2KK0<mzgBb8Ef)C zJLTZ9+g9>RUz&(a9v-KjAI_1sW!m68z7A|e1Mtk~J-wKs$Igm4%D$FW=ZDoCX8rzr zhxum?Fo)M!v1b(H*)T;-e7UttSUF@FtA1Fverf3#e$luMxYcqb4Yq6N8?}^}O+ALt z*?We6Y;y~`UkRX8r5v@DzF}c)47|PN!wg9qDq=rJsj$9r<KdFpNLX?0C@Rc7hhg*7 zx$Y}2V3*>+`Z);L&;%FI%1(y3>ju2C(GeO|XXhAja}xWY(v?PZl|jQ>8GfDf7@nK) z3iV@Cz-)Ls+wyrhJEfq91b;JS*aI^mt3QjjeLqK|tIEmj8Yu{gTL5OPXgyndDTq0@ zM-fYNv@k8MiCQiAPE}Lfg}X-Wr&paH(!UnU<j==IqSm1XodJpXevCKz);%W`kvyzw zJ%HoCEXJRXi}1&sk@df9Gyu{zvXdWAhMRy;+ndP7WM>0^Ihy@&+yM;z6|hjD9aZMH zL)os2XfHY}$p(7Fu~SZsW3T;5MK}99IL=`ol<nMvdcC@!>l6tN0dKhVCl^qOWv#^1 zD~2q3r-vTngJ_<MF{oHh!AH`D)Kn#hhR#vNpEpc}4;uS%+OUQ7+Ebg^^;g4q$s=(D z6FsqWW<E4t9?Q=U`bghSOy|XG5Je+{)>0WZpB$a2$lC-A<FkHx4i>tu{F@6=u&de$ z%nw)K*;Q{DpC2QLNxUlN_8!4r!EyZVG?eJNPJ+>X$4TNNZ*t`nL+1oE)4NT{aP5O3 zjI-rQxqk&K^-@fOJOwz**b##dQP$R3#nF~|$L`x0z_!{LiTFsra-m*kH``+~mM3F! z$Y*nP_KWT{C|h+F=f7OXMy%V7Q^yDM7avE0b)OkgRQrLOZL%T8)`BhnDgpg`7kxjj zfp(ry1TJg=)0HU2xNN?`tzD@~tg2GUs3TR3=C;Y;+I^WQpDd*gwyk`wrW5WJZLEJ) zEsCk{3^~G^Zpp#kv2XZqONyW=ET4K_p22f_HnaT(-$8%>ajsXonGRIs!Jh0Enx$Vz zw#Un{G64s%Eb%<)ky%9I%tK*g-a0%w+!*2~vFNRmf>v(vM0dIZ4DD5b^d(xfVoMV_ z-WkowJy#WezI%@z$-Pati~Q5+c1DVqQ*CDVsjXw*zaQh6lat1)3EmMIB>^tpeS&RU zcYvr|U&<a=`$$8N%)w?$PgwQu2f6ZmEWhi*0R50y1NxU7!8bPv{$xZ^jaQ+xSNb3{ z+<H!rTiVlalgF@b^;6j2VtbtIIsuEMC|SX0(A3ZUjL72zgSBQg=xbYu8b(b_!1%3X z>AYp^?oEB*uCb@S$1>Eh$a^AhyGI#agW^cYIU&1tUnV*3^N}q%`<j3AA(n)9or4pn z<=M^3hxv_-BjD4FBJ@&D=3l+tPgmWR!nEr)e24sQw*6He9xS^+zA&Le2Re)i+>kFK zwDBy{&|-%FxITLQQ2~)ze3*k_SI{`V3X6V^1Sx?8f2}`@z3-(09WFAC;kXpTr%3W4 z`To51`*K$Cz9K8no#B^&4?oxA3O}T8G_HBN4}E>gaFdk~hy2rEe|$_J=aN|7%r_7g zIoUyMdoFJx%)oy2;gA2zi=;ZT0WXG2NW2*SUphvCwG}mJYf;hG#rXEaFsK~)h04A- zg-W$U#p=y3NqNdb61Z(YbNYiG9FN>a`u9gtnUiHG{c;uryWSG^re365MWZ0UXBq7- z*~7T`YSIn1nGE}2J#|oz!YHtTFq;wRwQLS73xC4h&aR}}G=2$k)=2)B9cpfulbn%R z&pL{B8eW11fj6&kSBG6O+7>=KT|tvkOEK!{UD_07!}vBzvbWw5&iwLDOd2P_#`i}H z)g8B^@=ASXcfJ{$Vw=jbN{8`v$Z<Sbngnj9jp*gkNGu0Jz;BNND#p!%w|B~+O{Wb1 z^eZygpDV$RPHR>;eH$^mKTU*Fg7k3g{Y!A=W;Xk`>Nl)9Gm~F6?=&%;d=xgN=-~av zJJ|K~+u&fsFe9Liq`#TV9xInX#j5kjw9Tfkj#QF&*ZuixZ%Rn?Zd=}vkK}FlWuc0* zH!JEMAccJztksfbcvtWRr<wo6IFAwh@l!k46ets~oFdxGYRgaNf2ugbd0A<G_s<6~ zE!c>^D6hxf8E(l}9D7K9%+o-_k0%^7p7#=)t@DZQ#9&q~IE&u9s>TWjv~cmy1`=^p zn=fDGOnuGn!reqSxcY4lNjYT4KQ+%orLC@FLBnDiGjGdz+YVsfDgC+dP-#+~>Ee9x zJ<$?%vE06L;Tob@Hz7lXc~fK{&N!AVtY~Yfdt^Ga?(Bg#!ez4y1t;&m5f^+vW<M%@ zi1>{2A}%v0ni-gQPaL&NM?602rr=g&r=Yd!4AVJXP1t^Hl`v{sreOc=!n!71J%Nqz zNUcHng}P*?A>z%~>;&P5P6|D*7Yaqvn>kVU({hJ>eg1;QLu15Ae^-gm%)3!H)qbF^ zq(@ym|7nnT%bxjlmz68+>(X3>%XD3tx{LQ~z0c2J9w@5_4KgYnlzJNKQe!4@d1>tq zJD!Xe%B}t`PRyTP*U}#+E_(ly8yNkU3<SSsl+-f0f>Fwh=Df|s`&1^(3Y>`EHfbWF znp8`(=bMv7&tsVrekx?qwg%?LM*~g<V)2{*Fe)tG%-MS8<Mbzb@KO0P{xv%!oSc*{ z&e-*Yw8bFAYv?dlhjws7g_5Lu#woJ+q>Nx{hNn>H;~*3N2gJ%l?$pijyecjdz7u+- zKjSi*v;^(Gg-omcYq3#+XoomczgfKS)M()}>p+I}YY`VNwh^+ve!>kIj|B5mw=k*| z!%2UgED82?6$}Jd3u7u`#l?~`%;jD=X5*Wgj2`d6sWiVA`>hNUr(_li4pWyp+cUg4 zGMBIA3~j_ygRYAohgfjIa+`&cu3>`dyX=IIL<@vI|7z-fistSW4;))7Oq^O%J6$JB zxa$69Vg1gOy3|9pb&`!a0{AJ<#M|7h{jK;*(5vPrR*TXQPmx+GR9PVwd~2R0*0_F6 z{N2w(I5l=JUH#$**%GhHq(3j@xG9;!&ZbHt{^tQ_%svS?Itu3dQu@onhpf3~CdN^p z$;`Wph$uwk1k;*xk>bc`dU28*bKT1f#~%y;x3qYyzkh@~vxXsK3rooP&kx{B%Q)ip zH<LN<wuL16Ws>{r)VSyDIS4TJrPey~Y|EU3a6?m>*FSIw`nHurd6Nky2NaQSEzz9W zBTY=-ZO&#d*5usW1K_*$C>;I0Tm-p&9o(#;di?co^PpT!o>)o@$B0o+=*jULAoANw zj2Y_>DJccGcxgASEqn}H$N9s;KSt~rG7i4hP-raLg`e&}1dFwitoFVp{&CEpxo*f9 zc$j7cdtY2-P8!@rR}EkTPaVKH-wLpM_FBk1e1eO3R!Tn26g7x5FE(MkkqXLZdXch^ ziu~HEseEuBi^W><sC(x$dHcSAu8idnwFn=jU56_YJz3qq-_h)O3QToj`3}vA{JQ5E zFkyo&tN8mCSkIhKG<`FnKG>M-ReT4LbtAC-+EBh;{yplk@3`lEd(iT=EN|^LgjE(@ z@c<*)r=+vcmCb31M(v&P(B|Vrwlqfa(iIYH#XbrCjzT!5p48<(^@zl#%R?Au!cXC| z=yY!SiZfiXf-@H%be=BA%I5~6j}yTNZD_R05ULm4V``gwg_m0wk^>bE!e6UpsjJ*7 zagYC9Cgr6fX3Y9azx+|8qPs4xT+P9G%*_F9Mqa^z%>5)OJlOY$D2+(uJbN9umP0o= zw*x1cw+B!-+o@38m(Wxf68VXHFr$k;xLier9p$uVk~-b^UJBMdAI}{STQg4#rqPAl zPjj~{XE56~`qJ-v|1jC1OYxfhP2o535&C48J>950jBJm1A>vB2(@A*7u0djH9Gx@E zin^|_M$^An#QA~?;;S#0(7TtT=yw~N+TXK+xqT9$<mv3eVtGXhY!^;|Lg8X=uX!TA z)CplO#t)-?vTFzkX22+!JzUhE2BJQ-mhMrE2b11sWO`i<-7Qi<eH~M}yJrsBk~f{s zXGI@r-t$~imJtBGMl+y-RVR*5%Y-_b@{GKzHTk(kg|TS60;hB?F+TrJ(X{)fxUEbH zqR-pmSls}sKX4e&s(&J1b}q!g&T&wmbs0uJ*+E>Nd=O8K?IgRL?C7SPm!!ZY7WMaN z^P%pZ+|+H9?pD7;WUsn&6GaDX=!chX<mSO7j7|wBB{TfUh5G?`UGXcocJy(49lj58 za$Ft669+Bkp}G+5Bu6`4WO0*bGg<%U5|e46#83Arq1V-HsaoV+s(YC-WJ<VLP^!wb zZ;cZ_n9mE9Q|8i-bJjumsCal47z<giEa*ayaX2<}7>E|fWs^qnTH#OcDL5j!j#y5< zMvtt^hm@gX`5{Rk$&XuOxY_$6==_&nq)2v9_FWS~D&E|oj<c?TgyeLhqU(mf7Pa6v zZ7Qx+af2;Sv$1Z?C|oj14U#vgvPWHtFu~3rN2seX9k%K;D`y|sHKz_EjE~ZYoOBU2 z9d@6KTUty~F8v_KS3ZG;C`ECzpB&O`KODMoF<pD(Bt2y@o$_z9usk`DNEiF!@fLHi zP&@<+%nh-)aV3q4H;0cJQZPZB$(?e~qo#dB`H&@(nZ2EZHe^W_T)fVZ?CuwoH4tD) zUI7M;`zdUa8;Oa$=m6(D=n|Kj*2LsEm!nT;D%oY*f)Uy!RNGFI_U4bp>yJN^0Lx9Z z#cKqL<)-7VagNj=p`J!~4^XL7wy<UHIF$0pAf8S6+=@*#(57BP(_h43nxejNKxR0K zlxE|byJK)%#$M!dhSRpRg}}9%*BIB=(Ho+fX?XU-Z`h=?mnH}AhgYKS5Hr*U`uDBl z!fvJz<9tc7AoD3|Pqhd0<u%k(dJbOkQUj9yiTkv43cW8)aJ5hc)W>|I6`d4bB%TCj z*&o`K6-}mJs)Ms{o)NkJzwjttmA-coh_B6PX3DQ6*H*0b61JUH<wR?Pj1d3wWS;hY z($lq+Nw1G4{&C4%yt)RFH!c;wvN}bpk{HH&fdRSam%|lJY~e<pSd833Bed!glQR#l z)VbVU%*ZTx#kqaSCQ9u+jJmNTxw1H2=o+$sS+wy}-GV4J>how3F^jFBL*tHfkISwy zD@GCweZNygrcB&Ktlcw7f3^Z0w^km?o*W^H`}Z-mzke}b<%R?IB$)Jg=`wNEBd|cr zl_XlOA6$XG^iR(tCidZZCVJ-_dNA4u7ki)LN)(%EzHu{S{k@QDdw7^J^Vm$<lh)Df zB1=%yj3F+$;WXid68!B~0I#G^bsNnB7Gh?LA^mhB8h1L+#PN%hX<u0aBQ9N!cHgGs zL`x_7d{{Th-@cq0tO<m`k^~YVvy~e^=MwjeeM1i9J|RiFLdgM}W3Zo0K`Z5h#CF1( z|C%dTEgkXle<`qQ=eULFtQ(H90ru3hVmC;tK4!ShwH$x<7+zX5lomb><T|<(_%Brz zWZ92d$Xwe7;-*&8NHaK%uS(?7<EKp8KOJ<E^`jAAkUBKyQknU&xMt{QW}Vhi?skX{ z_a6l|=hD%D|5t$>ojG@x$`H%xP6nB28it|(1ra}O&}oxtJC$8>CW0!h8_wRdF9641 zGx(CQWWH8DAHHPoX1o7vWiN>xSjo}4{A4iWNvIj>OF6PO=~MWnWf6Q5j^w-Y=d#(- zRp9UVmtJd#;B7+G_zmL@K*7AZ?9mxL_|N1Y5xj20A6gV6PgbJj>dzvO9{vZGC=chO znm^+UH3k043Q6|GRW)9)%LwE{2Cte(oAT4O3_$b9Dfm#X#!qbQ#DqeYogJpbPy3<4 ze++tve>a5Syc-u_$yP&F(@TxLdaV%DzpAp@lb>PHnWq1iH{+}FHgpWC(En-w%hwti z?4-SImloIr4-%;hT=754bvk!*zS3Zt(x6~HI`dcXOi_BrS*o8k8#?rLLH#zzEkAgk zE~?u_p6+_hY5QrBtL3k0Sg01r$Lix(w~OTHsbqH5nh#+6ct4xymFXBbu8$=T5_$Di zW7xS<)}Z9D?~qV%ii~+)%&K{K(4V`Sz_#NHF%YM~>p9PH*S^#2xrGGJevBtJ++z_q z#I)kI@FzGXWjFl#G86144uk!>7<8~Ur&r&MB+bvSk~1;Ubke2Oc<|`{x{)4(2T#pK z=6F~Gb9A}{m5z$#%Vcx;xfg@+<y57qpX~mybvG~aCud&6s%rJYI|4`GaFQQiR<xIY zQE%rskdp%Y``WQ%?jQ1{Q}i3&nav~1&qk4*cjCw%b`os3k%&Lmi{MHZN5>`=!#IN; zGSsPp1fD>u)|f%sRd-Ql{3kAZ{Zx9n_Y?PY@Ssv&`ixxf*Mo8069)}-<Jo287T!L- zht0S*#<9Lx*75WSC$?g>9$PAmL7n9;tjn#_&@?n1ibP+&!NN&9al>gRY*>{Is_8|b zc~XW|^O=p#-G*4g+CcDx9n7-HFTlU+7Np3fl8&S;^o)rNK3G2vzs(lVvnN;6mAm(l ztjW{q&I7LGoP*||75x=w{%bvPGgJnn$pW^zwuCh;4n*>2S$&M;VaJP{JUhO84=a-R zV!|p7vFEQDW<W>EAi*@Njz2xu48!AAvJ;PFq5P!BDA}_LHVjnL)omrXJyw}j`_xFJ z_f?`@lox9_`WvqKdV+K`+rmk^vt;LtAGAo#gTx<NL8k4HqxZ^GXia`OtwAfS2r8#~ zr<LHmP>a{?Tg8uDcvi$GoGEdf@mHE(mU|v7xZ|`i`4zTb`OaTnk0kq(0vo+Rk?-b8 zp!fJtzI3h~+xx-;oIWt@qj#(Mj0e|YV|zC|m{fyn_iDq8FLHeFxG`{eYz8gJAf&_D zj%*8-gAFgI5x<L4bZD0yEbVdP>Mb0pWB69Of4XQANFMk~-&-cK*sjD^t^bS#)850i zLpqLgOOCUVLHpQjs|k35K8Bm!W_S#DGNpT)fmbO7ud6a3S6xh{P3OUsJ3pZDlm*r% z9DwTVT`)~ilU^^4hx^ihi2qVU8fThJE1tjQGR{7xyMpVu^pgF;#Ox0GQ&e}K<2D#V z_NilJ%d0UoVdETTf`X7sNxw#I|7_sny3*j$DD`@*Okr;vZKlRK{ototg1N0#VD(ap zq$^B<^kD<oQaKImC-iZPBhS#!<}%PeVGCs55aANrv0&i07rv?w5uBQp53f9P#QLcR z>7T?QaOmcsGkePeJ@~p@8SjjaCJrAv2|rT_J}cd#r#(6dr~aC&oWs#Se<z{CG)sQ* zE=k9@dE5ETt47wRNsi=S=g9JURZi%lw3+>|S{ZOt09M!Q@?)H<=>*#`d~ohwcqT8) zXXG5?#%{{wTc4eRe;>}^fw%K`FOeyG?`WWi#=JZUFIxrtBIW{_V{?qm{l1(w-Hu}V z1E<mJ{a3i3c3O1(hh=0CC()^OMs)R(uO#8XDQ1hzXPUh!ohU5KW*wcI`8b2f`uneR z*<<Q4P?T0hei(Z|ezCdGb%#1i$7}Ktk!@nV{T3)?ai7$m8^v|#h>7bCkrB4;+DK+l zBjz=GoHX|-lIvCVBpqhq`8(HW7ipp&O#G-jD+z~lTS@eXUv%`z!POWv773)S;dgPr zaCrMCI{nE@`gZ$CL8|94QuRTfFSNq?{#RbCX_^neto=SdX-^aD{;D9-KFe@^_jp!m zQy~(MblflM96@?EpJmSFpQRW1&t!PkT+T$g44DR55^isatJM?GUgbCab4(Z3J}4%q zJ$3QEg$llPcttLh{vaPN+o1WzU|PD*onbHTp_UJ1pv6dystWH@=_wD;YfA#Vz-upP zUcOcDKl29v#q$j88MuK@w~u05!v{U7rNuj8uY?Y7vvDqNH+aLvY_}!Dg4;--$r6k^ z_6>iII0DPBSuzp|!}*Sj`tW&w8<wBRB90%V@mG5+`C}%J4|`XW>$YvQv-l=ySe{S( z|45?y<}vj2R4J_ZEyeAOTSM+d{S>$Bze2O8!EAl~0bW*iyJK;>tB4wJ-okG2d5Z<g zU4Y&X(N3sEl|577v`aU%mTUx{%q-ZI>x6n<N9e@-T>SUC5v=|5;ltv1viP3~xjn6o zdt=r@yT0;_!+Ux1c2XfN>DQs3muiyN_D85j)kAL4m>zD#?q4J<?^kW4{&Wx?xkEa7 z))2KNqILM%WDgEectWK5Imh&G{e0ziSGsxidaTKH#UEE2xE}?bFfej8*_@{Wip9f$ zY>MGRV&@a%CF@~bgD$7_Jd3+nyc^n*?~;}?D(Lw{7jD?DBe9`<Oso;*4)(AhyL&ds zKXs?~-tD82t0TqhzHA0pA0g=!Em=X2OdSCS2gORwDldAy)tZ#n`0(M<seGrAJAdg{ z7QeQz1cvuXvnOP4qLEe0pc&2rEDp}+R!){-OZG7kV`KvXFU|4KnN+Cg3?f<Y=1{g` zfNbV6VD0<Abf8KORPCZEH+W%d&r1+XU2~>WPwyo^<m#zNs-vG?**}Tw6n`WW%s$iF zt+Tnp*_L3l^e1=qqdcv;WX1#q%|f?&XFj&<8L>aVk6pOVoqzB5i5f|-qk8cdap_4R z<exXffmmbKC3-i}(PD_%`N?Ru(i;xUNh14InuV$ny10Hu90}Z-Oa@HG!Nc1Yw0&Qx zi1E;o!nI?*klBY-siE2|RFyKvw{t_GQS$)3Z?cz$D+}?uNdoPCV8luNE+%`5*YR!@ z2ibK4Ls<#CdG&`xU99bfm-zIbDQ`8Y8~7!95Y{Hm4;X5}Qq@@0ab1R`{Vw>*CJH1E z$?$Qp325c9iZLus!^xVWaoD5vj(gKpgOfsDk=E92c$1q2y=R_LbD5(wf6@YyG}#;~ zW@@ARQ!^6e9U@fFenH;GSz%AzU*_f4L~`$o6|Pf##I2lCk29AB@Z6w+;=B7aZ~C(v z-)W=}p?VCi?mCF+w=D66)*62Nt_9SwJ%^qNJ4{PNI)r*YR^aP4-hx-*cHkfLhx%Mx z1W!D@iPiZM{HQpKOaH!+nI3@5l>7uDtD436<yzx2y>(>E&M4-lD4nY~`;jbClEA#T zl#_J==4yE*_!%qmHFu2w>ksoQQwLAQbQ|{F=rWLco{xbyMuE-eNR*v)1x0hyH2E5} zalBRZA}F6@O-{tCVU3JG?RonDwDl#>RDFNnWF8_iBy$=hilT7O{+?S>NTrD8R4N)( zC=G6sF+;hO%tMAkR79M;50VTeR2qa-B1w~qQ19*ctmi+x?|as|>)gH8cb{{|bN2pB zTMpkwhgn+=MF(puve(k0V6ky6QR~*FJIr)R#J(HIwOx#q`AC!R76KpJ8)3p+y@ZAj z+`%IO-N?I{1CCSAp@@-Qbb9_g)?wuZ`f$!pWFd2?!au~aqCEc(J<wdkBi>rd^!!5@ zovleHO1>_vsPsho+B-jT*GhtAzJ0?wzD0Pp-A>k~mPI97vzbE{FNw*c%L3zN9QF1| z0Xpd@%vKL)Q^`&oT%cD%J;|<Rc;glEi(9f}$E+AEsnNla^gfU8E1abcX|)Rs7Lvrj z=pe*>GA8|-c&F&NPx0J6SJmhZaFeZ?9s+I72_({?hZ<g3k9VYAB8MCD;8@l#<Rjxm z%4M&hev$!Q2fqvGI1h5x>@bq!zQ@n%da<(aT(~Jb4mL}a;vZO-JXe&(@inFBi$ES6 z@7#*(^mC|dBL#T&mx;u^V?G{i&OlFiIhL?+>NujuJ4mKXHD!-yNYTDOZnLAxnBHd8 zgbXiBpi}z{h}TFFerz)b!rOxxv0WSBMACUArJjXel-A)dk9e^0Nf->=&?Y>m6z~j* zLW@Ou%X|OyqVpC~)T&?e(2{xya-mQaq@F33-@d(^xPN|x0=xy9^v{co;F5;|M>+Ba z<F(DKe23g!>U>=tHrk-enp&)3^X`|y*90F{<7p4ul#ojAU$~YXjOP=Bdwgsx;XrJ1 zEs@F|1-5A5IMI=JCZWRxl)I1wjQ=Hq!z8aF&zmRlbm?>DrG}ePeMSdT%#J2T!|Pz7 z@EA46HJ``)<Sc-@C&VC%$0d>ruHly(&7o)764IT!4YizUK+C%QsZYvf*zYS%2T2KZ zq(h4A{D$N7R<ABv*y=MoXSF3=T%1c%W7^=YEuXdc(FM_7r)b+(Vc=)GA40yq#UyJo z;OVV!e8F@)A^8r(2usseO_9XyA@36G&|eI8`+g#slW&kLdWzpIT?OQ6C8{evhN2n| zK#$iRSXMTICH!LX_-$Ko{njFUm{G=cFUPS8!s}S^&?s8tYbCunT%G&=`8j&!lS4$F zY9!vXrb73}+rXPS2_A=}VeeUL)cfRka$=t)ikdxyoOw5?b-cs~vYgREAC<b$n5!#p zYB>*4N$O<U^Qq9FC`U?5<nhPG!_4QEiD;2PeDtI*jH9~4NuWv=h55_N%1u)qqK<Xd zXalE&p&V`5#(QV*bM9r9cP4|L5Iv=$ZI2;cJ1D`LSS6wp;fv7)Lw)+)u@Z7wZkYT@ zQXo8$KQZLf6<ts*$wNce%9&5<Q;_+v5DpX7Aj0cpfiu&A+WsShcrBd_tzE+S=WH1= zzv(&Z5lE!N%wAFT=PilW>q?yTwv-C8?L}6KeYlCbhMnuV$kfM*y`tuYrL@nmmt&pD z`=6fR8-5*bgnuO#=cbXNMp+&S9GQxxPKiVMuUcxFMKYzb?L7sDn((oK$<&J1D~YQ1 zess}h6Rs;uLy_aWh{TEtrul~tS}|IL#fPWEk1gkz9-rHsB^*6)Ec}DjWhv<7AargT zi!~@7&QdplZ|+8nh2=De!<wvNtRdbrqE07hdQ?Ok@s81V3+~VkpCie!UB=8z)zyOG z)p`i1*CG3RUD^Fr_b6l&L;jpGhyIK#^xI@UYB%SQPu=3Ah08^rI((9mJC!N9dl_Yy zUcvRd3z7JdAsnw^K#V$rQ2O>jT=ia>QIvmyE3L&~*`uYXo9~S7O1{R+7-tya(Q`=n z!3&V6IR}pyTg~-6K8|LcV`=vfT+3Grccc47s^q=&cBss&#YbghV8Q`?vQ9w@zFJ8O zY=e)OQ+J=ELuzuw*WG~Zi=^;)-A!;HegaWcO~5Ta5vb<mc(A(Mgbr`|i9d+Q!@Dz@ zAijGYS)5vinxmRHylBN9l=WMk%*a`fQaq}V!czt(Pddx?<zA$-zFKn!&zNy<nf!(? zHP^@@TXnWZTAP-UOdw^8+rZ?41byCmG5R|9JZVU`W45f{1i^XYbjXQ^#NIA~S-0LB z0$l>hxv9BuKuncTf`8eD-0v6;&&ESjvT(iRZXP;Hwd0|E8;Qs2LB?~W7LQNXgY0@5 zT|A>kxDf~(OE%KoCa2iLZ_U`fS6>t1hNpB9uZ4bSz8UTZM-a!6ZETCjDSFv-VYq0t zkM67c1!vZm0d9!IXG(rUFZ@P}PUcc+I!*BHKn}diJww_A02aNN5y-?Q%%uO~BB9@t z%Kt@JYYA{9Q{FLeS@!XZg@o_G2TuHFe9A_fz8TkpJ9oHHXO=IdZ(ZS|TBj43vq6+} zb-R#r*9Wk{s2N#tOB1~+Nkv3j2#@sNg>4zB@P1MN(s{iTwXQ#dJl@|$*+bI!gGL?R z8vKKC`P70uV-&E$!bm*s(xQK08l9?x!X43mT;7G8@p#6x<7NJxkI;mvG6FoJjq1`# zBUgq);a%`GNS<|!q?%p^`wdwr*5nm@*Hwp{RnO2$(eZ@W6$X77?dXw5C|U9RI1~Iq znlw18ftrLkxD+lSjeXguOU53g)h}}#i_gN=(QKGvYexURDTvb0>VulbQ67k`NFZbH zK4CSrhj3|iK5Dvl7qS@{x>x-(sytT(#bpNICKAs6xn+u5_Nv3#&MKnmeu!F}YC`{H zB}j{a5<Y5vfLLp7#4aQaoP~2>Sz$0PzPOHPoC-vo@s~ivW)7PY6-o?kPeALDrG$;l zB__fZMCKhYj()YIm2?F~gQu$#+2JfiU+NqpoRe*oivO8X_vO8eIp2tBC>TNU3-`gK zf`yRbWQ1Szq(fU%Dr((fK`u)zN2xQ+NuSCjNLf<Pbj6O~do5km1l>sH&F_gsBRhz! zMU9khr3gu)l?9gEMyj(`l=Pm>Lj}AG-$_LGRErJE?BLduJdBk+U|!$$+j035!D7oP zPHk}+`KjHAcI*!Uy>FvvOgD#IaLOb$Ruf6L#t79jqX22Xm4xpBkr26+CS6}7X$@yl zIFS}eG@gy1MpZq0zp;<(eOgS}&H91kCPtE(^ZHQP`2dozQ;6nyHJP&8h$QU4a}zWN z9HIVmjUX;%BauIS9*e~30qVKQynUYv^A8!2Bo!IfO=<?tZa81Af9E$oI$NIJu`8au z{<(s9n6k`;CygNaIg`w?G$%59uaG&b!$G020n+u|P_p3@#P{DxdjzIIhU`gr>1#~J ziS6T&e!UvBhoOkUi4a)gF%?@2l4<g7-=SXJuVk%c5#pE>z#PLPq;S!0B$nBS`1{V1 zR^D<L)d|6I&C=vrs|1X-EG3Ocs!;g%3t*uC0N!s?rV6GOFjMZxBlY>3#8P+*N+IjX zsx`Bqy!rtQe%B!0ufD_SI9?}Zm^uvb{s1=rp-Cbt%(3fIDYP_C0?!;sBIac&7<X;K z*-FtUyv7P9u^WlduoyjRR!dcoNJy$YAkaC)L-lA01Wuj?tw+<Ky1AIT?c|4(Th8N< zVkh8KegcaJCh&cW3avWF6wjU*hGJF*TH@)OSX*B5Wh8pxI*3S0un8w+$r8N{=vU=c z=6bXeyi{4p>ZAxv4;Syl`LFq8hhGR7TAv5==LwXrUk=3kJCHvweL;Vx0ljR77&~gl zAtRj0Y~o`Ba8Au8&vTkd(Q?4KraR!`Tqmqrz8Y5eUIMeo!zkHLgyihhAw0BEidY`q zO}Ut+!D7{Iu%fvRs-H|Dp{LK1>C)Nw?v;f^WLq5BX0#h(K5D`xjX*p!PXSuKrjxar zS=9J!ITE;W103&~MU_<D#%KB?kcL7gsr9~t+G4iDm-)Y#BFO?Mo{)-+T$jRyRuh=H z>Kp!E_5)p5Bf_I)#7}~?VJ6wDE0BwOU&KRJ9p!AZ9AwtX;|dWi#5zclD!w}`2<kz- zU+jpZ=XKcE-cQA|e@N`L-{jWu6=<gCZt{%0!CPwLpnQ7@Jjjz~Q+`RJ{g(s-BVGvB z|7l62b}19vWqh10zYOiPF~uD?n;iSW%ft=a;_!y^C&_eeMbLb^7LFxnqp|JwFe&;r zte3Jt+%KxMpF2T|0tT23n=XQ{*f^L}d;kjHCXl=h$5~TdXZq@$2CQd$i1d}8CMR#5 z#c?VWd3fkJX?k=Na+Vpvh{H8lS$qs$KV6SYojlO5Mn2f3d|~=|Qiizwu_5q<BcNKc z5V_Vgp^ZDwz;Hz?wO2O=d!14zLsKZ?=>dWnwmH@DV}QDu-N4a3dm2lRlPSwcok2Fp znt)x|H0rcm0>^4)2U;`lG7b`ml`^;cGg^a#jLetw=#%3Y&Q*=x@)LXRF;CX_Q5&{Q zAs!=hf%if;9ed8P!rSFram9*mY+|j0Q*#@bg1k<wwy_#phc7}?s$Vmw&KY7oqmN}P z++!%S%I<Qb{kGTxtwI$6YjG#+BA36fXX+)-;uM|?*=MJT|1{?_ty=fWUcH=GE@Etm z8cenEepeN^9^i_TGVWk$*N0edY=Bq(^s5^Qi+X^rCN99A1XK6K*AMY$t_u9_yN<f8 z)hYQGj!1ajW~|qyB#1e?NUaKYCza>lVS_J*NZhUp8h=TnVckfK_GBW(m&%Y9mqH0G zdXM!RT*>NjyRbKTLfsYQK5<-5;!t~U#Lg~b?lk~T^#4T3SMZjR-R>{(mfOvE>*Yks z>zgbir1AhyX$&aeUR_>(I!FT*HqNAup7Ej9%I2b>`~qZrS`N?3xJ|kJY{mXUKM=J$ z7>|CViGr#Qeo|+HI)Bzt4<>xV{$&}KUGLOT@LDm_#Z{-~m0QEo{a!fWc`&ocIvEGd zS^#pqdkl`tkte_ZP*h)d4sxo$&an*JfsAXP;H-TOsC{j)MTD0InKnF^qiH9?1PJt~ zQLE(0U9=Y|h4tbjg;vV8N0n@Ba>ngm`edJm3TLrfYI&+r26ffQgK=$>BL)gLIbyHh zQg7Bv;Pi8AnUK=?WI^Uzb0yCg4DWFD9Jrw(jq<fkaFhLaybwQR*4<o*rHbR2b>FI} zzzK2a9(ND6-Kqf<cXmNW_y+toLJK>XzCb5qr$TJoVf3Q^16mP1mvpbMWrFrh0Od80 zkt;6>9~lclb-(=40ap$3Qy@1wP}zm9&($PR6Yt^^m%FiE(q<ky?HFK5>3T3JAc}|6 zGpXD6)j50hif~GL0G2YzE1#zvjfMt}Aiq>CVD`Vp0bWnApIH$4!T>nDaKksEO;E`@ zMdowkZRWO59^NWG5hQGTvDwPkNPDXYyzmzowV56!zcC0eXqiS1&OU~kHEG=Cj&Qvy zPX<LiT7uWePoy$FGt>-oU+`VkjoGJ@aO8(e*!_zNva9-x!MY9ic2=XH@9p@$TpD$2 z(G;vVV-s5HozL9PGh>oUU*c~e(O5(*h_T;Ph&4M^s9!hQnK9ljv{6SAzmGqJG6kkB zuZ$VE{`wVUE*^)c_g^me;hkBJmmikLZN91G2NQ|()WpzJgCKC;yo~vvu7+ehr4ZT{ zh%WxYl=<6Os>Nv?wv90*4{|0@p|MJklQtbs*__M-YYSMuXRpwWP7U&5)i9oLPng^Z z3q!@Np?KO=KV<S#9{!jXFudz>*#Djo%Is)kJWoVZJf52%+H3F#)e>ZdWyuGuIZ@zm z8{k4jv<Qw#ZbbohPGDXBmQtMffjLd8m?e3q89VcI6#i>9*;t!TDSZ)xm3}t(N?0(Z zZ!?*g{i?u?4N*vF%^S|C@9OA5N;Q75NeP+hrEwbTODNaGcbwPOcNq1L#nhD^T^_y> zZh+huD}kS@4HhfegR&fAa7olHtbR$9nt4(U9TLq$v7^Okq|}(=+kW7LJ-o)$n0w%P zmxao(h6zp=JAfmcQ!)DpGq;Laj(TPabJ|1^yxZqM?8JDyZ`uUxl5E3_CdxDWvx1P7 zj4O^FNJB&R$|SbItq~vem&PhVx!8B`H4~)21+142F!uemSlhve9E^C3Tc62e?#)`n zg!W;}6hu@rFCf#g6lUGCY;ri+9tFDzf%%_u{9(;IWIg#98uVBEXNaBCmmmBuzFLc) z7pk-`DfkF7_;ZS6p3^0c?*{Ojt5K-=wHuD`(JJ>0GAGy`kv*RcsECqH=tQ6m(P@am z<HP4L9gm!u$lh1XW5aLM*^Fc5v)=#WSen0}4)S8~m2rm1<erh`(JKkm--*T&W9PE} z;H$$-d2<s?X>o2T{&n~$8UTHI^Un?@X}l!X@RK5u7Eh7i(OK-htJ^8XDQ~E%&Kay$ z)KhYNogW=PBn5_}`=D);Fe|m|F2_5&fW$nDrb|L<iAlC8NxiJdsu%aOr)1+;?MN+_ zn=Q`vJL|Cve4oP#saa4^_>8DFRr0`F<RVoYoXj5F(o7D`n1RGg&d?2aRB%eid}M#~ z7#-!^NtTRABd>l*feHK$i57oD*qpCO>Q+9p@4|bc^!zOKOl5q9@KHhHmDfb>u1PoO zA>A~pjgrKru6tSEgF@`p>e;m3+hNGI+{OKT#DMGcX1u`lu1>tR@f2uvmnev?j%Jt? zBXZ$@7fEP~hNV6ex$}~?LD@4;c;O&Pf85Q-F9bKdA6IA5Qdpj3d3nO%5*9o7?8a^5 zlhCgDG>pp|DZj4d3$pIVVMlcrvGj_d&&l0kXDdx%^URl^db@eFg2)n9Jzh^h=geSl zd==QAc(sBl*Wwyn7U{@#ug`=PIh)w693^&J#%b!c`53;kat6JB;~rS7B?j+_6g{=y zko~P|Pi9};g=Yi{7z^E6B4;fPAs0{MsV-_nzRwS-8hI1#tC!J`l`y%aCr>V)r@%wQ z0qxeig18Y~WKcGfMQ5yOBke+(_aV}fb#<&D8aZV!r^pRHL<ZxY`6Z-ty$48VIWotr z!{MW|CNY`4jhwXcWb$1ekU-}^vfptX+FB|_pZFn4_iP9QoA6X>BHoPd-1di>`DSQ- z!E|yyD3TM)Y$<={99JIr=mnLto=b4zJTfGo0b4#@f|Y4AdF-;!He8q8kzBO@8{O38 z0gblb;g**$TOz-Zeqd$_OE<)^!vlrb=9f6J7hW%rrC?Gb*Mr7o7$UA{AUoQjOn))3 zXRvh>R*1_cPMV0+-t9x0p%2mHdrDNKbtO8}DNlSNm!P;ZZ4ef3Wvbn8qcfv}NHUyv z9zFg%4jhyof>E*~d*F@+jb69Xa|(~Mo%d{6`k4jIw47zHEDxoVWyNW3O9?%yEzQ;P zol5(Ky@zzX5t1j2A~C1wWL88A(rfpGBj@`d?Nu8T?P2J-=NTCPB@`o89|Gt#q=OWq z3h`iAU%3EFzTH8>crK4QK35;&#YJkk;b}PrEgNJZ@Xednl+s&AJ;;`k`RuvRGimP7 zdpdkv8x<o~MUOq-%)W_rV9nw_614CVJvC<{6dij4!%qhw>{2pu@KmSAtfd*QR7<&j z-Z^qzT0<bn-buzEvZu=eo#>0t%xJ$W?PUtTP9WazS)veo$`O>jpQF6xF<Ad(8Z#(; z2y2&^kY1r4eC`>8kLB*g`3=%Iy1yHKS_3U4;=mp3Kf)?W=Fm<CugNSy0@eA=;lxkL zjaI#C4|^{yre}5rk?iz$L?<i@x@tV3+UhcTjcbu%?F@QfY#o}nHV|zrTgGGC+b7W; z`z>LUa1iyBa|Krn)pDXzJ~Jlv4!HN(6-K^)FQy{0Q3pAV=Ta{@J#+}JJQYP<-?I&$ zDa|7F3%Io3hkX@Q#!}qZaS^afeKxpd6=VCO523Ys2hlztLtju<L5l`7QCry<f+dDz zOP?F|KCuCs9C?knxX*{wIb=i8dTq2|V-EDrsmI0*2XV-^-K_A}bUZjulgOS9XB&<u z;{z70mZ5L!P>PQzNL-76UzrDyj(a#hWnT%8f+x_+hOUzOYFqLtZzK0jXdC^UBe<T& zy@gWo9Vm8Ch8TQLCEH)$rKwzh+SS{P1oC9WS-l=}a-bp(nkP-81ASHL?QP|Pl-f*z zr}+!$HfWPsF+a$-p<04V^ogF@BSAWuI*12M!GUHOxIM^@Db*;V{G$7rPU$BoqS*rS zU91J+q|zu+A_A`4$<Wu`8|j4)m(k9)1@x^~YTRbpj`nM?q&rfN(mef=osgxPPgWUZ zvJdrN;@OD@k)N6@%v~nTwmcGLMX4j?%;X>B=NmsN-J}3LUo{E0K8hzq+74%E%q71o z9MBx6w*oM60a>{|icH&(i6v&;KwJ0tlbesFNpiI&&MK8g(}eq&bMZ2)Ri_%=7!w1( zN1xEWn}Nrkd!7r{2l}y)ayKohs!2O7xCl|EWsp0-p^Fp^;qAv%R8&w+l9ujfofv;$ zzcN@_ZarHYo`^P%En{9}T9O#eByg-h1yRR~sWHoA9PPSlCh1r<-t_n-swxk`Ymdz! z4S&qgj4xNH>3tW__CyWx$;BPqF7p)WlSyWDRY^-($%psgY*0vgcATYoev{}GD~{4i zD&esB;we^NE|`9C@VwxfV9$PSvm-BCe`Cox=H%ywg|tfVd~kWD&wlz^0RGMkK;^c; zce~i199fr&Z0D4q6H!Ve{>~gcp?U(+3>iWrJ%4b~bais&{Bx|rTl*SiNp@2cyX!F@ zCcz6gWp?Ea750t04jZU<wBq!YO8WWIaIV3e17syXkX_ZB0!QCZq^;iUr00~!&|l@! zuyf2~kp3tOFSXO)kh>Nb2|}!Yp+`_N^$q&7VlMLCG?h-&D1xP#s>C1)kl1X&z<B%% zq>vU0ygBU)@UYGke0}3hygJgFvfOzFuT!Z(`F(X*sHcL8*?bxD+=Af5r+KWb#CUGG zWIHK;=)<Ny4W|!X$s_lFNwaG?!E}1zGP-2n57NF>gm&9I2>9$Tv{rzle~(QB;aST; z%UhC_m{tn=ytguc!oHJE6J^NbC-8`xZw-oAwG4;Z=umymcNylv0TfS(g7GOGaO1k+ z6n+5V#`@#x8eKTU-$5P^UnTeQ6zQjKx`6s!D}rjaR47+Y<yPP5C%?+a(^l_2@G2&g zzV)YzZA+R{kq~~Gy}MbPEqkTH+9+GF1Dr?9>D2<;qOldb+L@QY6tC@I5*8g|bES8( z>)j@k`;NV&B=$K|oUBiLdOr}w4N1^rsY+J1R4~UL6@kXpsjRF2e#=rkiOJb+%l0m* z0_n9=X!AqzaO#VAh1#TBWUAgR)Z!^i)en2nKl<z;dnOC6$9?IXTgzBbC_tlK!)WTB zXfl&G*^;g1FQnO&ZUMk$$R@N;qjSqVVDQ2w+%&OE!1QXOb*+xXr}Q`Oe<+YAPBtL@ zV^g5uVK6S_9Hho(&V$8uoupJDAAPG4Bq?W1XPIJx#Un0~uq%Va_3B0L_fBbc^qmGO zQ~yyevOt)f*1Vq%7_UWbtrH<UW#hB-eK$AKeS`~_iu19}tD~Sl6pxJkL(o^H#q?OI zFwVa^3CZ<LMaQ)-<4vxnIR2PE$c;b5@Iw>O*N_S@;hZLwqslP0DITlLk0ML8a`5pS z7A?LVhC>A)T6#e&>hIc2D<ySsZWsmA)2@#rUZQ8&=2~kWo?GijTg#Z!-O1aD(MS^A zwq^yJQ8ts+Pt1k$Blq#rdC$qhI!D;(EQAeSWwWyD(!rHdg>?rG(OU%b5BHF{C`_dk zZ6$ih=YAZ%r**n)b%QUG6A-s$EuX36Xm{M|-AzsU`CX6=9D!U8kD-}+_R(@YmeqIQ zi7|FF_tA#(yJ=!B#~n)urHvq;{eE!?M<@0^*lms$gb3(x5x1W{|FaL2KGi|$UU|B6 zb|hJQdovi@2g9OxaX5U@5_+5q=msPR2Ye_B!U@&Hx55YiRLsO>`a?w7CzpDea2GG` zK1oTmP>dYE4vd$KGC{odF=o|~W<+8W;G0Aj#Cm!Na+4ytVP7ji!BCk!C>sFds!Zvv zlF!MOMhV#NHj%A-{D~AuNV4}ke&8e0f`F%QwUCs#T#(}xLc)Y&z+j~jzH_*j%nDe? zx_{Y8UMI|^uD5-}vMF__WwR`^)l?c|wQzKFNECPnG?ht*s0;H*OdWZr=V04E0`E#` z2wJUDNe|nuA^aQ0^w_FX6)}U=Y`*+8+`vtwjYr?%+@~^h#>5PysJw<Azx)fu)5!q! z<16S70{&ApRE`u!YZC1{vgoL5E3)8jLR&HxqRK3Jym!4V%4v3_ET6}ac6u=nm{kH# zcJF2K$4Hx0jjV;KlUtauD|0E%{Aujdb9Tt(K^op{BE~*HYe?GT_s|7ZQ|SF`?dbG1 zQrtC_Hnfsz53DVnOZQHy0MV}&c-q5gbbC!BTao1nrao?Xg0B>r^7J5S_6-I(p^cbX zU&cm^#)G5H52`qYw-?4cB_i?tm&luVWp;eeEv&Nn9+e!b#frAgBpDhTVT-3SJ~)zz zRCkwx$J`X;FTRj5e%S!S*8c2ZjuHLxV=T7LjAt{}u7D~vXIB1?Aoe11CVf~bOyB^p zB6b(U*l@p(?3N5~b{$_t5ILq#tnQc)pO;bak+=0d`7GW{61A+!%xf_?O=cR~l}t$4 zn%yKMeTZznZ3`2ghT{{bKN9x}gh~=zKv{VS{;+ukmVGgSO}>}PteLY43~$J={wcz2 zaE3FxdCvm+W{_jW;D<lp8mmF)6+9t_?Zw&NRtx-MY8z^aOduLNz3FsyX_^;PufTTf z9!CZ^?zErZ5^BF=A?R~v!M)flLYuwB*WWc@>MVzDaoQ-*yH`NXQ3(=hn8rB%AfPw7 z5U+W47%3R9VS0w_(A~{^NC}z)KN$_gT@rw{+iZdaeSfxSMH{v2YC8z6+QJ>TuA4PE z+yeQ5AK0x~$~;=Tzk_nPl|o-w@r!=D%ouLS-^6R!QP}oj2`S$c3?keL%CS|5Tk3iY zI@YBDFDDld&JKj$`V~liej8Cd5P=@$3MQN)Kgp<=K2<AhNm}YZBYI{B4&7q|lXEkv z3y#_NSK$sAZ3$#MUgeWJeVBMC?yqpN;ZYSSGX`+&*j9FnqXD!l6_bF<0J0~glg;#b zh75<N(#5eKDauL~eD-v+E+6yRmCiSz@}~;>yk|XDZ3rbA0qZT--Nr;-(SgwW0>SCh zWMq-10hdKYu;`pbru5ofYU_>ws$xkMBYwON2QU3X#h%S!o;DhSIxnad1^;P;MdjP* zaa(G*1;&}kyyGJ|X&8fN6}2;!=?@_6KpfdMGz^~vH>LZZ)64s*Xn2KIlc)nOgg3Db zxc;hSi=#4G!pFcbyiKK-oW-`E0?3y>Lt@~c43nx#aBlT}Ja(}FY`q-Gq8%16@a!H| z(2*1f@N#pAQAG!KS^fz%yHpc3D?%hH6X>~(i@+s0p(4Y05f~oqBezDTu*|9;x&?h_ zGVhr}zu0q9mGzZ<q0_)t4P-LqH{-DX;vLM5WFg?JNFfiTb=lOm80K)yd%Wr9WmauX z4DovS6oTYR&|`N|yqONdZ`M5IA=q??FsmxjwH4jv&FpgGxv8I|Y!feg<M9X&D0UL7 z>5oawQwMg+SOwg>$>DAbECl11O>psk9cjCe$|?=2(B+Q>eKx!#$3x!GR|5<QyMG!< z*^z_p?RTP=mPK>Yi)RtCN*Lu(49VE)1HF1WEHCsc1bv&J41aNvS~evAKM2_H1XJF2 zt5{_5@B-;sRKr<Qb00NaUx|JM-bB6%XPMKzMd;}kH8^WvL^wM<z*8m*-4k963p2yZ z;$*zZmF_+qxh)zlUXlYdr5t=Br2{wJ_QR^7%}CSuCN7eu(1r#@u&x=#P229`?^YVb z{`gxQtt|16fjMQbnD8K<n<&6@|Fw1sKQGZ#<3BT`<pe_de{Z5n{Lw@m(Z8wBe4Qkf zaYEXB^CTV7zsYobhh&v;iQ0UxBwf+Jk#YQpWL4o={Od`&8vk}m3kk{p`(T1+#)kX> zL4m-#y!5D``S<bP1)hjMD_K|cukqra6W9RXJy}=dUyg|Xr$du1KT}ZfpLT)&ehY<m z{EaC(qJK>S{8K3^;|7=U<5F}q{!NYx3H^76jrRP<f&u|u-SNM#@xKcSH}NN=>S+8M zZ2P~D5VDzXpQ<DJ7Z&4trmBpau$vzu=<x5K!~fl(a5ukNP$2k%2LJ!3{Cqq*&2n7r z2|krJ(@s(FA^hF`cfU}u_RT--lmD}KSSJ2=ogN`a|Nm(VR?QJuD8&A*@gwM?DD<~p I@t@lN1Ex^<B>(^b From aa07810d3fd6b53bcf6853d84b5948011487527b Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 12 Apr 2022 14:40:43 +0200 Subject: [PATCH 204/512] add option to train with rough terrain --- .../src/LeggedRobotMpcnetInterface.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index c2cd925d6..20bb7a245 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -91,6 +91,13 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr [&, i](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { return leggedRobotRaisimConversionsPtrs_[i]->inputToRaisimPdTargets(time, input, state, q, dq); }))); + if (raisimRolloutSettings.generateTerrain_) { + raisim::TerrainProperties terrainProperties; + terrainProperties.zScale = raisimRolloutSettings.terrainRoughness_; + terrainProperties.seed = raisimRolloutSettings.terrainSeed_ + i; + auto terrainPtr = static_cast<RaisimRollout*>(rolloutPtrs[i].get())->generateTerrain(terrainProperties); + leggedRobotRaisimConversionsPtrs_[i]->setTerrain(*terrainPtr); + } } else { rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(leggedRobotInterfacePtrs_[i]->getRollout().clone())); } From d0ff261ee60fe6a7f04a1bf586eb2e1c6f47a8da Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 12 Apr 2022 14:41:35 +0200 Subject: [PATCH 205/512] update policy --- .../policy/legged_robot.onnx | Bin 35344 -> 35344 bytes .../policy/legged_robot.pt | Bin 41391 -> 41391 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx index 220d9843b9373e8adf31c2b7671b48f75927ed20..f8e5e13363f59f3b5005542108523b629f62da0e 100644 GIT binary patch literal 35344 zcmeGE2T&Ew7XAx^A~}PIWJE#1fMnRyYcMOKm{2hxDk_So1W`;NK|nxqQc*x9DhP_e zo?e3q5CjtjP*F^nMKNGN?|#oY?+M@co_|^AR^7UFORA>!Y-Tb2?BDeC?q1Itq@`4) zHf-@<@4I}pmc;0hqs<*YC-srFk(FD&%+J$zlZU_8dLPw3qiwb1Tz&jD26%b~dAgeq zlKJNwQ#n})kIkNbj(rY^OUZs5DvGt!k{!3+$Isux$KQSQAn8B%O>YmC?YGIpJHXSl zaj3Mo^kC`1%HmEnL&d~`S+W1u{ruMSYnS9Vr{A1@jli!({q9e{IsF=eUyJ(PpMG=t zH3GjD^}9d)=Jaa>el6;EfBMbo*9iPt)bIZEo71lm__e6t{pmNSUnB5qQNR1sZ%)5P z;Mbyp*+b$IvU;+jw4-RV6{R2TwWOzc_)iP)cDEQL`S(59VWMO9s(mcXwPa@hBZ+A_ zNa`QE|Cz${krjo>s;v@bK7CfX`*`|~G#?r0>9uOLzpToC{BoI>ho9(_qqQU_d9GXM zZZ$~ak5^MA*?u108&-St|0Az?NL<`hQC4D^r~kj+E67Uv`+E5Jjnx!CB;H3>L)6}A z(RnPiB<Fbetai5^B=N_q>>oNT{zHewU+J(I`41hI|IlIcKk5)6val5Cuo59!ztVr5 z$7Xk15u(5E{vDB*-`^z%i~o(0j_2kLp1%HnF!B>Y^7iua^za@1r%0{8KKc)}*8fmz z_dlxrgPOHSt<68E+5dxD$RE;dWdBmyKl%Now10xM)sp(#1s!evKWh7%2J`>aF#7+9 z=U+$vfyeGo4Ws`@4Fg0N>_mL*wWR*mt43S=J0SbN7WdbH{uK8oAWQQ<#aaH3;{FPd z#s3k|zmEPLkmcw<HT<u)od}Sn=(e-8_!H2d_uU`jEG_;M(4Wo!HK2bT{f9WqKgIp8 z_u@#=)e{Y6mR4Hw|1?_MN8A2a+eH*_X)cPl{*QRO|6{!9UbnRQk9hn48gDK-zm;gS z7Hu}7zq8en5xF|cJ^bA*%m+#SefOV3gvbc$Bm2iEuo6Yu{d)@*qyK9Q`l2)2iZ)T7 zwY1lg`EvmO*9dMZIw%^!t<1G#{~W<B{@V~PYTQbM!)o+@G;Z}@8?R6m1zTvz{pnP> zTUh^_ZI!P2r){-IRb*RfN^6R2t6)_zvHG#U_7(MiC+c?_zfSKrtKY1Coxrb0{(q%k z8I1Y$&-}ke{%+&f$^B;ao7Jxq`1Q#DXZqD2%Wl!E&B{Wwi6+ukmj9Zy+5B_eEt<7i zi6)v>R{u6@`(su4_ci!`EWA~f|5k)FVvU#o2+`7Uy|2G%bAM^IKZ=CL{?%&SskA>+ zbQJFVm<8HuSr9NzAF}ezSj9b`LFbbSJNMpQSliVA4{fvI#@hR^`>QmJ@Ylry)upub z=p*=iR|3p6x6|#KAtYkE5$buY$0r8|;5fZpjFL8>i{iVe&%33V5yD`Zg$<9m3{5L9 zk^a{YGg0(><>8RCxL#?VpzqFSw9<GCzEx9#t#3XE`^MadA~P1|ecT5g6M2|@UYu>- zqR-}ExeJ>nn~>#BAt2@X5?nS#z=T9K=I!=I`f=kaVr7*kynkys&itkbClbCfvqsB6 zzrJ4xgM(?Lj680fIUY@J55YxlDQI^-4JYYm(VrJn!Q)h4=-DR@PK%GC@tYIman^29 zROCwrshC5`@km@W?-1&i)S=d+6xvj%&VRVmh0i*M@ILn?`5x8NSbu9i+ID5*D5+hD zeWjSehLgyi&!IRy;S}9)#T;s{%V9vLKERL?&{@NR<d4%ZFe4UD%Fh7>H%~}2mV|WI zc`P-jkTFyfwl9gpS<{NB;p})=JIb6?zK`V$l&(O`=R{nR>4@Lu0@1*I0G94wfmH?y z{L_{itW^`|_df5#H~JjFoF_a^aIL_stZJ;*lZQFA-DLKvcG~uC3iK=K2RC=$CyNdb zggXuSr0Ags9vV3oKf2hEik<<m{A@IlxV;1R9X5nTJIv{>VM^q-nhRRonhLFHOK8^M z5{7-Whuk~!jysVQ3Cp79W2apv=#3DA3#D#&)-)RZMqYp%R}FS@RwC_c&Y<UVM!~^G z(RGZrL}u$FuJ+z2&bIJ@052%v+4-%^!i{~I`?H4=zm@@H&mBt3!mbM3>NcR<0(sDy zoP(Q<?qS02P_W7Q!bz_jf;*O+fQ7HVFylWBBi=#rxap)Jtn>?k_eTQI_O%4qY|F(j z+j4|bYYX7OS4kEI_rdcgLh!BU4LIYI1$(t6F!w<a+PCk6!|A$orn40})YV8viu*y* zkq)jl=o}H7){k26eo5!+`O|w7T=9$G8vR-BO9I;>(Q^MC(%_qn(_7PVtKn7BVfu)k zV}jsOKY3m+E(xjyZw1cdcF=+b8)AGi2rAzULGi<<ap$yca1bVu5^?}jiq!Zwdt{*G z$`!EC?T?=~)nQdYCQMf>BF#PVWMx@2PI>7`+>dxb;q{LWZqp~h%Ng?AhHr;x<BA^P z2j#_3WxY)3Uwnbs%qWN1gFeytgG*qyLlo>vx`MfRa{<np&<}G{Nbjy(;vHXr4RVKJ z7%PSP@{91Yp#yfs>7tUX62CX$21=@n^REuv!kk4rF|Pe1-7{PkPbD_dZ8HnV+|i54 z@UvY=%w|+xJh~H9y5`a~Dq29k?cgSsG2}`d1@E|MZqvzlYW_$D&h$#cLR%d+&#yn+ zZF<Rdj<unO1e)~H(*3YMZv#x7HVR%{4S>>|6xu%KGKrK5Lu(}#2Of>!;ySaTed%|+ zZFLXz#>|4|Zy})l$sAv8*n-oT^Tc~!Un;b-v#*=Knz>`J0vwzJq14?1GG8!cOt&HV znsSQvojjPj=LeH#Vl#>Imq$cu<b9Z`dXT$5wS^J7y29vPr*Z$;(<CJFD}DGN5|U&0 z(4({G;>(NIL91syoXjkMZ?X~aDkv8wZBl3FUF`(M)SA^C`vzS5l|#{;MrfMe2-PE` zq2hJ0aOgE@VZY#)<YZwrX=chX;8Qx?aX^#U#I_Lw*~<)bw1cj@br3JO_QM}76ER8N z9XFrsz@O(wqk5ngl=bFO^5G;G<vE%^cv6yBE>faj(=T(<&#H-W)@1P6mxUi6y{DxY zdKh8ydr}$Dix(Wf;+A(oP#&C43}-IGr`e^LW|j&${T4907Ugnz=T6{)1~)iSHv;6X z&ydmx1*nr$ht$f`bjFH-*mJf5b{v}z7w%QVTdgX{@)3ish#C^Qw2fRA)MBj#<r+sl z;mkUMh>4*zdapfSJ#XxOIANy?nTm-x?QR@iH{HYSdmRS{M!&$^lA(M)A47gD`by3f zO~M<Ap_tsV5xgDhD6BDn0%<i|dyJCm(%0k`6DpXsU=&HVm4}b)U^p>Lf^!c{fbS!f zVd6$@FrLu{ty9{_9Ovs~ozfVXJA}i<xg`Sq3*zunI~m-ojj8y+3S2P!C4?Ji;V>T! zaQ|2Ul0%emv*uC!_-+Jf8^0x+l@$2ALzhtR@GGL`<qZpa8_;;jIZm%&F|{%nggdu| zqn7$*@+A5ul^%VUMhvQ^@fwqHnW_m)F$e|~oCK%OM}v;SV!CEtJ(NH21BKRdeDgJ$ z?0EY?=&(qc4ahi)`qhJB>+U2_QNK*jxJQ8EOIz%IK9x2d-wHBZE0lDuqVlTS(dgz^ zaIB4lE@m5+Djc?7{^dA$8A)>8X+q{hx(yWg%!BZwQ?Z}(8KQ8#g_hi$3r2ne8H4-5 zXgugE{n@?}r^>d_+s;1F-8>Jc=>6oj$Y+y{X>0NI$qcSJ?g_fA{ziWEQN((hP1le0 zf>o}^!MNXGY*n<uBWr_U>w8J|nv4dn(67U_>~pB|IRPr=1vGH@G1@WD2<8_a5U5UE z$UQ5Ff`Ah`@WkX2Ghys5;TH!D2q4;MHd}Np+vd>u8@#YCRhJvB-A$)U+$R&i79(!l zO*QqGfa?4RT<ID~YBRNY-Azj%YHk<vLN^4HihUp~tOz{TDB)qNR2<q-0BvSn@G<B# zChqN|H<d=SucQmW=J_rRPt(BOkAWbbL22x;)zum{Lt*CZ6R@IR4c(EIN8cG9bofMq zn0fZnc<t+6?D*)2%bFgO-4`>dPUk#&?_Cc*QXh{_iw?qrPA}Z(wwoLf#`6;@&Vq5z zFf_?@!>l)fP;z7*+<BRXgVJ_UWj+E5muRr(y7K6f?RL25ViVkT&w>X^W?aS`S2QdD z*xBC<AF6&}21_?G(MI=~yPDFFxZpE6+;oX_F!|j2?@J+n@k9Dzq!P}n74@--P24Mc zZ3sGb6oo^@$g{qsFk34gPq}V`dE_Pr#WaK3Qce6?HV|h&i-%ng&w*st0nDt|#z9g~ zN$<61;JZEp_pGnM!Xx5rc-t6|*%O3`^IlQSEDw;I8Aq9YjocfvRWQUUk>L{@!DxgB zbew)b^HUPgO}K|SJLnKC6}wB%oY6&n^AR}roEW3+y&tFL4aO(s{*Y`n6BiW>r2A5~ z;k}|V;u`LVU!PZDuCEy^bsh+9!>aHBF+#`9&cd3j>d<bk#kZ`vgw`<^h?2k-e4{^L zW#KGbKTwG(EEq%^(j@TA*+}fC+)0}ClOS3}6K{wZ4e!)|pv`xxpQ|Amd(n+(e0@zb zD{9~b7YbjCw~}+k#pH>?K+r!vSdjfHoxXr!Lf33<@a_Fd{JwP)AK6J@>M5-zn$7<I z+el8%dCQ7;y_+e#`L-N38kV!+;R!qm9L7tirm&lq?_uMf$FsHD)A?`hTiM2s8Eo>7 z^{oBc&3v(8Am3kD!E30;@+%D5u{kz{HQm)my6@jw`uBPBpSf$N$Kubu&+>$P=Q{`N zWp$3-WBaQ)^0nore6aBtR$jl6?J2qoOSDaDHhWBF%d6{H<CsP4dxbH4>-`@%XI&g$ zF?lzCqqYN=F4SQ+r-rc?rRyOweHia!ZGhcNH27)n1KBxkQy^|<1m9Yz#X66hz%EYc z!!DZLhaGNti?2_+!7ga0?DZZU$J>hK><r~}RyF!4QJ*)BeZTSu%Z#4H9~_W^Ee|iU z@mn<cJ#l?%VgmGeahI=rg|abg;$O(GoyzgwJ`P|ln34PiX=(mmz(bT1Ae!{u#5-TQ z&YKRA?)(44`tJsYlf23KV;C~kkI#6Y#+&XQB(3+iHO60BMt{0lP7e$ho&7F&dPfE; z$AprntL4#mwGPg`WFV{xSi*h1+)XOpzbB1b<ngwe8d|o$plvp}P_VonSBYH(t&6Sf zmCD)dttEXO6?@HCtr8hNWSkF|<MEcB(+#8A4>Rc?!5Xyf{>~)3UMAyOK9T5IZL~AF zij1?7g_r$>RGbZ<8|UWIxR?%-;TBG=)ak;d>$~Wuh!8T|LJHH@I?*Q!Zg97IjY+av zDXnWiNj^-q<FxKPp*yvwkt@bEuy0v1`||54W}cpLjYIA%w%;Hl$LX)OL(dFDcD3C= zKF}bCg*UP6)aVH{*=@;~S$GO&okDbo4+hVk4CcVfNrL$PelRd<0O*g}M^5LSW2Rs0 z1&;%Xc%-s{SRQ1_z8mZ5{;Qru;`1<^dha_kR8tSv-BW?QQEKGM_fCSd`aqe7F}bqw z6#M2*AAarwCsxJYf*mQPTC;ra6O<Sm&&t@v(XHMS*kQi*eAcDAc=wSwOHUi)ebXpP zmDj>9aSeDH_ko;yVuyQ2WRuM{hV*KcE<`35Fst0|aJJPZWT=7?(lABb8U2lZ*MCp@ z?rtQ@*DN6=$JA*+bt}!9dyY7CDuKk4+uX}#iR57a!(ep$3?HDG&A!h|bu?YK7v*Ya z@}IRE;D(W8%_(gg_TsTbh-iNT7hTu$YQEXH|En(l+I|l%FY3awL>pE;Vlgh?xD#^~ z4uQ(o4>;nO1n)i0gWv6&2IpVsfXs+gnl<V+*&n0};+KchxSn1*T>J--e<2Sl50l7X zJ#j9*W(e77hUC&{alX>f0{Y*(L6nz9*IZ4pXCD=<MDC&#e|nH6YdYx!G|w=H#4>4U zn|hOsGGcLW;9R=P_b}C7xtHD>J_H<#ztQ7%17O$2crJ5a6_a%A0a+R*4O7Jz(MyA0 z(od3-WZfbbkKY=}^?UM|TT(BB$JXp8-d6f(GrPCCd8IOHZ|;wtnx^#UsRl@X9)%~u z4ET`?=0ZL-b)4rciHAzG+3b9z8v<HTCf1!f61tvDYVw2GdD}2_yDiwxal!Gydui)P zPkf!3M-IKeMf~O{F|DEd@ywb$8quf%W80p9Y<mS!&zMQR9X(7ZnLVTn9e)r9X?t4N z{FPKo_Q#(Jrl>z;3Zv-dMZG1?F>+De^oP@0ev-f#&a&Z-=OQMv$q5>~p<^ptTM>n; zYUTKERaf!Dk&`ffwmJkVv_iZ4SO}gKf^)7tW^7M#9RK+oD4j}!$^KzP&+r0V39W*J zId_TnlYxBK+8TN-%?|x8Qfijgm#lrDM>pE2!jjMOIPu0(VzuFxXtGj7CZuN3?6tcY z*S#iCuk`?!;u-Yqafg~wJ6*B!KsG9WmuFvv!~iEZ1ST$vpf9bD!|p+?*vw4BREKZm zXl4Sgc`Ji9#^!K9PmFA5Yl!zX4LBlZPH*qzg)!Gwfb^It%*Ls0bnY_34e>IDyY@PC zMoczUUZ6|6NA;54y>GdYwG$}@e<a)jbwS!@4P1Xs7Ao3@<L%FH$<cS_HR6L5+0+g8 zG_-s!Peym6)w@t4UgJn43(^2<CD{+JeaY-O>v8lMX{w#Q2CGyhaK-Bf+;+uX9OGKU zonM|L5KB?T*g7M~4_S`Ee&V1bu0hH@x6`c0xpY8RFg2)Bgs`$3#I{0{COlon+^A5( zvCdIc_o)S5JG39O`zZ4=U8m{ErD9+;ZycLfG@!<DUk)mUe<qcelJLn&XT0+BDeW`X zm!Fnz0SOk#^yk?Xbk4LKVl(^#Sr)tr^5nCq`9ou9KV}Y<wQ6Kw;xUYK>%hmkJPlMH zKof7L5FN>dbeZ))gl|@~_QFZJLE<0@Z#%%0wHgY|Qq&>#=uT?x6o>o1$%6R&RbXb8 zz&pRtV9V8tL0Ww=IX`0)B-nFAY1M4PDRv9{`O2}&HT$E6Un#kl6HUflV6c3u4F22_ zNtCKGaN&hx%ne$^_3-iNBJmg;r?{iZ4sAH(HJT=7Ig>5pUeNI^{i&Ps7n<Pzi85Z+ zWQ&wMec7Y{Ay4c`TK#c4zONFSt|P}Qj$6zZ3=gicJuJtHUCHCmb7Jr$IRk4CrVDer zR`X@L70d`fBR;Bm7RffLW8@+uiAJd}k?wPf>2dLfdse!jdEc9^UuBAt%N4MCo;^8p zvzXeh{6es{nCWg_PIpRrkh|A53nvTqkPFTlXnRc#wA7{(W67n&W|!!5%@89`oGy^d z$wv6$Q3Xv;SEy<KG0@Rt#3R->EFFwpE>JQ_3DK=rsJRZ|nEqxgX^SS7b{g>N-Ua%! z_yG7Ooe^q|xJXk{wdl2o=Oi>Jix#BHlMio$afsMqqMJ01lp7Qf96plzXT2qB8zX33 zl{K@cd<8vjrAOSw4^fTmD@<^#6Fl``=?Bvu;sxz^qIx;?=$h{+q3gzXs+O{CLoedm zMj6yzYKiBMY=UWH7%-~W;S-B@khdS4;eLxY&b|B)T7K+9n;!{~F}IZ-D=eiS=KKVQ zxc+QW^9<Bco(t=r_Y#Sia;Es^M7pV{mC^wfq|xsd(biGm^3?m`wI4glh}Tj~l=pT* zHyY#l8;7}f?TJu2SrxZCMe@e}0qiTLoYgNF%x`g#WgE7XP^amN#6?^ORm-iwR4tS2 z^P0qsS<xsgU+#ow9_*)<gPpnZhIJ?zqyd`Udx`!wY5MxPAK9GH2Q5#^VWPnhj&}0Q zF`sfW>%kAk!%Q7|jK7lR-Fh%SUK@9(J)^7qJIK6SMFQ=>G|u&8qVV$S2x{J1%#NEO z$;eulvui^2p;G)N4$9jFv8{vz%~8WB#}(WomHSKxy8#`N#bH};jbKWuIqlJ_AZCWK zWLd(#sstZtdgeVx3N9j9u^^KA^cvCufo0@P^e7VCJCMdNxl95pYN^YTWYSUF4+QUJ zQT*LQa>~t{X3QAPbk36J%?)FS7)G-?O8aZF)p=gOu7OW>W_b3#8NKSZ4F>kC<YkYZ z!wt+)R8_Qt^@C&RXNi8~XM+iLS+7RboC`$U_y%$HS&CMDMsT%>-e|Wu4Aurr#RLUY zSmz^4Pw0EnS*LuM6>n~merNil(Greqou!0>6Yf&!ans2MSuvRNX^fDGSwRdZp5RNn zGufVmgS?qRH^ds8VWq3|U``)%e(}3vp!3aOmE{of@{BLEUa$rQIvJ0hbpgq5u4pdY zMlSjsr#tJ+=sqT$Gd7q*0t&6*q{LPtTvtlZrpVx+pdM!V%?ISCsyHOIln_{VkbW)E zglo8kzAMrpr}qsbVVCo%uucd1d=>|f^tou$vj-hE*gM+aeZkk6G3=s!VW@8u&y8U} zp{I&HHXpwT;UkZOw(EH6zvww5c4j4RtmvdJK@rvO0vedN_lub*uWCYT5s&v(#yMlp z(faQybZ2U}uxW%5eWmk>2Ja9G^;QK?xA?8p@qGp{vaO-g2glNbOV$W9z6!Zp7Y5<3 zL@ze&d^!83buoLWx`wZJb!F9Ku7kgd2kUs<i?wFO*!qpS$k&~Ow=yZL<Fy`c%{)(# zKH<f$4KQToI_rd5yUxRhgV!MDjDV-N6G&L;9QH+>CSMV@527YY(eL?V>5U>eIOYDD zX@A|yttj8e*oODTnf8&?lGg@2)y6C?SBK!-qqN2#7gdkz@Rgej@K%s$j&ZAqo$tGd zv$$3XP2(2vPnx5cX9***=_m^k<sG14X9!UrMCZ9W5z~|V;(;`O8a+%KVeU9`L`*cs z^~z(6qY;{}8BWhm$YQSg#uDB2RmAA#VEk57Ox2d@VUDRfd_AQG5t1*-h<jIv(z_!h zY_c>czBx_57JHJ+Ps?E7ib@FCSIFCZyvDYko5)Xn<&R@7-6UUyJE{0CRd8A$1|B07 zz$*B>DB(JQ>E8I1Mov3Ty><<xPi^NC^A+>)<7OM!C{(1M!h!@g=>b&9GL`mgbrNJ8 zZ>N<px#ZZg9@?Ry2s7V!)0bwt5bWK{@p@(SV$CJ0@nACT?2|++Rd%yt9cncuS~7fV zkyMS<*ap_na~$td-ORpKKt9cEB0Hr0H4bq20<uNzU_1H?maB&1*#2@bSnvg&#P0^X zb*p$In@;-KXA{0ot;hS2N&7QP_)Lu`Ho)Wnd2arU9&2nMQ4vaD=k4cEJLNFR>}!Bh zD{{%<P7AD;d_=t3BFKtKP6WtOe$$l(K4d$~>qv;P!Ec-4p3NM#XuBd`p7jo2ZtTMz z>|e-NZt>xh@4sh@#KX}k^AINAVu|0iRkV0>4*9r39Va-;^9^E;A<FwQS}9$jc^A&0 z{Glv(l4Q;8t`)#S&o4CM!C0DURZX0aDqx)QUFP}BJ5=xGA~I#Kmf-8g9dzV26WYIM z3)`%7jgRgB0#A;aU85)516zHZi8`Ch8m-IW$G_9$Qv<fMb=3td#>qJ{ZYR(>^fXue z_$jU~G=O&I6{%b9h7&G*rlct!_P1Qc%?o?D%hP=5BEc?r?=Oxs^dvDvEuFBE7rEXM zCxj0gk_F-07tnB(E4294Q0h?KA755QFqi%FX@gZV4takR@3aar_;G5@4PWaT?Sr`( zJyZiP51$7k-#o{nt0wIE{NtFnFb8Y08_4VluerdHjf`c;HPY=y$QIjo<UnCBt$ewS zjKA#-)x}ew!RZ)YHz}kY9}U4MN15x0b|u=64w7K^1fu+4Ejf@?OxpI#fw(-9NRdOK zq%;vz>R%IM{abjmUC;4LMISH_Oy=ix6UUbrQnU4SJXLMj==eN9oF7@Li~%$5kV#@) zXgjnHk32sQVUHhRp6z(HOTmt940sQ#i>#oo{Rq@8O=Z2~N}%$RfRAWM;nnY|q4k(d z&O^}@B(x4Or@@*22wF=IwfI(zg8q<x=@|Dz7)aV=qiBx!K3aC92%`>vrCH0Jc-K{R z{M6VTcyRnsM=|pWP+M^x>L%@_!(yiqI%GE=?iy6H|8X=_OY8%Fe<oF|_((OkhSSg= z+u_5mKvFlwnZ`HDLcxpGBr^R6of29~4qsi&X!~CxN(~aAw<DW`s;Pr-O)||$_vc&{ zBFR0ACtTS9WmFqvjQ7S}<F*%$!ZC9d*!$<w@q^tDUZLwUUnH}W*N!a`1_x@f?jn<F zTJ9@4_q-b)d9Dz8H_YXZCFD@IWiH%|l|fj4-wWnlYz2*pE75R-Jg`2_u$gb8e)f6z z=#C}HSd&TN;!jT8q>}c{kwHtlq40d(S!(`Gg$h<F<MzC#)XQ`UxgKT!dFK-t@9a7F z#WaviykUbcrpWLuP5L$a8=vsKf(iWXB1KlJx0frVdvN%^`wZ)=#Y$a2iuv!4!<M*m z=$j&%kKt~*jwI3N8lilb$#C|kS0k*lw`3)XtoaprF05&$xU`-qWAc~9v7`T1W$d&$ z>rSQcVa{#Fe@6&8S~-MoH|`Obe7nuOv%3h~%mK_2**XTyqPRmtx#}94z^P0$;wo=9 zb5{zs3y-gt6lzMAGRf89<Y-$LV=t`7+)xejpyh;vlgtmCab^MvXYb*RXLJeYDOs_b zeoEuPL}cFXI7Ll6HAIDXf5`jGG|fMATQwTTZrrE%+(OVhSchNI;7)wPcEh=31@5zF zrv3ZsF7lwV0Gx}{sH~h0<Rr$T)teV^J8eCzsgcL|Q={>b`FR?cD+h`>!-zw>1O4GJ z7@j9-Ler^xLZ@e0kdYz8M^m<uZ5^+fiiyYRr1XQ>DxXM&*Q4mwr}Df2TbX<Fb`yVp zYqIy932gH0i!o*S5LzEj>bdXG<T(?&mRYjj#gy2~#E`qcx0n{$moPHMmTVMQLv*q- z`ZP~Qv!7xRp?!nKOm#+&GYhb`?I{Fqj)7&e;V||<GIZN0^B(ph?_k$$I(=LQ`!@1B zh9#}zGta!HLM3C?%Qgl#%dUm4F^8c4BWqfrIt<iJWm&1??htW};dQ=6LeHiUCae1t zWcMau-@;%x-7%D%^4tn`xLJ@-J7ZY0Ehlh8|I>`MT?_5^^aW(hnE*E%tYO7=H}pO& zO8+SCrTtrn@@nCixQQD_z~x>W@?=XTQIC{^K^+Fr>G=`nOMT>aq<N5cD@C{b+U;08 zw}GCPa-dFBk%^jN0fHUjkmfmnyq|F$_IwBf@8?BA>)JbnyZjdSzdOLK^He7~JDqUB zi@VtQBog+{-%Go{jmH=<Gy3dFEAh)I2GiII+HlyAtq+iAtLhVQ+Y34NgkmJ_G3>{- zx0FFiW)YDc?F@&`G;`UrZP{&kKIk%9ntzbk9|I@d#PBU@urV(cZme2CIW;>lB^3~{ zqy@W=KcGv^7L!XeztWv8Zm>WKFrAr#-^vASM0YyAnq|N$d_My2n>29ih7(wR;~ACE zs>2JylMv)n2s*B#+0A#5+N@NjTc0oG9b<cN;*alef0YHFYuioy88HF#;SQPgO@dW# zR$~|S9mto3s<K8EGkK#93p_A>GJnyrneOb<&7~Gv@>2`cVN(5mSho8ieP?%@*zKQ; z=MGtdjr}I9pYoF=$@wCb%z(ZR^dLjzgC`$<&6tdPPVT<eB|ihYsh9UruGiovX^wkL zKDwxp7!6O@Kdy<=K}YGBFVb))<u%D)dKJCQ-I({*06$+_kR2&2>4k&`bVr~8cturn z;>}ikE57F@n*_s=Mkkyw#}_{&#6exX9h~=5WUp)7rMV$0;QFSSOg5RurdbchC4EoP zyVHlkj3HNOyabP<Tk^r-u@*Z+@E*n-*aNk5AHk#HRVaITDfVh#!S5B_uzKP!%<_oj zhxY`*<d!aS^~G|$HCGokzHEY(JE8@`BTn>{b}p2!3&nk%cX06P{erdvZ<-n^$$EUq z!0VZ%WZGd>_S#WxShHV@$=*F3JA~t~UEvxU6mDV7)u!{3adTO@Evwjm^)~D*%i~np zaUh)}%7f1rykiQ)?IE#zD{&p+3402{=>*#p5KO1A;b<6V-X>2U`CKNU2U^guaw=0| zwgBFaoe5<>G+{>KJrXW`nrxOxf-kq5X?>C=xv8B&97m+%fp8uC^0JADZ(l-hJiLOP z*{5MmLpCAZF3|9BAJvyF;*`A+b|ue5!wN}Kf8;E9Za>bH7d)bQvl!v_>JhZ@WGyV! zvtSRZyTFvH0MZp1Cy2}~fb#QqFym)6?N}wlS5&PcG3^@gd3GzfDW$@I7n-c>xiU1F zE$y&0q>kLKRpH&^i!g6{0^htSgLuDIWkc%AaGP!obeCnpv{)1Rm94=IS|x;i(}ZGk z%@_qTnI_)|VUu;`Xk?aJ)tM}P-YVxjLT(Zi89DJwUl%dP!*Y@FNap8BDYB<l9V4}; zblE4Smi)4F<FJ{}fLk7F@NCr~n(pk#t)Hz<rr$cooH9B=GL-L;e)YZ%ZDSRg)hSa* zd(bdi**B5|ikB0=Np2))?Oo19<30@t@}pA^pC{XlM0E&z4CoU%5IIDu)Ulr=iJf&w zxc@^%<=V6383*&3ut=(m*;>C)An#{NpFLD$mS5MP@3#yilI^if&Q=SWa`6`P<B~du z!G%n9@IJbJ`g<a+r^!_xOQPBaL*U-kT58rDO;@KrBW(8!BDSZKwhYRmGM^-{%Upvf zhb$0jo=wKZUtydLRxv+HCCHGp+rll+FLH-Q^d&XtzcE+$Z)C<@4Wa|DCDUCuACkwH zQV5*V!15E)G-}5WF5rYZe4BTTUap!$+f@amCnx}wl^eJPmn2}TUlI=7{+V8B^@KUH z2Z(&@X=qy&PK#D}q2{&Q+@(@mD!#3iV^?Mh_s+OMo=;ID)mt`k2Jg>Nv8poeu4wvx zB1}qHs9?g}{I-KBn0|+>PfBEbBg`CJUiM@5SZb3e)^eDz!hpPEI?4F@Nf30-LMT|< zN=gi>gbq1TWLw{h<l-JhL7-BJ!z8cIOhWQYZl1X$ck9TxYAMcu$=z-SJIQ|LY)le! zskVm^H*yuQN0hnk*-M!<re-AlQ#s?8GZYQW%gE6bDcbe)8nyOZOlF_`z=*dubDzd% z()3PkDr<9-DYCppe_s1SO%}+LRY{HH+uSq4_xBtHPu7jY%tmu~Fw%{?GGPSqa4vO_ ze^pPTN}Wh**9*bCyLUKCy;*czmB?}l)27<9_K^h3D|Ca2A{-mE1Zys<2}<dH7%n%T zoVhFHR_5uGS&GleoSPTuF-0+Me7HFYP;DapIuq%5O*gDHw}La#%|h4f<6)CR7<R4f zWd>b1EJ{sjkUaMdpw%UVP02@?h36H>!V&Rg?f%oWpluCvwS54&7c9nTT)seJ6=a~% zX)05#w^Q)ME16{PT+U6M)t6)iUnHyj+)0mvJncPunucFpEJ~fKllv0pWcR0&w0C7p zwdSCkOtIB6GR0+HbyoUTCTdF*m-llDtbH9;9Z4u>u6&wneD_VTVncr%5+v&5^IXVG z{R-x7{Yma{U;-07PMUnG)x>3n62b}T_l0LlwlI(M?vO#niF8tn9~r4V0P@E#B3iS1 z>9bo^G_p0G-pl$V>>Vmcwwey2{SQuMZbhjuxnGA8v$W?_ttebDSAGO}`#Mw@{aTUQ z1Wkn7U*&0wvjf?3qL&-4JCe#uX=B%I6ME+GesEb=ND5r^v3Gm}ouQ|QvG+>I>y3f5 zV&pPRNDZM{J)y8^S3elKBOSIs+aa(#Z;KVnHxjS!Gr4_9H@O8r;<yFvsjxC<BIo|o zoSRgUE}W5KP5kujiP1zYrbJnTG|kSbYWp@AxRLpS#-wT{Y5ROKK70uYEczzuPlJii z$Oe*-HXC+y2Qf+Aab*5;38p#?$Q-hU+<h~OxSINK`+Nry6CVp&Qm4q=@3|=q9loA> z>F)%=EB0}ZLe2<n=kFxKwp-PU&qt7Z$K^?{Q3fM@^Qf?{<tv?R)y^y*Vni3jtfJx_ zw~1l&A?nwgLql^0aFNO9Nz*wC+Ent5Q%6U-;6^>sZh1-WPaIB5lxvtBCBxDE(=@nn zv!<G@>_^P<bD1t)lCd8#fkbUpVSZdk=Ir|>Zbz#q6>uS%UaWjd%z{;E^n*cQSTzkT zv?h=dyI64A{DYnk)F541H_7#Bml-veY53r_Ar$l}BQLjBk*N10vAu8$#%)T2K2AGm zr<WaQ#vjIB{RpO~q=>d&oW!U+G9mfuVt9X-Ea_RHARuL_jOi}XBQF1K8SoDWb!o<4 zSMrTcNAZXxTBG@va~Q_JaM6G@LoALQ&B&uoS`JWaBSF6%Pk^Sax44aytvUa@8O*^O zO4Mt{H158uF&EzciqY%Q<@Q~iz|g7ZI2-c`933Ic{d|zl{1;dD&nMCT&6Q19AVrdD zE6JoDV^TU(8?F_<rM+fSm>KYdN;DiLek$?Qbh<uMJG(FH9xZ3W7yRHptlTUtw%^7M znwspmEh>gRTl18cv`8cMVk{2vHe;=k<u9*$1m`Z4fzeGfw$(`kWT)g)PsJ3PT9C^< z-m?m|f6B0*90ucc%?`n?m43{og{QIdPCtCO?JfRzQYP4}dW$ppz8r53UIc+#Wg+#t zEx7uH!u)mb=?^?jic{Y*Ht+rLv_ca6e72rl7*omR51hu1@789Yy4vuK+x0Mm`m!Ty zMqqrE4;$HI>NxY<KK`(-5PUXy6PJn~7_Hug*N$9*!l{qpK++~U<mL|;^7#^E?aE{( zH<ZGWxoW)S4i-9eIz{Dc#t_pJ3$teZpc_8gaJS52sgqqI_rfus+>Jj&4R3E3>dsCg z=j(^DVjmZ>PYvYx`)1-bCqr-YImi0c3^!cH9(%u?J)1I#o>D6njC}i^Smi##SO>T2 z4Tq%JKKD(bGr@xY7<88CEu@^(h+1aJ^rg7YyM><KzL=M?Fb98|n^ayP8nV_%!7>*; zXu6$BTLzvM`Yh3+;Wl+NvXGKhcdRio;v@a`G>B?!pNJEi`m*?;&B0PGAIzt1s2Q#g zjuYFDv8Dw@aMkPu4f|ri=MNvtKAs`ZM!ReB{dpsPZQDS0`kH%!-1SM=U}phO40>q! z$x5s+c}OFUpTg{#IsE?HMNoaD5EO>Hu`vT*66XvgkDNT&FyAoYT%Av(_R==07}`mk zw!FcL!zHBMelu0KAI`6RWx&78DxkL(PaurkdUWp8t(i3~lP#0Fgn22Fd_wbT)_1Ek z`(PdNjq_G;S>g-e*^hp#nQFCg(d}#+&@!Dj+0~6-;`{LTHnDtE@g(kXzuWZQTnXwo z;sM~b4yx~H3NPC}5=Nqgj(_rq)(@`WygY@3HD;jK!;G1dbBoM45=%#?_!Fh(A#B0e zB0kC0R`Be}d%Pjs!}_w8*xH@Ojvcy`UpCT%^glif9}l;|x`!H$HD@ogk2j8maa|wD z$=!CaCu=6vERur!w=MK-dMp*sdPcq|G_hi)har61a&Gt~BmRhafBJ)+#+;sP&AS)P zf?1bNQ^v;u#Am*Qj>HLk(uhq|`y`NU`s?_w{SuLmw&L}s%7R*FDIe&(5u=m**=NS0 z?Bct@{MH?^to8UHaL-Fd@vjHij4=QqqEf`WE!%K(RxlfvtBMN`-KA>MhrxEN1$|J~ zUzAN2r%QNM41fCy*8U8|u<gNQ-F|fvS~i{T`drPJl*TcugPO=v$9vpxYavlS|42A+ zLjnD%H~`wM<+%J!LvZNYjr?bwel-?g?kKULga5c~0z5dd7gpGXLiY+;{>zai)c39p zsNA2)U5M4=4yCHW^iPSfI&%(WuhSNW^@y=|#=j$z*I0qVcwj?K#rff02f6l<x4?s! z<~}Jp;KIDOV4`G!u*I5KC8tzJrN@vr8WYK;yK?YRSrZa!7jcK?bkg&S3US-{G-!D- ztVX;~F8?@T1Y7!3j@{N$hGiaEWcwZ#w7EI_qme>>%CupyS6&`H?_S`X;W5q=FT$7p zA!PrpTbMcYBi>Tbf#T`MV6|cftGD4KXAg?3LS7>0z4j#iG^`YUjuI#CQrTqG{Ty<5 z-UM=ceFtWow7|t_;`m}uGsYT9vW9X8_=|dlM7_=pnwOZeS0DQGUu~n=caOsP%eqN0 z!fiMv8|<a&i)z@UjRmZ8^(JCHZYz4rXN$@^JD9$&J)ucF8dF^Y$;oAfu<vRO%k{Ye zzC+z<lhk*JR|+7<-xN{PC6O@sMHLhH?LOyKznnWX(3+d>rHCJwmyoZ=N5P7B-^oL< z#V8-OpIqyj3gh@8*j!Z&2CFXO##OhFTTu-eXE`=S#fSt(2C>N&Vytp)0$hFb67TB` z<!ZLpf$Ebi_Jit1oN}lyo9SS|YR`KDHx5biM&b9c^Tb{B+oH?I$4SD@(cg&JSQkD$ zRT=CLyd&MuYnhcdY?)e&gGmMA`1#XQ8k4sk602qS{;Q|)6;m^)_WE~@9aTJ^eJ77! z7haC8(h2;y0ke2F%@bJqK8s(@>p4!iHG|ilzW}Gt`~U@SEZGNF#Q7I-Ua;=&b*hc3 zut{nsKF=8h330p89we#K!RyeXJVEeyQ9RZ2{{Z_{4pHBgTF}e<q_y2&X`Z|>EV^)q z6r2_ls%+2!1J00@a!_L3+BNw7<{$Zj@ZpXPtHf&Vm&H;G=O553y_YpoSj4;8v_i1d z4gUN3&7$g_5`6on5?ySz!|DzpiB(0i>xvAYnD!mIm+7*buNY$Has@W~yE1Q|-xrFH zC}E1v1KQ`xCWvufC)k@RfoM}hvi3UTgO8^%tzrbu_AVozkG`eD)XV6U?9Ui+P#gD! z_3%x@qSzSsT>fRV8~^mc1pa#bSibwb25U0S5Dw1BXVphlLYB81`&IcWdEuvtE~_?U zp1v44tSXJgCXHCP<vtGGE5&y&vtgS)oB$#j0$0z+(Kj_im^Wc5#KY(%)!d^(HG_wv z&!ZkDIaLK*>Z(ZbwWrk7VHtJ05=D;MJcl_U!`PzCt^5roX<jbo4ZqBT(l=@HICfPS z`&#N0ovxKqZ6URh53tooS-~fIfFDRVt+j=zdvq}W*#Oa_2N$v8QeW^Yse_PX*>vx% z2D0b71f7}ffopdpaQ4*<I_r<aw%8J4Q!<&%Hs~P3o<<NaBT6zB4!|+37a01yAB|4D z3JN<a*qNgaqjjWljemR&YyVJ<UGY$keKmF~8~nfuE^GGV53Unp*h$?QDYrDf=4L#n zQ!oVPCN06h3sXths~Yl@3}M?R9R*==E%lgL#9w$ih%RrEgnhXaIjv=qaDJs6_Fte4 z;sSM|7;}X-1*yZ9uPaIC*g{$~Rvoq-zC`iKe0n4DE-qWPgx4Hsh!U^uYVJBLV7JR8 z@y0HBWZ<0fyh>Ceel7BWk*<-fi`+%_ph-SSa#%=$h5ppjv;v$qhSITSo0um}F0A$F zP?ETAgHT6Hn%yZF2$n*3D6}?XPwCsk&tpb#wo{pX^`Vk{?<0k6Q*xOH`bLcXFcto6 z&~0+{Vl171@DeWSJxC!Wf_)Y-0qXB@ydu%zT?UBpV%n)VXY>-*uVf3Hs@#Fc=jh<> zMP2;JqdEM|nTfdh{3$#zNgw=%tcLInxs2xG;q=*&EZFp@o9s^$xlB#B$>usKW=)DJ z`%r%~DOz1hBBzgLtMg;=o3b(Fo6HrJv$~_|k9oKU#No8|2NDrjjBOUy>}aQ{e0SIx z@}ciI$BapH;NCD9cBk7B=m>p{bLY>Zog;(j3$Boj4%BoEJQ%{doD*aB*(Jjjp9b*R z>kB^vis9#$6gKZ@1lR=?k-^)d+40^jFs-m1gsD5wtN0LgGE#saD|Jw2(M7UIS{HW3 z2GXRG3rySodnEHt5YyXF5*IkLaNnYW3T8;~g<Gmfnv@Hlm*)cBZ?gFl^eWn@=Ft4F zPjHCge(-+Wz>hwEgPU@7BzvRm2kz7TOvZ&%Xm2*dEI)aOF~5SrB`M@*NIJc-D-I_u znu395Q8>{!o%q#Gfh0D86fcygCNB%f<J+0!?SNeRP<We`K7UOYjg6r*>)QmGVg>Zu zo)j2yzdzMn`WD9Uku}2xjjB0)?E?R31Hsf;SGdA6r*JGr@O7o45_)}k{>u~5_vEq) z+<9y$4&5>WH9BUJ)ss}Hd|(DXY<*pQJzEuTomhghmKKak|NUfDRt8*MP)6^!MiR3N zV&ErxiijV5Kyt?ClUe5<(~$!+VZo|Ea;I1WGp4*H<40Dot=*X*Rd@+L7O3;r?leFZ zAL`iuPA8jO83}!s+Omy%1nldBmFx?&V?Eo=*>h7H(7Nsxqp+wQqOJ;%f5X$_H_wQv zTmw84Rn`S}_CiZp2Gv--0If}TfR{oU*L~lbT1-RobNF+*iXDoPqjr-O8hKP>oe7g+ zxRz`=bCDjJvyfhywu2S#E6(>f&%gtT8~L|Sv|0C?$Fb~7JIln)<o$U+UVLLE{qW8e zPCWSusmro~#Okr9R?8BG3&R?d(fBs-qF_&6Bg}4miB0kqSg@cRE*^G*%f;zXw$p+= zd#8W~O+P?m*Qm40gT~;~=lyX_+87%4I1KluCg2?A0kQv_M)q!3<==Y}ewyNL7%}KB z=pI|ZHU%DqfLn&py*`1M#h!uV*`rwa8qbxe$k)WZ(Bi|q(s0D?ll=Fzy?l(KH2>&I zEbx;Xsn+uET;vt$YS?dsw@-z0n{*2K*g`k{?od<qwo)KD?VL)sDzI!`*gIi+e^L5V zR5h5Lm`2>^m0-Zn0P_5<BC$Al4R<8mfvQy{Q0!g|g^q4Dqo!YHuYL3fFZUbZBv8N| zG?q;>TtZR=%IvsEZGI+OhKUY@on#UM--1E}O#^yx-%(j!*e`<HBrf2O3FGO-DQ7V@ zK!I(`AIOHfhSBetYP3b7m0nHErc>5i(aU*51o@id==WtUWWD+%F4a^V8;g>;G0JzS z*%Vzq^2<vy)hUM>evaWIA75q_4V+k+j;Gwiu>;UMeHI_n)Qui<_4#MHYW!l~7~H7z zk~};8oGe_o1+P!rMX!f0hg*H~FzGYF6uSgeeEb3%%cjDmFLKaw;Rcm>c!sRiE#(YP zU8P@CkI`{UJjk)Q3i5cEmoP5)138r%QSJXx7bHwh5V?0AV4BZD$?$MCzUgMofW%Zd zGPs|k*C$<A9_Pls)+%Go-$bLv5hECxvX{3LTC$OPMr6}vW&9?r#>1(`sP-|3I^@XX zfF(PrpKBe=j*wx^9c!?q^%$o9QGz|eik@vc!|Xf!1%~cFi30xt#6b9w6fI0g#@3a+ z+%}c-aydq}9Vcv^QaahMKAXSi?TV4t?(*M)5}2JILXd0$dP2jRf8*Sa_fx0Pxu+_* zRujs;Zqfw)lohlc&}3r`Em(=qS7?v34p^J-W}D-^vCoeyT+v|)l4L!|CRn4`>l_&4 zl|tt+(PX2GF*Md5ActHt$cgtGIHnfx!)qh9{PZL0mKaQ!p9XBq-DG}w!EOjuk$_mo zY`#9{Jw_BOIlkK-$gd9wro;M8$D@O+*ilXE`H@!${K$U^BbOh<KIjHs3zA@R_f-Dv zE<1inqXhcS*^A9d-BhKf4_~uo2j=B!^Vc6=q*BGTg7s?w!>1VGafQjuiN3vL$50z= z@VB5cVmxhsqX@Cl7s-gykJPXBE2L;}e5x3MJ12J4B+oVA_aAJ=`nAA*6gRBdb|H#g z-{mEkFsX{T8QAfkDoS9-8BO-{W=T*v=E4qq7S8yjbTX#r8n}$KjRI%(F_i8}gu8K} zIMc9={G}`VuRO>f|8T}>^OWln)T6#HTpxF?`fxKx=Gt)dz`$fKx#cC;4!%J0MCDd7 zg)C9f@n^2<NdOynjb0Zw#8aWO$wo;TIC}Uy?U-Cn*LA(6>dWR)-&-m$fzx1?6|SJ^ zs);xx>oAdj-a!M084$nA&giB$t$JqPh45co*+1p|n=5NSAwuwAW0LUa9#w&q@0jYA zF22ev>AT?mldTSg4#vV)FE#9D*6kBsKe0hjG|AIp%`<&L_K)Q1n*-AX?J9#DdTu0C zfAH(DckjMa%^&oxj@$gYI$w2EbxKyFgK+uH>Klzq9i-MT6lh5+38uequU1vQUcGc_ zxZumG27%SX$3lsr_XM8~^$6N|+3H!(Cpc)zh1eBz<O@p+76`U22&yc2dPz{ULqVu| za<3rr=3JpmMQQcT6Njs%h_ldozJah<E38^=w+z>$qv0U;`AD@BTPL(S_)=&;q)VvP zKUQ#5ZIwV`<7}Z`R;)1h)HlJ5iD!g^G$RE=mN;{(KA>>e-iZ#D{)Zg4jqIvkbN7bB zlKfI(Zl<X~g<D;n_I{gif?KfQjmlNQpkao>sOVXYzPbc+YuJ}+t04=j-?W7YkHrND zZ^z#ddM{tiXhg?Wy?-7jnA5Jr^$Zv+Q0qz-j%hhASh%WII5lh-8)m|?Z-SNhBqb>} z$To%qjWUOjJ(4`1Je`H=c<lP9P8>UyqOnX5wGKQFU7yFX17p760@0JD6Zw9yv$cWy z(9r>&o6h39;nM8SK{C9z=;<RXSw)1i!-#9<XjrLzf$L{59Pe1!v*z>karks3P1`mJ z71G9n){95<kxV7($Y${}rptJr^klHq$%a`mwrpu;Ayg?Q(S8T}ve}s-B&*E~wO{Rq z;+m~&#XC1%SM>B%TjxvI8nm0$HmpLg4QA|#{yT73m@b@bd<zb<`mvEq68SzCzc2^m z%~`L~?HI36P8%<Yqv;GQs$%mLRosNEi(nr)^*)ha*cHxC-W$#PcN#ME&>OUx5x|-& zw9()yC;nOc41VRD`RwJx1#EGmCa&vm#6Q0&;C=7fv1#KDvd@Q~!`Aupc&$5*>=5I0 zzBqb3f5Q3zzE4zR=f#~z>xYBbE8mXr+6u?vgT-!`zq1f$tqDbs3GeBn9?^0&ErsqL z(FB_n3Eh2MnXOpW3dPkT7jxn@x_6o~WRHl%`ur+*^g5XvDA|ntT*mO88;*m#^9=N> z3WA&Une^5UE!Iq!i5HB{L1q3cH1~JmHR|JuZQD>BbXAw#sgaKR!HqvDDioeKc^~nR zDFfXROIf=Oxzu~`EzF;P6azm0Uv-^%G#6pm_hpwgODM9ZL=jnjGxt4~N@&w6p(I*t zQ7S6gRn``fC8F$GW&O?E_lRggLb636t=grO>is?Md*1Ur?|Yu--#K&U%sDf2&vjkj z@8^3fQ^7sU*@v4&sQo^5&^U?3MM^hN&7M3`%v2Ee^mIVgQCW8PegI|ReViCw!$j=j zr#Rbpu`e&nP>#7VR8(*#E7H0Qj^Zr%c`StGw~C=u)-<pkYL}^~v%=J}E+MuYi%}wb zZ?N{Fm8?Z_7~q&}#ZSg$*%{w)P8pcPTFx(E4F+eiT5^0;<b&;0L;NCEMW_quCtZO{ z9(`h(Cw$<V+<yF<XW+aZ`yXzEPTUeKMF$Az5X~_W+}3@|X{qQ44*B&4VI0uO6|%WS zyf2ldcYUa>&v~_!4lj?cw{!i+RloG6esMq~N7y5S`!(|dkt*<lv-)Bc@x4jPyt?O; zMXz>(g^}@<`m5oWEDQ&>Te$dbqUi&Xw1}Dt2P6}mGLfV7v6zLlPktkDFEgE95I3Ku zk2!Ky>N#_jZ${99+kX+SgFLydDlvqfP&(aH=Rio1S+usU5|LH@kF#vBiZj5kLyU%& za@%egkP`py687<S#G#w1<{GzUIJ*|9bF9DETfCh4LMMC}<|atjaQKXV5N{K!=%*V8 zXv3wl<Q)%p#&=s6{ULKB@mXXCciq_|^hB;LJsslAjD3(Jiw}fwJ%71y9N{v$KYKHI z;To@wot7g<W%TPiY{Z!5!M8Y~3YUrMyCuZF)>HNQ72!nD`3X*++#qrD$0?>%k6VAb z=`pe4Nf^27ImH}wEkHlr=HUAmqJdQAD%gMG9R1DRlnuA5CI6C3(er=%nP5tfGOuz1 zD%MV*0qzCypIV4S-TBZv&K0Kj*D;che&9nhjd;-45pTFq2lj+G65qU3Ei57&fkyZw zYVX~`h;VlU3vGF@c6tv}nZZRRa)LN9A%!`7f+jhchInq(c}%M%F?DJOU`CA$9BjD` zbhI7t`w}A{+#v&-u9btWLyhF(ks~PUx-%V}a{!K29>NYzUyzbLkJh}R0rpMOz`0Nb zF3#_S0=p|<N?SW9clM!Y_H*$io)J#1-3=M}2*LcV5onE~BR1b#i^4WXLxE4}_{_8n zYqwFF<=YH!#1&O)dPgI3PbwO>UEGL&E<TUPlwFX@lT`4?@Dkcr9sqwyOyExw4;h+| z!_++~M(Q`tA*WT6STQLP*%|O-D*q+&`=>}9^Nz=}oYpXz<`2=`mVQQHPb^cVmJ52F zddbgsISi-x4*A3(f!sUjOX^4LWBBCPfxgt9`qj}3fXV6<@@BmfcBYhw<rkj=C9f&k zO2>eCh_cDU8>T>iN+Wqug~O?px<vNrI1}$0TtL={9+alfku7!uWU0}8a(Y)NtoW9P z9;MzumGaA=>9zoLc0!jsD?=3Bo>_?W*KGj(`?P>Vkq{nB|3gk6_aU!ym!VsO1tb|f z7Yur}GW)wX1HnUjV2bDPDJ%A5yycgYqm88~_Oc$6M9*S^_D10HYktgRaTqyyKAtI( z)kUR${^BMTJNPU<0FHWeV882+!0o@Yi0-3VASV10bJw63EmOXTg7nRJ|9})k_J*Q9 z(IUVpNkCPW1R9N7h2;8rP|bS>q+9V3jWq>8E8FeJ&&LFsn{_Zc#1ioN?tIw#QX1`F zHbER&+e<E}{tLcjyP>6dSLwXWN5D5b4-8BdkRL{0(2Do>lYBcqg9PLAB$Sn6EQU{z z+h)5HZoB<R$pK9uVE2+#-`&R8L_0zIod!hz*&as4i62%V9taR3#&~a5XFPla$eg*W znbrX=@g}kre9q@%q`hP5nAx9+q`4o-NYe-8`W8pDN<j)q*b4xqNEhV3k{^t^$-r{G z<sf2(0s6Zqohv?Ph~6kS1DyCrK}gXd?CQafOgHLa@iY>hd{V_+yd6T=-}ysJaMQt+ zKbJ^1b#tU}@hbUZ#Wl2sH^WOdT0#E&>yJdNw~_o67Pwg08?E&i2PeMAq2v`gC^dZm zlwDYhZitQGpm2T+hi`&t$4N$JERdPy=80!!TjP;~C*jLm-Qdhxe#o5_4TNPFkoCD3 z-2e{Ixn2!7O9-Gn^CdvaQy236?{8@Qcr4T{3nVkv{{`l*alG21o3trUVq^wvp|2Qk zl=|T%5o}_~zVM3$&XzBb#9A#<I9!7&lpF*0Tb?kE!w?+qzY030y?9i67_75ZMZRa1 z*xsqz_;`USQ)Z(8&D+MnJSq;gls+amaOYry50>z%-857C^e)gD@?<WoI)(Zr3Yn0m zQPk-*jQ;KA8F7zE0h`V*97F!QSSC>lsrnY;`A?i-w|z6?)82@l#F*l&yS2#OJ_c<& zcMd5!^B~(#m*6UeZaml+36oFWM>Cf+L1+tN#tv3M@s00bOpr93`K$zcJ#)as_Cu&b z*`NGvHxFpor;t15@*uO&WypxG!c|ePac_qP<q>d@G`M#Yo>Tfx_W!LyaKH%8lKu<S zFO{RN>LXY<Di8eLQ;H=x>a6hAFvy(A!U7dZz$5)O`WY92II$eicl#O++?I|PTdv3D zr|a>BrP*-%WkdMis<QuCXaDauU^hA?#pU;m<W>tM(%MTF<FOzyJTZA3xNnxi+g0SC zmV6`b)V$0&6}F5Be%DF&q`yHMV`kx48*k><>>TdrfI8B0OAxZPwm|L88syf{O(4nN z6rbq3fSkjt{wG!T|Hfbb7gaX-p%3sMcf&UaJwdbA2>R`s1_rtek)urmnAJQNF4UMs zn<$CUA)DOLaDo~x{;ES+o+_s<%~OKqR)^VzFMX)pd)laYT7;TDZcX*^e*q6QTA;rC zHp)y*tKt5-2c$NL#F0YM4BC{-4hQ{#d}$Y8wE8PH^yV|@pREp0?TO@iskD=1+Bgc7 zNCAaE9oQ*#XLjv}Pspk-0oe)<(L$G`p`P(Argr=(ILH@9j<MZf!_CX!{q!nIJ>n9( z^F=GPl5w=m(deV@J|BVMUY69^UomXq2_+yl#lXbaO_WuJqNU4E9r6N?;q(eU4U6-i zVtAByy${)F);}Lu$zK885*2Z<cO{(OQ3yx19Kjm-Tj0o^tyuJkIes2656TTDk`H!r zpkuc$Gc~UlB*z#d+lEAN$0{6dPMJX+asl}A#5rnc?ke`vkw{jxHW6L;oyxAR%*Js| zIh2C0A^ST|j9OXa27CLS@+LELAThfQZZW}Vp4nv<d;ru{=LV!ctitY-KLekINx{>$ z=U_<5bzFE$oYFb46#A}PPhJZ8MD9}h1`G)vGczIxBV#I1<T5@u(~yo9tv`&x%>+E@ zbQ_IFRDplzgs7`Zxs(l0@jPO8nF^iAX5)2w;T`rqTrn!r&~RZ08+mPEAIN)AU$Z~r z`)navnRf*ioD5@s-ipU_4~Ro0zd}}6K^C_5KEum@aj^f;FZAM!BGohVh<>HH6rD(K zLc4vQF*pC-MUzcjhJ0NBc5gop4796|Sz#Bt|C9!|_E-Y_&FO4wwE!!*g+o0G5`o!q zv54=|EimAt++g47&mKq+q?WzDh82|!sCl+E__)nI_|fYOa`ACyl%Mp2L$^Od<z#o* z`QrmTaKH=0O+z5V(}nHR{Dzk-mqKE_{ow4cX~5|TfnsUB=(;!}$4-7kibV^NTaW<q zIigIq6nHc8`6_Tymjw?!jli$&7^qjVl={0XtRY@f9^1ReP+#=dz|-FnV0n-x6=ZY= z=Rcl9xd12jl7k3(=$Z(1W*)-R|E$>Pu2Otbe+r*X2xB*8EXU%a_mM`&OZ>OxD>D(F z43BtR1W&aN;o~!b_+vmW+1t>@ELy%8W&F7dE^mDTme`8n^+W1-Z>a=GEj|ZIPE})% zgBq;X%`I$Eybm=z>ck%V*Tu>Pltc4xkCAwR7MnpFWW&>bqNOuhFje6qbO_v!e|MGR zwFft1{_kG_$V!2-K|0J9DxNWPTaL%}3*&kEXEEP3Pv-glTfln6fpw;}Sg)@2pjqY` zY6w@xqnE^CPa%)7P7DWbi)5j^PbRc`_zQGhK1%VOmWKI%Lh&-k2ngf#EtCEG@P~bn zG7fBk5}qZv-E;tZZ+}3o>TqK#4|L$)_L5i^-oynhKKQQ0OXzigpBjr7f}-U$u%kKy zzRHnc<NSKb!|MdmOPN>b2XFct9%TYQ8#Y?34^xHf)t@tWg}%~4LAGT7hh@mT4uFgs z3aB$u5H^|+;AvwnCX$sYX|S-ND|j=;_cGYMFbmv2ri-_zoyD&o#6V-)b`+AULiKoF zgI^Cd1CDAJIHhwP>P%YT>Nq8I;Z_oCe!UrZ6v^YY@kLOsYl2J_97D~ybK$njtEkm~ z*HM(nFt7l%sE>9dX&!HP{)ZMA5A;E0BK&LyW5T}gE@y_!C84(IMO-5`15D$0P_<Gu z>@Q9v6_mLcKTDertzv3n*0(0uW%Ls+oBbMksojS6=|$`|^EiAyd<!LMI|e%h1*kMX z2LAFhqWmP}q4byxJkY7hy2oT;XN541LqR{grL>Ke-mlH5y7mL>$|mseY8RL_A;AkV zdVuzc?Lhl$2_P)>5;e8xvvu52tgDlUhpA8;Cq9=AT3Cec=@#O3H)?42s&nwBbw4wA zZYw)^Lxy^G`Y2U&)r-xs*usi$)4-w;6<BA?3!b=l5Z+j33EbCHcus;a<yfi9F7J4Z zA7->LDjh~>zuQvmob(?2$-T=2bBEC2twFFB>N1PpW^qpURU`7pFSMw$ky-s?KJ}@d z!m0ZV@t6Kd%H3*HgQUZDDut|Nj`>W$;Daq}wD1hvBN0m76Ah!*WJywe$8+JtPXRo1 z<`w+CN*Bs{uL4>*dw`I|5bW?lSO8sz*4;E(`m`7dcWXjE-`T8W@*LLw%NMlj_6!)a zEJ59!4a_q?S(r68K%U0)sK<*g!s0qbJc57VTg+wFtMEJ0-Ts_1&@yBt9c0;%(=k{h z)rK|uD#to2gyQ9&FR&_>ik2P=!eDQ03At5!A*R>VQ=Ur%*^|%K;Hj`?%C#zm_a4p$ zwsbq(`FcLy5i5<1N3+4m{Rr@3OoHrQJ_%H!K7rO9+2Hu^N9b#ED{=^^13x^Zft5o7 z;S{@@bZb+`@518|b0>z{=3H#4>Us}6dPOs44tyYZpdCjzQj}QpYV6)lP#4n#c}(^> z_`BE<H~iUvb(;k#$Kx`P_sx**zEw~+PJng27KKEv&BAle2*9NNC|q3K0~QH3qeE|b zN`mr2{ARBSxHBpP+mzp<Q%MJa>Ap!Y$D<v^bmpQJ`3lhe_8c7E%7sm;-uRC7J1Q%F zxn(w6L*+Ej!Mf%GP_0do&3AIaEuwFr^`0uopA-#U4rjo0ml9N_aTeN%I^t&=M!-iI zl3Ks_ITYRzjA#57Fn$>ZI6gWS_o^*{H{7DB<A&bUm2;NJ!tXDdOq@XquM-jD#bquZ zuLEwTwQ&8$LKr&v8MT!R<Hy>?@ObA^C@2zZsXqP&J7h^W6tTtFc&Gz@G&h870uOxs zI)U6}OW@>PS-c(1VXKPeU_7xDFa1~wWma?{=Z#Z%uXG&jA0LE2cW~H=*9X`KuXWic zM?Zt92eVMzKp~RT`h}`}?<4NXGe}xQ2<i{4LmjDiP>s(duiX-cXnZ;5#h9@>w|vAd z#u+>%yo!<<iDIk9EGgB0+t@rqId+EQ3p&5kuu^l3HIy8tM0@W*g&W84w3!$F8k0{| zMeDGcbS*CS`~vJ()?ss@4zjP^o5~UMB4U3&L#K0e@J>&}{4#7LjYAbs(v&dXC@%yK zFZ{`=HIT)M!LL#7jdSF;WB0+vgZadWqblX1aTRrx#6$Vbui3?=%~a2(7p$jYBsBc@ z2$UD!g6T%v*yS%CQ}c49*?$vD(ZI>exTbF-ZsmpI3{&RR(9<VyTU-=oT`8*fP#^5J z%)-{OW{lPw0=ZXMfdPtSQg@t3haG(wvT-}QvtkiAG~Ne-*5x6mdubr0R|sG6yN*@~ zHG!TYb++PjDlSR<g%+<^&2j_>s3(VNsH~%w*mX|@oZnoI!?(vk)9FmACbEFF`Ev;u z278g1n}t;7@FO?J7FfP>AAEWCCR9812wswS4_><r;p^)2;I1Q8VD6G}Fu!053(V1G z$MZ(eN&_8ac9jcTr*mK$y$dYg6HlyDIL%I;+`%3>tOt~ND80<JAF#$=9kv{iCApth zK*M?=%KySuX8yh+nBi3kW#VnnvNtO1jzx*AjswEnAyK?*?Nf%&LJA!i)<iGQeut}W z|Dz}O>Tr+tj-#dR`fQ$6Gp^xR#gRs}Xk5Svw&5Ij`TP>_#OpLt_w5Eo-5#WDc>;J- z?uxd&QNYYxe>|<ah_EsdgxQ0(REG0Bs-x}?r8T475S|iGarUgCy1pe+fkVPjGVL`M zjOVE$d<)=hauN03&;b**=YUMA0IS`>p<<KP!WgxWNF(JU80bucOo<rfYwM4FN8bVY zk4l(sxd``;@Y+LrO<>UVjG0zaMP}ZSNVp&vt^A&c+S{%I=WCzP5tSLHD(W0Na#oxb z;WxyUjx0Otr4P;)R%e^eO|f&2?q>J4UB}L^M(PLpwAn+?Y^khy#n3qH4|7*b1^)Z> znK}ICB)MOEKj5V7#(m}+u>5lgd`9yJiX3U=wMvhH_>o9L&LSW6$N?xlp@jk-rlQJ| z_Kd0EJMQB3bKsk?P$c->7R}T>MEVH}G2yxrhv=PUuf!<>=>d_3d_z|#&@@QhY4O0~ zq$4Xtm9aa|NK!GMim_iiq6}-(Ab9!^ZM8a&pZfN~drQn=c;^r>Rd9g{t}-m;z{e&B z2EkW9#K?t9NRTx_p!*&g*z#8|u4{h=`d0|?oGQB*Y0D^Jb07$L9&bi&f3D(#q3iG~ znJ^^F+0V9AhEv-{wP9FYK2=<#Poa<+s&vCm)>9;w(*Hh=NAgnOjJyV$q!P_i26C*! zr%aqm&SB4gOu?^P`C#x`3^irEcs05WRE~LrrgTedjIED>j>su=dOQ_5Y`6eBH>`q3 zS9+s+CKJFwN)kSI<|6ZFZh-B$PTH6<;A@-^#1@ayM%g6D#aXOYOe^dco7b?$;x8$@ zIg3&rY=VbZKVj{M^RUE~YW&G@5j&RbiZ9AbvR`%mNY{FA_^{<F-m1772wtdTa`Jio zeRnQV_SPDDmUF0p?Ym*Sj5Ow3^95{qunT8xi@?@PGC|E62CRAG4BhsmLfx;u=;n|M zZ;du*k2UY*g;K2{E9xFV$Cq>2aQ!zxJmWoUM&7|CBWl#~+A&zVNsbM1u7-l&1hLIG zdn#$0h-E~CAY~*h2oI;*!mraSFo|kNNlP<OJ0gaUs<xq@Ge7a~=m)StvkXj@-6u}@ z@Wyji%RrZ~AlTV>7nSzMqMw=CKzXq`zBum#VJ)8uzIWGxu6|MAzwk16cv6dUdoqas z$!GAO4BApHKApO>ouxD?<teSi3jC$S8*IL0P5s!=hK3r3sP!?TjIhZuY`HA~7M-2L zPV1Jz3HM}LBrcWx*RcfpS}8)m<ZLLu;3D|_@+CYdlnvhe34tY{1Mr!<5TqO;P(EFX zEH)Boz^nuXA{A)yoDXjpiogqf6PYXP*RU0Kx$xZ{A-3*{isf6oE7YTX6Kt8EFS~%f zj=_p+tdE*L`%_p8AL{3szRecEDThqx$k{_G7PX@ONdfkO`#T)`{W0d>BET*_qQUCy zbA?gTyqf0n8MsVK0K72Y3j5$XsB9+%-zohBuY=3L*APp*pvwe3n{x%Mu+qWreLI=A z({}-1stAiXh19~6zL-DQui;a!Vna%mH><bO30M7A#)U3I@J`4O%v_PioU2o6_%^Ep zj|vRqH)I7Kie1m{lQ;@CTL!~>SccU;p#i&JE5j<EBwR2xoBFeMFPPt(0i5qa>er`V zFxgHS3A}iNwwl~Ws-Ggk-r3QhEBFSup*4&3{KLhYvL*3^$}N20MJg+AvH;$1>ZNRx zELgLXe3<je1s53aWLxu$p~rF={P_16erqUUxujGJHD5RhJ7Rk14>*?snnAevv<>-c zM-%maK!crUb``&ucEW<2%HUC25&yTU?0?Qd{*NkK{bmWuS?UU(@-JeNezby;!?vI* zERE@IaY5Ss(x_&1937hN3lk@e&~dwJAS`?hOum|r2+cue^3)sDEwTWuDptoSf0|Gw zD1l2?^5~%@>3Fj?$-KJl1Vz%Pz(r66hjb<3y6v-ZVDSee$di@-2UYg}-rj#vW#`(u z!@let*f2W*-dm8wTk7{wmhY9}#nW?G|0hn^RV)`;d$(f;r73u2h?hM)`WwZI=i*-( z!|-567qri>gaMUaP{pJk_Ny7O8!wo#*V9Or(RG32yD6%VdmK*hbfor0eTQ8ZGL-e& z!+1)v4SWgSMpa}Sg$}y4P{uc$$6zQ?LZAR9kAH?o8;h{7*-@By<}qYG#=!@Fa&S&{ z9h^v6%Vz#FWgQjH;2($Q<F{#U)Qd&ptWmTL6+rdkJ8~~@#{6)&rfmSvRd=BV)H2z> zTO!zR+-20w+seSuRg?<MZ==3U=COl{`c&KCE_gb64O_27;@Xc6&?CJDzWwzT6a0EC zZ7j#uua99IDzmAH*FW&{896GhD}bGjvSan<l)(J-6srEs3*@^YkTup^fW;SG$9|q9 z+wHH-hRlRe5i)Oq?N=9Y>RK>xKUzY$9o<Qk<xX*}?LX893%Hs2O&zPho#etDnJvXV zvHuE3Q}6)&$vCYx3B0hlud#;??29B`#w{jP{-_h@k3w#lyc*~9yf<_p>q!8&B2MFe z586n>fo@$dMbE63rkR7nL?;zX6HoKZW9XH{W?2ObEEq-zx<J~s`-??joDKJ*c^FqV zT$GTXS3+#tImS74)|~TOu#ga5nn4Vh3DGX)xm?O6jaxQ2&AAg<MXyzN<**4!^ppNf zVnv59eJpwxp(2q>bT0EFZq=0$890HsZS6qoew^Sq-9g+9OJ%w9o(R#N$t`r9v<2sF ztu-<EK#EYiIYO_qY2g~*t|aR1A8=M?Lt1`TKG(N=IV~6RuRb^(5Sjmu(6w9C$nT-g zxi8$$an^5+CCoXe$hVoj77gtyh#t8woRde)Nw@KOdYRJw`n`$UIbvl_922{Ex*C6` z!#%`k>aYkGg@h87xn&&10>JU|n^#{fDoVfQ_!H+&Z{u*1#ps>pK7_LTVNSU<ANP5Q z7?Dx`)FM#)cAc@(6n9gLWL;!;3ccd|JaSC_9Q`PS;0Q#Fa8lL`aX#HC;>cGtaa?Nb zEd(4!EZ#kHBm}07xaLomS@a8B;&>krC*H1>C7gfD(V>(h{YXcaP_K}t3;nD(bAwJ2 z4@~#a9!rNgZ%^fM@^)!*sjsp1--bl#7VkRxwT2$Y+3+JV@YsxzKWf9l>MJZ({S2p< z&rYM)?8vlOJP}MUd7@5;j@+i7JKv<!qZ|m2Y5~sR_Gn_hnJiH>U_vV%*CGGb)>;JH z#?fc94$vR#hw1Tcdh{>HMI3ES53X6^R!(~XAJby0L`Q1I5J|Rb<Y3SSZs6W@dL4L2 zyxeO{m@JPYGOr(`@63!5`~6!8Rns)OsOuf=-w<!%B7c(*|J+LN>(rww`#x}r-K3Du zg?kpp9a_})gZ<1$&1ocKpapw&4C3FVWw`&(ZG5#jA8+o~rcQMq!wN52(Tx#BO2(oA z=KHn6(!GVqY~@DE)UFBa->{TzS+<<bJP=P+sR`iHa4{-ur#JjyZAtYkUXB}i4uu@W zK74(bCcM}iOl>@{gMOE41oxHv#ZwP>kz!ugD3M!|Y}bX)s9^dpT4`uRmFXqJ6YKk# z){QpQRl8jLw$%xX)$lyb<aHeM*9)1yI1kO%hf|hs#o6rC#nf51C@SNwDzLV4p_I>E zVRaj$uuJ6+9Q#aw3Ru?;^II?*F)x<&-{s5B91FpJo%pHOkvG`gvclAJcRl!Lrx8yv zd<D(V`csO^BsJ~Xf%}yH;;^#}O#YS$%}$ttszuK1$OjenOolZ~-@Ocq?OjfN&rgTV z`RTY;bRngl+yqZvAt+$k30<VbsJt&D?E6cO%vKRkZh*^S!k{yX`TT-I&a_354b?5= zk3TA;@BJv~@pLC$bKZe`l@f)9oNVh0^5t={ni?jH1yFYRc}5}d1}OV62i!IBWFD)} zW<s;0nDo78KsCpNjPuH6hWWW95XnN)9dk&tv2I%4-+}pFP{wU^ia_bC8lxZ-gM>@n zNj)BmI@@_3-r6Kh(jJ1?e!B_7EEpwC9Xe6a;YDCp?GfChCPKdFcR}LcTT$z&9<XrZ zdUVLf1Qp97a!J5GMmS9jnydaqba?@?2$ljZ%Ga2s!9Bo1Kb@umHlwVl1z<o_ikIsg zkLEX>1A%tm$eIoOusQT9e8ydX9$7`gPy4=+SC`eG@RDAbe5nLkZm);OIng+qXUgL^ zoCRQH55Cm=3+?c41*iNxQMIQi8E9C8`8CeLoC|ia;#W4}_A4Pr0}3zD$RqW`9s%(@ zOKPn|8`&=%3)R1#1cn~|Kw%&j9`hD~9ovji=E_&3xpycuO$$X&x6H*=ss_}(*B)qA znmdvm?<BJpC19cI1-MAE6U~Q>w8XF{ejYN9%=o$mhXotMPaQkK)!}GxCZLB5oe)Cq z#$6yagP)4XO+=6Vl*qvkO#trog$7k0nS-D7VRE)FWWV-64R=v^=OG_e7zR<8eLT(y ze1u0<uY>7LBj8ebCz9mG!SDB<;6LvW3V0uZySJ~x+M&zor_DBaROtn&8K*}|R$5}g z1L}+jbqvKt#iGp%cfph&X;?A+71$x(j8FW2kCV8U$&Vc!geMQ|iKu^0=KNKJKVD0- zmHT+P%7X(?+#n841UBMVXE)&&yG}t3*#_`5Zw}n-EDGt41(fEaY^YXUi#GKu!;q8D znH6rA(V5RBu=->i(rgcei`QAfxgjgq;l1`0*GLA%&fNz!XA8ls{aeW!-&R81mC@)d zC!W-aK7lnTMJOMxOqgt3fi~+erd~BXKx@M_v8q=NXtb+j%2QlG)9OmXRP!q{Yc&TO zz6)c_Gi9*s;@4!1fd}Z2Edgg@{ZN^CB-o!*0QQ*(Q<epXm>dyJQpU|5L@tU$eXI4* zu&O8c7qTCj4vSEkzx<JoyC`@N&dVB}vBFkc17Qy@zU$@@QG7})h!=$L2t1m@bJ{$K z2DetH@p2?Yfs2k3+T89APp?cwuFkXZ5x)7bCU`Dc@;VxJ-h4=)E#X*9uoCwyWJ&O- z6xH~yg#Q*tki{AkOe`-ROeK6T*6z5%dvFQRLWz%QZfC)>>@_S^kOXdA*a<S|QXJ(r z2A+ysMB;r~`1o&Kc22-waB*WE@V+xnB)qFZrMty|a>fO$TEt?ThNt-Fs(GN&Z7b{* z(c=L(FX_7bmyuS*DU>&ALXdq1aOc*G%<i2uX(L$3yzkx3ToIV&a{r#76}@hf2N&+g zdIk%4ZSzGYp<4kQ)>{vh1rmUOaRehW8UxPi1`<C1R3NcCj2vHgmsHV@COIQDXvs=- z6r`%l;0+pd`GHoVO@#m}A2oB<3Z;{F*Z9G3kpPq|{)U8Td*H*-hj!P6!2G`g#JXqJ zC@@tJndO?HBkSeCE-xFjrT9AS+xs4*`>W%uaW8PN$AG+ks-O8B{sQ>3nV`*16)ZLm zK<>;A#J}bxV`?mdDwgczoRkX(BB~{**Wm-Q8hDHj7-%Dt2`l^}M*&8Z9z|=6#n{gK zN6^-!8s^lMyYNI~A;`+9Asgj*rq=z>Ky})31YU&VBVYf4Lq=av{QgbA_I3q86|T6n z;}Ocy3j@xZXmB|A1UE;vi<ovT0?mn{;J;O6|Fh2iKdNk8;&EK9{Rb`Go(E+cIyeOq z&$-7OvgqSmFVr`*#&Gxw_2?t!6EJzvUC1B&09J4{NXK($!8%@8hH>{Bj%V$8VC0mC zHcvM)I*)F0Zy4?X->%t_H`~SQ4Na7Qv+8Vd|2>C21CR1{EcN%3m6|OfI!jh`Hoquu hBSTT`J@ULQSWN!Eqe}mAl(#?M|9h^0HNTA5{{qvLJ)-~s literal 35344 zcmeGE2T&Ew7XAwpB?pltA}AmTCUV--YY-I?3@GM=pa>=e1Vl^(1(BeL1POwQ3L+v& zkv+Wz44{Ij7*SL(fLSqMmb>3`&U?c5z2{%nxmCAr-IA)QJ$uh$`r)^Gdb-!M23Z+3 znZR`+tAiK&>quFQv@m!4lqw-_E3dHHXN_-gn0Lss)d6Y}7Ir!cGXmBGh5GtN_<ETe z$o=z;nS#8Ocew8wCy9J<8TpSxMX~le@?%y9tO@ZB2=THoko|Mt?DkN3=`in=p}uB~ zLuJKfjbx2f#ocO$iit(CV*juE`K{^KzLMXZeslUY0>2jZyFdNr^lJouE$VlF`pxOr z2>e>q@BZ|g)2|WuwW#0y={KieBk*fczx&f~PQOOr*P?#+r{A1@jli!({q9e{IsF=e zUyF)l^Tnm)_2flqN6}^{N<TX2$WHPOnH0Lx%hI6F-}mHAMaLY}BrMH!<UIe8#I!Py z`N!^mrZ5BKMPc&&{6v}006(t)-;j~!BiH&a^Yagp@B1Gg`YiKaBRZvpPM`6<t5$hg z8%X`}YNjGD?Y%P4-&^C4yk@?*xS6uNl#g%7zuqg!_X!F14p`%?EuJqfA+ITF&q8z_ zOPxN`y#xHcYz(CSc$NP{hvk3hu>30>mLvb6!|ES8Z2w0cB1D!}A|2KuM5~vEtnv=` zvJ)Zt`|jTnEnD+<$&uoJW2EaF9_Sk!@&}_eB1kKj1^9XgTl^_f=dX|cL#@p})Y|`# zYX6{SBT{So4{8qopceIqG+X(<l=e@4e<|&sAnkNy{`Q4hnE#L3{-(kFKQ&nVAMyO_ z=s)n-|Ea;^f7GBQ!eB4r<Det+w_aso`R{-n{#x8$1Nu|kpMb2){}gBSKZ^S+K$ibU zK>s@WcR*Gae`@$&FFO$+E74_VW%(zdKd-w##93MXC!jx@|7$@1I{FWBR)327U$4cH zqHj+$lv!EpDE`xE@v^Y{ueOUQ-pX7QZ}T7V_W#Ft(Y0=6`ycTR|25uRbbf2mW+U2c zMZagKBPVio7JG+yS(+R4`TOoahX|1oBq9IDUtleYwEy=OEG_<P3;Lon+le+&pS5z( zk^6H1|JMj^CORk@!L7}8<o_JOE&tmPE^6Fbgu~k6KN`3GuZ^Ej69rr9DE#SEd0E>0 zn{Ab;_NQ%?peC}dv}LtLwpFB>nAmyeU%N&9---I&#;?=+&FVL+UnlVEk^f)mSA$0X z`rrJ&M*eQ&*U9~6^_$hN6ZrMW|7ZHuAIomhtj*d|w23Ct)>i+Twb}l2-7T86S&Jr` z*4F<vYx`qW`S&&We=NM!RR30lG-CO(kP)J#<LclLvt|w1et#4RIsesa-0ir=wv-Hz z+wFn{<&n6~_B4c^y9D&rV>;-DE;e*3vyvVQ!PWH`-fN4ai8ph>y_KVn3*=zM)((1R z)E!bYtc!~;bA|&t>#1+;BDkKX%j%D~M9*|Arz*{q+P}U@`ftpFz=T#R*ky!Cjt9W} zxCFWkROVjy9>*bJ8BpcElxD6OiEE@sLrrosh*#>O`?WW8^!<a7cT0ioUKE2X!w=!9 zy?xldi5ZamF$i<#I-=u-6>vy7os+}_vLy5rnI4r(ZklXIKYJZi8y<?f_iW*WiYJu0 zg<-+hTjchuFjDVt$GkpY0uN=nQT&THj+)UQx96-w`Tpw!?TwoJbJM-T(YynU47B7; zR#Q;lR|5J0EbiSYK%W>jIJV^#ylsue!|6Ndyihf^YuXy<*z|^;SX@D;9a{@8P8p)U z(=h7M(@l1^rEoqrS$Jo~HfsFPA8$8Sl9L%1VWqVO&OiN#PQIN)JBM818lnz^O}`Rc zyCMjeoMs?+dp^!N`jL1HOT}-?WFeu+5T{DHqS_l5u<?(e*DjyHI}OQr%OesUtHk-9 z1*&}0B`rQ@&?8)W^(HlIFu=;R%XlE!9kroSn6d2ybv*QwnuHxDk=y`i{&t!~`pqLn z;`JDRVIqcIs0U2=Mt<5NEua68F)><&`?VS9G}uZ$l(Vp7%|uWxp3Gf%OQBrD3??i% zNV<lX!<%Yz{5-D$*_i8?JhKVwyVR*|W;05R73byOTk=M$RC%>$Dp>5%f#>$_#}T2b z=<4u}OgcqrK*>%<dTtbb@M8?5PaaBop9R9F+9SBlp$falD#D!NYh<SNBsAdX(-jvK zN#js0n3NewD~9B8ddl~RL27?^GDnKlvA;<I534|)u?;M9(7@v8t?>E6ec-EF!TPf^ z+<K|Xo*kyiKIv_O0F7=it_*^uFDu}EfDJqi-UYjK3^+ftM!MX~4RRL^g|vBLv}61u z3_VNn^4n7IeqIBfCJ)i`=>*J3yh6FzhiGZ{T=e*U8bcipWAckUJhLkX2Hp!KvZ33B z15=a`z6!9*MS$-cw~&~Ky-ZntC)Svc#an06;P&PnFw0n-x9_#&-8^*Q$Fb?e?Ccag z6>%4bUXB7`k{u(MQ6$XkGR3zQK{S2!ce-&-wlH<-3o>0|9^RN<i)_nAxPWuPV6HkG zh_7ej10~6iF>SOdS^#p}SAv9CBocc$Dt|Q*$(#+aV|*Z{l5gatSRZn2L^kS77N_H* z2ZOjxDkhhS^Y?$~L+ES*6T&L-OrA4_HN7DRqi2Fy$X-;ANT!o64FK_%nY6@Q9rqM^ z!`3gQq`O-eu4&1kQv5+2!kB?{QW$ZbeVF!LYfhE&wDGBoB0P#145RXzxbVoGkWgzx z?W~0GNqP<%d$P1+WeLpBTZHou>>&9wx8k!?Pw}k3CM?`^n(Ui5gy#lI^7ao>z|kxT zwDs4ZN?9llebPYgx*Jft@`rRt%m?Z?+y=jVZKt8D%LNKHWu#oy9;?qjK+jGqDD4;y z{VUbzrH&nNr1?1bBqcK*(L3Qo(Pikbum^`Jms5p|6l`1D7i#*ckm{`(WZmdUsOxKw zV{4M|X;Cx&DA|BhvcF*c#mngLq{BbBr^|Q0+KziOTJVzhF`Vshg%vAzaC3VGz@ob8 zv^rOx61#YkWvm2|ac5w>=M|L3+gP4b3)7PqLqYc_${4oLii@Rm$^8b9KQSEo$~1!Y zSq+@ql<g=N@|lDzNQ94{G-1!aUO4M`0Jp3NhWqm_J8pgYnMN#l0Ee@h!FhWh&hPL} zyskKaRr9|O@}=%*ZUi{!qX1rnE8>lm$5eaCC5L+JZ0hpN0_zIaGOvAV9X8a}fa!}$ z@SCcKA08c}`-h0p8C5or*L#(GjVr(>_m6QOChiBnBlqxqS{pUmau;v5Edo^wZSKP4 zGTf_@0|!4J!SgC3VX_MWPTCiT8YIyPSyJrJM^B+|SRY(+-~eVfZ-x5{ra*d*0;aV{ zFe}#06qxVm4>yKngS(D4?if{sVF$vf+-E0vd+!KbT%eC1nGLuxNrv1yY>yFf3cQgH zM^yGNL*~6cAOHCz`1EdJie0rxqG>!XxeCIAyLJfM<+kGIOWHW7r3vbsE`hu5LHwvr z@WdD;*3nXjop7DSS*?T6VD4G)xsU`2Ter~zU1P|awitnnrX-Yq&445617Wm@Dt7g3 zA@gR`!Pp^pV6W3uDj#Hxu_G^%v&K{KLg*=+GOdf;QBlQ@9;b1<%{Hi0DI<|5q#2X< zYPce}jk6l=P9k2<qDP;{;?8|fnZx7zK=RPVnEtL8r&}usJ}PL!+I_mb*2!@gp%@9W z`5B<S=N-CSdP`)NZet!B-K2RZ`#|DOS6I~eiFT_x5goNPm^G*kyA=u`wO|OSMmG_y z_|f!9-&)$+unFXh(@5^#TlBj8Hu$LC7qiTwp?ceKI@x>&nJ&8@KGtR<EIA6@@3pC2 zy8&$|Uk%KVF4#VGAP#ZY!&+ZOc7pR6==oYgXJu_iBli<<DJqmkxXmWDL)A!g<Swpy zAE5rUM<hzSKRPaVg!>K2VErzK3RJeDyG$kxt4t*)9}M8$H<zN<&?LHqF5yfzq|gmM zK2&3_=)Qe?KbfOiL#_9f;kbpFpnY!{8tt6{>CX?sig9*m%rv3=q}Qb3vLz&KkmP%g ztU|pcQ=qL;aG!5MSR6$L^vflu=W5bN=1!!T8iCK#9c0<X0@_wlPlK+mCVMgyi9@j# z*AsM#RNi)=6{nA2{>A06Ipqj}?!)xPju23|XNsE(qd?K^FuD$q#+%0iAY;QR3<{Zz zZ<VSs>efR{()QzWe@gRDXRGkGn`QX4ajp1d{#@Kv(~4$V-MIeS0Q{OAP0N1dGE-*D zP=*_XW%fRB?h3-a3CTnz@;MGl&cHU6n^0gJ4iSTta8%(^+Us%|?)J8jIL%?OGv*|0 zZWu%zmhL4NilX59{X>vbCJQoVyU3aB3?kX*09q!S!&bxNF!FIAdgg@TtBGRl^6~0y z`rIuzbH!O)ygVOV$vFrzyp2t-Be)|`ZUXn@$xt;}2AG9joLR9o?u(m=tEvR>E!YiQ zHa<h6g|qM*IZtI5f2YA;cj8>nr5J8hg~U{ur*ct{ke~wVm)SzgbxpkLE$U}fi9e9; zS#vmcDky!p3WIG5Anm6!M4yvk4;{J)rKNq@f*TSnJr)Vd{xQ%ncrP5>=?5Q-hhx6! z5VTSQlI44fNM)_0<+Bs<i+Tf&(_H|!7afGw9p}(;tO`~%o~M3Q4{7V{8EEb2ila_W zLkD&(o>=ArQM=|aZIL7C<--2>f)PiRT^6|2u#KAe%Jvh@X8-?fB)5sNt9I~*=kMf8 zy$bNFO(MH`+i^DU)lR;sqmtE*oxv8m9%ik+7qToD!+$z-h;P*5cw_&W?BiHVzH|Ca z_C!=ED|0-Azf%y!n?*~=O8%{-f1fx1nY(s-EN-XPhqt&@OvN7P@i+HZvR+5@oi3&v z;g?lMvq5J@u=Ty$*n-Q2Y_L)?FPp!Rue*}O?|S9JHV8fVRqCa%Hm#k#(O||esI2D4 zpLoj#E*{|IoEOJlR~^ZZJ><`~pB~IMyi;cT{mkKI!wuP%vfaE>Mk`w%DaK08RN!a5 zO5=CU3SrA9Uf~A}b>)>7X!8;JHy|@bhzSGoG5A0fU)Ute(<x2-I~gnXasNtwwN5fr z%RJ-{9+}Cjuan@Db#<JQUa31x>>0(X_5X<l_Xe`*-!k|~Gb-5=f&r}A5LwCpAJ%_2 zFx(XNeyGv2PfYlQ(P_L{oPn&~-_{s^X&L?LX1TQwe!Itq)_hRnsuPva-M5A7o7zkU zDw)!@okMZ=r#4z#*hX)~Kcj9Jb>Mlv98J=<U^4_NP@$Z_&p&>NFDi4#LlK4S%Dc7j zFme=~RHunwe(rz;WiIfsBMra2^u{M@6G@n<G_LlZf#E;(iF9MNpkMPDS}WN}2h^*= zP4!8b&=Cz%@io-=^J}^lBFSy1Q6ynPKh$zPLOe6;NWtr9t|G9UJlb`StX`K-7CpPo z{jl9ehOIH<$6U;1QgdcuX!aT>DT5JC65{Q=!GZOBvs*At)YyiS>RD*D*At$6up;(3 zt3cO#FZMe#3-a$@C+GY&l5g`r(S5;&@J6hRy67K7|2O(Du;(+a{`rW>Snz=kJbaL< z&J=^4^<sEe&5<O{e@;5?m~nlV55O{S9UQtq8ND{>f&+-doQa1}IIJ1$7EZ(y3CguE z<e%~vW4EH&=6QUb5)aFNYH`@I5eN47f{Wi%G2Ce^#>Z}8T<hm@bE;!;MdDp<qE8H* zU1~;c4+hgV-y*KwH=Z8#8ALNLpG3F)EzFP)4TRNdAgX0DaP)*UjuD$nUoO-{r_1X| z#)F}ZO4deNy`ztC-yUxoE_;T`y$t~4?c=Fx&@C3z`||8=b6i3O<Nc>OB%-|q_w15^ zRgwcRxa9`+r1XKI9TJ%Cl?%f^iot=34e0dx4N;uB5;ImdV{o+*`WEjN3<+_DY19KH zoTV9i1yyvHd_$vzeL>@~33d83432n6fUk@UMESSUblVYl{fHAIqaXpFdYV8_qk#Sp zFs$mO6}5%7;!cS-U*UN5Y*<t|l8c-C7FHb9qc?nh!l<GINIf|UX}bmS?BdCA9S>}{ zm<h%PJE~n2w=&u0Uzr2m3FQ9xTDmeP7q(n#z$%~JWM$D|YVznBxzO^OK2k3yE6l>l zGFeH`(97qP#H66G`x`Ajsz}GLD5Xui03VL-g6sQ=xV#spwKtdK@>ABm=2zbG#@>h_ zK$lGejT{*|;gu<Vk3Iv6snan&-3;BM^oWja87eNc$9{7az$1A&);4Xeb_tyU%q(~G zxq6ukT;PS|?mT*XY#{wOq=k-N_>EEZ)P<`pmq>7V2E}cCsqU2@#Nn+zlpa4x56{kG z6m<=#bV&x6^|}Nn?oo5P7=Icx#Mbca@~JrB@f2*(WbvYSC--8+9$XnA3CB*w(0P(0 zsbQOtZ2q7R86#(5uR<L3@0>|(wzQB?K^on+wv)Qq_Rz1lD!GL(TWHG6FwtjW^gBpF zm2@dBeszazt;%EU`o17D0?!dcG637e4M}F13E6wPKYPMmoLxNr8O9EM=dk!vFO2(? zhP4})u;0&(BTMsb$h-I)eE6lEWLem<O5Q8+>8MDs>|I1n9zLPpA1afzc49D7>pN8_ z+5kjBKrW5GkB--^pz-}Hdh(JnH7U#_BVs#=*-&v>KX3;b*=0|iDlgNYFSe4+0lJKy zYd0}$lSHRQLlAHaO^QvUf#c^8^LuXGboDwm$f5_-JF75ogEPuF?t!kOIZS7{AvhJ9 z2?LV~VNv2+vbIziF8b+_GbhIr^EN+hJ8+5?>TLnD&|vD?u@eTL9gUNeyy<h7&kP%J ziq0{W!;i&#sinhV8XeI}Q>xVQ`ID>6)499oH+C1(@hOs;@18~~O<#j=b1^=&k>s1> zuknlJOq{N}r=pJEC)hPUo_?(0u-;RVEict$17FO7$9xh@RO<jK$qYPZcONX?_(5bq zs!(NmBHSK66Jo5!piw|7d>JazuP+A}KaG4Z>!tF}#bkfT7oxS~4Y#&Df(XhLIBu>E zGrF!1W?xXl0pHIsO6NDzm#e*4FU1`^H`;}TaD&=?LB|ED%8Pmb_w(47=k}mcdNOPu zq{KT#C}AE(VxGSwbd|f2=jN6$J@61{>P!SD^H*GuP9!vi6@h10kSHNx4eCZIuxYO} zbaWVF@XW8Am*RF>6@8L=D`wE?N1xFk`hun#>>z&c?oz$5bL7VD3Cy?`lK6hCD|B9p z$B5p0w8S5ryhdH)YwNYx8^uNV>Bkwm<W3b$UD!y~EiIt>>;`&IX%2j!_lSlJpTwP8 zKZ;n_X+gQII4M-GBtyAK*xWRj+85}9-l}QXdMSbK%6Ubv<?bWe$q(qHHy5bMvJ=E< zmI-n9{w!FTcbYjH{gyG9tU)fe>?Sjdx6^kQE!fU6I&6-*JWlPdVMo2b%+D#3sjU+w z#P-{H!N@g+)VS6Ie&nf=>JT0N<A@UKaOePeT{Q)B7Tlt}?fsCKs3kW(6w+O@CSygv za?<Hi!hBInBE{chXz!Z~jK#9CxY)`WO<Rx*nL3qjSIHp)$$QNAPkQu&g?^2UdoLHm z#ZZ|p8JeA?1`)|0VO)_eJL{6GQ|}iIr#pQFtlGm0xH@q@bJt0Q{pnmo*Xk9)#O!#~ zimIg9odsmfuE$KXk{&5P@|>J#md3RB3ygvVPmXt3P<b&)D2sawqpdgN^>#P9;%EXn zF)NI|bZg;O?v=&jyQYjCnMyyH^~F6i#|Zsqw-OVX$Mp774QOdmWxIVIvBP$z@DGOU z=Z(%MvDM2n`9050g5JXMtXGIF|9DdwDawuooT9_piV|F<>$gMX_8E-w<4OD>?GAWr z*hR!9p2FRVTXFKULMZR_;ENmtpzb>yGlr(qgrr<*Ib9hGw3X0>c}(UeekNaEd?(#6 z8#ykcf~+;@4`TEBqVCCSw8Ho`)@?eE1`5i&UtvzI&J-oym~`-ATRt&yYemVG(iPYc z<IU&xXz|6T)%ixdr$ook8gGnBg=AkPJh8?UL#qygev&hzt7(Rb3r-7n&uODEdIIWn zxtn{Y^N~Et7>1T%lJw@NFrk&y54tF%i7+*fNzJ}bbmP4fM6&oAP498Wd9E)Rx=!0E zV{I$i_q$4uTYY7B8}{LQYLjsH@}=zP*^zkBd^n#ucn555d;>Ermf+-ReOWJ)u`uAG zHG8rA2&T-M%-ZG~bIK#c*y7nk@IaC{tUD*k`|T6wtCO?gnv4hG*b+L!BZ_99zd_Hr zCXv}{2B36X6@pWW$lY8av*Gnn8nGiwnEY)J<94<Nyk<2rt=VZf+18}iC4GGDoVIwF znpR3I?rMSWHaU2xx)d(lr1&!M46J!D7&DKar;|M9faU@T2s3Y_Gs9vWmP#fPcDX&) zB)5^L<GQF#p(F0C+C}WD9EtYvTzc3|7q5MY=K>DMf>!JZl%P^5Q`QG%_VytI7zdJ^ zCCfC8l7rCW27-Ra)2Q~|)8v8rDE=<5%5UiJ#RHY)2TF_P1=VZdsH7XM_-;V96z!*z z&kZ8}bvj`EK^gO>UBI49JsMFMPi;rf!unI@OjvFOWT*zwF|XWUwAvRssH}|H{H}$v zRx%_`W;;oea0Mp0hZ6B9&SU2frt0l??pUpmz$6RKX3H{Ro5e0z(UD3NL$1I(Q(<if zU(fC_GIdg(^qs!@dI=_)oWXS7l-**i$eyFSYag3gk(<gba4;+vD-uH)SCcwy8uDJW zkkqB4KGhLYbPxjV2ST`5Gud;I$E?O5bho<_e#Fbn!jRKMy~ve3w0<enwrM4CTdxws z`fL)vO9B)dG)3b_5AAODhiCO-Alvm3E}Y9_SMLxd)(UpAqmSjlnw@>A?}BDxnsy$B zyUoTh&n^(UA}PI8jdT3hlI)YC;Qj2^@KGug+bz{$AiYZutW5%!fgG0*zaEpHE&z`P zo@$s$pu{{)EbbP^x`~@eV_^jO`hF6T5cg&_xmA<6&Kk}=d?>tqxsz=CuEM4mi?dW+ z8{3YH)wVUK3l_??v!g9ulR&$P;C!0L>$l!ARhQ%O#rOWKa}a|eWBOxpO9|uqKpHR3 z9gWYVT8Y}@GHMhw7=0vj$Taa>Tygaj-FEB)@!Kr{fy2#k&JZzJJ9HH02qjT<Nxg8J zrxrbJ-%5fm<#1hV%t8EkjAMkjHpISDWhGFvcKef~{MDoKtc`9IY&>bg-n)^_!Qo5j z@MsC2y>mM$Rm_Bs2S=d6YZZ8**^OGK`mjfoJ!#4J5p0p;IUKqug((Sm3>EWvRIH4E zbwUf;t(*_Rt4e8<Z8tY`UVkzyelqEK?@5FEEuzn+>B5r49kf>c1@lRjA%=%l@L8B8 z?c3VJxgOQzl~=gKoBi>;@;zJj-8@TnV|o)EtGSQ9x{(Vrr__UQw>IjJ$!6{ixJ6P{ zt6*k<GJGGl1fHLifYC3vVPLThtScOW%fG~eqg@G!DTqg1h5cm1?PglDyHMEQ*&a?E z{6?PM{z_wfjZiN|1MV-&rSGS7(CeN#bdg;I37d9{?3*EviPb7>{r=VPC3!15`MC`< zes+Jp(&QPI>w3e5Q8h%TrkRnmXy!JFje=Im4$K(020qt^;sRwIetRB=G1+Uu<y8{O zoZZDdJyH&%rYEBN&puFc%NOsCm`z?B8i>AX(T;@|&yw_tL3HW)<6KdgDAN@5g*FVy zqbD0&=+Ogr>4fAcu9EJgsbg=#^C3IHVYW1c9V_8mCnv+g@h!M?awO0;Ifw{;O{L~F z(N<<Cd){pzzHS=|6$d@Znn*eRVN@(eTF-&e3TLqAPM%0(A$R}jc$yVc$|NPr;c&Zc zbaQJvy?#24C<Q(y+uLKw6{($c&D^a5pGG};R$vB-u?8UXT9pe(9Zm-6U4m_ICV;eo zHt!NXs&?%G1@>9q7Culigl=3vn%I6jPTvh`Mc2vYkR&6+u5WopZxrdn@?ll<ozX13 z=$ggMIhISt{@jeE@oPaMri>O_N8)E}W+K<D$2A}3Q*tAoJR7M=n|1qu*nlEBW~&Mq zj2;PnONK-E4J%I9J(XEpDuW$Q>IH*t81cJ3vryh48ct$D?f!`l?189a_VVc<exuH4 zYV>9fnM($+Vh=wNu5&n_WZyu3$SkIPRdevti1W03<9t+je2ErC|DbZFQaHy=0j~at zg=fa|(YHs3-tyT+o(HJlmXR8QVpDloXCM^jo0c*v^JK}YBoo@a`6qeN#*-u&3(VOk zj;jxA^6M|S)Si8|ogLwv1tw3d_$?yCaZ|!urvCtAzWUQ)2p#uDuzi*tn%t=)!TW^d zxcw0vbiEe7#Z^Ov8HJ?4k^I@Ya*&y%z$(o&<A=XF4?d>a{3nwc?DB|#WUCRu!4D>) zW{(XrzDi&rx_2#*=m&*1k}!VV0C2mzlOzR(Q0b(@RCm#Q{QhnN*_15Bjy^P@cFL^* zwfQ~~eBNhQewv{iUETPI{4||PEgI*-@a^5O=tz8xZSN8qT~SIlDca%K`ZyBb=O@jm z9Zuf`C)2L?qe)*$f8miEvNSE@IyK7@m(>$xO#ZSs*5Yqf#%|%o_D47tMlfqs+)mv| znde|oR#kK7RP3p7nk|CJ2R!E{Yb4aXVh=W3Q#r${N&?*x$xO4iIJup43>SxwC+tUC z-fX+Iz)j(bFi-s|lN!tmmQBnT===3$vs4+u=#}MyF{!heio{XeXdfwogrqG$xhR4& z>(Uez-u)r(FVi&t%x%?bcDym8)#II^x{o5UVjb|kYZ-B<Ge*8x3z-iISP-1!xQ^LR zDxVDCk6rje^RIuv!nONIP5Tm2`eYB4^y!0(zwJSDGY5#xvgY)AvZ>v;pVUQh4f*1; zn|8+WM7P18%P7`|nFhCE(YPqGz{QXh`z^ug*Jfir8wC#S{%EZq!1!Kh;VwVw5;m^u zM#*(%@N`3=po_$_eHv>x4<A_)=Qj@>*9XJ1xWO#QI>N030hqeBg*;J?LES06*pl}X zgC-q<0u5_Hq#J|F6eIDV>p^<xgf#2dT8~lUi^u@=?XY<<!KJ(v-Wzolr3M*6@R|tZ zvvP>vMKxCSZ6o>cVhlUxQ~^xivygZ`9)lAv)}ms2I&1QL710fM;2&HcMFJKg9jqD6 z_8C!&i=1siy-S_fF&&O;7fwc>G!JC;XRy~tYTM_^-@^5Y_n9xUKWUNdXVC6Qf*{#j zM0-{U7%CFDdncZoSNN103pM0wX&VWW=g=ucjQRdH6WO`eFym1Q{kCKr%xN4$H!nMf znhW>Q5qTw4{O1lls~Lxmjtoh&>xU2XZ;=IVJ<R?&!(d9|95Ai8L~h>rPF7`}gmvc| zp;9Fg8r?-X3I7V{$DSb3HGofIO|V_EfzCU66x@!!0k<A+axmB(ZkULnqxd=0m>$Va zQwjwK*%dV1HJu#ZpG1dWO|DU&GY(a?E6@>dkq;l=(K~j@7@PM9t~H9<4892Y4&fwK zZ7M8Chym%FYIwqb8+S1FG`f~_<FLoN@Tk-u&S=afHx;beIfrD)V3!8sS!GCF&hLU# zR|eyfe$TLSup+DMrog|>?Tho>#_>m#Utz{wSM1GN%7)A@#IXX7X4_xG&%=D+S>+en zY2O2uKKgv(w~<ui#}rt%ESX-^u|n0brDV3D2Z}4?)A>6Vao#`bxaEE^WXvQ}e#n%o zWcHeBY@R9&?sxsj*p^p<T0K*gEN?|Y77_yoMXuN`o%-ERpqKBbkuk$QkQ2!TWRAE# z88&e}nOG^yTI*C1<+YnY_tasM6J)@9OmoCtk+V==%!_WyEW-IWljzRxD^W&j7WtCT z2p-wKgoTlY0^Rnr$PO;U$Pi6v>Na<rta71dM4=6=zGFolCO_nyGoI6ab<45v%LCB- zlmN%ABI%1{RZL6Sftqg;*+mKkSiOEH6(lTz_e&m9E!{v+-w;O^`y22fgU`a|XMJJ# z=K*l-dptd|+=X)OC-G%}S-NhOBF>t)i@#=@4qv7#v)}d)<p=llW4A`%p_cX2*ai`| zu<mK_?u0cz*|0wiP#2{tdY3`0`XQLI_Z!hjQsz5#9e`C&rOI!_+31|fP`lTPge_hM zI;T{@K<O4Ug*uVFhYN|qpjOi9b^*7`?<4CLhTz=^+K{asNqY)IV9SRp>Ug~iVdZIR z*~8Fa_qFJ{#{<_)c*?L-Luq5D77^X?iC_2=VsdsTnC3MRwYkyo?mokA)nAIyi38wC z`9v^`J_{P_CsPSGk^d%Vi_WQon3WA>fV7e7K5FMiR{3G}PZ6s=&A4&S68P9u$jtj9 z2dUdz$ibp}WPX7TWcMqC)kEjtJCToeC~6>{Sk;WLGOp36@9&}fNjJ<te2<RXDGBpB zXMp?huSE9XG5GN^h!32ji^Zo3;l#-xbU9y06$_SPUa}{@Xml>QUp)>6-+hje&Rh8P zf?;rM?f~|}f>Pq{pn{?4v3z-cC8`=0Q0=>u*qE+Vod0YK8JZ}EC&cRT_|6VuRm(x) z%WAALaiZTkcXPK_sxxMfa)l4Bo+1`rdx+e{YQgmRw}rv3+H~O9$>dnrDY9Xd45k=L zleT*uROj&?CM{i+I3+IRrcQSy`bqDFFIP?yq+PJ1(;waAYWBG@`h`}4=@naQO5_S? z*7hWl=5d}Zf2PSi?kyt<Yd?~2NsYv==@@->s)`;6N@vhO7K+-Fh@uMR-t;*_Bpi~+ zEnjt}ud603ns<{ZUN0f@1AWLM+D=p|N6~AarQpi>3(SI8B}RAuY)Ym{z}78qxXqWv z;BLYrx_m<yvn%Nfd61q%a{YEO3Iprtvd_bC^-(WWsUHcZz(T~7r_2ktP)y!^o%-i_ zkmDHyr(YR|Qxl4a?!q|Qb7MC#GS0#3HAu^5CgHAPXA*Em3)hW3;+U|uiR;yvN^(=? z;ds`7lts<OT`NaIac~9oH*-O<)EpOYlcra$$3w&QP-e7R1J%wFCnGeqIUVN>WW=i! zF1N>&?mp1MWlhL(G`9QB#Mwxb>EeB9t+^`RnYL5#y1<_sHZGOC8=vkth<(jmk6s|) zz1jsoy34tnE)vj`bJj8FwYE@ip$|Q_-Gu4U*g?WCDu9vYN?}JwHq+;f8OA>eC1VFk z;LXn2BtW`~9B4U3=SM1Vo;#)Jq(?WnmF#{xu-+8TtF@9nCwj>k&n@(AO(d}mdO}y% z)(e^!ZJ~1`3dpyvNN!!pI_fa2iPWu$Wz>TYGX8!C$f;&OGG=)&-SKt?d7^cSb5ks$ z{?86WlcxsGik%BNCGvDby)k}HeM~AX<Veb)C^BnV0mGDJlSm5(+`eKKmAPNa1v=+* z1r^oQukIA-7CX)@GVp-%0|)4s_pyw9*f?@&WjK}?o6z|_GIV?Y;rQrm2X&b)3v=oO znCYQRVq$iJ*EnsO8+n!6eRC`w{>q*?_B@ezb=)Eb>y9%IzB)NZ%sRpJu=_~R5H(UU zQ5Jjh=9B%8wo-1_o0_1FSICR(CgCl2S#oF9cwx~}31(uzN^WNE?V4KaXlD7j7G`~o z7qwj5$r*o@hj?2lsGWX@Sv}H^%os5gtmHgMjp_?-r1yUA?W-6fBc+Iad=0=<Qj62- zog-kUi<WX1o>H}W_egGsG$_=klj3>ybXdW7GQU)odtD|)`fT4%`b3G-P@Ob7);@&% zxHMXrwABT~=MSW(byLZOJ=aOs!Kd8YD;JpjTpi3#)<bD&0e)WjiL}Ld6VqyWR9U=~ zoHg2qFQ@0yP_41d=JDTZvfgBZpxKwY7fImoQNz);Hj0dzZ-yI}^a{S@4<<J{<Z$e} zp=A4!bELG>keWYT#;s^9!HlK-xk<~5nR&-OG0er33lh~dg_iSh!d8it7kAK)-jQV7 zkx=HOekw8C>_Dd}2)S_|g2{e|Jnm!QbY}UdwOrpb14T=repu{(k1J0ZKwEsHNzwEa z;pD@+$XnyKnz~0mj7O|JCA*)HOpSg*n{Q5xzgYpz>2{+f&;5w@fzQNp#%;k9pOs{t zs}e??RiUd|q-fyRyWElw^^7F%MxA6*nPcM}@kN{#bXLictiAql@boBhe((%pmLJ6! zetbx5)#4eWL%X?cd_MO=@Q$)0jxcKJ$}qvw45k);5xV%LaZ8V8lcNjk$cld1r1AL` z0&R*^yR4Q;`H})XVRck1Q3g~^FOfX6OiE`f(xZYtR3Nf&(k_SL_-RVm^Q0V29@;=( zbuoJNViFA)?ui>q>S>79HfGD-pJbPsEo5%;=8iC9@W8c1%ow=dv3>bA^3`4+=RQ+H zHhLX5e$ZOnm@twA6)z=b(V|CO{@XI(9}emwee)O^JH;CZE+2`N@n%#n;5bzOw4&=? zWK)`P3>T<|qs;XAw8?)t+_tz*XRk>Wj$VyIp?E!0e$$ZCH%(%^7t{*XRxRbmYr6^k zY@3<sdv6Gnbcpcw00Hw~T-iULMEf^aw$(R+gleQQ`RB{%Dt9kV#X(3M4m{)5WekHu zBU!Mj_<?&j_>+OX-|5wr=g~?2A$@j9kGGC9L~EyG{Oybm{=wT&cD@+miUp(iqDj`I z&f+5+T9eDK>Q!_4;(i~3&(9EEoY@YpJIYz{8`k_N<?#^RJefV3>4-V(E_}cb#`dP& zurJON22QA^i^bYVR;DJ{ZY{)~fvMzdo-Y@Zbe_IdTu;`UI>Y&&4II?!kl3(%xH6{> z=F`*s*^T?K%{7OAu%#U=vUjkXf=+-lN#V^?yEyc0pp!?+Ir$tBIMwMLVqXr{VfzGc zhH+w7nd~+mtA}oZC$XkbmKaC3Z(0ED_bb`K0tMFb%{;7JDTD6Q2Eg}koZvxnyvV`q zCVX}zz5e4QscDWT7nZ-Lr{$EfeX<hX-m8H@BI}>ZIrB0z-Joc~cJ|dpHLz}cgnH$V z+1&-3_|#2qc)hoYk#SxPAzHWCtv8}s=?Eis%1}Kx{j3BgZ*idx2F_qR<1E-d*n^i> ziNTkJ-mH1SQfj$09Q>ozMQ*tguH1NpY;Jx-{I(AOZ~uOnEzuyzS8Jmi@-|ax;R()X z!W%M1#~+=<l-WZY=YqAvN;alt25V+f%x`+H0x!e7$al}%usStI^d0X<i3VNFh|FNG zFJ8!w4Vwvj^3>s2s1EBR=}khH9e|p5L)hT-oe(=!lD#xam4&cwQth4ul0WWYy#vyw zjh(^|y)JmAXaKyQ(McY(e_)IkX~Sfx&!pt(Tq-_83hqx?4NCo#S@jz=F!RWM@Yy`y zX<>=1lcjwF?5pbwty7g@kpFiG7TB|!e$Hd<)?UH4X^F73z?7{K8JwL<TA6CqP&lP` z8jj?QW7|d#q5j%yp`-H{XFl!~th(>S49wGpWBQg<ZKgUI%Y7uvo$k_EhxXFJPj8US z16u@<S*3z7hbxStAX|8Fdjlz4bOr_tSw_nJ{qXIYRI1ijv$mr72aH?Sg>sWRac6!M zj?Uf+@@l~94D#mh>bT>#k=LMi<6fH3l!(rvtl&ZoN!)bXnRlLXj?;Z22Tx6=@t;H` zPm5ZDxwJd_?AuLy=#t(8)b;~mm$rxD>8!Ej<2Ok<<WVFJRx!t8{nfBn!-1Q$MH@@B zhp=x-rP$fWIULtHzgB0QJNr&LpUs<n7#a*)$@;RRpdT8EZ@PA~+Coi!fYnz1YVmMZ zO-7lW>X<^EDztH0TRrxy6T_elEhxF72rcvD_z9QCv1DQ!R4(_YX6rZMEceAEqGB`M za9<OLja0(urZ-e0)JWj|$qK@#F(|&-Pi8+E3gr%t?6y8%saf1d469jE8!oEA@Yv(a ze)}xrba4G*ev-Tlj&ofD%wTKqUv-<GEZ2xbDmOuU*m+u0BhTA=<dK#aak%O6XoOex z{P`^#aH7wB%zkm33Jp(_zN1Y=bvGZVid_m_xA_~mz1x73^cK-k(z}U=#2B=T8xAwh z8q&FEWjWLI8pw?G!N8Dw=t|GW<b`#-tkAUfZJ$Z(lXb}?GNKu}Qh-fJnt(*ylDDa) zyh(Z_&RQD-d%lgwu?Nifg1&Kh)RE$}EDPStLx6XgG<tB{WNfGYyw~$t=o{$+-yBQH z9#0!=o+u4f<29H`k44oMd>!2oZ-f`b^N8`8eh^Zl#2oZ9V6F@wO%fh?;`8Mhp!9A{ zEj=)kz2Ey8(qlYu64wg7Hp;krWG2~mSb>jPx{)VURWw0M9$sD&gE{?I;1t>Oq7vsc z+*w$FW!H|8;s81Nz->3?J<Y|<XD*U+!xTXCcrktZbt&1>n+F|_v`K?YIW1Ygmo8dd zP7AYCA$M3RwTRV%W1HR+ZHZ*438`S~{Yz1Ip^cMnhf!_&%zTLLJrBO^@7UKD2l1Dy zCS&UiMSkPYL%e5B1>F0dMGAc6uy$!4Km49F+i>;@MqeGl+bM{_@{5P)xCQEX@p~2O z5A4Ako@E#my@FdiO9~UlDe()3j3MFC{m}i6CTKP&(}2bvs`j>*dM>yuIP5-Flpa|Q zlOH+p8y?o9{DG=k$-8HGVKM8Jp_mIFHJy1)*X{7)$V}L;9F1JV6!uD3A84J{2c?!K zae9mQfPCjdxTn4r))EU|T>B;jTN-mFO7F>_oXKc>@)@_(_$Rn`PJ&)*2e2)81r|;1 zwC95inE1ytx9&_KD-It3oyIS8isNnS;&BmT3o`l2RrC41i|wGes-)KC(muS^gisJ0 z#aBse;wO*<%#!cVr@iQ38#p$QZS2hjW1UT)-~S<8zjlIsZ)wG&cp83tzm!$jnuEIr zkEJr(lR<M&1}2?R;^X{nVb5w0?)p?++!rnhm4%+fXVY_LQimZ}+;YP=QBA09olj;+ zP~ox{hw<2vh3uC2EdGY;bXdQ>l^+$k9U@kzz-HHUcD?d?K6=J3)O2!$z;6|7TB5uY zX^LZWCs?uu>E5Ux3f$eHhsng#D!8!!N18J@3iB?W1?}h@w!LZ)yK9&Vd(T&gJv2j| zvyC_;2$-tI?pZOGmL3Qri3WX$q+9?=SCrvf-sxf4glu~1#wV<3HsXiqMYFs2?}PW@ zA82^b86108kvAS8&X3F<$3F3~L9aQYd)4rxypooFZRm|G{=z&DbbtSqIzJ7=){22} zAT<pWCQ9*O>VP?ic`RC-$=_Z@P*Qms*p=>sai$rZTCP8x{&+2Pl}%w}I<GJ$R!wB= z+*lfa#SC}yCCtIRL*$OpZt_+41f0q`0`EjqNQHBWwR?WV@vXZB?0A<*GTqOQO*k*h zUmoJb8W_)Krwe6j=N)&YH}n=_&GkX}Jw1kZ-S~#?x3xra)(|#-9L@`lh{okPBWc1o z9ac=3OzkBLIM>k)<ijR?Dn7CgXsswA$@yDpt5+Ya?SGKoTJVHX+-1+Hm2M=lOP#6g zq$oUV_YUG)WBJ^?7*=FHk|CWd*je4t{7n^aR*_WU@@+wE$YnMDl%+iHxS|2IZ?3@W zM>FuTz8Krqq5)~1jihmc4R7<xnDt*d8OHfnf<a|pUgzp3GABP188rrUN9~|0Q5m@D zj4V`yOeBY2>ci6smx)n>6pUCBL)rQosx6)k!%y3@=3|e-+EtRQO}Sp}R=M;1EPd75 zz?rtJh22PIZg3j?;cUf!YYWHTbt7vPRe4_U;20CRd<)ZfC5WG~<RLCH&O-+ibN+_K zR5I&$3+@bY<Ygzlf&j3E63oRA$=zf{;%+8u(i>EGbb%9;e-n7r%W|8y2(UQ+1DrlH z0BlFfVx-1Nu)g_{+<0xtuYYj`LiQcx@5E}=E>QJkx13&0#}p*<KT`7HP(>UKEgH@L zTy%xq{@#dxuCfN^AG<`P)RN!@n4;Ko2Q)GAWZNbd^MlW?hej$6i8^h}MDg7~Zkh>= z<Q{V?cXd&>%#B>c?G*AXCz>-mqlgo23TU8(1U0hWLR02wgQw#aE@;Y19xojfRS?~U zCBuaLtA{+>6ZVo#-{8VOZK#7Ti+gy@S&rRCT0y(Mj;~gT<Lgt8QI{Q6+_h61u_<5; z#0Be+wvsNoL0aTNwf9ga?L40_)QsjoZYPI6En$+BW2s}}9)hkLAgxrMsE3~+{f`>s z=&O2U`9>sLTO=TH(wkF73aT*oEFz;L&bQB$V8<+d3@KwRYulzPJ5@S5K*h{c_)=8$ zICaQjcIGh?zU+D;T^2Nj9i|sgr8oY7lY$&vH2WLgDSbzBlnnV~?OR~A?iG~GX(kb; zq*xE9dD!1^9yC`Mk^AjMWRrg*OiG^1IR%*EtIew%WgpiOWlJGm{L)3I%w536Nay3K zIB`CtHjXcTv;saXXW3)*6su%Kl|qhXIB0GPAGYWOUhzK3EJ+&A8|EuH)$~TQch=vA z6?a>S>Rxf^|H+sytQ&#1%ftA~lO*`K@_Arb&!ME-dmwr;to=bVwr<Ek;geiRRGIb~ zY`2V~m0Sw9%>M$t-r`DC$7I91d0Uvot1;y6`lB#m>n>K}^jLPAp*kI25QY0U9^&UL z|G_FBAH*IsH(;Z$e!`AJFTfz@CEM>&H_p4~$4<=3g_EDsx!E#{S+yq_WcTWy5ME;p zWlaM1j`lJ(Vn+$6m`K7)i3)UY{>0T+nA3SzpOZ3><vc}m>F^~lXk9`IbCvr^br+20 zuB5A@^{Q6VzgQh6Sj^_tqVDi=Ek`()94#lwz!|j?PY&_Nu8iiZFQoCam4q--r5nv| zc(dIKqn(1EZsuoQFM!jV)Oiy!hWAabhKIT8tev<En=@|{AF|{i44W9s#yO3Hl<FxE z6la9e=?}OTnGeDbeXo%Ex|LLo&*XM^YoO!63g*(zS~5dS^hiL>Y&!GsCMM|SBfLJ& z0Mpl2!~M$bwK}U*_|prA*Ge>|GbgvE66xJfLH!ew+luYDK|_-58<oLr3Ga)p>zp8c znI+lzVHKzTD2i?glE%#n4e)(Y8RO~Iiw$QsW8|_xYziCCJ9|FkZe7?zRY%$I->aHo zOr|8%-_XF1F1b*2UyVNyK<Th?9dzM!dtPl-GE+X;n)SQYpC0z^&oXXV{KS$jEU;36 zd~XZ(Ov+IhQ!)g{>Ui;fwlDdkV~UW~I>i_iTVa=tJm2lIfH^(No$If$ls8@9O!A{+ za6<Y(9PI3a5x&dGtcjuI`tTsKFhZW0a`ZH{{%l~EIDb9e|MoID^Qn|fc)E}q6RQBr zGz=kK?=h8Bu0+fIg&<g02r_D&?ETO8`7oJvtgDd~7_ImWD6&azuQXv{-(h}1kAUy{ zI-75vnvBBF$LZpSN3hS^Gz_MlC>MMIo1A`<_Js=k!GHu_{qt^Q6VE!lJo}z@c1od5 zY7BX{cn;1Oe3<FJ6iDk&>SEBFo1#icc{Hkg#Lyi#DEEF1b$hoRmRq)yv%6Qbb_M;N z7ImIxt=^hDeOM&PP6ZAOwW?S;yB}&;`LeKlGppo0f^B{(&1F6kIph0B@Dm$i!E=of zAFPtX-@K!Pv&A1!8RNI$H_Hz{U7CVNC&lvP_l_W^FDKH9j5vPcx1(I-ny*yxqbe|0 zjuEp47Tl!F!Rd?(k$c3zmE(!HFZ(Gtu8hNNUh%c%fz$bC-_-a%jf&7ac_3C8SwdX? zV?1zZ4}bmrZobR>7UUSrhPs+za4OClzY4<mMbF$pVu>0r_JrY=cT4deL-pCGCnrO6 z`h7-@lj6rtABdF;Z`1WF`tpoQBzGlJ9KXLbAOVKUX>6=4dueJNQ5v*_RIqZa`91}{ z`Q%o1j>B54ee{H^txx6I_;c*Av0XT*p9d?*)1vB=c7X7<BcD|Lh>x?_&rdYz!<UcK z23tuhUcttSuFlWLDOoan&g~PV%jp6BxOk7Z_7ly`!j<taUD<!-LH_uMGj8G6?&rXi z(f*L?Y|c4o<kHrY+PF<i!m-XF5$$enr|0$NqtO#tSZ!lOJ;utz2IGUI_MtA;cLp$< z?QWC#D>cyMWHULR^O7sA@g_?yW|8o;BC_^&A-OE&i$_0<0Y#Nj5VP?H9s2xvO}F(D zvbeNT@LycnKjr<KD|=^cl;GehTfyfsmG%ep@1L3xTvc=EYKGvFTC8x<yuLMWP5TS) zn03{>d~{x5+*VtCwS2HJ;+wf(cK>X_d|4gg`=CjVcK6N+XFNM3NFO>txaZqO$9RVy z4&FNh9Qf~EHC|Wrgykuc!j7*q1#<&N3UkC;9JVMP5y;zxJ5HLXz>H8=<H}0aYGzCO z2sDDUnYmtxf&{}GjtSf2gvOa;1f^dmFq5in1zzHj!op@F!L|(&%=_wL+|<b$!jPUc z;R_`z!Q?tUf!V{Ig3xYpVE~&iOrG`5am2@b;Z3gzH8s;c1eJ~Q9B28-As}tI;EMYe z!PLd?1f?3!Yi=`}1iQi_m@Qvt2*X!YIxg9GT2OoG<f*%dwg|Rlk8tc;Fi8;bR+(x3 z=`CnYt#|CEr)vtnbqN|$KRGsMN(%Ic|8NK|F&4(Z2^Vhvmg6{PYrN26{vE+)DN{j~ zx|pC{AzUzi?o`JYIGXEsc`!f9OB~#Wfbg!el<0}S0x+Ca548!yiPnr5-Zbtq-E>r& zyunHo&N+?R2_x8N*GJNM55w{1^ZU@3?I58IK4`M@1}?go4yo-@d_~v+($q(VFI&8k zik=%K%fy<9wPYbajwyf*OQj&MzLj`n&*nE7N%8cd4Igntij6Vt$3K}b!M<4%Mirc< zvT@_bLbW%Nk8Y>%+v|5QGPw+9UyZ}Q&UXAe<1>&qM3rAXUX^wAzK3P^RDnw?;Z|s+ zF{yqU{J<qD`1F|^duil2c1Nf)uVk*r(+MZ(P?Z4w#oePIacc%UZQyM7gWxvSJKiGk z=XIG4^IYMC?+mu2`a4>RrvFjjY*4xC5(Fk@lh-GYvco!6VB2#QUOjd%TsW%F$EqIV z3w+b)f^$*)iu;w=M{5w+XfA~B?=NDb*##IeOddV7ocYZk0(no9wQNqNBHw?I2Aivq z%7=@Vd7X})?5Ja3f&Of!b7rey!1W23WVM$}TYVSDv^e0_Oi@9PPY3k9oQIjZm%z?O zhMn`&j4w~PjSKVNz&f=lc+tO>sS=v;v+jPPnK?X7Tg||O_DV1gI|PfSj)3CYQ8?5% zh?kz9icy1if?d*X)ToUnPJ41;+Y@gn{b&gt3&LS$d@)=RE#-F3FJ|kc#n=o{0sPD% zI{fsaCG59sH?nK>K4SNO)ph34SVeKaH)c%8m@)HE<_ym{dmoh{DwPIJl87`Ak_ttL zGFK|IgbX2~c+T1TPzjMzG)tvOr6QH2+~>XTde?jJUGM$(tmluj&U((-zy1AwzTf@) zBU;<Kow{_+mU?@m0Vi)y#s~ceu)FI4yb^RlWGDe=4#(ieNk#0bm(IGD9;YnYP1vL* z2Us7cJ0L8*4(sXdgx~FXi9uopuFV>OdVvVmEI2_OaP36n+KV`?PW<e<fs<^}87C_1 zbu=|0V~OvY+v4KLJXjPg#7^g*q&Cg##21x~sdu5T@fJdc66#SV@0KrQO}ok{g-3U> z$3=5m+>M_|3tT`nHxv<n@8)whD(<DDtfM%WmiBVng?+i;-M^;2zbEO9sRi6k%_oTF zZb<?}UFCZJhQ#CjeViYid`)=`SLkDQ`v|kM)!gRwC7k&7=Z)88yEq9AT1}$*5}dC_ z4DHq9MqKm|<gUwl%W+!qioSj4qh04)DcU_zmGn6ClBnU#(89Xa#P*aBV)N^C&N)q4 zV*MSGxn%0cakMca&+-=&vbO{oYr}s;nD-U}eJLY+tQDE)Pj-YwlA-Oh7-zftvXyk7 zxiaUn+YPQ__i<v&9frQ)-OZ`@_2B9c613r+c4EGTGg)y(k_b4c&0!WelFRGoG3x(B z$)~`9w%v7tkQK8dJ0#laRKIh?Q>Q$_yv2yjGwr9_1P&72B?091h=rWnf4ABlbqye$ z<4ftW(RECg+H7*M(@|PU;v`WU<V3U#ZzbmhSd$4NL!1y3ijglrMe-+?;<42SalG9y zvbw5?mX&)mmP6Sv>HKYA;j$d4Pr9Qw{f1C`-xIKy`i??F>v7Ywf7s6E0vbUJKvFG& zhn+5igsF@0rS2uLR!0iVc`w57X_OJ#c7N#2?I*}nTY2ELp%TcsD+U})7P46~>p;*? zBfRmq87k*)#EMZ*@uAhL@b9nYxOta29NoDY*HtRP^9Jfz=BFe!p^OQC*?g2B`4Ia& z<;f&}Yw6nlgLuD4J0K-Z!NO=7+0=%D3h_<wZLK1@&4WVM4cvnBdGOhd+*#mOHA7@7 zXv0^-8rVnrJ@SfPg(t?Zz>H91Sktuvu1kx@T?OOJ=KKNp##@vfxvLAyzaEB>PfAeB zc~3m;x(&))S0+~0Z9@`=nqdK%!Aw5kIa1t0aIaAWW}pn-ccv9}+AqXQl?~W6XMe(% z{$9+dtaqr}+>wp!(WWPA&x563T+q)VVUS*TpGi@xL@N)UAi2ZO$;ExOOcqZvy87-A zvwYD8_~^GaDAzhl&U2IlPYdfo;+PcrHLStxGH)Pi64gNOcU#2wI-B<N-$51#$&#V= zB;zQeg^0sFjEZC>@#ePxott`|jGVU`S?vs@HTNk3w`Uw+*4<B@PZ5PtJN%J>Xdv3c zeSt<McEU^P$56hdBu+N=M+%R0k&$8q;pi5Q*2(Qac>x8)8l@FrQd0-1&%EK3jQwO_ z^$ce#A&)K`^Ft#34rGV7DxBGx59U{?5OrcL=+8twuvsXN?l<S7pN+3U&7%l(%3cyA z>0AYuwJ^AMS{1HU)M3}HoB&G%PC>-?6qL}5!1Ax_$mHF};5mbzKu{|etm^eb8lw}) z{ckIib@nLGwQi)r`wL*r-nB4R;|Zf-6^wiB3~~18UgpqITUcOyi(Xs$i_{vQplkd| zbWzWfR4cYY>b-*Cdz&YHiIQYgzs-Qmp<2dpV-dJCT1?;TmIdqP_>yx6TEQe&0-WC? z1N<f0$Z^>#4861vdG9DA-({8(uD9Hnx1S?mkZwG~A5)HE-R7bfMFFIIsTvc0-vsQh zIgKQg<I&Ac3b_6pfO#P+$j7!XKzO4cJ^lR!Gxw-CvoqeE>#HvWbRCwU;HIUFc8n2O z6Q+U$;!=t7?P18Sp^{Nf^}t%Xjz~J&1*|#g#dLaaMg_J;xK)d0E(xZfFKTM&t?(e@ z6BZA&W^F@jPD#L(!`Hy^wjo$^u?l<i`$An_2yOS8Lk!3=0Y~PHA?wMnWW+={*d_A; z)V%U#TFYOMqmN{v>K{{7azhFhDMf*V<RDm9`U<QbR>0Ap|B%0wr@`%EYe+<OlHcE~ zh2cw21CK;kFqHEUO^?0fE?oH-k6v1YC%<T6<$6IdujxE^!0!vip5_CA)%WNZyB30z zT0wAy;v{$&IgJ_x8u6EAIacP9BC{=|5fx@~;EzX(z(&V%o>U|cuFX}3tL}O-t&w8r zn`02@d~_U~dp8Q?p2na%I+ElOX*u?1avv}sBAEt0Idosl2D}$vhbE8zLO0DZHt4wv z>=HgeFq99rtK3EWx42+u92ealHo_X)Y=NzA9+-X9lzC}eg}w_k0nG*%$l0omogXh} z1XrFww~BwFr=PW9;G0c2{}cjRDh~jfuZ<?0e&F1s7-r4%Rh%$?HM99xIbQe11^;-R z2R-&3hl{WKAs25x)@02RWV6r+7HfK84dMOZu*og-cS4V~YZb=(kS`tx+K!jFrZGlt ze)!9CA6T$05`EQigGWSN;oZCLSagF4mAr}r`}gVLg*h`|_4Z=;Kf1F2J!k*#ZNT<T zi}Z+*!74(o9WWmiZLnxAKNQ4<xKQQ~IKv~+4aMKWS;fz}dp-nnZ|}WBd)7S%fqcm* z^ZgVv!O-BXB#U<Y_JELIpUA`yqC6}6GShQ+7Wu0+7I+Kv@H~|NF)RCD^_TyLD_dCg z8Epm!*mK#tSh8R#XiKOfR&L6K-4WN|ebWzMVzVAjxtmPoO;u7Rg#)Ov_c3maJ<lHJ z&^Wt$Keckw8D~;lGAhgtyNZdirCnqAePJzpy6Xq~?V+vxoDH+6xUU1Cp(qo-e^^GP zxjexFsc~#?dK_hyvzV0}QNojh|6qMzDiohHfzN2=Aa{i!u*ksxIvdxcnIBF_TWJTb z*R;h8p9-Pa%hlk5r8y=4&yo^N)CUDi9z)3oEH1OsM<I__HWvnmQ<K#^Cc3u}hPTaP z0~=GIa@z)2;j)ZspE^N_OE1JPB%U(1bsFf|iD;C!>nqc8Wj$`O5=QzP_QGP-hoETb zajI#79K|$zM6>7a1Uqg?vfHBcS;^nk%tw)r%qzPL6n;XCOn%wL%(kjRkD9e%ZRRf? z1AmX%Y!k(%Co7>Wa1H&F9%9YPW;JKNh=cyu)!D*89<-`&N={y8ki5S}vqNJkW&H6a zI3>>li*t?GuEYl}t5}AII9y!h6pJH7|KJq<-3Y9Zr_=}Z7+<mt*4Sr}OIPVZ)9Nay z7i|PaQ#;7S<}}oMDv`;``h+A_PLdjPpOfJZJP>S13U~bWz^Ab>CH$ib{tf20?<~@` zKR!ptUg)MV6}~)?YMRYO1*;{gmGzY{Q2a8h(d>vT8nWPnj=8+ibuoTd_#3LOFQ(7< z@5XgTz1Y60fb=#t#`3{=c$%Mpk;&8K-7VrcHpv_(1z#kM|MA1RELre~mucoRlEjIJ z>w$2E9}LW$0wtBzVA<R(z_F2~GIlFbsRpEdWTjN|pb^3PJbnWClOeUD#e|CGG23rL zo?z!2Mbx^arBwUMEPTu75!UP8Nl`CW!*S^%9-xyzu?x0AD(4n_Gb@y`9J!6howAWt z&Ps56wG`#5@d7?Qn$E1eB8A0{?FUkvY%n~>1RIRK0?K!fV;!Njn6mhZaODyBW!#v} zuKWUX5)a!~dR0^3&D7Z4%bT!MmmB-)oh0-S-^03SR#KeYYU*_E7reJD2x^S?0Ec(H zt-@G3evs9O^DcQ)H@6-|%_}pwt1IK#&&LEQt%<RwWD8xuXXZii#aU3=xD)Iq){$?X zUnXTXUP1-(QqXyy2kCwP08Wovi4!&)LUqDc)NH?VypVk+j=SR8e7IbYy5pTfEiQQn z4ew{M*RA+iV)8!IR9Zu2KlR5ppS(#+hf{FoXe28+6bp`e3BajpE2KLA6<l~tmg;^n zhD5~OsMqh`;*VW!c<ILwaP~?QdYjt~c1_+xcb}I6_p>FSBVU?K&|ic;f9a;LSR^o} zDMCz3Hby7noLKYzZ7?!U1MjiVZEm}IkPYh#z^;-*K)hC$iVk@L1qACs`SNLOm8Zo% z<U!e)Z*7ou>no(egJA;Re8V<_wfIA8GB$vQfX_e=jNiKg?2o#V!-1<|`RZx(kADzJ zEg8TUTz%2LRjR0VE&|$>Z-JNgG@1Kj33!tw4*DN{MUqno(VE4l*kc=~fHBd7tLvqj zbCRm5zI;onIWL#33q8z=JI$e#Wc8^84S*ZvPqW2JeXutt6-HzZ!PRz`*}7R{xHefA zi&;K^a&f2dMP~|!*R^1Uk#^>HdIfHNWX{A$#^NKd_F}p956LYXk`c4r0u{TFaMPd& z%Dh}ji|x}yKabrZX3h)XW<__Zpv8}+T&$=A50#p|rcbbUWO}IdHW#Yx#&K5peKI94 z;zg0QN$h}VHFa@QI{Xuu4DI9W*sjV+u+GXKr#>>phnHLd3u><5P@Pv`+jf%bkvfl( z&C`M57GpS}KtML;6P%mk3`I2+;0~D{6j&`wE}C19T7MLx{-1HA78%EIV)9Xyj4I4o zCWgn)ZKOU-Yc-EG|Dsm>il9z*$ik|F$uN$873`g;fQG-fQN~Y>!ei7GoKv$NYNbg- zl|x_P+)^{Bz@A47!XF?J0ZB@wAp-`C*kRB9<>=#uIIOQ6i)6aERQ&w&)B?Xpz|UP5 z_Uw6uht;2APZ2e^yiEjd3Cv_87ss$O>nKRXrLjJKU)aK~T=@NS5Uh#`f$B^dPi5T5 z5~fesSN<B5l-oNz5jLy&vgLaF3t1De@mxQ6qMwZ~WeD4M9Z+ZU@lTM@uWz5ya*jQ` zmtut$t)RMQj$vJ|A6Pf3osMju!Aa8(uw?Cirf8ily3_R-klGx)A$K1BW>pFr<7n7h zngeBeW0{xJ!uU#j9_zm6J!TY4s622RpRr#{-6san`d`uP4OtC%t4fthSRRG_D)jB& zRYTbSZY%uWufPf)7qeIV`4l_nC&KZkW7JC%VcccX2@71DVZpuYyivvx{>xIpj$hJ2 z?fa8RA*db5vR3e}!3Dx0t`B(jeL@|0F7~=E1Md6q;PM-X!Paak`1JG(Y`S9@SS{9R zP6$x6-_4z26V&)o8E@PRVhpgA&27-~_6Ak!a2+rD#Dm;2s&Hg{0$$_c0JZr{si6)o zZ0660!s{I2!R1Eqev<=iT^4}Pg{7fQtSQ{@c@}p{)}TQdEoMl#9oghM<D&f*yiD<I z*y-^c=<UdbO|xZSa@`zQoS}vVJQ|_yI&Ufz8L~J`p*d=IA#R9Vf$x`Zro?#Ry&u(4 z7;AR00&}C-33~<hNmVG@m*R@th197dUmwA(CuUP$UR{Ed8cV>jdktXIkMB^fzZa|L zpM{(5UIsH2rc~le32N`a9H`x-htC%IqNm|C$nUox9?n+B6?+X}V!9kO!e!`E?|y2u z)rmd({s7)<zo~h2gD>kfIL_9pCsN0M>|wKZ-NS-kKH$On2};ey1^Vqxp^B|Ssd=hA zvg+grSoX{YDWCa^#)VUO??pZcU(|q)HyXf{T63x|+XoB6VA4`2jn&;;&I*OyMH>e= z*jr42i5Uw+AIE<I$5s5u<6Jsb$G4fX>(2%%Ht#^uab>u|r-N8!C`+X73WKHRgh;KC zLp=Dq8}!Xi1TIenfZk*_j5yzi6K5XcYqMiuJsiV>MK0JTL!6r6Ww6((en8vC?%-2K zBo)2)6~}6w1gm1?0>6!41V?9CvOf>Yu(P-D!yoUBK?xZT4*fMoMaP4H`8NS__ti|8 zeP<txw@`<}OV=Ugfo;H>mxpfa5O1E^uV`<)A%@a*N<lNbj9EA;fy>sMhIX?b!|~}U zmKnT?M+bOqG;I?oKco~-_w8ln-MX-i@GiW*<P#q8zmFG`PXeysI!fY{Fo;x;hi`W+ zzym2>pziZ|vhXDzTsznX-2SachZav0n-B7~YEpURvzZzccH0>JSf-DSbGD;Q9_1of zIs<E-4O3J7mXu;6L5fe9;>ClrvHmOqmo~()&(vzEuwZ_;S;~x!*yDv|O1t6N5_vY} zM+&@0-hijuI$^JaJNTIY7W(kQAd-`FfssoIsCZBY7hMa4TD|&Mt=R{B?Dj%Zp>{~E zDIA&M9^@06g&jK-(9b_npyYZmwbs>#I&@HvjTF_T2J0m$k+>1C;E<&K@XNE*tS=Ao zkx(5RkvM=8uJJX~+;142U5HnS*h8h?ZmdEBMC)aRDA_-Xto+{3u(7`fh2^k#+SDB_ z+$w;dRTP6gbzNX-M>I?oc|$HW90vF1%A%|*{D}Q*j<0lT!S+4+z{=7B`P1)Fmps7M zT5WL0nQ>e#kz+6BaR$pBxX9jY$_L1~3Z5B-M18pn^f{@BDwFQPr90wK>VIr+zMKHT zzZBss(-YYD!d}|i#0>U4u7H=7&42+9`fu>Jg-=v_KrMeVbGO<QF6n#=WY3R)phOXz z>Zpbzi^o7&ITt0}eE^(w29V&CJBr(Wn>lo~2<{~qSfeD!RvuAjz2~s(=;1)>rzEfJ zkZ&s$)o9GVy2^!Tdz#r@yUtO67k0v~RqE`vkhdtrRuTO1>_le`X{eG7DCt~tytpkF z|DjI7q}!7C)}iNM=%Er`%S<7O-vQv{kqeyg`fLzX?1Ox{sxZ{{38$s!F(d!`4_crf zhzzympxDSP<Rj(H&QEus_K#%POXL@_1^!E@6!%sTpJ&7R2ie2>2B)ak8oVN}(gCVi zs2?`<N8pMXFRbSJiSa%#i;dCPjRYotp+mmT<k=Br9JDHsG9R)>8<(lF>Cdg979T`8 z3TF7PbOE}h{Rik==>q9tCA{qBF!<;r2gUz(A_vFyxX8Plb3V(IN*G)TohsbWi$ChD zYSt&}RdWxk(I{?TYDQ3*lfrDyu?*aunt?C0oo0W<t%8bKYE*ZLFur^tlJz+I2dvIt zMepPa0n4~K_>yTQ5V2msrrC{R&E{Qr-1P%mBQAurXYU5jK9m8^Jr(GU_j<HY|Glj{ z7RR!+ok%Uz0xx(`Pm8DLfXgjg;jun0^*jC_gn?m{bXXZMdu775I%Oi!nPXH*b~$9) zLU4?aI<;v28<v^Wf!7C*Vq+BpIQZ>8`>R0(ieWok{->S2zi>CytJA<zOZcfk`y1e~ zW(ZE^9JbrOG8wObIEajQ_M^8TnP)J%K-KtCh*g})!kQy!P|uiM!+RD|+cLn_!xx|p zWz7a|QHFPpM6k)v{Mm;2C#cCIvGAF?Ec^5)0feUrI92c$#QU^SIpf|`*Csw{n9Rcd zLl}HdxB$nCj8K{fLfQ$wc)3j)zB)S@YF|xYbeogmaPMVa2ZjSU#Vc5}2>k$FJ}*Qf zmsdl-*K+~5<cI{<1=0HUpHat?3?wlx2Y7}aXLHu9p`zm@sD^?sR6u}mvtibCcC>IG z8)>abHMW*ObDu+$uhb~}$GnD(kbeUI>KRg$zY?rn`w%>^n#=w^DvLu-2vX&t10cEA z4j$XD2qcPw*dLLinB1)d!oK?BDkp~Yv>8O}J5;dh2Mfs5wSg6K5_os82z+F;53Aj+ z#llhP$Zp#?WHGXe+8i7U{6cm#r-%x$gi;|ja6}W8^2)DvG`X{LT)x7-hYQ)5LH*_( z!6fVJb_P9EodA^wpTpV(MpRpdF${FSjuj>J(a<+r==vv|Rm)q7L>KeJ2j5$O-^Wze zw;_S`x$%TJb~O=sa_<uT+r~lTD*-h6NCaKj@B~aZ4}*W1pUH0)($t5=ew0847aSfC zuus%$!I}~2)Y<86aHQZfjJEZMKGYQa_sEj6nTTdb%^$Mgt4~s{yVLR3P2X`{RuXg& zU5MMpwOHMRG#pf!O1|Cs9WO{uAj1PADe)FdU}b%u*l_MC_#hy|SWj!>OrisoiERdx zMh+-0bPVNMCZoPgL$q&WAUabL#d4>w!<B!Z!<!|1&?L5zn*FbYow^y%wu&Ka9&3U3 zzf8tspo6`YX$+@7pJ4LRw0SacBd8kbM!iWUY#FcYQ)oXwtDH=-N_J!T*28hsx=|Tf zui5!OyR!eSgZ!r}EBgKf92dWcowmF47N|-jLB2s@8RwYs#}`0jaWp>Ps)C2@BG3vH zfX5~I*~XKtAh5{;FRSqckrlT{(fyMk$l4G~g^i%k1y=Z*$qr!K{S3tY{6rqV=?Bl0 znL>_u2iUk$7>;awz<EH+U>!M8{Qq%f|LgYt4_DS;l_q{-q(W7P+(%)jMW{2WvaIvQ z1I)Ecc5q%&5!UiM3pB-LSmVbhpzIMFyd}Z{c=p->wSp~JcgTaS%;yKhrblpga2@=v z8_VLiRVZTZB9PLZg@uxN5X9mTIJQ`X+Iv%%a{0Lry9xcpOYf!NyP~IXma#XRIHE#z zl@qL{&p0mmc^vj_v*cCs2vCoE<ax`70<YRdlg&`h#UY|Suyef##pkU;sV$pNN&ne^ z$CO{7oLjBTv6d^?!CICLri|Em96R<zS1-KTsZDkG=Cc2yPauVv5m?&v0;i6&gMF6k zus3H86yG96WnGGevV;~p6Y&YAtnURG7PsNhuk(0j^Fg+niRVa+S+XB4bW+dV_EM9? z5_X-t6t$^e8#_!cpmuK$hoi!=)EcXo;1blPx`G$7W+F7Kx4TZ!8tRmrXgFRFoDUai zc~bMo8}akEt7yff2;w8(C$4s+38D7Jg1(z{o8$7wl;#%L+Rc9wLErGH<C@K+(CvIy zbPU&rm~hVH7F2H|wuj4crIqC9@8RX#7Mb~UQIiU7%!YGP>XaG74k=pY%XMNCNpq$i z$q{q6PjRZ^#OWUoZRxYcagC;Vj|rjZeEQSwJbL<>8Qr9GgJ!g3h{n)X!bMG=oITH# zIP)!!C{rq=%~BT=TR&zIfg#_xJC-OC=H}I$iLMS}#%T%tukRK8SlgG_>X%O|betg7 ze4Y}%i>m03o=t@Ir9yg*aVGuSM4QyMY~vKq)g`}dKge;oyqo)Wi7j#L(`BxbK|S4I zm)&G^Xg=rVhWVs)zbaYTVn*!A4<H?yWO#W#L~nV5=!#jtIU$<<#Gh-?#L|myM0kug z6K?US$$4KC;mebRl29|T6sdDBr`XY}uVv8NIS!16`Eqgt-zK6yIizVrO*;46@I1O> z<N}8?;=wU+yGZNBn9{wIK7@7J3BrHbEkb7Eh25jX?;MMNVsv}MT4JQ}0cWIqJ-26j zGKX(7<Q_8M*twH=v_kw*j!4}oH}}mmLa<~p_w%-A^aj&Y#1&$idvW>_cb7u~(YM^0 zvq{;T_R$a`u6mjgA49LwZ4o!P3F9va)3g#|@r5Do*TuqgsA(>pgzf0GrX#d`N0Hrw zURQd<sp6*6#@?p+)EaurJ0H%iBN=x4D^y8oH(TOw<0Crl({FnFP&dcRzl*p&qhous zL!5YjXFD(E7(^e=4WO6RWfMZY{-ctBgY+h)-E?f>Y3`DM2-?<5f^m*^A{U7`(0&sJ zB-bo}xNwG|CGbz~kMJy7`#@Gx_LFWRsG22C@=J3^mX{L|@+M61n%%U?VSQ5IjuElp z(++y@rVSIYErdp%CkdBhR`i)FCyu9?0}&J6N-w!?2IZU@;2h31Fkj#y(vO2o#-{H; z=M_Wx+-Sj4CNhljp7rdrb9`j(umJSS5P?1h?$r9*d~BbW8+&H+gk7lMHOOzGh<EpU z0S|EnR@vMWzc#v#Tkp+d8>DzSy&)Y+=aCY16w9(+YbP+%IR?|a&yfW?9oY{@bg6uc zc$hkCHOpCT377dUpcWnc2H&DR)SQrYlyxqLmx0a0|GX@zv>&qoSF;_q+zf&jc@=^B zo6S(pOGW%jC>x4!ma;Qxs_=noEzr}Q&%V8?$^$xM+3s8Em~(m`b>nX@e!6=ut9!?T zIs|sGv2UiKWLOwW-1f#Q=9#R|{nOO+=tulWE1Kcxy0bIGsjU0$Q<RBk51wz{j$c-` z;KQMx@y_KFaMhIs?DNu>aL@gJ=*)RrczI7AJYO}Bavl+7pX`ldt>@G;@3iORLu;ad z;ND|Mrb&W&z<ozv6PhM9?@W=&52f+cP&5*$njs`|qnLgD06wc1BtM)t0$mzD;9j5? z*Z;~@qUy5^RFHp2t{3$KoY`FZ-sO$V{g-*j=hhIZ{ihHW->3kE=Dm#go*43Z)@eFG z<s4b2|DDLy&t#6wOC+onECKpm3znXMKx%4|Q&A9r3Qs$t?zrpBo6$6e&b-0c5OPQ& z1p(I#6JF)z9#UtX5i@Ea2d{W8#&4GPkho+qFj;FrqkS2`>6aqV*(r@KdkDe<PpoiW zM?QM|MhSG+4=^Lw<$>d}LU63;53{ay38S}78(YTA!oYehl78ca9}27nI%bpflMyrU zyX!Ui`oc4&+4>Fgy|M<hFI$2V&p8moGc`C-v=&ZnT#IrS<e_<CN??}pC*E>xgl4Q) z!EgHKQCNE{>?xx`p)y3Qh$OgqE*9ONt%~ROEe47$=18hqg6(iF1D8?~$k`?jNHt3r z+?>dhSQh4kvbD2-=%u4j?R`3V=tTi@*jR~;YHfi;aw&58>j;A+A<||YND;dwICw5U zUL#-#D@LM0?ztYOM)@JS92f<@Mm&PYcjf^l{lARig>X_JI;E+3h6TcpJ)yz-9pF{W zcl?rX1c(I80-e`2K!D0V#$Le+)@wV`V*?O8q#vLa!GA!r)eiu2Q;}$VJTTeu7`and zvDk|_IMwtoh!%{cO<qr->#EY&?^z4r`EwxEycAHIMRDefHOMP|59*E-fn2eA*j#%G z4hO5_y01e(Bf0=b#lAvkd6gjJXBxnO+a>4|(nHSpl_BrnGB9kg6FSbdVpcyphQCu~ zAY<+`a7o#ds;qT^&zA||vyLL<jjoF@`}i3+D8+{ZTtkt4zaCOR9WZP~5i%3ZBDY6I zVv~Rq*hfKyjrG%ktE+aPfYMK-7f(*gJ@F54iqk-FQ7Wi@V!>3BImln)8@ick$S%q+ zfnSH_<5eb-@Xdxq!YlF)I4R+bzqrJLQGRbAB$9^q9U4Xaqa=BST?zc%Z$sapTi`~> zZZP4+fk(c@G3{3K;lQ!O(Aw-ftut>8UaIc{`_epcu%QU_wjC#podw{Cv@wuTdyii4 zu*N5oQlO&aDfm(-6J=`-fTha0p!%^a#ZCW;elO)oV|mu#eLD@JMz#Rg?--mE8w3|p z{h-*!P4IyqPjco#|4((i(ZS9fxY{EX?mV9j4Z;}I)VUa|m^^^Cadk*`X)rCx?uJ30 z_n1TJH-TRC4kW%{0jztGj{n9ekz4kwW4YRT^a1T;?R3ph#PDORPO2cuec$j7p-P~y zmjb@XS0F!Lk9(|4F&^=d2MRVUkY2k4{*%gN^sZhf<D?H0rw>~LG3{lHk@7{<I<|*A zvT7Hp*JlnFen#YDM{~3)pazV7%_7zNRuJRq+9>-=1(A5WiPWjQ!A&n(#*E)s33N2` zIWb9fxZ9!;sQLENqB9?v3k5sroqGx1+#twAaqlxqv3t1tzy3y11Iy9y#vw+ZU5<!~ zcC^Yi7_6U+MBdwO5L<fqG51LtS^E4x{$u%2>rgCGzAS+Bp$Yn|bO<auJx=G7vyty@ z226z_ZWy*e<~OgQV&yKNW0`=`=J*47`El}b*){I?ay|4$?i2x4QVFfUE#RJeILNH{ zLHf%y0F7`9FeUjN?X$cI>iK7bE@KBc75xQu^CZq^Kb2Xxs$LKl56ORLtnm#Mb?kj* z5i>Bs1?{zZc)jQfyvwcxT;+WStcETy_cjiLU5%@;SD+s{@OmF|+SCU~#Uzq{NQ>Dy z-h}^0SN6Z>?EiFSy)u^Zx(^rPStT(r+UR)G@^6C7tGb`uvT{8_#IlyoiHxIvuS^9w z8J6%%$}Mn2x|=(v@+`{eyb3h0nStjSCRi(|6TEIKLVM4LFtg9N^7gpa;PxC}UM2Q3 zv{_ADB4|hG&X9w=6H9{vWu?R=Mfqh##RWup2N{a$?2zLf!D99QTvhsytGx61{_o!k JE)kFx`(IxQ=<NUi diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt index 6aa01ac3dcf466092a2232e3e8ac2499686979d2..95e91b1aa90b0c8ba56cc46bdbf11d7b969e27fc 100644 GIT binary patch delta 25988 zcmYhicU({Z|Nq|}+Eb~t3yF}%^LZZklTBF}MG{FF4MZs8*-+8eP})Q^G?d2koX6v& zlo2A5qDV4cLK2eo?fv=VclrHs{(D@`<8sbp-EQ~W^-<m=R^B95(V!uqN($lG-!-74 zmJ2}(^da}M1+5tS19ZQu&@MgC!FT8mys|w99lkFhdQ1i^59pB`VFj)z?1vxECBRDS z7(S*IhN6xclgXZY$UxpivS4xnIVEF&-IIoJz?ZG$Y#2qZ&bHwQ*+!<--9;LYE>NfN zjk*hAjRLYq$(1iIc!%rE=8?~8N^s=U2qPZ%0!l1saQ&7Ao{KqHdqb4IAFNMbe()R) zE;UD9j$t6}{Q*`TjDkhUYSiart@zu%YslI<oq5seMVx;rLgndS)S6kcASwP6QN#qF zl$9s@oEH-Fr>4YxV;Zr$kxrKA3v%(l+h@S@nmCMRiGt(iLSpu*61~oiMkOVI$VkNs z(#lQ}ch`KPSALUB>QBS>iq*N1XG7#&pD7pcLW&zzttGcQHxS#Qb7ZD;I3eQFl!@UI z6#FBBtUP@U?|oneEsx|#&|7@~({j+=O@q|mTF^cl57h$sb)c}(8`90BAY;8Njjb@8 zouUQDJQB#76(!hkZ4&s-v_f^m@vMQ;1Bm;POgu6j$Zxqr#9+%rQjxu#)Eg*pZyq<0 z7Bx{W@x2(=8jwTI4{&7B`Wlj(D<s0n^02PuD_Xmw7k7VI29o8H@Fe;La=)Mrf@gOw zqmoyiB!9+y@@<t3su`UKUX3wG;^}e7x?l+I$E|SqR3-FOZ55f_=>%Qr9yr&eoTB?< zQP1@+Y-P$x*mi0Id25#mlPyJ{xnd(}oEAg&&uE78>ow`6xyg9w{#o2~ekSC#jt^zb z96}xGXIpw^vbM!91$@%1L>f1AQJeOOQ!my|NBbX7M6u5>zIx&zf8)))M9xhfCM`Wr z4jcE7)6o%NbN(kQ<6}yWdsM=v!JpK^?^BUqND?_%Z3sU5!(jMg5V8Fz0X9bq$j_ta z8EM}l$QhHO!9<MQs0<^Yy*uD~KrSTeNRWb;f)HZgn*|p#^sw`CYm`6Kie`xJhm?zb zY)eQJ5?LXM=SF|P8zvvXJ&V?ppZs?GuPP87>OM*4WIsc90#nJ#t_*U-@FD7(){mR0 z5a^eb=O!nlfRO*0zkI=QTy)0<nN^2C-6vBbdZCsGRvd*qSc1w?4oMS~sBxcSWug4R z1DLI+L4F*(N$P_#VWnaTx<8tPe6GfjWgooJmW!TH{OB8R<H{xQVYNKF_g6k{-9E~U zC~t=Px!afnrOn7@brq~N`i_(GJRq8P3c^z!kb+At0FCB&#3c>=2`@l?Nk!z2+y$6Q zOA~#0cOqyr<dLBSJ)$&0iAy})L8R10xxt)Ha^77)5_-Sk*y$6<wd8yFsB;l=nY9^B zZyX}XVs+i^LIJ1@x!~O@lK}lX&Mv-6p$7>V{1Rf=gVjmcs$Uka|B-@Cwz~AC{TlH6 z-Us&Wd>fq4*TQ$VW<&O+z2LNBrT_*X20_L7G~7F{4V{!eLFOvaM7uDGO?Z0_dbj>2 zPpx~%<aul0{;x35`EEr9_Xd*{)D7g9C5{<8JNug(c2ds_wgYeZA*k3g8#4c+(7dmP zXe{j-7GG+Dw_FZI??jxD^3Q%GJ>vy9sphedoE}rm`t>j?Tu@808*5S6$ua!u<w;15 zkHv*+UC4*qk6_a19;nVNf?pG&U@)WrmK;>4T_3&$YT8^{YyKx#FIfd8&syQ$%2p6s z%0SJ>P-aTI3?mu(0aX_Z(S53l1bxrI$8)rhO?)>pn9xR1g?-q+GmkW{mn455FD5Ah z`7Pvdbszb6V-`_AGzqT$xrEV3HG9f?7ME8oh32>`;jxT1R_2`$ndL5pfUI2dt$!F- zG>=kD>M*Jc`a_x>#z^p&5U2{xK!(oS$eVK&B;Dc+oR@T?!rcqlOHGx;?aoH1ylDyY zbFZU{C<VAFr4DE6YO!GTc5O1+SOdqeY=Gt-A$*=x54iy%Fcj5*BDQv;Hhv3fnT^@j znFFjvUkEZclp%h;H-xV9v!T*X4>A>#$%^L*<k7TPHtS;o<jney6qHZlE^~3{ulpEk zDp^80k|Rj!<9*=AyNO}90Tjuo5#K8q2`fGdP$w0^U*k3trP|8FH`)X$*GRBi4xNVI zGn8TRJ{>Sy-3?vKy3xAjkC4C8Ja92($>xG`zJ9YPe9%dSEkZLas$E0erhkA)vs^MY zKohonD*`D~C30A+kbL`M2|8w<(P1S8?o$3;GWo(FQrqPRoBrG(W~PEBc5=~XY;9mf z1V<yuB=t5l5c34f%zBQajD$Ezb1B)TY7WZ`LP3Qrf!Z4}psTPM?{>WfRWJ90LRS^} zG!}!7e}2jE+?DB|vyDVwXaYx~Q$R(%4PW091&SYR$=CNz_+Hr&kY&4|{Ot}buX>Ca zKN$lDLCZ-PqK=XZg$wpxKg(d3u@w6?ouNiDY@jH>6(S3rh~)C?Na4|AT>iubjQ49( z1}{R1nb8>j*Xu)^COpPZmj}St`>tf!<bQ0i{5iBQ-Isi<KFi)u7$B>5{z8An6v-`o z4)2-23wEq812ahz(xqraF8YRo;K;BP-7c$1w(H*{zVv&z8Bhr-RYmyF^eec})fhGu z=kQe*Z(`pSor0iBT^KOGOD&oo&iv$QLJ-m+7Hh}HvwI!huy+@^c}9<&rSlc9lz4#_ zkChU#FB)s<dw}YODB`pJBx=dj;q(rAz$up@>OZ|Ol2RHVfD<Q5z;m|}xnO;UOzA6v zZi^xK7E()+6W`(|O0(!enIf=xA5J3EHOZfEhd}f!#_>~k3N>w}fb-f)*e=<CkLO;( zUkvkj-%$wVYA-|D#}Y~3HwUuq-fI+nI}_`^b;UhjMoGW=LgMJ22QT05BKtN*qZ~$% z#4W061hdhp#5{8&$^CQ)$}hUYvk&RSC_Nl2b5T(2p-DFlUBVv6>`3hGd+>ZqF1%E- zV9%~wPYjCy1R56PmFfs(BGXF67{8#NYso;e+YfZ%-d)s3U1s<E-U^pDzrz2`P$I4^ z<MXWMAUkNU10mN62_u*yg5HT&z}iVk<l6e9;EJA*khuGxwpELaUDYOQ-z7o#t0s`j z%^{h$bcm7k8}z6B9R%(<OJeslkm8G?bY%BDkc|x?$*zM~E7ud`oD(pW)yjUd*a4=F z$rN|m0gNp@p|AEOzMOWNY-D1oMx%UOA@Upxu3y(9`c{_2rAdU+@yjMFE}4*lssoT} z?M&Q@v~gD2QPNX#6|IkSAY<?ANkO0mY+bGm-BatyOJqzO4lidK9;!pH)g<om?z?1e zTr*PQ+d^Q>2&pSxL-uGZVFfoMd?#IkTyH!{B$eNy`}!#mqoPGR+<vS@)8A@B$l+(g z_i6;qzr7J@j(x<LH4RY7M!;C<5!6&#iUt(4LBGs|e{L`X{|8f<_2+aT@Xr|9|LZFX zn6Ly^J4wqt2>j+&F+oJ6VnWImjmat51|}OXZ7bm>KS|@Pj-IDuz>AKIJk6m)Q#lFM zH2R=dES>N^iEcTT!TstzLbrZ9OQ#;+L)-fv=1L6@aT-hwr>P##Z8zv8_v6#(|8+5} zAvt5Yh=|DYlzqN>DK>_V0{cZ#{FnPun18XVeCfb>!pB2gy+z7z{)>SlykediGx$N% z&iQ5*^Qdw!zhsFwZ}&TW{<*)Y!YA74{9Y9!-e|{Z;mH0z`z>Fe3AsE!VZz~$!po{N zg=x91JjUyZu%mS=PkPTL{v;VC{>tHAp{nvD;nuB@{GU7S@U36H7BCW1didY-NBO<n z1mT+Zi+EacVRl7*mzjzpH~vw#kh-Ebcljm96&Tg(ME=PqF3hT$3gMH=3qonMoSD19 zfGM4HLZ}uk%ihz~<jMWGD0HN6GS+z?7<<zpW|Brczff%lUt-@{W^!&kQ*iAUfA!+) zjFHw!zNyD@cE^Z-FjEs3^X44L=N+9fB;5VHgXeL%f+@(H##do?3e$&=GK)5b@;|9O z<Qq*jWKPAbq4d=ysLrWBh1RBS!cW~{%$0;7=4ny~<L9-L(u|3(AAX;}U)QU|j!rb; zs|}?x^B$M+H|=O)oK8%oPngs6r%)v>MM;_tv5i9^f|*tj7AwVZsViv^CXu0U>d2vQ zD>0KD#d8ncfT14?XzjS4#BE&bE#@Ra&~=9$>FWdUgN@|NbQ$`eku2xOByrXg6bUp= zMe8$XfsalzD`_yDJhQf^tz7lV^pynNJ9>~&NS_ar{_Ds6vUNmvLM|seZ5t;D$Vi1b zy60d`oGo3ESq$}xDOfU3oIaNshH|?th|XX%lr|ipYrbsc^u~9=?zbP{NJuoTV^~ji z?X{pQHI9?1C-k7Y^)v9+NYW=gk~y*4KdHPVD|%POF_NTEg<J26l4+~0v5L(bqOy^p zSMjsZwc%vk93Cm)mL|q<2i_V|IR6u|UL8bRDRkq|dPnYE?`qCx-3GetLJ?h>tVR4a zjJfwu_*~#~J34(q9{qlL6Y1LE%1wIaK%1InaHTN|xyrdYWH?!kc1^fJ=DsqbAN;z= z=_r)J$n0p?ASfnlc1IA;MZ>s%w3D#uX*k+aa1RbEA{<?&OxNt_f>NOu>0I276IUq1 zIm?se*5!KW|Cq{ZOWh}utLAaud&@w6`D(JiJ_MfJ%EX<=C(#y6CTTWqg1XCt#OlB* zPV-h0vhAKij2`OI0?iDP4I8=Y2bVb4rCG>R_A2OEZl&$^7GOV<PI6hWp^yaq(58bs zW^;GeO4B?2>R?VHM~bvA;+ky*l&p;ebab?V-YZpZ>rMo!aEl~F*0AAQ#Au%D7Vh;K zW!m|C6n!E%o0D#x39m>F4E2X_V)LSC-K7m&oBkR4#3?CyX1gR;Mr3K}?H4&mnF`K6 zDHQRn*Av0xepT+b{{Zg_QsEqQE^(I6L^)$M5jx_wE8P$`h0~R6$I}zfg2nyMZ1bQW zEIr>vetEFyLUbt9_B|vm!HUd2aZ`SCRJgEp$81J1GMq;ZU*yl(_fROg;41$^sVcMO zV{Lu@n;A@4S!DgP&Hsc3sqgBi?u+0_d1VQN!`aFFbn%zG#VOVNZ%vAJ*E&Aicbdf8 z&zf_#J~u4Y-m2Tx-aTLq!|aM+r1f=qD2dOzB7KlK6lKQv6*ls3W@j>!V|5tjkTY-L zG&iAkWjG_@I>vt+<SlH~jp9#}%w#(1ocQuo4r5}j#m_1G$D8@An%6C6%71lSP%6B8 z(UOw?cZ2U3w~W8HGTqLwN}0E1iUDu_7f1WozrQf?AA5!IN;N!@SwHyi6RMdftGgMi z8LHHEFHhFrrJebhy_!EHy-~R0)B$Gj{1WD8h#TAgQH?6z6(aN=bLTn3Or|SuEtP!k zA)owIqh2XbuWwr*%gzqI%99ajoZ(-)QNrKRdaS;%Jd9s-dXV>2?HRxF$1%2any_Bb z^nkzWaVWLr1<mf>d<l<wsE}L9kw`In5p*4iVZM6WaAC`8slU_={NmqEHkh79+f{Es zy7M=n2H1|`KDY1_%yqywb~9vl)v*fqe~`Vu8_6?&XR<1}4hgn}IP<^y=-JzcJ0rue z5qz(69V;!|itJ6)(ej_$*orJ6E>V*p3Gr#{{v!;<%eEq#)u#!gpUBqf?}DrvWq8(d z0hyXOkq;%akW`y8G@UC$>w6lhseK1<&ILCnGJhBJSL`KD8@^yI$8q&^))4I&VUXKp zT>(rjd<f!O%OUOVJyhoA$Nb(YB&peQpnuN;&+?Om!u8>J=|pE@x4jmJu8joo&za=p zPi1b|Y9&r&Eh6D(_2{1)8`+zRk>qa5YBD<YH0jrN$GVTx(S55_yrXO%jL8p@&x3bZ zMuf-KJub!u7h~{-MG9nMq96e;vlJtA;cFc5Zz6rjH;#yISjuMG-N83nx>)gT(QLK; zdDOX~lN!3gV|m5bsmD(7)b?lo)b#Kjtcdyw^fbMresSbvWW6|zs;t){ZnPGEcFGH+ z<@24HXKKmb!FkmFRo_upS|gRB%j4B5rczH$-T41CxT73F-!xESVyKp7-BjtUThz}j z$D#ad0luGp5m%_s1RIxq_|%}eP&7*hSN%4_(^srQT|11CQ;{U;&-_FEJnTnZ5YEI` zpIxG;U`_PQr<L9LXf2Z1I}Lpgu0vOfy;)!N8Pu!BQXG9|8k@+7vO(L!N!hsocBD9z z5{#UVV~bSHap|AGq)B%fJdfK4ue{nwz=iv$>aQsO(ZL)P6_(20u<XP$wNr4=bldUs zAPrNU$MI8{BE&0+$JGve{3>=4R(slkYd$z(^YTx)ziA)LTjGiX{H(#wwv9FATcDvE zI?(!B3GbXa$Um^WlbU?(uK<0?^T0C-a+!kc`^Z190Cj)AM16eql9_mGCnd6R2*uAi zO@XQ+Yu|f>a+%=C_t+XhDRdhl@nx?mgROVj1(6%Tag!y#>r@A;yFm=faVOFVk!5|? z8n9k|;#9uoVz#wg$bT2niiQeBSS8<RCThYEKT-1&6=5T|O|5Kk#)~u*vAm-=(u#1$ zo(sj$D-UHTn>HJT&$YyVw`B_DRC<`zYQND2u~#UhXfN69C5CNQn-aMU3Lkx3&8Ad^ z@Ex!JVdRCG=<J_V%EQ18Yoz2-FXx`aOXF2Z(yY1E=-)k9dcF%KR&Gy<rF`*nuK{#K z@GTZ6&CSQ@nce70@^XApwvPmbi4o|nM3K%Tta5)KE9&7*eovTB`gR|M*H<5*lgq_G zC>n{Rlv$KBRE#eoCvdCRC(ZKWc$<zqN_*l?efaei4;+pL^DBW=*2=%gZgVUOnfZuX zP?pFlcP{~dS!J;Q_?j;WwszoN21Fw_hnHA>xiKXbW=LOF=tqw09<$E9fcAIgqPCwt z<AyU7>XztX|5I9A=l3dd_>v8KWq}6R-R(z{=vdrR`hZ#`R3Vlh9UynvPqy~S4P@Hm z%_c88hP&i1vms5d@I#+o{BOG`S$#+mEqM5aXC-z+K$H^{v7Y~BqVw1d9yvC%e)k&j z<0u=FbE6h}I!0lam>4|KZCw0*PK8ApkI1ve2uM153;#|vLdRP$+rPUU<W~O&Q9(-Z zdq@j9z4Ota>t0;0y@&d>Y!WhbOrthxc0v2`nRpgcO{!14C7o@CwAa4fl;zFsl@O!# zjq3VajiGxMh${U>2B~GZ{n`N{b)o?M+Ez;Bc?O)+`cPm`<`D7nMC6rOg-2t<F)x~j zo>rYBfi9V3s>4cBCa5RLGxEUojMe|i`RC3PuRF+zh&afN%X!_D|E9b6<70|KG4BZB zHOU0V#9}Jx50WK=BZraaT1Dcjs|LpEjpU)x8Q!tbnf&1Y9x@%7@9^p<Q4+ntmmQms zFC5xeM>(tu!b|4c<9p49)cWIVP~sjNa^z_;b_=Vf{wMJl4W50qbzI_a{XdC+$I?cS zbjJ_v8So&L&%9ByPapoZIRkaKTVdw~4M?<E6U+=n8EY+RCS;8V?v2+c#lxnw!?7|t zb&?j8&D+nJz4oKGZo5mzG1BzU!}IB<Vqef5!xosX?n2w@8#mlqahozh5hOxViN$Nq zbG<=-KqMm>A`RYf$19%;U{9U_9NQKl^wGUXQ5gd`P(BS^9(Cfr8@O@HKYqsZp2p)P zQay}hsuE0_bCs<fc!G9|gi`(7BebgW4EpeM5p58j%58et3iFhm9r6vI(l=i8L70yN zeQGR<yL?0o$$n=cA$koxFKeQM`=}|E{GUAd92Y=wq2LLDgX1s!n1@CG`9XzPId~*Y zB*DHF@N?s3cxCL2ma1Px2ez#zG6(F)i+z(o?O6hKdlL_wANjN2Cv~Evs5y8^Ljt-! zFAUbE{l;x-`^cHW7`jJu5%>8(1gBS<fRlfvbBinTNNiI+t>JIQ{VI^97uI+{r{L)m zF1}a=_!I8JI%|R_*`DFhM?~kkHDH5YU2cc^Nq8Qr2!bUs5K?l1T)rwto9>zc{wr2e zsX?EqEm~iZ6<-9_^+`ZPR5^~ADFVM6GVzp^`w6OyCnFoG@IZJq`WGWf=W3m&7u?@T zA6RyVK0cVo#hG@3;5v5;=Dw0{Xh`lMvwYTZx7B^<;k-}e7IzshEI137j)rohSL2B0 zE;-N&xXejus6uP!b2593NA~oL;g=^T(jC9=GjEJ$;3Jt$c&p!Yw({={Jklg&skfKV zR@cMG(xe*OUT(*?o-pX@HU~6)Z6?=xO`KC$#}m-^gQOuZHX4hhUPax0+6|5m_i(%7 zCFq%N&yk7Rmh_}0HRSMuoAAl!BzE_6W3?Z5p}kd~Ks(739{%_UyLR~ySkr^DyxqB{ zMqi1=Y(*^F*@aGx{Y1Qu5RlF2#24f+)qnI8o>*jtJ%Yrs-vMo^<&rO}UMSFo5pxdR z@t%do9!0^l@)`8sEujr@3hKnsGm8E)eJKdO#zR?<5gjz^Iw^dhLc604T&j~azOy+2 zOn=`2!M}N2WP2$&I{iC26(7p2$(l{%WNu-@w%6ou%P>0_mjnmAQqU9Qz2xxkK=Ntd zd8)JFE<0uRRGcOFa|4}O{}@>;ktHj849NCUd6ZrpgG!EFBVM}=IpfN8Tv41K-TP_- zcj#X`r@F5U?7lw0a+i#`EdFjTEMpYU_-zd78Y$ouxRd;9FC)u$uO?#Oh7rn11Jxi? zb{!qZT6xSS{X3<|r0J)K$T@HJ#m=i}exDO3aAS-)pZ1lgS@|4p2-7C7QstoIvJyxn zgdvY9s-W(d4a@F~q4qNeX%T@E6#h9*W;%xh#7%cd+Vhlr+yV5Qz!s4AE+O}9x{0sr zZF*6g2UoGHjr?*{Am&g>F17fP8}hHgXO|e=A14ViWi`-tEeqb{%L};JfKF=v3JLsL z`3?S2cM`%*Si_K2qy5TIJy>b*g1sR*%t!_;p}IcK#CCNEWnI+34<jU?(VCB*G@d8? zByCy=nKiTruO;MW7PlR8(5*w}WS#yg^7eKV%vo{|hn&}?JG{@q@ZM&`(`!e^OfP`x zh&{O$E6~EpR}-Q6?ONnjq)wK{6@gm&AeAoBkDJeHg3Fmj^y0rOXj-}#*`r$gl<}aL z$u;Qo4`VbC=!dUJi*Z@3HTU6B8QWv0046pmq(=5PvWeSB*DBU<W4s7DD0?b-o}mNt zqG}=MYZJ838ikn?-hz*Q72FaqQ#cp9SaLdS9j&mWAKE0u>5Kpt#sX&10rKjg)UOP? z9vX3;Q8~m-Bb4WKsf$}zdY4n$X~ODl?n3h`n$Vrxb|g9|KOQIC#yey;;+?g8B&GNo zH?>UX>V&U|xoH9ErH_+XIZZCetO(yUzf4wKtYJK>V?a<jzl+t>Y~@BSD$~ye2kD|* zA1>d19Vh2vNMypxiD|zN9J#q0F3xm7o-1jh5-&wNSD16N+a8cRSuLz?+bq1(V+L_c z{DA(Pzrh9zd+@WX&(Lx(XQ#f;;R&8z!_<#4Jmp~{yZDC={kfhd={u~*m#z`obKV+3 zgMyPQokrENhx`U1cy|jIDfJt+$seb0%7oHOa}?;OhtI>{s5t34`38P1G6z-PMaVdR z8<Mo|fi^!(#PJ1~|A@geo)m-BBO?&;pTH?3sc?>8zTibwzfr$K34ZjjfqfpJ3OW7V zlz>d4A52Ms;<|~Xj|hH{tLz!h=khmf?)rkZG`8XtoK(3GK@>4eU%=T8t8s1`$I0xW zWKP#%qJ!7uQ0S~Jq1Kz25oT#U?QJ2!9euu(d=G7=H&>^PPsRyo33CrNz11Nbqm}TS zS9z%KRyg|DFHb$1J%V&kd`7Js^U&d6_wjI2D|Qm>t3yA$l+Zk<c>adyt(3=I1M**3 z9A>Xa(JpSq4tkq!qWf<c*49Y`1$W;g;m$NI+q{@~-s96L84{fAPz?Mkb|wvfRuS`N z3EKIvGK{Yr%G19Z%wxs5&F4;F>2sn)<)k<ycAX%_*E-M?iDta_y&-(nHY0*}+pW>{ zSITf#`vX3fxC`0r7(pst_i)t1^LTEd26$Gfkg!%EH0k-0>+}DkbK+(@<Z(50ezOWO zw-X2byA!#>4eq2x<{ix6Rt;i_k>I{R3o_kH@D;;TuuR68JYUs^J}Fc5%Iz;eYGW|@ z9Wa><$g(7Hk<kLusc!)nJtFDDR=)Jv7zb=0@E4CH{Kgt@6EN!|WX~L~LmoD@uyXZf zI6g9j@0RqE2PVaE_~8tYkdAgR7<fmVa+Dg1xMDJ=rwu;YSpgS+o1o!A?5SD;BR5ou zD^}sEi`5{GKZDHpR0_&-+p*i~?_|4@AQrj?p26rw9yj=Q7kB%uIrsSB5c+;w6vuX7 z#%ji6_?rJMEIfJ=D@jYj^zIe7E&V#K@f#U;SyF%pW|Q$?k=wNH6LFuDg)XR8(~5m3 zxaxifTJN6=S74>a{pR_jhu;{eFzV;56nbfy&g-CY@euiG>qCYGQH6AMq$!up)RJQF zFUWCW9kG*aqn_ULrSoNd_|c=!u^``+Z1TqJm{lV+=ePz={4PaSt4pH&W~01XOI0#4 z_$@wvF^2kj=oVVNyO7`KtVg>W=Hj-JI8a~vmYZ7IOn0n#$$48vfYrbIsI2%ZWX^Ko zX1{zu3nt|&asLKq;O?VmNX^sLq;<SD-AmiiJx?BkOY8~4ZKmnYy-(qhLk^iAZOa<J z<73bAd8nJF*z}F3@qTANmTGjx*XK?_dk3DPpcMsp!_5qo)+tHO23)|4B%4r2kpWje zluk+##_-g+i#eV|H~n~T4V`n)K|nTdD+itCG7{z*1vWpk>6(a3+=4%;<Z`eNMT9w6 zS49kaIJZFArXBG5R3+#iyAP@IAJAL(9&*885^On8jWjI=kj|y=L|ny$8z|_*3oT8t zZLSbnf9695vjxrG7RO(qA>c-iZsZQ^pN6!5BBFfm2h=zU44~zJDkU763s&`#^q%Bg zR%b^MWcgHpa@+zu^PMiYaY_Pb>V%1~M}}-!{)82=SH!z|jqvMJ-(XSIKW1dRsqkRu z0G@GgI#)2Snbe5sk%(EfctCsu+$H&N=ClQR>?6Pi{*TbCM_!a_Sv-1Iwi&N`r$JbO z<{t7>Zwh~&wFKlnTS8~KO`_ZC{?Nw1O&Y?|;%MHsrF8q(1Uj%s3KTNl5{bAy+zgS) zP(@9lKUg^ter*g=P8a7)+IVzy;&O=6|AY<GQc(B93}8!SY5ygAi2tkqkoqSr!nCBo z_P%jj=xBs2+n=*Pwe+yS);9u6T?)nvzZKwncXN^3xzG53?r*mGL=4w=N{*8jvmzDF z94GqPkK{=ia7{7aIn9Gxx$So^5Vtpd_1#ZRxV_Jp&^eQeVNU2D_J*-8{2Lo$_rE(z z?KIhmcxhY7Q@d3}{e?U^Y4ig}^fitfqz6duKm=dSzEFTW)DS2Q8smL;(s9L6N7hE- zKjGAsD)6rVIF|Uf1pltPgQv%v5&q_dBxKqt?rf|!QtFm&D74xP;!V%!>n&bHj&kNC z=_}l(lL~ay=VB6Y57Sn)8GxSL!RzOpCQtl3;iiQhggxv*HX81rv00g;okX~#KtT|^ z`5{Z0Sx_ivkdJS984`!FPEvR8IqI4#i9C(Buu2Xm(1Kk-*!yrZ_V|-ab{}6s-YAD+ zRo+gnr6P=Wd1V5jb%k_s(R3Pz)X=4?DmicIXnOj$0n%5H2EWw}xkTMaj<!_eoIYoh zbV`Lg{V9#SZ54rF!Eypd%0A=TeF11ky~E$lOKaxLKZM#MzGJ~aI(Ax>3=dZ=f`bcv z@lESNWT~hCFWiLK?zsoz+AdHFY*;iLD+$E@0bZ?|2tty>8Ar83m+Yj5rS^X*skJ$@ z_Om9~zxXld*jqs4&t4;+ou_d9Nt;QEx&kK{HV>dS*ZabqmRz!a;$kF`T*u}Yj(ht@ z=lNIO&j;@^9=*?XE8J67A|gw_pmn#mkQ|qAGT$N_)huPv(syp)u`M0UhdXg)kNfy` zZO0vI-p;+9xRffdyA95-HMy|q?~q*92hNtdPD=Xp>BF`CP`XBq3vs(90Ew>>WWiTQ zI?+YiAv|1yo+Tv#`!koo@Xxt~!ZnnFgDtA<lO+fB?&8tkqvTiQZD=sMf<~^~;ve%H zuh7k#iQ1(k(5A*4xU?%8k7k=7?WqPNWzt9feD!qn?NKdi?~*}#%+8=YM~!KZ$Ir+= z^(>-&jB(J96J*k<t{iPxp-vkol#?$dzG!XgeEP?#ySS&Jhh7;a!%A5nA`UL`P;^R# z`)Pg!20fD)>DYAcUz-K^&zlGVNqHbQIR*WC{Tg;l=An0gLZIY$H$3;01llPa7c!;T zel;I=+m_%!tPA!pM98Y1aAM}4z@8PXT*{R%I}iVDljQ2o>N>n%c9y=sW01QN;LlCw zE)X>L9OtJ$og0-hCVRUskUzGQ;k#2dIP<nq6N_4L*N8ZG+w(sX{Oth|TPMy<Jz&V0 z?$`_`l*U!gnUgTnSRB2yTMtiR1!yl*g#WZg(c9oFXgI`yOm4Tvg6Aq{(cF2a<b(f1 z_WjQrNF-gF!@SG1*-?KY797y<`TWF&v}#{&+QJQ_`j<Aj>@Eq{LwX>4ZU!4ur`7OP zw2izH?<Mc3a?%sMlG`DF5Uq6xhMPp0GdW@ikKSrSwO=B+^nC*TXZd!d)0u_bZUQ~_ zc?^=4X$!FU%XfIa^)0OTIRb5;5Q*A@FQSXaqMY|1Az72BKn8WMl3g#;Id$vFaI2}4 zUXp0f*&Y=kywC3B(wt3PYr!1wnypM8{OTv~t>hgnN{w-I@=<7u>R>*S^E5IFBF%yY z)SHb>^oMRkZjx;-`Jl9cNUXU62N~sw<bQP$zlL88FO@|^UMl}zIc5TFBe?d?0_V@z z3{S+Su!%ofQOW)#s5&%*ebnNPO}dnD&8q>tcY;48jLgD^mt8|rQs>af8y(Cydd7|% zdxsxMPsWRi4M^IbCR~9^V8+5U1QwZOtqH}xsoDV2ncq<gs)inO1z6!afdm$R#1e<4 z{;xWuvgU}Wa!Ld3AkbXm2~YDj605v;xH&m-d@tWYJABZF6oCr2=kW%zS@t~4_q|7) zw7$dHo^kZ=)h`?;cb<%8^}_C~c5p1LfPED{ple+ZUHY@Q)ycNpg$#;g&D~*OD@{KY z9)_Qroar4WzCpXaGChC!e)3)6F8UJeLYHS96o8X?Ehzhk;qseWv?RI&NdrT0u(62v z+a833lMjIX6brZi<dghsbugH=oXh@a!#Qi5BtQ1+koOrL^vfx7+^on2^gg<iTvvNZ zvUI{=>D_LkY2Z$G>t}O+*M)Omg)`~ODs5!7S%wZQyi0!>Dd3(>oKD|;CfEXk$faDp z7DZ}5Ie}MZ4ZI&4CVa7J95Y9at6v$#I#uM+gKvM37r)i$*!F$g&lAhI=_(~qn3+b` zzk7-OR|Rr&j3yJgDHlk9H^n{LW5R{}4xz)9-=ihN?&#RLVB~qQg!VYNiGSt%cj0`; zkM+Ug9<~AB57k!*65WM;6BLC<cAn)KN$g@i&&jAwL@(`c8E#_&pGNRs$4=$z{xRU6 zJ_y1q>iRsvq<2gp=gmhRMZCtHUd${*C#H3!BJ+E(62tD6;y<LL8UB+3yC`NMf32#9 zJ&_3IOSl8G`Oz2qz}N-CPj;a~)i4>p`lJ%R%cg#w;MgfU-Y<#Ee5n~({BB!G#=Y#k zkao`yUU~MDcRiw-S#GeI$HgZykGrz@bKCrxLy=qfy7K4w4`&AOuhw1RXOVb*)qE$$ z{L>(B!*wiNHA7W6>9Hi^oz%kADcSSh*UsmU+*ai4SN1V07PJWGR8{cn9dGj%<^iKF zS|||um(6C>LjKhUXCi*~zXMF|I(_Qf@fX6Eo-w?YYoqygykpe+>`wcJdvp06YF~Iq z57<#21NF>Ity}fm6I^++S2pmhm&GyH$Pg3eCCkwJrG+@;IKSfj72d>4i02b9slHf7 zhI!B1!;cZT@OVkG%qBZOzP9>)UfFzsi10;;EI+IMiG844Roxt|@4_`L3Uv{W(wMoY zCsF<CG0goCK2JQnkC(Qzhxhq<5l_9miRWJ9XfN*6XaC=GXTJE)Swg$VGwr*?Q+d9- z<oNFwtMc7`sWHcCXXd`CD&L@7iMbpwkEa=Qlz-c18{;*jm-qfy0k2?-k&qS)N7sMt zkzrbV>zKEO(|B%HpZMJmY+3b#3wXp}uKl9XFlP3I3})%ZZ2PH$!HmUY1HMdO74yQa zlF2;b#P_-;&U@w>$=9(}<rj5ZGZPP+Qh#e}?Sq%ZGN*EOF`w#tnE{t+%$V~Oo{5o{ z(Dw3r-n~mAY>SN+6JZp^7bGsxr=A6^5(aM1WLBX6_^-Fm;aks+<!4{m&0PQ8&)>PH zg|BCm!4$Rs$Lwi{vv*go<jW1UGCLklV=A70<Q01;V!z~@_H){d>2JHc*iS}3v9hHx zbZmS^ewAJ!U4N=bZgC-5`^bbo_UI7Nc-e|C_D!Uf?Hiylpj7~++b?6=g{x_sWld=3 zsu^6%%-LM_t~k0{U!0VN$<m>leBtAK2fAbGY|=Q+B;-$gN-k_Mf|Sl+diAc2%zx># zU`NSc^8I!;Ip=eZmcFXMwI>hZOF#eOg;ulZE7OwT$jUCZb@c)|ciDOJzI6kUts%sQ zx<G>d3Vg8L%hO=HGK_Y3FURGjPo+<JoS?IA=%M-Z+-dEYvz&S33F2PygG4_Ur}wS6 z2Zb$!3!fCt?b+hb{XP^z{%#PX-$q>IwyH|eFFdEgpG~vIqro>|cWMtkQJbQFdbg3M zT7OCCDHf8xW`pe!8&o~TjqCfU%bgTt&4<jbGeLIyZ2DVaCNvjjl3E!v+90V3j-KVy z$l)QlE6UOZU;4NYsm|<rX>Z{^_x*g!hbP#fmptnC-4j&9wHE5fA6?4-)(P->vWclV z?L@svJAr#PEUCX#s7{LY^$Arhj`PY+vl@XH(Ul)6=!Ug7`@moVD>$Bag3a815?$k2 zQ?Wkh*<LXrg`{(^Qkx28+y97B-{Zu7yL3g^xFH;8a{8=>WE7Sv^`xd9`++C8O(N@? zlqkkaf;hTbv+U$ol#SCv9JGH560JQzn)Icq4`S|E?prHvJ=THDR<Fc+7g*zBRZLmz z+rdg@$O^ztZxl0Sm#}@XB5Kh-$Ib}uKu*&$8G7GZoO5C_>XuO)2P@;SPE!mDT=tc! zStSO|$8+Jia5BC>F9JUA_)6u@tifR=osg7Tf*oAz;V>_f<SBhXJf~9#^=%`m&0~1u zo>p`$z#CulmZ1WzYKWL&4CE&ZmO=Sg9u{_KVP{L4Og1c_rib1~as>|ba{0Sdmr^ts z3?D^SUVD&6cQhRGm4-H#IXHXa8_Lf2IM`$y$4}O2l4?Cm`sQ0NESlkoRR<nYIaA_^ z<h9A9NZ}#YfksBY*PFZunM7p`uOp$sbKrB^CY0M7iB1akbx_9#C9&t6c9fnaMu(qI zzz+hnsAnIW5N!7c%j!?;?$6U9DbF9c;SMnLl!5DaMCi+*fI}VQNPggb(zkd8WH$Ao z)Ut<IK^P0)ZapS{{=<0Rhj8-9brCT+KAU;cynwvYdPy0@PNNhm9Eij&16G<ogaxrD zqVZa@Es*vjgG|hPgEq=FlOw-AkVN4b>Qh@A-}`PY4zGVf<^P=sKi(>F6+6bc#%J9i zXBi8FfsN$NsWs%~mSbS3+JK%EsK8n`8DQEb(?<96K>u1TUel!wAxB@Zb3M-BlS3tN z?Px4Ex)%sjSIh&=khub`ce^7koTZGTHFto~1WCx*xt_ZCbs?B9jKrsSag=G~5n@PB z1obd&zV+(4c<uD5^qYp;czKu+(eueijms+7vNU(pw77zAV>HZ)F6I%d|3X>2Y-OT4 z^(_@;>4n-<OVG*a0DQ$R0`1Jdgmzd<(GHjPviZ^iBTCuB5k*Xi#ZMPc!@YXm=wHZA zY||@EXOHc{rk*nBcGx)T_j?|hw>}U$9+#ua12W{8Y!JYBrQ^QJIAQZR5?x)K0r}Tu zkh`fCUVG0I1Pc@JX159CfQSy%1Zz?yZzJJh<sCj=7e-_yDoBSJN1^+rxJKZ=5dKXK zr-}^+*=SW!&<)#8Oxnf~&3*A;uO-4Z-{a8pyrmp{DG^;v-h{H4QgXthA3c#y!E#TH z$>Cq-oXWnxC}nj4^1VL5kN>X*mu{6s+F8j&uZSZH8lI5RMUzm4$9m|Lo`%Xky=Lle zox#TC$8f<bYrcSbY6+Xxr?6W$G1LNy%j}2Ft?XIxpF-i^Bg{mfN@};+PBP7M^0-q@ zVdEcZp#9TUB5m<_Bt9pcm3|e4PMHVt{r>3!e`_c;u;K=#J3W%(_0?dDg$6iC&zvQz z44JZBt^B*Xe6;X>GjF+MCbjIG80sw&2ZiFV0xZcmB0t`ASav}Y>HHPvuXsKm2c}D4 z+w->gz)E$r#b*ItSA2o-@BDx=_ZX0z0Uxxx!;-pitcx8Adx`dN+34;vJv4RBKJ3YE z#9~Wdvo>?2ak<4N-chwMB&}D1JDon_dEF22E=v<^JvfiN%-4YM(t~*Eob9sQ!&?XN z`otRc*x4I!r13J!$*-Xr)z*`fJD;O#8M85Zd7K;={)hI?`hw$ju0czx$`LN#OiJ7C z<NRr%$ZZXS_6Hvk=Bu{ze{L>9%?UE-f0F;}wqf`GUjn<^$;Cf5;V`*o@(0gwEdbSq zHr^%q7s5kMIn3ep$@LAbQ9O~$)0hKxgOD`k28adUhH{}H<s5SgtuTFn=RA7H^R7LO zW^E|IYkxMfruQp_7p*p;ujiIgmG|W8t*o{FuiJrgmBasE0$b&{+-9=iy<a1T(C=}} zoJi|l>7g~$9JrRMX<Vq;JX&7Al^!j54jww@4Tn9K(pAEnv{{@xJ*+T~>w58*tn*Lc zYL-TG9W8xi>n2_L@R<|zZJAqeGGi(iFxP;5_0Z&23?HJ`buWViK@`_jH;G=pU=h9f zv>5HaR*arLr&GY)O7EcEdNKWIRM+9DVimnwIfGV>DMae7E9haLi!?QB370o9jXZvJ zn@$SW;$joT8sdWVInh;PT#d3BZGNDb^L1jmU*9Iuv#A+evy2Sa6ZDG6@d+^(Kgcb= z`-uDBFaMHuIq3Cx`ux9VwUfAnh=`1gh)BwD-}&0+H?EK{r~Ta7;q?D&T7Tb1Nq)SU z{CH5UpEByZRN&tDoB!sSEUB9pf!^$tCxJV4iOXFBras7n9c%lFYKDhVYp^_ds-{Nf z^!|stZ3>{s>lWD|@(?E7?xG*mt))9X#2pm>SkRNoWx22g0ql9t&$vnN1lD<#iH-QX ziS5_lRO<RRwBYe~6tkuqzfG-23v4F92MtDmMd={C&!qq-#Py-G8za$!n|g5fQ8@k{ z6^5qImL?g#j(EVWgMIqP45g}7;G4bGXvE2mo%Cz~3v^bX2WAbBwJnwYFm{b{oov>? zD_BEI8W}sR96Sc2s}1R$cG}z_gYz_eil?1o7B!sfP9>Se*I>;xLU>7nQ1Bi-OXc`1 z;U{VA2klc6L4Rfzsx4@uR<{2E&zxj(vF;9<lSiYhjy*X0p*NEFF_k#={HCU8O(y<5 zDsXA08hY^iEh1~g;Hsw?da&;r{ppz)=kn5#R<WN=&yZGa@NyX-67!R2S-S*$#BUKj zHPD{Rz5ARz7xatLxYmrkn05+dWnTyv)r2<*BdBS>jwD*1Lx*h)@xyvOIGI{R?b!H? zwH2D9DGEvipHL)%m|ys}{xBAgZbe?ZJy7`-bsQw@!nrO@i1&6pYaDpWe%O|b@-!}h zaoKe)Nb4LueCdqCH2*{**W%3m&>2rzJB+0ouIbp&x345aRPO-XUcZM^3p_`%$Mm?5 z_OZmPWQbf%wxQLdHWROX0#cxm4=Q6L#PW&+=eNL<iw;bO8~^EotmPS;JM$^Z4$%YA zw&^%w^bejc`WMNMGXW~EQjp1HQ8uH&6bUQ{x;smht23Mp8a*8Xq`Wnz;bEFR-CyEE z*xS-vt&tWzZAm5EUu^}+S7o5v=?R)?Oq0YzE;u~!0@m?K#68nZ!J+gQF0-2m;ro)< z%&dAU<;qL6^@I#KiMr#vCLi#3DJkUdPLs0E8LZ^MYu4kIEV;5f8~Iu56PvYvg!g@v ziOykx2JzOKhW}l=1F7#%k;)SW+zdAtxQwScxGt9>`Q<wFxyuCa4SGyu<F`;3Ble&r z_x8iuOGin>F<Y=*w~8zbO~hR@yvfJRODKQ16YXE8M0G`Elk2-L;iy&>nBP4B6MAcq z`dMf6tMCF|V(|)Za`=mQGWPhU;QkmAN@<XP3e$+b=`u=j*DmZQ(L~9e`ilQL`f^M7 zX3$7SIy6NsrBhF9a)u7$RL%BNWJik}_pAOP8M#;u3)iZ{A%!mJ-7+6S*MyOE?XM}@ zYL?}GG=b8!bXa=e1e$Ev3=bme;q<!aNM}Hs8}e<y?df(zu>Us37U|;1_vK`~&qftI ze#n!>9b1w0-p=tWASGzgnOuC%H=0_XXb!g~y#%UsHU3=2YnUlmPu}L7Bg((!>A|o# zVC76<@wO=ZVQv{j8+DQU)CzKj_X`zfo+i6L%Mu$iE6ABFf{xJ*$gf=!E{a&;rvgsE z#I<h+nR&~oeNNrjWgB8mcbUO+dtJOb?i^Nj)5Bk9{y~2dKeJ)Ji!llPhFCXse)?fe zvZs9l)bvg#Pk($yg<q^1M2!^bGkfiEM3oDNW_=~rUm}obg9DN(N(a&+MUQ+8L~GaW zA+xT_V4ZWjNxiBB+5S=Rl0Bvv&Qj|e*c)Cce33L&5`WVeE{A!M(Ea1WL{t-1c^|{M zuM6<Rp-^m4uLviub|BjtEqwaTHmajWiOgSq3hTX@P1^Ivt(chdcy?HetG9}Pwd(@< zQpv;yhph8NG4cnhyPHA=e3p|3|K4D+`GMSu%d_FMV0J40*SH<8TX7!QOm9ZpLJz_v z`CM%E$_#q1SV3Kj8q!X_LJ~Iik=F$rKBPPmCqGR?x>B3)wz=8_ep%y|=4!lGA`eA& z=TKL>3>k|wb%-C20IVDnNY<|jAi7}(SXi9qmj9<oSE-ePjQVDDWA#BeZO<Zs(vG!= zRs70G2FlT1S{g)ie+B9}AA=S&Q>4mCmi!AoiInQkl1<H5s197hj&ez4mBedsShj_j zAJ>8WU9)g<?s62o;6J?Zu?F6#{1cx(@EudT=AvL}dHms?0)!3Nq4Zm2c%irwouMno zDK6N|6-^Iqu)QEBphd<p?HjBJ45XeVEqNKt`JtWMRlOR@a=$Tm>i!yZ&ip1Nck(3C ztO!IhV%MnAResQ8tp{2!{P3O~(}<Lp0uj2}qw7yfv8~T|0@YGVeZB961yY{qdHZ2z zDL)o9FV`ft?Q$?lZ6z|3+KOz#$3JYf2pVt{G^4guV=~fTgEKM|8hZa~J9t|5(}5>4 zz-(1BMoW|k+4zUi@+WXb|1c@<jzM$mG-0r(8NVsbfxwjOjF#nXoOWgsZjXA8B0_R; z(HVI(@;Q{4id;Z?DGN}QK@lR;XW#?5pOJ5C6i%q0OT|`g$7R-&(H4PdKGr<<fC`Ow zgg2fv9+@_Zc0n(x6nbILp$!fadK<a7sugtil-tC&RhH;%okMP1JP0f1QDA&amrE`^ zjy{hphZm1^h)dfmc>Fhu*!(>WXI;ARmEsCKvhE-75;W+N`>TnrvJ32a{|8CLRZ*o+ z7UP2@U08r8)}Yq?<M^ho0((halC+QGp_U(|sZ)N(5Z-4-ZggB=zw{<U#Zpyr%<&{= zb|8ozq^fBBA`>on)dc!Za5;8dsfbpI>Jrr|YnY~%iL!PrVdrgcWvaX!$@Q1nc#g?( zw(5>QkqXfSt*?nl|ELW9_<lb+d|FIE=2Xj(WCK$czvZYa0aa+t%fFPTg*uFyjiLL| zlVM?!4v9{Ghj$$4L#~}Ae4Rt-?E30trfp{ww(2US7p#_|Cd{p(cSq<$o#+!{bSWI- zyATRlr$$aWY-jsbUQl85UcyTig`=Sj{AFjX@aQ;eVqqAMww)Ga)t?TK!PkdbR5TuL zZ+AP11O6D|iHEMDPcbu5=pSvI<k5x>)wJMM9;v9WMH2YK6Nu=SSLoVCKYVueEb8qV zdCtl(4vCN$T30E%;hZ|h>EFD=r7ov9`h^94xbY}xkNR*E3Y*AYs*tEE+QA-^IQ&CG z68#h0F(*TFcM{d}%}CU&1Fa9(O02~!*_LEKVt4oi_y#$V(+bnTKVSl`)c3|~t_4ur zKRrQ`*ENXo_?3VoYm|t|>E~Ex!AdkTVH{ZdK98Z|wj;yEm0ZQwOnUTm9%o_j72*x9 z(=zpwVV#&2xA{vc;0+eAW6t<-{NZ{abT!Wf1-z78X<P+Tzt$5g<|w)yP=*CJEpQf< z!I~MYLqWyXP%Uu;G5!^}F-?{jg^W^OPhO&b<Mu6O+#iB}9v&;#f_4&&zm({r+N`PQ zMB8P|+|&iJAEMxy;X-UiV+n7sy@UPp|G1kL6z!gMLO}G5lh}FmcjB#LPwto9hR7LZ zptF7<KH&bI61na}_SL+_t3slLUxMyXpI?+xr*;VuZXu*hT$!w!--K`dR>6WZUzvNB z#&}ToI}Sb0Fq3x#;f+a0u*2|K^#7VT^Jp%<KmHS?Y}t}hB9%fVA$iYzT`ek7(yl!% zQd)^9-eieF@wSITWi6$WyyxC|S3-(x*+WH=k5tl*-@ETQzu)=&_xfYb%$zwhckX#E z58VYKGWyD5a%IOsp89VgTmN(_JsataV~g(MPmTQT_-dhuE8VaGtA*BpoAPdKS>udN zu>>wXKACbQ_n>b~G`6h%$sXLa4@Tc{<_?|I!E#+C!ii^#;M<jI2z|ukNJ9h)xV;{K zEYaZd11>|zG8ytWV=;M^EeH2^_A;%#jqKjLmzf2DQq+E7FqzM30=?hFZ0Fxq1K+e8 zM5LWYRc=k<3QlFxdN1*&qdpsNKCz88t}cM`CEK{piV)^Q#2i|F14Gc=H=tmy10mnU zC#qUT<D#T!Qmh*}KTDIs#wF;wgm|?Zl&7JVx>Wz5A$e?-%v7BSLz9m_M7nj;=pRuI zQFYX!srqWb|9xK-f=2bA*{>_n*e};nz>3kJ_~`-powE-me0PKidkettau#PcP>ma( zEaO)G=SCO3s6)U1ZQakRfXx;O*gZ!9Oni&P{r2%p|MB-Ec+~^qe0BnPzhEOW+Pj$! zoHB#sLPhdDz>8-Vcan^qA45hq+VJ9Uwem?pXd1fd_>;U*R|NY{E~Lj`GWfa-vYb{P zc_w;6)Z15(t`Pz>U-=xCc%v%Pn>dDR%$5<EEH1-3`<8Hag%!9@fpW11%kcEpUOL|5 z7szI}!h(6f=v@_mx_F!%Oym86&P(UO{IC<JYt}`69C4?;F{QK(_z7ejvx7@e55Xt( zl2EtN2Xgay843v+3+67a7RA=rP=b^;mD!tyu5}sH(oye_b5ju7yW9o=+Qqq7mT`XG zn42UmfqnZb;I-L$ob9d1-A(>NdybF5SH@*=1)fK^sJ5>-TQZQ^BwVFY^%%KUJCU4I zsc3+|SB+ZP%5!BB9T4L3lA4UIB<W8dQTeOM&?zuw&lU4v>%LzksC_X>FeyZ~Hxy{N zQVY{<-%PZ6wjt|_6M4Un`;a+j^vJkuPh2snnhP8Ek=|RhPNXHg0H=@GA~l?bbq}X< zOTTDvF(<uoNns|Y;c}LY!yU>u^?$(TbiAhrvb3R<`45%saiCT&eh^fi0g1I$^wid$ z?8{Y0$Tr?t`06Ikk!y{jeyVW@k9x)q&b}jTFN@*@dT$|t%9SMNznR3Ma2)M<7{t7E z%OGVYQFQv(8??DmNPRnEM6XVmiZrjJ(epFa=}W^+Fy~V@pJrF-;i8ONG<|C-6{VD+ zbymHs$DHTPe7|bc?|_i!f-fj3YmgN5oIy()T%a(=8p>>L(i*)i@@7B>bW@esH|IAX z&39Lj@6HIM)P4vhCFh_fai2v}o}yqe7Gmt!Wi-097tNSbPwQH>EPv&X0Bzn1Zc{%W zS@uvr(dphxM5XMwW%o%*Zq98bdU8!OS}yUKE|^(DuXh(gKu0@GU$7K^RxroUPkx01 z*(OlZdL2r3#bB54Tqt<K<ATbfIkgs5YPu+a-L0q((i5*T55SfTc^x8GYfn6!3*#W} z#Z7id=z*GKLrALRWs=9go=rop{U*tKY`6_hCEUs|A9`i!OiKwPD=5w{f|BJI$*izd zh)h4n1#a*XC3c*LLg~xEB_<HX`~jkII*|AedBeA}9;n3HmRx!+3z;7epy0S6V(ot$ zU8~y8Xu7>XW6PvL%O?f-tE#~XQ51=fb7MCs1S9@y<4!g&Nr|dXok3qOsb;;i=F&wQ z6>!^=IQq?eh*S9dlFJ^$=QP8zg}xpW@lG)wWL4UKWMh#77yKj(29IrIZ$_jN2S+=0 z&3-Rh+U5+Ko;8B{vi($Nwmje?ws4ATBCZS5>AU9nDE?pqfoH?4q+S7$N*yEq|C`T* z?oE%0(H~{Pb5f$->F<fN{ti?VpbhCo5sXX9diqP>11)=HMn77Q;cCmLh!UT7a)Uf8 zt|40y%M1>(S>ysWyxhj%6BDsa%?+CI^%i)B-vy~?@fMAqBZrZIbgTPwpY;s!u=8_p zS~wp|XPa_+)0OQ0*U|mu9=PED8P!?>TW=G7a&v+3ZE6GK=HrKM6inw@XZ+;p{b^vn zm_Gy7emt{7wuAwL5ccX!wopVOSmkB9Y(YZ>Tbb!CymfSxP-E0>CaN$H-DvvEEEJZ~ zG=Fu}UVF#FX3P*>^T>({*Ii&|tobb5G}Z*W509o-A}Q17bD#WEbk3&4O4djESn_Ya zY9qAUm^V0W61SslCpr>v4xU6Qus`-CEc{yd8MPN=f^AMLk(HYSsgYsS<kLrJh&>7i zMe=m>%JcM{Q4#S-lLN)nS;(T*f(%(qgKmKal;3|XwE2(>@$o|X&iV{G^QM={Uv`Tu zkGnz}<s*r(CWKVI7srTsw2^te=^S!%GesA_=z;q_DH@uW0sf_dsFeK+<@@aEXUF;Y zuf$mV66vsQ7jsDV!dzy|jQKbOOyPW#5<OC}f*K4<K#=Au61vis?tZj|7B{^Ik5i%G zC>scilcJ#COo`jQP|RRB+dx(?iN}4xe`$b#f0#>n)Jue8XJF?Aq4bpOA^5!LDvWz) zO7c}^fttQ7mbtYPf~pyA(w|`X>F&oQ_uq$<!3Zjq^{*>A6I*wifRBSQ`pyr<2A+55 zv2hO=Q}bFf^8H7MUvC9<Wu~y#+kv_~5UW0l7s<HBnVf3i3wGJ@+3=D-Xofn)jV`rd zIhguJ8@l!lz-E~N)+cs1`m$gAIUn+-IUCE!0~rfqLll{iHO9d6353{v<I&eOHE`iu z0JwB#3r&lg5&N=_CVoj`5ARb$llV4t%f}Yl)fo&IH(w<Ef0oiv2?O$>vk|$b=74@! zJ}JAVgZZT=<?+MP2zutD9KNF%OfTq+#I3b?keiT=Wart6)%OZEW!(aNCjAJtn>U(k zj~qumme<ihPgOXc9s{qO_7Yar9Q08>1nsD${kPi5E`#mph5c{BuXTVeGC<=PYx?Id z4+r(f(f`(JV}-xhVW+z~J$>vBpWc1-fk;m*p-+YP!0SjBOxiFHpK7MWY`+pY-Mx#m z4Esr!4gG~SCu1&cK|gY1BzVlXX0-N?G*+un#oMGNaC!bJSU2CE(|u!1la{XFo>^89 z{)m1yCTl*oGE)tfmnMSaxwqttc>^*}TtS~)oeyRU-D#;ce;5hmPEg2Q15)i;5Z~Yf zQMY;-y(QhKrFSwKKG{#4U2d?0+QX<KyaNr`siIKzeUP}MoRFzE$f94Pp*gx2W$db= z&PEQ*S5vWl(QAx+qW6=h5$(jsLmQk!3R%et6RwedWmo9=!u97i)N1_+`Yj?HO8BMb zP~@tJYt&mvnx8Uk_*8*b=&i!Brqk#Ssr#g5^(<H;R;e#Zb97#923T}V#A|q8VNucr zDBk!E-WfimvM+bhLCs3~H@_baESp7>cL#HZKfPc@?PpZ=QCxlAIGL*dau*k8&hv!V zZODI`X>j+jKfT=5O!?CeB=VXvT}X`oD7^byJgrH%jaFS#!PPf3;b5W!lX7k~eIr~# zTNSFQc9thLQeDlB3g3w3Je}~!QZu}E{w<<pIe{z}>)xArUzkkEg%El7G}<tGA6&=` zBvuQefw!8#u^R!bQIkA*cjP7VPpYLl1uL0cgDrgMTWk+`LmIFq@-+$^{Q#Yk7QnBD z3Q{W2Ky{k&$Z~cZO$wYufAy3jN$(xx)!RzimGS@%mZc!nZwF;>FOw;<*{qTag|kui zR40EFD!u*~_Icl8?q<Ft>1!FGcj0XE{9ZBa(lW+Z)a=0e;Ysv4IGh)p#?OSiMa?vR zxRAVY8pGv3bV8x6>hNP-BRGtWf$<+Tu<VmOs<&>m#V)@R)KIL<?Yxvt)4e0Oif!@8 zrB?;}mFCeilSR;<mkg`I^vG{4qQ@rYBK)bGN^CS>6wnG1^~w)NO_n3U$*K<@B~Rf@ zQi~|02_T!VYs2m8&1PoIN~6r~C~lLCBEIi*6BXZ|j63z`bB<4z&<ZXd>UXQc2dAqf z&eoDWx=szPuD{9L*S&+{m0qKfr6(+!7ArCbqODP@*DO*X6^uM2?;_Xb4#?|J3#+Hz zMoxOUl9ktr&>3AZ;OT-k>68O8Z9|3dEk}+*VQa4n6Tjsb9D2Nzu`sfSZ8CYx>C&w{ zc~^b%;jJRGr$(K8^_+!9wT3aNr;SPUvwCLeg&IqJvzS8P%jD?luV}QE23vSjKs2?d z!|SSIVo-6O9EkaVaQ_-4aiNRUPE96bzDv{3M(Rk(Z;SY**P$hsD*249wi7dSTN+J| zZ4i2PKVz@XmO`SUKTK8Pab`)C7nx8UMb6f}MIA51F|7OQ^zNO}B*bTkJ$XkB{%ooy zFCVTattve9)9WNvQYvG&ypV>|t^#pP@CT`E+z0DrlaPGl188yzB-wkNsYZ1J`{MQj zB6+5f#rydQ!i#HOp>AtcROso>YJYt~Bp&9mE#jT}od6kOmVzEr_s56HT-}V03L=>k zK?W9fJtLV5^EFYYsT_^itBt-eU1(|Pa`1X$EaV+(M7i1zg%+tH=#11e^z4En&tq(^ z#d7B#Ohi-<yUA!2TYvpYp$x0dq<I?vpN|rm$Dsn|Me$EYQg;InUsqziQ+6>2^$k$m z_q&X1>P)J0HxJ#2mLZ?tR}<5H+tIqm-x$f(3ikWb1QOS!No39HnC$uWWVrel(c2=A zoCMF&pN)@%Ute4DIu9?Q3D1q7eU1ZLX*C<|w!Lj3|6eHyxov}DK7Zu#H?=gg^R?EJ zB@e~UOMoWPTzeTs%&#QJ^c3Od)E!jxQk8d`B!Z#bX7s2<$nH;{g4QZ_qxE%9$xTHG zc4?pyI;m2QMs`J#r5X;j*vJGPov#pXs96f`3IX)<{y}Ey(`#Z)N*$%|JO&d#kD=vJ z*O{$F3TW%>OMG-F@d3$fI>=PDjz_P3B^dRWPf?h{7<g{8k}1^U^E#cQPzrw!yK=1* zO7?w*4!G__KP}|R;Nu4*uxh(lI8{S!(njdq_j_b;e`uk`)H)`|#1UEBZ7NKTJI#c6 zhOp_w)^MmdpfDH_)=23A`~1rv-ri&5=yWf>_)XtrhwP{1GkvA^*lQjUjPH`s=zFmS zb<~j-TE(>qALV*7@3flH)SO7Nyw(-X(HsvMOShqk>jue(`iCUA@e+BR{9QOWQx2Wh zpGwAES;^FgC^KolXCi~xZlanU$lEAC8};@23(xl|5;L!5(C}NH)Y@7gPyU@j)^PG1 zA}gawKR4)+N7oX;?r;{$w3|W)mzI$=T8cF6buQ{X?m_bBIMRq1KQi&BKe(SA2{V1- z!25#_Z+_7Nn!o2ba{g=2UKZ4`TZY2fEv+%IKXn<qbJ&Poo*yS%6K#rIrz}Le%O*0p zO6sV5UFyT8KhuEE&dK0C7ZfrA@6Bjw;11-G{YU&#O+!cKl%a^&b>P$Q#R&St(dKSx zrZ5)JdUO!Ae42|k=pSJ(pO}F3ju?w0hKfwv&pM&M;Ze57%?5n;US{9<JrXY1%tyke z`oir+LFn}@c{HdS&y23SAuOr=O;(t+GJB@$^2wG^CnEW#0qLB-N?Zq1iGS*NHaMyX zl|L~i<+*=YHEKz=yedVSwLPe9nIXwl5-~oxhIHrmRq(V<REP^kB7=-H<}){nSvcDY zg`8GqhH5DD_-i@q)7VN1o}MSq3f?0FUlnq`eJbcYTt$s1TA|rzG1#2iJ4A{+)Y0eU zI#jdjC8KJ$incfCK<0=%)N}eF3i&#Rwq|+KaQ9dkVdF!(oXtVwk~qyfC5ZW%n@t*@ zEoYS9>7fiY3EFm67X93-z(aX4|Eui2VnPJ}&!^TB<b<dlj=#78{UNKVWRQS}H2PSJ zSqvD8*QzxV;pj$uIw_xM0mWw0<j<`LC_i1#9$#U~y0yeJS6+=J&TCe&Z5w8=fvx{B zT0bYVm!DZNWaSgq%*cu*vt`-g_PGCDHqS5L_y79TQv|l>P52R8WRRe^04@JH1KqaQ zgzB6=GH4(}6Hay#>9QNhRrwOpUp<8>UMEE--?+;JZW&^~?LQ^VS$GCdT^VJ0CL|O; z7QN?28ONeh2~4NE7+_P1xtE9E!IP(Xpj&5v8*S7<);fdiQ;a4tnQ3gt1t+RGJO+Qa zn8v3y8gF=K_q#IgTOZJZ=8^R6nLawyna4Y&QqSuB-9zi9Z3B<fvJg|V05-V#!{)<Z z$q;>ja$@=zv#+l7fr0>rKODtdLkrl939InZeog#-gBkbSTZ_h%6L^kjHof%l2oC<K zZ)yMJGIwpV5RSOJAiMk_dR~qHnO0wa0a+_MAW7g(rq>O@^dB!E`D_BSqU<(Y->AyX z_rdUHQkNJ2I0Hg|hQZpkL*&?i8C!1<Mr_O@*^ibPsO8clqSN3loV-qeib`i;iGi*7 zy|z5pW*{lL=U>OA-W(w^)N#Z&zk1`x(aXtw)!V!|eP59YKkXe2vv4Rpc6Br!(WVbw z5ysqr*JGr$m9R3ii<up(chSQxwd8^Kc236F2;9u-h&)fcE*+EsM>{PjZ-^na6CMbU z?3hRb%}Pjc7C}zUrZhNcfc$yyMby2Q(PiaQn0{-rm@k(BMyrmA45xtQveuheKQkNn zRR$kPz%Oks!*DU~SR;?m@6_PNak|{0rU`iU!PmUBqXJrHZVa8;Kgr#D1vFppEeX1H zpQeb`bBPVvP<TBH6bv13=!70*8&A<Yn|(OoM1XMPr0=Ns#TlaL--T>EKhgYaxu|vF zDWbN}kUR9BHrJD!N$R&-@ew0;l-hPp7Oh>Cfb(Qt(Ddk0T!gsKeB$(I+<usH&o}L5 zlO?ynhoO<!K&4Q)ts#Y+tX<9No$aT;E{)(`yJIdSXF1z3vVpwbC{0{vw*#$yL#9~j zLr>EHVx)7)($06JbXq>^yibVm3<d^w8!*<X^=Qp?ei)e-?S{ryOvjmvvpK<r1-uWP zU+F911$+X}r;Yuwc=60#oa3C`Xxyz?w8PMhmb_KB6g_^4JB}}gC7%b-y>sSpA=#d2 zWXnKCUoGj23nP-rAJ8v_ax9^L4Fb>XVGUpCa@UQ<ks-W_d9cEi+nK!<)`~-34F8A) zNZR+nn@B58Fx#DI-UD=I%3<#J$Vf`&nQ&SwWkI#;Hs@h`oSqlB;SV#!vf-C$+-V<K zY`WA7cBV&B$=^vhei4A5nCIA1dxp+S_Qm08Ds=1B7NR=(8Z20BOxp9tiIrkWvV&8h zfqnnMp<#a-;O&d}hZEJ1f8J_x_D3P3cRQRp;8l)xS-xfsO@&CQ=$&xFu}m_oI38L} z<=71O>2&6y<J^x)BSpqwWGQ{@4L5Mc3fhw{!d`QK=-(^L{kpz`oM<rv<u-fvX_yv! zHAWRye~*L%3F{%{u%<BJrvz?Z`UR~xXaWjLfsg(5B{@UqD{SkWdf3g4X1^<1(5>lx zpf}c-f~P4miHa@^i3>%a)R!Um7C9JD(twEKZS2+cU8HDR7CloG3$-6-i6lp)aUBt} z@$F$be5N*!=Iu^K-WM=vvg^5bbA+7rs#$PRUY_o2dCJ;C2i+u@O?%w<ekifNo+iv3 zp!EuqAZPVWIG`AhwT|6m7lI;INRMP)4&5W)XWfS3xsqt7ObVLYmWr-zvO*0<-_ZDb z#&mnEB>i}yf`;jg!a8zE+%v5#q*mep6+85C)mu02_k#2I%ez4C<zxZOb}*z-+80UO zHW3ya&&0O;LU&}k<TQ1WPZ47>-!M|W`=DI%JdLsQK=&N8;Bu7+vm+|u#LS(fT;?xa z8hcV4C(9=KJAz@w$A^r^pElOHbPs!Vf+@S&S&<Ix$wj|!&4s;R{-U=M+o^m=BC7tp z5|(h&X~n}r&~|!8k2{G`SbGa0{xOUB(aO5WBiIW^8B1WL;s~hv)I-~}X0oEwC7{xo zjK8TIr`A`caDs&~*4)$yuda^bbOT@0t~)K%)pIg;DSQ<0=lwwvi|x3$7%^ca=?m)b zE@t+>TEG<3a9ExxNq>BQPeRj=LS*3>Zrp)YT>i><qIvX-<(r2bpG#>@=MD$nr5i>^ za7)Io<s3Bb(A!^=xjmegrB(eJPIL1Xy4wC5WPX~D+bboxkKxX6xTS_@QWbEQ;nVKa zsSpu<jxK~zWbBn1s8zD!b!@vtwA{WyqViR8V*f-KWQIv`|8J5mKLfTsZAO_7B!tSx zCV@7~*TFItW3fZ4I+tiPz-0y+T9!FUh}!bPh_USu4355tbrrU84rYzuEAxu`d-Rkz z)ssv6J{3?qGjBNXMu^0$7j(9A3>O*u7y2D1<5QJ7G+~bdPWh|EEzFRDoa<w0^pSQl zqS75gZ4dJ<#z>2U&?1z4(U!LJ2OiMa{MmG!OCI`hqmRr|%Oln)KWNYuO?o-tCs#fz z1c&ZS<9aF_xc5m`T+OA$Tz`=|)?1|mSJq@;wYde5?BalbD^;P7t{T+N=@d<$A|bA< zj;1+!&uNKg8=ZMkhU<4U!{y)Z02<{7RYl?ClW02gDIgm0ckA{LjSI>|!`F}=dH0iv zicto;l7}d#`aRLNa3nUBA?Su#H>~%Yg|ib*bFapZ=Hx;@agMtQ`4lTp7dr*uUYYx3 z^~C5xW0~XJ$purWEblu>;wBLHLknQ#g-JBy!+0^z`5DcBAqCF4CE#~6g<PyJLl^!^ z^ND@RZhFWkf?Zh1P}?a>Xj51&GRs|o)@i>%v)%_GXI+Bgw~nWa8lN(xdn7p@Sp^C{ z`PhE$HEJ3>L*#ZT6)$|Niub;i!~ZQljeXlq;HAb$?#f{y4Y)U1B;ydviRvz~lQO5n zM!^p1@pL5$_)mo1qv^PHIsXO-bBf7s`)uy%`>AA4`6#%Ywv3(VI0}mP%h7RLG(nQ5 zh7?09Nx7FAc>dmxx)x`V?8Rzu=GqHFJ2#V8!7bF$aR;X{L5E8Bnu}U2wqWlu0&a#~ zI-0P4DW@C~Nq=V_fjJw3v7OvAd_^w<2`siEU!fb>$JfsXo8$guv4K0&S#F0-=lP?^ z!^eb^CXU8@-UOI0+zDBx2KfGzg)n?m7an&h;s3rBpuZzzXp?ms(>_I)SvX6Xd+gPK zs-A_B%~xK~ZG%?`_yyq)K~_-un&lMHB+hQU1Sg>xL)XvSfn9Sw;eLS+y|sQ4<)7R3 znVWMXm8-Llq^FAR)1>87z;*fo2t1a?XlyqmAFd~Z`@4RW7%R4t${WzB5*g-Tv<iMZ z<rK<3a2o}$o`(xF!ss8R8IYm3QOpJ1NmYh6(F-664>Z4_Adei{WNeD(*{tOH10JDo zQcEo3m#>G{v&LY)!*xEq@$aP@H?JjKbG*n$HVdEk(6IEl;)m^?NZ`xnQBZlL431np z0mCPAVAwMnr{4$ybFXYP?aX<+)TI_yWwnAZ#)mrRTqQQT3NW;P5*@Sc8QL~_GMo+b zAcEYdOjF`(l+f(O42~Q{x7cE6GtMWxHPT#`C;uUem9gW})9t|JQwn#7R8cdPRFd(# zlTO!31ecC7ZeGzV*1BpAew8;wFHin~mIM-LtuUa;uJRCSR7rhvqtUQm9C>v%oG#yH zO+5@k=&~7c$hFuS1ULfaY?UW^J(;MZAp!M`Pa|)I4dixrFWI&@l-To2n|KKlndHxf zXqeqLj%e)agGF4hXx7xZq6gJaxp&79jaggCW<9!37t<iF<hGbfJVl=S)hYgKI_6XU z&6#wj=WMF}#vUD5u1w@T;_2JQ-oly`6<U912bG;~%qWjbL{7=^P_-qGv^54JgQpVU zDtjME-e^bs)TJ3{ZBYlAGa&)CIC-Gv9CaFR-G`RW$;XZT2_Td80tPbGxa#IIc*yx% zj%)71QDUISh+PZt^9wxOd!+z>q~>^Es}X*(vW%LR)H4d(S|LPCzT-Y|B<Iryq%T(n zAH=yj->yNZ&5I}M2ewdCeIIaE$m6s9ZKlL{6-C2_-NXscq``B~p}p$qME$TH6R&d! zc|LkZuCCuoDp&bnNhwKgoKZYYiagHsb!y_Bb+>5VuU5>2+jDN5D<^rpfPDM10q%4T zLyThzpfD|b|9~uF*Z?Zhn@9URp7AcEKZkYCduX|QKF!?1zYEW<*}%)3ILPB0<HyaJ z#A|gD2|K8U_joO$@4Ls*gRzT9Kt}+*7!yI)Gwo>Mk63ikTZQ|&4{@s$&%x}eEin1! z7F_Oe15Vb90SreYkU`iZxRo*&!|zLMuClx+{NqF}z&V!AK6j7%8+(xpRUFN|s|*8< zzv4NWxaTh$TsgWB63uAC{Xo`zQkIw=<iNGe)W;2DJ<tQ&7<5_z<Me<p!q#zO5%knJ zni3g{c5ce0Cx=g>?iNL4e5IQDL^Q)gr(DR{nFCpt4x+iMYjE{|8#wQL1vWed>O;bC ztj-P;%~Qflf;BmNoJaYQ76>ob^MgNLe!TMWKk4NgvYc>a5bG|<<8BHsk!RMAY1l~x z+>|i^`)>#!e-l(mt#l))icBHaM@`7f^y$0|jV0u-V=X$Www#U8m!!|Lqu51C&BVZZ zG8g=-2d%V8B|1Msx!{hMSW(*sk9qT+eY<!(b%|Tc=R(W->F$kFxDRQn-1ZZp^tiY} z^5H=@+IrZN)~q^9YW(*=y;M3C{6I9?Jc25Ae5B9wR>F&4a!~v96_I}X2pyVyo7K5r zMSiK=BujSeMmNLrQO7K2VYu%%bU!Aj&~0EcNbB7}a$k0Xeg=kI!$5qgyiPPek{<)t zr;W69{yrJ@ggfBgiFw%Q(|M|XT^Hs=U*yb%^Kr12E^>dVME?j2>9v>{RCOSgSft9+ z@jLj$bwdfP3mSusEJd`o@g|M=7rwSKA&<)+F_*9Xf|-f;D9>#?(iRS&?5%N>S+D`W zY+A`W+ucNGZXsO49~*}f)z)#ZUBub<>K5*gSA-bQ=SPtzkUQ$8+$Y;s+7@F?Hr_8_ z8}$h8E!P0<z6mrXY2Yv&V=VonlKixt1g1vkaK$ATI%24j&Avup6gmpB5vEk4Hx(8+ zN0Uv=d34-v20SlLLRUA$qdQ-Zu}m>g{<mITeD}dS;t=VJm|<-k+7iX>$vg-C%F+;K znZlK(ex*S<V=cdUdvHfj`jS~ASJN9)P4L`uF|wc%!B9pI%-M5=j-U?UyhQ*j`d4y& zXU(}C&!y>!^%rS{pr0s<MsOleADW(~$<=f`BQiO~yrYMJ23qU#=`Dp7%pIvg<TKNZ zmbn>|F%leU{iFzC=bxe3w+D!8@o$J$XSo;&1kHEOilR1ZbBR|fXz3xq1ClzTGfzYC z(a+92tK|=ogSI*MJwF$G9%<ko;?}M5O*=f{Lm+b`x{J|&QpU!|9_QKO4!C_G5?aFj zslAR;)Bp1O#TUF|{yzgY^s))xx<;DpE|r3sB~J>kRj_EI8B3BTM6pq|J+NTfQ<N^w zR)uC^q%g^isnL=K99~UoBz5S0|8?m2s4;Nk+F$Z!#a(jvb01N2+(b^)D}xoQ&Nyc6 zC2=Z|bb9hNB;Wmpc+Ap9t}kt=gVw4-d#SDeYYP9^R3N~KDboM&(1MOcbLoE&XMuK- zx%5B6oM26|y5K~TuJk|dn;<@Eq4d9^v!G5qh%3fQ0@dV&(*Jr8;^P)d|7#`)!jkVw z|BLAhmR>QJ{uja*_=<;ruCss@4`RQ!Uhq>q{8Oa_CRfd+|LH`6W8&f8gAwFhHJAE# JOI=q>{|_L$aUB2v delta 25988 zcmYhic{mpD|Nd`{>||fdzLX`h+%wlT+7KnJ+9gVR(Mw9|CKTC|eJzru5>eduT+h)e zYbEWew2*dEsg$qx=a1j<`(yr_<Cx``xt{0wIM1v4nppKUu?tOV{J69%Jn@7Zl-5SV ze#fgg^hO6JpWcwk_jRGUPk|BlS%*D5&O^_$qa^V`0rq;#k~gIj@wV7ra?R!m5?J-K z@int>iS_}qv1vW-F4AQTrge~Oy<3T5Cn3(CA0VxRxp;TNV?rG_h9q7Iw!I(*Zh8vr z=dlZXFxi`h>%BIS>^;`7M{*_>BzI!bI$iL(_l3-Sei|1&lw}6jN5l4gWzbkSfhkPP z!pYx*p<opccoExhnL;Kj4hbkQv=J?hDnJj+jzN&KHmFPu1>GJ;d`WQ$uJPOprN1Ad zN1yhho12~J&$rIuS5ku@`it)mHp{f&SpI&H(b`YFY*XjnTNDasaxU0<_jJx|7s0A0 z&tZd|3>2nPupwFnpO5*3zdk+!XEIaB>QEJ?fAJpNd*}<f6mXF&J|BiZG#Y_{s}=G2 zF^E#1rLh}k<ie9}am4i1W_Z+Ahc0Ke;_VJ<u;%J(;>~}QMEXqbu+33t@Qg|4AZ%MO z1YV_a$gyHrdG<T<u}X(OTcmM9yAdps@Bo!BZg|G#aB}bVC3w=D3=e%Gfmbie{aCBS zwRdQ6`IBG6rn?V_WwRmFW!#36WG~Rfb;7K;ON3YUkC^Q}gCbcS-1+A!iVRwf_!Xiz zA--ik>}|P;A>j}D=Ln>B%`4i>cn6%)q;a2NEE=w5aLS(fSfR?BZTU)Yt(qmCxArvZ zpIVE*G}yw(>WjcccSG{>cDUKEN*uF0L2QmFC-ZGOXS_p+QyEZ%DxY4sQFscbg(`uE z%Me=7NXX7}seD>;RTO#gcQ(%SHb-LvyYbJaN{Dl*hru~=cxBZ+wA^6<7;<aKwiY?m zX0Cx3WJi*VCPl2iLJu-b*TQdCN-)~a576#2inz#h2HxVL233b+@kq;a%+)`}4kNSh z!;ebLbt`q|?N~eBsWymB>w@v7j~DT`o%|X2UC42KT-%ThvTP$;{XB8OdUKqydN1j9 ze+{A63Eci#js4#X@Dj6Eu;kr5$V$9J*cD}@dT<r^j9!IMmot$3p$M)WkH&gEyODJ0 zF`-_XJmB9H^t(~;tt|#chZoW{#eE>KodXZAW#C8r6zprN$~p7LrgNS?+W7DJrO5L7 zLTC(s3g)+?urSGqrj}I*i~23#>&0M_xoeahT$v|KU-SVj6<ZDWmo@>@br83}Dr~q) z6_>={q~mvsqrbDCk@mwBo_K6K788jC<UEnc+)V_uG6JW#?}l{r2YnQofbLDp18r|n z!goJB1&hu|hvXVj?)hH>9J&JGd3)>NTG1@n+x`WeKD-=T{#OVJ;mKq{hYl9~m`%>v zszOe=KaTxXjRpsG@jZ=+ARm7kOlV6inY0(pT5*ObhuISOB29QFC5K;!Pr)`t?d-nD zRGiRcOq}e6_^0GbFkQm%N$>V^cumoISW}XMikHX2K;t{OzF8fwJ9HJDTy4U!dg7e( zi!{u$Ov0K5dqA-!6wKc?qo-bm#HscbF^L`~ys0zb*YB4kbXP4UJEI2GDmg>L^%t<D z&mLFzPQ_YvDx{+~1y^=nz#Ed1X`jQXc(~#=){^DtfR#cmk<ChjXJN`%Fi8<L#HykF zGb8a8WoMWpNP>42o$&Wu1T4(^1vgu7!)8})?nRF-H~8ro6lQfnhyQt4vDqFjZckxX z{m{YduPh}E1qK8;#iLwPc^rB48g^fD2PEMU)TTAzrO5%fbkK&-MqT70zqOhKK5xb{ zm!@K6sW$9zT@6;X=kX@~H-i3Kn~1;vRL40d$MAJt3B+s*!OvIU=Ec4nA>nIZ;4`_M zc-FBAY|xoh=$6xAR5m}yGSyySYm8y?cM5;lCkOY_-Vn{e4wst_dBknN4z84j(VsUo zxkOwMV2clRm>;xAABJC_C#OtA$g=twxM=Jy`hBz%-abFi4$nV@gDQLATgEeD9P<<& zJX?>I>@?XH?;0pn%*UrkD&dx*HTHHxn3dcJ=7vdRUakc5@AW&ZymtZwmXttVXDoic zb|KEpmxYWjF?!qn<&<rT7QSzihrP5l`H*5$0eegKl8Ga(_-jukZe42t-{}ZAm?VWB zo^gioqq3Z_Hj5NbZ2|h50T(}V8E+Vip{qPJP@+XV1m4BM)5lYUFDJ&rNQWj&?rO(Z zTsyFr?rHe0ir~_0d4@M#o0-?mfbZkUV7Tf!-q4bS6Jp~?Nk4x!+Vd=$a#I(_wZF4) zWu_jUX{H4IKVs18WmoVVlP9>)brF#Xc7P++t?0VxLTCwXgoTUy(Gx`_`0jHR+-Jn$ zD~dHJ@{%NN_Du!0g*;>Jr+T6A&%WgB`y-Hg@*RD~eF9E44}i>}F<9y#OMRDB$6+UR zISu~hxezWFiKUCPux8E>xOIF*(wpMwH^vW0(d7v^G1UXFZ~I9Gm0Xdw${xs_{0s(V zOL2Os306AXjx^$DlDEoDWUToRo@knZ3JM>RZkagzT~!%!Ef3>{xC_MFHU%w}K83$G z<pB;niwD1H5~r7jq`7t%=F=wq_}C&nF!9odrj2sUyjj=qkKgBrZ|*TL_PT^SqC!cy z=L*zhu7WxvkFyOYF&Hd<jiNNQfVY*0pEoCChoO8zDaL}AR5saLmyRyK&|$xIR)e2; z5(y-MtXV`FiP*4#sI3|w;}=e$l}ZBQaIyyGuFK|Q&7Lh_T(}HpzCVq(&2<7(x*cQ| zd`8W;r{kmuaqdUu4$x1sz~pfhe$I6P4v0cJlM2w)RqEumtt+Y`#(2ZK6ttzalsvn5 zlLX)0g>teJkxP{Z`y;p!)je_{7q3=AaqCvhPpd?D@C>=1@*kG%u>gKKKMKovo&gUX zNqBI6C(ep!gy8>Hz*qSOh<f-6k~D+Zf`5|SyA_I@BVUTknEM#su2}_fg2!N~F$f3# z=)mv1!=&bK0ljdA6rtJ4P~*G--?#&~XI?UrihK`~le6HN;sac2x(|m>mIs^iO=QgN zHhwzRg^u#ot#E4eWz27$Ok6e<qLzv%-2J=^=hsMMDa+&NT3!|spHKqRlWlRV(FJV% zW;ZO!-wU7Si!fW=Rhi6HF|d5wbqLs6j6Ki|9BlLm+CPW0l~JCQSF$&*_m;x+IzQI3 z$^lLuT@E|yDf}nI6T2N80ONJO!2g485$S+Y67oA0RxQ~C`;6-WStxL1ViZnDP{aqe zIO48ub-3$4{;?5xt|W7b;LMRlSbq2}o-(5pXZ)Lm58set%F0@Cb+s~6dS8qo=OeMg z=4jkJr4XM^4Z_2wQ=!<x1ngBX%H7zAByzWt+7*fLOSO3k%++0sAFV%)AE(@a>2nm} zV%seeRR4-RUa<@uf;_<H@?vmd!r;;tHym|*CH*YYn%pkef)BJPC?2<iSfgje$4g53 zw|TP71sM^M3o_|`YWnH<lZ{s%*^$DXS(D0D`<24)8HvoUxC>0tr&O+@w~o;~vWzMB zIK$YFmNN_+&HXGZ<JvS>&UEu~=FO4mT;I~=%%!MmM(V<U+>_EM?thn%^tx6!UqnP? zetPgWy>!PZp8O|aQPk-jj?~EPI_DCD=Z(uk>IG$Yv#1W0Bf|Bol?7idw1iJA`vo6g z-=a*PH8tF=ogxhXV@s{j%A?juYYV>xFW@=#+z>7sD5EmXb%Z&84)Wq%{<`?5?sVZs z{RDn@^o6x);=<nF%c)g6t%dobT`n<lm6VLrKHh@WvV3})stQ|Ets+<<xq(s()}&YY zB~l4S_jw7&jtWh)XH(U`=g|uq94SB1NMU)WF%=gfMt^ItVi$R<3IF?%A^ae3PkCR_ zrz~HkQlW#Q!ktX9FxhvAH|=||@PXevfncc*Ro5oNveSRM?97---SLW{76lAZ)oSkr zk7)iO>iFJpI_CE>;l6EkyugE3siuz0jZe#BsF=KIJmt~_)XuL8bmu>R>T&u_-XOUu zDE-q<HK+gNb!LlG22=mK>^o;FjQ_Gvc<fI;Z+2|F&~D8WiZ5Y7<*JHMFJ<>p?yDB@ zKEO<N((Ngnjh`s?G{M5B3KHYfw-g)keQ)BXgsDhlSu|&H^fozkRug@JIuNeB3YrPi zn1OC<vij9Nc<}x?R%UupX!8azOT7>4TQhO`O9}4c-V)S4L6NHoI7r5Y9NHq%jvU0x z;Z1ZYj@TrDi*7zfK6xv+L&g#uc{PIzuasb-Ehceq*N8D+0{0TWtm`7?sQVn;;1B4# zXCwUiJcO;2Yw(J@M?ra(6E|df4Huaxal71=7!Us*sOeF{Y{ofun??qm9;C+U1uDY3 zfr(6q^;{+;bQULXtIv^nmx;OJPVU3gvsmomGG?*f3TBvk1UGpPQT#1kI%2g4zO-=} zbFN{O57VE@aZ!I}fI@u--kq3-K3_h|SoJC5xc7>j>XAa+a@K%5qI8}s-Izhv-iYG1 zJ+Ffa8k6x1^>ujkTPw6#w%}=2GT@^zi{lUP=9ZX+G5OhYoYrJDra&#7+ZT|7`*=$j zoAbXh`PWHSu26xU-SZ&Hz7Q?m#eWL3yIdeP+nl+zp%*LPE`n^`4(v2Tidp&2lB-R4 z1nY{w;QcBKp>=Z;T`#oce4qX#+4&sF*hS+PFYB=B-ZH#?(KKAuWCP}=!JOopbcmXg zik*^9fLhaG<eF1}<KFt?>hIHW@7jHMd3+VVb4{L0T~oze;Y*4zS<`Z0xrsKnv?7rC zljn(!?>dQ`p1dbZZmefYGM$-Mm+K*MeIo1)dI5_)|APgn9TN*#Jhndy8b&l=t!XOf zS$3G2e#@FmnDrkQ;C2&*rq+U~=|=p`b$onNV|Yo<05;u8aP`z9%zvJ3WXNa+?}nQw z$A9x8o;#K9#^k(=WQLXPq1DbAN+SyJsbC3iv@o7osoe%AwXB#odtSn7N|}+kt3|g~ zOyg|Y%Ng~1t>AmoPAKCoN~P?aN;TD=qW-oP@>XbU7anvx$SaxMEx08aC_r!iHE#bk zB3zMNBv{#Wglg)PqtL+%0{>r_Pd(UupZC2@q_Lp>tdQ@rgR(twUeL7cG%xnnlZHzw z?YyJ)hK<r@vb-;rtkCbSH+9liT(C6f70+${bK&)y?_JtnDF_$G>C(RG&!}qNm{8K> zJheV4gj)47m6thLg<5u#W=m`~@#Z>N(kDbqDV1yDtfR$0D%5{9MLzS(sQ^a}Hu8fD zH9f(?`EiuH%Wah^;e9(T-Wl)90*%hY)ask8@Unj=uP)G6VD_34THL%vO_}aaSDuxl zw(K?Hu~X;LUUk~6{y%B@5t<`(-h6~ondm~d$lekrZ_1<|xfM`$S1joQn;v1a*j}ph z^cMQkfoZ(!f7iGi;(KnP-DAsygP)eNIePN+Ot(WqP1$&=W|tdvrGE{rxW$n^D%Hme zv1VAciet2BVi^qj?1fmDe&TRpGV!SJXYKoPaYEL0G<}{I(jQq&dV4Ie(T;~`Ci9K# z*;5CNkN<&-Q#KhOQ&B<<!Ta6LprgM};%6o$h`+>G0V%$fVnqzfDI=FZ!d179(8t!O z;VuhJl-D{D&9R=w<tQ&jyM9>0ieI**La+if4nBf?J`3U37dvR$EQ3F7Tm`jNnmEf` zAC!N{fi+`A1*sI0qjFsk@JI@q{jCw!^z4P*QnwH-XM?6i3W-zA9#ko_l8;~2Xprk3 zfR?_vhNtLMkjwcK(6#d{m7#8gpZ6O;fYMvy7r79IhtA@(Jyy87eLh~A5)17`L+q-; z7r577nj2^}!4+Tj<A{f+$(5|NFzUGuD__#07SygIviq8F5uL`4Jd8wh1;wbNK?<jl zwDI-k25EEUPlMT7=G>waKk&04KlVe;8`5bvmy5V-Bpj~ELbE^5BR@__qSV^kY?59T zS+M^IE$Dwj&%9s5=Jc(@7vAh+y=E-O_kKB|3d2LR_FPr;sJITr4JweI{RZr2yLzfR zP7if|b0#7$bA@YzHqb>9D)b&#nw=|UNGSgPyR42}71jGoOqidXMMr4+5QmLBg_Cz^ zAn(UKWZT(8XC+DFgByZ~x%5u5TJV$%3~$6GseDpsF9(TMK}7wY39-~TK+W|IBTH2` zkb*5m)FRFKXk@Z6(I4yOogVzb;`3v?HIy33=5HcWL38L9e_cGbrV#O`oYSFdCte|c zhU<{iG&OR&sgV3=c!{d-9U#YC<xqn01$4#`P{(myyhOv8Te@Hv%@R9?iO3^#S~vrF zeOXE;Zsp@l^B+juFdr@K_9F(LhRNc;H`tsLhmeV5gAl#VMvJyD!O;c}Sp$b)=ytJy z+)vxteTSU+xXAIEa7o!u+Hh!CSRF)@lcsBFy;3Kl-z|>5HLn$xFmkN!*D;jQSHoJY zIE6|+l?ppLRnSt!Kzh=P8)!rzi?X&UqafL4dPwCgE1X?S{5O=-Z!*d#&uiZ7tB(ip zE|XYRG^&C`dru-yPi>*q%JkT<+tz4zHUBt~)ruuoSE@r@Cc*_F^XUi9Pf=LICgJF} zr|hIdGVI3K#ezUH31l*7771>g%^F2n($%3lL@Xwms#qUNT<WV>tz=&?G?`13a_6B% z@qTQZ|0+`CYza3Eg=~p<68WsBM_x(3W&=WFk>P}OWYIBMyrBOgI?QkG!__Cxfp1SB zHfb=1t&8@tD90LwD-IIJkuUUt;R>`_`5mf$9?0IPcuIe|r-F6=*pSng74Rv|gXn1D zE?i#r9QpLCL*&Ol^iQo(biLmZQwQ7VZ@o)!*zDuTH_j9F<#mzK!8d|w3m(9yk{K}a z*$}kq#1UV+F$>)h4a5^46(TX84&l?y(@?zOE<9gj1a(D>k_NE`_}rw*DVJ!l>p~hx zaRv{6zc&M|m|KA_{-=g6PSU~)Ti3ETA|{frb9bS(dxufxn@>pfQ53mpEJuecsd7IO z?<2cDnynX6CATL!p|>(i$;jcK<f<L;!TfG3ayj}AqrO74UZ<6aUK5~=F#>YE-x3Vg zIU{G20wjOPhJEIIj(ihqM3d|1Vcr@eaDU*%iZ3`qu9f~Ek3JgVoxLle@EAdcI(HD6 zVnl}BzC(UO6uW5j0vw&<!>;02z|!7%@cm^0_T6z9&%CsW%<~Tv;j9<UB2LpRap`0~ zFp%7h_FG>ge}_#umm89>g9O5hUF%_%X9{cSy$L=)3BX0`BFGnGZyYY|iCbG2gY<H1 zCUGGT_v|o*X?bJFXMHLDpVmH9R4H(k6%lcj9hdXE>F=j5-?4IrDK+r=9Hn{-vF|mU zKssL(i-QFeEB`_1v$eoN<`te$`b4npU9jN#_M5`BwNKDakwlX5_BT7s3ehV$j%*FQ zi$Z>WpyS?2qgzwXuy<Q0&_8cPBY&~G|K}nOXtZrvKQ8gt|DVKfo#x0dKKGHVLjQ4@ zxtlq<Xf|p-dX8GKG6Q!WxQK7tyhFpQOd+W?ktz6H#aI`=AXVKDpdmVoJIoV8Zs%@h z!H7F#Far8us0(;boXC~655n8x8vJPUckXMKv#a9r2~5nF7pVSJ2E6SmXHw=p1hM28 zt~)h`ambs==i~-7VdV8cTz5Yi%P0;*x?vt!tlo!a%rVFAR&`|TyBjgm+yHfxonhJ| z2@-wgJj%AWW7PiHGtzNpsA$#$EO&>4atAXK@?b$zac~$ja(*Uq>@LP(%@eqt4M|w5 zc{#3}=fT|ieT0!wng&m0AF<B02ITP(ek3W_{Dr-8b{Sl8kR)cyx8qXXE_7=4VWx4a zD#O;lC-Rdvq7B#NxOE53IJsZv+4oZK+2<~4B<#pUI`LUMEAMcQ+-owzH5orK&#8l5 z<#dosP1Gbg=py;2)W_MDPiV?`8iRu_>2t+9bBMT^4Lx#(C2BziO>_7SWsKGPXXuz3 zho)yXfXnFs>{00feLMl2a*Kuo(tjXHbSpvg)foL3rfeYHjH_L9=-CTRvCa8&*fi1- zeM)Yj<C;=P_pvxOC+7o^T`)o$OnO3x%_%_7`xKz%moFR#D@OABIs7+R)V1xDk?Uba zV^@i*Rt!JPD~@TD7my+!Ic7m!72YXxhBIiI3zhXb_)g2Daj`lR-W31By30z1=|Nke z*0LL1&lS=BR#u=ETmYk@6plz7rCV3aKy-o~Bm|$Nt^SGP+8h;h?|2Uqv6O?j{dGvP zaueQ}{~MjII*&Xi<shDu3X`@~lg}iZ)2<O!3QeyqDK6l_LoAwznfX_&nP?qT?p4S` zaKC(tS(-4LxwRk%t_9o!)6R_y^VA0qDV;(W=O1Obsp~M4cMbPW*u&TlT!$gIT;h<o z03G&GU_1?;;zx&4*#&16U?P7vQsCvHeno3AAAF9qS`UM<1b+!I(|-`C3df&^thn5& z&p0n`ziXA>dFGp~9=Fx25!~9nx#w@>u&>NEZr<c7hL?YyIiCL+wwLe1219qzoHyh4 zU@#T#<TOA*i9d67%^}jXAWh&?70Z3(i!+A9gN=#PO^}GKFC!8&oiTzow3S*)_dYp8 zEAv;BkRmk&?7qX7_P_lfq{b|Oqs#Y^T1f{+ep4nFZjb>nXFZ$tSBNt={garPr{7?U z+d14N2N8}Mxy?3~RWrGdg23s6KW#tf7#=$m!O8VSqeFgT`1g4SqC4d|o_0}%>3lj! zq-4CAmv3Le`*v@b{XPVpINM10ukt(5=8+E4`lJjkK5-hg6e`h2&1R5~pF4$Trypf) zk|fwGxj>G@xN&wp>u^Mj0c>;4Z)(1<mkYhW1w7^YkW7sU6B*Kr#l-7Sh1V!J6c}<{ z8zw-;D<|T3<2f;~=s;U~zk<{28hCdj5zMg#5-~SLLmg-N$n}sX-M`ZZSNM#Qf1<C6 z!mJmN?HNdREYu}6lL#`ZdWHOqM(O<Tvruo24C?9nLgaq$C5vVr<M=CnBP;4IoUc=8 z%1b!U+%L3enhNr{+CBR@88<~nQ^kxqYJi|Y?KoGec^`M@CF27beb~q43|Bj05NZ<n zCNR<d4pxmh1}EJa2&=sU>I1jfU#XSQbkB~Bl8c7$=i5PbSr@%}c_LxgPba0`G+y~y zie#Lr5>DJPnf&12q{gzuph;sfQ*>n$$INqJ{_E0g@*6$E-BiBIq&ClEnlB&bwB9B% zYEpg-U6a7QkUr0xT$zgh@OLI+*BBSBy=nw4bqIpwdp59t)>$;Q`Yh}*eva0yr<uD7 zStQXe6<Msd!o%tm=Au5}NlETlda^p+pnR9?Jg-8}m{dn@d@m+FKVoP@I)>#%6_Rtx zx;W2cA`E4&VBU=yHVroYWaj@oz{Izx;B$Kuag69f%<mqq#1_BSF;))`VLo#f@~W3% z!xSZ~v+pyWRA!6Sxhyg@><*C<lVf!1)9{u77g*clMc!w}fSFb_QEnG7u~V{`shjSh zO^Z$N-EGgIU;h!TmD0mr%~E*v&I~SMW)wHJl)+R?3Ky{HGgsW6kH3A~h0jHW@UcEy z&V?wh;3%7i-18s<M#1|H42Mo=I%B`gH9Kb*H)Qsphi17@k|yce{+~Wq2tUx#9y8aZ zE1BH>?F=U|V?NV9HXgM4eFu|-Tf&H2V~{X<2jpsYv!|9ilbh`i5N*VR<@wt1)u9YE z#0YVBSsqsIj%J^Y@+IMHYyr1;+go7Ot(gLJ9nxKwFt@1}WZBP1?y`yjzIINRIqG!~ zHdUIrzB!L^&zm*)TaP*?d3d6$#*atfUKodm9t|?jtR<n{x(yf2bH_y;m&SF=T>LLb z9p-*cMKy2ZiTbWvNQHC2t>)R(oS6G)?fnm=g%6X!?~*dQ9iR+rF7HQcaux8S<4?h6 zLqBquY20*li-zk~!5DW`Pn498%e!5yIXF07M^|25W@_eKf*Bv=Au8=0M8qD2MZR;e zk%$e`*CN18qJ>y;>0G?m%M#yioP%$8Y=O+s6ta@D!MoR<fHt{m@><!D<@ZV6B2M}4 zaBBB-<mxMr+kBrO(+&B!QC=A*)+*xCG(8aWZNMf={h2+)f`d@?rh{9Hp*~_h+^$%~ zOcZ*+_wxq<Cbw{6lMZsjuIk*wb9=b^NuFe}ggz7g<sM#hM4tKlyabOJ%tHK*dbINU zH>}#z4f=&A_;_XO88lXD!^AC+WwyUi#72##aN<-Tc@$PnHvJNZ{#<>i+-{EJQdO}f zl#`O~-OQ&OZrq8t|G{?Gl}(?P2XcO|hqxO3IOg#8ZCuXg4iNwR4qn#{GkWvp;Z56< zm{Ny5jJ9qaGbMfic|3L^TIqkukYv*M-@z|LVKeICaDzEcs<C74=LUc{4yNsmQ#g}V z6`Vw9D_QY^2mTY)*{H!#@_y(in!8Yx_-3XuwIZt+m!4dt<MaldI;@4~2eeQNEmWxF z&7rs~Q-U@e*hg<~?nL+H<IucEV#stP7az#F4{>8%a8W)A*Wp2U%|A5{oYG{N;s3O` zCA#m(y1|=p%#vmzw?F4OES2SSEa%~`LnqOp3HIEN{mPvDYEk_Ctra>=$D)0It;ngv zyO7;iF?#ET44iv&2acVtkNamYC0Z}mA^)=psJTU^>Gy68SF7bwjEP$k8QW~d;ZL$q zzUVl1k$-^sL!-Yr_Voq$^kN(gHxHA-kTMv(znxR_ZU-mH&9LnB2N(#t4O1&dkU)GX zBYR8|MX0OcSDUB8izGi(`!S0yekOvKylzI`|CW(`Ge@aad&eD`LIM4FteS*gw<6y? z%)lydJ;@-OIq|YFT>ZG8`Q2mBXf#lKT4vY=X1<mOvk4TG)yHs;^=g>VU{SnE!InF) z%@35zI`N6qYFyO!B-}w?#>bo6aQB?W=zZZU93UTx<whnU%Xz0!<(+ao<Kj+i*lh-S zO#$eAryo(+<3jWr!-y^1B>{VKU~Y>#`SIr<I(;daS>hSM?AyyX<szg_nAdf3j8x13 znz~QUwg1@(X2Rz#2;XB22jX79(TgHYLcv!cC#JzdDOard%bQcL$7Gp`1f%jtgHzl7 z5jXVQC82p7jM^+F)7FT=<H}OBt+pM_Zi&RnQoZzSi+<EGNrmK`6(!t9J2=~Bh;MB( zLk@iV=_E+_mb9xOa5QYj8`6j1ylkH9MBj8!{qH1qwXqNp_j5S?6Q=4a=HY;N4N{fR zfoE@sA<6$UHJ^{5;GY`!xy=y>%-$|^w6?`}A5`KqTDHi1W&x@Xa>fsJ@1h#fM7H(3 z4W8BZ3aMlbpj~lNkUUop_z|UpsJucz5?b#dcjFgC{P$uKv;I1}@4_j(onmpdrZ`s> zuFv@^a@?o=JDDGH<2?kCHO#>VEAIIP0X}iJiQBw6lleQX4Yx1U=hlV1A|cKi=+D|V za>7E0b#f7-ly3(!oAcoha||b3mxF8jo}j)iO<2PI<`dapTTpy>HZQC$7wswyAb|p1 zyvO+=@5<c=tlF<XWUARtVqv6;Ck{*?0Sf-yl+-!Q?twH{*}`J3C}<XwwD<;!EpXz3 zcDdr)=Es<q2IIk6*$bvrq6atj9DvF(KhX31!216;fr~QOO2kHfl6`^h^oap2*tL+q zld<b_B`Z91xzs0)*iZzMJat?6t5ig;8T~=VXWLO~=xMYkvLC$<P{lHT+sK@`%ixrM z1urYdhB^9r0d}kOCQtw9bGkVnnCDG*IfDin*D_m*$rzF3^7v`cnVbgM&BwW)F$=Lq zjvmu_S`yA=M{vF;{vaQI;X>g?fdsOTQG^nkDkSANl}mB?1e2RK!;t4YvPedP7|Cx% zkKdJ}wc9F5um3VK&Fro7Vvqrqnl_@hXF5!MS|^lA%|mCdtik;I0_IojKa6*VGD@N4 z$o9E4cf&1%NRRQE)43IxZ4QB`0DWf0lwOV<F~<B$uMdHhjyZn)^)~mjUI$ME7pVAi zi@QB-D>kh)0EJni%ud(K=)vR=NaXEzS-&6=mUX=*RvUZBE0jo5?$5)zv1J%^-09-# zaPr#Jid!`PHWb#Sp$q%7u@mFS?Od&eZ-yV>5+4U~^;3>8BjM5bvAzoT=m*6|lD{eZ zyXY^94QOWahWwfKl_E?(odZFAfW95g#zUtpNs2EaMn?m|%P9pe$R}c>3rAU#rbOJ| zeTFRfG6x+SFF#D*^A0_GQcR@I_~1=1Cm~camxwRjB{XyWNLn7I5n1g#w06&7E^pBy zCNfr*sW1A>Y}vw>Y_iC?#C<B>!9_S~Fby|OW4nNTOrXLi?vGtHcR=kS{%dN%FhQEQ zW=R*i<1mT)bw~w5j)*fAdtRW#ZWqkot$}1qcX8h%q=DY5i9){w!8tdUUhDLlENjsL z-FMS5TicB0tIERGU@3ghatG+O)_~-}RN}&4mr14%EM!&%N25(48=8`&#W+f{n0XOC znUqmF%!bCroZ`GM_<q+kF6y;e(}rN03-nGWUAn`lYVQ+VGu4u5PP4*07heJmSu@i2 z)fs#K3FGt%W)ta|qWI3YD`?aEWG=A&C>L<~A;rHCN7f2jsh)L1sNuO7;eWa(MY5Ma zM59gp=wHT1`s;Kh=H1Lqj98ig?SCQW8fSV1CLc&;PK>U@14SQkq;n7sV1DC&_v{&` z;YjY2T^IN5d_3d1H5JyZ{06l-33!h5G-w_&<V=pHz^<xf`qjp7Fg5Wg9kw%qk-1`z z92{>`%QGLLcVfzX)^T()WKb=neBvrJVmXJz>=`8a_KD<vh6UNNVkb#IeUKB3Ucw9h zKEYQ{i(u>M21fqhY3}#cSnkF|0(Q~UVfV8{7(^}HwG1mf`tb-`kYY5BgB#Gfflkt$ zV9k|}w{awPi*j0tG^gn@2-muX$c+_R#Bq_*M))6&ap=(3eW@fO@>1#laEuJ&$d`V5 z1P{ra1h@5z$LCWOk)?Y{Xj&#a^dK8Gltx0<4ISurIY8!<Eifb}$~DB_Ksy_K!J~RD zim1FsOYa^*yBsY*A#{LzEOLOa)*Fy>=VKJ};{$#8>L#3CZi9I;Eoj98Nj$LP4)2ao z8H`n>|L=CTM!gb!nBK&=^34}chKH6q%=wVpB=ooxlb)=?xv%(-y;$OcwG&Q(;ieO4 zvWzlk_239r33r0k2d1O7-7ZM4Xf>Gh`Epf-qKH~~56cJF;%_F=9K1S54lJ30k~(uh zB2f>f%nZSUGo_gAS526CKX!n(#9x@*kp!*M#~{bbpU=e&=rHXS6gN3w2u}YvjPI|r zXZ+;Em<M;&#^-}Nqw;Dpm!_2uA<}no+cGIe#9xQe^O(XY{aFr!T2D#dwHqw|%2}A> zsKNy^mYg=vg*(#Tjjy&DF)e}l+`q^pM15=kmo+|x<bhjghy7CU=P6>D)e1~b2|pUE zP=?&tfe$!oSvN|XejWGy%!091d%5#$EKhdOo_m+w#ys)f&WupAxTT8~n3X+7FlS^c zvvqwK{v;XAEOK~;O0W^r9z2b+l@j7QmrIP$K%eoJ4uh$|g?NVHT4u^n13Y=PP^gh` zfO;?TL0}YNP3iSc7q;>fuJh*ou@MT2oL#0oJRrOrP%E$<OA_7^aS%oc0;pm40zuLF zb=3MWRe_SGs_<J_h2V<v6yd2x9ibH$#!ITzVl7$}ggT!uQ7dU7@Ao}bYSQ}OymK)! z!tY(q!V{%24K@W2D2d2I;fJjS!qLaJ!bZ)@Le@~3YS?pw;?L7Fqvf?dsr0V}RJmrc z&^CD{wdQ>ewKL?aV8bj8%FgaQZ@9gM8grW^{CEGk@PSbvwPsVHP`%{{trzf!3Y>9H z*m8FzWmHluTx69Y{AF!K8`(GWN++4npV#l@%{jAG@M@Mb#s6?dplMzwtar(6wA?p^ z_iXtTTB%2uPgh;BrM4Arq31Lzvx}Az;p&G(SUKSrFJy8M_2*(FHT$GD6&7X0hE2cM z=)U716*$g<5=av@o9GM9B)JHEE~W|B^X9O=c3$*yk(E?kVo2li>Quqker;jPKsJvz z;L9`jJ}ESfvJrNV1W=ACN2nl=Yn1ZvQ<r;j{BOMJ|0W7=)i0q28t(80DwYZEu21BN ztipnQ<~*0hbb(Mk_7G31_LCsL_c0}YdZysxy2rxhHpi&5)TrR(Xo+C+oTJozFL&Nb zEq`HvfdqA7tu6I_&jn%gfy;uULr*E2l+)DA>^{MlnUcaiHu=H?a1o|7h6~kNPPyFS zcY6w#A1iGvYv^vA!YmT5eiOjE7M|v^yHb}{@^+^FHrx}YeE20?-`C0W3u>n>jTt*% zZIPkg-dry@>9I?=KYxqRqc)e4crt-Hy=AX(rRG*)bn$V)tStwG&VI72d!!pZLu!t2 z)37-$u-!ssr!zuX_#yZnmLoL!FQ<{8`>>PRb)KW*MU?~tUKP{<HET9_(N>}LeluF^ zrX{uf!v^8&t4{2ebs<8sHlCWtcMztZbK|YGokK;1-4M>YWQ$eZ>aim4BAO!BMa*I_ zo3`>BGJej|0hh0Ug0(WMwQU*qI8%hq?-#?H(xh;J`C?|-brJ5qpEsXNUp4HqNBkld zwbp>GJ$}enMxE2LTMI8OFTsrtZLVG+4@LGFGsgEcnM0t$`7Id+wrvonc4pE=8|QNG z!cCaM>9II@f)B^@vBw^PQ<)iizv5S98>1MqlyS`G;k-!&@XyblN%=kj2`1mdSFY~D zCpT8GW~=zNB=4C9JeSDDQoPyRSc)#bqg#VaO{Q?KF6c7HzD0AL*HVFZd<S#+Z#O*J zI*Bv6>C5ax8@TA+Q7ji4%2C(-LB}qG3%Grp8U6Gg?ioh1Jd?%TSbs9N`1&!%dhK19 zVs{IkRb7Godp^QOuVK9K>{RYa*)zP2fBPRv&vM3RwiV#4bJ~pifHe1T`$5i8v5tLX zGzIo8I*7!#^NDh!EOSTjhQ25<N>9G|n@;Rfg5Q0SMC#laC7XYc-O+>a<2rHrU6v(k zHwZu-JN*PfXD?9aK00A_wJv&@^d`iU7YIAftYB|HD<A>a`e>s+#e`paxe^uIb+a<t zqUa|%$Aw#TGU;-&Z&bcn1{<y&M>(q7Bl4>T%|3#W!tW7YWziN=e0(nHjJd@2eoA45 z8JAfnN|nea5#*U>O&-Q?qm8vK*-z%G`0U!5&^!As4X0-!>m}wwvLg+-{nS9l8<ofz zUvd23LkB(-v=ovDy_%@A?gcw=Ne#{QC`SBKf7qorX0fL0jKDr>0wBjFM5)&ey2N~t zvF(WP;eajr)&7!xnf;h;a_l96XBVMc9<xYX<{YYjtQz8^Yw+(COGy6I0-_zNi6&Tm z7$3=&WXy3P{%V#*LT^RmyX8Vuti{KKlafPMGo#6Ed0m)te<sqnVn-Ct%W^I5<)|d- zC@pV&ht{*72TgI~Xk}U<Dqk`INtYbLdT&$deNT(n{Z^XX!5ddFl~_jR{hf<<$zfu| z&7q}SW<l^IQCK8qfhz|hQGVuKwpy!;oY{F0eK~LsAKqAiH2G$KS&Qs2S}ZcDv1yD$ zk`LBm^S2w&^Qdp|Ok@B_ZJB`DE*YRLIy+cbbq8E$G*>wI0;4YB9Wp=o4{CDwj!=Fw zk&cZ;)*Bv>#mohm_*4;+ZT_N2@o1s-%irXZt`cl|d<Bi?a46ZX3^A*uA>-*H;upJ( z@H-=<uwY^xZmKzk`-AnN_Ddf!h%ADG(a*_=kYqBHUXNaQm*9YqyY$$ma^n9>8Hc`Z z!*eG&us)CZ@Qo=)X_FqK60NmNRn0v7#6tp3%$1@qx1Yqhhtu(E1rgZdxrey+m=bl; zf<xz@BDUf=^!kVhu-<Zn4*}{rT=XVm>~n4d*;4j__8SLD`A7aCUTF#nK9!8lKb+21 z(Rn0D_A9xXVZqHPJdMBfO@W2ha=3SS9OV~r6UEEA!{>R?=#!{Fl8{OvJNA7dqMvB` zEVlp!ExwKecU?o5L$;z}KOPSM8pGaln1WyM_hUy}{x_kq_9B>V7J%=k_(HIS6!v!> zqOIJ;@PLvPQr3G*UT$!N_=F^^G4~jLCXqpMC%-_mwer#V2P%vp^$Ynmdz^|DIHI?= zgy`VFYUKG1Q2fN#C_8x*p15KqzO!kZIUAoqkBt4v-nKmK<C}~(X60h@P?j|E+h&4} z^&RXSQ%h842Mgu6t$5ek4t8JaRb(2ufyhjqiff;y!rv%Odi8F7P_3yW@5n~Z#l)5z z=zjqEv<{Km@f9{mR3S6dB=lLWl583;R7We9!ho+DQg`Bz(vn&DpF##}df^fsqqLto zzTXi|H1c3AwN4WLjlpen_`=P!>3usq?IWQd%(Wv6w^XA~Uvg;u`}3)xR3no6xsr;z z-bfo)T^6LC@?eK9FF?kV3wcoqwa_`e0qF&H3#G^2v)M%(g&Vh1<2pi|Jt(-%YDRAp z?EdnL9DLzL`d9R^W}FwHDsPd6&cSHeNCfd;cbQMEzAFlXhbeT~lmC5ah1hUkG|@UE zM$E7^`KY-M%{V?JETrX0;B^-L-a`bTFrC<4y+}&6+L5vSQIet<gw)iA=m+H&1w&q@ zq*wJAg>{lC!@pNh$Ko)QQTd$?Qm#h^VbjrXxo>2L{Z&*aDv#Q&=HTCvfuBj|IN^N! zLyPl1*NsABG5s&y5iaZKga6qX?2BOmx>aKe%cSSSW|!0G!uWdN(3j12tmsFZ8+^cT z=O*&s%N@jR<$XkJB+#OLhU~_n#{bjU9%k_q{(lMVR<Gqd{L(z&;-qOX;dB&^v^?DC z^;MjGUi(8(USUc}+1Cj3B4UKU79^v*G<*Cx=^6@G>J%teogis#7trL3w&+QkH5l${ zLob_8k?mO_tbDrXxQ%s0*A)ZDYp9R^FM+L9LHPemU@v;kchu)QsZ8MP9##>N7y8_T zQ+14ArGaZ}S|zuo;V=_?Z5nfPERHF?UCxBaCv(!p>$od-lDOla+?Zye54S_L8i!@P zWbQXxa%<}vIQL6mncV?8uCt1cGTlnn+?=w_+{>#|nC2k`X41cWPI{ja(^YeVbIp3p z+>8`qB$mtaIp0qi+;QLknA-VwI305jPJXTCc<^)|XQv4vL8ll(N}{;(c4>|*Z0Clg z?3p)Ob=)rPWZWS2iaTAooKxK|#wF`&yC!{7b)EmihEdV_2c<oFOy-{~Zo#rT=F+&4 z{h!Nfd0{&5&3O9!zr)8%TtY-dT3SRT{m`~qTKa!g$o02o+`7XV|JOwws?Z*j8$V5M zJg7EEAKB)?|C+Ob2!`d^hD3Sr+StV^r*|Sfc?<F^)f`Uzd`7CupN&tq0pi)Jjo%kf zBuNH#OctexFDfK(Yc6zf6*XQ^7GBP5f7*m!McR-BSJdIxzZAT-#tnb(&46DY{o$?3 ze6-g>5_b77gMI%DkYrl}HL3F&X%g?_6P=q%_<`yINa#I`CE^9dbmTLM#gXWds|`w+ zHwiR6D$$bcE2#AIVfNzgTJ(B6{oJ*`7_A?8#Qt@RLsok%x!J7+bb7uogy!vWl`x#< zDkl1pGb}m4b$W)7M71~&SIq_c!X^0ausw3l-+^`g3t`eVUtIj0-;Hhr9YlZD{3Its zjPMtc8scVf8a971z<NJMNW;I^bk^EoqIc#rQCcp7Q*VmEQxzUcTJs+DKCxt#x9UKR zzc!e!RRF&TUF?EI@yhvSAhhblPV45wr38hh4>Iq#)+4cC$zRP~k>~K%e;O=w9Rxis zKitY6O^1E1b0Gdm1nqHi6}z(G2y9Dy%Ff>qjjwOAB#x&;$g^?Hc5`DqIlEyp$-aFV zJWqAeCd18$(P&0WHB$KOB}teqvWk3Mrw*>S_oJ*A=Coq&LDG;iL3lF9pX`&qMkapU ziA|5W6Q$sX3}h;E%n4fvL{otO{9Qf@f7t~&$EENNaUBThx(`3nCSdbkG05~Qz*E1A z;F60G;QILsl3TPLvbJ|Z$araHW7P@D<iA;XG4TPhS(3D~tP*&Me<4vqWvuqbjJW=^ z!j(Q^c%zgQj@tZ~WI9fR?n+l$N>&X2{Lzl})k?`<ismy)9ow499YtLeAAACL)jYhu z&YC^y{S|LJt55E4_=jyO5^(xu8z3+3(2{-*P1W{+=GJU%YM9dCCKpTR+5V<W{1eb~ z_a?GEzW~Q{G(-J{6KH$I8DjSO9%||OOkS(jqHUJ@&=zTNtfpVg%8N+g^1(l(>MUQ5 zxNoZ_?Z+{^n%R%LPgbx+A1s<41Qu`$_kZTLKlF#Oa1%_nEXHd2Qe@sI3m83o4a=o3 zh4@TM@QTt$+Kx3Kx6T<Rt&+t)$xETBJ+{FubQz|7y<o%L+wAVOet@2?CXeRqCf`lE z$jo(rXr(2(_-<DR3gOpg5r|VJx_AB}m#+r6`od*$W<@S7r)x+g&t<W>pU=Vk92M8r z_^Vh=WDmz|T?9IB7DBT+1FfQc?1%9l>-KPQe7-T7tQNN>M$d$ZKWu=rtbJij_9)is zTTW)gbfHixgPaWOBc6^w$nS@B?7EL#ByIV47x`q^UNVX$aJ^(TsrvK;#nu<mPRbw9 zvfVe35z>KYqDCmY#tao+)nYDri82B118~HA$R(g-4A1?U0Zj)3nb8|_(Wc@VXed4( zUj2HBa_t-$dH?P3&L$F1A6rk%UcDuwuM|+2lL%g}F-l}BA~2GrP{+*YeBgE4<F;>~ z$mI@GVpg7wrXA@+mgb`5rd|rN?sq1xb+^gC53z{9Q<v8F7(_<T#KCpF31EmJNk=ls zZugbQw#SoQs(OV9w)=rq`|4qL#4M2F<>3Ca`E*~c5q2%N6z)zc$LkZnqOfWO+!~~h zu3dIVw$J!M@T{bfl<UV}%g_+w(VL2=T%QRG<o(Hew-K5NZzL-%Cc^isLNeXu3^^SB zn55OK!uz*(>36G+lRwOHy7y-!u|2UE)mePT8#}Au)eLd2GyWbIFwxAl+bbQkgMQ-U z?(yXNMHX%@kz;DB^_krteDNDD3D4)N^kNC|EI9A<9NT>f!jU`Eg^EiP@uR8BakTwx zFy5Jtf0>Uzfx$!!@r%)D%@~oHRfSId_X}wReqqCE!x2?0%d)Gq>6uq1KwgUq=!{;Y z<!|xH$6bEoOeuw9XSy+XpJCI<;0si`LIAh<+iK?HjT|t}Ove0UljS+raCs<#NGRGY zj{9pp(R<tJc<JsktgbyDyV`zYgS8`Z``!w?q(69k*QWzkHBQ5a3MFxGuPKBq|IPZz z9V7LJFB5;cEVA_M00|}^NTy*53L1J!^!MIC_aDup=Y9}}Z*x3wUq?KIkM)pqo3X2( z%{@NXbW?-5UsVA=|6U`3PwGkfx;COZ-3~Wgk03qrEAi;+*W|yc3)mY6Y>>kh4P5Ie zipo{%kU1NP`R!APbEyH=-?11TcO;PG`Jc$Wf|E!y`NcTEZXsq{E{&(JX2{EbgxX$o zmA-!XD{bhlhFZH$pygG^$WSZaj_I4N&E%`fz@k9`WAphox3WU2>B{)t<5Nz4*m{o< zF>Ug}e~Xk*!+4hXecCzVQdWXK*Dr+pwGYYI%SphAHKF^%<>a`p_qfcdMSX7P=wB*H zsA@EtjD2aL?Y7K;0Q*^B(FMq4(IRq8F&|OlJ@n{LeKO3qGZ095jj_>eG?D6;B6+zg zI6V0~o?D^I_;z@>j{Q<|9fus_Jn$mEJAV!R)K!uBH%mam^egcEym-)vsv~)QrD*o? zH*}}GKB}#Jk1lmeLPmTGEo;Z23%zzkMnoLf9Q}%Cj{DB;m!4$Xcz}7ycQ5(q*~Rj= z7fM6bQw!P&Eh58~%8;{swlHYLV`L`vhCF&V-bwCKVg@(7W~@@vxECg;IOAJMOvBb} zE@$8})?eq&`2FX|y*X5aD)NqCSg6f7j&CKdK5z_29$Q8$yjj4NY4+l;M*T=+ej}Wa ziv{n2a$MWz!&SIYSasu6K4h6^l7yrJGJUB6lxoU@8~p~YN*qDIKa8Tmk8LcQbrFRb zYGIMp%AkAs9=T}x8Lk|<1%|Q;Tu^y_llDS+&J^`>dt-jmN5jVVuvTw_=4gMe;D-iR zbybyXb9#rggB;+#O*&5AC=ZwRSU_m~X>5=*i`G@=TSDU6tHKj2pOI*Nin!h$WCygr zqt{thFnzB$d0?|wXfN@XtpBea(SkQfaPlWP*mDVqSKT9-KW4#dkB>Cjuj!f<_86Qe z-6a?7e={eHCU8HRlHkPFP0Y*{k<e;8mCK%zg86M<@N&CA@LsIU_?gYYI;{>&E5Ei9 z(tN!c$6`}fVVVe2wZa5Sl0@<T8{%Bh$#MLhoQLm8`5=}#N0#|Sk-S^?$qkPrv_i!Y z%Rf@WA!!xpX@QWA_-szXQ*wpLe<stO*9F+mw~c<BmjT|6W=(FH?oBJ7#p6X8)yVFt z2HqGq5x-K}gj*gE;D6kG4exm|1+vfHA`5(0V)eCRc&}|6S-v;gWs`U!VzxSiAo&@3 zH@Bb6DCa?8{c+?}&qJCQ3dk8xUAQ+K&+aUd#u`Vaff$hhshSBORX73Z&@L!BSDJ3O znTSI#7*dliWDw25tLTNQ4fm8&;v%&CI83DZoSx*kP-xhL&x(7Ji=&1prs5RwzA+hX zzM_pCh83WAaSQy&)+gcR@x*baFWhXjrS}$G#92zgWcDXdJX7TtnOsvt^M|?!V=sk{ zN*zN<Vjh@I{y~sv6zh}vm#+Uh%ARi$BD}!v|21*u(Nw)*`!|LN2}w$!M4FIr_H%Cu z32D$Ik|d;214$eqB9si3Au5Ut6%B^{-1{hBDw2w#Q3+`tHK@F&cdg&Me*fKn?6vk@ z&sk?b&%UnDb#XIe*4I3qaTqo{PbEq_ge~yds7CahuV;@L7}+S#|4l#tx&<zV*D!-O zVq+#MvH#ID(Mw}9a$or_6a?pBMdD7z(XbYqCVUYm^>pa8@3n*!6~I=@F|bRbnH;;x zV`k$Yn&vzVzu|4hYsXchUgSt#n7^&jn%P0(_T3@+^;snTuoNf>8#Ki)l79NBIRN_V zB|yIS8{GIWm)&wuoMFyC#Lh0uhV6%j({-zwiBbA>FmYOe!EU`E#zV-tTb1Y@5JIx9 zOoJ~g-orPUqu6Px4rAyedMYFd9L8|mf%q^?ezgh~Ht<x#SPG>)G_m-TB-Xk_kjBE@ zMEL8=d?F>ehKX>hByl~}ob#?p@bT>-68>9-O_?akQgtnSd|pEIxH&`MrO?UFp7EY+ zo#g_yS9xrG@R7N6J09Qs9>v-QF}OpF(-z+?W!62D#hWW<W1mb1QG0oT8U&3;AL(qe zSTYAU+^M4b%f1r-G%46>Vk|`W2@()8X&PqNNTaH6ea(Kik@Tu%2MM~B&Gl}d4wC1i zt#?amLF{K$Rthym3Ed_9osyyKOr5<De#MY|(w4=+nOkVp<;!OsN+9Qyj>5NsDX8~e z1>R_W!jV-&*t5!RwDk8Bw#fQFoaCLtlx}_r6`njQT@>zy(3%<alX4zx+jNfZwfMwM z@)$)X$GejLFK%?(2yfcASO<I)4^ol(8|J$zL-fz6U|+B)9p2H;IhJVh${U>F!^wDF z`H2Pl*~63#&uF4{nkVSHwj5ZtpdQwJ(n8%iS<I8s4@k-u6+D`+48JG)!s{ziFkAR` zKW;6a3895kFz{!rm~<^A(fRSHqj-|+X>X?0X@xbTY%QUxV1T@8|3#zM8Q|Cy4S2df zhkjY`oVL1UQ}0>3N$}za<iygUm{_U8)}P!0Ka=;duCHe@b61SwFB<k?xy~B6F|C?t zS2r^XGn%;wiD}Rw{Tziy?6<>@`klB+S({JD<uE!c1RUNap<LZz=GED9n6@Mlo&OGj z(g*AC(UcYB&FL|?PA$s1@May!s2E56uAk?Mg2j>Epr5p1LN2}1;6O`GJ)-lH_i`8M zA)0D;A6`#52v#d(A-JrR?{G~9ueo>8&ovV0V+A4X-u9l#cr?)tM$CIUjluViCqYGl zo0tty;9u;G#Yl5^n5}pX`yb|tAF_~p`f4uCj6TOCB`cuGto>Bj(Memcrjub?Uy_8* zSkfYMh;CoGPvFxymevW3K`B-b<ld`tn^R55xUsij|A%=XtEa^~?3yMDIVDtN`*LIW zt<pPac$k=O9ym`wkLy53*K$ablVii~_R+Q?T?m|fiGDU%jyD}M8TYarV)s{w=i);^ zD*6H~Hjl&~*vv$355w(WJt=96Cw)^jX|v7{kPwG!=Im1ez1dS?c&Q2OYBS?>oKu;# z=j8BtcfDYou+4x^bIZh`R#9*T4~R~>Sh1(}7PGgn2JzwAv#G%cce0X<W+h&HCtQyS zpJdrU{>ZJR!&S5K)|BhCJlqo%U*4icQGcj{kqo*!DZ-sUvCucs6W8@?(+56>$?MH3 z7&BEvP;4|5LiK8D@{G<gDjxD=Q<9-r2^Ib&Zyxg`Np1#apOC~YXEgb+8xEqnz65rP zZ6+9YoAEK?wwH(lADK~NcDVBU8Q5w6Q;@KH78*XRCEHHakn@&jaa^ki2I4BA!k9wR z)~S5mN(DHYq{t3iX3U#>xDGx>TKspzrEK8tF=U?s!ST;rP_uug5SewuV1{@nTO~CD z3TH~g+|beB^ym;t+PZ_vCY_-=-k$jT^E?ufEW^$|Jx{dY!DvyQ&u%{Vha<mOUx99E z>>_`S7SS1vE5Rh;6L_DEueKQUrBM~<NQBZXw5yLJ@k9R7BO(*}SuE}Lewj^%O9#}P zZIh?zM^sz?S3dF(*`EJDqk3$rOSrkP*zzpL!W3q^ic{6Y6b~!C3zw=NR>fA?Yu*(^ zKI1tjd4n3w7E749FO}23Gfbc}C7EenBT3qm%5d$jxrF^@!5b%-3!D^NYI4=@Fsa*k z!FrcGfv*2>HdB=m%-&con3KAKsYsm0&GwNINJ(4pu0^}Ke~OOrH*?9l!x1*Z=O2t| z<y>2+9HK<bSS$SEc!5~ePDH+VBr;zQVE(pj>rmz-x!66LFT3%R=Cyvs!jKcBy3<!I znuwuzF)qAz;22Ifwu0D9b56HEi_WtDOC6N9lb=3mv?q=yIt>Bbkz!p~rq>SM_It@H z2YpiP?~6<Btq@`!yBDlF1JGP|Gqdi-UG8>QZ%t$9CzK8~hF5zE1-&Gm9nx6ME%cEm zasD1?9kvbn;>NQeZw(JlZN}7)yQEt=8g&*7;@#Z87&QMh<ZGA<BApmquM~*|js^7e zWm(q0qaOE4dXv%W2_Rfc(2qC6C)0#?P-dI~Y}>vY`OIwMe^ZTB{n$vpzL~?$smg~X z$GnK!%Q@(BQ-n&L8LZ*!O+;sx75}Vt8rkfHbi8I1J7kIoy=^T(y;q&rHZs8wFIV(Q zUx=*kQnq!fmSxV+2N;(4l=&(DmlnzY0Ilaq5G4PAXf59%P6{g#c=Rxy^C*19*+DhA zbM7$-8p@%~4hiP>$D_!uG>4^KDRjWs9^4z}P~rOjP}A!KoswHhCI248I?Xt=wq{8B ztP%Jk?*Uom)X$uBp9~8c-NC5h7P;T{n`}CI1wyYk!bO!tXml3q5CIi1g1t<lssV-F zv4+?w-9SA`O2Db)132}sAqCr<q0LYNttJ0MjU|!nVlk~}CBK1YIA)MDCzGhjo#bkD zcY9RTsz7UeK)!zaOdrll#@O60xYsDY>9(7YXSIu@sx5+52ckjtz8YQ**v}QjUPZ^! zPdNFd4s@LhfNL5nh2*}XIqQB}o{V>BAa0lRsl)ZdP}MRXeMj`+#qmn4vXdhJK1U4M zI@$AQmEYl!M~*m{>BsKyEJQm2N3$&N;g88a(0B1C?Xm0!Qy*PEabPOd__F{)*C*4P z+GeO4>_=AUFGNYjJnDJSn_KgzmJ9TcCUfQsjra)*?vNGRE3tWzEI2>%Cw6z=2}EO! zP`bPW1(`_ntdzLoSsB#-=>dBCX*!uR`761coKM^(b;)FxFyeAio;BCLM3h4!K&R>q z$qv%v7cREO!;#BTS7J4dI9i0B_mk+M-y2a*W;ywp#|XMC-hx-8zCcIVS%>WS3mCaW z6Pi9vw{}&zQ9Y$_CTw|VMy*_5aJENY(-F0SSorf9XnsEc=glJNn`Bi?PdSL19}-z_ z#eA#`J46Ksyy1)Q3p!F~E2!^@qiX~7_#NZxK-f1NOn!`pd%xr9*+2)%bzZ@@qvUDm zCM8_%a+nw1v&ev-OO)Axlau)I{Ug|YQ4gtU{bII3ymY~z7Q^Su=De%^D7sl)tWONC zhgkK~u;BOr(MVF}dvvUTRZpeLA0*kRY*!E+HzUDo*MoMID(DS+z$~CP<oKCFqByRD z^f=wXgrO%$sMijBG*1h%v?6JL;SM3he7!`iTYC{UUZtk}4Bh4&f{w=);`Vv37<SQ4 z+SoIah<A13zpI-V)*S+)+$N&7G73IFW!QbXei)TF8oJ9}KtHMuG{Rh|l#{s5M8N`W zQ^$!Z-V1=Vk?M4Ha#Jt)W7c2sY7J?|aCcw$)>O!N{1hrcYQkMoQ1pa&=4*=$?Lycx z$sIq7Ye`P;9fOxQHRHP@_vqf=U!c7E6L_9^LhTPpgGbL&a6bQw$QP8spSMB$R(BmN zt}2AfSAx*tdLdQH_ru&|H{M%Jaz3rJhw+bIW29{iA10U#Wh+OsH&&e^&Q>b8Q<xFU zm*-tXRsDRb^=LjD-J6P@eKBNGq5@u)sKxV#o)a?>2Ze7d@sgnp9q38p+Bd2*#$7oz z&+b%_8LN*Gg`1UvC7$gy+Z?s%7&})|7F<R4Oq0VDeOdDO$#bgx@)(nzp-OBLy~HGl zBhgLzQuB7>d_nq+SyZ^B>j@_&9vIz1Gr^LInCeo6e43e%MA8>tCxLyM%*(+GL^0$W z8Axg*vzp3iU)3dgDky_NJ$WeVOd?7ul>0E`ERnKGA`jN7Gs7J<!Q10LQEDwEo?Cs0 zH|-=U7pKvCKV+cg`VD4P>@Y^><O)g_OToUF4?<3OTLK;(=%RspdYQvXKgqL<Y?9-D zm{A;4OV|IHj9W@pqe}f$Faj2KFL=egaoUN=X{|IMcOf}{gy51Edt7v&i0F95(f+nH zVlXiqE4L%Puq+7=7u%A}*G6Kf-C64cAx+$%#v+oF;(>EnJ#u01N<6$#I2DSwRnP!q z2PA&eaqWIt+R_>i4Xrzw*=h|`D^rq8(bVF!ZTFBV?^3v&ej}Q8>MoZ#FVlMBtlv!B zOj)u-ayS)DSH*{m4+-Aq2XK?^Q_1JK8P?<2_e^WlDgnQ`Q}E|gId|Vd3YxO(tb^Wb z)r|G>p=Ak%Outa$AlY?O5e!T>);xcn#SFP-jPcz&iQPCUyx+5eY?i%5PTj4do{@^2 z+aVb`zpIVg$eyHQ>W$#KS_e6Hd63L;i=iK@BZ);&H{Bwt7c_gv(3QLM$v|%;7g`!h zttK~-+U>E7`nCcl!2cAfYW62{0=LnFAD0qg_sA;FNvVhi^qqkwHw|1KyArZXhtfUu z6Y)ptOLB3B0!cZ&mn>hO&oHH8WMqaFCTv(v<({77w%X=#`4yGazqX2ek~q(K>n()x zQ>W;hFR_eeusyl8aTk_OG^CzBax`I-33k;zrw&Wx!M#?1M;9uS=;%X2SZ%LGb0Y6> zY4`1@$vaD?>~$hp{rmyZ3q8*~`(<Ojd--LipFKf>Ca8&HWAfOa>q$;_?W5e`57j~8 zE#ysBQ_TZsdGc`6+?pakDaK{<Ms8V7d$q_siV6JhE)!P0nwp06a1(zGg?I}Y5G^^) zY?<m$mQI-jW(vZEq+0b2H+9WP?&G^?A}6DSL)Phmk@QGz<e<BNT_XN+yYY&uc|0LG z&t*ZeK7$l{SklS)bBX6UdG7rM88Rf{BpI?-lJ3+_r*@V*$e&xYYm)XkfTZUbdQ~Tt z+&I=sdJA50A6srPc{$pcl{^+@Wd-<S<9G5{7`=uVRSrd!wSJ_|-~_&1l0$cnv}1&G z2dXna90fu1I_g{`g(lNX&_cAAO!G9xaNj|}&%E)Z?YRQleV#-T&i+Tv_2|>-uhw%L z8cXqr-zaW=U@_xy-VK8tjJP0iY240o9xhu9Bjv@<>9;lF%>3D%%oW{KBGeaJ(Zz~2 zoc-5r<fK(D_igJECh&U*H~iWd@rTa{EDm_Wm8XoRch^Ocq9rLcu4fLDj}sqP*LL|Z z3u7%QN$Vy@HAd9T9I#;mjPq&sCns9^+Mj5h`aw*WwhOv_Hj+?Ban_|yg>JelL%05V z#QA=$XQX*2Y9p5_WXk4R<D0mV&~r(FWF8NIf~(WS3D~8?IBzeb|Lp~_P>W{_PN#AE z`8@88;4@{XoMqH9lwqE!F)S+nS>xcJ&iR#OkrJ<3vSCCPX?)#6;IR_bx*%dwex^Ww za4j8~C<m(IEL*PeQA$@R(GtNBDiAw4>9>7@aqi+_*xy}_hA(EqaCHfK=VlV!Y~qID zrS){j$o)*r@xSD-ngtw<Si_xV=HRJ&iFjm8n0061e)7vw7gzQTLpCaun>#K9!w*a) zLB)RL-w|8p$<9gt=TnbOEs0m(q&q#D#x7WcV*;n*#du>ncJq0t{A)%--(*pGqzqT7 z?n1dGp0p_-5ZY(7(iPiNYi4i3ni|P^ru@D>r)!kNtXU<hQQPFl&DC<M@waGZmK<-Z zNzx`Y?V|<EzuRt+zUK1(d}`fPmjrWR$GY8Qr$#!Hcl`q0<h+_wv8o|fr~0_iBa`9u zR2IxC{^0RF0c6bJZ+d6rb+j4!g7)1S%bUmPqq$8P-+tsd|Lo&V)>8s;!>Va~(R_1K zJL4Oi-k!s68dS6S>HHM7U0+&rb6F=i9xP`i+syfC%5x#A*_AChYAwWU_Aox<$75$x z8k~r8gE8|e=~{`$B=e{ySnMmr{xPYfE_WRlophakR0<<uMz(PMZvzJ+Z4w)t2QBWk z;7PCYb>S!Qv12y>ET$7?WF2H9f-ZwCN#Uob_HyXfKwYOQ*!Y|k*wl_a&AuJ4%?=T# z)a)gMcbKflJXTJMf$mr%xR4k}6CzeY=TmX4NTA4CfAGMI8|Bb>@o4xxzzLou$BV7X zPlV5kq^*Chkm}|rawG5yy{e#$ovy>M{kR4OiH&`#V9U!bbAqCI3GBO@YGB^jg=5QK zvT6Add}@RfwhlHia<*Gwhj8Qrc3)c*E4$l(T`*}ZT<t3b*BA$CrDqEkOY6Ym*)hDm zNdkU)tzoC<`%zQBT@VnZF80QU;l}W@MA-a+_$Q2pH31_qOR7PTr}mic$rV!Bn#-Ke zybolKb^zK)D6^--SAx0KMmGBHQr37zF(2_o1>Od)Cc@ut?XV>^Tl_npM5zWHJQ8_? zZC&fd+66CzW4Y>3wo{w+kzPY~u0I9UpC_=}G7drPB5C&4G*uRYKaoo3B#{2|1naGk zHih@pd>wSamZH({WoZw2*7=p0=&c2=GCxS^tCdu8sSG?_umy&VP-fNJs$tpLlS1$j zdfIrE%G;P)Ho%G6;n1;28O8<thHU~%HsY@bJ1e9GKc**wU%nArEw(ayd^?y*)tyi^ z_9~pswr3yDo<IY%Lg0B%88_Yj9c+4P!;Hz*f-+rGs<up>*m2)Tpv@z?{Pb};{#6?} zdMZW`nR!kSY}LXj34~cS1qltL(EA#Up0J*j2l(U1?Wt64xTdJ0`48BK_M*c49z2w{ z7iVYfgQ04`YL8pPKhk!_fvNXkF#I?@(3FU_VztjpLmDI6ZF$?J|8Y9q3h>HsG5=j$ z80UR=8<+l2m;D%VjQS3qq848X>(?2KS2OL%H{pOZozNAD<5i|(*(fy})Ue{_$7o@x z)&%y$IT?1vc@FJ+JVn~}&g^H|JT`a58EDY&AYm6uKzC;(e&|hOwQ4l^(PsPjJH;lf znw&Db$U24ERA}Mi$Mx7BDuF?J?xOUDA~em5<LBMBXNk*WxEL5fjl&{PxZHUy*<B%| zd!B0I<f+3js_6sO*l8ee{%!`rbRsByI7wD?Pl9qQYj*#TU(`768wOYVigt;ii-pJ5 zu>(KkYzo5G^7Dtvp}pgFV8)w6z@~QIRiP0lT#SIu;On%ydMIzXFqhnY6NeEmXCu6` z<gdr<K^Gsf0h{%vo!01ICBtVMiF?bwQk7XLG*mbMPM`PS{ITA2nrs?bC^ZLX#hJj; zI(@pbPM$N$sD`7lKDc#99`t79VX{{(FJEIM`Z#1h+Z~!rB6l}KZwjynlI9_iH05WC zC~ue%iOWNx;n=`jv^!<Y=MRs=5^EvF#hEks)e8mqh)Jgf_O93|&a<t4y&Ts?`oMto zIdaTxCN{gs!lk(y%>0*Pz=E%(d*Ti7hGZ_8cx?pis2;`?`0Ft(CbP+bE;oD~cm#%h z-Y%l2ma$I<e?Ug`LY&Waz~D?}Oq+U?>_4N(@AV7k$)!v5fN<nccza6%+(&J|1@hO$ zg|X>)s4yQd+$$r+n-%CYr!>rcm4m`-H_3mK6+!cSG5z?<kHie-!t*XI(%?`|OT&&+ z@3rN$FjEzBCa2OFu_K`@;tSD|N`~qk6>NRLIn?o*X`}PpK-9S`4`K(e!@ADT?E9PJ zc;W3!uGq0ui4XsKns>{tfG58*NxqK)iu`hUlP9ulLtP6--I>DAQj~zeo2RM$Ds{a1 z`x5Go>Bly=3m6o&feTqKg9q$~@m>?=kX=zD(D|VzXf`O*&5iw3?c*SITXkD-#@S9R zjs${hmkqz?MLiBZbx9=s=$eqPDQ0bsDCNL6O<P{mF#+D3T?QwWqmXM@z_tVrfsVyP zP{uEb8!ImL8Y=GJd7>TyA!G(GsdXQ=nNH*khkYUAvR!fFl|IgI;$Lv?nGb{JR$!6; z4rVlU(*CdFHll!d=E1`SWW$+LpxyYBF0gK=4hwHWY`*X)e{qv1e|+sMD85uGa=3K@ zA2cE4$L{4XNk#DU$N|h8I*Lz!GfK48ZY$e3m;)2FBS3f53uwJ}nf+pF#-n694t()r z758Q1;qi7<E+H8-j~&4zap6~7fCU`evXE<Cq=P4RNyEiLH{uiVnwkGxA7(so!Vki| zO{i?1N0v&_n)PqaVA)wOHYPrkZ*yD%VPPHow8#Y5y(I;Nju~v2au^@A^e}4LSi{zV z3N}4)s10d~V{_)2vU(Y7P<JPAk0zZVE>~61Yt%QIJ$^6d-mC+ysBE_L(m3|;WEJ+w zI&Jp!QgzN^_i4fAMQW_@*aka#?o=>I)Eh#i6*iL$B{}}?=dpNUUKXut`;HaO2K<Dv zQEb}D6Yyo%SGueJ8rnTl;wMg#<fmrYv)w*3akaa6<1#7XhmF(~?QF~BZ+I+3=P$pg z?W<tys2BsMQq%E(iwqA&R+xQ;$D*}I`Swi&rIi;8Vb-}5U~hDUQ_BgUOJ0UR?}Y`7 zTu%#QXx2pRR>so!7Gpfbmof#pr^!Px(fLc~GE`-rh0o$MqvC&wqGNyJ_>MFIJJ%tS zEb;ee4_ue$Z%?pc^(K0<OKRjq9_Jlt+gLBGZXJieGopFN@DKE)g(;FceGq;Vn(%_N z;&-(BRC>T(o0X_Zrk2wAoa5{U@-;%2N=_XDBR7<i<h*^fWAzXejVhoIR&_H<hb=j^ zbKxY`&z8#1--~s#K0|y*ET5Ab&5B({GNES!yZloWe_v${t3)ng;Qk<X$89yf%5*4i zy`cfM?r*@>k|X$1s4KxfzN-Q0ZjGdI-b{YxyNPVTMpv*8xCnX|hx6KZz7zMnqsXW+ zpfl|tRoQz4Bd*Dddw5*PnYX&|YTj*P&>#a-e4{B_Urn_nv%uu4B|F`&1VT1Rvop)b ziuNg7=a=iMincDZU}wyl%B<X$PXE}N@dJ-{p>QyCsz^zd=LOHon8?5wrm-c6U+Vh; zy(i|PmEm;0ZN?(9{QO-!w8NU0pZ^Xvg9Vgg4(^anBO4Oan9TVfP_gR<Cnz5fEUcI3 zgfRjv&ie{iuZ;$asqz@9aRtopza?$&P5H1lEwJN60sk;|q-d3@8yj<Vi;&LAPv-xm z<iY8RIJ&cFHviYVMVtUI;9sk32hXxwL`E$M-hdHGytYC^12^`uOEEwGdKffPNl4Uw z%(zIV0l9BnW1#So+jzK_Ivow?cDJXHzU(N@_?i;B%*>}-XGl>4^B9`qt_5z^EnLuo zjXd5e5R*f~N8mfThJW{hXZwTSvKf0E_*V_J&^zM^-m_I;_md9Ls;}iM7328&)H3RD z@Dg{gDjb_OZ-=;T+T?L*FWn<6Hkvy7DU*JkKQPId=DqABr@#9$Ny@R*I`J4m$32jK zZYWXTb&ZTFnTWIRj3t5LNcP>8f<)o`4^>5qs<5&Sk<pgqJC{kZbNpUHik+$G@e*a5 zi`G_9v8)Q;in+%{6V9;9$_)7nt%-Df&;oYy*mx=%{s*oIveA3R06skTnPd;s=aaP_ zfLZ7}D0OcpyQ^f_g*F~I%Gv{(D~rg}&LR>K&<OLBS8_I+jZyecxXD`nWi3%Qt-+f= zd+CCetGH;{Jlqr~$?p)w@x@&m;A<eumeo_dBrhg}tS{iWl_`9%_hoEZbA|Cun#=3w z4YR2pjA9>#wZn!-9YpoGB#in#kuR*Bg6-wO{O$Qtd|bH)=+|>7?eqo6SUJ|Rz?iL_ zFh*F@og<AZi{FDqj6J=`rEu#5ZqU}dj#PC{7JT-IVG{2|lSg4CFmK;sR_dx9yI)_O zn&j`rli{a%_rO1_^7(OW!E`+~>dtq3e)<jQWxr)dbbUgPC;qHUW)58Wp24k<Tg$3- zA0cU5{=%;6iEyDwz&_Mk&+a}bECm%qX?QDDfzHj}x%!If)Z@-;aslKyH}MgBm+u=| zdmx3m!~LZ?t7da88R}@hse_CvR)={rR`6<jAMy%!&vFjgBW<L&E)_|2pXSS2X7iOd z()ks`YGA6$Cp2zb!+uhnZL{r_n8|I;hpQ3lydjyxuS>3k7s4EMc9x_Ao9z+7@9-^v z$u6;MoQ*xCR4#y^I0KZ;c*fn8`&#pLcnhho-AL8=qujwY8fZPHg1L1_M3zd3YnH25 z&}C;Ln4tSz*lMqb86lPM^kRZYdy@)()yqUA)tJFt*_TRW(_Vr4cO>mfow!FsnjOCP zi1_ciYdAWF+Cav7Q*!9*CQiL;FO3P3MWJE?{Pw=UxUC+<hHFBMT)!2Yg6HzKZhhQ? z8^@^Xw3+<xOU*Fns5I2KY2Y`994LCK#-G|u>16xo)T`B!SKE}#l)IX<{trgcGiye% zj8i7>Qre68W-5@kW(IpLr9=pGN++P5_G;eW;w@h?rwG}RRg7M-8TQT`%71cL#ax}{ z%#G6U<Bh_aNnTVm&dU(z8*P1X_qsr`++`<eH3=eKyN5ChO0H7#A9}MAJ;Uh9kGILS z@8`(8S6<wlSVdT`p${2jUs7r1i)ebX5CoxxAg9*DKK&tl$_LAZvW^C3V6fo_px8BO z-)P9fi8K7FegQxHeHP!mC>d*hoTqDFoW&s@({UT^L4|EMu*v2x>GV?M3pO9%)qkWR zn^<S{w(bk<>5;*isnMiwtvfCqe}?&VYb&k4qJu#n?u%K-p=fZiixCH`DfeYNbrOC~ zfI!nuQkS-cos~bz#=GY#YxZ%v%~x+}b`fx(Kk^bwSByXnvvn*43fW<{Q`qKLvfR-w zu@8Q73h&Yo3vSyD#9b>X{QZa8xI*$7m7Dkx{FnRV_gf3FWPU6^_xKcY^>!kyI1<OZ z43uz@+ka7|Z>qqwlo6qE0}D<v)4}GN15xN=pyhlbp2&Iy)*It+|LS;A`PL<T-+&rF zq)`b5UB_UBfhol0y~I<ekMXTv()ix#4<K7_1=LnghN`$V_)8GXd-pkml&=~u(arFI zpJe!jlXTfvS6m?~<0+%S$?$ed#^6P-b{e)}xR7U5BDt1GN&Nj*k8IWtq_MH`?5#z$ zWY{=gQo$;)(@!Y!%~$rZ?p7fv>gpyT^{G4?{~tTqt{2CRSjY-;M^g3q2cf3jnop|i z;^StV<XsGh@a6VeU?FYBE6y~dTk`U7L8cs^-F}($+C0NQH=pq4{^H}>E>-3KjSZMg zK6&{6Ghh?5%!T)!X2XKn0g!4towL%&p&eJWaQ{dt>sqTsoOM5eULWg;2Ho<oWu^gL zXg3u0Oe`Rx7dlwqvzZajYA2o>HPG-%Gr6AqmOEFyhWOshB)ig!NJx7jxh=B}OTNwl zC6#Fq9o|MKy?$N&$=sK$J$F&?Po^0*UOkn~J|Xk39y;}9wx!I!66e%$IhHd28s<_L z9@k9WmZLB8ulFtWSdNv<zo2vKeeocU7(YlIaokGgUk*Ziot4bLWI}4v@d}xLn0~5V zuBFUB2tPGaJp6N=Q#tV<_IvA6e~5>Fs#L0Zo~6t`ohWsyc=)$4sU>-q(*GW*H}CrY E08F3^`Tzg` From 31a9110e6401ad112ef56ec930873fcb9f1f2680 Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Wed, 13 Apr 2022 14:10:48 +0200 Subject: [PATCH 206/512] add PIPG package --- ocs2_pipg/CMakeLists.txt | 91 ++ ocs2_pipg/include/ocs2_pipg/OcpSize.h | 38 + ocs2_pipg/include/ocs2_pipg/PIPG.h | 285 ++++ ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h | 40 + ocs2_pipg/package.xml | 17 + ocs2_pipg/src/OcpSize.cpp | 41 + ocs2_pipg/src/PIPG.cpp | 1473 +++++++++++++++++++ ocs2_pipg/test/testPIPGSolver.cpp | 308 ++++ 8 files changed, 2293 insertions(+) create mode 100644 ocs2_pipg/CMakeLists.txt create mode 100644 ocs2_pipg/include/ocs2_pipg/OcpSize.h create mode 100644 ocs2_pipg/include/ocs2_pipg/PIPG.h create mode 100644 ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h create mode 100644 ocs2_pipg/package.xml create mode 100644 ocs2_pipg/src/OcpSize.cpp create mode 100644 ocs2_pipg/src/PIPG.cpp create mode 100644 ocs2_pipg/test/testPIPGSolver.cpp diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_pipg/CMakeLists.txt new file mode 100644 index 000000000..37e5588ff --- /dev/null +++ b/ocs2_pipg/CMakeLists.txt @@ -0,0 +1,91 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_pipg) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_core + ocs2_qp_solver +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIRS} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + LIBRARIES + ${PROJECT_NAME} + # DEPENDS +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +# Multiple shooting solver library +add_library(${PROJECT_NAME} + src/PIPG.cpp + src/OcpSize.cpp +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) + add_clang_tooling( + TARGETS + ${PROJECT_NAME} + SOURCE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR + ) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +############# +## Testing ## +############# + +catkin_add_gtest(test_${PROJECT_NAME} + test/testPIPGSolver.cpp +) +target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main +) \ No newline at end of file diff --git a/ocs2_pipg/include/ocs2_pipg/OcpSize.h b/ocs2_pipg/include/ocs2_pipg/OcpSize.h new file mode 100644 index 000000000..20a89e45b --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/OcpSize.h @@ -0,0 +1,38 @@ +#pragma once + +#include <vector> + +#include <ocs2_core/Types.h> + +namespace ocs2 { +namespace pipg { + +struct OcpSize { + int numStages; // Number of stages (N), all vectors below must be of size N+1 + std::vector<int> numInputs; // Number of inputs + std::vector<int> numStates; // Number of states + std::vector<int> numIneqConstraints; // Number of general inequality constraints + + /** Constructor for N stages with constant state and inputs and without constraints */ + explicit OcpSize(int N = 0, int nx = 0, int nu = 0) + : numStages(N), numStates(N + 1, nx), numInputs(N + 1, nu), numIneqConstraints(N + 1, 0) { + numInputs.back() = 0; + } +}; + +bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept; + +/** + * Extract sizes based on the problem data + * + * @param dynamics : Linearized approximation of the discrete dynamics. + * @param cost : Quadratic approximation of the cost. + * @param constraints : Linearized approximation of constraints. + * @return Derived sizes + */ +OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints); + +} // namespace pipg +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG.h b/ocs2_pipg/include/ocs2_pipg/PIPG.h new file mode 100644 index 000000000..d3207b0ea --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/PIPG.h @@ -0,0 +1,285 @@ +#pragma once + +#include "ocs2_pipg/OcpSize.h" +#include "ocs2_pipg/PIPG_Settings.h" + +#include <ocs2_core/Types.h> +#include <ocs2_core/misc/Benchmark.h> +#include <ocs2_core/thread_support/ThreadPool.h> + +#include <Eigen/Sparse> + +#include <algorithm> +#include <condition_variable> +#include <mutex> +#include <numeric> + +namespace ocs2 { + +class Pipg { + public: + using Settings = pipg::Settings; + using OcpSize = pipg::OcpSize; + // solver status + enum class SolverStatus { SUCCESS, MAX_ITER }; + static std::string status2string(SolverStatus s) { + std::string res; + switch (s) { + case SolverStatus::SUCCESS: + res = "SUCCESS"; + break; + + case SolverStatus::MAX_ITER: + res = "FAIL"; + break; + + default: + break; + } + return res; + } + + Pipg() = default; + explicit Pipg(pipg::Settings pipgSettings); + ~Pipg(); + + /** + * Solve generic QP problem + * + * @note For constraints, it is Gz = g + * + * @param cost: Hessian(H) and gradient(h). + * @param constraints: Linear equality constraint matrix(G) and value(g). + * @param mu + * @param lambda + * @param sigma + * @param result + * @return SolverStatus + */ + SolverStatus solveDenseQP(const ScalarFunctionQuadraticApproximation& cost, const VectorFunctionLinearApproximation& constraints, + const vector_t& EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, vector_t& result) { + Eigen::SparseMatrix<scalar_t> H = cost.dfdxx.sparseView(); + auto& h = cost.dfdx; + Eigen::SparseMatrix<scalar_t> G = constraints.dfdx.sparseView(); + auto& g = constraints.f; + + return solveDenseQP(H, h, G, g, EInv, mu, lambda, sigma, result); + }; + + SolverStatus solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, + const vector_t& g, const vector_t& EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, + vector_t& result); + /** + * Solve Optimal Control type QP + * + * @param x0: Initial state + * @param dynamics: Dynamics array with N elements(The findal stage do NOT require dynamics). + * @param cost: State and input cost array with N+1 elements. + * @param constraints: State-input linear equality constraints. It is optional. + * @param mu: Lower bound of hessian(H). + * @param lambda: Upper bound of hessian(H). + * @param sigma: Upper bound of constraint matrix(G'G). + * @return SolverStatus + */ + SolverStatus solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t& scalingVectors, + const vector_array_t* EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, + const ScalarFunctionQuadraticApproximation& costM /**< [in] For testing only. */, + const VectorFunctionLinearApproximation& constraintsM /**< [in] For testing only. */); + + void calculatePreConditioningFactors(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, const int iteration, + vector_t& DOut, vector_t& EOut, scalar_t& cOut) const; + + void preConditioningInPlaceInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, const int iteration, vector_array_t& DOut, + vector_array_t& EOut, vector_array_t& scalingVectors, scalar_t& cOut, + const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G); + + vector_t HAbsRowSumInParallel(const std::vector<ScalarFunctionQuadraticApproximation>& cost) const; + /** + * @brief Scale data array in-place. + * + * @param [in] D + * @param [in] E + * @param [in] c + * @param [in, out] dynamics + * @param [in, out] cost + * @param [out] scalingVectors + */ + void scaleDataInPlace(const vector_t& D, const vector_t& E, const scalar_t c, std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors) const; + /** + * Construct linear constraint matrix + * + * @param x0 + * @param dynamics + * @param constraints + * @param res + */ + void getConstraintMatrix(const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + VectorFunctionLinearApproximation& res) const; + + void getConstraintMatrixSparse(const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + Eigen::SparseMatrix<scalar_t>& G, vector_t& g) const; + /** + * @brief Get the Cost Matrix object + * + * @param x0 + * @param cost + * @param res + */ + void getCostMatrix(const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, + ScalarFunctionQuadraticApproximation& res) const; + + /** + * @brief Get the Cost Matrix Sparse object + * + * @param x0 + * @param cost + * @param H + * @param h + */ + void getCostMatrixSparse(const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, + Eigen::SparseMatrix<scalar_t>& H, vector_t& h) const; + + void GGTAbsRowSumInParallel(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + vector_t& res); + + /** + * @brief + * + * @param size + */ + void resize(const OcpSize& size); + + /** + * @brief Get the Num Stacked Variables object + * + * @return int + */ + int getNumDecisionVariables() const { return numDecisionVariables; } + + int getNumDynamicsConstraints() const { return numDynamicsConstraints; } + + int getNumGeneralEqualityConstraints() const { + const auto totalNumberOfGeneralEqualityConstraints = + std::accumulate(ocpSize_.numIneqConstraints.begin(), ocpSize_.numIneqConstraints.end(), 0); + return totalNumberOfGeneralEqualityConstraints; + } + + void getStackedSolution(vector_t& res) const; + + void unpackSolution(const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const; + + void packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) const; + + void getStateInputTrajectoriesSolution(vector_array_t& xTrajectory, vector_array_t& uTrajectory) const; + + void descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const; + + std::string getBenchmarkingInformationDense() const; + scalar_t getTotalRunTimeInMilliseconds() const { return parallelizedQPTimer_.getTotalInMilliseconds(); } + scalar_t getAverageRunTimeInMilliseconds() const { return parallelizedQPTimer_.getAverageInMilliseconds(); } + + const Settings& settings() const { return pipgSettings_; } + Settings& settings() { return pipgSettings_; } + + private: + void verifySizes(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints) const; + + void runParallel(std::function<void(int)> taskFunction); + + void invSqrtInfNorm(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& scalingVectors, const int N, + const std::function<void(vector_t&)>& limitScaling, vector_array_t& D, vector_array_t& E); + + void scaleDataOneStepInPlace(const vector_array_t& D, const vector_array_t& E, const int N, + std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors); + + private: + ThreadPool threadPool_; + + Settings pipgSettings_; + + OcpSize ocpSize_; + + // data buffer for parallelized QP + vector_array_t X_, W_, V_, U_; + vector_array_t XNew_, UNew_, WNew_; + scalar_array_t startIndexArray_; + + // Problem size + int numDecisionVariables; + int numDynamicsConstraints; + + benchmark::RepeatedTimer denseQPTimer_; + benchmark::RepeatedTimer parallelizedQPTimer_; + + // Profiling + benchmark::RepeatedTimer step1v_; + benchmark::RepeatedTimer step2z_; + benchmark::RepeatedTimer step3w_; + benchmark::RepeatedTimer step4CheckConvergence_; +}; + +vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat); +vector_t matrixInfNormCols(const Eigen::SparseMatrix<scalar_t>& mat); +vector_t matrixRowwiseAbsSum(const Eigen::SparseMatrix<scalar_t>& mat); +void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix<scalar_t>& mat); + +template <typename T> +vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat) { + if (mat.rows() == 0 || mat.cols() == 0) { + return vector_t(0); + } else { + return mat.array().abs().matrix().rowwise().maxCoeff(); + } +} + +template <typename T, typename... Rest> +vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixBase<Rest>&... rest) { + vector_t temp = matrixInfNormRows(rest...); + if (mat.rows() == 0 || mat.cols() == 0) { + return temp; + } else if (temp.rows() != 0) { + return mat.array().abs().matrix().rowwise().maxCoeff().cwiseMax(temp); + } else { + return mat.array().abs().matrix().rowwise().maxCoeff(); + } +} + +template <typename T> +vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat) { + if (mat.rows() == 0 || mat.cols() == 0) { + return vector_t(0); + } else { + return mat.array().abs().matrix().colwise().maxCoeff().transpose(); + } +} + +template <typename T, typename... Rest> +vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixBase<Rest>&... rest) { + vector_t temp = matrixInfNormCols(rest...); + if (mat.rows() == 0 || mat.cols() == 0) { + return temp; + } else if (temp.rows() != 0) { + return mat.array().abs().matrix().colwise().maxCoeff().transpose().cwiseMax(temp); + } else { + return mat.array().abs().matrix().colwise().maxCoeff().transpose(); + } +} + +template <typename T> +void scaleMatrixInPlace(const vector_t* rowScale, const vector_t* colScale, Eigen::MatrixBase<T>& mat) { + if (rowScale != nullptr) mat.array().colwise() *= rowScale->array(); + if (colScale != nullptr) mat *= colScale->asDiagonal(); +} + +} // namespace ocs2 diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h b/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h new file mode 100644 index 000000000..e327513ed --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h @@ -0,0 +1,40 @@ +#pragma once + +#include <ocs2_core/Types.h> + +#include <string> + +namespace ocs2 { +namespace pipg { + +struct Settings { + Settings() = default; + Settings(size_t nThreads, int threadPriority, size_t maxNumIterations, scalar_t absoluteTolerance, scalar_t relativeTolerance, + size_t checkTerminationInterval, bool displayShortSummary) + : nThreads(nThreads), + threadPriority(threadPriority), + maxNumIterations(maxNumIterations), + absoluteTolerance(absoluteTolerance), + relativeTolerance(relativeTolerance), + checkTerminationInterval(checkTerminationInterval), + displayShortSummary(displayShortSummary){}; + + /** Number of threads used in the multi-threading scheme. */ + size_t nThreads = 1; + /** Priority of threads used in the multi-threading scheme. */ + int threadPriority = 99; + /** Maximum number of iterations of DDP. */ + size_t maxNumIterations = 15; + /** Termination criteria. **/ + scalar_t absoluteTolerance = 1e-3; + scalar_t relativeTolerance = 1e-2; + /** Number of iterations between consecutive calculation of termination conditions. **/ + size_t checkTerminationInterval = 25; + /** This value determines to display the a summary log. */ + bool displayShortSummary = false; +}; + +// Settings loadSettings(const std::string& filename, const std::string& fieldName = "ddp", bool verbose = true); + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_pipg/package.xml b/ocs2_pipg/package.xml new file mode 100644 index 000000000..ec355a7a6 --- /dev/null +++ b/ocs2_pipg/package.xml @@ -0,0 +1,17 @@ +<?xml version="1.0"?> +<package format="2"> + <name>ocs2_pipg</name> + <version>0.0.0</version> + <description>A numerical implementation of PIPG.</description> + + <maintainer email="zhengfuaj@gmail.com">Zhengyu Fu</maintainer> + + <license>TODO</license> + + <buildtool_depend>catkin</buildtool_depend> + <depend>ocs2_core</depend> + + <!-- Test dependancy --> + <depend>ocs2_qp_solver</depend> + +</package> \ No newline at end of file diff --git a/ocs2_pipg/src/OcpSize.cpp b/ocs2_pipg/src/OcpSize.cpp new file mode 100644 index 000000000..3a269bd54 --- /dev/null +++ b/ocs2_pipg/src/OcpSize.cpp @@ -0,0 +1,41 @@ +#include "ocs2_pipg/OcpSize.h" + +namespace ocs2 { +namespace pipg { + +bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept { + // use && instead of &= to enable short-circuit evaluation + bool same = lhs.numStages == rhs.numStages; + same = same && (lhs.numInputs == rhs.numInputs); + same = same && (lhs.numStates == rhs.numStates); + same = same && (lhs.numIneqConstraints == rhs.numIneqConstraints); + return same; +} + +OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints) { + const int numStages = dynamics.size(); + + OcpSize problemSize(dynamics.size()); + + // State inputs + for (int k = 0; k < numStages; k++) { + problemSize.numStates[k] = dynamics[k].dfdx.cols(); + problemSize.numInputs[k] = dynamics[k].dfdu.cols(); + } + problemSize.numStates[numStages] = dynamics[numStages - 1].dfdx.rows(); + problemSize.numInputs[numStages] = 0; + + // Constraints + if (constraints != nullptr) { + for (int k = 0; k < numStages + 1; k++) { + problemSize.numIneqConstraints[k] = (*constraints)[k].f.size(); + } + } + + return problemSize; +} + +} // namespace pipg +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/src/PIPG.cpp b/ocs2_pipg/src/PIPG.cpp new file mode 100644 index 000000000..4bfbfb058 --- /dev/null +++ b/ocs2_pipg/src/PIPG.cpp @@ -0,0 +1,1473 @@ + +#include "ocs2_pipg/PIPG.h" + +#include <iostream> + +namespace ocs2 { + +Pipg::Pipg(pipg::Settings pipgSettings) + : pipgSettings_(pipgSettings), threadPool_(std::max(pipgSettings.nThreads, size_t(1)) - 1, pipgSettings.threadPriority) { + Eigen::initParallel(); +}; + +Pipg::SolverStatus Pipg::solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, + const vector_t& g, const vector_t& EInv, const scalar_t mu, const scalar_t lambda, + const scalar_t sigma, vector_t& result) { + if (const int N = ocpSize_.numStages < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + + denseQPTimer_.startTimer(); + + // Cold start + vector_t z = vector_t::Zero(H.cols()); + vector_t z_old; + + scalar_t alpha, beta; + vector_t v, w; + + w.setZero(g.rows()); + + // Iteration number + size_t k = 0; + bool isConverged = false; + scalar_t constraintsViolationInfNorm; + scalar_t zNorm; + while (k < settings().maxNumIterations && !isConverged) { + alpha = 2.0 / ((k + 1.0) * mu + 2.0 * lambda); + beta = (k + 1) * mu / (2.0 * sigma); + + step1v_.startTimer(); + z_old = z; + // v = w + beta * (G * z - g); + v = w - beta * g; + v.noalias() += beta * (G * z); + step1v_.endTimer(); + + step2z_.startTimer(); + // z = z_old - alpha * (H * z_old + h + G.transpose() * v); + z = z_old - alpha * h; + z.noalias() -= alpha * (H * z_old); + z.noalias() -= alpha * (G.transpose() * v); + step2z_.endTimer(); + + step3w_.startTimer(); + // w = w + beta * (G * z - g); + w -= beta * g; + w.noalias() += beta * (G * z); + step3w_.endTimer(); + + if (k % settings().checkTerminationInterval == 0) { + step4CheckConvergence_.startTimer(); + zNorm = z.squaredNorm() < 0.1 ? 1.0 : z.squaredNorm(); + constraintsViolationInfNorm = (EInv.asDiagonal() * (G * z - g)).lpNorm<Eigen::Infinity>(); + isConverged = constraintsViolationInfNorm <= settings().absoluteTolerance && + (z - z_old).squaredNorm() <= settings().relativeTolerance * settings().relativeTolerance * zNorm; + step4CheckConvergence_.endTimer(); + } + + k++; + } + denseQPTimer_.endTimer(); + result = z; + + if (settings().displayShortSummary) { + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n++++++++++++++ PIPG-DenseQP " << (isConverged ? "Success" : "Fail") << " +++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + std::cerr << "Number of Iterations: " << k << " out of " << settings().maxNumIterations << "\n"; + std::cerr << "Relative change of primal solution : " << std::sqrt((z - z_old).squaredNorm() / zNorm) << "\n"; + std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; + std::cerr << "Run time: " << denseQPTimer_.getLastIntervalInMilliseconds() << "\n"; + } + return Pipg::SolverStatus::SUCCESS; +}; + +Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints, + const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, + const scalar_t lambda, const scalar_t sigma, const ScalarFunctionQuadraticApproximation& costM, + const VectorFunctionLinearApproximation& constraintsM) { + verifySizes(dynamics, cost, constraints); + const int N = ocpSize_.numStages; + if (N < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + if (scalingVectors.size() != N) { + throw std::runtime_error("[PIPG: solveOCPInParallel] The size of scalingVectors doesn't match the number of stage."); + } + + // Disable Eigen's internal multithreading + Eigen::setNbThreads(1); + + parallelizedQPTimer_.startTimer(); + + vector_array_t primalResidualArray(N); + scalar_array_t constraintsViolationInfNormArray(N); + scalar_t constraintsViolationInfNorm; + + scalar_t solutionSSE, solutionSquaredNorm; + scalar_array_t solutionSEArray(N); + scalar_array_t solutionSquaredNormArray(N); + + // initial state + X_[0] = x0; + XNew_[0] = x0; + // cold start + for (int t = 0; t < N; t++) { + X_[t + 1].setZero(dynamics[t].dfdx.rows()); + U_[t].setZero(dynamics[t].dfdu.cols()); + W_[t].setZero(dynamics[t].dfdx.rows()); + // WNew_ will NOT be filled, but will be swapped to W_ in iteration 0. Thus, initialize WNew_ here. + WNew_[t].setZero(dynamics[t].dfdx.rows()); + } + + scalar_t alpha = 2.0 / (mu + 2.0 * lambda); + scalar_t beta = mu / (2.0 * sigma); + scalar_t betaLast = 0; + + // // test + // vector_t v_test, w_test, z_test, old_z_test, old_old_z_test, old_w_test; + // scalar_t old_alpha = alpha, old_beta = beta; + // z_test.setZero(getNumDecisionVariables()); + // w_test.setZero(std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0)); + // auto& G = constraintsM.dfdx; + // auto& g = constraintsM.f; + // auto& H = costM.dfdxx; + // auto& h = costM.dfdx; + // // test end + + size_t k = 0; + std::atomic_int timeIndex{1}, finishedTaskCounter{0}; + std::atomic_bool keepRunning{true}, shouldWait{true}; + bool isConverged = false; + + std::mutex mux; + std::condition_variable iterationFinished; + std::vector<int> threadsWorkloadCounter(threadPool_.numThreads() + 1, 0); + + auto updateVariablesTask = [&](int workerId) { + int t; + int workerOrder; + + while (keepRunning) { + // Reset workerOrder in case all tasks have been assigned and some workers cannot enter the following while loop, keeping the + // workerOrder from previous iterations. + workerOrder = 0; + while ((t = timeIndex++) <= N) { + if (t == N) { + std::lock_guard<std::mutex> lk(mux); + shouldWait = true; + } + // Multi-thread performance analysis + threadsWorkloadCounter[workerId]++; + + // PIPG algorithm + const auto& A = dynamics[t - 1].dfdx; + const auto& B = dynamics[t - 1].dfdu; + const auto& C = scalingVectors[t - 1]; + const auto& b = dynamics[t - 1].f; + + const auto& R = cost[t - 1].dfduu; + const auto& Q = cost[t].dfdxx; + const auto& P = cost[t - 1].dfdux; + const auto& hx = cost[t].dfdx; + const auto& hu = cost[t - 1].dfdu; + + if (k != 0) { + // Update W of the iteration k - 1. Move the update of W to the front of the calculation of V to prevent data race. + // vector_t primalResidual = C * X_[t] - A * X_[t - 1] - B * U_[t - 1] - b; + primalResidualArray[t - 1] = -b; + primalResidualArray[t - 1].array() += C.array() * X_[t].array(); + primalResidualArray[t - 1].noalias() -= A * X_[t - 1]; + primalResidualArray[t - 1].noalias() -= B * U_[t - 1]; + if (EInv != nullptr) { + constraintsViolationInfNormArray[t - 1] = (*EInv)[t - 1].cwiseProduct(primalResidualArray[t - 1]).lpNorm<Eigen::Infinity>(); + } else { + constraintsViolationInfNormArray[t - 1] = primalResidualArray[t - 1].lpNorm<Eigen::Infinity>(); + } + + WNew_[t - 1] = W_[t - 1] + betaLast * primalResidualArray[t - 1]; + + // What stored in UNew and XNew is the solution of iteration k - 2 and what stored in U and X is the solution of iteration k + // - 1. By convention, iteration starts from 0 and the solution of iteration -1 is the initial value. Reuse UNew and XNew + // memory to store the difference between the last solution and the one before last solution. + UNew_[t - 1] -= U_[t - 1]; + XNew_[t] -= X_[t]; + + solutionSEArray[t - 1] = UNew_[t - 1].squaredNorm() + XNew_[t].squaredNorm(); + solutionSquaredNormArray[t - 1] = U_[t - 1].squaredNorm() + X_[t].squaredNorm(); + } + + // V_[t - 1] = W_[t - 1] + (beta + betaLast) * (C * X_[t] - A * X_[t - 1] - B * U_[t - 1] - b); + V_[t - 1] = W_[t - 1] - (beta + betaLast) * b; + V_[t - 1].array() += (beta + betaLast) * C.array() * X_[t].array(); + V_[t - 1].noalias() -= (beta + betaLast) * (A * X_[t - 1]); + V_[t - 1].noalias() -= (beta + betaLast) * (B * U_[t - 1]); + + // UNew_[t - 1] = U_[t - 1] - alpha * (R * U_[t - 1] + P * X_[t - 1] + hu - B.transpose() * V_[t - 1]); + UNew_[t - 1] = U_[t - 1] - alpha * hu; + UNew_[t - 1].noalias() -= alpha * (R * U_[t - 1]); + UNew_[t - 1].noalias() -= alpha * (P * X_[t - 1]); + UNew_[t - 1].noalias() += alpha * (B.transpose() * V_[t - 1]); + + // XNew_[t] = X_[t] - alpha * (Q * X_[t] + hx + C * V_[t - 1]); + XNew_[t] = X_[t] - alpha * hx; + XNew_[t].array() -= alpha * C.array() * V_[t - 1].array(); + XNew_[t].noalias() -= alpha * (Q * X_[t]); + + if (t != N) { + const auto& ANext = dynamics[t].dfdx; + const auto& BNext = dynamics[t].dfdu; + const auto& CNext = scalingVectors[t]; + const auto& bNext = dynamics[t].f; + + // dfdux + const auto& PNext = cost[t].dfdux; + + // vector_t VNext = W_[t] + (beta + betaLast) * (CNext * X_[t + 1] - ANext * X_[t] - BNext * U_[t] - bNext); + vector_t VNext = W_[t] - (beta + betaLast) * bNext; + VNext.array() += (beta + betaLast) * CNext.array() * X_[t + 1].array(); + VNext.noalias() -= (beta + betaLast) * (ANext * X_[t]); + VNext.noalias() -= (beta + betaLast) * (BNext * U_[t]); + + XNew_[t].noalias() += alpha * (ANext.transpose() * VNext); + // Add dfdxu * du if it is not the final state. + XNew_[t].noalias() -= alpha * (PNext.transpose() * U_[t]); + } + + workerOrder = ++finishedTaskCounter; + } + + if (workerOrder != N) { + std::unique_lock<std::mutex> lk(mux); + iterationFinished.wait(lk, [&shouldWait] { return !shouldWait; }); + lk.unlock(); + } else { + betaLast = beta; + // Adaptive step size + alpha = 2.0 / ((static_cast<scalar_t>(k) + 1.0) * mu + 2.0 * lambda); + beta = (static_cast<scalar_t>(k) + 1.0) * mu / (2.0 * sigma); + + if (k != 0 && k % settings().checkTerminationInterval == 0) { + constraintsViolationInfNorm = + *(std::max_element(constraintsViolationInfNormArray.begin(), constraintsViolationInfNormArray.end())); + + solutionSSE = std::accumulate(solutionSEArray.begin(), solutionSEArray.end(), 0.0); + solutionSquaredNorm = std::accumulate(solutionSquaredNormArray.begin(), solutionSquaredNormArray.end(), 0.0); + solutionSquaredNorm = solutionSquaredNorm < 0.1 ? 1.0 : solutionSquaredNorm; + + isConverged = constraintsViolationInfNorm <= settings().absoluteTolerance && + solutionSSE <= settings().relativeTolerance * settings().relativeTolerance * solutionSquaredNorm; + + keepRunning = k < settings().maxNumIterations && !isConverged; + } + + XNew_.swap(X_); + UNew_.swap(U_); + WNew_.swap(W_); + + // /** + // * ********************************* + // * ************ Test begin ********* + // * ********************************* + // */ + // old_old_z_test = old_z_test; + // old_z_test = z_test; + // old_w_test = w_test; + + // v_test = w_test + old_beta * (G * z_test - g); + // z_test = z_test - old_alpha * (H * z_test + h + G.transpose() * v_test); + // w_test = w_test + old_beta * (G * z_test - g); + + // old_alpha = alpha; + // old_beta = beta; + + // vector_t V_all(V_.size() * ocpSize_.numStates.front()), W_all(W_.size() * ocpSize_.numStates.front()), + // X_all(X_.size() * ocpSize_.numStates.front()), U_all(U_.size() * ocpSize_.numInputs.front()), + // Z_all(getNumDecisionVariables()); + // int curRow = 0; + // for (int i = 0; i < N; i++) { + // const auto nx = ocpSize_.numStates[i + 1]; + // V_all.segment(curRow, nx) = V_[i]; + // W_all.segment(curRow, nx) = W_[i]; + // curRow += nx; + // } + // getStackedSolution(Z_all); + + // if (!V_all.isApprox(v_test)) { + // std::cerr << "k: " << k << "\n"; + // std::cerr << "v_test: " << v_test.transpose() << "\n"; + // std::cerr << "v_all: " << V_all.transpose() << "\n"; + // std::cerr << "diff:v " << (V_all - v_test).transpose() << std::endl; + // } + // if (!Z_all.isApprox(z_test)) { + // std::cerr << "k: " << k << "\n"; + // std::cerr << "z_test: " << z_test.transpose() << "\n"; + // std::cerr << "z_all: " << Z_all.transpose() << "\n"; + // for (auto& v : X_) std::cerr << v.transpose() << "\n"; + // std::cerr << "\n"; + // for (auto& v : U_) std::cerr << v.transpose() << "\n"; + // std::cerr << "diff:z " << (Z_all - z_test).transpose() << "\n" << std::endl; + // } + // if (k != 0 && !W_all.isApprox(old_w_test)) { + // std::cerr << "k: " << k << "\n"; + // std::cerr << "w_test: " << w_test.transpose() << "\n"; + // std::cerr << "w_all: " << W_all.transpose() << "\n"; + // std::cerr << "diff:w " << (W_all - w_test).transpose() << std::endl; + // } + // if (k != 0 && k % settings().checkTerminationInterval == 0 && + // (std ::abs((EInv.asDiagonal() * (G * old_z_test - g)).lpNorm<Eigen::Infinity>() - constraintsViolationInfNorm) > + // settings().absoluteTolerance)) { + // std::cerr << "k: " << k << "\n"; + // std::cerr << "base: " << (G * old_z_test - g).cwiseAbs().maxCoeff() << "\n"; + // std::cerr << "my: " << constraintsViolationInfNorm << "\n\n"; + // } + // if (k != 0 && k % settings().checkTerminationInterval == 0 && + // (std::abs((old_z_test - old_old_z_test).squaredNorm() - solutionSSE) > 1e-12 || + // std::abs(old_z_test.squaredNorm() - solutionSquaredNorm) > 1e-12)) { + // std::cerr << "k: " << k << "\n"; + // std::cerr << "solutionSSE diff: " << std::scientific << std::abs((old_z_test - old_old_z_test).squaredNorm() - solutionSSE) + // << "\n"; + // std::cerr << "solutionSSE: " << solutionSSE << "\n"; + // std::cerr << "base: " << (old_z_test - old_old_z_test).squaredNorm() << "\n"; + // std::cerr << "solutionSquaredNorm diff: " << std::scientific << std::abs(old_z_test.squaredNorm() - solutionSquaredNorm) << + // "\n"; std::cerr << "solutionSquaredNorm: " << solutionSquaredNorm << "\n"; std::cerr << "base: " << old_z_test.squaredNorm() << + // "\n\n"; + // } + // /** + // * ********************************* + // * ************ Test end *********** + // * ********************************* + // */ + + ++k; + finishedTaskCounter = 0; + timeIndex = 1; + { + std::lock_guard<std::mutex> lk(mux); + shouldWait = false; + } + iterationFinished.notify_all(); + } + } + }; + runParallel(std::move(updateVariablesTask)); + + parallelizedQPTimer_.endTimer(); + + if (settings().displayShortSummary) { + scalar_t totalTasks = std::accumulate(threadsWorkloadCounter.cbegin(), threadsWorkloadCounter.cend(), 0.0); + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n++++++++++++++ PIPG-Parallel " << (isConverged ? "Success" : "Fail") << " +++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + std::cerr << "Number of Iterations: " << k << " out of " << settings().maxNumIterations << "\n"; + std::cerr << "Relative change of primal solution : " << std::sqrt(solutionSSE / solutionSquaredNorm) << "\n"; + std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; + std::cerr << "Thread workload(ID: # of finished tasks): "; + for (int i = 0; i < threadsWorkloadCounter.size(); i++) { + std::cerr << i << ": " << threadsWorkloadCounter[i] << "(" << static_cast<scalar_t>(threadsWorkloadCounter[i]) / totalTasks * 100.0 + << "%) "; + } + std::cerr << "\nRun time: " << parallelizedQPTimer_.getLastIntervalInMilliseconds() << "\n\n"; + } + + Eigen::setNbThreads(0); // Restore default setup. + + return Pipg::SolverStatus::SUCCESS; +}; + +void Pipg::calculatePreConditioningFactors(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, + const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut) const { + const int nz = H.rows(); + const int nc = G.rows(); + + // Init output + cOut = 1.0; + DOut.setOnes(nz); + EOut.setOnes(nc); + + for (int i = 0; i < iteration; i++) { + vector_t D, E; + const vector_t infNormOfHCols = matrixInfNormCols(H); + const vector_t infNormOfGCols = matrixInfNormCols(G); + + D = infNormOfHCols.array().max(infNormOfGCols.array()); + E = matrixInfNormRows(G); + + for (int i = 0; i < D.size(); i++) { + if (D(i) > 1e+4) D(i) = 1e+4; + if (D(i) < 1e-4) D(i) = 1.0; + } + + for (int i = 0; i < E.size(); i++) { + if (E(i) > 1e+4) E(i) = 1e+4; + if (E(i) < 1e-4) E(i) = 1.0; + } + + D = D.array().sqrt().inverse(); + E = E.array().sqrt().inverse(); + + scaleMatrixInPlace(D, D, H); + + scaleMatrixInPlace(E, D, G); + + h.array() *= D.array(); + + DOut = DOut.cwiseProduct(D); + EOut = EOut.cwiseProduct(E); + + scalar_t infNormOfh = h.lpNorm<Eigen::Infinity>(); + if (infNormOfh < 1e-4) infNormOfh = 1.0; + + const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); + scalar_t c_temp = std::max(infNormOfHColsUpdated.mean(), infNormOfh); + if (c_temp > 1e+4) c_temp = 1e+4; + if (c_temp < 1e-4) c_temp = 1.0; + + scalar_t gamma = 1.0 / c_temp; + + H *= gamma; + h *= gamma; + + cOut *= gamma; + } +} + +void Pipg::invSqrtInfNorm(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& scalingVectors, const int N, + const std::function<void(vector_t&)>& limitScaling, vector_array_t& D, vector_array_t& E) { + // Helper function + auto procedure = [&limitScaling](vector_t& dst, const vector_t& norm) { + dst = norm; + limitScaling(dst); + dst.array() = dst.array().sqrt().inverse(); + }; + + procedure(D[0], matrixInfNormCols(cost[0].dfduu, dynamics[0].dfdu)); + procedure(E[0], matrixInfNormRows(dynamics[0].dfdu, scalingVectors[0].asDiagonal().toDenseMatrix())); + + std::atomic_int timeStamp{1}; + auto task = [&](int workerId) { + int k; + while ((k = timeStamp++) < N) { + procedure(D[2 * k - 1], + matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux, scalingVectors[k - 1].asDiagonal().toDenseMatrix(), dynamics[k].dfdx)); + procedure(D[2 * k], matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu, dynamics[k].dfdu)); + procedure(E[k], matrixInfNormRows(dynamics[k].dfdx, dynamics[k].dfdu, scalingVectors[k].asDiagonal().toDenseMatrix())); + } + }; + + runParallel(std::move(task)); + + procedure(D[2 * N - 1], matrixInfNormCols(cost[N].dfdxx, scalingVectors[N - 1].asDiagonal().toDenseMatrix())); +} + +void Pipg::scaleDataOneStepInPlace(const vector_array_t& D, const vector_array_t& E, const int N, + std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors) { + scaleMatrixInPlace(&(D[0]), &(D[0]), cost.front().dfduu); + scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdu); + scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdux); + + std::atomic_int timeStamp{1}; + auto scaleCost = [&](int workerId) { + int k; + while ((k = timeStamp++) < N) { + auto& Q = cost[k].dfdxx; + auto& R = cost[k].dfduu; + auto& P = cost[k].dfdux; + auto& q = cost[k].dfdx; + auto& r = cost[k].dfdu; + + scaleMatrixInPlace(&(D[2 * k - 1]), &(D[2 * k - 1]), Q); + scaleMatrixInPlace(&(D[2 * k - 1]), nullptr, q); + scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k - 1]), P); + + scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k]), R); + scaleMatrixInPlace(&(D[2 * k]), nullptr, r); + } + }; + + runParallel(std::move(scaleCost)); + + scaleMatrixInPlace(&(D.back()), &(D.back()), cost[N].dfdxx); + scaleMatrixInPlace(&(D.back()), nullptr, cost[N].dfdx); + + // Scale G & g + auto& B_0 = dynamics[0].dfdu; + auto& C_0 = scalingVectors[0]; + /** + * \tilde{B} = E * B * D + * scaling = E * I * D + * \tilde{g} = E * g + */ + scaleMatrixInPlace(&(E[0]), &(D[0]), B_0); + C_0.array() *= E[0].array() * D[1].array(); + + scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].dfdx); + scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].f); + + timeStamp = 1; + auto scaleConstraints = [&](int workerId) { + int k; + while ((k = timeStamp++) < N) { + auto& A_k = dynamics[k].dfdx; + auto& B_k = dynamics[k].dfdu; + auto& b_k = dynamics[k].f; + auto& C_k = scalingVectors[k]; + + scaleMatrixInPlace(&(E[k]), &(D[2 * k - 1]), A_k); + scaleMatrixInPlace(&(E[k]), &(D[2 * k]), B_k); + C_k.array() *= E[k].array() * D[2 * k + 1].array(); + + scaleMatrixInPlace(&(E[k]), nullptr, b_k); + } + }; + + runParallel(std::move(scaleConstraints)); +} + +void Pipg::preConditioningInPlaceInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, const int iteration, + vector_array_t& DOut, vector_array_t& EOut, vector_array_t& scalingVectors, scalar_t& cOut, + const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, + const Eigen::SparseMatrix<scalar_t>& G) { + const int N = ocpSize_.numStages; + if (N < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + + auto limitScaling = [](vector_t& vec) { + for (int i = 0; i < vec.size(); i++) { + if (vec(i) > 1e+4) vec(i) = 1e+4; + if (vec(i) < 1e-4) vec(i) = 1.0; + } + }; + + // Init output + cOut = 1.0; + DOut.resize(2 * N); + EOut.resize(N); + scalingVectors.resize(N); + for (int i = 0; i < N; i++) { + DOut[2 * i].setOnes(ocpSize_.numInputs[i]); + DOut[2 * i + 1].setOnes(ocpSize_.numStates[i + 1]); + EOut[i].setOnes(ocpSize_.numStates[i + 1]); + scalingVectors[i].setOnes(ocpSize_.numStates[i + 1]); + } + + // /** + // * Test start + // */ + // Eigen::SparseMatrix<scalar_t> HTest = H; + // vector_t hTest = h; + // Eigen::SparseMatrix<scalar_t> GTest = G; + // /** + // * Test end + // */ + + vector_array_t D(2 * N), E(N); + for (int i = 0; i < iteration; i++) { + invSqrtInfNorm(dynamics, cost, scalingVectors, N, limitScaling, D, E); + scaleDataOneStepInPlace(D, E, N, dynamics, cost, scalingVectors); + + for (int k = 0; k < DOut.size(); k++) { + DOut[k].array() *= D[k].array(); + } + for (int k = 0; k < EOut.size(); k++) { + EOut[k].array() *= E[k].array(); + } + + scalar_t infNormOfh = (cost.front().dfdu + cost.front().dfdux * x0).lpNorm<Eigen::Infinity>(); + for (int k = 1; k < N; k++) { + infNormOfh = std::max(infNormOfh, std::max(cost[k].dfdx.lpNorm<Eigen::Infinity>(), cost[k].dfdu.lpNorm<Eigen::Infinity>())); + } + infNormOfh = std::max(infNormOfh, cost[N].dfdx.lpNorm<Eigen::Infinity>()); + if (infNormOfh < 1e-4) infNormOfh = 1.0; + + scalar_t sumOfInfNormOfH = matrixInfNormCols(cost[0].dfduu).derived().sum(); + for (int k = 1; k < N; k++) { + sumOfInfNormOfH += matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux).derived().sum(); + sumOfInfNormOfH += matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu).derived().sum(); + } + sumOfInfNormOfH += matrixInfNormCols(cost[N].dfdxx).derived().sum(); + + scalar_t c_temp = std::max(sumOfInfNormOfH / getNumDecisionVariables(), infNormOfh); + if (c_temp > 1e+4) c_temp = 1e+4; + if (c_temp < 1e-4) c_temp = 1.0; + + scalar_t gamma = 1.0 / c_temp; + + for (int k = 0; k <= N; ++k) { + cost[k].dfdxx *= gamma; + cost[k].dfduu *= gamma; + cost[k].dfdux *= gamma; + cost[k].dfdx *= gamma; + cost[k].dfdu *= gamma; + } + + cOut *= gamma; + + // /** + // * Test start + // */ + // auto isEqual = [](scalar_t a, scalar_t b) -> bool { + // return std::abs(a - b) < std::numeric_limits<scalar_t>::epsilon() * std::min(std::abs(a), std::abs(b)); + // }; + // vector_t DTest, ETest; + // vector_t infNormOfHCols = matrixInfNormCols(HTest); + // vector_t infNormOfGCols = matrixInfNormCols(GTest); + + // DTest = infNormOfHCols.array().max(infNormOfGCols.array()); + // ETest = matrixInfNormRows(GTest); + + // limitScaling(DTest); + // limitScaling(ETest); + + // std::cerr << "DTest: " << DTest.transpose() << "\n"; + // std::cerr << "ETest : " << ETest.transpose() << "\n"; + // std::cerr << "G :\n " << GTest.toDense() << "\n"; + + // DTest = DTest.array().sqrt().inverse(); + // ETest = ETest.array().sqrt().inverse(); + + // scaleMatrixInPlace(DTest, DTest, HTest); + + // scaleMatrixInPlace(ETest, DTest, GTest); + + // hTest.array() *= DTest.array(); + + // scalar_t infNormOfhTest = hTest.lpNorm<Eigen::Infinity>(); + // if (infNormOfhTest < 1e-4) infNormOfhTest = 1.0; + + // const vector_t infNormOfHColsUpdated = matrixInfNormCols(HTest); + // scalar_t c_tempTest = std::max(infNormOfHColsUpdated.mean(), infNormOfhTest); + // if (c_tempTest > 1e+4) c_tempTest = 1e+4; + // if (c_tempTest < 1e-4) c_tempTest = 1.0; + + // scalar_t gammaTest = 1.0 / c_tempTest; + + // HTest *= gamma; + // hTest *= gamma; + + // ocs2::vector_t DStacked(H.cols()), EStacked(G.rows()); + // int curRow = 0; + // for (auto& v : D) { + // DStacked.segment(curRow, v.size()) = v; + // curRow += v.size(); + // } + // curRow = 0; + // for (auto& v : E) { + // EStacked.segment(curRow, v.size()) = v; + // curRow += v.size(); + // } + // if (!DStacked.isApprox(DTest)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "DStacked: " << DStacked.transpose() << "\n"; + // std::cerr << "DTest : " << DTest.transpose() << "\n"; + // std::cerr << "diff : " << (DStacked - DTest).transpose() << std::endl; + // std::cerr << "R: \n" << cost[0].dfduu << std::endl; + // std::cerr << "B: \n" << dynamics[0].dfdu << std::endl; + // } + // if (!EStacked.isApprox(ETest)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "EStacked: " << EStacked.transpose() << "\n"; + // std::cerr << "ETest: " << ETest.transpose() << "\n"; + // std::cerr << "diff: " << (ETest - EStacked).transpose() << std::endl; + // } + // if (!isEqual(infNormOfhTest, infNormOfh)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "infNormOfhTest: " << infNormOfhTest << "\n"; + // std::cerr << "infNormOfh: " << infNormOfh << "\n"; + // std::cerr << "diff: " << (infNormOfhTest - infNormOfh) << std::endl; + // } + // if (!isEqual(infNormOfHColsUpdated.sum(), sumOfInfNormOfH)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "infNormOfHColsUpdated.sum(): " << infNormOfHColsUpdated.sum() << "\n"; + // std::cerr << "sumOfInfNormOfH: " << sumOfInfNormOfH << "\n"; + // std::cerr << "diff: " << (infNormOfHColsUpdated.sum() - sumOfInfNormOfH) << std::endl; + // } + // if (!isEqual(c_temp, c_tempTest)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "c_temp: " << c_temp << "\n"; + // std::cerr << "c_tempTest: " << c_tempTest << "\n"; + // std::cerr << "diff: " << (c_temp - c_tempTest) << std::endl; + // } + // /** + // * Test end + // */ + } +} + +vector_t Pipg::HAbsRowSumInParallel(const std::vector<ScalarFunctionQuadraticApproximation>& cost) const { + const int N = ocpSize_.numStages; + const int nu_0 = ocpSize_.numInputs[0]; + vector_t res(getNumDecisionVariables()); + res.head(nu_0) = cost[0].dfduu.cwiseAbs().rowwise().sum(); + + int curRow = nu_0; + for (int k = 1; k < N; k++) { + const int nx_k = ocpSize_.numStates[k]; + const int nu_k = ocpSize_.numInputs[k]; + if (cost[k].dfdxx.size() != 0) { + res.segment(curRow, nx_k) = cost[k].dfdxx.cwiseAbs().rowwise().sum(); + } + if (cost[k].dfdux.size() != 0) { + res.segment(curRow, nx_k) += cost[k].dfdux.transpose().cwiseAbs().rowwise().sum(); + } + if (cost[k].dfdux.size() != 0) { + res.segment(curRow + nx_k, nu_k) = cost[k].dfdux.cwiseAbs().rowwise().sum(); + } + if (cost[k].dfduu.size() != 0) { + res.segment(curRow + nx_k, nu_k) += cost[k].dfduu.cwiseAbs().rowwise().sum(); + } + curRow += nx_k + nu_k; + } + const int nx_N = ocpSize_.numStates[N]; + res.tail(nx_N) = cost[N].dfdxx.cwiseAbs().rowwise().sum(); + return res; +} + +void Pipg::scaleDataInPlace(const vector_t& D, const vector_t& E, const scalar_t c, + std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors) const { + const int N = ocpSize_.numStages; + if (N < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + + scalingVectors.resize(N); + + /** + * Scale H & h + * + */ + const int nu_0 = ocpSize_.numInputs.front(); + // Scale row - Pre multiply D + cost.front().dfduu.array().colwise() *= c * D.head(nu_0).array(); + // Scale col - Post multiply D + cost.front().dfduu *= D.head(nu_0).asDiagonal(); + + cost.front().dfdu.array() *= c * D.head(nu_0).array(); + + cost.front().dfdux.array().colwise() *= c * D.head(nu_0).array(); + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize_.numStates[k]; + const int nu_k = ocpSize_.numInputs[k]; + auto& Q = cost[k].dfdxx; + auto& R = cost[k].dfduu; + auto& P = cost[k].dfdux; + auto& q = cost[k].dfdx; + auto& r = cost[k].dfdu; + + Q.array().colwise() *= c * D.segment(currRow, nx_k).array(); + Q *= D.segment(currRow, nx_k).asDiagonal(); + q.array() *= c * D.segment(currRow, nx_k).array(); + + P *= D.segment(currRow, nx_k).asDiagonal(); + currRow += nx_k; + + R.array().colwise() *= c * D.segment(currRow, nu_k).array(); + R *= D.segment(currRow, nu_k).asDiagonal(); + r.array() *= c * D.segment(currRow, nu_k).array(); + + P.array().colwise() *= c * D.segment(currRow, nu_k).array(); + + currRow += nu_k; + } + + const int nx_N = ocpSize_.numStates[N]; + cost[N].dfdxx.array().colwise() *= c * D.tail(nx_N).array(); + cost[N].dfdxx *= D.tail(nx_N).asDiagonal(); + cost[N].dfdx.array() *= c * D.tail(nx_N).array(); + + // Scale G & g + auto& B_0 = dynamics[0].dfdu; + auto& C_0 = scalingVectors[0]; + const int nx_1 = ocpSize_.numStates[1]; + /** + * \tilde{B} = E * B * D + * scaling = E * I * D + * \tilde{g} = E * g + */ + B_0.array().colwise() *= E.head(nx_1).array(); + B_0 *= D.head(nu_0).asDiagonal(); + + C_0 = E.head(nx_1).array() * D.segment(nu_0, nx_1).array(); + + dynamics[0].dfdx.array().colwise() *= E.head(nx_1).array(); + dynamics[0].f.array() *= E.head(nx_1).array(); + + currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const int nu_k = ocpSize_.numInputs[k]; + const int nx_k = ocpSize_.numStates[k]; + const int nx_next = ocpSize_.numStates[k + 1]; + auto& A_k = dynamics[k].dfdx; + auto& B_k = dynamics[k].dfdu; + auto& b_k = dynamics[k].f; + auto& C_k = scalingVectors[k]; + + A_k.array().colwise() *= E.segment(currRow, nx_next).array(); + A_k *= D.segment(currCol, nx_k).asDiagonal(); + + B_k.array().colwise() *= E.segment(currRow, nx_next).array(); + B_k *= D.segment(currCol + nx_k, nu_k).asDiagonal(); + + C_k = E.segment(currRow, nx_next).array() * D.segment(currCol + nx_k + nu_k, nx_next).array(); + + b_k.array() *= E.segment(currRow, nx_next).array(); + + currRow += nx_next; + currCol += nx_k + nu_k; + } +} + +void Pipg::resize(const OcpSize& ocpSize) { + if (ocpSize_ == ocpSize) { + return; + } + + ocpSize_ = ocpSize; + const int N = ocpSize_.numStages; + + startIndexArray_.resize(N); + startIndexArray_.front() = 0; + std::partial_sum(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end() - 1, startIndexArray_.begin() + 1); + + numDecisionVariables = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); + numDecisionVariables += std::accumulate(ocpSize_.numInputs.begin(), ocpSize_.numInputs.end(), 0); + numDynamicsConstraints = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); + + X_.resize(N + 1); + W_.resize(N); + V_.resize(N); + U_.resize(N); + XNew_.resize(N + 1); + UNew_.resize(N); + WNew_.resize(N); +} + +void Pipg::getStackedSolution(vector_t& res) const { + packSolution(X_, U_, res); +} + +void Pipg::unpackSolution(const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, + vector_array_t& uTrajectory) const { + const int N = ocpSize_.numStages; + xTrajectory.resize(N + 1); + uTrajectory.resize(N); + + xTrajectory.front() = x0; + + int curRow = 0; + for (int i = 0; i < N; i++) { + const auto nx = ocpSize_.numStates[i + 1]; + const auto nu = ocpSize_.numInputs[i]; + xTrajectory[i + 1] = stackedSolution.segment(curRow + nu, nx); + uTrajectory[i] = stackedSolution.segment(curRow, nu); + + curRow += nx + nu; + } +} + +void Pipg::packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) const { + stackedSolution.resize(getNumDecisionVariables()); + + int curRow = 0; + for (int i = 0; i < ocpSize_.numStages; i++) { + const auto nx = ocpSize_.numStates[i + 1]; + const auto nu = ocpSize_.numInputs[i]; + stackedSolution.segment(curRow, nx + nu) << uTrajectory[i], xTrajectory[i + 1]; + curRow += nx + nu; + } +} + +void Pipg::descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { + const int N = ocpSize_.numStages; + if (D.size() != xTrajectory.size() + uTrajectory.size() - 1) { + throw std::runtime_error("[PIPG]::descaleSolution - Size doesn't match."); + } + + for (int k = 0; k < N; k++) { + uTrajectory[k].array() *= D[2 * k].array(); + xTrajectory[k + 1].array() *= D[2 * k + 1].array(); + } +} + +void Pipg::getStateInputTrajectoriesSolution(vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { + xTrajectory.resize(X_.size()); + uTrajectory.resize(U_.size()); + + std::copy(X_.begin(), X_.end(), xTrajectory.begin()); + std::copy(U_.begin(), U_.end(), uTrajectory.begin()); +} + +void Pipg::runParallel(std::function<void(int)> taskFunction) { + threadPool_.runParallel(std::move(taskFunction), settings().nThreads); +} + +void Pipg::verifySizes(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints) const { + if (dynamics.size() != ocpSize_.numStages) { + throw std::runtime_error("[PIPG] Inconsistent size of dynamics: " + std::to_string(dynamics.size()) + " with " + + std::to_string(ocpSize_.numStages) + " number of stages."); + } + if (cost.size() != ocpSize_.numStages + 1) { + throw std::runtime_error("[PIPG] Inconsistent size of cost: " + std::to_string(cost.size()) + " with " + + std::to_string(ocpSize_.numStages + 1) + " nodes."); + } + if (constraints != nullptr) { + if (constraints->size() != ocpSize_.numStages + 1) { + throw std::runtime_error("[PIPG] Inconsistent size of constraints: " + std::to_string(constraints->size()) + " with " + + std::to_string(ocpSize_.numStages + 1) + " nodes."); + } + } +} + +void Pipg::getConstraintMatrix(const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + VectorFunctionLinearApproximation& res) const { + const int N = ocpSize_.numStages; + if (N < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { + throw std::runtime_error("[PIPG] The size of scalingVectors doesn't match the number of stage."); + } + + // Preallocate full constraint matrix + auto& G = res.dfdx; + auto& g = res.f; + if (constraints != nullptr) { + G.setZero(getNumDynamicsConstraints() + getNumGeneralEqualityConstraints(), getNumDecisionVariables()); + + } else { + G.setZero(getNumDynamicsConstraints(), getNumDecisionVariables()); + } + g.setZero(G.rows()); + + // Initial state constraint + const int nu_0 = ocpSize_.numInputs.front(); + const int nx_1 = ocpSize_.numStates[1]; + if (scalingVectorsPtr == nullptr) { + G.topLeftCorner(nx_1, nu_0 + nx_1) << -dynamics.front().dfdu, matrix_t::Identity(nx_1, nx_1); + } else { + G.topLeftCorner(nx_1, nu_0 + nx_1) << -dynamics.front().dfdu, scalingVectorsPtr->front().asDiagonal().toDenseMatrix(); + } + + // k = 0. Absorb initial state into dynamics + // The initial state is removed from the decision variables + // The first dynamics becomes: + // x[1] = A[0]*x[0] + B[0]*u[0] + b[0] + // = B[0]*u[0] + (b[0] + A[0]*x[0]) + // = B[0]*u[0] + \tilde{b}[0] + // numState[0] = 0 --> No need to specify A[0] here + g.head(nx_1) = dynamics.front().f; + g.head(nx_1).noalias() += dynamics.front().dfdx * x0; + + int currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const auto& dynamics_k = dynamics[k]; + // const auto& constraints_k = constraints; + const int nu_k = ocpSize_.numInputs[k]; + const int nx_k = ocpSize_.numStates[k]; + const int nx_next = ocpSize_.numStates[k + 1]; + + // Add [-A, -B, I(C)] + if (scalingVectorsPtr == nullptr) { + G.block(currRow, currCol, nx_next, nx_k + nu_k + nx_next) << -dynamics_k.dfdx, -dynamics_k.dfdu, matrix_t::Identity(nx_next, nx_next); + } else { + G.block(currRow, currCol, nx_next, nx_k + nu_k + nx_next) << -dynamics_k.dfdx, -dynamics_k.dfdu, + (*scalingVectorsPtr)[k].asDiagonal().toDenseMatrix(); + } + + // Add [b] + g.segment(currRow, nx_next) = dynamics_k.f; + + currRow += nx_next; + currCol += nx_k + nu_k; + } + + if (constraints != nullptr) { + currCol = nu_0; + // === Constraints === + // for ocs2 --> C*dx + D*du + e = 0 + // for pipg --> C*dx + D*du = -e + // Initial general constraints + const int nc_0 = ocpSize_.numIneqConstraints.front(); + if (nc_0 > 0) { + const auto& constraint_0 = (*constraints).front(); + G.block(currRow, 0, nc_0, nu_0) = constraint_0.dfdu; + g.segment(currRow, nc_0) = -constraint_0.f; + g.segment(currRow, nc_0) -= constraint_0.dfdx * x0; + currRow += nc_0; + } + + for (int k = 1; k < N; ++k) { + const int nc_k = ocpSize_.numIneqConstraints[k]; + const int nu_k = ocpSize_.numInputs[k]; + const int nx_k = ocpSize_.numStates[k]; + if (nc_k > 0) { + const auto& constraints_k = (*constraints)[k]; + + // Add [C, D, 0] + G.block(currRow, currCol, nc_k, nx_k + nu_k) << constraints_k.dfdx, constraints_k.dfdu; + // Add [-e] + g.segment(currRow, nc_k) = -constraints_k.f; + currRow += nc_k; + } + + currCol += nx_k + nu_k; + } + + // Final general constraint + const int nc_N = ocpSize_.numIneqConstraints[N]; + if (nc_N > 0) { + const auto& constraints_N = (*constraints)[N]; + G.bottomRightCorner(nc_N, constraints_N.dfdx.cols()) = constraints_N.dfdx; + g.tail(nc_N) = -constraints_N.f; + } + } +} + +void Pipg::getConstraintMatrixSparse(const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, + const vector_array_t* scalingVectorsPtr, Eigen::SparseMatrix<scalar_t>& G, vector_t& g) const { + const int N = ocpSize_.numStages; + if (N < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { + throw std::runtime_error("[PIPG] The size of scalingVectors doesn't match the number of stage."); + } + + const int nu_0 = ocpSize_.numInputs[0]; + const int nx_1 = ocpSize_.numStates[1]; + const int nx_N = ocpSize_.numStates[N]; + + int nnz = nx_1 * (nu_0 + nx_1); + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize_.numStates[k]; + const int nu_k = ocpSize_.numInputs[k]; + const int nx_next = ocpSize_.numStates[k + 1]; + nnz += nx_next * (nx_k + nu_k + nx_next); + } + + if (constraints != nullptr) { + const int nc_0 = ocpSize_.numIneqConstraints[0]; + nnz += nc_0 * nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize_.numStates[k]; + const int nu_k = ocpSize_.numInputs[k]; + const int nc_k = ocpSize_.numIneqConstraints[k]; + nnz += nc_k * (nx_k + nu_k); + } + const int nc_N = ocpSize_.numIneqConstraints[N]; + + nnz += nc_N * nx_N; + } + std::vector<Eigen::Triplet<scalar_t>> tripletList; + tripletList.reserve(nnz); + + auto emplaceBackMatrix = [&tripletList](const int startRow, const int startCol, const matrix_t& mat) { + for (int j = 0; j < mat.cols(); j++) { + for (int i = 0; i < mat.rows(); i++) { + if (mat(i, j) != 0) { + tripletList.emplace_back(startRow + i, startCol + j, mat(i, j)); + } + } + } + }; + + if (constraints != nullptr) { + G.resize(getNumDynamicsConstraints() + getNumGeneralEqualityConstraints(), getNumDecisionVariables()); + } else { + G.resize(getNumDynamicsConstraints(), getNumDecisionVariables()); + } + g.setZero(G.rows()); + + // Initial state constraint + emplaceBackMatrix(0, 0, -dynamics.front().dfdu); + if (scalingVectorsPtr == nullptr) { + emplaceBackMatrix(0, nu_0, matrix_t::Identity(nx_1, nx_1)); + } else { + emplaceBackMatrix(0, nu_0, scalingVectorsPtr->front().asDiagonal()); + } + + // k = 0. Absorb initial state into dynamics + // The initial state is removed from the decision variables + // The first dynamics becomes: + // x[1] = A[0]*x[0] + B[0]*u[0] + b[0] + // = B[0]*u[0] + (b[0] + A[0]*x[0]) + // = B[0]*u[0] + \tilde{b}[0] + // numState[0] = 0 --> No need to specify A[0] here + g.head(nx_1) = dynamics.front().f; + g.head(nx_1).noalias() += dynamics.front().dfdx * x0; + + int currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const auto& dynamics_k = dynamics[k]; + // const auto& constraints_k = constraints; + const int nx_k = ocpSize_.numStates[k]; + const int nu_k = ocpSize_.numInputs[k]; + const int nx_next = ocpSize_.numStates[k + 1]; + + // Add [-A, -B, I] + emplaceBackMatrix(currRow, currCol, -dynamics_k.dfdx); + emplaceBackMatrix(currRow, currCol + nx_k, -dynamics_k.dfdu); + if (scalingVectorsPtr == nullptr) { + emplaceBackMatrix(currRow, currCol + nx_k + nu_k, matrix_t::Identity(nx_next, nx_next)); + } else { + emplaceBackMatrix(currRow, currCol + nx_k + nu_k, (*scalingVectorsPtr)[k].asDiagonal()); + } + + // Add [b] + g.segment(currRow, nx_next) = dynamics_k.f; + + currRow += nx_next; + currCol += nx_k + nu_k; + } + + if (constraints != nullptr) { + currCol = nu_0; + // === Constraints === + // for ocs2 --> C*dx + D*du + e = 0 + // for pipg --> C*dx + D*du = -e + // Initial general constraints + const int nc_0 = ocpSize_.numIneqConstraints.front(); + if (nc_0 > 0) { + const auto& constraint_0 = (*constraints).front(); + emplaceBackMatrix(currRow, 0, constraint_0.dfdu); + + g.segment(currRow, nc_0) = -constraint_0.f; + g.segment(currRow, nc_0) -= constraint_0.dfdx * x0; + currRow += nc_0; + } + + for (int k = 1; k < N; ++k) { + const int nc_k = ocpSize_.numIneqConstraints[k]; + const int nu_k = ocpSize_.numInputs[k]; + const int nx_k = ocpSize_.numStates[k]; + if (nc_k > 0) { + const auto& constraints_k = (*constraints)[k]; + + // Add [C, D, 0] + emplaceBackMatrix(currRow, currCol, constraints_k.dfdx); + emplaceBackMatrix(currRow, currCol + nx_k, constraints_k.dfdu); + // Add [-e] + g.segment(currRow, nc_k) = -constraints_k.f; + currRow += nc_k; + } + + currCol += nx_k + nu_k; + } + + // Final general constraint + const int nc_N = ocpSize_.numIneqConstraints[N]; + if (nc_N > 0) { + const auto& constraints_N = (*constraints)[N]; + emplaceBackMatrix(currRow, currCol, constraints_N.dfdx); + g.segment(currRow, nc_N) = -constraints_N.f; + } + } + + G.setFromTriplets(tripletList.begin(), tripletList.end()); + assert(G.nonZeros() <= nnz); +} + +void Pipg::getCostMatrix(const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, + ScalarFunctionQuadraticApproximation& res) const { + const int N = ocpSize_.numStages; + + // Preallocate full Cost matrices + auto& H = res.dfdxx; + auto& h = res.dfdx; + auto& c = res.f; + H.setZero(getNumDecisionVariables(), getNumDecisionVariables()); + h.setZero(H.cols()); + c = 0.0; + + // k = 0. Elimination of initial state requires cost adaptation + vector_t r_0 = cost[0].dfdu; + r_0 += cost[0].dfdux * x0; + + const int nu_0 = ocpSize_.numInputs[0]; + H.topLeftCorner(nu_0, nu_0) = cost[0].dfduu; + h.head(nu_0) = r_0; + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize_.numStates[k]; + const int nu_k = ocpSize_.numInputs[k]; + + // Add [ Q, P' + // P, Q ] + H.block(currRow, currRow, nx_k + nu_k, nx_k + nu_k) << cost[k].dfdxx, cost[k].dfdux.transpose(), cost[k].dfdux, cost[k].dfduu; + + // Add [ q, r] + h.segment(currRow, nx_k + nu_k) << cost[k].dfdx, cost[k].dfdu; + + // Add nominal cost + c += cost[k].f; + + currRow += nx_k + nu_k; + } + + const int nx_N = ocpSize_.numStates[N]; + H.bottomRightCorner(nx_N, nx_N) = cost[N].dfdxx; + h.tail(nx_N) = cost[N].dfdx; + c += cost[N].f; +} + +void Pipg::getCostMatrixSparse(const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, + Eigen::SparseMatrix<scalar_t>& H, vector_t& h) const { + const int N = ocpSize_.numStages; + + const int nu_0 = ocpSize_.numInputs[0]; + int nnz = nu_0 * nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize_.numStates[k]; + const int nu_k = ocpSize_.numInputs[k]; + nnz += (nx_k + nu_k) * (nx_k + nu_k); + } + const int nx_N = ocpSize_.numStates[N]; + + nnz += nx_N * nx_N; + std::vector<Eigen::Triplet<scalar_t>> tripletList; + tripletList.reserve(nnz); + + auto emplaceBackMatrix = [&tripletList](const int startRow, const int startCol, const matrix_t& mat) { + for (int j = 0; j < mat.cols(); j++) { + for (int i = 0; i < mat.rows(); i++) { + if (mat(i, j) != 0) { + tripletList.emplace_back(startRow + i, startCol + j, mat(i, j)); + } + } + } + }; + + h.setZero(getNumDecisionVariables()); + + // k = 0. Elimination of initial state requires cost adaptation + vector_t r_0 = cost[0].dfdu; + r_0 += cost[0].dfdux * x0; + + // R0 + emplaceBackMatrix(0, 0, cost[0].dfduu); + h.head(nu_0) = r_0; + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize_.numStates[k]; + const int nu_k = ocpSize_.numInputs[k]; + + // Add [ Q, P' + // P, Q ] + emplaceBackMatrix(currRow, currRow, cost[k].dfdxx); + emplaceBackMatrix(currRow, currRow + nx_k, cost[k].dfdux.transpose()); + emplaceBackMatrix(currRow + nx_k, currRow, cost[k].dfdux); + emplaceBackMatrix(currRow + nx_k, currRow + nx_k, cost[k].dfduu); + + // Add [ q, r] + h.segment(currRow, nx_k + nu_k) << cost[k].dfdx, cost[k].dfdu; + + currRow += nx_k + nu_k; + } + + emplaceBackMatrix(currRow, currRow, cost[N].dfdxx); + h.tail(nx_N) = cost[N].dfdx; + + H.resize(getNumDecisionVariables(), getNumDecisionVariables()); + H.setFromTriplets(tripletList.begin(), tripletList.end()); + assert(H.nonZeros() <= nnz); +} + +void Pipg::GGTAbsRowSumInParallel(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, + const vector_array_t* scalingVectorsPtr, vector_t& res) { + const int N = ocpSize_.numStages; + if (N < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { + throw std::runtime_error("[PIPG] The size of scalingVectors doesn't match the number of stage."); + } + Eigen::setNbThreads(1); // No multithreading within Eigen. + + matrix_array_t tempMatrixArray(N); + vector_array_t absRowSumArray(N); + + std::atomic_int timeIndex{0}; + auto task = [&](int workerId) { + int k; + while ((k = timeIndex++) < N) { + const auto nx_next = ocpSize_.numStates[k + 1]; + const auto& B = dynamics[k].dfdu; + tempMatrixArray[k] = + (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_next, nx_next) + : (*scalingVectorsPtr)[k].cwiseProduct((*scalingVectorsPtr)[k]).asDiagonal().toDenseMatrix()); + tempMatrixArray[k] += B * B.transpose(); + + if (k != 0) { + const auto& A = dynamics[k].dfdx; + tempMatrixArray[k] += A * A.transpose(); + } + + absRowSumArray[k] = tempMatrixArray[k].cwiseAbs().rowwise().sum(); + + if (k != 0) { + const auto& A = dynamics[k].dfdx; + absRowSumArray[k] += (A * (scalingVectorsPtr == nullptr ? matrix_t::Identity(A.cols(), A.cols()) + : (*scalingVectorsPtr)[k - 1].asDiagonal().toDenseMatrix())) + .cwiseAbs() + .rowwise() + .sum(); + } + if (k != N - 1) { + const auto& ANext = dynamics[k + 1].dfdx; + absRowSumArray[k] += (ANext * (scalingVectorsPtr == nullptr ? matrix_t::Identity(ANext.cols(), ANext.cols()) + : (*scalingVectorsPtr)[k].asDiagonal().toDenseMatrix())) + .transpose() + .cwiseAbs() + .rowwise() + .sum(); + } + } + }; + runParallel(std::move(task)); + + res.setZero(getNumDynamicsConstraints()); + int curRow = 0; + for (const auto& v : absRowSumArray) { + res.segment(curRow, v.size()) = v; + curRow += v.size(); + } + + Eigen::setNbThreads(0); // Restore default setup. + + // /** + // * ********************************* + // * ************ Test begin ********* + // * ********************************* + // */ + // matrix_t m(getNumDynamicsConstraints(), getNumDynamicsConstraints()); + // const int nx_1 = ocpSize_.numStates[1]; + // const int nx_2 = ocpSize_.numStates[2]; + // const auto& A1 = dynamics[1].dfdx; + // m.topLeftCorner(nx_1, nx_1 + nx_2) << tempMatrixArray[0], + // -(A1 * (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_1, nx_1) : (*scalingVectorsPtr)[0].asDiagonal().toDenseMatrix())) + // .transpose(); + + // curRow = nx_1; + // for (int i = 1; i < N - 1; i++) { + // const int nx_i1 = ocpSize_.numStates[i + 1]; + // const int nx_i2 = ocpSize_.numStates[i + 2]; + // const auto& ANext = dynamics[i + 1].dfdx; + + // m.block(curRow, curRow, nx_i1, nx_i1 + nx_i2) << tempMatrixArray[i], + // -(ANext * (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_i1, nx_i1) : + // (*scalingVectorsPtr)[i].asDiagonal().toDenseMatrix())) + // .transpose(); + // curRow += nx_i1; + // } + // const int nx_N = ocpSize_.numStates[N]; + // m.block(curRow, curRow, nx_N, nx_N) = tempMatrixArray[N - 1]; + // std::cerr << "GGT: \n" << m.selfadjointView<Eigen::Upper>().toDenseMatrix() << "\n\n"; + // /** + // * ********************************* + // * ************ Test end ********* + // * ********************************* + // */ +} + +std::string Pipg::getBenchmarkingInformationDense() const { + const auto step1v = step1v_.getTotalInMilliseconds(); + const auto step2z = step2z_.getTotalInMilliseconds(); + const auto step3w = step3w_.getTotalInMilliseconds(); + const auto step4CheckConvergence = step4CheckConvergence_.getTotalInMilliseconds(); + + const auto benchmarkTotal = step1v + step2z + step3w + step4CheckConvergence; + + std::stringstream infoStream; + if (benchmarkTotal > 0.0) { + const scalar_t inPercent = 100.0; + infoStream << "\n########################################################################\n"; + infoStream << "The benchmarking is computed over " << step1v_.getNumTimedIntervals() << " iterations. \n"; + infoStream << "PIPG Dense Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "\tstep1v :\t" << step1v_.getAverageInMilliseconds() << " [ms] \t\t(" + << step1v / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tstep2z :\t" << step2z_.getAverageInMilliseconds() << " [ms] \t\t(" + << step2z / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tstep3w :\t" << step3w_.getAverageInMilliseconds() << " [ms] \t\t(" + << step3w / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tstep4CheckConvergence :\t" << step4CheckConvergence_.getAverageInMilliseconds() << " [ms] \t\t(" + << step4CheckConvergence / benchmarkTotal * inPercent << "%)\n"; + } + return infoStream.str(); +} + +Pipg::~Pipg() { + if (settings().displayShortSummary) { + std::cerr << getBenchmarkingInformationDense() << std::endl; + } +} + +vector_t matrixInfNormCols(const Eigen::SparseMatrix<scalar_t>& mat) { + vector_t infNorm; + infNorm.setZero(mat.cols()); + + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + infNorm(j) = std::max(infNorm(j), std::abs(it.value())); + } + } + return infNorm; +} + +vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat) { + vector_t infNorm; + infNorm.setZero(mat.rows()); + + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + int i = it.row(); + infNorm(i) = std::max(infNorm(i), std::abs(it.value())); + } + } + return infNorm; +} + +vector_t matrixRowwiseAbsSum(const Eigen::SparseMatrix<scalar_t>& mat) { + vector_t rowAbsSum; + rowAbsSum.setZero(mat.rows()); + + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + int i = it.row(); + rowAbsSum(i) += std::abs(it.value()); + } + } + return rowAbsSum; +} + +void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix<scalar_t>& mat) { + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + if (it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1) { + throw std::runtime_error("[scaleMatrixInPlace] it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1"); + } + it.valueRef() *= rowScale(it.row()) * colScale(j); + } + } +} + +} // namespace ocs2 diff --git a/ocs2_pipg/test/testPIPGSolver.cpp b/ocs2_pipg/test/testPIPGSolver.cpp new file mode 100644 index 000000000..a4c2edb3f --- /dev/null +++ b/ocs2_pipg/test/testPIPGSolver.cpp @@ -0,0 +1,308 @@ +#include <gtest/gtest.h> + +#include "ocs2_pipg/PIPG.h" + +#include <ocs2_qp_solver/QpSolver.h> +#include <ocs2_qp_solver/test/testProblemsGeneration.h> + +#include <Eigen/Sparse> + +class PIPGSolverTest : public testing::Test { + protected: + // x_0, x_1, ... x_{N - 1}, X_{N} + static constexpr size_t N_ = 5; // numStages + static constexpr size_t nx_ = 4; + static constexpr size_t nu_ = 3; + static constexpr size_t nc_ = 0; + static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); + static constexpr size_t numConstraints = N_ * (nx_ + nc_); + static constexpr bool verbose = true; + + PIPGSolverTest() + : pipgSolver({ + 8, // nThreads + 50, // threadPriority + 30000, // maxNumIterations + 1e-10, // absoluteTolerance + 1e-3, // relativeTolerance + 1, // checkTerminationInterval + verbose, // displayShortSummary + }) { + srand(10); + + // Construct OCP problem + lqProblem = ocs2::qp_solver::generateRandomLqProblem(N_, nx_, nu_, nc_); + x0 = ocs2::vector_t::Random(nx_); + + for (int i = 0; i < N_; i++) { + dynamicsArray.push_back(lqProblem[i].dynamics); + costArray.push_back(lqProblem[i].cost); + constraintsArray.push_back(lqProblem[i].constraints); + } + costArray.push_back(lqProblem[N_].cost); + constraintsArray.push_back(lqProblem[N_].constraints); + + pipgSolver.resize(ocs2::pipg::extractSizesFromProblem(dynamicsArray, costArray, &constraintsArray)); + pipgSolver.getCostMatrix(x0, costArray, costApproximation); + pipgSolver.getConstraintMatrix(x0, dynamicsArray, nullptr, nullptr, constraintsApproximation); + } + + std::vector<ocs2::qp_solver::LinearQuadraticStage> lqProblem; + ocs2::vector_t x0; + ocs2::ScalarFunctionQuadraticApproximation costApproximation; + ocs2::VectorFunctionLinearApproximation constraintsApproximation; + std::vector<ocs2::VectorFunctionLinearApproximation> dynamicsArray; + std::vector<ocs2::ScalarFunctionQuadraticApproximation> costArray; + std::vector<ocs2::VectorFunctionLinearApproximation> constraintsArray; + + ocs2::Pipg pipgSolver; +}; + +constexpr size_t PIPGSolverTest::numDecisionVariables; +constexpr size_t PIPGSolverTest::numConstraints; + +TEST_F(PIPGSolverTest, PIPGcorrectness) { + // SolveDenseQP use Gz + g = 0 for constraints + auto QPconstraints = constraintsApproximation; + QPconstraints.f = -QPconstraints.f; + ocs2::vector_t primalSolutionQP; + std::tie(primalSolutionQP, std::ignore) = ocs2::qp_solver::solveDenseQp(costApproximation, QPconstraints); + // primalSolutionQP.setZero(numDecisionVariables); + + Eigen::JacobiSVD<ocs2::matrix_t> svd(costApproximation.dfdxx); + ocs2::vector_t s = svd.singularValues(); + ocs2::scalar_t lambda = s(0); + ocs2::scalar_t mu = s(svd.rank() - 1); + Eigen::JacobiSVD<ocs2::matrix_t> svdGTG(constraintsApproximation.dfdx.transpose() * constraintsApproximation.dfdx); + ocs2::scalar_t sigma = svdGTG.singularValues()(0); + + ocs2::vector_t primalSolutionPIPG; + pipgSolver.solveDenseQP(costApproximation, constraintsApproximation, ocs2::vector_t::Ones(pipgSolver.getNumDynamicsConstraints()), mu, + lambda, sigma, primalSolutionPIPG); + // primalSolutionPIPG.setZero(numDecisionVariables); + + ocs2::vector_t primalSolutionPIPGParallel; + ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); + + pipgSolver.solveOCPInParallel(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma, costApproximation, + constraintsApproximation); + pipgSolver.getStackedSolution(primalSolutionPIPGParallel); + // primalSolutionPIPGParallel.setZero(numDecisionVariables); + + auto calculateConstraintViolation = [&](const ocs2::vector_t& sol) -> ocs2::scalar_t { + return (constraintsApproximation.dfdx * sol - constraintsApproximation.f).cwiseAbs().maxCoeff(); + }; + auto calculateCost = [&](const ocs2::vector_t& sol) -> ocs2::scalar_t { + return (0.5 * sol.transpose() * costApproximation.dfdxx * sol + costApproximation.dfdx.transpose() * sol)(0); + }; + ocs2::scalar_t QPCost = calculateCost(primalSolutionQP); + ocs2::scalar_t QPConstraintViolation = calculateConstraintViolation(primalSolutionQP); + + ocs2::scalar_t PIPGCost = calculateCost(primalSolutionPIPG); + ocs2::scalar_t PIPGConstraintViolation = calculateConstraintViolation(primalSolutionPIPG); + + ocs2::scalar_t PIPGParallelCost = calculateCost(primalSolutionPIPGParallel); + ocs2::scalar_t PIPGParallelCConstraintViolation = calculateConstraintViolation(primalSolutionPIPGParallel); + + if (verbose) { + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n++++++++++++++ [TestPIPG] Correctness ++++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + + std::cerr << "mu: " << mu << " lambda: " << lambda << " sigma: " << sigma << "\n"; + + std::cerr << "QP-PIPG: " << (primalSolutionQP - primalSolutionPIPG).cwiseAbs().sum() << "\n"; + std::cerr << "PIPG-PIPGParallel: " << (primalSolutionPIPG - primalSolutionPIPGParallel).cwiseAbs().sum() << "\n"; + + std::cerr << "QP:\n"; + std::cerr << "cost: " << QPCost << " " + << "constraint-violation: " << QPConstraintViolation << "\n\n"; + + std::cerr << "PIPG:\n"; + std::cerr << "cost: " << PIPGCost << " " + << "constraint-violation: " << PIPGConstraintViolation << "\n\n"; + + std::cerr << "PIPGParallel:\n"; + std::cerr << "cost: " << PIPGParallelCost << " " + << "constraint-violation: " << PIPGParallelCConstraintViolation << "\n\n" + << std::endl; + } + EXPECT_TRUE(primalSolutionQP.isApprox(primalSolutionPIPG, pipgSolver.settings().absoluteTolerance * 10.0)) + << "Inf-norm of (QP - PIPG): " << (primalSolutionQP - primalSolutionPIPG).cwiseAbs().maxCoeff(); + + EXPECT_TRUE(primalSolutionPIPGParallel.isApprox(primalSolutionPIPG, pipgSolver.settings().absoluteTolerance * 10.0)) + << "Inf-norm of (PIPG - PIPGParallel): " << (primalSolutionPIPGParallel - primalSolutionPIPG).cwiseAbs().maxCoeff(); + + // Threshold is the (absoluteTolerance) * (2-Norm of the hessian H)[lambda] + EXPECT_TRUE(std::abs(QPCost - PIPGCost) < pipgSolver.settings().absoluteTolerance * lambda) + << "Absolute diff is [" << std::abs(QPCost - PIPGCost) << "] which is larger than [" + << pipgSolver.settings().absoluteTolerance * lambda << "]"; + + EXPECT_TRUE(std::abs(PIPGParallelCost - PIPGCost) < pipgSolver.settings().absoluteTolerance) + << "Absolute diff is [" << std::abs(PIPGParallelCost - PIPGCost) << "] which is larger than [" + << pipgSolver.settings().absoluteTolerance * lambda << "]"; + + ASSERT_TRUE(std::abs(PIPGConstraintViolation) < pipgSolver.settings().absoluteTolerance); + EXPECT_TRUE(std::abs(QPConstraintViolation - PIPGConstraintViolation) < pipgSolver.settings().absoluteTolerance * 10.0); + EXPECT_TRUE(std::abs(PIPGParallelCConstraintViolation - PIPGConstraintViolation) < pipgSolver.settings().absoluteTolerance * 10.0); +} + +TEST_F(PIPGSolverTest, preConditioning) { + auto costScaledWithFactors = costApproximation; + auto constraintScaledWithFactors = constraintsApproximation; + ocs2::vector_t D, E; + ocs2::scalar_t c; + Eigen::SparseMatrix<ocs2::scalar_t> HScaledFromCalculation = costApproximation.dfdxx.sparseView(), + GScaledFromCalculation = constraintsApproximation.dfdx.sparseView(); + ocs2::vector_t hScaledFromCalculation = costApproximation.dfdx; + + pipgSolver.calculatePreConditioningFactors(HScaledFromCalculation, hScaledFromCalculation, GScaledFromCalculation, 5, D, E, c); + + costScaledWithFactors.dfdxx = c * D.asDiagonal() * costScaledWithFactors.dfdxx * D.asDiagonal(); + costScaledWithFactors.dfdx = c * D.asDiagonal() * costScaledWithFactors.dfdx; + constraintScaledWithFactors.dfdx = E.asDiagonal() * constraintScaledWithFactors.dfdx * D.asDiagonal(); + constraintScaledWithFactors.f = E.asDiagonal() * constraintScaledWithFactors.f; + + // The scaled matrix given by calculatePreConditioningFactors function and the one calculated with D, E and c factors should be the same. + EXPECT_TRUE(costScaledWithFactors.dfdxx.isApprox(HScaledFromCalculation.toDense())); // H + EXPECT_TRUE(costScaledWithFactors.dfdx.isApprox(hScaledFromCalculation)); // h + EXPECT_TRUE(constraintScaledWithFactors.dfdx.isApprox(GScaledFromCalculation.toDense())); // G + + // Normalized the inf-Norm of both rows and cols of KKT matrix to 1 + ocs2::matrix_t KKT(costApproximation.dfdxx.rows() + constraintsApproximation.dfdx.rows(), + costApproximation.dfdxx.rows() + constraintsApproximation.dfdx.rows()); + KKT << costApproximation.dfdxx, constraintsApproximation.dfdx.transpose(), constraintsApproximation.dfdx, + ocs2::matrix_t::Zero(constraintsApproximation.dfdx.rows(), constraintsApproximation.dfdx.rows()); + + ocs2::matrix_t KKTScaled(costApproximation.dfdxx.rows() + constraintsApproximation.dfdx.rows(), + costApproximation.dfdxx.rows() + constraintsApproximation.dfdx.rows()); + KKTScaled << costScaledWithFactors.dfdxx, constraintScaledWithFactors.dfdx.transpose(), constraintScaledWithFactors.dfdx, + ocs2::matrix_t::Zero(constraintsApproximation.dfdx.rows(), constraintsApproximation.dfdx.rows()); + + ocs2::vector_t infNormOfKKT = KKT.cwiseAbs().rowwise().maxCoeff(); + ocs2::vector_t infNormOfKKTScaled = KKTScaled.cwiseAbs().rowwise().maxCoeff(); + EXPECT_LT((infNormOfKKTScaled.array() - 1).abs().maxCoeff(), (infNormOfKKT.array() - 1).abs().maxCoeff()); + + std::vector<ocs2::vector_t> scalingVectors; + pipgSolver.scaleDataInPlace(D, E, c, dynamicsArray, costArray, scalingVectors); + + ocs2::ScalarFunctionQuadraticApproximation costConstructedFromScaledDataArray; + ocs2::VectorFunctionLinearApproximation constraintConstructedFromScaledDataArray; + pipgSolver.getCostMatrix(x0, costArray, costConstructedFromScaledDataArray); + pipgSolver.getConstraintMatrix(x0, dynamicsArray, nullptr, &scalingVectors, constraintConstructedFromScaledDataArray); + + EXPECT_TRUE(costScaledWithFactors.dfdxx.isApprox(costConstructedFromScaledDataArray.dfdxx)); // H + EXPECT_TRUE(costScaledWithFactors.dfdx.isApprox(costConstructedFromScaledDataArray.dfdx)); // h + EXPECT_TRUE(constraintScaledWithFactors.dfdx.isApprox(constraintConstructedFromScaledDataArray.dfdx)); // G + EXPECT_TRUE(constraintScaledWithFactors.f.isApprox(constraintConstructedFromScaledDataArray.f)); // g +} + +TEST_F(PIPGSolverTest, preConditioningSimplified) { + auto costScaledWithFactors = costApproximation; + auto constraintScaledWithFactors = constraintsApproximation; + ocs2::vector_t D, E; + ocs2::scalar_t c; + Eigen::SparseMatrix<ocs2::scalar_t> HScaledFromCalculation = costApproximation.dfdxx.sparseView(), + GScaledFromCalculation = constraintsApproximation.dfdx.sparseView(); + ocs2::vector_t hScaledFromCalculation = costApproximation.dfdx; + + pipgSolver.calculatePreConditioningFactors(HScaledFromCalculation, hScaledFromCalculation, GScaledFromCalculation, 5, D, E, c); + + ocs2::vector_array_t DArray, EArray; + ocs2::scalar_t cMy; + ocs2::vector_array_t scalingVectors(N_); + pipgSolver.preConditioningInPlaceInParallel(x0, dynamicsArray, costArray, 5, DArray, EArray, scalingVectors, cMy, + costApproximation.dfdxx.sparseView(), costApproximation.dfdx, + constraintsApproximation.dfdx.sparseView()); + + ocs2::vector_t DStacked(pipgSolver.getNumDecisionVariables()), EStacked(pipgSolver.getNumDynamicsConstraints()); + int curRow = 0; + for (auto& v : DArray) { + DStacked.segment(curRow, v.size()) = v; + curRow += v.size(); + } + curRow = 0; + for (auto& v : EArray) { + EStacked.segment(curRow, v.size()) = v; + curRow += v.size(); + } + + // The scaled matrix given by calculatePreConditioningFactors function and the one calculated with D, E and c factors should be the + // same. + EXPECT_TRUE(DStacked.isApprox(D)); + EXPECT_TRUE(EStacked.isApprox(E)); + EXPECT_FLOAT_EQ(c, cMy); + + ocs2::ScalarFunctionQuadraticApproximation costConstructedFromScaledDataArray; + ocs2::VectorFunctionLinearApproximation constraintConstructedFromScaledDataArray; + ocs2::vector_t gScaledFromCalculation = E.cwiseProduct(constraintsApproximation.f); + pipgSolver.getCostMatrix(x0, costArray, costConstructedFromScaledDataArray); + pipgSolver.getConstraintMatrix(x0, dynamicsArray, nullptr, &scalingVectors, constraintConstructedFromScaledDataArray); + + EXPECT_TRUE(HScaledFromCalculation.isApprox(costConstructedFromScaledDataArray.dfdxx)); // H + EXPECT_TRUE(hScaledFromCalculation.isApprox(costConstructedFromScaledDataArray.dfdx)); // h + EXPECT_TRUE(GScaledFromCalculation.isApprox(constraintConstructedFromScaledDataArray.dfdx)); // G + EXPECT_TRUE(gScaledFromCalculation.isApprox(constraintConstructedFromScaledDataArray.f)); // g +} + +TEST_F(PIPGSolverTest, constructSparseCostApproximation) { + Eigen::SparseMatrix<ocs2::scalar_t> H; + ocs2::vector_t h; + pipgSolver.getCostMatrixSparse(x0, costArray, H, h); + + EXPECT_TRUE(costApproximation.dfdxx.isApprox(H.toDense())); + EXPECT_TRUE(costApproximation.dfdx.isApprox(h)); +} + +TEST_F(PIPGSolverTest, HAbsRowSumInParallel) { + ocs2::vector_t rowwiseSum = pipgSolver.HAbsRowSumInParallel(costArray); + + EXPECT_TRUE(rowwiseSum.isApprox(costApproximation.dfdxx.cwiseAbs().rowwise().sum())); +} + +TEST_F(PIPGSolverTest, descaleSolution) { + ocs2::vector_array_t D(2 * N_); + ocs2::vector_t DStacked(numDecisionVariables); + ocs2::vector_array_t x(N_ + 1), u(N_); + x[0].setRandom(nx_); + for (int i = 0; i < N_; i++) { + D[2 * i].setRandom(nu_); + D[2 * i + 1].setRandom(nx_); + u[i].setRandom(nu_); + x[i + 1].setRandom(nx_); + } + int curRow = 0; + for (auto& v : D) { + DStacked.segment(curRow, v.size()) = v; + curRow += v.size(); + } + std::cerr << "DStacked:\n" << DStacked.transpose() << "\n\n"; + ocs2::vector_t packedSolution; + pipgSolver.packSolution(x, u, packedSolution); + std::cerr << "packedSolution:\n" << packedSolution.transpose() << "\n\n"; + + packedSolution.array() *= DStacked.array(); + + ocs2::vector_t packedSolutionMy; + pipgSolver.descaleSolution(D, x, u); + pipgSolver.packSolution(x, u, packedSolutionMy); + EXPECT_TRUE(packedSolutionMy.isApprox(packedSolution)) << std::setprecision(6) << "DescaledSolution: \n" + << packedSolutionMy.transpose() << "\nIt should be \n" + << packedSolution.transpose(); +} + +TEST_F(PIPGSolverTest, constructSparseConstraintsApproximation) { + Eigen::SparseMatrix<ocs2::scalar_t> G; + ocs2::vector_t g; + ocs2::vector_array_t scalingVectors(N_); + for (auto& v : scalingVectors) { + v = ocs2::vector_t::Random(nx_); + } + ocs2::VectorFunctionLinearApproximation constraintsApproximation; + pipgSolver.getConstraintMatrix(x0, dynamicsArray, &constraintsArray, &scalingVectors, constraintsApproximation); + + pipgSolver.getConstraintMatrixSparse(x0, dynamicsArray, &constraintsArray, &scalingVectors, G, g); + + EXPECT_TRUE(constraintsApproximation.dfdx.isApprox(G.toDense())); + EXPECT_TRUE(constraintsApproximation.f.isApprox(g)); +} \ No newline at end of file From 5bb7f4da44a7db1da48ad58d808a2ecc6851aed2 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Apr 2022 16:36:11 +0200 Subject: [PATCH 207/512] load config from yaml file --- .../python/ocs2_ballbot_mpcnet/config.py | 87 ------- .../ocs2_ballbot_mpcnet/config/ballbot.yaml | 122 +++++++++ .../python/ocs2_ballbot_mpcnet/helper.py | 74 +++--- .../python/ocs2_ballbot_mpcnet/mpcnet.py | 78 +++--- .../python/ocs2_legged_robot_mpcnet/config.py | 145 ----------- .../config/legged_robot.yaml | 239 ++++++++++++++++++ .../config/legged_robot_laptop.yaml | 239 ++++++++++++++++++ .../python/ocs2_legged_robot_mpcnet/helper.py | 126 +++++---- .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 113 +++------ .../python/ocs2_mpcnet_core/config.py | 44 +++- .../loss/behavioral_cloning.py | 6 +- .../ocs2_mpcnet_core/loss/cross_entropy.py | 6 +- .../ocs2_mpcnet_core/memory/circular.py | 57 ++--- .../python/ocs2_mpcnet_core/policy/linear.py | 21 +- .../policy/mixture_of_linear_experts.py | 25 +- .../policy/mixture_of_nonlinear_experts.py | 29 +-- .../ocs2_mpcnet_core/policy/nonlinear.py | 23 +- ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt | 1 + 18 files changed, 884 insertions(+), 551 deletions(-) delete mode 100644 ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py create mode 100644 ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml delete mode 100644 ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py create mode 100644 ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml create mode 100644 ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot_laptop.yaml diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py deleted file mode 100644 index e61ffaea5..000000000 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config.py +++ /dev/null @@ -1,87 +0,0 @@ -############################################################################### -# Copyright (c) 2022, Farbod Farshidian. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -############################################################################### - -"""Ballbot configuration variables. - -Sets robot-specific configuration variables for ballbot. -""" - -from ocs2_mpcnet_core import config - -# -# config -# - -# data type for tensor elements -DTYPE = config.DTYPE - -# device on which tensors will be allocated -DEVICE = config.DEVICE - -# -# ballbot_config -# - -# name of the robot -NAME = "ballbot" - -# (generalized) time dimension -TIME_DIM = 1 - -# state dimension -STATE_DIM = 10 - -# input dimension -INPUT_DIM = 3 - -# target trajectories state dimension -TARGET_STATE_DIM = STATE_DIM - -# target trajectories input dimension -TARGET_INPUT_DIM = INPUT_DIM - -# observation dimension -OBSERVATION_DIM = STATE_DIM - -# action dimension -ACTION_DIM = INPUT_DIM - -# observation scaling -# fmt: off -OBSERVATION_SCALING = [ - 1.0, 1.0, 1.0, 1.0, 1.0, # pose - 1.0, 1.0, 1.0, 1.0, 1.0 # twist -] -# fmt: on - -# action scaling -ACTION_SCALING = [1.0, 1.0, 1.0] - -# input cost for behavioral cloning -R = [2.0, 2.0, 2.0] diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml new file mode 100644 index 000000000..17a645088 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml @@ -0,0 +1,122 @@ +config: + # + # general + # + # name of the robot + NAME: "ballbot" + # description of the training run + DESCRIPTION: "description" + # state dimension + STATE_DIM: 10 + # input dimension + INPUT_DIM: 3 + # target trajectories state dimension + TARGET_STATE_DIM: 10 + # target trajectories input dimension + TARGET_INPUT_DIM: 3 + # observation dimension + OBSERVATION_DIM: 10 + # action dimension + ACTION_DIM: 3 + # expert number + EXPERT_NUM: 1 + # default state + DEFAULT_STATE: + - 0.0 # pose x + - 0.0 # pose y + - 0.0 # pose yaw + - 0.0 # pose pitch + - 0.0 # pose roll + - 0.0 # twist x + - 0.0 # twist y + - 0.0 # twist yaw + - 0.0 # twist pitch + - 0.0 # twist roll + # default target state + DEFAULT_TARGET_STATE: + - 0.0 # pose x + - 0.0 # pose y + - 0.0 # pose yaw + - 0.0 # pose pitch + - 0.0 # pose roll + - 0.0 # twist x + - 0.0 # twist y + - 0.0 # twist yaw + - 0.0 # twist pitch + - 0.0 # twist roll + # + # loss + # + # epsilon to improve numerical stability of logs and denominators + EPSILON: 1.e-8 + # whether to cheat by adding the gating loss + CHEATING: False + # parameter to control the relative importance of both loss types + LAMBDA: 1.0 + # dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) + EXPERT_FOR_MODE: + 0: 0 + # input cost for behavioral cloning + R: + - 2.0 # torque + - 2.0 # torque + - 2.0 # torque + # + # memory + # + # capacity of the memory + CAPACITY: 100000 + # + # policy + # + # observation scaling + OBSERVATION_SCALING: + - 1.0 # pose x + - 1.0 # pose y + - 1.0 # pose yaw + - 1.0 # pose pitch + - 1.0 # pose roll + - 1.0 # twist x + - 1.0 # twist y + - 1.0 # twist yaw + - 1.0 # twist pitch + - 1.0 # twist roll + # action scaling + ACTION_SCALING: + - 1.0 # torque + - 1.0 # torque + - 1.0 # torque + # + # rollout + # + # RaiSim or TimeTriggered rollout for data generation and policy evaluation + RAISIM: False + # settings for data generation + DATA_GENERATION_TIME_STEP: 0.1 + DATA_GENERATION_DURATION: 3.0 + DATA_GENERATION_DATA_DECIMATION: 1 + DATA_GENERATION_THREADS: 2 + DATA_GENERATION_TASKS: 10 + DATA_GENERATION_SAMPLES: 2 + DATA_GENERATION_SAMPLING_VARIANCE: + - 0.01 # pose x + - 0.01 # pose y + - 0.01745329251 # pose yaw: 1.0 / 180.0 * pi + - 0.01745329251 # pose pitch: 1.0 / 180.0 * pi + - 0.01745329251 # pose roll: 1.0 / 180.0 * pi + - 0.05 # twist x + - 0.05 # twist y + - 0.08726646259 # twist yaw: 5.0 / 180.0 * pi + - 0.08726646259 # twist pitch: 5.0 / 180.0 * pi + - 0.08726646259 # twist roll: 5.0 / 180.0 * pi + # settings for computing metrics + POLICY_EVALUATION_TIME_STEP: 0.1 + POLICY_EVALUATION_DURATION: 3.0 + POLICY_EVALUATION_THREADS: 1 + POLICY_EVALUATION_TASKS: 5 + # + # training + # + BATCH_SIZE: 32 + LEARNING_RATE: 1.e-2 + LEARNING_ITERATIONS: 10000 diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py index 42fe99bec..803c67e9a 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py @@ -35,11 +35,10 @@ import numpy as np from typing import Tuple +from ocs2_mpcnet_core import config from ocs2_mpcnet_core import helper from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray -from ocs2_ballbot_mpcnet import config - def get_default_event_times_and_mode_sequence(duration: float) -> Tuple[np.ndarray, np.ndarray]: """Get the event times and mode sequence describing the default mode schedule. @@ -59,55 +58,64 @@ def get_default_event_times_and_mode_sequence(duration: float) -> Tuple[np.ndarr return helper.get_event_times_and_mode_sequence(0, duration, event_times_template, mode_sequence_template) -def get_random_initial_state() -> np.ndarray: +def get_random_initial_state(state_dimension: int, default_state: [float]) -> np.ndarray: """Get a random initial state. Samples a random initial state for the robot. + Args: + state_dimension: The dimension of the state given by an integer. + default_state: The default state given by a Python array containing floats. + Returns: - x: A random initial state given by a NumPy array of shape (X) containing floats. + x: A random initial state given by a NumPy array containing floats. """ max_linear_velocity_x = 0.5 max_linear_velocity_y = 0.5 - max_euler_angle_derivative_z = 45.0 * np.pi / 180.0 - max_euler_angle_derivative_y = 45.0 * np.pi / 180.0 - max_euler_angle_derivative_x = 45.0 * np.pi / 180.0 - random_state = np.zeros(config.STATE_DIM) - random_state[5] = np.random.uniform(-max_linear_velocity_x, max_linear_velocity_x) - random_state[6] = np.random.uniform(-max_linear_velocity_y, max_linear_velocity_y) - random_state[7] = np.random.uniform(-max_euler_angle_derivative_z, max_euler_angle_derivative_z) - random_state[8] = np.random.uniform(-max_euler_angle_derivative_y, max_euler_angle_derivative_y) - random_state[9] = np.random.uniform(-max_euler_angle_derivative_x, max_euler_angle_derivative_x) - return random_state - - -def get_random_target_state() -> np.ndarray: + max_euler_angle_derivative_z = 45.0 / 180.0 * np.pi + max_euler_angle_derivative_y = 45.0 / 180.0 * np.pi + max_euler_angle_derivative_x = 45.0 / 180.0 * np.pi + random_deviation = np.zeros(state_dimension) + random_deviation[5] = np.random.uniform(-max_linear_velocity_x, max_linear_velocity_x) + random_deviation[6] = np.random.uniform(-max_linear_velocity_y, max_linear_velocity_y) + random_deviation[7] = np.random.uniform(-max_euler_angle_derivative_z, max_euler_angle_derivative_z) + random_deviation[8] = np.random.uniform(-max_euler_angle_derivative_y, max_euler_angle_derivative_y) + random_deviation[9] = np.random.uniform(-max_euler_angle_derivative_x, max_euler_angle_derivative_x) + return np.array(default_state) + random_deviation + + +def get_random_target_state(target_state_dimension: int, default_target_state: [float]) -> np.ndarray: """Get a random target state. Samples a random target state for the robot. + Args: + target_state_dimension: The dimension of the target state given by an integer. + default_target_state: The default target state given by a Python array containing floats. + Returns: - x: A random target state given by a NumPy array of shape (X) containing floats. + x: A random target state given by a NumPy array containing floats. """ max_position_x = 1.0 max_position_y = 1.0 - max_orientation_z = 45.0 * np.pi / 180.0 - random_state = np.zeros(config.TARGET_STATE_DIM) - random_state[0] = np.random.uniform(-max_position_x, max_position_x) - random_state[1] = np.random.uniform(-max_position_y, max_position_y) - random_state[2] = np.random.uniform(-max_orientation_z, max_orientation_z) - return random_state + max_orientation_z = 45.0 / 180.0 * np.pi + random_deviation = np.zeros(target_state_dimension) + random_deviation[0] = np.random.uniform(-max_position_x, max_position_x) + random_deviation[1] = np.random.uniform(-max_position_y, max_position_y) + random_deviation[2] = np.random.uniform(-max_orientation_z, max_orientation_z) + return np.array(default_target_state) + random_deviation def get_tasks( - n_tasks: int, duration: float + config: config.Config, tasks_number: int, duration: float ) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: """Get tasks. Get a random set of task that should be executed by the data generation or policy evaluation. Args: - n_tasks: Number of tasks given by an integer. + config: An instance of the configuration class. + tasks_number: Number of tasks given by an integer. duration: Duration of each task given by a float. Returns: @@ -116,17 +124,19 @@ def get_tasks( - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. """ - initial_observations = helper.get_system_observation_array(n_tasks) - mode_schedules = helper.get_mode_schedule_array(n_tasks) - target_trajectories = helper.get_target_trajectories_array(n_tasks) - for i in range(n_tasks): + initial_observations = helper.get_system_observation_array(tasks_number) + mode_schedules = helper.get_mode_schedule_array(tasks_number) + target_trajectories = helper.get_target_trajectories_array(tasks_number) + for i in range(tasks_number): initial_observations[i] = helper.get_system_observation( - 0, 0.0, get_random_initial_state(), np.zeros(config.INPUT_DIM) + 0, 0.0, get_random_initial_state(config.STATE_DIM, config.DEFAULT_STATE), np.zeros(config.INPUT_DIM) ) mode_schedules[i] = helper.get_mode_schedule(*get_default_event_times_and_mode_sequence(duration)) target_trajectories[i] = helper.get_target_trajectories( duration * np.ones((1, 1)), - get_random_target_state().reshape((1, config.TARGET_STATE_DIM)), + get_random_target_state(config.TARGET_STATE_DIM, config.DEFAULT_TARGET_STATE).reshape( + (1, config.TARGET_STATE_DIM) + ), np.zeros((1, config.TARGET_INPUT_DIM)), ) return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index 60c44014e..d6b5ff1ff 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -35,6 +35,7 @@ """ import os +import sys import time import datetime import torch @@ -42,49 +43,25 @@ from torch.utils.tensorboard import SummaryWriter +from ocs2_mpcnet_core.config import Config from ocs2_mpcnet_core.helper import bmv, bmm from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss as Loss from ocs2_mpcnet_core.memory.circular import CircularMemory as Memory from ocs2_mpcnet_core.policy.linear import LinearPolicy as Policy -from ocs2_ballbot_mpcnet import config from ocs2_ballbot_mpcnet import helper from ocs2_ballbot_mpcnet import MpcnetInterface -def main(): - # settings for data generation by applying behavioral policy - data_generation_time_step = 0.1 - data_generation_duration = 3.0 - data_generation_data_decimation = 1 - data_generation_n_threads = 2 - data_generation_n_tasks = 10 - data_generation_n_samples = 2 - data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order="F") - for i in range(0, 2): - data_generation_sampling_covariance[i, i] = 0.01**2 # position - for i in range(2, 5): - data_generation_sampling_covariance[i, i] = (1.0 * np.pi / 180.0) ** 2 # orientation - for i in range(5, 7): - data_generation_sampling_covariance[i, i] = 0.05**2 # linear velocity - for i in range(7, 10): - data_generation_sampling_covariance[i, i] = (5.0 * np.pi / 180.0) ** 2 # angular velocity - - # settings for computing metrics by applying learned policy - policy_evaluation_time_step = 0.1 - policy_evaluation_duration = 3.0 - policy_evaluation_n_threads = 1 - policy_evaluation_n_tasks = 5 - - # rollout settings for data generation and policy evaluation - raisim = False +def main(config_file_path: str) -> None: + # config + config = Config(config_file_path) # mpcnet interface - mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads, raisim) + mpcnet_interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) # logging - description = "description" - folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + description + folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + config.DESCRIPTION writer = SummaryWriter("runs/" + folder) os.makedirs(name="policies/" + folder) @@ -92,11 +69,10 @@ def main(): loss = Loss() # memory - memory_capacity = 100000 - memory = Memory(memory_capacity, config.STATE_DIM, config.INPUT_DIM, config.OBSERVATION_DIM, config.ACTION_DIM) + memory = Memory(config) # policy - policy = Policy(config.OBSERVATION_DIM, config.ACTION_DIM, config.OBSERVATION_SCALING, config.ACTION_SCALING) + policy = Policy(config) policy.to(config.DEVICE) print("Initial policy parameters:") print(list(policy.named_parameters())) @@ -107,24 +83,23 @@ def main(): torch.save(obj=policy, f=save_path + ".pt") # optimizer - batch_size = 2**5 - learning_rate = 1e-2 - learning_iterations = 10000 - optimizer = torch.optim.Adam(policy.parameters(), lr=learning_rate) + optimizer = torch.optim.Adam(policy.parameters(), lr=config.LEARNING_RATE) def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - data_generation_n_tasks, data_generation_duration + config, + config.DATA_GENERATION_TASKS, + config.DATA_GENERATION_DURATION, ) mpcnet_interface.startDataGeneration( alpha, policy_file_path, - data_generation_time_step, - data_generation_data_decimation, - data_generation_n_samples, - data_generation_sampling_covariance, + config.DATA_GENERATION_TIME_STEP, + config.DATA_GENERATION_DATA_DECIMATION, + config.DATA_GENERATION_SAMPLES, + np.diag(np.power(np.array(config.DATA_GENERATION_SAMPLING_VARIANCE), 2)), initial_observations, mode_schedules, target_trajectories, @@ -134,12 +109,14 @@ def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - policy_evaluation_n_tasks, policy_evaluation_duration + config, + config.POLICY_EVALUATION_TASKS, + config.POLICY_EVALUATION_DURATION, ) mpcnet_interface.startPolicyEvaluation( alpha, policy_file_path, - policy_evaluation_time_step, + config.POLICY_EVALUATION_TIME_STEP, initial_observations, mode_schedules, target_trajectories, @@ -153,8 +130,8 @@ def start_policy_evaluation(policy, alpha=0.0): time.sleep(1.0) print("==============\nStarting training.\n==============") - for iteration in range(learning_iterations): - alpha = 1.0 - 1.0 * iteration / learning_iterations + for iteration in range(config.LEARNING_ITERATIONS): + alpha = 1.0 - 1.0 * iteration / config.LEARNING_ITERATIONS # data generation if mpcnet_interface.isDataGenerationDone(): @@ -221,7 +198,7 @@ def start_policy_evaluation(policy, alpha=0.0): dHdx, dHdu, H, - ) = memory.sample(batch_size) + ) = memory.sample(config.BATCH_SIZE) # take an optimization step def closure(): @@ -242,7 +219,7 @@ def closure(): optimizer.step(closure) # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) - if iteration == learning_iterations - 1: + if iteration == config.LEARNING_ITERATIONS - 1: while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): time.sleep(1.0) @@ -269,4 +246,7 @@ def closure(): if __name__ == "__main__": - main() + if len(sys.argv) > 1: + main(sys.argv[1]) + else: + main("./config/ballbot.yaml") diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py deleted file mode 100644 index a99c10fbb..000000000 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config.py +++ /dev/null @@ -1,145 +0,0 @@ -############################################################################### -# Copyright (c) 2022, Farbod Farshidian. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -############################################################################### - -"""Legged robot configuration variables. - -Sets robot-specific configuration variables for legged robot. -""" - -from ocs2_mpcnet_core import config - -# -# config -# - -# data type for tensor elements -DTYPE = config.DTYPE - -# device on which tensors will be allocated -DEVICE = config.DEVICE - -# -# legged_robot_config -# - -# name of the robot -NAME = "legged_robot" - -# (generalized) time dimension -TIME_DIM = 12 - -# state dimension -STATE_DIM = 24 - -# input dimension -INPUT_DIM = 24 - -# target trajectories state dimension -TARGET_STATE_DIM = STATE_DIM - -# target trajectories input dimension -TARGET_INPUT_DIM = INPUT_DIM - -# observation dimension -OBSERVATION_DIM = 12 + STATE_DIM - -# action dimension -ACTION_DIM = INPUT_DIM - -# expert number -EXPERT_NUM = 3 - -# default state -# fmt: off -DEFAULT_STATE = [ - 0.0, 0.0, 0.0, # normalized linear momentum - 0.0, 0.0, 0.0, # normalized angular momentum - 0.0, 0.0, 0.575, # position - 0.0, 0.0, 0.0, # orientation - -0.25, 0.6, -0.85, # joint positions LF - -0.25, -0.6, 0.85, # joint positions LH - 0.25, 0.6, -0.85, # joint positions RF - 0.25, -0.6, 0.85 # joint positions RH -] -# fmt: on - -# observation scaling -# fmt: off -OBSERVATION_SCALING = [ - 1.0, 1.0, 1.0, 1.0, # swing phases - 1.0, 1.0, 1.0, 1.0, # swing phase rates - 1.0, 1.0, 1.0, 1.0, # sinusoidal bumps - 1.0, 1.0, 1.0, # normalized linear momentum - 1.0, 1.0, 1.0, # normalized angular momentum - 1.0, 1.0, 1.0, # position - 1.0, 1.0, 1.0, # orientation - 1.0, 1.0, 1.0, # joint positions LF - 1.0, 1.0, 1.0, # joint positions LH - 1.0, 1.0, 1.0, # joint positions RF - 1.0, 1.0, 1.0 # joint positions RH -] -# fmt: on - -# action scaling -# fmt: off -ACTION_SCALING = [ - 100.0, 100.0, 100.0, # contact forces LF - 100.0, 100.0, 100.0, # contact forces LH - 100.0, 100.0, 100.0, # contact forces RF - 100.0, 100.0, 100.0, # contact forces RH - 10.0, 10.0, 10.0, # joint velocities LF - 10.0, 10.0, 10.0, # joint velocities LH - 10.0, 10.0, 10.0, # joint velocities RF - 10.0, 10.0, 10.0, # joint velocities RH -] -# fmt: on - -# (diagonally dominant) nominal centroidal inertia normalized by robot mass -NORMALIZED_INERTIA = [1.62079 / 52.1348, 4.83559 / 52.1348, 4.72382 / 52.1348] - -# input cost for behavioral cloning -# fmt: off -R = [ - 0.001, 0.001, 0.001, # contact forces LF - 0.001, 0.001, 0.001, # contact forces LH - 0.001, 0.001, 0.001, # contact forces RF - 0.001, 0.001, 0.001, # contact forces RH - 5.0, 5.0, 5.0, # joint velocities LF - 5.0, 5.0, 5.0, # joint velocities LH - 5.0, 5.0, 5.0, # joint velocities RF - 5.0, 5.0, 5.0, # joint velocities RH -] -# fmt: on - -# dictionary for cheating with the gating loss -# assigns each of the OCS2 modes to an expert that is responsible for covering the corresponding contact configuration -EXPERT_FOR_MODE = dict([(i, None) for i in range(16)]) -EXPERT_FOR_MODE[15] = 0 # stance -EXPERT_FOR_MODE[6] = 1 # trot -EXPERT_FOR_MODE[9] = 2 # trot diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml new file mode 100644 index 000000000..a873bcd08 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml @@ -0,0 +1,239 @@ +config: + # + # general + # + # name of the robot + NAME: "legged_robot" + # description of the training run + DESCRIPTION: "description" + # state dimension + STATE_DIM: 24 + # input dimension + INPUT_DIM: 24 + # target trajectories state dimension + TARGET_STATE_DIM: 24 + # target trajectories input dimension + TARGET_INPUT_DIM: 24 + # observation dimension + OBSERVATION_DIM: 36 + # action dimension + ACTION_DIM: 24 + # expert number + EXPERT_NUM: 3 + # default state + DEFAULT_STATE: + - 0.0 # normalized linear momentum x + - 0.0 # normalized linear momentum y + - 0.0 # normalized linear momentum z + - 0.0 # normalized angular momentum x + - 0.0 # normalized angular momentum y + - 0.0 # normalized angular momentum z + - 0.0 # position x + - 0.0 # position y + - 0.575 # position z + - 0.0 # orientation z + - 0.0 # orientation y + - 0.0 # orientation x + - -0.25 # joint position LF HAA + - 0.6 # joint position LF HFE + - -0.85 # joint position LF KFE + - -0.25 # joint position LH HAA + - -0.6 # joint position LH HFE + - 0.85 # joint position LH KFE + - 0.25 # joint position RF HAA + - 0.6 # joint position RF HFE + - -0.85 # joint position RF KFE + - 0.25 # joint position RH HAA + - -0.6 # joint position RH HFE + - 0.85 # joint position RH KFE + # default target state + DEFAULT_TARGET_STATE: + - 0.0 # normalized linear momentum x + - 0.0 # normalized linear momentum y + - 0.0 # normalized linear momentum z + - 0.0 # normalized angular momentum x + - 0.0 # normalized angular momentum y + - 0.0 # normalized angular momentum z + - 0.0 # position x + - 0.0 # position y + - 0.575 # position z + - 0.0 # orientation z + - 0.0 # orientation y + - 0.0 # orientation x + - -0.25 # joint position LF HAA + - 0.6 # joint position LF HFE + - -0.85 # joint position LF KFE + - -0.25 # joint position LH HAA + - -0.6 # joint position LH HFE + - 0.85 # joint position LH KFE + - 0.25 # joint position RF HAA + - 0.6 # joint position RF HFE + - -0.85 # joint position RF KFE + - 0.25 # joint position RH HAA + - -0.6 # joint position RH HFE + - 0.85 # joint position RH KFE + # + # loss + # + # epsilon to improve numerical stability of logs and denominators + EPSILON: 1.e-8 + # whether to cheat by adding the gating loss + CHEATING: True + # parameter to control the relative importance of both loss types + LAMBDA: 10.0 + # dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) + EXPERT_FOR_MODE: + 6: 1 # trot + 9: 2 # trot + 15: 0 # stance + # input cost for behavioral cloning + R: + - 0.001 # contact force LF x + - 0.001 # contact force LF y + - 0.001 # contact force LF z + - 0.001 # contact force LH x + - 0.001 # contact force LH y + - 0.001 # contact force LH z + - 0.001 # contact force RF x + - 0.001 # contact force RF y + - 0.001 # contact force RF z + - 0.001 # contact force RH x + - 0.001 # contact force RH y + - 0.001 # contact force RH z + - 5.0 # joint velocity LF HAA + - 5.0 # joint velocity LF HFE + - 5.0 # joint velocity LF KFE + - 5.0 # joint velocity LH HAA + - 5.0 # joint velocity LH HFE + - 5.0 # joint velocity LH KFE + - 5.0 # joint velocity RF HAA + - 5.0 # joint velocity RF HFE + - 5.0 # joint velocity RF KFE + - 5.0 # joint velocity RH HAA + - 5.0 # joint velocity RH HFE + - 5.0 # joint velocity RH KFE + # + # memory + # + # capacity of the memory + CAPACITY: 400000 + # + # policy + # + # observation scaling + OBSERVATION_SCALING: + - 1.0 # swing phase LF + - 1.0 # swing phase LH + - 1.0 # swing phase RF + - 1.0 # swing phase RH + - 1.0 # swing phase rate LF + - 1.0 # swing phase rate LH + - 1.0 # swing phase rate RF + - 1.0 # swing phase rate RH + - 1.0 # sinusoidal bump LF + - 1.0 # sinusoidal bump LH + - 1.0 # sinusoidal bump RF + - 1.0 # sinusoidal bump RH + - 1.0 # normalized linear momentum x + - 1.0 # normalized linear momentum y + - 1.0 # normalized linear momentum z + - 1.0 # normalized angular momentum x + - 1.0 # normalized angular momentum y + - 1.0 # normalized angular momentum z + - 1.0 # position x + - 1.0 # position y + - 1.0 # position z + - 1.0 # orientation z + - 1.0 # orientation y + - 1.0 # orientation x + - 1.0 # joint position LF HAA + - 1.0 # joint position LF HFE + - 1.0 # joint position LF KFE + - 1.0 # joint position LH HAA + - 1.0 # joint position LH HFE + - 1.0 # joint position LH KFE + - 1.0 # joint position RF HAA + - 1.0 # joint position RF HFE + - 1.0 # joint position RF KFE + - 1.0 # joint position RH HAA + - 1.0 # joint position RH HFE + - 1.0 # joint position RH KFE + # action scaling + ACTION_SCALING: + - 100.0 # contact force LF x + - 100.0 # contact force LF y + - 100.0 # contact force LF z + - 100.0 # contact force LH x + - 100.0 # contact force LH y + - 100.0 # contact force LH z + - 100.0 # contact force RF x + - 100.0 # contact force RF y + - 100.0 # contact force RF z + - 100.0 # contact force RH x + - 100.0 # contact force RH y + - 100.0 # contact force RH z + - 10.0 # joint velocity LF HAA + - 10.0 # joint velocity LF HFE + - 10.0 # joint velocity LF KFE + - 10.0 # joint velocity LH HAA + - 10.0 # joint velocity LH HFE + - 10.0 # joint velocity LH KFE + - 10.0 # joint velocity RF HAA + - 10.0 # joint velocity RF HFE + - 10.0 # joint velocity RF KFE + - 10.0 # joint velocity RH HAA + - 10.0 # joint velocity RH HFE + - 10.0 # joint velocity RH KFE + # + # rollout + # + # RaiSim or TimeTriggered rollout for data generation and policy evaluation + RAISIM: True + # weights defining how often a gait is chosen for rollout + WEIGHTS_FOR_GAITS: + stance: 1.0 + trot_1: 2.0 + trot_2: 2.0 + # settings for data generation + DATA_GENERATION_TIME_STEP: 0.0025 + DATA_GENERATION_DURATION: 4.0 + DATA_GENERATION_DATA_DECIMATION: 4 + DATA_GENERATION_THREADS: 12 + DATA_GENERATION_TASKS: 12 + DATA_GENERATION_SAMPLES: 2 + DATA_GENERATION_SAMPLING_VARIANCE: + - 0.05 # normalized linear momentum x + - 0.05 # normalized linear momentum y + - 0.05 # normalized linear momentum z + - 0.00135648942 # normalized angular momentum x: 1.62079 / 52.1348 * 2.5 / 180.0 * pi + - 0.00404705526 # normalized angular momentum y: 4.83559 / 52.1348 * 2.5 / 180.0 * pi + - 0.00395351148 # normalized angular momentum z: 4.72382 / 52.1348 * 2.5 / 180.0 * pi + - 0.01 # position x + - 0.01 # position y + - 0.01 # position z + - 0.00872664625 # orientation z: 0.5 / 180.0 * pi + - 0.00872664625 # orientation y: 0.5 / 180.0 * pi + - 0.00872664625 # orientation x: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH KFE: 0.5 / 180.0 * pi + # settings for computing metrics + POLICY_EVALUATION_TIME_STEP: 0.0025 + POLICY_EVALUATION_DURATION: 4.0 + POLICY_EVALUATION_THREADS: 3 + POLICY_EVALUATION_TASKS: 3 + # + # training + # + BATCH_SIZE: 128 + LEARNING_RATE: 1.e-3 + LEARNING_ITERATIONS: 100000 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot_laptop.yaml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot_laptop.yaml new file mode 100644 index 000000000..285a4e6fa --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot_laptop.yaml @@ -0,0 +1,239 @@ +config: + # + # general + # + # name of the robot + NAME: "legged_robot" + # description of the training run + DESCRIPTION: "description" + # state dimension + STATE_DIM: 24 + # input dimension + INPUT_DIM: 24 + # target trajectories state dimension + TARGET_STATE_DIM: 24 + # target trajectories input dimension + TARGET_INPUT_DIM: 24 + # observation dimension + OBSERVATION_DIM: 36 + # action dimension + ACTION_DIM: 24 + # expert number + EXPERT_NUM: 3 + # default state + DEFAULT_STATE: + - 0.0 # normalized linear momentum x + - 0.0 # normalized linear momentum y + - 0.0 # normalized linear momentum z + - 0.0 # normalized angular momentum x + - 0.0 # normalized angular momentum y + - 0.0 # normalized angular momentum z + - 0.0 # position x + - 0.0 # position y + - 0.575 # position z + - 0.0 # orientation z + - 0.0 # orientation y + - 0.0 # orientation x + - -0.25 # joint position LF HAA + - 0.6 # joint position LF HFE + - -0.85 # joint position LF KFE + - -0.25 # joint position LH HAA + - -0.6 # joint position LH HFE + - 0.85 # joint position LH KFE + - 0.25 # joint position RF HAA + - 0.6 # joint position RF HFE + - -0.85 # joint position RF KFE + - 0.25 # joint position RH HAA + - -0.6 # joint position RH HFE + - 0.85 # joint position RH KFE + # default target state + DEFAULT_TARGET_STATE: + - 0.0 # normalized linear momentum x + - 0.0 # normalized linear momentum y + - 0.0 # normalized linear momentum z + - 0.0 # normalized angular momentum x + - 0.0 # normalized angular momentum y + - 0.0 # normalized angular momentum z + - 0.0 # position x + - 0.0 # position y + - 0.575 # position z + - 0.0 # orientation z + - 0.0 # orientation y + - 0.0 # orientation x + - -0.25 # joint position LF HAA + - 0.6 # joint position LF HFE + - -0.85 # joint position LF KFE + - -0.25 # joint position LH HAA + - -0.6 # joint position LH HFE + - 0.85 # joint position LH KFE + - 0.25 # joint position RF HAA + - 0.6 # joint position RF HFE + - -0.85 # joint position RF KFE + - 0.25 # joint position RH HAA + - -0.6 # joint position RH HFE + - 0.85 # joint position RH KFE + # + # loss + # + # epsilon to improve numerical stability of logs and denominators + EPSILON: 1.e-8 + # whether to cheat by adding the gating loss + CHEATING: True + # parameter to control the relative importance of both loss types + LAMBDA: 10.0 + # dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) + EXPERT_FOR_MODE: + 6: 1 # trot + 9: 2 # trot + 15: 0 # stance + # input cost for behavioral cloning + R: + - 0.001 # contact force LF x + - 0.001 # contact force LF y + - 0.001 # contact force LF z + - 0.001 # contact force LH x + - 0.001 # contact force LH y + - 0.001 # contact force LH z + - 0.001 # contact force RF x + - 0.001 # contact force RF y + - 0.001 # contact force RF z + - 0.001 # contact force RH x + - 0.001 # contact force RH y + - 0.001 # contact force RH z + - 5.0 # joint velocity LF HAA + - 5.0 # joint velocity LF HFE + - 5.0 # joint velocity LF KFE + - 5.0 # joint velocity LH HAA + - 5.0 # joint velocity LH HFE + - 5.0 # joint velocity LH KFE + - 5.0 # joint velocity RF HAA + - 5.0 # joint velocity RF HFE + - 5.0 # joint velocity RF KFE + - 5.0 # joint velocity RH HAA + - 5.0 # joint velocity RH HFE + - 5.0 # joint velocity RH KFE + # + # memory + # + # capacity of the memory + CAPACITY: 100000 + # + # policy + # + # observation scaling + OBSERVATION_SCALING: + - 1.0 # swing phase LF + - 1.0 # swing phase LH + - 1.0 # swing phase RF + - 1.0 # swing phase RH + - 1.0 # swing phase rate LF + - 1.0 # swing phase rate LH + - 1.0 # swing phase rate RF + - 1.0 # swing phase rate RH + - 1.0 # sinusoidal bump LF + - 1.0 # sinusoidal bump LH + - 1.0 # sinusoidal bump RF + - 1.0 # sinusoidal bump RH + - 1.0 # normalized linear momentum x + - 1.0 # normalized linear momentum y + - 1.0 # normalized linear momentum z + - 1.0 # normalized angular momentum x + - 1.0 # normalized angular momentum y + - 1.0 # normalized angular momentum z + - 1.0 # position x + - 1.0 # position y + - 1.0 # position z + - 1.0 # orientation z + - 1.0 # orientation y + - 1.0 # orientation x + - 1.0 # joint position LF HAA + - 1.0 # joint position LF HFE + - 1.0 # joint position LF KFE + - 1.0 # joint position LH HAA + - 1.0 # joint position LH HFE + - 1.0 # joint position LH KFE + - 1.0 # joint position RF HAA + - 1.0 # joint position RF HFE + - 1.0 # joint position RF KFE + - 1.0 # joint position RH HAA + - 1.0 # joint position RH HFE + - 1.0 # joint position RH KFE + # action scaling + ACTION_SCALING: + - 100.0 # contact force LF x + - 100.0 # contact force LF y + - 100.0 # contact force LF z + - 100.0 # contact force LH x + - 100.0 # contact force LH y + - 100.0 # contact force LH z + - 100.0 # contact force RF x + - 100.0 # contact force RF y + - 100.0 # contact force RF z + - 100.0 # contact force RH x + - 100.0 # contact force RH y + - 100.0 # contact force RH z + - 10.0 # joint velocity LF HAA + - 10.0 # joint velocity LF HFE + - 10.0 # joint velocity LF KFE + - 10.0 # joint velocity LH HAA + - 10.0 # joint velocity LH HFE + - 10.0 # joint velocity LH KFE + - 10.0 # joint velocity RF HAA + - 10.0 # joint velocity RF HFE + - 10.0 # joint velocity RF KFE + - 10.0 # joint velocity RH HAA + - 10.0 # joint velocity RH HFE + - 10.0 # joint velocity RH KFE + # + # rollout + # + # RaiSim or TimeTriggered rollout for data generation and policy evaluation + RAISIM: True + # weights defining how often a gait is chosen for rollout + WEIGHTS_FOR_GAITS: + stance: 1.0 + trot_1: 2.0 + trot_2: 2.0 + # settings for data generation + DATA_GENERATION_TIME_STEP: 0.0025 + DATA_GENERATION_DURATION: 4.0 + DATA_GENERATION_DATA_DECIMATION: 4 + DATA_GENERATION_THREADS: 6 + DATA_GENERATION_TASKS: 6 + DATA_GENERATION_SAMPLES: 2 + DATA_GENERATION_SAMPLING_VARIANCE: + - 0.05 # normalized linear momentum x + - 0.05 # normalized linear momentum y + - 0.05 # normalized linear momentum z + - 0.00135648942 # normalized angular momentum x: 1.62079 / 52.1348 * 2.5 / 180.0 * pi + - 0.00404705526 # normalized angular momentum y: 4.83559 / 52.1348 * 2.5 / 180.0 * pi + - 0.00395351148 # normalized angular momentum z: 4.72382 / 52.1348 * 2.5 / 180.0 * pi + - 0.01 # position x + - 0.01 # position y + - 0.01 # position z + - 0.00872664625 # orientation z: 0.5 / 180.0 * pi + - 0.00872664625 # orientation y: 0.5 / 180.0 * pi + - 0.00872664625 # orientation x: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH KFE: 0.5 / 180.0 * pi + # settings for computing metrics + POLICY_EVALUATION_TIME_STEP: 0.0025 + POLICY_EVALUATION_DURATION: 4.0 + POLICY_EVALUATION_THREADS: 1 + POLICY_EVALUATION_TASKS: 1 + # + # training + # + BATCH_SIZE: 128 + LEARNING_RATE: 1.e-3 + LEARNING_ITERATIONS: 100000 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py index 4ef244197..4029963f1 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py @@ -32,14 +32,14 @@ Provides robot-specific helper functions for legged robot. """ +import random import numpy as np -from typing import Tuple, List +from typing import Tuple, Dict +from ocs2_mpcnet_core import config from ocs2_mpcnet_core import helper from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray -from ocs2_legged_robot_mpcnet import config - def get_stance(duration: float) -> Tuple[np.ndarray, np.ndarray]: """Get the stance gait. @@ -61,48 +61,56 @@ def get_stance(duration: float) -> Tuple[np.ndarray, np.ndarray]: return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) -def get_random_initial_state_stance() -> np.ndarray: +def get_random_initial_state_stance(state_dimension: int, default_state: [float]) -> np.ndarray: """Get a random initial state for stance. Samples a random initial state for the robot in the stance gait. + Args: + state_dimension: The dimension of the state given by an integer. + default_state: The default state given by a Python array containing floats. + Returns: - x: A random initial state given by a NumPy array of shape (X) containing floats. + x: A random initial state given by a NumPy array containing floats. """ max_normalized_linear_momentum_x = 0.1 max_normalized_linear_momentum_y = 0.1 max_normalized_linear_momentum_z = 0.1 - max_normalized_angular_momentum_x = config.NORMALIZED_INERTIA[0] * 30.0 * np.pi / 180.0 - max_normalized_angular_momentum_y = config.NORMALIZED_INERTIA[1] * 30.0 * np.pi / 180.0 - max_normalized_angular_momentum_z = config.NORMALIZED_INERTIA[2] * 30.0 * np.pi / 180.0 - random_deviation = np.zeros(config.STATE_DIM) + max_normalized_angular_momentum_x = 1.62079 / 52.1348 * 30.0 / 180.0 * np.pi + max_normalized_angular_momentum_y = 4.83559 / 52.1348 * 30.0 / 180.0 * np.pi + max_normalized_angular_momentum_z = 4.72382 / 52.1348 * 30.0 / 180.0 * np.pi + random_deviation = np.zeros(state_dimension) random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) - return np.array(config.DEFAULT_STATE) + random_deviation + return np.array(default_state) + random_deviation -def get_random_target_state_stance() -> np.ndarray: +def get_random_target_state_stance(target_state_dimension: int, default_target_state: [float]) -> np.ndarray: """Get a random target state for stance. Samples a random target state for the robot in the stance gait. + Args: + target_state_dimension: The dimension of the target state given by an integer. + default_target_state: The default target state given by a Python array containing floats. + Returns: - x: A random target state given by a NumPy array of shape (X) containing floats. + x: A random target state given by a NumPy array containing floats. """ max_position_z = 0.075 - max_orientation_z = 25.0 * np.pi / 180.0 - max_orientation_y = 15.0 * np.pi / 180.0 - max_orientation_x = 25.0 * np.pi / 180.0 - random_deviation = np.zeros(config.TARGET_STATE_DIM) + max_orientation_z = 25.0 / 180.0 * np.pi + max_orientation_y = 15.0 / 180.0 * np.pi + max_orientation_x = 25.0 / 180.0 * np.pi + random_deviation = np.zeros(target_state_dimension) random_deviation[8] = np.random.uniform(-max_position_z, max_position_z) random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) random_deviation[10] = np.random.uniform(-max_orientation_y, max_orientation_y) random_deviation[11] = np.random.uniform(-max_orientation_x, max_orientation_x) - return np.array(config.DEFAULT_STATE) + random_deviation + return np.array(default_target_state) + random_deviation def get_trot_1(duration: float) -> Tuple[np.ndarray, np.ndarray]: @@ -145,59 +153,67 @@ def get_trot_2(duration: float) -> Tuple[np.ndarray, np.ndarray]: return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) -def get_random_initial_state_trot() -> np.ndarray: +def get_random_initial_state_trot(state_dimension: int, default_state: [float]) -> np.ndarray: """Get a random initial state for trot. Samples a random initial state for the robot in a trot gait. + Args: + state_dimension: The dimension of the state given by an integer. + default_state: The default state given by a Python array containing floats. + Returns: - x: A random initial state given by a NumPy array of shape (X) containing floats. + x: A random initial state given by a NumPy array containing floats. """ max_normalized_linear_momentum_x = 0.5 max_normalized_linear_momentum_y = 0.25 max_normalized_linear_momentum_z = 0.25 - max_normalized_angular_momentum_x = config.NORMALIZED_INERTIA[0] * 60.0 * np.pi / 180.0 - max_normalized_angular_momentum_y = config.NORMALIZED_INERTIA[1] * 60.0 * np.pi / 180.0 - max_normalized_angular_momentum_z = config.NORMALIZED_INERTIA[2] * 35.0 * np.pi / 180.0 - random_deviation = np.zeros(config.STATE_DIM) + max_normalized_angular_momentum_x = 1.62079 / 52.1348 * 60.0 / 180.0 * np.pi + max_normalized_angular_momentum_y = 4.83559 / 52.1348 * 60.0 / 180.0 * np.pi + max_normalized_angular_momentum_z = 4.72382 / 52.1348 * 35.0 / 180.0 * np.pi + random_deviation = np.zeros(state_dimension) random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) - return np.array(config.DEFAULT_STATE) + random_deviation + return np.array(default_state) + random_deviation -def get_random_target_state_trot() -> np.ndarray: +def get_random_target_state_trot(target_state_dimension: int, default_target_state: [float]) -> np.ndarray: """Get a random target state for trot. Samples a random target state for the robot in a trot gait. + Args: + target_state_dimension: The dimension of the target state given by an integer. + default_target_state: The default target state given by a Python array containing floats. + Returns: - x: A random target state given by a NumPy array of shape (X) containing floats. + x: A random target state given by a NumPy array containing floats. """ max_position_x = 0.3 max_position_y = 0.15 - max_orientation_z = 30.0 * np.pi / 180.0 - random_deviation = np.zeros(config.TARGET_STATE_DIM) + max_orientation_z = 30.0 / 180.0 * np.pi + random_deviation = np.zeros(target_state_dimension) random_deviation[6] = np.random.uniform(-max_position_x, max_position_x) random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) - return np.array(config.DEFAULT_STATE) + random_deviation + return np.array(default_target_state) + random_deviation def get_tasks( - n_tasks: int, duration: float, choices: List[str] + config: config.Config, tasks_number: int, duration: float ) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: """Get tasks. Get a random set of task that should be executed by the data generation or policy evaluation. Args: - n_tasks: Number of tasks given by an integer. + config: An instance of the configuration class. + tasks_number: Number of tasks given by an integer. duration: Duration of each task given by a float. - choices: Different gaits for the tasks given by a list containing strings with the gait names. Returns: A tuple containing the components of the task. @@ -205,44 +221,62 @@ def get_tasks( - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. """ - initial_observations = helper.get_system_observation_array(n_tasks) - mode_schedules = helper.get_mode_schedule_array(n_tasks) - target_trajectories = helper.get_target_trajectories_array(n_tasks) - for i in range(n_tasks): + initial_observations = helper.get_system_observation_array(tasks_number) + mode_schedules = helper.get_mode_schedule_array(tasks_number) + target_trajectories = helper.get_target_trajectories_array(tasks_number) + choices = random.choices( + list(config.WEIGHTS_FOR_GAITS.keys()), k=tasks_number, weights=list(config.WEIGHTS_FOR_GAITS.values()) + ) + for i in range(tasks_number): if choices[i] == "stance": initial_observations[i] = helper.get_system_observation( - 15, 0.0, get_random_initial_state_stance(), np.zeros(config.INPUT_DIM) + 15, + 0.0, + get_random_initial_state_stance(config.STATE_DIM, config.DEFAULT_STATE), + np.zeros(config.INPUT_DIM), ) mode_schedules[i] = helper.get_mode_schedule(*get_stance(duration)) target_trajectories[i] = helper.get_target_trajectories( duration * np.ones((1, 1)), - get_random_target_state_stance().reshape((1, config.TARGET_STATE_DIM)), + get_random_target_state_stance(config.TARGET_STATE_DIM, config.DEFAULT_TARGET_STATE).reshape( + (1, config.TARGET_STATE_DIM) + ), np.zeros((1, config.TARGET_INPUT_DIM)), ) elif choices[i] == "trot_1": initial_observations[i] = helper.get_system_observation( - 15, 0.0, get_random_initial_state_trot(), np.zeros(config.INPUT_DIM) + 15, + 0.0, + get_random_initial_state_trot(config.STATE_DIM, config.DEFAULT_STATE), + np.zeros(config.INPUT_DIM), ) mode_schedules[i] = helper.get_mode_schedule(*get_trot_1(duration)) target_trajectories[i] = helper.get_target_trajectories( duration * np.ones((1, 1)), - get_random_target_state_trot().reshape((1, config.TARGET_STATE_DIM)), + get_random_target_state_trot(config.TARGET_STATE_DIM, config.DEFAULT_TARGET_STATE).reshape( + (1, config.TARGET_STATE_DIM) + ), np.zeros((1, config.TARGET_INPUT_DIM)), ) elif choices[i] == "trot_2": initial_observations[i] = helper.get_system_observation( - 15, 0.0, get_random_initial_state_trot(), np.zeros(config.INPUT_DIM) + 15, + 0.0, + get_random_initial_state_trot(config.STATE_DIM, config.DEFAULT_STATE), + np.zeros(config.INPUT_DIM), ) mode_schedules[i] = helper.get_mode_schedule(*get_trot_2(duration)) target_trajectories[i] = helper.get_target_trajectories( duration * np.ones((1, 1)), - get_random_target_state_trot().reshape((1, config.TARGET_STATE_DIM)), + get_random_target_state_trot(config.TARGET_STATE_DIM, config.DEFAULT_TARGET_STATE).reshape( + (1, config.TARGET_STATE_DIM) + ), np.zeros((1, config.TARGET_INPUT_DIM)), ) return initial_observations, mode_schedules, target_trajectories -def get_one_hot(mode: int) -> np.ndarray: +def get_one_hot(mode: int, expert_number: int, expert_for_mode: Dict[int, int]) -> np.ndarray: """Get one hot encoding of mode. Get a one hot encoding of a mode represented by a discrete probability distribution, where the sample space is the @@ -250,10 +284,12 @@ def get_one_hot(mode: int) -> np.ndarray: Args: mode: The mode of the system given by an integer. + expert_number: The number of experts given by an integer. + expert_for_mode: A dictionary that assigns modes to experts. Returns: p: Discrete probability distribution given by a NumPy array of shape (P) containing floats. """ - one_hot = np.zeros(config.EXPERT_NUM) - one_hot[config.EXPERT_FOR_MODE[mode]] = 1.0 + one_hot = np.zeros(expert_number) + one_hot[expert_for_mode[mode]] = 1.0 return one_hot diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index b6fc98217..c3b291f3b 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -35,86 +35,46 @@ """ import os +import sys import time import datetime -import random import torch import numpy as np from torch.utils.tensorboard import SummaryWriter +from ocs2_mpcnet_core.config import Config from ocs2_mpcnet_core.helper import bmv, bmm from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss as ExpertsLoss from ocs2_mpcnet_core.loss.cross_entropy import CrossEntropyLoss as GatingLoss from ocs2_mpcnet_core.memory.circular import CircularMemory as Memory from ocs2_mpcnet_core.policy.mixture_of_nonlinear_experts import MixtureOfNonlinearExpertsPolicy as Policy -from ocs2_legged_robot_mpcnet import config from ocs2_legged_robot_mpcnet import helper from ocs2_legged_robot_mpcnet import MpcnetInterface -def main(): - # settings for data generation by applying behavioral policy - data_generation_time_step = 0.0025 - data_generation_duration = 4.0 - data_generation_data_decimation = 4 - data_generation_n_threads = 12 - data_generation_n_tasks = 12 - data_generation_n_samples = 2 - data_generation_sampling_covariance = np.zeros((config.STATE_DIM, config.STATE_DIM), order="F") - for i in range(0, 3): - data_generation_sampling_covariance[i, i] = 0.05**2 # normalized linear momentum - for i in range(3, 6): - data_generation_sampling_covariance[i, i] = ( - config.NORMALIZED_INERTIA[i - 3] * 2.5 * np.pi / 180.0 - ) ** 2 # normalized angular momentum - for i in range(6, 9): - data_generation_sampling_covariance[i, i] = 0.01**2 # position - for i in range(9, 12): - data_generation_sampling_covariance[i, i] = (0.5 * np.pi / 180.0) ** 2 # orientation - for i in range(12, 24): - data_generation_sampling_covariance[i, i] = (0.5 * np.pi / 180.0) ** 2 # joint positions - - # settings for computing metrics by applying learned policy - policy_evaluation_time_step = 0.0025 - policy_evaluation_duration = 4.0 - policy_evaluation_n_threads = 3 - policy_evaluation_n_tasks = 3 - - # rollout settings for data generation and policy evaluation - raisim = True +def main(config_file_path: str) -> None: + # config + config = Config(config_file_path) # mpcnet interface - mpcnet_interface = MpcnetInterface(data_generation_n_threads, policy_evaluation_n_threads, raisim) + mpcnet_interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) # logging - description = "description" - folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + description + folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + config.DESCRIPTION writer = SummaryWriter("runs/" + folder) os.makedirs(name="policies/" + folder) # loss - epsilon = 1e-8 # epsilon to improve numerical stability of logs and denominators - my_lambda = 10.0 # parameter to control the relative importance of both loss types experts_loss = ExpertsLoss() - gating_loss = GatingLoss(epsilon) + gating_loss = GatingLoss(config) # memory - memory_capacity = 400000 - memory = Memory( - memory_capacity, - config.STATE_DIM, - config.INPUT_DIM, - config.OBSERVATION_DIM, - config.ACTION_DIM, - config.EXPERT_NUM, - ) + memory = Memory(config) # policy - policy = Policy( - config.OBSERVATION_DIM, config.ACTION_DIM, config.EXPERT_NUM, config.OBSERVATION_SCALING, config.ACTION_SCALING - ) + policy = Policy(config) policy.to(config.DEVICE) print("Initial policy parameters:") print(list(policy.named_parameters())) @@ -125,36 +85,23 @@ def main(): torch.save(obj=policy, f=save_path + ".pt") # optimizer - batch_size = 2**7 - learning_iterations = 100000 - learning_rate_default = 1e-3 - learning_rate_gating_net = learning_rate_default - learning_rate_expert_nets = learning_rate_default - optimizer = torch.optim.Adam( - [ - {"params": policy.gating_net.parameters(), "lr": learning_rate_gating_net}, - {"params": policy.expert_nets.parameters(), "lr": learning_rate_expert_nets}, - ], - lr=learning_rate_default, - ) - - # weights for ["stance", "trot_1", "trot_2"] - weights = [1, 2, 2] + optimizer = torch.optim.Adam(policy.parameters(), lr=config.LEARNING_RATE) def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) - choices = random.choices(["stance", "trot_1", "trot_2"], k=data_generation_n_tasks, weights=weights) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - data_generation_n_tasks, data_generation_duration, choices + config, + config.DATA_GENERATION_TASKS, + config.DATA_GENERATION_DURATION, ) mpcnet_interface.startDataGeneration( alpha, policy_file_path, - data_generation_time_step, - data_generation_data_decimation, - data_generation_n_samples, - data_generation_sampling_covariance, + config.DATA_GENERATION_TIME_STEP, + config.DATA_GENERATION_DATA_DECIMATION, + config.DATA_GENERATION_SAMPLES, + np.diag(np.power(np.array(config.DATA_GENERATION_SAMPLING_VARIANCE), 2)), initial_observations, mode_schedules, target_trajectories, @@ -163,14 +110,15 @@ def start_data_generation(policy, alpha=1.0): def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) - choices = random.choices(["stance", "trot_1", "trot_2"], k=policy_evaluation_n_tasks, weights=weights) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - policy_evaluation_n_tasks, policy_evaluation_duration, choices + config, + config.POLICY_EVALUATION_TASKS, + config.POLICY_EVALUATION_DURATION, ) mpcnet_interface.startPolicyEvaluation( alpha, policy_file_path, - policy_evaluation_time_step, + config.POLICY_EVALUATION_TIME_STEP, initial_observations, mode_schedules, target_trajectories, @@ -184,8 +132,8 @@ def start_policy_evaluation(policy, alpha=0.0): time.sleep(1.0) print("==============\nStarting training.\n==============") - for iteration in range(learning_iterations): - alpha = 1.0 - 1.0 * iteration / learning_iterations + for iteration in range(config.LEARNING_ITERATIONS): + alpha = 1.0 - 1.0 * iteration / config.LEARNING_ITERATIONS # data generation if mpcnet_interface.isDataGenerationDone(): @@ -197,7 +145,7 @@ def start_policy_evaluation(policy, alpha=0.0): data[i].t, data[i].x, data[i].u, - helper.get_one_hot(data[i].mode), + helper.get_one_hot(data[i].mode, config.EXPERT_NUM, config.EXPERT_FOR_MODE), data[i].observation, data[i].actionTransformation, data[i].hamiltonian, @@ -252,7 +200,7 @@ def start_policy_evaluation(policy, alpha=0.0): dHdx, dHdu, H, - ) = memory.sample(batch_size) + ) = memory.sample(config.BATCH_SIZE) # take an optimization step def closure(): @@ -264,7 +212,7 @@ def closure(): # compute the empirical loss empirical_experts_loss = experts_loss(x, x, input, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) empirical_gating_loss = gating_loss(p, expert_weights) - empirical_loss = empirical_experts_loss + my_lambda * empirical_gating_loss + empirical_loss = empirical_experts_loss + config.LAMBDA * empirical_gating_loss # compute the gradients empirical_loss.backward() # logging @@ -277,7 +225,7 @@ def closure(): optimizer.step(closure) # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) - if iteration == learning_iterations - 1: + if iteration == config.LEARNING_ITERATIONS - 1: while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): time.sleep(1.0) @@ -304,4 +252,7 @@ def closure(): if __name__ == "__main__": - main() + if len(sys.argv) > 1: + main(sys.argv[1]) + else: + main("./config/legged_robot.yaml") diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py index a59823129..95fd1a139 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py @@ -27,15 +27,47 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### -"""Configuration variables. +"""Configuration class. -Sets general configuration variables. +Provides a class that handles the configuration parameters. """ +import yaml import torch -# data type for tensor elements -DTYPE = torch.float -# device on which tensors will be allocated -DEVICE = torch.device("cuda") +class Config: + """Config. + + Loads configuration parameters from a YAML file and provides access to them as attributes of this class. + + Attributes: + DTYPE: The PyTorch data type. + DEVICE: The PyTorch device to select. + """ + + def __init__(self, config_file_path: str) -> None: + """Initializes the Config class. + + Initializes the Config class by setting fixed attributes and by loading attributes from a YAML file. + + Args: + config_file_path: A string with the path to the configuration file. + """ + # + # class config + # + # data type for tensor elements + self.DTYPE = torch.float + # device on which tensors will be allocated + self.DEVICE = torch.device("cuda") + # + # yaml config + # + with open(config_file_path, "r") as stream: + try: + config = yaml.safe_load(stream) + for key, value in config["config"].items(): + setattr(self, key, value) + except yaml.YAMLError as exception: + print(exception) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py index 990971556..c1ec56134 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py @@ -54,15 +54,15 @@ class BehavioralCloningLoss: R: A (1,U,U) tensor with the input cost matrix. """ - def __init__(self, R: np.ndarray) -> None: + def __init__(self, config: config.Config) -> None: """Initializes the BehavioralCloningLoss class. Initializes the BehavioralCloningLoss class by setting fixed attributes. Args: - R: A NumPy array of shape (U, U) with the input cost matrix. + config: An instance of the configuration class. """ - self.R = torch.tensor(R, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) + self.R = torch.tensor(config.R, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) def __call__(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: """Computes the mean behavioral cloning loss. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py index cf5702f27..848f4d184 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py @@ -53,15 +53,15 @@ class CrossEntropyLoss: epsilon: A (1) tensor with a small epsilon used to stabilize the logarithm. """ - def __init__(self, epsilon: float) -> None: + def __init__(self, config: config.Config) -> None: """Initializes the CrossEntropyLoss class. Initializes the CrossEntropyLoss class by setting fixed attributes. Args: - epsilon: A float used to stabilize the logarithm. + config: An instance of the configuration class. """ - self.epsilon = torch.tensor(epsilon, device=config.DEVICE, dtype=config.DTYPE) + self.epsilon = torch.tensor(config.EPSILON, device=config.DEVICE, dtype=config.DTYPE) def __call__(self, p_target: torch.Tensor, p_predicted: torch.Tensor) -> torch.Tensor: """Computes the mean cross entropy loss. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py index d0e583277..8420f38b4 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py @@ -64,51 +64,46 @@ class CircularMemory: H: A (C) tensor for the Hamiltonians at the development/expansion points. """ - def __init__( - self, - capacity: int, - state_dimension: int, - input_dimension: int, - observation_dimension: int, - action_dimension: int, - expert_number: int = 1, - ) -> None: + def __init__(self, config: config.Config) -> None: """Initializes the CircularMemory class. Initializes the CircularMemory class by setting fixed attributes, initializing variable attributes and pre-allocating memory. Args: - capacity: An integer defining the capacity, i.e. maximum size, C of the memory. - state_dimension: An integer defining the dimension X of the state. - input_dimension: An integer defining the dimension U of the input. - observation_dimension: An integer defining the dimension O of the observation. - action_dimension: An integer defining the dimension A of the action. - expert_number: An integer defining the number of experts E equal to the number of individually identifiable - items P in the sample space of the discrete probability distributions of the modes. + config: An instance of the configuration class. """ # init variables - self.capacity = capacity + self.device = config.DEVICE + self.capacity = config.CAPACITY self.size = 0 self.position = 0 # pre-allocate memory - self.t = torch.zeros(capacity, device=config.DEVICE, dtype=config.DTYPE) - self.x = torch.zeros(capacity, state_dimension, device=config.DEVICE, dtype=config.DTYPE) - self.u = torch.zeros(capacity, input_dimension, device=config.DEVICE, dtype=config.DTYPE) - self.p = torch.zeros(capacity, expert_number, device=config.DEVICE, dtype=config.DTYPE) - self.observation = torch.zeros(capacity, observation_dimension, device=config.DEVICE, dtype=config.DTYPE) + self.t = torch.zeros(config.CAPACITY, device=config.DEVICE, dtype=config.DTYPE) + self.x = torch.zeros(config.CAPACITY, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE) + self.u = torch.zeros(config.CAPACITY, config.INPUT_DIM, device=config.DEVICE, dtype=config.DTYPE) + self.p = torch.zeros(config.CAPACITY, config.EXPERT_NUM, device=config.DEVICE, dtype=config.DTYPE) + self.observation = torch.zeros( + config.CAPACITY, config.OBSERVATION_DIM, device=config.DEVICE, dtype=config.DTYPE + ) self.action_transformation_matrix = torch.zeros( - capacity, input_dimension, action_dimension, device=config.DEVICE, dtype=config.DTYPE + config.CAPACITY, config.INPUT_DIM, config.ACTION_DIM, device=config.DEVICE, dtype=config.DTYPE ) self.action_transformation_vector = torch.zeros( - capacity, input_dimension, device=config.DEVICE, dtype=config.DTYPE + config.CAPACITY, config.INPUT_DIM, device=config.DEVICE, dtype=config.DTYPE + ) + self.dHdxx = torch.zeros( + config.CAPACITY, config.STATE_DIM, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE + ) + self.dHdux = torch.zeros( + config.CAPACITY, config.INPUT_DIM, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE + ) + self.dHduu = torch.zeros( + config.CAPACITY, config.INPUT_DIM, config.INPUT_DIM, device=config.DEVICE, dtype=config.DTYPE ) - self.dHdxx = torch.zeros(capacity, state_dimension, state_dimension, device=config.DEVICE, dtype=config.DTYPE) - self.dHdux = torch.zeros(capacity, input_dimension, state_dimension, device=config.DEVICE, dtype=config.DTYPE) - self.dHduu = torch.zeros(capacity, input_dimension, input_dimension, device=config.DEVICE, dtype=config.DTYPE) - self.dHdx = torch.zeros(capacity, state_dimension, device=config.DEVICE, dtype=config.DTYPE) - self.dHdu = torch.zeros(capacity, input_dimension, device=config.DEVICE, dtype=config.DTYPE) - self.H = torch.zeros(capacity, device=config.DEVICE, dtype=config.DTYPE) + self.dHdx = torch.zeros(config.CAPACITY, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE) + self.dHdu = torch.zeros(config.CAPACITY, config.INPUT_DIM, device=config.DEVICE, dtype=config.DTYPE) + self.H = torch.zeros(config.CAPACITY, device=config.DEVICE, dtype=config.DTYPE) def push( self, @@ -181,7 +176,7 @@ def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: - dHdu_batch: A (B,U) tensor with the input gradients of the Hamiltonian approximations. - H_batch: A (B) tensor with the Hamiltonians at the development/expansion points. """ - indices = torch.randint(0, self.size, (batch_size,), device=config.DEVICE) + indices = torch.randint(low=0, high=self.size, size=(batch_size,), device=self.device) t_batch = self.t[indices] x_batch = self.x[indices] u_batch = self.u[indices] diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py index 42c321cce..f85e4b17b 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py @@ -53,32 +53,23 @@ class LinearPolicy(torch.nn.Module): linear: The linear neural network layer. """ - def __init__( - self, - observation_dimension: int, - action_dimension: int, - observation_scaling: np.ndarray, - action_scaling: np.ndarray, - ) -> None: + def __init__(self, config: config.Config) -> None: """Initializes the LinearPolicy class. Initializes the LinearPolicy class by setting fixed and variable attributes. Args: - observation_dimension: An integer defining the observation dimension. - action_dimension: An integer defining the action dimension. - observation_scaling: A NumPy array of shape (O) defining the observation scaling. - action_scaling: A NumPy array of shape (A) defining the action scaling. + config: An instance of the configuration class. """ super().__init__() self.name = "LinearPolicy" - self.observation_dimension = observation_dimension - self.action_dimension = action_dimension + self.observation_dimension = config.OBSERVATION_DIM + self.action_dimension = config.ACTION_DIM self.observation_scaling = ( - torch.tensor(observation_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) self.action_scaling = ( - torch.tensor(action_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) self.linear = torch.nn.Linear(self.observation_dimension, self.action_dimension) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py index 54ff093bb..94a190303 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py @@ -56,35 +56,24 @@ class MixtureOfLinearExpertsPolicy(torch.nn.Module): expert_nets: The expert networks. """ - def __init__( - self, - observation_dimension: int, - action_dimension: int, - expert_number: int, - observation_scaling: np.ndarray, - action_scaling: np.ndarray, - ) -> None: + def __init__(self, config: config.Config) -> None: """Initializes the MixtureOfLinearExpertsPolicy class. Initializes the MixtureOfLinearExpertsPolicy class by setting fixed and variable attributes. Args: - observation_dimension: An integer defining the observation dimension. - action_dimension: An integer defining the action dimension. - expert_number: An integer defining the number of experts. - observation_scaling: A NumPy array of shape (O) defining the observation scaling. - action_scaling: A NumPy array of shape (A) defining the action scaling. + config: An instance of the configuration class. """ super().__init__() self.name = "MixtureOfLinearExpertsPolicy" - self.observation_dimension = observation_dimension - self.action_dimension = action_dimension - self.expert_number = expert_number + self.observation_dimension = config.OBSERVATION_DIM + self.action_dimension = config.ACTION_DIM + self.expert_number = config.EXPERT_NUM self.observation_scaling = ( - torch.tensor(observation_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) self.action_scaling = ( - torch.tensor(action_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) # gating self.gating_net = torch.nn.Sequential( diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py index 57ab846f5..9379f9f1b 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py @@ -59,37 +59,26 @@ class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): expert_nets: The expert networks. """ - def __init__( - self, - observation_dimension: int, - action_dimension: int, - expert_number: int, - observation_scaling: np.ndarray, - action_scaling: np.ndarray, - ) -> None: + def __init__(self, config: config.Config) -> None: """Initializes the MixtureOfNonlinearExpertsPolicy class. Initializes the MixtureOfNonlinearExpertsPolicy class by setting fixed and variable attributes. Args: - observation_dimension: An integer defining the observation dimension. - action_dimension: An integer defining the action dimension. - expert_number: An integer defining the number of experts. - observation_scaling: A NumPy array of shape (O) defining the observation scaling. - action_scaling: A NumPy array of shape (A) defining the action scaling. + config: An instance of the configuration class. """ super().__init__() self.name = "MixtureOfNonlinearExpertsPolicy" - self.observation_dimension = observation_dimension - self.gating_hidden_dimension = int((observation_dimension + expert_number) / 2) - self.expert_hidden_dimension = int((observation_dimension + action_dimension) / 2) - self.action_dimension = action_dimension - self.expert_number = expert_number + self.observation_dimension = config.OBSERVATION_DIM + self.gating_hidden_dimension = int((config.OBSERVATION_DIM + config.EXPERT_NUM) / 2) + self.expert_hidden_dimension = int((config.OBSERVATION_DIM + config.ACTION_DIM) / 2) + self.action_dimension = config.ACTION_DIM + self.expert_number = config.EXPERT_NUM self.observation_scaling = ( - torch.tensor(observation_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) self.action_scaling = ( - torch.tensor(action_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) # gating self.gating_net = torch.nn.Sequential( diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py index c80f7bc0b..3ebb2d7e1 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py @@ -57,33 +57,24 @@ class NonlinearPolicy(torch.nn.Module): linear2: The second linear neural network layer. """ - def __init__( - self, - observation_dimension: int, - action_dimension: int, - observation_scaling: np.ndarray, - action_scaling: np.ndarray, - ) -> None: + def __init__(self, config: config.Config) -> None: """Initializes the NonlinearPolicy class. Initializes the NonlinearPolicy class by setting fixed and variable attributes. Args: - observation_dimension: An integer defining the observation dimension. - action_dimension: An integer defining the action dimension. - observation_scaling: A NumPy array of shape (O) defining the observation scaling. - action_scaling: A NumPy array of shape (A) defining the action scaling. + config: An instance of the configuration class. """ super().__init__() self.name = "NonlinearPolicy" - self.observation_dimension = observation_dimension - self.hidden_dimension = int((observation_dimension + action_dimension) / 2) - self.action_dimension = action_dimension + self.observation_dimension = config.OBSERVATION_DIM + self.hidden_dimension = int((config.OBSERVATION_DIM + config.ACTION_DIM) / 2) + self.action_dimension = config.ACTION_DIM self.observation_scaling = ( - torch.tensor(observation_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) self.action_scaling = ( - torch.tensor(action_scaling, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) self.linear1 = torch.nn.Linear(self.observation_dimension, self.hidden_dimension) self.activation = torch.nn.Tanh() diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt b/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt index 535112152..243bf94a5 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt +++ b/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt @@ -4,6 +4,7 @@ ###### Requirements without version specifiers ###### black numpy +pyyaml tensorboard torch # From db04afa1da7f3fdd6c36cec3fd0ad943e6225a2f Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 21 Apr 2022 17:24:05 +0200 Subject: [PATCH 208/512] move training code into common class --- .gitignore | 1 - ocs2_doc/docs/mpcnet.rst | 12 +- .../python/ocs2_ballbot_mpcnet/helper.py | 142 ------ .../python/ocs2_ballbot_mpcnet/mpcnet.py | 321 ++++--------- .../python/ocs2_ballbot_mpcnet/train.py | 69 +++ .../python/ocs2_legged_robot_mpcnet/helper.py | 295 ------------ .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 445 +++++++++--------- .../python/ocs2_legged_robot_mpcnet/train.py | 71 +++ .../python/ocs2_mpcnet_core/helper.py | 21 +- .../python/ocs2_mpcnet_core/mpcnet.py | 322 +++++++++++++ .../python/ocs2_mpcnet_core/policy/linear.py | 1 - .../policy/mixture_of_linear_experts.py | 1 - .../policy/mixture_of_nonlinear_experts.py | 1 - .../ocs2_mpcnet_core/policy/nonlinear.py | 1 - 14 files changed, 815 insertions(+), 888 deletions(-) delete mode 100644 ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py create mode 100644 ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py delete mode 100644 ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py create mode 100644 ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py create mode 100644 ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py diff --git a/.gitignore b/.gitignore index 53dc52591..46f83bb33 100644 --- a/.gitignore +++ b/.gitignore @@ -25,5 +25,4 @@ ocs2_ddp/test/ddp_test_generated/ *.out *.synctex.gz .vscode/ -policies/ runs/ diff --git a/ocs2_doc/docs/mpcnet.rst b/ocs2_doc/docs/mpcnet.rst index 2427c6a1a..11e8f44d8 100644 --- a/ocs2_doc/docs/mpcnet.rst +++ b/ocs2_doc/docs/mpcnet.rst @@ -66,13 +66,13 @@ To train an MPC-Net policy, run: cd <path_to_ocs2_repo>/ocs2_mpcnet/ocs2_<robot>_mpcnet/python/ocs2_<robot>_mpcnet source <path_to_catkin_ws>/devel/setup.bash source <path_to_venvs>/mpcnet/bin/activate - python3 <robot>_mpcnet.py + python3 train.py # Example: cd ~/git/ocs2/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet source ~/catkin_ws/devel/setup.bash source ~/venvs/mpcnet/bin/activate - python3 ballbot_mpcnet.py + python3 train.py To monitor the training progress with Tensorboard, run: @@ -112,7 +112,7 @@ To deploy the default policy stored in the robot-specific package's :code:`polic source devel/setup.bash roslaunch ocs2_ballbot_mpcnet ballbot_mpcnet.launch -To deploy a new policy stored in the robot-specific package's :code:`python/ocs2_<robot>_mpcnet/policies` folder, replace :code:`<path>` with the absolute file path to the final policy and run: +To deploy a new policy stored in the robot-specific package's :code:`python/ocs2_<robot>_mpcnet/runs` folder, replace :code:`<path>` with the absolute file path to the final policy and run: .. code-block:: bash @@ -123,7 +123,7 @@ To deploy a new policy stored in the robot-specific package's :code:`python/ocs2 # Example: cd ~/catkin_ws source devel/setup.bash - roslaunch ocs2_ballbot_mpcnet ballbot_mpcnet.launch policyFile:='/home/user/git/ocs2/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/policies/2022-04-01_12-00-00_ballbot_description/final_policy.onnx' + roslaunch ocs2_ballbot_mpcnet ballbot_mpcnet.launch policyFile:='/home/user/git/ocs2/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/runs/2022-04-01_12-00-00_ballbot_description/final_policy.onnx' How to Set Up a New Robot ~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -141,7 +141,9 @@ The most important classes/files that have to be implemented are: * **<Robot>MpcnetDefinition**: Defines how OCS2 state variables are transformed to the policy observations. and how policy actions are transformed to OCS2 control inputs. * **<Robot>MpcnetInterface**: Provides the interface between C++ and Python, allowing to exchange data and policies. -* **<robot>_mpcnet.py**: Implements the main training script. +* **<robot>.yaml**: Stores the configuration parameters. +* **mpcnet.py**: Adds robot-specific methods, e.g. implements the tasks that the robot should execute, for the MPC-Net training. +* **train.py**: Starts the main training script. References ~~~~~~~~~~ diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py deleted file mode 100644 index 803c67e9a..000000000 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py +++ /dev/null @@ -1,142 +0,0 @@ -############################################################################### -# Copyright (c) 2022, Farbod Farshidian. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -############################################################################### - -"""Ballbot helper functions. - -Provides robot-specific helper functions for ballbot. -""" - -import numpy as np -from typing import Tuple - -from ocs2_mpcnet_core import config -from ocs2_mpcnet_core import helper -from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray - - -def get_default_event_times_and_mode_sequence(duration: float) -> Tuple[np.ndarray, np.ndarray]: - """Get the event times and mode sequence describing the default mode schedule. - - Creates the default event times and mode sequence for a certain time duration. - - Args: - duration: The duration of the mode schedule given by a float. - - Returns: - A tuple containing the components of the mode schedule. - - event_times: The event times given by a NumPy array of shape (K-1) containing floats. - - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. - """ - event_times_template = np.array([1.0], dtype=np.float64) - mode_sequence_template = np.array([0], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(0, duration, event_times_template, mode_sequence_template) - - -def get_random_initial_state(state_dimension: int, default_state: [float]) -> np.ndarray: - """Get a random initial state. - - Samples a random initial state for the robot. - - Args: - state_dimension: The dimension of the state given by an integer. - default_state: The default state given by a Python array containing floats. - - Returns: - x: A random initial state given by a NumPy array containing floats. - """ - max_linear_velocity_x = 0.5 - max_linear_velocity_y = 0.5 - max_euler_angle_derivative_z = 45.0 / 180.0 * np.pi - max_euler_angle_derivative_y = 45.0 / 180.0 * np.pi - max_euler_angle_derivative_x = 45.0 / 180.0 * np.pi - random_deviation = np.zeros(state_dimension) - random_deviation[5] = np.random.uniform(-max_linear_velocity_x, max_linear_velocity_x) - random_deviation[6] = np.random.uniform(-max_linear_velocity_y, max_linear_velocity_y) - random_deviation[7] = np.random.uniform(-max_euler_angle_derivative_z, max_euler_angle_derivative_z) - random_deviation[8] = np.random.uniform(-max_euler_angle_derivative_y, max_euler_angle_derivative_y) - random_deviation[9] = np.random.uniform(-max_euler_angle_derivative_x, max_euler_angle_derivative_x) - return np.array(default_state) + random_deviation - - -def get_random_target_state(target_state_dimension: int, default_target_state: [float]) -> np.ndarray: - """Get a random target state. - - Samples a random target state for the robot. - - Args: - target_state_dimension: The dimension of the target state given by an integer. - default_target_state: The default target state given by a Python array containing floats. - - Returns: - x: A random target state given by a NumPy array containing floats. - """ - max_position_x = 1.0 - max_position_y = 1.0 - max_orientation_z = 45.0 / 180.0 * np.pi - random_deviation = np.zeros(target_state_dimension) - random_deviation[0] = np.random.uniform(-max_position_x, max_position_x) - random_deviation[1] = np.random.uniform(-max_position_y, max_position_y) - random_deviation[2] = np.random.uniform(-max_orientation_z, max_orientation_z) - return np.array(default_target_state) + random_deviation - - -def get_tasks( - config: config.Config, tasks_number: int, duration: float -) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: - """Get tasks. - - Get a random set of task that should be executed by the data generation or policy evaluation. - - Args: - config: An instance of the configuration class. - tasks_number: Number of tasks given by an integer. - duration: Duration of each task given by a float. - - Returns: - A tuple containing the components of the task. - - initial_observations: The initial observations given by an OCS2 system observation array. - - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. - - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. - """ - initial_observations = helper.get_system_observation_array(tasks_number) - mode_schedules = helper.get_mode_schedule_array(tasks_number) - target_trajectories = helper.get_target_trajectories_array(tasks_number) - for i in range(tasks_number): - initial_observations[i] = helper.get_system_observation( - 0, 0.0, get_random_initial_state(config.STATE_DIM, config.DEFAULT_STATE), np.zeros(config.INPUT_DIM) - ) - mode_schedules[i] = helper.get_mode_schedule(*get_default_event_times_and_mode_sequence(duration)) - target_trajectories[i] = helper.get_target_trajectories( - duration * np.ones((1, 1)), - get_random_target_state(config.TARGET_STATE_DIM, config.DEFAULT_TARGET_STATE).reshape( - (1, config.TARGET_STATE_DIM) - ), - np.zeros((1, config.TARGET_INPUT_DIM)), - ) - return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index d6b5ff1ff..e5fcffb06 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python3 - ############################################################################### # Copyright (c) 2022, Farbod Farshidian. All rights reserved. # @@ -29,224 +27,109 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### -"""Ballbot MPC-Net. +"""Ballbot MPC-Net class. -Main script for training an MPC-Net policy for ballbot. +Provides a class that handles the MPC-Net training for ballbot. """ -import os -import sys -import time -import datetime -import torch import numpy as np - -from torch.utils.tensorboard import SummaryWriter - -from ocs2_mpcnet_core.config import Config -from ocs2_mpcnet_core.helper import bmv, bmm -from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss as Loss -from ocs2_mpcnet_core.memory.circular import CircularMemory as Memory -from ocs2_mpcnet_core.policy.linear import LinearPolicy as Policy - -from ocs2_ballbot_mpcnet import helper -from ocs2_ballbot_mpcnet import MpcnetInterface - - -def main(config_file_path: str) -> None: - # config - config = Config(config_file_path) - - # mpcnet interface - mpcnet_interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) - - # logging - folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + config.DESCRIPTION - writer = SummaryWriter("runs/" + folder) - os.makedirs(name="policies/" + folder) - - # loss - loss = Loss() - - # memory - memory = Memory(config) - - # policy - policy = Policy(config) - policy.to(config.DEVICE) - print("Initial policy parameters:") - print(list(policy.named_parameters())) - dummy_observation = torch.randn(1, config.OBSERVATION_DIM, device=config.DEVICE, dtype=config.DTYPE) - print("Saving initial policy.") - save_path = "policies/" + folder + "/initial_policy" - torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") - torch.save(obj=policy, f=save_path + ".pt") - - # optimizer - optimizer = torch.optim.Adam(policy.parameters(), lr=config.LEARNING_RATE) - - def start_data_generation(policy, alpha=1.0): - policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - config, - config.DATA_GENERATION_TASKS, - config.DATA_GENERATION_DURATION, - ) - mpcnet_interface.startDataGeneration( - alpha, - policy_file_path, - config.DATA_GENERATION_TIME_STEP, - config.DATA_GENERATION_DATA_DECIMATION, - config.DATA_GENERATION_SAMPLES, - np.diag(np.power(np.array(config.DATA_GENERATION_SAMPLING_VARIANCE), 2)), - initial_observations, - mode_schedules, - target_trajectories, - ) - - def start_policy_evaluation(policy, alpha=0.0): - policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - config, - config.POLICY_EVALUATION_TASKS, - config.POLICY_EVALUATION_DURATION, - ) - mpcnet_interface.startPolicyEvaluation( - alpha, - policy_file_path, - config.POLICY_EVALUATION_TIME_STEP, - initial_observations, - mode_schedules, - target_trajectories, - ) - - try: - print("==============\nWaiting for first data.\n==============") - start_data_generation(policy) - start_policy_evaluation(policy) - while not mpcnet_interface.isDataGenerationDone(): - time.sleep(1.0) - - print("==============\nStarting training.\n==============") - for iteration in range(config.LEARNING_ITERATIONS): - alpha = 1.0 - 1.0 * iteration / config.LEARNING_ITERATIONS - - # data generation - if mpcnet_interface.isDataGenerationDone(): - # get generated data - data = mpcnet_interface.getGeneratedData() - for i in range(len(data)): - # push t, x, u, p, observation, action transformation, Hamiltonian into memory - memory.push( - data[i].t, - data[i].x, - data[i].u, - torch.ones(1, device=config.DEVICE, dtype=config.DTYPE), - data[i].observation, - data[i].actionTransformation, - data[i].hamiltonian, - ) - # logging - writer.add_scalar("data/new_data_points", len(data), iteration) - writer.add_scalar("data/total_data_points", len(memory), iteration) - print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) - # start new data generation - start_data_generation(policy, alpha) - - # policy evaluation - if mpcnet_interface.isPolicyEvaluationDone(): - # get computed metrics - metrics = mpcnet_interface.getComputedMetrics() - survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) - incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) - # logging - writer.add_scalar("metric/survival_time", survival_time, iteration) - writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) - print( - "iteration", - iteration, - "received metrics:", - "incurred_hamiltonian", - incurred_hamiltonian, - "survival_time", - survival_time, - ) - # start new policy evaluation - start_policy_evaluation(policy) - - # intermediate policies - if (iteration % 1000 == 0) and (iteration > 0): - print("Saving intermediate policy for iteration", iteration) - save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) - torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") - torch.save(obj=policy, f=save_path + ".pt") - - # extract batch from memory - ( - t, - x, - u, - p, - observation, - action_transformation_matrix, - action_transformation_vector, - dHdxx, - dHdux, - dHduu, - dHdx, - dHdu, - H, - ) = memory.sample(config.BATCH_SIZE) - - # take an optimization step - def closure(): - # clear the gradients - optimizer.zero_grad() - # prediction - action = policy(observation) - input = bmv(action_transformation_matrix, action) + action_transformation_vector - # compute the empirical loss - empirical_loss = loss(x, x, input, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) - # compute the gradients - empirical_loss.backward() - # logging - writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) - # return empirical loss - return empirical_loss - - optimizer.step(closure) - - # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) - if iteration == config.LEARNING_ITERATIONS - 1: - while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): - time.sleep(1.0) - - print("==============\nTraining completed.\n==============") - - except KeyboardInterrupt: - # let data generation and policy evaluation finish (to avoid a segmentation fault) - while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): - time.sleep(1.0) - print("==============\nTraining interrupted.\n==============") - pass - - print("Final policy parameters:") - print(list(policy.named_parameters())) - - print("Saving final policy.") - save_path = "policies/" + folder + "/final_policy" - torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") - torch.save(obj=policy, f=save_path + ".pt") - - writer.close() - - print("Done. Exiting now.") - - -if __name__ == "__main__": - if len(sys.argv) > 1: - main(sys.argv[1]) - else: - main("./config/ballbot.yaml") +from typing import Tuple + +from ocs2_mpcnet_core import helper +from ocs2_mpcnet_core import mpcnet +from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray + + +class BallbotMpcnet(mpcnet.Mpcnet): + """Ballbot MPC-Net. + + Adds robot-specific methods for the MPC-Net training. + """ + + @staticmethod + def get_default_event_times_and_mode_sequence(duration: float) -> Tuple[np.ndarray, np.ndarray]: + """Get the event times and mode sequence describing the default mode schedule. + + Creates the default event times and mode sequence for a certain time duration. + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ + event_times_template = np.array([1.0], dtype=np.float64) + mode_sequence_template = np.array([0], dtype=np.uintp) + return helper.get_event_times_and_mode_sequence(0, duration, event_times_template, mode_sequence_template) + + def get_random_initial_state(self) -> np.ndarray: + """Get a random initial state. + + Samples a random initial state for the robot. + + Returns: + x: A random initial state given by a NumPy array containing floats. + """ + max_linear_velocity_x = 0.5 + max_linear_velocity_y = 0.5 + max_euler_angle_derivative_z = 45.0 / 180.0 * np.pi + max_euler_angle_derivative_y = 45.0 / 180.0 * np.pi + max_euler_angle_derivative_x = 45.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.STATE_DIM) + random_deviation[5] = np.random.uniform(-max_linear_velocity_x, max_linear_velocity_x) + random_deviation[6] = np.random.uniform(-max_linear_velocity_y, max_linear_velocity_y) + random_deviation[7] = np.random.uniform(-max_euler_angle_derivative_z, max_euler_angle_derivative_z) + random_deviation[8] = np.random.uniform(-max_euler_angle_derivative_y, max_euler_angle_derivative_y) + random_deviation[9] = np.random.uniform(-max_euler_angle_derivative_x, max_euler_angle_derivative_x) + return np.array(self.config.DEFAULT_STATE) + random_deviation + + def get_random_target_state(self) -> np.ndarray: + """Get a random target state. + + Samples a random target state for the robot. + + Returns: + x: A random target state given by a NumPy array containing floats. + """ + max_position_x = 1.0 + max_position_y = 1.0 + max_orientation_z = 45.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.TARGET_STATE_DIM) + random_deviation[0] = np.random.uniform(-max_position_x, max_position_x) + random_deviation[1] = np.random.uniform(-max_position_y, max_position_y) + random_deviation[2] = np.random.uniform(-max_orientation_z, max_orientation_z) + return np.array(self.config.DEFAULT_TARGET_STATE) + random_deviation + + def get_tasks( + self, tasks_number: int, duration: float + ) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: + """Get tasks. + + Get a random set of task that should be executed by the data generation or policy evaluation. + + Args: + tasks_number: Number of tasks given by an integer. + duration: Duration of each task given by a float. + + Returns: + A tuple containing the components of the task. + - initial_observations: The initial observations given by an OCS2 system observation array. + - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. + - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. + """ + initial_observations = helper.get_system_observation_array(tasks_number) + mode_schedules = helper.get_mode_schedule_array(tasks_number) + target_trajectories = helper.get_target_trajectories_array(tasks_number) + for i in range(tasks_number): + initial_observations[i] = helper.get_system_observation( + 0, 0.0, self.get_random_initial_state(), np.zeros(self.config.INPUT_DIM) + ) + mode_schedules[i] = helper.get_mode_schedule(*self.get_default_event_times_and_mode_sequence(duration)) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + self.get_random_target_state().reshape((1, self.config.TARGET_STATE_DIM)), + np.zeros((1, self.config.TARGET_INPUT_DIM)), + ) + return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py new file mode 100644 index 000000000..199b63230 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 + +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Ballbot MPC-Net. + +Main script for training an MPC-Net policy for ballbot. +""" + +import sys + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss +from ocs2_mpcnet_core.memory.circular import CircularMemory +from ocs2_mpcnet_core.policy.linear import LinearPolicy + +from ocs2_ballbot_mpcnet.mpcnet import BallbotMpcnet +from ocs2_ballbot_mpcnet import MpcnetInterface + + +def main(config_file_path: str) -> None: + # config + config = Config(config_file_path) + # interface + interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) + # loss + loss = HamiltonianLoss() + # memory + memory = CircularMemory(config) + # policy + policy = LinearPolicy(config) + # mpcnet + mpcnet = BallbotMpcnet(config, interface, memory, policy, loss) + # train + mpcnet.train() + + +if __name__ == "__main__": + if len(sys.argv) > 1: + main(sys.argv[1]) + else: + main("./config/ballbot.yaml") diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py deleted file mode 100644 index 4029963f1..000000000 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py +++ /dev/null @@ -1,295 +0,0 @@ -############################################################################### -# Copyright (c) 2022, Farbod Farshidian. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# * Neither the name of the copyright holder nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -############################################################################### - -"""Legged robot helper functions. - -Provides robot-specific helper functions for legged robot. -""" - -import random -import numpy as np -from typing import Tuple, Dict - -from ocs2_mpcnet_core import config -from ocs2_mpcnet_core import helper -from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray - - -def get_stance(duration: float) -> Tuple[np.ndarray, np.ndarray]: - """Get the stance gait. - - Creates the stance event times and mode sequence for a certain time duration: - - contact schedule: STANCE - - swing schedule: - - - Args: - duration: The duration of the mode schedule given by a float. - - Returns: - A tuple containing the components of the mode schedule. - - event_times: The event times given by a NumPy array of shape (K-1) containing floats. - - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. - """ - event_times_template = np.array([1.0], dtype=np.float64) - mode_sequence_template = np.array([15], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) - - -def get_random_initial_state_stance(state_dimension: int, default_state: [float]) -> np.ndarray: - """Get a random initial state for stance. - - Samples a random initial state for the robot in the stance gait. - - Args: - state_dimension: The dimension of the state given by an integer. - default_state: The default state given by a Python array containing floats. - - Returns: - x: A random initial state given by a NumPy array containing floats. - """ - max_normalized_linear_momentum_x = 0.1 - max_normalized_linear_momentum_y = 0.1 - max_normalized_linear_momentum_z = 0.1 - max_normalized_angular_momentum_x = 1.62079 / 52.1348 * 30.0 / 180.0 * np.pi - max_normalized_angular_momentum_y = 4.83559 / 52.1348 * 30.0 / 180.0 * np.pi - max_normalized_angular_momentum_z = 4.72382 / 52.1348 * 30.0 / 180.0 * np.pi - random_deviation = np.zeros(state_dimension) - random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) - random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) - random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) - random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) - random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) - random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) - return np.array(default_state) + random_deviation - - -def get_random_target_state_stance(target_state_dimension: int, default_target_state: [float]) -> np.ndarray: - """Get a random target state for stance. - - Samples a random target state for the robot in the stance gait. - - Args: - target_state_dimension: The dimension of the target state given by an integer. - default_target_state: The default target state given by a Python array containing floats. - - Returns: - x: A random target state given by a NumPy array containing floats. - """ - max_position_z = 0.075 - max_orientation_z = 25.0 / 180.0 * np.pi - max_orientation_y = 15.0 / 180.0 * np.pi - max_orientation_x = 25.0 / 180.0 * np.pi - random_deviation = np.zeros(target_state_dimension) - random_deviation[8] = np.random.uniform(-max_position_z, max_position_z) - random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) - random_deviation[10] = np.random.uniform(-max_orientation_y, max_orientation_y) - random_deviation[11] = np.random.uniform(-max_orientation_x, max_orientation_x) - return np.array(default_target_state) + random_deviation - - -def get_trot_1(duration: float) -> Tuple[np.ndarray, np.ndarray]: - """Get the first trot gait. - - Creates the first trot event times and mode sequence for a certain time duration: - - contact schedule: LF_RH, RF_LH - - swing schedule: RF_LH, LF_RH - - Args: - duration: The duration of the mode schedule given by a float. - - Returns: - A tuple containing the components of the mode schedule. - - event_times: The event times given by a NumPy array of shape (K-1) containing floats. - - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. - """ - event_times_template = np.array([0.35, 0.7], dtype=np.float64) - mode_sequence_template = np.array([9, 6], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) - - -def get_trot_2(duration: float) -> Tuple[np.ndarray, np.ndarray]: - """Get the second trot gait. - - Creates the second trot event times and mode sequence for a certain time duration: - - contact schedule: RF_LH, LF_RH - - swing schedule: LF_RH, RF_LH - - Args: - duration: The duration of the mode schedule given by a float. - - Returns: - A tuple containing the components of the mode schedule. - - event_times: The event times given by a NumPy array of shape (K-1) containing floats. - - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. - """ - event_times_template = np.array([0.35, 0.7], dtype=np.float64) - mode_sequence_template = np.array([6, 9], dtype=np.uintp) - return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) - - -def get_random_initial_state_trot(state_dimension: int, default_state: [float]) -> np.ndarray: - """Get a random initial state for trot. - - Samples a random initial state for the robot in a trot gait. - - Args: - state_dimension: The dimension of the state given by an integer. - default_state: The default state given by a Python array containing floats. - - Returns: - x: A random initial state given by a NumPy array containing floats. - """ - max_normalized_linear_momentum_x = 0.5 - max_normalized_linear_momentum_y = 0.25 - max_normalized_linear_momentum_z = 0.25 - max_normalized_angular_momentum_x = 1.62079 / 52.1348 * 60.0 / 180.0 * np.pi - max_normalized_angular_momentum_y = 4.83559 / 52.1348 * 60.0 / 180.0 * np.pi - max_normalized_angular_momentum_z = 4.72382 / 52.1348 * 35.0 / 180.0 * np.pi - random_deviation = np.zeros(state_dimension) - random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) - random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) - random_deviation[2] = np.random.uniform(-max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0) - random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) - random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) - random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) - return np.array(default_state) + random_deviation - - -def get_random_target_state_trot(target_state_dimension: int, default_target_state: [float]) -> np.ndarray: - """Get a random target state for trot. - - Samples a random target state for the robot in a trot gait. - - Args: - target_state_dimension: The dimension of the target state given by an integer. - default_target_state: The default target state given by a Python array containing floats. - - Returns: - x: A random target state given by a NumPy array containing floats. - """ - max_position_x = 0.3 - max_position_y = 0.15 - max_orientation_z = 30.0 / 180.0 * np.pi - random_deviation = np.zeros(target_state_dimension) - random_deviation[6] = np.random.uniform(-max_position_x, max_position_x) - random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) - random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) - return np.array(default_target_state) + random_deviation - - -def get_tasks( - config: config.Config, tasks_number: int, duration: float -) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: - """Get tasks. - - Get a random set of task that should be executed by the data generation or policy evaluation. - - Args: - config: An instance of the configuration class. - tasks_number: Number of tasks given by an integer. - duration: Duration of each task given by a float. - - Returns: - A tuple containing the components of the task. - - initial_observations: The initial observations given by an OCS2 system observation array. - - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. - - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. - """ - initial_observations = helper.get_system_observation_array(tasks_number) - mode_schedules = helper.get_mode_schedule_array(tasks_number) - target_trajectories = helper.get_target_trajectories_array(tasks_number) - choices = random.choices( - list(config.WEIGHTS_FOR_GAITS.keys()), k=tasks_number, weights=list(config.WEIGHTS_FOR_GAITS.values()) - ) - for i in range(tasks_number): - if choices[i] == "stance": - initial_observations[i] = helper.get_system_observation( - 15, - 0.0, - get_random_initial_state_stance(config.STATE_DIM, config.DEFAULT_STATE), - np.zeros(config.INPUT_DIM), - ) - mode_schedules[i] = helper.get_mode_schedule(*get_stance(duration)) - target_trajectories[i] = helper.get_target_trajectories( - duration * np.ones((1, 1)), - get_random_target_state_stance(config.TARGET_STATE_DIM, config.DEFAULT_TARGET_STATE).reshape( - (1, config.TARGET_STATE_DIM) - ), - np.zeros((1, config.TARGET_INPUT_DIM)), - ) - elif choices[i] == "trot_1": - initial_observations[i] = helper.get_system_observation( - 15, - 0.0, - get_random_initial_state_trot(config.STATE_DIM, config.DEFAULT_STATE), - np.zeros(config.INPUT_DIM), - ) - mode_schedules[i] = helper.get_mode_schedule(*get_trot_1(duration)) - target_trajectories[i] = helper.get_target_trajectories( - duration * np.ones((1, 1)), - get_random_target_state_trot(config.TARGET_STATE_DIM, config.DEFAULT_TARGET_STATE).reshape( - (1, config.TARGET_STATE_DIM) - ), - np.zeros((1, config.TARGET_INPUT_DIM)), - ) - elif choices[i] == "trot_2": - initial_observations[i] = helper.get_system_observation( - 15, - 0.0, - get_random_initial_state_trot(config.STATE_DIM, config.DEFAULT_STATE), - np.zeros(config.INPUT_DIM), - ) - mode_schedules[i] = helper.get_mode_schedule(*get_trot_2(duration)) - target_trajectories[i] = helper.get_target_trajectories( - duration * np.ones((1, 1)), - get_random_target_state_trot(config.TARGET_STATE_DIM, config.DEFAULT_TARGET_STATE).reshape( - (1, config.TARGET_STATE_DIM) - ), - np.zeros((1, config.TARGET_INPUT_DIM)), - ) - return initial_observations, mode_schedules, target_trajectories - - -def get_one_hot(mode: int, expert_number: int, expert_for_mode: Dict[int, int]) -> np.ndarray: - """Get one hot encoding of mode. - - Get a one hot encoding of a mode represented by a discrete probability distribution, where the sample space is the - set of P individually identified items given by the set of E individually identified experts. - - Args: - mode: The mode of the system given by an integer. - expert_number: The number of experts given by an integer. - expert_for_mode: A dictionary that assigns modes to experts. - - Returns: - p: Discrete probability distribution given by a NumPy array of shape (P) containing floats. - """ - one_hot = np.zeros(expert_number) - one_hot[expert_for_mode[mode]] = 1.0 - return one_hot diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index c3b291f3b..e9a6e56be 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python3 - ############################################################################### # Copyright (c) 2022, Farbod Farshidian. All rights reserved. # @@ -29,230 +27,235 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ############################################################################### -"""Legged robot MPC-Net. +"""Legged robot MPC-Net class. -Main script for training an MPC-Net policy for legged robot. +Provides a class that handles the MPC-Net training for legged robot. """ -import os -import sys -import time -import datetime -import torch +import random import numpy as np - -from torch.utils.tensorboard import SummaryWriter - -from ocs2_mpcnet_core.config import Config -from ocs2_mpcnet_core.helper import bmv, bmm -from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss as ExpertsLoss -from ocs2_mpcnet_core.loss.cross_entropy import CrossEntropyLoss as GatingLoss -from ocs2_mpcnet_core.memory.circular import CircularMemory as Memory -from ocs2_mpcnet_core.policy.mixture_of_nonlinear_experts import MixtureOfNonlinearExpertsPolicy as Policy - -from ocs2_legged_robot_mpcnet import helper -from ocs2_legged_robot_mpcnet import MpcnetInterface - - -def main(config_file_path: str) -> None: - # config - config = Config(config_file_path) - - # mpcnet interface - mpcnet_interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) - - # logging - folder = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME + "_" + config.DESCRIPTION - writer = SummaryWriter("runs/" + folder) - os.makedirs(name="policies/" + folder) - - # loss - experts_loss = ExpertsLoss() - gating_loss = GatingLoss(config) - - # memory - memory = Memory(config) - - # policy - policy = Policy(config) - policy.to(config.DEVICE) - print("Initial policy parameters:") - print(list(policy.named_parameters())) - dummy_observation = torch.randn(1, config.OBSERVATION_DIM, device=config.DEVICE, dtype=config.DTYPE) - print("Saving initial policy.") - save_path = "policies/" + folder + "/initial_policy" - torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") - torch.save(obj=policy, f=save_path + ".pt") - - # optimizer - optimizer = torch.optim.Adam(policy.parameters(), lr=config.LEARNING_RATE) - - def start_data_generation(policy, alpha=1.0): - policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - config, - config.DATA_GENERATION_TASKS, - config.DATA_GENERATION_DURATION, +from typing import Tuple + +from ocs2_mpcnet_core import helper +from ocs2_mpcnet_core import mpcnet +from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray + + +class LeggedRobotMpcnet(mpcnet.Mpcnet): + """Legged robot MPC-Net. + + Adds robot-specific methods for the MPC-Net training. + """ + + @staticmethod + def get_stance(duration: float) -> Tuple[np.ndarray, np.ndarray]: + """Get the stance gait. + + Creates the stance event times and mode sequence for a certain time duration: + - contact schedule: STANCE + - swing schedule: - + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ + event_times_template = np.array([1.0], dtype=np.float64) + mode_sequence_template = np.array([15], dtype=np.uintp) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) + + def get_random_initial_state_stance(self) -> np.ndarray: + """Get a random initial state for stance. + + Samples a random initial state for the robot in the stance gait. + + Returns: + x: A random initial state given by a NumPy array containing floats. + """ + max_normalized_linear_momentum_x = 0.1 + max_normalized_linear_momentum_y = 0.1 + max_normalized_linear_momentum_z = 0.1 + max_normalized_angular_momentum_x = 1.62079 / 52.1348 * 30.0 / 180.0 * np.pi + max_normalized_angular_momentum_y = 4.83559 / 52.1348 * 30.0 / 180.0 * np.pi + max_normalized_angular_momentum_z = 4.72382 / 52.1348 * 30.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.STATE_DIM) + random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) + random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) + random_deviation[2] = np.random.uniform( + -max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0 ) - mpcnet_interface.startDataGeneration( - alpha, - policy_file_path, - config.DATA_GENERATION_TIME_STEP, - config.DATA_GENERATION_DATA_DECIMATION, - config.DATA_GENERATION_SAMPLES, - np.diag(np.power(np.array(config.DATA_GENERATION_SAMPLING_VARIANCE), 2)), - initial_observations, - mode_schedules, - target_trajectories, + random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) + random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) + random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) + return np.array(self.config.DEFAULT_STATE) + random_deviation + + def get_random_target_state_stance(self) -> np.ndarray: + """Get a random target state for stance. + + Samples a random target state for the robot in the stance gait. + + Returns: + x: A random target state given by a NumPy array containing floats. + """ + max_position_z = 0.075 + max_orientation_z = 25.0 / 180.0 * np.pi + max_orientation_y = 15.0 / 180.0 * np.pi + max_orientation_x = 25.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.TARGET_STATE_DIM) + random_deviation[8] = np.random.uniform(-max_position_z, max_position_z) + random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) + random_deviation[10] = np.random.uniform(-max_orientation_y, max_orientation_y) + random_deviation[11] = np.random.uniform(-max_orientation_x, max_orientation_x) + return np.array(self.config.DEFAULT_TARGET_STATE) + random_deviation + + @staticmethod + def get_trot_1(duration: float) -> Tuple[np.ndarray, np.ndarray]: + """Get the first trot gait. + + Creates the first trot event times and mode sequence for a certain time duration: + - contact schedule: LF_RH, RF_LH + - swing schedule: RF_LH, LF_RH + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ + event_times_template = np.array([0.35, 0.7], dtype=np.float64) + mode_sequence_template = np.array([9, 6], dtype=np.uintp) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) + + @staticmethod + def get_trot_2(duration: float) -> Tuple[np.ndarray, np.ndarray]: + """Get the second trot gait. + + Creates the second trot event times and mode sequence for a certain time duration: + - contact schedule: RF_LH, LF_RH + - swing schedule: LF_RH, RF_LH + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ + event_times_template = np.array([0.35, 0.7], dtype=np.float64) + mode_sequence_template = np.array([6, 9], dtype=np.uintp) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) + + def get_random_initial_state_trot(self) -> np.ndarray: + """Get a random initial state for trot. + + Samples a random initial state for the robot in a trot gait. + + Returns: + x: A random initial state given by a NumPy array containing floats. + """ + max_normalized_linear_momentum_x = 0.5 + max_normalized_linear_momentum_y = 0.25 + max_normalized_linear_momentum_z = 0.25 + max_normalized_angular_momentum_x = 1.62079 / 52.1348 * 60.0 / 180.0 * np.pi + max_normalized_angular_momentum_y = 4.83559 / 52.1348 * 60.0 / 180.0 * np.pi + max_normalized_angular_momentum_z = 4.72382 / 52.1348 * 35.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.STATE_DIM) + random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) + random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) + random_deviation[2] = np.random.uniform( + -max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0 ) - - def start_policy_evaluation(policy, alpha=0.0): - policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" - torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) - initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - config, - config.POLICY_EVALUATION_TASKS, - config.POLICY_EVALUATION_DURATION, + random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) + random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) + random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) + return np.array(self.config.DEFAULT_STATE) + random_deviation + + def get_random_target_state_trot(self) -> np.ndarray: + """Get a random target state for trot. + + Samples a random target state for the robot in a trot gait. + + Returns: + x: A random target state given by a NumPy array containing floats. + """ + max_position_x = 0.3 + max_position_y = 0.15 + max_orientation_z = 30.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.TARGET_STATE_DIM) + random_deviation[6] = np.random.uniform(-max_position_x, max_position_x) + random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) + random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) + return np.array(self.config.DEFAULT_TARGET_STATE) + random_deviation + + def get_tasks( + self, tasks_number: int, duration: float + ) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: + """Get tasks. + + Get a random set of task that should be executed by the data generation or policy evaluation. + + Args: + tasks_number: Number of tasks given by an integer. + duration: Duration of each task given by a float. + + Returns: + A tuple containing the components of the task. + - initial_observations: The initial observations given by an OCS2 system observation array. + - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. + - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. + """ + initial_observations = helper.get_system_observation_array(tasks_number) + mode_schedules = helper.get_mode_schedule_array(tasks_number) + target_trajectories = helper.get_target_trajectories_array(tasks_number) + choices = random.choices( + list(self.config.WEIGHTS_FOR_GAITS.keys()), + k=tasks_number, + weights=list(self.config.WEIGHTS_FOR_GAITS.values()), ) - mpcnet_interface.startPolicyEvaluation( - alpha, - policy_file_path, - config.POLICY_EVALUATION_TIME_STEP, - initial_observations, - mode_schedules, - target_trajectories, - ) - - try: - print("==============\nWaiting for first data.\n==============") - start_data_generation(policy) - start_policy_evaluation(policy) - while not mpcnet_interface.isDataGenerationDone(): - time.sleep(1.0) - - print("==============\nStarting training.\n==============") - for iteration in range(config.LEARNING_ITERATIONS): - alpha = 1.0 - 1.0 * iteration / config.LEARNING_ITERATIONS - - # data generation - if mpcnet_interface.isDataGenerationDone(): - # get generated data - data = mpcnet_interface.getGeneratedData() - for i in range(len(data)): - # push t, x, u, p, observation, action transformation, Hamiltonian into memory - memory.push( - data[i].t, - data[i].x, - data[i].u, - helper.get_one_hot(data[i].mode, config.EXPERT_NUM, config.EXPERT_FOR_MODE), - data[i].observation, - data[i].actionTransformation, - data[i].hamiltonian, - ) - # logging - writer.add_scalar("data/new_data_points", len(data), iteration) - writer.add_scalar("data/total_data_points", len(memory), iteration) - print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) - # start new data generation - start_data_generation(policy, alpha) - - # policy evaluation - if mpcnet_interface.isPolicyEvaluationDone(): - # get computed metrics - metrics = mpcnet_interface.getComputedMetrics() - survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) - incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) - # logging - writer.add_scalar("metric/survival_time", survival_time, iteration) - writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) - print( - "iteration", - iteration, - "received metrics:", - "incurred_hamiltonian", - incurred_hamiltonian, - "survival_time", - survival_time, + for i in range(tasks_number): + if choices[i] == "stance": + initial_observations[i] = helper.get_system_observation( + 15, + 0.0, + self.get_random_initial_state_stance(), + np.zeros(self.config.INPUT_DIM), + ) + mode_schedules[i] = helper.get_mode_schedule(*self.get_stance(duration)) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + self.get_random_target_state_stance().reshape((1, self.config.TARGET_STATE_DIM)), + np.zeros((1, self.config.TARGET_INPUT_DIM)), + ) + elif choices[i] == "trot_1": + initial_observations[i] = helper.get_system_observation( + 15, + 0.0, + self.get_random_initial_state_trot(), + np.zeros(self.config.INPUT_DIM), + ) + mode_schedules[i] = helper.get_mode_schedule(*self.get_trot_1(duration)) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + self.get_random_target_state_trot().reshape((1, self.config.TARGET_STATE_DIM)), + np.zeros((1, self.config.TARGET_INPUT_DIM)), + ) + elif choices[i] == "trot_2": + initial_observations[i] = helper.get_system_observation( + 15, + 0.0, + self.get_random_initial_state_trot(), + np.zeros(self.config.INPUT_DIM), + ) + mode_schedules[i] = helper.get_mode_schedule(*self.get_trot_2(duration)) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + self.get_random_target_state_trot().reshape((1, self.config.TARGET_STATE_DIM)), + np.zeros((1, self.config.TARGET_INPUT_DIM)), ) - # start new policy evaluation - start_policy_evaluation(policy) - - # intermediate policies - if (iteration % 10000 == 0) and (iteration > 0): - print("Saving intermediate policy for iteration", iteration) - save_path = "policies/" + folder + "/intermediate_policy_" + str(iteration) - torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") - torch.save(obj=policy, f=save_path + ".pt") - - # extract batch from memory - ( - t, - x, - u, - p, - observation, - action_transformation_matrix, - action_transformation_vector, - dHdxx, - dHdux, - dHduu, - dHdx, - dHdu, - H, - ) = memory.sample(config.BATCH_SIZE) - - # take an optimization step - def closure(): - # clear the gradients - optimizer.zero_grad() - # prediction - action, expert_weights = policy(observation) - input = bmv(action_transformation_matrix, action) + action_transformation_vector - # compute the empirical loss - empirical_experts_loss = experts_loss(x, x, input, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) - empirical_gating_loss = gating_loss(p, expert_weights) - empirical_loss = empirical_experts_loss + config.LAMBDA * empirical_gating_loss - # compute the gradients - empirical_loss.backward() - # logging - writer.add_scalar("objective/empirical_experts_loss", empirical_experts_loss.item(), iteration) - writer.add_scalar("objective/empirical_gating_loss", empirical_gating_loss.item(), iteration) - writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) - # return empirical loss - return empirical_loss - - optimizer.step(closure) - - # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) - if iteration == config.LEARNING_ITERATIONS - 1: - while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): - time.sleep(1.0) - - print("==============\nTraining completed.\n==============") - - except KeyboardInterrupt: - # let data generation and policy evaluation finish (to avoid a segmentation fault) - while (not mpcnet_interface.isDataGenerationDone()) or (not mpcnet_interface.isPolicyEvaluationDone()): - time.sleep(1.0) - print("==============\nTraining interrupted.\n==============") - pass - - print("Final policy parameters:") - print(list(policy.named_parameters())) - - print("Saving final policy.") - save_path = "policies/" + folder + "/final_policy" - torch.onnx.export(model=policy, args=dummy_observation, f=save_path + ".onnx") - torch.save(obj=policy, f=save_path + ".pt") - - writer.close() - - print("Done. Exiting now.") - - -if __name__ == "__main__": - if len(sys.argv) > 1: - main(sys.argv[1]) - else: - main("./config/legged_robot.yaml") + return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py new file mode 100644 index 000000000..767619466 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Legged robot MPC-Net. + +Main script for training an MPC-Net policy for legged robot. +""" + +import sys + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss +from ocs2_mpcnet_core.loss.cross_entropy import CrossEntropyLoss +from ocs2_mpcnet_core.memory.circular import CircularMemory +from ocs2_mpcnet_core.policy.mixture_of_nonlinear_experts import MixtureOfNonlinearExpertsPolicy + +from ocs2_legged_robot_mpcnet.mpcnet import LeggedRobotMpcnet +from ocs2_legged_robot_mpcnet import MpcnetInterface + + +def main(config_file_path: str) -> None: + # config + config = Config(config_file_path) + # interface + interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) + # loss + experts_loss = HamiltonianLoss() + gating_loss = CrossEntropyLoss(config) + # memory + memory = CircularMemory(config) + # policy + policy = MixtureOfNonlinearExpertsPolicy(config) + # mpcnet + mpcnet = LeggedRobotMpcnet(config, interface, memory, policy, experts_loss, gating_loss) + # train + mpcnet.train() + + +if __name__ == "__main__": + if len(sys.argv) > 1: + main(sys.argv[1]) + else: + main("./config/legged_robot.yaml") diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py index 08da92d2a..120672ab7 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py @@ -34,7 +34,7 @@ import torch import numpy as np -from typing import Tuple +from typing import Tuple, Dict from ocs2_mpcnet_core import ( size_array, @@ -288,3 +288,22 @@ def get_event_times_and_mode_sequence( mode_sequence = np.append(mode_sequence, mode_sequence_template) mode_sequence = np.append(mode_sequence, np.array([default_mode], dtype=np.uintp)) return event_times, mode_sequence + + +def get_one_hot(mode: int, expert_number: int, expert_for_mode: Dict[int, int]) -> np.ndarray: + """Get one hot encoding of mode. + + Get a one hot encoding of a mode represented by a discrete probability distribution, where the sample space is the + set of P individually identified items given by the set of E individually identified experts. + + Args: + mode: The mode of the system given by an integer. + expert_number: The number of experts given by an integer. + expert_for_mode: A dictionary that assigns modes to experts. + + Returns: + p: Discrete probability distribution given by a NumPy array of shape (P) containing floats. + """ + one_hot = np.zeros(expert_number) + one_hot[expert_for_mode[mode]] = 1.0 + return one_hot diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py new file mode 100644 index 000000000..50a26649c --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -0,0 +1,322 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""MPC-Net class. + +Provides a class that handles the MPC-Net training. +""" + +import time +import datetime +import torch +import numpy as np +from typing import Optional, Tuple +from torch.utils.tensorboard import SummaryWriter + + +from ocs2_mpcnet_core import helper +from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss +from ocs2_mpcnet_core.loss.cross_entropy import CrossEntropyLoss +from ocs2_mpcnet_core.memory.circular import CircularMemory + + +class Mpcnet: + """MPC-Net. + + Implements the main methods for the MPC-Net training. + """ + + def __init__( + self, + config: Config, + interface: object, + memory: CircularMemory, + policy: torch.nn.Module, + experts_loss: HamiltonianLoss, + gating_loss: Optional[CrossEntropyLoss] = None, + ) -> None: + """Initializes the Mpcnet class. + + Initializes the Mpcnet class by setting fixed and variable attributes. + + Args: + config: An instance of the configuration class. + interface: An instance of the interface class. + memory: An instance of a memory class. + policy: An instance of a policy class. + experts_loss: An instance of a loss class used as experts loss. + gating_loss: An instance of a loss class used as gating loss. + """ + # config + self.config = config + # interface + self.interface = interface + # logging + self.log_dir = ( + "./runs/" + + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + + "_" + + config.NAME + + "_" + + config.DESCRIPTION + ) + self.writer = SummaryWriter(self.log_dir) + # loss + self.experts_loss = experts_loss + self.gating_loss = gating_loss + # memory + self.memory = memory + # policy + self.policy = policy + self.policy.to(config.DEVICE) + self.dummy_observation = torch.randn(1, config.OBSERVATION_DIM, device=config.DEVICE, dtype=config.DTYPE) + # optimizer + self.optimizer = torch.optim.Adam(self.policy.parameters(), lr=config.LEARNING_RATE) + + def get_tasks( + self, tasks_number: int, duration: float + ) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: + """Get tasks. + + Get a random set of task that should be executed by the data generation or policy evaluation. + + Args: + tasks_number: Number of tasks given by an integer. + duration: Duration of each task given by a float. + + Returns: + A tuple containing the components of the task. + - initial_observations: The initial observations given by an OCS2 system observation array. + - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. + - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. + """ + raise NotImplementedError() + + def start_data_generation(self, policy: torch.nn.Module, alpha: float = 1.0): + """Start data generation. + + Start the data generation rollouts to receive new data. + + Args: + policy: The current learned policy. + alpha: The weight of the MPC policy in the rollouts. + """ + policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=self.dummy_observation, f=policy_file_path) + initial_observations, mode_schedules, target_trajectories = self.get_tasks( + self.config.DATA_GENERATION_TASKS, + self.config.DATA_GENERATION_DURATION, + ) + self.interface.startDataGeneration( + alpha, + policy_file_path, + self.config.DATA_GENERATION_TIME_STEP, + self.config.DATA_GENERATION_DATA_DECIMATION, + self.config.DATA_GENERATION_SAMPLES, + np.diag(np.power(np.array(self.config.DATA_GENERATION_SAMPLING_VARIANCE), 2)), + initial_observations, + mode_schedules, + target_trajectories, + ) + + def start_policy_evaluation(self, policy: torch.nn.Module, alpha: float = 0.0): + """Start policy evaluation. + + Start the policy evaluation rollouts to validate the current performance. + + Args: + policy: The current learned policy. + alpha: The weight of the MPC policy in the rollouts. + """ + policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=self.dummy_observation, f=policy_file_path) + initial_observations, mode_schedules, target_trajectories = self.get_tasks( + self.config.POLICY_EVALUATION_TASKS, + self.config.POLICY_EVALUATION_DURATION, + ) + self.interface.startPolicyEvaluation( + alpha, + policy_file_path, + self.config.POLICY_EVALUATION_TIME_STEP, + initial_observations, + mode_schedules, + target_trajectories, + ) + + def train(self) -> None: + """Train. + + Run the main training loop of MPC-Net. + """ + try: + # save initial policy + save_path = self.log_dir + "/initial_policy" + torch.onnx.export(model=self.policy, args=self.dummy_observation, f=save_path + ".onnx") + torch.save(obj=self.policy, f=save_path + ".pt") + + print("==============\nWaiting for first data.\n==============") + self.start_data_generation(self.policy) + self.start_policy_evaluation(self.policy) + while not self.interface.isDataGenerationDone(): + time.sleep(1.0) + + print("==============\nStarting training.\n==============") + for iteration in range(self.config.LEARNING_ITERATIONS): + alpha = 1.0 - 1.0 * iteration / self.config.LEARNING_ITERATIONS + + # data generation + if self.interface.isDataGenerationDone(): + # get generated data + data = self.interface.getGeneratedData() + for i in range(len(data)): + # push t, x, u, p, observation, action transformation, Hamiltonian into memory + self.memory.push( + data[i].t, + data[i].x, + data[i].u, + helper.get_one_hot(data[i].mode, self.config.EXPERT_NUM, self.config.EXPERT_FOR_MODE), + data[i].observation, + data[i].actionTransformation, + data[i].hamiltonian, + ) + # logging + self.writer.add_scalar("data/new_data_points", len(data), iteration) + self.writer.add_scalar("data/total_data_points", len(self.memory), iteration) + print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) + # start new data generation + self.start_data_generation(self.policy, alpha) + + # policy evaluation + if self.interface.isPolicyEvaluationDone(): + # get computed metrics + metrics = self.interface.getComputedMetrics() + survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) + incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) + # logging + self.writer.add_scalar("metric/survival_time", survival_time, iteration) + self.writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) + print( + "iteration", + iteration, + "received metrics:", + "incurred_hamiltonian", + incurred_hamiltonian, + "survival_time", + survival_time, + ) + # start new policy evaluation + self.start_policy_evaluation(self.policy) + + # save intermediate policy + if (iteration % int(0.1 * self.config.LEARNING_ITERATIONS) == 0) and (iteration > 0): + save_path = self.log_dir + "/intermediate_policy_" + str(iteration) + torch.onnx.export(model=self.policy, args=self.dummy_observation, f=save_path + ".onnx") + torch.save(obj=self.policy, f=save_path + ".pt") + + # extract batch from memory + ( + t, + x, + u, + p, + observation, + action_transformation_matrix, + action_transformation_vector, + dHdxx, + dHdux, + dHduu, + dHdx, + dHdu, + H, + ) = self.memory.sample(self.config.BATCH_SIZE) + + # normal closure only evaluating the experts loss for standard networks + def normal_closure(): + # clear the gradients + self.optimizer.zero_grad() + # prediction + action = self.policy(observation) + input = helper.bmv(action_transformation_matrix, action) + action_transformation_vector + # compute the empirical loss + empirical_loss = self.experts_loss(x, x, input, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + # compute the gradients + empirical_loss.backward() + # logging + self.writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) + # return empirical loss + return empirical_loss + + # cheating closure also adding the gating loss for mixture of experts networks + def cheating_closure(): + # clear the gradients + self.optimizer.zero_grad() + # prediction + action, expert_weights = self.policy(observation) + input = helper.bmv(action_transformation_matrix, action) + action_transformation_vector + # compute the empirical loss + empirical_experts_loss = self.experts_loss(x, x, input, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + empirical_gating_loss = self.gating_loss(p, expert_weights) + empirical_loss = empirical_experts_loss + self.config.LAMBDA * empirical_gating_loss + # compute the gradients + empirical_loss.backward() + # logging + self.writer.add_scalar("objective/empirical_experts_loss", empirical_experts_loss.item(), iteration) + self.writer.add_scalar("objective/empirical_gating_loss", empirical_gating_loss.item(), iteration) + self.writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) + # return empirical loss + return empirical_loss + + # take an optimization step + if self.config.CHEATING: + self.optimizer.step(cheating_closure) + else: + self.optimizer.step(normal_closure) + + # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) + if iteration == self.config.LEARNING_ITERATIONS - 1: + while (not self.interface.isDataGenerationDone()) or (not self.interface.isPolicyEvaluationDone()): + time.sleep(1.0) + + print("==============\nTraining completed.\n==============") + + # save final policy + save_path = self.log_dir + "/final_policy" + torch.onnx.export(model=self.policy, args=self.dummy_observation, f=save_path + ".onnx") + torch.save(obj=self.policy, f=save_path + ".pt") + + except KeyboardInterrupt: + # let data generation and policy evaluation finish (to avoid a segmentation fault) + while (not self.interface.isDataGenerationDone()) or (not self.interface.isPolicyEvaluationDone()): + time.sleep(1.0) + print("==============\nTraining interrupted.\n==============") + pass + + self.writer.close() diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py index f85e4b17b..a204a1323 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py @@ -33,7 +33,6 @@ """ import torch -import numpy as np from ocs2_mpcnet_core import config from ocs2_mpcnet_core.helper import bmv diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py index 94a190303..a060de6b3 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py @@ -33,7 +33,6 @@ """ import torch -import numpy as np from typing import Tuple from ocs2_mpcnet_core import config diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py index 9379f9f1b..fc259c516 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py @@ -33,7 +33,6 @@ """ import torch -import numpy as np from typing import Tuple from ocs2_mpcnet_core import config diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py index 3ebb2d7e1..2b3950cfc 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py @@ -33,7 +33,6 @@ """ import torch -import numpy as np from ocs2_mpcnet_core import config from ocs2_mpcnet_core.helper import bmv From 09d2f80dc31ac9e91b2ae4fe9cb6a770b3c0e58b Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 22 Apr 2022 09:21:03 +0200 Subject: [PATCH 209/512] remove legged robot yaml for laptop --- .../config/legged_robot_laptop.yaml | 239 ------------------ 1 file changed, 239 deletions(-) delete mode 100644 ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot_laptop.yaml diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot_laptop.yaml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot_laptop.yaml deleted file mode 100644 index 285a4e6fa..000000000 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot_laptop.yaml +++ /dev/null @@ -1,239 +0,0 @@ -config: - # - # general - # - # name of the robot - NAME: "legged_robot" - # description of the training run - DESCRIPTION: "description" - # state dimension - STATE_DIM: 24 - # input dimension - INPUT_DIM: 24 - # target trajectories state dimension - TARGET_STATE_DIM: 24 - # target trajectories input dimension - TARGET_INPUT_DIM: 24 - # observation dimension - OBSERVATION_DIM: 36 - # action dimension - ACTION_DIM: 24 - # expert number - EXPERT_NUM: 3 - # default state - DEFAULT_STATE: - - 0.0 # normalized linear momentum x - - 0.0 # normalized linear momentum y - - 0.0 # normalized linear momentum z - - 0.0 # normalized angular momentum x - - 0.0 # normalized angular momentum y - - 0.0 # normalized angular momentum z - - 0.0 # position x - - 0.0 # position y - - 0.575 # position z - - 0.0 # orientation z - - 0.0 # orientation y - - 0.0 # orientation x - - -0.25 # joint position LF HAA - - 0.6 # joint position LF HFE - - -0.85 # joint position LF KFE - - -0.25 # joint position LH HAA - - -0.6 # joint position LH HFE - - 0.85 # joint position LH KFE - - 0.25 # joint position RF HAA - - 0.6 # joint position RF HFE - - -0.85 # joint position RF KFE - - 0.25 # joint position RH HAA - - -0.6 # joint position RH HFE - - 0.85 # joint position RH KFE - # default target state - DEFAULT_TARGET_STATE: - - 0.0 # normalized linear momentum x - - 0.0 # normalized linear momentum y - - 0.0 # normalized linear momentum z - - 0.0 # normalized angular momentum x - - 0.0 # normalized angular momentum y - - 0.0 # normalized angular momentum z - - 0.0 # position x - - 0.0 # position y - - 0.575 # position z - - 0.0 # orientation z - - 0.0 # orientation y - - 0.0 # orientation x - - -0.25 # joint position LF HAA - - 0.6 # joint position LF HFE - - -0.85 # joint position LF KFE - - -0.25 # joint position LH HAA - - -0.6 # joint position LH HFE - - 0.85 # joint position LH KFE - - 0.25 # joint position RF HAA - - 0.6 # joint position RF HFE - - -0.85 # joint position RF KFE - - 0.25 # joint position RH HAA - - -0.6 # joint position RH HFE - - 0.85 # joint position RH KFE - # - # loss - # - # epsilon to improve numerical stability of logs and denominators - EPSILON: 1.e-8 - # whether to cheat by adding the gating loss - CHEATING: True - # parameter to control the relative importance of both loss types - LAMBDA: 10.0 - # dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) - EXPERT_FOR_MODE: - 6: 1 # trot - 9: 2 # trot - 15: 0 # stance - # input cost for behavioral cloning - R: - - 0.001 # contact force LF x - - 0.001 # contact force LF y - - 0.001 # contact force LF z - - 0.001 # contact force LH x - - 0.001 # contact force LH y - - 0.001 # contact force LH z - - 0.001 # contact force RF x - - 0.001 # contact force RF y - - 0.001 # contact force RF z - - 0.001 # contact force RH x - - 0.001 # contact force RH y - - 0.001 # contact force RH z - - 5.0 # joint velocity LF HAA - - 5.0 # joint velocity LF HFE - - 5.0 # joint velocity LF KFE - - 5.0 # joint velocity LH HAA - - 5.0 # joint velocity LH HFE - - 5.0 # joint velocity LH KFE - - 5.0 # joint velocity RF HAA - - 5.0 # joint velocity RF HFE - - 5.0 # joint velocity RF KFE - - 5.0 # joint velocity RH HAA - - 5.0 # joint velocity RH HFE - - 5.0 # joint velocity RH KFE - # - # memory - # - # capacity of the memory - CAPACITY: 100000 - # - # policy - # - # observation scaling - OBSERVATION_SCALING: - - 1.0 # swing phase LF - - 1.0 # swing phase LH - - 1.0 # swing phase RF - - 1.0 # swing phase RH - - 1.0 # swing phase rate LF - - 1.0 # swing phase rate LH - - 1.0 # swing phase rate RF - - 1.0 # swing phase rate RH - - 1.0 # sinusoidal bump LF - - 1.0 # sinusoidal bump LH - - 1.0 # sinusoidal bump RF - - 1.0 # sinusoidal bump RH - - 1.0 # normalized linear momentum x - - 1.0 # normalized linear momentum y - - 1.0 # normalized linear momentum z - - 1.0 # normalized angular momentum x - - 1.0 # normalized angular momentum y - - 1.0 # normalized angular momentum z - - 1.0 # position x - - 1.0 # position y - - 1.0 # position z - - 1.0 # orientation z - - 1.0 # orientation y - - 1.0 # orientation x - - 1.0 # joint position LF HAA - - 1.0 # joint position LF HFE - - 1.0 # joint position LF KFE - - 1.0 # joint position LH HAA - - 1.0 # joint position LH HFE - - 1.0 # joint position LH KFE - - 1.0 # joint position RF HAA - - 1.0 # joint position RF HFE - - 1.0 # joint position RF KFE - - 1.0 # joint position RH HAA - - 1.0 # joint position RH HFE - - 1.0 # joint position RH KFE - # action scaling - ACTION_SCALING: - - 100.0 # contact force LF x - - 100.0 # contact force LF y - - 100.0 # contact force LF z - - 100.0 # contact force LH x - - 100.0 # contact force LH y - - 100.0 # contact force LH z - - 100.0 # contact force RF x - - 100.0 # contact force RF y - - 100.0 # contact force RF z - - 100.0 # contact force RH x - - 100.0 # contact force RH y - - 100.0 # contact force RH z - - 10.0 # joint velocity LF HAA - - 10.0 # joint velocity LF HFE - - 10.0 # joint velocity LF KFE - - 10.0 # joint velocity LH HAA - - 10.0 # joint velocity LH HFE - - 10.0 # joint velocity LH KFE - - 10.0 # joint velocity RF HAA - - 10.0 # joint velocity RF HFE - - 10.0 # joint velocity RF KFE - - 10.0 # joint velocity RH HAA - - 10.0 # joint velocity RH HFE - - 10.0 # joint velocity RH KFE - # - # rollout - # - # RaiSim or TimeTriggered rollout for data generation and policy evaluation - RAISIM: True - # weights defining how often a gait is chosen for rollout - WEIGHTS_FOR_GAITS: - stance: 1.0 - trot_1: 2.0 - trot_2: 2.0 - # settings for data generation - DATA_GENERATION_TIME_STEP: 0.0025 - DATA_GENERATION_DURATION: 4.0 - DATA_GENERATION_DATA_DECIMATION: 4 - DATA_GENERATION_THREADS: 6 - DATA_GENERATION_TASKS: 6 - DATA_GENERATION_SAMPLES: 2 - DATA_GENERATION_SAMPLING_VARIANCE: - - 0.05 # normalized linear momentum x - - 0.05 # normalized linear momentum y - - 0.05 # normalized linear momentum z - - 0.00135648942 # normalized angular momentum x: 1.62079 / 52.1348 * 2.5 / 180.0 * pi - - 0.00404705526 # normalized angular momentum y: 4.83559 / 52.1348 * 2.5 / 180.0 * pi - - 0.00395351148 # normalized angular momentum z: 4.72382 / 52.1348 * 2.5 / 180.0 * pi - - 0.01 # position x - - 0.01 # position y - - 0.01 # position z - - 0.00872664625 # orientation z: 0.5 / 180.0 * pi - - 0.00872664625 # orientation y: 0.5 / 180.0 * pi - - 0.00872664625 # orientation x: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LF HAA: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LF HFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LF KFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LH HAA: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LH HFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LH KFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RF HAA: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RF HFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RF KFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RH HAA: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RH HFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RH KFE: 0.5 / 180.0 * pi - # settings for computing metrics - POLICY_EVALUATION_TIME_STEP: 0.0025 - POLICY_EVALUATION_DURATION: 4.0 - POLICY_EVALUATION_THREADS: 1 - POLICY_EVALUATION_TASKS: 1 - # - # training - # - BATCH_SIZE: 128 - LEARNING_RATE: 1.e-3 - LEARNING_ITERATIONS: 100000 From fb64dfef784228042cb89be775cb2674644bfeb0 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 22 Apr 2022 15:36:38 +0200 Subject: [PATCH 210/512] remove weight compensating bias --- .../src/LeggedRobotMpcnetDefinition.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp index b92a3bbeb..8c19df882 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -96,9 +96,10 @@ std::pair<matrix_t, vector_t> LeggedRobotMpcnetDefinition::getActionTransformati actionTransformationMatrix.block<3, 3>(3, 3) = R; actionTransformationMatrix.block<3, 3>(6, 6) = R; actionTransformationMatrix.block<3, 3>(9, 9) = R; - const auto contactFlags = modeNumber2StanceLeg(modeSchedule.modeAtTime(t)); - const vector_t actionTransformationVector = weightCompensatingInput(centroidalModelInfo_, contactFlags); - return {actionTransformationMatrix, actionTransformationVector}; + // TODO(areske): check why less robust with weight compensating bias? + // const auto contactFlags = modeNumber2StanceLeg(modeSchedule.modeAtTime(t)); + // const vector_t actionTransformationVector = weightCompensatingInput(centroidalModelInfo_, contactFlags); + return {actionTransformationMatrix, vector_t::Zero(24)}; } bool LeggedRobotMpcnetDefinition::isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, From 0e1c85bbeca161d37578fe3adb1cc13c7dc3e13d Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 22 Apr 2022 15:36:56 +0200 Subject: [PATCH 211/512] update policy --- .../policy/legged_robot.onnx | Bin 35344 -> 35344 bytes .../policy/legged_robot.pt | Bin 41391 -> 41391 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx index f8e5e13363f59f3b5005542108523b629f62da0e..ef5939cdf95406be90eccda36633f1a68187c0f6 100644 GIT binary patch literal 35344 zcmeGE2T&B-7XOQrvxtCzfQl#xh#*Lw-m4Len8lnB6p^5yfH@FEP(%?BL_`!3MI<W< z(|hePAPOcF#e^9|F()v;IrrXk!~cKouTI@p_3G8DLsi%GOt0zv?a!>ed+)VYqoS;> z9JVSVblLnxh6;8zc7xnHQsvZ~)YL=$!u^*m_l*b)U7{^#=WM7xWl8wbNPqv;{yu|D zRsZ?MN?lFCccp*0yWBomWwlQhQd<{8HP6r`;Ss(|B7E#jRes;Mx@DoJxZF1+(%-7y zLPb`kpGrS1Sucr&j7$_S^Z&ZPzcl^1pX4u2e{uSA1pb`WU;gwLr$0yF&sqKDPk(Xx za|HgJ)nESf7pFf*;Lln8<xhWc`f~*SoYi0c^cSZ;N8ry{{pC-8ar$!v{+!ic{`428 zKS$utSw->tWEIqUsY%0*(#=^Kesncd8S5J{HZsJ=-c;%Dduo=_W3JkA_Ja&nz5fx! z9BiumkKO+aVJ=aVnyKjoNF$$10(_SEN7xLqS>YcTuqZ;U+kbrM7w8)<J*AzY(g^>> zi+vnS6@GiQ>Z+#b8xpq2xBG8#&3&@6R+?%Ge*O{vdf!D&DPo!Ll5h_L*?qEdYI@T4 z?4;+hH&mMJyJV4%qp8AguWG;Pu>TJo_J5?q-sT@V2LD5c)BmVLipYMjREL8UQRu>m z#l9<joTZ5VzWaAXf#H9b93}fVM#lat!~B;;{KhC;3MnLTiNEhMyWd3`{_)X&sCE2@ zT9^M(?Qhf^rD~o2LCy6a)S`cr=A`zA(*DWs52gJRq_d&&-+oZLLI0z+ziAlspBn7` zk9hud^dER!e%E04KWfmIVsMf2aWz!_n^)P{|2rVpKNk1LfPNSEJD|aXeit|Re-!se zfb9Q|fc|y#?|=r|{jTAE?RHW?gQeYWu>J3Ve(!g`i5qPHpMZXE{*M9u>*znk4gOu+ z|JoOAq`#hYC>!ixsPWH4i;tc2e@(kc?FSE%+B^P-z03d6UfS0OJN<{f>wmQ$Bt5@_ zbaRw$PSWo=8>&jPI`e%aeC!9AD*b);pF@N+5hSPf+Z8xSEnWV-1$(>y+Cp#XnVqGZ z)Mp2~8mj(2fd6X*w~`)|j^GZ14Ap)g!R`Os5H4-pL5jn{?mrrL_^*vuYD<mn4b^|o zRQcFD{#)8=m-g>zt3+*S+R8x1K$^CS(w31q@A2nuQU7<W{%YgT)BB6nU#$K-fj@8g z{}sP78S>|^`G2+itBpTT?k`q<vHJ4_{=DV?Gk*2kvRgW9bFi0g(uuUg;D629oc_7) zmd@H7q!UdChku*3{kE$7`x^W|7T(%ge=9;77!(*WP`Y#sT^3<=xx0$aZ$&~Lf3zC+ zD(%i%<YB4ZBb>@Eqlcpd$=V{2d5IO+{fi+h=?sI)tX6JEa4G7oc!=?h8-S!|32fs| z@zQ>=;u71(G(RFzEIOV-#vjp!WG^k~w>ks{+>(&3wK16RBNh!J%Bq!pw?N9^RGev< z#^o55l1noKiSel%vLTZrKc8m71oc-$<Q|Xm3X0HrIv<Q1B58t80?O@p1FGM9@Hy9P zaMzw~Sh(jC^z1bp;$Hi5IrBbp2OqD8`cuj{B0&z073Om3U+3V=Z+?*cLBjcLy-EkX zs=(w;$I$PoGp;F$f?&;J(7bB_)~y<Ny`~2l*sX=z)?5SG)hF=S@q3W4bqJn1G884Y z9$?>NExKLtg>=^v)N<0MM#>wp*l#VC%{Y#O{hng-xhuG!={rJAIT}r$gJq53*m8I< zwhwJ4W2;TbE8SXpr#t~_FqxD*Jw&>g^oAhYLQ=6X6~?(VfXUP-+!=U>Zc{0t^OAbN zul`p^zl<V!Nu&&GWnYtklIvt&?rG?2SwLqVQ9;*hvE+G55$EI4NhX|FisAa%cqMKc zJfE2j>u)Nsx68HI@U7ptgO$c4dbTwxZZ1T<YeV3^%`u@QeW)1u8$ycZYx<&(3rFos zK_}xiIXv8pbpQGVR<>x6*(b7aq3;CTYdae1T{@t>d=Ey4>T&l)Q;FuF2-=*ggr@6F zuxEJ?IFIq5^K_%RC6Gcseg8$4JgUQi8IN(Y-)S<Dvw>{}eb}6DhHUENUhuZ72?W*` z;4AB7+|+lRQ2lZUb@i(xr|TOr<dHy5{b-_lHf#pp6|X?FzL|TH{|>VAJK*A&F6`oK zEmU{8M?Ah3!0B2&=srLXuy#9q9siofujO&OY6e8^O90JES4{Tpqyq;R5clK9aKguH z;M2nfjMT?t-6u^JTM-P+)!X6T07bS&?mg;!-wCgC^1!Gp8jV)F!Q!??`lT%%2X9zS z^r<FEGEpUy&v=9LI0?NyU<b$#jD>C?h;c$ZZtkB#GnEEWomHyvHJ?D+lNUs^?tpN8 zlMY^ba0yITFp?sfO}T0imPXv7Rzu5RVcjZH{eBi^qy)n4hXeR~-9M4O3nVmh>>HT8 zs}yD|RK<kh*QkY2I>>JxKs`mORK5R0B6lwoYu!@lg_`Zy(W3&}<c;75e8(j-FW{v2 zM_~5ACq&`Qa_-jRV9|QHOdO<|Pv4HtrKb+gq5?RP{aXiMxYjrl>NtcZ*^h#_s|{pI zMGd!MRy(LoDS+pRYjF0#dTM%b6t8kXnSZLh2rEbE(~gBj&}G6yuzNclm2dPRR4;+9 zwR#14tGZ!!PJak87>d@#9cUF^K;|B3#&Dw~{BkG*`fsQstt$_ZvsX@HmuqqmlQo)- zpOl719;vYLNij@3yG!i+z7*+^nP8q<PD)3vq$f+0Ft%R?Mwu7ki?Yr5%i{o+jqA=* z_8!6*QFpf6<pRdn>*C`Xr_q1#Y1}*eDv7@|lh_|J$72Bp#A}qJX{)sX?A2Th4flG2 z&hhK8a$+RB%(TR_J>(ICE4ap2?}XteOvUbb?yzo$4W!m0U2>y?_7RoAIsYNFTHc%V zT7ZIS%SHM%Iv3u3orX`VoiU<y6U_HC;ET=;<k!bp^J_LJ^S@f^LA+U?tgs#_epuQD zqkF{D5xpX)!H@lLZf_7iI-f_*?z&F%-iG3;nSQkCxB=_4p$qHrqZpz!g28t2B(Chr zH~P#n8gJL`qGexe=$`UlM73ZzjI7SXlM_wooEz_j8-p&B?<+FtrV%Dka_trvFW}*N z+)nV(?#-v~R^YXb9>C?P-S`aU449Hu52GscA$Zn3*xu}gk5XR<5sE2rJ|_-$pD7T3 zRD6$#B}(l5uNLy!UV=G+SMkWo{V<Fr)6Q5MdgbwUai`b<JExrCPL=5iue(+V(F3*; z=lcs#WlIR<&)lM&p?gHPAJkL$;zi!f)?y=1y~HiWab)`aZcwdb&syAiuxW-VAPRPt z`qnpWicQ0y<1-*?RVztdsxR;v)hM$x5_Vs(!LcuSVpDhlrrSTI=e4@=JssM}H-iQ& zdHk3JZ@EvBo^^pOtNLKN;&Nz^t0$7L@;Ex8QE<A^gLI2I2-A+q($nMfsbkw^nrwNG zSOn%`2e%5XEJwomu61xMx(vC1`{>ea4<qBYKxM!loVE5Y9qXzKcT(K&;J13RsrEa~ z8Cxs#>bZ$%j1Gq6V}y=>6_5Q!_6FOFp)@q(84W(CgXZ6J$gqnJ;8JOZVFq<{a@Y&7 z{OOCKwMv-Pxf1apPv=^!!|xs0XnQ^pY%-rq?`^lSMCJ)PYin0uUZe$0Zn`YG#exlO zO(Z{p9bw;=FBq}?GV*&8iNYg&s&O=mtWQeDm)(-VUnGaJ_qRai_ynjc71JGAa(uzb zb}(PJ9|JDTLqF9L?yO7^JRLulEc9LoCr|Z<Lm%|0<&I5g)vJ<dtRDl8COgQN-Z41a z_&m8<u7XR8ThPie631na#23k@@Y-e-_S)qczO(;?XP@52Q;rq*qb3DwqI2<+%Vy&9 zdIGf{Xiru=xJSwZz39c>GazS+8f<JD3<?(aVE5N5a6MRu=hwd==azpaV=Hq>`b9Z7 zJ8vtUThonb59$FkZfVkS*+-$EcoS(Teb41z&8M&M2iJabI(%{dhV$&=(c0<&#8$qA zLv5>Za@Sg%fBFZUNc~P$x1PeuUBuY+ju@tvKBKv6v2;^a984W^3=heMptXKGJ(App zyYtJqgvU;JXjc=SSTPGwa~M@=HGwZlOGt23I9jWoA$IlN*m`FS$?NURjj$`krrIsE zwB-VsyUCO|j*=m1osXdJ?u+z-^ER}r&4Rby2K*Hng43Q9;LZE*A!&3TJhHaK7%xWc z*T=#)u%+8Bxsd6Zv!P+)VeC2Ikm&c?PJitP#E?DXXtMAFZE_djC-M$H?bMWh4)7?G z$AIPt>@brf56^qimed=fU6Yf@O5@FFE8i?mcwEO_c<c(d)lKP?T|Qt8hw$K=Gg99! zg=e?k!?d3xaN?^(9CrB*3@>a1zrH>3_>2&$x8^8l4ew4{Ke>}D)>e43C4(#L+srjz zHiLqKSM<4DFMR)L3+9Ju;n|3EXgCkpJU#<&W%D>F+l{tq<zQ8A8O%7YhtDITiRm+a zc2B%qcs?%?Loa6ID$4?Rbl?y)f)l8k$AiH;6KGmx%zrw54z^v^<7dgggx1nqAavG1 z+R26B^!5T+PP4<&tGhv-rw%;u)}_1350gtS_1LUrjBmmUt80IR<A`r5*x#lWd=4>T z-#tnXK3z*|O?S~3r&{Q*^v7by3<LOnqC4C?dy{T2_2t3_U*V<?bs=AMdV-a|ijH(P z`~ROt@)|RwehvRob_<i~6~@EFM85ltEPhmeH1D+N0NXeH7%NVW=QH~yFs+{PY>DoE zzQ^VZ80}EOKio5m|5$3sECz+JS2BUj@n$@;ijh;1|65D{K5za#cJ1|4Hn+@yUnKvO z9j3)RjPm5?uj|I=-RsWlkI83!#=5Yb%8mRm^`~rXQ!T^IZhVD_4!rla<+EC3+^eoD zv)LIs>_UPzo4Hh8;@i)R?-~A(-@0QKb6;~AZq@waEW@i=W4(^M+L>B9)!(0uO`FEY zChGGU`46Fjzs_HNYKk521hmsXz-zT<@|k`QdGCc@{GLZu%+$9L^M7iySDrP@MY))T zjeP*YdA594BYpQt-6PodHL);f%V1t?CL<Zq62NwN7)ur+un>=UR<F>P-RU))Js)7h zTbZfI|NpT5dji8tV_xG{E^bl=JIGI9R`I4Pz5cew_`{UZ@7XM`hkc?0mXH;4b0BE> z7_w`75?%1IhVWHRP_+G#P`FnX@3h~cPk-5hRb(AG<ot_%d@zM|d9jPX9Qp~{_FGA0 zX$GG^aRq<#*e;eF?8l!^J4yQQ?!}(Tl)%R2h~XZ4U_skjxHGal{_GQiy?cZS(XX!2 zq2qd@uZ=6Y?U0E>$E1=3m;K;vxsE#P?4cve-x71vDdfTt7hD&ZM3n7)i4$z2Qyw{x zSq=M0R6!CwZ#<N~eUnOt{cxgPH*I0@M~|U=?|dR}qf8qTw)1P&HIlB=a@YdZ$HZx` z6)!o|lcfe{yYHNyj`M6zV~713csI9z8~F1&dH!R&^agB+%U#6ara6yJ_tt@C=c`!S z-Hf=ueo8KAF01Y;wgjhiE8-#=gC-+xagFuHU@*EHePVc!j$Z8#T&Wfq7agL(?w9HI zh#f?;B!*w7!thv_gXG79ExgrPBYxoS0yv_*j_em6LXxg5ybSq{qoeMT@Qp?=I#ik5 zvCIRBRUpVsorW=4+7NJT1P&Ooi9YLyhF+d=&{9@Ubi$^S?Os#JtUaaF!|fnFep?1t z?kJ~<E7b7kntOC-{8?^pk0&I~V<1+^wiB&${g}<%bg1l|gn^sCv2C{}@w$%cxH4A4 zUhPdFX|{%V1T=Zg${>~y5smkxlS8!^l~@s_4Ssc}u{}c()(RGgy>*%7^+6iunF>ov z+fe00C>onB!R~jGiQji+x@>#|w1vOtw(YXVY|(ac*J&4t`z0kDuDP6~{D>qM`-hVm zc~KmfRzwdgMzM2w-I(q7y^_fqSu9&fVf8iBVYt^MmNG+)#i{wjq$Dv|UY^I7PN+t! z?WXkmo72*pj|aY7AcODyiiq)efr_{Ng8PA4G~1<?D^@s(fqb*DndpJBvmx&Lm`>bg zHxSlg219$h(INZ{TzIOSj$YJ@<}DsY7Y(~A=Ib4VN&XigF-gQeuCAp6Rd3;09|cL* z1`pOxFP_!(n$L<C4`Zhs6ye*kCwMQvK*R!8K#{{+{BX$`Bqrr_<&8H&<t=?M7n$NL z&B5e~^G@iQ;z2YA=aUb^uTcA&`E<<L3~pMkCb{$Qj!5~GEEsg2qm74dP`9Zebm*K~ za_P(o%8Xn{n8iX;ZT^ke^vD!d+J&*o<$CVB56iLft_5(VY$>02;UHgKtHO%wLm~5L z0H3a~7AyQZ5JoQJLxWC}MPtvRajO|A(X?P8QPS%@XwRD|<>G*X?d0a#Nlb20A{Z<5 zqNG&^AGG`95TT5&{^E=q)0Bvs-7;)%zRRuZ7EDVeM`>aBdn)eog8X9JM1D;bWTHkP zH_uGXz1sF9Z!9Zv&nk+AsY@vx7TF9s0bfD7km4=rRDMJLLRh$W3WQEc6fIk^9U5mn zLDMn=_%zoKOgE_Uuhc?u<nuh}e(sXA@7))xJv>EIYqO|o)Lyz}lO|Mj>y7U_t-v7S zC0YB^m$=<hL0L~_Fne`_b7+u*57XrNw967oyodASJ>}eUN6d4#>0`sXJLU40y|&@g zjfQN%<}fUGIS51I?D&w_IIL3<<D>8|qNk2-{MDbA@Rr6$dMU>W!k;i4(x?v4hZe!u zDIam|#hH*06han9zaxD{A0oL8%c*vfugJ&)$+(=$WXy}pf(iGS=3SBJRySwSw^=om z@A`plY&Z<o1A6nmrk8Mk-5ECV`D|Wwy^O>=)`d0X^k=J0<8bvFEp{x}6|z6p(-2)f z2w3|SikDS_?H&zgR$_%cm#8wih&*2Xx&bXYu$@o3cLs*51VXoFCA>0X3T?=~%&i)c zMCYy>k3WW2iL6HKqG8+53V!okg&U_b$(HpUG^St>IriF%l}mcEmoetN^ZU#0IfHid zt*!|y<>*^%O|W4jkLx4-I+<NvRfJ=&JO?@T`>^ssva~;4Byo#>(08Xh=+S9Y$;gd` zP!-w$PF-i9>Y6@OG-)s{vzm#9B`1l!Qz3nFql(J9Y!goU1(WyleMxZ7@pQ{>S!(Qm zgC09C3+9HQ<SDNNd(4}#`S2Ou`<EW8bGpv=adQwNA&Ym)UBHjdwdN<KIilbF(=@O$ zg@s`O?u}IB3o3i^E)KgfrZ^o-q&M{QCFA)YZ)fo#%}bcVvLx)E-w9QYNw~MthJT=J zhN^kms4>cg<XkM`M)&<n?=Q8%&3%xjKYmB;W~-xy>;ut@s1IaH>RZz5t*ub47bj$W z*Wx{H`Ac|VDm|am0sZbobE~vIQ@ApikMBQ=l)uTL=Yv#Xt6dR(dDs&#SWG8N!X4=O z4+v*HO`x%AIxPF7iA`@EAZ?*7x9+9}y<=KH=Z-reoL6vy`mlC7&BO@xe72MPo;$hC zE-}I$?jmXH7?1usjxepZf|UJgrE#BPu$O)m+tXDB_m>qy`|+Oc6%nVI{-*QT_@OJy zFEhk>8*gz<x8nK2B{SHX2kLNYUNt$}aSR6d>(QdP5%9!+Ekwzu;+|bmu!l*&MO_PC z-+hRo-POrFeiu2CZ$teqiLl%FX&7{98t0_Hl=houSXB_Sg7cZJCye>l#GO)d5M1qg z;fz3nIgdQ>x~3ewj-Tv)XGIY+Q9H^m3{z(fcaLEHomg(xRR$UIFW^A_K{92X41Qf7 zhoQr3LG>3C=Wz;b(u_i~>6oL~y7oGFcFcr>_X?q|NE_yDP(VXPXR58K0qYtD(3*$! z)O$~p_-OHMns~toZ%+P92dyxJ=gYf6=lBW|tf>$D^%gQbvkqi>2eVU2$0Zs1?|8|_ zWd3V<9KWakLVi@O1^>OZo$OaOgTpT#V9zx#sUe90@yk|va+wUBvg{BUWBgi}nIdr4 zCOyFj?FsaQDy8j_NpLw^9(dg}+S7h44cfYwRPL8ScfVIuR;Q3Gn5GNEpK^HMa~_R| zJHTx{n<IWO;XNIp`2p@os<Ap=8=@9X=6}s;;!CfzNEeCvywTq7Y`}+XYIn8|R0~1i zacc=%e7h?i9wOnU?A2zunwPl#Ig6>==0y;0af?e^?FyUI9jSW=0TVuk+&$sT+0Qse ziuGQS(nv)#PaF$b20mcu_nDA3L)bDeg&egzPt!wWF*YfJeo73ZAEq2&E5;O(Ay2H> z9H&0~-q#Z)``1}W7A9u1;w%{!{X-yUj`brQ1rqWkJzX05tcKxr8EDh)$S*Kc0(DFy zWjAg3G`SowZ?NK<1_0i1-b^wt?t#nmK9Q*{24qybKs{`a3DJ$`x!O@lT>Nwk@ai@Q z+8z4f^G`aM_)G<4x<!%J>$luWe>FneoZH+og$TZI&_IdoLlZW1i3WfA@IumTtjoN9 zD!PAoWyH>g@R&VC!j?a8p*<d6#4BoxsI&2LdcS@$|87?l;#yUv5#&LRN4c=RH_p+s z{8)axSqyx7x{T{R>=$>{eg-}Dp`9uzS`rftXYlqbBWus}q0<)bp)EFTg8!p7A;37A zG`&pa*UQF2W#d;|^nEJXH){<0NoS$#a~Jog*ZcC!x)9cUoyX#C-iEKqN^EWHV7xNd zh!6D5;EtVcfQZlXEHA4SWy9`3Xv|Wg-~1J0UTnaTm!IL*YD>_!-cFpfm5GnKIXAnh zn%MF!oM0ObRwE}<wa!tLYWUJJTYXqwG7GwB4k0@)jO7hxZsz;fSHP8~X_B4}nf#d^ z)$n3k1;5hZARp9Ihrd6DC&TV+g3X>o;k@P!bo!AA$&rcN+6E;dvR4e&C+(#d?uSEa z>LL_E8I~0$A-QEoduc6!-TNnk>jphKW8wiixUHu8)sA0WEK|h(--qF{D{17`=4`U~ zoFe)Mz2S<L`$9m{7XIY*8w~eO1dq2v-IsS!c31FB#Y26b@x|LbBsHgX!MNI$nt0FU z?*(b0Q*8-OwSG+3?{;R@&JtQu&d_cBW;{Ksh88w>@(06LvuO=U81A|QJ3ji8?C)k+ z+NFpD1;>+em7Vl!a|svS^E&x4GLSlTN{G>yQfbh8i>M}vz_X$IxZUdy&@+9WLCc!G zv`T%h<h*J--)E|kB>&1pSaqyF|L{N!Y`xWl!HRL<G0PBP+A#86v=0K_C6T%M4~Sy* zSnO#w1=HTRz^Q0`2s)gA7EOut<P|}fQ(i-53<i*n8dFGcIYQ3_E74yIwaMy9UXc2u zma~dKL=$uqgr(nY;7~^%?VkKbB(GC}ZoRrnx()5mn>C%`9g1Cev$zeAGclQ|n*>PK zYew_>rTVN}tPJ~g&WGQqc#S@}b)1>m+<@qS74&|S8GSM-4C^PH#QY6Yc|(yu>o{kL z9)rJ-gMllEqLT)cg+&tOMXO2K*cUW9NK&mjz=1AU6-~RI8cSANsBmnD9cdeNnFe*J zfmXPP@8)}e=~f7EN`D)3Kd&bU(2K48C5ye@?PE)-wP3^Usl2YiO#X9cAVwLbp;moA zc$2$~Kj~M6zBht+_rw@D=4Qzkv5Rp3)Hbxg9*d7JG{E^Yhr#x60UdW%4=g<9L-nc| zf`9ZLT5+V1C}n(g%^n#oeo0Gdl1&ypRX(0BclM=L!ZgeY$zmU_ZDK|$A2EpQCrREl zomswDk-YsW$K0p4;?@*7j4(J)yn^fzPBns@k|%Vyo`Ml6=h1$v2VBBzc(Y|PE8laF zeDJn{Uk-W1Wsf%tOnV4&k!BdHK&ZKe4y;jY7S<n>!!lPlIB>gxF85S{qr>vKZpJr> zmyky1Cz|35rSCYlxdAmSOxT8-Z~2hPs_x6vdh;#QKa=WFy;%CK5zNtQ0%#YE!ImRa zrHP4OR5dXU%gc+&gaCi=x;Y$g4#*_V^*L~G;&pJzSWP{OH-cl)U8<HhkX%>KrS~42 zp{$QKdKf(A-Ws{W*l=4sH~*^uNnIdTbcU=_7(jxr?IDuRF1WXpV@vNRvbuNs`MjhA zN#B9vS>LlOA>+wD7OHN~9L&Grg>G-q_Tg)M1#56em<Y@kO4ngFlVJH|JDk=3mN4VN zR_Z9%NWDKSgWHM<aCn6-JUP+>zbx^_mxin9*>n9#)3V*dM-_M2=DV0)u{^<jw5_IE z6O)OIz9qfls*Tt0-{PWD#oUrNk70LkIeT|}BXeIE<bHL~H@?$5m|vp1p1=4>Q*yh@ z5_ThbIy447CljLf;{G(k&b8eq?Q1RBo&CGfT&<aJ^LmU9>kF`LM+y4Oj|C%{70jgm zE%Yk92E}uYA=3UONldI2nhTU*)jc($YNrA&Mm<5d>KonR^@i?pjHl^E8Dw1UM!|q| z<M$u<f$c<%1*|LMOYA;i?9F?;C~OknUHk^OtlETM-&WvO>*df<-GfOyH{nH#XY|sW zzIaAgp1<u^i<-?6^6~0wNLu`vij!R7@y$Atccup$>D|Ct?Uq&PC5@Q;VR5Y8->Qtg zR`w}hAS_+%CCEB%6b2ZpGKCYfNWl7hZvX3Au#TL8PM;P+{Bje%&-NT~k<wVcQEVU* z&b14x9<9Tt4Ntl4PL5>m{jG4i?LE$r3BVS0Kk~!0H%ptnUnrV7h9%Evg@Tp+dE@?p zoabP3^0_*O_lr74te)vf3-5lD_lIGcf5x^XdWyHpp}6og@lIJmm6SKqtcEh|m~M>g zQ+C4|vvkPmA4KOrEF!6Q)oG)CGC@|!_0*Ea(dYDorziHo+EiyKKctQK)0UC34eo4W zz7~!we?)F=FCzDE_2MQht)Q~Qu7T_0ZP4@XY8WG)i?=H`qw|g|;p~Y_Sa!&W)xCAa zIj>Jbm+ZbcL060EZLdIZI!PV-MPur{KpeJK72Je0>@QS8sA^XTvA4l(+h(AlXB4=8 z?#~ySJcllO*W)t9i;yrg7m7QFu%5%UVfpQ@BwU)~vHlUyzpFS1W(P~eUwt(BZvzw9 zomw4c7%-9sWKH1ZFD<~)pO53s4W4M2+7-^V_2ql7RA3(_4`RLE+`{Dx6xjXQ?yT;~ z5PoBz0FgO+@apY#EITL`=C<4NG57l7`ga#0YGWJ5nT_LP$3DQO#&DSJa|By9F2G0f zUXY?)2wr`Ekf~95c(}j`YvcRTOs6>7u~!#%<g6k|ITE`1Km{>bYX#H4tAS$ooA_nv zd$0=9<<4TK;N3+NI*&)uAw^Rmeqbm1Da)gETqf>qtcK3)KG6E*4m})wjI^kA($rQx zm=pb$ws)+>_p5JnVIL%r(sK!hgidC4TBp&`ZVr53yk9tfX&3v@T!DH`8DhuK6KJmc zl3ZUf5?%$5<2P#5;WD2jc)2GGl3gCc$JR=m9yXSgsOr(AP~dl1ejr8zufvlk<Ix~C z8_)Ji#-JD-enlBUpPH@o*Z={J7P|07-JI~j^xo|8>U8>WtqPnQeg=LvT^4YdH-9g9 zAX{Kfc*8UKcr^Ye?(RJsuQfE``KDk_^_wEU>4O@7$TCJKS(uKYEfh5ecH@0~yMyN! zdkB|p1<d<Jea3XcJx;{@;v@MmlfHbypjKF(uPV(COd~yZx6#^!eW-t26+u28o_6m7 zqxTx)Ii;^;fh3RwG#n@Mk9&f!cp~f(?FRR%1W2%~AU1FJ;-K|YN!YHT?BVm9l(RGh z)srX4a;r8slSlEi&g(e1oV@`zVh<5{*+sY_KL!V`I6-tpXF)-EHw-B3#k=U<fmD;3 z*yGe}^vrIDu$EdBnQcSns=<VTMKHM{8&6cJ1Nkrl?rkj>O4{$hJf}<=<?s&T-TcX# z+)s3(Y8Z0&02(LM(~0?Q@bJ<++<ol?xWPJJ@mLS(GIJpRppOk&TDq~Wp1N#VkRml# zy9$$Yevy}U^P%&TJUh016uY=@G4E&=!nB+dVQbqIl-;V%W<;v6m4>psdE+4TPBnrT zDpk0`4slxEEjV9r8Z}j4(2#+J2--gAb?gx|9JCOoEZGd%k2Wy#2^L_m?-r=Te$sAQ zfwdQw<2=y_?s)1_I@@6YR4tPTx*B<OqFgE&nps4O*jzYWC5PQzy7S9R<H*I!H)zVG zZ0h%FB4!xIL))%qbd;?@{}JXGx=Db_3z6hO?J8=LlS>!2*9yxg9wn9LlhJVINt~Ge zjyoBB6$*F0g~z(Bg3pg#=vdN1wk2PKsQfJKsiMNN?A{9}%7?O1mJF&|L~yr4gMYK~ z8Vc_f`JkuM`6El>$;@>RFzVbPtocN#?An8*Mo$J?3!?aYQ^&xu2rD+%%o)4r>+_OL z34-Fzrx5im1i2Tn{Iei$dSHPG`((Bm&%`M5nQgQA%{9;Pu$ntozNy4PhhBs3M#5j% zCC24pY4p3iDLwpgJ1XzUB`UjI_*YJPd`%BKR<l-*&0n?C?S#1@3mc)s&wi%BR=A7! z<D)i1Xg_IKsn2LIw<-o<({`#Bo+F&mizF%fu7duzp5W<MLoB`x5q1Y`;SR}maH=8; zVe<+-Y7x?x%epXulMm4lI(rz<=ntpJHM8Tw;f?#ay@73<O{@;x<QXBZR&*62rp|%( zdPCvS@ekFHrii#z3rdA^TaS^*7o)|s3L}Kg5viO)YXDbKB&OkM6KQ(xF2QNiI{1Fc zo6OP~E`;o;5N6HkK~^7frNf^abNK8k@mzVGo4i;|{bp8^$03FymlHnp*5Nooac`k$ zOk%lk>ggN7Q!Ed0ONWq2h6g#}!5#5V{S5MUq8c8Y-<xhapd-k{+0gs`18I1BBpeRv zPu9Ke28D5Mm@`?0mif8RW0yw@pKis{#tTWrqwOMBts{>ChI^^rfzhC8xf@^h@(@NX zjwdm*4-ogcDI84h%1w!M5W4Ty$Ek<M(ypd6#134nP(CynJ(HqIt9lBjJx@#`M#c!4 zcPoSw6|DjZ?x-GAl}I#mwBX2@M$RkN3PyGlxWS#N)xR`DY3~z0+@$nnWWb7z;$Cg9 z>2)oDP_Mc4On86paFP{?@|q|tSwDd5GRcGcy1t*V=FNK{t@WJvx|0k|A8Rg(bX)>W zIVVJ6ITyI^FH5Oa&vc=}MwQC<IKVx%*hqIJt|DbW4stocS!D6IQ3UJ+VdS8m-0@zP z<YcETU6N-_f)m#YQ}#+jIIN1q^qxswL~+D=m$qP%*@aXcbEK}1gGv32F5I;JbH&W_ zhA?1V1obwo=B|uN;5@=M(}7;!$o+SpxUVUBq}``eI9W9S)Jl7hokxEOvn4Hb?2R8( zzV9Y(n2$4FwVWuZ6+4J5R<zS4PkNI*#U-Tn*)>{aTt}>Po2d1au@D|J982yk7iuR= zps(Nf5+9}KT=Cj=ZsEpyI->7QIxx<S`dVx!2HNW=D$NvJ-|=KtPe(d8p`Pg7yh>K* zmI-y3AcQ0qxs9zK?`GWBOM0CF)w7nXlaTARqS_Az^wKU__@+_Lh3+~oHXoouH@(Xf z_E<j>ttC}La+fIZuNy_wx3r2QZw?{>j#s&9R+~x1<9auXk)!E}eZ@4%M<Tk^dW|~Q z$l<srU+B6Q{-oQ1Ze+Scvrx0Kjt-8JaLj5kH}CN$;nUk~w4a5oFs4IasBVfRp%(+G z%DEjhr^_zk$)x)<X|^2q*kdziq;ivNpFE80ZLt;V$}Vw+y|T#4G#zZPd@owF=qIr} zVgWOY41|@L6G5|d7deq109)6a(P=5QH1nDcG0l>QHe4<e@^qB&i9sy)ah(B(hq=;a zC!I;hrybaA-BZ*#{s5WqvYE^5wisF`$CJWpeeuM;TX1`yCVG<Aad%x72w%oH;^(tr z&~Ma1;kokxI(MuOcl~33!J)lLd|+z;?S67F@mQKdwk}Mq=59P9r>pwXr?R63b=zB1 z^y;Cww7)G;&FD%G9oa!oh8n_v3Ipn_9a-I{{ETp*ejZn18pRD)vJ|Sm1h_rzt4fsi zWV@}LNw~H|c{2R?Tv)F&og4C`jcYX#gdV$11wZeL;#?urO{t3tF%4@ZI=&UOBYg~+ zZR0>+8J;3x<9dp0XWeoA@_sd0x8@p|eW8`=8(HD_X%eBw!Debar-3W~<z5|S?gLIu zdxdW8eaZ8;o^)1=9O14i(goYKxOA`OT%*?(ZtO-A?#A(9WYCIC?r4fRxue}dmAdqx zQ|{Z-<TyR>n4LsNwe6$}f2m`KWgeZT8YeV23-tclV07-|4(Duj;Hb`U`n66EG{k3x z*Y}5zMThp_&H1l6wQl_|*(!-l-7uAYnYfUojIX3?F2qsO;RETN4L^ugjPwzg|F#VH zM+SA?;Y>3ex^W;bi2VqcJOSTrsw2+k&!OUUDIHWWhjO)(Az_)1;Pfs6A|}Lvtbs0_ zIopKZ4c<!XyVnQ<Tno7c-wn9&<}<0K*C1+z+sK_)Z-vc0ENP<Y8uDMVvcErx_HS8P z`JgcVr{)Dx_FYU;R(kNU2Tov1;8s$&;x#!{nSpvyv6#6cm%Qqs1}CmO!Yg{dOnaOX zwv4(2%^khq+0}M-v$YdqC)Lt1u7>2$&S<{t#d9bw{>6kUXSm%eN40v)VM{N)#y;6C zxct@>kW?EJH@TJg=E+dHY@{mOpLAEG^f8}pP&8trKFvq%9-gFV#wzl8)hG;OugLOD zIe5B%9CS!senPKWa@o9rs9&;xyCrt`wOoej?>GV>!wi|hvsV5<lr4)Ne4YPcvJxN0 z=7}HJneeCGk})u70$lEu$bNc7liAmEAxyTrxN)5ktDUxA+<qe(_xWmIs-+5>`Scj= zTl$?dpLG<&0`?QrZi^smr81N{=~9i+8Srf`<8FNFO5A3S0!0UB5~J+{(A^6T^m|2C z`*=f!qX(%k62VCC0br4C!<OGSmSh}1iS+|=M5fK&yw-OezNJez4%nf_o2OauvfniL z4Fh(=u_w>ytNdv&PSuMRNT=&to;a|Av*UQrhbnx@X9ZXvaEq+WQfD#c?)=%5gS1c0 zMbXk;wmh30iA&#Z;tXHuP>He!E|*?M=ZbE)<w+m()GI@y<7Z**l?9-vaSN^~_LOuV za+V*sZaTkuSU1RMxlCnE#_>^a^zb6t$*XB<vE*cRmiRuM_k7TWclT&R<?jg`=!cV) z*Ss-UPZz~r*<{}k3l_&a^Y*5t&?{~k84`1tUgHd4t=Ul+kg^m-Dc?zGPb+ZxI-Gk} zqK&_bt`Vm_UNm}m75V5so#U!f_{ZaSAq!g03;8RsV5gg8952Jy+m7Ia><yXz-A9<V z(1z7~n9k0u-ONBbrF+xg6&lA_^HXj&aiT7%G)`0m@-M_>$X6e_rt5XqH|`Oh9C?W} zKfML*Gq!TW<R%L^MV7yJyi%BRHHrM#-bU=VYG9C1PIg#za*k?Wglku7iPslRsypfc zMA#bhYeXgPRhz%@J=zMH`}Z5jwfgeSnx?$f-C|BrX#zF=qQF}RbY+`t&DlovK$zEQ z!w0>c&Ua4B!gs!3;q1GmxS{zxY`W5gz5QXq?;5Dc`qr!<tpPG9Sx`+RABT}?Q%=&h zCqq&G>QQp6|4Tu>RUUU8Hpbk2-D$~vGuUep$WQeuXW4TL>Bl{KlHKdK5e2gb-gZw5 z-`1ce3DTbjO(|x4=I0x*{XrQ&S<_vlzdw_#zF7gS$*cJLuNQ-yG~=k_Gl`#nXFa+! zcIDeI)=48;G9)ZVk^i9e9cJD4rk!W@(0*%vz|+SLxUlFO`5C5(55zt3s^eDfg85{+ ze$Y62{Ou4j|BETh?47||PKbg0=s-zG4`ii>(|OgmOIXnJWd6Ea1&m5R3f?_p*%aM@ zl8^5%vu!^1?5A%Mt~+nU>VtqDrl}ZErOKMVOy}Nxenj0rRj^MFPtuv^y>Vx;GIV=> zkjyfELIsDda4Av;hq?DbkBQUq(32<heV_fXGcAPP_S{NercEI;FDIh#^ARSut1$CE z@qEoZTlV9!EW05skxTbfVtr@d74pBDk)5Z)S#W(dANkIP_1C;lrVc&<tD{SJQHmCO z=ZmENl|Ns%Z8#LCIH7gOMoibZgpX(Mh1<>2DEo=8q-CEUWCr)<=6C%-1NIC7)DVQa zhZ!V!-F)CZ;%Q^<etPPQ16&F##sSg_>$=b5>4(MRC2`w*CCWD!@aOOJVe{5Sp?8`U zcD>k-U)gsD?um3@;^0MaKesy+e=vioSz}pE*(Yji79!mF*&QY?5u>T*D||lk8|HP{ zj+?!D;5EMwxNbD1F`>KYOXDu=^16q_N;8+1$y^}U_OA!dT7x%SKc05qb%9vV`wTZa zP1v28^MqC23t-ezWFcKj_^Zi_@uIx6I%8WpvwB|yyXtgUzt3vSZe1(+{_ZT>{9eQ# z9(|t7-Ps$4SZqYo3!9khtpK7Ye-RU*8~<wQSL_(FNy@D+n{wg=-qm!Y3d$$Q@uZVj zXz+%1dWLh6FLo0d=Rwp)N1qSOYb8UirxN#%1>o9PLE|>)!0ZA6RHGCnZS9@>YNat) z^(GBB&1->~=Kc9#r=wKYa5Nw4zKnf{c?fU#3aFnyfv!xO$#&%G@Efljqk5$V%+FYn z4=q>Xd)8FJuhZR7cI6Z4GT<;ila}oV#Zu1M2C>-H1HPV?#i`vM(xz87bY{5=xprk2 z(Ux-|x5tg7uT&25Ms=g1YDp5mZ(9c2_^O&bR6U2k4k);fPtaofG_CoUG8=Kq?gZXE za0E_s&c}tRQM9<)9kpe{u*JNN)XbZMmrpE51)K`z+XsQ?1`BYOKEAN2dn#2u*;~jN z`js3kOyuTG>x1`Yyo8XK6Tv$BE!SiJP$HjrngmYNfGcV8#74`GynW`vj!OHN3LHQK zTTjVq2LZflkKzgMG0d^Y41Pt&cH9Mac(iya>-Dm`B<x)RoBp)}3-?aJ5Z8rxe9mV< z{J;n7vTAX7Yc}cfeH$)YP|DK3N-%a~EKax3VyC4QEJ2!zu&b~WAHO58ZAD+qNt8vi zhE24jgD{IUTfAT2LMl6L*~!jM>~c4V%^uzk<BT;WvrjLBomwU&C|8ZeN5-<KycSq< zN`pTfzlXip^o?H?+YKsPFH;v=71mO_m6wDarvb~m;M>d*)ZC~PKhD)eqe&L@RzL|D zmT2&Lhc57H!yM4TRt<IRYIyTA3ban~0lmQ$k=3eVAw9|uL!3>84OydbV()Iq4EM8| z<63;5_9IYoD4ID}H?pw<Hu7l?H?pT!o`K5ArS$oTi?IH-nxv`4oE;kZQ2bJS5=Qm+ zfop*ttjps9R<0euEXsGXOg}ev=Svm-C|HdKR?z_U{pnb(dk{Q2f=rt>4hLMg3B_s7 z^ty9TV&@cw>jE6%{TIaT-+l>UBYWa?e+fQtv17Y_?&Z}by11hzou9g48P0i|%3M`$ z<1H04{?N!y?ATt0+fS`$J9DihA7c`w6=M%kskkdrt@oHy9zq+^y7PX6#Q0|BI(RrC z9#dTsnS#O%qV~ZVw^gnXR%Xa?Q63`9daF*Y8_K!W<>|C$k~&@1oJ`anZK4&cwg^u) zwbM}##vnYX0c1vK|N1FLq~}TYr(9w-(9iv2$vNTNy7|Jyt#VBHSpgh!Xyytu<LS6> zmuPgP5)K;UhF3oABLg`<GRk8CSaWl*Bs*4`nOjAVzVjzCYo`z+#l_^NQYKw(a-3V5 zyPm3?-p%<;$tNMbl8D|9m1^C|dBR-1_q4L^Ds7st4o^On;yN}Il6swn@elU0tQS`9 z#w)h7$ZR!9Lgq#KU`AJrYcpWe4R7MR&=klmyNiccKF6-cdvJikb(|h(#d*%~B)2oJ zlEsbt@S3_R6h2L+FBaZ|<SG5c&weDpu*-{RSf(GT{g_SLQ%7Uf>>zQ~q%N@iQ=qsl zd?wi)X#q9z3&`=Hn_Tm-*Q9iW9ZT4$$Txm#My0MZ+^YsqZdI+Bd)zKpHh0l$Sdg|A z2AnS9!!kYCl?rFFO?ewSHZI19yDOnrwJ9H9S5FVzn1F*@)zG{x3o}P9hl2Zz#*Ats zot}&7f&g7UMc<s;_vJjT-~19bPCV*1Q4+)%dFZ3ewF+*;R#|phZVg$dYK-}jckpf0 zKzi=T1pc+hA()cg!pg^Xz&Oo0th_%ZPSNgsv|$%g^yx0OHjRRyt2>xwWh9$$`7Q=; z*28sWuc7G5NO*Q`8xFZ%!+jX(Oe4*s>1y3r7F(2ueM7F1eG$v}Q=TtryJ8_NT${w3 z#OB~_LrpMxrjF~mX57EZgI{l_3u8_hVU)#q{@C48y5(asaC@a$-P@gft%Wu?)W1d6 z`hcuD`hvLDZ>LQkHpA5GJhq0bfYk>>Sfo);qwg<*Yr+w_<x@C@$orGT<RNHi6a_k8 zGllR<JGl2X4u+6Bc*3uMwB}C38$Arj&QKY_ZI!0*+HoVNu55*89qzk6ys}=X`!EQT zmk%Sewa=4n`;=I`;~bP#6w}wXNs_&18ln3=S5`jhDY2&aNv_Q)(lUK8#y2I9jk6BG zN?93H%aHnkZ6q9#7O34emS?4{saR7m1?_?kV0m>u`DN1t)`<x2C9$A9?h&@Hs3M(< z3Q1ok%S3pP_^qp_7O4b7_fI9(pQE|qHG!O;`v8ph?1ES3nXuURc_hO+4{CMCGx+|L zcg{@Y`y|vd)A`waSM8Df=G-i4i1Y-5ma#07oPfv;XW?l{Hep5G=rWZ~L1EEwd}*YI zTPEqSI|pkZL#8(=-yz40TlSIA6Gn9ZwmNdUF@?Nr>q&b*R>b=)Jz)4xE0A4$hdMPp zf}{Q>bm6GgqJE<*pfFb!#otYMg&kjD&Zb=Vv)?P(oWA<}<iUgqbLa5JURPko`60CB z&JSGU(wpt`Qvly*5h&Aq5PZxhP&;pVR{Y`yF1W7E{7*JwQEGp3wcv%&eg9}!zO@UE z$~9&R<KK{3Zz#3wJ(@XPkprja&$uOIJ`^Xt7SucDvD<Z1Ij!t?j=WmPtS>Y}xQj2J zXn6^|fBEvG5(81~RSqLX>A*Je{K^~N{ByL2=6;5}%phy#eJvH8&fKNTc1JNIUwO9R z;4&W13<8G_73}NgbU1WY1TB7b7`!YFY`=VgFHh3IuU(HmT<r;NKPh;`c%j2IYgk;g zpQ<-5r~W%LFnZ!0>e@bm#;PXa;}u7EN#HL~Y&2&(^SVmL-JH&Udija1(JJSwYJ2h} zi;dX1pL=1&v20#kS<0gQ4<PHB2VPqX@W8_)>~1g$O-%LK#GR_lHA|O;)^=cZsxJnb z7eHr#AKYv11Uu!OByQV4P*%DD`;=CZy<Q`!ZAvpGd9OtEnMJ6~tjLeZBSO<QP1f~7 z6_mMbVtM`2A$HLUex_A0&n>FNF^Tn%_ArV`;wxdLmI{t2G30Lsm-8mRJNPY8(nlvw zEQWsFRbbxHK{RF`fqdUM@<K}w7rtB%7i*65n-6R77n)y_ijpmmr!q@0yuFQXo)-Xy zF)1WAbPAl=W(@wl)5*0d_XM9b9bu2x9P&14KaLAn2%YW?uyL&!J~A?Q@7-wUzV@P# z`yt;`@Wjf6f4;7O;Z=lzhhumRvr8~wp&ER7YKG#n7F@J2h&Ju(U)9AT3;N7n$R1>- z@^zwAs^qRf)4aoA+P*&6S2c=GdfFRKTpAA>N1UctjwI5~5n|&0pqc9!`i)aO@q@fy zIg%t>$I=61W<jRYJB*9j#ZUU81r{$t+{MA>l8lB+{1*L>7*`a<e;=_1Ul~4w>cbP6 z%lap5-I0?l345{Hjdh$#-4VC}^Jw|pP1t$tB}v^?ifgsZXzl6E%v~b|FMg;bf0&j1 zR~+QGe>mf{Qe4&#6EFYZUayU*p7P@}9&n9^ubI2WL5<#USUQPO*Bg)Dj#iRYan%sk zluif6S5mFonULfqi$|}uP#^Y*i+q<vPwH7fpFvx3TE_=Bv9v_2Ux$(~MR7LduYW<e zW-K5M5w;{|Uopv7`A7^_t%LuPmHl1bzhz||$`VDn%JQOdtCU6erm2X_H<Y@K->|pp zT<U|WW8O_-$0RM$n$;=QgC_5;4hXOo>srkbM{c|=ZXUA9O+9O`+rEwn(YwV))kEY& z)r*;iNbZVI9T%hH_FT!>tw3Vz7H}&{^kAx|TgsZH)w*H5#p7#xRbMlf5gipdi>rcb zM1K95XvXVoQRV_OxA?@K;@JFcBCj8>s!yBkbyFRD)y;mHvgm-^ZMP27aPevGjrevV zh*Zms+;ki?xMbJQ;su4*+{|O{h;x7J5l__h7GIbf>LRUwaBcsdD@xaCa|_fk7VT2G z>3XN)ncJrm64AW0AJq{;z4)p30r8QMm#g0R?Ga@SP_2G@IIp^Q*cS2o+(Xr8wp^~Z z?l(&Ot6_@BYsp79@BO(|<<TkP{Le|^EpL8Q$-Q>smUp`=E`NSh6xFj>q!X4PN-xS0 zJ8Ou=$7X+befPt$YP|uvEv$d$_F>p-@qn_A)x*c_A@kR2;QYldY`Sd+%wE+6<LYZc zUy);XUX)|frhbrIGZ4%7)k5a4Ct&k*uZX_623;3Q!%RzFLc`0;w2=Eq6TZ9ga>^mt zWkLij+>ye`Jo`aClvH>R>tZ}|c_=gU`-Z!(yrF5ys_^C9OFXvK6ccja($U)l*gkg| zQTka4R?^#2c=0(%o;rxX`O$+7)P6_B=3V(|o$frJDlIHiN#T>E8K}h047~J;(wk8U zWQRsP02jl&Lyux!fD(-HI0bE=YVm?~6j6O<$#kczg{A45{5>~$HmFM;nr@Y#g)o}0 zmwg6&t~?Z}_@UvR5b(54<U8*KGOfyZeDWj{_k7UjPxU*5?omV7Mnivg%x3{yb{@y> z4^iS9XWYcy#TNYBty$1KUK-ZwDy9J?IrzwA29^&sWFMw#u#o|#{Q2?E+3{Yxc?Yjz zw!NVa58ZCWm;wFR(2M2d`Wz?rE!zYS4*QCY3Qr-iUpQJ_oWKlr)<K<~F<-lDI7>14 zK-zTs^OG_qkbV6<)pThjSraw!^AR~-TIk20@SBel>w2>#?#oapRKcW~3heljfmp37 zeS$r#AMU(%1h>VmCR#h&IJZwL=~nx5;OtsXbmSeev|5LMe7_OSjeLfC=kWZF=GD-r zU>;wr*_Ah(w1Ypsa4bLZ@%R6$t}_kC>I>VxIrBVbh*ahbNxAo4Ye^E3CaI*T6d{$A zOn*{@q5%~dixe4>4ENq^Z6%c^G!cm=5k>tqNW*)-?{hrI^StlzykFO`KkQ@g57%|A z^ZcC`CU!yOsPJNRFX{?y>O`Pt^$hYV^eHsCs{p4KdqUqg<0M_$o_m>50rxuYLgpVW zQC4dZRM+o?ZDJ>RhIJ?4`fU^k=NF^<9*FBw(d1k&?nj+Nz8qU3sDxk((pHV)jEk3Y z$rpm5Q*R$h!!-K$t}vND7>S-fR!1A=&4#AR_qjqBOD;d46z0jUK)wdnoOaMhATzFn z`W3dJ^I4+YXsZI+@kfE_IGWEmoaZsw##5<nYYpvn0{8NE4OsDa#Mv^P!v%caM>}4; zOHO0sYgMYXd<ykYU5GEQYQZ>kB=b+K8s^WQu1o2y>gU<-9^`kes-fJ9PVjnete}+a z9yf}w)?pre&*nA#N@Gqg*I^XQj2aIqSTLNH0^2Zpm&unBVrphwrNlk^?PnI|Qk#Cq zGnu!HsRKo|%*mrdjD)E((~@?Ts>~1N^(f0zkq4vrU!E27;mUkQPx>vjFV~H^5iCtT zWI8G3#yM0!3Zl{_WSNQh0_OT7dq&;xHZxtTg4w@5m2qHq@!!rXVk!)++4SBueC^5C z)W1$aR9-+k)d%(1(L1+!-IlkRAKFHZDZg_lYyCOY_|+FY?O|=|olP=>3f!s4Z%bK9 zKZ}}nGLav=QIC>2VN2Qi>QU=+<LPzdwT<V`)G{(IV$|Ei!;E>wO(rw8meCzu!&sba z#ygZMX{kB?&;{dmu+DiZtSdi=yNXA#@`FhjrG61uu3_<l>jFBj>;d?*SrQ(aH2`(| zTG)&?xwNLsMRM(`GVXMk0_YiA*&7EdiAUo-wshe(Z1X$|2X2ibWrOEINoX?IyjBbC zI4BP8)Zc{pTCylQqMHr0TTiad6ypYDBf$FAe0q_46#g-_i%peC$GKl!*e7Mxc+cZZ z98u*DPJd{HH+~(!)n<b7oV%QSdn|(;68!O^>8|iW?OSYE(@ajD+6PlszQUy?M!@x) zFKO6N4U|0>;e_m!;LU?9BKpaf6xRW8{_jROVR9XuRExtAO)^-DRe}b0Ff6Xz3)IAf z;Qqz4Vd(s9HXu%fjN~04(R(E!o7f1%B57*zEJbpE@;aUPLYQ-D^8&*Yi_nGvYZ5%a zgvw57WWNe%+uzSsiRa=M@XC%!@L14Jwr<T~<a!WjdvAjKGq$tm)|=sg3pyZm><S%Q zAr1uAY+CDU3ws|Qp>NI91WWqQu%5$HL3q_vw&<?~eP>S>!|Y3fGn1r<#M5W^{)hJZ z*QAKr6e0qDZdL@;Pz=6TE6k<@NYa734zTOCr7;Odk1*qv@_6Q3XHvdj5APN!1&_0D z;iLOK1fz>2P{4z<`_+1IrD`=}awU+J89r5K*pUXreAm#+Wc%qvJ2y7x><E=H+n5~t zQ9x~p>%-?~WYQPXzOp9$vShv6Lm<(u3o~{;;n}Q-r&snK0FMg<<3LOyC`pe8yK-kk zhvEQOdaxZ!cgDkl91CD&mxbRcmtf1oTZrtye7MHrDA>DIU?3MiiY=6cNN=?{KCx1n z+MA+=)9<{1ANM%GkXuvWlTjONZRG}jH;cfN;+jMzONgMFX<)mfB<uU+B~CIqO3IBI zakP*Z>tU-wxW|~@eY%^~Q!SzE#q#M=^Nn<ez9~Dn-H45^eF?-D6T0@eG3b69%y=)& z1b;^OjP1~0#%Rl3Hbi?S=y|9LMsKyV=cC)`CHLM?&zBg(T`h$mvf&=yi${6x*BpV^ zk&W!1DNC@ac{A4QoX>uV(t%ACXK;|e4J)eq2dp}+4{^X%kdw@?#u9I7OQ{;_HKzee zdntTyBoL#OhID!u$HwGKvA8Op%^zq5-9-}cT$MRCC^R9G6|<PAqt9@w&TG*7DG5BM z&a(d7>%gI`MfChsDR?`c;Dl-uP!N5HJbo7pE&9%b77a+uwx$ptCz_-w4p2Gszp=~C zDS$tQ*Vy~iC)#y~F3jwbC)>*>=|uYy%BeJyUSVbht!yZA@cLK#fCDF?@26x+F@#Tt zYiuAbDFRdS-*sRgG9&lC`O}{&8%go6WOycG01vKNLKS{G&dOA*gC1%G-kX=s&QhUp zUnvVe2C9%1*4MxxtE~X}noz4#oxqKkv(eQ^Q7o-7$vZmt14)VfiBB16<7mgZZ0zcL z#G`Hx-bcpq!=00O>z^S!<Lxk5eZr8Y8GUrO{wsj|FnG1-FkJgp2^wpq<2g&K$Q~U7 z5;k3m3{P*N$G(n(YXalv-SI?fNT;2)5&wgqn%)L#GA^+4Nh}R_2`11JA>^2m4Utxm zB950cvHrVHFz%KF%6eVNCK&p|_X^9&E1MEtb6pG@^TG)(zgrHLZhynxdUpvv4UGip zCn0RwT8cMZ`vA3*`@s<<b&_M!#h5Kygk3$h!=$yv^i~x$`02+<uth$La`YKs<Sd)8 zs@8U37Fthl9CCm$(Sly*xD%Jy8_(H<9~0E$vn1+SA2{8vNi_O9!TOCduvW&KP5dna z|3_8!zxwR|GY0t0&>i4CRawrMoZU!SI6Nd3YidAd<Qn8EHVE#A&LAhQiK2~HE&1ig z2`|F=ETc0$2Bfo1Y|1GIsK`m<z1<qHX5~)OeT2gMBNA}-6DJt5=@Pxog9F}I6UqNa zmHp57%m1dzmU;bPN47fC^BT;cEGFQGe-0kVdx>98nuF=OjKImdk~l4WL_QTH5!JCo zuzRo=ED6ozDl+0p52NLvp%~7U!6~Tu=Xsbve+BoR=LB1;YKiBWH^eDO!C_D2G*aSS zfqSy#ppQ^Bx+G!19rBNYVQWqZe9T8+ME^9>o0>|Te!d3@>FH#Pd>igg*Mhmt*RguA zGQ1Kx4;EFwXXm=<(CXjPab)l$uCF)^uPFGywJDc@lv*&_KKBqb3KK)`qOUp}c|FLD zzA}f8|BXPkX73nJkPH&HeZ)C&o6s)0gvdLdBu{RyhGUC*VQ}v>Zkl{Eyl-;~H^}yr zzH83#riKqx6Yy8ptlq&8w?J5(DG$YO@JW?w0BPQ$2*nm#z>%Yw;BWCNFyT{-m47}4 z3pF|LJMJ4B+r1u~Sx~^0PK*+{xRt2g@jGX`q@8={zM8xK<QHfC506VT5JL`Y0$Bd+ zQ=C@1Bj-C;h>KP)A_L_NRt=m-KD~C~dapT<g<JlmJyn;3dQn|yt6~Zp475mw_z|-I z_XbdHR0d_$l3}I4BfR^j3+xLL#`z)fxS-b&C*R1x7e`Zo&=5!aEFWM^6>G>}oQ_{A z1#^Xqr#d_qyMTJ)3lTfHh*(Cnlh<3vN%V;e&`MyI9enx+hRFrOj*d*I9wbY?T<QbA z^aVGOUNYPgrz`XY?kjP$7w5*T@?gHTC~6;;fl-G&ahrH2@EP}Jm#8em^s;1dp=Sy> zW>$y;cP8PEoatmy`bPNGJP{n9=84D8-GHZO8WC&3O;*FAT43W9jWky~Bmbxy&@Oil zdi2E^$?sP|Ki027Q?5!Ny^B*(szft*qzz#lPD2TWUg(Y3DzXNifSIGcaPie4=waId zW?H_*>a!t9G+Kk@48llkMkD^PV+mOGTpr{}SOcH5a$2}-E~HBj;qOV|Aj3q6^xZxV zYD6~>v2zz-_^ahy+2G-(djFfmEc_*Um7RlAyw1VH-w_ZhI0*iH5`;pW-OWke7ktkd zxum2*AKI&~=f=woND89^oNE=h<n8q^VD@5oWbt)!&%ugJ(TZmm6fT8t7OV!cT06<U zJ`=oGzXZhDY`_&WUeX@F<nf(%t-Pr-yMcG6J~ZFNvEFU6+(MPLAbT(YCESU3@LT>H z{mjls3v#{zUe*wW-w(reMXLl>>^sCDVH~b6v4UN{eekCZX3&w9MBXD-c$?>JqLdBc zl__3i`qFJc<!BjBxOoZRl1(S4?j->4_CrMfffA8U{R({dtbi^1TyeJB7+o<d54f+2 z1qWBhlN`DdAN^TJbxT;nS(?7w(D^Wwy5GM^H?V*!I8(+|RMX@fdyWfBizl=$kGuX> z4b3s1gO0qsjNVB<0a}rD!1MDmsF^y#J~tm`@*YfuPh$^bDqDpc_SZps3I<^29)Y6S zB!&Gf_j9(gG2HQyX>`dyWq5m19i6#Pg=F=b5Py6Q1kdb%%RSoRYc!3hh9BatPm3TD zVH4!&Tg=Tm|CbvIzX&%7!VnsIPr}G@Pm&fA$yTkM*3^47fehZb0h-3zNb_zh&gJw& ztYTdN<IpHpxa<$F+EtK|STW$9=ZnmO7jrV!n(*L+JpAPpf{MdeBkhOhse&(I?3vLN zyk|g%950p!(GT~suFe2${<Z|odHs#mdXRvoH}B@M=RL(ECx3Dabv5BtYZ1<CZ44U2 zvE0I&Mnqb1CV08O4$Y`ZK+e;aAfL`Wbm038)K=dFzx%fk@iK2N^fm-$=PFrA)k?T9 zb_h-v1pcVRO@N^oS28HC4*T;v*e4y0_K$z_=+A*P^LM)ytez4ACKDLCC$5%UbrvRT z9VEyGei4Z|EDh@hXMu=}(xz>Wci`u!a=6gSfa^2Ofu|=1Vc*McAS?5oe3ZY=X?ktu zZq^-S`@{*BYxRM<!Y80s^b6+k*+e)mT$GbK$%j|{=5XSRm4U|m%Rr}#0;?`p!0@PP z$Rb>Yq=|O2mV9e6^Y(FK%1>oGhDN}Is3=(q3y9BhRk&ki0sNZLz`nUH1qYKj?!ZR{ z)DY*zIq5nec^iH1#-i2G^TZRf;&C5Yp1J}><;Kx%a(!TJ=StF;tAupAl(@?Kx#UDu zIXC(AGqI0QAg&HAaHhZsRQg^CrB)qfbw%n(YP}e@|7V;4V%UM-)&2l$mWn`!>-s=H zqaVC~rGr1jFCu@;6WN0%JVKScBIiQep>rb-+lLEG^56_~$Xo>@^;&@re+N;|Uk=v& zodSCdv{9Da8r1G4&IQy;5sRK@aC{3z-;%uqLc(W|^R3dvEm4H@>5V|I%OmXTXC>r& z=|y&GasdSPC!vl&aU5*hkA)Ur!-*S@(K=^rvDQsFGF`wc+TN&u_b-ew_SU|fRd@i% zF%kUFeSX}lRuwMRG^EK%CzQLO6aWTpWJ2jz1>9FDj@yWPk<h;{p@V@xk@@ruR(=sd zdnzZONcuQ-nn)owl_%jDV1zpJXK_>1gSodlqr~sE4W1q2jp)eT=$O+x5LsgiHu~Kq zJNNR5Wu+O(NS7nqOT&=UPymW>HHVcaW|LQPTBt_Yl*H>kM_{i89MIJ#Pg*FFs&*Y# zg~^eW^m;)S1D`AiEaaZ~>2O<yEy0gTL~`=1NYu(ohBTCtVYe!*7a~u>Zm6J}7gLD7 z{w;F!cOPYxmJG~RsiBW8;^>OmWjyDP5`=p(_?nOj_8plIFG`8S-@Jz;rbLkoINu6P zE3U&0B}4FLnF|3g4>#S>FhIe*%TS+65_I^Uh1RN9lFesCxd;7XXiTM?3-Pk%*6Vb_ zT*WjJHfJw95c~-|Nt=YSx6+`x!xxZ!$Qy*dd`QUjFQnT40$j0Ph+G=lj{L2|Q081| zxODn6@F^n|Tyfokhw|P4Z9y<vcy|+7mve-abytEOmqB7Dq=GVL^wU9Uc}<)DP9^C{ z5@`GHcO<>^0!;G@!Eq_AWa_*b+>3v#xcZmfM0r;zayK?Yw^m1RR>fi5)QDdsq`!q2 z+>_wiD|p=ExnD`wsio*@0Son=%;=u++2pQb9@u&PBaoZR;W5QUz%y+jzLpe(lVugb z-E$py+Z$W#@kvlKeuN|MCr{CSf0{fF%HSpfMnTI7QxxJBk5*b8MZxDSV3B1eO!8ff z-t`JKIW-AGg|7|dpF$1jV${k_KX4dUR`mk|mAhPgqcM85`8oKQAmGrY9+APMZ1RRQ zV`JSQ`k$}^WPa>o@~r(6u1(Y@SI+9Q84)ZybCw}7RxgDE3jo|5Ylod@#lXvL9<br8 zBw9aU2koV0n&!R~b|~6Y3oSO-qUyaPP^3_X_<8IBFWe^ZXN?K+;=~~B#1^vZ9coB~ z7X~G*W|K)zL&&~VAn$I>gufjsh`+#StFM!Sd%o&Y)$9yZ<*ABFX5GZp#q~sYr5}_z zTaEwF#h{nI1rEH^;rt5!C0isF$m5D{#3{UrQ%#!*jWP?-8ryDi*F+eJCN>GwnC)D& zbqMrMHiU@+wr*;mzQgeO5&=A70z9^ek*g0nkbZL=d9vdhI4#zU;`Tiv@qh1-N#{KH z!Os`U)+*4tdwod$;WlzEBZuV2HDK+`i$J|J3%3oPV}<9@q*!(ViBfNa3G<Snj{X=W zA(jcZm<#y1bq`@e?EI!-`<V_?^FIU4ZFk{Fkg$M9JB|m-a!IlID>g`c6pkld!sX8# zptxKLC|f+2_7PR#w5tEb8K-1XmDvZ(YD<!6*K%y~uL#UjaV3S@pR-ol1$ffw91h)G zjU`>y;qz*xc<iGN(M_ztx_0rbbnIKM!)AmF^f>~W)m6C3cu8~-HWQJ{p$>1=643ZD zNiM`znR_zJ7&Z2(a9>;X$lmX#xqM&P^sV{<-r?K|Q*Op{34AT&o_QA@9=nNN_!*L2 ze@}AXyzi0j04c7(y@R{;?lttWo=MbmrOC0~b|iP?0$%>28?3xG1)LV?0l7By*g7T$ z-??yuEn0b=dTJDfleZfasW0v*wk!*NaF2ITsIEajT(>pNHOwUm+B=b@@KtE`^*C3T z5C-SZc*x!FABGx()4`L1JCun;Jz2MEJybD`Ai82@BxB<heA8kS+;6SHF5Z!3JXDzI zMjXMXZ$y$qr$fnDaSkrd_JGrU8-YuI76|)ZNoRipcunR3vQH(8bx|FoyLSabl)j%O zW{O-w*lKPh*36;ziV=D*p-Y!6H=zwmW^wV9GAC5(j4roWaRWsL+=8o9NkdZt`S`n< z^zS+j#*cr2kspSL`#nWe*l`SGEt*G^k{y7^pJh;%YX*D6GGS2UCED>{A2NTB4c^qY z9UnLLAbxwyNzr}*T=kg}WXq^?N{v&=M*l}p)83Ce5$GV8i*y{GHRz%Jl3!qNpD_x1 zU(Ct&wV;-9MUozH6d|n@C{>U#p}Xk|c#ypcci#!)q&HoIn_oo3&f$DIU|5thHMim7 z9$zH;wjF?ZH3L+nTpzFdMhFplITL?bGMBU`E0XBX?<tWJ%J@6m)VTKALr}0{1zbf8 z;Lgng7M<du-c7IRceCut;XenPc8z&*d3WWS##Dw;kg-0hKk@;W%P!z-MmL~{K0to! zFXlvrZ^O5{1L2V`Qc&*LHmL1j%DReA!}19o;1n8Q+ZLH}3U2zy?m`J1+WEx3XNf3U zMT?>h4~^m0Bd75WZw3$68o<vrYe9cd0Z?=qBin8~C&Oo4>4p9?QS!z@r15Sev-_Du z)7{u-Q0henI<?UnjlIf4s;9)!4BZ@<Q7(x>KELD!?Jwc%ei`lnuMA9Bw{bm3Cg5b0 zHEg@s#Myq%gA!Fq^zny~$YnZ{IpW4Re5X4+llT^zh`xiS{F8tus8?I+CCEk-Yhv1I z3O8rk!ji^v+&(@Be{y#Nnm6Tu%>6}(*Wir~eG_Slh&X~y(JYyoAOK39JE6~y4#BM} zJYY!bO{8RR-h_8<BG+PT;E{}U*phyU3pk>Ke$I_!CuY=hI@1hcU?{=fFQ*WPdlD$Y zdKJ9<Cl@>;XJMMWKC)J815s@%th-JUv7I$aIu|Iwqh~vSNy~Hi*G&rT=voBZ_sC+^ zE_cq&>J0HPo8sV@cM^5!yg^s@K85R_XL6m&c~G{+1s?R1;k-MP9jxYQL*c=h+|1}A zE>KAcgq!Z;JYP-W3Z1pl?A^75*!9A(FfoU3hONZwY!#7DhvZar81xEpg7bx3!9JTu z*m<K27&xIq?z|lWw|hsh>%(pM5h?(db}67^Z4B4ZZqB(LTg1W9!<@EN7h(G#r?yMA zDRgNe=l*5|oBlbEoZ3{xsn<<IK4EG||H>dfdPo+JZ`nkOnp3#!5MgflHAmw7W+(h@ za*WKLI}G(QzvF-Z$tM4!D*IpeApfDtYX0iSpCz4O*RmDxtEB_}bKj7(UKW9d%R0e~ zDgc)bHh~@C2J{bqaX4Jo#|DW^g^}a0vE%M~a)f6L|M__m|5b1S`5#}g9Zv(u;r41! zyP_T&tA57I)`vh5Yd#6LI6&SeU14W59w83D{m4%bBl7=IW&iW_{+lZMe8wa4xm=ta z+v|et;u_HMt!9XyufbL1W|EyN#_==vKzQu3i9jv;f_Vwb*zJu2=hiw6)$J7FJn!sc zQ%_HUvtAT2ZYS1(1tsF(>zjjUROJ$kK3Pe!!Cb`NZz1m@LbzH}JGdjJ3?|*FB<E9t zNuRL>3EwWxg?K2Uzu)}{G|}YV8~VULm`#K|Hlbe+V_@mVX(+-y5sehY!iY_?h*-#L z=;*N?wH|$h%W?}yQpzs08!RSL!H!6|-GqH<c8BPsjDpWV7u7v;Ma+a5I?YIP`i*MD z_2DZb@=c8-zo`Igi?i_iJqx*^Xh-zY#Q=uA4T06jkMvAfazPiKkV5$om?Uk7^!2>B zTS`mONvBbAUVAQA*D)9U(iw;Owj)U4eLDBgfl(+u<c2G>ePFLqIb5_U3cYEz<V?&G zkcs_wPQS|$xm3BrvyIu{(Pj}QZT_zYE!*cjuf02YU+65pgYh-~6IYJ1ONwH8nk=bB zOWxQsb84s<#hpyQqBIp1yo2AVeV3>4Riu$D7iPXU-=_BX6!Xt|w^PTuj@vt}(xaLk zjrdYc9+cUWYU<l4q~5B(;6IxpNhue$^OsSvRJx~~{mfW%e*C=0yk8v&OyJXFj9ixx z-E~u$m85m)&M+h9SnFNh{6iGeeOiq!u35p4i95xc6RAk0#SZhF9ZZ;y|N1j?uE^8h z7wqAGn<Y+_?fS;F95&#8U;l}DZ0^pSDUar_Jrzg=dh0U}J@fc}hl=<+)592M^#k7S z@8Z;%u9wVK4}ZRgWRRed8f7wsE>KkcLB>tfm)iSOl2t6#qCZ=gFkS_xm{{i?p7RG? z=8~KYlelsvv;Nss`ueXn3V*o7JZ^c$e$T$kgy?KwU$%AgmkFCP(GP}rE6eyy>9ba5 zAf3kyYKbwrUz8}v1*yD^)AOhxwU7MP>KN+Wk6TRTssJkS+-?3)`WWA7RtzI4|B7d^ zCysx%b|=s0auHw0D}!ItlhJr`UOM0GLonrw@_999gLv`}%Nf1cV*aZBrpB{&;*5sL zVdlqhGQVqGB&Ghsf#G(f@ExCVyu{cX=12DydssJv5?$KKFYm2jw%;A5x`vfm{=aW| z8$?|x?e^`|t-Y;`bDs(2?c~N-9uK1?CS!P_LB))I?kS4y3uc_F&hpZ8B`D<~8M+`c zkor_`(0);LIi)h_OJ$V&!<%hLc#6iA{D`v}lx+Jbb8Cwx6&pIi_xn4S3D0$<{@9<Q zgK9JEMWnOoL}xcjb!;`&GRuuAIygy5nLo0ZQohb4sZXcQ?@ppzc+2_U=W4K55<4ir zwUN}fnJg8&IDsvXU&!8a)naY$erOb3B*bn#tUyr%XZTjX4A`XEc9e0xD0jGTfJi*c zAS*+zzz|smyz)uo_*6YSX`s!{UOEfD->u8NkPm{&FZxN`<pmtyR|3uPenA?nCWzt9 z*<fH!3|XM*1VN-HDElgdHrxt=4M(Q{X|NUjM4@CqyuscqDS_+$e1e*XLrH!8e&qG* z8o8Tl!AU!u$6t*NxG4e<1^iM(Pn(9|@)ZxEb5|`axA_1s%+5j6eAHkWGsXrLKO|9) zywJ&=$z)Z3Iq7L%hjL8w;EPSFNJcfB7eB+Ct4J;6EMv3agsmxZ+wq$9x0mN0trkHc zbKSWTcm*QoQq=wW8GD8i=F-R2P=;qFcU{?p{8W8Ko)6sv*YgvR75f;}F1|vvwpS2- zlP(fp_Y96&8o@OyWKlz#4|mhxI9FQyi1=3>;oNiMxvW7YF8{bVr&0KtT&k@g|2Eli z7wR{m=j=*u`G!i)&&3wK*|H9AvHOfa$)$tc<7K#z%>-JH#@STgspQq*I{G7LPbbXX zO?iu5rr}i@2tB9<k=sPU>bFU>bwMZ_y8kzQ#@YpEPS)Zf%OR@$It$<_S=^_09IsT} zk9BW}vvpb{Kv#1I`0-g2RBl_#CY#M><-B);qf02<T=WEwd`bhswsqjCOfhYhYEA5n zR?#ZW0#ldVeTHELu}}$8peG>-E9)*}nhY<~qfuely!a>Ylio6Vi~CL-m3IR>b1d+B zwvFLOouHME9|nm}S?psYhP_j!Q7-YvaEsj(e8k@l9Gabii*r5TmA`)QvBer<*q}qU zeOyNrI|}eM<@s#nL=JdVR=|Ex^Cy!B6R~=hC<OOUunN_hIIVLzcobql+x*%APPm?d z%R3RgV6%pF9J|iuR+!?*-Eri!*eqC(r~$O}MDW^)0zij((UP<Vj<dQ?EVj)9^ldr3 zZHggoTX2Aw-`EA&g(~<@Kreo<I2b#<8^g<vWq>UA<9OFcJ2F3)0sW7y@j~HjeCoY0 z!BaHRb5&F5y50y)*Iy1h)I>;uFAqFC+)V!~qXSQGGNk8oJw$p#jUeLa468k_pFT8y z2FaDn1yu)Mfk&nrfrTt0Ewyfdk!*$g5(2PZb1GR<zlGeFo=TJiJn}S&Hrjnu2TLwE zD9Ea~2>V6jNLa^yyi1Ws8rFHi>A`w1X-+kc*&@M;{Y@v|LM-S-3BmCChBTP2mBSt^ ze<m;}hr^jW|Aq7SN^k+qr@*mI@4?O3Ip9}nv_OUZOKM&c#`eM_NS$8+WlRrXlSAD= zNDw0`^(Bf`8Zqa1*H@DpMU~(MSV4L$df;&vz+HL}OUfG($n2s?uwr2x5xUik4W9!N zC*?<kBpUEg3EMcr5%OqHIgz%Q2J14$>=g#9=m#t36SsASFwMJ{JrG)nO;66kzY9gN zDD4FI%VfbXBl*B}tQYJYcuL3oJclpBa{P8c8#RlX!$S+?(0hwa;KkF2YK1klrJNUx zmtRa8l#Wt*f|0kx?hI6}(ZjT+EC7K@z(~T1D5ZVC=X%PZ^zuhU`j!~nW&It*==+lj z+L6pnm4I@Zs<7T3gG-UZFm7!=XydMs9e?|Q4SS0uZ`UT#E}dYFDN8Qo%!4aZ%;CtS zDlE(s<OMuLV2b}${P^lwyw|K0ynZ80X+=-P9{(J|np7K@6|Ds~3vD8v&=IB{Q6%<X z+E`Pr7t%*W*`c$sL}jiI@%xfOYASs|vx^6A{%M5tI8WH~F&Ww(PbaT5%5hME0Ju@! zP8K`&gGb9R6N?y%ycODo)k6=%>4Or;>zOI(eqsW^*E687W&@kvUV!WGWz!MQjp-TI z2|&lf6&9D4V`HH^V295&`o8EGU3=b|b`07EiZnI>+29(w*iwSo;wuI`cTNXFdYf># za32^xc9G6>9RUwo<L&P^oyR_Ijdbkh3HH}O8OuAH3a;Pkq8}NZp;lDs<JD`&v0_#) zyE4^>7G1K8$iKM_7TtS?#~OZv)t9@$%%9)+4;;=?1}5?#q>lxbdV9!{zDWGacoFtJ zJOvcY`AB>2wg>l|?ZDuZd6ej^TcA_=Aox<b1J`se0!QNvSw`tPYwEca(6Oo5rQVz_ zzIO<WxTZqM$sov^FvFzzGJJD(71`!#1?Q<taqel~u!qcP>H>Vsir2g14W>#&zH<~F zwyXz|Rh1y@XFA3Wnz-1Zg;kuJ3HQ4=(+Sy-oao`>jBA1@R)J|%EIET6)x3}E^qcV5 zno}S)YYb2Sa1sBHs_cLD+5e%+Zq&O%9(2Xwu%`F$hUX<}M6-mc^qXKKs=hF_c9wL) zz*+X!raIXB;tUkJWPvgz&(o2zD{!BxKCBJM2b;Px*(6m7tTeX)w2jS%hU%vPtb+8Y zzX;4X+)li8rDb+*-m^7qpI~F@&QN7}X<12OWl3ofNx>pR$r+nf1uIx=|DRLM|9vXh P&-Z^nE9xksDD}SpS)fLa literal 35344 zcmeGE2T&Ew7XAx^A~}PIWJE#1fMnRyYcMOKm{2hxDk_So1W`;NK|nxqQc*x9DhP_e zo?e3q5CjtjP*F^nMKNGN?|#oY?+M@co_|^AR^7UFORA>!Y-Tb2?BDeC?q1Itq@`4) zHf-@<@4I}pmc;0hqs<*YC-srFk(FD&%+J$zlZU_8dLPw3qiwb1Tz&jD26%b~dAgeq zlKJNwQ#n})kIkNbj(rY^OUZs5DvGt!k{!3+$Isux$KQSQAn8B%O>YmC?YGIpJHXSl zaj3Mo^kC`1%HmEnL&d~`S+W1u{ruMSYnS9Vr{A1@jli!({q9e{IsF=eUyJ(PpMG=t zH3GjD^}9d)=Jaa>el6;EfBMbo*9iPt)bIZEo71lm__e6t{pmNSUnB5qQNR1sZ%)5P z;Mbyp*+b$IvU;+jw4-RV6{R2TwWOzc_)iP)cDEQL`S(59VWMO9s(mcXwPa@hBZ+A_ zNa`QE|Cz${krjo>s;v@bK7CfX`*`|~G#?r0>9uOLzpToC{BoI>ho9(_qqQU_d9GXM zZZ$~ak5^MA*?u108&-St|0Az?NL<`hQC4D^r~kj+E67Uv`+E5Jjnx!CB;H3>L)6}A z(RnPiB<Fbetai5^B=N_q>>oNT{zHewU+J(I`41hI|IlIcKk5)6val5Cuo59!ztVr5 z$7Xk15u(5E{vDB*-`^z%i~o(0j_2kLp1%HnF!B>Y^7iua^za@1r%0{8KKc)}*8fmz z_dlxrgPOHSt<68E+5dxD$RE;dWdBmyKl%Now10xM)sp(#1s!evKWh7%2J`>aF#7+9 z=U+$vfyeGo4Ws`@4Fg0N>_mL*wWR*mt43S=J0SbN7WdbH{uK8oAWQQ<#aaH3;{FPd z#s3k|zmEPLkmcw<HT<u)od}Sn=(e-8_!H2d_uU`jEG_;M(4Wo!HK2bT{f9WqKgIp8 z_u@#=)e{Y6mR4Hw|1?_MN8A2a+eH*_X)cPl{*QRO|6{!9UbnRQk9hn48gDK-zm;gS z7Hu}7zq8en5xF|cJ^bA*%m+#SefOV3gvbc$Bm2iEuo6Yu{d)@*qyK9Q`l2)2iZ)T7 zwY1lg`EvmO*9dMZIw%^!t<1G#{~W<B{@V~PYTQbM!)o+@G;Z}@8?R6m1zTvz{pnP> zTUh^_ZI!P2r){-IRb*RfN^6R2t6)_zvHG#U_7(MiC+c?_zfSKrtKY1Coxrb0{(q%k z8I1Y$&-}ke{%+&f$^B;ao7Jxq`1Q#DXZqD2%Wl!E&B{Wwi6+ukmj9Zy+5B_eEt<7i zi6)v>R{u6@`(su4_ci!`EWA~f|5k)FVvU#o2+`7Uy|2G%bAM^IKZ=CL{?%&SskA>+ zbQJFVm<8HuSr9NzAF}ezSj9b`LFbbSJNMpQSliVA4{fvI#@hR^`>QmJ@Ylry)upub z=p*=iR|3p6x6|#KAtYkE5$buY$0r8|;5fZpjFL8>i{iVe&%33V5yD`Zg$<9m3{5L9 zk^a{YGg0(><>8RCxL#?VpzqFSw9<GCzEx9#t#3XE`^MadA~P1|ecT5g6M2|@UYu>- zqR-}ExeJ>nn~>#BAt2@X5?nS#z=T9K=I!=I`f=kaVr7*kynkys&itkbClbCfvqsB6 zzrJ4xgM(?Lj680fIUY@J55YxlDQI^-4JYYm(VrJn!Q)h4=-DR@PK%GC@tYIman^29 zROCwrshC5`@km@W?-1&i)S=d+6xvj%&VRVmh0i*M@ILn?`5x8NSbu9i+ID5*D5+hD zeWjSehLgyi&!IRy;S}9)#T;s{%V9vLKERL?&{@NR<d4%ZFe4UD%Fh7>H%~}2mV|WI zc`P-jkTFyfwl9gpS<{NB;p})=JIb6?zK`V$l&(O`=R{nR>4@Lu0@1*I0G94wfmH?y z{L_{itW^`|_df5#H~JjFoF_a^aIL_stZJ;*lZQFA-DLKvcG~uC3iK=K2RC=$CyNdb zggXuSr0Ags9vV3oKf2hEik<<m{A@IlxV;1R9X5nTJIv{>VM^q-nhRRonhLFHOK8^M z5{7-Whuk~!jysVQ3Cp79W2apv=#3DA3#D#&)-)RZMqYp%R}FS@RwC_c&Y<UVM!~^G z(RGZrL}u$FuJ+z2&bIJ@052%v+4-%^!i{~I`?H4=zm@@H&mBt3!mbM3>NcR<0(sDy zoP(Q<?qS02P_W7Q!bz_jf;*O+fQ7HVFylWBBi=#rxap)Jtn>?k_eTQI_O%4qY|F(j z+j4|bYYX7OS4kEI_rdcgLh!BU4LIYI1$(t6F!w<a+PCk6!|A$orn40})YV8viu*y* zkq)jl=o}H7){k26eo5!+`O|w7T=9$G8vR-BO9I;>(Q^MC(%_qn(_7PVtKn7BVfu)k zV}jsOKY3m+E(xjyZw1cdcF=+b8)AGi2rAzULGi<<ap$yca1bVu5^?}jiq!Zwdt{*G z$`!EC?T?=~)nQdYCQMf>BF#PVWMx@2PI>7`+>dxb;q{LWZqp~h%Ng?AhHr;x<BA^P z2j#_3WxY)3Uwnbs%qWN1gFeytgG*qyLlo>vx`MfRa{<np&<}G{Nbjy(;vHXr4RVKJ z7%PSP@{91Yp#yfs>7tUX62CX$21=@n^REuv!kk4rF|Pe1-7{PkPbD_dZ8HnV+|i54 z@UvY=%w|+xJh~H9y5`a~Dq29k?cgSsG2}`d1@E|MZqvzlYW_$D&h$#cLR%d+&#yn+ zZF<Rdj<unO1e)~H(*3YMZv#x7HVR%{4S>>|6xu%KGKrK5Lu(}#2Of>!;ySaTed%|+ zZFLXz#>|4|Zy})l$sAv8*n-oT^Tc~!Un;b-v#*=Knz>`J0vwzJq14?1GG8!cOt&HV znsSQvojjPj=LeH#Vl#>Imq$cu<b9Z`dXT$5wS^J7y29vPr*Z$;(<CJFD}DGN5|U&0 z(4({G;>(NIL91syoXjkMZ?X~aDkv8wZBl3FUF`(M)SA^C`vzS5l|#{;MrfMe2-PE` zq2hJ0aOgE@VZY#)<YZwrX=chX;8Qx?aX^#U#I_Lw*~<)bw1cj@br3JO_QM}76ER8N z9XFrsz@O(wqk5ngl=bFO^5G;G<vE%^cv6yBE>faj(=T(<&#H-W)@1P6mxUi6y{DxY zdKh8ydr}$Dix(Wf;+A(oP#&C43}-IGr`e^LW|j&${T4907Ugnz=T6{)1~)iSHv;6X z&ydmx1*nr$ht$f`bjFH-*mJf5b{v}z7w%QVTdgX{@)3ish#C^Qw2fRA)MBj#<r+sl z;mkUMh>4*zdapfSJ#XxOIANy?nTm-x?QR@iH{HYSdmRS{M!&$^lA(M)A47gD`by3f zO~M<Ap_tsV5xgDhD6BDn0%<i|dyJCm(%0k`6DpXsU=&HVm4}b)U^p>Lf^!c{fbS!f zVd6$@FrLu{ty9{_9Ovs~ozfVXJA}i<xg`Sq3*zunI~m-ojj8y+3S2P!C4?Ji;V>T! zaQ|2Ul0%emv*uC!_-+Jf8^0x+l@$2ALzhtR@GGL`<qZpa8_;;jIZm%&F|{%nggdu| zqn7$*@+A5ul^%VUMhvQ^@fwqHnW_m)F$e|~oCK%OM}v;SV!CEtJ(NH21BKRdeDgJ$ z?0EY?=&(qc4ahi)`qhJB>+U2_QNK*jxJQ8EOIz%IK9x2d-wHBZE0lDuqVlTS(dgz^ zaIB4lE@m5+Djc?7{^dA$8A)>8X+q{hx(yWg%!BZwQ?Z}(8KQ8#g_hi$3r2ne8H4-5 zXgugE{n@?}r^>d_+s;1F-8>Jc=>6oj$Y+y{X>0NI$qcSJ?g_fA{ziWEQN((hP1le0 zf>o}^!MNXGY*n<uBWr_U>w8J|nv4dn(67U_>~pB|IRPr=1vGH@G1@WD2<8_a5U5UE z$UQ5Ff`Ah`@WkX2Ghys5;TH!D2q4;MHd}Np+vd>u8@#YCRhJvB-A$)U+$R&i79(!l zO*QqGfa?4RT<ID~YBRNY-Azj%YHk<vLN^4HihUp~tOz{TDB)qNR2<q-0BvSn@G<B# zChqN|H<d=SucQmW=J_rRPt(BOkAWbbL22x;)zum{Lt*CZ6R@IR4c(EIN8cG9bofMq zn0fZnc<t+6?D*)2%bFgO-4`>dPUk#&?_Cc*QXh{_iw?qrPA}Z(wwoLf#`6;@&Vq5z zFf_?@!>l)fP;z7*+<BRXgVJ_UWj+E5muRr(y7K6f?RL25ViVkT&w>X^W?aS`S2QdD z*xBC<AF6&}21_?G(MI=~yPDFFxZpE6+;oX_F!|j2?@J+n@k9Dzq!P}n74@--P24Mc zZ3sGb6oo^@$g{qsFk34gPq}V`dE_Pr#WaK3Qce6?HV|h&i-%ng&w*st0nDt|#z9g~ zN$<61;JZEp_pGnM!Xx5rc-t6|*%O3`^IlQSEDw;I8Aq9YjocfvRWQUUk>L{@!DxgB zbew)b^HUPgO}K|SJLnKC6}wB%oY6&n^AR}roEW3+y&tFL4aO(s{*Y`n6BiW>r2A5~ z;k}|V;u`LVU!PZDuCEy^bsh+9!>aHBF+#`9&cd3j>d<bk#kZ`vgw`<^h?2k-e4{^L zW#KGbKTwG(EEq%^(j@TA*+}fC+)0}ClOS3}6K{wZ4e!)|pv`xxpQ|Amd(n+(e0@zb zD{9~b7YbjCw~}+k#pH>?K+r!vSdjfHoxXr!Lf33<@a_Fd{JwP)AK6J@>M5-zn$7<I z+el8%dCQ7;y_+e#`L-N38kV!+;R!qm9L7tirm&lq?_uMf$FsHD)A?`hTiM2s8Eo>7 z^{oBc&3v(8Am3kD!E30;@+%D5u{kz{HQm)my6@jw`uBPBpSf$N$Kubu&+>$P=Q{`N zWp$3-WBaQ)^0nore6aBtR$jl6?J2qoOSDaDHhWBF%d6{H<CsP4dxbH4>-`@%XI&g$ zF?lzCqqYN=F4SQ+r-rc?rRyOweHia!ZGhcNH27)n1KBxkQy^|<1m9Yz#X66hz%EYc z!!DZLhaGNti?2_+!7ga0?DZZU$J>hK><r~}RyF!4QJ*)BeZTSu%Z#4H9~_W^Ee|iU z@mn<cJ#l?%VgmGeahI=rg|abg;$O(GoyzgwJ`P|ln34PiX=(mmz(bT1Ae!{u#5-TQ z&YKRA?)(44`tJsYlf23KV;C~kkI#6Y#+&XQB(3+iHO60BMt{0lP7e$ho&7F&dPfE; z$AprntL4#mwGPg`WFV{xSi*h1+)XOpzbB1b<ngwe8d|o$plvp}P_VonSBYH(t&6Sf zmCD)dttEXO6?@HCtr8hNWSkF|<MEcB(+#8A4>Rc?!5Xyf{>~)3UMAyOK9T5IZL~AF zij1?7g_r$>RGbZ<8|UWIxR?%-;TBG=)ak;d>$~Wuh!8T|LJHH@I?*Q!Zg97IjY+av zDXnWiNj^-q<FxKPp*yvwkt@bEuy0v1`||54W}cpLjYIA%w%;Hl$LX)OL(dFDcD3C= zKF}bCg*UP6)aVH{*=@;~S$GO&okDbo4+hVk4CcVfNrL$PelRd<0O*g}M^5LSW2Rs0 z1&;%Xc%-s{SRQ1_z8mZ5{;Qru;`1<^dha_kR8tSv-BW?QQEKGM_fCSd`aqe7F}bqw z6#M2*AAarwCsxJYf*mQPTC;ra6O<Sm&&t@v(XHMS*kQi*eAcDAc=wSwOHUi)ebXpP zmDj>9aSeDH_ko;yVuyQ2WRuM{hV*KcE<`35Fst0|aJJPZWT=7?(lABb8U2lZ*MCp@ z?rtQ@*DN6=$JA*+bt}!9dyY7CDuKk4+uX}#iR57a!(ep$3?HDG&A!h|bu?YK7v*Ya z@}IRE;D(W8%_(gg_TsTbh-iNT7hTu$YQEXH|En(l+I|l%FY3awL>pE;Vlgh?xD#^~ z4uQ(o4>;nO1n)i0gWv6&2IpVsfXs+gnl<V+*&n0};+KchxSn1*T>J--e<2Sl50l7X zJ#j9*W(e77hUC&{alX>f0{Y*(L6nz9*IZ4pXCD=<MDC&#e|nH6YdYx!G|w=H#4>4U zn|hOsGGcLW;9R=P_b}C7xtHD>J_H<#ztQ7%17O$2crJ5a6_a%A0a+R*4O7Jz(MyA0 z(od3-WZfbbkKY=}^?UM|TT(BB$JXp8-d6f(GrPCCd8IOHZ|;wtnx^#UsRl@X9)%~u z4ET`?=0ZL-b)4rciHAzG+3b9z8v<HTCf1!f61tvDYVw2GdD}2_yDiwxal!Gydui)P zPkf!3M-IKeMf~O{F|DEd@ywb$8quf%W80p9Y<mS!&zMQR9X(7ZnLVTn9e)r9X?t4N z{FPKo_Q#(Jrl>z;3Zv-dMZG1?F>+De^oP@0ev-f#&a&Z-=OQMv$q5>~p<^ptTM>n; zYUTKERaf!Dk&`ffwmJkVv_iZ4SO}gKf^)7tW^7M#9RK+oD4j}!$^KzP&+r0V39W*J zId_TnlYxBK+8TN-%?|x8Qfijgm#lrDM>pE2!jjMOIPu0(VzuFxXtGj7CZuN3?6tcY z*S#iCuk`?!;u-Yqafg~wJ6*B!KsG9WmuFvv!~iEZ1ST$vpf9bD!|p+?*vw4BREKZm zXl4Sgc`Ji9#^!K9PmFA5Yl!zX4LBlZPH*qzg)!Gwfb^It%*Ls0bnY_34e>IDyY@PC zMoczUUZ6|6NA;54y>GdYwG$}@e<a)jbwS!@4P1Xs7Ao3@<L%FH$<cS_HR6L5+0+g8 zG_-s!Peym6)w@t4UgJn43(^2<CD{+JeaY-O>v8lMX{w#Q2CGyhaK-Bf+;+uX9OGKU zonM|L5KB?T*g7M~4_S`Ee&V1bu0hH@x6`c0xpY8RFg2)Bgs`$3#I{0{COlon+^A5( zvCdIc_o)S5JG39O`zZ4=U8m{ErD9+;ZycLfG@!<DUk)mUe<qcelJLn&XT0+BDeW`X zm!Fnz0SOk#^yk?Xbk4LKVl(^#Sr)tr^5nCq`9ou9KV}Y<wQ6Kw;xUYK>%hmkJPlMH zKof7L5FN>dbeZ))gl|@~_QFZJLE<0@Z#%%0wHgY|Qq&>#=uT?x6o>o1$%6R&RbXb8 zz&pRtV9V8tL0Ww=IX`0)B-nFAY1M4PDRv9{`O2}&HT$E6Un#kl6HUflV6c3u4F22_ zNtCKGaN&hx%ne$^_3-iNBJmg;r?{iZ4sAH(HJT=7Ig>5pUeNI^{i&Ps7n<Pzi85Z+ zWQ&wMec7Y{Ay4c`TK#c4zONFSt|P}Qj$6zZ3=gicJuJtHUCHCmb7Jr$IRk4CrVDer zR`X@L70d`fBR;Bm7RffLW8@+uiAJd}k?wPf>2dLfdse!jdEc9^UuBAt%N4MCo;^8p zvzXeh{6es{nCWg_PIpRrkh|A53nvTqkPFTlXnRc#wA7{(W67n&W|!!5%@89`oGy^d z$wv6$Q3Xv;SEy<KG0@Rt#3R->EFFwpE>JQ_3DK=rsJRZ|nEqxgX^SS7b{g>N-Ua%! z_yG7Ooe^q|xJXk{wdl2o=Oi>Jix#BHlMio$afsMqqMJ01lp7Qf96plzXT2qB8zX33 zl{K@cd<8vjrAOSw4^fTmD@<^#6Fl``=?Bvu;sxz^qIx;?=$h{+q3gzXs+O{CLoedm zMj6yzYKiBMY=UWH7%-~W;S-B@khdS4;eLxY&b|B)T7K+9n;!{~F}IZ-D=eiS=KKVQ zxc+QW^9<Bco(t=r_Y#Sia;Es^M7pV{mC^wfq|xsd(biGm^3?m`wI4glh}Tj~l=pT* zHyY#l8;7}f?TJu2SrxZCMe@e}0qiTLoYgNF%x`g#WgE7XP^amN#6?^ORm-iwR4tS2 z^P0qsS<xsgU+#ow9_*)<gPpnZhIJ?zqyd`Udx`!wY5MxPAK9GH2Q5#^VWPnhj&}0Q zF`sfW>%kAk!%Q7|jK7lR-Fh%SUK@9(J)^7qJIK6SMFQ=>G|u&8qVV$S2x{J1%#NEO z$;eulvui^2p;G)N4$9jFv8{vz%~8WB#}(WomHSKxy8#`N#bH};jbKWuIqlJ_AZCWK zWLd(#sstZtdgeVx3N9j9u^^KA^cvCufo0@P^e7VCJCMdNxl95pYN^YTWYSUF4+QUJ zQT*LQa>~t{X3QAPbk36J%?)FS7)G-?O8aZF)p=gOu7OW>W_b3#8NKSZ4F>kC<YkYZ z!wt+)R8_Qt^@C&RXNi8~XM+iLS+7RboC`$U_y%$HS&CMDMsT%>-e|Wu4Aurr#RLUY zSmz^4Pw0EnS*LuM6>n~merNil(Greqou!0>6Yf&!ans2MSuvRNX^fDGSwRdZp5RNn zGufVmgS?qRH^ds8VWq3|U``)%e(}3vp!3aOmE{of@{BLEUa$rQIvJ0hbpgq5u4pdY zMlSjsr#tJ+=sqT$Gd7q*0t&6*q{LPtTvtlZrpVx+pdM!V%?ISCsyHOIln_{VkbW)E zglo8kzAMrpr}qsbVVCo%uucd1d=>|f^tou$vj-hE*gM+aeZkk6G3=s!VW@8u&y8U} zp{I&HHXpwT;UkZOw(EH6zvww5c4j4RtmvdJK@rvO0vedN_lub*uWCYT5s&v(#yMlp z(faQybZ2U}uxW%5eWmk>2Ja9G^;QK?xA?8p@qGp{vaO-g2glNbOV$W9z6!Zp7Y5<3 zL@ze&d^!83buoLWx`wZJb!F9Ku7kgd2kUs<i?wFO*!qpS$k&~Ow=yZL<Fy`c%{)(# zKH<f$4KQToI_rd5yUxRhgV!MDjDV-N6G&L;9QH+>CSMV@527YY(eL?V>5U>eIOYDD zX@A|yttj8e*oODTnf8&?lGg@2)y6C?SBK!-qqN2#7gdkz@Rgej@K%s$j&ZAqo$tGd zv$$3XP2(2vPnx5cX9***=_m^k<sG14X9!UrMCZ9W5z~|V;(;`O8a+%KVeU9`L`*cs z^~z(6qY;{}8BWhm$YQSg#uDB2RmAA#VEk57Ox2d@VUDRfd_AQG5t1*-h<jIv(z_!h zY_c>czBx_57JHJ+Ps?E7ib@FCSIFCZyvDYko5)Xn<&R@7-6UUyJE{0CRd8A$1|B07 zz$*B>DB(JQ>E8I1Mov3Ty><<xPi^NC^A+>)<7OM!C{(1M!h!@g=>b&9GL`mgbrNJ8 zZ>N<px#ZZg9@?Ry2s7V!)0bwt5bWK{@p@(SV$CJ0@nACT?2|++Rd%yt9cncuS~7fV zkyMS<*ap_na~$td-ORpKKt9cEB0Hr0H4bq20<uNzU_1H?maB&1*#2@bSnvg&#P0^X zb*p$In@;-KXA{0ot;hS2N&7QP_)Lu`Ho)Wnd2arU9&2nMQ4vaD=k4cEJLNFR>}!Bh zD{{%<P7AD;d_=t3BFKtKP6WtOe$$l(K4d$~>qv;P!Ec-4p3NM#XuBd`p7jo2ZtTMz z>|e-NZt>xh@4sh@#KX}k^AINAVu|0iRkV0>4*9r39Va-;^9^E;A<FwQS}9$jc^A&0 z{Glv(l4Q;8t`)#S&o4CM!C0DURZX0aDqx)QUFP}BJ5=xGA~I#Kmf-8g9dzV26WYIM z3)`%7jgRgB0#A;aU85)516zHZi8`Ch8m-IW$G_9$Qv<fMb=3td#>qJ{ZYR(>^fXue z_$jU~G=O&I6{%b9h7&G*rlct!_P1Qc%?o?D%hP=5BEc?r?=Oxs^dvDvEuFBE7rEXM zCxj0gk_F-07tnB(E4294Q0h?KA755QFqi%FX@gZV4takR@3aar_;G5@4PWaT?Sr`( zJyZiP51$7k-#o{nt0wIE{NtFnFb8Y08_4VluerdHjf`c;HPY=y$QIjo<UnCBt$ewS zjKA#-)x}ew!RZ)YHz}kY9}U4MN15x0b|u=64w7K^1fu+4Ejf@?OxpI#fw(-9NRdOK zq%;vz>R%IM{abjmUC;4LMISH_Oy=ix6UUbrQnU4SJXLMj==eN9oF7@Li~%$5kV#@) zXgjnHk32sQVUHhRp6z(HOTmt940sQ#i>#oo{Rq@8O=Z2~N}%$RfRAWM;nnY|q4k(d z&O^}@B(x4Or@@*22wF=IwfI(zg8q<x=@|Dz7)aV=qiBx!K3aC92%`>vrCH0Jc-K{R z{M6VTcyRnsM=|pWP+M^x>L%@_!(yiqI%GE=?iy6H|8X=_OY8%Fe<oF|_((OkhSSg= z+u_5mKvFlwnZ`HDLcxpGBr^R6of29~4qsi&X!~CxN(~aAw<DW`s;Pr-O)||$_vc&{ zBFR0ACtTS9WmFqvjQ7S}<F*%$!ZC9d*!$<w@q^tDUZLwUUnH}W*N!a`1_x@f?jn<F zTJ9@4_q-b)d9Dz8H_YXZCFD@IWiH%|l|fj4-wWnlYz2*pE75R-Jg`2_u$gb8e)f6z z=#C}HSd&TN;!jT8q>}c{kwHtlq40d(S!(`Gg$h<F<MzC#)XQ`UxgKT!dFK-t@9a7F z#WaviykUbcrpWLuP5L$a8=vsKf(iWXB1KlJx0frVdvN%^`wZ)=#Y$a2iuv!4!<M*m z=$j&%kKt~*jwI3N8lilb$#C|kS0k*lw`3)XtoaprF05&$xU`-qWAc~9v7`T1W$d&$ z>rSQcVa{#Fe@6&8S~-MoH|`Obe7nuOv%3h~%mK_2**XTyqPRmtx#}94z^P0$;wo=9 zb5{zs3y-gt6lzMAGRf89<Y-$LV=t`7+)xejpyh;vlgtmCab^MvXYb*RXLJeYDOs_b zeoEuPL}cFXI7Ll6HAIDXf5`jGG|fMATQwTTZrrE%+(OVhSchNI;7)wPcEh=31@5zF zrv3ZsF7lwV0Gx}{sH~h0<Rr$T)teV^J8eCzsgcL|Q={>b`FR?cD+h`>!-zw>1O4GJ z7@j9-Ler^xLZ@e0kdYz8M^m<uZ5^+fiiyYRr1XQ>DxXM&*Q4mwr}Df2TbX<Fb`yVp zYqIy932gH0i!o*S5LzEj>bdXG<T(?&mRYjj#gy2~#E`qcx0n{$moPHMmTVMQLv*q- z`ZP~Qv!7xRp?!nKOm#+&GYhb`?I{Fqj)7&e;V||<GIZN0^B(ph?_k$$I(=LQ`!@1B zh9#}zGta!HLM3C?%Qgl#%dUm4F^8c4BWqfrIt<iJWm&1??htW};dQ=6LeHiUCae1t zWcMau-@;%x-7%D%^4tn`xLJ@-J7ZY0Ehlh8|I>`MT?_5^^aW(hnE*E%tYO7=H}pO& zO8+SCrTtrn@@nCixQQD_z~x>W@?=XTQIC{^K^+Fr>G=`nOMT>aq<N5cD@C{b+U;08 zw}GCPa-dFBk%^jN0fHUjkmfmnyq|F$_IwBf@8?BA>)JbnyZjdSzdOLK^He7~JDqUB zi@VtQBog+{-%Go{jmH=<Gy3dFEAh)I2GiII+HlyAtq+iAtLhVQ+Y34NgkmJ_G3>{- zx0FFiW)YDc?F@&`G;`UrZP{&kKIk%9ntzbk9|I@d#PBU@urV(cZme2CIW;>lB^3~{ zqy@W=KcGv^7L!XeztWv8Zm>WKFrAr#-^vASM0YyAnq|N$d_My2n>29ih7(wR;~ACE zs>2JylMv)n2s*B#+0A#5+N@NjTc0oG9b<cN;*alef0YHFYuioy88HF#;SQPgO@dW# zR$~|S9mto3s<K8EGkK#93p_A>GJnyrneOb<&7~Gv@>2`cVN(5mSho8ieP?%@*zKQ; z=MGtdjr}I9pYoF=$@wCb%z(ZR^dLjzgC`$<&6tdPPVT<eB|ihYsh9UruGiovX^wkL zKDwxp7!6O@Kdy<=K}YGBFVb))<u%D)dKJCQ-I({*06$+_kR2&2>4k&`bVr~8cturn z;>}ikE57F@n*_s=Mkkyw#}_{&#6exX9h~=5WUp)7rMV$0;QFSSOg5RurdbchC4EoP zyVHlkj3HNOyabP<Tk^r-u@*Z+@E*n-*aNk5AHk#HRVaITDfVh#!S5B_uzKP!%<_oj zhxY`*<d!aS^~G|$HCGokzHEY(JE8@`BTn>{b}p2!3&nk%cX06P{erdvZ<-n^$$EUq z!0VZ%WZGd>_S#WxShHV@$=*F3JA~t~UEvxU6mDV7)u!{3adTO@Evwjm^)~D*%i~np zaUh)}%7f1rykiQ)?IE#zD{&p+3402{=>*#p5KO1A;b<6V-X>2U`CKNU2U^guaw=0| zwgBFaoe5<>G+{>KJrXW`nrxOxf-kq5X?>C=xv8B&97m+%fp8uC^0JADZ(l-hJiLOP z*{5MmLpCAZF3|9BAJvyF;*`A+b|ue5!wN}Kf8;E9Za>bH7d)bQvl!v_>JhZ@WGyV! zvtSRZyTFvH0MZp1Cy2}~fb#QqFym)6?N}wlS5&PcG3^@gd3GzfDW$@I7n-c>xiU1F zE$y&0q>kLKRpH&^i!g6{0^htSgLuDIWkc%AaGP!obeCnpv{)1Rm94=IS|x;i(}ZGk z%@_qTnI_)|VUu;`Xk?aJ)tM}P-YVxjLT(Zi89DJwUl%dP!*Y@FNap8BDYB<l9V4}; zblE4Smi)4F<FJ{}fLk7F@NCr~n(pk#t)Hz<rr$cooH9B=GL-L;e)YZ%ZDSRg)hSa* zd(bdi**B5|ikB0=Np2))?Oo19<30@t@}pA^pC{XlM0E&z4CoU%5IIDu)Ulr=iJf&w zxc@^%<=V6383*&3ut=(m*;>C)An#{NpFLD$mS5MP@3#yilI^if&Q=SWa`6`P<B~du z!G%n9@IJbJ`g<a+r^!_xOQPBaL*U-kT58rDO;@KrBW(8!BDSZKwhYRmGM^-{%Upvf zhb$0jo=wKZUtydLRxv+HCCHGp+rll+FLH-Q^d&XtzcE+$Z)C<@4Wa|DCDUCuACkwH zQV5*V!15E)G-}5WF5rYZe4BTTUap!$+f@amCnx}wl^eJPmn2}TUlI=7{+V8B^@KUH z2Z(&@X=qy&PK#D}q2{&Q+@(@mD!#3iV^?Mh_s+OMo=;ID)mt`k2Jg>Nv8poeu4wvx zB1}qHs9?g}{I-KBn0|+>PfBEbBg`CJUiM@5SZb3e)^eDz!hpPEI?4F@Nf30-LMT|< zN=gi>gbq1TWLw{h<l-JhL7-BJ!z8cIOhWQYZl1X$ck9TxYAMcu$=z-SJIQ|LY)le! zskVm^H*yuQN0hnk*-M!<re-AlQ#s?8GZYQW%gE6bDcbe)8nyOZOlF_`z=*dubDzd% z()3PkDr<9-DYCppe_s1SO%}+LRY{HH+uSq4_xBtHPu7jY%tmu~Fw%{?GGPSqa4vO_ ze^pPTN}Wh**9*bCyLUKCy;*czmB?}l)27<9_K^h3D|Ca2A{-mE1Zys<2}<dH7%n%T zoVhFHR_5uGS&GleoSPTuF-0+Me7HFYP;DapIuq%5O*gDHw}La#%|h4f<6)CR7<R4f zWd>b1EJ{sjkUaMdpw%UVP02@?h36H>!V&Rg?f%oWpluCvwS54&7c9nTT)seJ6=a~% zX)05#w^Q)ME16{PT+U6M)t6)iUnHyj+)0mvJncPunucFpEJ~fKllv0pWcR0&w0C7p zwdSCkOtIB6GR0+HbyoUTCTdF*m-llDtbH9;9Z4u>u6&wneD_VTVncr%5+v&5^IXVG z{R-x7{Yma{U;-07PMUnG)x>3n62b}T_l0LlwlI(M?vO#niF8tn9~r4V0P@E#B3iS1 z>9bo^G_p0G-pl$V>>Vmcwwey2{SQuMZbhjuxnGA8v$W?_ttebDSAGO}`#Mw@{aTUQ z1Wkn7U*&0wvjf?3qL&-4JCe#uX=B%I6ME+GesEb=ND5r^v3Gm}ouQ|QvG+>I>y3f5 zV&pPRNDZM{J)y8^S3elKBOSIs+aa(#Z;KVnHxjS!Gr4_9H@O8r;<yFvsjxC<BIo|o zoSRgUE}W5KP5kujiP1zYrbJnTG|kSbYWp@AxRLpS#-wT{Y5ROKK70uYEczzuPlJii z$Oe*-HXC+y2Qf+Aab*5;38p#?$Q-hU+<h~OxSINK`+Nry6CVp&Qm4q=@3|=q9loA> z>F)%=EB0}ZLe2<n=kFxKwp-PU&qt7Z$K^?{Q3fM@^Qf?{<tv?R)y^y*Vni3jtfJx_ zw~1l&A?nwgLql^0aFNO9Nz*wC+Ent5Q%6U-;6^>sZh1-WPaIB5lxvtBCBxDE(=@nn zv!<G@>_^P<bD1t)lCd8#fkbUpVSZdk=Ir|>Zbz#q6>uS%UaWjd%z{;E^n*cQSTzkT zv?h=dyI64A{DYnk)F541H_7#Bml-veY53r_Ar$l}BQLjBk*N10vAu8$#%)T2K2AGm zr<WaQ#vjIB{RpO~q=>d&oW!U+G9mfuVt9X-Ea_RHARuL_jOi}XBQF1K8SoDWb!o<4 zSMrTcNAZXxTBG@va~Q_JaM6G@LoALQ&B&uoS`JWaBSF6%Pk^Sax44aytvUa@8O*^O zO4Mt{H158uF&EzciqY%Q<@Q~iz|g7ZI2-c`933Ic{d|zl{1;dD&nMCT&6Q19AVrdD zE6JoDV^TU(8?F_<rM+fSm>KYdN;DiLek$?Qbh<uMJG(FH9xZ3W7yRHptlTUtw%^7M znwspmEh>gRTl18cv`8cMVk{2vHe;=k<u9*$1m`Z4fzeGfw$(`kWT)g)PsJ3PT9C^< z-m?m|f6B0*90ucc%?`n?m43{og{QIdPCtCO?JfRzQYP4}dW$ppz8r53UIc+#Wg+#t zEx7uH!u)mb=?^?jic{Y*Ht+rLv_ca6e72rl7*omR51hu1@789Yy4vuK+x0Mm`m!Ty zMqqrE4;$HI>NxY<KK`(-5PUXy6PJn~7_Hug*N$9*!l{qpK++~U<mL|;^7#^E?aE{( zH<ZGWxoW)S4i-9eIz{Dc#t_pJ3$teZpc_8gaJS52sgqqI_rfus+>Jj&4R3E3>dsCg z=j(^DVjmZ>PYvYx`)1-bCqr-YImi0c3^!cH9(%u?J)1I#o>D6njC}i^Smi##SO>T2 z4Tq%JKKD(bGr@xY7<88CEu@^(h+1aJ^rg7YyM><KzL=M?Fb98|n^ayP8nV_%!7>*; zXu6$BTLzvM`Yh3+;Wl+NvXGKhcdRio;v@a`G>B?!pNJEi`m*?;&B0PGAIzt1s2Q#g zjuYFDv8Dw@aMkPu4f|ri=MNvtKAs`ZM!ReB{dpsPZQDS0`kH%!-1SM=U}phO40>q! z$x5s+c}OFUpTg{#IsE?HMNoaD5EO>Hu`vT*66XvgkDNT&FyAoYT%Av(_R==07}`mk zw!FcL!zHBMelu0KAI`6RWx&78DxkL(PaurkdUWp8t(i3~lP#0Fgn22Fd_wbT)_1Ek z`(PdNjq_G;S>g-e*^hp#nQFCg(d}#+&@!Dj+0~6-;`{LTHnDtE@g(kXzuWZQTnXwo z;sM~b4yx~H3NPC}5=Nqgj(_rq)(@`WygY@3HD;jK!;G1dbBoM45=%#?_!Fh(A#B0e zB0kC0R`Be}d%Pjs!}_w8*xH@Ojvcy`UpCT%^glif9}l;|x`!H$HD@ogk2j8maa|wD z$=!CaCu=6vERur!w=MK-dMp*sdPcq|G_hi)har61a&Gt~BmRhafBJ)+#+;sP&AS)P zf?1bNQ^v;u#Am*Qj>HLk(uhq|`y`NU`s?_w{SuLmw&L}s%7R*FDIe&(5u=m**=NS0 z?Bct@{MH?^to8UHaL-Fd@vjHij4=QqqEf`WE!%K(RxlfvtBMN`-KA>MhrxEN1$|J~ zUzAN2r%QNM41fCy*8U8|u<gNQ-F|fvS~i{T`drPJl*TcugPO=v$9vpxYavlS|42A+ zLjnD%H~`wM<+%J!LvZNYjr?bwel-?g?kKULga5c~0z5dd7gpGXLiY+;{>zai)c39p zsNA2)U5M4=4yCHW^iPSfI&%(WuhSNW^@y=|#=j$z*I0qVcwj?K#rff02f6l<x4?s! z<~}Jp;KIDOV4`G!u*I5KC8tzJrN@vr8WYK;yK?YRSrZa!7jcK?bkg&S3US-{G-!D- ztVX;~F8?@T1Y7!3j@{N$hGiaEWcwZ#w7EI_qme>>%CupyS6&`H?_S`X;W5q=FT$7p zA!PrpTbMcYBi>Tbf#T`MV6|cftGD4KXAg?3LS7>0z4j#iG^`YUjuI#CQrTqG{Ty<5 z-UM=ceFtWow7|t_;`m}uGsYT9vW9X8_=|dlM7_=pnwOZeS0DQGUu~n=caOsP%eqN0 z!fiMv8|<a&i)z@UjRmZ8^(JCHZYz4rXN$@^JD9$&J)ucF8dF^Y$;oAfu<vRO%k{Ye zzC+z<lhk*JR|+7<-xN{PC6O@sMHLhH?LOyKznnWX(3+d>rHCJwmyoZ=N5P7B-^oL< z#V8-OpIqyj3gh@8*j!Z&2CFXO##OhFTTu-eXE`=S#fSt(2C>N&Vytp)0$hFb67TB` z<!ZLpf$Ebi_Jit1oN}lyo9SS|YR`KDHx5biM&b9c^Tb{B+oH?I$4SD@(cg&JSQkD$ zRT=CLyd&MuYnhcdY?)e&gGmMA`1#XQ8k4sk602qS{;Q|)6;m^)_WE~@9aTJ^eJ77! z7haC8(h2;y0ke2F%@bJqK8s(@>p4!iHG|ilzW}Gt`~U@SEZGNF#Q7I-Ua;=&b*hc3 zut{nsKF=8h330p89we#K!RyeXJVEeyQ9RZ2{{Z_{4pHBgTF}e<q_y2&X`Z|>EV^)q z6r2_ls%+2!1J00@a!_L3+BNw7<{$Zj@ZpXPtHf&Vm&H;G=O553y_YpoSj4;8v_i1d z4gUN3&7$g_5`6on5?ySz!|DzpiB(0i>xvAYnD!mIm+7*buNY$Has@W~yE1Q|-xrFH zC}E1v1KQ`xCWvufC)k@RfoM}hvi3UTgO8^%tzrbu_AVozkG`eD)XV6U?9Ui+P#gD! z_3%x@qSzSsT>fRV8~^mc1pa#bSibwb25U0S5Dw1BXVphlLYB81`&IcWdEuvtE~_?U zp1v44tSXJgCXHCP<vtGGE5&y&vtgS)oB$#j0$0z+(Kj_im^Wc5#KY(%)!d^(HG_wv z&!ZkDIaLK*>Z(ZbwWrk7VHtJ05=D;MJcl_U!`PzCt^5roX<jbo4ZqBT(l=@HICfPS z`&#N0ovxKqZ6URh53tooS-~fIfFDRVt+j=zdvq}W*#Oa_2N$v8QeW^Yse_PX*>vx% z2D0b71f7}ffopdpaQ4*<I_r<aw%8J4Q!<&%Hs~P3o<<NaBT6zB4!|+37a01yAB|4D z3JN<a*qNgaqjjWljemR&YyVJ<UGY$keKmF~8~nfuE^GGV53Unp*h$?QDYrDf=4L#n zQ!oVPCN06h3sXths~Yl@3}M?R9R*==E%lgL#9w$ih%RrEgnhXaIjv=qaDJs6_Fte4 z;sSM|7;}X-1*yZ9uPaIC*g{$~Rvoq-zC`iKe0n4DE-qWPgx4Hsh!U^uYVJBLV7JR8 z@y0HBWZ<0fyh>Ceel7BWk*<-fi`+%_ph-SSa#%=$h5ppjv;v$qhSITSo0um}F0A$F zP?ETAgHT6Hn%yZF2$n*3D6}?XPwCsk&tpb#wo{pX^`Vk{?<0k6Q*xOH`bLcXFcto6 z&~0+{Vl171@DeWSJxC!Wf_)Y-0qXB@ydu%zT?UBpV%n)VXY>-*uVf3Hs@#Fc=jh<> zMP2;JqdEM|nTfdh{3$#zNgw=%tcLInxs2xG;q=*&EZFp@o9s^$xlB#B$>usKW=)DJ z`%r%~DOz1hBBzgLtMg;=o3b(Fo6HrJv$~_|k9oKU#No8|2NDrjjBOUy>}aQ{e0SIx z@}ciI$BapH;NCD9cBk7B=m>p{bLY>Zog;(j3$Boj4%BoEJQ%{doD*aB*(Jjjp9b*R z>kB^vis9#$6gKZ@1lR=?k-^)d+40^jFs-m1gsD5wtN0LgGE#saD|Jw2(M7UIS{HW3 z2GXRG3rySodnEHt5YyXF5*IkLaNnYW3T8;~g<Gmfnv@Hlm*)cBZ?gFl^eWn@=Ft4F zPjHCge(-+Wz>hwEgPU@7BzvRm2kz7TOvZ&%Xm2*dEI)aOF~5SrB`M@*NIJc-D-I_u znu395Q8>{!o%q#Gfh0D86fcygCNB%f<J+0!?SNeRP<We`K7UOYjg6r*>)QmGVg>Zu zo)j2yzdzMn`WD9Uku}2xjjB0)?E?R31Hsf;SGdA6r*JGr@O7o45_)}k{>u~5_vEq) z+<9y$4&5>WH9BUJ)ss}Hd|(DXY<*pQJzEuTomhghmKKak|NUfDRt8*MP)6^!MiR3N zV&ErxiijV5Kyt?ClUe5<(~$!+VZo|Ea;I1WGp4*H<40Dot=*X*Rd@+L7O3;r?leFZ zAL`iuPA8jO83}!s+Omy%1nldBmFx?&V?Eo=*>h7H(7Nsxqp+wQqOJ;%f5X$_H_wQv zTmw84Rn`S}_CiZp2Gv--0If}TfR{oU*L~lbT1-RobNF+*iXDoPqjr-O8hKP>oe7g+ zxRz`=bCDjJvyfhywu2S#E6(>f&%gtT8~L|Sv|0C?$Fb~7JIln)<o$U+UVLLE{qW8e zPCWSusmro~#Okr9R?8BG3&R?d(fBs-qF_&6Bg}4miB0kqSg@cRE*^G*%f;zXw$p+= zd#8W~O+P?m*Qm40gT~;~=lyX_+87%4I1KluCg2?A0kQv_M)q!3<==Y}ewyNL7%}KB z=pI|ZHU%DqfLn&py*`1M#h!uV*`rwa8qbxe$k)WZ(Bi|q(s0D?ll=Fzy?l(KH2>&I zEbx;Xsn+uET;vt$YS?dsw@-z0n{*2K*g`k{?od<qwo)KD?VL)sDzI!`*gIi+e^L5V zR5h5Lm`2>^m0-Zn0P_5<BC$Al4R<8mfvQy{Q0!g|g^q4Dqo!YHuYL3fFZUbZBv8N| zG?q;>TtZR=%IvsEZGI+OhKUY@on#UM--1E}O#^yx-%(j!*e`<HBrf2O3FGO-DQ7V@ zK!I(`AIOHfhSBetYP3b7m0nHErc>5i(aU*51o@id==WtUWWD+%F4a^V8;g>;G0JzS z*%Vzq^2<vy)hUM>evaWIA75q_4V+k+j;Gwiu>;UMeHI_n)Qui<_4#MHYW!l~7~H7z zk~};8oGe_o1+P!rMX!f0hg*H~FzGYF6uSgeeEb3%%cjDmFLKaw;Rcm>c!sRiE#(YP zU8P@CkI`{UJjk)Q3i5cEmoP5)138r%QSJXx7bHwh5V?0AV4BZD$?$MCzUgMofW%Zd zGPs|k*C$<A9_Pls)+%Go-$bLv5hECxvX{3LTC$OPMr6}vW&9?r#>1(`sP-|3I^@XX zfF(PrpKBe=j*wx^9c!?q^%$o9QGz|eik@vc!|Xf!1%~cFi30xt#6b9w6fI0g#@3a+ z+%}c-aydq}9Vcv^QaahMKAXSi?TV4t?(*M)5}2JILXd0$dP2jRf8*Sa_fx0Pxu+_* zRujs;Zqfw)lohlc&}3r`Em(=qS7?v34p^J-W}D-^vCoeyT+v|)l4L!|CRn4`>l_&4 zl|tt+(PX2GF*Md5ActHt$cgtGIHnfx!)qh9{PZL0mKaQ!p9XBq-DG}w!EOjuk$_mo zY`#9{Jw_BOIlkK-$gd9wro;M8$D@O+*ilXE`H@!${K$U^BbOh<KIjHs3zA@R_f-Dv zE<1inqXhcS*^A9d-BhKf4_~uo2j=B!^Vc6=q*BGTg7s?w!>1VGafQjuiN3vL$50z= z@VB5cVmxhsqX@Cl7s-gykJPXBE2L;}e5x3MJ12J4B+oVA_aAJ=`nAA*6gRBdb|H#g z-{mEkFsX{T8QAfkDoS9-8BO-{W=T*v=E4qq7S8yjbTX#r8n}$KjRI%(F_i8}gu8K} zIMc9={G}`VuRO>f|8T}>^OWln)T6#HTpxF?`fxKx=Gt)dz`$fKx#cC;4!%J0MCDd7 zg)C9f@n^2<NdOynjb0Zw#8aWO$wo;TIC}Uy?U-Cn*LA(6>dWR)-&-m$fzx1?6|SJ^ zs);xx>oAdj-a!M084$nA&giB$t$JqPh45co*+1p|n=5NSAwuwAW0LUa9#w&q@0jYA zF22ev>AT?mldTSg4#vV)FE#9D*6kBsKe0hjG|AIp%`<&L_K)Q1n*-AX?J9#DdTu0C zfAH(DckjMa%^&oxj@$gYI$w2EbxKyFgK+uH>Klzq9i-MT6lh5+38uequU1vQUcGc_ zxZumG27%SX$3lsr_XM8~^$6N|+3H!(Cpc)zh1eBz<O@p+76`U22&yc2dPz{ULqVu| za<3rr=3JpmMQQcT6Njs%h_ldozJah<E38^=w+z>$qv0U;`AD@BTPL(S_)=&;q)VvP zKUQ#5ZIwV`<7}Z`R;)1h)HlJ5iD!g^G$RE=mN;{(KA>>e-iZ#D{)Zg4jqIvkbN7bB zlKfI(Zl<X~g<D;n_I{gif?KfQjmlNQpkao>sOVXYzPbc+YuJ}+t04=j-?W7YkHrND zZ^z#ddM{tiXhg?Wy?-7jnA5Jr^$Zv+Q0qz-j%hhASh%WII5lh-8)m|?Z-SNhBqb>} z$To%qjWUOjJ(4`1Je`H=c<lP9P8>UyqOnX5wGKQFU7yFX17p760@0JD6Zw9yv$cWy z(9r>&o6h39;nM8SK{C9z=;<RXSw)1i!-#9<XjrLzf$L{59Pe1!v*z>karks3P1`mJ z71G9n){95<kxV7($Y${}rptJr^klHq$%a`mwrpu;Ayg?Q(S8T}ve}s-B&*E~wO{Rq z;+m~&#XC1%SM>B%TjxvI8nm0$HmpLg4QA|#{yT73m@b@bd<zb<`mvEq68SzCzc2^m z%~`L~?HI36P8%<Yqv;GQs$%mLRosNEi(nr)^*)ha*cHxC-W$#PcN#ME&>OUx5x|-& zw9()yC;nOc41VRD`RwJx1#EGmCa&vm#6Q0&;C=7fv1#KDvd@Q~!`Aupc&$5*>=5I0 zzBqb3f5Q3zzE4zR=f#~z>xYBbE8mXr+6u?vgT-!`zq1f$tqDbs3GeBn9?^0&ErsqL z(FB_n3Eh2MnXOpW3dPkT7jxn@x_6o~WRHl%`ur+*^g5XvDA|ntT*mO88;*m#^9=N> z3WA&Une^5UE!Iq!i5HB{L1q3cH1~JmHR|JuZQD>BbXAw#sgaKR!HqvDDioeKc^~nR zDFfXROIf=Oxzu~`EzF;P6azm0Uv-^%G#6pm_hpwgODM9ZL=jnjGxt4~N@&w6p(I*t zQ7S6gRn``fC8F$GW&O?E_lRggLb636t=grO>is?Md*1Ur?|Yu--#K&U%sDf2&vjkj z@8^3fQ^7sU*@v4&sQo^5&^U?3MM^hN&7M3`%v2Ee^mIVgQCW8PegI|ReViCw!$j=j zr#Rbpu`e&nP>#7VR8(*#E7H0Qj^Zr%c`StGw~C=u)-<pkYL}^~v%=J}E+MuYi%}wb zZ?N{Fm8?Z_7~q&}#ZSg$*%{w)P8pcPTFx(E4F+eiT5^0;<b&;0L;NCEMW_quCtZO{ z9(`h(Cw$<V+<yF<XW+aZ`yXzEPTUeKMF$Az5X~_W+}3@|X{qQ44*B&4VI0uO6|%WS zyf2ldcYUa>&v~_!4lj?cw{!i+RloG6esMq~N7y5S`!(|dkt*<lv-)Bc@x4jPyt?O; zMXz>(g^}@<`m5oWEDQ&>Te$dbqUi&Xw1}Dt2P6}mGLfV7v6zLlPktkDFEgE95I3Ku zk2!Ky>N#_jZ${99+kX+SgFLydDlvqfP&(aH=Rio1S+usU5|LH@kF#vBiZj5kLyU%& za@%egkP`py687<S#G#w1<{GzUIJ*|9bF9DETfCh4LMMC}<|atjaQKXV5N{K!=%*V8 zXv3wl<Q)%p#&=s6{ULKB@mXXCciq_|^hB;LJsslAjD3(Jiw}fwJ%71y9N{v$KYKHI z;To@wot7g<W%TPiY{Z!5!M8Y~3YUrMyCuZF)>HNQ72!nD`3X*++#qrD$0?>%k6VAb z=`pe4Nf^27ImH}wEkHlr=HUAmqJdQAD%gMG9R1DRlnuA5CI6C3(er=%nP5tfGOuz1 zD%MV*0qzCypIV4S-TBZv&K0Kj*D;che&9nhjd;-45pTFq2lj+G65qU3Ei57&fkyZw zYVX~`h;VlU3vGF@c6tv}nZZRRa)LN9A%!`7f+jhchInq(c}%M%F?DJOU`CA$9BjD` zbhI7t`w}A{+#v&-u9btWLyhF(ks~PUx-%V}a{!K29>NYzUyzbLkJh}R0rpMOz`0Nb zF3#_S0=p|<N?SW9clM!Y_H*$io)J#1-3=M}2*LcV5onE~BR1b#i^4WXLxE4}_{_8n zYqwFF<=YH!#1&O)dPgI3PbwO>UEGL&E<TUPlwFX@lT`4?@Dkcr9sqwyOyExw4;h+| z!_++~M(Q`tA*WT6STQLP*%|O-D*q+&`=>}9^Nz=}oYpXz<`2=`mVQQHPb^cVmJ52F zddbgsISi-x4*A3(f!sUjOX^4LWBBCPfxgt9`qj}3fXV6<@@BmfcBYhw<rkj=C9f&k zO2>eCh_cDU8>T>iN+Wqug~O?px<vNrI1}$0TtL={9+alfku7!uWU0}8a(Y)NtoW9P z9;MzumGaA=>9zoLc0!jsD?=3Bo>_?W*KGj(`?P>Vkq{nB|3gk6_aU!ym!VsO1tb|f z7Yur}GW)wX1HnUjV2bDPDJ%A5yycgYqm88~_Oc$6M9*S^_D10HYktgRaTqyyKAtI( z)kUR${^BMTJNPU<0FHWeV882+!0o@Yi0-3VASV10bJw63EmOXTg7nRJ|9})k_J*Q9 z(IUVpNkCPW1R9N7h2;8rP|bS>q+9V3jWq>8E8FeJ&&LFsn{_Zc#1ioN?tIw#QX1`F zHbER&+e<E}{tLcjyP>6dSLwXWN5D5b4-8BdkRL{0(2Do>lYBcqg9PLAB$Sn6EQU{z z+h)5HZoB<R$pK9uVE2+#-`&R8L_0zIod!hz*&as4i62%V9taR3#&~a5XFPla$eg*W znbrX=@g}kre9q@%q`hP5nAx9+q`4o-NYe-8`W8pDN<j)q*b4xqNEhV3k{^t^$-r{G z<sf2(0s6Zqohv?Ph~6kS1DyCrK}gXd?CQafOgHLa@iY>hd{V_+yd6T=-}ysJaMQt+ zKbJ^1b#tU}@hbUZ#Wl2sH^WOdT0#E&>yJdNw~_o67Pwg08?E&i2PeMAq2v`gC^dZm zlwDYhZitQGpm2T+hi`&t$4N$JERdPy=80!!TjP;~C*jLm-Qdhxe#o5_4TNPFkoCD3 z-2e{Ixn2!7O9-Gn^CdvaQy236?{8@Qcr4T{3nVkv{{`l*alG21o3trUVq^wvp|2Qk zl=|T%5o}_~zVM3$&XzBb#9A#<I9!7&lpF*0Tb?kE!w?+qzY030y?9i67_75ZMZRa1 z*xsqz_;`USQ)Z(8&D+MnJSq;gls+amaOYry50>z%-857C^e)gD@?<WoI)(Zr3Yn0m zQPk-*jQ;KA8F7zE0h`V*97F!QSSC>lsrnY;`A?i-w|z6?)82@l#F*l&yS2#OJ_c<& zcMd5!^B~(#m*6UeZaml+36oFWM>Cf+L1+tN#tv3M@s00bOpr93`K$zcJ#)as_Cu&b z*`NGvHxFpor;t15@*uO&WypxG!c|ePac_qP<q>d@G`M#Yo>Tfx_W!LyaKH%8lKu<S zFO{RN>LXY<Di8eLQ;H=x>a6hAFvy(A!U7dZz$5)O`WY92II$eicl#O++?I|PTdv3D zr|a>BrP*-%WkdMis<QuCXaDauU^hA?#pU;m<W>tM(%MTF<FOzyJTZA3xNnxi+g0SC zmV6`b)V$0&6}F5Be%DF&q`yHMV`kx48*k><>>TdrfI8B0OAxZPwm|L88syf{O(4nN z6rbq3fSkjt{wG!T|Hfbb7gaX-p%3sMcf&UaJwdbA2>R`s1_rtek)urmnAJQNF4UMs zn<$CUA)DOLaDo~x{;ES+o+_s<%~OKqR)^VzFMX)pd)laYT7;TDZcX*^e*q6QTA;rC zHp)y*tKt5-2c$NL#F0YM4BC{-4hQ{#d}$Y8wE8PH^yV|@pREp0?TO@iskD=1+Bgc7 zNCAaE9oQ*#XLjv}Pspk-0oe)<(L$G`p`P(Argr=(ILH@9j<MZf!_CX!{q!nIJ>n9( z^F=GPl5w=m(deV@J|BVMUY69^UomXq2_+yl#lXbaO_WuJqNU4E9r6N?;q(eU4U6-i zVtAByy${)F);}Lu$zK885*2Z<cO{(OQ3yx19Kjm-Tj0o^tyuJkIes2656TTDk`H!r zpkuc$Gc~UlB*z#d+lEAN$0{6dPMJX+asl}A#5rnc?ke`vkw{jxHW6L;oyxAR%*Js| zIh2C0A^ST|j9OXa27CLS@+LELAThfQZZW}Vp4nv<d;ru{=LV!ctitY-KLekINx{>$ z=U_<5bzFE$oYFb46#A}PPhJZ8MD9}h1`G)vGczIxBV#I1<T5@u(~yo9tv`&x%>+E@ zbQ_IFRDplzgs7`Zxs(l0@jPO8nF^iAX5)2w;T`rqTrn!r&~RZ08+mPEAIN)AU$Z~r z`)navnRf*ioD5@s-ipU_4~Ro0zd}}6K^C_5KEum@aj^f;FZAM!BGohVh<>HH6rD(K zLc4vQF*pC-MUzcjhJ0NBc5gop4796|Sz#Bt|C9!|_E-Y_&FO4wwE!!*g+o0G5`o!q zv54=|EimAt++g47&mKq+q?WzDh82|!sCl+E__)nI_|fYOa`ACyl%Mp2L$^Od<z#o* z`QrmTaKH=0O+z5V(}nHR{Dzk-mqKE_{ow4cX~5|TfnsUB=(;!}$4-7kibV^NTaW<q zIigIq6nHc8`6_Tymjw?!jli$&7^qjVl={0XtRY@f9^1ReP+#=dz|-FnV0n-x6=ZY= z=Rcl9xd12jl7k3(=$Z(1W*)-R|E$>Pu2Otbe+r*X2xB*8EXU%a_mM`&OZ>OxD>D(F z43BtR1W&aN;o~!b_+vmW+1t>@ELy%8W&F7dE^mDTme`8n^+W1-Z>a=GEj|ZIPE})% zgBq;X%`I$Eybm=z>ck%V*Tu>Pltc4xkCAwR7MnpFWW&>bqNOuhFje6qbO_v!e|MGR zwFft1{_kG_$V!2-K|0J9DxNWPTaL%}3*&kEXEEP3Pv-glTfln6fpw;}Sg)@2pjqY` zY6w@xqnE^CPa%)7P7DWbi)5j^PbRc`_zQGhK1%VOmWKI%Lh&-k2ngf#EtCEG@P~bn zG7fBk5}qZv-E;tZZ+}3o>TqK#4|L$)_L5i^-oynhKKQQ0OXzigpBjr7f}-U$u%kKy zzRHnc<NSKb!|MdmOPN>b2XFct9%TYQ8#Y?34^xHf)t@tWg}%~4LAGT7hh@mT4uFgs z3aB$u5H^|+;AvwnCX$sYX|S-ND|j=;_cGYMFbmv2ri-_zoyD&o#6V-)b`+AULiKoF zgI^Cd1CDAJIHhwP>P%YT>Nq8I;Z_oCe!UrZ6v^YY@kLOsYl2J_97D~ybK$njtEkm~ z*HM(nFt7l%sE>9dX&!HP{)ZMA5A;E0BK&LyW5T}gE@y_!C84(IMO-5`15D$0P_<Gu z>@Q9v6_mLcKTDertzv3n*0(0uW%Ls+oBbMksojS6=|$`|^EiAyd<!LMI|e%h1*kMX z2LAFhqWmP}q4byxJkY7hy2oT;XN541LqR{grL>Ke-mlH5y7mL>$|mseY8RL_A;AkV zdVuzc?Lhl$2_P)>5;e8xvvu52tgDlUhpA8;Cq9=AT3Cec=@#O3H)?42s&nwBbw4wA zZYw)^Lxy^G`Y2U&)r-xs*usi$)4-w;6<BA?3!b=l5Z+j33EbCHcus;a<yfi9F7J4Z zA7->LDjh~>zuQvmob(?2$-T=2bBEC2twFFB>N1PpW^qpURU`7pFSMw$ky-s?KJ}@d z!m0ZV@t6Kd%H3*HgQUZDDut|Nj`>W$;Daq}wD1hvBN0m76Ah!*WJywe$8+JtPXRo1 z<`w+CN*Bs{uL4>*dw`I|5bW?lSO8sz*4;E(`m`7dcWXjE-`T8W@*LLw%NMlj_6!)a zEJ59!4a_q?S(r68K%U0)sK<*g!s0qbJc57VTg+wFtMEJ0-Ts_1&@yBt9c0;%(=k{h z)rK|uD#to2gyQ9&FR&_>ik2P=!eDQ03At5!A*R>VQ=Ur%*^|%K;Hj`?%C#zm_a4p$ zwsbq(`FcLy5i5<1N3+4m{Rr@3OoHrQJ_%H!K7rO9+2Hu^N9b#ED{=^^13x^Zft5o7 z;S{@@bZb+`@518|b0>z{=3H#4>Us}6dPOs44tyYZpdCjzQj}QpYV6)lP#4n#c}(^> z_`BE<H~iUvb(;k#$Kx`P_sx**zEw~+PJng27KKEv&BAle2*9NNC|q3K0~QH3qeE|b zN`mr2{ARBSxHBpP+mzp<Q%MJa>Ap!Y$D<v^bmpQJ`3lhe_8c7E%7sm;-uRC7J1Q%F zxn(w6L*+Ej!Mf%GP_0do&3AIaEuwFr^`0uopA-#U4rjo0ml9N_aTeN%I^t&=M!-iI zl3Ks_ITYRzjA#57Fn$>ZI6gWS_o^*{H{7DB<A&bUm2;NJ!tXDdOq@XquM-jD#bquZ zuLEwTwQ&8$LKr&v8MT!R<Hy>?@ObA^C@2zZsXqP&J7h^W6tTtFc&Gz@G&h870uOxs zI)U6}OW@>PS-c(1VXKPeU_7xDFa1~wWma?{=Z#Z%uXG&jA0LE2cW~H=*9X`KuXWic zM?Zt92eVMzKp~RT`h}`}?<4NXGe}xQ2<i{4LmjDiP>s(duiX-cXnZ;5#h9@>w|vAd z#u+>%yo!<<iDIk9EGgB0+t@rqId+EQ3p&5kuu^l3HIy8tM0@W*g&W84w3!$F8k0{| zMeDGcbS*CS`~vJ()?ss@4zjP^o5~UMB4U3&L#K0e@J>&}{4#7LjYAbs(v&dXC@%yK zFZ{`=HIT)M!LL#7jdSF;WB0+vgZadWqblX1aTRrx#6$Vbui3?=%~a2(7p$jYBsBc@ z2$UD!g6T%v*yS%CQ}c49*?$vD(ZI>exTbF-ZsmpI3{&RR(9<VyTU-=oT`8*fP#^5J z%)-{OW{lPw0=ZXMfdPtSQg@t3haG(wvT-}QvtkiAG~Ne-*5x6mdubr0R|sG6yN*@~ zHG!TYb++PjDlSR<g%+<^&2j_>s3(VNsH~%w*mX|@oZnoI!?(vk)9FmACbEFF`Ev;u z278g1n}t;7@FO?J7FfP>AAEWCCR9812wswS4_><r;p^)2;I1Q8VD6G}Fu!053(V1G z$MZ(eN&_8ac9jcTr*mK$y$dYg6HlyDIL%I;+`%3>tOt~ND80<JAF#$=9kv{iCApth zK*M?=%KySuX8yh+nBi3kW#VnnvNtO1jzx*AjswEnAyK?*?Nf%&LJA!i)<iGQeut}W z|Dz}O>Tr+tj-#dR`fQ$6Gp^xR#gRs}Xk5Svw&5Ij`TP>_#OpLt_w5Eo-5#WDc>;J- z?uxd&QNYYxe>|<ah_EsdgxQ0(REG0Bs-x}?r8T475S|iGarUgCy1pe+fkVPjGVL`M zjOVE$d<)=hauN03&;b**=YUMA0IS`>p<<KP!WgxWNF(JU80bucOo<rfYwM4FN8bVY zk4l(sxd``;@Y+LrO<>UVjG0zaMP}ZSNVp&vt^A&c+S{%I=WCzP5tSLHD(W0Na#oxb z;WxyUjx0Otr4P;)R%e^eO|f&2?q>J4UB}L^M(PLpwAn+?Y^khy#n3qH4|7*b1^)Z> znK}ICB)MOEKj5V7#(m}+u>5lgd`9yJiX3U=wMvhH_>o9L&LSW6$N?xlp@jk-rlQJ| z_Kd0EJMQB3bKsk?P$c->7R}T>MEVH}G2yxrhv=PUuf!<>=>d_3d_z|#&@@QhY4O0~ zq$4Xtm9aa|NK!GMim_iiq6}-(Ab9!^ZM8a&pZfN~drQn=c;^r>Rd9g{t}-m;z{e&B z2EkW9#K?t9NRTx_p!*&g*z#8|u4{h=`d0|?oGQB*Y0D^Jb07$L9&bi&f3D(#q3iG~ znJ^^F+0V9AhEv-{wP9FYK2=<#Poa<+s&vCm)>9;w(*Hh=NAgnOjJyV$q!P_i26C*! zr%aqm&SB4gOu?^P`C#x`3^irEcs05WRE~LrrgTedjIED>j>su=dOQ_5Y`6eBH>`q3 zS9+s+CKJFwN)kSI<|6ZFZh-B$PTH6<;A@-^#1@ayM%g6D#aXOYOe^dco7b?$;x8$@ zIg3&rY=VbZKVj{M^RUE~YW&G@5j&RbiZ9AbvR`%mNY{FA_^{<F-m1772wtdTa`Jio zeRnQV_SPDDmUF0p?Ym*Sj5Ow3^95{qunT8xi@?@PGC|E62CRAG4BhsmLfx;u=;n|M zZ;du*k2UY*g;K2{E9xFV$Cq>2aQ!zxJmWoUM&7|CBWl#~+A&zVNsbM1u7-l&1hLIG zdn#$0h-E~CAY~*h2oI;*!mraSFo|kNNlP<OJ0gaUs<xq@Ge7a~=m)StvkXj@-6u}@ z@Wyji%RrZ~AlTV>7nSzMqMw=CKzXq`zBum#VJ)8uzIWGxu6|MAzwk16cv6dUdoqas z$!GAO4BApHKApO>ouxD?<teSi3jC$S8*IL0P5s!=hK3r3sP!?TjIhZuY`HA~7M-2L zPV1Jz3HM}LBrcWx*RcfpS}8)m<ZLLu;3D|_@+CYdlnvhe34tY{1Mr!<5TqO;P(EFX zEH)Boz^nuXA{A)yoDXjpiogqf6PYXP*RU0Kx$xZ{A-3*{isf6oE7YTX6Kt8EFS~%f zj=_p+tdE*L`%_p8AL{3szRecEDThqx$k{_G7PX@ONdfkO`#T)`{W0d>BET*_qQUCy zbA?gTyqf0n8MsVK0K72Y3j5$XsB9+%-zohBuY=3L*APp*pvwe3n{x%Mu+qWreLI=A z({}-1stAiXh19~6zL-DQui;a!Vna%mH><bO30M7A#)U3I@J`4O%v_PioU2o6_%^Ep zj|vRqH)I7Kie1m{lQ;@CTL!~>SccU;p#i&JE5j<EBwR2xoBFeMFPPt(0i5qa>er`V zFxgHS3A}iNwwl~Ws-Ggk-r3QhEBFSup*4&3{KLhYvL*3^$}N20MJg+AvH;$1>ZNRx zELgLXe3<je1s53aWLxu$p~rF={P_16erqUUxujGJHD5RhJ7Rk14>*?snnAevv<>-c zM-%maK!crUb``&ucEW<2%HUC25&yTU?0?Qd{*NkK{bmWuS?UU(@-JeNezby;!?vI* zERE@IaY5Ss(x_&1937hN3lk@e&~dwJAS`?hOum|r2+cue^3)sDEwTWuDptoSf0|Gw zD1l2?^5~%@>3Fj?$-KJl1Vz%Pz(r66hjb<3y6v-ZVDSee$di@-2UYg}-rj#vW#`(u z!@let*f2W*-dm8wTk7{wmhY9}#nW?G|0hn^RV)`;d$(f;r73u2h?hM)`WwZI=i*-( z!|-567qri>gaMUaP{pJk_Ny7O8!wo#*V9Or(RG32yD6%VdmK*hbfor0eTQ8ZGL-e& z!+1)v4SWgSMpa}Sg$}y4P{uc$$6zQ?LZAR9kAH?o8;h{7*-@By<}qYG#=!@Fa&S&{ z9h^v6%Vz#FWgQjH;2($Q<F{#U)Qd&ptWmTL6+rdkJ8~~@#{6)&rfmSvRd=BV)H2z> zTO!zR+-20w+seSuRg?<MZ==3U=COl{`c&KCE_gb64O_27;@Xc6&?CJDzWwzT6a0EC zZ7j#uua99IDzmAH*FW&{896GhD}bGjvSan<l)(J-6srEs3*@^YkTup^fW;SG$9|q9 z+wHH-hRlRe5i)Oq?N=9Y>RK>xKUzY$9o<Qk<xX*}?LX893%Hs2O&zPho#etDnJvXV zvHuE3Q}6)&$vCYx3B0hlud#;??29B`#w{jP{-_h@k3w#lyc*~9yf<_p>q!8&B2MFe z586n>fo@$dMbE63rkR7nL?;zX6HoKZW9XH{W?2ObEEq-zx<J~s`-??joDKJ*c^FqV zT$GTXS3+#tImS74)|~TOu#ga5nn4Vh3DGX)xm?O6jaxQ2&AAg<MXyzN<**4!^ppNf zVnv59eJpwxp(2q>bT0EFZq=0$890HsZS6qoew^Sq-9g+9OJ%w9o(R#N$t`r9v<2sF ztu-<EK#EYiIYO_qY2g~*t|aR1A8=M?Lt1`TKG(N=IV~6RuRb^(5Sjmu(6w9C$nT-g zxi8$$an^5+CCoXe$hVoj77gtyh#t8woRde)Nw@KOdYRJw`n`$UIbvl_922{Ex*C6` z!#%`k>aYkGg@h87xn&&10>JU|n^#{fDoVfQ_!H+&Z{u*1#ps>pK7_LTVNSU<ANP5Q z7?Dx`)FM#)cAc@(6n9gLWL;!;3ccd|JaSC_9Q`PS;0Q#Fa8lL`aX#HC;>cGtaa?Nb zEd(4!EZ#kHBm}07xaLomS@a8B;&>krC*H1>C7gfD(V>(h{YXcaP_K}t3;nD(bAwJ2 z4@~#a9!rNgZ%^fM@^)!*sjsp1--bl#7VkRxwT2$Y+3+JV@YsxzKWf9l>MJZ({S2p< z&rYM)?8vlOJP}MUd7@5;j@+i7JKv<!qZ|m2Y5~sR_Gn_hnJiH>U_vV%*CGGb)>;JH z#?fc94$vR#hw1Tcdh{>HMI3ES53X6^R!(~XAJby0L`Q1I5J|Rb<Y3SSZs6W@dL4L2 zyxeO{m@JPYGOr(`@63!5`~6!8Rns)OsOuf=-w<!%B7c(*|J+LN>(rww`#x}r-K3Du zg?kpp9a_})gZ<1$&1ocKpapw&4C3FVWw`&(ZG5#jA8+o~rcQMq!wN52(Tx#BO2(oA z=KHn6(!GVqY~@DE)UFBa->{TzS+<<bJP=P+sR`iHa4{-ur#JjyZAtYkUXB}i4uu@W zK74(bCcM}iOl>@{gMOE41oxHv#ZwP>kz!ugD3M!|Y}bX)s9^dpT4`uRmFXqJ6YKk# z){QpQRl8jLw$%xX)$lyb<aHeM*9)1yI1kO%hf|hs#o6rC#nf51C@SNwDzLV4p_I>E zVRaj$uuJ6+9Q#aw3Ru?;^II?*F)x<&-{s5B91FpJo%pHOkvG`gvclAJcRl!Lrx8yv zd<D(V`csO^BsJ~Xf%}yH;;^#}O#YS$%}$ttszuK1$OjenOolZ~-@Ocq?OjfN&rgTV z`RTY;bRngl+yqZvAt+$k30<VbsJt&D?E6cO%vKRkZh*^S!k{yX`TT-I&a_354b?5= zk3TA;@BJv~@pLC$bKZe`l@f)9oNVh0^5t={ni?jH1yFYRc}5}d1}OV62i!IBWFD)} zW<s;0nDo78KsCpNjPuH6hWWW95XnN)9dk&tv2I%4-+}pFP{wU^ia_bC8lxZ-gM>@n zNj)BmI@@_3-r6Kh(jJ1?e!B_7EEpwC9Xe6a;YDCp?GfChCPKdFcR}LcTT$z&9<XrZ zdUVLf1Qp97a!J5GMmS9jnydaqba?@?2$ljZ%Ga2s!9Bo1Kb@umHlwVl1z<o_ikIsg zkLEX>1A%tm$eIoOusQT9e8ydX9$7`gPy4=+SC`eG@RDAbe5nLkZm);OIng+qXUgL^ zoCRQH55Cm=3+?c41*iNxQMIQi8E9C8`8CeLoC|ia;#W4}_A4Pr0}3zD$RqW`9s%(@ zOKPn|8`&=%3)R1#1cn~|Kw%&j9`hD~9ovji=E_&3xpycuO$$X&x6H*=ss_}(*B)qA znmdvm?<BJpC19cI1-MAE6U~Q>w8XF{ejYN9%=o$mhXotMPaQkK)!}GxCZLB5oe)Cq z#$6yagP)4XO+=6Vl*qvkO#trog$7k0nS-D7VRE)FWWV-64R=v^=OG_e7zR<8eLT(y ze1u0<uY>7LBj8ebCz9mG!SDB<;6LvW3V0uZySJ~x+M&zor_DBaROtn&8K*}|R$5}g z1L}+jbqvKt#iGp%cfph&X;?A+71$x(j8FW2kCV8U$&Vc!geMQ|iKu^0=KNKJKVD0- zmHT+P%7X(?+#n841UBMVXE)&&yG}t3*#_`5Zw}n-EDGt41(fEaY^YXUi#GKu!;q8D znH6rA(V5RBu=->i(rgcei`QAfxgjgq;l1`0*GLA%&fNz!XA8ls{aeW!-&R81mC@)d zC!W-aK7lnTMJOMxOqgt3fi~+erd~BXKx@M_v8q=NXtb+j%2QlG)9OmXRP!q{Yc&TO zz6)c_Gi9*s;@4!1fd}Z2Edgg@{ZN^CB-o!*0QQ*(Q<epXm>dyJQpU|5L@tU$eXI4* zu&O8c7qTCj4vSEkzx<JoyC`@N&dVB}vBFkc17Qy@zU$@@QG7})h!=$L2t1m@bJ{$K z2DetH@p2?Yfs2k3+T89APp?cwuFkXZ5x)7bCU`Dc@;VxJ-h4=)E#X*9uoCwyWJ&O- z6xH~yg#Q*tki{AkOe`-ROeK6T*6z5%dvFQRLWz%QZfC)>>@_S^kOXdA*a<S|QXJ(r z2A+ysMB;r~`1o&Kc22-waB*WE@V+xnB)qFZrMty|a>fO$TEt?ThNt-Fs(GN&Z7b{* z(c=L(FX_7bmyuS*DU>&ALXdq1aOc*G%<i2uX(L$3yzkx3ToIV&a{r#76}@hf2N&+g zdIk%4ZSzGYp<4kQ)>{vh1rmUOaRehW8UxPi1`<C1R3NcCj2vHgmsHV@COIQDXvs=- z6r`%l;0+pd`GHoVO@#m}A2oB<3Z;{F*Z9G3kpPq|{)U8Td*H*-hj!P6!2G`g#JXqJ zC@@tJndO?HBkSeCE-xFjrT9AS+xs4*`>W%uaW8PN$AG+ks-O8B{sQ>3nV`*16)ZLm zK<>;A#J}bxV`?mdDwgczoRkX(BB~{**Wm-Q8hDHj7-%Dt2`l^}M*&8Z9z|=6#n{gK zN6^-!8s^lMyYNI~A;`+9Asgj*rq=z>Ky})31YU&VBVYf4Lq=av{QgbA_I3q86|T6n z;}Ocy3j@xZXmB|A1UE;vi<ovT0?mn{;J;O6|Fh2iKdNk8;&EK9{Rb`Go(E+cIyeOq z&$-7OvgqSmFVr`*#&Gxw_2?t!6EJzvUC1B&09J4{NXK($!8%@8hH>{Bj%V$8VC0mC zHcvM)I*)F0Zy4?X->%t_H`~SQ4Na7Qv+8Vd|2>C21CR1{EcN%3m6|OfI!jh`Hoquu hBSTT`J@ULQSWN!Eqe}mAl(#?M|9h^0HNTA5{{qvLJ)-~s diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt index 95e91b1aa90b0c8ba56cc46bdbf11d7b969e27fc..3eb2df09de1b2432d554fb9e9020961f0b7d2a7f 100644 GIT binary patch delta 25991 zcmYhid00;0`#s(~Yu2DaW(}H<dY-fIog|TY$V|vsrexOZNi-`|B29({q&d;^oPD1N zNt6hQ6q1r5^USx;`;XuC`{Vp`u5(>ypS{oCYu#(DyRb^3uu7q*vadvVsR)~$o}sT$ zhaRIxk<=KT_{ElD-yi*i!ta})Jhg*698iQt+uJa*Q3516l^6COk!>ElPgdygoaF>> zm5DDTkZI@i;m{;q7`!VGY?~^{f$A`f`m+yBf{Uf9%VHp5WFjuGPU14nipcE+Yl->g zOd?6)B;;><DoodWOT_MxsHmh29anO|ykRSgS{j84hu?$xZ&R9i*B+0h#baLD7qGDO zg7A0CxJ<v#T=w(-p!TvVPKZ)~vb<AV^3TP%;Ma0E^r@0tdf*PTeOro$BFb=i`xxAv z9|{551)%+SIM{V);k|37WQ3mNB^Tdv4br6-vFySVh&nJ9FQ4<oN(T>cHr<1w`el&p zR*1ST`pisqKNc+CgT?bN;K=3ec<6dP`ak@QaIFN*<}JqJ#x3~r%t-9^Y#~#mR^+W= zHES-3f@^q)6t?G*K34r<okJceU6lw^T^qp4Hx!RZ);6<vwL<0>XA1v@)sw+T^Vw~& zD(sPeM^+c!BPUK>fkD=1*@AOw=yrD>=}O4wmU?_A(=TquEyn3sA3hhl7No;}50r$S zZMuTjfnQv9xjEUp$PSgG^U&z-Sa@n*#up}g%8=gY6Rh8{*8^NR=3E2@N8b_28LvsC z@6R8w<E0i^bTJiIEt`%R4n9!p`VG2E(s1uaBkrl#muNc%vz9~^9QdCVT9mAVF_S%* zpW$9^10;|yzyFa9&u-#~qt9{n@+)KpXAkiv1BAuD`U$?zEulxp3f9)1#kY2cFk;YD zUixM%b6Z|cuGBV4Fz^{qF8_JR(j?KaZ2Maf|JTC3$oT;2Ip5&c<UYc!ce<$Q`h<A= zJPTK<ji9fs5n%N}_&Mzzi`+x;p!!kRdLjz6%iZwMvhQrf$g{-#LK#l~d>59Q+Jl+q zG`#smTi90`04>sk@WfVG*sbsp4SpYicbSqbFe~1RX1heNzO#}2=#0dXl3m1@X_GiB zbuznZ7L1u%$sXAr2E`HkKraw6oR7rlVF@fn#gQ58REM8A1Ug^5CgPCO{JjVRyxn>m z25uKfLggaHNrB%S+{A1=i(%ExokaR^As$Uw3y<1t>65-+$RLS-CEGgXJ<L8<1oKy^ zW0cojHry;36r*j~II%j@9M(n@o@~TwQ39*Eb`ZarmSU%(8T^6YxM4vJ&ir@|7L9m8 zl&ZFIP3r^1|0$%Pqk0bO@j1mVXD?(txR8?vY;lY3RI+jOSQh6z5yI~@kU6E-ILX3p zkZ8;~3th3haZz?H8<;(js-0G)?W$|Ae1b9iwkjX`Om73Ho_VNxe*j@dQEZRRTgcj} zhv}KaV4aC4+7*05n=NO_lG81?#Vihg<Q|1#lAEMs$7yo4{u1`Ns{moCK5W{|BsB9# zg#9lHV8+#BvN0cvkeyo~fuW~LNRjsrcBv>1_YFRZp+ocWb#XNQ^Ei#gQ~L@D8MzoH z?kgO3t-;7zLwr8}3a%V^1v3`iA(6Ki5a-;XSho7KY`5xO)?sG?8QSZi;fVzpT(}22 zW^9EwDb{$^R1q<tlxuwZf%m#NQ0AWH4k3r_A+cJ5Y{UI;Y=F2JuCE-+q>8hMr9bim zU*2Lr_nv|eKj&h*bPNV}M8Jx1CN%&02>M^R9o;QarT<>mf-KsYY`624wH0-Ok7*>E zU>VFz{+xvC8SC)bjVyBY*gcljvk`YLSk4|^FcAhw`Us|f3Sh5R0646l$rVfLf3a8A zd+|~AF;@KZ8cQqrN7T=HfwwdZFU_!Gi|>Er?>p9!-`i7I!~`oSyxRok{uJ(oAAzO% z{b}-XC8}@M3U$7E^r-4ln3Gfs6U%cTVBr%u*fI&9CBEi^l@s7bW;h<NIxG9E{1Iad zRfLcKUXpjtl_<$vdk4?$I0@s0L+txLdshGapzOPBIDVf~#a%8o;@|0%@_TI$kTFmF zQ7tBr(W)l)ePf#VQEM%OACt)YMY@9b<u@2p5KiVj)dQ)4v+z=6D$MPd0OEi#pfTqc zKHQgt>n_ZP(48G5akDW`k4jN)^Hw-sV=uueZz!?PtATmW?d*oG9<><NNq(6$VBz!U zBp~J~iF?%tVs;L|WaVwppioOHe=4F+a3k+>-<0TuWy9Pud3I%54jbKB#|~LPA;Z_6 z!f)J8w6XSv8#*_kY;Q4gtDmB4i!*qK$3Xe&G+emnF`MFM2+au+5oZ6YB@xxXS>}{# z-qIq1X!!)dp)$gzy^X}d-u=Ph)<(AR=qna*-2jLF&Lrb+jRM#5A-KupCY!zKHCX>$ zh8wF@F!lQm#B9oz3=hHI-_p_HMl9H;boC<LBP^7Afn)UbrFCm`;i1S-*!yz0;MoyN z{zw8w!-<$57<{k}X<95%dS=YD&WDo!;tt^(y+g24tbp=QV<2T(6x=M5vBRkf^z5Z> z7#eaCSJ(LAa`i&)s$4#_Pn$wk%~}PQE)Rp;Pe#o8a0J>|mJ_Z2Cc|i}!{kT*FkEDQ zgWM@m!_5UR(Ps2koSN>9lGleW<K1XA;hpO%{NVfrueLwJ%cD#2&$R@+w)Yf%ag8QR z-%V%EBb>?h)+eN7?Id=q|9r@d(SZFAM}pGuCvf~{1-ND3#2f#;CfB!pB~!{zk>pzn zaMkYsTXId0=sTLi{3dNSHT^uCEr=iuMIX7GJ2~tv{*iFqm*&BbF~88yDH82$PQ$+P z9?0$7g|l_4amAHCa53>W+0}6wXZMkzPO}Vri(au)8v9s8ML76QF2h{;K(sUNX6Fud z;_;kfF6y}p<{o>97q>42)E>{&I;`MF+y)X5x&`gjtB6zWEbM5WOtSiq;U+jq^6+7G z3@dtBLzYAgB%>$Fk)-d>V9@bftY%C+I#s7a&ny#KFGq0hi?jIP=|_n3$%1EgP8c>x zV9x*TgJ0ml;%~c>c`1vaVa6G>SkaFdTOMTp4zI<)w5jY6{|D_)`QsPz0lpm3?!6E2 zEQR9gmSFrQ8KOYiZcJh?6Yq<U&5k2G%%jmku|*d3{3ciP+zlRS4rFtVEd_JP#q9T0 zy?=NSylVOgbN^1j8E<28d|fkm<u$_cK^Aynejqd2eI9hZ`m&BM?xfz%1~0ul$`uc4 z;acj3z}d5JS(kz(e*6-HIh%CxYH%`08g2l#OgoBA=@cE)MXXad6Dv*?!~6?I*cH5& z418rQJdthVyZmBt<E?buX?+%+oz8_uZ~^t9kzn${3Lfq>r(Z5yhxj@px=`^AbQCoK z|NR;yU0MY$JvCrG*9m=g=|R>w18ALP$c~kqA-7#?u|>sv7QWw<C$0Xo1t<JUz+v{) zurycTPds5PyL}I<9(atszWkE?OMWgJebfYgU+fDHu0CJ~i<WVlM%HulJYC680}Gh$ zqpEh!d)3b(4LP|Yjl`vWEfdqM2G1W`yPJNNj}hc7H&K`oOZ(nWr4w`ZQkONSg%k72 zgn~nnG-Ys<plcB+Y%n}YO`~gY@2FDRmbQ?7F6t)?cMKHX%B>YfKZq3m?-d@~ihL)_ z$;nMl+#Fz$=rUlEWK?mi_>`)mc<N47@sqh~vJy#=XqqIW;(B6hMcJ%}ve9w6;@!Iv zq>i(XOINS9lNs79mTletNY*koLZq3RAv*CbSo~qVnRKjzSh`-&5-Zg6((o_?QJ0Fj z=xn9AXmwMlxYc)@C}H<zso|#nvT4<p(!1tz;`8D$vWkFf67lk3f_VPBba9IR5K&~T zg=}9=ym-=|x6&&^GDPYl?}(g(RK=$i9*MpU+#<Wey_Y?T1+jXGnaE(27I(<)tIR*| zu4rgjv+UHLG}#QpS+bfX8(klGp<DOwQ{rTUPSIK|bMY~?2X4)!uS8!iR*L<S{z!xQ zT3P$7)3S5klDdlb%hSZEw(8QJGg;F9n_^@iPvuIhV(O%JgD1-VHOvuD+VEL4>*T45 zlD!GCoUd`RnD>7w6yCXT+w>mGO1jRALoEu#2AiV9$@!VGF<LTN*`nWWAO4K4_|F7I zt7>0~K8=4Tvn~EC^_rSSR_xKj73*Dvc@E!T(at^?E~%{sV`WZgeqDlb5rg5-wGmiy zq8d{Ey#V|63^99u7j#xx(JLF?K*O6lmdAZ&QNKl0K{XKjOb>=thZ8utSAUp?iW>E> zE5LJgp2CpjzwmhddzN%a9e!MYgJlN>V$`V~<`d7u!6iXN<!?FIgw(Pv1=m4x$k&lR z`0OE!(Eq?>Lv`rf@9vZ)E<|~?1RD2xAC3Kf6mP#}>_KQ0Ijj{4z=a93HlD|<)haOA z<1%!9sm2<+P@?|IS}>fm2R0{b(<dTD!Ld&k4m?na!+9TCEB^}UDMiRvTaNwG0%4q8 zEdAcRR?sbvl;DdODVX-jm|h;7i|(Ogh5h|j3S~?Ep>E7n;ptcv+Bp9K9xoV9mmEli zmT4MvsE&-SF3iMdR`apMv!C$ES4;3-J&@j*)+Jo9JWfYVDi97f+{D~RjTmM-Sn#}6 zLhdbg5q_mxVfOf+*r?PFv4gju&8_Kz$q~s-xM^fgtB-jJ309v-r{OR<Go=#J?|o$2 zu8kyhhBkgZr$9fwHKP}oufQ2M`wJV~gOJZt!?*=X!i5bZP^#WrTW%VRNA8}(_<g&G z?vYMT^koM-;Cvm%xRnqC#nD(KHK5O*Ho|r9SD3Mw(!(vgV8B^FS|HKZq5WnarWaOC zp);QUX6UjG56DlTwY!Qr8&8DUMT7C#hWlWBQwREG&4#5fK4ZLwoA5L-AGUgIqIPc` z=#iRrU}V(-^-2k%LFI>G;VQ8Ju37ZfR4OzXm<SWk?4S?3mI{0}#VWFbni=jAhG$I? zqH+R2;@Q%Qag3v%Zpx!;=T3V6o)KL<#vW|+ZwV*79fXY4xsa|kgDxH5B$%yxOEf?0 z(Kek`^z;!$;eCw`UH?}{)^H$0=6+fvOBvozylUPcH}f@HMVmStrRyc5WDmPDrJ}bk zqEPSS6&247#Wi`V;yNQaskY$=nR`Q&M0$8ux72>1h1hado5*c*r?hESiP-1lVbS9& zGsJo>_bQa<n9FW|OA%H6j*}%!H<#(y4y)L&GeRbq>hPD|-;`yj%gIUxmx`5Vx48{D zktSaDCsdYneYkk<$ue2O0Xdn9jhF0NT&cJqV}t0izP5PhzFpFf53-~%Gec&nA!!$H zOY@Ok3D6MN$sUUJD@Kaj=sIz{ik9q0Xr}D)T{oH0plaDb(|p;Eg)uUBev`C)%t=}P zASXV)Wv<lh*K_eF&voMT)px|LV9CF~Q7vkAsFr;<8&(nh=eXF(YNYsc=@XGzx0(2* zbCir`P8RR{I)xWo9TE3W2$u#(mRO24504U$T52g?m=?;-|6Epa`dFDv(_2a09@#Ck z%fBj13MrFWyq_x@ajJ@~*DK)ENB+lUes+O!uYORTw~sYty=VHjf59%JGsIyo&&FQv zMapxx$=~H_uzzR=nEPGh6JMlpCf;Z8La9D`=&nk*!7KSI`wCDpwW5~K9k+@(KRUwJ ztlWj?I!}}A4N+wIJX5-UpE9{|@hW7PYSG=>oB1^^3-Q7bC80xWJ6SkK%8j4Ai+%6c z#K)+_v$QYXeB-$yw&h+D+g|8Tj=rveE5G-$BHJshK5aUFy{F0C!~EI!ffM0&Svwn4 zQiTc0+awS@^BK#{9!4gfT8ftz7ZLs0<5^hBO!DIP5mbD)6tl_+IsI=5{II@E68cKm z_DW5r&g;Q|8w|1vwvxU|a<F59J#27I;a5u(@J;$&+`Ux|`0xs%w38E07^;i6eqH9m zpU4Z|^}eM0$9TH9!wCaiv&AXVk_!GyF9ZJb$Pi~wcml=if026;l-8^~F6;A{lKNNH ztSxaZe`=vETb*N0VtR_XkbGq_1kP}#U#{`DSS)vahzXh0c8s6h-H!wp_Tx|f8^PV! z(j=2@i+~{!>Zo%60lW43&c)|=QoJls0e&vmCE~6<thP*^kCv=f<JN52%g<jGCks0e zEBjoa&4#plVcrf)wpk&U+)KI64(yo9-~6OTbXX@hx%46_E}SE?E?&cHb|;q)YKS9B zOXqUawA#3E7a#t3>Kk#Q{ct?^JyX0w(#lQ`PU3RnzVOy<TDZ`sj;J(SK;p(mk@MV8 zZf46~BDt5DK;G;*L9*jR$)+@WaL-x|x%=)gjfbI-d3*$MbUDIa>Sr^D$Q7v7;R<u7 z9w1v+X0c%918jtz9JUnMvBNX<#apBMviKWM;O!Q72)wQejqjbAlcNv$Q>6e2$|k6J zL=I_5f3ns?jbGaMltow{z`S7<61H2;m!CSy7=?R`+kCW{w=~S=E-Gbkxpqsq1}huB zbL}ubwCpKSo`780q2Z*tJwUc#N)q||Mk*WC^-nfz#Z5lYYzTQ=XGq>(zr&y2UC&Lb zeIb4{X*g`Uc7p7@RLi!q_oB%cJcv^468^91BxYk*#VjAX@*j6e%%L*>7+dG>%qv>_ zC9{rN0b5;4jz`J(;VSK%gL;YhxnN9kTf}VNn>CEi9K^*33H+W6b)FT5@);d9r1_)@ zoGP?q15Q|@TK-Vkt^*HPjrnuZ@-Bir5~uS1Ys<<0BjY*O7<H%)MHW_MO)_`y$9pdW zV1%UgG`VIBXuC2R7kP3Rr`sVu?)sIVc1nl*9dv=eC4R?ETyFtMP1?9N?-v*DmM!+o zP2y(QI)bCK824TN;<kEk0xW$OCDsj;a>2%n@mh2wDfu^_Y@@bV`_-R&S5SdjzoX#T z_72uLcar$TyF<KY{(P9)7eP`xCY~Q^AZD$(JiJ|FfHRyfko}G;30-O}o)hCqt~|A; zr8^awhVd`afzhuqI^-uy9%jaNdyM8o=G5ZU@-1u|er9zWf3cN+yV&6NZZhZaAdZt+ z(VG{)5Re8z>B&f#_eBqeo5r(|Qwnj5`2Y+WsE*wOuSvL`FQ3VU?S^pkbGW$6{0`@= z{FmLgsV03jy`i8ngo8~e`+YbN4-Rui4IOp#IG@C<UcLj#brpK#c>y0b$RA$mOvh)= z*`liQJ^Y?0o-qAp9+|TC1%LhJS-8JpCy7r0s9c%L7GHP`W>IY<R?i5JTQ|vUr}>X( z6Q{0)h<RDuN`t=e?t21Rp?yT`v8Y4V$DxuLnyw|b8!mE7y4+#U?q2qD$WsV!2^E}! z5B9=yD(-sFN{-$!LF2ZEWZ@D`DAQcPhyPLdADved{;qY`l#_GU?4|SOiLdQ^*AKSn z5ZyPJF0)QuA|Byhhxv0$NYc)^bfQuxxwT<19=@PRmy|k4^A4eCyH~2reBd4u&sXx% z$?l*lXt1r##!xbIBR0p1*^cdDETz#C0+*fTR!tSig3@sOKZ?IB$KKYjm*V^VkK*4P z;UYQb`<;KY(u*5&$riL2lJEY<Sx5R)_Vkw>8E7FRdUPgwPPvQkG9%EiC!B2V%p#LE zBnkP6q4-#4>TawXESv*XTJ`fZWVp@{UWq)RrmzfWAA5nG>vY_=?Cg)(3-a0HBYj|z zToFC1GC<hxzYBuq9;Sh_V<oV?tv|NJ#GvQTS0pSx9#?4Bv*vhHNUOTci~{tbc*7Vt zS@en@?PJaveT`>31AeiK`A4BxXA#VcK2Oy92GF&m_rtItCHiuAseA16PT~DCJGl30 zJ1w()DVt5ANcgI^?6_nZ-Na?1wnqXsR?mT+@huS0BIz&m*RFzF&dKbORvWfn@Pezx zi=eOS5WL{{5(0hJKv9x5C|{9cq2X$*TA>R{6Gp(B14-mx)-3X4Q5Mtxd5?@U5qiar zuY5@JLUL?urjYyNJ@%2zq<1{N38N<65$Yz-5iU3W7M%VQ32_6IsQcX2ywpBfFqOo6 z2unxH3A>F>Vn?2g8Lk<F@1A=LEf?Hz+=@@!Y{Ti~qM`+iGO&S5157bdITm;PSxkzC zodd1DQBdIT0XJVXk!|bbSw>(e%WUyrQCAY#nfK8|u1nw+P4D1sbW89bi)T;u0)!J2 z`nlgz%At=#Pf-5Xc$7G7zk|<Le#YI0bHH(PDeJud7lQh%frf@8Fj}XDAJ4Xu-&U!Z z;;Si4@+{_ZgfHmfrY!V0rbC94BE8eC3A-X^vwG!+WYOmZ{3L^EjGGokavrOagSIEw znvD^x;rKutAHM{?*oBir{b#e!y{iAwA;Zw=RH_6njV~k4zPqW(953p>>k7D}jih%! zdQt5i2K4*Fxm2}Og<78JM`Ki~$Xzo435%m)Cw%D(rCB%^4nxxW7MM`l1ye^gkRcB3 z%*Y-veArypXFw2!Bv!E3>nD*}kF-g;iW6BBm&eJU8x36Ue)cUQm?T<D<gm5+5Ghey zj7q0+Ao$sI;aq2A<wgIiXdC>LK1(^yqJ2+6<Tpy>GWU_c?|K!P)XjqWt==geoQB!? zR^Vp1Q23lT0HbB*#H&n4h+2CQR@+a2*a?@h*4+_RO+)#yC#JxQv2%!)=|<evYR$G< zWfO_>VwNu%{FIyeTbtc@StIH<q?s&8vI4th0>7YMOBiP`kEC>l(Xbn#?tat%(4Q$8 zbnNl3M0BJ}%wBcF{FAfDpMf`UK-gzknC%Ene-^QKi*3P!SECEwII>l<?NKiUpjg!x z2ToZ<3=W)QVOP(x>ss-cTpLCf+}V#3tJ`|05%YyC-7*8NZJWqae0sS2q3LAutPrwq zPADGd3fO_4<>F=)2N-IyROmV#L}PaNS6Zye6f%#U6Y`5VJjI_9g5pAvvk(cF+xyay zb|Y!*)ARJDMk6uZSx#num<A>>Z}>-c-LmxC{osB`Bon6?2;KhX61pX`1Cq8Bplzi( z^K;lCjMCa89D38A%l_{iTN_c%C2cdnBQ4hG&rXqmAq_Bn>K%Aa`=ep-e&KTe?Whv; z0}r$_Vd&|9LRat^SUkm5xYUvWJM(5^T;NW=a9;n)meMfnymEz9_S9q5W=FyMXdN?f z$^;3Oyk|P+{h`z)AK!#1k;&;xscpanLDR_u_WjU?-=2XqD|ikyt2-^u{20U^dmqiV zbeQ9zEN!y8ZYw|0i_qm?C()75zw)NH!|1@O%|gnU`|M4^Php&e36wf12)^_7&>j{d zjJrAvHFSrNr#s5&;F2)v)qfIQBzc%l_kJ5p>n~QqH~(v>d~SiTp&E$osRCZjumHw| zbiu%0<=sH?gLLhgh@IL-(3a7_H#Stb-TNcrKCIzn|JFJ}k!m3M6(-{zOUkg+OCIOB ztKeekN!$~u0p*=T$@awD%2ggW;KQyw80R=ZXtg;GM}KsJr1fbt(bD{eZ?!KACcevs ztL6LnR%K*;Y8Jt!;2&VR`-$vcYB-DuRut3|q)_TNQc#|tPmEp96Z0l9nRPxNf_L?& zBZ3VuPVph{Aa%ka)rZhV8pAhqy&*po6>$n=;-cw>uzqGHd`Y~-zo=G+&Im!+`&LIn zFG+j_PYZXd?QA7n89xVRA8y1M_gZm!%nZ6KO~TdpX(jU>&cuo|J!;;hCluUD!^4Gn z!ms-u&~3X8PISKpLpr`g?khbSQ+R;4P$<Wki%P<dpAwmgcs+Yr_MOa~q5$rft%y}( z8+rB2oV^YmkALmL`F+;C0=YQ*8J^m32fQjo%q`d-pOItWo-_+~TGkMA>3THEm`>*Z zQ-#L^%;=FmbLkx)Wnp!hI*xe!06wn}bJw-blEC1>c)CUdeZm#6)$$Gao`1tXe~^ve za?kMnqB4QFC4hOA66>tr!Q>`fVBt#+a^}ZIF;mIaJ~)s_Vbqm;xRui*b8}iMI0mmK z$E|zUbeo^>tj0hHu?ej7G~XcP=&dFlSCT;ES*GwsT@aSA7Ap7YDYy^tN6mL%q2Qwe z-BR!a6yiTK&mYm~T#x|A$T0daW2m5N6d<&lzem64&dh$#0?O^&Ob>d#B$AyaHe`w4 zP29LuiVg*~m>Ay&*X9OM&#u*U`$Rh^IBbv4`k2xZc^eG1ctpuoW9YE3!p3W2jOlwB z3WNG!bo@mU?jgmoYfcCc{LF<F-45jYFN(+09dXyp0vTS)#crQMW*MlBL01gu)icp( zWpy17{Arblhs8w^+gW|-+iS|S*!Dad`Bx8MD<faRlE}7LS2&}t41Yv*xF=gzSbe&N z*yLY^#o1kO^_(}7r;(L6j0ezwmT9!rAOhUK9ij7#3UK)`Md5av674a_69RpmgoWl0 zAx$?9gGO$Jy#en?W85#$x*i8c?vjrrW&Z-Q;b|S>z>iqup93=%%Hi3rwbb7!h$fBJ zfGGnXkavkOq<G>A)|LK(nDu&<!OfL8|9C8(Yc3#<y*trG&VVKkZsXR)rB^Qh*AL?( zROs42FEKth2jct!nWR^G?Kfty@Z>*7;o{R~)Zes$P98ptUYE?-E;wcd3H`SJ#=y2~ zIG|QVxRWmuCXD`qP03SeX(kU=p0?cM&-QpzH=S&}{Fd|?EwCQl@nm+~ICddo9gEV^ zB{xqsuvITcv8nHR@#A|iUC?-+?(paE-nvBL$Ljaw+F=_S=o3n3Iv$__r$@j^ha`wt zI)O@Fw#Zd_R?36UmrMBH31jdcRwE4D8wmx4ZDfGKO(C>mIDNMK5qTRHhZ*X3u`?nC zUtkp*Zn2L0FK92ihD^W*ciyqGa4RfMwc-=E^ZbyZgK)S}E_93~usOtqc@5nI=j*4! zrBpS#u)_u1G&CzmKb3bsxur}3BNmUMMO)v1!U;|Eo4SQO@%h0%82`X0hdVh>{seE- z(3dKRf<VpD9)Haq1pHGSe0gOE{Bh4me^S9)nMbq7Uo6B${9sx*+mL1ty~@OA7NW&W zKhR7qVt=_T(!yOQdtaIhekVTR3N;<Pm;V(#g9`=2ILQzgmUM#79o39Ct>vj=cqIxt zYlYoTfv_NI5QGo668f#Na_>H!4d?T$$<!4}SbDpGT2+-}<NB}Us8SV`Y`cr0|8C$f zuXK3rw-mI>bU2Hxi!dXy9#176#|+6OW|nk@808*e^_{19`7s=3X^q8QM)eRT850HO zRy|@Br6gEk#|fL~*Fji_Yh}0F5chr=ABf4SoA73xywH5|5bHdbhFNyc_;t$f;d8`U zmiNFNl>0=Ja}!2$ixdq6)1pr-F<FZi+P-GInHuh%n8&O?DL}fxL_D$f5$|Z0$$ojB zVjDIWF*Wb`>~!B;Ch2)=juzqh%)%v<*9d7B8l2w<YZk?lDkB5oSEw32168PSeuH~^ z-!S_5pqdakN?&LkI-FLt8VFx%EOG0%qe8|~sQg-Vo2~b%f#|EDLYUN)PENWBkv&)G z6Td;Y>0g5IbwMpQuT~c_CpQS!Up|LLPD9WrO+$hQH@jfkn;bU%Ni&&wL6sa;cudlq zFEXb+$JvdXEBwisr^WY&?P5`Dhokz($u#8L5qLd0)Lo~jgnpm6s&e$8Gz>G_NFC%$ z!S>4`p*$=ITnE<)x7xbFxN{(B%)BACR=J4tXDtK+o9$?!bPf}j6tk-%-jiE3k`m^< zU?+awAdeQ?W7*LwJ8}Qf4cL=)oMoj<g@H>eh<Do&67;QrOZiIJ+@!s@&EN>{ZP>## zZ(0L1eg}`Xx<Xjc9N|rft$R!HF#1Zxg3F$6%?-#NDujyl1-V==dj6WD&~b8rFt)TG zUaAbkw||PTZPOw0S#szj?0nsYlWTS9iH3vZ$oMg+7v)YA{!Rlep^9t`N`iGe&vG80 z7NP4FXSS?<Eju)PD*A1)!;?Fj*_jVw;d+*lpjXikm-ycW6E{EM@EZ5tipkvl!6i$& zL+vB9v<|01ud)QK)@$_IXI+e6eSlKa88oJ6A(dDx`$%r5%wo+qf&`6a7hw65-SDtG zgInFLDA?FJ3zB<haNDZAkY3Uu-r1*B)O;ln<(>{<A195*J5jp0`@<`-!eM>(jjybj zccG4C&X@tSZ~$yvzLz`{i)hQT=iJMoZW#G@U*)Er*+TlwK9xNN-E`e>D|#{ZwS?tq zjTM~VFQ(gD3I4H~ASlXLL;L165c^Rb`W##ZX6`opMCJZWJFI~u(+<9Vyp5paV?|wZ zvY~5Zqub+2igXsINEg=)hn2BM+4==C)>$?HK9tNOZR;|Lu6GZvy7CCSk4@yp`46E{ zOHNSZmrG=uAE;E`43Rtl^(Tomd5IJ4d6rHMla=XUi{p@(r$z%mJQX_K&a#v?O<}L- z9Qol?FFcO@0l#)RLH(IZVbq6oP$`Vy4%GqnN%F#x%EMXk#>sFjydA6+UxJM^frxs+ zYsEzsTw?8nHV<uJdD1AzuE=9|K96MYCi@VRUi>GTx5iV^r3G~VSB1*$+hb`m$78=R zS624OlYY3nA6Cwo3V|_Ksh*o%CEK_RFNBmpY+^iIi$5!@jy0!0M@#r0gD(o^{RhFC z4agQeRYmt&6&mI=3(o&dBM&eY;<T-(Q{Q^BtKNX0Y#xE5hQ7y#V|74sAhm&5UwZ`q zeAMasrtxrRixx9%nk@J@9>b}&s_q`?3AD-l1uflr9~M4J5+3TOgVr@~*ypDyENIYo zcN}8|@|{D3A-hisYxML;u+27M_A^!CgqIn$-&}_1(gHm}O733=)u3-`A!^41Chrac z-#|}rm76Fb+nnz*uO*tK<FEnVXzwD`EpOPwx>f8h%_I&k(WGJC9--lmoiOp>cmdu= z3TBQ?$hU&fcave|hAAh6$uDN`@gLGLdD%(9sJuU26x5em6?d`&`?c8T70d8sRkV;2 zC@)OE;DKH*Ho_n4gJ?gx8!VH)u}}Z$ru<JAY5g<fQJb!uT$}Fy<QNU=A~E^h#y+Td zLesPv@WsKM{k=5^YtAdcplJ`u;6egZIxETg-~rrse`V-C*UGO`=m$GLKW84BFJi1{ zIQ;iBf&J6*CK+#^@eTJ^W8|G8Qa0lv8*cc4O<NcU3QkfC9<dkOBZ~RK6|w04#}9u_ z9rnNB$j|*Fe=@O>x=S7nzKb97l=0wJ@7^(8qSIH}QfY><ke`->8)tlG4<@gHgXgV# zO|g$m6sFI7UbqWBHT`M%Mg?K^jZJ*aQB@fF<fP2!@O&~hTbX=$v5&qtI19TI3NVF? zru?mI_;P!oP-f!->-U_4h#LiXIywMbhZ|$?T4jk4I8~Sa`{s{eZ6drHv<O-u1?8tM zqrdC+K<<+Mbo=CR`X)03wl5osN`cS8W9mX$bKovJmzIeU(VOUIG6B^CJgEL1YyPS2 z4K$B_Pd*R}TK-@nmHn`#M`ap<RYhN%SoaJSzV^ka7x`pf))Dq<%Q&HHx5R@!^&S90 z?SW84{qXS*2Vq@KBc9L>ga{26YGvswT-TdI6FlGJX|vHndBbS>+x#<RjCw<LUd0Rl z?R^g#T|O+|Y!S2!%Y*UDcF`AA4uZ997`1l$CRjCjQ18NtkXn&K?k-o5#ku~zWIF1R z$am{T(MRrx)Lk<Cg0yj>Aa;q^C3{@yARa&Ig_~?-iFl9hMp>J#hIm)Ndg(*6n<C>c z3KckAUiPi3TD)aZmNa$29r3}YLvEh4EX7qG!=&n!Q^mH8MdGjTLELWiMEXEgO{{<7 zj&zzhL>xcc#cfE4oiudJJ<;!mFxi^>2W5Sl<hZ7*`n*Jqv)~>E4U-+LxhZnpFP1eQ z?aO7A%#iMpB#TDw)D_2tbc?*)tz~aN`O8KYYjfYmZjpW+sw_UY>8tEo<N)cnh3{nd z>?X^O<?WWvOI{;hv%pGLH#=SGxBsMcV|<WIHs`iz^EYMjv8Jc8l~ettQ`Oe>@}>8( zM7bQX_~Jf^%*SM@c<X&NUN_g2`{0x<^UX|_g?K#{dA+ufo$aG33!gbtw(vne?(*+? zF?)ShcJJB){#(jTS)lo1{%L))beg=4Z1?Rh(advFS?+@xSx3A`)@iCFv-qed_81!@ zS~4(QysqzCX-&}{@u~0EWd*ZVi^ES<OS|HGq@F`1dt_?b&qO1(NTjJ{8%2xGpOl*W zCQ3^lCsrhkiI>{G4iGP;8KRQZbt3J$Jeg%kmULEIWksrsvdq{zQue(&O4>Akr`YI; zyG&RgE%kUHh{8jT%f2_SaD(!}V#O&BrFkv+vb8t6#ZBG%y!2DMXtCl%vDuxq;_F*$ zWL}b1Yw-e4ADP3UAn}i1dqj%svSe0i$zrZGK;~7LDvD225$ktpa+y2Vh~H)Ia~r=q zPi)Y+RGgUopU8d?igbq;NVlgNi?!~&mt9|BA`aQ`L+bZ$v@AGnqWG^{GPkZQ(M>@k zg$wuc5gYc*5nmhXBRje8mss8IuA934Wtk+xXrTD?<_NL3Xu9;<Xk)%Oyg}?YZ>RX{ z5MCTGA&k!p9mijvXv&Ye`MN@Jyd1wWQb#QAI3{)cJ%Ep}cM%WIP!u9tJ5c38BF+pf zhCnSDdA2A{kcuxt!T>Yge#%gIwb?>=qP-6EpR}Rm{8&M{RE3UQ@B}Y8{*d6HtM;U0 z<Q^Pr;t6EuY;x|4CS81e9b7t~N;Jqy`jc+J9dL!enw<^v|Gop0$PIY$;tuNj`vTsK z86jx6pJrc%4-i!QY(er{m)@`Jg6T8rz^kbY@|<5oj{R}ke^FmJC+p$YW!2%XyS_AG zV-(J6%frWaB=hNUn{;@x%#dmt28%)m+X?wGCj^I(Bk*IC4fR?7ocDLr7Vgebpn;<& z3)xT%)GL=ZKYzd<lgSJ5pZn6p*-64>eQW$__zWL))so8@;nb18N6ID?qv_gwlvY|$ z<@pcby~8k=J41_Js$V2r9dJm<&AN;JlEPSFa$2Zxq*G7GIHW8XpLmXE%kuG4rHhbr zaT$HY&lILFE)e{@N6{B6=Cc(pAK1G-@nrMobL<44L`?5~=3|!j!)KlIxwnEF7dCpc zc!AP+4oW#9ce{w}T%}0nv`27GnH%^GJN|IToV;1muQJx<&?UZenI{rR)?%%ehuBQR z9n9joGGA``hFF-aC*MDqkb+h7_$XU@zR!Zq<iI2`t2)`p-n@$=0i(*vea$S+F~$j9 zhRxy(svKEZpIb5+uLv|uojeYUVEPu*WR-)?bMJQrF}tjvqIZ_lxD}H(vR&y{n3up4 z-v_H?(p`rooc^Im5`Ld&i=36%g6RHY@6dznnu{un^>-or?W0*%+Egh1=Lh#j%*8>M z%yHG*`Ka5F$u8)-@;iSVCwI?f@~`{)<F9?;%;<<BkXwg&ogxz!_i#G78#sV-{=J?Y zo_GwVKctZ3JQo`dUgp#CZP?Du5<IFT848)<#>CW8fzA7oNw^?iPK`5W631IOV$~SJ zRrg`5R0py8v3t?($|m5)8L+>rTiERh0nGDd51V!{ksO(Ph;4f7g05*Y(ss{@jgwDd z$*<&*shZG7hBh#9;V?STYC1IZRlv-pB2pJw#eLE=hoj2|ajp{KF={L>fo+Ayc(XBW z+<w==nARta6z+RQ?%FINBeW=9EAt^TwHnwKwwhU1#o(liEAW;^Kh%45hxAve=O({5 zXKG{jLHors(55KCpoSf6ldcFa&G&_Y0hSOkvWV?jp~5Tui^s2lBe?Nl0dRS79K@R* z=Oz2{9&qF42g8t!pTKpiim<vWnH*g9id+pDNq)!d?sZ)MVaZdJjmr5&VqEh<(`GNT z-rr2*&c-qIkGpuiH+F*P@*KQ!vVc4xGw|_<$8gA-2xo7HVBV!Lv_JWa%ory@x$9MI z&?ABpbw89-xx~7%`HD9JO7QNMJk%J`AIcMZ+;ln%x!W^c(P#c3h+ELY@7++qY!ZgD zKPMEKBIgM^G>^c?HyLDNPYc=Dai81s^AtM+d91y|j8-Yy!Tzy*=&KP)#8+emeNU8d z4t;zfRC@wm(mNow>@9M$U5<f%i6!Gqv<O+FM~10L98oXsH9PhA9B53xiyGIJV3X4~ zvd79F^EnS39isw$Obp?o8zX0T%7bKH2B{Z{as9tG;>=&isI_Le+xsDzYr|vC@i8zX z+790QGK3T9rC41@iK>4oyH}dZw%X>B=P%^Nrn~#GssHU~CgOTBbhjxim&h%{+28>& zvAXE?v7WaPT7ZjH<hxQs&|vf;^!pf%B?XH}mG@Lu^>Y}t6lTNYw^1<aP&_^}&SUE` zhk&pCTAbk3M($2Ok0bVov0ZKxGujXd13Oiy?*kibZnP%k%Q14IWHBFqCzD;QP2sjb z8qN)N3L_Hp5fdRRH;)aMyFu13y1?C1?BU8zJ8>TCR*{p&%ZOG$376%dB3rRkiOk+O zkjPmsW5M#Rr2F6*E@|Q$a=Rwf?N;S!w#cV~3t9ey|J`wp7p2CK%h#K@yTgu&XB1ko zIrBa<-6Jjh%$P--;-qP){i2$TuYJjSF8z^^Ip>?nke}bAx7}041FW@4U@K1?EVtmK z)}8Fx@bPSEq$<fA`Iei#*^ShCxscArF=EA`*U3YTedJ@odRFpqJUJj4#LM(9^ER`m z5H2K!d0({SvTFB}Hxpw(?bkXG{jg<Nbsk=%&cao*9bt@-x-dEJE1Rn6e^i_U_ju)t z6WL-LJ=A{q9wHqs61Bnt67(~ku}dZ_%l#U!J30w=czbbSDS(F`OIhNDEo8o~5h_I` z^6yP<v2v?Q)-yMmgdFK%17DwE|7V4UyG|ea|8rp1`S`A1Vp)v0n<Ol#@)cZ}eOCO& zBwJSC_k-VF_)%8o;=qM<r1HO)l|##uV<2~S1Wi&q&F$2h!CDQiplo#pS=OAyM;NLw zz0sFQeUCj1GD;>;2;6&r1;`k*_J7oV;zTFz|8rnVCrx&-l$<IaMb{{{3ujmXg^A<n ziV!`T^`tK~o}42L=yfxWs5a8^n(e}rht&c`i)g8p0eqb0KvQ4JxmVm%6&4*e5Ne|I zg$0`xE0+x(LM^tm(F2DU3huk>py}E_&U%YfXsk7G*QlyyzAIM>Q<CP=eX+*$XigiH z(t8qG*FF%xec;i__%zk+PN6Bw+vu!SlW5wr3Sr=~M$GxEFT5RhO>k8$5H?L|g@7yv zs$*vCKC^GI@O$?@SR6Bw%7(~QPI$RmIP76wxe7rD^oSH{l?DmTmR>@atv&s}DPPmi zXl{G2`}}{`bRR_}IXN{oIl09D0z6Fp8V_*cGmi?{bo&3YsLd(D46WYBw0e!|{)yiM zyd`^AZy?(h7Q?!2lgY7zam@eoH9{*~AphVqJ}*Nao4cD?`#%S;*?N=Yj`_zvx6Tpz zygo+jHh#g*lQxy|>?qBdv7J6BJ0=_oSWa&wT_VGdTMDn_3Ss{?#4R3a;NQ6in!Wqt z-vNQx-*gkd_w8NgIn@G}*}F-|qfsg7IXRI;xt@es)*)<+K^pTe=^;Z0&LK7DTrp&A z98q;%MqD7C&3QJ8ENnPILVJ(l8|I#@=Y1j>|Hp;tM8pV@=gUyBe-2T!S7i-R2kGvR zMxrw}Q}9=RPFyl<Xl1U2kQk8eeq>%U`q^K>Z_c~n!xG6^Zp7bvr0dT?%v@`Y+gxQJ z()MHXW*I=sm^)b1cL;HR*G_7*gQPk#Yj8=nA+F-dXf>gUYpgW~6CXYHqF*-i*|ie5 zB3&@g&t(Dbb?jj9VWM3aMnlvDEZa1y@=t3FwYh3WM;t#3=k!C!Nxlu@4CUdCB=9%- zggzl#_M3svMpe>moduOPYeB(xE{3J*!|JjLXgfB7z52EnEXRez%i>yMuxTDSIB5=9 zm{!C*MA_`ZBRSl0xP&Qh*TBEKpRgm5SGgsoFM7R$5m+JLO?0mh7VMWKLwWx=TpRsM zh<`Mb8jjY)9s4Q;$=i%5lH|}2&w)18E?*}^1@FZty<ImNugkGCR3Db#yn@|Fm0=G* z9I?NlQ28#KZ5o#d8;Uwn?bAjyAF=`aHXkC(f2*>fX~EFB<s%n=%o)?g2W2{QZxQ#~ zD(I!XjU@cpN^TA7JwLKSIW8%moly?$RoL_-g2V5O%Gp|}LOP!y)LxqhUXx}D3G+3C zaE+BPGfoE9b$+yHx)f~=4rIUIU+GQrd*B;?IsCXhpO{bMnJoSvJYAd0(p{^$0;Nm1 zmbUQG#0bpC^urUMlZj|i0};Lr0nh#-HkQuERhLVc&l*dXwSFR7GhT8>Mr%j$GgsC? zY@As5ysMgxP;bJkOO+~h8a#x-Mv=lb%N0Vw`tic$QOfYE>;*o_IV%=cZ-@L*J^1PN z7^t)=VLR@>=gXUnVW@Z@F4P`L>c<=biv$m%Ju-)U@~UUf4|3S#t4F!Hr?g3PTeDd8 zvOJi4zs?#Zx%ZjKH;{QQt|qsuE;7N)m24Wmib#k4BKD>!;&P`=Lisi$_v2?2glTSP zp}u%C&8o?!(rPuKpmrmq{9R3xmG)ri@^1*<L3HD~D`d@-t7zUagcNEI7Xm|jzc|~O z4pBLUwr3BL2YY4;3TtA)T*;D=4g-|5cCW;-d@<YgV+`(3QXv{nLD=2$nA@orz=|r* zv%D=InXJ!i@=u5tFMn7{W@zPcenT|er4E;<xxCmtHGd!YZf0!!))p{W{S$io)TlM{ zrIMUguqtB?Y@8D-4%&VY8W+C6fyE~9Wyx|FDAAyAH8!GmSC$0&UccSD1)s_^+Ag!i z>Qts4n!#csw4qe5KYsjf117<5$ezE;h^R>o<;SVQkhk}_Q4I?4X|5tos;gvVmKU8i zPQm@u1V4BC0ro;)ms8Z*G9KIa_Y-WRH(`luHjE8-qJjIu@ur##pKbXeZXYe8cmCeS zCN0Tlc01Dsw!9E<Y@;T0dFI2EIiGRQtpyOZE|9F>`+*Gb$t9;6wlV#<WnwcABvUi% z$mG{`ycPGHWz{QkyIN9NPwF*Bbv_CE8_s~8ZGXCK;B7p4vr3rJwTP<!Cs#RZpR3T2 zIZW6!FdTR7))mSE+#vmPEen(w8o}y4KcOI~930ZLgdv4CXt6<EPzcVVn)ggt;pu}k z?nxDRsjUUQ78R_YFo!jy*Ks>1#IYqI)9{a1h1h1oF}CU8Rergj8-M?D3W@pe8w)$@ zNXp(#5=ts9gg0SB>6njo?wO9qX@^^skZ`^SJEH6b?+eB#VLxXJcXsCEl=?1E(0mFz zS`YPZt6L;|{U7$>$~Sg?t}pT4p9d8i8^A?pKC15?z{E30Vvx-O>{ob+IJ@Mr7xyce zylXsvX?XzoxMCRzu$abTj>|LimG@cM4S5*aZzE}^Dv&nxA-0^UqO<-P2{&Et(E*}S z5`HVBQkPTy)aR5PotZQmmp{G2)|Mv-oA4}VY*nUb%PpwusN)z`kc@@BaDJn38vWBN zu?Mzn5R`)Aa9GZFs2JVrqLtgz*4})PdX_$FO>`xhxAHlkK|k5k&Gr~Q09o?$56o$i zCThvIieHC*B6AXZh-Ht&ftMPE^QpZhv`5p*O3M4P8=2o=aPwYnr|wq<^&@HIu!W@L zeI~oHP8|+7<>QYw3#=JFk8Id7irx5xaCMv&G*--mpfB3^uxAt`t#aT(9%!-VfoIv0 zsTcVhN-j{lshiEUGDD-K2T6{_5iZ&_j8Efkkxt3CX}Hp0G|a6oCB^?bSooJPv@{MC z(sbnTWN{vJU$Ag54Zb26N8G^1PdY+QaX<9i-^4v^illiP<_o)9HQ}<KlwAE*2DU4W zSbq2fc(HO1geoRt+Obeb6Dq+~Qy1PnZo`dzHHjZRM$YBfv*owNs5fmcO4jAh<y?$6 zv%z!wRh$jm&MjSJ#83Y9kh`ohig$Ce#QAFpW<K-4d)f-{E^@Ye^Y(nfO5?mxGhS0@ zczh0Xn)h)F?+9>I@im;z$tH6`<nZTz;kePO8r1&@vMf$Xm^nX>M3jw|*;U_zao-j| z_LDrgnXeCi5+w=tQy#<gwY4Cm!IoWXt7Ws&9?H%aJYummX82(CSLV2V2y|`JgYVNy zNr1L7(0eb5SISM0>mMLoj=NBK)c6Cf{CtT1Ob(}M!&cFW)x+uUj&5>NwKp*Qx)m*U zzhV7I7|7mquuDO5Y)()vnQZ=!Uy#6acV|joV6gsl_DP+w?yYf9m)={;8zwOe=P7L6 zfecc9QV!jhzh&|UdBlINA$YZOc=~H^5;y!bci?KKtabWFHbMInG*?Pd8mSMVYi84b ziyzXW`j@>;c*fK$qpx85DV;f89RN~(9e6Zt5Y|7^!82nkxj7m7!YPUNZEjfRdM1iq z16zhSaY?(}AUb(8a}Ok7MZ?JBi_19Y`DLWQ=nW~_s*FQpr$DO7Qs}q*D<PfzAjU6& zoOimxk^|*&UtBQz61$0gnsZv%J~@w!ePJUkb{Rl3-p!~y88W<bRcyLYkSZta{lk;0 zvccrr*-9dLk(}JyX)Xn?n@7>Udo=YQq5_(jM2a8S(<FsV7}{V%AKC&okBKHJx6+`_ z?+fvLX+kD;^UTAcjNjXMgR7nx$3@N?4wLj8p?lN-?D}GWv9HuXPA`;n+-njQ{cD8I z#gDikrC^%pIHFR%%}Vgxphd5oStTJY=7z$gzsl~t?H<C_K#J*eDur!bFPUlEEv(mA z!^W6jU{7mj(+|f&5%;JITI)Q>g-};v(EaP|DxE^74GDuU?Ll1s@&C9x&hy#j|JTHs zMq~AcZ9f?@M2gHwiA*K^W!%@fMFSe8c~Tmb1{$aow^=eIBq}mBhaws7eVy0cAVs5; zCK^d;P%6^&+&yc(&w9U|ANE>%SbMK^?&J6!M^^_`kT)kr{q4cq_Z-=Ec^I9y`Y>%B z-7c(p(JuHINFB8IX?&PW6kKQ&_2Qaeb4l)^sq7DxCUD+5xV*k+%rmRgu(Nj=i+S(} zdQ%kGuBiX;_ELR*?eYWM`Ac_U>vvgJc&H6!0v|&_<R+rs+>4R#MCg9&4JKBbgSORP zVmGKC@li457B^KBYrd5etRuj}eKu9@@u0N-a(d2M8-hh8i$GOvJlTJJ8n3f3o*!9X z2Dh8$Nru>F@t1Y0;oZD4e!I?5-hYS&-!hda6CcMyyxRo0rnV35erG{SPy)B>u7VI` z7>V^sIrMtV7D!85gF*nqbElJ#JhY*P>Kox;o)<WZwCDn_0{UNjP4$O;f4L|okD`&k zCgSGX>EvO2Hra4h9#{E);)?qjgJ04fzVhCEhB;o~+Bv}`Shb&voLd?mANGbXPIi^l zT+#%CYDa41y_A3Iua0)LB{<jWB@H`h&#LVuw4{`wQ&>D+T2w<%-*w}UZV6%Y?k3?D z$9>rSbrs3}WrAl_i$uhKos^tb+)sO(OSp(3_sDPewbZUhLiB%}B_}5Bajr=s@Mc0T zcQCAgULN)aT6gBqDwU;@Yf2sbu(|q@W4FCv$N7=`^MV>keAtBR<YT~fksiXliR71a zF8F;;B1^TO5&7zAIK*TQrhjsPixJx3e_}V9HHi{v<!wP&Qd&d%=vb2On&Gh9;S{~R zPJ#YiJ&1(N@`SYCwVXxVak^V`x3KBgXgJ<oNC&5Ua+cL7Lnp%ll7SOO@+M7}dD~(K z-Xul@N4!#)ijkirOf7=fKC8_JM)hHTuln%O@(uLW!*XUa`aVSXZKEwsCiInPRv^~T zti)rYxxAk9D%O3~99{qWL5{B7M&#}K!@0m9(r--&IXCScjqsOLD_Ppol{+HnfQ!>e zh?yeC_Sukjk6YBgTN%{1IP(LS7ck8-0WNAMGnZ>xf&hkW*IyYl{G7`+R;z>P;9Oo) zXCeQ+XDx>7r=v){egu5V-^^F~R^jsd{=7><B%F6L=Zn}4Xt|h-WA8=b%j<XH+T{~q zec~jYenkt+TvtH#js?Q1h{Lq()M=t{pw}_mJwp7Ro~22n57CRIGib2=a%v&W!z1et zv95+#rmy%F{kaj6l+5|e{EMQb^G{#qGQUlPiK%^YtIjpz=|2|XVk7ibaD#5gi!eF$ z8jekLg`1cSpZ3gVrH7A_E^iC?Yg<Sh4tuk;>Cd5WkO@Y~5o&6t0XvnOg|MT2@tmU* z6g;|1gWVM2%*12dK!XRwQ%I*P5{BbDg<m+W`7ZXC@+YDPol;U-q~sEuK9p~r|D9BO z7_y9qli4_nnK0<&RBSypS6T=0mntR1U}<SFnd!F*Jf$he2bNjHzWxXt^|}WR2STW8 zaWsr8dP0>8N0ECf`Sj^a6O{28gswWTxlVmYn6||lude77AW0RXoG+6da+YLW!(k%n zQN<k5St*>-lECUd=kbL}yCue>W-#L`+u^{gTo#}*mf4#A#Onh;q4o2R_yKm}zCdR% zSuOoV8$AnxXWQVSkq?Ch&l2glzKztoYco8OmxB{hvj5d7UHq}p8{g}N&?{F*lBUfE zg|CV(ki2{Yy=`8>eYLKp>Ru^C)JNN#-gX>>_gWru;b~%S<ENK!a9t_;TprC_R{Oi$ z8S|6x@m|Mo)C}Wqd{dJ=Qr*bzr_6^&|F>jjcn;>J6Lz(|g>>vPm(nx`(NwvaZ})tO zwqYl+eP0RstcU{rKHHd4eJ2>6Zh+#Y1`ss%JxNHY6`D^fh+xN4Wuj!G2oCx~K(p#6 z-RJp<W{!)a8AS)k^!#W+hYaNN3Vve;QD%Oj=lBwvE{uBcly?rC#Sa#L!aX};vA44f z6Rm=wyIPk?++y*D*&BNElQCY_l;t1!)}mUognYdd0!bUbQ*n|bynIkc3NPz2cW?E( z+&>BB&VU5&|BPzGw5g%CqG6>gg-shg1(|Wtf~A2HldD)n{KAfLc^_-RDrf=PeOnE2 z!AAVBy+_1F3e)&TDHbVQ?GSdn2*uZTUvqoy#*w8hiEyd?3ohv6hpj5UQml9=OJAHP z6wRH=QWmtq$?YR~gOO`FxBpDZ_v%RAH~c*LcRA1BK9tEz4RaC6KPrXd)0c>M>Ncv- zFPa{@dk(wj8(>)KLD*@M0Y^sq(-qH)NZJz>+NhmEkX_)0s7u=jmW+@J<GHXa%^pgR z55ku8&1BkL7v^<L9o<V`kcWGVNXtV*Zl)AXmzmfAjxUp8$deG5Dqf0@F2tk#zC*&5 ziYyUqKCaK|IvsJz$4XGmHpZEn>P%~I8G>CU9XBEZ)1I!yiMy1*Nl3?$!UYIW8UX9Z zj>hEV1*qp14vybP^2J7PK{Y1~H_P9E-3#-fxMw^YGHDP5KN>)`Nb54Je#h~j%Z`G{ z(Gqd5j~f4T)Nb~;R)gvJiQK8*p_#nw&6POidpSN3xuIU#0Jz$2%n#iz$GT>ZVTPX` zV(>~i*0R`z)x8?eN3Rti=*VHb^C*;M`$xgj4r@O0sWFCqz5(IU?HFS+osXLK44WFa zz+#_M*cvTGQ)N9Nb<k<>H2zKIh8N<AlXh4eH-ct~>|$tljwb9ovV$ZYk<gHWGGesL z0_Oix2Km7c@W-YvVBxRHUBMo~TU8Bu%ERgSqPY+^st0}h$)Z(E7UncoLr?ZFX!{|> ziXzUFR^=X=)}{qZB06bD_b&Vr@`wxUl0fQ^jkrEwHmg&=gyU?Mz^@H?g6P`KOxD$0 zhFVPr#Nz@g&{Xq1xwq0CKCGM0NB6J8%|1!+{%|0qI6Q~1Z5MEU;51UAq(zegfZu1{ zMf6AAgIBL+piWdaUNKBT|40pf+c|<hHHq}Rr67gx9QdMvcKB@mQ1&t;gFfG-2v;Xv zhCfZW1f1wC;-9V?#a3DoUhncTJQMc^4-Q?74R@RHTGKjC>8Cs&+ojAOH;)ubR%c*9 zD@C<Y19_k2gTd{`SlA-d23Yu)`b_PCr<^nMjSJ!fjg0x-W7;72n3A+-VII*v*iLJA z=c0DG5`t_TydJCyQ*sROszNVWDG{wDes{~sigGs)Hh95d=Y!x<wHtPumyyw(IXEV4 zE(y$>z@EQ-Ksj?gP^zpT!4~aKMla%Moo6{XT)7YTqmC0<nKiiWSR{_xRzWnKuYg>? zgJ5~ukay603~5FSQTO6vbj$95z}8xHHc3Y2*q;fbieR>=EE_ATRDg6%hNp?8LP^JC zSZ0?+!)-r9oYN|@Gyfa)QVK-wDL~`Qdg^to9iHD@h6fuezzITm`SZHc-^x+^vtgsr z+}w!`aMNU){pG2t@*S9c<S%(|vjTd)$+Gi%J=l%h4g5Hh^-SG90TSEipiH6)6D<f* zWZU&*c+<u)=$)ny?-Z+Wn+@W;!iR9}<Rw&7dPmofI*l;M2R+ZffV)S{ggG1IA^U}h zna(r=o!p0@0(qokxYU`s9*oPJCv)X#o9JR&OQ_l`5j6W3Qm?*gWI|REDPl|EQdM6Z z>@b)QJ{v=B+`2EKsW-E!?*}hDpce=2nawy(rUq9{HpPHg0WMq*BF}1fP@^OHbah9q z5bSk^Tri!DdJ8MjE8{a)8F2?r@9%_{nr(v5?@Sz5(n^w38X)}GAsnKp$PU?j5h_Y2 zFb{JERjtnOq^v*xX?p_-U*viJ*Yo*P8%1$sVdygqzj_>NzELW(>nN$w>Vs`3!}+Ik zr^5NI7Hp}BJ*sMJ^OD%zg8cs15dL#La?%X_8-H(Fu+oTqGl|E`k@9?2`(i%6<_(@u zcEJmuF5sBsA3-yk@YgfN7#x^Rf5{G~C%*2*e*5x?Vx|NC!A^^>5$W2nnq69K#g6?> z6{dPDaIyx!_>COf=Hkqkd&EP)2&tYlYzmlK6oU}Emnv^LB3#xABB|Prg7(iL;O1LH z%zlm+4*Kokj>~j&O3r3N{5CCWw%(XKbbTf#yS~5BqpL$Bx-OCilXBribS{^(ww)Ut zr9orewu(j7@{YpRxl5p<UQc*Y-c|i#jx)Do<yqls;&~GEZi={8Zn6-+HI0*N^W(~j z#B@u#7tP4e6zpb&!mpd&WRb=sVg0@`VbKy@5^~&;PI_y=q0~Ti+kTImy+KTU7gm#( z>-C%+Dtzd}6ET8(&S~eV38lit*PjG8v8)JUHjO8<^p0}Ev&Z87+6PFdmogq*F_gv@ zXb62`M$?v6qv)27AUNSal7xO72&ZG5@W^aMdd^o0+TEHWe0vy08?Pr3*Y+D+wT3ME z>E%$Zf+?V8eh}Xqx(Xf};z;D;0^+hXm4n#>xH&<#!r+72IQPUfI$*eHf!LOd5=tke zpj%P|X;Vq%1}zhlt?rRR){`=!qO46I>$<DQR3#9#BkFMKawF#%WdZI31@6C|wCcZV z0d#1E4>v1gGqKzjEjDcbNbjiw1b8l`m$!`MP9#~7a8EB`W0)nUI?I*o4I3fs{Paag zZ@VhKXV-^jh^Co32aVeZO-Cx61CLzie!V|SErw(WWuuj-tZo7K+ANx8ChQ>RejnwI ztUE+D{PZATBM9zehH&ME=A^PmhHfmhBI^=%33GC!$_=X`kwX_!2j>`Kl{rW-%2Fj& z=f_dUm+MIV0#$Ba-cm7hyDwOVZl&IO)tu<I$8OFwFrJR`{7G6qf8%;n3rUAhk5E}< z3Cd@6$^J8cg~gIqI_>^%Dr+3eP4uzHJLX=3a<Q$m*|rY4@zqdrxVVJWzG<LU26e<L zzlmDSnFd=TCt=CcV4-&AO#1QDa^j=#mMh-X!L5$2r<08z&`~i?bh+7H5z!eGN>O2< z;P{y*i-wG&OLx~3tp|5VNd7sY4tERd6N;Rs)z5G;F!q$*sbBS?U=_0dUafO&mkzy| zDFZ+ImvRA_<ziDyMH>6LP&jP$!g-ezFHcbohgEeRL}gE#IOxF`;y3ONH_sxTl)bEX zGIO6oD{_lzl8;E@e6y{A+Sl~O>92m!(08lIz=DBfzHPHm6J1CD3zu-rVgtAA<u~D5 zXEGgOW-UzZ))uOpf=IxPwN&xyK6*qoQ+PG2g(fZT%e{1s=kygHkiD}flAKm+q3+yG zPS5ZV*`BU}cg?>z?_BeTn4dC(g+)5T_AD<@6P?W@703J_G0cR{ORc3@4H{(lA&Il- zR*_Jsp@6S+qPVZ2Iv}3tNH<s7lkRW(u-R&ebI*(dGV^^imo;z$w9Sqqr>nKaUdDTH z@31CXN$a>L4l9KpQ^(==D}gY=<EZe~zJM;B=EL3lI#RIhXc8AB`q9Ca{}GYvrc{!+ zI<1<!|AJhqGN!L(rU)w552^Eq=i;*?t%=fs0rdE(eY7$_4=l@csQsXz>S3jqg@XEJ zT*>foZjyqzQ1!#l>9w&EQ8=9Kw0$Ar+7o2Sr1GT@rZJxz|Eis9GZF;dgTn=1?;GNL zA;3vNRgnx2Y$O`X%S5z0V=7rZ+LnIMyGR134{^3$^w{yomk<)Vvw<wW-bS_cEpWy> ziJ*J5nHns)%a#6hsSY>w0lTIgVPJ<bdE4nm7q#{!+#Pwka<4j<;Tg;|dhX$-MH_MV z%O{dC+p@Sbsix%dpl+(5s!QjzjHM|tTHv}kiF&lJ+)r2kRl#oaLOM?=MrdvpXv?m3 zXg|yau3Brr8I4J_w@wTCi?0YDTgH<$#}DI!6(2d}fg>=*B8kiu&80uQR+H2j7wFFG zF?9H(QS`Cs_y4Zgv6TaV{hv>5n06*!bHj-&6Pyq|3Rgybg_~}GpJVHYz3E#hyL6V0 zIk|*#wX<ROW*@=s^H$h8GX`XIH0i>{M)b+LL{dMvMzC}|&8_^U!_6>VNX<RRPzy{Z zk3Vz@@w(<TVffDfJ+0zM_rw3sryiO%b&st`)<2N{qjsH~`z0o++g<snf(mS1n@CP? z`$#TcIDlH=QJA$YpM20&hKk!S@V3@+HfXv6wtC!z=59lHbEktnkOsA)X4TTEj(X(y z{s?}+jjJdw{>y|adwA5=m#XV7VViD##9`U382oS!NU9BpQ{V0Q>D2_OE$^-bEwi3D zD||i1MDqH~<J$@xr0YhC7VIG3cX(hR`#^%T`oinH>Ci2Gsb?D2l3S*CiONkgcv516 zy`_Da_P$fFexe@JdDF%hgj=(?|L*a>jke?Ss6z2G8)=n}Qwpy2p9!}N6WAZm2(tKI zJ_O1P7B_~9^jYn^JaNbU2+UpHAJfbg*}~W7sqxugoav%780eQrh7Vi=hqm{Fvv!)a z|C9spb1CEQe;YuY7J7iZtv!hx<O49+6ADIrAR#{9aA2G(sV{N{cW+BD%NWgqTMQ%z z$}6$n@`&^BW^Z2omj>S|<>oB+Nt<m%>1Mpl&;Go~@*tdl^@e^pHV>vNdD4^86@AYu zTXyowbl&Z`B46@d4#NB%lI@37SY)XSe<k%O9aeM0d6S_v&t?bVrp{PS?}G-F^mD~v z>5cSk8;E;e4MR7rbEsc_1$NzD3G)3PLWBGe$>8x<_)($r`H+bNL3E(?7L_rY&WC@} z!W(2iudJrdQc_e{!j}x*?U^d?;@Xb=e(mN!dkfj#;En6FG*RrCO>)PZu^8T-A3OXk z7{+WS<0DVd22KZdnVbR3)J^D|`ile%u>iZ?N!*)~LHM_*f!H1Pq!E*<$XA#799Naf zznqaNLgpXL3&*zM$^A}}>3ko)-g+|cKUR-vKY4-at4Fh%uKDcpu6PF074FkWM`)aC z#m{-v#5t>`(HQ3<kbNg6<9mJR&H?wBam)*>biYZOUq6J71&Q3mzOx0KBf~!}zaT8R zlSF>+Z6{+B`=h^5O7>awaO0FkKZJ(cwZ!v>8rAeDfUVXB{7&Z*m#X-myl(qx=JM-4 za&61`X0_qG#gk%AUSTFR_#wwz`3+#P)}}04Wi2f08O{55&gXmP9m3Dcd*RCGO(<%< z2C=tQS?6yvK68{jGp^Z2+Wh*UWMwsxe4R+<&8ejAuO^_Z=*}5(e&l;WwoMi@PZ(f+ z?qFKdVgfm4Yx%jJr7U~tY5MiBmgHbqGLbX6%Ud6A<=gKnOZ>H$K~t&;pY{Dd?0t5Q zpRMNNteuxdLLQVsTgnc;<>LnED|Hq%d}i@09*3br;{d+nMx9iq>O%sL$n#z5zhF^| zHx>0<K1@gK{0*;P+T!Y>pX5)V8a@*b!8_v;x$CC0Y1o+Qw7hdXS@B~y%NlxsH=h{^ z$0F8B*6SiWdm@8Z>fFfu-=^^QoXWr>;|zG~MzJ}Xqa<Iy++xW-W7(hONf>%fpVj*V zJwemZuS$tE{g}^v{{Dixd@B>NZ_g{~!fW2RzqlU^e0!8EGI&J=+eEk-q=6G%hM}w1 zd_4Z@75y?S5B8_8r;pqc>HG9KWZ|s@6#l%xlnzB^IxLQ_S!T_C-;!bXr6q$IZVJqJ z@e|=#uL;?IaSK~lU(E-79?eFowUD|0RX|8Y3GbY$&OR>}A*uhcimywa1jVU#Xth2X zGy31emy2`YQL|J;ujnPMxxSFKZYZ~6Ko|8pJRY#WAk;lSKvF_i0Ph+{8}sw%#UHkC zGf*0LmKN65eV;+QHq4O3>|HMD_h2P|?eQ?SEHoUw(=BkojS>8I<9&EI$d-xMt$`L% z{$MEXlHz2Crm>oH->9|8dg1Y(!7zKH7>BEUz_;!{u~2m{#(U~wgKrmxPN6h1Ad|i~ zP-VA5pA!qUe0r|Wb<&U*2AoxYUN3A09h`ZcSS|Yw_j`=k<AuwF9o{R!V-vFVswMoL zlnr=8RtjV!XE2K|MIg$o(_kaMD>IwWHuCH96&C-+nLjb*8d<u3D2z9Y#^Kjvnd3u0 zq9uC+6JQ|!VN)-5kB^l;I%77cq5_|&IZ?TO6{I|=5>M-VqCIX~xS)3jNgw+$bhL&x zzqYWAOt6|uT)v(J$Hp=mBhrAyCk0RnmzT76^oaNng{fHeDIH^%wZcNvk^DNlGgMP= z3LoIInRP`zhfjPN)UTLHx2G>;`|>sT=-cP1)>$3qYaq`Dlq&E;YO3JxrGY53{grg} zKY?$gP00RHl(Qd=SnTKuy_aNg?!f1?>BDHcu+)Jx+|DF}`Z|zD)7|L@MbS}SzitXt zZA{{GlMk@y57p$k(pCIhAm=h;w>ryJv*O?PiN-kxck`xeC*wT(W4JmkoEBHR;2@bm zY&ESTHOrRZt%_ij!?|F(cMQ0R%)nmS2^~8)jVe_R6^=~kB}Y#uaLeWm!<Igt!ut1K zV3pm;>E=x!vZ91bWUW_!xScLbMyuP9&NmM1jPx^8gaXvDc9VqI3gB6L1}nU$vT?c# z_-);LF%xX?Oz|dW_<pb?@bhjqzqcDt=gh(Nj;pbJ$#+5g%m-`^)#9YKY@+%r88@#y z%QAW;7!@6b^Uc)RB`LPzuO<(fr+ZNJ@-u<tZN_*cK?Y6k#?q2*!pzdGv8BG1T<Ec8 zl|4PE`oy0tp40);4b&uyFKveX>PEyrUzx=PMX~V0R@iy5KYt?bFnbsKliv|F5H7Uc zq7K%IthF|gmjsqmzhG7D%$iJ1_0QthrD~`@%ZxtsD*?Td{vuxM_;p@+qAl85E2D-@ z4R0z9RM*KrqxZQY5~3s)GQxdvz5Q@ObjSm}h7Lrgm&a<#)%jr^r=a9`1hcPhWYa98 z`Sj<}?Dg$8pjf$yzMXsn!X7D0np#cSarfuq_u@+M80iBIYh9V@%ag2hkRLNE-OsXo zMNaJTk1G6qG6Z!jA^_?~(rM~XVcnFiWZt~#XnFks6sOzMd-g+!ja?vy`i+AxKM?o+ z{3`^y55aq@B>2q1hGqWA;Z-D>xUVLIpDWsoOFGk-qv9icsA$3;ckjXOz2|W6#W1!% z-$L>=GC@j<Jx7J&0Z2vaU+_rjdU`i~Fz-7?jGq>U!t<GNnC6hc<mB!X<t}?nzOYT$ zexNTG?&^$(I#sCE-BK>3G=tX6QlXohQ;715SX#DYkMJtCgL*uhitwxkkm=*tkFRlS zMxi7x^(Gq)BV4|gTotZ{t`NKu`?7v-PQr29W=?ccEsjqAd6Py2Dd3o?PI&uUE*Zu7 z5)apvV8t!LlI$p{0k?ym`Miqs*)@mg%WohL6tZZDQ8~9MKa46~I>`CVIY!nSCK0XQ ziq)F43x%axU+9IpJG5zq3cUJu7DL$tNHV+xGoIzJL+>nH47TlMLD|ZZ-B~w8^x1*| z7}Ku9=IcGc&jG2Bf9?sM*!~s=7#v1RoqITct%a18b|a4t+#wqpbFo212~NLGq3>2d zg_JoX#BYA@hKaY<(7-HTQu{TVcBD<gs>S}|s#&VA_uE==`<8{|V2~Nq$gU*i{tvk3 zi66<?$u?~FetEv}r>Gef1}t!?vZUOOS`(L;Oh>kK%|}?7o(Ptgiuk}RS9ZJ1o+S56 z#&L}saO;!pU|2nz_p_;|1@~v-e{ITW+Ai%;aSw)*EsREbw38mU4RocSCZDQp%H{sJ zM(g9>L$uc!CohRVr|+tbeHzNR$%!)TQs12<RHS5p$ATVXXZR?3_0&xMqw8^)likWn zr**@0wI!@{BqeqcE_{TZDk=K*gjx*`2md?!nE8buHuKgKTo<o}q31qA(QS8lb2S;q z->c!e-0f+QX#@??jABtmg=oCKf#h!8%wKeSPdnsK)6=_>c%!Hz_()F;MEY-3FpO)) zydAE5n2jb(y{M1jW;6KnPtMXkUsHg~kvh1KdiYwiL10_oiLC7z*>UC_ajf4<o4Vp* z?mZscwkU!{mmaL?Ur!@i)<A=BithQg1=q{2A_*zuQBOY{G<vgyEf;LyX>SaSCy%kh z_ate{pC`imx;kWkKp(+rhnnzlTr{WB&jPR5wm3e&9VV?n8-ppq6UpL1*GO`%0*f2B z1ZCvK^rLl>B<FG?41Vg!N@u+$R<woWkG@D+=l_RsO}k0-q5{}1(+8CgNZ$qPAUGwZ z(;gYfva@YzSaWg?+V~e>Y4tJkceJPptP&7hN}{0O^cUE%t%~%lIZce240Fb##5c5_ znxzp8$eT;7zDIDAYSwbTE|wVQri!<h8L_A@h2(%$A=GNlVDRfTZ=aRG58GYKhOfxx z2MluO<MR(mh0+QJt<zW#semBS6?k2eO<2)Dx>>PDkQ1$$gzxpWaL+6a_V{QG9OyHY zl<w=x3|ezZK!rZdORgiA8l`=0?L+9$m-5)sstc3;Sb)ri$JFlb3plgNh_3bsaUL<H z3{L0Ep!k;&FSqXpEQ!r`x$^4*TVkxu&;E}vVd)ay!1FdNxHg`)KK_k69fq=8UpWvh zf3p?)G#>>Y)0x!9Tb32SyN@gH4PvV*8?h*DB)N0)oiI3W3Ir#rqDQ^~lbi90Ec!&L z`OqoM?si|Wd;5mlNLD~`(nmq1dl`FFH<we-j^oIO)y(R8Gi-5K&L^1P1n<Agd5?s( zsQlpwBSjg&q?LQy?@J~Aw>TD>N9gf=##k}$hBUOh{Df{k7|!&U%d(Y6H%t2~$AE2D z8S9PDfa6!3q1Cqz*KLjg>mNVh$E$Sk?a-pnL)^gW4+YmqPqdw91sjU;s7hlnUA6xJ zMtD7@jvX^;lu{DD+;)nWto;k}jizjW;Q-0>2cr4>xA))JPW4j0s&)upvO%9+{gVUR z&S&%D3ujrxssd?qKp}W0p2ULZNjO-?1C54jGq3$h%<+&W3#jeJ>a^v!*7PLw`1!)q z<{q%=x1Yo$kAi**_aRqd2g&htr`D;>loWn&uFopMe$0aW4mu??C9AOkqU%*~&LNf+ zj>v$hHQV@w7VCI!%>|sAP!H+P!<i)R0&G`T#K|Rk{DXC+ywUP~{GM=WBXz|F7%^B8 zmYtENfN}{OTOLE+N!j?-@5A6mO*tQbqCbDV`6DSS*#m`&iv+z#$uxeMALvD<lBj?= za5>okRt?P{qJ}w71)p>c;jrft(wUTp)BRRMkIP+%-erO>^i5rcHrlxCx}ooKeECIq zW#PcT4L!;54#KDtk$iuXn_#(G8GgJrLGig(T(jDrHsy}2QZ+jS!xpb*&$80^I_EU1 z;37xUy#rxh?l3e~3a7JP4~2@GGa!2MB@w-SDuMP)7893e&0P0{pPYQfZ}MfkJ4vyM zq6JeIL6+TTjET(TXZ=tIvv=!V#Oq8Y2kzeF_h^5`n4)n0*W{h}LGKMzpYUQ1VXs)| zsY;fFhO9Qaj#I2V1@~bYEnOOmJ?Gz(w9K=(OWlOlUW#Wf{ZsKqm->bO)kUl?mR|b* z4A{g&wj%Ml5twl6H}`Q@Wc8fi-?6|k4tlc=iv1hC;e>RNQPG-#KhInsJ7TIKuqlI% zin~D7YZpS2rwpEHXr(^v8yECBiB@Wv!LTujIIp|QNh}4zMszC(bL1Dpv9NbE@xV%A zyVaUR<`$D<ieHJ&j!^h#z-9$>%DhW+Ih-r^uOK?@^<f9Oe>Ki&M%fN>|2pQ<X69(6 zZO%59``7%ImX+-&_b=+4_CPvFL&kY&8aa+~|B?{W;~eGwWfRhpa?0fXf%<6^jyTBu z!|>C>rNckpIZcobQpYzhtw%ci)1}gEbET(|Dn)65(&68uNjoDQq!;xrtv$E)e*o(2 Ba>f7v delta 25991 zcmYhicU({Z|Nq|}+Eb~t3yF}%^LZZklTBF}MG{FF4MZs8*-+8eP})Q^G?d2koX6v& zlo2A5qDV4cLK2eo?fv=VclrHs{(D@`<8sbp-EQ~W^-<m=R^B95(V!uqN($lG-!-74 zmJ2}(^da}M1+5tS19ZQu&@MgC!FT8mys|w99lkFhdQ1i^59pB`VFj)z?1vxECBRDS z7(S*IhN6xclgXZY$UxpivS4xnIVEF&-IIoJz?ZG$Y#2qZ&bHwQ*+!<--9;LYE>NfN zjk*hAjRLYq$(1iIc!%rE=8?~8N^s=U2qPZ%0!l1saQ&7Ao{KqHdqb4IAFNMbe()R) zE;UD9j$t6}{Q*`TjDkhUYSiart@zu%YslI<oq5seMVx;rLgndS)S6kcASwP6QN#qF zl$9s@oEH-Fr>4YxV;Zr$kxrKA3v%(l+h@S@nmCMRiGt(iLSpu*61~oiMkOVI$VkNs z(#lQ}ch`KPSALUB>QBS>iq*N1XG7#&pD7pcLW&zzttGcQHxS#Qb7ZD;I3eQFl!@UI z6#FBBtUP@U?|oneEsx|#&|7@~({j+=O@q|mTF^cl57h$sb)c}(8`90BAY;8Njjb@8 zouUQDJQB#76(!hkZ4&s-v_f^m@vMQ;1Bm;POgu6j$Zxqr#9+%rQjxu#)Eg*pZyq<0 z7Bx{W@x2(=8jwTI4{&7B`Wlj(D<s0n^02PuD_Xmw7k7VI29o8H@Fe;La=)Mrf@gOw zqmoyiB!9+y@@<t3su`UKUX3wG;^}e7x?l+I$E|SqR3-FOZ55f_=>%Qr9yr&eoTB?< zQP1@+Y-P$x*mi0Id25#mlPyJ{xnd(}oEAg&&uE78>ow`6xyg9w{#o2~ekSC#jt^zb z96}xGXIpw^vbM!91$@%1L>f1AQJeOOQ!my|NBbX7M6u5>zIx&zf8)))M9xhfCM`Wr z4jcE7)6o%NbN(kQ<6}yWdsM=v!JpK^?^BUqND?_%Z3sU5!(jMg5V8Fz0X9bq$j_ta z8EM}l$QhHO!9<MQs0<^Yy*uD~KrSTeNRWb;f)HZgn*|p#^sw`CYm`6Kie`xJhm?zb zY)eQJ5?LXM=SF|P8zvvXJ&V?ppZs?GuPP87>OM*4WIsc90#nJ#t_*U-@FD7(){mR0 z5a^eb=O!nlfRO*0zkI=QTy)0<nN^2C-6vBbdZCsGRvd*qSc1w?4oMS~sBxcSWug4R z1DLI+L4F*(N$P_#VWnaTx<8tPe6GfjWgooJmW!TH{OB8R<H{xQVYNKF_g6k{-9E~U zC~t=Px!afnrOn7@brq~N`i_(GJRq8P3c^z!kb+At0FCB&#3c>=2`@l?Nk!z2+y$6Q zOA~#0cOqyr<dLBSJ)$&0iAy})L8R10xxt)Ha^77)5_-Sk*y$6<wd8yFsB;l=nY9^B zZyX}XVs+i^LIJ1@x!~O@lK}lX&Mv-6p$7>V{1Rf=gVjmcs$Uka|B-@Cwz~AC{TlH6 z-Us&Wd>fq4*TQ$VW<&O+z2LNBrT_*X20_L7G~7F{4V{!eLFOvaM7uDGO?Z0_dbj>2 zPpx~%<aul0{;x35`EEr9_Xd*{)D7g9C5{<8JNug(c2ds_wgYeZA*k3g8#4c+(7dmP zXe{j-7GG+Dw_FZI??jxD^3Q%GJ>vy9sphedoE}rm`t>j?Tu@808*5S6$ua!u<w;15 zkHv*+UC4*qk6_a19;nVNf?pG&U@)WrmK;>4T_3&$YT8^{YyKx#FIfd8&syQ$%2p6s z%0SJ>P-aTI3?mu(0aX_Z(S53l1bxrI$8)rhO?)>pn9xR1g?-q+GmkW{mn455FD5Ah z`7Pvdbszb6V-`_AGzqT$xrEV3HG9f?7ME8oh32>`;jxT1R_2`$ndL5pfUI2dt$!F- zG>=kD>M*Jc`a_x>#z^p&5U2{xK!(oS$eVK&B;Dc+oR@T?!rcqlOHGx;?aoH1ylDyY zbFZU{C<VAFr4DE6YO!GTc5O1+SOdqeY=Gt-A$*=x54iy%Fcj5*BDQv;Hhv3fnT^@j znFFjvUkEZclp%h;H-xV9v!T*X4>A>#$%^L*<k7TPHtS;o<jney6qHZlE^~3{ulpEk zDp^80k|Rj!<9*=AyNO}90Tjuo5#K8q2`fGdP$w0^U*k3trP|8FH`)X$*GRBi4xNVI zGn8TRJ{>Sy-3?vKy3xAjkC4C8Ja92($>xG`zJ9YPe9%dSEkZLas$E0erhkA)vs^MY zKohonD*`D~C30A+kbL`M2|8w<(P1S8?o$3;GWo(FQrqPRoBrG(W~PEBc5=~XY;9mf z1V<yuB=t5l5c34f%zBQajD$Ezb1B)TY7WZ`LP3Qrf!Z4}psTPM?{>WfRWJ90LRS^} zG!}!7e}2jE+?DB|vyDVwXaYx~Q$R(%4PW091&SYR$=CNz_+Hr&kY&4|{Ot}buX>Ca zKN$lDLCZ-PqK=XZg$wpxKg(d3u@w6?ouNiDY@jH>6(S3rh~)C?Na4|AT>iubjQ49( z1}{R1nb8>j*Xu)^COpPZmj}St`>tf!<bQ0i{5iBQ-Isi<KFi)u7$B>5{z8An6v-`o z4)2-23wEq812ahz(xqraF8YRo;K;BP-7c$1w(H*{zVv&z8Bhr-RYmyF^eec})fhGu z=kQe*Z(`pSor0iBT^KOGOD&oo&iv$QLJ-m+7Hh}HvwI!huy+@^c}9<&rSlc9lz4#_ zkChU#FB)s<dw}YODB`pJBx=dj;q(rAz$up@>OZ|Ol2RHVfD<Q5z;m|}xnO;UOzA6v zZi^xK7E()+6W`(|O0(!enIf=xA5J3EHOZfEhd}f!#_>~k3N>w}fb-f)*e=<CkLO;( zUkvkj-%$wVYA-|D#}Y~3HwUuq-fI+nI}_`^b;UhjMoGW=LgMJ22QT05BKtN*qZ~$% z#4W061hdhp#5{8&$^CQ)$}hUYvk&RSC_Nl2b5T(2p-DFlUBVv6>`3hGd+>ZqF1%E- zV9%~wPYjCy1R56PmFfs(BGXF67{8#NYso;e+YfZ%-d)s3U1s<E-U^pDzrz2`P$I4^ z<MXWMAUkNU10mN62_u*yg5HT&z}iVk<l6e9;EJA*khuGxwpELaUDYOQ-z7o#t0s`j z%^{h$bcm7k8}z6B9R%(<OJeslkm8G?bY%BDkc|x?$*zM~E7ud`oD(pW)yjUd*a4=F z$rN|m0gNp@p|AEOzMOWNY-D1oMx%UOA@Upxu3y(9`c{_2rAdU+@yjMFE}4*lssoT} z?M&Q@v~gD2QPNX#6|IkSAY<?ANkO0mY+bGm-BatyOJqzO4lidK9;!pH)g<om?z?1e zTr*PQ+d^Q>2&pSxL-uGZVFfoMd?#IkTyH!{B$eNy`}!#mqoPGR+<vS@)8A@B$l+(g z_i6;qzr7J@j(x<LH4RY7M!;C<5!6&#iUt(4LBGs|e{L`X{|8f<_2+aT@Xr|9|LZFX zn6Ly^J4wqt2>j+&F+oJ6VnWImjmat51|}OXZ7bm>KS|@Pj-IDuz>AKIJk6m)Q#lFM zH2R=dES>N^iEcTT!TstzLbrZ9OQ#;+L)-fv=1L6@aT-hwr>P##Z8zv8_v6#(|8+5} zAvt5Yh=|DYlzqN>DK>_V0{cZ#{FnPun18XVeCfb>!pB2gy+z7z{)>SlykediGx$N% z&iQ5*^Qdw!zhsFwZ}&TW{<*)Y!YA74{9Y9!-e|{Z;mH0z`z>Fe3AsE!VZz~$!po{N zg=x91JjUyZu%mS=PkPTL{v;VC{>tHAp{nvD;nuB@{GU7S@U36H7BCW1didY-NBO<n z1mT+Zi+EacVRl7*mzjzpH~vw#kh-Ebcljm96&Tg(ME=PqF3hT$3gMH=3qonMoSD19 zfGM4HLZ}uk%ihz~<jMWGD0HN6GS+z?7<<zpW|Brczff%lUt-@{W^!&kQ*iAUfA!+) zjFHw!zNyD@cE^Z-FjEs3^X44L=N+9fB;5VHgXeL%f+@(H##do?3e$&=GK)5b@;|9O z<Qq*jWKPAbq4d=ysLrWBh1RBS!cW~{%$0;7=4ny~<L9-L(u|3(AAX;}U)QU|j!rb; zs|}?x^B$M+H|=O)oK8%oPngs6r%)v>MM;_tv5i9^f|*tj7AwVZsViv^CXu0U>d2vQ zD>0KD#d8ncfT14?XzjS4#BE&bE#@Ra&~=9$>FWdUgN@|NbQ$`eku2xOByrXg6bUp= zMe8$XfsalzD`_yDJhQf^tz7lV^pynNJ9>~&NS_ar{_Ds6vUNmvLM|seZ5t;D$Vi1b zy60d`oGo3ESq$}xDOfU3oIaNshH|?th|XX%lr|ipYrbsc^u~9=?zbP{NJuoTV^~ji z?X{pQHI9?1C-k7Y^)v9+NYW=gk~y*4KdHPVD|%POF_NTEg<J26l4+~0v5L(bqOy^p zSMjsZwc%vk93Cm)mL|q<2i_V|IR6u|UL8bRDRkq|dPnYE?`qCx-3GetLJ?h>tVR4a zjJfwu_*~#~J34(q9{qlL6Y1LE%1wIaK%1InaHTN|xyrdYWH?!kc1^fJ=DsqbAN;z= z=_r)J$n0p?ASfnlc1IA;MZ>s%w3D#uX*k+aa1RbEA{<?&OxNt_f>NOu>0I276IUq1 zIm?se*5!KW|Cq{ZOWh}utLAaud&@w6`D(JiJ_MfJ%EX<=C(#y6CTTWqg1XCt#OlB* zPV-h0vhAKij2`OI0?iDP4I8=Y2bVb4rCG>R_A2OEZl&$^7GOV<PI6hWp^yaq(58bs zW^;GeO4B?2>R?VHM~bvA;+ky*l&p;ebab?V-YZpZ>rMo!aEl~F*0AAQ#Au%D7Vh;K zW!m|C6n!E%o0D#x39m>F4E2X_V)LSC-K7m&oBkR4#3?CyX1gR;Mr3K}?H4&mnF`K6 zDHQRn*Av0xepT+b{{Zg_QsEqQE^(I6L^)$M5jx_wE8P$`h0~R6$I}zfg2nyMZ1bQW zEIr>vetEFyLUbt9_B|vm!HUd2aZ`SCRJgEp$81J1GMq;ZU*yl(_fROg;41$^sVcMO zV{Lu@n;A@4S!DgP&Hsc3sqgBi?u+0_d1VQN!`aFFbn%zG#VOVNZ%vAJ*E&Aicbdf8 z&zf_#J~u4Y-m2Tx-aTLq!|aM+r1f=qD2dOzB7KlK6lKQv6*ls3W@j>!V|5tjkTY-L zG&iAkWjG_@I>vt+<SlH~jp9#}%w#(1ocQuo4r5}j#m_1G$D8@An%6C6%71lSP%6B8 z(UOw?cZ2U3w~W8HGTqLwN}0E1iUDu_7f1WozrQf?AA5!IN;N!@SwHyi6RMdftGgMi z8LHHEFHhFrrJebhy_!EHy-~R0)B$Gj{1WD8h#TAgQH?6z6(aN=bLTn3Or|SuEtP!k zA)owIqh2XbuWwr*%gzqI%99ajoZ(-)QNrKRdaS;%Jd9s-dXV>2?HRxF$1%2any_Bb z^nkzWaVWLr1<mf>d<l<wsE}L9kw`In5p*4iVZM6WaAC`8slU_={NmqEHkh79+f{Es zy7M=n2H1|`KDY1_%yqywb~9vl)v*fqe~`Vu8_6?&XR<1}4hgn}IP<^y=-JzcJ0rue z5qz(69V;!|itJ6)(ej_$*orJ6E>V*p3Gr#{{v!;<%eEq#)u#!gpUBqf?}DrvWq8(d z0hyXOkq;%akW`y8G@UC$>w6lhseK1<&ILCnGJhBJSL`KD8@^yI$8q&^))4I&VUXKp zT>(rjd<f!O%OUOVJyhoA$Nb(YB&peQpnuN;&+?Om!u8>J=|pE@x4jmJu8joo&za=p zPi1b|Y9&r&Eh6D(_2{1)8`+zRk>qa5YBD<YH0jrN$GVTx(S55_yrXO%jL8p@&x3bZ zMuf-KJub!u7h~{-MG9nMq96e;vlJtA;cFc5Zz6rjH;#yISjuMG-N83nx>)gT(QLK; zdDOX~lN!3gV|m5bsmD(7)b?lo)b#Kjtcdyw^fbMresSbvWW6|zs;t){ZnPGEcFGH+ z<@24HXKKmb!FkmFRo_upS|gRB%j4B5rczH$-T41CxT73F-!xESVyKp7-BjtUThz}j z$D#ad0luGp5m%_s1RIxq_|%}eP&7*hSN%4_(^srQT|11CQ;{U;&-_FEJnTnZ5YEI` zpIxG;U`_PQr<L9LXf2Z1I}Lpgu0vOfy;)!N8Pu!BQXG9|8k@+7vO(L!N!hsocBD9z z5{#UVV~bSHap|AGq)B%fJdfK4ue{nwz=iv$>aQsO(ZL)P6_(20u<XP$wNr4=bldUs zAPrNU$MI8{BE&0+$JGve{3>=4R(slkYd$z(^YTx)ziA)LTjGiX{H(#wwv9FATcDvE zI?(!B3GbXa$Um^WlbU?(uK<0?^T0C-a+!kc`^Z190Cj)AM16eql9_mGCnd6R2*uAi zO@XQ+Yu|f>a+%=C_t+XhDRdhl@nx?mgROVj1(6%Tag!y#>r@A;yFm=faVOFVk!5|? z8n9k|;#9uoVz#wg$bT2niiQeBSS8<RCThYEKT-1&6=5T|O|5Kk#)~u*vAm-=(u#1$ zo(sj$D-UHTn>HJT&$YyVw`B_DRC<`zYQND2u~#UhXfN69C5CNQn-aMU3Lkx3&8Ad^ z@Ex!JVdRCG=<J_V%EQ18Yoz2-FXx`aOXF2Z(yY1E=-)k9dcF%KR&Gy<rF`*nuK{#K z@GTZ6&CSQ@nce70@^XApwvPmbi4o|nM3K%Tta5)KE9&7*eovTB`gR|M*H<5*lgq_G zC>n{Rlv$KBRE#eoCvdCRC(ZKWc$<zqN_*l?efaei4;+pL^DBW=*2=%gZgVUOnfZuX zP?pFlcP{~dS!J;Q_?j;WwszoN21Fw_hnHA>xiKXbW=LOF=tqw09<$E9fcAIgqPCwt z<AyU7>XztX|5I9A=l3dd_>v8KWq}6R-R(z{=vdrR`hZ#`R3Vlh9UynvPqy~S4P@Hm z%_c88hP&i1vms5d@I#+o{BOG`S$#+mEqM5aXC-z+K$H^{v7Y~BqVw1d9yvC%e)k&j z<0u=FbE6h}I!0lam>4|KZCw0*PK8ApkI1ve2uM153;#|vLdRP$+rPUU<W~O&Q9(-Z zdq@j9z4Ota>t0;0y@&d>Y!WhbOrthxc0v2`nRpgcO{!14C7o@CwAa4fl;zFsl@O!# zjq3VajiGxMh${U>2B~GZ{n`N{b)o?M+Ez;Bc?O)+`cPm`<`D7nMC6rOg-2t<F)x~j zo>rYBfi9V3s>4cBCa5RLGxEUojMe|i`RC3PuRF+zh&afN%X!_D|E9b6<70|KG4BZB zHOU0V#9}Jx50WK=BZraaT1Dcjs|LpEjpU)x8Q!tbnf&1Y9x@%7@9^p<Q4+ntmmQms zFC5xeM>(tu!b|4c<9p49)cWIVP~sjNa^z_;b_=Vf{wMJl4W50qbzI_a{XdC+$I?cS zbjJ_v8So&L&%9ByPapoZIRkaKTVdw~4M?<E6U+=n8EY+RCS;8V?v2+c#lxnw!?7|t zb&?j8&D+nJz4oKGZo5mzG1BzU!}IB<Vqef5!xosX?n2w@8#mlqahozh5hOxViN$Nq zbG<=-KqMm>A`RYf$19%;U{9U_9NQKl^wGUXQ5gd`P(BS^9(Cfr8@O@HKYqsZp2p)P zQay}hsuE0_bCs<fc!G9|gi`(7BebgW4EpeM5p58j%58et3iFhm9r6vI(l=i8L70yN zeQGR<yL?0o$$n=cA$koxFKeQM`=}|E{GUAd92Y=wq2LLDgX1s!n1@CG`9XzPId~*Y zB*DHF@N?s3cxCL2ma1Px2ez#zG6(F)i+z(o?O6hKdlL_wANjN2Cv~Evs5y8^Ljt-! zFAUbE{l;x-`^cHW7`jJu5%>8(1gBS<fRlfvbBinTNNiI+t>JIQ{VI^97uI+{r{L)m zF1}a=_!I8JI%|R_*`DFhM?~kkHDH5YU2cc^Nq8Qr2!bUs5K?l1T)rwto9>zc{wr2e zsX?EqEm~iZ6<-9_^+`ZPR5^~ADFVM6GVzp^`w6OyCnFoG@IZJq`WGWf=W3m&7u?@T zA6RyVK0cVo#hG@3;5v5;=Dw0{Xh`lMvwYTZx7B^<;k-}e7IzshEI137j)rohSL2B0 zE;-N&xXejus6uP!b2593NA~oL;g=^T(jC9=GjEJ$;3Jt$c&p!Yw({={Jklg&skfKV zR@cMG(xe*OUT(*?o-pX@HU~6)Z6?=xO`KC$#}m-^gQOuZHX4hhUPax0+6|5m_i(%7 zCFq%N&yk7Rmh_}0HRSMuoAAl!BzE_6W3?Z5p}kd~Ks(739{%_UyLR~ySkr^DyxqB{ zMqi1=Y(*^F*@aGx{Y1Qu5RlF2#24f+)qnI8o>*jtJ%Yrs-vMo^<&rO}UMSFo5pxdR z@t%do9!0^l@)`8sEujr@3hKnsGm8E)eJKdO#zR?<5gjz^Iw^dhLc604T&j~azOy+2 zOn=`2!M}N2WP2$&I{iC26(7p2$(l{%WNu-@w%6ou%P>0_mjnmAQqU9Qz2xxkK=Ntd zd8)JFE<0uRRGcOFa|4}O{}@>;ktHj849NCUd6ZrpgG!EFBVM}=IpfN8Tv41K-TP_- zcj#X`r@F5U?7lw0a+i#`EdFjTEMpYU_-zd78Y$ouxRd;9FC)u$uO?#Oh7rn11Jxi? zb{!qZT6xSS{X3<|r0J)K$T@HJ#m=i}exDO3aAS-)pZ1lgS@|4p2-7C7QstoIvJyxn zgdvY9s-W(d4a@F~q4qNeX%T@E6#h9*W;%xh#7%cd+Vhlr+yV5Qz!s4AE+O}9x{0sr zZF*6g2UoGHjr?*{Am&g>F17fP8}hHgXO|e=A14ViWi`-tEeqb{%L};JfKF=v3JLsL z`3?S2cM`%*Si_K2qy5TIJy>b*g1sR*%t!_;p}IcK#CCNEWnI+34<jU?(VCB*G@d8? zByCy=nKiTruO;MW7PlR8(5*w}WS#yg^7eKV%vo{|hn&}?JG{@q@ZM&`(`!e^OfP`x zh&{O$E6~EpR}-Q6?ONnjq)wK{6@gm&AeAoBkDJeHg3Fmj^y0rOXj-}#*`r$gl<}aL z$u;Qo4`VbC=!dUJi*Z@3HTU6B8QWv0046pmq(=5PvWeSB*DBU<W4s7DD0?b-o}mNt zqG}=MYZJ838ikn?-hz*Q72FaqQ#cp9SaLdS9j&mWAKE0u>5Kpt#sX&10rKjg)UOP? z9vX3;Q8~m-Bb4WKsf$}zdY4n$X~ODl?n3h`n$Vrxb|g9|KOQIC#yey;;+?g8B&GNo zH?>UX>V&U|xoH9ErH_+XIZZCetO(yUzf4wKtYJK>V?a<jzl+t>Y~@BSD$~ye2kD|* zA1>d19Vh2vNMypxiD|zN9J#q0F3xm7o-1jh5-&wNSD16N+a8cRSuLz?+bq1(V+L_c z{DA(Pzrh9zd+@WX&(Lx(XQ#f;;R&8z!_<#4Jmp~{yZDC={kfhd={u~*m#z`obKV+3 zgMyPQokrENhx`U1cy|jIDfJt+$seb0%7oHOa}?;OhtI>{s5t34`38P1G6z-PMaVdR z8<Mo|fi^!(#PJ1~|A@geo)m-BBO?&;pTH?3sc?>8zTibwzfr$K34ZjjfqfpJ3OW7V zlz>d4A52Ms;<|~Xj|hH{tLz!h=khmf?)rkZG`8XtoK(3GK@>4eU%=T8t8s1`$I0xW zWKP#%qJ!7uQ0S~Jq1Kz25oT#U?QJ2!9euu(d=G7=H&>^PPsRyo33CrNz11Nbqm}TS zS9z%KRyg|DFHb$1J%V&kd`7Js^U&d6_wjI2D|Qm>t3yA$l+Zk<c>adyt(3=I1M**3 z9A>Xa(JpSq4tkq!qWf<c*49Y`1$W;g;m$NI+q{@~-s96L84{fAPz?Mkb|wvfRuS`N z3EKIvGK{Yr%G19Z%wxs5&F4;F>2sn)<)k<ycAX%_*E-M?iDta_y&-(nHY0*}+pW>{ zSITf#`vX3fxC`0r7(pst_i)t1^LTEd26$Gfkg!%EH0k-0>+}DkbK+(@<Z(50ezOWO zw-X2byA!#>4eq2x<{ix6Rt;i_k>I{R3o_kH@D;;TuuR68JYUs^J}Fc5%Iz;eYGW|@ z9Wa><$g(7Hk<kLusc!)nJtFDDR=)Jv7zb=0@E4CH{Kgt@6EN!|WX~L~LmoD@uyXZf zI6g9j@0RqE2PVaE_~8tYkdAgR7<fmVa+Dg1xMDJ=rwu;YSpgS+o1o!A?5SD;BR5ou zD^}sEi`5{GKZDHpR0_&-+p*i~?_|4@AQrj?p26rw9yj=Q7kB%uIrsSB5c+;w6vuX7 z#%ji6_?rJMEIfJ=D@jYj^zIe7E&V#K@f#U;SyF%pW|Q$?k=wNH6LFuDg)XR8(~5m3 zxaxifTJN6=S74>a{pR_jhu;{eFzV;56nbfy&g-CY@euiG>qCYGQH6AMq$!up)RJQF zFUWCW9kG*aqn_ULrSoNd_|c=!u^``+Z1TqJm{lV+=ePz={4PaSt4pH&W~01XOI0#4 z_$@wvF^2kj=oVVNyO7`KtVg>W=Hj-JI8a~vmYZ7IOn0n#$$48vfYrbIsI2%ZWX^Ko zX1{zu3nt|&asLKq;O?VmNX^sLq;<SD-AmiiJx?BkOY8~4ZKmnYy-(qhLk^iAZOa<J z<73bAd8nJF*z}F3@qTANmTGjx*XK?_dk3DPpcMsp!_5qo)+tHO23)|4B%4r2kpWje zluk+##_-g+i#eV|H~n~T4V`n)K|nTdD+itCG7{z*1vWpk>6(a3+=4%;<Z`eNMT9w6 zS49kaIJZFArXBG5R3+#iyAP@IAJAL(9&*885^On8jWjI=kj|y=L|ny$8z|_*3oT8t zZLSbnf9695vjxrG7RO(qA>c-iZsZQ^pN6!5BBFfm2h=zU44~zJDkU763s&`#^q%Bg zR%b^MWcgHpa@+zu^PMiYaY_Pb>V%1~M}}-!{)82=SH!z|jqvMJ-(XSIKW1dRsqkRu z0G@GgI#)2Snbe5sk%(EfctCsu+$H&N=ClQR>?6Pi{*TbCM_!a_Sv-1Iwi&N`r$JbO z<{t7>Zwh~&wFKlnTS8~KO`_ZC{?Nw1O&Y?|;%MHsrF8q(1Uj%s3KTNl5{bAy+zgS) zP(@9lKUg^ter*g=P8a7)+IVzy;&O=6|AY<GQc(B93}8!SY5ygAi2tkqkoqSr!nCBo z_P%jj=xBs2+n=*Pwe+yS);9u6T?)nvzZKwncXN^3xzG53?r*mGL=4w=N{*8jvmzDF z94GqPkK{=ia7{7aIn9Gxx$So^5Vtpd_1#ZRxV_Jp&^eQeVNU2D_J*-8{2Lo$_rE(z z?KIhmcxhY7Q@d3}{e?U^Y4ig}^fitfqz6duKm=dSzEFTW)DS2Q8smL;(s9L6N7hE- zKjGAsD)6rVIF|Uf1pltPgQv%v5&q_dBxKqt?rf|!QtFm&D74xP;!V%!>n&bHj&kNC z=_}l(lL~ay=VB6Y57Sn)8GxSL!RzOpCQtl3;iiQhggxv*HX81rv00g;okX~#KtT|^ z`5{Z0Sx_ivkdJS984`!FPEvR8IqI4#i9C(Buu2Xm(1Kk-*!yrZ_V|-ab{}6s-YAD+ zRo+gnr6P=Wd1V5jb%k_s(R3Pz)X=4?DmicIXnOj$0n%5H2EWw}xkTMaj<!_eoIYoh zbV`Lg{V9#SZ54rF!Eypd%0A=TeF11ky~E$lOKaxLKZM#MzGJ~aI(Ax>3=dZ=f`bcv z@lESNWT~hCFWiLK?zsoz+AdHFY*;iLD+$E@0bZ?|2tty>8Ar83m+Yj5rS^X*skJ$@ z_Om9~zxXld*jqs4&t4;+ou_d9Nt;QEx&kK{HV>dS*ZabqmRz!a;$kF`T*u}Yj(ht@ z=lNIO&j;@^9=*?XE8J67A|gw_pmn#mkQ|qAGT$N_)huPv(syp)u`M0UhdXg)kNfy` zZO0vI-p;+9xRffdyA95-HMy|q?~q*92hNtdPD=Xp>BF`CP`XBq3vs(90Ew>>WWiTQ zI?+YiAv|1yo+Tv#`!koo@Xxt~!ZnnFgDtA<lO+fB?&8tkqvTiQZD=sMf<~^~;ve%H zuh7k#iQ1(k(5A*4xU?%8k7k=7?WqPNWzt9feD!qn?NKdi?~*}#%+8=YM~!KZ$Ir+= z^(>-&jB(J96J*k<t{iPxp-vkol#?$dzG!XgeEP?#ySS&Jhh7;a!%A5nA`UL`P;^R# z`)Pg!20fD)>DYAcUz-K^&zlGVNqHbQIR*WC{Tg;l=An0gLZIY$H$3;01llPa7c!;T zel;I=+m_%!tPA!pM98Y1aAM}4z@8PXT*{R%I}iVDljQ2o>N>n%c9y=sW01QN;LlCw zE)X>L9OtJ$og0-hCVRUskUzGQ;k#2dIP<nq6N_4L*N8ZG+w(sX{Oth|TPMy<Jz&V0 z?$`_`l*U!gnUgTnSRB2yTMtiR1!yl*g#WZg(c9oFXgI`yOm4Tvg6Aq{(cF2a<b(f1 z_WjQrNF-gF!@SG1*-?KY797y<`TWF&v}#{&+QJQ_`j<Aj>@Eq{LwX>4ZU!4ur`7OP zw2izH?<Mc3a?%sMlG`DF5Uq6xhMPp0GdW@ikKSrSwO=B+^nC*TXZd!d)0u_bZUQ~_ zc?^=4X$!FU%XfIa^)0OTIRb5;5Q*A@FQSXaqMY|1Az72BKn8WMl3g#;Id$vFaI2}4 zUXp0f*&Y=kywC3B(wt3PYr!1wnypM8{OTv~t>hgnN{w-I@=<7u>R>*S^E5IFBF%yY z)SHb>^oMRkZjx;-`Jl9cNUXU62N~sw<bQP$zlL88FO@|^UMl}zIc5TFBe?d?0_V@z z3{S+Su!%ofQOW)#s5&%*ebnNPO}dnD&8q>tcY;48jLgD^mt8|rQs>af8y(Cydd7|% zdxsxMPsWRi4M^IbCR~9^V8+5U1QwZOtqH}xsoDV2ncq<gs)inO1z6!afdm$R#1e<4 z{;xWuvgU}Wa!Ld3AkbXm2~YDj605v;xH&m-d@tWYJABZF6oCr2=kW%zS@t~4_q|7) zw7$dHo^kZ=)h`?;cb<%8^}_C~c5p1LfPED{ple+ZUHY@Q)ycNpg$#;g&D~*OD@{KY z9)_Qroar4WzCpXaGChC!e)3)6F8UJeLYHS96o8X?Ehzhk;qseWv?RI&NdrT0u(62v z+a833lMjIX6brZi<dghsbugH=oXh@a!#Qi5BtQ1+koOrL^vfx7+^on2^gg<iTvvNZ zvUI{=>D_LkY2Z$G>t}O+*M)Omg)`~ODs5!7S%wZQyi0!>Dd3(>oKD|;CfEXk$faDp z7DZ}5Ie}MZ4ZI&4CVa7J95Y9at6v$#I#uM+gKvM37r)i$*!F$g&lAhI=_(~qn3+b` zzk7-OR|Rr&j3yJgDHlk9H^n{LW5R{}4xz)9-=ihN?&#RLVB~qQg!VYNiGSt%cj0`; zkM+Ug9<~AB57k!*65WM;6BLC<cAn)KN$g@i&&jAwL@(`c8E#_&pGNRs$4=$z{xRU6 zJ_y1q>iRsvq<2gp=gmhRMZCtHUd${*C#H3!BJ+E(62tD6;y<LL8UB+3yC`NMf32#9 zJ&_3IOSl8G`Oz2qz}N-CPj;a~)i4>p`lJ%R%cg#w;MgfU-Y<#Ee5n~({BB!G#=Y#k zkao`yUU~MDcRiw-S#GeI$HgZykGrz@bKCrxLy=qfy7K4w4`&AOuhw1RXOVb*)qE$$ z{L>(B!*wiNHA7W6>9Hi^oz%kADcSSh*UsmU+*ai4SN1V07PJWGR8{cn9dGj%<^iKF zS|||um(6C>LjKhUXCi*~zXMF|I(_Qf@fX6Eo-w?YYoqygykpe+>`wcJdvp06YF~Iq z57<#21NF>Ity}fm6I^++S2pmhm&GyH$Pg3eCCkwJrG+@;IKSfj72d>4i02b9slHf7 zhI!B1!;cZT@OVkG%qBZOzP9>)UfFzsi10;;EI+IMiG844Roxt|@4_`L3Uv{W(wMoY zCsF<CG0goCK2JQnkC(Qzhxhq<5l_9miRWJ9XfN*6XaC=GXTJE)Swg$VGwr*?Q+d9- z<oNFwtMc7`sWHcCXXd`CD&L@7iMbpwkEa=Qlz-c18{;*jm-qfy0k2?-k&qS)N7sMt zkzrbV>zKEO(|B%HpZMJmY+3b#3wXp}uKl9XFlP3I3})%ZZ2PH$!HmUY1HMdO74yQa zlF2;b#P_-;&U@w>$=9(}<rj5ZGZPP+Qh#e}?Sq%ZGN*EOF`w#tnE{t+%$V~Oo{5o{ z(Dw3r-n~mAY>SN+6JZp^7bGsxr=A6^5(aM1WLBX6_^-Fm;aks+<!4{m&0PQ8&)>PH zg|BCm!4$Rs$Lwi{vv*go<jW1UGCLklV=A70<Q01;V!z~@_H){d>2JHc*iS}3v9hHx zbZmS^ewAJ!U4N=bZgC-5`^bbo_UI7Nc-e|C_D!Uf?Hiylpj7~++b?6=g{x_sWld=3 zsu^6%%-LM_t~k0{U!0VN$<m>leBtAK2fAbGY|=Q+B;-$gN-k_Mf|Sl+diAc2%zx># zU`NSc^8I!;Ip=eZmcFXMwI>hZOF#eOg;ulZE7OwT$jUCZb@c)|ciDOJzI6kUts%sQ zx<G>d3Vg8L%hO=HGK_Y3FURGjPo+<JoS?IA=%M-Z+-dEYvz&S33F2PygG4_Ur}wS6 z2Zb$!3!fCt?b+hb{XP^z{%#PX-$q>IwyH|eFFdEgpG~vIqro>|cWMtkQJbQFdbg3M zT7OCCDHf8xW`pe!8&o~TjqCfU%bgTt&4<jbGeLIyZ2DVaCNvjjl3E!v+90V3j-KVy z$l)QlE6UOZU;4NYsm|<rX>Z{^_x*g!hbP#fmptnC-4j&9wHE5fA6?4-)(P->vWclV z?L@svJAr#PEUCX#s7{LY^$Arhj`PY+vl@XH(Ul)6=!Ug7`@moVD>$Bag3a815?$k2 zQ?Wkh*<LXrg`{(^Qkx28+y97B-{Zu7yL3g^xFH;8a{8=>WE7Sv^`xd9`++C8O(N@? zlqkkaf;hTbv+U$ol#SCv9JGH560JQzn)Icq4`S|E?prHvJ=THDR<Fc+7g*zBRZLmz z+rdg@$O^ztZxl0Sm#}@XB5Kh-$Ib}uKu*&$8G7GZoO5C_>XuO)2P@;SPE!mDT=tc! zStSO|$8+Jia5BC>F9JUA_)6u@tifR=osg7Tf*oAz;V>_f<SBhXJf~9#^=%`m&0~1u zo>p`$z#CulmZ1WzYKWL&4CE&ZmO=Sg9u{_KVP{L4Og1c_rib1~as>|ba{0Sdmr^ts z3?D^SUVD&6cQhRGm4-H#IXHXa8_Lf2IM`$y$4}O2l4?Cm`sQ0NESlkoRR<nYIaA_^ z<h9A9NZ}#YfksBY*PFZunM7p`uOp$sbKrB^CY0M7iB1akbx_9#C9&t6c9fnaMu(qI zzz+hnsAnIW5N!7c%j!?;?$6U9DbF9c;SMnLl!5DaMCi+*fI}VQNPggb(zkd8WH$Ao z)Ut<IK^P0)ZapS{{=<0Rhj8-9brCT+KAU;cynwvYdPy0@PNNhm9Eij&16G<ogaxrD zqVZa@Es*vjgG|hPgEq=FlOw-AkVN4b>Qh@A-}`PY4zGVf<^P=sKi(>F6+6bc#%J9i zXBi8FfsN$NsWs%~mSbS3+JK%EsK8n`8DQEb(?<96K>u1TUel!wAxB@Zb3M-BlS3tN z?Px4Ex)%sjSIh&=khub`ce^7koTZGTHFto~1WCx*xt_ZCbs?B9jKrsSag=G~5n@PB z1obd&zV+(4c<uD5^qYp;czKu+(eueijms+7vNU(pw77zAV>HZ)F6I%d|3X>2Y-OT4 z^(_@;>4n-<OVG*a0DQ$R0`1Jdgmzd<(GHjPviZ^iBTCuB5k*Xi#ZMPc!@YXm=wHZA zY||@EXOHc{rk*nBcGx)T_j?|hw>}U$9+#ua12W{8Y!JYBrQ^QJIAQZR5?x)K0r}Tu zkh`fCUVG0I1Pc@JX159CfQSy%1Zz?yZzJJh<sCj=7e-_yDoBSJN1^+rxJKZ=5dKXK zr-}^+*=SW!&<)#8Oxnf~&3*A;uO-4Z-{a8pyrmp{DG^;v-h{H4QgXthA3c#y!E#TH z$>Cq-oXWnxC}nj4^1VL5kN>X*mu{6s+F8j&uZSZH8lI5RMUzm4$9m|Lo`%Xky=Lle zox#TC$8f<bYrcSbY6+Xxr?6W$G1LNy%j}2Ft?XIxpF-i^Bg{mfN@};+PBP7M^0-q@ zVdEcZp#9TUB5m<_Bt9pcm3|e4PMHVt{r>3!e`_c;u;K=#J3W%(_0?dDg$6iC&zvQz z44JZBt^B*Xe6;X>GjF+MCbjIG80sw&2ZiFV0xZcmB0t`ASav}Y>HHPvuXsKm2c}D4 z+w->gz)E$r#b*ItSA2o-@BDx=_ZX0z0Uxxx!;-pitcx8Adx`dN+34;vJv4RBKJ3YE z#9~Wdvo>?2ak<4N-chwMB&}D1JDon_dEF22E=v<^JvfiN%-4YM(t~*Eob9sQ!&?XN z`otRc*x4I!r13J!$*-Xr)z*`fJD;O#8M85Zd7K;={)hI?`hw$ju0czx$`LN#OiJ7C z<NRr%$ZZXS_6Hvk=Bu{ze{L>9%?UE-f0F;}wqf`GUjn<^$;Cf5;V`*o@(0gwEdbSq zHr^%q7s5kMIn3ep$@LAbQ9O~$)0hKxgOD`k28adUhH{}H<s5SgtuTFn=RA7H^R7LO zW^E|IYkxMfruQp_7p*p;ujiIgmG|W8t*o{FuiJrgmBasE0$b&{+-9=iy<a1T(C=}} zoJi|l>7g~$9JrRMX<Vq;JX&7Al^!j54jww@4Tn9K(pAEnv{{@xJ*+T~>w58*tn*Lc zYL-TG9W8xi>n2_L@R<|zZJAqeGGi(iFxP;5_0Z&23?HJ`buWViK@`_jH;G=pU=h9f zv>5HaR*arLr&GY)O7EcEdNKWIRM+9DVimnwIfGV>DMae7E9haLi!?QB370o9jXZvJ zn@$SW;$joT8sdWVInh;PT#d3BZGNDb^L1jmU*9Iuv#A+evy2Sa6ZDG6@d+^(Kgcb= z`-uDBFaMHuIq3Cx`ux9VwUfAnh=`1gh)BwD-}&0+H?EK{r~Ta7;q?D&T7Tb1Nq)SU z{CH5UpEByZRN&tDoB!sSEUB9pf!^$tCxJV4iOXFBras7n9c%lFYKDhVYp^_ds-{Nf z^!|stZ3>{s>lWD|@(?E7?xG*mt))9X#2pm>SkRNoWx22g0ql9t&$vnN1lD<#iH-QX ziS5_lRO<RRwBYe~6tkuqzfG-23v4F92MtDmMd={C&!qq-#Py-G8za$!n|g5fQ8@k{ z6^5qImL?g#j(EVWgMIqP45g}7;G4bGXvE2mo%Cz~3v^bX2WAbBwJnwYFm{b{oov>? zD_BEI8W}sR96Sc2s}1R$cG}z_gYz_eil?1o7B!sfP9>Se*I>;xLU>7nQ1Bi-OXc`1 z;U{VA2klc6L4Rfzsx4@uR<{2E&zxj(vF;9<lSiYhjy*X0p*NEFF_k#={HCU8O(y<5 zDsXA08hY^iEh1~g;Hsw?da&;r{ppz)=kn5#R<WN=&yZGa@NyX-67!R2S-S*$#BUKj zHPD{Rz5ARz7xatLxYmrkn05+dWnTyv)r2<*BdBS>jwD*1Lx*h)@xyvOIGI{R?b!H? zwH2D9DGEvipHL)%m|ys}{xBAgZbe?ZJy7`-bsQw@!nrO@i1&6pYaDpWe%O|b@-!}h zaoKe)Nb4LueCdqCH2*{**W%3m&>2rzJB+0ouIbp&x345aRPO-XUcZM^3p_`%$Mm?5 z_OZmPWQbf%wxQLdHWROX0#cxm4=Q6L#PW&+=eNL<iw;bO8~^EotmPS;JM$^Z4$%YA zw&^%w^bejc`WMNMGXW~EQjp1HQ8uH&6bUQ{x;smht23Mp8a*8Xq`Wnz;bEFR-CyEE z*xS-vt&tWzZAm5EUu^}+S7o5v=?R)?Oq0YzE;u~!0@m?K#68nZ!J+gQF0-2m;ro)< z%&dAU<;qL6^@I#KiMr#vCLi#3DJkUdPLs0E8LZ^MYu4kIEV;5f8~Iu56PvYvg!g@v ziOykx2JzOKhW}l=1F7#%k;)SW+zdAtxQwScxGt9>`Q<wFxyuCa4SGyu<F`;3Ble&r z_x8iuOGin>F<Y=*w~8zbO~hR@yvfJRODKQ16YXE8M0G`Elk2-L;iy&>nBP4B6MAcq z`dMf6tMCF|V(|)Za`=mQGWPhU;QkmAN@<XP3e$+b=`u=j*DmZQ(L~9e`ilQL`f^M7 zX3$7SIy6NsrBhF9a)u7$RL%BNWJik}_pAOP8M#;u3)iZ{A%!mJ-7+6S*MyOE?XM}@ zYL?}GG=b8!bXa=e1e$Ev3=bme;q<!aNM}Hs8}e<y?df(zu>Us37U|;1_vK`~&qftI ze#n!>9b1w0-p=tWASGzgnOuC%H=0_XXb!g~y#%UsHU3=2YnUlmPu}L7Bg((!>A|o# zVC76<@wO=ZVQv{j8+DQU)CzKj_X`zfo+i6L%Mu$iE6ABFf{xJ*$gf=!E{a&;rvgsE z#I<h+nR&~oeNNrjWgB8mcbUO+dtJOb?i^Nj)5Bk9{y~2dKeJ)Ji!llPhFCXse)?fe zvZs9l)bvg#Pk($yg<q^1M2!^bGkfiEM3oDNW_=~rUm}obg9DN(N(a&+MUQ+8L~GaW zA+xT_V4ZWjNxiBB+5S=Rl0Bvv&Qj|e*c)Cce33L&5`WVeE{A!M(Ea1WL{t-1c^|{M zuM6<Rp-^m4uLviub|BjtEqwaTHmajWiOgSq3hTX@P1^Ivt(chdcy?HetG9}Pwd(@< zQpv;yhph8NG4cnhyPHA=e3p|3|K4D+`GMSu%d_FMV0J40*SH<8TX7!QOm9ZpLJz_v z`CM%E$_#q1SV3Kj8q!X_LJ~Iik=F$rKBPPmCqGR?x>B3)wz=8_ep%y|=4!lGA`eA& z=TKL>3>k|wb%-C20IVDnNY<|jAi7}(SXi9qmj9<oSE-ePjQVDDWA#BeZO<Zs(vG!= zRs70G2FlT1S{g)ie+B9}AA=S&Q>4mCmi!AoiInQkl1<H5s197hj&ez4mBedsShj_j zAJ>8WU9)g<?s62o;6J?Zu?F6#{1cx(@EudT=AvL}dHms?0)!3Nq4Zm2c%irwouMno zDK6N|6-^Iqu)QEBphd<p?HjBJ45XeVEqNKt`JtWMRlOR@a=$Tm>i!yZ&ip1Nck(3C ztO!IhV%MnAResQ8tp{2!{P3O~(}<Lp0uj2}qw7yfv8~T|0@YGVeZB961yY{qdHZ2z zDL)o9FV`ft?Q$?lZ6z|3+KOz#$3JYf2pVt{G^4guV=~fTgEKM|8hZa~J9t|5(}5>4 zz-(1BMoW|k+4zUi@+WXb|1c@<jzM$mG-0r(8NVsbfxwjOjF#nXoOWgsZjXA8B0_R; z(HVI(@;Q{4id;Z?DGN}QK@lR;XW#?5pOJ5C6i%q0OT|`g$7R-&(H4PdKGr<<fC`Ow zgg2fv9+@_Zc0n(x6nbILp$!fadK<a7sugtil-tC&RhH;%okMP1JP0f1QDA&amrE`^ zjy{hphZm1^h)dfmc>Fhu*!(>WXI;ARmEsCKvhE-75;W+N`>TnrvJ32a{|8CLRZ*o+ z7UP2@U08r8)}Yq?<M^ho0((halC+QGp_U(|sZ)N(5Z-4-ZggB=zw{<U#Zpyr%<&{= zb|8ozq^fBBA`>on)dc!Za5;8dsfbpI>Jrr|YnY~%iL!PrVdrgcWvaX!$@Q1nc#g?( zw(5>QkqXfSt*?nl|ELW9_<lb+d|FIE=2Xj(WCK$czvZYa0aa+t%fFPTg*uFyjiLL| zlVM?!4v9{Ghj$$4L#~}Ae4Rt-?E30trfp{ww(2US7p#_|Cd{p(cSq<$o#+!{bSWI- zyATRlr$$aWY-jsbUQl85UcyTig`=Sj{AFjX@aQ;eVqqAMww)Ga)t?TK!PkdbR5TuL zZ+AP11O6D|iHEMDPcbu5=pSvI<k5x>)wJMM9;v9WMH2YK6Nu=SSLoVCKYVueEb8qV zdCtl(4vCN$T30E%;hZ|h>EFD=r7ov9`h^94xbY}xkNR*E3Y*AYs*tEE+QA-^IQ&CG z68#h0F(*TFcM{d}%}CU&1Fa9(O02~!*_LEKVt4oi_y#$V(+bnTKVSl`)c3|~t_4ur zKRrQ`*ENXo_?3VoYm|t|>E~Ex!AdkTVH{ZdK98Z|wj;yEm0ZQwOnUTm9%o_j72*x9 z(=zpwVV#&2xA{vc;0+eAW6t<-{NZ{abT!Wf1-z78X<P+Tzt$5g<|w)yP=*CJEpQf< z!I~MYLqWyXP%Uu;G5!^}F-?{jg^W^OPhO&b<Mu6O+#iB}9v&;#f_4&&zm({r+N`PQ zMB8P|+|&iJAEMxy;X-UiV+n7sy@UPp|G1kL6z!gMLO}G5lh}FmcjB#LPwto9hR7LZ zptF7<KH&bI61na}_SL+_t3slLUxMyXpI?+xr*;VuZXu*hT$!w!--K`dR>6WZUzvNB z#&}ToI}Sb0Fq3x#;f+a0u*2|K^#7VT^Jp%<KmHS?Y$=jbB9*cwA$iYzT`ek7(yl!% zQd)^9-eieF@wSITWi6#h-gED~D<MU;?4cs5k5tl*-@ETQzu)=&_xfYb%$zwhckX#E z5A6jaGWyD5a%IOsp6YKQ+wf#MJsataV~g+NPfh&o_-c`eE8DOEtAy5q+qm7>vep@! zVhLP!d@AKi??K;~Xlz;YlRda;AB?`^%pE$Zh2^?Sg_F+~!?!Cn5c-hEk;VuVaC<%e zP^!ii1YCxYWisS%#$xg^TMqJf_A+g~P3+#gmzf2DQq+E7FqzM(1I=${w)5|*fN$Cj zBGOEw<8Mvj3QuLy1~2iZqahn_KCz88t|^3yCEK{J$`Iy#gdwfCfg$MbYfv!Pf{<_G z6ICyxaZyq<Db|ggpQBD;;}UdTLcH1y%F|FwZK``vpFFZkW~xtwp{Yk7Anp1Y^p7Zq zC_8G<R9zL||ISy2piv)@!K*4X_RDn?uwpbQe!5S7=j=lX-yLDn-a_!ZoW+?9)ZnJa z%ea;QxzR<>>(TFjTlcf$!DfpD>^4*Y6W?NSzkMRpfBYQ@UUi>1pPfYBE!c>R_HL#F zr_A8EP?3BO@Zy=pog`!D$B>atHoW*-ZG2J~nucyV{v@we6~X?K3;C!w6?|O=SxzI5 zJQY1BsvRpx_Xq)+Kkgisc&#kbnLLJT%9atCEH1}d`<8HaMU}WufpW2W%ka#$UOLg@ z7szI}!Gd|e=-u)Dbnyf^n8Eu6U6;;*`C%tc+pL@XIO0xwW6Eed@Ds=cW(Sv`8iG&i zB%vOo_vGfYaugCY7R+5-ElRAfp#&*SDzi5YUF$ZcWux9A=jI@^cexD$w2O1ED(C#X zF*ik80{iw=!Yi}&INMv1yPN!lemp(`Uzw1_6?z`wqT0XWY{@`slW>(rHDKgg<3w^! zrJ@1;UKMI(E6<fnbV7*B3u-d9iljezNae34LzlpmJy*hmt^0nFppL~P!K4V;-cX?7 zO07(feGAd}xD8ofoXq=u+=m#R(IFGEJ#pof8ZK<Y2YPSOI+2F(0-Qc#i&StL);^rd zE&Za##hmoUrA3*ThRazp4tFTu)c-!4)A^1b$kK#1=08-r$AMZs|3Oei1|-&1(^Ff2 zvM*L0A=`Lo;j5cCN3Jo7`YFdDJnAVsXmCf^Q69w$^xi@O$5oM>|7H`5q6zflgCORG zTLvjNiJ~*V-k>c_Lh9QYBYJtlRHS|-jh>&aN?+)20>e)|e41UYgNrk6(e$mURFqPV z)>-wk9){1D`F=I1-vJ@d1z%87)*vbTcm^$Pbb+EAYbdw5No#eo$m;<u&`woiU!UKA z)Zbn~zB?n3QpX{bl$?W_#eEh@d5VI?SctJ>m(l35UNmc31Fdh<u>4gp0yKFmxJ~_h zWciW$iB9)kBIC=CTlSok<P2{s(UWUh&~k~-biwRWdcCI@0y;Zr`hunSvw}H(cJeD6 z$Toq}w(C&3D+arS=R)Cg9v4&|&8f61Q`1EW>~2L}ke+;%xevBv$m<ZfT6f~XT$liH z&u_9rLJ!m|8$wbgFOxj}^=uk)?Ker@W5aE5D&<y&`Oqs%XIn}bSwTrbF_bR9NalpC zLS*JSE^vdFD6#WA6iHtOE-`^977P%z(}Bc)$Q!<$^+2W8w&c<?S;+iw00qYl5o`b3 z=vwu5M&0c>8e1+68a^q=Us(lCh@wb*oEy7AAsF#r8F#UHNlH|C`YiftNe%0rHJ2{h zsDRra$I)-*L!83r7hLujKBpd*E%f!6jCYFhAgj{;BO8kyxZuZGFnDYudov=HI5^s| zYxaB5vUX?K^t1_7m+hxo2J(Q9*up8UnYb=Yr*B*4qxgdf1fCAFk~)P%Ds_zb|8G7U zdNw^GMt{Z;o|6*wPJc(7b$6iJ08L0Qj$m9;*3)0Q9%$K1Gy1`L3|Ch%O_cboiyP!w zagEuESY~jL%_0}5{^fQCpO}ngYH!etueZQ6{4PjEi??X>965{xq({}C`>dmnhn=5+ z)57^!I@^@no33Q{zmD!N_rL}J&#2ZA*m|4rlUoXfZ&Di>Hy=NAqi`nIHtQ!(=T9T^ z#r!F-_7j;MvZV~@g|Js=vqd5j!H!#|%@#IRvQ?Sh!dpj23Drj3W}=D$(T(QM%tB!q zP4ibp9d&msY{m@HH4m+*aNPxV)|$`4O=C^4`|xObC6Y3IKKbOIqH{JSR<a?|$C7{Z zWjmog#=OB9Q@9=FJJFGVbMQDyf&H;BVd2-J&#0p?6Kr#0iLBfdNR14mCZ9e)W9(5l zD3Yg}SDvSDjf#m!nj9#m&OsJ!7G%g`2J{Hjpd$a3(B^$I#K#NiTkA9E%<EpJVA(CQ zJnjl@l8+?9+7MFxP8=iV(I)2ArgO;6%@keyq66;xq-bbf2Kbi+qB8a`RP3{-pB?Ap zzY=5d3#7%iU(6xd3v-z<v*zOvFop9`O7uwO3aU3O0YU07N$5&jy8Gc4TGIRuJWhpz zqii57PKttlGbL{KLNSBkY$I8{Bp&w#|D^!}{$VcRVJ{JmorRqjgwj*8hv4&~t1#iM zDJd8~2UK)rvCOTV5LCl(Q~m_QPj^2ixj!FL1|z6c*1xXgY;4_Q0zMAL=sQ0Y>v`Ux z$0pooOwH@a$afzge!Ugcmz%;~ZwKmfU#$8lUL+HmW^>Ac&)H?i4d4ZT&<u5n8(k{F zaxne1CUox`fXy-ktWWH2^ku*Jb3Wuvb2gTf`!W{9hA1*2Ym9;C69}>UCZexvYT?4S z0C4Ha7Mhl{AofKcP5hF?9^R*drtod(mJhA8yDJzjZoWwR|171U5_;r)R}*qg%>mu8 z0#bfW3-ilP%Hs!R5%kOlIebSkm|oBtiQDS(AU7c!$<DJCtM8R;%DM&kO!^UOH*Yl8 z5jlZ+EU%}5p2~1MJqBJn?Io<TIq0GS2-;Cc`)_rSU3%NmbNk<fU*`Z@WPrvo*7VO^ z9uDe{qyMed#0r0}!%lZqdivNMKE3<$J&~SVN}mYtf!C2Nn6hCWKGi~r*?uK*x@Q+> z8TOMd8~O|FPR3l?f_~)2Nbs0%Eoki@X{=JIjJHWm;`02*W9<TaPW!bnO<KBwdumxp z_#^t+n5_BS%1jklUX}=s=iZPn=8ecaaRq&Rbv~Fabf;z3{9z=JJ3%3L4M=rpKzySQ zMBVCTbe8m>*50XT_+&qEcDcb0Y7V2y@J=*fr;I{X_d(*43PPseAd7yDhL-4Fl(DOt zIvY7KUrojKMXxdPiQZ41M05}z4^40mDPkonO}Hldm0h9Z3)i37P^<MP=(mV)DCL)# zL$RwOu2pR%X@28i!>3BLLT43@HJw3sNad5()pKBtSf#!s&Cz*v8DP;l8L#1eg+)mh zpk(7)c&q<_%D&h|2i2?S--3QPuxt)Z-W|;8|MY?tb)Ql72XXaz<5a5p%UxWYInNVb zw;}(jr@`IB{`7Kp3+2x^kjQJ!bRjYRqwwx;@w7JKHd=LUJg&K+4hIt@n3Qv?>1*K< z+NMxLHM2aik@9M8RQN_L=jnt;mYLzT^KTI)%SmLpSohw{`@&>OE`-Rtr_lz3eQ+T& zkXS8<2Ht7{$8H3$M$Pi%?U5JAKdFvt6|Q7*^|tV#Z?Qe(4XMGJ$X6(E^nG+nS^&Qq zD@mC^4b`j1BTIugniM#N{`y#fB)xZ#mv5?QcglS@Se}AVza5mnxlE?XX0u8z6wXH3 zQ>}tgsO<V9*ynwVxtsZxq_1U!-bDuF*}W3jrD2S(sMvw^gOliUa5yhGjh_j3i(6>? za1nX!G=?j9;DkckRN=?ECU6)V0~0@}VcExdRA=32i(P)DsIg=mxARgqP4|xAD!0WW zm)`N%uPl$AnJR+*ykuAvrbB*X5j{3J7vWD8RAQqZqkvYBsF!{?YN{LwPF8;KFnJnh zl3Gk5O#s<^Z5wV^Z#FY)P8wx)M{%2E6mh=OO;nOU6?f^*=Num|p_N=bH0)M}_fA(y zoUJ8$be#%X-Efo1*S>?|m0qEdWhX3}7b`LcqODPz*BnwP6^uM2?;_Xb4#?|JE32d0 zPELBcl9ktr(HU(q;OT-U>5>C6ZDTy)TaFxs!q#3DCVnd@JoIQOV_{?u+hp>X(`8$E z@~*n%{ToGQPpvBX>Ny9EY71jhPaBiyrwz=|a}}2QW-&#+m&wu9U(sj{HMZ!cfT(NE zgjdxiM6dEZIS}(6;r=y9;zBp6o1RR@e3z!5jZ~46-xl#ruR}{NRq+{HO($mPwlta< z+bHzxdCFcjkV2y3KTLJvab`)i7nxKOMb6g0L7mUVF|2%5diTy~65=z&p1h+1e>T;S z7Z28xw(&gl)9WNvQYvS+JeP*kt^#pP@CT`C+6U`plaPGVeQ0(HB-wkNsaj1V`~3C- zB6+5X#rydQ!i#HOq8@8yROIQ-YJPo8Bp&3kt>T^fod6kOmVypb|Hp^PT-}0>3L=>k zL3$Q;A4f75=BuMFQ#l&3R}+0<y3x|I<>2+$SjaongmN_>2rW`W&>5+x=;;MTp2yf+ zi{;Kgn24y4>?Wg8Y{T`(MKY`=ljf}ld_GEK9)${+=OsTGN$m|hd|iq4PT9pA)YU_A z-|sT6sk5op-8^(7T84anS3^wqZAa@KePblsD%tN#6G&XQI*~Q2XR_xvkl~tNL}!aU zauPg4e>Of8etl)h>pHxICOk8O4nqgF%E|!kw!Lj3|6dsixov}DK7ZixH?_8~^EK9z zB@e{TOMp62Uwauv%&#KHbQIy{^c__6LYa4)B!a%&X7sRC$nH;{hSn<fp!M}n$W28F zc4?pyIyt@qjqHvjOVu1`iIE9BJYOl?P`eb|6$0qz{e#T(C)dQ9lqyQ!c?>3h9z!dl zt}|PU70_0LOMG-F@jl6HKFCzJO+>GJB^cEgPf(b`7<gv0k}1;Q^SYd)Pzrw!yK=1* zO7?w<4!G__KP}|R;G_E_uzI^#I8{OI(njdq_j_b;e`t}~^m-=8#1UEBZ7NERJI#c6 zhOp_w)^MmdpePs-)=23-`|Qgf-ri#q=u9uZ_)XtrhwP^nFnwkB*lQjUjPH`s=zED8 zb<~m;TE(>sALe>8Z#7!b^qfetyv`LFs!xQBrQ6Wtb%W%6!vhlBbcwu5{w^GxEr(9) zPA3zttYjKO#xZHXXCuAX9-^Eb$lEAyfckp<h39(}iJ8|jX#6ct>TE5LC;!eMt3TC{ z$jYeG&y70d;k87tJDi0w?WWPerR8Leh9V7nm5X|hdyoP{M;a01M<)OD2lulhVYW{k zc)$1I%`aX+3-%mG&VTLM%Yu4#%TPGGr7Z^br!Hf64jZw{3*v-pqD_(Ow1r4}*<>bH zNflMBOMTG%X9n<D!wlXtK@lVH-i($8?m!;df5b1<40Ob>97V*g1D}2`M$jLQHup#~ zMX`X^ql2jR(_FMc_XvCW#3ZD1#8@0LRAkzJ)(idhkFp=#Y`}N#W%jM#L*atWd?ajc zDB4~egkIf}M}yk&%;@?X!qU3mWQ9o^vuCC@pKJ+rB9gBgk=FUE#C0&0_@_=}gQJR3 z#baYqk^6^Lp_XLJ%QB>1_Yt)((<iw~BE~0IpYHs=3ZB%9ig4jbq?eJ#eC9?m3k|GL z$mwy+P%UL1eXU@9n%YR=lk?<h;X9<~JD!~Hm=0PGR#D^0R><Hi2AflRhe)x9D*BvU zk7`%FV3h4v(T+wf$Q+S}KAwJnLcSW(wk%H??j8#xY<x(!vpJ|;5~q2m1u;K!vq{s_ z<;=LZIw(U$g0`QPML+i{@K9dN|0=sLnGnJM^QkoiIUy>C<1cPNf5>Vo86+SgwLaEj z4g>n)wQ7w-IJyy^PAVo_K#7?&`Ex4*Do!`B$5)uLZmsdmm6v0Q^O{v``-WL;VB3F; z#?Ptj<)>B*S^1bXGqPfdfh;@R5%<5#=J^%){$HPZn!xtF2|r?s3=)(SqUAqlq1*QA zP?OU~2K8iU!pSZoU48?(j=My3S5IR~)=AN+H|{clTZY(g`%ejT7M{V=S4LT$2?@oI zMDMs!#<8eO0@Il;df1d=?#1D^@c2m{XxHoECL2|dway^>6r)K@W*Xah!HKF5kHOz9 zX7FjP+H2m~{jQAr*88-uWh8xbrjHJF<?&99Z(w!)?xFQFwt>fKS%|4!02^HWVe{dy zWQg8JIWc{V*;iM3UqJxF?~mfGp@nS5q*ZunzdC-m!Hj$6twH0-32Z1bpqCyT!NEUu zE$ts)=B`Z@!Vz~DWLGdm&#Ul1)0*qgA!}tPBnjNf%=#gi`QtexpG{y^l;4Ky8<n~F zJ{Vq4=@tV3XF=%CFj%{Gh#VU*V;l6sh>dw9`@u2;wO)Eiv>LsIQ`ZSlaoHR!F|ZZC z)0F4h^(002{Oh^Yn<GT}T8{YUS8x0%dO68gzRff2`-)8XX>Vzmg+tM?tE2IVc3tR> zFy;ol9wCjbgq1NUVRo$EMGw2wk^A1;IT>Rka5JkX@;vdnbWjEy?KGgGF^1Glx-UGk zV=@UeD<#2M1Ua>s(%_&0^5>lwQT1L%msLn%`mNbwzFY<vtvV*sp9Yr8+HPXq%xvIS z>wO>rzcjfF{l&O*jXXZTQ;nOzX>*5~C*joxU-8n83TU~xF?4DEBzNx>(gK|~B<NN? zO%bi<5*xFj=z10?=sV!hNgt7IJVkG9_ThjN0m6+_zN3=oXNaPIH?r~kL<_FvqPB&n zh{{5J?$Cdl+{ffh(y-l%j~Kb5)V6!7Xzi*5oG0^~rbmzBBE)^>6Q@Vxj>D9DwrMY$ zEV%{V4~@in<BNpb8dJ#0y49S{*?#)#(g^OAJLW=imb0BB8_BDU(!|xE18B`_GR;yK zJ~j^^Mmm=)?RrbfW)!f_`-BM3VqkE$9%G%_fYw~+hmm>FZfI=fOq{tmn-gqUz<b~I zmA({Sz$fs0+SDJ57th|sIU4Rp6K>6+o%&|9^o^>e=+O(@d3-S}`8<H`oim3E$@WAo zTLv=v>PTN)7?DhVkA5jsU<uu85O`(}tN&b^yKXdr4B=JG{S~I%&g`|YRvhwT_(v>2 z(*7g7j<n(g2JS@t9-uSR4s*XpMp81*gwt3l3(DQMIS<?8^t`|gzn>+R4ZqCbPW#AW z)1_XpGd+q*{!YU2ivaw@Jjd3$Gjv|EFAh%|Pq$ueCCa0(!Ggucq$6*FSSgkyJGk*Q zu<t)OH0)0UynPY>aH0zG&s$B-{wQK}Zih1myeiNx%U7(vsSqg@zZFh8mPv*cCqkR4 z9Gl@jlg>VLocl3lq{tYIETxaV<_6AKK}XU>*lX?&{d;A(U)Oh#6Rl=2uHBw}5~jgk zjZucx-y`8b!g@$KtS$`rDS=y-enBe^nt;Mm;A4MXNlxGS3fpGb0K2)->~}>Ax;4EI zbjBJ}@H9mxQPD*qaiQpw>N4csDhC5fY7kMfjlH_Qn-p)$qGyU@q3**Rk>rRpt~0^_ z-yW93XX^53-tJ`NeF1|yyPkV%DCDeH&4G*Z@^oM86V?_w=_bi+`q7Q=hY}kaXu|9P z+MqB6a#r7j1B&rj<JdiRAt+*n^hnm_&^_{f&TSZ;D~Wc>q@cO&sp#4!E7W-OHI2V# zOt;5M(hnCZX_(e1tR<JkJ=MrUDy0rkxkDFMzj5P!FF23Cyba`DOcj8EgFcPYyh!4< ziLl^!Cbs1lxg*mhr>Tp4iWrmmnvv?=2Nja%X^fo*y62b$m#amX9Z>}*X73~wGJoOH z*puQoSvJw#5ezFnJYYQjw6o4-d)TX!Oxe}WigaL4F8Y0IF6{mC7rl|#PUS-qQO)O- zu!NgQD<2errqfe;+)0eW+FJzik66r)9;b~wg1vB*u>@8siGb=)A8EVBY*uu-6vlTY z<8R}SQ|qfzIKjdgt8eOpmsdw|+JUcV_nlVi>N%CW6g~?0^Zp=-#dcg=jF>Qz^ab_z zlrZ~WE?`P%I4sYUq(8pDBcbU>A+l%;H{rl4u3%+6Q9t^{^7R9b&!x1abB6=((hZ{{ zxFr+Uat>;D=<Tn`+#XKD(yCz%r@na$U2XpjGC$479aWOthj3>&+*(W2>3DFL;nSYf z=@1cqjxK~zWbBn%s8h1yb#A*vG~B*H;<&5i#Qw=J$PAN`{@)~Beim$d(t<MYO9;mu zn*y3FUkl4vjKvOZs$8Pc0GAo4Z&~gnA!^SHBgVEvFgW@m)>hcYIhZwpugpvC@6l7@ zR8KDL`&3Bn%)H^iYatS|UeMX9F<fNqU+8z7iceK((S$t;IOVSrw=hEra;}f1(MLMS zh$?powLQ$c7$YqXLW@xHMO)gzAGl9r3k>Kwmpt_2Mjx4@l1Hple$b#R>hyBJPp)E4 z2oBwu#(k`G;NB%!akZBgbN$7tSZ9?MTv?NWRpu5#vWo-$tyGOZxT;Y*r&Bb2nuNHr zI-2I_Jfo$a?R54<8Lr>a3|D-+189^VR2PSnPokO3r+{e0->v<Ts9hLG)O_{nk+(mY zsF-nJSNZ_u)Vw3Q7LLTGDg@my>w)!tb8vRTY3}9N(VSf9C(dy<A)jL9>0+k<+$)n$ zR!@#DGL|{com?=D%JROGByJLMKePZ=UYJ5N-cJ<soS)Ky=ThLDTMB+RQ^>`Ja&+OZ zG@sa~?52l&BG`pR47HuMgf@reBD35TXr1P3H0NCqa@Hm&e(OZKsObqqdPb7-k=3B! zQ-JN~UZbYLvqWx}Qt`q!%6RV^IsD(^)7ZDe1YW3(<gOeR(tvwYMKTVtoT&a1J0)`_ zY!vLE9#2-Hfd5439h!;Tmh*3bFsFp<w$J9Cyqiw;RE&blY0KEjj-#M>zZ{*gMI9u0 zDo8Q3id1;1famZ1sC#i1$zH4iXRbXbv}-eY8Qe-89d~eQleDOGueqqzVhi>jBj9G) zrK3sfmvZAmBI)n!BVf287~9D`#aDDPkicRq@)f#~eSF;lusQBe7VEh)T@`lNbe=zo zJbX+zW%6ju=S_n7!kv(1s)zHZErj8l+VH4b3IF%45d9q?Lz}JBn2u@M%)&Y2xJO=% zsQPIb*?i?W-8OiIfL{=PA7lk(uUJkIP2ub&N^lbDF?9XB9oRM36Y>jv=&kirDF58H z&z#|nRIc7WlAbEgr%B7Ff$Piz5O^$&QQNLh-d|4!_qY8hF;;9PRWzbgr83OH=<)c? zv{NYiz-<(~dLAyy2%~?LW<iF|Mllz3CmlbuiCzFnxUc>V1$pGqW@A%4&t@gpAMg-; zlUiaKzkEHsnllFT9j^1?wSO<&xOpw<HuNGN*erbBL(S6TiXXOnEP*eZM?uw*ayWAF z1Pq_dfnm>RoPHw+%)PSFj5Fu)QkObdmDL8q7$54KbCuX=E5OkHDRj)Xr)b;gsc<&T zg9vh;FwKdtP(q6rGdOY--C~QO-ME18)<|<%p8N+WR>qD?Pqzb?Pbu6TQccaqr;?1{ zU38{aBDi#xbMuN{vewmx_+{P@y*%{?S`tX0tx}IByUIhTQ5E&gjYh+MapdLMaJqb( zHTBR7q045)A=eUX5a0-uvsIqxe9T0hjR~l4Vj6iPY$UgPddarMp~RkF*33(g$RvL* zM1w*51fsU94;FF3qB+y&itg7u;ocrYG-hoToAofCE~Y_T>1{EUc$z%-t4sXXbS$9! zo3rU`PXns@+8!NPK90zH#M3uTy+ySt<7va49aMI{F*9yLB63QOhw3ePq`fH^={=DE zSJ`|dd7}gIQ<r9-wZ)ypa8d$naq>VdIjS_?x(_WiEWl0u2_Td890oE~xSE!7c)<Bv zPH5@IQDUISh+PZtvkN@jd!-P6pyqgAn-PAzvYeWhHZTg?+8{(szT-Y|B<It6q$^hr z@5Q+~-|j)E%Zn$f2ewdCT_12($m6s9?WV+d6-C4PJ;VvmronU1p}nf<MD?%^6R&j$ zc|LqfuCCuos#f`6NhwKgf>At8iagHsb*bZ>^|xr=uQtqt+jDN5D<^rpkbL{H0q%4S zLyThzpfC-be?S&7YycJM%%goCPk9&8pTW9kA8Cbr0nOaPzY9;V*}#jOILPB0<3}x- z#A|gD2|K8Q_joO$?|LTCgRzT9KxY8G7!yI)GaYE*k63ikdp!4bAL3Rio&$sFtuXcG z7F^+R15P%G0SreYkY3nBxRo*&!|zLM?l^f-_=m|{fO9N0ICqcx8+(xpRUFN|tqKE< zzv3C0yyq_)Ts67~63u91ejw{UB}+^Xa^PBL>*B_-9_YSp3_7iVaeBZPVcP_;2zqJ) zO^J*}J2&Ohlfx%bPpcv_zEVSdB3j^qQ!eD}%z-RR2hrTswYX-$4V-tr1RI_L^&w$6 zR%-`}<|*MN!Rnkn&ZGQD3xt>J_`x49KVHSepY-w#Sxz`Ih;^6baW{pR$W!Y_H0-1T zZqAs5{Wk=VzX{5uPP&OyN2U<#qbB4<`b=Ji+7j~Du?`(oS<c4jO44W9QS2h67NTc8 zl?(p$5v{aIC0aj1x!}$hSW(jkk9qx$eY1EXb%|Tc=Rzy`>F$lwxc6zw-1ZZp^tiY} z^8S7g+IrZN)~-5BYW?>>gH$>d{6I9?Jc25AexT3tR>JdNa!~i=C6Rvf5FMI&o7Kv% zCcnntBujSeMmNI?Q0E+HVYu%%lphmR<TfxBq;>8fxi7mxHv>biejvV7Q7@Vp$&Z2S zGe%lEf1e6_!X0q$<UDNj={!}vt__CK7ddm`d>pKyjoe=-(Lcf>dM#!aRUSws7OC=d z;toD>-B1eag2rGYOA)PWx=CaHg|Dql$fJsf%;jspV0Pj?%5$5DG=&2wdutqJ7Hq&T znpd*Ub~n+PTL_o($Ht*Vm37=J7jgEzrj`5S6(I)n`BCHv<c_K-_sO=6w#QhLjroOa zlMcbX6>7lcn?Q4t8V=Jk#?n8k$WPlTU}|&@S6*_VBZjKj>}v!@p`#!hVM-->Q(=*F zG}*+QN5}1E!LyPibag{Ky7ToI%aj1+f9uu8ckjO?4w1fy8P>$1tx?>b%yZyBP8z~2 zQ@FC!uQVuUtmPMP5ANtmUovOpYI<Y337%UaMix{d7|QqvhI_8i5!3;kw+LWG|4Od! ztU0&inKV7I{vxdu^pkO-5uC`=ho+~gbG4mMiA+uj@8}_*f!5l5dP`vib4O|r`OG$> z<!;7gj08v8J}E-j`KQR>_5g7$`3=#kEEgkzpyke4QPf6FF7ZkwEjt8wKvGL|=1B-X z`q`Ohwfq5c&@|`17vzG^LpA(E+`1ii(+*F1AIKbu?q+l!m$UJ)$9cB66K-FKgw}9> zYOkf#{J;Eu$p!D2|IdI8y==m_u9YUc%cP)o$>XAHl`Pt5#*(B-QEXJ*M_4f92}&1d zt3tCdQkde#)M`ir4zD4#l3Fz1e;qnLY7E@C_Lsa~ahDwa+(%R#H<1$!<G_klWgN5i zlDP4abY}83B;WIzc+Am6t}kq<gT|^Nd#SDeYYP9^R4BPaV3~MT`X3-#(3xm1{SV_T z&`dIy{>PXTtVvcCoJi7^{zrZj#3wD3{#SJt)Qbmk$yiCCoV-x_UlT%n+(PMp-2_2c z@?GhFL4Co}E9TPwV)z1I@$k=g7O>($?D#eaeu{^Gx|G1=s=4$(rATm0Jp6k!f*kQ6 MzNsC8_g72*4;>(J+W-In From 2b05c246ea6cadd41bf8f2c7cd5322145c2839fb Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Sun, 24 Apr 2022 11:32:26 +0200 Subject: [PATCH 212/512] add PIPG-SQP --- ocs2_pipg/CMakeLists.txt | 16 +- ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h | 24 +- ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h | 41 ++ .../include/ocs2_pipg/mpc/PipgMpcSolver.h | 158 +++++ ocs2_pipg/package.xml | 2 + ocs2_pipg/src/PIPG_Settings.cpp | 45 ++ ocs2_pipg/src/mpc/PipgMpcSolver.cpp | 620 ++++++++++++++++++ .../ocs2_ballbot/config/mpc/pipg.info | 12 + .../ocs2_ballbot_ros/CMakeLists.txt | 12 + .../launch/ballbot_pipg.launch | 22 + .../ocs2_ballbot_ros/package.xml | 1 + .../ocs2_ballbot_ros/src/PipgBallbotNode.cpp | 90 +++ .../ocs2_legged_robot/config/mpc/pipg.info | 12 + .../ocs2_legged_robot_ros/CMakeLists.txt | 16 + .../launch/legged_robot_pipg.launch | 50 ++ .../ocs2_legged_robot_ros/package.xml | 1 + .../src/LeggedRobotPipgMpcNode.cpp | 93 +++ 17 files changed, 1193 insertions(+), 22 deletions(-) create mode 100644 ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h create mode 100644 ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h create mode 100644 ocs2_pipg/src/PIPG_Settings.cpp create mode 100644 ocs2_pipg/src/mpc/PipgMpcSolver.cpp create mode 100644 ocs2_robotic_examples/ocs2_ballbot/config/mpc/pipg.info create mode 100644 ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_pipg.launch create mode 100644 ocs2_robotic_examples/ocs2_ballbot_ros/src/PipgBallbotNode.cpp create mode 100644 ocs2_robotic_examples/ocs2_legged_robot/config/mpc/pipg.info create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_pipg.launch create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPipgMpcNode.cpp diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_pipg/CMakeLists.txt index 37e5588ff..b2d4a77eb 100644 --- a/ocs2_pipg/CMakeLists.txt +++ b/ocs2_pipg/CMakeLists.txt @@ -3,6 +3,8 @@ project(ocs2_pipg) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_core + ocs2_sqp + ocs2_mpc ocs2_qp_solver ) @@ -10,12 +12,13 @@ find_package(catkin REQUIRED COMPONENTS ${CATKIN_PACKAGE_DEPENDENCIES} ) +find_package(Boost REQUIRED) + find_package(Eigen3 3.3 REQUIRED NO_MODULE) ################################### ## catkin specific configuration ## ################################### - catkin_package( INCLUDE_DIRS include @@ -24,23 +27,25 @@ catkin_package( ${CATKIN_PACKAGE_DEPENDENCIES} LIBRARIES ${PROJECT_NAME} - # DEPENDS + DEPENDS + Boost ) ########### ## Build ## ########### - include_directories( include ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ) -# Multiple shooting solver library add_library(${PROJECT_NAME} - src/PIPG.cpp src/OcpSize.cpp + src/PIPG_Settings.cpp + src/PIPG.cpp + src/mpc/PipgMpcSolver.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} @@ -80,7 +85,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ ############# ## Testing ## ############# - catkin_add_gtest(test_${PROJECT_NAME} test/testPIPGSolver.cpp ) diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h b/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h index e327513ed..e78d18738 100644 --- a/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h +++ b/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h @@ -3,38 +3,30 @@ #include <ocs2_core/Types.h> #include <string> - namespace ocs2 { namespace pipg { struct Settings { - Settings() = default; - Settings(size_t nThreads, int threadPriority, size_t maxNumIterations, scalar_t absoluteTolerance, scalar_t relativeTolerance, - size_t checkTerminationInterval, bool displayShortSummary) - : nThreads(nThreads), - threadPriority(threadPriority), - maxNumIterations(maxNumIterations), - absoluteTolerance(absoluteTolerance), - relativeTolerance(relativeTolerance), - checkTerminationInterval(checkTerminationInterval), - displayShortSummary(displayShortSummary){}; - /** Number of threads used in the multi-threading scheme. */ size_t nThreads = 1; /** Priority of threads used in the multi-threading scheme. */ int threadPriority = 99; - /** Maximum number of iterations of DDP. */ - size_t maxNumIterations = 15; + /** Maximum number of iterations of PIPG. */ + size_t maxNumIterations = 7000; /** Termination criteria. **/ scalar_t absoluteTolerance = 1e-3; scalar_t relativeTolerance = 1e-2; /** Number of iterations between consecutive calculation of termination conditions. **/ - size_t checkTerminationInterval = 25; + size_t checkTerminationInterval = 10; + /** Number of pre-conditioning run. **/ + int numScaling = 3; + /** The static lower bound of the cost hessian H. **/ + scalar_t lowerBoundH = 5e-6; /** This value determines to display the a summary log. */ bool displayShortSummary = false; }; -// Settings loadSettings(const std::string& filename, const std::string& fieldName = "ddp", bool verbose = true); +Settings loadSettings(const std::string& filename, const std::string& fieldName = "pipg", bool verbose = true); } // namespace pipg } // namespace ocs2 diff --git a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h new file mode 100644 index 000000000..4e6917758 --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h @@ -0,0 +1,41 @@ +#pragma once + +#include "ocs2_pipg/mpc/PipgMpcSolver.h" + +#include <ocs2_mpc/MPC_BASE.h> + +namespace ocs2 { +class PipgMpc final : public MPC_BASE { + public: + /** + * Constructor + * + * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use multiple shooting + * settings directly. + * @param settings : settings for the multiple shooting solver. + * @param [in] optimalControlProblem: The optimal control problem formulation. + * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. + */ + PipgMpc(mpc::Settings mpcSettings, multiple_shooting::Settings settings, pipg::Settings pipgSetting, + const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) + : MPC_BASE(std::move(mpcSettings)) { + solverPtr_.reset(new PipgMpcSolver(std::move(settings), std::move(pipgSetting), optimalControlProblem, initializer)); + }; + + ~PipgMpc() override = default; + + PipgMpcSolver* getSolverPtr() override { return solverPtr_.get(); } + const PipgMpcSolver* getSolverPtr() const override { return solverPtr_.get(); } + + protected: + void calculateController(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override { + if (settings().coldStart_) { + solverPtr_->reset(); + } + solverPtr_->run(initTime, initState, finalTime); + } + + private: + std::unique_ptr<PipgMpcSolver> solverPtr_; +}; +} // namespace ocs2 diff --git a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h new file mode 100644 index 000000000..ad865f452 --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h @@ -0,0 +1,158 @@ +#pragma once + +#include "ocs2_pipg/PIPG.h" + +#include <ocs2_core/initialization/Initializer.h> +#include <ocs2_core/integration/SensitivityIntegrator.h> +#include <ocs2_core/misc/Benchmark.h> +#include <ocs2_core/thread_support/ThreadPool.h> + +#include <ocs2_oc/oc_problem/OptimalControlProblem.h> +#include <ocs2_oc/oc_solver/SolverBase.h> + +#include <ocs2_sqp/MultipleShootingSettings.h> +#include <ocs2_sqp/MultipleShootingSolverStatus.h> +#include <ocs2_sqp/TimeDiscretization.h> + +namespace ocs2 { + +class PipgMpcSolver : public SolverBase { + public: + /** + * Constructor + * + * @param settings : settings for the multiple shooting solver. + * @param [in] optimalControlProblem: The optimal control problem formulation. + * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. + */ + PipgMpcSolver(multiple_shooting::Settings sqpSettings, pipg::Settings pipgSettings, const OptimalControlProblem& optimalControlProblem, + const Initializer& initializer); + + ~PipgMpcSolver() override; + + void reset() override; + + scalar_t getFinalTime() const override { return primalSolution_.timeTrajectory_.back(); }; + + void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } + + size_t getNumIterations() const override { return totalNumIterations_; } + + const PerformanceIndex& getPerformanceIndeces() const override { return getIterationsLog().back(); }; + + const std::vector<PerformanceIndex>& getIterationsLog() const override; + + ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const override { + throw std::runtime_error("[PipgMpcSolver] getValueFunction() not available yet."); + }; + + ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) override { + throw std::runtime_error("[PipgMpcSolver] getHamiltonian() not available yet."); + } + + vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override { + throw std::runtime_error("[PipgMpcSolver] getStateInputEqualityConstraintLagrangian() not available yet."); + } + + private: + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override; + + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const ControllerBase* externalControllerPtr) override { + if (externalControllerPtr == nullptr) { + runImpl(initTime, initState, finalTime); + } else { + throw std::runtime_error("[PipgMpcSolver::run] This solver does not support external controller!"); + } + } + + /** Run a task in parallel with settings.nThreads */ + void runParallel(std::function<void(int)> taskFunction); + + /** Get profiling information as a string */ + std::string getBenchmarkingInformation() const; + + std::string getBenchmarkingInformationPIPG() const; + + /** Initializes for the state-input trajectories */ + void initializeStateInputTrajectories(const vector_t& initState, const std::vector<AnnotatedTime>& timeDiscretization, + vector_array_t& stateTrajectory, vector_array_t& inputTrajectory); + + /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ + PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u); + + /** Computes only the performance metrics at the current {t, x(t), u(t)} */ + PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u); + + /** Returns solution of the QP subproblem in delta coordinates: */ + struct OcpSubproblemSolution { + vector_array_t deltaXSol; // delta_x(t) + vector_array_t deltaUSol; // delta_u(t) + scalar_t armijoDescentMetric; // inner product of the cost gradient and decision variable step + }; + OcpSubproblemSolution getOCPSolution(const vector_t& delta_x0); + + /** Set up the primal solution based on the optimized state and input trajectories */ + void setPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); + + /** Compute 2-norm of the trajectory: sqrt(sum_i v[i]^2) */ + static scalar_t trajectoryNorm(const vector_array_t& v); + + /** Compute total constraint violation */ + scalar_t totalConstraintViolation(const PerformanceIndex& performance) const; + + /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ + multiple_shooting::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u); + + /** Determine convergence after a step */ + multiple_shooting::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, + const multiple_shooting::StepInfo& stepInfo) const; + + // Problem definition + multiple_shooting::Settings settings_; + DynamicsDiscretizer discretizer_; + DynamicsSensitivityDiscretizer sensitivityDiscretizer_; + std::vector<OptimalControlProblem> ocpDefinitions_; + std::unique_ptr<Initializer> initializerPtr_; + + // Threading + ThreadPool threadPool_; + + // Solution + PrimalSolution primalSolution_; + + // Solver interface + pipg::Settings pipgSettings_; + Pipg pipgSolver_; + + // LQ approximation + std::vector<VectorFunctionLinearApproximation> dynamics_; + std::vector<ScalarFunctionQuadraticApproximation> cost_; + std::vector<VectorFunctionLinearApproximation> constraints_; + std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + + // Iteration performance log + std::vector<PerformanceIndex> performanceIndeces_; + + // Benchmarking + size_t numProblems_{0}; + size_t totalNumIterations_{0}; + benchmark::RepeatedTimer initializationTimer_; + benchmark::RepeatedTimer linearQuadraticApproximationTimer_; + benchmark::RepeatedTimer solveQpTimer_; + benchmark::RepeatedTimer linesearchTimer_; + benchmark::RepeatedTimer computeControllerTimer_; + + // PIPG Solver + benchmark::RepeatedTimer constructH_; + benchmark::RepeatedTimer constructG_; + benchmark::RepeatedTimer GTGMultiplication_; + benchmark::RepeatedTimer lambdaEstimation_; + benchmark::RepeatedTimer sigmaEstimation_; + benchmark::RepeatedTimer preConditioning_; +}; + +} // namespace ocs2 diff --git a/ocs2_pipg/package.xml b/ocs2_pipg/package.xml index ec355a7a6..891ea579f 100644 --- a/ocs2_pipg/package.xml +++ b/ocs2_pipg/package.xml @@ -10,6 +10,8 @@ <buildtool_depend>catkin</buildtool_depend> <depend>ocs2_core</depend> + <depend>ocs2_sqp</depend> + <depend>ocs2_mpc</depend> <!-- Test dependancy --> <depend>ocs2_qp_solver</depend> diff --git a/ocs2_pipg/src/PIPG_Settings.cpp b/ocs2_pipg/src/PIPG_Settings.cpp new file mode 100644 index 000000000..55a65d2e3 --- /dev/null +++ b/ocs2_pipg/src/PIPG_Settings.cpp @@ -0,0 +1,45 @@ +#include "ocs2_pipg/PIPG_Settings.h" + +#include <ocs2_core/misc/LoadData.h> + +#include <boost/property_tree/info_parser.hpp> +#include <boost/property_tree/ptree.hpp> + +#include <iostream> + +namespace ocs2 { +namespace pipg { + +Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { + boost::property_tree::ptree pt; + boost::property_tree::read_info(filename, pt); + + Settings settings; + + if (verbose) { + std::cerr << "\n #### PIPG Settings: "; + std::cerr << "\n #### =============================================================================\n"; + } + + loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); + loadData::loadPtreeValue(pt, settings.threadPriority, fieldName + ".threadPriority", verbose); + + loadData::loadPtreeValue(pt, settings.maxNumIterations, fieldName + ".maxNumIterations", verbose); + loadData::loadPtreeValue(pt, settings.absoluteTolerance, fieldName + ".absoluteTolerance", verbose); + loadData::loadPtreeValue(pt, settings.relativeTolerance, fieldName + ".relativeTolerance", verbose); + + loadData::loadPtreeValue(pt, settings.numScaling, fieldName + ".numScaling", verbose); + loadData::loadPtreeValue(pt, settings.lowerBoundH, fieldName + ".lowerBoundH", verbose); + + loadData::loadPtreeValue(pt, settings.checkTerminationInterval, fieldName + ".checkTerminationInterval", verbose); + loadData::loadPtreeValue(pt, settings.displayShortSummary, fieldName + ".displayShortSummary", verbose); + + if (verbose) { + std::cerr << " #### =============================================================================" << std::endl; + } + + return settings; +} + +} // namespace pipg +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/src/mpc/PipgMpcSolver.cpp b/ocs2_pipg/src/mpc/PipgMpcSolver.cpp new file mode 100644 index 000000000..45ede93ab --- /dev/null +++ b/ocs2_pipg/src/mpc/PipgMpcSolver.cpp @@ -0,0 +1,620 @@ +#include "ocs2_pipg/mpc/PipgMpcSolver.h" + +#include <ocs2_core/control/FeedforwardController.h> + +#include <ocs2_sqp/MultipleShootingInitialization.h> +#include <ocs2_sqp/MultipleShootingTranscription.h> + +#include <iostream> +#include <numeric> + +namespace ocs2 { + +PipgMpcSolver::PipgMpcSolver(multiple_shooting::Settings sqpSettings, pipg::Settings pipgSettings, + const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) + : SolverBase(), + settings_(std::move(sqpSettings)), + threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority), + pipgSolver_(pipgSettings), + pipgSettings_(pipgSettings) { + Eigen::setNbThreads(1); // No multithreading within Eigen. + Eigen::initParallel(); + + // Dynamics discretization + discretizer_ = selectDynamicsDiscretization(settings_.integratorType); + sensitivityDiscretizer_ = selectDynamicsSensitivityDiscretization(settings_.integratorType); + + // Clone objects to have one for each worker + for (int w = 0; w < settings_.nThreads; w++) { + ocpDefinitions_.push_back(optimalControlProblem); + } + + // Operating points + initializerPtr_.reset(initializer.clone()); + + if (optimalControlProblem.equalityConstraintPtr->empty()) { + settings_.projectStateInputEqualityConstraints = false; // True does not make sense if there are no constraints. + } +} + +std::string PipgMpcSolver::getBenchmarkingInformationPIPG() const { + const auto constructH = constructH_.getTotalInMilliseconds(); + const auto constructG = constructG_.getTotalInMilliseconds(); + const auto GTGMultiplication = GTGMultiplication_.getTotalInMilliseconds(); + const auto preConditioning = preConditioning_.getTotalInMilliseconds(); + const auto lambdaEstimation = lambdaEstimation_.getTotalInMilliseconds(); + const auto sigmaEstimation = sigmaEstimation_.getTotalInMilliseconds(); + const auto pipgRuntime = pipgSolver_.getTotalRunTimeInMilliseconds(); + + const auto benchmarkTotal = + constructH + constructG + GTGMultiplication + preConditioning + lambdaEstimation + sigmaEstimation + pipgRuntime; + + std::stringstream infoStream; + if (benchmarkTotal > 0.0) { + const scalar_t inPercent = 100.0; + infoStream << "\n########################################################################\n"; + infoStream << "The benchmarking is computed over " << constructH_.getNumTimedIntervals() << " iterations. \n"; + infoStream << "PIPG Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "\tconstructH :\t" << std::setw(10) << constructH_.getAverageInMilliseconds() << " [ms] \t(" + << constructH / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tconstructG :\t" << std::setw(10) << constructG_.getAverageInMilliseconds() << " [ms] \t(" + << constructG / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tGTGMultiplication :\t" << std::setw(10) << GTGMultiplication_.getAverageInMilliseconds() << " [ms] \t(" + << GTGMultiplication / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tpreConditioning :\t" << std::setw(10) << preConditioning_.getAverageInMilliseconds() << " [ms] \t(" + << preConditioning / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tlambdaEstimation :\t" << std::setw(10) << lambdaEstimation_.getAverageInMilliseconds() << " [ms] \t(" + << lambdaEstimation / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tsigmaEstimation :\t" << std::setw(10) << sigmaEstimation_.getAverageInMilliseconds() << " [ms] \t(" + << sigmaEstimation / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tPIPG runTime :\t" << std::setw(10) << pipgSolver_.getAverageRunTimeInMilliseconds() << " [ms] \t(" + << pipgRuntime / benchmarkTotal * inPercent << "%)\n"; + } + return infoStream.str(); +} + +PipgMpcSolver::~PipgMpcSolver() { + if (settings_.printSolverStatistics) { + std::cerr << getBenchmarkingInformationPIPG() << "\n" << getBenchmarkingInformation() << std::endl; + } +} + +void PipgMpcSolver::reset() { + // Clear solution + primalSolution_ = PrimalSolution(); + performanceIndeces_.clear(); + + // reset timers + numProblems_ = 0; + totalNumIterations_ = 0; + linearQuadraticApproximationTimer_.reset(); + solveQpTimer_.reset(); + linesearchTimer_.reset(); + computeControllerTimer_.reset(); +} + +std::string PipgMpcSolver::getBenchmarkingInformation() const { + const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); + const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); + const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds(); + const auto computeControllerTotal = computeControllerTimer_.getTotalInMilliseconds(); + + const auto benchmarkTotal = linearQuadraticApproximationTotal + solveQpTotal + linesearchTotal + computeControllerTotal; + + std::stringstream infoStream; + if (benchmarkTotal > 0.0) { + const scalar_t inPercent = 100.0; + infoStream << "\n########################################################################\n"; + infoStream << "The benchmarking is computed over " << totalNumIterations_ << " iterations. \n"; + infoStream << "SQP Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "\tLQ Approximation :\t" << std::setw(10) << linearQuadraticApproximationTimer_.getAverageInMilliseconds() + << " [ms] \t(" << linearQuadraticApproximationTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tSolve QP :\t" << std::setw(10) << solveQpTimer_.getAverageInMilliseconds() << " [ms] \t(" + << solveQpTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tLinesearch :\t" << std::setw(10) << linesearchTimer_.getAverageInMilliseconds() << " [ms] \t(" + << linesearchTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tCompute Controller :\t" << std::setw(10) << computeControllerTimer_.getAverageInMilliseconds() << " [ms] \t(" + << computeControllerTotal / benchmarkTotal * inPercent << "%)\n"; + } + return infoStream.str(); +} + +const std::vector<PerformanceIndex>& PipgMpcSolver::getIterationsLog() const { + if (performanceIndeces_.empty()) { + throw std::runtime_error("[PipgMpcSolver]: No performance log yet, no problem solved yet?"); + } else { + return performanceIndeces_; + } +} + +void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++ SQP solver is initialized ++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + } + + // Determine time discretization, taking into account event times. + const auto& eventTimes = this->getReferenceManager().getModeSchedule().eventTimes; + const auto timeDiscretization = timeDiscretizationWithEvents(initTime, finalTime, settings_.dt, eventTimes); + + // Initialize the state and input + vector_array_t x, u; + initializeStateInputTrajectories(initState, timeDiscretization, x, u); + + // Initialize references + for (auto& ocpDefinition : ocpDefinitions_) { + const auto& targetTrajectories = this->getReferenceManager().getTargetTrajectories(); + ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; + } + + // Bookkeeping + performanceIndeces_.clear(); + + int iter = 0; + multiple_shooting::Convergence convergence = multiple_shooting::Convergence::FALSE; + while (convergence == multiple_shooting::Convergence::FALSE) { + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\nSQP iteration: " << iter << "\n"; + } + // Make QP approximation + linearQuadraticApproximationTimer_.startTimer(); + const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u); + linearQuadraticApproximationTimer_.endTimer(); + + // Solve QP + solveQpTimer_.startTimer(); + const vector_t delta_x0 = initState - x[0]; + const auto deltaSolution = getOCPSolution(delta_x0); + solveQpTimer_.endTimer(); + + // Apply step + linesearchTimer_.startTimer(); + const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u); + performanceIndeces_.push_back(stepInfo.performanceAfterStep); + linesearchTimer_.endTimer(); + + // Check convergence + convergence = checkConvergence(iter, baselinePerformance, stepInfo); + + // Next iteration + ++iter; + ++totalNumIterations_; + } + + computeControllerTimer_.startTimer(); + setPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + computeControllerTimer_.endTimer(); + + ++numProblems_; + + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\nConvergence : " << toString(convergence) << "\n"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++ SQP solver has terminated ++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + } +} + +void PipgMpcSolver::runParallel(std::function<void(int)> taskFunction) { + threadPool_.runParallel(std::move(taskFunction), settings_.nThreads); +} + +void PipgMpcSolver::initializeStateInputTrajectories(const vector_t& initState, const std::vector<AnnotatedTime>& timeDiscretization, + vector_array_t& stateTrajectory, vector_array_t& inputTrajectory) { + const int N = static_cast<int>(timeDiscretization.size()) - 1; // // size of the input trajectory + stateTrajectory.clear(); + stateTrajectory.reserve(N + 1); + inputTrajectory.clear(); + inputTrajectory.reserve(N); + + // Determine till when to use the previous solution + scalar_t interpolateStateTill = timeDiscretization.front().time; + scalar_t interpolateInputTill = timeDiscretization.front().time; + if (primalSolution_.timeTrajectory_.size() >= 2) { + interpolateStateTill = primalSolution_.timeTrajectory_.back(); + interpolateInputTill = primalSolution_.timeTrajectory_[primalSolution_.timeTrajectory_.size() - 2]; + } + + // Initial state + const scalar_t initTime = getIntervalStart(timeDiscretization[0]); + if (initTime < interpolateStateTill) { + stateTrajectory.push_back( + LinearInterpolation::interpolate(initTime, primalSolution_.timeTrajectory_, primalSolution_.stateTrajectory_)); + } else { + stateTrajectory.push_back(initState); + } + + for (int i = 0; i < N; i++) { + if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { + // Event Node + inputTrajectory.push_back(vector_t()); // no input at event node + stateTrajectory.push_back(multiple_shooting::initializeEventNode(timeDiscretization[i].time, stateTrajectory.back())); + } else { + // Intermediate node + const scalar_t time = getIntervalStart(timeDiscretization[i]); + const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); + vector_t input, nextState; + if (time > interpolateInputTill || nextTime > interpolateStateTill) { // Using initializer + std::tie(input, nextState) = + multiple_shooting::initializeIntermediateNode(*initializerPtr_, time, nextTime, stateTrajectory.back()); + } else { // interpolate previous solution + std::tie(input, nextState) = multiple_shooting::initializeIntermediateNode(primalSolution_, time, nextTime, stateTrajectory.back()); + } + inputTrajectory.push_back(std::move(input)); + stateTrajectory.push_back(std::move(nextState)); + } + } +} + +PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_t& delta_x0) { + // Solve the QP + OcpSubproblemSolution solution; + auto& deltaXSol = solution.deltaXSol; + auto& deltaUSol = solution.deltaUSol; + + const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); + if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { + throw std::runtime_error("[PipgMpcSolver] Please project State-Input equality constraints."); + } + + // without constraints, or when using projection, we have an unconstrained QP. + pipgSolver_.resize(pipg::extractSizesFromProblem(dynamics_, cost_, nullptr)); + + constructH_.startTimer(); + constructH_.endTimer(); + + constructG_.startTimer(); + constructG_.endTimer(); + + vector_array_t D, E; + vector_array_t scalingVectors; + scalar_t c; + + preConditioning_.startTimer(); + pipgSolver_.preConditioningInPlaceInParallel(delta_x0, dynamics_, cost_, pipgSettings_.numScaling, D, E, scalingVectors, c, + Eigen::SparseMatrix<scalar_t>(), vector_t(), Eigen::SparseMatrix<scalar_t>()); + preConditioning_.endTimer(); + + lambdaEstimation_.startTimer(); + vector_t rowwiseAbsSumH = pipgSolver_.HAbsRowSumInParallel(cost_); + scalar_t lambdaScaled = rowwiseAbsSumH.maxCoeff(); + lambdaEstimation_.endTimer(); + + GTGMultiplication_.startTimer(); + vector_t rowwiseAbsSumGGT; + pipgSolver_.GGTAbsRowSumInParallel(dynamics_, nullptr, &scalingVectors, rowwiseAbsSumGGT); + GTGMultiplication_.endTimer(); + + sigmaEstimation_.startTimer(); + scalar_t sigmaScaled = rowwiseAbsSumGGT.maxCoeff(); + sigmaEstimation_.endTimer(); + + scalar_t maxScalingFactor = -1; + for (auto& v : D) { + if (v.size() != 0) { + maxScalingFactor = std::max(maxScalingFactor, v.maxCoeff()); + } + } + scalar_t muEstimated = pipgSettings_.lowerBoundH * c * maxScalingFactor * maxScalingFactor; + + vector_array_t EInv(E.size()); + std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); + Pipg::SolverStatus pipgStatus = + pipgSolver_.solveOCPInParallel(delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled, + ScalarFunctionQuadraticApproximation(), VectorFunctionLinearApproximation()); + pipgSolver_.getStateInputTrajectoriesSolution(deltaXSol, deltaUSol); + + // To determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + solution.armijoDescentMetric = 0.0; + for (int i = 0; i < cost_.size(); i++) { + if (cost_[i].dfdx.size() > 0) { + solution.armijoDescentMetric += cost_[i].dfdx.dot(deltaXSol[i]); + } + if (cost_[i].dfdu.size() > 0) { + solution.armijoDescentMetric += cost_[i].dfdu.dot(deltaUSol[i]); + } + } + + pipgSolver_.descaleSolution(D, deltaXSol, deltaUSol); + + // remap the tilde delta u to real delta u + if (settings_.projectStateInputEqualityConstraints) { + vector_t tmp; // 1 temporary for re-use. + for (int i = 0; i < deltaUSol.size(); i++) { + if (constraintsProjection_[i].f.size() > 0) { + tmp.noalias() = constraintsProjection_[i].dfdu * deltaUSol[i]; + deltaUSol[i] = tmp + constraintsProjection_[i].f; + deltaUSol[i].noalias() += constraintsProjection_[i].dfdx * deltaXSol[i]; + } + } + } + + return solution; +} + +void PipgMpcSolver::setPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { + // Clear old solution + primalSolution_.clear(); + + // Correct for missing inputs at PreEvents + for (int i = 0; i < time.size(); ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { + u[i] = u[i - 1]; + } + } + + // Construct nominal time, state and input trajectories + primalSolution_.stateTrajectory_ = std::move(x); + u.push_back(u.back()); // Repeat last input to make equal length vectors + primalSolution_.inputTrajectory_ = std::move(u); + primalSolution_.timeTrajectory_.reserve(time.size()); + for (size_t i = 0; i < time.size(); i++) { + primalSolution_.timeTrajectory_.push_back(time[i].time); + if (time[i].event == AnnotatedTime::Event::PreEvent) { + primalSolution_.postEventIndices_.push_back(i + 1); + } + } + primalSolution_.modeSchedule_ = this->getReferenceManager().getModeSchedule(); + + primalSolution_.controllerPtr_.reset(new FeedforwardController(primalSolution_.timeTrajectory_, primalSolution_.inputTrajectory_)); +} + +PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, + const vector_array_t& x, const vector_array_t& u) { + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + + std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); + dynamics_.resize(N); + cost_.resize(N + 1); + constraints_.resize(N + 1); + constraintsProjection_.resize(N); + + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; + PerformanceIndex workerPerformance; // Accumulate performance in local variable + const bool projection = settings_.projectStateInputEqualityConstraints; + + int i = timeIndex++; + while (i < N) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + // Event node + auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); + workerPerformance += result.performance; + dynamics_[i] = std::move(result.dynamics); + cost_[i] = std::move(result.cost); + constraints_[i] = std::move(result.constraints); + constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + } else { + // Normal, intermediate node + const scalar_t ti = getIntervalStart(time[i]); + const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); + auto result = + multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, projection, ti, dt, x[i], x[i + 1], u[i]); + workerPerformance += result.performance; + dynamics_[i] = std::move(result.dynamics); + cost_[i] = std::move(result.cost); + constraints_[i] = std::move(result.constraints); + constraintsProjection_[i] = std::move(result.constraintsProjection); + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + const scalar_t tN = getIntervalStart(time[N]); + auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); + workerPerformance += result.performance; + cost_[i] = std::move(result.cost); + constraints_[i] = std::move(result.constraints); + } + + // Accumulate! Same worker might run multiple tasks + performance[workerId] += workerPerformance; + }; + runParallel(std::move(parallelTask)); + + // Account for init state in performance + performance.front().dynamicsViolationSSE += (initState - x.front()).squaredNorm(); + + // Sum performance of the threads + PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); + totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + return totalPerformance; +} + +PerformanceIndex PipgMpcSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, + const vector_array_t& x, const vector_array_t& u) { + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + + std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; + PerformanceIndex workerPerformance; // Accumulate performance in local variable + + int i = timeIndex++; + while (i < N) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + // Event node + workerPerformance += multiple_shooting::computeEventPerformance(ocpDefinition, time[i].time, x[i], x[i + 1]); + } else { + // Normal, intermediate node + const scalar_t ti = getIntervalStart(time[i]); + const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); + workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + const scalar_t tN = getIntervalStart(time[N]); + workerPerformance += multiple_shooting::computeTerminalPerformance(ocpDefinition, tN, x[N]); + } + + // Accumulate! Same worker might run multiple tasks + performance[workerId] += workerPerformance; + }; + runParallel(std::move(parallelTask)); + + // Account for init state in performance + performance.front().dynamicsViolationSSE += (initState - x.front()).squaredNorm(); + + // Sum performance of the threads + PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); + totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + return totalPerformance; +} + +scalar_t PipgMpcSolver::trajectoryNorm(const vector_array_t& v) { + scalar_t norm = 0.0; + for (const auto& vi : v) { + norm += vi.squaredNorm(); + } + return std::sqrt(norm); +} + +scalar_t PipgMpcSolver::totalConstraintViolation(const PerformanceIndex& performance) const { + return std::sqrt(performance.dynamicsViolationSSE + performance.equalityConstraintsSSE); +} + +multiple_shooting::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, + vector_array_t& x, vector_array_t& u) { + using StepType = multiple_shooting::StepInfo::StepType; + + /* + * Filter linesearch based on: + * "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming" + * https://link.springer.com/article/10.1007/s10107-004-0559-y + */ + if (settings_.printLinesearch) { + std::cerr << std::setprecision(9) << std::fixed; + std::cerr << "\n=== Linesearch ===\n"; + std::cerr << "Baseline:\n" << baseline << "\n"; + } + + // Baseline costs + const scalar_t baselineConstraintViolation = totalConstraintViolation(baseline); + + // Update norm + const auto& dx = subproblemSolution.deltaXSol; + const auto& du = subproblemSolution.deltaUSol; + const scalar_t deltaUnorm = trajectoryNorm(du); + const scalar_t deltaXnorm = trajectoryNorm(dx); + + // Prepare step info + multiple_shooting::StepInfo stepInfo; + + scalar_t alpha = 1.0; + vector_array_t xNew(x.size()); + vector_array_t uNew(u.size()); + do { + // Compute step + for (int i = 0; i < u.size(); i++) { + if (du[i].size() > 0) { // account for absence of inputs at events. + uNew[i] = u[i] + alpha * du[i]; + } + } + for (int i = 0; i < x.size(); i++) { + xNew[i] = x[i] + alpha * dx[i]; + } + + // Compute cost and constraints + const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew); + const scalar_t newConstraintViolation = totalConstraintViolation(performanceNew); + + // Step acceptance and record step type + const bool stepAccepted = [&]() { + if (newConstraintViolation > settings_.g_max) { + // High constraint violation. Only accept decrease in constraints. + stepInfo.stepType = StepType::CONSTRAINT; + return newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); + } else if (newConstraintViolation < settings_.g_min && baselineConstraintViolation < settings_.g_min && + subproblemSolution.armijoDescentMetric < 0.0) { + // With low violation and having a descent direction, require the armijo condition. + stepInfo.stepType = StepType::COST; + return performanceNew.merit < (baseline.merit + settings_.armijoFactor * alpha * subproblemSolution.armijoDescentMetric); + } else { + // Medium violation: either merit or constraints decrease (with small gamma_c mixing of old constraints) + stepInfo.stepType = StepType::DUAL; + return performanceNew.merit < (baseline.merit - settings_.gamma_c * baselineConstraintViolation) || + newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); + } + }(); + + if (settings_.printLinesearch) { + std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepInfo.stepType) + << (stepAccepted ? std::string{" (Accepted)"} : std::string{" (Rejected)"}) << "\n"; + std::cerr << "|dx| = " << alpha * deltaXnorm << "\t|du| = " << alpha * deltaUnorm << "\n"; + std::cerr << performanceNew << "\n"; + } + + if (stepAccepted) { // Return if step accepted + x = std::move(xNew); + u = std::move(uNew); + + stepInfo.stepSize = alpha; + stepInfo.dx_norm = alpha * deltaXnorm; + stepInfo.du_norm = alpha * deltaUnorm; + stepInfo.performanceAfterStep = performanceNew; + stepInfo.totalConstraintViolationAfterStep = newConstraintViolation; + return stepInfo; + } else { // Try smaller step + alpha *= settings_.alpha_decay; + + // Detect too small step size during back-tracking to escape early. Prevents going all the way to alpha_min + if (alpha * deltaXnorm < settings_.deltaTol && alpha * deltaUnorm < settings_.deltaTol) { + if (settings_.printLinesearch) { + std::cerr << "Exiting linesearch early due to too small primal steps |dx|: " << alpha * deltaXnorm + << ", and or |du|: " << alpha * deltaUnorm << " are below deltaTol: " << settings_.deltaTol << "\n"; + } + break; + } + } + } while (alpha >= settings_.alpha_min); + + // Alpha_min reached -> Don't take a step + stepInfo.stepSize = 0.0; + stepInfo.stepType = StepType::ZERO; + stepInfo.dx_norm = 0.0; + stepInfo.du_norm = 0.0; + stepInfo.performanceAfterStep = baseline; + stepInfo.totalConstraintViolationAfterStep = baselineConstraintViolation; + + if (settings_.printLinesearch) { + std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; + } + + return stepInfo; +} + +multiple_shooting::Convergence PipgMpcSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, + const multiple_shooting::StepInfo& stepInfo) const { + using Convergence = multiple_shooting::Convergence; + if ((iteration + 1) >= settings_.sqpIteration) { + // Converged because the next iteration would exceed the specified number of iterations + return Convergence::ITERATIONS; + } else if (stepInfo.stepSize < settings_.alpha_min) { + // Converged because step size is below the specified minimum + return Convergence::STEPSIZE; + } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.costTol && + totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { + // Converged because the change in merit is below the specified tolerance while the constraint violation is below the minimum + return Convergence::METRICS; + } else if (stepInfo.dx_norm < settings_.deltaTol && stepInfo.du_norm < settings_.deltaTol) { + // Converged because the change in primal variables is below the specified tolerance + return Convergence::PRIMAL; + } else { + // None of the above convergence criteria were met -> not converged. + return Convergence::FALSE; + } +} + +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/pipg.info b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/pipg.info new file mode 100644 index 000000000..8fa92d821 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/pipg.info @@ -0,0 +1,12 @@ +pipg +{ + nThreads 8 + threadPriority 50 + maxNumIterations 7000 + absoluteTolerance 1e-3 + relativeTolerance 1e-2 + numScaling 3 + lowerBoundH 0.2 + checkTerminationInterval 10 + displayShortSummary false +} \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt index 5121de843..acdd97ea5 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt @@ -14,6 +14,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_pipg ocs2_ros_interfaces ocs2_robotic_tools ocs2_ballbot @@ -120,6 +121,17 @@ target_link_libraries(ballbot_sqp ) target_compile_options(ballbot_sqp PRIVATE ${OCS2_CXX_FLAGS}) +## PIPG node for ballbot +add_executable(ballbot_pipg + src/PipgBallbotNode.cpp +) +add_dependencies(ballbot_pipg + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(ballbot_pipg + ${catkin_LIBRARIES} +) +target_compile_options(ballbot_pipg PRIVATE ${OCS2_CXX_FLAGS}) ######################### ### CLANG TOOLING ### diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_pipg.launch b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_pipg.launch new file mode 100644 index 000000000..dd293a1dc --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_pipg.launch @@ -0,0 +1,22 @@ +<launch> + <arg name="rviz" default="true"/> + <arg name="multiplot" default="false"/> + <arg name="task_name" default="mpc"/> + + <group if="$(arg rviz)"> + <include file="$(find ocs2_ballbot_ros)/launch/visualize.launch"/> + </group> + + <group if="$(arg multiplot)"> + <include file="$(find ocs2_ballbot_ros)/launch/multiplot.launch"/> + </group> + + <node pkg="ocs2_ballbot_ros" type="ballbot_pipg" name="ballbot_pipg" + output="screen" args="$(arg task_name)" launch-prefix=""/> + + <node pkg="ocs2_ballbot_ros" type="ballbot_dummy_test" name="ballbot_dummy_test" + output="screen" args="$(arg task_name)" launch-prefix="gnome-terminal --"/> + + <node if="$(arg rviz)" pkg="ocs2_ballbot_ros" type="ballbot_target" name="ballbot_target" + output="screen" args="$(arg task_name)" launch-prefix="gnome-terminal --"/> +</launch> diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml index 3a9e207f5..cb9459c26 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml @@ -24,6 +24,7 @@ <depend>ocs2_ddp</depend> <depend>ocs2_mpc</depend> <depend>ocs2_sqp</depend> + <depend>ocs2_pipg</depend> <depend>ocs2_ros_interfaces</depend> <depend>ocs2_ballbot</depend> <depend>ocs2_robotic_tools</depend> diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/PipgBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/PipgBallbotNode.cpp new file mode 100644 index 000000000..a08d8792d --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/PipgBallbotNode.cpp @@ -0,0 +1,90 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include <ros/init.h> +#include <ros/package.h> + +#include <ocs2_pipg/mpc/PipgMpc.h> +#include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> +#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> + +#include <ocs2_ballbot/BallbotInterface.h> + +// Boost +#include <boost/filesystem/operations.hpp> +#include <boost/filesystem/path.hpp> + +int main(int argc, char** argv) { + const std::string robotName = "ballbot"; + + // task file + std::vector<std::string> programArgs{}; + ::ros::removeROSArgs(argc, argv, programArgs); + if (programArgs.size() <= 1) { + throw std::runtime_error("No task file specified. Aborting."); + } + std::string taskFileFolderName = std::string(programArgs[1]); + + // Initialize ros node + ros::init(argc, argv, robotName + "_mpc"); + ros::NodeHandle nodeHandle; + + // Robot interface + const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; + const std::string pipgConfigFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/pipg.info"; + const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); + + // Load PIPG settings + boost::filesystem::path pipgConfigPath(pipgConfigFile); + if (boost::filesystem::exists(pipgConfigPath)) { + std::cerr << "[pipgBallbotExample] Loading pipg config file: " << pipgConfigFile << std::endl; + } else { + throw std::invalid_argument("[pipgBallbotExample] Pipg config file not found: " + pipgConfigPath.string()); + } + ocs2::pipg::Settings pipgSettings = ocs2::pipg::loadSettings(pipgConfigFile, "pipg"); + + // ROS ReferenceManager + std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr( + new ocs2::RosReferenceManager(robotName, ballbotInterface.getReferenceManagerPtr())); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // MPC + ocs2::PipgMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.sqpSettings(), pipgSettings, + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); + + mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + + // Launch MPC ROS node + ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); + mpcNode.launchNodes(nodeHandle); + + // Successful exit + return 0; +} diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/pipg.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/pipg.info new file mode 100644 index 000000000..381ff95e3 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/pipg.info @@ -0,0 +1,12 @@ +pipg +{ + nThreads 8 + threadPriority 50 + maxNumIterations 7000 + absoluteTolerance 1e-3 + relativeTolerance 1e-2 + numScaling 3 + lowerBoundH 5e-6 + checkTerminationInterval 10 + displayShortSummary false +} \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt index d5c30446e..619eb400e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt @@ -16,6 +16,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_pipg ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model @@ -118,6 +119,21 @@ target_link_libraries(legged_robot_sqp_mpc ) target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS}) +## PIPG-MPC node for legged robot +add_executable(legged_robot_pipg_mpc + src/LeggedRobotPipgMpcNode.cpp +) +add_dependencies(legged_robot_pipg_mpc + ${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(legged_robot_pipg_mpc + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(legged_robot_pipg_mpc PRIVATE ${OCS2_CXX_FLAGS}) + # Dummy node add_executable(legged_robot_dummy src/LeggedRobotDummyNode.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_pipg.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_pipg.launch new file mode 100644 index 000000000..0cdefc30f --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_pipg.launch @@ -0,0 +1,50 @@ +<?xml version="1.0" ?> + +<launch> + <!-- visualization config --> + <arg name="rviz" default="true" /> + <arg name="description_name" default="legged_robot_description"/> + <arg name="multiplot" default="false"/> + + <!-- The task file for the mpc. --> + <arg name="taskFile" default="$(find ocs2_legged_robot)/config/mpc/task.info"/> + <!-- The reference related config file of the robot --> + <arg name="referenceFile" default="$(find ocs2_legged_robot)/config/command/reference.info"/> + <!-- The URDF model of the robot --> + <arg name="urdfFile" default="$(find ocs2_robotic_assets)/resources/anymal_c/urdf/anymal.urdf"/> + <!-- The file defining gait definition --> + <arg name="gaitCommandFile" default="$(find ocs2_legged_robot)/config/command/gait.info"/> + <!-- The pipg configuration file --> + <arg name="pipgConfigFile" default="$(find ocs2_legged_robot)/config/mpc/pipg.info" /> + + <!-- rviz --> + <group if="$(arg rviz)"> + <param name="$(arg description_name)" textfile="$(arg urdfFile)"/> + <arg name="rvizconfig" default="$(find ocs2_legged_robot_ros)/rviz/legged_robot.rviz" /> + <node pkg="rviz" type="rviz" name="rviz" args="-d $(arg rvizconfig)" output="screen" /> + </group> + + <!-- multiplot --> + <group if="$(arg multiplot)"> + <include file="$(find ocs2_legged_robot_ros)/launch/multiplot.launch"/> + </group> + + <!-- make the files into global parameters --> + <param name="taskFile" value="$(arg taskFile)" /> + <param name="referenceFile" value="$(arg referenceFile)" /> + <param name="urdfFile" value="$(arg urdfFile)" /> + <param name="gaitCommandFile" value="$(arg gaitCommandFile)"/> + <param name="pipgConfigFile" value="$(arg pipgConfigFile)" /> + + <node pkg="ocs2_legged_robot_ros" type="legged_robot_pipg_mpc" name="legged_robot_pipg_mpc" + output="screen" launch-prefix=""/> + + <node pkg="ocs2_legged_robot_ros" type="legged_robot_dummy" name="legged_robot_dummy" + output="screen" launch-prefix="gnome-terminal --"/> + + <node pkg="ocs2_legged_robot_ros" type="legged_robot_target" name="legged_robot_target" + output="screen" launch-prefix="gnome-terminal --"/> + + <node pkg="ocs2_legged_robot_ros" type="legged_robot_gait_command" name="legged_robot_gait_command" + output="screen" launch-prefix="gnome-terminal --"/> +</launch> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml index aa2943272..a23aab486 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml @@ -24,6 +24,7 @@ <depend>ocs2_ddp</depend> <depend>ocs2_mpc</depend> <depend>ocs2_sqp</depend> + <depend>ocs2_pipg</depend> <depend>ocs2_robotic_tools</depend> <depend>ocs2_pinocchio_interface</depend> <depend>ocs2_centroidal_model</depend> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPipgMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPipgMpcNode.cpp new file mode 100644 index 000000000..071ed54af --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPipgMpcNode.cpp @@ -0,0 +1,93 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include <ros/init.h> +#include <ros/package.h> + +#include <ocs2_legged_robot/LeggedRobotInterface.h> +#include <ocs2_pipg/mpc/PipgMpc.h> + +#include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> +#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> + +#include "ocs2_legged_robot_ros/gait/GaitReceiver.h" + +// Boost +#include <boost/filesystem/operations.hpp> +#include <boost/filesystem/path.hpp> + +using namespace ocs2; +using namespace legged_robot; + +int main(int argc, char** argv) { + const std::string robotName = "legged_robot"; + + // Initialize ros node + ros::init(argc, argv, robotName + "_mpc"); + ros::NodeHandle nodeHandle; + // Get node parameters + std::string taskFile, urdfFile, referenceFile, pipgConfigFile; + nodeHandle.getParam("/taskFile", taskFile); + nodeHandle.getParam("/urdfFile", urdfFile); + nodeHandle.getParam("/referenceFile", referenceFile); + + nodeHandle.getParam("/pipgConfigFile", pipgConfigFile); + boost::filesystem::path pipgConfigPath(pipgConfigFile); + if (boost::filesystem::exists(pipgConfigPath)) { + std::cerr << "[pipgLeggedExample] Loading pipg config file: " << pipgConfigFile << std::endl; + } else { + throw std::invalid_argument("[pipgLeggedExample] Pipg config file not found: " + pipgConfigPath.string()); + } + pipg::Settings pipgSettings = pipg::loadSettings(pipgConfigFile, "pipg"); + + // Robot interface + LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); + + // Gait receiver + auto gaitReceiverPtr = + std::make_shared<GaitReceiver>(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + + // ROS ReferenceManager + auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // MPC + PipgMpc mpc(interface.mpcSettings(), interface.sqpSettings(), pipgSettings, interface.getOptimalControlProblem(), + interface.getInitializer()); + + mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + + // Launch MPC ROS node + MPC_ROS_Interface mpcNode(mpc, robotName); + mpcNode.launchNodes(nodeHandle); + + // Successful exit + return 0; +} From 2cc6965e92bf5e6bec2bbbab33bf670cb56980c6 Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Sun, 24 Apr 2022 18:46:14 +0200 Subject: [PATCH 213/512] fix pipg test --- ocs2/package.xml | 1 + ocs2_pipg/test/testPIPGSolver.cpp | 25 +++++++++++++++---------- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/ocs2/package.xml b/ocs2/package.xml index 834b4090e..c4c62e738 100644 --- a/ocs2/package.xml +++ b/ocs2/package.xml @@ -17,6 +17,7 @@ <exec_depend>ocs2_qp_solver</exec_depend> <exec_depend>ocs2_ddp</exec_depend> <exec_depend>ocs2_sqp</exec_depend> + <exec_depend>ocs2_pipg</exec_depend> <exec_depend>ocs2_ros_interfaces</exec_depend> <exec_depend>ocs2_python_interface</exec_depend> <exec_depend>ocs2_pinocchio</exec_depend> diff --git a/ocs2_pipg/test/testPIPGSolver.cpp b/ocs2_pipg/test/testPIPGSolver.cpp index a4c2edb3f..2e6aea865 100644 --- a/ocs2_pipg/test/testPIPGSolver.cpp +++ b/ocs2_pipg/test/testPIPGSolver.cpp @@ -7,6 +7,20 @@ #include <Eigen/Sparse> +ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, + ocs2::scalar_t relativeTolerance, bool verbose) { + ocs2::pipg::Settings settings; + settings.nThreads = nThreads; + settings.maxNumIterations = maxNumIterations; + settings.absoluteTolerance = absoluteTolerance; + settings.relativeTolerance = relativeTolerance; + settings.checkTerminationInterval = 1; + settings.numScaling = 3; + settings.displayShortSummary = verbose; + + return settings; +} + class PIPGSolverTest : public testing::Test { protected: // x_0, x_1, ... x_{N - 1}, X_{N} @@ -18,16 +32,7 @@ class PIPGSolverTest : public testing::Test { static constexpr size_t numConstraints = N_ * (nx_ + nc_); static constexpr bool verbose = true; - PIPGSolverTest() - : pipgSolver({ - 8, // nThreads - 50, // threadPriority - 30000, // maxNumIterations - 1e-10, // absoluteTolerance - 1e-3, // relativeTolerance - 1, // checkTerminationInterval - verbose, // displayShortSummary - }) { + PIPGSolverTest() : pipgSolver(configurePipg(8, 30000, 1e-10, 1e-3, verbose)) { srand(10); // Construct OCP problem From 7a274eaed940ddaebf5233d04fe17473a7556ac1 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 26 Apr 2022 09:26:14 +0200 Subject: [PATCH 214/512] add base class for loss --- .../python/ocs2_ballbot_mpcnet/train.py | 2 +- .../python/ocs2_legged_robot_mpcnet/train.py | 2 +- .../python/ocs2_mpcnet_core/loss/base.py | 92 +++++++++++++++++++ .../loss/behavioral_cloning.py | 53 ++++++++--- .../ocs2_mpcnet_core/loss/cross_entropy.py | 52 ++++++++--- .../ocs2_mpcnet_core/loss/hamiltonian.py | 54 +++++++---- .../python/ocs2_mpcnet_core/mpcnet.py | 15 ++- 7 files changed, 214 insertions(+), 56 deletions(-) create mode 100644 ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py index 199b63230..ff6b99a02 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py @@ -51,7 +51,7 @@ def main(config_file_path: str) -> None: # interface interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) # loss - loss = HamiltonianLoss() + loss = HamiltonianLoss(config) # memory memory = CircularMemory(config) # policy diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py index 767619466..376d1f18b 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py @@ -52,7 +52,7 @@ def main(config_file_path: str) -> None: # interface interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) # loss - experts_loss = HamiltonianLoss() + experts_loss = HamiltonianLoss(config) gating_loss = CrossEntropyLoss(config) # memory memory = CircularMemory(config) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py new file mode 100644 index 000000000..f64002a7d --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py @@ -0,0 +1,92 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Base loss. + +Provides a base class for all loss classes. +""" + +import torch + +from ocs2_mpcnet_core.config import Config + + +class BaseLoss: + """Base loss. + + Provides the interface to all loss classes. + """ + + def __init__(self, config: Config) -> None: + """Initializes the Loss class. + + Initializes the Loss class. + + Args: + config: An instance of the configuration class. + """ + pass + + def __call__( + self, + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + p_query: torch.Tensor, + p_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: + """Computes the loss. + + Computes the mean loss for a batch. + + Args: + x_query: A (B,X) tensor with the query (e.g. predicted) states. + x_nominal: A (B,X) tensor with the nominal (e.g. target) states. + u_query: A (B,U) tensor with the query (e.g. predicted) inputs. + u_nominal: A (B,U) tensor with the nominal (e.g. target) inputs. + p_query: A (B,P) tensor with the query (e.g. predicted) discrete probability distributions. + p_nominal: A (B,P) tensor with the nominal (e.g. target) discrete probability distributions. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + H: A (B) tensor with the Hamiltonians at the nominal points. + + Returns: + A (1) tensor containing the mean loss. + """ + raise NotImplementedError() diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py index c1ec56134..5dda2c2d3 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py @@ -33,13 +33,13 @@ """ import torch -import numpy as np -from ocs2_mpcnet_core import config +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss.base import BaseLoss from ocs2_mpcnet_core.helper import bdot, bmv -class BehavioralCloningLoss: +class BehavioralCloningLoss(BaseLoss): r"""Behavioral cloning loss. Uses a simple quadratic function as loss: @@ -54,7 +54,7 @@ class BehavioralCloningLoss: R: A (1,U,U) tensor with the input cost matrix. """ - def __init__(self, config: config.Config) -> None: + def __init__(self, config: Config) -> None: """Initializes the BehavioralCloningLoss class. Initializes the BehavioralCloningLoss class by setting fixed attributes. @@ -62,21 +62,46 @@ def __init__(self, config: config.Config) -> None: Args: config: An instance of the configuration class. """ - self.R = torch.tensor(config.R, device=config.DEVICE, dtype=config.DTYPE).unsqueeze(dim=0) - - def __call__(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: - """Computes the mean behavioral cloning loss. - - Computes the mean behavioral cloning loss for a batch of size B using the cost matrix. + super().__init__(config) + self.R = torch.tensor(config.R, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + + def __call__( + self, + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + p_query: torch.Tensor, + p_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: + """Computes the loss. + + Computes the mean loss for a batch. Args: - u_predicted: A (B, U) tensor with the predicted inputs. - u_target: A (B, U) tensor with the target inputs. + x_query: A (B,X) tensor with the query (e.g. predicted) states. + x_nominal: A (B,X) tensor with the nominal (e.g. target) states. + u_query: A (B,U) tensor with the query (e.g. predicted) inputs. + u_nominal: A (B,U) tensor with the nominal (e.g. target) inputs. + p_query: A (B,P) tensor with the query (e.g. predicted) discrete probability distributions. + p_nominal: A (B,P) tensor with the nominal (e.g. target) discrete probability distributions. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + H: A (B) tensor with the Hamiltonians at the nominal points. Returns: - A (1) tensor containing the mean behavioral cloning loss. + A (1) tensor containing the mean loss. """ - return torch.mean(self.compute(u_predicted, u_target)) + return torch.mean(self.compute(u_query, u_nominal)) def compute(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: """Computes the behavioral cloning losses for a batch. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py index 848f4d184..93cc706c0 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py @@ -34,11 +34,12 @@ import torch -from ocs2_mpcnet_core import config +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss.base import BaseLoss from ocs2_mpcnet_core.helper import bdot, bmv -class CrossEntropyLoss: +class CrossEntropyLoss(BaseLoss): r"""Cross entropy loss. Uses the cross entropy between two discrete probability distributions as loss: @@ -53,7 +54,7 @@ class CrossEntropyLoss: epsilon: A (1) tensor with a small epsilon used to stabilize the logarithm. """ - def __init__(self, config: config.Config) -> None: + def __init__(self, config: Config) -> None: """Initializes the CrossEntropyLoss class. Initializes the CrossEntropyLoss class by setting fixed attributes. @@ -61,30 +62,55 @@ def __init__(self, config: config.Config) -> None: Args: config: An instance of the configuration class. """ + super().__init__(config) self.epsilon = torch.tensor(config.EPSILON, device=config.DEVICE, dtype=config.DTYPE) - def __call__(self, p_target: torch.Tensor, p_predicted: torch.Tensor) -> torch.Tensor: - """Computes the mean cross entropy loss. - - Computes the mean cross entropy loss for a batch, where the logarithm is stabilized by a small epsilon. + def __call__( + self, + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + p_query: torch.Tensor, + p_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: + """Computes the loss. + + Computes the mean loss for a batch. Args: - p_target: A (B,P) tensor with the target discrete probability distributions. - p_predicted: A (B,P) tensor with the predicted discrete probability distributions. + x_query: A (B,X) tensor with the query (e.g. predicted) states. + x_nominal: A (B,X) tensor with the nominal (e.g. target) states. + u_query: A (B,U) tensor with the query (e.g. predicted) inputs. + u_nominal: A (B,U) tensor with the nominal (e.g. target) inputs. + p_query: A (B,P) tensor with the query (e.g. predicted) discrete probability distributions. + p_nominal: A (B,P) tensor with the nominal (e.g. target) discrete probability distributions. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + H: A (B) tensor with the Hamiltonians at the nominal points. Returns: - A (1) tensor containing the mean cross entropy loss. + A (1) tensor containing the mean loss. """ - return torch.mean(self.compute(p_target, p_predicted)) + return torch.mean(self.compute(p_query, p_nominal)) - def compute(self, p_target: torch.Tensor, p_predicted: torch.Tensor) -> torch.Tensor: + def compute(self, p_predicted: torch.Tensor, p_target: torch.Tensor) -> torch.Tensor: """Computes the cross entropy losses for a batch. Computes the cross entropy losses for a batch, where the logarithm is stabilized by a small epsilon. Args: - p_target: A (B,P) tensor with the target discrete probability distributions. p_predicted: A (B,P) tensor with the predicted discrete probability distributions. + p_target: A (B,P) tensor with the target discrete probability distributions. Returns: A (B) tensor containing the cross entropy losses. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py index cbeac66f1..3def1f538 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py @@ -34,10 +34,12 @@ import torch +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss.base import BaseLoss from ocs2_mpcnet_core.helper import bdot, bmv -class HamiltonianLoss: +class HamiltonianLoss(BaseLoss): r"""Hamiltonian loss. Uses the linear quadratic approximation of the Hamiltonian as loss: @@ -50,12 +52,24 @@ class HamiltonianLoss: where the state x is of dimension X and the input u is of dimension U. """ + def __init__(self, config: Config) -> None: + """Initializes the HamiltonianLoss class. + + Initializes the HamiltonianLoss class. + + Args: + config: An instance of the configuration class. + """ + super().__init__(config) + def __call__( self, x_query: torch.Tensor, x_nominal: torch.Tensor, u_query: torch.Tensor, u_nominal: torch.Tensor, + p_query: torch.Tensor, + p_nominal: torch.Tensor, dHdxx: torch.Tensor, dHdux: torch.Tensor, dHduu: torch.Tensor, @@ -63,24 +77,26 @@ def __call__( dHdu: torch.Tensor, H: torch.Tensor, ) -> torch.Tensor: - """Computes the mean Hamiltonian loss. + """Computes the loss. - Computes the mean Hamiltonian loss for a batch of size B using the provided linear quadratic approximations. + Computes the mean loss for a batch. Args: - x_query: A (B,X) tensor with the states the Hamiltonian loss should be computed for. - x_nominal: A (B,X) tensor with the states that were used as development/expansion points. - u_query: A (B,U) tensor with the inputs the Hamiltonian loss should be computed for. - u_nominal: A (B,U) tensor with the inputs that were used as development/expansion point. - dHdxx: A (B,X,X) tensor with the state-state Hessians of the approximations. - dHdux: A (B,U,X) tensor with the input-state Hessians of the approximations. - dHduu: A (B,U,U) tensor with the input-input Hessians of the approximations. - dHdx: A (B,X) tensor with the state gradients of the approximations. - dHdu: A (B,U) tensor with the input gradients of the approximations. - H: A (B) tensor with the Hamiltonians at the development/expansion points. + x_query: A (B,X) tensor with the query (e.g. predicted) states. + x_nominal: A (B,X) tensor with the nominal (e.g. target) states. + u_query: A (B,U) tensor with the query (e.g. predicted) inputs. + u_nominal: A (B,U) tensor with the nominal (e.g. target) inputs. + p_query: A (B,P) tensor with the query (e.g. predicted) discrete probability distributions. + p_nominal: A (B,P) tensor with the nominal (e.g. target) discrete probability distributions. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + H: A (B) tensor with the Hamiltonians at the nominal points. Returns: - A (1) tensor containing the mean Hamiltonian loss. + A (1) tensor containing the mean loss. """ return torch.mean(self.compute(x_query, x_nominal, u_query, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H)) @@ -106,11 +122,11 @@ def compute( x_nominal: A (B,X) tensor with the states that were used as development/expansion points. u_query: A (B,U) tensor with the inputs the Hamiltonian loss should be computed for. u_nominal: A (B,U) tensor with the inputs that were used as development/expansion point. - dHdxx: A (B,X,X) tensor with the state-state Hessians of the approximations. - dHdux: A (B,U,X) tensor with the input-state Hessians of the approximations. - dHduu: A (B,U,U) tensor with the input-input Hessians of the approximations. - dHdx: A (B,X) tensor with the state gradients of the approximations. - dHdu: A (B,U) tensor with the input gradients of the approximations. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. H: A (B) tensor with the Hamiltonians at the development/expansion points. Returns: diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index 50a26649c..44e031ff1 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -43,8 +43,7 @@ from ocs2_mpcnet_core import helper from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray from ocs2_mpcnet_core.config import Config -from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss -from ocs2_mpcnet_core.loss.cross_entropy import CrossEntropyLoss +from ocs2_mpcnet_core.loss.base import BaseLoss from ocs2_mpcnet_core.memory.circular import CircularMemory @@ -60,8 +59,8 @@ def __init__( interface: object, memory: CircularMemory, policy: torch.nn.Module, - experts_loss: HamiltonianLoss, - gating_loss: Optional[CrossEntropyLoss] = None, + experts_loss: BaseLoss, + gating_loss: Optional[BaseLoss] = None, ) -> None: """Initializes the Mpcnet class. @@ -266,7 +265,7 @@ def normal_closure(): action = self.policy(observation) input = helper.bmv(action_transformation_matrix, action) + action_transformation_vector # compute the empirical loss - empirical_loss = self.experts_loss(x, x, input, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + empirical_loss = self.experts_loss(x, x, input, u, p, p, dHdxx, dHdux, dHduu, dHdx, dHdu, H) # compute the gradients empirical_loss.backward() # logging @@ -279,11 +278,11 @@ def cheating_closure(): # clear the gradients self.optimizer.zero_grad() # prediction - action, expert_weights = self.policy(observation) + action, weights = self.policy(observation) input = helper.bmv(action_transformation_matrix, action) + action_transformation_vector # compute the empirical loss - empirical_experts_loss = self.experts_loss(x, x, input, u, dHdxx, dHdux, dHduu, dHdx, dHdu, H) - empirical_gating_loss = self.gating_loss(p, expert_weights) + empirical_experts_loss = self.experts_loss(x, x, input, u, p, p, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + empirical_gating_loss = self.gating_loss(x, x, u, u, weights, p, dHdxx, dHdux, dHduu, dHdx, dHdu, H) empirical_loss = empirical_experts_loss + self.config.LAMBDA * empirical_gating_loss # compute the gradients empirical_loss.backward() From 907ee1b64a12ba4c770f7943d03e3b84f312a783 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 26 Apr 2022 14:40:59 +0200 Subject: [PATCH 215/512] add base class for policy --- .../python/ocs2_mpcnet_core/loss/base.py | 4 +- .../python/ocs2_mpcnet_core/mpcnet.py | 15 ++-- .../python/ocs2_mpcnet_core/policy/base.py | 90 +++++++++++++++++++ .../python/ocs2_mpcnet_core/policy/linear.py | 27 +++--- .../policy/mixture_of_linear_experts.py | 21 ++--- .../policy/mixture_of_nonlinear_experts.py | 21 ++--- .../ocs2_mpcnet_core/policy/nonlinear.py | 27 +++--- 7 files changed, 134 insertions(+), 71 deletions(-) create mode 100644 ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py index f64002a7d..6da703264 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py @@ -44,9 +44,9 @@ class BaseLoss: """ def __init__(self, config: Config) -> None: - """Initializes the Loss class. + """Initializes the BaseLoss class. - Initializes the Loss class. + Initializes the BaseLoss class. Args: config: An instance of the configuration class. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index 44e031ff1..9365b9099 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -45,6 +45,7 @@ from ocs2_mpcnet_core.config import Config from ocs2_mpcnet_core.loss.base import BaseLoss from ocs2_mpcnet_core.memory.circular import CircularMemory +from ocs2_mpcnet_core.policy.base import BasePolicy class Mpcnet: @@ -58,7 +59,7 @@ def __init__( config: Config, interface: object, memory: CircularMemory, - policy: torch.nn.Module, + policy: BasePolicy, experts_loss: BaseLoss, gating_loss: Optional[BaseLoss] = None, ) -> None: @@ -119,7 +120,7 @@ def get_tasks( """ raise NotImplementedError() - def start_data_generation(self, policy: torch.nn.Module, alpha: float = 1.0): + def start_data_generation(self, policy: BasePolicy, alpha: float = 1.0): """Start data generation. Start the data generation rollouts to receive new data. @@ -146,7 +147,7 @@ def start_data_generation(self, policy: torch.nn.Module, alpha: float = 1.0): target_trajectories, ) - def start_policy_evaluation(self, policy: torch.nn.Module, alpha: float = 0.0): + def start_policy_evaluation(self, policy: BasePolicy, alpha: float = 0.0): """Start policy evaluation. Start the policy evaluation rollouts to validate the current performance. @@ -257,12 +258,12 @@ def train(self) -> None: H, ) = self.memory.sample(self.config.BATCH_SIZE) - # normal closure only evaluating the experts loss for standard networks + # normal closure only evaluating the experts loss def normal_closure(): # clear the gradients self.optimizer.zero_grad() # prediction - action = self.policy(observation) + action = self.policy(observation)[0] input = helper.bmv(action_transformation_matrix, action) + action_transformation_vector # compute the empirical loss empirical_loss = self.experts_loss(x, x, input, u, p, p, dHdxx, dHdux, dHduu, dHdx, dHdu, H) @@ -273,12 +274,12 @@ def normal_closure(): # return empirical loss return empirical_loss - # cheating closure also adding the gating loss for mixture of experts networks + # cheating closure also adding the gating loss (only relevant for mixture of experts networks) def cheating_closure(): # clear the gradients self.optimizer.zero_grad() # prediction - action, weights = self.policy(observation) + action, weights = self.policy(observation)[:2] input = helper.bmv(action_transformation_matrix, action) + action_transformation_vector # compute the empirical loss empirical_experts_loss = self.experts_loss(x, x, input, u, p, p, dHdxx, dHdux, dHduu, dHdx, dHdu, H) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py new file mode 100644 index 000000000..54cc65a13 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py @@ -0,0 +1,90 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Base policy. + +Provides a base class for all policy classes. +""" + +import torch + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.helper import bmv + +class BasePolicy(torch.nn.Module): + """Base policy. + + Provides the interface to all policy classes. + + Attributes: + observation_scaling: A (1,O,O) tensor for the observation scaling. + action_scaling: A (1,A,A) tensor for the action scaling. + """ + + def __init__(self, config: Config) -> None: + """Initializes the BasePolicy class. + + Initializes the BasePolicy class. + + Args: + config: An instance of the configuration class. + """ + super().__init__() + self.observation_scaling = ( + torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) + self.action_scaling = ( + torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) + + def scale_observation(self, observation: torch.Tensor) -> torch.Tensor: + """Scale observation. + + Scale the observation with a fixed diagonal matrix. + + Args: + observation: A (B,O) tensor with the observations. + + Returns: + scaled_observation: A (B,O) tensor with the scaled observations. + """ + return bmv(self.observation_scaling, observation) + + def scale_action(self, action: torch.Tensor) -> torch.Tensor: + """Scale action. + + Scale the action with a fixed diagonal matrix. + + Args: + action: A (B,A) tensor with the actions. + + Returns: + scaled_action: A (B,A) tensor with the scaled actions. + """ + return bmv(self.action_scaling, action) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py index a204a1323..9682135cb 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py @@ -33,12 +33,13 @@ """ import torch +from typing import Tuple -from ocs2_mpcnet_core import config -from ocs2_mpcnet_core.helper import bmv +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.policy.base import BasePolicy -class LinearPolicy(torch.nn.Module): +class LinearPolicy(BasePolicy): """Linear policy. Class for a simple linear neural network policy. @@ -47,12 +48,10 @@ class LinearPolicy(torch.nn.Module): name: A string with the name of the policy. observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. action_dimension: An integer defining the action (i.e. output) dimension of the policy. - observation_scaling: A (1,O,O) tensor for the observation scaling. - action_scaling: A (1,A,A) tensor for the action scaling. linear: The linear neural network layer. """ - def __init__(self, config: config.Config) -> None: + def __init__(self, config: Config) -> None: """Initializes the LinearPolicy class. Initializes the LinearPolicy class by setting fixed and variable attributes. @@ -60,19 +59,13 @@ def __init__(self, config: config.Config) -> None: Args: config: An instance of the configuration class. """ - super().__init__() + super().__init__(config) self.name = "LinearPolicy" self.observation_dimension = config.OBSERVATION_DIM self.action_dimension = config.ACTION_DIM - self.observation_scaling = ( - torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) - ) - self.action_scaling = ( - torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) - ) self.linear = torch.nn.Linear(self.observation_dimension, self.action_dimension) - def forward(self, observation: torch.Tensor) -> torch.Tensor: + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor]: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. @@ -83,7 +76,7 @@ def forward(self, observation: torch.Tensor) -> torch.Tensor: Returns: action: A (B,A) tensor with the predicted actions. """ - scaled_observation = bmv(self.observation_scaling, observation) + scaled_observation = self.scale_observation(observation) unscaled_action = self.linear(scaled_observation) - action = bmv(self.action_scaling, unscaled_action) - return action + action = self.scale_action(unscaled_action) + return action, diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py index a060de6b3..2527d5e67 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py @@ -35,11 +35,12 @@ import torch from typing import Tuple -from ocs2_mpcnet_core import config +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.policy.base import BasePolicy from ocs2_mpcnet_core.helper import bmv -class MixtureOfLinearExpertsPolicy(torch.nn.Module): +class MixtureOfLinearExpertsPolicy(BasePolicy): """Mixture of linear experts policy. Class for a mixture of experts neural network policy with linear experts. @@ -49,13 +50,11 @@ class MixtureOfLinearExpertsPolicy(torch.nn.Module): observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. action_dimension: An integer defining the action (i.e. output) dimension of the policy. expert_number: An integer defining the number of experts. - observation_scaling: A (1,O,O) tensor for the observation scaling. - action_scaling: A (1,A,A) tensor for the action scaling. gating_net: The gating network. expert_nets: The expert networks. """ - def __init__(self, config: config.Config) -> None: + def __init__(self, config: Config) -> None: """Initializes the MixtureOfLinearExpertsPolicy class. Initializes the MixtureOfLinearExpertsPolicy class by setting fixed and variable attributes. @@ -63,17 +62,11 @@ def __init__(self, config: config.Config) -> None: Args: config: An instance of the configuration class. """ - super().__init__() + super().__init__(config) self.name = "MixtureOfLinearExpertsPolicy" self.observation_dimension = config.OBSERVATION_DIM self.action_dimension = config.ACTION_DIM self.expert_number = config.EXPERT_NUM - self.observation_scaling = ( - torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) - ) - self.action_scaling = ( - torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) - ) # gating self.gating_net = torch.nn.Sequential( torch.nn.Linear(self.observation_dimension, self.expert_number), torch.nn.Softmax(dim=1) @@ -95,13 +88,13 @@ def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor action: A (B,A) tensor with the predicted actions. expert_weights: A (B,E) tensor with the predicted expert weights. """ - scaled_observation = bmv(self.observation_scaling, observation) + scaled_observation = self.scale_observation(observation) expert_weights = self.gating_net(scaled_observation) expert_actions = torch.stack( [self.expert_nets[i](scaled_observation) for i in range(self.expert_number)], dim=2 ) unscaled_action = bmv(expert_actions, expert_weights) - action = bmv(self.action_scaling, unscaled_action) + action = self.scale_action(unscaled_action) return action, expert_weights diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py index fc259c516..6fed0da20 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py @@ -35,11 +35,12 @@ import torch from typing import Tuple -from ocs2_mpcnet_core import config +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.policy.base import BasePolicy from ocs2_mpcnet_core.helper import bmv -class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): +class MixtureOfNonlinearExpertsPolicy(BasePolicy): """Mixture of nonlinear experts policy. Class for a mixture of experts neural network policy with nonlinear experts, where the hidden layer dimension is the @@ -52,13 +53,11 @@ class MixtureOfNonlinearExpertsPolicy(torch.nn.Module): expert_hidden_dimension: An integer defining the dimension of the hidden layer for the expert networks. action_dimension: An integer defining the action (i.e. output) dimension of the policy. expert_number: An integer defining the number of experts. - observation_scaling: A (1,O,O) tensor for the observation scaling. - action_scaling: A (1,A,A) tensor for the action scaling. gating_net: The gating network. expert_nets: The expert networks. """ - def __init__(self, config: config.Config) -> None: + def __init__(self, config: Config) -> None: """Initializes the MixtureOfNonlinearExpertsPolicy class. Initializes the MixtureOfNonlinearExpertsPolicy class by setting fixed and variable attributes. @@ -66,19 +65,13 @@ def __init__(self, config: config.Config) -> None: Args: config: An instance of the configuration class. """ - super().__init__() + super().__init__(config) self.name = "MixtureOfNonlinearExpertsPolicy" self.observation_dimension = config.OBSERVATION_DIM self.gating_hidden_dimension = int((config.OBSERVATION_DIM + config.EXPERT_NUM) / 2) self.expert_hidden_dimension = int((config.OBSERVATION_DIM + config.ACTION_DIM) / 2) self.action_dimension = config.ACTION_DIM self.expert_number = config.EXPERT_NUM - self.observation_scaling = ( - torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) - ) - self.action_scaling = ( - torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) - ) # gating self.gating_net = torch.nn.Sequential( torch.nn.Linear(self.observation_dimension, self.gating_hidden_dimension), @@ -106,13 +99,13 @@ def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor action: A (B,A) tensor with the predicted actions. expert_weights: A (B,E) tensor with the predicted expert weights. """ - scaled_observation = bmv(self.observation_scaling, observation) + scaled_observation = self.scale_observation(observation) expert_weights = self.gating_net(scaled_observation) expert_actions = torch.stack( [self.expert_nets[i](scaled_observation) for i in range(self.expert_number)], dim=2 ) unscaled_action = bmv(expert_actions, expert_weights) - action = bmv(self.action_scaling, unscaled_action) + action = self.scale_action(unscaled_action) return action, expert_weights diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py index 2b3950cfc..607752895 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py @@ -33,12 +33,13 @@ """ import torch +from typing import Tuple -from ocs2_mpcnet_core import config -from ocs2_mpcnet_core.helper import bmv +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.policy.base import BasePolicy -class NonlinearPolicy(torch.nn.Module): +class NonlinearPolicy(BasePolicy): """Nonlinear policy. Class for a simple nonlinear neural network policy, where the hidden layer dimension is the mean of the input and @@ -49,14 +50,12 @@ class NonlinearPolicy(torch.nn.Module): observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. hidden_dimension: An integer defining the dimension of the hidden layer. action_dimension: An integer defining the action (i.e. output) dimension of the policy. - observation_scaling: A (1,O,O) tensor for the observation scaling. - action_scaling: A (1,A,A) tensor for the action scaling. linear1: The first linear neural network layer. activation: The activation to get the hidden layer. linear2: The second linear neural network layer. """ - def __init__(self, config: config.Config) -> None: + def __init__(self, config: Config) -> None: """Initializes the NonlinearPolicy class. Initializes the NonlinearPolicy class by setting fixed and variable attributes. @@ -64,22 +63,16 @@ def __init__(self, config: config.Config) -> None: Args: config: An instance of the configuration class. """ - super().__init__() + super().__init__(config) self.name = "NonlinearPolicy" self.observation_dimension = config.OBSERVATION_DIM self.hidden_dimension = int((config.OBSERVATION_DIM + config.ACTION_DIM) / 2) self.action_dimension = config.ACTION_DIM - self.observation_scaling = ( - torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) - ) - self.action_scaling = ( - torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) - ) self.linear1 = torch.nn.Linear(self.observation_dimension, self.hidden_dimension) self.activation = torch.nn.Tanh() self.linear2 = torch.nn.Linear(self.hidden_dimension, self.action_dimension) - def forward(self, observation: torch.Tensor) -> torch.Tensor: + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor]: """Forward method. Defines the computation performed at every call. Computes the output tensors from the input tensors. @@ -90,7 +83,7 @@ def forward(self, observation: torch.Tensor) -> torch.Tensor: Returns: action: A (B,A) tensor with the predicted actions. """ - scaled_observation = bmv(self.observation_scaling, observation) + scaled_observation = self.scale_observation(observation) unscaled_action = self.linear2(self.activation(self.linear1(scaled_observation))) - action = bmv(self.action_scaling, unscaled_action) - return action + action = self.scale_action(unscaled_action) + return action, From 81caafd9bfb98a1b19b25d6c0ff6cb8051a1f4b0 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 26 Apr 2022 15:19:51 +0200 Subject: [PATCH 216/512] add base class for memory --- .../python/ocs2_mpcnet_core/memory/base.py | 118 ++++++++++++++++++ .../ocs2_mpcnet_core/memory/circular.py | 15 +-- .../python/ocs2_mpcnet_core/mpcnet.py | 4 +- 3 files changed, 128 insertions(+), 9 deletions(-) create mode 100644 ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py new file mode 100644 index 000000000..993c22030 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py @@ -0,0 +1,118 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +############################################################################### + +"""Base memory. + +Provides a base class for all memory classes. +""" + +import torch +import numpy as np +from typing import Tuple, List + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core import ScalarFunctionQuadraticApproximation + + +class BaseMemory: + """Base memory. + + Provides the interface to all memory classes. + """ + + def __init__(self, config: Config) -> None: + """Initializes the BaseMemory class. + + Initializes the BaseMemory class. + + Args: + config: An instance of the configuration class. + """ + pass + + def push( + self, + t: float, + x: np.ndarray, + u: np.ndarray, + p: np.ndarray, + observation: np.ndarray, + action_transformation: List[np.ndarray], + hamiltonian: ScalarFunctionQuadraticApproximation, + ) -> None: + """Pushes data into the memory. + + Pushes one data sample into the memory. + + Args: + t: A float with the time. + x: A NumPy array of shape (X) with the observed state. + u: A NumPy array of shape (U) with the optimal input. + p: A NumPy array of shape (P) tensor for the observed discrete probability distributions of the modes. + observation: A NumPy array of shape (O) with the generalized times. + action_transformation: A list containing NumPy arrays of shape (U,A) and (U) with the action transformation. + hamiltonian: An OCS2 scalar function quadratic approximation representing the Hamiltonian around x and u. + """ + raise NotImplementedError() + + def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: + """Samples data from the memory. + + Samples a batch of data from the memory. + + Args: + batch_size: An integer defining the batch size B. + + Returns: + A tuple containing the sampled batch of data. + - t_batch: A (B) tensor with the times. + - x_batch: A (B,X) tensor with the observed states. + - u_batch: A (B,U) tensor with the optimal inputs. + - p_batch: A (B,P) tensor with the observed discrete probability distributions of the modes. + - observation_batch: A (B,O) tensor with the observations. + - action_transformation_matrix_batch: A (B,U,A) tensor with the action transformation matrices. + - action_transformation_vector_batch: A (B,U) tensor with the action transformation vectors. + - dHdxx_batch: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + - dHdux_batch: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + - dHduu_batch: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + - dHdx_batch: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + - dHdu_batch: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + - H_batch: A (B) tensor with the Hamiltonians at the development/expansion points. + """ + raise NotImplementedError() + + def __len__(self) -> int: + """The length of the memory. + + Return the length of the memory given by the current size. + + Returns: + An integer describing the length of the memory. + """ + raise NotImplementedError() diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py index 8420f38b4..70decc31b 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py @@ -36,11 +36,12 @@ import numpy as np from typing import Tuple, List -from ocs2_mpcnet_core import config +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.memory.base import BaseMemory from ocs2_mpcnet_core import ScalarFunctionQuadraticApproximation -class CircularMemory: +class CircularMemory(BaseMemory): """Circular memory. Stores data in a circular memory that overwrites old data if the size of the memory reaches its capacity. @@ -64,7 +65,7 @@ class CircularMemory: H: A (C) tensor for the Hamiltonians at the development/expansion points. """ - def __init__(self, config: config.Config) -> None: + def __init__(self, config: Config) -> None: """Initializes the CircularMemory class. Initializes the CircularMemory class by setting fixed attributes, initializing variable attributes and @@ -115,9 +116,9 @@ def push( action_transformation: List[np.ndarray], hamiltonian: ScalarFunctionQuadraticApproximation, ) -> None: - """Pushes data into the circular memory. + """Pushes data into the memory. - Pushes one data sample into the circular memory. + Pushes one data sample into the memory. Args: t: A float with the time. @@ -153,9 +154,9 @@ def push( self.position = (self.position + 1) % self.capacity def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: - """Samples data from the circular memory. + """Samples data from the memory. - Samples a batch of data from the circular memory. + Samples a batch of data from the memory. Args: batch_size: An integer defining the batch size B. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index 9365b9099..3ba9818a7 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -44,7 +44,7 @@ from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray from ocs2_mpcnet_core.config import Config from ocs2_mpcnet_core.loss.base import BaseLoss -from ocs2_mpcnet_core.memory.circular import CircularMemory +from ocs2_mpcnet_core.memory.base import BaseMemory from ocs2_mpcnet_core.policy.base import BasePolicy @@ -58,7 +58,7 @@ def __init__( self, config: Config, interface: object, - memory: CircularMemory, + memory: BaseMemory, policy: BasePolicy, experts_loss: BaseLoss, gating_loss: Optional[BaseLoss] = None, From 66f4f732cc7ab068572e8e216bde835411851b52 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 26 Apr 2022 15:20:32 +0200 Subject: [PATCH 217/512] run black --- .../ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py | 1 + .../ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py | 2 +- .../python/ocs2_mpcnet_core/policy/nonlinear.py | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py index 54cc65a13..a3faccf16 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py @@ -37,6 +37,7 @@ from ocs2_mpcnet_core.config import Config from ocs2_mpcnet_core.helper import bmv + class BasePolicy(torch.nn.Module): """Base policy. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py index 9682135cb..ba9c9fe9c 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py @@ -79,4 +79,4 @@ def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor]: scaled_observation = self.scale_observation(observation) unscaled_action = self.linear(scaled_observation) action = self.scale_action(unscaled_action) - return action, + return (action,) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py index 607752895..f2f9f95e1 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py @@ -86,4 +86,4 @@ def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor]: scaled_observation = self.scale_observation(observation) unscaled_action = self.linear2(self.activation(self.linear1(scaled_observation))) action = self.scale_action(unscaled_action) - return action, + return (action,) From c872e66c98b5d3b5bfff06dc6715218c019d4ade Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Tue, 26 Apr 2022 15:41:41 +0200 Subject: [PATCH 218/512] reduce unnecessarily large weight for gating loss --- .../python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml index a873bcd08..e330b3442 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml @@ -80,7 +80,7 @@ config: # whether to cheat by adding the gating loss CHEATING: True # parameter to control the relative importance of both loss types - LAMBDA: 10.0 + LAMBDA: 1.0 # dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) EXPERT_FOR_MODE: 6: 1 # trot From 0dedb6652688c4a0977f911b5353a244cea640d4 Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Wed, 27 Apr 2022 14:19:35 +0200 Subject: [PATCH 219/512] load settings from confiuration file --- ocs2_pipg/CMakeLists.txt | 13 ++++--- ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h | 25 ++++-------- ocs2_pipg/src/PIPG_Settings.cpp | 42 +++++++++++++++++++++ ocs2_pipg/test/testPIPGSolver.cpp | 30 ++++++++------- 4 files changed, 73 insertions(+), 37 deletions(-) create mode 100644 ocs2_pipg/src/PIPG_Settings.cpp diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_pipg/CMakeLists.txt index 37e5588ff..520d6a4f0 100644 --- a/ocs2_pipg/CMakeLists.txt +++ b/ocs2_pipg/CMakeLists.txt @@ -10,12 +10,13 @@ find_package(catkin REQUIRED COMPONENTS ${CATKIN_PACKAGE_DEPENDENCIES} ) +find_package(Boost REQUIRED) + find_package(Eigen3 3.3 REQUIRED NO_MODULE) ################################### ## catkin specific configuration ## ################################### - catkin_package( INCLUDE_DIRS include @@ -24,23 +25,24 @@ catkin_package( ${CATKIN_PACKAGE_DEPENDENCIES} LIBRARIES ${PROJECT_NAME} - # DEPENDS + DEPENDS + Boost ) ########### ## Build ## ########### - include_directories( include ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ) -# Multiple shooting solver library add_library(${PROJECT_NAME} - src/PIPG.cpp src/OcpSize.cpp + src/PIPG_Settings.cpp + src/PIPG.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} @@ -80,7 +82,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ ############# ## Testing ## ############# - catkin_add_gtest(test_${PROJECT_NAME} test/testPIPGSolver.cpp ) diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h b/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h index e327513ed..d1b028aab 100644 --- a/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h +++ b/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h @@ -1,40 +1,29 @@ #pragma once -#include <ocs2_core/Types.h> - #include <string> +#include <ocs2_core/Types.h> + namespace ocs2 { namespace pipg { struct Settings { - Settings() = default; - Settings(size_t nThreads, int threadPriority, size_t maxNumIterations, scalar_t absoluteTolerance, scalar_t relativeTolerance, - size_t checkTerminationInterval, bool displayShortSummary) - : nThreads(nThreads), - threadPriority(threadPriority), - maxNumIterations(maxNumIterations), - absoluteTolerance(absoluteTolerance), - relativeTolerance(relativeTolerance), - checkTerminationInterval(checkTerminationInterval), - displayShortSummary(displayShortSummary){}; - /** Number of threads used in the multi-threading scheme. */ - size_t nThreads = 1; + size_t nThreads = 3; /** Priority of threads used in the multi-threading scheme. */ int threadPriority = 99; - /** Maximum number of iterations of DDP. */ - size_t maxNumIterations = 15; + /** Maximum number of iterations of PIPG. */ + size_t maxNumIterations = 3000; /** Termination criteria. **/ scalar_t absoluteTolerance = 1e-3; scalar_t relativeTolerance = 1e-2; /** Number of iterations between consecutive calculation of termination conditions. **/ - size_t checkTerminationInterval = 25; + size_t checkTerminationInterval = 1; /** This value determines to display the a summary log. */ bool displayShortSummary = false; }; -// Settings loadSettings(const std::string& filename, const std::string& fieldName = "ddp", bool verbose = true); +Settings loadSettings(const std::string& filename, const std::string& fieldName = "pipg", bool verbose = true); } // namespace pipg } // namespace ocs2 diff --git a/ocs2_pipg/src/PIPG_Settings.cpp b/ocs2_pipg/src/PIPG_Settings.cpp new file mode 100644 index 000000000..f5f46f203 --- /dev/null +++ b/ocs2_pipg/src/PIPG_Settings.cpp @@ -0,0 +1,42 @@ +#include "ocs2_pipg/PIPG_Settings.h" + +#include <iostream> + +#include <boost/property_tree/info_parser.hpp> +#include <boost/property_tree/ptree.hpp> + +#include <ocs2_core/misc/LoadData.h> + +namespace ocs2 { +namespace pipg { + +Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { + boost::property_tree::ptree pt; + boost::property_tree::read_info(filename, pt); + + Settings settings; + + if (verbose) { + std::cerr << "\n #### PIPG Settings: "; + std::cerr << "\n #### =============================================================================\n"; + } + + loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); + loadData::loadPtreeValue(pt, settings.threadPriority, fieldName + ".threadPriority", verbose); + + loadData::loadPtreeValue(pt, settings.maxNumIterations, fieldName + ".maxNumIterations", verbose); + loadData::loadPtreeValue(pt, settings.absoluteTolerance, fieldName + ".absoluteTolerance", verbose); + loadData::loadPtreeValue(pt, settings.relativeTolerance, fieldName + ".relativeTolerance", verbose); + + loadData::loadPtreeValue(pt, settings.checkTerminationInterval, fieldName + ".checkTerminationInterval", verbose); + loadData::loadPtreeValue(pt, settings.displayShortSummary, fieldName + ".displayShortSummary", verbose); + + if (verbose) { + std::cerr << " #### =============================================================================" << std::endl; + } + + return settings; +} + +} // namespace pipg +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/test/testPIPGSolver.cpp b/ocs2_pipg/test/testPIPGSolver.cpp index a4c2edb3f..c2e769f3c 100644 --- a/ocs2_pipg/test/testPIPGSolver.cpp +++ b/ocs2_pipg/test/testPIPGSolver.cpp @@ -1,16 +1,29 @@ +#include "ocs2_pipg/PIPG.h" + #include <gtest/gtest.h> -#include "ocs2_pipg/PIPG.h" +#include <Eigen/Sparse> #include <ocs2_qp_solver/QpSolver.h> #include <ocs2_qp_solver/test/testProblemsGeneration.h> -#include <Eigen/Sparse> +ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, + ocs2::scalar_t relativeTolerance, bool verbose) { + ocs2::pipg::Settings settings; + settings.nThreads = nThreads; + settings.maxNumIterations = maxNumIterations; + settings.absoluteTolerance = absoluteTolerance; + settings.relativeTolerance = relativeTolerance; + settings.checkTerminationInterval = 1; + settings.displayShortSummary = verbose; + + return settings; +} class PIPGSolverTest : public testing::Test { protected: // x_0, x_1, ... x_{N - 1}, X_{N} - static constexpr size_t N_ = 5; // numStages + static constexpr size_t N_ = 10; // numStages static constexpr size_t nx_ = 4; static constexpr size_t nu_ = 3; static constexpr size_t nc_ = 0; @@ -18,16 +31,7 @@ class PIPGSolverTest : public testing::Test { static constexpr size_t numConstraints = N_ * (nx_ + nc_); static constexpr bool verbose = true; - PIPGSolverTest() - : pipgSolver({ - 8, // nThreads - 50, // threadPriority - 30000, // maxNumIterations - 1e-10, // absoluteTolerance - 1e-3, // relativeTolerance - 1, // checkTerminationInterval - verbose, // displayShortSummary - }) { + PIPGSolverTest() : pipgSolver(configurePipg(8, 30000, 1e-10, 1e-3, verbose)) { srand(10); // Construct OCP problem From f166aea6b12a92581aa78bc52034b8fbb11e48f9 Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Wed, 27 Apr 2022 15:35:41 +0200 Subject: [PATCH 220/512] move OcpSize to ocs2_oc --- ocs2_oc/CMakeLists.txt | 1 + .../include/ocs2_oc/oc_problem}/OcpSize.h | 2 +- .../src/oc_problem}/OcpSize.cpp | 4 +- ocs2_pipg/CMakeLists.txt | 2 +- ocs2_pipg/include/ocs2_pipg/PIPG.h | 59 ++++++++----------- ocs2_pipg/package.xml | 1 + ocs2_pipg/src/OcpSize.cpp | 41 ------------- ocs2_pipg/src/PIPG.cpp | 44 +++++++++++--- ocs2_pipg/test/testPIPGSolver.cpp | 2 +- ocs2_sqp/hpipm_catkin/CMakeLists.txt | 2 +- .../include/hpipm_catkin/HpipmInterface.h | 2 +- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 1 + 12 files changed, 71 insertions(+), 90 deletions(-) rename {ocs2_sqp/hpipm_catkin/include/hpipm_catkin => ocs2_oc/include/ocs2_oc/oc_problem}/OcpSize.h (99%) rename {ocs2_sqp/hpipm_catkin/src => ocs2_oc/src/oc_problem}/OcpSize.cpp (97%) delete mode 100644 ocs2_pipg/src/OcpSize.cpp diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 0c99d0ee8..59b5001e9 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -53,6 +53,7 @@ add_library(${PROJECT_NAME} src/oc_data/Metrics.cpp src/oc_problem/OptimalControlProblem.cpp src/oc_problem/LoopshapingOptimalControlProblem.cpp + src/oc_problem/OcpSize.cpp src/oc_solver/SolverBase.cpp src/oc_problem/OptimalControlProblem.cpp src/rollout/PerformanceIndicesRollout.cpp diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h b/ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h similarity index 99% rename from ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h rename to ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h index 425644c90..414131f08 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h @@ -34,7 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> namespace ocs2 { -namespace hpipm_interface { +inline namespace hpipm_interface { /** * Size of the optimal control problem to be solved with the HpipmInterface diff --git a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp b/ocs2_oc/src/oc_problem/OcpSize.cpp similarity index 97% rename from ocs2_sqp/hpipm_catkin/src/OcpSize.cpp rename to ocs2_oc/src/oc_problem/OcpSize.cpp index 53d5263fb..cc42d063b 100644 --- a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp +++ b/ocs2_oc/src/oc_problem/OcpSize.cpp @@ -27,10 +27,10 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "hpipm_catkin/OcpSize.h" +#include "ocs2_oc/oc_problem/OcpSize.h" namespace ocs2 { -namespace hpipm_interface { +inline namespace hpipm_interface { bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept { // use && instead of &= to enable short-circuit evaluation diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_pipg/CMakeLists.txt index 520d6a4f0..bbc713bed 100644 --- a/ocs2_pipg/CMakeLists.txt +++ b/ocs2_pipg/CMakeLists.txt @@ -3,6 +3,7 @@ project(ocs2_pipg) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_core + ocs2_oc ocs2_qp_solver ) @@ -40,7 +41,6 @@ include_directories( ) add_library(${PROJECT_NAME} - src/OcpSize.cpp src/PIPG_Settings.cpp src/PIPG.cpp ) diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG.h b/ocs2_pipg/include/ocs2_pipg/PIPG.h index d3207b0ea..949d8bf42 100644 --- a/ocs2_pipg/include/ocs2_pipg/PIPG.h +++ b/ocs2_pipg/include/ocs2_pipg/PIPG.h @@ -1,45 +1,39 @@ #pragma once -#include "ocs2_pipg/OcpSize.h" -#include "ocs2_pipg/PIPG_Settings.h" +#include <string> + +#include <Eigen/Sparse> #include <ocs2_core/Types.h> #include <ocs2_core/misc/Benchmark.h> #include <ocs2_core/thread_support/ThreadPool.h> +#include <ocs2_oc/oc_problem/OcpSize.h> -#include <Eigen/Sparse> - -#include <algorithm> -#include <condition_variable> -#include <mutex> -#include <numeric> +#include "ocs2_pipg/PIPG_Settings.h" namespace ocs2 { +namespace pipg { +enum class SolverStatus { SUCCESS, MAX_ITER }; +inline std::string toString(SolverStatus s) { + switch (s) { + case SolverStatus::SUCCESS: + return std::string("SUCCESS"); + + case SolverStatus::MAX_ITER: + return std::string("MAX_ITER"); + + default: + throw std::invalid_argument("[pipg::toString] Invalid solver status."); + } +} +} // namespace pipg class Pipg { public: + using OcpSize = ocs2::OcpSize; using Settings = pipg::Settings; - using OcpSize = pipg::OcpSize; - // solver status - enum class SolverStatus { SUCCESS, MAX_ITER }; - static std::string status2string(SolverStatus s) { - std::string res; - switch (s) { - case SolverStatus::SUCCESS: - res = "SUCCESS"; - break; - - case SolverStatus::MAX_ITER: - res = "FAIL"; - break; - - default: - break; - } - return res; - } + using SolverStatus = ocs2::pipg::SolverStatus; - Pipg() = default; explicit Pipg(pipg::Settings pipgSettings); ~Pipg(); @@ -165,11 +159,7 @@ class Pipg { int getNumDynamicsConstraints() const { return numDynamicsConstraints; } - int getNumGeneralEqualityConstraints() const { - const auto totalNumberOfGeneralEqualityConstraints = - std::accumulate(ocpSize_.numIneqConstraints.begin(), ocpSize_.numIneqConstraints.end(), 0); - return totalNumberOfGeneralEqualityConstraints; - } + int getNumGeneralEqualityConstraints() const; void getStackedSolution(vector_t& res) const; @@ -193,6 +183,8 @@ class Pipg { const std::vector<ScalarFunctionQuadraticApproximation>& cost, const std::vector<VectorFunctionLinearApproximation>* constraints) const; + void verifyOcpSize(const OcpSize& ocpSize) const; + void runParallel(std::function<void(int)> taskFunction); void invSqrtInfNorm(const std::vector<VectorFunctionLinearApproximation>& dynamics, @@ -213,7 +205,6 @@ class Pipg { // data buffer for parallelized QP vector_array_t X_, W_, V_, U_; vector_array_t XNew_, UNew_, WNew_; - scalar_array_t startIndexArray_; // Problem size int numDecisionVariables; diff --git a/ocs2_pipg/package.xml b/ocs2_pipg/package.xml index ec355a7a6..b8e4728e3 100644 --- a/ocs2_pipg/package.xml +++ b/ocs2_pipg/package.xml @@ -10,6 +10,7 @@ <buildtool_depend>catkin</buildtool_depend> <depend>ocs2_core</depend> + <depend>ocs2_oc</depend> <!-- Test dependancy --> <depend>ocs2_qp_solver</depend> diff --git a/ocs2_pipg/src/OcpSize.cpp b/ocs2_pipg/src/OcpSize.cpp deleted file mode 100644 index 3a269bd54..000000000 --- a/ocs2_pipg/src/OcpSize.cpp +++ /dev/null @@ -1,41 +0,0 @@ -#include "ocs2_pipg/OcpSize.h" - -namespace ocs2 { -namespace pipg { - -bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept { - // use && instead of &= to enable short-circuit evaluation - bool same = lhs.numStages == rhs.numStages; - same = same && (lhs.numInputs == rhs.numInputs); - same = same && (lhs.numStates == rhs.numStates); - same = same && (lhs.numIneqConstraints == rhs.numIneqConstraints); - return same; -} - -OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<ScalarFunctionQuadraticApproximation>& cost, - const std::vector<VectorFunctionLinearApproximation>* constraints) { - const int numStages = dynamics.size(); - - OcpSize problemSize(dynamics.size()); - - // State inputs - for (int k = 0; k < numStages; k++) { - problemSize.numStates[k] = dynamics[k].dfdx.cols(); - problemSize.numInputs[k] = dynamics[k].dfdu.cols(); - } - problemSize.numStates[numStages] = dynamics[numStages - 1].dfdx.rows(); - problemSize.numInputs[numStages] = 0; - - // Constraints - if (constraints != nullptr) { - for (int k = 0; k < numStages + 1; k++) { - problemSize.numIneqConstraints[k] = (*constraints)[k].f.size(); - } - } - - return problemSize; -} - -} // namespace pipg -} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/src/PIPG.cpp b/ocs2_pipg/src/PIPG.cpp index 4bfbfb058..cc7906c51 100644 --- a/ocs2_pipg/src/PIPG.cpp +++ b/ocs2_pipg/src/PIPG.cpp @@ -1,7 +1,10 @@ #include "ocs2_pipg/PIPG.h" +#include <condition_variable> #include <iostream> +#include <mutex> +#include <numeric> namespace ocs2 { @@ -71,16 +74,17 @@ Pipg::SolverStatus Pipg::solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, co denseQPTimer_.endTimer(); result = z; + Pipg::SolverStatus status = isConverged ? Pipg::SolverStatus::SUCCESS : Pipg::SolverStatus::MAX_ITER; if (settings().displayShortSummary) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; - std::cerr << "\n++++++++++++++ PIPG-DenseQP " << (isConverged ? "Success" : "Fail") << " +++++++++++++"; + std::cerr << "\n++++++++++++++ PIPG-DenseQP " << pipg::toString(status) << " +++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; std::cerr << "Number of Iterations: " << k << " out of " << settings().maxNumIterations << "\n"; std::cerr << "Relative change of primal solution : " << std::sqrt((z - z_old).squaredNorm() / zNorm) << "\n"; std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; std::cerr << "Run time: " << denseQPTimer_.getLastIntervalInMilliseconds() << "\n"; } - return Pipg::SolverStatus::SUCCESS; + return status; }; Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, @@ -357,10 +361,11 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect parallelizedQPTimer_.endTimer(); + Pipg::SolverStatus status = isConverged ? Pipg::SolverStatus::SUCCESS : Pipg::SolverStatus::MAX_ITER; if (settings().displayShortSummary) { scalar_t totalTasks = std::accumulate(threadsWorkloadCounter.cbegin(), threadsWorkloadCounter.cend(), 0.0); std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; - std::cerr << "\n++++++++++++++ PIPG-Parallel " << (isConverged ? "Success" : "Fail") << " +++++++++++++"; + std::cerr << "\n++++++++++++++ PIPG-Parallel " << pipg::toString(status) << " +++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; std::cerr << "Number of Iterations: " << k << " out of " << settings().maxNumIterations << "\n"; std::cerr << "Relative change of primal solution : " << std::sqrt(solutionSSE / solutionSquaredNorm) << "\n"; @@ -375,7 +380,7 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect Eigen::setNbThreads(0); // Restore default setup. - return Pipg::SolverStatus::SUCCESS; + return status; }; void Pipg::calculatePreConditioningFactors(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, @@ -832,14 +837,11 @@ void Pipg::resize(const OcpSize& ocpSize) { if (ocpSize_ == ocpSize) { return; } + verifyOcpSize(ocpSize); ocpSize_ = ocpSize; const int N = ocpSize_.numStages; - startIndexArray_.resize(N); - startIndexArray_.front() = 0; - std::partial_sum(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end() - 1, startIndexArray_.begin() + 1); - numDecisionVariables = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); numDecisionVariables += std::accumulate(ocpSize_.numInputs.begin(), ocpSize_.numInputs.end(), 0); numDynamicsConstraints = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); @@ -931,6 +933,26 @@ void Pipg::verifySizes(const std::vector<VectorFunctionLinearApproximation>& dyn } } +void Pipg::verifyOcpSize(const OcpSize& ocpSize) const { + auto isNotEmpty = [](const std::vector<int>& v) { return std::any_of(v.cbegin(), v.cend(), [](int s) { return s != 0; }); }; + + if (isNotEmpty(ocpSize.numInputBoxConstraints)) { + throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support input box constraints."); + } + if (isNotEmpty(ocpSize.numStateBoxConstraints)) { + throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support state box constraints."); + } + if (isNotEmpty(ocpSize.numInputBoxSlack)) { + throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support input slack variables."); + } + if (isNotEmpty(ocpSize.numStateBoxSlack)) { + throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support state slack variables."); + } + if (isNotEmpty(ocpSize.numIneqSlack)) { + throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support inequality slack variables."); + } +} + void Pipg::getConstraintMatrix(const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, VectorFunctionLinearApproximation& res) const { @@ -1389,6 +1411,12 @@ void Pipg::GGTAbsRowSumInParallel(const std::vector<VectorFunctionLinearApproxim // */ } +int Pipg::getNumGeneralEqualityConstraints() const { + const auto totalNumberOfGeneralEqualityConstraints = + std::accumulate(ocpSize_.numIneqConstraints.begin(), ocpSize_.numIneqConstraints.end(), 0); + return totalNumberOfGeneralEqualityConstraints; +} + std::string Pipg::getBenchmarkingInformationDense() const { const auto step1v = step1v_.getTotalInMilliseconds(); const auto step2z = step2z_.getTotalInMilliseconds(); diff --git a/ocs2_pipg/test/testPIPGSolver.cpp b/ocs2_pipg/test/testPIPGSolver.cpp index c2e769f3c..5fd5d8dfe 100644 --- a/ocs2_pipg/test/testPIPGSolver.cpp +++ b/ocs2_pipg/test/testPIPGSolver.cpp @@ -46,7 +46,7 @@ class PIPGSolverTest : public testing::Test { costArray.push_back(lqProblem[N_].cost); constraintsArray.push_back(lqProblem[N_].constraints); - pipgSolver.resize(ocs2::pipg::extractSizesFromProblem(dynamicsArray, costArray, &constraintsArray)); + pipgSolver.resize(ocs2::extractSizesFromProblem(dynamicsArray, costArray, &constraintsArray)); pipgSolver.getCostMatrix(x0, costArray, costApproximation); pipgSolver.getConstraintMatrix(x0, dynamicsArray, nullptr, nullptr, constraintsApproximation); } diff --git a/ocs2_sqp/hpipm_catkin/CMakeLists.txt b/ocs2_sqp/hpipm_catkin/CMakeLists.txt index ed6443e6e..9cad4c92a 100644 --- a/ocs2_sqp/hpipm_catkin/CMakeLists.txt +++ b/ocs2_sqp/hpipm_catkin/CMakeLists.txt @@ -5,6 +5,7 @@ project(hpipm_catkin) find_package(catkin REQUIRED COMPONENTS blasfeo_catkin ocs2_core + ocs2_oc ocs2_qp_solver ) @@ -79,7 +80,6 @@ include_directories( add_library(${PROJECT_NAME} src/HpipmInterface.cpp src/HpipmInterfaceSettings.cpp - src/OcpSize.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h index c876217c7..2158e1503 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h @@ -36,9 +36,9 @@ extern "C" { } #include <ocs2_core/Types.h> +#include <ocs2_oc/oc_problem/OcpSize.h> #include "hpipm_catkin/HpipmInterfaceSettings.h" -#include "hpipm_catkin/OcpSize.h" namespace ocs2 { diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 298f527cc..6165b5321 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/control/FeedforwardController.h> #include <ocs2_core/control/LinearController.h> #include <ocs2_core/penalties/penalties/RelaxedBarrierPenalty.h> +#include <ocs2_oc/oc_problem/OcpSize.h> #include "ocs2_sqp/MultipleShootingInitialization.h" #include "ocs2_sqp/MultipleShootingTranscription.h" From 18e4f9bebf6a2487500cdf18c24f5d391ea30f47 Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Thu, 28 Apr 2022 19:24:43 +0200 Subject: [PATCH 221/512] move pre conditioning to ocs2_oc --- ocs2_oc/CMakeLists.txt | 24 +- .../oc_problem/OcpMatrixConstruction.h | 35 + .../include/ocs2_oc/pre_condition/Scaling.h | 68 ++ ocs2_oc/src/lintTarget.cpp | 7 + .../src/oc_problem/OcpMatrixConstruction.cpp | 381 ++++++++ ocs2_oc/src/pre_condition/Scaling.cpp | 519 +++++++++++ ocs2_oc/test/testMatrixConstruction.cpp | 65 ++ ocs2_oc/test/testScaling.cpp | 160 ++++ ocs2_pipg/include/ocs2_pipg/PIPG.h | 186 +--- ocs2_pipg/src/PIPG.cpp | 817 ------------------ ocs2_pipg/test/testPIPGSolver.cpp | 158 +--- 11 files changed, 1306 insertions(+), 1114 deletions(-) create mode 100644 ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h create mode 100644 ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h create mode 100644 ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp create mode 100644 ocs2_oc/src/pre_condition/Scaling.cpp create mode 100644 ocs2_oc/test/testMatrixConstruction.cpp create mode 100644 ocs2_oc/test/testScaling.cpp diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 59b5001e9..473685b88 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -54,8 +54,10 @@ add_library(${PROJECT_NAME} src/oc_problem/OptimalControlProblem.cpp src/oc_problem/LoopshapingOptimalControlProblem.cpp src/oc_problem/OcpSize.cpp - src/oc_solver/SolverBase.cpp src/oc_problem/OptimalControlProblem.cpp + src/oc_problem/OcpMatrixConstruction.cpp + src/oc_solver/SolverBase.cpp + src/pre_condition/Scaling.cpp src/rollout/PerformanceIndicesRollout.cpp src/rollout/RolloutBase.cpp src/rollout/RootFinder.cpp @@ -160,3 +162,23 @@ target_link_libraries(test_trajectory_spreading ${Boost_LIBRARIES} gtest_main ) + +catkin_add_gtest(test_matrix_construction + test/testMatrixConstruction.cpp +) +target_link_libraries(test_matrix_construction + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + gtest_main +) + +catkin_add_gtest(test_scaling + test/testScaling.cpp +) +target_link_libraries(test_scaling + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + gtest_main +) diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h b/ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h new file mode 100644 index 000000000..eecde465b --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h @@ -0,0 +1,35 @@ +#pragma once + +#include <Eigen/Sparse> + +#include <ocs2_core/Types.h> + +#include "ocs2_oc/oc_problem/OcpSize.h" + +namespace ocs2 { + +/** + * @brief Get liner constraint matrix G from the dynamics, cost and constraint arrays. + * + * @param ocpSize: The size of optimal control problem. + * @param x0: Initial state. + * @param dynamics: Dynamics array. + * @param constraints: Constraints array. + * @param scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. After + * scaling, they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param res: The resulting constraints approxmation. + */ +void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + VectorFunctionLinearApproximation& res); + +void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + Eigen::SparseMatrix<scalar_t>& G, vector_t& g); + +void getCostMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, + ScalarFunctionQuadraticApproximation& res); + +void getCostMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, + Eigen::SparseMatrix<scalar_t>& H, vector_t& h); +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h new file mode 100644 index 000000000..f38256295 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h @@ -0,0 +1,68 @@ +#pragma once + +#include <Eigen/Sparse> + +#include <ocs2_core/Types.h> +#include <ocs2_core/thread_support/ThreadPool.h> + +#include "ocs2_oc/oc_problem/OcpSize.h" + +namespace ocs2 { + +/** + * Calculate the scaling factor D, E and c, and scale the input matrix(vector) H, h and G in place. The scaled matrix(vector) are defined as + * \f[ + * \tilde{H} = cDHD, \tilde{h} = cDh, + * \tilde{G} = EGD, \tilde{g} = Eg + * \f] + * + * + * @param[in, out] H Cost hessian. + * @param[in, out] h Linear cost term. + * @param[in, out] G Linear constraint matrix. + * @param[in] iteration Number of iteration. + * @param[out] DOut Scaling factor D + * @param[out] EOut Scaling factor E + * @param[out] cOut Scaling factor c + */ +void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, + const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut); + +/** + * @brief Calculate the scaling factor D, E and c, and scale the input dynamics, cost data in place in parallel. + * + * + * @param[in] x0 Initial state + * @param[in] ocpSize The size of the oc problem. + * @param[in] iteration Number of iteration. + * @param[in, out] dynamics The dynamics array od all time points. + * @param[in, out] cost The cost array of all time points. + * @param[out] DOut + * @param[out] EOut + * @param[out] scalingVectors + * @param[out] cOut + * @param[in] threadPool External thread pool. + * @param[in] H For test only. Can be removed. + * @param[in] h For test only. Can be removed. + * @param[in] G For test only. Can be removed. + */ +void preConditioningInPlaceInParallel(const vector_t& x0, const OcpSize& ocpSize, const int iteration, + std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, + vector_array_t& scalingVectors, scalar_t& cOut, ThreadPool& threadPool, + const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G); +/** + * @brief Scale the dynamics and cost array and construct scaling vector array from the given scaling factors E, D and c. + * + * @param[in] ocpSize + * @param[in] D + * @param[in] E + * @param[in] c + * @param[in, out] dynamics + * @param[in, out] cost + * @param[out] scalingVectors + */ +void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, + std::vector<vector_t>& scalingVectors); +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 53bdd6639..ea9045841 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -4,10 +4,17 @@ // oc_data #include <ocs2_oc/oc_data/PrimalSolution.h> +// oc_problem +#include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> +#include <ocs2_oc/oc_problem/OcpSize.h> + // oc_solver #include <ocs2_oc/oc_solver/PerformanceIndex.h> #include <ocs2_oc/oc_solver/SolverBase.h> +// pre_condition +#include <ocs2_oc/pre_condition/Scaling.h> + // synchronized_module #include <ocs2_oc/synchronized_module/LoopshapingReferenceManager.h> #include <ocs2_oc/synchronized_module/LoopshapingSynchronizedModule.h> diff --git a/ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp b/ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp new file mode 100644 index 000000000..cf05ca7ad --- /dev/null +++ b/ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp @@ -0,0 +1,381 @@ +#include "ocs2_oc/oc_problem/OcpMatrixConstruction.h" + +#include <numeric> + +namespace ocs2 { +// Helper functions +namespace { +int getNumDecisionVariables(const OcpSize& ocpSize) { + return std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), + std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0)); +} + +int getNumDynamicsConstraints(const OcpSize& ocpSize) { + return std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0); +} + +int getNumGeneralEqualityConstraints(const OcpSize& ocpSize) { + return std::accumulate(ocpSize.numIneqConstraints.begin(), ocpSize.numIneqConstraints.end(), (int)0); +} +} // namespace + +void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + VectorFunctionLinearApproximation& res) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[getConstraintMatrix] The number of stages cannot be less than 1."); + } + if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { + throw std::runtime_error("[getConstraintMatrix] The size of scalingVectors doesn't match the number of stage."); + } + + // Preallocate full constraint matrix + auto& G = res.dfdx; + auto& g = res.f; + if (constraints != nullptr) { + G.setZero(getNumDynamicsConstraints(ocpSize) + getNumGeneralEqualityConstraints(ocpSize), getNumDecisionVariables(ocpSize)); + + } else { + G.setZero(getNumDynamicsConstraints(ocpSize), getNumDecisionVariables(ocpSize)); + } + g.setZero(G.rows()); + + // Initial state constraint + const int nu_0 = ocpSize.numInputs.front(); + const int nx_1 = ocpSize.numStates[1]; + if (scalingVectorsPtr == nullptr) { + G.topLeftCorner(nx_1, nu_0 + nx_1) << -dynamics.front().dfdu, matrix_t::Identity(nx_1, nx_1); + } else { + G.topLeftCorner(nx_1, nu_0 + nx_1) << -dynamics.front().dfdu, scalingVectorsPtr->front().asDiagonal().toDenseMatrix(); + } + + // k = 0. Absorb initial state into dynamics + // The initial state is removed from the decision variables + // The first dynamics becomes: + // x[1] = A[0]*x[0] + B[0]*u[0] + b[0] + // = B[0]*u[0] + (b[0] + A[0]*x[0]) + // = B[0]*u[0] + \tilde{b}[0] + // numState[0] = 0 --> No need to specify A[0] here + g.head(nx_1) = dynamics.front().f; + g.head(nx_1).noalias() += dynamics.front().dfdx * x0; + + int currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const auto& dynamics_k = dynamics[k]; + // const auto& constraints_k = constraints; + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + const int nx_next = ocpSize.numStates[k + 1]; + + // Add [-A, -B, I(C)] + if (scalingVectorsPtr == nullptr) { + G.block(currRow, currCol, nx_next, nx_k + nu_k + nx_next) << -dynamics_k.dfdx, -dynamics_k.dfdu, matrix_t::Identity(nx_next, nx_next); + } else { + G.block(currRow, currCol, nx_next, nx_k + nu_k + nx_next) << -dynamics_k.dfdx, -dynamics_k.dfdu, + (*scalingVectorsPtr)[k].asDiagonal().toDenseMatrix(); + } + + // Add [b] + g.segment(currRow, nx_next) = dynamics_k.f; + + currRow += nx_next; + currCol += nx_k + nu_k; + } + + if (constraints != nullptr) { + currCol = nu_0; + // === Constraints === + // for ocs2 --> C*dx + D*du + e = 0 + // for pipg --> C*dx + D*du = -e + // Initial general constraints + const int nc_0 = ocpSize.numIneqConstraints.front(); + if (nc_0 > 0) { + const auto& constraint_0 = (*constraints).front(); + G.block(currRow, 0, nc_0, nu_0) = constraint_0.dfdu; + g.segment(currRow, nc_0) = -constraint_0.f; + g.segment(currRow, nc_0) -= constraint_0.dfdx * x0; + currRow += nc_0; + } + + for (int k = 1; k < N; ++k) { + const int nc_k = ocpSize.numIneqConstraints[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + if (nc_k > 0) { + const auto& constraints_k = (*constraints)[k]; + + // Add [C, D, 0] + G.block(currRow, currCol, nc_k, nx_k + nu_k) << constraints_k.dfdx, constraints_k.dfdu; + // Add [-e] + g.segment(currRow, nc_k) = -constraints_k.f; + currRow += nc_k; + } + + currCol += nx_k + nu_k; + } + + // Final general constraint + const int nc_N = ocpSize.numIneqConstraints[N]; + if (nc_N > 0) { + const auto& constraints_N = (*constraints)[N]; + G.bottomRightCorner(nc_N, constraints_N.dfdx.cols()) = constraints_N.dfdx; + g.tail(nc_N) = -constraints_N.f; + } + } +} + +void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + Eigen::SparseMatrix<scalar_t>& G, vector_t& g) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[getConstraintMatrixSparse] The number of stages cannot be less than 1."); + } + if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { + throw std::runtime_error("[getConstraintMatrixSparse] The size of scalingVectors doesn't match the number of stage."); + } + + const int nu_0 = ocpSize.numInputs[0]; + const int nx_1 = ocpSize.numStates[1]; + const int nx_N = ocpSize.numStates[N]; + + int nnz = nx_1 * (nu_0 + nx_1); + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nx_next = ocpSize.numStates[k + 1]; + nnz += nx_next * (nx_k + nu_k + nx_next); + } + + if (constraints != nullptr) { + const int nc_0 = ocpSize.numIneqConstraints[0]; + nnz += nc_0 * nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nc_k = ocpSize.numIneqConstraints[k]; + nnz += nc_k * (nx_k + nu_k); + } + const int nc_N = ocpSize.numIneqConstraints[N]; + + nnz += nc_N * nx_N; + } + std::vector<Eigen::Triplet<scalar_t>> tripletList; + tripletList.reserve(nnz); + + auto emplaceBackMatrix = [&tripletList](const int startRow, const int startCol, const matrix_t& mat) { + for (int j = 0; j < mat.cols(); j++) { + for (int i = 0; i < mat.rows(); i++) { + if (mat(i, j) != 0) { + tripletList.emplace_back(startRow + i, startCol + j, mat(i, j)); + } + } + } + }; + + if (constraints != nullptr) { + G.resize(getNumDynamicsConstraints(ocpSize) + getNumGeneralEqualityConstraints(ocpSize), getNumDecisionVariables(ocpSize)); + } else { + G.resize(getNumDynamicsConstraints(ocpSize), getNumDecisionVariables(ocpSize)); + } + g.setZero(G.rows()); + + // Initial state constraint + emplaceBackMatrix(0, 0, -dynamics.front().dfdu); + if (scalingVectorsPtr == nullptr) { + emplaceBackMatrix(0, nu_0, matrix_t::Identity(nx_1, nx_1)); + } else { + emplaceBackMatrix(0, nu_0, scalingVectorsPtr->front().asDiagonal()); + } + + // k = 0. Absorb initial state into dynamics + // The initial state is removed from the decision variables + // The first dynamics becomes: + // x[1] = A[0]*x[0] + B[0]*u[0] + b[0] + // = B[0]*u[0] + (b[0] + A[0]*x[0]) + // = B[0]*u[0] + \tilde{b}[0] + // numState[0] = 0 --> No need to specify A[0] here + g.head(nx_1) = dynamics.front().f; + g.head(nx_1).noalias() += dynamics.front().dfdx * x0; + + int currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const auto& dynamics_k = dynamics[k]; + // const auto& constraints_k = constraints; + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nx_next = ocpSize.numStates[k + 1]; + + // Add [-A, -B, I] + emplaceBackMatrix(currRow, currCol, -dynamics_k.dfdx); + emplaceBackMatrix(currRow, currCol + nx_k, -dynamics_k.dfdu); + if (scalingVectorsPtr == nullptr) { + emplaceBackMatrix(currRow, currCol + nx_k + nu_k, matrix_t::Identity(nx_next, nx_next)); + } else { + emplaceBackMatrix(currRow, currCol + nx_k + nu_k, (*scalingVectorsPtr)[k].asDiagonal()); + } + + // Add [b] + g.segment(currRow, nx_next) = dynamics_k.f; + + currRow += nx_next; + currCol += nx_k + nu_k; + } + + if (constraints != nullptr) { + currCol = nu_0; + // === Constraints === + // for ocs2 --> C*dx + D*du + e = 0 + // for pipg --> C*dx + D*du = -e + // Initial general constraints + const int nc_0 = ocpSize.numIneqConstraints.front(); + if (nc_0 > 0) { + const auto& constraint_0 = (*constraints).front(); + emplaceBackMatrix(currRow, 0, constraint_0.dfdu); + + g.segment(currRow, nc_0) = -constraint_0.f; + g.segment(currRow, nc_0) -= constraint_0.dfdx * x0; + currRow += nc_0; + } + + for (int k = 1; k < N; ++k) { + const int nc_k = ocpSize.numIneqConstraints[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + if (nc_k > 0) { + const auto& constraints_k = (*constraints)[k]; + + // Add [C, D, 0] + emplaceBackMatrix(currRow, currCol, constraints_k.dfdx); + emplaceBackMatrix(currRow, currCol + nx_k, constraints_k.dfdu); + // Add [-e] + g.segment(currRow, nc_k) = -constraints_k.f; + currRow += nc_k; + } + + currCol += nx_k + nu_k; + } + + // Final general constraint + const int nc_N = ocpSize.numIneqConstraints[N]; + if (nc_N > 0) { + const auto& constraints_N = (*constraints)[N]; + emplaceBackMatrix(currRow, currCol, constraints_N.dfdx); + g.segment(currRow, nc_N) = -constraints_N.f; + } + } + + G.setFromTriplets(tripletList.begin(), tripletList.end()); + assert(G.nonZeros() <= nnz); +} + +void getCostMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, + ScalarFunctionQuadraticApproximation& res) { + const int N = ocpSize.numStages; + + // Preallocate full Cost matrices + auto& H = res.dfdxx; + auto& h = res.dfdx; + auto& c = res.f; + H.setZero(getNumDecisionVariables(ocpSize), getNumDecisionVariables(ocpSize)); + h.setZero(H.cols()); + c = 0.0; + + // k = 0. Elimination of initial state requires cost adaptation + vector_t r_0 = cost[0].dfdu; + r_0 += cost[0].dfdux * x0; + + const int nu_0 = ocpSize.numInputs[0]; + H.topLeftCorner(nu_0, nu_0) = cost[0].dfduu; + h.head(nu_0) = r_0; + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + + // Add [ Q, P' + // P, Q ] + H.block(currRow, currRow, nx_k + nu_k, nx_k + nu_k) << cost[k].dfdxx, cost[k].dfdux.transpose(), cost[k].dfdux, cost[k].dfduu; + + // Add [ q, r] + h.segment(currRow, nx_k + nu_k) << cost[k].dfdx, cost[k].dfdu; + + // Add nominal cost + c += cost[k].f; + + currRow += nx_k + nu_k; + } + + const int nx_N = ocpSize.numStates[N]; + H.bottomRightCorner(nx_N, nx_N) = cost[N].dfdxx; + h.tail(nx_N) = cost[N].dfdx; + c += cost[N].f; +} + +void getCostMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, + Eigen::SparseMatrix<scalar_t>& H, vector_t& h) { + const int N = ocpSize.numStages; + + const int nu_0 = ocpSize.numInputs[0]; + int nnz = nu_0 * nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + nnz += (nx_k + nu_k) * (nx_k + nu_k); + } + const int nx_N = ocpSize.numStates[N]; + + nnz += nx_N * nx_N; + std::vector<Eigen::Triplet<scalar_t>> tripletList; + tripletList.reserve(nnz); + + auto emplaceBackMatrix = [&tripletList](const int startRow, const int startCol, const matrix_t& mat) { + for (int j = 0; j < mat.cols(); j++) { + for (int i = 0; i < mat.rows(); i++) { + if (mat(i, j) != 0) { + tripletList.emplace_back(startRow + i, startCol + j, mat(i, j)); + } + } + } + }; + + h.setZero(getNumDecisionVariables(ocpSize)); + + // k = 0. Elimination of initial state requires cost adaptation + vector_t r_0 = cost[0].dfdu; + r_0 += cost[0].dfdux * x0; + + // R0 + emplaceBackMatrix(0, 0, cost[0].dfduu); + h.head(nu_0) = r_0; + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + + // Add [ Q, P' + // P, Q ] + emplaceBackMatrix(currRow, currRow, cost[k].dfdxx); + emplaceBackMatrix(currRow, currRow + nx_k, cost[k].dfdux.transpose()); + emplaceBackMatrix(currRow + nx_k, currRow, cost[k].dfdux); + emplaceBackMatrix(currRow + nx_k, currRow + nx_k, cost[k].dfduu); + + // Add [ q, r] + h.segment(currRow, nx_k + nu_k) << cost[k].dfdx, cost[k].dfdu; + + currRow += nx_k + nu_k; + } + + emplaceBackMatrix(currRow, currRow, cost[N].dfdxx); + h.tail(nx_N) = cost[N].dfdx; + + H.resize(getNumDecisionVariables(ocpSize), getNumDecisionVariables(ocpSize)); + H.setFromTriplets(tripletList.begin(), tripletList.end()); + assert(H.nonZeros() <= nnz); +} +} // namespace ocs2 diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/pre_condition/Scaling.cpp new file mode 100644 index 000000000..7f5c47fa4 --- /dev/null +++ b/ocs2_oc/src/pre_condition/Scaling.cpp @@ -0,0 +1,519 @@ +#include "ocs2_oc/pre_condition/Scaling.h" + +#include <atomic> +#include <numeric> + +namespace ocs2 { +// Internal helper functions +namespace { + +vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat) { + vector_t infNorm; + infNorm.setZero(mat.rows()); + + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + int i = it.row(); + infNorm(i) = std::max(infNorm(i), std::abs(it.value())); + } + } + return infNorm; +} + +template <typename T> +vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat) { + if (mat.rows() == 0 || mat.cols() == 0) { + return vector_t(0); + } else { + return mat.array().abs().matrix().rowwise().maxCoeff(); + } +} + +template <typename T, typename... Rest> +vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixBase<Rest>&... rest) { + vector_t temp = matrixInfNormRows(rest...); + if (mat.rows() == 0 || mat.cols() == 0) { + return temp; + } else if (temp.rows() != 0) { + return mat.array().abs().matrix().rowwise().maxCoeff().cwiseMax(temp); + } else { + return mat.array().abs().matrix().rowwise().maxCoeff(); + } +} + +vector_t matrixInfNormCols(const Eigen::SparseMatrix<scalar_t>& mat) { + vector_t infNorm; + infNorm.setZero(mat.cols()); + + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + infNorm(j) = std::max(infNorm(j), std::abs(it.value())); + } + } + return infNorm; +} + +template <typename T> +vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat) { + if (mat.rows() == 0 || mat.cols() == 0) { + return vector_t(0); + } else { + return mat.array().abs().matrix().colwise().maxCoeff().transpose(); + } +} + +template <typename T, typename... Rest> +vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixBase<Rest>&... rest) { + vector_t temp = matrixInfNormCols(rest...); + if (mat.rows() == 0 || mat.cols() == 0) { + return temp; + } else if (temp.rows() != 0) { + return mat.array().abs().matrix().colwise().maxCoeff().transpose().cwiseMax(temp); + } else { + return mat.array().abs().matrix().colwise().maxCoeff().transpose(); + } +} + +void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix<scalar_t>& mat) { + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + if (it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1) { + throw std::runtime_error("[scaleMatrixInPlace] it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1"); + } + it.valueRef() *= rowScale(it.row()) * colScale(j); + } + } +} + +template <typename T> +void scaleMatrixInPlace(const vector_t* rowScale, const vector_t* colScale, Eigen::MatrixBase<T>& mat) { + if (rowScale != nullptr) mat.array().colwise() *= rowScale->array(); + if (colScale != nullptr) mat *= colScale->asDiagonal(); +} + +void invSqrtInfNormInParallel(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& scalingVectors, + const int N, const std::function<void(vector_t&)>& limitScaling, vector_array_t& D, vector_array_t& E, + ThreadPool& threadPool) { + // Helper function + auto procedure = [&limitScaling](vector_t& dst, const vector_t& norm) { + dst = norm; + limitScaling(dst); + dst.array() = dst.array().sqrt().inverse(); + }; + + procedure(D[0], matrixInfNormCols(cost[0].dfduu, dynamics[0].dfdu)); + procedure(E[0], matrixInfNormRows(dynamics[0].dfdu, scalingVectors[0].asDiagonal().toDenseMatrix())); + + std::atomic_int timeStamp{1}; + auto task = [&](int workerId) { + int k; + while ((k = timeStamp++) < N) { + procedure(D[2 * k - 1], + matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux, scalingVectors[k - 1].asDiagonal().toDenseMatrix(), dynamics[k].dfdx)); + procedure(D[2 * k], matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu, dynamics[k].dfdu)); + procedure(E[k], matrixInfNormRows(dynamics[k].dfdx, dynamics[k].dfdu, scalingVectors[k].asDiagonal().toDenseMatrix())); + } + }; + + threadPool.runParallel(std::move(task), threadPool.numThreads() + 1U); + + procedure(D[2 * N - 1], matrixInfNormCols(cost[N].dfdxx, scalingVectors[N - 1].asDiagonal().toDenseMatrix())); +} + +void scaleDataOneStepInPlaceInParallel(const vector_array_t& D, const vector_array_t& E, const int N, + std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors, + ThreadPool& threadPool) { + scaleMatrixInPlace(&(D[0]), &(D[0]), cost.front().dfduu); + scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdu); + scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdux); + + std::atomic_int timeStamp{1}; + auto scaleCost = [&](int workerId) { + int k; + while ((k = timeStamp++) < N) { + auto& Q = cost[k].dfdxx; + auto& R = cost[k].dfduu; + auto& P = cost[k].dfdux; + auto& q = cost[k].dfdx; + auto& r = cost[k].dfdu; + + scaleMatrixInPlace(&(D[2 * k - 1]), &(D[2 * k - 1]), Q); + scaleMatrixInPlace(&(D[2 * k - 1]), nullptr, q); + scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k - 1]), P); + + scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k]), R); + scaleMatrixInPlace(&(D[2 * k]), nullptr, r); + } + }; + + threadPool.runParallel(std::move(scaleCost), threadPool.numThreads() + 1U); + + scaleMatrixInPlace(&(D.back()), &(D.back()), cost[N].dfdxx); + scaleMatrixInPlace(&(D.back()), nullptr, cost[N].dfdx); + + // Scale G & g + auto& B_0 = dynamics[0].dfdu; + auto& C_0 = scalingVectors[0]; + /** + * \f$ + * \tilde{B} = E * B * D, + * scaling = E * I * D, + * \tilde{g} = E * g, + * \f$ + */ + scaleMatrixInPlace(&(E[0]), &(D[0]), B_0); + C_0.array() *= E[0].array() * D[1].array(); + + scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].dfdx); + scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].f); + + timeStamp = 1; + auto scaleConstraints = [&](int workerId) { + int k; + while ((k = timeStamp++) < N) { + auto& A_k = dynamics[k].dfdx; + auto& B_k = dynamics[k].dfdu; + auto& b_k = dynamics[k].f; + auto& C_k = scalingVectors[k]; + + scaleMatrixInPlace(&(E[k]), &(D[2 * k - 1]), A_k); + scaleMatrixInPlace(&(E[k]), &(D[2 * k]), B_k); + C_k.array() *= E[k].array() * D[2 * k + 1].array(); + + scaleMatrixInPlace(&(E[k]), nullptr, b_k); + } + }; + + threadPool.runParallel(std::move(scaleConstraints), threadPool.numThreads() + 1U); +} +} // namespace + +void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, + const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut) { + const int nz = H.rows(); + const int nc = G.rows(); + + // Init output + cOut = 1.0; + DOut.setOnes(nz); + EOut.setOnes(nc); + + for (int i = 0; i < iteration; i++) { + vector_t D, E; + const vector_t infNormOfHCols = matrixInfNormCols(H); + const vector_t infNormOfGCols = matrixInfNormCols(G); + + D = infNormOfHCols.array().max(infNormOfGCols.array()); + E = matrixInfNormRows(G); + + for (int i = 0; i < D.size(); i++) { + if (D(i) > 1e+4) D(i) = 1e+4; + if (D(i) < 1e-4) D(i) = 1.0; + } + + for (int i = 0; i < E.size(); i++) { + if (E(i) > 1e+4) E(i) = 1e+4; + if (E(i) < 1e-4) E(i) = 1.0; + } + + D = D.array().sqrt().inverse(); + E = E.array().sqrt().inverse(); + + scaleMatrixInPlace(D, D, H); + + scaleMatrixInPlace(E, D, G); + + h.array() *= D.array(); + + DOut = DOut.cwiseProduct(D); + EOut = EOut.cwiseProduct(E); + + scalar_t infNormOfh = h.lpNorm<Eigen::Infinity>(); + if (infNormOfh < 1e-4) infNormOfh = 1.0; + + const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); + scalar_t c_temp = std::max(infNormOfHColsUpdated.mean(), infNormOfh); + if (c_temp > 1e+4) c_temp = 1e+4; + if (c_temp < 1e-4) c_temp = 1.0; + + scalar_t gamma = 1.0 / c_temp; + + H *= gamma; + h *= gamma; + + cOut *= gamma; + } +} + +void preConditioningInPlaceInParallel(const vector_t& x0, const OcpSize& ocpSize, const int iteration, + std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, + vector_array_t& scalingVectors, scalar_t& cOut, ThreadPool& threadPool, + const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[preConditioningInPlaceInParallel] The number of stages cannot be less than 1."); + } + + auto limitScaling = [](vector_t& vec) { + for (int i = 0; i < vec.size(); i++) { + if (vec(i) > 1e+4) vec(i) = 1e+4; + if (vec(i) < 1e-4) vec(i) = 1.0; + } + }; + + // Init output + cOut = 1.0; + DOut.resize(2 * N); + EOut.resize(N); + scalingVectors.resize(N); + for (int i = 0; i < N; i++) { + DOut[2 * i].setOnes(ocpSize.numInputs[i]); + DOut[2 * i + 1].setOnes(ocpSize.numStates[i + 1]); + EOut[i].setOnes(ocpSize.numStates[i + 1]); + scalingVectors[i].setOnes(ocpSize.numStates[i + 1]); + } + + // /** + // * Test start + // */ + // Eigen::SparseMatrix<scalar_t> HTest = H; + // vector_t hTest = h; + // Eigen::SparseMatrix<scalar_t> GTest = G; + // /** + // * Test end + // */ + + vector_array_t D(2 * N), E(N); + for (int i = 0; i < iteration; i++) { + invSqrtInfNormInParallel(dynamics, cost, scalingVectors, N, limitScaling, D, E, threadPool); + scaleDataOneStepInPlaceInParallel(D, E, N, dynamics, cost, scalingVectors, threadPool); + + for (int k = 0; k < DOut.size(); k++) { + DOut[k].array() *= D[k].array(); + } + for (int k = 0; k < EOut.size(); k++) { + EOut[k].array() *= E[k].array(); + } + + scalar_t infNormOfh = (cost.front().dfdu + cost.front().dfdux * x0).lpNorm<Eigen::Infinity>(); + for (int k = 1; k < N; k++) { + infNormOfh = std::max(infNormOfh, std::max(cost[k].dfdx.lpNorm<Eigen::Infinity>(), cost[k].dfdu.lpNorm<Eigen::Infinity>())); + } + infNormOfh = std::max(infNormOfh, cost[N].dfdx.lpNorm<Eigen::Infinity>()); + if (infNormOfh < 1e-4) infNormOfh = 1.0; + + scalar_t sumOfInfNormOfH = matrixInfNormCols(cost[0].dfduu).derived().sum(); + for (int k = 1; k < N; k++) { + sumOfInfNormOfH += matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux).derived().sum(); + sumOfInfNormOfH += matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu).derived().sum(); + } + sumOfInfNormOfH += matrixInfNormCols(cost[N].dfdxx).derived().sum(); + + int numDecisionVariables = std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), + std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0)); + + scalar_t c_temp = std::max(sumOfInfNormOfH / static_cast<scalar_t>(numDecisionVariables), infNormOfh); + if (c_temp > 1e+4) c_temp = 1e+4; + if (c_temp < 1e-4) c_temp = 1.0; + + scalar_t gamma = 1.0 / c_temp; + + for (int k = 0; k <= N; ++k) { + cost[k].dfdxx *= gamma; + cost[k].dfduu *= gamma; + cost[k].dfdux *= gamma; + cost[k].dfdx *= gamma; + cost[k].dfdu *= gamma; + } + + cOut *= gamma; + + // /** + // * Test start + // */ + // auto isEqual = [](scalar_t a, scalar_t b) -> bool { + // return std::abs(a - b) < std::numeric_limits<scalar_t>::epsilon() * std::min(std::abs(a), std::abs(b)); + // }; + // vector_t DTest, ETest; + // vector_t infNormOfHCols = matrixInfNormCols(HTest); + // vector_t infNormOfGCols = matrixInfNormCols(GTest); + + // DTest = infNormOfHCols.array().max(infNormOfGCols.array()); + // ETest = matrixInfNormRows(GTest); + + // limitScaling(DTest); + // limitScaling(ETest); + + // std::cerr << "DTest: " << DTest.transpose() << "\n"; + // std::cerr << "ETest : " << ETest.transpose() << "\n"; + // std::cerr << "G :\n " << GTest.toDense() << "\n"; + + // DTest = DTest.array().sqrt().inverse(); + // ETest = ETest.array().sqrt().inverse(); + + // scaleMatrixInPlace(DTest, DTest, HTest); + + // scaleMatrixInPlace(ETest, DTest, GTest); + + // hTest.array() *= DTest.array(); + + // scalar_t infNormOfhTest = hTest.lpNorm<Eigen::Infinity>(); + // if (infNormOfhTest < 1e-4) infNormOfhTest = 1.0; + + // const vector_t infNormOfHColsUpdated = matrixInfNormCols(HTest); + // scalar_t c_tempTest = std::max(infNormOfHColsUpdated.mean(), infNormOfhTest); + // if (c_tempTest > 1e+4) c_tempTest = 1e+4; + // if (c_tempTest < 1e-4) c_tempTest = 1.0; + + // scalar_t gammaTest = 1.0 / c_tempTest; + + // HTest *= gamma; + // hTest *= gamma; + + // ocs2::vector_t DStacked(H.cols()), EStacked(G.rows()); + // int curRow = 0; + // for (auto& v : D) { + // DStacked.segment(curRow, v.size()) = v; + // curRow += v.size(); + // } + // curRow = 0; + // for (auto& v : E) { + // EStacked.segment(curRow, v.size()) = v; + // curRow += v.size(); + // } + // if (!DStacked.isApprox(DTest)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "DStacked: " << DStacked.transpose() << "\n"; + // std::cerr << "DTest : " << DTest.transpose() << "\n"; + // std::cerr << "diff : " << (DStacked - DTest).transpose() << std::endl; + // std::cerr << "R: \n" << cost[0].dfduu << std::endl; + // std::cerr << "B: \n" << dynamics[0].dfdu << std::endl; + // } + // if (!EStacked.isApprox(ETest)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "EStacked: " << EStacked.transpose() << "\n"; + // std::cerr << "ETest: " << ETest.transpose() << "\n"; + // std::cerr << "diff: " << (ETest - EStacked).transpose() << std::endl; + // } + // if (!isEqual(infNormOfhTest, infNormOfh)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "infNormOfhTest: " << infNormOfhTest << "\n"; + // std::cerr << "infNormOfh: " << infNormOfh << "\n"; + // std::cerr << "diff: " << (infNormOfhTest - infNormOfh) << std::endl; + // } + // if (!isEqual(infNormOfHColsUpdated.sum(), sumOfInfNormOfH)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "infNormOfHColsUpdated.sum(): " << infNormOfHColsUpdated.sum() << "\n"; + // std::cerr << "sumOfInfNormOfH: " << sumOfInfNormOfH << "\n"; + // std::cerr << "diff: " << (infNormOfHColsUpdated.sum() - sumOfInfNormOfH) << std::endl; + // } + // if (!isEqual(c_temp, c_tempTest)) { + // std::cerr << "i: " << i << "\n"; + // std::cerr << "c_temp: " << c_temp << "\n"; + // std::cerr << "c_tempTest: " << c_tempTest << "\n"; + // std::cerr << "diff: " << (c_temp - c_tempTest) << std::endl; + // } + // /** + // * Test end + // */ + } +} + +void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, + std::vector<vector_t>& scalingVectors) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + + scalingVectors.resize(N); + + // Scale H & h + const int nu_0 = ocpSize.numInputs.front(); + // Scale row - Pre multiply D + cost.front().dfduu.array().colwise() *= c * D.head(nu_0).array(); + // Scale col - Post multiply D + cost.front().dfduu *= D.head(nu_0).asDiagonal(); + + cost.front().dfdu.array() *= c * D.head(nu_0).array(); + + cost.front().dfdux.array().colwise() *= c * D.head(nu_0).array(); + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + auto& Q = cost[k].dfdxx; + auto& R = cost[k].dfduu; + auto& P = cost[k].dfdux; + auto& q = cost[k].dfdx; + auto& r = cost[k].dfdu; + + Q.array().colwise() *= c * D.segment(currRow, nx_k).array(); + Q *= D.segment(currRow, nx_k).asDiagonal(); + q.array() *= c * D.segment(currRow, nx_k).array(); + + P *= D.segment(currRow, nx_k).asDiagonal(); + currRow += nx_k; + + R.array().colwise() *= c * D.segment(currRow, nu_k).array(); + R *= D.segment(currRow, nu_k).asDiagonal(); + r.array() *= c * D.segment(currRow, nu_k).array(); + + P.array().colwise() *= c * D.segment(currRow, nu_k).array(); + + currRow += nu_k; + } + + const int nx_N = ocpSize.numStates[N]; + cost[N].dfdxx.array().colwise() *= c * D.tail(nx_N).array(); + cost[N].dfdxx *= D.tail(nx_N).asDiagonal(); + cost[N].dfdx.array() *= c * D.tail(nx_N).array(); + + // Scale G & g + auto& B_0 = dynamics[0].dfdu; + auto& C_0 = scalingVectors[0]; + const int nx_1 = ocpSize.numStates[1]; + + // \tilde{B} = E * B * D, + // scaling = E * D, + // \tilde{g} = E * g + B_0.array().colwise() *= E.head(nx_1).array(); + B_0 *= D.head(nu_0).asDiagonal(); + + C_0 = E.head(nx_1).array() * D.segment(nu_0, nx_1).array(); + + dynamics[0].dfdx.array().colwise() *= E.head(nx_1).array(); + dynamics[0].f.array() *= E.head(nx_1).array(); + + currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + const int nx_next = ocpSize.numStates[k + 1]; + auto& A_k = dynamics[k].dfdx; + auto& B_k = dynamics[k].dfdu; + auto& b_k = dynamics[k].f; + auto& C_k = scalingVectors[k]; + + A_k.array().colwise() *= E.segment(currRow, nx_next).array(); + A_k *= D.segment(currCol, nx_k).asDiagonal(); + + B_k.array().colwise() *= E.segment(currRow, nx_next).array(); + B_k *= D.segment(currCol + nx_k, nu_k).asDiagonal(); + + C_k = E.segment(currRow, nx_next).array() * D.segment(currCol + nx_k + nu_k, nx_next).array(); + + b_k.array() *= E.segment(currRow, nx_next).array(); + + currRow += nx_next; + currCol += nx_k + nu_k; + } +} + +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/test/testMatrixConstruction.cpp b/ocs2_oc/test/testMatrixConstruction.cpp new file mode 100644 index 000000000..0911b3049 --- /dev/null +++ b/ocs2_oc/test/testMatrixConstruction.cpp @@ -0,0 +1,65 @@ +#include <gtest/gtest.h> + +#include "ocs2_oc/oc_problem/OcpMatrixConstruction.h" + +#include "ocs2_oc/oc_problem/OcpSize.h" +#include "ocs2_oc/test/testProblemsGeneration.h" + +class MatrixConstructionTest : public testing::Test { + protected: + // x_0, x_1, ... x_{N - 1}, X_{N} + static constexpr size_t N_ = 10; // numStages + static constexpr size_t nx_ = 4; + static constexpr size_t nu_ = 3; + static constexpr size_t nc_ = 5; + static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); + static constexpr size_t numConstraints = N_ * (nx_ + nc_); + + MatrixConstructionTest() { + srand(0); + + x0 = ocs2::vector_t::Random(nx_); + for (int i = 0; i < N_; i++) { + dynamicsArray.push_back(ocs2::getRandomDynamics(nx_, nu_)); + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + } + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + + ocpSize_ = ocs2::extractSizesFromProblem(dynamicsArray, costArray, &constraintsArray); + } + + ocs2::OcpSize ocpSize_; + ocs2::vector_t x0; + std::vector<ocs2::VectorFunctionLinearApproximation> dynamicsArray; + std::vector<ocs2::ScalarFunctionQuadraticApproximation> costArray; + std::vector<ocs2::VectorFunctionLinearApproximation> constraintsArray; +}; + +TEST_F(MatrixConstructionTest, sparseConstraintsApproximation) { + Eigen::SparseMatrix<ocs2::scalar_t> G; + ocs2::vector_t g; + ocs2::vector_array_t scalingVectors(N_); + for (auto& v : scalingVectors) { + v = ocs2::vector_t::Random(nx_); + } + ocs2::VectorFunctionLinearApproximation constraintsApproximation; + ocs2::getConstraintMatrix(ocpSize_, x0, dynamicsArray, &constraintsArray, &scalingVectors, constraintsApproximation); + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, &constraintsArray, &scalingVectors, G, g); + + EXPECT_TRUE(constraintsApproximation.dfdx.isApprox(G.toDense())); + EXPECT_TRUE(constraintsApproximation.f.isApprox(g)); +} + +TEST_F(MatrixConstructionTest, sparseCostApproximation) { + Eigen::SparseMatrix<ocs2::scalar_t> H; + ocs2::vector_t h; + + ocs2::ScalarFunctionQuadraticApproximation costApproximation; + ocs2::getCostMatrix(ocpSize_, x0, costArray, costApproximation); + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H, h); + + EXPECT_TRUE(costApproximation.dfdxx.isApprox(H.toDense())); + EXPECT_TRUE(costApproximation.dfdx.isApprox(h)); +} diff --git a/ocs2_oc/test/testScaling.cpp b/ocs2_oc/test/testScaling.cpp new file mode 100644 index 000000000..288a7205a --- /dev/null +++ b/ocs2_oc/test/testScaling.cpp @@ -0,0 +1,160 @@ +#include <gtest/gtest.h> + +#include <ocs2_core/thread_support/ThreadPool.h> + +#include "ocs2_oc/oc_problem/OcpMatrixConstruction.h" +#include "ocs2_oc/pre_condition/Scaling.h" +#include "ocs2_oc/test/testProblemsGeneration.h" + +class ScalingTest : public testing::Test { + protected: + // x_0, x_1, ... x_{N - 1}, X_{N} + static constexpr size_t N_ = 10; // numStages + static constexpr size_t nx_ = 4; + static constexpr size_t nu_ = 3; + static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); + static constexpr size_t numConstraints = N_ * nx_; + + ScalingTest() { + srand(0); + + x0 = ocs2::vector_t::Random(nx_); + for (int i = 0; i < N_; i++) { + dynamicsArray.push_back(ocs2::getRandomDynamics(nx_, nu_)); + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + } + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + + ocpSize_ = ocs2::extractSizesFromProblem(dynamicsArray, costArray, nullptr); + } + + ocs2::OcpSize ocpSize_; + ocs2::vector_t x0; + std::vector<ocs2::VectorFunctionLinearApproximation> dynamicsArray; + std::vector<ocs2::ScalarFunctionQuadraticApproximation> costArray; +}; + +TEST_F(ScalingTest, preConditioningSparseMatrix) { + ocs2::vector_t D, E; + ocs2::scalar_t c; + + Eigen::SparseMatrix<ocs2::scalar_t> H; + ocs2::vector_t h; + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H, h); + // Copy of the original matrix/vector + const Eigen::SparseMatrix<ocs2::scalar_t> H_src = H; + const ocs2::vector_t h_src = h; + + Eigen::SparseMatrix<ocs2::scalar_t> G; + ocs2::vector_t g; + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, nullptr, G, g); + // Copy of the original matrix/vector + const Eigen::SparseMatrix<ocs2::scalar_t> G_src = G; + const ocs2::vector_t g_src = g; + + // Test 1: Construct the stacked cost and constraints matrices first and scale next. + ocs2::preConditioningSparseMatrixInPlace(H, h, G, 5, D, E, c); + + // After pre-conditioning, H, h, G will be scaled in place. g remains the same. + const Eigen::SparseMatrix<ocs2::scalar_t> H_ref = c * D.asDiagonal() * H_src * D.asDiagonal(); + const ocs2::vector_t h_ref = c * D.asDiagonal() * h_src; + const Eigen::SparseMatrix<ocs2::scalar_t> G_ref = E.asDiagonal() * G_src * D.asDiagonal(); + const ocs2::vector_t g_ref = E.asDiagonal() * g_src; + + // The scaled matrix given by calculatePreConditioningFactors function and the one calculated with D, E and c factors should be the same. + ASSERT_TRUE(H_ref.isApprox(H)); // H + ASSERT_TRUE(h_ref.isApprox(h)); // h + ASSERT_TRUE(G_ref.isApprox(G)) << "G_ref:\n" << G_ref << "\nG:\n" << G.toDense(); // G + + // Normalized the inf-Norm of both rows and cols of KKT matrix should be closer to 1 + const ocs2::matrix_t KKT = (ocs2::matrix_t(H_src.rows() + G_src.rows(), H_src.rows() + G_src.rows()) << H_src.toDense(), + G_src.transpose().toDense(), G_src.toDense(), ocs2::matrix_t::Zero(G_src.rows(), G_src.rows())) + .finished(); + + const ocs2::matrix_t KKTScaled = (ocs2::matrix_t(H.rows() + G.rows(), H.rows() + G.rows()) << H.toDense(), G.transpose().toDense(), + G.toDense(), ocs2::matrix_t::Zero(G.rows(), G.rows())) + .finished(); + + // Inf-norm of row vectors + ocs2::vector_t infNormRows = KKT.cwiseAbs().rowwise().maxCoeff(); + ocs2::vector_t infNormRowsScaled = KKTScaled.cwiseAbs().rowwise().maxCoeff(); + EXPECT_LT((infNormRowsScaled.array() - 1).abs().maxCoeff(), (infNormRows.array() - 1).abs().maxCoeff()); + // Inf-norm of column vectors + ocs2::vector_t infNormCols = KKT.cwiseAbs().colwise().maxCoeff(); + ocs2::vector_t infNormColsScaled = KKTScaled.cwiseAbs().colwise().maxCoeff(); + EXPECT_LT((infNormColsScaled.array() - 1).abs().maxCoeff(), (infNormCols.array() - 1).abs().maxCoeff()); + + // Test 2: Scale const and dynamics data of each time step first and then construct the stacked matrices from the scaled data. + std::vector<ocs2::vector_t> scalingVectors; + ocs2::scaleDataInPlace(ocpSize_, D, E, c, dynamicsArray, costArray, scalingVectors); + + Eigen::SparseMatrix<ocs2::scalar_t> H_scaledData; + ocs2::vector_t h_scaledData; + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H_scaledData, h_scaledData); + Eigen::SparseMatrix<ocs2::scalar_t> G_scaledData; + ocs2::vector_t g_scaledData; + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, &scalingVectors, G_scaledData, g_scaledData); + EXPECT_TRUE(H_ref.isApprox(H_scaledData)); // H + EXPECT_TRUE(h_ref.isApprox(h_scaledData)); // h + EXPECT_TRUE(G_ref.isApprox(G_scaledData)); // G + EXPECT_TRUE(g_ref.isApprox(g_scaledData)); // g +} + +TEST_F(ScalingTest, preConditioningInPlaceInParallel) { + ocs2::ThreadPool threadPool(5, 99); + + ocs2::vector_t D_ref, E_ref; + ocs2::scalar_t c_ref; + + Eigen::SparseMatrix<ocs2::scalar_t> H_ref; + ocs2::vector_t h_ref; + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H_ref, h_ref); + // Copy of the original matrix/vector + const Eigen::SparseMatrix<ocs2::scalar_t> H_src = H_ref; + const ocs2::vector_t h_src = h_ref; + + Eigen::SparseMatrix<ocs2::scalar_t> G_ref; + ocs2::vector_t g_ref; + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, nullptr, G_ref, g_ref); + // Copy of the original matrix/vector + const Eigen::SparseMatrix<ocs2::scalar_t> G_src = G_ref; + const ocs2::vector_t g_src = g_ref; + // Generate reference + ocs2::preConditioningSparseMatrixInPlace(H_ref, h_ref, G_ref, 5, D_ref, E_ref, c_ref); + g_ref = E_ref.asDiagonal() * g_src; + + // Test start + ocs2::vector_array_t D_array, E_array; + ocs2::scalar_t c; + ocs2::vector_array_t scalingVectors(N_); + ocs2::preConditioningInPlaceInParallel(x0, ocpSize_, 5, dynamicsArray, costArray, D_array, E_array, scalingVectors, c, threadPool, H_src, + h_src, G_src); + + ocs2::vector_t D_stacked(D_ref.rows()), E_stacked(E_ref.rows()); + int curRow = 0; + for (auto& v : D_array) { + D_stacked.segment(curRow, v.size()) = v; + curRow += v.size(); + } + curRow = 0; + for (auto& v : E_array) { + E_stacked.segment(curRow, v.size()) = v; + curRow += v.size(); + } + + EXPECT_TRUE(D_stacked.isApprox(D_ref)); + EXPECT_TRUE(E_stacked.isApprox(E_ref)); + EXPECT_DOUBLE_EQ(c, c_ref); + + Eigen::SparseMatrix<ocs2::scalar_t> H_scaledData; + ocs2::vector_t h_scaledData; + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H_scaledData, h_scaledData); + Eigen::SparseMatrix<ocs2::scalar_t> G_scaledData; + ocs2::vector_t g_scaledData; + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, &scalingVectors, G_scaledData, g_scaledData); + + EXPECT_TRUE(H_ref.isApprox(H_scaledData)); // H + EXPECT_TRUE(h_ref.isApprox(h_scaledData)); // h + EXPECT_TRUE(G_ref.isApprox(G_scaledData)); // G + EXPECT_TRUE(g_ref.isApprox(g_scaledData)); // g +} diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG.h b/ocs2_pipg/include/ocs2_pipg/PIPG.h index 949d8bf42..6da3afd5e 100644 --- a/ocs2_pipg/include/ocs2_pipg/PIPG.h +++ b/ocs2_pipg/include/ocs2_pipg/PIPG.h @@ -34,127 +34,63 @@ class Pipg { using Settings = pipg::Settings; using SolverStatus = ocs2::pipg::SolverStatus; + /** + * @brief Construct a new Pipg with pipg setting object. + * + * @param pipgSettings: pipg setting + */ explicit Pipg(pipg::Settings pipgSettings); ~Pipg(); /** - * Solve generic QP problem - * - * @note For constraints, it is Gz = g + * @brief Solve generic QP problem. * - * @param cost: Hessian(H) and gradient(h). - * @param constraints: Linear equality constraint matrix(G) and value(g). - * @param mu - * @param lambda - * @param sigma - * @param result + * @param H: Cost hessian. + * @param h: Cost linear component. + * @param G: Linear equality constraint matrix. + * @param g: Linear equality constraint values + * @param EInv: Inverse of the scaling matrix E. Used to calculate un-sacled termination criteria. + * @param mu: the lower bound of the cost hessian H. + * @param lambda: the upper bound of the cost hessian H. + * @param sigma: the upper bound of \f$ G^TG \f$ + * @param result: Stacked result. * @return SolverStatus */ - SolverStatus solveDenseQP(const ScalarFunctionQuadraticApproximation& cost, const VectorFunctionLinearApproximation& constraints, - const vector_t& EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, vector_t& result) { - Eigen::SparseMatrix<scalar_t> H = cost.dfdxx.sparseView(); - auto& h = cost.dfdx; - Eigen::SparseMatrix<scalar_t> G = constraints.dfdx.sparseView(); - auto& g = constraints.f; - - return solveDenseQP(H, h, G, g, EInv, mu, lambda, sigma, result); - }; - SolverStatus solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, vector_t& result); + /** - * Solve Optimal Control type QP + * @brief Solve Optimal Control type QP in parallel. * - * @param x0: Initial state - * @param dynamics: Dynamics array with N elements(The findal stage do NOT require dynamics). - * @param cost: State and input cost array with N+1 elements. - * @param constraints: State-input linear equality constraints. It is optional. - * @param mu: Lower bound of hessian(H). - * @param lambda: Upper bound of hessian(H). - * @param sigma: Upper bound of constraint matrix(G'G). + * @param x0 Initial state + * @param dynamics + * @param cost + * @param constraints + * @param scalingVectors Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. After + * scaling, they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param EInv Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. + * @param mu + * @param lambda + * @param sigma + * @param costM For testing only. Can be removed. + * @param constraintsM For testing only. Can be removed. * @return SolverStatus */ SolverStatus solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, - const ScalarFunctionQuadraticApproximation& costM /**< [in] For testing only. */, - const VectorFunctionLinearApproximation& constraintsM /**< [in] For testing only. */); - - void calculatePreConditioningFactors(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, const int iteration, - vector_t& DOut, vector_t& EOut, scalar_t& cOut) const; - - void preConditioningInPlaceInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, const int iteration, vector_array_t& DOut, - vector_array_t& EOut, vector_array_t& scalingVectors, scalar_t& cOut, - const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G); + const ScalarFunctionQuadraticApproximation& costM, const VectorFunctionLinearApproximation& constraintsM); vector_t HAbsRowSumInParallel(const std::vector<ScalarFunctionQuadraticApproximation>& cost) const; - /** - * @brief Scale data array in-place. - * - * @param [in] D - * @param [in] E - * @param [in] c - * @param [in, out] dynamics - * @param [in, out] cost - * @param [out] scalingVectors - */ - void scaleDataInPlace(const vector_t& D, const vector_t& E, const scalar_t c, std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors) const; - /** - * Construct linear constraint matrix - * - * @param x0 - * @param dynamics - * @param constraints - * @param res - */ - void getConstraintMatrix(const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, - VectorFunctionLinearApproximation& res) const; - - void getConstraintMatrixSparse(const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, - Eigen::SparseMatrix<scalar_t>& G, vector_t& g) const; - /** - * @brief Get the Cost Matrix object - * - * @param x0 - * @param cost - * @param res - */ - void getCostMatrix(const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, - ScalarFunctionQuadraticApproximation& res) const; - - /** - * @brief Get the Cost Matrix Sparse object - * - * @param x0 - * @param cost - * @param H - * @param h - */ - void getCostMatrixSparse(const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, - Eigen::SparseMatrix<scalar_t>& H, vector_t& h) const; void GGTAbsRowSumInParallel(const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, vector_t& res); - /** - * @brief - * - * @param size - */ void resize(const OcpSize& size); - /** - * @brief Get the Num Stacked Variables object - * - * @return int - */ int getNumDecisionVariables() const { return numDecisionVariables; } int getNumDynamicsConstraints() const { return numDynamicsConstraints; } @@ -177,6 +113,7 @@ class Pipg { const Settings& settings() const { return pipgSettings_; } Settings& settings() { return pipgSettings_; } + const OcpSize& size() const { return ocpSize_; } private: void verifySizes(const std::vector<VectorFunctionLinearApproximation>& dynamics, @@ -187,14 +124,6 @@ class Pipg { void runParallel(std::function<void(int)> taskFunction); - void invSqrtInfNorm(const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& scalingVectors, const int N, - const std::function<void(vector_t&)>& limitScaling, vector_array_t& D, vector_array_t& E); - - void scaleDataOneStepInPlace(const vector_array_t& D, const vector_array_t& E, const int N, - std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors); - private: ThreadPool threadPool_; @@ -202,7 +131,7 @@ class Pipg { OcpSize ocpSize_; - // data buffer for parallelized QP + // Data buffer for parallelized QP vector_array_t X_, W_, V_, U_; vector_array_t XNew_, UNew_, WNew_; @@ -220,57 +149,6 @@ class Pipg { benchmark::RepeatedTimer step4CheckConvergence_; }; -vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat); -vector_t matrixInfNormCols(const Eigen::SparseMatrix<scalar_t>& mat); vector_t matrixRowwiseAbsSum(const Eigen::SparseMatrix<scalar_t>& mat); -void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix<scalar_t>& mat); - -template <typename T> -vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat) { - if (mat.rows() == 0 || mat.cols() == 0) { - return vector_t(0); - } else { - return mat.array().abs().matrix().rowwise().maxCoeff(); - } -} - -template <typename T, typename... Rest> -vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixBase<Rest>&... rest) { - vector_t temp = matrixInfNormRows(rest...); - if (mat.rows() == 0 || mat.cols() == 0) { - return temp; - } else if (temp.rows() != 0) { - return mat.array().abs().matrix().rowwise().maxCoeff().cwiseMax(temp); - } else { - return mat.array().abs().matrix().rowwise().maxCoeff(); - } -} - -template <typename T> -vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat) { - if (mat.rows() == 0 || mat.cols() == 0) { - return vector_t(0); - } else { - return mat.array().abs().matrix().colwise().maxCoeff().transpose(); - } -} - -template <typename T, typename... Rest> -vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixBase<Rest>&... rest) { - vector_t temp = matrixInfNormCols(rest...); - if (mat.rows() == 0 || mat.cols() == 0) { - return temp; - } else if (temp.rows() != 0) { - return mat.array().abs().matrix().colwise().maxCoeff().transpose().cwiseMax(temp); - } else { - return mat.array().abs().matrix().colwise().maxCoeff().transpose(); - } -} - -template <typename T> -void scaleMatrixInPlace(const vector_t* rowScale, const vector_t* colScale, Eigen::MatrixBase<T>& mat) { - if (rowScale != nullptr) mat.array().colwise() *= rowScale->array(); - if (colScale != nullptr) mat *= colScale->asDiagonal(); -} } // namespace ocs2 diff --git a/ocs2_pipg/src/PIPG.cpp b/ocs2_pipg/src/PIPG.cpp index cc7906c51..e07c9e6a1 100644 --- a/ocs2_pipg/src/PIPG.cpp +++ b/ocs2_pipg/src/PIPG.cpp @@ -383,329 +383,6 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect return status; }; -void Pipg::calculatePreConditioningFactors(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, - const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut) const { - const int nz = H.rows(); - const int nc = G.rows(); - - // Init output - cOut = 1.0; - DOut.setOnes(nz); - EOut.setOnes(nc); - - for (int i = 0; i < iteration; i++) { - vector_t D, E; - const vector_t infNormOfHCols = matrixInfNormCols(H); - const vector_t infNormOfGCols = matrixInfNormCols(G); - - D = infNormOfHCols.array().max(infNormOfGCols.array()); - E = matrixInfNormRows(G); - - for (int i = 0; i < D.size(); i++) { - if (D(i) > 1e+4) D(i) = 1e+4; - if (D(i) < 1e-4) D(i) = 1.0; - } - - for (int i = 0; i < E.size(); i++) { - if (E(i) > 1e+4) E(i) = 1e+4; - if (E(i) < 1e-4) E(i) = 1.0; - } - - D = D.array().sqrt().inverse(); - E = E.array().sqrt().inverse(); - - scaleMatrixInPlace(D, D, H); - - scaleMatrixInPlace(E, D, G); - - h.array() *= D.array(); - - DOut = DOut.cwiseProduct(D); - EOut = EOut.cwiseProduct(E); - - scalar_t infNormOfh = h.lpNorm<Eigen::Infinity>(); - if (infNormOfh < 1e-4) infNormOfh = 1.0; - - const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); - scalar_t c_temp = std::max(infNormOfHColsUpdated.mean(), infNormOfh); - if (c_temp > 1e+4) c_temp = 1e+4; - if (c_temp < 1e-4) c_temp = 1.0; - - scalar_t gamma = 1.0 / c_temp; - - H *= gamma; - h *= gamma; - - cOut *= gamma; - } -} - -void Pipg::invSqrtInfNorm(const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& scalingVectors, const int N, - const std::function<void(vector_t&)>& limitScaling, vector_array_t& D, vector_array_t& E) { - // Helper function - auto procedure = [&limitScaling](vector_t& dst, const vector_t& norm) { - dst = norm; - limitScaling(dst); - dst.array() = dst.array().sqrt().inverse(); - }; - - procedure(D[0], matrixInfNormCols(cost[0].dfduu, dynamics[0].dfdu)); - procedure(E[0], matrixInfNormRows(dynamics[0].dfdu, scalingVectors[0].asDiagonal().toDenseMatrix())); - - std::atomic_int timeStamp{1}; - auto task = [&](int workerId) { - int k; - while ((k = timeStamp++) < N) { - procedure(D[2 * k - 1], - matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux, scalingVectors[k - 1].asDiagonal().toDenseMatrix(), dynamics[k].dfdx)); - procedure(D[2 * k], matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu, dynamics[k].dfdu)); - procedure(E[k], matrixInfNormRows(dynamics[k].dfdx, dynamics[k].dfdu, scalingVectors[k].asDiagonal().toDenseMatrix())); - } - }; - - runParallel(std::move(task)); - - procedure(D[2 * N - 1], matrixInfNormCols(cost[N].dfdxx, scalingVectors[N - 1].asDiagonal().toDenseMatrix())); -} - -void Pipg::scaleDataOneStepInPlace(const vector_array_t& D, const vector_array_t& E, const int N, - std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors) { - scaleMatrixInPlace(&(D[0]), &(D[0]), cost.front().dfduu); - scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdu); - scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdux); - - std::atomic_int timeStamp{1}; - auto scaleCost = [&](int workerId) { - int k; - while ((k = timeStamp++) < N) { - auto& Q = cost[k].dfdxx; - auto& R = cost[k].dfduu; - auto& P = cost[k].dfdux; - auto& q = cost[k].dfdx; - auto& r = cost[k].dfdu; - - scaleMatrixInPlace(&(D[2 * k - 1]), &(D[2 * k - 1]), Q); - scaleMatrixInPlace(&(D[2 * k - 1]), nullptr, q); - scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k - 1]), P); - - scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k]), R); - scaleMatrixInPlace(&(D[2 * k]), nullptr, r); - } - }; - - runParallel(std::move(scaleCost)); - - scaleMatrixInPlace(&(D.back()), &(D.back()), cost[N].dfdxx); - scaleMatrixInPlace(&(D.back()), nullptr, cost[N].dfdx); - - // Scale G & g - auto& B_0 = dynamics[0].dfdu; - auto& C_0 = scalingVectors[0]; - /** - * \tilde{B} = E * B * D - * scaling = E * I * D - * \tilde{g} = E * g - */ - scaleMatrixInPlace(&(E[0]), &(D[0]), B_0); - C_0.array() *= E[0].array() * D[1].array(); - - scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].dfdx); - scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].f); - - timeStamp = 1; - auto scaleConstraints = [&](int workerId) { - int k; - while ((k = timeStamp++) < N) { - auto& A_k = dynamics[k].dfdx; - auto& B_k = dynamics[k].dfdu; - auto& b_k = dynamics[k].f; - auto& C_k = scalingVectors[k]; - - scaleMatrixInPlace(&(E[k]), &(D[2 * k - 1]), A_k); - scaleMatrixInPlace(&(E[k]), &(D[2 * k]), B_k); - C_k.array() *= E[k].array() * D[2 * k + 1].array(); - - scaleMatrixInPlace(&(E[k]), nullptr, b_k); - } - }; - - runParallel(std::move(scaleConstraints)); -} - -void Pipg::preConditioningInPlaceInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, const int iteration, - vector_array_t& DOut, vector_array_t& EOut, vector_array_t& scalingVectors, scalar_t& cOut, - const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, - const Eigen::SparseMatrix<scalar_t>& G) { - const int N = ocpSize_.numStages; - if (N < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); - } - - auto limitScaling = [](vector_t& vec) { - for (int i = 0; i < vec.size(); i++) { - if (vec(i) > 1e+4) vec(i) = 1e+4; - if (vec(i) < 1e-4) vec(i) = 1.0; - } - }; - - // Init output - cOut = 1.0; - DOut.resize(2 * N); - EOut.resize(N); - scalingVectors.resize(N); - for (int i = 0; i < N; i++) { - DOut[2 * i].setOnes(ocpSize_.numInputs[i]); - DOut[2 * i + 1].setOnes(ocpSize_.numStates[i + 1]); - EOut[i].setOnes(ocpSize_.numStates[i + 1]); - scalingVectors[i].setOnes(ocpSize_.numStates[i + 1]); - } - - // /** - // * Test start - // */ - // Eigen::SparseMatrix<scalar_t> HTest = H; - // vector_t hTest = h; - // Eigen::SparseMatrix<scalar_t> GTest = G; - // /** - // * Test end - // */ - - vector_array_t D(2 * N), E(N); - for (int i = 0; i < iteration; i++) { - invSqrtInfNorm(dynamics, cost, scalingVectors, N, limitScaling, D, E); - scaleDataOneStepInPlace(D, E, N, dynamics, cost, scalingVectors); - - for (int k = 0; k < DOut.size(); k++) { - DOut[k].array() *= D[k].array(); - } - for (int k = 0; k < EOut.size(); k++) { - EOut[k].array() *= E[k].array(); - } - - scalar_t infNormOfh = (cost.front().dfdu + cost.front().dfdux * x0).lpNorm<Eigen::Infinity>(); - for (int k = 1; k < N; k++) { - infNormOfh = std::max(infNormOfh, std::max(cost[k].dfdx.lpNorm<Eigen::Infinity>(), cost[k].dfdu.lpNorm<Eigen::Infinity>())); - } - infNormOfh = std::max(infNormOfh, cost[N].dfdx.lpNorm<Eigen::Infinity>()); - if (infNormOfh < 1e-4) infNormOfh = 1.0; - - scalar_t sumOfInfNormOfH = matrixInfNormCols(cost[0].dfduu).derived().sum(); - for (int k = 1; k < N; k++) { - sumOfInfNormOfH += matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux).derived().sum(); - sumOfInfNormOfH += matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu).derived().sum(); - } - sumOfInfNormOfH += matrixInfNormCols(cost[N].dfdxx).derived().sum(); - - scalar_t c_temp = std::max(sumOfInfNormOfH / getNumDecisionVariables(), infNormOfh); - if (c_temp > 1e+4) c_temp = 1e+4; - if (c_temp < 1e-4) c_temp = 1.0; - - scalar_t gamma = 1.0 / c_temp; - - for (int k = 0; k <= N; ++k) { - cost[k].dfdxx *= gamma; - cost[k].dfduu *= gamma; - cost[k].dfdux *= gamma; - cost[k].dfdx *= gamma; - cost[k].dfdu *= gamma; - } - - cOut *= gamma; - - // /** - // * Test start - // */ - // auto isEqual = [](scalar_t a, scalar_t b) -> bool { - // return std::abs(a - b) < std::numeric_limits<scalar_t>::epsilon() * std::min(std::abs(a), std::abs(b)); - // }; - // vector_t DTest, ETest; - // vector_t infNormOfHCols = matrixInfNormCols(HTest); - // vector_t infNormOfGCols = matrixInfNormCols(GTest); - - // DTest = infNormOfHCols.array().max(infNormOfGCols.array()); - // ETest = matrixInfNormRows(GTest); - - // limitScaling(DTest); - // limitScaling(ETest); - - // std::cerr << "DTest: " << DTest.transpose() << "\n"; - // std::cerr << "ETest : " << ETest.transpose() << "\n"; - // std::cerr << "G :\n " << GTest.toDense() << "\n"; - - // DTest = DTest.array().sqrt().inverse(); - // ETest = ETest.array().sqrt().inverse(); - - // scaleMatrixInPlace(DTest, DTest, HTest); - - // scaleMatrixInPlace(ETest, DTest, GTest); - - // hTest.array() *= DTest.array(); - - // scalar_t infNormOfhTest = hTest.lpNorm<Eigen::Infinity>(); - // if (infNormOfhTest < 1e-4) infNormOfhTest = 1.0; - - // const vector_t infNormOfHColsUpdated = matrixInfNormCols(HTest); - // scalar_t c_tempTest = std::max(infNormOfHColsUpdated.mean(), infNormOfhTest); - // if (c_tempTest > 1e+4) c_tempTest = 1e+4; - // if (c_tempTest < 1e-4) c_tempTest = 1.0; - - // scalar_t gammaTest = 1.0 / c_tempTest; - - // HTest *= gamma; - // hTest *= gamma; - - // ocs2::vector_t DStacked(H.cols()), EStacked(G.rows()); - // int curRow = 0; - // for (auto& v : D) { - // DStacked.segment(curRow, v.size()) = v; - // curRow += v.size(); - // } - // curRow = 0; - // for (auto& v : E) { - // EStacked.segment(curRow, v.size()) = v; - // curRow += v.size(); - // } - // if (!DStacked.isApprox(DTest)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "DStacked: " << DStacked.transpose() << "\n"; - // std::cerr << "DTest : " << DTest.transpose() << "\n"; - // std::cerr << "diff : " << (DStacked - DTest).transpose() << std::endl; - // std::cerr << "R: \n" << cost[0].dfduu << std::endl; - // std::cerr << "B: \n" << dynamics[0].dfdu << std::endl; - // } - // if (!EStacked.isApprox(ETest)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "EStacked: " << EStacked.transpose() << "\n"; - // std::cerr << "ETest: " << ETest.transpose() << "\n"; - // std::cerr << "diff: " << (ETest - EStacked).transpose() << std::endl; - // } - // if (!isEqual(infNormOfhTest, infNormOfh)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "infNormOfhTest: " << infNormOfhTest << "\n"; - // std::cerr << "infNormOfh: " << infNormOfh << "\n"; - // std::cerr << "diff: " << (infNormOfhTest - infNormOfh) << std::endl; - // } - // if (!isEqual(infNormOfHColsUpdated.sum(), sumOfInfNormOfH)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "infNormOfHColsUpdated.sum(): " << infNormOfHColsUpdated.sum() << "\n"; - // std::cerr << "sumOfInfNormOfH: " << sumOfInfNormOfH << "\n"; - // std::cerr << "diff: " << (infNormOfHColsUpdated.sum() - sumOfInfNormOfH) << std::endl; - // } - // if (!isEqual(c_temp, c_tempTest)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "c_temp: " << c_temp << "\n"; - // std::cerr << "c_tempTest: " << c_tempTest << "\n"; - // std::cerr << "diff: " << (c_temp - c_tempTest) << std::endl; - // } - // /** - // * Test end - // */ - } -} - vector_t Pipg::HAbsRowSumInParallel(const std::vector<ScalarFunctionQuadraticApproximation>& cost) const { const int N = ocpSize_.numStages; const int nu_0 = ocpSize_.numInputs[0]; @@ -735,104 +412,6 @@ vector_t Pipg::HAbsRowSumInParallel(const std::vector<ScalarFunctionQuadraticApp return res; } -void Pipg::scaleDataInPlace(const vector_t& D, const vector_t& E, const scalar_t c, - std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors) const { - const int N = ocpSize_.numStages; - if (N < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); - } - - scalingVectors.resize(N); - - /** - * Scale H & h - * - */ - const int nu_0 = ocpSize_.numInputs.front(); - // Scale row - Pre multiply D - cost.front().dfduu.array().colwise() *= c * D.head(nu_0).array(); - // Scale col - Post multiply D - cost.front().dfduu *= D.head(nu_0).asDiagonal(); - - cost.front().dfdu.array() *= c * D.head(nu_0).array(); - - cost.front().dfdux.array().colwise() *= c * D.head(nu_0).array(); - - int currRow = nu_0; - for (int k = 1; k < N; ++k) { - const int nx_k = ocpSize_.numStates[k]; - const int nu_k = ocpSize_.numInputs[k]; - auto& Q = cost[k].dfdxx; - auto& R = cost[k].dfduu; - auto& P = cost[k].dfdux; - auto& q = cost[k].dfdx; - auto& r = cost[k].dfdu; - - Q.array().colwise() *= c * D.segment(currRow, nx_k).array(); - Q *= D.segment(currRow, nx_k).asDiagonal(); - q.array() *= c * D.segment(currRow, nx_k).array(); - - P *= D.segment(currRow, nx_k).asDiagonal(); - currRow += nx_k; - - R.array().colwise() *= c * D.segment(currRow, nu_k).array(); - R *= D.segment(currRow, nu_k).asDiagonal(); - r.array() *= c * D.segment(currRow, nu_k).array(); - - P.array().colwise() *= c * D.segment(currRow, nu_k).array(); - - currRow += nu_k; - } - - const int nx_N = ocpSize_.numStates[N]; - cost[N].dfdxx.array().colwise() *= c * D.tail(nx_N).array(); - cost[N].dfdxx *= D.tail(nx_N).asDiagonal(); - cost[N].dfdx.array() *= c * D.tail(nx_N).array(); - - // Scale G & g - auto& B_0 = dynamics[0].dfdu; - auto& C_0 = scalingVectors[0]; - const int nx_1 = ocpSize_.numStates[1]; - /** - * \tilde{B} = E * B * D - * scaling = E * I * D - * \tilde{g} = E * g - */ - B_0.array().colwise() *= E.head(nx_1).array(); - B_0 *= D.head(nu_0).asDiagonal(); - - C_0 = E.head(nx_1).array() * D.segment(nu_0, nx_1).array(); - - dynamics[0].dfdx.array().colwise() *= E.head(nx_1).array(); - dynamics[0].f.array() *= E.head(nx_1).array(); - - currRow = nx_1; - int currCol = nu_0; - for (int k = 1; k < N; ++k) { - const int nu_k = ocpSize_.numInputs[k]; - const int nx_k = ocpSize_.numStates[k]; - const int nx_next = ocpSize_.numStates[k + 1]; - auto& A_k = dynamics[k].dfdx; - auto& B_k = dynamics[k].dfdu; - auto& b_k = dynamics[k].f; - auto& C_k = scalingVectors[k]; - - A_k.array().colwise() *= E.segment(currRow, nx_next).array(); - A_k *= D.segment(currCol, nx_k).asDiagonal(); - - B_k.array().colwise() *= E.segment(currRow, nx_next).array(); - B_k *= D.segment(currCol + nx_k, nu_k).asDiagonal(); - - C_k = E.segment(currRow, nx_next).array() * D.segment(currCol + nx_k + nu_k, nx_next).array(); - - b_k.array() *= E.segment(currRow, nx_next).array(); - - currRow += nx_next; - currCol += nx_k + nu_k; - } -} - void Pipg::resize(const OcpSize& ocpSize) { if (ocpSize_ == ocpSize) { return; @@ -953,366 +532,6 @@ void Pipg::verifyOcpSize(const OcpSize& ocpSize) const { } } -void Pipg::getConstraintMatrix(const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, - VectorFunctionLinearApproximation& res) const { - const int N = ocpSize_.numStages; - if (N < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); - } - if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { - throw std::runtime_error("[PIPG] The size of scalingVectors doesn't match the number of stage."); - } - - // Preallocate full constraint matrix - auto& G = res.dfdx; - auto& g = res.f; - if (constraints != nullptr) { - G.setZero(getNumDynamicsConstraints() + getNumGeneralEqualityConstraints(), getNumDecisionVariables()); - - } else { - G.setZero(getNumDynamicsConstraints(), getNumDecisionVariables()); - } - g.setZero(G.rows()); - - // Initial state constraint - const int nu_0 = ocpSize_.numInputs.front(); - const int nx_1 = ocpSize_.numStates[1]; - if (scalingVectorsPtr == nullptr) { - G.topLeftCorner(nx_1, nu_0 + nx_1) << -dynamics.front().dfdu, matrix_t::Identity(nx_1, nx_1); - } else { - G.topLeftCorner(nx_1, nu_0 + nx_1) << -dynamics.front().dfdu, scalingVectorsPtr->front().asDiagonal().toDenseMatrix(); - } - - // k = 0. Absorb initial state into dynamics - // The initial state is removed from the decision variables - // The first dynamics becomes: - // x[1] = A[0]*x[0] + B[0]*u[0] + b[0] - // = B[0]*u[0] + (b[0] + A[0]*x[0]) - // = B[0]*u[0] + \tilde{b}[0] - // numState[0] = 0 --> No need to specify A[0] here - g.head(nx_1) = dynamics.front().f; - g.head(nx_1).noalias() += dynamics.front().dfdx * x0; - - int currRow = nx_1; - int currCol = nu_0; - for (int k = 1; k < N; ++k) { - const auto& dynamics_k = dynamics[k]; - // const auto& constraints_k = constraints; - const int nu_k = ocpSize_.numInputs[k]; - const int nx_k = ocpSize_.numStates[k]; - const int nx_next = ocpSize_.numStates[k + 1]; - - // Add [-A, -B, I(C)] - if (scalingVectorsPtr == nullptr) { - G.block(currRow, currCol, nx_next, nx_k + nu_k + nx_next) << -dynamics_k.dfdx, -dynamics_k.dfdu, matrix_t::Identity(nx_next, nx_next); - } else { - G.block(currRow, currCol, nx_next, nx_k + nu_k + nx_next) << -dynamics_k.dfdx, -dynamics_k.dfdu, - (*scalingVectorsPtr)[k].asDiagonal().toDenseMatrix(); - } - - // Add [b] - g.segment(currRow, nx_next) = dynamics_k.f; - - currRow += nx_next; - currCol += nx_k + nu_k; - } - - if (constraints != nullptr) { - currCol = nu_0; - // === Constraints === - // for ocs2 --> C*dx + D*du + e = 0 - // for pipg --> C*dx + D*du = -e - // Initial general constraints - const int nc_0 = ocpSize_.numIneqConstraints.front(); - if (nc_0 > 0) { - const auto& constraint_0 = (*constraints).front(); - G.block(currRow, 0, nc_0, nu_0) = constraint_0.dfdu; - g.segment(currRow, nc_0) = -constraint_0.f; - g.segment(currRow, nc_0) -= constraint_0.dfdx * x0; - currRow += nc_0; - } - - for (int k = 1; k < N; ++k) { - const int nc_k = ocpSize_.numIneqConstraints[k]; - const int nu_k = ocpSize_.numInputs[k]; - const int nx_k = ocpSize_.numStates[k]; - if (nc_k > 0) { - const auto& constraints_k = (*constraints)[k]; - - // Add [C, D, 0] - G.block(currRow, currCol, nc_k, nx_k + nu_k) << constraints_k.dfdx, constraints_k.dfdu; - // Add [-e] - g.segment(currRow, nc_k) = -constraints_k.f; - currRow += nc_k; - } - - currCol += nx_k + nu_k; - } - - // Final general constraint - const int nc_N = ocpSize_.numIneqConstraints[N]; - if (nc_N > 0) { - const auto& constraints_N = (*constraints)[N]; - G.bottomRightCorner(nc_N, constraints_N.dfdx.cols()) = constraints_N.dfdx; - g.tail(nc_N) = -constraints_N.f; - } - } -} - -void Pipg::getConstraintMatrixSparse(const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, - const vector_array_t* scalingVectorsPtr, Eigen::SparseMatrix<scalar_t>& G, vector_t& g) const { - const int N = ocpSize_.numStages; - if (N < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); - } - if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { - throw std::runtime_error("[PIPG] The size of scalingVectors doesn't match the number of stage."); - } - - const int nu_0 = ocpSize_.numInputs[0]; - const int nx_1 = ocpSize_.numStates[1]; - const int nx_N = ocpSize_.numStates[N]; - - int nnz = nx_1 * (nu_0 + nx_1); - for (int k = 1; k < N; ++k) { - const int nx_k = ocpSize_.numStates[k]; - const int nu_k = ocpSize_.numInputs[k]; - const int nx_next = ocpSize_.numStates[k + 1]; - nnz += nx_next * (nx_k + nu_k + nx_next); - } - - if (constraints != nullptr) { - const int nc_0 = ocpSize_.numIneqConstraints[0]; - nnz += nc_0 * nu_0; - for (int k = 1; k < N; ++k) { - const int nx_k = ocpSize_.numStates[k]; - const int nu_k = ocpSize_.numInputs[k]; - const int nc_k = ocpSize_.numIneqConstraints[k]; - nnz += nc_k * (nx_k + nu_k); - } - const int nc_N = ocpSize_.numIneqConstraints[N]; - - nnz += nc_N * nx_N; - } - std::vector<Eigen::Triplet<scalar_t>> tripletList; - tripletList.reserve(nnz); - - auto emplaceBackMatrix = [&tripletList](const int startRow, const int startCol, const matrix_t& mat) { - for (int j = 0; j < mat.cols(); j++) { - for (int i = 0; i < mat.rows(); i++) { - if (mat(i, j) != 0) { - tripletList.emplace_back(startRow + i, startCol + j, mat(i, j)); - } - } - } - }; - - if (constraints != nullptr) { - G.resize(getNumDynamicsConstraints() + getNumGeneralEqualityConstraints(), getNumDecisionVariables()); - } else { - G.resize(getNumDynamicsConstraints(), getNumDecisionVariables()); - } - g.setZero(G.rows()); - - // Initial state constraint - emplaceBackMatrix(0, 0, -dynamics.front().dfdu); - if (scalingVectorsPtr == nullptr) { - emplaceBackMatrix(0, nu_0, matrix_t::Identity(nx_1, nx_1)); - } else { - emplaceBackMatrix(0, nu_0, scalingVectorsPtr->front().asDiagonal()); - } - - // k = 0. Absorb initial state into dynamics - // The initial state is removed from the decision variables - // The first dynamics becomes: - // x[1] = A[0]*x[0] + B[0]*u[0] + b[0] - // = B[0]*u[0] + (b[0] + A[0]*x[0]) - // = B[0]*u[0] + \tilde{b}[0] - // numState[0] = 0 --> No need to specify A[0] here - g.head(nx_1) = dynamics.front().f; - g.head(nx_1).noalias() += dynamics.front().dfdx * x0; - - int currRow = nx_1; - int currCol = nu_0; - for (int k = 1; k < N; ++k) { - const auto& dynamics_k = dynamics[k]; - // const auto& constraints_k = constraints; - const int nx_k = ocpSize_.numStates[k]; - const int nu_k = ocpSize_.numInputs[k]; - const int nx_next = ocpSize_.numStates[k + 1]; - - // Add [-A, -B, I] - emplaceBackMatrix(currRow, currCol, -dynamics_k.dfdx); - emplaceBackMatrix(currRow, currCol + nx_k, -dynamics_k.dfdu); - if (scalingVectorsPtr == nullptr) { - emplaceBackMatrix(currRow, currCol + nx_k + nu_k, matrix_t::Identity(nx_next, nx_next)); - } else { - emplaceBackMatrix(currRow, currCol + nx_k + nu_k, (*scalingVectorsPtr)[k].asDiagonal()); - } - - // Add [b] - g.segment(currRow, nx_next) = dynamics_k.f; - - currRow += nx_next; - currCol += nx_k + nu_k; - } - - if (constraints != nullptr) { - currCol = nu_0; - // === Constraints === - // for ocs2 --> C*dx + D*du + e = 0 - // for pipg --> C*dx + D*du = -e - // Initial general constraints - const int nc_0 = ocpSize_.numIneqConstraints.front(); - if (nc_0 > 0) { - const auto& constraint_0 = (*constraints).front(); - emplaceBackMatrix(currRow, 0, constraint_0.dfdu); - - g.segment(currRow, nc_0) = -constraint_0.f; - g.segment(currRow, nc_0) -= constraint_0.dfdx * x0; - currRow += nc_0; - } - - for (int k = 1; k < N; ++k) { - const int nc_k = ocpSize_.numIneqConstraints[k]; - const int nu_k = ocpSize_.numInputs[k]; - const int nx_k = ocpSize_.numStates[k]; - if (nc_k > 0) { - const auto& constraints_k = (*constraints)[k]; - - // Add [C, D, 0] - emplaceBackMatrix(currRow, currCol, constraints_k.dfdx); - emplaceBackMatrix(currRow, currCol + nx_k, constraints_k.dfdu); - // Add [-e] - g.segment(currRow, nc_k) = -constraints_k.f; - currRow += nc_k; - } - - currCol += nx_k + nu_k; - } - - // Final general constraint - const int nc_N = ocpSize_.numIneqConstraints[N]; - if (nc_N > 0) { - const auto& constraints_N = (*constraints)[N]; - emplaceBackMatrix(currRow, currCol, constraints_N.dfdx); - g.segment(currRow, nc_N) = -constraints_N.f; - } - } - - G.setFromTriplets(tripletList.begin(), tripletList.end()); - assert(G.nonZeros() <= nnz); -} - -void Pipg::getCostMatrix(const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, - ScalarFunctionQuadraticApproximation& res) const { - const int N = ocpSize_.numStages; - - // Preallocate full Cost matrices - auto& H = res.dfdxx; - auto& h = res.dfdx; - auto& c = res.f; - H.setZero(getNumDecisionVariables(), getNumDecisionVariables()); - h.setZero(H.cols()); - c = 0.0; - - // k = 0. Elimination of initial state requires cost adaptation - vector_t r_0 = cost[0].dfdu; - r_0 += cost[0].dfdux * x0; - - const int nu_0 = ocpSize_.numInputs[0]; - H.topLeftCorner(nu_0, nu_0) = cost[0].dfduu; - h.head(nu_0) = r_0; - - int currRow = nu_0; - for (int k = 1; k < N; ++k) { - const int nx_k = ocpSize_.numStates[k]; - const int nu_k = ocpSize_.numInputs[k]; - - // Add [ Q, P' - // P, Q ] - H.block(currRow, currRow, nx_k + nu_k, nx_k + nu_k) << cost[k].dfdxx, cost[k].dfdux.transpose(), cost[k].dfdux, cost[k].dfduu; - - // Add [ q, r] - h.segment(currRow, nx_k + nu_k) << cost[k].dfdx, cost[k].dfdu; - - // Add nominal cost - c += cost[k].f; - - currRow += nx_k + nu_k; - } - - const int nx_N = ocpSize_.numStates[N]; - H.bottomRightCorner(nx_N, nx_N) = cost[N].dfdxx; - h.tail(nx_N) = cost[N].dfdx; - c += cost[N].f; -} - -void Pipg::getCostMatrixSparse(const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, - Eigen::SparseMatrix<scalar_t>& H, vector_t& h) const { - const int N = ocpSize_.numStages; - - const int nu_0 = ocpSize_.numInputs[0]; - int nnz = nu_0 * nu_0; - for (int k = 1; k < N; ++k) { - const int nx_k = ocpSize_.numStates[k]; - const int nu_k = ocpSize_.numInputs[k]; - nnz += (nx_k + nu_k) * (nx_k + nu_k); - } - const int nx_N = ocpSize_.numStates[N]; - - nnz += nx_N * nx_N; - std::vector<Eigen::Triplet<scalar_t>> tripletList; - tripletList.reserve(nnz); - - auto emplaceBackMatrix = [&tripletList](const int startRow, const int startCol, const matrix_t& mat) { - for (int j = 0; j < mat.cols(); j++) { - for (int i = 0; i < mat.rows(); i++) { - if (mat(i, j) != 0) { - tripletList.emplace_back(startRow + i, startCol + j, mat(i, j)); - } - } - } - }; - - h.setZero(getNumDecisionVariables()); - - // k = 0. Elimination of initial state requires cost adaptation - vector_t r_0 = cost[0].dfdu; - r_0 += cost[0].dfdux * x0; - - // R0 - emplaceBackMatrix(0, 0, cost[0].dfduu); - h.head(nu_0) = r_0; - - int currRow = nu_0; - for (int k = 1; k < N; ++k) { - const int nx_k = ocpSize_.numStates[k]; - const int nu_k = ocpSize_.numInputs[k]; - - // Add [ Q, P' - // P, Q ] - emplaceBackMatrix(currRow, currRow, cost[k].dfdxx); - emplaceBackMatrix(currRow, currRow + nx_k, cost[k].dfdux.transpose()); - emplaceBackMatrix(currRow + nx_k, currRow, cost[k].dfdux); - emplaceBackMatrix(currRow + nx_k, currRow + nx_k, cost[k].dfduu); - - // Add [ q, r] - h.segment(currRow, nx_k + nu_k) << cost[k].dfdx, cost[k].dfdu; - - currRow += nx_k + nu_k; - } - - emplaceBackMatrix(currRow, currRow, cost[N].dfdxx); - h.tail(nx_N) = cost[N].dfdx; - - H.resize(getNumDecisionVariables(), getNumDecisionVariables()); - H.setFromTriplets(tripletList.begin(), tripletList.end()); - assert(H.nonZeros() <= nnz); -} - void Pipg::GGTAbsRowSumInParallel(const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, vector_t& res) { @@ -1449,31 +668,6 @@ Pipg::~Pipg() { } } -vector_t matrixInfNormCols(const Eigen::SparseMatrix<scalar_t>& mat) { - vector_t infNorm; - infNorm.setZero(mat.cols()); - - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - infNorm(j) = std::max(infNorm(j), std::abs(it.value())); - } - } - return infNorm; -} - -vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat) { - vector_t infNorm; - infNorm.setZero(mat.rows()); - - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - int i = it.row(); - infNorm(i) = std::max(infNorm(i), std::abs(it.value())); - } - } - return infNorm; -} - vector_t matrixRowwiseAbsSum(const Eigen::SparseMatrix<scalar_t>& mat) { vector_t rowAbsSum; rowAbsSum.setZero(mat.rows()); @@ -1487,15 +681,4 @@ vector_t matrixRowwiseAbsSum(const Eigen::SparseMatrix<scalar_t>& mat) { return rowAbsSum; } -void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix<scalar_t>& mat) { - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - if (it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1) { - throw std::runtime_error("[scaleMatrixInPlace] it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1"); - } - it.valueRef() *= rowScale(it.row()) * colScale(j); - } - } -} - } // namespace ocs2 diff --git a/ocs2_pipg/test/testPIPGSolver.cpp b/ocs2_pipg/test/testPIPGSolver.cpp index 5fd5d8dfe..47c9d5c85 100644 --- a/ocs2_pipg/test/testPIPGSolver.cpp +++ b/ocs2_pipg/test/testPIPGSolver.cpp @@ -1,11 +1,12 @@ -#include "ocs2_pipg/PIPG.h" - #include <gtest/gtest.h> +#include "ocs2_pipg/PIPG.h" + #include <Eigen/Sparse> +#include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> +#include <ocs2_oc/test/testProblemsGeneration.h> #include <ocs2_qp_solver/QpSolver.h> -#include <ocs2_qp_solver/test/testProblemsGeneration.h> ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, ocs2::scalar_t relativeTolerance, bool verbose) { @@ -35,23 +36,21 @@ class PIPGSolverTest : public testing::Test { srand(10); // Construct OCP problem - lqProblem = ocs2::qp_solver::generateRandomLqProblem(N_, nx_, nu_, nc_); x0 = ocs2::vector_t::Random(nx_); for (int i = 0; i < N_; i++) { - dynamicsArray.push_back(lqProblem[i].dynamics); - costArray.push_back(lqProblem[i].cost); - constraintsArray.push_back(lqProblem[i].constraints); + dynamicsArray.push_back(ocs2::getRandomDynamics(nx_, nu_)); + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); } - costArray.push_back(lqProblem[N_].cost); - constraintsArray.push_back(lqProblem[N_].constraints); + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); pipgSolver.resize(ocs2::extractSizesFromProblem(dynamicsArray, costArray, &constraintsArray)); - pipgSolver.getCostMatrix(x0, costArray, costApproximation); - pipgSolver.getConstraintMatrix(x0, dynamicsArray, nullptr, nullptr, constraintsApproximation); + ocs2::getCostMatrix(pipgSolver.size(), x0, costArray, costApproximation); + ocs2::getConstraintMatrix(pipgSolver.size(), x0, dynamicsArray, nullptr, nullptr, constraintsApproximation); } - std::vector<ocs2::qp_solver::LinearQuadraticStage> lqProblem; ocs2::vector_t x0; ocs2::ScalarFunctionQuadraticApproximation costApproximation; ocs2::VectorFunctionLinearApproximation constraintsApproximation; @@ -65,13 +64,12 @@ class PIPGSolverTest : public testing::Test { constexpr size_t PIPGSolverTest::numDecisionVariables; constexpr size_t PIPGSolverTest::numConstraints; -TEST_F(PIPGSolverTest, PIPGcorrectness) { - // SolveDenseQP use Gz + g = 0 for constraints +TEST_F(PIPGSolverTest, correctness) { + // ocs2::qp_solver::SolveDenseQP use Gz + g = 0 for constraints auto QPconstraints = constraintsApproximation; QPconstraints.f = -QPconstraints.f; ocs2::vector_t primalSolutionQP; std::tie(primalSolutionQP, std::ignore) = ocs2::qp_solver::solveDenseQp(costApproximation, QPconstraints); - // primalSolutionQP.setZero(numDecisionVariables); Eigen::JacobiSVD<ocs2::matrix_t> svd(costApproximation.dfdxx); ocs2::vector_t s = svd.singularValues(); @@ -81,9 +79,9 @@ TEST_F(PIPGSolverTest, PIPGcorrectness) { ocs2::scalar_t sigma = svdGTG.singularValues()(0); ocs2::vector_t primalSolutionPIPG; - pipgSolver.solveDenseQP(costApproximation, constraintsApproximation, ocs2::vector_t::Ones(pipgSolver.getNumDynamicsConstraints()), mu, - lambda, sigma, primalSolutionPIPG); - // primalSolutionPIPG.setZero(numDecisionVariables); + pipgSolver.solveDenseQP(costApproximation.dfdxx.sparseView(), costApproximation.dfdx, constraintsApproximation.dfdx.sparseView(), + constraintsApproximation.f, ocs2::vector_t::Ones(pipgSolver.getNumDynamicsConstraints()), mu, lambda, sigma, + primalSolutionPIPG); ocs2::vector_t primalSolutionPIPGParallel; ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); @@ -91,7 +89,6 @@ TEST_F(PIPGSolverTest, PIPGcorrectness) { pipgSolver.solveOCPInParallel(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma, costApproximation, constraintsApproximation); pipgSolver.getStackedSolution(primalSolutionPIPGParallel); - // primalSolutionPIPGParallel.setZero(numDecisionVariables); auto calculateConstraintViolation = [&](const ocs2::vector_t& sol) -> ocs2::scalar_t { return (constraintsApproximation.dfdx * sol - constraintsApproximation.f).cwiseAbs().maxCoeff(); @@ -151,113 +148,6 @@ TEST_F(PIPGSolverTest, PIPGcorrectness) { EXPECT_TRUE(std::abs(PIPGParallelCConstraintViolation - PIPGConstraintViolation) < pipgSolver.settings().absoluteTolerance * 10.0); } -TEST_F(PIPGSolverTest, preConditioning) { - auto costScaledWithFactors = costApproximation; - auto constraintScaledWithFactors = constraintsApproximation; - ocs2::vector_t D, E; - ocs2::scalar_t c; - Eigen::SparseMatrix<ocs2::scalar_t> HScaledFromCalculation = costApproximation.dfdxx.sparseView(), - GScaledFromCalculation = constraintsApproximation.dfdx.sparseView(); - ocs2::vector_t hScaledFromCalculation = costApproximation.dfdx; - - pipgSolver.calculatePreConditioningFactors(HScaledFromCalculation, hScaledFromCalculation, GScaledFromCalculation, 5, D, E, c); - - costScaledWithFactors.dfdxx = c * D.asDiagonal() * costScaledWithFactors.dfdxx * D.asDiagonal(); - costScaledWithFactors.dfdx = c * D.asDiagonal() * costScaledWithFactors.dfdx; - constraintScaledWithFactors.dfdx = E.asDiagonal() * constraintScaledWithFactors.dfdx * D.asDiagonal(); - constraintScaledWithFactors.f = E.asDiagonal() * constraintScaledWithFactors.f; - - // The scaled matrix given by calculatePreConditioningFactors function and the one calculated with D, E and c factors should be the same. - EXPECT_TRUE(costScaledWithFactors.dfdxx.isApprox(HScaledFromCalculation.toDense())); // H - EXPECT_TRUE(costScaledWithFactors.dfdx.isApprox(hScaledFromCalculation)); // h - EXPECT_TRUE(constraintScaledWithFactors.dfdx.isApprox(GScaledFromCalculation.toDense())); // G - - // Normalized the inf-Norm of both rows and cols of KKT matrix to 1 - ocs2::matrix_t KKT(costApproximation.dfdxx.rows() + constraintsApproximation.dfdx.rows(), - costApproximation.dfdxx.rows() + constraintsApproximation.dfdx.rows()); - KKT << costApproximation.dfdxx, constraintsApproximation.dfdx.transpose(), constraintsApproximation.dfdx, - ocs2::matrix_t::Zero(constraintsApproximation.dfdx.rows(), constraintsApproximation.dfdx.rows()); - - ocs2::matrix_t KKTScaled(costApproximation.dfdxx.rows() + constraintsApproximation.dfdx.rows(), - costApproximation.dfdxx.rows() + constraintsApproximation.dfdx.rows()); - KKTScaled << costScaledWithFactors.dfdxx, constraintScaledWithFactors.dfdx.transpose(), constraintScaledWithFactors.dfdx, - ocs2::matrix_t::Zero(constraintsApproximation.dfdx.rows(), constraintsApproximation.dfdx.rows()); - - ocs2::vector_t infNormOfKKT = KKT.cwiseAbs().rowwise().maxCoeff(); - ocs2::vector_t infNormOfKKTScaled = KKTScaled.cwiseAbs().rowwise().maxCoeff(); - EXPECT_LT((infNormOfKKTScaled.array() - 1).abs().maxCoeff(), (infNormOfKKT.array() - 1).abs().maxCoeff()); - - std::vector<ocs2::vector_t> scalingVectors; - pipgSolver.scaleDataInPlace(D, E, c, dynamicsArray, costArray, scalingVectors); - - ocs2::ScalarFunctionQuadraticApproximation costConstructedFromScaledDataArray; - ocs2::VectorFunctionLinearApproximation constraintConstructedFromScaledDataArray; - pipgSolver.getCostMatrix(x0, costArray, costConstructedFromScaledDataArray); - pipgSolver.getConstraintMatrix(x0, dynamicsArray, nullptr, &scalingVectors, constraintConstructedFromScaledDataArray); - - EXPECT_TRUE(costScaledWithFactors.dfdxx.isApprox(costConstructedFromScaledDataArray.dfdxx)); // H - EXPECT_TRUE(costScaledWithFactors.dfdx.isApprox(costConstructedFromScaledDataArray.dfdx)); // h - EXPECT_TRUE(constraintScaledWithFactors.dfdx.isApprox(constraintConstructedFromScaledDataArray.dfdx)); // G - EXPECT_TRUE(constraintScaledWithFactors.f.isApprox(constraintConstructedFromScaledDataArray.f)); // g -} - -TEST_F(PIPGSolverTest, preConditioningSimplified) { - auto costScaledWithFactors = costApproximation; - auto constraintScaledWithFactors = constraintsApproximation; - ocs2::vector_t D, E; - ocs2::scalar_t c; - Eigen::SparseMatrix<ocs2::scalar_t> HScaledFromCalculation = costApproximation.dfdxx.sparseView(), - GScaledFromCalculation = constraintsApproximation.dfdx.sparseView(); - ocs2::vector_t hScaledFromCalculation = costApproximation.dfdx; - - pipgSolver.calculatePreConditioningFactors(HScaledFromCalculation, hScaledFromCalculation, GScaledFromCalculation, 5, D, E, c); - - ocs2::vector_array_t DArray, EArray; - ocs2::scalar_t cMy; - ocs2::vector_array_t scalingVectors(N_); - pipgSolver.preConditioningInPlaceInParallel(x0, dynamicsArray, costArray, 5, DArray, EArray, scalingVectors, cMy, - costApproximation.dfdxx.sparseView(), costApproximation.dfdx, - constraintsApproximation.dfdx.sparseView()); - - ocs2::vector_t DStacked(pipgSolver.getNumDecisionVariables()), EStacked(pipgSolver.getNumDynamicsConstraints()); - int curRow = 0; - for (auto& v : DArray) { - DStacked.segment(curRow, v.size()) = v; - curRow += v.size(); - } - curRow = 0; - for (auto& v : EArray) { - EStacked.segment(curRow, v.size()) = v; - curRow += v.size(); - } - - // The scaled matrix given by calculatePreConditioningFactors function and the one calculated with D, E and c factors should be the - // same. - EXPECT_TRUE(DStacked.isApprox(D)); - EXPECT_TRUE(EStacked.isApprox(E)); - EXPECT_FLOAT_EQ(c, cMy); - - ocs2::ScalarFunctionQuadraticApproximation costConstructedFromScaledDataArray; - ocs2::VectorFunctionLinearApproximation constraintConstructedFromScaledDataArray; - ocs2::vector_t gScaledFromCalculation = E.cwiseProduct(constraintsApproximation.f); - pipgSolver.getCostMatrix(x0, costArray, costConstructedFromScaledDataArray); - pipgSolver.getConstraintMatrix(x0, dynamicsArray, nullptr, &scalingVectors, constraintConstructedFromScaledDataArray); - - EXPECT_TRUE(HScaledFromCalculation.isApprox(costConstructedFromScaledDataArray.dfdxx)); // H - EXPECT_TRUE(hScaledFromCalculation.isApprox(costConstructedFromScaledDataArray.dfdx)); // h - EXPECT_TRUE(GScaledFromCalculation.isApprox(constraintConstructedFromScaledDataArray.dfdx)); // G - EXPECT_TRUE(gScaledFromCalculation.isApprox(constraintConstructedFromScaledDataArray.f)); // g -} - -TEST_F(PIPGSolverTest, constructSparseCostApproximation) { - Eigen::SparseMatrix<ocs2::scalar_t> H; - ocs2::vector_t h; - pipgSolver.getCostMatrixSparse(x0, costArray, H, h); - - EXPECT_TRUE(costApproximation.dfdxx.isApprox(H.toDense())); - EXPECT_TRUE(costApproximation.dfdx.isApprox(h)); -} - TEST_F(PIPGSolverTest, HAbsRowSumInParallel) { ocs2::vector_t rowwiseSum = pipgSolver.HAbsRowSumInParallel(costArray); @@ -294,19 +184,3 @@ TEST_F(PIPGSolverTest, descaleSolution) { << packedSolutionMy.transpose() << "\nIt should be \n" << packedSolution.transpose(); } - -TEST_F(PIPGSolverTest, constructSparseConstraintsApproximation) { - Eigen::SparseMatrix<ocs2::scalar_t> G; - ocs2::vector_t g; - ocs2::vector_array_t scalingVectors(N_); - for (auto& v : scalingVectors) { - v = ocs2::vector_t::Random(nx_); - } - ocs2::VectorFunctionLinearApproximation constraintsApproximation; - pipgSolver.getConstraintMatrix(x0, dynamicsArray, &constraintsArray, &scalingVectors, constraintsApproximation); - - pipgSolver.getConstraintMatrixSparse(x0, dynamicsArray, &constraintsArray, &scalingVectors, G, g); - - EXPECT_TRUE(constraintsApproximation.dfdx.isApprox(G.toDense())); - EXPECT_TRUE(constraintsApproximation.f.isApprox(g)); -} \ No newline at end of file From 7fc4c77f71ef0df0f5b0ab39fb218b3cb81bc343 Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Thu, 28 Apr 2022 20:39:16 +0200 Subject: [PATCH 222/512] move helper functions to separate file --- ocs2_pipg/CMakeLists.txt | 2 + ocs2_pipg/include/ocs2_pipg/HelperFunctions.h | 16 ++ ocs2_pipg/include/ocs2_pipg/PIPG.h | 10 +- ocs2_pipg/src/HelperFunctions.cpp | 154 ++++++++++++++++++ ocs2_pipg/src/PIPG.cpp | 141 ---------------- ocs2_pipg/test/testHelper.cpp | 64 ++++++++ ocs2_pipg/test/testPIPGSolver.cpp | 6 - 7 files changed, 237 insertions(+), 156 deletions(-) create mode 100644 ocs2_pipg/include/ocs2_pipg/HelperFunctions.h create mode 100644 ocs2_pipg/src/HelperFunctions.cpp create mode 100644 ocs2_pipg/test/testHelper.cpp diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_pipg/CMakeLists.txt index bbc713bed..76253e25e 100644 --- a/ocs2_pipg/CMakeLists.txt +++ b/ocs2_pipg/CMakeLists.txt @@ -42,6 +42,7 @@ include_directories( add_library(${PROJECT_NAME} src/PIPG_Settings.cpp + src/HelperFunctions.cpp src/PIPG.cpp ) target_link_libraries(${PROJECT_NAME} @@ -84,6 +85,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ############# catkin_add_gtest(test_${PROJECT_NAME} test/testPIPGSolver.cpp + test/testHelper.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} diff --git a/ocs2_pipg/include/ocs2_pipg/HelperFunctions.h b/ocs2_pipg/include/ocs2_pipg/HelperFunctions.h new file mode 100644 index 000000000..26f593570 --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/HelperFunctions.h @@ -0,0 +1,16 @@ +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_core/thread_support/ThreadPool.h> +#include <ocs2_oc/oc_problem/OcpSize.h> + +namespace ocs2 { +namespace pipg { + +vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost); + +vector_t GGTAbsRowSumInParallel(const OcpSize& ocpSize, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + ThreadPool& threadPool); +} // namespace pipg +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG.h b/ocs2_pipg/include/ocs2_pipg/PIPG.h index 6da3afd5e..63a5cde0f 100644 --- a/ocs2_pipg/include/ocs2_pipg/PIPG.h +++ b/ocs2_pipg/include/ocs2_pipg/PIPG.h @@ -83,12 +83,6 @@ class Pipg { const vector_array_t* EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, const ScalarFunctionQuadraticApproximation& costM, const VectorFunctionLinearApproximation& constraintsM); - vector_t HAbsRowSumInParallel(const std::vector<ScalarFunctionQuadraticApproximation>& cost) const; - - void GGTAbsRowSumInParallel(const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, - vector_t& res); - void resize(const OcpSize& size); int getNumDecisionVariables() const { return numDecisionVariables; } @@ -114,6 +108,7 @@ class Pipg { const Settings& settings() const { return pipgSettings_; } Settings& settings() { return pipgSettings_; } const OcpSize& size() const { return ocpSize_; } + ThreadPool& getThreadPool() { return threadPool_; } private: void verifySizes(const std::vector<VectorFunctionLinearApproximation>& dynamics, @@ -148,7 +143,4 @@ class Pipg { benchmark::RepeatedTimer step3w_; benchmark::RepeatedTimer step4CheckConvergence_; }; - -vector_t matrixRowwiseAbsSum(const Eigen::SparseMatrix<scalar_t>& mat); - } // namespace ocs2 diff --git a/ocs2_pipg/src/HelperFunctions.cpp b/ocs2_pipg/src/HelperFunctions.cpp new file mode 100644 index 000000000..3ab596634 --- /dev/null +++ b/ocs2_pipg/src/HelperFunctions.cpp @@ -0,0 +1,154 @@ +#include "ocs2_pipg/HelperFunctions.h" + +#include <atomic> +#include <numeric> + +namespace ocs2 { +namespace pipg { + +namespace { +int getNumDecisionVariables(const OcpSize& ocpSize) { + return std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), + std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0)); +} + +int getNumDynamicsConstraints(const OcpSize& ocpSize) { + return std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0); +} + +int getNumGeneralEqualityConstraints(const OcpSize& ocpSize) { + return std::accumulate(ocpSize.numIneqConstraints.begin(), ocpSize.numIneqConstraints.end(), (int)0); +} +} // namespace + +vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost) { + const int N = ocpSize.numStages; + const int nu_0 = ocpSize.numInputs[0]; + vector_t res(getNumDecisionVariables(ocpSize)); + res.head(nu_0) = cost[0].dfduu.cwiseAbs().rowwise().sum(); + + int curRow = nu_0; + for (int k = 1; k < N; k++) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + if (cost[k].dfdxx.size() != 0) { + res.segment(curRow, nx_k) = cost[k].dfdxx.cwiseAbs().rowwise().sum(); + } + if (cost[k].dfdux.size() != 0) { + res.segment(curRow, nx_k) += cost[k].dfdux.transpose().cwiseAbs().rowwise().sum(); + } + if (cost[k].dfdux.size() != 0) { + res.segment(curRow + nx_k, nu_k) = cost[k].dfdux.cwiseAbs().rowwise().sum(); + } + if (cost[k].dfduu.size() != 0) { + res.segment(curRow + nx_k, nu_k) += cost[k].dfduu.cwiseAbs().rowwise().sum(); + } + curRow += nx_k + nu_k; + } + const int nx_N = ocpSize.numStates[N]; + res.tail(nx_N) = cost[N].dfdxx.cwiseAbs().rowwise().sum(); + return res; +} + +vector_t GGTAbsRowSumInParallel(const OcpSize& ocpSize, const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + ThreadPool& threadPool) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[GGTAbsRowSumInParallel] The number of stages cannot be less than 1."); + } + if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { + throw std::runtime_error("[GGTAbsRowSumInParallel] The size of scalingVectors doesn't match the number of stage."); + } + Eigen::setNbThreads(1); // No multithreading within Eigen. + + matrix_array_t tempMatrixArray(N); + vector_array_t absRowSumArray(N); + + std::atomic_int timeIndex{0}; + auto task = [&](int workerId) { + int k; + while ((k = timeIndex++) < N) { + const auto nx_next = ocpSize.numStates[k + 1]; + const auto& B = dynamics[k].dfdu; + tempMatrixArray[k] = + (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_next, nx_next) + : (*scalingVectorsPtr)[k].cwiseProduct((*scalingVectorsPtr)[k]).asDiagonal().toDenseMatrix()); + tempMatrixArray[k] += B * B.transpose(); + + if (k != 0) { + const auto& A = dynamics[k].dfdx; + tempMatrixArray[k] += A * A.transpose(); + } + + absRowSumArray[k] = tempMatrixArray[k].cwiseAbs().rowwise().sum(); + + if (k != 0) { + const auto& A = dynamics[k].dfdx; + absRowSumArray[k] += (A * (scalingVectorsPtr == nullptr ? matrix_t::Identity(A.cols(), A.cols()) + : (*scalingVectorsPtr)[k - 1].asDiagonal().toDenseMatrix())) + .cwiseAbs() + .rowwise() + .sum(); + } + if (k != N - 1) { + const auto& ANext = dynamics[k + 1].dfdx; + absRowSumArray[k] += (ANext * (scalingVectorsPtr == nullptr ? matrix_t::Identity(ANext.cols(), ANext.cols()) + : (*scalingVectorsPtr)[k].asDiagonal().toDenseMatrix())) + .transpose() + .cwiseAbs() + .rowwise() + .sum(); + } + } + }; + threadPool.runParallel(std::move(task), threadPool.numThreads() + 1U); + + vector_t res = vector_t::Zero(getNumDynamicsConstraints(ocpSize)); + int curRow = 0; + for (const auto& v : absRowSumArray) { + res.segment(curRow, v.size()) = v; + curRow += v.size(); + } + + Eigen::setNbThreads(0); // Restore default setup. + + return res; + + // /** + // * ********************************* + // * ************ Test begin ********* + // * ********************************* + // */ + // matrix_t m(getNumDynamicsConstraints(), getNumDynamicsConstraints()); + // const int nx_1 = ocpSize.numStates[1]; + // const int nx_2 = ocpSize.numStates[2]; + // const auto& A1 = dynamics[1].dfdx; + // m.topLeftCorner(nx_1, nx_1 + nx_2) << tempMatrixArray[0], + // -(A1 * (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_1, nx_1) : (*scalingVectorsPtr)[0].asDiagonal().toDenseMatrix())) + // .transpose(); + + // curRow = nx_1; + // for (int i = 1; i < N - 1; i++) { + // const int nx_i1 = ocpSize.numStates[i + 1]; + // const int nx_i2 = ocpSize.numStates[i + 2]; + // const auto& ANext = dynamics[i + 1].dfdx; + + // m.block(curRow, curRow, nx_i1, nx_i1 + nx_i2) << tempMatrixArray[i], + // -(ANext * (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_i1, nx_i1) : + // (*scalingVectorsPtr)[i].asDiagonal().toDenseMatrix())) + // .transpose(); + // curRow += nx_i1; + // } + // const int nx_N = ocpSize.numStates[N]; + // m.block(curRow, curRow, nx_N, nx_N) = tempMatrixArray[N - 1]; + // std::cerr << "GGT: \n" << m.selfadjointView<Eigen::Upper>().toDenseMatrix() << "\n\n"; + // /** + // * ********************************* + // * ************ Test end ********* + // * ********************************* + // */ +} + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_pipg/src/PIPG.cpp b/ocs2_pipg/src/PIPG.cpp index e07c9e6a1..4247ee338 100644 --- a/ocs2_pipg/src/PIPG.cpp +++ b/ocs2_pipg/src/PIPG.cpp @@ -383,35 +383,6 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect return status; }; -vector_t Pipg::HAbsRowSumInParallel(const std::vector<ScalarFunctionQuadraticApproximation>& cost) const { - const int N = ocpSize_.numStages; - const int nu_0 = ocpSize_.numInputs[0]; - vector_t res(getNumDecisionVariables()); - res.head(nu_0) = cost[0].dfduu.cwiseAbs().rowwise().sum(); - - int curRow = nu_0; - for (int k = 1; k < N; k++) { - const int nx_k = ocpSize_.numStates[k]; - const int nu_k = ocpSize_.numInputs[k]; - if (cost[k].dfdxx.size() != 0) { - res.segment(curRow, nx_k) = cost[k].dfdxx.cwiseAbs().rowwise().sum(); - } - if (cost[k].dfdux.size() != 0) { - res.segment(curRow, nx_k) += cost[k].dfdux.transpose().cwiseAbs().rowwise().sum(); - } - if (cost[k].dfdux.size() != 0) { - res.segment(curRow + nx_k, nu_k) = cost[k].dfdux.cwiseAbs().rowwise().sum(); - } - if (cost[k].dfduu.size() != 0) { - res.segment(curRow + nx_k, nu_k) += cost[k].dfduu.cwiseAbs().rowwise().sum(); - } - curRow += nx_k + nu_k; - } - const int nx_N = ocpSize_.numStates[N]; - res.tail(nx_N) = cost[N].dfdxx.cwiseAbs().rowwise().sum(); - return res; -} - void Pipg::resize(const OcpSize& ocpSize) { if (ocpSize_ == ocpSize) { return; @@ -532,104 +503,6 @@ void Pipg::verifyOcpSize(const OcpSize& ocpSize) const { } } -void Pipg::GGTAbsRowSumInParallel(const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, - const vector_array_t* scalingVectorsPtr, vector_t& res) { - const int N = ocpSize_.numStages; - if (N < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); - } - if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { - throw std::runtime_error("[PIPG] The size of scalingVectors doesn't match the number of stage."); - } - Eigen::setNbThreads(1); // No multithreading within Eigen. - - matrix_array_t tempMatrixArray(N); - vector_array_t absRowSumArray(N); - - std::atomic_int timeIndex{0}; - auto task = [&](int workerId) { - int k; - while ((k = timeIndex++) < N) { - const auto nx_next = ocpSize_.numStates[k + 1]; - const auto& B = dynamics[k].dfdu; - tempMatrixArray[k] = - (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_next, nx_next) - : (*scalingVectorsPtr)[k].cwiseProduct((*scalingVectorsPtr)[k]).asDiagonal().toDenseMatrix()); - tempMatrixArray[k] += B * B.transpose(); - - if (k != 0) { - const auto& A = dynamics[k].dfdx; - tempMatrixArray[k] += A * A.transpose(); - } - - absRowSumArray[k] = tempMatrixArray[k].cwiseAbs().rowwise().sum(); - - if (k != 0) { - const auto& A = dynamics[k].dfdx; - absRowSumArray[k] += (A * (scalingVectorsPtr == nullptr ? matrix_t::Identity(A.cols(), A.cols()) - : (*scalingVectorsPtr)[k - 1].asDiagonal().toDenseMatrix())) - .cwiseAbs() - .rowwise() - .sum(); - } - if (k != N - 1) { - const auto& ANext = dynamics[k + 1].dfdx; - absRowSumArray[k] += (ANext * (scalingVectorsPtr == nullptr ? matrix_t::Identity(ANext.cols(), ANext.cols()) - : (*scalingVectorsPtr)[k].asDiagonal().toDenseMatrix())) - .transpose() - .cwiseAbs() - .rowwise() - .sum(); - } - } - }; - runParallel(std::move(task)); - - res.setZero(getNumDynamicsConstraints()); - int curRow = 0; - for (const auto& v : absRowSumArray) { - res.segment(curRow, v.size()) = v; - curRow += v.size(); - } - - Eigen::setNbThreads(0); // Restore default setup. - - // /** - // * ********************************* - // * ************ Test begin ********* - // * ********************************* - // */ - // matrix_t m(getNumDynamicsConstraints(), getNumDynamicsConstraints()); - // const int nx_1 = ocpSize_.numStates[1]; - // const int nx_2 = ocpSize_.numStates[2]; - // const auto& A1 = dynamics[1].dfdx; - // m.topLeftCorner(nx_1, nx_1 + nx_2) << tempMatrixArray[0], - // -(A1 * (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_1, nx_1) : (*scalingVectorsPtr)[0].asDiagonal().toDenseMatrix())) - // .transpose(); - - // curRow = nx_1; - // for (int i = 1; i < N - 1; i++) { - // const int nx_i1 = ocpSize_.numStates[i + 1]; - // const int nx_i2 = ocpSize_.numStates[i + 2]; - // const auto& ANext = dynamics[i + 1].dfdx; - - // m.block(curRow, curRow, nx_i1, nx_i1 + nx_i2) << tempMatrixArray[i], - // -(ANext * (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_i1, nx_i1) : - // (*scalingVectorsPtr)[i].asDiagonal().toDenseMatrix())) - // .transpose(); - // curRow += nx_i1; - // } - // const int nx_N = ocpSize_.numStates[N]; - // m.block(curRow, curRow, nx_N, nx_N) = tempMatrixArray[N - 1]; - // std::cerr << "GGT: \n" << m.selfadjointView<Eigen::Upper>().toDenseMatrix() << "\n\n"; - // /** - // * ********************************* - // * ************ Test end ********* - // * ********************************* - // */ -} - int Pipg::getNumGeneralEqualityConstraints() const { const auto totalNumberOfGeneralEqualityConstraints = std::accumulate(ocpSize_.numIneqConstraints.begin(), ocpSize_.numIneqConstraints.end(), 0); @@ -667,18 +540,4 @@ Pipg::~Pipg() { std::cerr << getBenchmarkingInformationDense() << std::endl; } } - -vector_t matrixRowwiseAbsSum(const Eigen::SparseMatrix<scalar_t>& mat) { - vector_t rowAbsSum; - rowAbsSum.setZero(mat.rows()); - - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - int i = it.row(); - rowAbsSum(i) += std::abs(it.value()); - } - } - return rowAbsSum; -} - } // namespace ocs2 diff --git a/ocs2_pipg/test/testHelper.cpp b/ocs2_pipg/test/testHelper.cpp new file mode 100644 index 000000000..c931ca244 --- /dev/null +++ b/ocs2_pipg/test/testHelper.cpp @@ -0,0 +1,64 @@ +#include <gtest/gtest.h> + +#include "ocs2_pipg/HelperFunctions.h" + +#include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> +#include <ocs2_oc/test/testProblemsGeneration.h> + +class HelperFunctionTest : public testing::Test { + protected: + // x_0, x_1, ... x_{N - 1}, X_{N} + static constexpr size_t N_ = 10; // numStages + static constexpr size_t nx_ = 4; + static constexpr size_t nu_ = 3; + static constexpr size_t nc_ = 0; + static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); + static constexpr size_t numConstraints = N_ * (nx_ + nc_); + static constexpr bool verbose = true; + + HelperFunctionTest() : threadPool_(5, 99) { + srand(10); + + // Construct OCP problem + x0 = ocs2::vector_t::Random(nx_); + + for (int i = 0; i < N_; i++) { + dynamicsArray.push_back(ocs2::getRandomDynamics(nx_, nu_)); + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + } + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + ocpSize_ = ocs2::extractSizesFromProblem(dynamicsArray, costArray, nullptr); + + ocs2::getCostMatrix(ocpSize_, x0, costArray, costApproximation); + } + + ocs2::OcpSize ocpSize_; + ocs2::vector_t x0; + ocs2::ScalarFunctionQuadraticApproximation costApproximation; + + std::vector<ocs2::VectorFunctionLinearApproximation> dynamicsArray; + std::vector<ocs2::ScalarFunctionQuadraticApproximation> costArray; + std::vector<ocs2::VectorFunctionLinearApproximation> constraintsArray; + + ocs2::ThreadPool threadPool_; +}; + +TEST_F(HelperFunctionTest, hessianAbsRowSum) { + ocs2::vector_t rowwiseSum = ocs2::pipg::hessianAbsRowSum(ocpSize_, costArray); + EXPECT_TRUE(rowwiseSum.isApprox(costApproximation.dfdxx.cwiseAbs().rowwise().sum())) << "rowSum:\n" << rowwiseSum.transpose(); +} + +TEST_F(HelperFunctionTest, GGTAbsRowSumInParallel) { + ocs2::VectorFunctionLinearApproximation constraintsApproximation; + ocs2::vector_array_t scalingVectors(N_); + for (auto& v : scalingVectors) { + v = ocs2::vector_t::Random(nx_); + } + + ocs2::getConstraintMatrix(ocpSize_, x0, dynamicsArray, nullptr, &scalingVectors, constraintsApproximation); + ocs2::vector_t rowwiseSum = ocs2::pipg::GGTAbsRowSumInParallel(ocpSize_, dynamicsArray, nullptr, &scalingVectors, threadPool_); + ocs2::matrix_t GGT = constraintsApproximation.dfdx * constraintsApproximation.dfdx.transpose(); + EXPECT_TRUE(rowwiseSum.isApprox(GGT.cwiseAbs().rowwise().sum())); +} \ No newline at end of file diff --git a/ocs2_pipg/test/testPIPGSolver.cpp b/ocs2_pipg/test/testPIPGSolver.cpp index 47c9d5c85..9d4bd6613 100644 --- a/ocs2_pipg/test/testPIPGSolver.cpp +++ b/ocs2_pipg/test/testPIPGSolver.cpp @@ -148,12 +148,6 @@ TEST_F(PIPGSolverTest, correctness) { EXPECT_TRUE(std::abs(PIPGParallelCConstraintViolation - PIPGConstraintViolation) < pipgSolver.settings().absoluteTolerance * 10.0); } -TEST_F(PIPGSolverTest, HAbsRowSumInParallel) { - ocs2::vector_t rowwiseSum = pipgSolver.HAbsRowSumInParallel(costArray); - - EXPECT_TRUE(rowwiseSum.isApprox(costApproximation.dfdxx.cwiseAbs().rowwise().sum())); -} - TEST_F(PIPGSolverTest, descaleSolution) { ocs2::vector_array_t D(2 * N_); ocs2::vector_t DStacked(numDecisionVariables); From acaecb70605e6b60aa29ba5aca34f7740befb0cf Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Thu, 28 Apr 2022 21:41:44 +0200 Subject: [PATCH 223/512] tidy up code --- ocs2_pipg/include/ocs2_pipg/PIPG.h | 21 +++--- ocs2_pipg/src/PIPG.cpp | 114 ++++++++++++++++------------- ocs2_pipg/test/testPIPGSolver.cpp | 2 - 3 files changed, 73 insertions(+), 64 deletions(-) diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG.h b/ocs2_pipg/include/ocs2_pipg/PIPG.h index 63a5cde0f..cc8f1606e 100644 --- a/ocs2_pipg/include/ocs2_pipg/PIPG.h +++ b/ocs2_pipg/include/ocs2_pipg/PIPG.h @@ -85,9 +85,9 @@ class Pipg { void resize(const OcpSize& size); - int getNumDecisionVariables() const { return numDecisionVariables; } + int getNumDecisionVariables() const { return numDecisionVariables_; } - int getNumDynamicsConstraints() const { return numDynamicsConstraints; } + int getNumDynamicsConstraints() const { return numDynamicsConstraints_; } int getNumGeneralEqualityConstraints() const; @@ -106,7 +106,6 @@ class Pipg { scalar_t getAverageRunTimeInMilliseconds() const { return parallelizedQPTimer_.getAverageInMilliseconds(); } const Settings& settings() const { return pipgSettings_; } - Settings& settings() { return pipgSettings_; } const OcpSize& size() const { return ocpSize_; } ThreadPool& getThreadPool() { return threadPool_; } @@ -120,9 +119,9 @@ class Pipg { void runParallel(std::function<void(int)> taskFunction); private: - ThreadPool threadPool_; + const Settings pipgSettings_; - Settings pipgSettings_; + ThreadPool threadPool_; OcpSize ocpSize_; @@ -131,16 +130,16 @@ class Pipg { vector_array_t XNew_, UNew_, WNew_; // Problem size - int numDecisionVariables; - int numDynamicsConstraints; + int numDecisionVariables_; + int numDynamicsConstraints_; benchmark::RepeatedTimer denseQPTimer_; benchmark::RepeatedTimer parallelizedQPTimer_; // Profiling - benchmark::RepeatedTimer step1v_; - benchmark::RepeatedTimer step2z_; - benchmark::RepeatedTimer step3w_; - benchmark::RepeatedTimer step4CheckConvergence_; + benchmark::RepeatedTimer vComputationTimer_; + benchmark::RepeatedTimer zComputationTimer_; + benchmark::RepeatedTimer wComputationTimer_; + benchmark::RepeatedTimer convergenceCheckTimer_; }; } // namespace ocs2 diff --git a/ocs2_pipg/src/PIPG.cpp b/ocs2_pipg/src/PIPG.cpp index 4247ee338..0f3d6bca9 100644 --- a/ocs2_pipg/src/PIPG.cpp +++ b/ocs2_pipg/src/PIPG.cpp @@ -24,55 +24,64 @@ Pipg::SolverStatus Pipg::solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, co // Cold start vector_t z = vector_t::Zero(H.cols()); - vector_t z_old; + vector_t z_old = vector_t::Zero(H.cols()); - scalar_t alpha, beta; - vector_t v, w; - - w.setZero(g.rows()); + vector_t v = vector_t::Zero(g.rows()); + vector_t w = vector_t::Zero(g.rows()); + vector_t constraintsViolation(g.rows()); // Iteration number size_t k = 0; bool isConverged = false; scalar_t constraintsViolationInfNorm; - scalar_t zNorm; while (k < settings().maxNumIterations && !isConverged) { - alpha = 2.0 / ((k + 1.0) * mu + 2.0 * lambda); - beta = (k + 1) * mu / (2.0 * sigma); + const scalar_t alpha = 2.0 / ((k + 1.0) * mu + 2.0 * lambda); + const scalar_t beta = (k + 1) * mu / (2.0 * sigma); - step1v_.startTimer(); - z_old = z; + z_old.swap(z); + vComputationTimer_.startTimer(); // v = w + beta * (G * z - g); - v = w - beta * g; - v.noalias() += beta * (G * z); - step1v_.endTimer(); + v = -g; + v.noalias() += G * z; + v *= beta; + v += w; + vComputationTimer_.endTimer(); - step2z_.startTimer(); + zComputationTimer_.startTimer(); // z = z_old - alpha * (H * z_old + h + G.transpose() * v); - z = z_old - alpha * h; - z.noalias() -= alpha * (H * z_old); - z.noalias() -= alpha * (G.transpose() * v); - step2z_.endTimer(); - - step3w_.startTimer(); + z = -h; + z.noalias() -= (H * z_old); + z.noalias() -= (G.transpose() * v); + z *= alpha; + z.noalias() += z_old; + zComputationTimer_.endTimer(); + + wComputationTimer_.startTimer(); // w = w + beta * (G * z - g); w -= beta * g; w.noalias() += beta * (G * z); - step3w_.endTimer(); + wComputationTimer_.endTimer(); if (k % settings().checkTerminationInterval == 0) { - step4CheckConvergence_.startTimer(); - zNorm = z.squaredNorm() < 0.1 ? 1.0 : z.squaredNorm(); - constraintsViolationInfNorm = (EInv.asDiagonal() * (G * z - g)).lpNorm<Eigen::Infinity>(); + convergenceCheckTimer_.startTimer(); + const scalar_t zNorm = z.squaredNorm(); + + constraintsViolation.noalias() = G * z; + constraintsViolation -= g; + constraintsViolation.cwiseProduct(EInv); + constraintsViolationInfNorm = constraintsViolation.lpNorm<Eigen::Infinity>(); + + const vector_t z_delta = z - z_old; + const scalar_t z_deltaNorm = z_delta.squaredNorm(); isConverged = constraintsViolationInfNorm <= settings().absoluteTolerance && - (z - z_old).squaredNorm() <= settings().relativeTolerance * settings().relativeTolerance * zNorm; - step4CheckConvergence_.endTimer(); + (z_deltaNorm <= settings().relativeTolerance * settings().relativeTolerance * zNorm || + z_deltaNorm <= settings().absoluteTolerance); + convergenceCheckTimer_.endTimer(); } - k++; + ++k; } denseQPTimer_.endTimer(); - result = z; Pipg::SolverStatus status = isConverged ? Pipg::SolverStatus::SUCCESS : Pipg::SolverStatus::MAX_ITER; if (settings().displayShortSummary) { @@ -80,10 +89,13 @@ Pipg::SolverStatus Pipg::solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, co std::cerr << "\n++++++++++++++ PIPG-DenseQP " << pipg::toString(status) << " +++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; std::cerr << "Number of Iterations: " << k << " out of " << settings().maxNumIterations << "\n"; - std::cerr << "Relative change of primal solution : " << std::sqrt((z - z_old).squaredNorm() / zNorm) << "\n"; + std::cerr << "Norm of delta primal solution: " << (z - z_old).norm() << "\n"; std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; std::cerr << "Run time: " << denseQPTimer_.getLastIntervalInMilliseconds() << "\n"; } + + result.swap(z); + return status; }; @@ -96,7 +108,7 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect verifySizes(dynamics, cost, constraints); const int N = ocpSize_.numStages; if (N < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + throw std::runtime_error("[PIPG: solveOCPInParallel] The number of stages cannot be less than 1."); } if (scalingVectors.size() != N) { throw std::runtime_error("[PIPG: solveOCPInParallel] The size of scalingVectors doesn't match the number of stage."); @@ -176,8 +188,8 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect const auto& R = cost[t - 1].dfduu; const auto& Q = cost[t].dfdxx; const auto& P = cost[t - 1].dfdux; - const auto& hx = cost[t].dfdx; - const auto& hu = cost[t - 1].dfdu; + const auto& q = cost[t].dfdx; + const auto& r = cost[t - 1].dfdu; if (k != 0) { // Update W of the iteration k - 1. Move the update of W to the front of the calculation of V to prevent data race. @@ -210,14 +222,14 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect V_[t - 1].noalias() -= (beta + betaLast) * (A * X_[t - 1]); V_[t - 1].noalias() -= (beta + betaLast) * (B * U_[t - 1]); - // UNew_[t - 1] = U_[t - 1] - alpha * (R * U_[t - 1] + P * X_[t - 1] + hu - B.transpose() * V_[t - 1]); - UNew_[t - 1] = U_[t - 1] - alpha * hu; + // UNew_[t - 1] = U_[t - 1] - alpha * (R * U_[t - 1] + P * X_[t - 1] + r - B.transpose() * V_[t - 1]); + UNew_[t - 1] = U_[t - 1] - alpha * r; UNew_[t - 1].noalias() -= alpha * (R * U_[t - 1]); UNew_[t - 1].noalias() -= alpha * (P * X_[t - 1]); UNew_[t - 1].noalias() += alpha * (B.transpose() * V_[t - 1]); - // XNew_[t] = X_[t] - alpha * (Q * X_[t] + hx + C * V_[t - 1]); - XNew_[t] = X_[t] - alpha * hx; + // XNew_[t] = X_[t] - alpha * (Q * X_[t] + q + C * V_[t - 1]); + XNew_[t] = X_[t] - alpha * q; XNew_[t].array() -= alpha * C.array() * V_[t - 1].array(); XNew_[t].noalias() -= alpha * (Q * X_[t]); @@ -260,10 +272,10 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect solutionSSE = std::accumulate(solutionSEArray.begin(), solutionSEArray.end(), 0.0); solutionSquaredNorm = std::accumulate(solutionSquaredNormArray.begin(), solutionSquaredNormArray.end(), 0.0); - solutionSquaredNorm = solutionSquaredNorm < 0.1 ? 1.0 : solutionSquaredNorm; isConverged = constraintsViolationInfNorm <= settings().absoluteTolerance && - solutionSSE <= settings().relativeTolerance * settings().relativeTolerance * solutionSquaredNorm; + (solutionSSE <= settings().relativeTolerance * settings().relativeTolerance * solutionSquaredNorm || + solutionSSE <= settings().absoluteTolerance); keepRunning = k < settings().maxNumIterations && !isConverged; } @@ -368,7 +380,7 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect std::cerr << "\n++++++++++++++ PIPG-Parallel " << pipg::toString(status) << " +++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; std::cerr << "Number of Iterations: " << k << " out of " << settings().maxNumIterations << "\n"; - std::cerr << "Relative change of primal solution : " << std::sqrt(solutionSSE / solutionSquaredNorm) << "\n"; + std::cerr << "Norm of delta primal solution: " << std::sqrt(solutionSSE) << "\n"; std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; std::cerr << "Thread workload(ID: # of finished tasks): "; for (int i = 0; i < threadsWorkloadCounter.size(); i++) { @@ -392,9 +404,9 @@ void Pipg::resize(const OcpSize& ocpSize) { ocpSize_ = ocpSize; const int N = ocpSize_.numStages; - numDecisionVariables = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); - numDecisionVariables += std::accumulate(ocpSize_.numInputs.begin(), ocpSize_.numInputs.end(), 0); - numDynamicsConstraints = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); + numDecisionVariables_ = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); + numDecisionVariables_ += std::accumulate(ocpSize_.numInputs.begin(), ocpSize_.numInputs.end(), 0); + numDynamicsConstraints_ = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); X_.resize(N + 1); W_.resize(N); @@ -510,10 +522,10 @@ int Pipg::getNumGeneralEqualityConstraints() const { } std::string Pipg::getBenchmarkingInformationDense() const { - const auto step1v = step1v_.getTotalInMilliseconds(); - const auto step2z = step2z_.getTotalInMilliseconds(); - const auto step3w = step3w_.getTotalInMilliseconds(); - const auto step4CheckConvergence = step4CheckConvergence_.getTotalInMilliseconds(); + const auto step1v = vComputationTimer_.getTotalInMilliseconds(); + const auto step2z = zComputationTimer_.getTotalInMilliseconds(); + const auto step3w = wComputationTimer_.getTotalInMilliseconds(); + const auto step4CheckConvergence = convergenceCheckTimer_.getTotalInMilliseconds(); const auto benchmarkTotal = step1v + step2z + step3w + step4CheckConvergence; @@ -521,15 +533,15 @@ std::string Pipg::getBenchmarkingInformationDense() const { if (benchmarkTotal > 0.0) { const scalar_t inPercent = 100.0; infoStream << "\n########################################################################\n"; - infoStream << "The benchmarking is computed over " << step1v_.getNumTimedIntervals() << " iterations. \n"; + infoStream << "The benchmarking is computed over " << vComputationTimer_.getNumTimedIntervals() << " iterations. \n"; infoStream << "PIPG Dense Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; - infoStream << "\tstep1v :\t" << step1v_.getAverageInMilliseconds() << " [ms] \t\t(" + infoStream << "\tvComputation :\t" << vComputationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" << step1v / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\tstep2z :\t" << step2z_.getAverageInMilliseconds() << " [ms] \t\t(" + infoStream << "\tzComputation :\t" << zComputationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" << step2z / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\tstep3w :\t" << step3w_.getAverageInMilliseconds() << " [ms] \t\t(" + infoStream << "\twComputation :\t" << wComputationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" << step3w / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\tstep4CheckConvergence :\t" << step4CheckConvergence_.getAverageInMilliseconds() << " [ms] \t\t(" + infoStream << "\tCheckConvergence :\t" << convergenceCheckTimer_.getAverageInMilliseconds() << " [ms] \t\t(" << step4CheckConvergence / benchmarkTotal * inPercent << "%)\n"; } return infoStream.str(); diff --git a/ocs2_pipg/test/testPIPGSolver.cpp b/ocs2_pipg/test/testPIPGSolver.cpp index 9d4bd6613..4b892d0a9 100644 --- a/ocs2_pipg/test/testPIPGSolver.cpp +++ b/ocs2_pipg/test/testPIPGSolver.cpp @@ -164,10 +164,8 @@ TEST_F(PIPGSolverTest, descaleSolution) { DStacked.segment(curRow, v.size()) = v; curRow += v.size(); } - std::cerr << "DStacked:\n" << DStacked.transpose() << "\n\n"; ocs2::vector_t packedSolution; pipgSolver.packSolution(x, u, packedSolution); - std::cerr << "packedSolution:\n" << packedSolution.transpose() << "\n\n"; packedSolution.array() *= DStacked.array(); From 44daf89ee7abc61bbfa46bc23cb266a9f4be5f7f Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Fri, 29 Apr 2022 10:24:55 +0200 Subject: [PATCH 224/512] Delete pipg ocpsize --- ocs2_pipg/include/ocs2_pipg/OcpSize.h | 38 --------------------------- 1 file changed, 38 deletions(-) delete mode 100644 ocs2_pipg/include/ocs2_pipg/OcpSize.h diff --git a/ocs2_pipg/include/ocs2_pipg/OcpSize.h b/ocs2_pipg/include/ocs2_pipg/OcpSize.h deleted file mode 100644 index 20a89e45b..000000000 --- a/ocs2_pipg/include/ocs2_pipg/OcpSize.h +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -#include <vector> - -#include <ocs2_core/Types.h> - -namespace ocs2 { -namespace pipg { - -struct OcpSize { - int numStages; // Number of stages (N), all vectors below must be of size N+1 - std::vector<int> numInputs; // Number of inputs - std::vector<int> numStates; // Number of states - std::vector<int> numIneqConstraints; // Number of general inequality constraints - - /** Constructor for N stages with constant state and inputs and without constraints */ - explicit OcpSize(int N = 0, int nx = 0, int nu = 0) - : numStages(N), numStates(N + 1, nx), numInputs(N + 1, nu), numIneqConstraints(N + 1, 0) { - numInputs.back() = 0; - } -}; - -bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept; - -/** - * Extract sizes based on the problem data - * - * @param dynamics : Linearized approximation of the discrete dynamics. - * @param cost : Quadratic approximation of the cost. - * @param constraints : Linearized approximation of constraints. - * @return Derived sizes - */ -OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<ScalarFunctionQuadraticApproximation>& cost, - const std::vector<VectorFunctionLinearApproximation>* constraints); - -} // namespace pipg -} // namespace ocs2 \ No newline at end of file From 2bccb947dd4a307fcc99d277e669e657a0a6b744 Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Fri, 29 Apr 2022 11:27:10 +0200 Subject: [PATCH 225/512] remove namespace --- ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h | 3 -- .../include/ocs2_oc/pre_condition/Scaling.h | 28 +++++++++++-------- ocs2_oc/src/oc_problem/OcpSize.cpp | 3 -- ocs2_pipg/include/ocs2_pipg/PIPG.h | 12 ++++---- .../include/hpipm_catkin/HpipmInterface.h | 2 +- .../hpipm_catkin/test/testHpipmInterface.cpp | 2 +- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 6 ++-- 7 files changed, 27 insertions(+), 29 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h b/ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h index 414131f08..efad15ec4 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h @@ -34,8 +34,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> namespace ocs2 { -inline namespace hpipm_interface { - /** * Size of the optimal control problem to be solved with the HpipmInterface * @@ -88,5 +86,4 @@ OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximat const std::vector<ScalarFunctionQuadraticApproximation>& cost, const std::vector<VectorFunctionLinearApproximation>* constraints); -} // namespace hpipm_interface } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h index f38256295..d0269ad07 100644 --- a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h +++ b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h @@ -37,10 +37,12 @@ void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector * @param[in] iteration Number of iteration. * @param[in, out] dynamics The dynamics array od all time points. * @param[in, out] cost The cost array of all time points. - * @param[out] DOut - * @param[out] EOut - * @param[out] scalingVectors - * @param[out] cOut + * @param[out] DOut Scaling factor D + * @param[out] EOut Scaling factor E + * @param[out] scalingVectors Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components of this type of matrix for every + * timestamp. + * @param[out] cOut Scaling factor c * @param[in] threadPool External thread pool. * @param[in] H For test only. Can be removed. * @param[in] h For test only. Can be removed. @@ -52,15 +54,17 @@ void preConditioningInPlaceInParallel(const vector_t& x0, const OcpSize& ocpSize vector_array_t& scalingVectors, scalar_t& cOut, ThreadPool& threadPool, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G); /** - * @brief Scale the dynamics and cost array and construct scaling vector array from the given scaling factors E, D and c. + * @brief Scale the dynamics and cost array in place and construct scaling vector array from the given scaling factors E, D and c. * - * @param[in] ocpSize - * @param[in] D - * @param[in] E - * @param[in] c - * @param[in, out] dynamics - * @param[in, out] cost - * @param[out] scalingVectors + * @param[in] ocpSize The size of the oc problem. + * @param[in] D Scaling factor D + * @param[in] E Scaling factor E + * @param[in] c Scaling factor c + * @param[in, out] dynamics Dynamics array + * @param[in, out] cost Cost array + * @param[out] scalingVectors Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components of this type of matrix for every + * timestamp. */ void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, diff --git a/ocs2_oc/src/oc_problem/OcpSize.cpp b/ocs2_oc/src/oc_problem/OcpSize.cpp index cc42d063b..cd207aa19 100644 --- a/ocs2_oc/src/oc_problem/OcpSize.cpp +++ b/ocs2_oc/src/oc_problem/OcpSize.cpp @@ -30,8 +30,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_oc/oc_problem/OcpSize.h" namespace ocs2 { -inline namespace hpipm_interface { - bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept { // use && instead of &= to enable short-circuit evaluation bool same = lhs.numStages == rhs.numStages; @@ -71,5 +69,4 @@ OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximat return problemSize; } -} // namespace hpipm_interface } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG.h b/ocs2_pipg/include/ocs2_pipg/PIPG.h index cc8f1606e..539f470f0 100644 --- a/ocs2_pipg/include/ocs2_pipg/PIPG.h +++ b/ocs2_pipg/include/ocs2_pipg/PIPG.h @@ -64,15 +64,15 @@ class Pipg { * @brief Solve Optimal Control type QP in parallel. * * @param x0 Initial state - * @param dynamics - * @param cost - * @param constraints + * @param dynamics: Dynamics array. + * @param cost: Cost array. + * @param constraints: Constraints array. Pass nullptr for an unconstrained problem. * @param scalingVectors Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. After * scaling, they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. * @param EInv Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. - * @param mu - * @param lambda - * @param sigma + * @param mu: the lower bound of the cost hessian H. + * @param lambda: the upper bound of the cost hessian H. + * @param sigma: the upper bound of \f$ G^TG \f$ * @param costM For testing only. Can be removed. * @param constraintsM For testing only. Can be removed. * @return SolverStatus diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h index 2158e1503..bd3786c70 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h @@ -48,7 +48,7 @@ namespace ocs2 { */ class HpipmInterface { public: - using OcpSize = hpipm_interface::OcpSize; + using OcpSize = ::ocs2::OcpSize; using Settings = hpipm_interface::Settings; /** diff --git a/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp b/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp index 2bdaa2af7..df63bab24 100644 --- a/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp +++ b/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp @@ -241,7 +241,7 @@ TEST(test_hpiphm_interface, noInputs) { cost.emplace_back(ocs2::getRandomCost(nx, 0)); cost[N].dfdx = -cost[N].dfdxx * xSolGiven[N]; - const auto ocpSize = ocs2::hpipm_interface::extractSizesFromProblem(system, cost, nullptr); + const auto ocpSize = ocs2::extractSizesFromProblem(system, cost, nullptr); hpipmInterface.resize(ocpSize); // Solve! diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 6165b5321..9b4263055 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -46,7 +46,7 @@ MultipleShootingSolver::MultipleShootingSolver(Settings settings, const OptimalC const Initializer& initializer) : SolverBase(), settings_(std::move(settings)), - hpipmInterface_(hpipm_interface::OcpSize(), settings.hpipmSettings), + hpipmInterface_(OcpSize(), settings.hpipmSettings), threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { Eigen::setNbThreads(1); // No multithreading within Eigen. Eigen::initParallel(); @@ -251,10 +251,10 @@ MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolu hpipm_status status; const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &constraints_)); + hpipmInterface_.resize(extractSizesFromProblem(dynamics_, cost_, &constraints_)); status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); } else { // without constraints, or when using projection, we have an unconstrained QP. - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, nullptr)); + hpipmInterface_.resize(extractSizesFromProblem(dynamics_, cost_, nullptr)); status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); } From d374f06c0aa5c5d66b41f8b7b21ee0baf0e0e8e3 Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Fri, 29 Apr 2022 12:59:05 +0200 Subject: [PATCH 226/512] fix after merge --- ocs2_pipg/CMakeLists.txt | 12 +- .../include/ocs2_pipg/mpc/PipgMpcSolver.h | 5 +- ocs2_pipg/src/mpc/PipgMpcSolver.cpp | 57 ++++---- ocs2_pipg/test/testPIPGSolver.cpp | 14 -- ocs2_pipg/test/testPipgMpc.cpp | 128 ++++++++++++++++++ 5 files changed, 162 insertions(+), 54 deletions(-) create mode 100644 ocs2_pipg/test/testPipgMpc.cpp diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_pipg/CMakeLists.txt index a3e486cd4..97d404df3 100644 --- a/ocs2_pipg/CMakeLists.txt +++ b/ocs2_pipg/CMakeLists.txt @@ -43,7 +43,6 @@ include_directories( ) add_library(${PROJECT_NAME} - src/OcpSize.cpp src/PIPG_Settings.cpp src/HelperFunctions.cpp src/PIPG.cpp @@ -95,4 +94,13 @@ target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES} gtest_main -) \ No newline at end of file +) + +catkin_add_gtest(test_pipg_mpc + test/testPipgMpc.cpp +) +target_link_libraries(test_pipg_mpc + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main +) diff --git a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h index ad865f452..12a19f666 100644 --- a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h +++ b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h @@ -125,7 +125,6 @@ class PipgMpcSolver : public SolverBase { PrimalSolution primalSolution_; // Solver interface - pipg::Settings pipgSettings_; Pipg pipgSolver_; // LQ approximation @@ -147,9 +146,7 @@ class PipgMpcSolver : public SolverBase { benchmark::RepeatedTimer computeControllerTimer_; // PIPG Solver - benchmark::RepeatedTimer constructH_; - benchmark::RepeatedTimer constructG_; - benchmark::RepeatedTimer GTGMultiplication_; + benchmark::RepeatedTimer GGTMultiplication_; benchmark::RepeatedTimer lambdaEstimation_; benchmark::RepeatedTimer sigmaEstimation_; benchmark::RepeatedTimer preConditioning_; diff --git a/ocs2_pipg/src/mpc/PipgMpcSolver.cpp b/ocs2_pipg/src/mpc/PipgMpcSolver.cpp index 45ede93ab..a52da7970 100644 --- a/ocs2_pipg/src/mpc/PipgMpcSolver.cpp +++ b/ocs2_pipg/src/mpc/PipgMpcSolver.cpp @@ -1,12 +1,14 @@ #include "ocs2_pipg/mpc/PipgMpcSolver.h" -#include <ocs2_core/control/FeedforwardController.h> +#include <iostream> +#include <numeric> +#include <ocs2_core/control/FeedforwardController.h> +#include <ocs2_oc/pre_condition/Scaling.h> #include <ocs2_sqp/MultipleShootingInitialization.h> #include <ocs2_sqp/MultipleShootingTranscription.h> -#include <iostream> -#include <numeric> +#include "ocs2_pipg/HelperFunctions.h" namespace ocs2 { @@ -15,8 +17,7 @@ PipgMpcSolver::PipgMpcSolver(multiple_shooting::Settings sqpSettings, pipg::Sett : SolverBase(), settings_(std::move(sqpSettings)), threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority), - pipgSolver_(pipgSettings), - pipgSettings_(pipgSettings) { + pipgSolver_(pipgSettings) { Eigen::setNbThreads(1); // No multithreading within Eigen. Eigen::initParallel(); @@ -38,29 +39,22 @@ PipgMpcSolver::PipgMpcSolver(multiple_shooting::Settings sqpSettings, pipg::Sett } std::string PipgMpcSolver::getBenchmarkingInformationPIPG() const { - const auto constructH = constructH_.getTotalInMilliseconds(); - const auto constructG = constructG_.getTotalInMilliseconds(); - const auto GTGMultiplication = GTGMultiplication_.getTotalInMilliseconds(); + const auto GGTMultiplication = GGTMultiplication_.getTotalInMilliseconds(); const auto preConditioning = preConditioning_.getTotalInMilliseconds(); const auto lambdaEstimation = lambdaEstimation_.getTotalInMilliseconds(); const auto sigmaEstimation = sigmaEstimation_.getTotalInMilliseconds(); const auto pipgRuntime = pipgSolver_.getTotalRunTimeInMilliseconds(); - const auto benchmarkTotal = - constructH + constructG + GTGMultiplication + preConditioning + lambdaEstimation + sigmaEstimation + pipgRuntime; + const auto benchmarkTotal = GGTMultiplication + preConditioning + lambdaEstimation + sigmaEstimation + pipgRuntime; std::stringstream infoStream; if (benchmarkTotal > 0.0) { const scalar_t inPercent = 100.0; infoStream << "\n########################################################################\n"; - infoStream << "The benchmarking is computed over " << constructH_.getNumTimedIntervals() << " iterations. \n"; + infoStream << "The benchmarking is computed over " << GGTMultiplication_.getNumTimedIntervals() << " iterations. \n"; infoStream << "PIPG Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; - infoStream << "\tconstructH :\t" << std::setw(10) << constructH_.getAverageInMilliseconds() << " [ms] \t(" - << constructH / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\tconstructG :\t" << std::setw(10) << constructG_.getAverageInMilliseconds() << " [ms] \t(" - << constructG / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\tGTGMultiplication :\t" << std::setw(10) << GTGMultiplication_.getAverageInMilliseconds() << " [ms] \t(" - << GTGMultiplication / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tGGTMultiplication :\t" << std::setw(10) << GGTMultiplication_.getAverageInMilliseconds() << " [ms] \t(" + << GGTMultiplication / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tpreConditioning :\t" << std::setw(10) << preConditioning_.getAverageInMilliseconds() << " [ms] \t(" << preConditioning / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tlambdaEstimation :\t" << std::setw(10) << lambdaEstimation_.getAverageInMilliseconds() << " [ms] \t(" @@ -259,35 +253,30 @@ PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_ } // without constraints, or when using projection, we have an unconstrained QP. - pipgSolver_.resize(pipg::extractSizesFromProblem(dynamics_, cost_, nullptr)); - - constructH_.startTimer(); - constructH_.endTimer(); - - constructG_.startTimer(); - constructG_.endTimer(); + pipgSolver_.resize(extractSizesFromProblem(dynamics_, cost_, nullptr)); vector_array_t D, E; vector_array_t scalingVectors; scalar_t c; preConditioning_.startTimer(); - pipgSolver_.preConditioningInPlaceInParallel(delta_x0, dynamics_, cost_, pipgSettings_.numScaling, D, E, scalingVectors, c, - Eigen::SparseMatrix<scalar_t>(), vector_t(), Eigen::SparseMatrix<scalar_t>()); + preConditioningInPlaceInParallel(delta_x0, pipgSolver_.size(), pipgSolver_.settings().numScaling, dynamics_, cost_, D, E, scalingVectors, + c, pipgSolver_.getThreadPool(), Eigen::SparseMatrix<scalar_t>(), vector_t(), + Eigen::SparseMatrix<scalar_t>()); preConditioning_.endTimer(); lambdaEstimation_.startTimer(); - vector_t rowwiseAbsSumH = pipgSolver_.HAbsRowSumInParallel(cost_); - scalar_t lambdaScaled = rowwiseAbsSumH.maxCoeff(); + const vector_t rowwiseAbsSumH = pipg::hessianAbsRowSum(pipgSolver_.size(), cost_); + const scalar_t lambdaScaled = rowwiseAbsSumH.maxCoeff(); lambdaEstimation_.endTimer(); - GTGMultiplication_.startTimer(); - vector_t rowwiseAbsSumGGT; - pipgSolver_.GGTAbsRowSumInParallel(dynamics_, nullptr, &scalingVectors, rowwiseAbsSumGGT); - GTGMultiplication_.endTimer(); + GGTMultiplication_.startTimer(); + const vector_t rowwiseAbsSumGGT = + pipg::GGTAbsRowSumInParallel(pipgSolver_.size(), dynamics_, nullptr, &scalingVectors, pipgSolver_.getThreadPool()); + GGTMultiplication_.endTimer(); sigmaEstimation_.startTimer(); - scalar_t sigmaScaled = rowwiseAbsSumGGT.maxCoeff(); + const scalar_t sigmaScaled = rowwiseAbsSumGGT.maxCoeff(); sigmaEstimation_.endTimer(); scalar_t maxScalingFactor = -1; @@ -296,7 +285,7 @@ PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_ maxScalingFactor = std::max(maxScalingFactor, v.maxCoeff()); } } - scalar_t muEstimated = pipgSettings_.lowerBoundH * c * maxScalingFactor * maxScalingFactor; + const scalar_t muEstimated = pipgSolver_.settings().lowerBoundH * c * maxScalingFactor * maxScalingFactor; vector_array_t EInv(E.size()); std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); diff --git a/ocs2_pipg/test/testPIPGSolver.cpp b/ocs2_pipg/test/testPIPGSolver.cpp index 4a63e0985..4b892d0a9 100644 --- a/ocs2_pipg/test/testPIPGSolver.cpp +++ b/ocs2_pipg/test/testPIPGSolver.cpp @@ -21,20 +21,6 @@ ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs return settings; } -ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, - ocs2::scalar_t relativeTolerance, bool verbose) { - ocs2::pipg::Settings settings; - settings.nThreads = nThreads; - settings.maxNumIterations = maxNumIterations; - settings.absoluteTolerance = absoluteTolerance; - settings.relativeTolerance = relativeTolerance; - settings.checkTerminationInterval = 1; - settings.numScaling = 3; - settings.displayShortSummary = verbose; - - return settings; -} - class PIPGSolverTest : public testing::Test { protected: // x_0, x_1, ... x_{N - 1}, X_{N} diff --git a/ocs2_pipg/test/testPipgMpc.cpp b/ocs2_pipg/test/testPipgMpc.cpp new file mode 100644 index 000000000..c270522b3 --- /dev/null +++ b/ocs2_pipg/test/testPipgMpc.cpp @@ -0,0 +1,128 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <gtest/gtest.h> + +#include "ocs2_pipg/mpc/PipgMpcSolver.h" + +#include <ocs2_core/initialization/DefaultInitializer.h> + +#include <ocs2_oc/synchronized_module/ReferenceManager.h> +#include <ocs2_oc/test/testProblemsGeneration.h> + +namespace ocs2 { +namespace { + +std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunctionLinearApproximation& dynamicsMatrices, + const ScalarFunctionQuadraticApproximation& costMatrices, + const ocs2::scalar_t tol) { + int n = dynamicsMatrices.dfdu.rows(); + int m = dynamicsMatrices.dfdu.cols(); + + ocs2::OptimalControlProblem problem; + + // System + problem.dynamicsPtr = getOcs2Dynamics(dynamicsMatrices); + + // Cost + problem.costPtr->add("intermediateCost", ocs2::getOcs2Cost(costMatrices)); + problem.finalCostPtr->add("finalCost", ocs2::getOcs2StateCost(costMatrices)); + + // Reference Manager + ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Ones(n)}, {ocs2::vector_t::Ones(m)}); + std::shared_ptr<ReferenceManager> referenceManagerPtr(new ReferenceManager(targetTrajectories)); + + problem.targetTrajectoriesPtr = &referenceManagerPtr->getTargetTrajectories(); + + problem.equalityConstraintPtr->add("intermediateCost", ocs2::getOcs2Constraints(getRandomConstraints(n, m, 0))); + + ocs2::DefaultInitializer zeroInitializer(m); + + // Solver settings + auto sqpSettings = []() { + ocs2::multiple_shooting::Settings settings; + settings.dt = 0.05; + settings.sqpIteration = 10; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = false; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 100; + + return settings; + }(); + + auto pipgSettings = [&]() { + ocs2::pipg::Settings settings; + settings.nThreads = 100; + settings.maxNumIterations = 30000; + settings.absoluteTolerance = tol; + settings.relativeTolerance = 1e-2; + settings.numScaling = 3; + settings.lowerBoundH = 1e-3; + settings.checkTerminationInterval = 1; + settings.displayShortSummary = true; + + return settings; + }(); + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = ocs2::vector_t::Ones(n); + + // Construct solver + ocs2::PipgMpcSolver solver(sqpSettings, pipgSettings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + + // Solve + solver.run(startTime, initState, finalTime); + return {solver.primalSolution(finalTime), solver.getIterationsLog()}; +} + +} // namespace +} // namespace ocs2 + +TEST(test_unconstrained, correctness) { + int n = 3; + int m = 2; + const double tol = 1e-9; + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto costs = ocs2::getRandomCost(n, m); + const auto result = ocs2::solve(dynamics, costs, tol); + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(result.second.size(), 2); + ASSERT_LT(result.second.back().dynamicsViolationSSE, tol); +} From 5a09e8fbcfa771b8bdbb9c8415bd428826a4439b Mon Sep 17 00:00:00 2001 From: Fu Zhengyu <zhengfuaj@gmail.com> Date: Fri, 29 Apr 2022 13:05:07 +0200 Subject: [PATCH 227/512] Move member variables under comment --- ocs2_pipg/include/ocs2_pipg/PIPG.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG.h b/ocs2_pipg/include/ocs2_pipg/PIPG.h index 539f470f0..a33e8ed85 100644 --- a/ocs2_pipg/include/ocs2_pipg/PIPG.h +++ b/ocs2_pipg/include/ocs2_pipg/PIPG.h @@ -133,10 +133,12 @@ class Pipg { int numDecisionVariables_; int numDynamicsConstraints_; + // Profiling + // Dense PIPG benchmark::RepeatedTimer denseQPTimer_; benchmark::RepeatedTimer parallelizedQPTimer_; - // Profiling + // Parallel PIPG benchmark::RepeatedTimer vComputationTimer_; benchmark::RepeatedTimer zComputationTimer_; benchmark::RepeatedTimer wComputationTimer_; From 0cdcc5fd1f8dbdcc6e59deaa713cd94d44f73993 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 13 May 2022 09:54:23 +0200 Subject: [PATCH 228/512] add gradient clipping --- ocs2_doc/docs/mpcnet.rst | 7 +++++++ .../python/ocs2_ballbot_mpcnet/config/ballbot.yaml | 2 ++ .../ocs2_legged_robot_mpcnet/config/legged_robot.yaml | 2 ++ .../ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py | 6 ++++++ 4 files changed, 17 insertions(+) diff --git a/ocs2_doc/docs/mpcnet.rst b/ocs2_doc/docs/mpcnet.rst index 11e8f44d8..39993c850 100644 --- a/ocs2_doc/docs/mpcnet.rst +++ b/ocs2_doc/docs/mpcnet.rst @@ -145,6 +145,13 @@ The most important classes/files that have to be implemented are: * **mpcnet.py**: Adds robot-specific methods, e.g. implements the tasks that the robot should execute, for the MPC-Net training. * **train.py**: Starts the main training script. +Known Issues +~~~~~~~~~~~~ + +Stiff inequality constraints can lead to very large Hamiltonians and gradients of the Hamilltonian near the log barrier. +This can obstruct the learning process and the policy might not learn something useful. +In that case, enable the gradient clipping in the robot's MPC-Net YAML configuration file and tune the gradient clipping value. + References ~~~~~~~~~~ diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml index 17a645088..2ac88d134 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml @@ -120,3 +120,5 @@ config: BATCH_SIZE: 32 LEARNING_RATE: 1.e-2 LEARNING_ITERATIONS: 10000 + GRADIENT_CLIPPING: False + GRADIENT_CLIPPING_VALUE: 1.0 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml index e330b3442..46f4597c1 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml @@ -237,3 +237,5 @@ config: BATCH_SIZE: 128 LEARNING_RATE: 1.e-3 LEARNING_ITERATIONS: 100000 + GRADIENT_CLIPPING: False + GRADIENT_CLIPPING_VALUE: 1.0 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index 3ba9818a7..46dbd452a 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -269,6 +269,9 @@ def normal_closure(): empirical_loss = self.experts_loss(x, x, input, u, p, p, dHdxx, dHdux, dHduu, dHdx, dHdu, H) # compute the gradients empirical_loss.backward() + # clip the gradients + if self.config.GRADIENT_CLIPPING: + torch.nn.utils.clip_grad_norm_(self.policy.parameters(), self.config.GRADIENT_CLIPPING_VALUE) # logging self.writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) # return empirical loss @@ -287,6 +290,9 @@ def cheating_closure(): empirical_loss = empirical_experts_loss + self.config.LAMBDA * empirical_gating_loss # compute the gradients empirical_loss.backward() + # clip the gradients + if self.config.GRADIENT_CLIPPING: + torch.nn.utils.clip_grad_norm_(self.policy.parameters(), self.config.GRADIENT_CLIPPING_VALUE) # logging self.writer.add_scalar("objective/empirical_experts_loss", empirical_experts_loss.item(), iteration) self.writer.add_scalar("objective/empirical_gating_loss", empirical_gating_loss.item(), iteration) From 426030a8bb676b6b2ca9eed279315db29c99bb6c Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Jul 2022 11:17:13 +0200 Subject: [PATCH 229/512] remove unnecessary key --- .../ocs2_ballbot_mpcnet/config/ballbot.yaml | 243 +++++---- .../config/legged_robot.yaml | 477 +++++++++--------- .../python/ocs2_mpcnet_core/config.py | 2 +- 3 files changed, 360 insertions(+), 362 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml index 17a645088..82dc5e1a8 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml @@ -1,122 +1,121 @@ -config: - # - # general - # - # name of the robot - NAME: "ballbot" - # description of the training run - DESCRIPTION: "description" - # state dimension - STATE_DIM: 10 - # input dimension - INPUT_DIM: 3 - # target trajectories state dimension - TARGET_STATE_DIM: 10 - # target trajectories input dimension - TARGET_INPUT_DIM: 3 - # observation dimension - OBSERVATION_DIM: 10 - # action dimension - ACTION_DIM: 3 - # expert number - EXPERT_NUM: 1 - # default state - DEFAULT_STATE: - - 0.0 # pose x - - 0.0 # pose y - - 0.0 # pose yaw - - 0.0 # pose pitch - - 0.0 # pose roll - - 0.0 # twist x - - 0.0 # twist y - - 0.0 # twist yaw - - 0.0 # twist pitch - - 0.0 # twist roll - # default target state - DEFAULT_TARGET_STATE: - - 0.0 # pose x - - 0.0 # pose y - - 0.0 # pose yaw - - 0.0 # pose pitch - - 0.0 # pose roll - - 0.0 # twist x - - 0.0 # twist y - - 0.0 # twist yaw - - 0.0 # twist pitch - - 0.0 # twist roll - # - # loss - # - # epsilon to improve numerical stability of logs and denominators - EPSILON: 1.e-8 - # whether to cheat by adding the gating loss - CHEATING: False - # parameter to control the relative importance of both loss types - LAMBDA: 1.0 - # dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) - EXPERT_FOR_MODE: - 0: 0 - # input cost for behavioral cloning - R: - - 2.0 # torque - - 2.0 # torque - - 2.0 # torque - # - # memory - # - # capacity of the memory - CAPACITY: 100000 - # - # policy - # - # observation scaling - OBSERVATION_SCALING: - - 1.0 # pose x - - 1.0 # pose y - - 1.0 # pose yaw - - 1.0 # pose pitch - - 1.0 # pose roll - - 1.0 # twist x - - 1.0 # twist y - - 1.0 # twist yaw - - 1.0 # twist pitch - - 1.0 # twist roll - # action scaling - ACTION_SCALING: - - 1.0 # torque - - 1.0 # torque - - 1.0 # torque - # - # rollout - # - # RaiSim or TimeTriggered rollout for data generation and policy evaluation - RAISIM: False - # settings for data generation - DATA_GENERATION_TIME_STEP: 0.1 - DATA_GENERATION_DURATION: 3.0 - DATA_GENERATION_DATA_DECIMATION: 1 - DATA_GENERATION_THREADS: 2 - DATA_GENERATION_TASKS: 10 - DATA_GENERATION_SAMPLES: 2 - DATA_GENERATION_SAMPLING_VARIANCE: - - 0.01 # pose x - - 0.01 # pose y - - 0.01745329251 # pose yaw: 1.0 / 180.0 * pi - - 0.01745329251 # pose pitch: 1.0 / 180.0 * pi - - 0.01745329251 # pose roll: 1.0 / 180.0 * pi - - 0.05 # twist x - - 0.05 # twist y - - 0.08726646259 # twist yaw: 5.0 / 180.0 * pi - - 0.08726646259 # twist pitch: 5.0 / 180.0 * pi - - 0.08726646259 # twist roll: 5.0 / 180.0 * pi - # settings for computing metrics - POLICY_EVALUATION_TIME_STEP: 0.1 - POLICY_EVALUATION_DURATION: 3.0 - POLICY_EVALUATION_THREADS: 1 - POLICY_EVALUATION_TASKS: 5 - # - # training - # - BATCH_SIZE: 32 - LEARNING_RATE: 1.e-2 - LEARNING_ITERATIONS: 10000 +# +# general +# +# name of the robot +NAME: "ballbot" +# description of the training run +DESCRIPTION: "description" +# state dimension +STATE_DIM: 10 +# input dimension +INPUT_DIM: 3 +# target trajectories state dimension +TARGET_STATE_DIM: 10 +# target trajectories input dimension +TARGET_INPUT_DIM: 3 +# observation dimension +OBSERVATION_DIM: 10 +# action dimension +ACTION_DIM: 3 +# expert number +EXPERT_NUM: 1 +# default state +DEFAULT_STATE: + - 0.0 # pose x + - 0.0 # pose y + - 0.0 # pose yaw + - 0.0 # pose pitch + - 0.0 # pose roll + - 0.0 # twist x + - 0.0 # twist y + - 0.0 # twist yaw + - 0.0 # twist pitch + - 0.0 # twist roll +# default target state +DEFAULT_TARGET_STATE: + - 0.0 # pose x + - 0.0 # pose y + - 0.0 # pose yaw + - 0.0 # pose pitch + - 0.0 # pose roll + - 0.0 # twist x + - 0.0 # twist y + - 0.0 # twist yaw + - 0.0 # twist pitch + - 0.0 # twist roll +# +# loss +# +# epsilon to improve numerical stability of logs and denominators +EPSILON: 1.e-8 +# whether to cheat by adding the gating loss +CHEATING: False +# parameter to control the relative importance of both loss types +LAMBDA: 1.0 +# dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) +EXPERT_FOR_MODE: + 0: 0 +# input cost for behavioral cloning +R: + - 2.0 # torque + - 2.0 # torque + - 2.0 # torque +# +# memory +# +# capacity of the memory +CAPACITY: 100000 +# +# policy +# +# observation scaling +OBSERVATION_SCALING: + - 1.0 # pose x + - 1.0 # pose y + - 1.0 # pose yaw + - 1.0 # pose pitch + - 1.0 # pose roll + - 1.0 # twist x + - 1.0 # twist y + - 1.0 # twist yaw + - 1.0 # twist pitch + - 1.0 # twist roll +# action scaling +ACTION_SCALING: + - 1.0 # torque + - 1.0 # torque + - 1.0 # torque +# +# rollout +# +# RaiSim or TimeTriggered rollout for data generation and policy evaluation +RAISIM: False +# settings for data generation +DATA_GENERATION_TIME_STEP: 0.1 +DATA_GENERATION_DURATION: 3.0 +DATA_GENERATION_DATA_DECIMATION: 1 +DATA_GENERATION_THREADS: 2 +DATA_GENERATION_TASKS: 10 +DATA_GENERATION_SAMPLES: 2 +DATA_GENERATION_SAMPLING_VARIANCE: + - 0.01 # pose x + - 0.01 # pose y + - 0.01745329251 # pose yaw: 1.0 / 180.0 * pi + - 0.01745329251 # pose pitch: 1.0 / 180.0 * pi + - 0.01745329251 # pose roll: 1.0 / 180.0 * pi + - 0.05 # twist x + - 0.05 # twist y + - 0.08726646259 # twist yaw: 5.0 / 180.0 * pi + - 0.08726646259 # twist pitch: 5.0 / 180.0 * pi + - 0.08726646259 # twist roll: 5.0 / 180.0 * pi +# settings for computing metrics +POLICY_EVALUATION_TIME_STEP: 0.1 +POLICY_EVALUATION_DURATION: 3.0 +POLICY_EVALUATION_THREADS: 1 +POLICY_EVALUATION_TASKS: 5 +# +# training +# +BATCH_SIZE: 32 +LEARNING_RATE: 1.e-2 +LEARNING_ITERATIONS: 10000 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml index a873bcd08..446b3fe6e 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml @@ -1,239 +1,238 @@ -config: - # - # general - # - # name of the robot - NAME: "legged_robot" - # description of the training run - DESCRIPTION: "description" - # state dimension - STATE_DIM: 24 - # input dimension - INPUT_DIM: 24 - # target trajectories state dimension - TARGET_STATE_DIM: 24 - # target trajectories input dimension - TARGET_INPUT_DIM: 24 - # observation dimension - OBSERVATION_DIM: 36 - # action dimension - ACTION_DIM: 24 - # expert number - EXPERT_NUM: 3 - # default state - DEFAULT_STATE: - - 0.0 # normalized linear momentum x - - 0.0 # normalized linear momentum y - - 0.0 # normalized linear momentum z - - 0.0 # normalized angular momentum x - - 0.0 # normalized angular momentum y - - 0.0 # normalized angular momentum z - - 0.0 # position x - - 0.0 # position y - - 0.575 # position z - - 0.0 # orientation z - - 0.0 # orientation y - - 0.0 # orientation x - - -0.25 # joint position LF HAA - - 0.6 # joint position LF HFE - - -0.85 # joint position LF KFE - - -0.25 # joint position LH HAA - - -0.6 # joint position LH HFE - - 0.85 # joint position LH KFE - - 0.25 # joint position RF HAA - - 0.6 # joint position RF HFE - - -0.85 # joint position RF KFE - - 0.25 # joint position RH HAA - - -0.6 # joint position RH HFE - - 0.85 # joint position RH KFE - # default target state - DEFAULT_TARGET_STATE: - - 0.0 # normalized linear momentum x - - 0.0 # normalized linear momentum y - - 0.0 # normalized linear momentum z - - 0.0 # normalized angular momentum x - - 0.0 # normalized angular momentum y - - 0.0 # normalized angular momentum z - - 0.0 # position x - - 0.0 # position y - - 0.575 # position z - - 0.0 # orientation z - - 0.0 # orientation y - - 0.0 # orientation x - - -0.25 # joint position LF HAA - - 0.6 # joint position LF HFE - - -0.85 # joint position LF KFE - - -0.25 # joint position LH HAA - - -0.6 # joint position LH HFE - - 0.85 # joint position LH KFE - - 0.25 # joint position RF HAA - - 0.6 # joint position RF HFE - - -0.85 # joint position RF KFE - - 0.25 # joint position RH HAA - - -0.6 # joint position RH HFE - - 0.85 # joint position RH KFE - # - # loss - # - # epsilon to improve numerical stability of logs and denominators - EPSILON: 1.e-8 - # whether to cheat by adding the gating loss - CHEATING: True - # parameter to control the relative importance of both loss types - LAMBDA: 10.0 - # dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) - EXPERT_FOR_MODE: - 6: 1 # trot - 9: 2 # trot - 15: 0 # stance - # input cost for behavioral cloning - R: - - 0.001 # contact force LF x - - 0.001 # contact force LF y - - 0.001 # contact force LF z - - 0.001 # contact force LH x - - 0.001 # contact force LH y - - 0.001 # contact force LH z - - 0.001 # contact force RF x - - 0.001 # contact force RF y - - 0.001 # contact force RF z - - 0.001 # contact force RH x - - 0.001 # contact force RH y - - 0.001 # contact force RH z - - 5.0 # joint velocity LF HAA - - 5.0 # joint velocity LF HFE - - 5.0 # joint velocity LF KFE - - 5.0 # joint velocity LH HAA - - 5.0 # joint velocity LH HFE - - 5.0 # joint velocity LH KFE - - 5.0 # joint velocity RF HAA - - 5.0 # joint velocity RF HFE - - 5.0 # joint velocity RF KFE - - 5.0 # joint velocity RH HAA - - 5.0 # joint velocity RH HFE - - 5.0 # joint velocity RH KFE - # - # memory - # - # capacity of the memory - CAPACITY: 400000 - # - # policy - # - # observation scaling - OBSERVATION_SCALING: - - 1.0 # swing phase LF - - 1.0 # swing phase LH - - 1.0 # swing phase RF - - 1.0 # swing phase RH - - 1.0 # swing phase rate LF - - 1.0 # swing phase rate LH - - 1.0 # swing phase rate RF - - 1.0 # swing phase rate RH - - 1.0 # sinusoidal bump LF - - 1.0 # sinusoidal bump LH - - 1.0 # sinusoidal bump RF - - 1.0 # sinusoidal bump RH - - 1.0 # normalized linear momentum x - - 1.0 # normalized linear momentum y - - 1.0 # normalized linear momentum z - - 1.0 # normalized angular momentum x - - 1.0 # normalized angular momentum y - - 1.0 # normalized angular momentum z - - 1.0 # position x - - 1.0 # position y - - 1.0 # position z - - 1.0 # orientation z - - 1.0 # orientation y - - 1.0 # orientation x - - 1.0 # joint position LF HAA - - 1.0 # joint position LF HFE - - 1.0 # joint position LF KFE - - 1.0 # joint position LH HAA - - 1.0 # joint position LH HFE - - 1.0 # joint position LH KFE - - 1.0 # joint position RF HAA - - 1.0 # joint position RF HFE - - 1.0 # joint position RF KFE - - 1.0 # joint position RH HAA - - 1.0 # joint position RH HFE - - 1.0 # joint position RH KFE - # action scaling - ACTION_SCALING: - - 100.0 # contact force LF x - - 100.0 # contact force LF y - - 100.0 # contact force LF z - - 100.0 # contact force LH x - - 100.0 # contact force LH y - - 100.0 # contact force LH z - - 100.0 # contact force RF x - - 100.0 # contact force RF y - - 100.0 # contact force RF z - - 100.0 # contact force RH x - - 100.0 # contact force RH y - - 100.0 # contact force RH z - - 10.0 # joint velocity LF HAA - - 10.0 # joint velocity LF HFE - - 10.0 # joint velocity LF KFE - - 10.0 # joint velocity LH HAA - - 10.0 # joint velocity LH HFE - - 10.0 # joint velocity LH KFE - - 10.0 # joint velocity RF HAA - - 10.0 # joint velocity RF HFE - - 10.0 # joint velocity RF KFE - - 10.0 # joint velocity RH HAA - - 10.0 # joint velocity RH HFE - - 10.0 # joint velocity RH KFE - # - # rollout - # - # RaiSim or TimeTriggered rollout for data generation and policy evaluation - RAISIM: True - # weights defining how often a gait is chosen for rollout - WEIGHTS_FOR_GAITS: - stance: 1.0 - trot_1: 2.0 - trot_2: 2.0 - # settings for data generation - DATA_GENERATION_TIME_STEP: 0.0025 - DATA_GENERATION_DURATION: 4.0 - DATA_GENERATION_DATA_DECIMATION: 4 - DATA_GENERATION_THREADS: 12 - DATA_GENERATION_TASKS: 12 - DATA_GENERATION_SAMPLES: 2 - DATA_GENERATION_SAMPLING_VARIANCE: - - 0.05 # normalized linear momentum x - - 0.05 # normalized linear momentum y - - 0.05 # normalized linear momentum z - - 0.00135648942 # normalized angular momentum x: 1.62079 / 52.1348 * 2.5 / 180.0 * pi - - 0.00404705526 # normalized angular momentum y: 4.83559 / 52.1348 * 2.5 / 180.0 * pi - - 0.00395351148 # normalized angular momentum z: 4.72382 / 52.1348 * 2.5 / 180.0 * pi - - 0.01 # position x - - 0.01 # position y - - 0.01 # position z - - 0.00872664625 # orientation z: 0.5 / 180.0 * pi - - 0.00872664625 # orientation y: 0.5 / 180.0 * pi - - 0.00872664625 # orientation x: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LF HAA: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LF HFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LF KFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LH HAA: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LH HFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position LH KFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RF HAA: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RF HFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RF KFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RH HAA: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RH HFE: 0.5 / 180.0 * pi - - 0.00872664625 # joint position RH KFE: 0.5 / 180.0 * pi - # settings for computing metrics - POLICY_EVALUATION_TIME_STEP: 0.0025 - POLICY_EVALUATION_DURATION: 4.0 - POLICY_EVALUATION_THREADS: 3 - POLICY_EVALUATION_TASKS: 3 - # - # training - # - BATCH_SIZE: 128 - LEARNING_RATE: 1.e-3 - LEARNING_ITERATIONS: 100000 +# +# general +# +# name of the robot +NAME: "legged_robot" +# description of the training run +DESCRIPTION: "description" +# state dimension +STATE_DIM: 24 +# input dimension +INPUT_DIM: 24 +# target trajectories state dimension +TARGET_STATE_DIM: 24 +# target trajectories input dimension +TARGET_INPUT_DIM: 24 +# observation dimension +OBSERVATION_DIM: 36 +# action dimension +ACTION_DIM: 24 +# expert number +EXPERT_NUM: 3 +# default state +DEFAULT_STATE: + - 0.0 # normalized linear momentum x + - 0.0 # normalized linear momentum y + - 0.0 # normalized linear momentum z + - 0.0 # normalized angular momentum x + - 0.0 # normalized angular momentum y + - 0.0 # normalized angular momentum z + - 0.0 # position x + - 0.0 # position y + - 0.575 # position z + - 0.0 # orientation z + - 0.0 # orientation y + - 0.0 # orientation x + - -0.25 # joint position LF HAA + - 0.6 # joint position LF HFE + - -0.85 # joint position LF KFE + - -0.25 # joint position LH HAA + - -0.6 # joint position LH HFE + - 0.85 # joint position LH KFE + - 0.25 # joint position RF HAA + - 0.6 # joint position RF HFE + - -0.85 # joint position RF KFE + - 0.25 # joint position RH HAA + - -0.6 # joint position RH HFE + - 0.85 # joint position RH KFE +# default target state +DEFAULT_TARGET_STATE: + - 0.0 # normalized linear momentum x + - 0.0 # normalized linear momentum y + - 0.0 # normalized linear momentum z + - 0.0 # normalized angular momentum x + - 0.0 # normalized angular momentum y + - 0.0 # normalized angular momentum z + - 0.0 # position x + - 0.0 # position y + - 0.575 # position z + - 0.0 # orientation z + - 0.0 # orientation y + - 0.0 # orientation x + - -0.25 # joint position LF HAA + - 0.6 # joint position LF HFE + - -0.85 # joint position LF KFE + - -0.25 # joint position LH HAA + - -0.6 # joint position LH HFE + - 0.85 # joint position LH KFE + - 0.25 # joint position RF HAA + - 0.6 # joint position RF HFE + - -0.85 # joint position RF KFE + - 0.25 # joint position RH HAA + - -0.6 # joint position RH HFE + - 0.85 # joint position RH KFE +# +# loss +# +# epsilon to improve numerical stability of logs and denominators +EPSILON: 1.e-8 +# whether to cheat by adding the gating loss +CHEATING: True +# parameter to control the relative importance of both loss types +LAMBDA: 10.0 +# dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) +EXPERT_FOR_MODE: + 6: 1 # trot + 9: 2 # trot + 15: 0 # stance +# input cost for behavioral cloning +R: + - 0.001 # contact force LF x + - 0.001 # contact force LF y + - 0.001 # contact force LF z + - 0.001 # contact force LH x + - 0.001 # contact force LH y + - 0.001 # contact force LH z + - 0.001 # contact force RF x + - 0.001 # contact force RF y + - 0.001 # contact force RF z + - 0.001 # contact force RH x + - 0.001 # contact force RH y + - 0.001 # contact force RH z + - 5.0 # joint velocity LF HAA + - 5.0 # joint velocity LF HFE + - 5.0 # joint velocity LF KFE + - 5.0 # joint velocity LH HAA + - 5.0 # joint velocity LH HFE + - 5.0 # joint velocity LH KFE + - 5.0 # joint velocity RF HAA + - 5.0 # joint velocity RF HFE + - 5.0 # joint velocity RF KFE + - 5.0 # joint velocity RH HAA + - 5.0 # joint velocity RH HFE + - 5.0 # joint velocity RH KFE +# +# memory +# +# capacity of the memory +CAPACITY: 400000 +# +# policy +# +# observation scaling +OBSERVATION_SCALING: + - 1.0 # swing phase LF + - 1.0 # swing phase LH + - 1.0 # swing phase RF + - 1.0 # swing phase RH + - 1.0 # swing phase rate LF + - 1.0 # swing phase rate LH + - 1.0 # swing phase rate RF + - 1.0 # swing phase rate RH + - 1.0 # sinusoidal bump LF + - 1.0 # sinusoidal bump LH + - 1.0 # sinusoidal bump RF + - 1.0 # sinusoidal bump RH + - 1.0 # normalized linear momentum x + - 1.0 # normalized linear momentum y + - 1.0 # normalized linear momentum z + - 1.0 # normalized angular momentum x + - 1.0 # normalized angular momentum y + - 1.0 # normalized angular momentum z + - 1.0 # position x + - 1.0 # position y + - 1.0 # position z + - 1.0 # orientation z + - 1.0 # orientation y + - 1.0 # orientation x + - 1.0 # joint position LF HAA + - 1.0 # joint position LF HFE + - 1.0 # joint position LF KFE + - 1.0 # joint position LH HAA + - 1.0 # joint position LH HFE + - 1.0 # joint position LH KFE + - 1.0 # joint position RF HAA + - 1.0 # joint position RF HFE + - 1.0 # joint position RF KFE + - 1.0 # joint position RH HAA + - 1.0 # joint position RH HFE + - 1.0 # joint position RH KFE +# action scaling +ACTION_SCALING: + - 100.0 # contact force LF x + - 100.0 # contact force LF y + - 100.0 # contact force LF z + - 100.0 # contact force LH x + - 100.0 # contact force LH y + - 100.0 # contact force LH z + - 100.0 # contact force RF x + - 100.0 # contact force RF y + - 100.0 # contact force RF z + - 100.0 # contact force RH x + - 100.0 # contact force RH y + - 100.0 # contact force RH z + - 10.0 # joint velocity LF HAA + - 10.0 # joint velocity LF HFE + - 10.0 # joint velocity LF KFE + - 10.0 # joint velocity LH HAA + - 10.0 # joint velocity LH HFE + - 10.0 # joint velocity LH KFE + - 10.0 # joint velocity RF HAA + - 10.0 # joint velocity RF HFE + - 10.0 # joint velocity RF KFE + - 10.0 # joint velocity RH HAA + - 10.0 # joint velocity RH HFE + - 10.0 # joint velocity RH KFE +# +# rollout +# +# RaiSim or TimeTriggered rollout for data generation and policy evaluation +RAISIM: True +# weights defining how often a gait is chosen for rollout +WEIGHTS_FOR_GAITS: + stance: 1.0 + trot_1: 2.0 + trot_2: 2.0 +# settings for data generation +DATA_GENERATION_TIME_STEP: 0.0025 +DATA_GENERATION_DURATION: 4.0 +DATA_GENERATION_DATA_DECIMATION: 4 +DATA_GENERATION_THREADS: 12 +DATA_GENERATION_TASKS: 12 +DATA_GENERATION_SAMPLES: 2 +DATA_GENERATION_SAMPLING_VARIANCE: + - 0.05 # normalized linear momentum x + - 0.05 # normalized linear momentum y + - 0.05 # normalized linear momentum z + - 0.00135648942 # normalized angular momentum x: 1.62079 / 52.1348 * 2.5 / 180.0 * pi + - 0.00404705526 # normalized angular momentum y: 4.83559 / 52.1348 * 2.5 / 180.0 * pi + - 0.00395351148 # normalized angular momentum z: 4.72382 / 52.1348 * 2.5 / 180.0 * pi + - 0.01 # position x + - 0.01 # position y + - 0.01 # position z + - 0.00872664625 # orientation z: 0.5 / 180.0 * pi + - 0.00872664625 # orientation y: 0.5 / 180.0 * pi + - 0.00872664625 # orientation x: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH KFE: 0.5 / 180.0 * pi +# settings for computing metrics +POLICY_EVALUATION_TIME_STEP: 0.0025 +POLICY_EVALUATION_DURATION: 4.0 +POLICY_EVALUATION_THREADS: 3 +POLICY_EVALUATION_TASKS: 3 +# +# training +# +BATCH_SIZE: 128 +LEARNING_RATE: 1.e-3 +LEARNING_ITERATIONS: 100000 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py index 95fd1a139..ee566ac01 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py @@ -67,7 +67,7 @@ def __init__(self, config_file_path: str) -> None: with open(config_file_path, "r") as stream: try: config = yaml.safe_load(stream) - for key, value in config["config"].items(): + for key, value in config.items(): setattr(self, key, value) except yaml.YAMLError as exception: print(exception) From ea47f3cacc2dfe21ceb41e497a1325230aba7202 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Jul 2022 11:21:21 +0200 Subject: [PATCH 230/512] throw yaml error --- .../ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py index ee566ac01..1943898aa 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py @@ -65,9 +65,6 @@ def __init__(self, config_file_path: str) -> None: # yaml config # with open(config_file_path, "r") as stream: - try: - config = yaml.safe_load(stream) - for key, value in config.items(): - setattr(self, key, value) - except yaml.YAMLError as exception: - print(exception) + config = yaml.safe_load(stream) + for key, value in config.items(): + setattr(self, key, value) From bf912623ad58168d0930e27606605df557be6e70 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Jul 2022 11:29:21 +0200 Subject: [PATCH 231/512] absolute path to config --- .../ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py | 2 +- .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index d6b5ff1ff..7bc2cf42e 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -249,4 +249,4 @@ def closure(): if len(sys.argv) > 1: main(sys.argv[1]) else: - main("./config/ballbot.yaml") + main(os.path.join(os.path.dirname(os.path.abspath(__file__)), "config/ballbot.yaml")) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index c3b291f3b..6ba5ebec8 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -255,4 +255,4 @@ def closure(): if len(sys.argv) > 1: main(sys.argv[1]) else: - main("./config/legged_robot.yaml") + main(os.path.join(os.path.dirname(os.path.abspath(__file__)), "config/legged_robot.yaml")) From 8e218bc9b2b06bc2b0d3e1872b3f1600b33b9257 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Jul 2022 12:29:41 +0200 Subject: [PATCH 232/512] fix magic numbers --- .../python/ocs2_ballbot_mpcnet/helper.py | 7 ++++++- .../python/ocs2_legged_robot_mpcnet/helper.py | 14 ++++++++------ 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py index 803c67e9a..bc8124cfa 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/helper.py @@ -124,12 +124,17 @@ def get_tasks( - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. """ + initial_mode = 0 + initial_time = 0.0 initial_observations = helper.get_system_observation_array(tasks_number) mode_schedules = helper.get_mode_schedule_array(tasks_number) target_trajectories = helper.get_target_trajectories_array(tasks_number) for i in range(tasks_number): initial_observations[i] = helper.get_system_observation( - 0, 0.0, get_random_initial_state(config.STATE_DIM, config.DEFAULT_STATE), np.zeros(config.INPUT_DIM) + initial_mode, + initial_time, + get_random_initial_state(config.STATE_DIM, config.DEFAULT_STATE), + np.zeros(config.INPUT_DIM), ) mode_schedules[i] = helper.get_mode_schedule(*get_default_event_times_and_mode_sequence(duration)) target_trajectories[i] = helper.get_target_trajectories( diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py index 4029963f1..c88b15ba5 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/helper.py @@ -221,6 +221,8 @@ def get_tasks( - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. """ + initial_mode = 15 + initial_time = 0.0 initial_observations = helper.get_system_observation_array(tasks_number) mode_schedules = helper.get_mode_schedule_array(tasks_number) target_trajectories = helper.get_target_trajectories_array(tasks_number) @@ -230,8 +232,8 @@ def get_tasks( for i in range(tasks_number): if choices[i] == "stance": initial_observations[i] = helper.get_system_observation( - 15, - 0.0, + initial_mode, + initial_time, get_random_initial_state_stance(config.STATE_DIM, config.DEFAULT_STATE), np.zeros(config.INPUT_DIM), ) @@ -245,8 +247,8 @@ def get_tasks( ) elif choices[i] == "trot_1": initial_observations[i] = helper.get_system_observation( - 15, - 0.0, + initial_mode, + initial_time, get_random_initial_state_trot(config.STATE_DIM, config.DEFAULT_STATE), np.zeros(config.INPUT_DIM), ) @@ -260,8 +262,8 @@ def get_tasks( ) elif choices[i] == "trot_2": initial_observations[i] = helper.get_system_observation( - 15, - 0.0, + initial_mode, + initial_time, get_random_initial_state_trot(config.STATE_DIM, config.DEFAULT_STATE), np.zeros(config.INPUT_DIM), ) From c7a474267e92aa81be935efbe2b61ff3dd741846 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Jul 2022 12:44:58 +0200 Subject: [PATCH 233/512] run black --- .../python/ocs2_ballbot_mpcnet/mpcnet.py | 8 ++------ .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 8 ++------ 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index 7bc2cf42e..0740814e5 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -89,9 +89,7 @@ def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - config, - config.DATA_GENERATION_TASKS, - config.DATA_GENERATION_DURATION, + config, config.DATA_GENERATION_TASKS, config.DATA_GENERATION_DURATION ) mpcnet_interface.startDataGeneration( alpha, @@ -109,9 +107,7 @@ def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - config, - config.POLICY_EVALUATION_TASKS, - config.POLICY_EVALUATION_DURATION, + config, config.POLICY_EVALUATION_TASKS, config.POLICY_EVALUATION_DURATION ) mpcnet_interface.startPolicyEvaluation( alpha, diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index 6ba5ebec8..d7b80bc8f 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -91,9 +91,7 @@ def start_data_generation(policy, alpha=1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - config, - config.DATA_GENERATION_TASKS, - config.DATA_GENERATION_DURATION, + config, config.DATA_GENERATION_TASKS, config.DATA_GENERATION_DURATION ) mpcnet_interface.startDataGeneration( alpha, @@ -111,9 +109,7 @@ def start_policy_evaluation(policy, alpha=0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = helper.get_tasks( - config, - config.POLICY_EVALUATION_TASKS, - config.POLICY_EVALUATION_DURATION, + config, config.POLICY_EVALUATION_TASKS, config.POLICY_EVALUATION_DURATION ) mpcnet_interface.startPolicyEvaluation( alpha, From 59ca4ce48d5aca3baa2e9d29ac3e0973a2d28567 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Jul 2022 16:01:07 +0200 Subject: [PATCH 234/512] absolute path to log dir --- .../python/ocs2_ballbot_mpcnet/train.py | 11 ++++++----- .../python/ocs2_legged_robot_mpcnet/train.py | 11 ++++++----- .../python/ocs2_mpcnet_core/mpcnet.py | 5 ++++- 3 files changed, 16 insertions(+), 11 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py index ee532ffef..bfbb8f8c7 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py @@ -46,9 +46,9 @@ from ocs2_ballbot_mpcnet import MpcnetInterface -def main(config_file_path: str) -> None: +def main(root_dir: str, config_file_name: str) -> None: # config - config = Config(config_file_path) + config = Config(os.path.join(root_dir, "config", config_file_name)) # interface interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) # loss @@ -58,13 +58,14 @@ def main(config_file_path: str) -> None: # policy policy = LinearPolicy(config) # mpcnet - mpcnet = BallbotMpcnet(config, interface, memory, policy, loss) + mpcnet = BallbotMpcnet(root_dir, config, interface, memory, policy, loss) # train mpcnet.train() if __name__ == "__main__": + root_dir = os.path.dirname(os.path.abspath(__file__)) if len(sys.argv) > 1: - main(sys.argv[1]) + main(root_dir, sys.argv[1]) else: - main(os.path.join(os.path.dirname(os.path.abspath(__file__)), "config/ballbot.yaml")) + main(root_dir, "ballbot.yaml") diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py index de07dfa7c..8f165628e 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py @@ -47,9 +47,9 @@ from ocs2_legged_robot_mpcnet import MpcnetInterface -def main(config_file_path: str) -> None: +def main(root_dir: str, config_file_name: str) -> None: # config - config = Config(config_file_path) + config = Config(os.path.join(root_dir, "config", config_file_name)) # interface interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) # loss @@ -60,13 +60,14 @@ def main(config_file_path: str) -> None: # policy policy = MixtureOfNonlinearExpertsPolicy(config) # mpcnet - mpcnet = LeggedRobotMpcnet(config, interface, memory, policy, experts_loss, gating_loss) + mpcnet = LeggedRobotMpcnet(root_dir, config, interface, memory, policy, experts_loss, gating_loss) # train mpcnet.train() if __name__ == "__main__": + root_dir = os.path.dirname(os.path.abspath(__file__)) if len(sys.argv) > 1: - main(sys.argv[1]) + main(root_dir, sys.argv[1]) else: - main(os.path.join(os.path.dirname(os.path.abspath(__file__)), "config/legged_robot.yaml")) + main(root_dir, "legged_robot.yaml") diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index 3ba9818a7..5dd21b112 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -56,6 +56,7 @@ class Mpcnet: def __init__( self, + root_dir: str, config: Config, interface: object, memory: BaseMemory, @@ -68,6 +69,7 @@ def __init__( Initializes the Mpcnet class by setting fixed and variable attributes. Args: + root_dir: The absolute path to the root directory. config: An instance of the configuration class. interface: An instance of the interface class. memory: An instance of a memory class. @@ -81,7 +83,8 @@ def __init__( self.interface = interface # logging self.log_dir = ( - "./runs/" + root_dir + + "/runs/" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_" + config.NAME From 3ab9e29f0d7b68cf7f62aeea754a4c972c0391fd Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Jul 2022 16:17:55 +0200 Subject: [PATCH 235/512] clean up imports --- .../python/ocs2_ballbot_mpcnet/__init__.py | 1 + .../python/ocs2_ballbot_mpcnet/train.py | 8 ++++---- .../python/ocs2_legged_robot_mpcnet/__init__.py | 1 + .../python/ocs2_legged_robot_mpcnet/train.py | 10 +++++----- .../python/ocs2_mpcnet_core/loss/__init__.py | 6 ++++++ .../python/ocs2_mpcnet_core/memory/__init__.py | 4 ++++ .../ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py | 6 +++--- .../python/ocs2_mpcnet_core/policy/__init__.py | 7 +++++++ 8 files changed, 31 insertions(+), 12 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py index 2973846dd..d5c07a503 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py @@ -1 +1,2 @@ from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import MpcnetInterface +from ocs2_ballbot_mpcnet.mpcnet import BallbotMpcnet diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py index bfbb8f8c7..4fa5c59ce 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py @@ -38,11 +38,11 @@ import os from ocs2_mpcnet_core.config import Config -from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss -from ocs2_mpcnet_core.memory.circular import CircularMemory -from ocs2_mpcnet_core.policy.linear import LinearPolicy +from ocs2_mpcnet_core.loss import HamiltonianLoss +from ocs2_mpcnet_core.memory import CircularMemory +from ocs2_mpcnet_core.policy import LinearPolicy -from ocs2_ballbot_mpcnet.mpcnet import BallbotMpcnet +from ocs2_ballbot_mpcnet import BallbotMpcnet from ocs2_ballbot_mpcnet import MpcnetInterface diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py index 4d878b2d3..688527a67 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py @@ -1 +1,2 @@ from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import MpcnetInterface +from ocs2_legged_robot_mpcnet.mpcnet import LeggedRobotMpcnet diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py index 8f165628e..f00533bba 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py @@ -38,12 +38,12 @@ import os from ocs2_mpcnet_core.config import Config -from ocs2_mpcnet_core.loss.hamiltonian import HamiltonianLoss -from ocs2_mpcnet_core.loss.cross_entropy import CrossEntropyLoss -from ocs2_mpcnet_core.memory.circular import CircularMemory -from ocs2_mpcnet_core.policy.mixture_of_nonlinear_experts import MixtureOfNonlinearExpertsPolicy +from ocs2_mpcnet_core.loss import HamiltonianLoss +from ocs2_mpcnet_core.loss import CrossEntropyLoss +from ocs2_mpcnet_core.memory import CircularMemory +from ocs2_mpcnet_core.policy import MixtureOfNonlinearExpertsPolicy -from ocs2_legged_robot_mpcnet.mpcnet import LeggedRobotMpcnet +from ocs2_legged_robot_mpcnet import LeggedRobotMpcnet from ocs2_legged_robot_mpcnet import MpcnetInterface diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/__init__.py index e69de29bb..c343a60cb 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/__init__.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/__init__.py @@ -0,0 +1,6 @@ +from .base import BaseLoss +from .behavioral_cloning import BehavioralCloningLoss +from .cross_entropy import CrossEntropyLoss +from .hamiltonian import HamiltonianLoss + +__all__ = ["BaseLoss", "BehavioralCloningLoss", "CrossEntropyLoss", "HamiltonianLoss"] diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/__init__.py index e69de29bb..7d777af72 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/__init__.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/__init__.py @@ -0,0 +1,4 @@ +from .base import BaseMemory +from .circular import CircularMemory + +__all__ = ["BaseMemory", "CircularMemory"] diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index 5dd21b112..34772be22 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -43,9 +43,9 @@ from ocs2_mpcnet_core import helper from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray from ocs2_mpcnet_core.config import Config -from ocs2_mpcnet_core.loss.base import BaseLoss -from ocs2_mpcnet_core.memory.base import BaseMemory -from ocs2_mpcnet_core.policy.base import BasePolicy +from ocs2_mpcnet_core.loss import BaseLoss +from ocs2_mpcnet_core.memory import BaseMemory +from ocs2_mpcnet_core.policy import BasePolicy class Mpcnet: diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py index e69de29bb..46ff16a7f 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py @@ -0,0 +1,7 @@ +from .base import BasePolicy +from .linear import LinearPolicy +from .mixture_of_linear_experts import MixtureOfLinearExpertsPolicy +from .mixture_of_nonlinear_experts import MixtureOfNonlinearExpertsPolicy +from .nonlinear import NonlinearPolicy + +__all__ = ["BasePolicy", "LinearPolicy", "MixtureOfLinearExpertsPolicy", "MixtureOfNonlinearExpertsPolicy", "NonlinearPolicy"] From ecd51b0e20a15be496d0dfc58650c79a9c5273ab Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Jul 2022 16:34:35 +0200 Subject: [PATCH 236/512] abstract forward method --- .../python/ocs2_mpcnet_core/policy/base.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py index a3faccf16..012712b65 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py @@ -33,6 +33,7 @@ """ import torch +from typing import Tuple from ocs2_mpcnet_core.config import Config from ocs2_mpcnet_core.helper import bmv @@ -64,6 +65,19 @@ def __init__(self, config: Config) -> None: torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, ...]: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + observation: A (B,O) tensor with the observations. + + Returns: + tuple: A tuple with the predictions, e.g. containing a (B,A) tensor with the predicted actions. + """ + raise NotImplementedError() + def scale_observation(self, observation: torch.Tensor) -> torch.Tensor: """Scale observation. From ab0108d9f9988fe6591a49807f3625cb0b716d87 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Wed, 20 Jul 2022 17:05:38 +0200 Subject: [PATCH 237/512] update doc for main mpcnet class --- .../ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index 34772be22..ee517c7da 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -52,6 +52,10 @@ class Mpcnet: """MPC-Net. Implements the main methods for the MPC-Net training. + + Takes a specific configuration, interface, memory, policy and loss function(s). + The task formulation has to be implemented in a robot-specific class derived from this class. + Provides the main training loop for MPC-Net. """ def __init__( From 00c390b0d252730fe50b21e75c9ff7d7fddc6a11 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 21 Jul 2022 10:19:16 +0200 Subject: [PATCH 238/512] fix typo in docstring --- ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py index 120672ab7..345e7d96f 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py @@ -60,7 +60,7 @@ def bdot(bv1: torch.Tensor, bv2: torch.Tensor) -> torch.Tensor: bv2: A (B,N) tensor containing a batch of vectors. Returns: - A (B,N) tensor containing the batch-wise dot product. + A (B) tensor containing the batch-wise dot product. """ return torch.sum(torch.mul(bv1, bv2), dim=1) From c41d881cd52f229abda20028825f67c5e342616f Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 28 Jul 2022 10:16:12 +0200 Subject: [PATCH 239/512] doc fix --- ocs2_core/include/ocs2_core/Types.h | 6 ++-- ocs2_core/test/testTypes.cpp | 48 ++++++++++++++--------------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/ocs2_core/include/ocs2_core/Types.h b/ocs2_core/include/ocs2_core/Types.h index d09cc26c0..699a9ecbc 100644 --- a/ocs2_core/include/ocs2_core/Types.h +++ b/ocs2_core/include/ocs2_core/Types.h @@ -81,7 +81,7 @@ using eigen_scalar_array2_t = std::vector<eigen_scalar_array_t>; using eigen_scalar_array3_t = std::vector<eigen_scalar_array2_t>; /** - * Defines the linear approximation + * Defines the linear approximation of a scalar function * f(x,u) = dfdx' dx + dfdu' du + f */ struct ScalarFunctionLinearApproximation { @@ -134,7 +134,7 @@ std::ostream& operator<<(std::ostream& out, const ScalarFunctionLinearApproximat * * @param[in] stateDim: Number of states. * @param[in] inputDim: Number of inputs. - * @param[in] data: Given quadratic approximation. + * @param[in] data: Given linear approximation. * @param[in] dataName: The name of the data which appears in the output error message. * @return The description of the error. If there was no error it would be empty; */ @@ -148,7 +148,7 @@ inline ScalarFunctionLinearApproximation operator*(scalar_t scalar, ScalarFuncti } /** - * Defines the quadratic approximation + * Defines the quadratic approximation of a scalar function * f(x,u) = 1/2 dx' dfdxx dx + du' dfdux dx + 1/2 du' dfduu du + dfdx' dx + dfdu' du + f */ struct ScalarFunctionQuadraticApproximation { diff --git a/ocs2_core/test/testTypes.cpp b/ocs2_core/test/testTypes.cpp index 947ca7c3f..b926efee5 100644 --- a/ocs2_core/test/testTypes.cpp +++ b/ocs2_core/test/testTypes.cpp @@ -2,30 +2,30 @@ Copyright (c) 2020, Farbod Farshidian. All rights reserved. Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - ******************************************************************************/ +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ #include <gtest/gtest.h> From a789013f4ffe92b654e56989297ba2dd489231db Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 28 Jul 2022 10:17:49 +0200 Subject: [PATCH 240/512] just operating over state-only terms for StateAugmentedLagrangianCollection --- .../StateAugmentedLagrangianCollection.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ocs2_core/src/augmented_lagrangian/StateAugmentedLagrangianCollection.cpp b/ocs2_core/src/augmented_lagrangian/StateAugmentedLagrangianCollection.cpp index 4189cffcf..b002109f9 100644 --- a/ocs2_core/src/augmented_lagrangian/StateAugmentedLagrangianCollection.cpp +++ b/ocs2_core/src/augmented_lagrangian/StateAugmentedLagrangianCollection.cpp @@ -90,10 +90,18 @@ ScalarFunctionQuadraticApproximation StateAugmentedLagrangianCollection::getQuad // accumulate terms for (size_t i = firstActiveInd + 1; i < terms_.size(); i++) { if (terms_[i]->isActive(time)) { - penalty += terms_[i]->getQuadraticApproximation(time, state, termsMultiplier[i], preComp); + const auto termPenalty = terms_[i]->getQuadraticApproximation(time, state, termsMultiplier[i], preComp); + penalty.f += termPenalty.f; + penalty.dfdx += termPenalty.dfdx; + penalty.dfdxx += termPenalty.dfdxx; } } + // make sure that input derivatives are empty + penalty.dfdu = vector_t(); + penalty.dfduu = matrix_t(); + penalty.dfdux = matrix_t(); + return penalty; } From ce853522d48b86b7bed148914e904510894b7be7 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 28 Jul 2022 19:30:21 +0200 Subject: [PATCH 241/512] core is updated --- .../constraint/StateConstraintCollection.h | 6 +- .../StateInputConstraintCollection.h | 6 +- .../constraint/LoopshapingStateConstraint.h | 2 +- .../LoopshapingStateInputConstraint.h | 2 +- .../include/ocs2_core/model_data/Metrics.h | 41 ++++++++- .../constraint/StateConstraintCollection.cpp | 19 ++-- .../StateInputConstraintCollection.cpp | 21 ++--- .../constraint/LoopshapingStateConstraint.cpp | 4 +- .../LoopshapingStateInputConstraint.cpp | 5 +- ocs2_core/src/model_data/Metrics.cpp | 89 +++++++++++++++---- .../constraint/testConstraintCollection.cpp | 11 ++- .../loopshaping/testLoopshapingConstraint.h | 26 +++--- ocs2_core/test/model_data/testMetrics.cpp | 67 ++++++++------ 13 files changed, 192 insertions(+), 107 deletions(-) diff --git a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h index 3bb8a0d53..a070aa46c 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h @@ -52,8 +52,8 @@ class StateConstraintCollection : public Collection<StateConstraint> { /** Returns the number of active constraints at given time. */ virtual size_t getNumConstraints(scalar_t time) const; - /** Get the constraint vector value */ - virtual vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const; + /** Get an array of all constraints */ + virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const; /** Get the constraint linear approximation */ virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, @@ -68,4 +68,4 @@ class StateConstraintCollection : public Collection<StateConstraint> { StateConstraintCollection(const StateConstraintCollection& other); }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h index f102c593b..0eaafb700 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h @@ -52,8 +52,8 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> { /** Returns the number of active constraints at given time. */ virtual size_t getNumConstraints(scalar_t time) const; - /** Get the constraint vector value */ - virtual vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const; + /** Get an array of all constraints */ + virtual vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const; /** Get the constraint linear approximation */ virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, @@ -68,4 +68,4 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> { StateInputConstraintCollection(const StateInputConstraintCollection& other); }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h index 0c94db368..150e29ff6 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h @@ -47,7 +47,7 @@ class LoopshapingStateConstraint final : public StateConstraintCollection { LoopshapingStateConstraint* clone() const override { return new LoopshapingStateConstraint(*this); } - vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; + vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h index 7314c1182..14830ed3c 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h @@ -41,7 +41,7 @@ class LoopshapingStateInputConstraint : public StateInputConstraintCollection { public: ~LoopshapingStateInputConstraint() override = default; - vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; + vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; protected: LoopshapingStateInputConstraint(const StateInputConstraintCollection& systemConstraint, diff --git a/ocs2_core/include/ocs2_core/model_data/Metrics.h b/ocs2_core/include/ocs2_core/model_data/Metrics.h index 2d205d377..ef6973dcc 100644 --- a/ocs2_core/include/ocs2_core/model_data/Metrics.h +++ b/ocs2_core/include/ocs2_core/model_data/Metrics.h @@ -68,8 +68,8 @@ struct MetricsCollection { scalar_t cost; // Equality constraints - vector_t stateEqConstraint; - vector_t stateInputEqConstraint; + vector_array_t stateEqConstraint; + vector_array_t stateInputEqConstraint; // Lagrangians std::vector<LagrangianMetrics> stateEqLagrangian; @@ -96,8 +96,8 @@ struct MetricsCollection { // Cost cost = 0.0; // Equality constraints - stateEqConstraint = vector_t(); - stateInputEqConstraint = vector_t(); + stateEqConstraint.clear(); + stateInputEqConstraint.clear(); // Lagrangians stateEqLagrangian.clear(); stateIneqLagrangian.clear(); @@ -120,6 +120,13 @@ inline scalar_t constraintsSquaredNorm(const std::vector<LagrangianMetrics>& met return s; } +/** Computes the sum of squared norm of an array of constraint terms */ +inline scalar_t constraintsSquaredNorm(const vector_array_t& constraintArray) { + scalar_t s = 0.0; + std::for_each(constraintArray.begin(), constraintArray.end(), [&s](const vector_t& v) { s += v.squaredNorm(); }); + return s; +} + /** * Serializes an array of LagrangianMetrics structures associated to an array of constraint terms. * @@ -128,6 +135,14 @@ inline scalar_t constraintsSquaredNorm(const std::vector<LagrangianMetrics>& met */ vector_t toVector(const std::vector<LagrangianMetrics>& termsLagrangianMetrics); +/** + * Serializes an array of constraint terms. + * + * @ param [in] constraintArray : An array of constraint terms. + * @return Serialized vector of the format : (..., constraintArray[i], ...). + */ +vector_t toVector(const vector_array_t& constraintArray); + /** * Gets the size of constraint terms. * @@ -136,6 +151,14 @@ vector_t toVector(const std::vector<LagrangianMetrics>& termsLagrangianMetrics); */ size_array_t getSizes(const std::vector<LagrangianMetrics>& termsLagrangianMetrics); +/** + * Gets the size of constraint terms. + * + * @ param [in] constraintArray : An array of constraint terms. + * @return An array of constraint terms size. It has the same size as the input array. + */ +size_array_t getSizes(const vector_array_t& constraintArray); + /** * Deserializes the vector to an array of LagrangianMetrics structures based on size of constraint terms. * @@ -146,6 +169,16 @@ size_array_t getSizes(const std::vector<LagrangianMetrics>& termsLagrangianMetri */ std::vector<LagrangianMetrics> toLagrangianMetrics(const size_array_t& termsSize, const vector_t& vec); +/** + * Deserializes the vector to an array of constraint terms. + * + * @param [in] termsSize : An array of constraint terms size. It as the same size as the output array. + * @param [in] vec : Serialized array of constraint terms of the format : + * (..., constraintArray[i], ...) + * @return An array of LagrangianMetrics structures associated to an array of constraint terms + */ +vector_array_t toConstraintArray(const size_array_t& termsSize, const vector_t& vec); + } // namespace ocs2 namespace ocs2 { diff --git a/ocs2_core/src/constraint/StateConstraintCollection.cpp b/ocs2_core/src/constraint/StateConstraintCollection.cpp index 4a59e9168..ac3231f07 100644 --- a/ocs2_core/src/constraint/StateConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateConstraintCollection.cpp @@ -62,20 +62,13 @@ size_t StateConstraintCollection::getNumConstraints(scalar_t time) const { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -vector_t StateConstraintCollection::getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const { - vector_t constraintValues; - constraintValues.resize(getNumConstraints(time)); - - // append vectors of constraint values from each constraintTerm - size_t i = 0; - for (const auto& constraintTerm : this->terms_) { - if (constraintTerm->isActive(time)) { - const auto constraintTermValues = constraintTerm->getValue(time, state, preComp); - constraintValues.segment(i, constraintTermValues.rows()) = constraintTermValues; - i += constraintTermValues.rows(); +vector_array_t StateConstraintCollection::getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const { + vector_array_t constraintValues(this->terms_.size()); + for (size_t i = 0; i < this->terms_.size(); ++i) { + if (this->terms_[i]->isActive(time)) { + constraintValues[i] = this->terms_[i]->getValue(time, state, preComp); } - } - + } // end of i loop return constraintValues; } diff --git a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp index 44264a06b..a6e1c8192 100644 --- a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp @@ -63,21 +63,14 @@ size_t StateInputConstraintCollection::getNumConstraints(scalar_t time) const { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -vector_t StateInputConstraintCollection::getValue(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const { - vector_t constraintValues; - constraintValues.resize(getNumConstraints(time)); - - // append vectors of constraint values from each constraintTerm - size_t i = 0; - for (const auto& constraintTerm : this->terms_) { - if (constraintTerm->isActive(time)) { - const auto constraintTermValues = constraintTerm->getValue(time, state, input, preComp); - constraintValues.segment(i, constraintTermValues.rows()) = constraintTermValues; - i += constraintTermValues.rows(); +vector_array_t StateInputConstraintCollection::getValue(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const { + vector_array_t constraintValues(this->terms_.size()); + for (size_t i = 0; i < this->terms_.size(); ++i) { + if (this->terms_[i]->isActive(time)) { + constraintValues[i] = this->terms_[i]->getValue(time, state, input, preComp); } - } - + } // end of i loop return constraintValues; } diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp index c382518b3..6c5f6685e 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp @@ -38,9 +38,9 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -vector_t LoopshapingStateConstraint::getValue(scalar_t t, const vector_t& x, const PreComputation& preComp) const { +vector_array_t LoopshapingStateConstraint::getValue(scalar_t t, const vector_t& x, const PreComputation& preComp) const { if (this->empty()) { - return vector_t::Zero(0); + return vector_array_t(); } const LoopshapingPreComputation& preCompLS = cast<LoopshapingPreComputation>(preComp); diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp index 14125e7f2..9ef11c9b8 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp @@ -38,9 +38,10 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -vector_t LoopshapingStateInputConstraint::getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation& preComp) const { +vector_array_t LoopshapingStateInputConstraint::getValue(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const { if (this->empty()) { - return vector_t::Zero(0); + return vector_array_t(); } const LoopshapingPreComputation& preCompLS = cast<LoopshapingPreComputation>(preComp); diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 8d1066934..49a756243 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -50,6 +50,23 @@ vector_t toVector(const std::vector<LagrangianMetrics>& termsLagrangianMetrics) return vec; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t toVector(const vector_array_t& constraintArray) { + size_t n = 0; + std::for_each(constraintArray.begin(), constraintArray.end(), [&n](const vector_t& v) { n += v.size(); }); + + vector_t vec(n); + size_t head = 0; + for (const auto& v : constraintArray) { + vec.segment(head, v.size()) = v; + head += v.size(); + } // end of i loop + + return vec; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -66,6 +83,22 @@ std::vector<LagrangianMetrics> toLagrangianMetrics(const size_array_t& termsSize return lagrangianMetrics; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_array_t toConstraintArray(const size_array_t& termsSize, const vector_t& vec) { + vector_array_t constraintArray; + constraintArray.reserve(termsSize.size()); + + size_t head = 0; + for (const auto& l : termsSize) { + constraintArray.emplace_back(vec.segment(head, l)); + head += l; + } // end of i loop + + return constraintArray; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -76,6 +109,16 @@ size_array_t getSizes(const std::vector<LagrangianMetrics>& termsLagrangianMetri return s; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +size_array_t getSizes(const vector_array_t& constraintArray) { + size_array_t s(constraintArray.size()); + std::transform(constraintArray.begin(), constraintArray.end(), s.begin(), + [](const vector_t& v) { return static_cast<size_t>(v.size()); }); + return s; +} + } // namespace ocs2 namespace ocs2 { @@ -102,10 +145,12 @@ LagrangianMetrics interpolate(const index_alpha_t& indexAlpha, const std::vector MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector<MetricsCollection>& dataArray) { // number of terms const auto ind = indexAlpha.second > 0.5 ? indexAlpha.first : indexAlpha.first + 1; - const size_t mumStateEq = dataArray[ind].stateEqLagrangian.size(); - const size_t mumStateIneq = dataArray[ind].stateIneqLagrangian.size(); - const size_t mumStateInputEq = dataArray[ind].stateInputEqLagrangian.size(); - const size_t mumStateInputIneq = dataArray[ind].stateInputIneqLagrangian.size(); + const size_t mumStateEqConst = dataArray[ind].stateEqConstraint.size(); + const size_t mumStateInputEqCost = dataArray[ind].stateInputEqConstraint.size(); + const size_t mumStateEqLag = dataArray[ind].stateEqLagrangian.size(); + const size_t mumStateIneqLag = dataArray[ind].stateIneqLagrangian.size(); + const size_t mumStateInputEqLag = dataArray[ind].stateInputEqLagrangian.size(); + const size_t mumStateInputIneqLag = dataArray[ind].stateInputIneqLagrangian.size(); MetricsCollection out; @@ -114,16 +159,24 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector [](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { return array[t].cost; }); // constraints - out.stateEqConstraint = interpolate(indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { - return array[t].stateEqConstraint; - }); - out.stateInputEqConstraint = - interpolate(indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); + out.stateEqConstraint.reserve(mumStateEqConst); + for (size_t i = 0; i < mumStateEqConst; i++) { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { + return array[t].stateEqConstraint[i]; + }); + out.stateEqConstraint.emplace_back(std::move(constraint)); + } + out.stateInputEqConstraint.reserve(mumStateInputEqCost); + for (size_t i = 0; i < mumStateInputEqCost; i++) { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { + return array[t].stateInputEqConstraint[i]; + }); + out.stateInputEqConstraint.emplace_back(std::move(constraint)); + } // state equality Lagrangian - out.stateEqLagrangian.reserve(mumStateEq); - for (size_t i = 0; i < mumStateEq; i++) { + out.stateEqLagrangian.reserve(mumStateEqLag); + for (size_t i = 0; i < mumStateEqLag; i++) { auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { return array[t].stateEqLagrangian[i].penalty; }); @@ -134,8 +187,8 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector } // end of i loop // state inequality Lagrangian - out.stateIneqLagrangian.reserve(mumStateIneq); - for (size_t i = 0; i < mumStateIneq; i++) { + out.stateIneqLagrangian.reserve(mumStateIneqLag); + for (size_t i = 0; i < mumStateIneqLag; i++) { auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { return array[t].stateIneqLagrangian[i].penalty; }); @@ -146,8 +199,8 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector } // end of i loop // state-input equality Lagrangian - out.stateInputEqLagrangian.reserve(mumStateInputEq); - for (size_t i = 0; i < mumStateInputEq; i++) { + out.stateInputEqLagrangian.reserve(mumStateInputEqLag); + for (size_t i = 0; i < mumStateInputEqLag; i++) { auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { return array[t].stateInputEqLagrangian[i].penalty; }); @@ -158,8 +211,8 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector } // end of i loop // state-input inequality Lagrangian - out.stateInputIneqLagrangian.reserve(mumStateInputIneq); - for (size_t i = 0; i < mumStateInputIneq; i++) { + out.stateInputIneqLagrangian.reserve(mumStateInputIneqLag); + for (size_t i = 0; i < mumStateInputIneqLag; i++) { auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { return array[t].stateInputIneqLagrangian[i].penalty; }); diff --git a/ocs2_core/test/constraint/testConstraintCollection.cpp b/ocs2_core/test/constraint/testConstraintCollection.cpp index 418275b98..d9c7a8012 100644 --- a/ocs2_core/test/constraint/testConstraintCollection.cpp +++ b/ocs2_core/test/constraint/testConstraintCollection.cpp @@ -107,13 +107,12 @@ TEST(TestConstraintCollection, getValue) { std::unique_ptr<TestDummyConstraint> constraintTerm2(new TestDummyConstraint()); constraintCollection.add("Constraint1", std::move(constraintTerm1)); constraintCollection.add("Constraint2", std::move(constraintTerm2)); - + const ocs2::vector_t expectedValue = (ocs2::vector_t(2) << 1.0, 2.0).finished(); constraintValues = constraintCollection.getValue(t, x, u, ocs2::PreComputation()); - ASSERT_EQ(constraintValues.rows(), 4); - EXPECT_EQ(constraintValues[0], 1.0); - EXPECT_EQ(constraintValues[1], 2.0); - EXPECT_EQ(constraintValues[2], 1.0); - EXPECT_EQ(constraintValues[3], 2.0); + + ASSERT_EQ(constraintValues.size(), 2); + EXPECT_TRUE(constraintValues[0].isApprox(expectedValue)); + EXPECT_TRUE(constraintValues[1].isApprox(expectedValue)); } TEST(TestConstraintCollection, getLinearApproximation) { diff --git a/ocs2_core/test/loopshaping/testLoopshapingConstraint.h b/ocs2_core/test/loopshaping/testLoopshapingConstraint.h index babfd868c..6750ab652 100644 --- a/ocs2_core/test/loopshaping/testLoopshapingConstraint.h +++ b/ocs2_core/test/loopshaping/testLoopshapingConstraint.h @@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_core/loopshaping/LoopshapingPreComputation.h" #include "ocs2_core/loopshaping/LoopshapingPropertyTree.h" #include "ocs2_core/loopshaping/constraint/LoopshapingConstraint.h" +#include "ocs2_core/model_data/Metrics.h" #include "testLoopshapingConfigurations.h" #include "testQuadraticConstraint.h" @@ -62,14 +63,15 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { void testStateInputConstraintEvaluation() const { // Evaluate system - vector_t g_system = systemConstraint->getValue(t, x_sys_, u_sys_, PreComputation()); + const auto g_system = systemConstraint->getValue(t, x_sys_, u_sys_, PreComputation()); // Evaluate loopshaping system preComp_->request(Request::Constraint, t, x_, u_); - vector_t g = loopshapingConstraint->getValue(t, x_, u_, *preComp_); + const auto g = toVector(loopshapingConstraint->getValue(t, x_, u_, *preComp_)); // The constraint should stay the same - EXPECT_LE((g_system - g).array().abs().maxCoeff(), tol); + EXPECT_TRUE(g_system.size() == g.size()); + EXPECT_TRUE(g_system.isApprox(g, tol)); } void testStateInputConstraintLinearApproximation() const { @@ -90,7 +92,7 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { // Reevaluate at disturbed state preComp_->request(Request::Constraint, t, x_ + x_disturbance_, u_ + u_disturbance_); - vector_t h_disturbance = loopshapingConstraint->getValue(t, x_ + x_disturbance_, u_ + u_disturbance_, *preComp_); + const auto h_disturbance = toVector(loopshapingConstraint->getValue(t, x_ + x_disturbance_, u_ + u_disturbance_, *preComp_)); // Evaluate approximation for (size_t i = 0; i < h.f.rows(); i++) { @@ -98,20 +100,20 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { 0.5 * x_disturbance_.transpose() * h.dfdxx[i] * x_disturbance_ + 0.5 * u_disturbance_.transpose() * h.dfduu[i] * u_disturbance_ + u_disturbance_.transpose() * h.dfdux[i] * x_disturbance_; - EXPECT_LE(std::abs(h_disturbance[i] - h_approximation), tol); + EXPECT_NEAR(h_disturbance[i], h_approximation, tol); } } void testStateOnlyConstraintEvaluation() const { // Evaluate system - vector_t g_system = systemStateConstraint->getValue(t, x_sys_, PreComputation()); + const auto g_system = systemStateConstraint->getValue(t, x_sys_, PreComputation()); // Evaluate loopshaping system preComp_->requestFinal(Request::Constraint, t, x_); - vector_t g = loopshapingStateConstraint->getValue(t, x_, *preComp_); + const auto g = toVector(loopshapingStateConstraint->getValue(t, x_, *preComp_)); // System part of the constraints should stay the same - EXPECT_LE((g_system - g).array().abs().maxCoeff(), tol); + EXPECT_TRUE(g_system.isApprox(g, tol)); } void testStateOnlyConstraintLinearApproximation() const { @@ -131,12 +133,12 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { // Reevaluate at disturbed state preComp_->requestFinal(Request::Constraint, t, x_ + x_disturbance_); - vector_t h_disturbance = loopshapingStateConstraint->getValue(t, x_ + x_disturbance_, *preComp_); + const auto h_disturbance = toVector(loopshapingStateConstraint->getValue(t, x_ + x_disturbance_, *preComp_)); // Evaluate approximation for (size_t i = 0; i < h.f.rows(); i++) { - scalar_t h_approximation = h.f(i) + h.dfdx.row(i) * x_disturbance_ + 0.5 * x_disturbance_.transpose() * h.dfdxx[i] * x_disturbance_; - EXPECT_LE(std::abs(h_disturbance[i] - h_approximation), tol); + const auto h_approximation = h.f(i) + h.dfdx.row(i) * x_disturbance_ + 0.5 * x_disturbance_.transpose() * h.dfdxx[i] * x_disturbance_; + EXPECT_NEAR(h_disturbance[i], h_approximation, tol); } } @@ -147,4 +149,4 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { std::unique_ptr<StateConstraintCollection> loopshapingStateConstraint; }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/test/model_data/testMetrics.cpp b/ocs2_core/test/model_data/testMetrics.cpp index 40866541c..48a60ca86 100644 --- a/ocs2_core/test/model_data/testMetrics.cpp +++ b/ocs2_core/test/model_data/testMetrics.cpp @@ -54,21 +54,29 @@ inline MetricsCollection interpolateNew(const LinearInterpolation::index_alpha_t const scalar_t alpha = indexAlpha.second; bool areSameSize = true; - const auto lhs_stateEq = toVector(dataArray[index].stateEqLagrangian); - const auto rhs_stateEq = toVector(dataArray[index + 1].stateEqLagrangian); - areSameSize = areSameSize && (lhs_stateEq.size() == rhs_stateEq.size()); + const auto lhs_stateEqConst = toVector(dataArray[index].stateEqConstraint); + const auto rhs_stateEqConst = toVector(dataArray[index + 1].stateEqConstraint); + areSameSize = areSameSize && (lhs_stateEqConst.size() == rhs_stateEqConst.size()); - const auto lhs_stateIneq = toVector(dataArray[index].stateIneqLagrangian); - const auto rhs_stateIneq = toVector(dataArray[index + 1].stateIneqLagrangian); - areSameSize = areSameSize && (lhs_stateIneq.size() == rhs_stateIneq.size()); + const auto lhs_stateInputEqConst = toVector(dataArray[index].stateInputEqConstraint); + const auto rhs_stateInputEqConst = toVector(dataArray[index + 1].stateInputEqConstraint); + areSameSize = areSameSize && (lhs_stateInputEqConst.size() == rhs_stateInputEqConst.size()); - const auto lhs_stateInputEq = toVector(dataArray[index].stateInputEqLagrangian); - const auto rhs_stateInputEq = toVector(dataArray[index + 1].stateInputEqLagrangian); - areSameSize = areSameSize && (lhs_stateInputEq.size() == rhs_stateInputEq.size()); + const auto lhs_stateEqLag = toVector(dataArray[index].stateEqLagrangian); + const auto rhs_stateEqLag = toVector(dataArray[index + 1].stateEqLagrangian); + areSameSize = areSameSize && (lhs_stateEqLag.size() == rhs_stateEqLag.size()); - const auto lhs_stateInputIneq = toVector(dataArray[index].stateInputIneqLagrangian); - const auto rhs_stateInputIneq = toVector(dataArray[index + 1].stateInputIneqLagrangian); - areSameSize = areSameSize && (lhs_stateInputIneq.size() == rhs_stateInputIneq.size()); + const auto lhs_stateIneqLag = toVector(dataArray[index].stateIneqLagrangian); + const auto rhs_stateIneqLag = toVector(dataArray[index + 1].stateIneqLagrangian); + areSameSize = areSameSize && (lhs_stateIneqLag.size() == rhs_stateIneqLag.size()); + + const auto lhs_stateInputEqLag = toVector(dataArray[index].stateInputEqLagrangian); + const auto rhs_stateInputEqLag = toVector(dataArray[index + 1].stateInputEqLagrangian); + areSameSize = areSameSize && (lhs_stateInputEqLag.size() == rhs_stateInputEqLag.size()); + + const auto lhs_stateInputIneqLag = toVector(dataArray[index].stateInputIneqLagrangian); + const auto rhs_stateInputIneqLag = toVector(dataArray[index + 1].stateInputIneqLagrangian); + areSameSize = areSameSize && (lhs_stateInputIneqLag.size() == rhs_stateInputIneqLag.size()); if (areSameSize) { const auto f = [alpha](const vector_t& lhs, const vector_t& rhs) -> vector_t { return alpha * lhs + (scalar_t(1.0) - alpha) * rhs; }; @@ -77,18 +85,15 @@ inline MetricsCollection interpolateNew(const LinearInterpolation::index_alpha_t out.cost = LinearInterpolation::interpolate( indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { return array[t].cost; }); // constraints - out.stateEqConstraint = LinearInterpolation::interpolate( - indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateEqConstraint; }); - out.stateInputEqConstraint = LinearInterpolation::interpolate( - indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); - out.stateEqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateEqLagrangian), f(lhs_stateEq, rhs_stateEq)); - out.stateIneqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateIneqLagrangian), f(lhs_stateIneq, rhs_stateIneq)); + out.stateEqConstraint = toConstraintArray(getSizes(dataArray[index].stateEqConstraint), f(lhs_stateEqConst, rhs_stateEqConst)); + out.stateInputEqConstraint = toConstraintArray(getSizes(dataArray[index].stateInputEqConstraint), f(lhs_stateInputEqConst, rhs_stateInputEqConst)); + // lagrangian + out.stateEqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateEqLagrangian), f(lhs_stateEqLag, rhs_stateEqLag)); + out.stateIneqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateIneqLagrangian), f(lhs_stateIneqLag, rhs_stateIneqLag)); out.stateInputEqLagrangian = - toLagrangianMetrics(getSizes(dataArray[index].stateInputEqLagrangian), f(lhs_stateInputEq, rhs_stateInputEq)); + toLagrangianMetrics(getSizes(dataArray[index].stateInputEqLagrangian), f(lhs_stateInputEqLag, rhs_stateInputEqLag)); out.stateInputIneqLagrangian = - toLagrangianMetrics(getSizes(dataArray[index].stateInputIneqLagrangian), f(lhs_stateInputIneq, rhs_stateInputIneq)); + toLagrangianMetrics(getSizes(dataArray[index].stateInputIneqLagrangian), f(lhs_stateInputIneqLag, rhs_stateInputIneqLag)); return out; } else { @@ -106,10 +111,16 @@ void random(const size_array_t& termsSize, std::vector<LagrangianMetrics>& lagra lagrangianMetrics = toLagrangianMetrics(termsSize, serializedLagrangianMetrics); } +void random(const size_array_t& termsSize, vector_array_t& constraintArray) { + const size_t length = std::accumulate(termsSize.begin(), termsSize.end(), termsSize.size()); + const vector_t serializedConstraintArray = vector_t::Random(length); + constraintArray = toConstraintArray(termsSize, serializedConstraintArray); +} + bool isApprox(const MetricsCollection& lhs, const MetricsCollection& rhs, scalar_t prec) { bool flag = std::abs(lhs.cost - rhs.cost) < prec; - flag = flag && lhs.stateEqConstraint.isApprox(rhs.stateEqConstraint, prec); - flag = flag && lhs.stateInputEqConstraint.isApprox(rhs.stateInputEqConstraint, prec); + flag = flag && toVector(lhs.stateEqConstraint).isApprox(toVector(rhs.stateEqConstraint), prec); + flag = flag && toVector(lhs.stateInputEqConstraint).isApprox(toVector(rhs.stateInputEqConstraint), prec); flag = flag && toVector(lhs.stateEqLagrangian).isApprox(toVector(rhs.stateEqLagrangian), prec); flag = flag && toVector(lhs.stateIneqLagrangian).isApprox(toVector(rhs.stateIneqLagrangian), prec); flag = flag && toVector(lhs.stateInputEqLagrangian).isApprox(toVector(rhs.stateInputEqLagrangian), prec); @@ -139,8 +150,8 @@ TEST(TestMetrics, testSwap) { ocs2::MetricsCollection termsMetricsCollection; termsMetricsCollection.cost = ocs2::vector_t::Random(1)(0); - termsMetricsCollection.stateEqConstraint = ocs2::vector_t::Random(2); - termsMetricsCollection.stateInputEqConstraint = ocs2::vector_t::Random(3); + ocs2::random(termsSize, termsMetricsCollection.stateEqConstraint); + ocs2::random(termsSize, termsMetricsCollection.stateInputEqConstraint); ocs2::random(termsSize, termsMetricsCollection.stateEqLagrangian); ocs2::random(termsSize, termsMetricsCollection.stateIneqLagrangian); ocs2::random(termsSize, termsMetricsCollection.stateInputEqLagrangian); @@ -174,8 +185,8 @@ TEST(TestMetrics, testInterpolation) { for (size_t i = 0; i < N; i++) { timeTrajectory[i] = i * 0.1; metricsCollectionTrajectory[i].cost = ocs2::vector_t::Random(1)(0); - metricsCollectionTrajectory[i].stateEqConstraint = ocs2::vector_t::Random(2); - metricsCollectionTrajectory[i].stateInputEqConstraint = ocs2::vector_t::Random(3); + ocs2::random(stateEqTermsSize, metricsCollectionTrajectory[i].stateEqConstraint); + ocs2::random(stateInputEqTermsSize, metricsCollectionTrajectory[i].stateInputEqConstraint); ocs2::random(stateEqTermsSize, metricsCollectionTrajectory[i].stateEqLagrangian); ocs2::random(stateIneqTermsSize, metricsCollectionTrajectory[i].stateIneqLagrangian); ocs2::random(stateInputEqTermsSize, metricsCollectionTrajectory[i].stateInputEqLagrangian); From 5f7aec462fdf44a1750e9ecbe6f27739ad303327 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 28 Jul 2022 19:31:59 +0200 Subject: [PATCH 242/512] solvers are updated --- ocs2_ddp/src/DDP_HelperFunctions.cpp | 11 +++++++---- ocs2_python_interface/src/PythonInterface.cpp | 2 +- .../ocs2_sqp/src/MultipleShootingTranscription.cpp | 6 ++---- .../ocs2_qp_solver/test/testOcs2QpSolver.cpp | 9 +++++---- 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/ocs2_ddp/src/DDP_HelperFunctions.cpp b/ocs2_ddp/src/DDP_HelperFunctions.cpp index 7ce907a54..7bb21cbe3 100644 --- a/ocs2_ddp/src/DDP_HelperFunctions.cpp +++ b/ocs2_ddp/src/DDP_HelperFunctions.cpp @@ -124,14 +124,17 @@ PerformanceIndex computeRolloutPerformanceIndex(const scalar_array_t& timeTrajec // - Final: state equality constraints // - PreJumps: state equality constraints // - Intermediates: state/state-input equality constraints - performanceIndex.equalityConstraintsSSE = problemMetrics.final.stateEqConstraint.squaredNorm(); + performanceIndex.equalityConstraintsSSE = constraintsSquaredNorm(problemMetrics.final.stateEqConstraint); - std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), - [&](const MetricsCollection& m) { performanceIndex.equalityConstraintsSSE += m.stateEqConstraint.squaredNorm(); }); + std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), [&](const MetricsCollection& m) { + performanceIndex.equalityConstraintsSSE += constraintsSquaredNorm(m.stateEqConstraint); + }); scalar_array_t equalityNorm2Trajectory(timeTrajectory.size()); std::transform(problemMetrics.intermediates.begin(), problemMetrics.intermediates.end(), equalityNorm2Trajectory.begin(), - [](const MetricsCollection& m) { return m.stateEqConstraint.squaredNorm() + m.stateInputEqConstraint.squaredNorm(); }); + [](const MetricsCollection& m) { + return constraintsSquaredNorm(m.stateEqConstraint) + constraintsSquaredNorm(m.stateInputEqConstraint); + }); performanceIndex.equalityConstraintsSSE += trapezoidalIntegration(timeTrajectory, equalityNorm2Trajectory); // Equality Lagrangians penalty diff --git a/ocs2_python_interface/src/PythonInterface.cpp b/ocs2_python_interface/src/PythonInterface.cpp index e6badf4eb..640e31847 100644 --- a/ocs2_python_interface/src/PythonInterface.cpp +++ b/ocs2_python_interface/src/PythonInterface.cpp @@ -202,7 +202,7 @@ vector_t PythonInterface::valueFunctionStateDerivative(scalar_t t, Eigen::Ref<co /******************************************************************************************************/ vector_t PythonInterface::stateInputEqualityConstraint(scalar_t t, Eigen::Ref<const vector_t> x, Eigen::Ref<const vector_t> u) { problem_.preComputationPtr->request(Request::Constraint, t, x, u); - return problem_.equalityConstraintPtr->getValue(t, x, u, *problem_.preComputationPtr); + return toVector(problem_.equalityConstraintPtr->getValue(t, x, u, *problem_.preComputationPtr)); } /******************************************************************************************************/ diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index 994a50c1d..d6fb87a08 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -102,10 +102,8 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt // Constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { - const vector_t constraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - if (constraints.size() > 0) { - performance.equalityConstraintsSSE = dt * constraints.squaredNorm(); - } + const auto constraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); + performance.equalityConstraintsSSE = dt * constraintsSquaredNorm(constraints); } return performance; diff --git a/ocs2_test_tools/ocs2_qp_solver/test/testOcs2QpSolver.cpp b/ocs2_test_tools/ocs2_qp_solver/test/testOcs2QpSolver.cpp index c47d63abe..c68c7c51f 100644 --- a/ocs2_test_tools/ocs2_qp_solver/test/testOcs2QpSolver.cpp +++ b/ocs2_test_tools/ocs2_qp_solver/test/testOcs2QpSolver.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/cost/QuadraticStateCost.h> #include <ocs2_core/cost/QuadraticStateInputCost.h> #include <ocs2_core/dynamics/LinearSystemDynamics.h> +#include <ocs2_core/model_data/Metrics.h> #include <ocs2_core/test/testTools.h> @@ -129,7 +130,7 @@ TEST_F(Ocs2QpSolverTest, satisfiesConstraints) { const auto& u0 = constrainedSolution.inputTrajectory[0]; preComputation.request(ocs2::Request::Constraint, t0, x0, u0); - const auto g0 = unconstrainedProblem.equalityConstraintPtr->getValue(t0, x0, u0, preComputation); + const auto g0 = ocs2::toVector(unconstrainedProblem.equalityConstraintPtr->getValue(t0, x0, u0, preComputation)); ASSERT_TRUE(g0.isZero(precision)); for (int k = 1; k < N; ++k) { @@ -138,10 +139,10 @@ TEST_F(Ocs2QpSolverTest, satisfiesConstraints) { const auto& u = constrainedSolution.inputTrajectory[k]; preComputation.request(ocs2::Request::Constraint, t, x, u); - const auto g = unconstrainedProblem.equalityConstraintPtr->getValue(t, x, u, preComputation); + const auto g = ocs2::toVector(unconstrainedProblem.equalityConstraintPtr->getValue(t, x, u, preComputation)); ASSERT_TRUE(g.isZero(precision)); - const auto h = unconstrainedProblem.stateEqualityConstraintPtr->getValue(t, x, preComputation); + const auto h = ocs2::toVector(unconstrainedProblem.stateEqualityConstraintPtr->getValue(t, x, preComputation)); ASSERT_TRUE(h.isZero(precision)); } @@ -149,7 +150,7 @@ TEST_F(Ocs2QpSolverTest, satisfiesConstraints) { const auto& xf = constrainedSolution.stateTrajectory[N]; preComputation.requestFinal(ocs2::Request::Constraint, tf, xf); - const auto gf = unconstrainedProblem.finalEqualityConstraintPtr->getValue(tf, xf, preComputation); + const auto gf = ocs2::toVector(unconstrainedProblem.finalEqualityConstraintPtr->getValue(tf, xf, preComputation)); ASSERT_TRUE(gf.isZero(precision)); } From 3143a02c9f3663841d9f98598fbb04df9d1cbbd7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cyrill=20P=C3=BCntener?= <34008738+wp99cp@users.noreply.github.com> Date: Wed, 3 Aug 2022 10:52:10 +0200 Subject: [PATCH 243/512] add raisim --- .github/workflows/ros-build-test.yml | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 2580bdc93..cd118bb63 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -30,7 +30,7 @@ jobs: - name: System deps run: | apt-get update - apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev + apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall - uses: actions/checkout@v2 with: @@ -46,6 +46,17 @@ jobs: git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets + + - name: Install RaiSim + run: | + git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech + cd ./raisim_tech | + mkdir build | + cd build | + cmake .. -DRAISIM_EXAMPLE=ON -DRAISIM_PY=ON -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") | + make -j4 && checkinstall + + - name: Build (${{ matrix.build_type }}) shell: bash run: | From f8397f62b750940cb0c7747d6e6dca07ac4a211e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cyrill=20P=C3=BCntener?= <34008738+wp99cp@users.noreply.github.com> Date: Wed, 3 Aug 2022 10:53:19 +0200 Subject: [PATCH 244/512] Cleanup code --- .github/workflows/ros-build-test.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index cd118bb63..664c32c78 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -56,7 +56,6 @@ jobs: cmake .. -DRAISIM_EXAMPLE=ON -DRAISIM_PY=ON -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") | make -j4 && checkinstall - - name: Build (${{ matrix.build_type }}) shell: bash run: | From 6087678ab9afbcb780eef69afb313f77693bf1f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cyrill=20P=C3=BCntener?= <34008738+wp99cp@users.noreply.github.com> Date: Wed, 3 Aug 2022 10:58:01 +0200 Subject: [PATCH 245/512] update installation path --- .github/workflows/ros-build-test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 664c32c78..856cd29cc 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -50,7 +50,7 @@ jobs: - name: Install RaiSim run: | git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech - cd ./raisim_tech | + cd src/raisim_tech | mkdir build | cd build | cmake .. -DRAISIM_EXAMPLE=ON -DRAISIM_PY=ON -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") | From 6977ee364917251e75f688d4ef1cfb93fd07bd9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cyrill=20P=C3=BCntener?= <34008738+wp99cp@users.noreply.github.com> Date: Wed, 3 Aug 2022 11:04:08 +0200 Subject: [PATCH 246/512] fix cd command --- .github/workflows/ros-build-test.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 856cd29cc..970636295 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -50,9 +50,9 @@ jobs: - name: Install RaiSim run: | git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech - cd src/raisim_tech | - mkdir build | - cd build | + cd src/raisim_tech + mkdir build + cd build cmake .. -DRAISIM_EXAMPLE=ON -DRAISIM_PY=ON -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") | make -j4 && checkinstall From c4db8b8853cc26d0887933be86811dcf3824588b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cyrill=20P=C3=BCntener?= <34008738+wp99cp@users.noreply.github.com> Date: Wed, 3 Aug 2022 11:13:38 +0200 Subject: [PATCH 247/512] try to fix installation process --- .github/workflows/ros-build-test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 970636295..ac8eb9f8b 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -53,7 +53,7 @@ jobs: cd src/raisim_tech mkdir build cd build - cmake .. -DRAISIM_EXAMPLE=ON -DRAISIM_PY=ON -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") | + cmake .. -DRAISIM_EXAMPLE=ON -DRAISIM_PY=ON -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") make -j4 && checkinstall - name: Build (${{ matrix.build_type }}) From ff144e12e6008ab2c7f1bd2191e54c7bdc14caae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Cyrill=20P=C3=BCntener?= <34008738+wp99cp@users.noreply.github.com> Date: Wed, 3 Aug 2022 12:20:52 +0200 Subject: [PATCH 248/512] Update ros-build-test.yml according to the comments of @farbod-farshidian --- .github/workflows/ros-build-test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index ac8eb9f8b..8816680ca 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -53,14 +53,14 @@ jobs: cd src/raisim_tech mkdir build cd build - cmake .. -DRAISIM_EXAMPLE=ON -DRAISIM_PY=ON -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") + cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") make -j4 && checkinstall - name: Build (${{ matrix.build_type }}) shell: bash run: | source /opt/ros/noetic/setup.bash - catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --ignore-pkg ocs2_raisim --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} + catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} - name: Test shell: bash From a3e7973cfcca8e687f4e3f9ebca2308332f36abb Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 3 Aug 2022 16:51:54 +0200 Subject: [PATCH 249/512] fixing getPartitionIntervalsFromTimeTrajectory --- .../include/ocs2_ddp/DDP_HelperFunctions.h | 20 +++++++++ ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h | 20 --------- ocs2_ddp/src/DDP_HelperFunctions.cpp | 29 +++++++++++++ ocs2_ddp/src/GaussNewtonDDP.cpp | 41 ++++--------------- ocs2_ddp/src/SLQ.cpp | 9 ++-- 5 files changed, 62 insertions(+), 57 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h b/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h index d7fecaa0a..1bf81fa23 100644 --- a/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h +++ b/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h @@ -135,6 +135,26 @@ void retrieveActiveNormalizedTime(const std::pair<int, int>& partitionInterval, const size_array_t& postEventIndices, scalar_array_t& normalizedTimeTrajectory, size_array_t& normalizedPostEventIndices); +/** + * Get the Partition Intervals From Time Trajectory. Intervals are defined as [start, end). + * + * Pay attention, the rightmost index of the end partition is (..., timeArray.size() - 1) , as the last value function is filled manually. + * The reason is though we don’t write to the end index, we do have to read it. Adding the last index to the final partition will + * cause a segmentation fault. There is no trivial method to distinguish the final partition from other partitions because, by design, + * partitions should be treated equally. + * + * Every time point that is equal or larger to the desiredPartitionPoint should be included in that partition. This logic here is the same + * as the event times. + * + * The last time of desiredPartitionPoints is filled manually. There is no round-off error involved. So it is safe to use == for + * floating-point numbers. The last time point is naturally included by using std::lower_bound. + * + * @param [in] timeTrajectory: time trajectory that will be divided + * @param [in] numWorkers: number of worker i.e. number of partitions + * @return array of index pairs indicating the start and end of each partition + */ +std::vector<std::pair<int, int>> computePartitionIntervals(const scalar_array_t& timeTrajectory, int numWorkers); + /** * Gets a reference to the linear controller from the given primal solution. */ diff --git a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h index b24f50c26..0d8170be1 100644 --- a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h +++ b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h @@ -229,26 +229,6 @@ class GaussNewtonDDP : public SolverBase { return getValueFunctionImpl(time, state, cachedPrimalData_.primalSolution, cachedDualData_.valueFunctionTrajectory); } - /** - * Get the Partition Intervals From Time Trajectory. Intervals are defined as [start, end). - * - * Pay attention, the rightmost index of the end partition is (..., timeArray.size() - 1) , as the last value function is filled manually. - * The reason is though we don’t write to the end index, we do have to read it. Adding the last index to the final partition will - * cause a segmentation fault. There is no trivial method to distinguish the final partition from other partitions because, by design, - * partitions should be treated equally. - * - * Every time point that is equal or larger to the desiredPartitionPoint should be included in that partition. This logic here is the same - * as the event times. - * - * The last time of desiredPartitionPoints is filled manually. There is no round-off error involved. So it is safe to use == for - * floating-point numbers. The last time point is naturally included by using std::lower_bound. - * - * @param [in] timeTrajectory: time trajectory that will be divided - * @param [in] numWorkers: number of worker i.e. number of partitions - * @return array of index pairs indicating the start and end of each partition - */ - std::vector<std::pair<int, int>> getPartitionIntervalsFromTimeTrajectory(const scalar_array_t& timeTrajectory, int numWorkers); - /** * Forward integrate the system dynamics with given controller in primalSolution and operating trajectories. In general, it uses * the given control policies and initial state, to integrate the system dynamics in the time period [initTime, finalTime]. diff --git a/ocs2_ddp/src/DDP_HelperFunctions.cpp b/ocs2_ddp/src/DDP_HelperFunctions.cpp index 7ce907a54..c00ce7858 100644 --- a/ocs2_ddp/src/DDP_HelperFunctions.cpp +++ b/ocs2_ddp/src/DDP_HelperFunctions.cpp @@ -300,4 +300,33 @@ void retrieveActiveNormalizedTime(const std::pair<int, int>& partitionInterval, [N, &partitionInterval](size_t i) -> size_t { return N - i + partitionInterval.first; }); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::vector<std::pair<int, int>> computePartitionIntervals(const scalar_array_t& timeTrajectory, int numWorkers) { + const scalar_t increment = (timeTrajectory.back() - timeTrajectory.front()) / static_cast<scalar_t>(numWorkers); + + scalar_array_t desiredPartitionPoints(numWorkers + 1); + desiredPartitionPoints.front() = timeTrajectory.front(); + for (size_t i = 1u; i < desiredPartitionPoints.size() - 1; i++) { + desiredPartitionPoints[i] = desiredPartitionPoints[i - 1] + increment; + } + desiredPartitionPoints.back() = timeTrajectory.back(); + + std::vector<std::pair<int, int>> partitionIntervals; + partitionIntervals.reserve(desiredPartitionPoints.size()); + + int endPos, startPos = 0; + for (size_t i = 1u; i < desiredPartitionPoints.size(); i++) { + const auto itr = std::upper_bound(timeTrajectory.begin(), timeTrajectory.end(), desiredPartitionPoints[i]); + endPos = (itr != timeTrajectory.end()) ? std::distance(timeTrajectory.begin(), itr) : (timeTrajectory.size() - 1); + if (endPos != startPos) { + partitionIntervals.emplace_back(startPos, endPos); + startPos = endPos; + } + } + + return partitionIntervals; +} + } // namespace ocs2 diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 9443b07cf..600711184 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -369,35 +369,6 @@ vector_t GaussNewtonDDP::getStateInputEqualityConstraintLagrangianImpl(scalar_t return DmDagger.transpose() * temp; } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -std::vector<std::pair<int, int>> GaussNewtonDDP::getPartitionIntervalsFromTimeTrajectory(const scalar_array_t& timeTrajectory, - int numWorkers) { - scalar_array_t desiredPartitionPoints(numWorkers + 1); - desiredPartitionPoints.front() = timeTrajectory.front(); - - const scalar_t increment = (timeTrajectory.back() - timeTrajectory.front()) / static_cast<scalar_t>(numWorkers); - for (size_t i = 1u; i < desiredPartitionPoints.size() - 1; i++) { - desiredPartitionPoints[i] = desiredPartitionPoints[i - 1] + increment; - } - desiredPartitionPoints.back() = timeTrajectory.back(); - - std::vector<std::pair<int, int>> partitionIntervals; - partitionIntervals.reserve(desiredPartitionPoints.size()); - - int endPos, startPos = 0; - for (size_t i = 1u; i < desiredPartitionPoints.size(); i++) { - const scalar_t& time = desiredPartitionPoints[i]; - endPos = std::distance(timeTrajectory.begin(), std::lower_bound(timeTrajectory.begin(), timeTrajectory.end(), time)); - if (endPos != startPos) { - partitionIntervals.emplace_back(startPos, endPos); - startPos = endPos; - } - } - - return partitionIntervals; -} /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -534,8 +505,7 @@ scalar_t GaussNewtonDDP::solveSequentialRiccatiEquationsImpl(const ScalarFunctio riccatiEquationsWorker(0, partitionInterval, finalValueFunction); } else { // solve it in parallel // do equal-time partitions based on available thread resource - std::vector<std::pair<int, int>> partitionIntervals = - getPartitionIntervalsFromTimeTrajectory(nominalPrimalData_.primalSolution.timeTrajectory_, ddpSettings_.nThreads_); + const auto partitionIntervals = computePartitionIntervals(nominalPrimalData_.primalSolution.timeTrajectory_, ddpSettings_.nThreads_); // hold the final value function of each partition std::vector<ScalarFunctionQuadraticApproximation> finalValueFunctionOfEachPartition(partitionIntervals.size()); @@ -559,7 +529,14 @@ scalar_t GaussNewtonDDP::solveSequentialRiccatiEquationsImpl(const ScalarFunctio if (ddpSettings_.checkNumericalStability_) { const int N = nominalPrimalData_.primalSolution.timeTrajectory_.size(); for (int k = N - 1; k >= 0; k--) { - const auto errorDescription = checkBeingPSD(nominalDualData_.valueFunctionTrajectory[k], "ValueFunction"); + // check size + auto errorDescription = checkSize(nominalPrimalData_.primalSolution.stateTrajectory_[k].size(), 0, + nominalDualData_.valueFunctionTrajectory[k], "ValueFunction"); + if (!errorDescription.empty()) { + throw std::runtime_error(errorDescription); + } + // check PSD + errorDescription = checkBeingPSD(nominalDualData_.valueFunctionTrajectory[k], "ValueFunction"); if (!errorDescription.empty()) { std::stringstream throwMsg; throwMsg << "at time " << nominalPrimalData_.primalSolution.timeTrajectory_[k] << ":\n"; diff --git a/ocs2_ddp/src/SLQ.cpp b/ocs2_ddp/src/SLQ.cpp index f2670e1e4..4023dfc44 100644 --- a/ocs2_ddp/src/SLQ.cpp +++ b/ocs2_ddp/src/SLQ.cpp @@ -239,7 +239,6 @@ void SLQ::riccatiEquationsWorker(size_t workerIndex, const std::pair<int, int>& * SsNormalized = [-10.0, ..., -2.0, -1.0, -0.0] */ vector_array_t& allSsTrajectory = allSsTrajectoryStock_[workerIndex]; - allSsTrajectory.clear(); integrateRiccatiEquationNominalTime(*riccatiIntegratorPtrStock_[workerIndex], *riccatiEquationsPtrStock_[workerIndex], partitionInterval, nominalTimeTrajectory, nominalEventsPastTheEndIndices, std::move(allSsFinal), SsNormalizedTime, SsNormalizedPostEventIndices, allSsTrajectory); @@ -266,7 +265,6 @@ void SLQ::integrateRiccatiEquationNominalTime(IntegratorBase& riccatiIntegrator, const int nominalTimeSize = SsNormalizedTime.size(); const int numEvents = SsNormalizedPostEventIndices.size(); auto partitionDuration = nominalTimeTrajectory[partitionInterval.second] - nominalTimeTrajectory[partitionInterval.first]; - const auto numTimeSteps = static_cast<size_t>(settings().maxNumStepsPerSecond_ * std::max(1.0, partitionDuration)); // Normalized switching time indices, start and end of the partition are added at the beginning and end using iterator_t = scalar_array_t::const_iterator; @@ -281,15 +279,16 @@ void SLQ::integrateRiccatiEquationNominalTime(IntegratorBase& riccatiIntegrator, // integrating the Riccati equations allSsTrajectory.clear(); - allSsTrajectory.reserve(numTimeSteps); + allSsTrajectory.reserve(nominalTimeSize); for (int i = 0; i <= numEvents; i++) { iterator_t beginTimeItr = SsNormalizedSwitchingTimesIndices[i].first; iterator_t endTimeItr = SsNormalizedSwitchingTimesIndices[i].second; - Observer observer(&allSsTrajectory); // solve Riccati equations + Observer observer(&allSsTrajectory); + const auto maxNumTimeSteps = static_cast<size_t>(settings().maxNumStepsPerSecond_ * std::max(1.0, partitionDuration)); riccatiIntegrator.integrateTimes(riccatiEquation, observer, allSsFinal, beginTimeItr, endTimeItr, settings().timeStep_, - settings().absTolODE_, settings().relTolODE_, numTimeSteps); + settings().absTolODE_, settings().relTolODE_, maxNumTimeSteps); if (i < numEvents) { allSsFinal = riccatiEquation.computeJumpMap(*endTimeItr, allSsTrajectory.back()); From ea4c76a80a5d5e8f32c0c8ac0df2bec73853135d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 3 Aug 2022 17:41:24 +0200 Subject: [PATCH 250/512] accounting for state-only case --- ocs2_core/src/Types.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_core/src/Types.cpp b/ocs2_core/src/Types.cpp index cdbf0ac00..6411a8dae 100644 --- a/ocs2_core/src/Types.cpp +++ b/ocs2_core/src/Types.cpp @@ -189,7 +189,7 @@ std::string checkSize(int stateDim, int inputDim, const ScalarFunctionQuadraticA if (data.dfdux.rows() != inputDim) { errorDescription << dataName << ".dfdux.rows() != " << inputDim << "\n"; } - if (data.dfdux.cols() != stateDim) { + if (data.dfdux.cols() != stateDim && inputDim > 0) { errorDescription << dataName << ".dfdux.cols() != " << stateDim << "\n"; } From cdaefb5d45bb6fb399c4e04872ddcdd89a63ca6f Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 4 Aug 2022 14:49:54 +0200 Subject: [PATCH 251/512] simplify log dir path --- .../python/ocs2_mpcnet_core/mpcnet.py | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index ee517c7da..4cb10e724 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -32,6 +32,7 @@ Provides a class that handles the MPC-Net training. """ +import os import time import datetime import torch @@ -86,15 +87,8 @@ def __init__( # interface self.interface = interface # logging - self.log_dir = ( - root_dir - + "/runs/" - + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") - + "_" - + config.NAME - + "_" - + config.DESCRIPTION - ) + timestamp = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + self.log_dir = os.path.join(root_dir, "runs", f"{timestamp}_{config.NAME}_{config.DESCRIPTION}") self.writer = SummaryWriter(self.log_dir) # loss self.experts_loss = experts_loss From 9f0f108bddd9132f2396a9c6bf1302ced82dbd64 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 4 Aug 2022 15:12:30 +0200 Subject: [PATCH 252/512] add abstract method decorator --- .../python/ocs2_mpcnet_core/loss/base.py | 6 ++++-- .../python/ocs2_mpcnet_core/memory/base.py | 12 ++++++++---- .../python/ocs2_mpcnet_core/mpcnet.py | 6 ++++-- .../python/ocs2_mpcnet_core/policy/base.py | 6 ++++-- 4 files changed, 20 insertions(+), 10 deletions(-) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py index 6da703264..329c58089 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py @@ -33,11 +33,12 @@ """ import torch +from abc import ABCMeta, abstractmethod from ocs2_mpcnet_core.config import Config -class BaseLoss: +class BaseLoss(metaclass=ABCMeta): """Base loss. Provides the interface to all loss classes. @@ -53,6 +54,7 @@ def __init__(self, config: Config) -> None: """ pass + @abstractmethod def __call__( self, x_query: torch.Tensor, @@ -89,4 +91,4 @@ def __call__( Returns: A (1) tensor containing the mean loss. """ - raise NotImplementedError() + pass diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py index 993c22030..5c1b26fec 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py @@ -35,12 +35,13 @@ import torch import numpy as np from typing import Tuple, List +from abc import ABCMeta, abstractmethod from ocs2_mpcnet_core.config import Config from ocs2_mpcnet_core import ScalarFunctionQuadraticApproximation -class BaseMemory: +class BaseMemory(metaclass=ABCMeta): """Base memory. Provides the interface to all memory classes. @@ -56,6 +57,7 @@ def __init__(self, config: Config) -> None: """ pass + @abstractmethod def push( self, t: float, @@ -79,8 +81,9 @@ def push( action_transformation: A list containing NumPy arrays of shape (U,A) and (U) with the action transformation. hamiltonian: An OCS2 scalar function quadratic approximation representing the Hamiltonian around x and u. """ - raise NotImplementedError() + pass + @abstractmethod def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: """Samples data from the memory. @@ -105,8 +108,9 @@ def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: - dHdu_batch: A (B,U) tensor with the input gradients of the Hamiltonian approximations. - H_batch: A (B) tensor with the Hamiltonians at the development/expansion points. """ - raise NotImplementedError() + pass + @abstractmethod def __len__(self) -> int: """The length of the memory. @@ -115,4 +119,4 @@ def __len__(self) -> int: Returns: An integer describing the length of the memory. """ - raise NotImplementedError() + pass diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index 4cb10e724..52734dc03 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -38,6 +38,7 @@ import torch import numpy as np from typing import Optional, Tuple +from abc import ABCMeta, abstractmethod from torch.utils.tensorboard import SummaryWriter @@ -49,7 +50,7 @@ from ocs2_mpcnet_core.policy import BasePolicy -class Mpcnet: +class Mpcnet(metaclass=ABCMeta): """MPC-Net. Implements the main methods for the MPC-Net training. @@ -102,6 +103,7 @@ def __init__( # optimizer self.optimizer = torch.optim.Adam(self.policy.parameters(), lr=config.LEARNING_RATE) + @abstractmethod def get_tasks( self, tasks_number: int, duration: float ) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: @@ -119,7 +121,7 @@ def get_tasks( - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. """ - raise NotImplementedError() + pass def start_data_generation(self, policy: BasePolicy, alpha: float = 1.0): """Start data generation. diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py index 012712b65..7b9c584c2 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py @@ -34,12 +34,13 @@ import torch from typing import Tuple +from abc import ABCMeta, abstractmethod from ocs2_mpcnet_core.config import Config from ocs2_mpcnet_core.helper import bmv -class BasePolicy(torch.nn.Module): +class BasePolicy(torch.nn.Module, metaclass=ABCMeta): """Base policy. Provides the interface to all policy classes. @@ -65,6 +66,7 @@ def __init__(self, config: Config) -> None: torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) ) + @abstractmethod def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, ...]: """Forward method. @@ -76,7 +78,7 @@ def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, ...]: Returns: tuple: A tuple with the predictions, e.g. containing a (B,A) tensor with the predicted actions. """ - raise NotImplementedError() + pass def scale_observation(self, observation: torch.Tensor) -> torch.Tensor: """Scale observation. From ce325597247e3f9d5a1fb1b8373b6312bf3c3e56 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 4 Aug 2022 16:11:54 +0200 Subject: [PATCH 253/512] run black --- .../python/ocs2_ballbot_mpcnet/mpcnet.py | 5 +---- .../python/ocs2_legged_robot_mpcnet/mpcnet.py | 15 +++------------ .../python/ocs2_mpcnet_core/mpcnet.py | 6 ++---- .../python/ocs2_mpcnet_core/policy/__init__.py | 8 +++++++- 4 files changed, 13 insertions(+), 21 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py index e95d611cc..86b8fe743 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -126,10 +126,7 @@ def get_tasks( target_trajectories = helper.get_target_trajectories_array(tasks_number) for i in range(tasks_number): initial_observations[i] = helper.get_system_observation( - initial_mode, - initial_time, - self.get_random_initial_state(), - np.zeros(self.config.INPUT_DIM), + initial_mode, initial_time, self.get_random_initial_state(), np.zeros(self.config.INPUT_DIM) ) mode_schedules[i] = helper.get_mode_schedule(*self.get_default_event_times_and_mode_sequence(duration)) target_trajectories[i] = helper.get_target_trajectories( diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py index 5f92ee55b..213b600f8 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -223,10 +223,7 @@ def get_tasks( for i in range(tasks_number): if choices[i] == "stance": initial_observations[i] = helper.get_system_observation( - initial_mode, - initial_time, - self.get_random_initial_state_stance(), - np.zeros(self.config.INPUT_DIM), + initial_mode, initial_time, self.get_random_initial_state_stance(), np.zeros(self.config.INPUT_DIM) ) mode_schedules[i] = helper.get_mode_schedule(*self.get_stance(duration)) target_trajectories[i] = helper.get_target_trajectories( @@ -236,10 +233,7 @@ def get_tasks( ) elif choices[i] == "trot_1": initial_observations[i] = helper.get_system_observation( - initial_mode, - initial_time, - self.get_random_initial_state_trot(), - np.zeros(self.config.INPUT_DIM), + initial_mode, initial_time, self.get_random_initial_state_trot(), np.zeros(self.config.INPUT_DIM) ) mode_schedules[i] = helper.get_mode_schedule(*self.get_trot_1(duration)) target_trajectories[i] = helper.get_target_trajectories( @@ -249,10 +243,7 @@ def get_tasks( ) elif choices[i] == "trot_2": initial_observations[i] = helper.get_system_observation( - initial_mode, - initial_time, - self.get_random_initial_state_trot(), - np.zeros(self.config.INPUT_DIM), + initial_mode, initial_time, self.get_random_initial_state_trot(), np.zeros(self.config.INPUT_DIM) ) mode_schedules[i] = helper.get_mode_schedule(*self.get_trot_2(duration)) target_trajectories[i] = helper.get_target_trajectories( diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py index 52734dc03..f0cd8d3fa 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -135,8 +135,7 @@ def start_data_generation(self, policy: BasePolicy, alpha: float = 1.0): policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=self.dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = self.get_tasks( - self.config.DATA_GENERATION_TASKS, - self.config.DATA_GENERATION_DURATION, + self.config.DATA_GENERATION_TASKS, self.config.DATA_GENERATION_DURATION ) self.interface.startDataGeneration( alpha, @@ -162,8 +161,7 @@ def start_policy_evaluation(self, policy: BasePolicy, alpha: float = 0.0): policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" torch.onnx.export(model=policy, args=self.dummy_observation, f=policy_file_path) initial_observations, mode_schedules, target_trajectories = self.get_tasks( - self.config.POLICY_EVALUATION_TASKS, - self.config.POLICY_EVALUATION_DURATION, + self.config.POLICY_EVALUATION_TASKS, self.config.POLICY_EVALUATION_DURATION ) self.interface.startPolicyEvaluation( alpha, diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py index 46ff16a7f..3e178b783 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py @@ -4,4 +4,10 @@ from .mixture_of_nonlinear_experts import MixtureOfNonlinearExpertsPolicy from .nonlinear import NonlinearPolicy -__all__ = ["BasePolicy", "LinearPolicy", "MixtureOfLinearExpertsPolicy", "MixtureOfNonlinearExpertsPolicy", "NonlinearPolicy"] +__all__ = [ + "BasePolicy", + "LinearPolicy", + "MixtureOfLinearExpertsPolicy", + "MixtureOfNonlinearExpertsPolicy", + "NonlinearPolicy", +] From 81f879b47b9dde6336d34a7dee181480e0e7c4bd Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 4 Aug 2022 16:52:16 +0200 Subject: [PATCH 254/512] extending SolverObserver --- ocs2_oc/CMakeLists.txt | 2 +- .../OptimalControlProblemHelperFunction.h | 58 +++-- .../include/ocs2_oc/oc_solver/SolverBase.h | 8 +- .../AugmentedLagrangianObserver.h | 107 ---------- .../synchronized_module/SolverObserver.h | 180 ++++++++++++++++ ocs2_oc/src/lintTarget.cpp | 1 + .../OptimalControlProblemHelperFunction.cpp | 79 ++++++- ocs2_oc/src/oc_solver/SolverBase.cpp | 7 +- .../AugmentedLagrangianObserver.cpp | 113 ---------- .../synchronized_module/SolverObserver.cpp | 200 ++++++++++++++++++ .../ocs2_cartpole/test/testCartpole.cpp | 8 +- .../ocs2_cartpole_ros/src/CartpoleMpcNode.cpp | 36 ++-- ocs2_ros_interfaces/CMakeLists.txt | 2 +- ...llbacks.h => SolverObserverRosCallbacks.h} | 10 +- ocs2_ros_interfaces/src/lintTarget.cpp | 1 + ...cks.cpp => SolverObserverRosCallbacks.cpp} | 24 +-- 16 files changed, 547 insertions(+), 289 deletions(-) delete mode 100644 ocs2_oc/include/ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h create mode 100644 ocs2_oc/include/ocs2_oc/synchronized_module/SolverObserver.h delete mode 100644 ocs2_oc/src/synchronized_module/AugmentedLagrangianObserver.cpp create mode 100644 ocs2_oc/src/synchronized_module/SolverObserver.cpp rename ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/{RosAugmentedLagrangianCallbacks.h => SolverObserverRosCallbacks.h} (88%) rename ocs2_ros_interfaces/src/synchronized_module/{RosAugmentedLagrangianCallbacks.cpp => SolverObserverRosCallbacks.cpp} (79%) diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index db09a027a..400235334 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -65,7 +65,7 @@ add_library(${PROJECT_NAME} src/synchronized_module/ReferenceManager.cpp src/synchronized_module/LoopshapingReferenceManager.cpp src/synchronized_module/LoopshapingSynchronizedModule.cpp - src/synchronized_module/AugmentedLagrangianObserver.cpp + src/synchronized_module/SolverObserver.cpp src/trajectory_adjustment/TrajectorySpreading.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h index c6320105b..c1f0b9ed4 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include <functional> + #include <ocs2_core/model_data/Metrics.h> #include <ocs2_core/model_data/Multiplier.h> @@ -135,39 +137,71 @@ void updateIntermediateMultiplierCollection(const OptimalControlProblem& ocp, sc MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection); /** - * Extracts a requested final Term LagrangianMetrics from the input MetricsCollection. + * Extracts a requested final constraint term vector from the input MetricsCollection. + * @param [in] ocp : A const reference to the optimal control problem. + * @param [in] name : The name of the term. + * @param [in] metricsColl : MetricsCollection. + * @param [out] metrics : A const pointer to the constraint term value. + */ +const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const MetricsCollection& metricsColl); + +/** + * Extracts a requested final term LagrangianMetrics from the input MetricsCollection. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. * @param [in] metricsColl : MetricsCollection. * @param [out] metrics : A const pointer to the term LagrangianMetrics. - * @return True if the term found in the final collection. */ -const LagrangianMetrics* extractFinalTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const MetricsCollection& metricsColl); +const LagrangianMetrics* extractFinalTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const MetricsCollection& metricsColl); + +/** + * Extracts an array of the requested pre-jump constraint term from the input MetricsCollection array. + * @param [in] ocp : A const reference to the optimal control problem. + * @param [in] name : The name of the term. + * @param [in] metricsCollArray : MetricsCollection array. + * @param [out] metricsArray : Array of const references to constraint term values. + * @return True if the term found in the pre-jump collection. + */ +bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::string& name, + const std::vector<MetricsCollection>& metricsCollArray, + std::vector<std::reference_wrapper<const vector_t>>& metricsArray); /** - * Extracts a requested pre-jump Term LagrangianMetrics array from the input MetricsCollection array. + * Extracts a requested pre-jump term LagrangianMetrics array from the input MetricsCollection array. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. * @param [in] metricsCollArray : MetricsCollection array. * @param [out] metricsArray : Array of const references to term LagrangianMetrics. * @return True if the term found in the pre-jump collection. */ -bool extractPreJumpTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollArray, - std::vector<LagrangianMetricsConstRef>& metricsArray); +bool extractPreJumpTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const std::vector<MetricsCollection>& metricsCollArray, + std::vector<LagrangianMetricsConstRef>& metricsArray); + +/** + * Extracts a trajectory of the requested intermediate constraint term from the input MetricsCollection trajectory. + * @param [in] ocp : A const reference to the optimal control problem. + * @param [in] name : The name of the term. + * @param [in] metricsCollTraj : MetricsCollection trajectory. + * @param [out] metricsTrajectory : Trajectory of const references to constraint term values. + * @return True if the term found in the intermediate collection. + */ +bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const std::string& name, + const std::vector<MetricsCollection>& metricsCollTraj, + std::vector<std::reference_wrapper<const vector_t>>& metricsTrajectory); /** - * Extracts a requested intermediate Term LagrangianMetrics trajectory from the input MetricsCollection trajectory. + * Extracts a requested intermediate term LagrangianMetrics trajectory from the input MetricsCollection trajectory. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. * @param [in] metricsCollTraj : MetricsCollection trajectory. * @param [out] metricsTrajectory : Trajectory of const references to term LagrangianMetrics. * @return True if the term found in the intermediate collection. */ -bool extractIntermediateTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollTraj, - std::vector<LagrangianMetricsConstRef>& metricsTrajectory); +bool extractIntermediateTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const std::vector<MetricsCollection>& metricsCollTraj, + std::vector<LagrangianMetricsConstRef>& metricsTrajectory); /** * Extracts a requested final Term Multiplier from the input MultiplierCollection. diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h index 753aa9461..d81614eb1 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h @@ -44,7 +44,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/oc_solver/PerformanceIndex.h> #include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> #include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h> -#include "ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h" +#include "ocs2_oc/synchronized_module/SolverObserver.h" namespace ocs2 { @@ -137,9 +137,7 @@ class SolverBase { * Adds one observer to the vector of modules that observe solver's dual solution and optimized metrics. * @note: These observer can slow down the MPC. Only employ them during debugging and remove them for deployment. */ - void addAugmentedLagrangianObserver(std::unique_ptr<AugmentedLagrangianObserver> observerModule) { - augmentedLagrangianObservers_.push_back(std::move(observerModule)); - } + void addSolverObserver(std::unique_ptr<SolverObserver> observerModule) { solverObservers_.push_back(std::move(observerModule)); } /** * @brief Returns a const reference to the definition of optimal control problem. @@ -271,7 +269,7 @@ class SolverBase { mutable std::mutex outputDisplayGuardMutex_; std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_; // this pointer cannot be nullptr std::vector<std::shared_ptr<SolverSynchronizedModule>> synchronizedModules_; - std::vector<std::unique_ptr<AugmentedLagrangianObserver>> augmentedLagrangianObservers_; + std::vector<std::unique_ptr<SolverObserver>> solverObservers_; }; } // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h b/ocs2_oc/include/ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h deleted file mode 100644 index 6d5ca524c..000000000 --- a/ocs2_oc/include/ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h +++ /dev/null @@ -1,107 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#pragma once - -#include <functional> -#include <string> - -#include <ocs2_core/Types.h> -#include <ocs2_core/model_data/Metrics.h> - -#include "ocs2_oc/oc_data/DualSolution.h" -#include "ocs2_oc/oc_data/ProblemMetrics.h" -#include "ocs2_oc/oc_problem/OptimalControlProblem.h" - -namespace ocs2 { - -/** - * A solver observer module that extract the metrics and multiplier corresponding to the requested term. - */ -class AugmentedLagrangianObserver { - public: - /** Input arguments are time stamp and a const reference array of the term's LagrangianMetrics. */ - using metrics_callback_t = std::function<void(const scalar_array_t&, const std::vector<LagrangianMetricsConstRef>&)>; - /** Input arguments are time stamp and a const reference array of the term's Multiplier. */ - using multiplier_callback_t = std::function<void(const scalar_array_t&, const std::vector<MultiplierConstRef>&)>; - - /** - * Constructor. - * @param [in] termsName: The name of the term used to add it to OptimalControlProblem. - * @note The name is case sensitive. - */ - explicit AugmentedLagrangianObserver(std::string termName) : termName_(std::move(termName)) {} - - /** - * Sets the callback for processing extracted metrics array. - * The callback should have the following signature: - * void callback(const scalar_array_t& timeTrajectory, const std::vector<LagrangianMetricsConstRef>& termMetrics) - */ - template <class CallbackType> - void setMetricsCallback(CallbackType&& callback) { - metricsCallback_ = std::forward<CallbackType>(callback); - } - - /** - * Sets the callback for processing extracted multiplier array. - * The callback should have the following signature: - * void callback(const scalar_array_t& timeTrajectory, const std::vector<MultiplierConstRef>& termMultiplierArray) - */ - template <class CallbackType> - void setMultiplierCallback(CallbackType&& callback) { - multiplierCallback_ = std::forward<CallbackType>(callback); - } - - /** - * Extracts the metrics associated to the requested term from the given problemMetrics. - * - * @param [in] ocp : The optimal control problem. - * @param [in] primalSolution : The primal solution. - * @param [in] problemMetrics: The metrics of current solution - */ - void extractTermMetrics(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, const ProblemMetrics& problemMetrics); - - /** - * Extracts the multipliers associated to the requested term from the given dualSolution. - * - * @param [in] ocp : The optimal control problem. - * @param [in] dualSolution: The dual solution - */ - void extractTermMultipliers(const OptimalControlProblem& ocp, const DualSolution& dualSolution); - - private: - const std::string termName_; - std::vector<LagrangianMetricsConstRef> termMetricsArray_{}; - std::vector<MultiplierConstRef> termMultiplierArray_{}; - - metrics_callback_t metricsCallback_; - multiplier_callback_t multiplierCallback_; -}; - -} // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/synchronized_module/SolverObserver.h b/ocs2_oc/include/ocs2_oc/synchronized_module/SolverObserver.h new file mode 100644 index 000000000..5772d1f2d --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/synchronized_module/SolverObserver.h @@ -0,0 +1,180 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <functional> +#include <memory> +#include <string> + +#include <ocs2_core/Types.h> +#include <ocs2_core/model_data/Metrics.h> + +#include "ocs2_oc/oc_data/DualSolution.h" +#include "ocs2_oc/oc_data/ProblemMetrics.h" +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" + +namespace ocs2 { + +/** + * A solver observer module that extract the metrics and multiplier corresponding to the requested term. + */ +class SolverObserver { + private: + struct PrivateToken {}; + + public: + /** The public constructor which cannot be used for instantiation. For that, use the following factory methods: + * - SolverObserver::ConstraintTermObserver + * - SolverObserver::LagrangianTermObserver + */ + explicit SolverObserver(PrivateToken token) {} + + /** The time of the metric that will be observed. */ + enum class Type { + Final, + PreJump, + Intermediate, + }; + + /** Input arguments are time stamp and a const reference array of the constraint term's value. */ + using constraint_callback_t = std::function<void(const scalar_array_t&, const std::vector<std::reference_wrapper<const vector_t>>&)>; + /** Input arguments are time stamp and a const reference array of the term's LagrangianMetrics. */ + using lagrangian_callback_t = std::function<void(const scalar_array_t&, const std::vector<LagrangianMetricsConstRef>&)>; + /** Input arguments are time stamp and a const reference array of the term's Multiplier. */ + using multiplier_callback_t = std::function<void(const scalar_array_t&, const std::vector<MultiplierConstRef>&)>; + + /** + * Sets the callback for processing extracted a constraint term. + * + * @param [in] type: It could be either at Final, PreJump, or Intermediate. + * @param [in] termsName: The name of the term used to add it to OptimalControlProblem. Note that the name is case sensitive. + * @param [in] constraintCallback: The callback for processing extracted constraint vector array. The callback should have the following + * signature: void callback(const scalar_array_t& timeStamp, const std::vector<std::reference_wrapper<const vector_t>>& termMetrics). It + * could any C-style function pointer, lambda, or std::function. For an example check constraint_callback_t. + */ + template <class ConstraintCallbackType> + static std::unique_ptr<SolverObserver> ConstraintTermObserver(Type type, const std::string& termName, + ConstraintCallbackType&& constraintCallback); + + /** + * Sets the callback for processing extracted a term's LagrangianMetrics and/or Multiplier. + * + * @param [in] type: It could be either at Final, PreJump, or Intermediate. + * @param [in] termsName: The name of the term used to add it to OptimalControlProblem. Note that the name is case sensitive. + * @param [in] lagrangianCallback: The callback for processing extracted LagrangianMetrics array. The callback should have the following + * signature: void callback(const scalar_array_t& timeStamp, std::vector<LagrangianMetricsConstRef>& termMetrics). It could any C-style + * function pointer, lambda, or std::function. For an example check lagrangian_callback_t). + * @param [in] multiplierCallback: The callback for processing extracted Multiplier array. The callback should have the following + * signature: void callback(const scalar_array_t& timeStamp, const std::vector<MultiplierConstRef>& termMultiplier). It could any C-style + * function pointer, lambda, or std::function. For an example check multiplier_callback_t. + */ + template <class LagrangianCallbackType, class MultiplierCallbackType = multiplier_callback_t> + static std::unique_ptr<SolverObserver> LagrangianTermObserver(Type type, const std::string& termName, + LagrangianCallbackType&& lagrangianCallback, + MultiplierCallbackType&& multiplierCallback = multiplier_callback_t()); + + private: + /** + * Constructor. + * @param [in] termsName: The name of the term used to add it to OptimalControlProblem. + * @note The name is case sensitive. + */ + template <class ConstraintCallbackType, class LagrangianCallbackType, class MultiplierCallbackType> + SolverObserver(Type type, std::string termName, ConstraintCallbackType constraintCallback, LagrangianCallbackType lagrangianCallback, + MultiplierCallbackType multiplierCallback) + : type_(type), + termName_(std::move(termName)), + constraintCallback_(std::move(constraintCallback)), + lagrangianCallback_(std::move(lagrangianCallback)), + multiplierCallback_(std::move(multiplierCallback)) {} + + /** + * Extracts the constraint value associated to the requested term from the given problemMetrics. + * + * @param [in] ocp : The optimal control problem. + * @param [in] primalSolution : The primal solution. + * @param [in] problemMetrics: The metrics of current solution + */ + void extractTermConstraint(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, const ProblemMetrics& problemMetrics); + + /** + * Extracts the LagrangianMetrics associated to the requested term from the given problemMetrics. + * + * @param [in] ocp : The optimal control problem. + * @param [in] primalSolution : The primal solution. + * @param [in] problemMetrics: The metrics of current solution + */ + void extractTermLagrangianMetrics(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, + const ProblemMetrics& problemMetrics); + + /** + * Extracts the multipliers associated to the requested term from the given dualSolution. + * + * @param [in] ocp : The optimal control problem. + * @param [in] dualSolution: The dual solution + */ + void extractTermMultipliers(const OptimalControlProblem& ocp, const DualSolution& dualSolution); + + /** + * Variables + */ + const Type type_ = Type::Final; + const std::string termName_ = ""; + constraint_callback_t constraintCallback_; + lagrangian_callback_t lagrangianCallback_; + multiplier_callback_t multiplierCallback_; + + /** SolverBase needs to call extractXXX private methods. */ + friend class SolverBase; +}; + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <class ConstraintCallbackType> +std::unique_ptr<SolverObserver> SolverObserver::ConstraintTermObserver(Type type, const std::string& termName, + ConstraintCallbackType&& constraintCallback) { + return std::unique_ptr<SolverObserver>(new SolverObserver(type, termName, std::forward<ConstraintCallbackType>(constraintCallback), + lagrangian_callback_t(), multiplier_callback_t())); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <class LagrangianCallbackType, class MultiplierCallbackType> +std::unique_ptr<SolverObserver> SolverObserver::LagrangianTermObserver(Type type, const std::string& termName, + LagrangianCallbackType&& lagrangianCallback, + MultiplierCallbackType&& multiplierCallback) { + return std::unique_ptr<SolverObserver>(new SolverObserver(type, termName, constraint_callback_t(), + std::forward<LagrangianCallbackType>(lagrangianCallback), + std::forward<MultiplierCallbackType>(multiplierCallback))); +} + +} // namespace ocs2 diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 1f5887c14..a5187f23f 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -21,6 +21,7 @@ #include <ocs2_oc/synchronized_module/ReferenceManager.h> #include <ocs2_oc/synchronized_module/ReferenceManagerDecorator.h> #include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> +#include <ocs2_oc/synchronized_module/SolverObserver.h> #include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h> // rollout diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp index bc88c40ff..9e76c02db 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp @@ -186,8 +186,22 @@ void updateIntermediateMultiplierCollection(const OptimalControlProblem& ocp, sc /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -const LagrangianMetrics* extractFinalTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const MetricsCollection& metricsColl) { +const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, const std::string& name, + const MetricsCollection& metricsColl) { + size_t index; + if (ocp.finalEqualityConstraintPtr->getTermIndex(name, index)) { + return &metricsColl.stateEqConstraint[index]; + + } else { + return nullptr; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +const LagrangianMetrics* extractFinalTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const MetricsCollection& metricsColl) { size_t index; if (ocp.finalEqualityLagrangianPtr->getTermIndex(name, index)) { return &metricsColl.stateEqLagrangian[index]; @@ -203,9 +217,30 @@ const LagrangianMetrics* extractFinalTermMetrics(const OptimalControlProblem& oc /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -bool extractPreJumpTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollArray, - std::vector<LagrangianMetricsConstRef>& metricsArray) { +bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::string& name, + const std::vector<MetricsCollection>& metricsCollArray, + std::vector<std::reference_wrapper<const vector_t>>& metricsArray) { + metricsArray.clear(); + + size_t index; + if (ocp.preJumpEqualityConstraintPtr->getTermIndex(name, index)) { + metricsArray.reserve(metricsCollArray.size()); + for (const auto& metricsColl : metricsCollArray) { + metricsArray.emplace_back(metricsColl.stateEqConstraint[index]); + } + return true; + + } else { + return false; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool extractPreJumpTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const std::vector<MetricsCollection>& metricsCollArray, + std::vector<LagrangianMetricsConstRef>& metricsArray) { metricsArray.clear(); size_t index; @@ -231,9 +266,37 @@ bool extractPreJumpTermMetrics(const OptimalControlProblem& ocp, const std::stri /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -bool extractIntermediateTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollTraj, - std::vector<LagrangianMetricsConstRef>& metricsTrajectory) { +bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const std::string& name, + const std::vector<MetricsCollection>& metricsCollTraj, + std::vector<std::reference_wrapper<const vector_t>>& metricsTrajectory) { + metricsTrajectory.clear(); + + size_t index; + if (ocp.equalityConstraintPtr->getTermIndex(name, index)) { + metricsTrajectory.reserve(metricsCollTraj.size()); + for (const auto& metricsColl : metricsCollTraj) { + metricsTrajectory.emplace_back(metricsColl.stateInputEqConstraint[index]); + } + return true; + + } else if (ocp.stateEqualityConstraintPtr->getTermIndex(name, index)) { + metricsTrajectory.reserve(metricsCollTraj.size()); + for (const auto& metricsColl : metricsCollTraj) { + metricsTrajectory.emplace_back(metricsColl.stateEqConstraint[index]); + } + return true; + + } else { + return false; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool extractIntermediateTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const std::vector<MetricsCollection>& metricsCollTraj, + std::vector<LagrangianMetricsConstRef>& metricsTrajectory) { metricsTrajectory.clear(); size_t index; diff --git a/ocs2_oc/src/oc_solver/SolverBase.cpp b/ocs2_oc/src/oc_solver/SolverBase.cpp index 3d22ddcf6..6c07ac091 100644 --- a/ocs2_oc/src/oc_solver/SolverBase.cpp +++ b/ocs2_oc/src/oc_solver/SolverBase.cpp @@ -102,13 +102,14 @@ void SolverBase::preRun(scalar_t initTime, const vector_t& initState, scalar_t f /******************************************************************************************************/ /******************************************************************************************************/ void SolverBase::postRun() { - if (!synchronizedModules_.empty() || !augmentedLagrangianObservers_.empty()) { + if (!synchronizedModules_.empty() || !solverObservers_.empty()) { const auto solution = primalSolution(getFinalTime()); for (auto& module : synchronizedModules_) { module->postSolverRun(solution); } - for (auto& observer : augmentedLagrangianObservers_) { - observer->extractTermMetrics(getOptimalControlProblem(), solution, getSolutionMetrics()); + for (auto& observer : solverObservers_) { + observer->extractTermConstraint(getOptimalControlProblem(), solution, getSolutionMetrics()); + observer->extractTermLagrangianMetrics(getOptimalControlProblem(), solution, getSolutionMetrics()); observer->extractTermMultipliers(getOptimalControlProblem(), getDualSolution()); } } diff --git a/ocs2_oc/src/synchronized_module/AugmentedLagrangianObserver.cpp b/ocs2_oc/src/synchronized_module/AugmentedLagrangianObserver.cpp deleted file mode 100644 index bb7c00404..000000000 --- a/ocs2_oc/src/synchronized_module/AugmentedLagrangianObserver.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#include "ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h" - -#include "ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h" - -namespace ocs2 { - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void AugmentedLagrangianObserver::extractTermMetrics(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, - const ProblemMetrics& problemMetrics) { - // search intermediate - if (extractIntermediateTermMetrics(ocp, termName_, problemMetrics.intermediates, termMetricsArray_)) { - if (!primalSolution.timeTrajectory_.empty() && metricsCallback_ != nullptr) { - metricsCallback_(primalSolution.timeTrajectory_, termMetricsArray_); - } - } - - // search pre_jump - else if (extractPreJumpTermMetrics(ocp, termName_, problemMetrics.preJumps, termMetricsArray_)) { - if (!primalSolution.postEventIndices_.empty() && metricsCallback_ != nullptr) { - scalar_array_t timeArray(primalSolution.postEventIndices_.size()); - std::transform(primalSolution.postEventIndices_.cbegin(), primalSolution.postEventIndices_.cend(), timeArray.begin(), - [&](size_t postInd) -> scalar_t { return primalSolution.timeTrajectory_[postInd - 1]; }); - metricsCallback_(timeArray, termMetricsArray_); - } - } - - // search final - else { - const LagrangianMetrics* metricsPtr = extractFinalTermMetrics(ocp, termName_, problemMetrics.final); - if (metricsPtr != nullptr) { - if (!primalSolution.timeTrajectory_.empty() && metricsCallback_ != nullptr) { - const scalar_array_t timeArray{primalSolution.timeTrajectory_.back()}; - termMetricsArray_.push_back(*metricsPtr); - metricsCallback_(timeArray, termMetricsArray_); - } - - } else { - throw std::runtime_error("[AugmentedLagrangianObserver::extractTermsMetrics] Term (" + termName_ + - ") does not exist in the Lagrangian collections!"); - } - } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void AugmentedLagrangianObserver::extractTermMultipliers(const OptimalControlProblem& ocp, const DualSolution& dualSolution) { - // search intermediate - if (extractIntermediateTermMultiplier(ocp, termName_, dualSolution.intermediates, termMultiplierArray_)) { - if (!dualSolution.timeTrajectory.empty() && multiplierCallback_ != nullptr) { - multiplierCallback_(dualSolution.timeTrajectory, termMultiplierArray_); - } - } - - // search pre_jump - else if (extractPreJumpTermMultiplier(ocp, termName_, dualSolution.preJumps, termMultiplierArray_)) { - if (!dualSolution.postEventIndices.empty() && multiplierCallback_ != nullptr) { - scalar_array_t timeArray(dualSolution.postEventIndices.size()); - std::transform(dualSolution.postEventIndices.cbegin(), dualSolution.postEventIndices.cend(), timeArray.begin(), - [&](size_t postInd) -> scalar_t { return dualSolution.timeTrajectory[postInd - 1]; }); - multiplierCallback_(timeArray, termMultiplierArray_); - } - } - - // search final - else { - const Multiplier* multiplierPtr = extractFinalTermMultiplier(ocp, termName_, dualSolution.final); - if (multiplierPtr != nullptr) { - if (!dualSolution.timeTrajectory.empty() && multiplierCallback_ != nullptr) { - const scalar_array_t timeArray{dualSolution.timeTrajectory.back()}; - termMultiplierArray_.push_back(*multiplierPtr); - multiplierCallback_(timeArray, termMultiplierArray_); - } - - } else { - throw std::runtime_error("[AugmentedLagrangianObserver::extractTermMultipliers] Term (" + termName_ + - ") does not exist in the Lagrangian collections!"); - } - } -} - -} // namespace ocs2 diff --git a/ocs2_oc/src/synchronized_module/SolverObserver.cpp b/ocs2_oc/src/synchronized_module/SolverObserver.cpp new file mode 100644 index 000000000..bbb11dce6 --- /dev/null +++ b/ocs2_oc/src/synchronized_module/SolverObserver.cpp @@ -0,0 +1,200 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_oc/synchronized_module/SolverObserver.h" + +#include "ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h" + +namespace ocs2 { + +namespace { +std::string toString(SolverObserver::Type type) { + switch (type) { + case SolverObserver::Type::Final: + return "Final"; + case SolverObserver::Type::PreJump: + return "PreJump"; + case SolverObserver::Type::Intermediate: + return "Intermediate"; + default: + throw std::runtime_error("[SolverObserver::toString] undefined type!"); + } +} +} // unnamed namespace + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void SolverObserver::extractTermConstraint(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, + const ProblemMetrics& problemMetrics) { + if (!constraintCallback_ || primalSolution.timeTrajectory_.empty()) { + return; + } + + bool termIsFound = true; + switch (type_) { + case Type::Final: { + const auto* termConstraintPtr = extractFinalTermConstraint(ocp, termName_, problemMetrics.final); + termIsFound = termConstraintPtr != nullptr; + if (termIsFound) { + const scalar_array_t timeArray{primalSolution.timeTrajectory_.back()}; + const std::vector<std::reference_wrapper<const vector_t>> termConstraintArray{*termConstraintPtr}; + constraintCallback_(timeArray, termConstraintArray); + } + break; + } + case Type::PreJump: { + std::vector<std::reference_wrapper<const vector_t>> termConstraintArray; + termIsFound = extractPreJumpTermConstraint(ocp, termName_, problemMetrics.preJumps, termConstraintArray); + if (termIsFound) { + scalar_array_t timeArray(primalSolution.postEventIndices_.size()); + std::transform(primalSolution.postEventIndices_.cbegin(), primalSolution.postEventIndices_.cend(), timeArray.begin(), + [&](size_t postInd) -> scalar_t { return primalSolution.timeTrajectory_[postInd - 1]; }); + constraintCallback_(timeArray, termConstraintArray); + } + break; + } + case Type::Intermediate: { + std::vector<std::reference_wrapper<const vector_t>> termConstraintArray; + termIsFound = extractIntermediateTermConstraint(ocp, termName_, problemMetrics.intermediates, termConstraintArray); + if (termIsFound) { + constraintCallback_(primalSolution.timeTrajectory_, termConstraintArray); + } + break; + } + default: + throw std::runtime_error("[SolverObserver::extractTermConstraint] undefined type!"); + } + + if (!termIsFound) { + throw std::runtime_error("[SolverObserver::extractTermConstraint] term (" + termName_ + ") does not exist in " + toString(type_) + + "-time constraint collections!"); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void SolverObserver::extractTermLagrangianMetrics(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, + const ProblemMetrics& problemMetrics) { + if (!lagrangianCallback_ || primalSolution.timeTrajectory_.empty()) { + return; + } + + bool termIsFound = true; + switch (type_) { + case Type::Final: { + const auto* lagrangianMetricsPtr = extractFinalTermLagrangianMetrics(ocp, termName_, problemMetrics.final); + termIsFound = lagrangianMetricsPtr != nullptr; + if (termIsFound) { + const scalar_array_t timeArray{primalSolution.timeTrajectory_.back()}; + const std::vector<LagrangianMetricsConstRef> termLagrangianMetricsArray{*lagrangianMetricsPtr}; + lagrangianCallback_(timeArray, termLagrangianMetricsArray); + } + break; + } + case Type::PreJump: { + std::vector<LagrangianMetricsConstRef> termLagrangianMetricsArray; + termIsFound = extractPreJumpTermLagrangianMetrics(ocp, termName_, problemMetrics.preJumps, termLagrangianMetricsArray); + if (termIsFound) { + scalar_array_t timeArray(primalSolution.postEventIndices_.size()); + std::transform(primalSolution.postEventIndices_.cbegin(), primalSolution.postEventIndices_.cend(), timeArray.begin(), + [&](size_t postInd) -> scalar_t { return primalSolution.timeTrajectory_[postInd - 1]; }); + lagrangianCallback_(timeArray, termLagrangianMetricsArray); + } + break; + } + case Type::Intermediate: { + std::vector<LagrangianMetricsConstRef> termLagrangianMetricsArray; + termIsFound = extractIntermediateTermLagrangianMetrics(ocp, termName_, problemMetrics.intermediates, termLagrangianMetricsArray); + if (termIsFound) { + lagrangianCallback_(primalSolution.timeTrajectory_, termLagrangianMetricsArray); + } + break; + } + default: + throw std::runtime_error("[SolverObserver::extractTermLagrangianMetrics] undefined type!"); + } + + if (!termIsFound) { + throw std::runtime_error("[SolverObserver::extractTermLagrangianMetrics] term (" + termName_ + ") does not exist in " + + toString(type_) + "-time Lagrangian collections!"); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void SolverObserver::extractTermMultipliers(const OptimalControlProblem& ocp, const DualSolution& dualSolution) { + if (!multiplierCallback_ || dualSolution.timeTrajectory.empty()) { + return; + } + + bool termIsFound = true; + switch (type_) { + case Type::Final: { + const auto* multiplierPtr = extractFinalTermMultiplier(ocp, termName_, dualSolution.final); + termIsFound = multiplierPtr != nullptr; + if (termIsFound) { + const scalar_array_t timeArray{dualSolution.timeTrajectory.back()}; + const std::vector<MultiplierConstRef> termMultiplierArray{*multiplierPtr}; + multiplierCallback_(timeArray, termMultiplierArray); + } + break; + } + case Type::PreJump: { + std::vector<MultiplierConstRef> termMultiplierArray; + termIsFound = extractPreJumpTermMultiplier(ocp, termName_, dualSolution.preJumps, termMultiplierArray); + if (termIsFound) { + scalar_array_t timeArray(dualSolution.postEventIndices.size()); + std::transform(dualSolution.postEventIndices.cbegin(), dualSolution.postEventIndices.cend(), timeArray.begin(), + [&](size_t postInd) -> scalar_t { return dualSolution.timeTrajectory[postInd - 1]; }); + multiplierCallback_(timeArray, termMultiplierArray); + } + break; + } + case Type::Intermediate: { + std::vector<MultiplierConstRef> termMultiplierArray; + termIsFound = extractIntermediateTermMultiplier(ocp, termName_, dualSolution.intermediates, termMultiplierArray); + if (termIsFound) { + multiplierCallback_(dualSolution.timeTrajectory, termMultiplierArray); + } + break; + } + default: + throw std::runtime_error("[SolverObserver::extractTermMultipliers] undefined type!"); + } + + if (!termIsFound) { + throw std::runtime_error("[SolverObserver::extractTermMultipliers] term (" + termName_ + ") does not exist in " + toString(type_) + + "-time Lagrangian collections!"); + } +} + +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp b/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp index 98ecb4aaa..f6bbc2480 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp @@ -39,7 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/penalties/Penalties.h> #include <ocs2_ddp/ILQR.h> #include <ocs2_ddp/SLQ.h> -#include <ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h> +#include <ocs2_oc/synchronized_module/SolverObserver.h> #include "ocs2_cartpole/CartPoleInterface.h" #include "ocs2_cartpole/package_path.h" @@ -199,12 +199,12 @@ TEST_P(TestCartpole, testDDP) { ddpPtr->getReferenceManager().setTargetTrajectories(initTargetTrajectories); // observer for InputLimits violation - std::unique_ptr<AugmentedLagrangianObserver> inputLimitsObserverModulePtr(new AugmentedLagrangianObserver("InputLimits")); - inputLimitsObserverModulePtr->setMetricsCallback( + auto inputLimitsObserverModulePtr = SolverObserver::LagrangianTermObserver( + SolverObserver::Type::Intermediate, "InputLimits", [&](const scalar_array_t& timeTrajectory, const std::vector<LagrangianMetricsConstRef>& termMetrics) { testInputLimitsViolation(timeTrajectory, termMetrics); }); - ddpPtr->addAugmentedLagrangianObserver(std::move(inputLimitsObserverModulePtr)); + ddpPtr->addSolverObserver(std::move(inputLimitsObserverModulePtr)); // run solver ddpPtr->run(0.0, cartPoleInterfacePtr->getInitialState(), timeHorizon); diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp index 8cb9e1fd1..1b25390e1 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp @@ -35,7 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_cartpole/CartPoleInterface.h> #include <ocs2_ddp/GaussNewtonDDP_MPC.h> #include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> -#include <ocs2_ros_interfaces/synchronized_module/RosAugmentedLagrangianCallbacks.h> +#include <ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h> int main(int argc, char** argv) { const std::string robotName = "cartpole"; @@ -62,22 +62,24 @@ int main(int argc, char** argv) { cartPoleInterface.getOptimalControlProblem(), cartPoleInterface.getInitializer()); // observer for the input limits constraints - const std::string observingLagrangianTerm = "InputLimits"; - const ocs2::scalar_array_t observingTimePoints{0.0, 0.5}; - std::vector<std::string> metricsTopicNames; - std::vector<std::string> multiplierTopicNames; - for (const auto& t : observingTimePoints) { - const int timeMs = static_cast<int>(t * 1000.0); - metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); - multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); - } - std::unique_ptr<ocs2::AugmentedLagrangianObserver> stateInputBoundsObserverPtr( - new ocs2::AugmentedLagrangianObserver(observingLagrangianTerm)); - stateInputBoundsObserverPtr->setMetricsCallback(ocs2::ros::createMetricsCallback( - nodeHandle, observingTimePoints, metricsTopicNames, ocs2::ros::CallbackInterpolationStrategy::linear_interpolation)); - stateInputBoundsObserverPtr->setMultiplierCallback(ocs2::ros::createMultiplierCallback( - nodeHandle, observingTimePoints, multiplierTopicNames, ocs2::ros::CallbackInterpolationStrategy::linear_interpolation)); - mpc.getSolverPtr()->addAugmentedLagrangianObserver(std::move(stateInputBoundsObserverPtr)); + auto createStateInputBoundsObserver = [&]() { + const std::string observingLagrangianTerm = "InputLimits"; + const ocs2::scalar_array_t observingTimePoints{0.0, 0.5}; + std::vector<std::string> metricsTopicNames; + std::vector<std::string> multiplierTopicNames; + for (const auto& t : observingTimePoints) { + const int timeMs = static_cast<int>(t * 1000.0); + metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); + multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); + } + auto lagrangianCallback = ocs2::ros::createLagrangiancallback(nodeHandle, observingTimePoints, metricsTopicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + auto multiplierCallback = ocs2::ros::createMultiplierCallback(nodeHandle, observingTimePoints, multiplierTopicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::LagrangianTermObserver(ocs2::SolverObserver::Type::Intermediate, observingLagrangianTerm, + std::move(lagrangianCallback), std::move(multiplierCallback)); + }; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver()); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); diff --git a/ocs2_ros_interfaces/CMakeLists.txt b/ocs2_ros_interfaces/CMakeLists.txt index 30895fc29..99d3cc8fc 100644 --- a/ocs2_ros_interfaces/CMakeLists.txt +++ b/ocs2_ros_interfaces/CMakeLists.txt @@ -64,7 +64,7 @@ add_library(${PROJECT_NAME} src/mrt/MRT_ROS_Dummy_Loop.cpp src/mrt/MRT_ROS_Interface.cpp src/synchronized_module/RosReferenceManager.cpp - src/synchronized_module/RosAugmentedLagrangianCallbacks.cpp + src/synchronized_module/SolverObserverRosCallbacks.cpp src/visualization/VisualizationHelpers.cpp src/visualization/VisualizationColors.cpp ) diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosAugmentedLagrangianCallbacks.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h similarity index 88% rename from ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosAugmentedLagrangianCallbacks.h rename to ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h index 7da2f92bd..f5a21d51a 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosAugmentedLagrangianCallbacks.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h @@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ros/ros.h> -#include <ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h> +#include <ocs2_oc/synchronized_module/SolverObserver.h> namespace ocs2 { namespace ros { @@ -42,7 +42,7 @@ enum class CallbackInterpolationStrategy { }; /** - * Creates a ROS-based callback for AugmentedLagrangianObserver that publishes a term's LagrangianMetrics at the + * Creates a ROS-based callback for SolverObserver that publishes a term's LagrangianMetrics at the * requested lookahead time points. * * @param [in] nodeHandle: ROS node handle. @@ -51,12 +51,12 @@ enum class CallbackInterpolationStrategy { * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. * @return A callback which can be set to SolverObserverModule in order to observe a requested term's LagrangianMetrics. */ -AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback( +SolverObserver::lagrangian_callback_t createLagrangiancallback( ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector<std::string>& topicNames, CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); /** - * Creates a ROS-based callback for AugmentedLagrangianObserver that publishes a term's Lagrange multiplier at the + * Creates a ROS-based callback for SolverObserver that publishes a term's Lagrange multiplier at the * requested lookahead time points. * * @param [in] nodeHandle: ROS node handle. @@ -65,7 +65,7 @@ AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback( * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. * @return A callback which can be set to SolverObserverModule in order to observe a requested term's multiplier. */ -AugmentedLagrangianObserver::multiplier_callback_t createMultiplierCallback( +SolverObserver::multiplier_callback_t createMultiplierCallback( ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector<std::string>& topicNames, CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); diff --git a/ocs2_ros_interfaces/src/lintTarget.cpp b/ocs2_ros_interfaces/src/lintTarget.cpp index b92babbb4..c816d2a99 100644 --- a/ocs2_ros_interfaces/src/lintTarget.cpp +++ b/ocs2_ros_interfaces/src/lintTarget.cpp @@ -17,6 +17,7 @@ // synchronized_module #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> +#include <ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h> // dummy target for clang toolchain int main() { diff --git a/ocs2_ros_interfaces/src/synchronized_module/RosAugmentedLagrangianCallbacks.cpp b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp similarity index 79% rename from ocs2_ros_interfaces/src/synchronized_module/RosAugmentedLagrangianCallbacks.cpp rename to ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp index e1e12907d..8c2f9931f 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/RosAugmentedLagrangianCallbacks.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_ros_interfaces/synchronized_module/RosAugmentedLagrangianCallbacks.h" +#include "ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h" #include "ocs2_core/misc/LinearInterpolation.h" #include "ocs2_ros_interfaces/common/RosMsgConversions.h" @@ -38,12 +38,11 @@ namespace ros { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback(::ros::NodeHandle& nodeHandle, - const scalar_array_t& observingTimePoints, - const std::vector<std::string>& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::lagrangian_callback_t createLagrangiancallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, + const std::vector<std::string>& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createMetricsCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error("[createLagrangiancallback] For each observing time points, you should provide a unique topic name!"); } std::vector<::ros::Publisher> metricsPublishers; @@ -67,7 +66,7 @@ AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback(::ros::Nod case CallbackInterpolationStrategy::linear_interpolation: return LinearInterpolation::interpolate(indexAlpha, termMetricsArray); default: - throw std::runtime_error("[createMetricsCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error("[createLagrangiancallback] This CallbackInterpolationStrategy is not implemented!"); } }(); metricsPublishers[i].publish(ros_msg_conversions::createMetricsMsg(t, metrics)); @@ -79,12 +78,11 @@ AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback(::ros::Nod /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -AugmentedLagrangianObserver::multiplier_callback_t createMultiplierCallback(::ros::NodeHandle& nodeHandle, - const scalar_array_t& observingTimePoints, - const std::vector<std::string>& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::multiplier_callback_t createMultiplierCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, + const std::vector<std::string>& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createMetricsCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error("[createMultiplierCallback] For each observing time points, you should provide a unique topic name!"); } std::vector<::ros::Publisher> multiplierPublishers; @@ -108,7 +106,7 @@ AugmentedLagrangianObserver::multiplier_callback_t createMultiplierCallback(::ro case CallbackInterpolationStrategy::linear_interpolation: return LinearInterpolation::interpolate(indexAlpha, termMultiplierArray); default: - throw std::runtime_error("[createMetricsCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error("[createMultiplierCallback] This CallbackInterpolationStrategy is not implemented!"); } }(); multiplierPublishers[i].publish(ros_msg_conversions::createMultiplierMsg(t, multiplier)); From 42e3a822d7996b44a419869b834d58111f55cd24 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 4 Aug 2022 17:13:11 +0200 Subject: [PATCH 255/512] fixing ddp unittest --- ocs2_ddp/test/HybridSlqTest.cpp | 30 +++++++++++++------------ ocs2_ddp/test/testDdpHelperFunction.cpp | 2 +- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/ocs2_ddp/test/HybridSlqTest.cpp b/ocs2_ddp/test/HybridSlqTest.cpp index 68606a90f..90a41e85a 100644 --- a/ocs2_ddp/test/HybridSlqTest.cpp +++ b/ocs2_ddp/test/HybridSlqTest.cpp @@ -33,8 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <iostream> #include <ocs2_oc/rollout/StateTriggeredRollout.h> -#include <ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> +#include <ocs2_oc/synchronized_module/SolverObserver.h> #include <ocs2_oc/test/dynamics_hybrid_slq_test.h> #include <ocs2_core/augmented_lagrangian/AugmentedLagrangian.h> @@ -123,7 +123,8 @@ TEST(HybridSlqTest, state_rollout_slq) { problem.costPtr->add("cost", std::move(cost)); problem.preJumpCostPtr->add("preJumpCost", std::move(preJumpCost)); problem.finalCostPtr->add("finalCost", std::move(finalCost)); - problem.inequalityLagrangianPtr->add("bounds", create(std::move(boundsConstraints), augmented::SlacknessSquaredHingePenalty::create({200.0, 0.1}))); + problem.inequalityLagrangianPtr->add("bounds", + create(std::move(boundsConstraints), augmented::SlacknessSquaredHingePenalty::create({200.0, 0.1}))); const vector_t xNominal = vector_t::Zero(STATE_DIM); const vector_t uNominal = vector_t::Zero(INPUT_DIM); @@ -136,22 +137,23 @@ TEST(HybridSlqTest, state_rollout_slq) { OperatingPoints operatingTrajectories(stateOperatingPoint, inputOperatingPoint); // Test 1: Check constraint compliance. It uses a solver observer to get metrics for the bounds constraints - std::unique_ptr<AugmentedLagrangianObserver> boundsConstraintsObserverPtr(new AugmentedLagrangianObserver("bounds")); - boundsConstraintsObserverPtr->setMetricsCallback([&](const scalar_array_t& timeTraj, const std::vector<LagrangianMetricsConstRef>& metricsTraj) { - constexpr scalar_t constraintViolationTolerance = 1e-1; - for (size_t i = 0; i < metricsTraj.size(); i++) { - const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); - EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(1), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(2), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(3), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - } - }); + auto boundsConstraintsObserverPtr = SolverObserver::LagrangianTermObserver( + SolverObserver::Type::Intermediate, "bounds", + [&](const scalar_array_t& timeTraj, const std::vector<LagrangianMetricsConstRef>& metricsTraj) { + constexpr scalar_t constraintViolationTolerance = 1e-1; + for (size_t i = 0; i < metricsTraj.size(); i++) { + const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); + EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(1), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(2), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(3), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + } + }); // setup SLQ SLQ slq(ddpSettings, stateTriggeredRollout, problem, operatingTrajectories); slq.setReferenceManager(referenceManager); - slq.addAugmentedLagrangianObserver(std::move(boundsConstraintsObserverPtr)); + slq.addSolverObserver(std::move(boundsConstraintsObserverPtr)); // run SLQ slq.run(startTime, initState, finalTime); diff --git a/ocs2_ddp/test/testDdpHelperFunction.cpp b/ocs2_ddp/test/testDdpHelperFunction.cpp index a779c5352..06c7ed6b0 100644 --- a/ocs2_ddp/test/testDdpHelperFunction.cpp +++ b/ocs2_ddp/test/testDdpHelperFunction.cpp @@ -57,7 +57,7 @@ TEST(extractPrimalSolution, eventAtInitTime) { primalSolution.postEventIndices_.push_back(primalSolution.timeTrajectory_.size()); primalSolution.modeSchedule_.modeSequence.push_back(s + 1); primalSolution.modeSchedule_.eventTimes.push_back(primalSolution.timeTrajectory_.back()); - } // end of s + } // end of s // an extra event at final time primalSolution.timeTrajectory_.push_back(primalSolution.timeTrajectory_.back()); From ed3d3543cab57febd604e41c76513a83cc25b5a2 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Thu, 4 Aug 2022 18:15:01 +0200 Subject: [PATCH 256/512] adapt to removal of debug caching flag --- ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp | 1 - .../ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index f76f23a34..dfe15a9a5 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -97,7 +97,6 @@ std::unique_ptr<MPC_BASE> BallbotMpcnetInterface::getMpc(BallbotInterface& ballb settings.displayShortSummary_ = false; settings.checkNumericalStability_ = false; settings.debugPrintRollout_ = false; - settings.debugCaching_ = false; settings.useFeedbackPolicy_ = true; return settings; }(); diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 20bb7a245..6c068bca9 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -128,7 +128,6 @@ std::unique_ptr<MPC_BASE> LeggedRobotMpcnetInterface::getMpc(LeggedRobotInterfac settings.displayShortSummary_ = false; settings.checkNumericalStability_ = false; settings.debugPrintRollout_ = false; - settings.debugCaching_ = false; settings.useFeedbackPolicy_ = true; return settings; }(); From 2fdb6cce51a339ab79d8e9436f39481917310463 Mon Sep 17 00:00:00 2001 From: Ruben Grandia <rubengrandia@gmail.com> Date: Fri, 5 Aug 2022 09:55:49 +0200 Subject: [PATCH 257/512] Update ros-build-test.yml check for test results --- .github/workflows/ros-build-test.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 8816680ca..c1c80c0b6 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -67,3 +67,4 @@ jobs: run: | source devel_isolated/setup.bash catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests + catkin_test_results From 15714b14a7c132cf5989aada975d7adc09e47363 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 5 Aug 2022 11:43:47 +0200 Subject: [PATCH 258/512] wrong matrix size --- .../ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp index 21fdd2119..2d99c9789 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp +++ b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp @@ -81,7 +81,7 @@ void CentroidalModelRbdConversions::computeBaseKinematicsFromCentroidalModel(con const Vector3 derivativeEulerAnglesZyx = vPinocchio.segment<3>(3); baseVelocity.tail<3>() = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives<scalar_t>(baseOrientation, derivativeEulerAnglesZyx); - const Matrix6 Adot = pinocchio::dccrba(model, data, qPinocchio, vPinocchio); + const auto Adot = pinocchio::dccrba(model, data, qPinocchio, vPinocchio); Vector6 centroidalMomentumRate = info.robotMass * getNormalizedCentroidalMomentumRate(pinocchioInterface_, info, input); centroidalMomentumRate.noalias() -= Adot * vPinocchio; centroidalMomentumRate.noalias() -= Aj * jointAccelerations.head(info.actuatedDofNum); From 80092292c72490cf7b1942f316cf10237c8b4489 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 5 Aug 2022 11:48:40 +0200 Subject: [PATCH 259/512] preventing asynchronous tracking in debug mode --- .../test/DoubleIntegratorNoRosIntegrationTest.cpp | 2 ++ .../ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp index 6eaff3204..aa0d941f9 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp @@ -153,6 +153,7 @@ TEST_F(DoubleIntegratorIntegrationTest, coldStartMPC) { ASSERT_NEAR(observation.state(0), goalState(0), tolerance); } +#ifdef NDEBUG TEST_F(DoubleIntegratorIntegrationTest, asynchronousTracking) { auto mpcPtr = getMpc(true); MPC_MRT_Interface mpcInterface(*mpcPtr); @@ -207,3 +208,4 @@ TEST_F(DoubleIntegratorIntegrationTest, asynchronousTracking) { ASSERT_NEAR(observation.state(0), goalState(0), tolerance); } +#endif diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp index 9fef6927f..f2be4e0c8 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp @@ -199,6 +199,7 @@ TEST_P(DummyMobileManipulatorParametersTests, synchronousTracking) { verifyTrackingQuality(observation.state); } +#ifdef NDEBUG TEST_P(DummyMobileManipulatorParametersTests, asynchronousTracking) { // Obtain mpc initialize(getTaskFile(), getLibFolder(), getUrdfFile()); @@ -258,6 +259,7 @@ TEST_P(DummyMobileManipulatorParametersTests, asynchronousTracking) { verifyTrackingQuality(observation.state); } +#endif /******************************************************************************************************/ /******************************************************************************************************/ From 9a5062fdc4fc60a8ec5e59640780d8ca627472c1 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 5 Aug 2022 12:34:07 +0200 Subject: [PATCH 260/512] set position error gain to zero --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index c1cf596d7..b7eae9ae6 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -9,7 +9,7 @@ legged_robot_interface model_settings { - positionErrorGain 20.0 + positionErrorGain 0.0 ; 20.0 phaseTransitionStanceTime 0.4 verboseCppAd true From 6f54a1a704292d76c7aeba7f35fba8319530f52a Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 5 Aug 2022 12:35:45 +0200 Subject: [PATCH 261/512] enable gradient clipping for legged robot mpcnet --- .../python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml index bfabae6ad..0b6dde905 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml @@ -236,5 +236,5 @@ POLICY_EVALUATION_TASKS: 3 BATCH_SIZE: 128 LEARNING_RATE: 1.e-3 LEARNING_ITERATIONS: 100000 -GRADIENT_CLIPPING: False +GRADIENT_CLIPPING: True GRADIENT_CLIPPING_VALUE: 1.0 From ab24711b9fc77dd06beecdc1d44c2bae3799b63b Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 5 Aug 2022 14:30:54 +0200 Subject: [PATCH 262/512] adding ocs2_mpcnet to ocs2 Metapackage --- ocs2/package.xml | 1 + ocs2_mpcnet/ocs2_mpcnet/package.xml | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ocs2/package.xml b/ocs2/package.xml index 834b4090e..c088f80f2 100644 --- a/ocs2/package.xml +++ b/ocs2/package.xml @@ -24,6 +24,7 @@ <exec_depend>ocs2_robotic_examples</exec_depend> <exec_depend>ocs2_thirdparty</exec_depend> <exec_depend>ocs2_raisim</exec_depend> + <exec_depend>ocs2_mpcnet</exec_depend> <export> <metapackage /> diff --git a/ocs2_mpcnet/ocs2_mpcnet/package.xml b/ocs2_mpcnet/ocs2_mpcnet/package.xml index 7cb3afff8..7eec28861 100644 --- a/ocs2_mpcnet/ocs2_mpcnet/package.xml +++ b/ocs2_mpcnet/ocs2_mpcnet/package.xml @@ -13,9 +13,9 @@ <buildtool_depend>catkin</buildtool_depend> + <exec_depend>ocs2_mpcnet_core</exec_depend> <exec_depend>ocs2_ballbot_mpcnet</exec_depend> <exec_depend>ocs2_legged_robot_mpcnet</exec_depend> - <exec_depend>ocs2_mpcnet_core</exec_depend> <export> <metapackage /> From 7dd7a76f8c39c71b441847bd9366fc6f4156a9d0 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 5 Aug 2022 14:48:15 +0200 Subject: [PATCH 263/512] bugfix --- ocs2_core/src/Types.cpp | 2 ++ ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h | 6 +++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/ocs2_core/src/Types.cpp b/ocs2_core/src/Types.cpp index c6d4e4583..3645fa031 100644 --- a/ocs2_core/src/Types.cpp +++ b/ocs2_core/src/Types.cpp @@ -446,6 +446,8 @@ VectorFunctionQuadraticApproximation& VectorFunctionQuadraticApproximation::setZ } } else { dfdu = matrix_t(); + dfdux.resize(nv); + dfduu.resize(nv); for (size_t i = 0; i < nv; i++) { dfdux[i] = matrix_t(); dfduu[i] = matrix_t(); diff --git a/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h b/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h index d9e69116f..a4922459a 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h +++ b/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h @@ -48,7 +48,7 @@ inline ScalarFunctionQuadraticApproximation getRandomCost(int n, int m) { ScalarFunctionQuadraticApproximation cost; cost.dfdxx = QPPR.topLeftCorner(n, n); cost.dfdx = vector_t::Random(n); - if (m > 0) { + if (m >= 0) { cost.dfdux = QPPR.bottomLeftCorner(m, n); cost.dfduu = QPPR.bottomRightCorner(m, m); cost.dfdu = vector_t::Random(m); @@ -69,7 +69,7 @@ inline std::unique_ptr<ocs2::StateCost> getOcs2StateCost(const ScalarFunctionQua inline VectorFunctionLinearApproximation getRandomDynamics(int n, int m) { VectorFunctionLinearApproximation dynamics; dynamics.dfdx = matrix_t::Random(n, n); - if (m > 0) { + if (m >= 0) { dynamics.dfdu = matrix_t::Random(n, m); } dynamics.f = vector_t::Random(n); @@ -84,7 +84,7 @@ inline std::unique_ptr<ocs2::LinearSystemDynamics> getOcs2Dynamics(const VectorF inline VectorFunctionLinearApproximation getRandomConstraints(int n, int m, int nc) { VectorFunctionLinearApproximation constraints; constraints.dfdx = matrix_t::Random(nc, n); - if (m > 0) { + if (m >= 0) { constraints.dfdu = matrix_t::Random(nc, m); } constraints.f = vector_t::Random(nc); From c009e386adb554a2a6974821a39962d8feb50b5d Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 5 Aug 2022 15:23:00 +0200 Subject: [PATCH 264/512] add onnxruntime to build --- .github/workflows/ros-build-test.yml | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 8816680ca..8d3116c44 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -56,6 +56,15 @@ jobs: cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") make -j4 && checkinstall + - name: Install ONNX Runtime + run: | + wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P src/microsoft + cd src/microsoft + tar xf onnxruntime-linux-x64-1.7.0.tgz + rsync -a onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime + rsync -a onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib + rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/cmake/onnxruntime + - name: Build (${{ matrix.build_type }}) shell: bash run: | From e1a186a0440e28d293552b20b5e33f37281ba96e Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 5 Aug 2022 15:24:48 +0200 Subject: [PATCH 265/512] removing ddp dependencies --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 1 - ocs2_sqp/ocs2_sqp/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 7af8598c8..237b8a4c4 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -3,7 +3,6 @@ project(ocs2_sqp) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_core - ocs2_ddp ocs2_mpc ocs2_oc ocs2_qp_solver diff --git a/ocs2_sqp/ocs2_sqp/package.xml b/ocs2_sqp/ocs2_sqp/package.xml index 132839f88..9402df863 100644 --- a/ocs2_sqp/ocs2_sqp/package.xml +++ b/ocs2_sqp/ocs2_sqp/package.xml @@ -10,7 +10,6 @@ <buildtool_depend>catkin</buildtool_depend> <depend>ocs2_core</depend> - <depend>ocs2_ddp</depend> <depend>ocs2_mpc</depend> <depend>ocs2_oc</depend> <depend>ocs2_qp_solver</depend> From 76be2f3a042fcbcfea614572905c7619c4104a42 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 5 Aug 2022 15:45:46 +0200 Subject: [PATCH 266/512] update system deps and donload to tmp --- .github/workflows/ros-build-test.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 8d3116c44..9ebf38a0c 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -30,7 +30,7 @@ jobs: - name: System deps run: | apt-get update - apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall + apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync - uses: actions/checkout@v2 with: @@ -58,12 +58,12 @@ jobs: - name: Install ONNX Runtime run: | - wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P src/microsoft - cd src/microsoft + cd /tmp + wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz tar xf onnxruntime-linux-x64-1.7.0.tgz - rsync -a onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime - rsync -a onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib - rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/cmake/onnxruntime + rsync -a /tmp/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime + rsync -a /tmp/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib + rsync -a /src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/cmake/onnxruntime - name: Build (${{ matrix.build_type }}) shell: bash From 8af3701fff5e5efbe173a1c3d66af7d6c4d3d4e7 Mon Sep 17 00:00:00 2001 From: Alexander Reske <areske@leggedrobotics.com> Date: Fri, 5 Aug 2022 16:05:27 +0200 Subject: [PATCH 267/512] wget and tar into local folder --- .github/workflows/ros-build-test.yml | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 9ebf38a0c..cd01f7e43 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -58,12 +58,11 @@ jobs: - name: Install ONNX Runtime run: | - cd /tmp - wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz - tar xf onnxruntime-linux-x64-1.7.0.tgz - rsync -a /tmp/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime - rsync -a /tmp/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib - rsync -a /src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/cmake/onnxruntime + wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P tmp/microsoft + tar xf tmp/microsoft/onnxruntime-linux-x64-1.7.0.tgz -C tmp/microsoft + rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime + rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib + rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime - name: Build (${{ matrix.build_type }}) shell: bash From 6c243a7985c3729e7282fd87ba572f9f8a9d6ed1 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 8 Aug 2022 10:38:57 +0200 Subject: [PATCH 268/512] adding ROS publisher for solver constraint and an exmaple in ocs2_legged_robot --- ocs2_msgs/CMakeLists.txt | 1 + ocs2_msgs/msg/constraint.msg | 4 + .../ocs2_cartpole_ros/src/CartpoleMpcNode.cpp | 2 +- .../config/multiplot/mpc_metrics.xml | 720 ++++++++++++++++++ .../ocs2_legged_robot_ros/gait/GaitReceiver.h | 5 +- .../launch/legged_robot_ddp.launch | 2 +- .../launch/multiplot.launch | 6 + .../src/LeggedRobotDdpMpcNode.cpp | 18 +- .../src/gait/GaitReceiver.cpp | 2 +- .../common/RosMsgConversions.h | 6 +- .../SolverObserverRosCallbacks.h | 15 +- .../src/common/RosMsgConversions.cpp | 17 +- .../SolverObserverRosCallbacks.cpp | 60 +- 13 files changed, 840 insertions(+), 18 deletions(-) create mode 100644 ocs2_msgs/msg/constraint.msg create mode 100644 ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/mpc_metrics.xml diff --git a/ocs2_msgs/CMakeLists.txt b/ocs2_msgs/CMakeLists.txt index d36f92f81..4aed95b53 100644 --- a/ocs2_msgs/CMakeLists.txt +++ b/ocs2_msgs/CMakeLists.txt @@ -19,6 +19,7 @@ add_message_files( mpc_flattened_controller.msg lagrangian_metrics.msg multiplier.msg + constraint.msg ) add_service_files( diff --git a/ocs2_msgs/msg/constraint.msg b/ocs2_msgs/msg/constraint.msg new file mode 100644 index 000000000..fe48dd804 --- /dev/null +++ b/ocs2_msgs/msg/constraint.msg @@ -0,0 +1,4 @@ +# MPC constraint + +float32 time +float32[] value \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp index 1b25390e1..6c932202c 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp @@ -72,7 +72,7 @@ int main(int argc, char** argv) { metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); } - auto lagrangianCallback = ocs2::ros::createLagrangiancallback(nodeHandle, observingTimePoints, metricsTopicNames, + auto lagrangianCallback = ocs2::ros::createLagrangianCallback(nodeHandle, observingTimePoints, metricsTopicNames, ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); auto multiplierCallback = ocs2::ros::createMultiplierCallback(nodeHandle, observingTimePoints, multiplierTopicNames, ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/mpc_metrics.xml b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/mpc_metrics.xml new file mode 100644 index 000000000..499e25910 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/mpc_metrics.xml @@ -0,0 +1,720 @@ +<?xml version="1.0" encoding="UTF-8"?> +<rqt_multiplot> + <table> + <background_color>#ffffff</background_color> + <foreground_color>#000000</foreground_color> + <link_cursor>false</link_cursor> + <link_scale>false</link_scale> + <plots> + <row_0> + <column_0> + <axes> + <axes> + <x_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </x_axis> + <y_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </y_axis> + </axes> + </axes> + <curves> + <curve_0> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/0</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>X</title> + </curve_0> + <curve_1> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/1</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Y</title> + </curve_1> + <curve_2> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/2</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Z</title> + </curve_2> + </curves> + <legend> + <visible>true</visible> + </legend> + <plot_rate>30</plot_rate> + <title>LF_FOOT_zeroVelocity</title> + </column_0> + <column_1> + <axes> + <axes> + <x_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </x_axis> + <y_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </y_axis> + </axes> + </axes> + <curves> + <curve_0> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/0</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>X</title> + </curve_0> + <curve_1> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/1</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Y</title> + </curve_1> + <curve_2> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/2</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Z</title> + </curve_2> + </curves> + <legend> + <visible>true</visible> + </legend> + <plot_rate>30</plot_rate> + <title>RF_FOOT_zeroVelocity</title> + </column_1> + </row_0> + <row_1> + <column_0> + <axes> + <axes> + <x_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </x_axis> + <y_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </y_axis> + </axes> + </axes> + <curves> + <curve_0> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/0</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>X</title> + </curve_0> + <curve_1> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/1</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Y</title> + </curve_1> + <curve_2> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/2</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Z</title> + </curve_2> + </curves> + <legend> + <visible>true</visible> + </legend> + <plot_rate>30</plot_rate> + <title>LH_FOOT_zeroVelocity</title> + </column_0> + <column_1> + <axes> + <axes> + <x_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </x_axis> + <y_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </y_axis> + </axes> + </axes> + <curves> + <curve_0> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/0</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>X</title> + </curve_0> + <curve_1> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/1</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Y</title> + </curve_1> + <curve_2> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/2</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_zeroVelocity/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Z</title> + </curve_2> + </curves> + <legend> + <visible>true</visible> + </legend> + <plot_rate>30</plot_rate> + <title>RH_FOOT_zeroVelocity</title> + </column_1> + </row_1> + </plots> + <track_points>false</track_points> + </table> +</rqt_multiplot> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h index e6327327d..d69946deb 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h @@ -43,9 +43,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace legged_robot { + class GaitReceiver : public SolverSynchronizedModule { public: - GaitReceiver(ros::NodeHandle nodeHandle, std::shared_ptr<GaitSchedule> gaitSchedulePtr, const std::string& robotName); + GaitReceiver(::ros::NodeHandle nodeHandle, std::shared_ptr<GaitSchedule> gaitSchedulePtr, const std::string& robotName); void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, const ReferenceManagerInterface& referenceManager) override; @@ -57,7 +58,7 @@ class GaitReceiver : public SolverSynchronizedModule { std::shared_ptr<GaitSchedule> gaitSchedulePtr_; - ros::Subscriber mpcModeSequenceSubscriber_; + ::ros::Subscriber mpcModeSequenceSubscriber_; std::mutex receivedGaitMutex_; std::atomic_bool gaitUpdated_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch index f9bb4c359..54827005b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch @@ -4,7 +4,7 @@ <!-- visualization config --> <arg name="rviz" default="true" /> <arg name="description_name" default="legged_robot_description"/> - <arg name="multiplot" default="false"/> + <arg name="multiplot" default="true"/> <!-- The task file for the mpc. --> <arg name="taskFile" default="$(find ocs2_legged_robot)/config/mpc/task.info"/> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch index 96fd2a252..26190f062 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch @@ -1,6 +1,12 @@ <?xml version="1.0" ?> <launch> + <arg name="metrics_config" default="$(find ocs2_legged_robot)/config/multiplot/mpc_metrics.xml" /> + + <node name="mpc_metrics" pkg="rqt_multiplot" type="rqt_multiplot" + args="--multiplot-run-all --multiplot-config $(arg metrics_config)" + output="screen"/> + <!-- Launch Performance Indices Multi-plot --> <include file="$(find ocs2_ros_interfaces)/launch/performance_indices.launch"> <arg name="mpc_policy_topic_name" value="legged_robot_mpc_policy"/> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp index b37272d69..9dec1509b 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_legged_robot/LeggedRobotInterface.h> #include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> +#include <ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h> #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" @@ -44,8 +45,8 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + ::ros::init(argc, argv, robotName + "_mpc"); + ::ros::NodeHandle nodeHandle; // Get node parameters std::string taskFile, urdfFile, referenceFile; nodeHandle.getParam("/taskFile", taskFile); @@ -69,6 +70,19 @@ int main(int argc, char** argv) { mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + // observer for zero velocity constraints + auto createStateInputBoundsObserver = [&](const std::string& termName) { + const ocs2::scalar_array_t observingTimePoints{0.0}; + const std::vector<std::string> topicNames{"metrics/" + termName + "/0MsLookAhead"}; + auto callback = + ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + }; + for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + } + // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); mpcNode.launchNodes(nodeHandle); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp index 82c452459..cd98a3b2d 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp @@ -37,7 +37,7 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GaitReceiver::GaitReceiver(ros::NodeHandle nodeHandle, std::shared_ptr<GaitSchedule> gaitSchedulePtr, const std::string& robotName) +GaitReceiver::GaitReceiver(::ros::NodeHandle nodeHandle, std::shared_ptr<GaitSchedule> gaitSchedulePtr, const std::string& robotName) : gaitSchedulePtr_(std::move(gaitSchedulePtr)), receivedGait_({0.0, 1.0}, {ModeNumber::STANCE}), gaitUpdated_(false) { mpcModeSequenceSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_mode_schedule", 1, &GaitReceiver::mpcModeSequenceCallback, this, ::ros::TransportHints().udp()); diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h index 435b2e70c..057c33e80 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h @@ -38,6 +38,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/oc_solver/PerformanceIndex.h> // MPC messages +#include <ocs2_msgs/constraint.h> #include <ocs2_msgs/lagrangian_metrics.h> #include <ocs2_msgs/mode_schedule.h> #include <ocs2_msgs/mpc_observation.h> @@ -78,8 +79,11 @@ ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime /** Reads the performance indices message. */ PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indices& performanceIndicesMsg); +/** Creates constraint message. */ +ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint); + /** Creates lagrangian_metrics message. */ -ocs2_msgs::lagrangian_metrics createMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics); +ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics); /** Creates multiplier message. */ ocs2_msgs::multiplier createMultiplierMsg(scalar_t time, MultiplierConstRef multiplier); diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h index f5a21d51a..b76dd4278 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h @@ -41,6 +41,19 @@ enum class CallbackInterpolationStrategy { linear_interpolation, }; +/** + * Creates a ROS-based callback for SolverObserver that publishes a constraint term at the requested lookahead time points. + * + * @param [in] nodeHandle: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of constraint. + * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to observe a requested term's constraint. + */ +SolverObserver::constraint_callback_t createConstraintCallback( + ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector<std::string>& topicNames, + CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + /** * Creates a ROS-based callback for SolverObserver that publishes a term's LagrangianMetrics at the * requested lookahead time points. @@ -51,7 +64,7 @@ enum class CallbackInterpolationStrategy { * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. * @return A callback which can be set to SolverObserverModule in order to observe a requested term's LagrangianMetrics. */ -SolverObserver::lagrangian_callback_t createLagrangiancallback( +SolverObserver::lagrangian_callback_t createLagrangianCallback( ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector<std::string>& topicNames, CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); diff --git a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp index c492d1c97..7c5e82bcd 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp @@ -216,7 +216,22 @@ TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_traject /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::lagrangian_metrics createMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics) { +ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint) { + ocs2_msgs::constraint constraintMsg; + + constraintMsg.time = time; + constraintMsg.value.resize(constraint.size()); + for (size_t i = 0; i < constraint.size(); i++) { + constraintMsg.value[i] = constraint(i); + } // end of i loop + + return constraintMsg; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics) { ocs2_msgs::lagrangian_metrics metricsMsg; metricsMsg.time = time; diff --git a/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp index 8c2f9931f..be5de2ad2 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp @@ -38,11 +38,55 @@ namespace ros { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -SolverObserver::lagrangian_callback_t createLagrangiancallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, +SolverObserver::constraint_callback_t createConstraintCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, + const std::vector<std::string>& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { + using vector_ref_array_t = std::vector<std::reference_wrapper<const vector_t>>; + + if (observingTimePoints.size() != topicNames.size()) { + throw std::runtime_error("[createConstraintCallback] For each observing time points, you should provide a unique topic name!"); + } + + std::vector<::ros::Publisher> constraintPublishers; + for (const auto& name : topicNames) { + constraintPublishers.push_back(nodeHandle.advertise<ocs2_msgs::constraint>(name, 1, true)); + } + + // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher + // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks + // associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, const vector_ref_array_t& termConstraintArray) { + if (!timeTrajectory.empty()) { + for (size_t i = 0; i < observingTimePoints.size(); i++) { + const auto t = timeTrajectory.front() + observingTimePoints[i]; + const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto constraint = [&]() -> vector_t { + switch (interpolationStrategy) { + case CallbackInterpolationStrategy::nearest_time: + return (indexAlpha.second > 0.5) ? termConstraintArray[indexAlpha.first].get() + : termConstraintArray[indexAlpha.first + 1].get(); + case CallbackInterpolationStrategy::linear_interpolation: + return LinearInterpolation::interpolate( + indexAlpha, termConstraintArray, + [](const vector_ref_array_t& array, size_t t) -> const vector_t& { return array[t].get(); }); + default: + throw std::runtime_error("[createConstraintCallback] This CallbackInterpolationStrategy is not implemented!"); + } + }(); + constraintPublishers[i].publish(ros_msg_conversions::createConstraintMsg(t, constraint)); + } + } + }; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +SolverObserver::lagrangian_callback_t createLagrangianCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector<std::string>& topicNames, CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createLagrangiancallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error("[createLagrangianCallback] For each observing time points, you should provide a unique topic name!"); } std::vector<::ros::Publisher> metricsPublishers; @@ -51,8 +95,8 @@ SolverObserver::lagrangian_callback_t createLagrangiancallback(::ros::NodeHandle } // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // has internal ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks associated - // with that handle will stop being called.") + // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks + // associated with that handle will stop being called.") return [=](const scalar_array_t& timeTrajectory, const std::vector<LagrangianMetricsConstRef>& termMetricsArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { @@ -66,10 +110,10 @@ SolverObserver::lagrangian_callback_t createLagrangiancallback(::ros::NodeHandle case CallbackInterpolationStrategy::linear_interpolation: return LinearInterpolation::interpolate(indexAlpha, termMetricsArray); default: - throw std::runtime_error("[createLagrangiancallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error("[createLagrangianCallback] This CallbackInterpolationStrategy is not implemented!"); } }(); - metricsPublishers[i].publish(ros_msg_conversions::createMetricsMsg(t, metrics)); + metricsPublishers[i].publish(ros_msg_conversions::createLagrangianMetricsMsg(t, metrics)); } } }; @@ -91,8 +135,8 @@ SolverObserver::multiplier_callback_t createMultiplierCallback(::ros::NodeHandle } // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // has internal ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks associated - // with that handle will stop being called.") + // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks + // associated with that handle will stop being called.") return [=](const scalar_array_t& timeTrajectory, const std::vector<MultiplierConstRef>& termMultiplierArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { From 02117b1ac565dea3c8c9fab59c7fba475bddf9bc Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 8 Aug 2022 17:35:20 +0200 Subject: [PATCH 269/512] renaming MetricsCollection to Metrics. Improving TrapezoidalIntegration. --- .../integration/TrapezoidalIntegration.h | 22 ++- .../include/ocs2_core/model_data/Metrics.h | 30 ++- ocs2_core/src/model_data/Metrics.cpp | 48 +++-- ocs2_core/test/model_data/testMetrics.cpp | 87 ++++----- ocs2_ddp/src/DDP_HelperFunctions.cpp | 83 ++------- .../LinearQuadraticApproximator.h | 24 +-- .../include/ocs2_oc/oc_data/ProblemMetrics.h | 6 +- .../OptimalControlProblemHelperFunction.h | 89 +++++---- .../ocs2_oc/oc_solver/PerformanceIndex.h | 43 +++++ .../LinearQuadraticApproximator.cpp | 18 +- .../OptimalControlProblemHelperFunction.cpp | 171 +++++++++--------- .../src/rollout/PerformanceIndicesRollout.cpp | 4 +- ocs2_sqp/ocs2_sqp/test/testTranscription.cpp | 15 +- 13 files changed, 322 insertions(+), 318 deletions(-) diff --git a/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h b/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h index 6adb50176..37088b127 100644 --- a/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h +++ b/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h @@ -29,22 +29,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include <cassert> #include <vector> namespace ocs2 { -template <typename SCALAR_T> -SCALAR_T trapezoidalIntegration(const std::vector<SCALAR_T>& timeTrajectory, const std::vector<SCALAR_T>& valueTrajectory) { +/** + * Compute the trapezoidal integration of a trajectory of VALUE_T given the time stamp timeTrajectory and initial value initialValue. + * + * @note It requires that the VALUE_T has overwrite operator+(VALUE_T, VALUE_T) and define VALUE_T::operator*(SCALAR_T) + */ +template <typename SCALAR_T, typename VALUE_T> +VALUE_T trapezoidalIntegration(const std::vector<SCALAR_T>& timeTrajectory, const std::vector<VALUE_T>& valueTrajectory, + VALUE_T initialValue) { + assert(timeTrajectory.size() == valueTrajectory.size()); + if (timeTrajectory.size() < 2) { - return 0.0; + return initialValue; } - SCALAR_T areaUnderCurve = 0.0; for (size_t k = 1; k < timeTrajectory.size(); k++) { - areaUnderCurve += 0.5 * (valueTrajectory[k] + valueTrajectory[k - 1]) * (timeTrajectory[k] - timeTrajectory[k - 1]); + auto temp = valueTrajectory[k - 1] + valueTrajectory[k]; + temp *= (0.5 * (timeTrajectory[k] - timeTrajectory[k - 1])); + initialValue += temp; } // end of k loop - return areaUnderCurve; + return initialValue; } } // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/model_data/Metrics.h b/ocs2_core/include/ocs2_core/model_data/Metrics.h index ef6973dcc..0213c525e 100644 --- a/ocs2_core/include/ocs2_core/model_data/Metrics.h +++ b/ocs2_core/include/ocs2_core/model_data/Metrics.h @@ -56,17 +56,20 @@ struct LagrangianMetricsConstRef { * The collection of cost, equality constraints, and LagrangianMetrics structure for all possible constraint terms (handled by * Lagrangian method) in a particular time point. * cost : The total cost in a particular time point. - * stateEqConstraint : A vector of all active state equality constraints. - * stateInputEqConstraint : A vector of all active state-input equality constraints. + * stateEqConstraint : An array of all state equality constraints. + * stateInputEqConstraint : An array of all state-input equality constraints. * stateEqLagrangian : An array of state equality constraint terms handled by Lagrangian method. * stateIneqLagrangian : An array of state inequality constraint terms handled by Lagrangian method. * stateInputEqLagrangian : An array of state-input equality constraint terms handled by Lagrangian method. * stateInputIneqLagrangian : An array of state-input inequality constraint terms handled by Lagrangian method. */ -struct MetricsCollection { +struct Metrics { // Cost scalar_t cost; + // Dynamics violation + vector_t dynamicsViolation; + // Equality constraints vector_array_t stateEqConstraint; vector_array_t stateInputEqConstraint; @@ -77,10 +80,12 @@ struct MetricsCollection { std::vector<LagrangianMetrics> stateInputEqLagrangian; std::vector<LagrangianMetrics> stateInputIneqLagrangian; - /** Exchanges the values of MetricsCollection */ - void swap(MetricsCollection& other) { + /** Exchanges the values of Metrics */ + void swap(Metrics& other) { // Cost std::swap(cost, other.cost); + // Dynamics violation + dynamicsViolation.swap(other.dynamicsViolation); // Equality constraints stateEqConstraint.swap(other.stateEqConstraint); stateInputEqConstraint.swap(other.stateInputEqConstraint); @@ -91,10 +96,12 @@ struct MetricsCollection { stateInputIneqLagrangian.swap(other.stateInputIneqLagrangian); } - /** Clears the value of the MetricsCollection */ + /** Clears the value of the Metrics */ void clear() { // Cost cost = 0.0; + // Dynamics violation + dynamicsViolation = vector_t(); // Equality constraints stateEqConstraint.clear(); stateInputEqConstraint.clear(); @@ -104,6 +111,9 @@ struct MetricsCollection { stateInputEqLagrangian.clear(); stateInputIneqLagrangian.clear(); } + + /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ + bool isApprox(const Metrics& other, scalar_t prec = 1e-8) const; }; /** Sums penalties of an array of LagrangianMetrics */ @@ -194,13 +204,13 @@ namespace LinearInterpolation { LagrangianMetrics interpolate(const index_alpha_t& indexAlpha, const std::vector<LagrangianMetricsConstRef>& dataArray); /** - * Linearly interpolates a trajectory of MetricsCollection. + * Linearly interpolates a trajectory of Metrics. * * @param [in] indexAlpha : index and interpolation coefficient (alpha) pair. - * @param [in] dataArray : A trajectory of MetricsCollection. - * @return The interpolated MetricsCollection at indexAlpha. + * @param [in] dataArray : A trajectory of Metrics. + * @return The interpolated Metrics at indexAlpha. */ -MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector<MetricsCollection>& dataArray); +Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector<Metrics>& dataArray); } // namespace LinearInterpolation } // namespace ocs2 diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 49a756243..3cb744663 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -31,6 +31,22 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +/** Returns true if *this is approximately equal to other, within the precision determined by prec. */ +bool Metrics::isApprox(const Metrics& other, scalar_t prec) const { + bool flag = std::abs(this->cost - other.cost) < prec * std::min(std::abs(this->cost), std::abs(other.cost)); + flag = flag && this->dynamicsViolation.isApprox(other.dynamicsViolation, prec); + flag = flag && toVector(this->stateEqConstraint).isApprox(toVector(other.stateEqConstraint), prec); + flag = flag && toVector(this->stateInputEqConstraint).isApprox(toVector(other.stateInputEqConstraint), prec); + flag = flag && toVector(this->stateEqLagrangian).isApprox(toVector(other.stateEqLagrangian), prec); + flag = flag && toVector(this->stateIneqLagrangian).isApprox(toVector(other.stateIneqLagrangian), prec); + flag = flag && toVector(this->stateInputEqLagrangian).isApprox(toVector(other.stateInputEqLagrangian), prec); + flag = flag && toVector(this->stateInputIneqLagrangian).isApprox(toVector(other.stateInputIneqLagrangian), prec); + return flag; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -142,7 +158,7 @@ LagrangianMetrics interpolate(const index_alpha_t& indexAlpha, const std::vector /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector<MetricsCollection>& dataArray) { +Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector<Metrics>& dataArray) { // number of terms const auto ind = indexAlpha.second > 0.5 ? indexAlpha.first : indexAlpha.first + 1; const size_t mumStateEqConst = dataArray[ind].stateEqConstraint.size(); @@ -152,23 +168,27 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector const size_t mumStateInputEqLag = dataArray[ind].stateInputEqLagrangian.size(); const size_t mumStateInputIneqLag = dataArray[ind].stateInputIneqLagrangian.size(); - MetricsCollection out; + Metrics out; // cost - out.cost = interpolate(indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { return array[t].cost; }); + out.cost = + interpolate(indexAlpha, dataArray, [](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].cost; }); + + // dynamics violation + out.dynamicsViolation = interpolate( + indexAlpha, dataArray, [](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].dynamicsViolation; }); // constraints out.stateEqConstraint.reserve(mumStateEqConst); for (size_t i = 0; i < mumStateEqConst; i++) { - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].stateEqConstraint[i]; }); out.stateEqConstraint.emplace_back(std::move(constraint)); } out.stateInputEqConstraint.reserve(mumStateInputEqCost); for (size_t i = 0; i < mumStateInputEqCost; i++) { - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint[i]; }); out.stateInputEqConstraint.emplace_back(std::move(constraint)); @@ -177,10 +197,10 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector // state equality Lagrangian out.stateEqLagrangian.reserve(mumStateEqLag); for (size_t i = 0; i < mumStateEqLag; i++) { - auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { + auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].stateEqLagrangian[i].penalty; }); - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].stateEqLagrangian[i].constraint; }); out.stateEqLagrangian.emplace_back(penalty, std::move(constraint)); @@ -189,10 +209,10 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector // state inequality Lagrangian out.stateIneqLagrangian.reserve(mumStateIneqLag); for (size_t i = 0; i < mumStateIneqLag; i++) { - auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { + auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].stateIneqLagrangian[i].penalty; }); - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].stateIneqLagrangian[i].constraint; }); out.stateIneqLagrangian.emplace_back(penalty, std::move(constraint)); @@ -201,10 +221,10 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector // state-input equality Lagrangian out.stateInputEqLagrangian.reserve(mumStateInputEqLag); for (size_t i = 0; i < mumStateInputEqLag; i++) { - auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { + auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].stateInputEqLagrangian[i].penalty; }); - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].stateInputEqLagrangian[i].constraint; }); out.stateInputEqLagrangian.emplace_back(penalty, std::move(constraint)); @@ -213,10 +233,10 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector // state-input inequality Lagrangian out.stateInputIneqLagrangian.reserve(mumStateInputIneqLag); for (size_t i = 0; i < mumStateInputIneqLag; i++) { - auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { + auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].stateInputIneqLagrangian[i].penalty; }); - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqLagrangian[i].constraint; }); out.stateInputIneqLagrangian.emplace_back(penalty, std::move(constraint)); diff --git a/ocs2_core/test/model_data/testMetrics.cpp b/ocs2_core/test/model_data/testMetrics.cpp index 48a60ca86..806ab6617 100644 --- a/ocs2_core/test/model_data/testMetrics.cpp +++ b/ocs2_core/test/model_data/testMetrics.cpp @@ -39,14 +39,14 @@ namespace ocs2 { namespace { /** - * Linearly interpolates a trajectory of MetricsCollections. + * Linearly interpolates a trajectory of Metricss. * * @param [in] indexAlpha : index and interpolation coefficient (alpha) pair. - * @param [in] dataArray : A trajectory of MetricsCollections. - * @return The interpolated MetricsCollection at indexAlpha. + * @param [in] dataArray : A trajectory of Metricss. + * @return The interpolated Metrics at indexAlpha. */ -inline MetricsCollection interpolateNew(const LinearInterpolation::index_alpha_t& indexAlpha, - const std::vector<MetricsCollection>& dataArray) { +inline Metrics interpolateNew(const LinearInterpolation::index_alpha_t& indexAlpha, + const std::vector<Metrics>& dataArray) { assert(dataArray.size() > 0); if (dataArray.size() > 1) { // Normal interpolation case @@ -80,10 +80,10 @@ inline MetricsCollection interpolateNew(const LinearInterpolation::index_alpha_t if (areSameSize) { const auto f = [alpha](const vector_t& lhs, const vector_t& rhs) -> vector_t { return alpha * lhs + (scalar_t(1.0) - alpha) * rhs; }; - MetricsCollection out; + Metrics out; // cost out.cost = LinearInterpolation::interpolate( - indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const scalar_t& { return array[t].cost; }); + indexAlpha, dataArray, [](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].cost; }); // constraints out.stateEqConstraint = toConstraintArray(getSizes(dataArray[index].stateEqConstraint), f(lhs_stateEqConst, rhs_stateEqConst)); out.stateInputEqConstraint = toConstraintArray(getSizes(dataArray[index].stateInputEqConstraint), f(lhs_stateInputEqConst, rhs_stateInputEqConst)); @@ -117,17 +117,6 @@ void random(const size_array_t& termsSize, vector_array_t& constraintArray) { constraintArray = toConstraintArray(termsSize, serializedConstraintArray); } -bool isApprox(const MetricsCollection& lhs, const MetricsCollection& rhs, scalar_t prec) { - bool flag = std::abs(lhs.cost - rhs.cost) < prec; - flag = flag && toVector(lhs.stateEqConstraint).isApprox(toVector(rhs.stateEqConstraint), prec); - flag = flag && toVector(lhs.stateInputEqConstraint).isApprox(toVector(rhs.stateInputEqConstraint), prec); - flag = flag && toVector(lhs.stateEqLagrangian).isApprox(toVector(rhs.stateEqLagrangian), prec); - flag = flag && toVector(lhs.stateIneqLagrangian).isApprox(toVector(rhs.stateIneqLagrangian), prec); - flag = flag && toVector(lhs.stateInputEqLagrangian).isApprox(toVector(rhs.stateInputEqLagrangian), prec); - flag = flag && toVector(lhs.stateInputIneqLagrangian).isApprox(toVector(rhs.stateInputIneqLagrangian), prec); - return flag; -} - } // unnamed namespace } // namespace ocs2 @@ -137,34 +126,35 @@ TEST(TestMetrics, testSerialization) { const size_t length = std::accumulate(termsSize.begin(), termsSize.end(), numConstraint); const ocs2::vector_t randomVec = ocs2::vector_t::Random(length); - const std::vector<ocs2::LagrangianMetrics> termsMetricsCollection = ocs2::toLagrangianMetrics(termsSize, randomVec); - const ocs2::vector_t serializedMetricsCollection = ocs2::toVector(termsMetricsCollection); + const std::vector<ocs2::LagrangianMetrics> l = ocs2::toLagrangianMetrics(termsSize, randomVec); + const ocs2::vector_t serialized_l = ocs2::toVector(l); - EXPECT_TRUE(serializedMetricsCollection == randomVec); - EXPECT_TRUE(serializedMetricsCollection.size() == length); - EXPECT_TRUE(getSizes(termsMetricsCollection) == termsSize); + EXPECT_TRUE(serialized_l == randomVec); + EXPECT_TRUE(serialized_l.size() == length); + EXPECT_TRUE(getSizes(l) == termsSize); } TEST(TestMetrics, testSwap) { const ocs2::size_array_t termsSize{0, 2, 0, 0, 3, 5}; - ocs2::MetricsCollection termsMetricsCollection; - termsMetricsCollection.cost = ocs2::vector_t::Random(1)(0); - ocs2::random(termsSize, termsMetricsCollection.stateEqConstraint); - ocs2::random(termsSize, termsMetricsCollection.stateInputEqConstraint); - ocs2::random(termsSize, termsMetricsCollection.stateEqLagrangian); - ocs2::random(termsSize, termsMetricsCollection.stateIneqLagrangian); - ocs2::random(termsSize, termsMetricsCollection.stateInputEqLagrangian); - ocs2::random(termsSize, termsMetricsCollection.stateInputIneqLagrangian); + ocs2::Metrics termsMetrics; + termsMetrics.cost = ocs2::vector_t::Random(1)(0); + termsMetrics.dynamicsViolation.setRandom(2); + ocs2::random(termsSize, termsMetrics.stateEqConstraint); + ocs2::random(termsSize, termsMetrics.stateInputEqConstraint); + ocs2::random(termsSize, termsMetrics.stateEqLagrangian); + ocs2::random(termsSize, termsMetrics.stateIneqLagrangian); + ocs2::random(termsSize, termsMetrics.stateInputEqLagrangian); + ocs2::random(termsSize, termsMetrics.stateInputIneqLagrangian); - auto termsMetricsCollectionRef = termsMetricsCollection; - ocs2::MetricsCollection termsMultiplierNew; - termsMetricsCollection.swap(termsMultiplierNew); + auto termsMetricsRef = termsMetrics; + ocs2::Metrics termsMetricsNew; + termsMetrics.swap(termsMetricsNew); - EXPECT_TRUE(ocs2::isApprox(termsMultiplierNew, termsMetricsCollectionRef, 1e-10)); + EXPECT_TRUE(termsMetricsNew.isApprox(termsMetricsRef, 1e-10)); - termsMetricsCollectionRef.clear(); - EXPECT_TRUE(ocs2::isApprox(termsMetricsCollection, termsMetricsCollectionRef, 1e-10)); + termsMetricsRef.clear(); + EXPECT_TRUE(termsMetrics.isApprox(termsMetricsRef, 1e-10)); } TEST(TestMetrics, testInterpolation) { @@ -181,16 +171,17 @@ TEST(TestMetrics, testInterpolation) { ocs2::vector_array_t stateIneqTrajectory(N); ocs2::vector_array_t stateInputEqTrajectory(N); ocs2::vector_array_t stateInputIneqTrajectory(N); - std::vector<ocs2::MetricsCollection> metricsCollectionTrajectory(N); + std::vector<ocs2::Metrics> metricsTrajectory(N); for (size_t i = 0; i < N; i++) { timeTrajectory[i] = i * 0.1; - metricsCollectionTrajectory[i].cost = ocs2::vector_t::Random(1)(0); - ocs2::random(stateEqTermsSize, metricsCollectionTrajectory[i].stateEqConstraint); - ocs2::random(stateInputEqTermsSize, metricsCollectionTrajectory[i].stateInputEqConstraint); - ocs2::random(stateEqTermsSize, metricsCollectionTrajectory[i].stateEqLagrangian); - ocs2::random(stateIneqTermsSize, metricsCollectionTrajectory[i].stateIneqLagrangian); - ocs2::random(stateInputEqTermsSize, metricsCollectionTrajectory[i].stateInputEqLagrangian); - ocs2::random(stateInputIneqTermsSize, metricsCollectionTrajectory[i].stateInputIneqLagrangian); + metricsTrajectory[i].cost = ocs2::vector_t::Random(1)(0); + metricsTrajectory[i].dynamicsViolation.setRandom(2); + ocs2::random(stateEqTermsSize, metricsTrajectory[i].stateEqConstraint); + ocs2::random(stateInputEqTermsSize, metricsTrajectory[i].stateInputEqConstraint); + ocs2::random(stateEqTermsSize, metricsTrajectory[i].stateEqLagrangian); + ocs2::random(stateIneqTermsSize, metricsTrajectory[i].stateIneqLagrangian); + ocs2::random(stateInputEqTermsSize, metricsTrajectory[i].stateInputEqLagrangian); + ocs2::random(stateInputIneqTermsSize, metricsTrajectory[i].stateInputIneqLagrangian); } // end of i loop constexpr ocs2::scalar_t prec = 1e-8; @@ -198,9 +189,9 @@ TEST(TestMetrics, testInterpolation) { for (size_t i = 0; i < timeTrajectoryTest.size(); i++) { const auto indexAlpha = ocs2::LinearInterpolation::timeSegment(timeTrajectoryTest[i], timeTrajectory); - const auto metricsCollection = ocs2::LinearInterpolation::interpolate(indexAlpha, metricsCollectionTrajectory); - const auto metricsCollectionNew = ocs2::interpolateNew(indexAlpha, metricsCollectionTrajectory); + const auto metrics = ocs2::LinearInterpolation::interpolate(indexAlpha, metricsTrajectory); + const auto metricsNew = ocs2::interpolateNew(indexAlpha, metricsTrajectory); - EXPECT_TRUE(ocs2::isApprox(metricsCollection, metricsCollectionNew, prec)); + EXPECT_TRUE(metrics.isApprox(metricsNew, prec)); } // end of i loop } diff --git a/ocs2_ddp/src/DDP_HelperFunctions.cpp b/ocs2_ddp/src/DDP_HelperFunctions.cpp index 7bb21cbe3..815e502ee 100644 --- a/ocs2_ddp/src/DDP_HelperFunctions.cpp +++ b/ocs2_ddp/src/DDP_HelperFunctions.cpp @@ -101,72 +101,21 @@ void computeRolloutMetrics(OptimalControlProblem& problem, const PrimalSolution& PerformanceIndex computeRolloutPerformanceIndex(const scalar_array_t& timeTrajectory, const ProblemMetrics& problemMetrics) { assert(timeTrajectory.size() == problemMetrics.intermediates.size()); - PerformanceIndex performanceIndex; - - // Total cost: - // - Final: state cost, state soft-constraints - // - PreJumps: state cost, state soft-constraints - // - Intermediates: state/state-input cost, state/state-input soft-constraints - performanceIndex.cost = problemMetrics.final.cost; - - std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), - [&](const MetricsCollection& m) { performanceIndex.cost += m.cost; }); - - scalar_array_t costTrajectory(timeTrajectory.size()); - std::transform(problemMetrics.intermediates.begin(), problemMetrics.intermediates.end(), costTrajectory.begin(), - [](const MetricsCollection& m) { return m.cost; }); - performanceIndex.cost += trapezoidalIntegration(timeTrajectory, costTrajectory); - - // Dynamics violation: - performanceIndex.dynamicsViolationSSE = 0.0; - - // Equality constraints' SSE: - // - Final: state equality constraints - // - PreJumps: state equality constraints - // - Intermediates: state/state-input equality constraints - performanceIndex.equalityConstraintsSSE = constraintsSquaredNorm(problemMetrics.final.stateEqConstraint); - - std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), [&](const MetricsCollection& m) { - performanceIndex.equalityConstraintsSSE += constraintsSquaredNorm(m.stateEqConstraint); - }); - - scalar_array_t equalityNorm2Trajectory(timeTrajectory.size()); - std::transform(problemMetrics.intermediates.begin(), problemMetrics.intermediates.end(), equalityNorm2Trajectory.begin(), - [](const MetricsCollection& m) { - return constraintsSquaredNorm(m.stateEqConstraint) + constraintsSquaredNorm(m.stateInputEqConstraint); - }); - performanceIndex.equalityConstraintsSSE += trapezoidalIntegration(timeTrajectory, equalityNorm2Trajectory); - - // Equality Lagrangians penalty - // - Final: state equality Lagrangians - // - PreJumps: state equality Lagrangians - // - Intermediates: state/state-input equality Lagrangians - performanceIndex.equalityLagrangian = sumPenalties(problemMetrics.final.stateEqLagrangian); - - std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), - [&](const MetricsCollection& m) { performanceIndex.equalityLagrangian += sumPenalties(m.stateEqLagrangian); }); - - scalar_array_t equalityPenaltyTrajectory(timeTrajectory.size()); - std::transform(problemMetrics.intermediates.begin(), problemMetrics.intermediates.end(), equalityPenaltyTrajectory.begin(), - [&](const MetricsCollection& m) { return sumPenalties(m.stateEqLagrangian) + sumPenalties(m.stateInputEqLagrangian); }); - performanceIndex.equalityLagrangian += trapezoidalIntegration(timeTrajectory, equalityPenaltyTrajectory); - - // Inequality Lagrangians penalty - // - Final: state inequality Lagrangians - // - PreJumps: state inequality Lagrangians - // - Intermediates: state/state-input inequality Lagrangians - performanceIndex.inequalityLagrangian = sumPenalties(problemMetrics.final.stateIneqLagrangian); - - std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), - [&](const MetricsCollection& m) { performanceIndex.inequalityLagrangian += sumPenalties(m.stateIneqLagrangian); }); - - scalar_array_t inequalityPenaltyTrajectory(timeTrajectory.size()); - std::transform( - problemMetrics.intermediates.begin(), problemMetrics.intermediates.end(), inequalityPenaltyTrajectory.begin(), - [&](const MetricsCollection& m) { return sumPenalties(m.stateIneqLagrangian) + sumPenalties(m.stateInputIneqLagrangian); }); - performanceIndex.inequalityLagrangian += trapezoidalIntegration(timeTrajectory, inequalityPenaltyTrajectory); - - return performanceIndex; + // Final + PerformanceIndex performanceIndex = toPerformanceIndex(problemMetrics.final); + + // PreJumps + std::for_each(problemMetrics.preJumps.cbegin(), problemMetrics.preJumps.cend(), + [&performanceIndex](const Metrics& m) { performanceIndex += toPerformanceIndex(m); }); + + // Intermediates + std::vector<PerformanceIndex> performanceIndexTrajectory; + performanceIndexTrajectory.reserve(problemMetrics.intermediates.size()); + std::for_each(problemMetrics.intermediates.cbegin(), problemMetrics.intermediates.cend(), + [&performanceIndexTrajectory](const Metrics& m) { performanceIndexTrajectory.push_back(toPerformanceIndex(m)); }); + + // Intermediates + return trapezoidalIntegration(timeTrajectory, performanceIndexTrajectory, performanceIndex); } /******************************************************************************************************/ @@ -263,7 +212,7 @@ scalar_t computeControllerUpdateIS(const LinearController& controller) { std::transform(controller.deltaBiasArray_.begin(), controller.deltaBiasArray_.end(), biasArraySquaredNorm.begin(), [](const vector_t& b) { return b.squaredNorm(); }); // integrates using the trapezoidal approximation method - return trapezoidalIntegration(controller.timeStamp_, biasArraySquaredNorm); + return trapezoidalIntegration(controller.timeStamp_, biasArraySquaredNorm, 0.0); } /******************************************************************************************************/ diff --git a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h index 55d574eac..f076843fb 100644 --- a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h +++ b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h @@ -161,7 +161,7 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr const vector_t& state); /** - * Compute the intermediate-time MetricsCollection (i.e. cost, softConstraints, and constraints). + * Compute the intermediate-time Metrics (i.e. cost, softConstraints, and constraints). * * @note It is assumed that the precomputation request is already made. * problem.preComputationPtr->request(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x, u) @@ -171,13 +171,13 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr * @param [in] state: The current state. * @param [in] input: The current input. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. - * @return The output MetricsCollection. + * @return The output Metrics. */ -MetricsCollection computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const vector_t& input, const MultiplierCollection& multipliers); +Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, + const MultiplierCollection& multipliers); /** - * Compute the event-time MetricsCollection based on pre-jump state value (i.e. cost, softConstraints, and constraints). + * Compute the event-time Metrics based on pre-jump state value (i.e. cost, softConstraints, and constraints). * * @note It is assumed that the precomputation request is already made. * problem.preComputationPtr->requestPreJump(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x) @@ -186,13 +186,13 @@ MetricsCollection computeIntermediateMetrics(OptimalControlProblem& problem, con * @param [in] time: The current time. * @param [in] state: The current state. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. - * @return The output MetricsCollection. + * @return The output Metrics. */ -MetricsCollection computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers); +Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers); /** - * Compute the final-time MetricsCollection (i.e. cost, softConstraints, and constraints). + * Compute the final-time Metrics (i.e. cost, softConstraints, and constraints). * * @note It is assumed that the precomputation request is already made. * problem.preComputationPtr->requestFinal(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x) @@ -201,9 +201,9 @@ MetricsCollection computePreJumpMetrics(OptimalControlProblem& problem, const sc * @param [in] time: The current time. * @param [in] state: The current state. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. - * @return The output MetricsCollection. + * @return The output Metrics. */ -MetricsCollection computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers); +Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers); } // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/oc_data/ProblemMetrics.h b/ocs2_oc/include/ocs2_oc/oc_data/ProblemMetrics.h index c5c26b8d0..8368cc447 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/ProblemMetrics.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/ProblemMetrics.h @@ -35,9 +35,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { struct ProblemMetrics { - MetricsCollection final; - std::vector<MetricsCollection> preJumps; - std::vector<MetricsCollection> intermediates; + Metrics final; + std::vector<Metrics> preJumps; + std::vector<Metrics> intermediates; /** Exchanges the content of ProblemMetrics */ void swap(ProblemMetrics& other) { diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h index c1f0b9ed4..80a4e7eb3 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h @@ -95,113 +95,108 @@ void updateDualSolution(const OptimalControlProblem& ocp, const PrimalSolution& /** * Updates in-place final MultiplierCollection for equality and inequality Lagrangians. - * Moreover it also updates the penalties of MetricsCollection based on the update of multipliers. + * Moreover it also updates the penalties of Metrics based on the update of multipliers. * * @param [in] ocp : A const reference to the optimal control problem. * @param [in] time : Final time. * @param [in] state : Final state. - * @param [in, out] metricsCollection: The final MetricsCollection. Its penalties will be updated based on - * the update of multiplierCollection. - * @param [out] multiplierCollection : The updated final MultiplierCollection. + * @param [in, out] metrics: The final Metrics. Its penalties will be updated based on the update of multiplierCollection. + * @param [out] multipliers : The updated final MultiplierCollection. */ -void updateFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection); +void updateFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, Metrics& metrics, + MultiplierCollection& multipliers); /** * Updates in-place pre-jump MultiplierCollection for equality and inequality Lagrangians. - * Moreover it also updates the penalties of MetricsCollection based on the update of multipliers. + * Moreover it also updates the penalties of Metrics based on the update of multipliers. * * @param [in] ocp : A const reference to the optimal control problem. * @param [in] time : Pre-jump time. * @param [in] state : Pre-jump state. - * @param [in, out] metricsCollection: The pre-jump MetricsCollection. Its penalties will be updated based on - * the update of multiplierCollection. - * @param [out] multiplierCollection : The updated pre-jump MultiplierCollection. + * @param [in, out] metrics: The pre-jump Metrics. Its penalties will be updated based on the update of multiplierCollection. + * @param [out] multipliers : The updated pre-jump MultiplierCollection. */ -void updatePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection); +void updatePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, Metrics& metrics, + MultiplierCollection& multipliers); /** * Updates in-place intermediate MultiplierCollection for equality and inequality Lagrangians. - * Moreover it also updates the penalties of MetricsCollection based on the update of multipliers. + * Moreover it also updates the penalties of Metrics based on the update of multipliers. * * @param [in] ocp : A const reference to the optimal control problem. * @param [in] time : Intermediate time. * @param [in] state : Intermediate state. * @param [in] input : Intermediate input. - * @param [in, out] metricsCollection: The intermediate MetricsCollection. Its penalties will be updated based on - * the update of multiplierCollection. - * @param [out] multiplierCollection : The updated Intermediate MultiplierCollection. + * @param [in, out] metrics: The intermediate Metrics. Its penalties will be updated based on the update of multiplierCollection. + * @param [out] multipliers : The updated Intermediate MultiplierCollection. */ void updateIntermediateMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, const vector_t& input, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection); + Metrics& metrics, MultiplierCollection& multipliers); /** - * Extracts a requested final constraint term vector from the input MetricsCollection. + * Extracts a requested final constraint term vector from the input Metrics. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. - * @param [in] metricsColl : MetricsCollection. - * @param [out] metrics : A const pointer to the constraint term value. + * @param [in] metrics : Metrics. + * @return A const pointer to the constraint term value. */ -const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const MetricsCollection& metricsColl); +const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const Metrics& metrics); /** - * Extracts a requested final term LagrangianMetrics from the input MetricsCollection. + * Extracts a requested final term LagrangianMetrics from the input Metrics. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. - * @param [in] metricsColl : MetricsCollection. - * @param [out] metrics : A const pointer to the term LagrangianMetrics. + * @param [in] metrics : Metrics. + * @return A const pointer to the term LagrangianMetrics. */ const LagrangianMetrics* extractFinalTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, - const MetricsCollection& metricsColl); + const Metrics& metrics); /** - * Extracts an array of the requested pre-jump constraint term from the input MetricsCollection array. + * Extracts an array of the requested pre-jump constraint term from the input Metrics array. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. - * @param [in] metricsCollArray : MetricsCollection array. - * @param [out] metricsArray : Array of const references to constraint term values. + * @param [in] metricsArray : Metrics array. + * @param [out] constraintArray : Array of const references to constraint term values. * @return True if the term found in the pre-jump collection. */ -bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollArray, - std::vector<std::reference_wrapper<const vector_t>>& metricsArray); +bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const std::vector<Metrics>& metricsArray, + std::vector<std::reference_wrapper<const vector_t>>& constraintArray); /** - * Extracts a requested pre-jump term LagrangianMetrics array from the input MetricsCollection array. + * Extracts a requested pre-jump term LagrangianMetrics array from the input Metrics array. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. - * @param [in] metricsCollArray : MetricsCollection array. - * @param [out] metricsArray : Array of const references to term LagrangianMetrics. + * @param [in] metricsArray : Metrics array. + * @param [out] lagrangianMetricsArray : Array of const references to term LagrangianMetrics. * @return True if the term found in the pre-jump collection. */ bool extractPreJumpTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollArray, - std::vector<LagrangianMetricsConstRef>& metricsArray); + const std::vector<Metrics>& metricsArray, + std::vector<LagrangianMetricsConstRef>& lagrangianMetricsArray); /** - * Extracts a trajectory of the requested intermediate constraint term from the input MetricsCollection trajectory. + * Extracts a trajectory of the requested intermediate constraint term from the input Metrics trajectory. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. - * @param [in] metricsCollTraj : MetricsCollection trajectory. - * @param [out] metricsTrajectory : Trajectory of const references to constraint term values. + * @param [in] metricsTraj : Metrics trajectory. + * @param [out] constraintTraj : Trajectory of const references to constraint term values. * @return True if the term found in the intermediate collection. */ -bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollTraj, - std::vector<std::reference_wrapper<const vector_t>>& metricsTrajectory); +bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const std::vector<Metrics>& metricsTraj, + std::vector<std::reference_wrapper<const vector_t>>& constraintTraj); /** - * Extracts a requested intermediate term LagrangianMetrics trajectory from the input MetricsCollection trajectory. + * Extracts a requested intermediate term LagrangianMetrics trajectory from the input Metrics trajectory. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. - * @param [in] metricsCollTraj : MetricsCollection trajectory. - * @param [out] metricsTrajectory : Trajectory of const references to term LagrangianMetrics. + * @param [in] metricsTraj : Metrics trajectory. + * @param [out] lagrangianMetricsTraj : Trajectory of const references to term LagrangianMetrics. * @return True if the term found in the intermediate collection. */ bool extractIntermediateTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollTraj, - std::vector<LagrangianMetricsConstRef>& metricsTrajectory); + const std::vector<Metrics>& metricsTraj, + std::vector<LagrangianMetricsConstRef>& lagrangianMetricsTraj); /** * Extracts a requested final Term Multiplier from the input MultiplierCollection. diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index ee45978ee..891a3bae8 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -33,6 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ostream> #include <ocs2_core/Types.h> +#include <ocs2_core/model_data/Metrics.h> namespace ocs2 { @@ -80,6 +81,30 @@ struct PerformanceIndex { this->inequalityLagrangian += rhs.inequalityLagrangian; return *this; } + + /** Multiply by a scalar */ + template <typename SCALAR_T> + PerformanceIndex& operator*=(const SCALAR_T c) { + this->merit *= static_cast<scalar_t>(c); + this->cost *= static_cast<scalar_t>(c); + this->dynamicsViolationSSE *= static_cast<scalar_t>(c); + this->equalityConstraintsSSE *= static_cast<scalar_t>(c); + this->equalityLagrangian *= static_cast<scalar_t>(c); + this->inequalityLagrangian *= static_cast<scalar_t>(c); + return *this; + } + + /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ + bool isApprox(const PerformanceIndex other, const scalar_t prec = 1e-8) const { + auto fuzzyCompares = [&](const scalar_t a, const scalar_t b) { return std::abs(a - b) < prec * std::min(std::abs(a), std::abs(b)); }; + bool isEqual = fuzzyCompares(this->merit, other.merit); + isEqual = isEqual && fuzzyCompares(this->cost, other.cost); + isEqual = isEqual && fuzzyCompares(this->dynamicsViolationSSE, other.dynamicsViolationSSE); + isEqual = isEqual && fuzzyCompares(this->equalityConstraintsSSE, other.equalityConstraintsSSE); + isEqual = isEqual && fuzzyCompares(this->equalityLagrangian, other.equalityLagrangian); + isEqual = isEqual && fuzzyCompares(this->inequalityLagrangian, other.inequalityLagrangian); + return isEqual; + } }; inline PerformanceIndex operator+(PerformanceIndex lhs, const PerformanceIndex& rhs) { @@ -87,6 +112,12 @@ inline PerformanceIndex operator+(PerformanceIndex lhs, const PerformanceIndex& return lhs; } +template <typename SCALAR_T> +PerformanceIndex operator*(const SCALAR_T c, PerformanceIndex rhs) { + rhs *= c; // Copied rhs + return rhs; +} + /** Swaps performance indices */ inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { std::swap(lhs.merit, rhs.merit); @@ -117,4 +148,16 @@ inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& pe return stream; } +/** Computes the PerformanceIndex based on a given Metrics */ +inline PerformanceIndex toPerformanceIndex(const Metrics& m) { + PerformanceIndex performanceIndex; + performanceIndex.merit = 0.0; + performanceIndex.cost = m.cost; + performanceIndex.dynamicsViolationSSE = m.dynamicsViolation.squaredNorm(); + performanceIndex.equalityConstraintsSSE = constraintsSquaredNorm(m.stateEqConstraint) + constraintsSquaredNorm(m.stateInputEqConstraint); + performanceIndex.equalityLagrangian = sumPenalties(m.stateEqLagrangian) + sumPenalties(m.stateInputEqLagrangian); + performanceIndex.inequalityLagrangian = sumPenalties(m.stateIneqLagrangian) + sumPenalties(m.stateInputIneqLagrangian); + return performanceIndex; +} + } // namespace ocs2 diff --git a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp index 149f503b3..e91cd2822 100644 --- a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp +++ b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp @@ -269,11 +269,11 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MetricsCollection computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const vector_t& input, const MultiplierCollection& multipliers) { +Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, + const MultiplierCollection& multipliers) { auto& preComputation = *problem.preComputationPtr; - MetricsCollection metrics; + Metrics metrics; // Cost metrics.cost = computeCost(problem, time, state, input); @@ -295,11 +295,11 @@ MetricsCollection computeIntermediateMetrics(OptimalControlProblem& problem, con /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MetricsCollection computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers) { +Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers) { auto& preComputation = *problem.preComputationPtr; - MetricsCollection metrics; + Metrics metrics; // Cost metrics.cost = computeEventCost(problem, time, state); @@ -317,11 +317,11 @@ MetricsCollection computePreJumpMetrics(OptimalControlProblem& problem, const sc /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MetricsCollection computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers) { +Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers) { auto& preComputation = *problem.preComputationPtr; - MetricsCollection metrics; + Metrics metrics; // Cost metrics.cost = computeFinalCost(problem, time, state); diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp index 9e76c02db..af6d7d175 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp @@ -64,11 +64,11 @@ void initializeDualSolution(const OptimalControlProblem& ocp, const PrimalSoluti for (size_t i = 0; i < primalSolution.postEventIndices_.size(); i++) { const auto cachedTimeIndex = cacheEventIndexBias + i; const auto& time = primalSolution.timeTrajectory_[i]; - auto& multiplierCollection = dualSolution.preJumps[i]; + auto& multipliers = dualSolution.preJumps[i]; if (cachedTimeIndex < cachedDualSolution.preJumps.size()) { - multiplierCollection = cachedDualSolution.preJumps[cachedTimeIndex]; + multipliers = cachedDualSolution.preJumps[cachedTimeIndex]; } else { - initializePreJumpMultiplierCollection(ocp, time, multiplierCollection); + initializePreJumpMultiplierCollection(ocp, time, multipliers); } } } @@ -77,11 +77,11 @@ void initializeDualSolution(const OptimalControlProblem& ocp, const PrimalSoluti dualSolution.intermediates.resize(primalSolution.timeTrajectory_.size()); for (size_t i = 0; i < primalSolution.timeTrajectory_.size(); i++) { const auto& time = primalSolution.timeTrajectory_[i]; - auto& multiplierCollection = dualSolution.intermediates[i]; + auto& multipliers = dualSolution.intermediates[i]; if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { - multiplierCollection = getIntermediateDualSolutionAtTime(cachedDualSolution, time); + multipliers = getIntermediateDualSolutionAtTime(cachedDualSolution, time); } else { - initializeIntermediateMultiplierCollection(ocp, time, multiplierCollection); + initializeIntermediateMultiplierCollection(ocp, time, multipliers); } } } @@ -89,28 +89,28 @@ void initializeDualSolution(const OptimalControlProblem& ocp, const PrimalSoluti /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void initializeFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multiplierCollection) { - ocp.finalEqualityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateEq); - ocp.finalInequalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateIneq); +void initializeFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multipliers) { + ocp.finalEqualityLagrangianPtr->initializeLagrangian(time, multipliers.stateEq); + ocp.finalInequalityLagrangianPtr->initializeLagrangian(time, multipliers.stateIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void initializePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multiplierCollection) { - ocp.preJumpEqualityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateEq); - ocp.preJumpInequalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateIneq); +void initializePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multipliers) { + ocp.preJumpEqualityLagrangianPtr->initializeLagrangian(time, multipliers.stateEq); + ocp.preJumpInequalityLagrangianPtr->initializeLagrangian(time, multipliers.stateIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void initializeIntermediateMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, - MultiplierCollection& multiplierCollection) { - ocp.stateEqualityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateEq); - ocp.stateInequalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateIneq); - ocp.equalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateInputEq); - ocp.inequalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateInputIneq); + MultiplierCollection& multipliers) { + ocp.stateEqualityLagrangianPtr->initializeLagrangian(time, multipliers.stateEq); + ocp.stateInequalityLagrangianPtr->initializeLagrangian(time, multipliers.stateIneq); + ocp.equalityLagrangianPtr->initializeLagrangian(time, multipliers.stateInputEq); + ocp.inequalityLagrangianPtr->initializeLagrangian(time, multipliers.stateInputIneq); } /******************************************************************************************************/ @@ -122,9 +122,9 @@ void updateDualSolution(const OptimalControlProblem& ocp, const PrimalSolution& if (!primalSolution.timeTrajectory_.empty()) { const auto& time = primalSolution.timeTrajectory_.back(); const auto& state = primalSolution.stateTrajectory_.back(); - auto& metricsCollection = problemMetrics.final; - auto& multiplierCollection = dualSolution.final; - updateFinalMultiplierCollection(ocp, time, state, metricsCollection, multiplierCollection); + auto& metrics = problemMetrics.final; + auto& multipliers = dualSolution.final; + updateFinalMultiplierCollection(ocp, time, state, metrics, multipliers); } // preJump @@ -134,9 +134,9 @@ void updateDualSolution(const OptimalControlProblem& ocp, const PrimalSolution& const auto timeIndex = primalSolution.postEventIndices_[i] - 1; const auto& time = primalSolution.timeTrajectory_[timeIndex]; const auto& state = primalSolution.stateTrajectory_[timeIndex]; - auto& metricsCollection = problemMetrics.preJumps[i]; - auto& multiplierCollection = dualSolution.preJumps[i]; - updatePreJumpMultiplierCollection(ocp, time, state, metricsCollection, multiplierCollection); + auto& metrics = problemMetrics.preJumps[i]; + auto& multipliers = dualSolution.preJumps[i]; + updatePreJumpMultiplierCollection(ocp, time, state, metrics, multipliers); } // intermediates @@ -146,51 +146,48 @@ void updateDualSolution(const OptimalControlProblem& ocp, const PrimalSolution& const auto& time = primalSolution.timeTrajectory_[i]; const auto& state = primalSolution.stateTrajectory_[i]; const auto& input = primalSolution.inputTrajectory_[i]; - auto& metricsCollection = problemMetrics.intermediates[i]; - auto& multiplierCollection = dualSolution.intermediates[i]; - updateIntermediateMultiplierCollection(ocp, time, state, input, metricsCollection, multiplierCollection); + auto& metrics = problemMetrics.intermediates[i]; + auto& multipliers = dualSolution.intermediates[i]; + updateIntermediateMultiplierCollection(ocp, time, state, input, metrics, multipliers); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void updateFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection) { - ocp.finalEqualityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateEqLagrangian, multiplierCollection.stateEq); - ocp.finalInequalityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateIneqLagrangian, multiplierCollection.stateIneq); +void updateFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, Metrics& metrics, + MultiplierCollection& multipliers) { + ocp.finalEqualityLagrangianPtr->updateLagrangian(time, state, metrics.stateEqLagrangian, multipliers.stateEq); + ocp.finalInequalityLagrangianPtr->updateLagrangian(time, state, metrics.stateIneqLagrangian, multipliers.stateIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void updatePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection) { - ocp.preJumpEqualityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateEqLagrangian, multiplierCollection.stateEq); - ocp.preJumpInequalityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateIneqLagrangian, multiplierCollection.stateIneq); +void updatePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, Metrics& metrics, + MultiplierCollection& multipliers) { + ocp.preJumpEqualityLagrangianPtr->updateLagrangian(time, state, metrics.stateEqLagrangian, multipliers.stateEq); + ocp.preJumpInequalityLagrangianPtr->updateLagrangian(time, state, metrics.stateIneqLagrangian, multipliers.stateIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void updateIntermediateMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, const vector_t& input, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection) { - ocp.stateEqualityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateEqLagrangian, multiplierCollection.stateEq); - ocp.stateInequalityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateIneqLagrangian, multiplierCollection.stateIneq); - ocp.equalityLagrangianPtr->updateLagrangian(time, state, input, metricsCollection.stateInputEqLagrangian, - multiplierCollection.stateInputEq); - ocp.inequalityLagrangianPtr->updateLagrangian(time, state, input, metricsCollection.stateInputIneqLagrangian, - multiplierCollection.stateInputIneq); + Metrics& metrics, MultiplierCollection& multipliers) { + ocp.stateEqualityLagrangianPtr->updateLagrangian(time, state, metrics.stateEqLagrangian, multipliers.stateEq); + ocp.stateInequalityLagrangianPtr->updateLagrangian(time, state, metrics.stateIneqLagrangian, multipliers.stateIneq); + ocp.equalityLagrangianPtr->updateLagrangian(time, state, input, metrics.stateInputEqLagrangian, multipliers.stateInputEq); + ocp.inequalityLagrangianPtr->updateLagrangian(time, state, input, metrics.stateInputIneqLagrangian, multipliers.stateInputIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, const std::string& name, - const MetricsCollection& metricsColl) { +const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const Metrics& metrics) { size_t index; if (ocp.finalEqualityConstraintPtr->getTermIndex(name, index)) { - return &metricsColl.stateEqConstraint[index]; + return &metrics.stateEqConstraint[index]; } else { return nullptr; @@ -201,13 +198,13 @@ const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, con /******************************************************************************************************/ /******************************************************************************************************/ const LagrangianMetrics* extractFinalTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, - const MetricsCollection& metricsColl) { + const Metrics& metrics) { size_t index; if (ocp.finalEqualityLagrangianPtr->getTermIndex(name, index)) { - return &metricsColl.stateEqLagrangian[index]; + return &metrics.stateEqLagrangian[index]; } else if (ocp.finalInequalityLagrangianPtr->getTermIndex(name, index)) { - return &metricsColl.stateIneqLagrangian[index]; + return &metrics.stateIneqLagrangian[index]; } else { return nullptr; @@ -217,16 +214,15 @@ const LagrangianMetrics* extractFinalTermLagrangianMetrics(const OptimalControlP /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollArray, - std::vector<std::reference_wrapper<const vector_t>>& metricsArray) { - metricsArray.clear(); +bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const std::vector<Metrics>& metricsArray, + std::vector<std::reference_wrapper<const vector_t>>& constraintArray) { + constraintArray.clear(); size_t index; if (ocp.preJumpEqualityConstraintPtr->getTermIndex(name, index)) { - metricsArray.reserve(metricsCollArray.size()); - for (const auto& metricsColl : metricsCollArray) { - metricsArray.emplace_back(metricsColl.stateEqConstraint[index]); + constraintArray.reserve(metricsArray.size()); + for (const auto& m : metricsArray) { + constraintArray.emplace_back(m.stateEqConstraint[index]); } return true; @@ -239,22 +235,22 @@ bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::s /******************************************************************************************************/ /******************************************************************************************************/ bool extractPreJumpTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollArray, - std::vector<LagrangianMetricsConstRef>& metricsArray) { - metricsArray.clear(); + const std::vector<Metrics>& metricsArray, + std::vector<LagrangianMetricsConstRef>& lagrangianMetricsArray) { + lagrangianMetricsArray.clear(); size_t index; if (ocp.preJumpEqualityLagrangianPtr->getTermIndex(name, index)) { - metricsArray.reserve(metricsCollArray.size()); - for (const auto& metricsColl : metricsCollArray) { - metricsArray.push_back(metricsColl.stateEqLagrangian[index]); + lagrangianMetricsArray.reserve(metricsArray.size()); + for (const auto& m : metricsArray) { + lagrangianMetricsArray.push_back(m.stateEqLagrangian[index]); } return true; } else if (ocp.preJumpInequalityLagrangianPtr->getTermIndex(name, index)) { - metricsArray.reserve(metricsCollArray.size()); - for (const auto& metricsColl : metricsCollArray) { - metricsArray.push_back(metricsColl.stateIneqLagrangian[index]); + lagrangianMetricsArray.reserve(metricsArray.size()); + for (const auto& m : metricsArray) { + lagrangianMetricsArray.push_back(m.stateIneqLagrangian[index]); } return true; @@ -266,23 +262,22 @@ bool extractPreJumpTermLagrangianMetrics(const OptimalControlProblem& ocp, const /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollTraj, - std::vector<std::reference_wrapper<const vector_t>>& metricsTrajectory) { - metricsTrajectory.clear(); +bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const std::vector<Metrics>& metricsTraj, + std::vector<std::reference_wrapper<const vector_t>>& constraintTraj) { + constraintTraj.clear(); size_t index; if (ocp.equalityConstraintPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.emplace_back(metricsColl.stateInputEqConstraint[index]); + constraintTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + constraintTraj.emplace_back(m.stateInputEqConstraint[index]); } return true; } else if (ocp.stateEqualityConstraintPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.emplace_back(metricsColl.stateEqConstraint[index]); + constraintTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + constraintTraj.emplace_back(m.stateEqConstraint[index]); } return true; @@ -295,36 +290,36 @@ bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const s /******************************************************************************************************/ /******************************************************************************************************/ bool extractIntermediateTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector<MetricsCollection>& metricsCollTraj, - std::vector<LagrangianMetricsConstRef>& metricsTrajectory) { - metricsTrajectory.clear(); + const std::vector<Metrics>& metricsTraj, + std::vector<LagrangianMetricsConstRef>& lagrangianMetricsTraj) { + lagrangianMetricsTraj.clear(); size_t index; if (ocp.equalityLagrangianPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.push_back(metricsColl.stateInputEqLagrangian[index]); + lagrangianMetricsTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + lagrangianMetricsTraj.push_back(m.stateInputEqLagrangian[index]); } return true; } else if (ocp.stateEqualityLagrangianPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.push_back(metricsColl.stateEqLagrangian[index]); + lagrangianMetricsTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + lagrangianMetricsTraj.push_back(m.stateEqLagrangian[index]); } return true; } else if (ocp.inequalityLagrangianPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.push_back(metricsColl.stateInputIneqLagrangian[index]); + lagrangianMetricsTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + lagrangianMetricsTraj.push_back(m.stateInputIneqLagrangian[index]); } return true; } else if (ocp.stateInequalityLagrangianPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.push_back(metricsColl.stateIneqLagrangian[index]); + lagrangianMetricsTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + lagrangianMetricsTraj.push_back(m.stateIneqLagrangian[index]); } return true; diff --git a/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp b/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp index 6d3c0e8b9..f2442d8d7 100644 --- a/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp +++ b/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp @@ -49,7 +49,7 @@ scalar_t rolloutCost(cost_wraper_t costWraper, const scalar_array_t& timeTraject costTrajectory.push_back(costWraper(timeTrajectory[i], stateTrajectory[i], inputTrajectory[i])); } - return trapezoidalIntegration(timeTrajectory, costTrajectory); + return trapezoidalIntegration(timeTrajectory, costTrajectory, 0.0); } /******************************************************************************************************/ @@ -68,7 +68,7 @@ scalar_t rolloutConstraint(constraints_wraper_t constraintWraper, const scalar_a constraintTrajectoryISE.push_back(constraint.squaredNorm()); } - return trapezoidalIntegration(timeTrajectory, constraintTrajectoryISE); + return trapezoidalIntegration(timeTrajectory, constraintTrajectoryISE, 0.0); } } // namespace PerformanceIndicesRollout diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp index f3c6fdce3..5a4262793 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp @@ -34,15 +34,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/test/circular_kinematics.h> #include <ocs2_oc/test/testProblemsGeneration.h> -namespace { -/** Helper to compare if two performance indices are identical */ -bool areIdentical(const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) { - return lhs.merit == rhs.merit && lhs.cost == rhs.cost && lhs.dynamicsViolationSSE == rhs.dynamicsViolationSSE && - lhs.equalityConstraintsSSE == rhs.equalityConstraintsSSE && lhs.equalityLagrangian == rhs.equalityLagrangian && - lhs.inequalityLagrangian == rhs.inequalityLagrangian; -} -} // namespace - using namespace ocs2; using namespace ocs2::multiple_shooting; @@ -62,7 +53,7 @@ TEST(test_transcription, intermediate_performance) { const auto performance = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_TRUE(performance.isApprox(transcription.performance, 1e-12)); } TEST(test_transcription, terminal_performance) { @@ -82,7 +73,7 @@ TEST(test_transcription, terminal_performance) { const auto transcription = setupTerminalNode(problem, t, x); const auto performance = computeTerminalPerformance(problem, t, x); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_TRUE(performance.isApprox(transcription.performance, 1e-12)); } TEST(test_transcription, event_performance) { @@ -107,5 +98,5 @@ TEST(test_transcription, event_performance) { const auto transcription = setupEventNode(problem, t, x, x_next); const auto performance = computeEventPerformance(problem, t, x, x_next); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_TRUE(performance.isApprox(transcription.performance, 1e-12)); } From 662ea0106f7b74e8916b30d8f5bafb1ecebf95c2 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 8 Aug 2022 17:53:48 +0200 Subject: [PATCH 270/512] passing dynamicsViolation to computeIntermediateMetrics --- .../approximate_model/LinearQuadraticApproximator.h | 3 ++- .../approximate_model/LinearQuadraticApproximator.cpp | 11 ++++++++++- .../OptimalControlProblemHelperFunction.cpp | 3 +-- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h index f076843fb..3a14543a0 100644 --- a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h +++ b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h @@ -171,10 +171,11 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr * @param [in] state: The current state. * @param [in] input: The current input. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. + * @param [in] dynamicsViolation: The violation dynamics. It depends on the transcription method. * @return The output Metrics. */ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, - const MultiplierCollection& multipliers); + const MultiplierCollection& multipliers, const vector_t& dynamicsViolation = vector_t()); /** * Compute the event-time Metrics based on pre-jump state value (i.e. cost, softConstraints, and constraints). diff --git a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp index e91cd2822..8b0d874f8 100644 --- a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp +++ b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp @@ -270,7 +270,7 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr /******************************************************************************************************/ /******************************************************************************************************/ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, - const MultiplierCollection& multipliers) { + const MultiplierCollection& multipliers, const vector_t& dynamicsViolation) { auto& preComputation = *problem.preComputationPtr; Metrics metrics; @@ -278,6 +278,9 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_ // Cost metrics.cost = computeCost(problem, time, state, input); + // Dynamics violation + metrics.dynamicsViolation = dynamicsViolation; + // Equality constraints metrics.stateEqConstraint = problem.stateEqualityConstraintPtr->getValue(time, state, preComputation); metrics.stateInputEqConstraint = problem.equalityConstraintPtr->getValue(time, state, input, preComputation); @@ -304,6 +307,9 @@ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t tim // Cost metrics.cost = computeEventCost(problem, time, state); + // Dynamics violation + // metrics.dynamicsViolation = vector_t(); + // Equality constraint metrics.stateEqConstraint = problem.preJumpEqualityConstraintPtr->getValue(time, state, preComputation); @@ -326,6 +332,9 @@ Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, // Cost metrics.cost = computeFinalCost(problem, time, state); + // Dynamics violation + // metrics.dynamicsViolation = vector_t(); + // Equality constraint metrics.stateEqConstraint = problem.finalEqualityConstraintPtr->getValue(time, state, preComputation); diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp index af6d7d175..7d61d273a 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp @@ -105,8 +105,7 @@ void initializePreJumpMultiplierCollection(const OptimalControlProblem& ocp, sca /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void initializeIntermediateMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, - MultiplierCollection& multipliers) { +void initializeIntermediateMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multipliers) { ocp.stateEqualityLagrangianPtr->initializeLagrangian(time, multipliers.stateEq); ocp.stateInequalityLagrangianPtr->initializeLagrangian(time, multipliers.stateIneq); ocp.equalityLagrangianPtr->initializeLagrangian(time, multipliers.stateInputEq); From 971f2c35664cadfd93ed7560bdb0051ba7f8d01b Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 14:07:02 +0200 Subject: [PATCH 271/512] getDualSolution returns a pointer --- ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h | 2 +- ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h | 2 +- ocs2_oc/src/oc_solver/SolverBase.cpp | 4 +++- ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h | 4 +--- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h index b24f50c26..e91c5b5ed 100644 --- a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h +++ b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h @@ -89,7 +89,7 @@ class GaussNewtonDDP : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const final; - const DualSolution& getDualSolution() const override { return optimizedDualSolution_; } + const DualSolution* getDualSolution() const override { return &optimizedDualSolution_; } const ProblemMetrics& getSolutionMetrics() const override { return optimizedProblemMetrics_; } diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h index d81614eb1..f574cb04a 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h @@ -195,7 +195,7 @@ class SolverBase { * * @return: The dual problem's solution. */ - virtual const DualSolution& getDualSolution() const = 0; + virtual const DualSolution* getDualSolution() const = 0; /** * @brief Returns the optimized value of the Metrics. diff --git a/ocs2_oc/src/oc_solver/SolverBase.cpp b/ocs2_oc/src/oc_solver/SolverBase.cpp index 6c07ac091..127038062 100644 --- a/ocs2_oc/src/oc_solver/SolverBase.cpp +++ b/ocs2_oc/src/oc_solver/SolverBase.cpp @@ -110,7 +110,9 @@ void SolverBase::postRun() { for (auto& observer : solverObservers_) { observer->extractTermConstraint(getOptimalControlProblem(), solution, getSolutionMetrics()); observer->extractTermLagrangianMetrics(getOptimalControlProblem(), solution, getSolutionMetrics()); - observer->extractTermMultipliers(getOptimalControlProblem(), getDualSolution()); + if (getDualSolution() != nullptr) { + observer->extractTermMultipliers(getOptimalControlProblem(), *getDualSolution()); + } } } } diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index 665fa58ee..910b39019 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -66,9 +66,7 @@ class MultipleShootingSolver : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } - const DualSolution& getDualSolution() const override { - throw std::runtime_error("[MultipleShootingSolver] getDualSolution() not available yet."); - } + const DualSolution* getDualSolution() const override { return nullptr; } const ProblemMetrics& getSolutionMetrics() const override { throw std::runtime_error("[MultipleShootingSolver] getSolutionMetrics() not available yet."); From a1aa691be8e714af7407d8460fe6c718321718b4 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 14:10:20 +0200 Subject: [PATCH 272/512] adding dynamicsViolation parameter to computePreJumpMetrics --- .../ocs2_oc/approximate_model/LinearQuadraticApproximator.h | 3 ++- ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h index 3a14543a0..a055da793 100644 --- a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h +++ b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h @@ -187,10 +187,11 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_ * @param [in] time: The current time. * @param [in] state: The current state. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. + * @param [in] dynamicsViolation: The violation dynamics. It depends on the transcription method. * @return The output Metrics. */ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers); + const MultiplierCollection& multipliers, const vector_t& dynamicsViolation = vector_t()); /** * Compute the final-time Metrics (i.e. cost, softConstraints, and constraints). diff --git a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp index 8b0d874f8..fc2b984d8 100644 --- a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp +++ b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp @@ -299,7 +299,7 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_ /******************************************************************************************************/ /******************************************************************************************************/ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers) { + const MultiplierCollection& multipliers, const vector_t& dynamicsViolation) { auto& preComputation = *problem.preComputationPtr; Metrics metrics; @@ -308,7 +308,7 @@ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t tim metrics.cost = computeEventCost(problem, time, state); // Dynamics violation - // metrics.dynamicsViolation = vector_t(); + metrics.dynamicsViolation = dynamicsViolation; // Equality constraint metrics.stateEqConstraint = problem.preJumpEqualityConstraintPtr->getValue(time, state, preComputation); From 68eec93a8a9720e3f893fb65e21cf3d79db47a68 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 14:20:53 +0200 Subject: [PATCH 273/512] adding ProblemMetrics to sqp --- .../include/ocs2_sqp/MultipleShootingSolver.h | 14 +++-- .../ocs2_sqp/MultipleShootingTranscription.h | 20 +++---- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 43 ++++++++++----- .../src/MultipleShootingTranscription.cpp | 54 ++++++++++--------- 4 files changed, 77 insertions(+), 54 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index 910b39019..3cede1351 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/initialization/Initializer.h> #include <ocs2_core/integration/SensitivityIntegrator.h> #include <ocs2_core/misc/Benchmark.h> +#include <ocs2_core/model_data/Metrics.h> #include <ocs2_core/thread_support/ThreadPool.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> @@ -68,9 +69,7 @@ class MultipleShootingSolver : public SolverBase { const DualSolution* getDualSolution() const override { return nullptr; } - const ProblemMetrics& getSolutionMetrics() const override { - throw std::runtime_error("[MultipleShootingSolver] getSolutionMetrics() not available yet."); - } + const ProblemMetrics& getSolutionMetrics() const override { return problemMetrics_; } size_t getNumIterations() const override { return totalNumIterations_; } @@ -131,7 +130,7 @@ class MultipleShootingSolver : public SolverBase { /** Computes only the performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u); + const vector_array_t& u, ProblemMetrics& problemMetrics); /** Returns solution of the QP subproblem in delta coordinates: */ struct OcpSubproblemSolution { @@ -156,7 +155,7 @@ class MultipleShootingSolver : public SolverBase { /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ multiple_shooting::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, - vector_array_t& u); + vector_array_t& u, ProblemMetrics& problemMetrics); /** Determine convergence after a step */ multiple_shooting::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, @@ -190,6 +189,11 @@ class MultipleShootingSolver : public SolverBase { // Iteration performance log std::vector<PerformanceIndex> performanceIndeces_; + // The ProblemMetrics associated to primalSolution_ + ProblemMetrics problemMetrics_; + // Memory used within the search strategy + ProblemMetrics problemMetricsNew_; + // Benchmarking size_t numProblems_{0}; size_t totalNumIterations_{0}; diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h index 92985b915..81a986817 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -66,11 +66,10 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); /** - * Compute only the performance index for a single intermediate node. - * Corresponds to the performance index returned by "setupIntermediateNode" + * Compute only the Metrics for a single intermediate node. */ -PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); +Metrics computeIntermediateMetrics(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); /** * Results of the transcription at a terminal node @@ -92,10 +91,9 @@ struct TerminalTranscription { TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); /** - * Compute only the performance index for the terminal node. - * Corresponds to the performance index returned by "setTerminalNode" + * Compute only the Metrics for the terminal node. */ -PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); +Metrics computeTerminalMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); /** * Results of the transcription at an event @@ -120,11 +118,9 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro const vector_t& x_next); /** - * Compute only the performance index for the event node. - * Corresponds to the performance index returned by "setupEventNode" + * Compute only the Metrics for the event node. */ -PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next); +Metrics computeEventMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 99e0a70c0..d212b6dc9 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -187,7 +187,7 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat // Apply step linesearchTimer_.startTimer(); - const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u); + const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, problemMetrics_); performanceIndeces_.push_back(stepInfo.performanceAfterStep); linesearchTimer_.endTimer(); @@ -449,27 +449,43 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec } PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u) { + const vector_array_t& x, const vector_array_t& u, + ProblemMetrics& problemMetrics) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; + // extract indices for setting up problemMetrics + int numEvents = 0, numIntermediates = 0; + std::vector<int> indices(N); + for (int i = 0; i < N; ++i) { + indices[i] = (time[i].event == AnnotatedTime::Event::PreEvent) ? numEvents++ : numIntermediates++; + } + + // resize + problemMetrics.clear(); + problemMetrics.preJumps.resize(numEvents); + problemMetrics.intermediates.resize(numIntermediates); + std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; - PerformanceIndex workerPerformance; // Accumulate performance in local variable int i = timeIndex++; while (i < N) { + const auto problemMetricsIndex = indices[i]; if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node - workerPerformance += multiple_shooting::computeEventPerformance(ocpDefinition, time[i].time, x[i], x[i + 1]); + problemMetrics.preJumps[problemMetricsIndex] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]); + performance[workerId] += toPerformanceIndex(problemMetrics.preJumps[problemMetricsIndex]); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + problemMetrics.intermediates[problemMetricsIndex] = + multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + performance[workerId] += dt * toPerformanceIndex(problemMetrics.intermediates[problemMetricsIndex]); } i = timeIndex++; @@ -477,20 +493,20 @@ PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector<An if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); - workerPerformance += multiple_shooting::computeTerminalPerformance(ocpDefinition, tN, x[N]); + problemMetrics.final = multiple_shooting::computeTerminalMetrics(ocpDefinition, tN, x[N]); + performance[workerId] += toPerformanceIndex(problemMetrics.final); } - - // Accumulate! Same worker might run multiple tasks - performance[workerId] += workerPerformance; }; runParallel(std::move(parallelTask)); - // Account for init state in performance + // Account for initial state in performance + problemMetrics.intermediates.front().dynamicsViolation += (initState - x.front()); performance.front().dynamicsViolationSSE += (initState - x.front()).squaredNorm(); // Sum performance of the threads PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + return totalPerformance; } @@ -509,7 +525,7 @@ scalar_t MultipleShootingSolver::totalConstraintViolation(const PerformanceIndex multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, - vector_array_t& x, vector_array_t& u) { + vector_array_t& x, vector_array_t& u, ProblemMetrics& problemMetrics) { using StepType = multiple_shooting::StepInfo::StepType; /* @@ -550,7 +566,7 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn } // Compute cost and constraints - const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew); + const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew, problemMetricsNew_); const scalar_t newConstraintViolation = totalConstraintViolation(performanceNew); // Step acceptance and record step type @@ -582,6 +598,7 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn if (stepAccepted) { // Return if step accepted x = std::move(xNew); u = std::move(uNew); + problemMetrics.swap(problemMetricsNew_); stepInfo.stepSize = alpha; stepInfo.dx_norm = alpha * deltaXnorm; @@ -604,6 +621,8 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn } while (alpha >= settings_.alpha_min); // Alpha_min reached -> Don't take a step + std::ignore = computePerformance(timeDiscretization, initState, xNew, uNew, problemMetrics); + stepInfo.stepSize = 0.0; stepInfo.stepType = StepType::ZERO; stepInfo.dx_norm = 0.0; diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index d6fb87a08..80e5c2c06 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -84,29 +84,26 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP return transcription; } -PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { - PerformanceIndex performance; +Metrics computeIntermediateMetrics(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { + Metrics metrics; // Dynamics - vector_t dynamicsGap = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); - dynamicsGap -= x_next; - performance.dynamicsViolationSSE = dt * dynamicsGap.squaredNorm(); + metrics.dynamicsViolation = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); + metrics.dynamicsViolation -= x_next; // Precomputation for other terms constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; optimalControlProblem.preComputationPtr->request(request, t, x, u); - // Costs - performance.cost = dt * computeCost(optimalControlProblem, t, x, u); + // Cost + metrics.cost = computeCost(optimalControlProblem, t, x, u); // Constraints - if (!optimalControlProblem.equalityConstraintPtr->empty()) { - const auto constraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - performance.equalityConstraintsSSE = dt * constraintsSquaredNorm(constraints); - } + metrics.stateEqConstraint = optimalControlProblem.stateEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + metrics.stateInputEqConstraint = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - return performance; + return metrics; } TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { @@ -127,15 +124,19 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont return transcription; } -PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { - PerformanceIndex performance; +Metrics computeTerminalMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { + Metrics metrics; constexpr auto request = Request::Cost + Request::SoftConstraint; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); - performance.cost = computeFinalCost(optimalControlProblem, t, x); + // Cost + metrics.cost = computeFinalCost(optimalControlProblem, t, x); + + // Equality constraint + metrics.stateEqConstraint = optimalControlProblem.finalEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - return performance; + return metrics; } EventTranscription setupEventNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, @@ -164,20 +165,23 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro return transcription; } -PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next) { - PerformanceIndex performance; +Metrics computeEventMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { + Metrics metrics; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics; optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); - // Dynamics - const vector_t dynamicsGap = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x) - x_next; - performance.dynamicsViolationSSE = dynamicsGap.squaredNorm(); + // Cost + metrics.cost = computeEventCost(optimalControlProblem, t, x); + + // Dynamics violation + metrics.dynamicsViolation = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x); + metrics.dynamicsViolation -= x_next; - performance.cost = computeEventCost(optimalControlProblem, t, x); + // Equality constraint + metrics.stateEqConstraint = optimalControlProblem.preJumpEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - return performance; + return metrics; } } // namespace multiple_shooting From 42a32f8b1ebac22c0264fca2e973b76a807e0519 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 14:34:40 +0200 Subject: [PATCH 274/512] legged robot example --- .../launch/legged_robot_ddp.launch | 3 ++- .../launch/legged_robot_sqp.launch | 1 + .../src/LeggedRobotDdpMpcNode.cpp | 26 +++++++++++-------- .../src/LeggedRobotSqpMpcNode.cpp | 22 ++++++++++++++-- 4 files changed, 38 insertions(+), 14 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch index 54827005b..d0d0f7611 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch @@ -4,7 +4,7 @@ <!-- visualization config --> <arg name="rviz" default="true" /> <arg name="description_name" default="legged_robot_description"/> - <arg name="multiplot" default="true"/> + <arg name="multiplot" default="false"/> <!-- The task file for the mpc. --> <arg name="taskFile" default="$(find ocs2_legged_robot)/config/mpc/task.info"/> @@ -28,6 +28,7 @@ </group> <!-- make the files into global parameters --> + <param name="multiplot" value="$(arg multiplot)"/> <param name="taskFile" value="$(arg taskFile)" /> <param name="referenceFile" value="$(arg referenceFile)" /> <param name="urdfFile" value="$(arg urdfFile)" /> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch index 36f6fe62f..8e11f2138 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch @@ -28,6 +28,7 @@ </group> <!-- make the files into global parameters --> + <param name="multiplot" value="$(arg multiplot)"/> <param name="taskFile" value="$(arg taskFile)" /> <param name="referenceFile" value="$(arg referenceFile)" /> <param name="urdfFile" value="$(arg urdfFile)" /> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp index 9dec1509b..355ba0234 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp @@ -48,7 +48,9 @@ int main(int argc, char** argv) { ::ros::init(argc, argv, robotName + "_mpc"); ::ros::NodeHandle nodeHandle; // Get node parameters + bool multiplot = false; std::string taskFile, urdfFile, referenceFile; + nodeHandle.getParam("/multiplot", multiplot); nodeHandle.getParam("/taskFile", taskFile); nodeHandle.getParam("/referenceFile", referenceFile); nodeHandle.getParam("/urdfFile", urdfFile); @@ -70,17 +72,19 @@ int main(int argc, char** argv) { mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - // observer for zero velocity constraints - auto createStateInputBoundsObserver = [&](const std::string& termName) { - const ocs2::scalar_array_t observingTimePoints{0.0}; - const std::vector<std::string> topicNames{"metrics/" + termName + "/0MsLookAhead"}; - auto callback = - ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); - return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); - }; - for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { - const std::string& footName = interface.modelSettings().contactNames3DoF[i]; - mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + if (multiplot) { + auto createStateInputBoundsObserver = [&](const std::string& termName) { + const ocs2::scalar_array_t observingTimePoints{0.0}; + const std::vector<std::string> topicNames{"metrics/" + termName + "/0MsLookAhead"}; + auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + }; + for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + } } // Launch MPC ROS node diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp index 13953b284..3f8650f51 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp @@ -33,6 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_legged_robot/LeggedRobotInterface.h> #include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> +#include <ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h> #include <ocs2_sqp/MultipleShootingMpc.h> #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" @@ -44,10 +45,12 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + ::ros::init(argc, argv, robotName + "_mpc"); + ::ros::NodeHandle nodeHandle; // Get node parameters + bool multiplot = false; std::string taskFile, urdfFile, referenceFile; + nodeHandle.getParam("/multiplot", multiplot); nodeHandle.getParam("/taskFile", taskFile); nodeHandle.getParam("/urdfFile", urdfFile); nodeHandle.getParam("/referenceFile", referenceFile); @@ -69,6 +72,21 @@ int main(int argc, char** argv) { mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + if (multiplot) { + auto createStateInputBoundsObserver = [&](const std::string& termName) { + const ocs2::scalar_array_t observingTimePoints{0.0}; + const std::vector<std::string> topicNames{"metrics/" + termName + "/0MsLookAhead"}; + auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + }; + for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + } + } + // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); mpcNode.launchNodes(nodeHandle); From 8d6f888c0c64b28adfdf3cab6f4c7e2e1caad440 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 14:40:53 +0200 Subject: [PATCH 275/512] fix trapezoidalIntegration unittest --- ocs2_core/test/integration/TrapezoidalIntegrationTest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_core/test/integration/TrapezoidalIntegrationTest.cpp b/ocs2_core/test/integration/TrapezoidalIntegrationTest.cpp index dcd69bd98..37f86baba 100644 --- a/ocs2_core/test/integration/TrapezoidalIntegrationTest.cpp +++ b/ocs2_core/test/integration/TrapezoidalIntegrationTest.cpp @@ -46,7 +46,7 @@ TEST(TrapezoidalIntegrationTest, Rectangle) { ocs2::scalar_array_t yTrj(numIntervals + 1, height); - const auto area = ocs2::trapezoidalIntegration(xTrj, yTrj); + const auto area = ocs2::trapezoidalIntegration(xTrj, yTrj, 0.0); ASSERT_NEAR(area, width * height, 1e-12); } @@ -65,6 +65,6 @@ TEST(TrapezoidalIntegrationTest, RightTriangle) { auto y = -dy; std::generate(yTrj.begin(), yTrj.end(), [&y, dy]() { return (y += dy); }); - const auto area = ocs2::trapezoidalIntegration(xTrj, yTrj); + const auto area = ocs2::trapezoidalIntegration(xTrj, yTrj, 0.0); ASSERT_NEAR(area, width * height / 2.0, 1e-12); } From cad6bb4bb205da7090f64262711d5685a9c58438 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 16:41:40 +0200 Subject: [PATCH 276/512] bugfix for fuzzy compare --- ocs2_core/src/model_data/Metrics.cpp | 2 +- ocs2_core/test/model_data/testMetrics.cpp | 3 +++ ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 3cb744663..191ceb846 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -36,7 +36,7 @@ namespace ocs2 { /******************************************************************************************************/ /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ bool Metrics::isApprox(const Metrics& other, scalar_t prec) const { - bool flag = std::abs(this->cost - other.cost) < prec * std::min(std::abs(this->cost), std::abs(other.cost)); + bool flag = std::abs(this->cost - other.cost) <= prec * std::min(std::abs(this->cost), std::abs(other.cost)); flag = flag && this->dynamicsViolation.isApprox(other.dynamicsViolation, prec); flag = flag && toVector(this->stateEqConstraint).isApprox(toVector(other.stateEqConstraint), prec); flag = flag && toVector(this->stateInputEqConstraint).isApprox(toVector(other.stateInputEqConstraint), prec); diff --git a/ocs2_core/test/model_data/testMetrics.cpp b/ocs2_core/test/model_data/testMetrics.cpp index 806ab6617..3c71576b5 100644 --- a/ocs2_core/test/model_data/testMetrics.cpp +++ b/ocs2_core/test/model_data/testMetrics.cpp @@ -84,6 +84,9 @@ inline Metrics interpolateNew(const LinearInterpolation::index_alpha_t& indexAlp // cost out.cost = LinearInterpolation::interpolate( indexAlpha, dataArray, [](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].cost; }); + // dynamics violation + out.dynamicsViolation = LinearInterpolation::interpolate( + indexAlpha, dataArray, [](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].dynamicsViolation; }); // constraints out.stateEqConstraint = toConstraintArray(getSizes(dataArray[index].stateEqConstraint), f(lhs_stateEqConst, rhs_stateEqConst)); out.stateInputEqConstraint = toConstraintArray(getSizes(dataArray[index].stateInputEqConstraint), f(lhs_stateInputEqConst, rhs_stateInputEqConst)); diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index 891a3bae8..0cb129c0f 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -96,7 +96,7 @@ struct PerformanceIndex { /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ bool isApprox(const PerformanceIndex other, const scalar_t prec = 1e-8) const { - auto fuzzyCompares = [&](const scalar_t a, const scalar_t b) { return std::abs(a - b) < prec * std::min(std::abs(a), std::abs(b)); }; + auto fuzzyCompares = [&](const scalar_t a, const scalar_t b) { return std::abs(a - b) <= prec * std::min(std::abs(a), std::abs(b)); }; bool isEqual = fuzzyCompares(this->merit, other.merit); isEqual = isEqual && fuzzyCompares(this->cost, other.cost); isEqual = isEqual && fuzzyCompares(this->dynamicsViolationSSE, other.dynamicsViolationSSE); From d87e73a7f9dd039081003c4f57b597f21b7d9781 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 17:01:32 +0200 Subject: [PATCH 277/512] bugfix --- ocs2_core/src/model_data/Metrics.cpp | 5 ++++- ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 191ceb846..624385c8c 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_core/model_data/Metrics.h" +#include <limits> + namespace ocs2 { /******************************************************************************************************/ @@ -36,7 +38,8 @@ namespace ocs2 { /******************************************************************************************************/ /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ bool Metrics::isApprox(const Metrics& other, scalar_t prec) const { - bool flag = std::abs(this->cost - other.cost) <= prec * std::min(std::abs(this->cost), std::abs(other.cost)); + bool flag = std::abs(this->cost - other.cost) <= prec * std::min(std::abs(this->cost), std::abs(other.cost)) || + std::abs(this->cost - other.cost) < std::numeric_limits<scalar_t>::min(); flag = flag && this->dynamicsViolation.isApprox(other.dynamicsViolation, prec); flag = flag && toVector(this->stateEqConstraint).isApprox(toVector(other.stateEqConstraint), prec); flag = flag && toVector(this->stateInputEqConstraint).isApprox(toVector(other.stateInputEqConstraint), prec); diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index 0cb129c0f..58ee34a8a 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include <iomanip> +#include <limits> #include <ostream> #include <ocs2_core/Types.h> @@ -96,7 +97,9 @@ struct PerformanceIndex { /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ bool isApprox(const PerformanceIndex other, const scalar_t prec = 1e-8) const { - auto fuzzyCompares = [&](const scalar_t a, const scalar_t b) { return std::abs(a - b) <= prec * std::min(std::abs(a), std::abs(b)); }; + auto fuzzyCompares = [&](const scalar_t a, const scalar_t b) { + return std::abs(a - b) <= prec * std::min(std::abs(a), std::abs(b)) || std::abs(a - b) < std::numeric_limits<scalar_t>::min(); + }; bool isEqual = fuzzyCompares(this->merit, other.merit); isEqual = isEqual && fuzzyCompares(this->cost, other.cost); isEqual = isEqual && fuzzyCompares(this->dynamicsViolationSSE, other.dynamicsViolationSSE); From 04c23fd1957dd1e05a998b99989dfdbfde71ff66 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 17:27:45 +0200 Subject: [PATCH 278/512] clang format --- ocs2_core/src/model_data/Metrics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 624385c8c..2ab367736 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -39,7 +39,7 @@ namespace ocs2 { /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ bool Metrics::isApprox(const Metrics& other, scalar_t prec) const { bool flag = std::abs(this->cost - other.cost) <= prec * std::min(std::abs(this->cost), std::abs(other.cost)) || - std::abs(this->cost - other.cost) < std::numeric_limits<scalar_t>::min(); + std::abs(this->cost - other.cost) < std::numeric_limits<scalar_t>::min(); flag = flag && this->dynamicsViolation.isApprox(other.dynamicsViolation, prec); flag = flag && toVector(this->stateEqConstraint).isApprox(toVector(other.stateEqConstraint), prec); flag = flag && toVector(this->stateInputEqConstraint).isApprox(toVector(other.stateInputEqConstraint), prec); From 619e93b3ee2d96dd119dbc149109a0851021b8c3 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 17:38:32 +0200 Subject: [PATCH 279/512] fixing unittest --- ocs2_sqp/ocs2_sqp/test/testTranscription.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp index 5a4262793..1b11fa8fe 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp @@ -51,7 +51,7 @@ TEST(test_transcription, intermediate_performance) { const vector_t u = (vector_t(2) << 0.1, 1.3).finished(); const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, true, t, dt, x, x_next, u); - const auto performance = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); + const auto performance = dt * toPerformanceIndex(computeIntermediateMetrics(problem, discretizer, t, dt, x, x_next, u)); ASSERT_TRUE(performance.isApprox(transcription.performance, 1e-12)); } @@ -71,7 +71,7 @@ TEST(test_transcription, terminal_performance) { scalar_t t = 0.5; const vector_t x = vector_t::Random(nx); const auto transcription = setupTerminalNode(problem, t, x); - const auto performance = computeTerminalPerformance(problem, t, x); + const auto performance = toPerformanceIndex(computeTerminalMetrics(problem, t, x)); ASSERT_TRUE(performance.isApprox(transcription.performance, 1e-12)); } @@ -96,7 +96,7 @@ TEST(test_transcription, event_performance) { const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); const auto transcription = setupEventNode(problem, t, x, x_next); - const auto performance = computeEventPerformance(problem, t, x, x_next); + const auto performance = toPerformanceIndex(computeEventMetrics(problem, t, x, x_next)); ASSERT_TRUE(performance.isApprox(transcription.performance, 1e-12)); } From cb9d81775fe2291a7384255cfe22e1c9f3d81c67 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 9 Aug 2022 18:01:10 +0200 Subject: [PATCH 280/512] operator*() --- ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index 58ee34a8a..ec02ebec3 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -115,6 +115,12 @@ inline PerformanceIndex operator+(PerformanceIndex lhs, const PerformanceIndex& return lhs; } +template <typename SCALAR_T> +inline PerformanceIndex operator*(PerformanceIndex lhs, const SCALAR_T c) { + lhs *= c; // Copied lhs + return lhs; +} + template <typename SCALAR_T> PerformanceIndex operator*(const SCALAR_T c, PerformanceIndex rhs) { rhs *= c; // Copied rhs From ea3b7b9e667ef7d1e50011f53c9cf449f7049b09 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 10 Aug 2022 16:38:31 +0200 Subject: [PATCH 281/512] simplifying ddp main loop --- ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h | 20 +- ocs2_ddp/src/GaussNewtonDDP.cpp | 254 ++++++++------------- 2 files changed, 100 insertions(+), 174 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h index 0d8170be1..c77327e87 100644 --- a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h +++ b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h @@ -307,9 +307,7 @@ class GaussNewtonDDP : public SolverBase { void projectLQ(const ModelData& modelData, const matrix_t& constraintRangeProjector, const matrix_t& constraintNullProjector, ModelData& projectedModelData) const; - /** - * Initialize the constraint penalty coefficients. - */ + /** Initialize the constraint penalty coefficients. */ void initializeConstraintPenalties(); /** @@ -318,16 +316,14 @@ class GaussNewtonDDP : public SolverBase { */ void updateConstraintPenalties(scalar_t equalityConstraintsSSE); - /** - * Runs the initialization method for Gauss-Newton DDP. - */ - void runInit(); + /** Initializes the nominal primal and dual solutions based on the optimized ones. */ + void initializeDualPrimal(); - /** - * Runs a single iteration of Gauss-Newton DDP. - * @param [in] lqModelExpectedCost: The expected cost based on the LQ model optimization. - */ - void runIteration(scalar_t lqModelExpectedCost); + /** Constructs and solves the LQ problem around the nominal trajectories. */ + void constructAndSolveLQ(); + + /** Based on the current LQ solution updates the optimized primal and dual solutions. */ + void takePrimalDualStep(scalar_t lqModelExpectedCost); /** * Checks convergence of the main loop of DDP. diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 600711184..a89d9dfc3 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -55,6 +55,9 @@ namespace ocs2 { GaussNewtonDDP::GaussNewtonDDP(ddp::Settings ddpSettings, const RolloutBase& rollout, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) : ddpSettings_(std::move(ddpSettings)), threadPool_(std::max(ddpSettings_.nThreads_, size_t(1)) - 1, ddpSettings_.threadPriority_) { + Eigen::setNbThreads(1); // no multithreading within Eigen. + Eigen::initParallel(); + // check OCP if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { throw std::runtime_error( @@ -863,19 +866,16 @@ void GaussNewtonDDP::updateConstraintPenalties(scalar_t equalityConstraintsSSE) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaussNewtonDDP::runInit() { - // disable Eigen multi-threading - Eigen::setNbThreads(1); - - initializationTimer_.startTimer(); - - // swap primal and dual data to cache - nominalDualData_.swap(cachedDualData_); - nominalPrimalData_.swap(cachedPrimalData_); +void GaussNewtonDDP::initializeDualPrimal() { + constexpr size_t taskId = 0; + constexpr scalar_t stepLength = 0.0; try { - constexpr size_t taskId = 0; - constexpr scalar_t stepLength = 0.0; + // adjust controller + if (!optimizedPrimalSolution_.controllerPtr_->empty()) { + std::ignore = trajectorySpread(optimizedPrimalSolution_.modeSchedule_, getReferenceManager().getModeSchedule(), + getLinearController(optimizedPrimalSolution_)); + } // clear before starting to fill nominalPrimalData_ nominalPrimalData_.clear(); @@ -904,10 +904,10 @@ void GaussNewtonDDP::runInit() { nominalPrimalData_.problemMetrics); // update dual - totalDualSolutionTimer_.startTimer(); + // totalDualSolutionTimer_.startTimer(); // ocs2::updateDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalPrimalData_.problemMetrics, // nominalDualData_.dualSolution); - totalDualSolutionTimer_.endTimer(); + // totalDualSolutionTimer_.endTimer(); // calculates rollout merit performanceIndex_ = @@ -923,14 +923,15 @@ void GaussNewtonDDP::runInit() { } } catch (const std::exception& error) { - const std::string msg = "[GaussNewtonDDP::runInit] Initial controller does not generate a stable rollout.\n"; + const std::string msg = "[GaussNewtonDDP::initializeDualPrimal] Initial controller does not generate a stable rollout.\n"; throw std::runtime_error(msg + error.what()); } - initializationTimer_.endTimer(); - - // initialize penalty coefficients - initializeConstraintPenalties(); +} +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void GaussNewtonDDP::constructAndSolveLQ() { // linearizing the dynamics and quadratizing the cost function along nominal trajectories linearQuadraticApproximationTimer_.startTimer(); approximateOptimalControlProblem(); @@ -941,93 +942,46 @@ void GaussNewtonDDP::runInit() { avgTimeStepBP_ = solveSequentialRiccatiEquations(nominalPrimalData_.modelDataFinalTime.cost); backwardPassTimer_.endTimer(); - // calculate controller + // calculate controller and store the result in unoptimizedController_ computeControllerTimer_.startTimer(); - // calculate controller. Result is stored in an intermediate variable. The optimized controller, the one after searching, will be - // swapped back to corresponding primalDataContainer in the search stage calculateController(); computeControllerTimer_.endTimer(); - - // display - if (ddpSettings_.displayInfo_) { - printRolloutInfo(); - } - - // TODO(mspieler): this is not exception safe - // restore default Eigen thread number - Eigen::setNbThreads(0); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaussNewtonDDP::runIteration(scalar_t lqModelExpectedCost) { - // disable Eigen multi-threading - Eigen::setNbThreads(1); - - // finding the optimal stepLength +void GaussNewtonDDP::takePrimalDualStep(scalar_t lqModelExpectedCost) { + // update primal: run search strategy and find the optimal stepLength searchStrategyTimer_.startTimer(); - - // swap primal and dual data to cache before running search strategy - nominalDualData_.swap(cachedDualData_); - nominalPrimalData_.swap(cachedPrimalData_); - - // run search strategy scalar_t avgTimeStep; const auto& modeSchedule = this->getReferenceManager().getModeSchedule(); - search_strategy::SolutionRef solution(avgTimeStep, nominalDualData_.dualSolution, nominalPrimalData_.primalSolution, - nominalPrimalData_.problemMetrics, performanceIndex_); + search_strategy::SolutionRef solution(avgTimeStep, optimizedDualSolution_, optimizedPrimalSolution_, optimizedProblemMetrics_, + performanceIndex_); const bool success = searchStrategyPtr_->run({initTime_, finalTime_}, initState_, lqModelExpectedCost, unoptimizedController_, - cachedDualData_.dualSolution, modeSchedule, solution); + nominalDualData_.dualSolution, modeSchedule, solution); - // revert to the old solution if search failed if (success) { avgTimeStepFP_ = 0.9 * avgTimeStepFP_ + 0.1 * avgTimeStep; - - } else { // If fail, copy the entire cache back. To keep the consistency of cached data, all cache should be left untouched. - nominalDualData_ = cachedDualData_; - nominalPrimalData_ = cachedPrimalData_; - performanceIndex_ = performanceIndexHistory_.back(); } - searchStrategyTimer_.endTimer(); // update dual totalDualSolutionTimer_.startTimer(); - ocs2::updateDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalPrimalData_.problemMetrics, - nominalDualData_.dualSolution); - performanceIndex_ = computeRolloutPerformanceIndex(nominalPrimalData_.primalSolution.timeTrajectory_, nominalPrimalData_.problemMetrics); - performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); + if (success) { + ocs2::updateDualSolution(optimalControlProblemStock_[0], optimizedPrimalSolution_, optimizedProblemMetrics_, optimizedDualSolution_); + performanceIndex_ = computeRolloutPerformanceIndex(optimizedPrimalSolution_.timeTrajectory_, optimizedProblemMetrics_); + performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); + } totalDualSolutionTimer_.endTimer(); - // update the constraint penalty coefficients - updateConstraintPenalties(performanceIndex_.equalityConstraintsSSE); - - // linearizing the dynamics and quadratizing the cost function along nominal trajectories - linearQuadraticApproximationTimer_.startTimer(); - approximateOptimalControlProblem(); - linearQuadraticApproximationTimer_.endTimer(); - - // solve Riccati equations - backwardPassTimer_.startTimer(); - avgTimeStepBP_ = solveSequentialRiccatiEquations(nominalPrimalData_.modelDataFinalTime.cost); - backwardPassTimer_.endTimer(); - - // calculate controller - computeControllerTimer_.startTimer(); - // calculate controller. Result is stored in an intermediate variable. The optimized controller, the one after searching, will be - // swapped back to corresponding primalDataContainer in the search stage - calculateController(); - computeControllerTimer_.endTimer(); - - // display - if (ddpSettings_.displayInfo_) { - printRolloutInfo(); + // if failed, use nominal and to keep the consistency of cached data, all cache should be left untouched + if (!success) { + optimizedDualSolution_ = nominalDualData_.dualSolution; + optimizedPrimalSolution_ = nominalPrimalData_.primalSolution; + optimizedProblemMetrics_ = nominalPrimalData_.problemMetrics; + performanceIndex_ = performanceIndexHistory_.back(); } - - // TODO(mspieler): this is not exception safe - // restore default Eigen thread number - Eigen::setNbThreads(0); } /******************************************************************************************************/ @@ -1062,26 +1016,18 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala std::cerr << this->getReferenceManager().getModeSchedule() << "\n"; } - initState_ = initState; + // set cost desired trajectories + for (auto& ocp : optimalControlProblemStock_) { + ocp.targetTrajectoriesPtr = &this->getReferenceManager().getTargetTrajectories(); + } + + // initialize parameters initTime_ = initTime; + initState_ = initState; finalTime_ = finalTime; performanceIndexHistory_.clear(); const auto initIteration = totalNumIterations_; - - // adjust controller - if (!optimizedPrimalSolution_.controllerPtr_->empty()) { - std::ignore = trajectorySpread(optimizedPrimalSolution_.modeSchedule_, getReferenceManager().getModeSchedule(), - getLinearController(optimizedPrimalSolution_)); - } - - // check if after the truncation the internal controller is empty - bool unreliableControllerIncrement = optimizedPrimalSolution_.controllerPtr_->empty(); - - // set cost desired trajectories - for (size_t i = 0; i < ddpSettings_.nThreads_; i++) { - const auto& targetTrajectories = this->getReferenceManager().getTargetTrajectories(); - optimalControlProblemStock_[i].targetTrajectoriesPtr = &targetTrajectories; - } + initializeConstraintPenalties(); // initialize penalty coefficients // display if (ddpSettings_.displayInfo_) { @@ -1090,89 +1036,75 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala std::cerr << "\n###################\n"; } - // run DDP initializer and update the member variables - runInit(); + // swap primal and dual data to cache + nominalDualData_.swap(cachedDualData_); + nominalPrimalData_.swap(cachedPrimalData_); + + // optimized --> nominal: initializes the nominal primal and dual solutions based on the optimized ones + initializationTimer_.startTimer(); + initializeDualPrimal(); + performanceIndexHistory_.push_back(performanceIndex_); + initializationTimer_.endTimer(); - // increment iteration counter - totalNumIterations_++; + // check if the rollout is a result of an empty controller + bool unreliableValueFunction = nominalPrimalData_.primalSolution.controllerPtr_->empty(); // convergence variables of the main loop bool isConverged = false; std::string convergenceInfo; // DDP main loop - while (!isConverged && (totalNumIterations_ - initIteration) < ddpSettings_.maxNumIterations_) { + while (true) { // display the iteration's input update norm (before caching the old nominals) - if (ddpSettings_.displayInfo_) { + if (ddpSettings_.displayInfo_ && totalNumIterations_ > initIteration) { std::cerr << "\n###################"; std::cerr << "\n#### Iteration " << (totalNumIterations_ - initIteration); std::cerr << "\n###################\n"; - std::cerr << "max feedforward norm: " << maxControllerUpdateNorm(unoptimizedController_) << "\n"; } - performanceIndexHistory_.push_back(performanceIndex_); - - // run the an iteration of the DDP algorithm and update the member variables - // the controller which is designed solely based on operation trajectories possibly has invalid feedforward. - // Therefore the expected cost/merit (calculated by the Riccati solution) is not reliable as well. - const scalar_t lqModelExpectedCost = - unreliableControllerIncrement ? performanceIndex_.merit : nominalDualData_.valueFunctionTrajectory.front().f; - - // run a DDP iteration and update the member variables - runIteration(lqModelExpectedCost); - - // increment iteration counter - totalNumIterations_++; + // nominal --> nominal: constructs and solves the LQ problem around the nominal trajectories + constructAndSolveLQ(); - // check convergence - std::tie(isConverged, convergenceInfo) = - searchStrategyPtr_->checkConvergence(unreliableControllerIncrement, performanceIndexHistory_.back(), performanceIndex_); - unreliableControllerIncrement = false; - } // end of while loop - - // display the final iteration's input update norm (before caching the old nominals) - if (ddpSettings_.displayInfo_) { - std::cerr << "\n###################"; - std::cerr << "\n#### Final Rollout"; - std::cerr << "\n###################\n"; - std::cerr << "max feedforward norm: " << maxControllerUpdateNorm(unoptimizedController_) << "\n"; - } - - performanceIndexHistory_.push_back(performanceIndex_); + // display + if (ddpSettings_.displayInfo_) { + std::cerr << "max feedforward norm: " << maxControllerUpdateNorm(unoptimizedController_) << "\n"; + } - // finding the final optimal stepLength and getting the optimal trajectories and controller - searchStrategyTimer_.startTimer(); + // if lqModelExpectedCost==true, the expected cost/merit calculated by the Riccati solution is not reliable + const auto lqModelExpectedCost = unreliableValueFunction ? performanceIndex_.merit : nominalDualData_.valueFunctionTrajectory.front().f; - // run search strategy - scalar_t avgTimeStep; - const auto& modeSchedule = this->getReferenceManager().getModeSchedule(); - const auto lqModelExpectedCost = nominalDualData_.valueFunctionTrajectory.front().f; - search_strategy::SolutionRef solution(avgTimeStep, optimizedDualSolution_, optimizedPrimalSolution_, optimizedProblemMetrics_, - performanceIndex_); - const bool success = searchStrategyPtr_->run({initTime_, finalTime_}, initState_, lqModelExpectedCost, unoptimizedController_, - nominalDualData_.dualSolution, modeSchedule, solution); + // nominal --> optimized: based on the current LQ solution updates the optimized primal and dual solutions + takePrimalDualStep(lqModelExpectedCost); - // revert to the old solution if search failed - if (success) { - avgTimeStepFP_ = 0.9 * avgTimeStepFP_ + 0.1 * avgTimeStep; + // iteration info + ++totalNumIterations_; + performanceIndexHistory_.push_back(performanceIndex_); - } else { // If fail, copy the entire cache back. To keep the consistency of cached data, all cache should be left untouched. - optimizedDualSolution_ = nominalDualData_.dualSolution; - optimizedPrimalSolution_ = nominalPrimalData_.primalSolution; - optimizedProblemMetrics_ = nominalPrimalData_.problemMetrics; - performanceIndex_ = performanceIndexHistory_.back(); - } + // display + if (ddpSettings_.displayInfo_) { + printRolloutInfo(); + } - searchStrategyTimer_.endTimer(); + // check convergence + std::tie(isConverged, convergenceInfo) = searchStrategyPtr_->checkConvergence( + unreliableValueFunction, *std::prev(performanceIndexHistory_.end(), 2), performanceIndexHistory_.back()); + unreliableValueFunction = false; - // update dual - totalDualSolutionTimer_.startTimer(); - ocs2::updateDualSolution(optimalControlProblemStock_[0], optimizedPrimalSolution_, optimizedProblemMetrics_, optimizedDualSolution_); - performanceIndex_ = computeRolloutPerformanceIndex(optimizedPrimalSolution_.timeTrajectory_, optimizedProblemMetrics_); - performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); - totalDualSolutionTimer_.endTimer(); + if (isConverged || (totalNumIterations_ - initIteration) == ddpSettings_.maxNumIterations_) { + break; - performanceIndexHistory_.push_back(performanceIndex_); + } else { + // update the constraint penalty coefficients + updateConstraintPenalties(performanceIndex_.equalityConstraintsSSE); + + // optimized --> nominal: use the optimized solution as the nominal for the next iteration + nominalDualData_.swap(cachedDualData_); + nominalPrimalData_.swap(cachedPrimalData_); + optimizedDualSolution_.swap(nominalDualData_.dualSolution); + optimizedPrimalSolution_.swap(nominalPrimalData_.primalSolution); + optimizedProblemMetrics_.swap(nominalPrimalData_.problemMetrics); + } + } // end of while loop // display if (ddpSettings_.displayInfo_ || ddpSettings_.displayShortSummary_) { @@ -1182,8 +1114,6 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala std::cerr << "Time Period: [" << initTime_ << " ," << finalTime_ << "]\n"; std::cerr << "Number of Iterations: " << (totalNumIterations_ - initIteration) << " out of " << ddpSettings_.maxNumIterations_ << "\n"; - printRolloutInfo(); - if (isConverged) { std::cerr << convergenceInfo << std::endl; } else if (totalNumIterations_ - initIteration == ddpSettings_.maxNumIterations_) { From b9ccc8cc5a2c5c356723a96cc845b1f042dd28db Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 10 Aug 2022 18:50:53 +0200 Subject: [PATCH 282/512] printout fix --- ocs2_ddp/src/GaussNewtonDDP.cpp | 39 +++++++++++++++++++-------------- 1 file changed, 23 insertions(+), 16 deletions(-) diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index a89d9dfc3..33983c191 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -836,6 +836,15 @@ void GaussNewtonDDP::initializeConstraintPenalties() { constraintPenaltyCoefficients_.penaltyCoeff = ddpSettings_.constraintPenaltyInitialValue_; constraintPenaltyCoefficients_.penaltyTol = 1.0 / std::pow(constraintPenaltyCoefficients_.penaltyCoeff, 0.1); + + // display + if (ddpSettings_.displayInfo_) { + std::string displayText = "Initial equality Constraints Penalty Parameters:\n"; + displayText += " Penalty Tolerance: " + std::to_string(constraintPenaltyCoefficients_.penaltyTol); + displayText += " Penalty Coefficient: " + std::to_string(constraintPenaltyCoefficients_.penaltyCoeff) + ".\n"; + + this->printString(displayText); + } } /******************************************************************************************************/ @@ -867,9 +876,6 @@ void GaussNewtonDDP::updateConstraintPenalties(scalar_t equalityConstraintsSSE) /******************************************************************************************************/ /******************************************************************************************************/ void GaussNewtonDDP::initializeDualPrimal() { - constexpr size_t taskId = 0; - constexpr scalar_t stepLength = 0.0; - try { // adjust controller if (!optimizedPrimalSolution_.controllerPtr_->empty()) { @@ -900,7 +906,7 @@ void GaussNewtonDDP::initializeDualPrimal() { nominalDualData_.dualSolution); totalDualSolutionTimer_.endTimer(); - computeRolloutMetrics(optimalControlProblemStock_[taskId], nominalPrimalData_.primalSolution, nominalDualData_.dualSolution, + computeRolloutMetrics(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalDualData_.dualSolution, nominalPrimalData_.problemMetrics); // update dual @@ -914,16 +920,8 @@ void GaussNewtonDDP::initializeDualPrimal() { computeRolloutPerformanceIndex(nominalPrimalData_.primalSolution.timeTrajectory_, nominalPrimalData_.problemMetrics); performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); - // display - if (ddpSettings_.displayInfo_) { - std::stringstream infoDisplay; - infoDisplay << " [Thread " << taskId << "] - step length " << stepLength << '\n'; - infoDisplay << std::setw(4) << performanceIndex_ << '\n'; - printString(infoDisplay.str()); - } - } catch (const std::exception& error) { - const std::string msg = "[GaussNewtonDDP::initializeDualPrimal] Initial controller does not generate a stable rollout.\n"; + const std::string msg = "[GaussNewtonDDP::initializeDualPrimal] the initial rollout is unstable!\n"; throw std::runtime_error(msg + error.what()); } } @@ -1032,7 +1030,7 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala // display if (ddpSettings_.displayInfo_) { std::cerr << "\n###################"; - std::cerr << "\n#### Iteration " << (totalNumIterations_ - initIteration) << " (Dynamics might have been violated)"; + std::cerr << "\n#### Initial Rollout"; std::cerr << "\n###################\n"; } @@ -1049,14 +1047,21 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala // check if the rollout is a result of an empty controller bool unreliableValueFunction = nominalPrimalData_.primalSolution.controllerPtr_->empty(); + // display + if (ddpSettings_.displayInfo_) { + if (unreliableValueFunction) { + std::cerr << "Note: system dynamics might have been violated in during initialization!\n"; + } + std::cerr << performanceIndex_ << '\n'; + } + // convergence variables of the main loop bool isConverged = false; std::string convergenceInfo; // DDP main loop while (true) { - // display the iteration's input update norm (before caching the old nominals) - if (ddpSettings_.displayInfo_ && totalNumIterations_ > initIteration) { + if (ddpSettings_.displayInfo_) { std::cerr << "\n###################"; std::cerr << "\n#### Iteration " << (totalNumIterations_ - initIteration); std::cerr << "\n###################\n"; @@ -1114,6 +1119,8 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala std::cerr << "Time Period: [" << initTime_ << " ," << finalTime_ << "]\n"; std::cerr << "Number of Iterations: " << (totalNumIterations_ - initIteration) << " out of " << ddpSettings_.maxNumIterations_ << "\n"; + printRolloutInfo(); + if (isConverged) { std::cerr << convergenceInfo << std::endl; } else if (totalNumIterations_ - initIteration == ddpSettings_.maxNumIterations_) { From 84a7e1d27f1c1b0fde118ebb3927cee0955f3c2c Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 10 Aug 2022 19:19:30 +0200 Subject: [PATCH 283/512] moving some DDP methods to DDP_HelperFunctions --- .../include/ocs2_ddp/DDP_HelperFunctions.h | 19 +++++ ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h | 30 +------ ocs2_ddp/src/DDP_HelperFunctions.cpp | 75 +++++++++++++++++ ocs2_ddp/src/GaussNewtonDDP.cpp | 82 ------------------- 4 files changed, 97 insertions(+), 109 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h b/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h index 1bf81fa23..371fa293c 100644 --- a/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h +++ b/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h @@ -80,6 +80,17 @@ PerformanceIndex computeRolloutPerformanceIndex(const scalar_array_t& timeTrajec scalar_t rolloutTrajectory(RolloutBase& rollout, scalar_t initTime, const vector_t& initState, scalar_t finalTime, PrimalSolution& primalSolution); +/** + * Projects the unconstrained LQ coefficients to constrained ones. + * + * @param [in] modelData: The model data. + * @param [in] constraintRangeProjector: The projection matrix to the constrained subspace. + * @param [in] constraintNullProjector: The projection matrix to the null space of constrained. + * @param [out] projectedModelData: The projected model data. + */ +void projectLQ(const ModelData& modelData, const matrix_t& constraintRangeProjector, const matrix_t& constraintNullProjector, + ModelData& projectedModelData); + /** * Extract a primal solution for the range [initTime, finalTime] from a given primal solution. It assumes that the * given range is within the solution time of input primal solution. @@ -94,6 +105,14 @@ scalar_t rolloutTrajectory(RolloutBase& rollout, scalar_t initTime, const vector void extractPrimalSolution(const std::pair<scalar_t, scalar_t>& timePeriod, const PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution); +/** + * Calculates max feedforward update norm of the controller. + * + * @param [in] controller: Control policy + * @return max feedforward update norm. + */ +scalar_t maxControllerUpdateNorm(const LinearController& controller); + /** * Computes the integral of the squared (IS) norm of the controller update. * diff --git a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h index c77327e87..a508dbd79 100644 --- a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h +++ b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h @@ -121,7 +121,9 @@ class GaussNewtonDDP : public SolverBase { * @param [in] taskFunction: task function * @param [in] N: number of times to run taskFunction, if N = 1 it is run in the main thread */ - void runParallel(std::function<void(void)> taskFunction, size_t N); + void runParallel(std::function<void(void)> taskFunction, size_t N) { + threadPool_.runParallel([&](int) { taskFunction(); }, N); + } /** * Takes the following steps: (1) Computes the Hessian of the Hamiltonian (i.e., Hm) (2) Based on Hm, it calculates @@ -258,21 +260,6 @@ class GaussNewtonDDP : public SolverBase { */ scalar_t calculateRolloutMerit(const PerformanceIndex& performanceIndex) const; - /** - * Calculates max feedforward update norm and max type-1 error update norm. - * - * @param maxDeltaUffNorm: max feedforward update norm. - * @param maxDeltaUeeNorm: max type-1 error update norm. - */ - - /** - * Calculates max feedforward update norm of the controller. - * - * @param [in] controller: Control policy - * @return max feedforward update norm. - */ - scalar_t maxControllerUpdateNorm(const LinearController& controller) const; - /** * Approximates the nonlinear problem as a linear-quadratic problem around the * nominal state and control trajectories. This method updates the following @@ -296,17 +283,6 @@ class GaussNewtonDDP : public SolverBase { void computeProjections(const matrix_t& Hm, const matrix_t& Dm, matrix_t& constraintRangeProjector, matrix_t& constraintNullProjector) const; - /** - * Projects the unconstrained LQ coefficients to constrained ones. - * - * @param [in] modelData: The model data. - * @param [in] constraintRangeProjector: The projection matrix to the constrained subspace. - * @param [in] constraintNullProjector: The projection matrix to the null space of constrained. - * @param [out] projectedModelData: The projected model data. - */ - void projectLQ(const ModelData& modelData, const matrix_t& constraintRangeProjector, const matrix_t& constraintNullProjector, - ModelData& projectedModelData) const; - /** Initialize the constraint penalty coefficients. */ void initializeConstraintPenalties(); diff --git a/ocs2_ddp/src/DDP_HelperFunctions.cpp b/ocs2_ddp/src/DDP_HelperFunctions.cpp index c00ce7858..8df3737f4 100644 --- a/ocs2_ddp/src/DDP_HelperFunctions.cpp +++ b/ocs2_ddp/src/DDP_HelperFunctions.cpp @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/PreComputation.h> #include <ocs2_core/integration/TrapezoidalIntegration.h> #include <ocs2_core/misc/LinearInterpolation.h> +#include <ocs2_oc/approximate_model/ChangeOfInputVariables.h> #include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> namespace ocs2 { @@ -184,6 +185,69 @@ scalar_t rolloutTrajectory(RolloutBase& rollout, scalar_t initTime, const vector return (finalTime - initTime) / static_cast<scalar_t>(primalSolution.timeTrajectory_.size()); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void projectLQ(const ModelData& modelData, const matrix_t& constraintRangeProjector, const matrix_t& constraintNullProjector, + ModelData& projectedModelData) { + // dimensions and time + projectedModelData.time = modelData.time; + projectedModelData.stateDim = modelData.stateDim; + projectedModelData.inputDim = modelData.inputDim - modelData.stateInputEqConstraint.f.rows(); + + // unhandled constraints + projectedModelData.stateEqConstraint.f = vector_t(); + + if (modelData.stateInputEqConstraint.f.rows() == 0) { + // Change of variables u = Pu * tilde{u} + // Pu = constraintNullProjector; + + // projected state-input equality constraints + projectedModelData.stateInputEqConstraint.f.setZero(projectedModelData.inputDim); + projectedModelData.stateInputEqConstraint.dfdx.setZero(projectedModelData.inputDim, projectedModelData.stateDim); + projectedModelData.stateInputEqConstraint.dfdu.setZero(modelData.inputDim, modelData.inputDim); + + // dynamics + projectedModelData.dynamics = modelData.dynamics; + changeOfInputVariables(projectedModelData.dynamics, constraintNullProjector); + + // dynamics bias + projectedModelData.dynamicsBias = modelData.dynamicsBias; + + // cost + projectedModelData.cost = modelData.cost; + changeOfInputVariables(projectedModelData.cost, constraintNullProjector); + + } else { + // Change of variables u = Pu * tilde{u} + Px * x + u0 + // Pu = constraintNullProjector; + // Px (= -CmProjected) = -constraintRangeProjector * C + // u0 (= -EvProjected) = -constraintRangeProjector * e + + /* projected state-input equality constraints */ + projectedModelData.stateInputEqConstraint.f.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.f; + projectedModelData.stateInputEqConstraint.dfdx.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.dfdx; + projectedModelData.stateInputEqConstraint.dfdu.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.dfdu; + + // Change of variable matrices + const auto& Pu = constraintNullProjector; + const matrix_t Px = -projectedModelData.stateInputEqConstraint.dfdx; + const matrix_t u0 = -projectedModelData.stateInputEqConstraint.f; + + // dynamics + projectedModelData.dynamics = modelData.dynamics; + changeOfInputVariables(projectedModelData.dynamics, Pu, Px, u0); + + // dynamics bias + projectedModelData.dynamicsBias = modelData.dynamicsBias; + projectedModelData.dynamicsBias.noalias() += modelData.dynamics.dfdu * u0; + + // cost + projectedModelData.cost = modelData.cost; + changeOfInputVariables(projectedModelData.cost, Pu, Px, u0); + } +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -252,6 +316,17 @@ void extractPrimalSolution(const std::pair<scalar_t, scalar_t>& timePeriod, cons } } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +scalar_t maxControllerUpdateNorm(const LinearController& controller) { + scalar_t maxDeltaUffNorm = 0.0; + for (const auto& deltaBias : controller.deltaBiasArray_) { + maxDeltaUffNorm = std::max(maxDeltaUffNorm, deltaBias.norm()); + } + return maxDeltaUffNorm; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 33983c191..698441651 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -36,7 +36,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/integration/TrapezoidalIntegration.h> #include <ocs2_core/misc/LinearAlgebra.h> -#include <ocs2_oc/approximate_model/ChangeOfInputVariables.h> #include <ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h> #include <ocs2_oc/rollout/InitializerRollout.h> #include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h> @@ -372,13 +371,6 @@ vector_t GaussNewtonDDP::getStateInputEqualityConstraintLagrangianImpl(scalar_t return DmDagger.transpose() * temp; } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void GaussNewtonDDP::runParallel(std::function<void(void)> taskFunction, size_t N) { - threadPool_.runParallel([&](int) { taskFunction(); }, N); -} - /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -613,17 +605,6 @@ void GaussNewtonDDP::calculateController() { } } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -scalar_t GaussNewtonDDP::maxControllerUpdateNorm(const LinearController& controller) const { - scalar_t maxDeltaUffNorm = 0.0; - for (const auto& deltaBias : controller.deltaBiasArray_) { - maxDeltaUffNorm = std::max(maxDeltaUffNorm, deltaBias.norm()); - } - return maxDeltaUffNorm; -} - /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -764,69 +745,6 @@ void GaussNewtonDDP::computeProjections(const matrix_t& Hm, const matrix_t& Dm, } } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void GaussNewtonDDP::projectLQ(const ModelData& modelData, const matrix_t& constraintRangeProjector, - const matrix_t& constraintNullProjector, ModelData& projectedModelData) const { - // dimensions and time - projectedModelData.time = modelData.time; - projectedModelData.stateDim = modelData.stateDim; - projectedModelData.inputDim = modelData.inputDim - modelData.stateInputEqConstraint.f.rows(); - - // unhandled constraints - projectedModelData.stateEqConstraint.f = vector_t(); - - if (modelData.stateInputEqConstraint.f.rows() == 0) { - // Change of variables u = Pu * tilde{u} - // Pu = constraintNullProjector; - - // projected state-input equality constraints - projectedModelData.stateInputEqConstraint.f.setZero(projectedModelData.inputDim); - projectedModelData.stateInputEqConstraint.dfdx.setZero(projectedModelData.inputDim, projectedModelData.stateDim); - projectedModelData.stateInputEqConstraint.dfdu.setZero(modelData.inputDim, modelData.inputDim); - - // dynamics - projectedModelData.dynamics = modelData.dynamics; - changeOfInputVariables(projectedModelData.dynamics, constraintNullProjector); - - // dynamics bias - projectedModelData.dynamicsBias = modelData.dynamicsBias; - - // cost - projectedModelData.cost = modelData.cost; - changeOfInputVariables(projectedModelData.cost, constraintNullProjector); - - } else { - // Change of variables u = Pu * tilde{u} + Px * x + u0 - // Pu = constraintNullProjector; - // Px (= -CmProjected) = -constraintRangeProjector * C - // u0 (= -EvProjected) = -constraintRangeProjector * e - - /* projected state-input equality constraints */ - projectedModelData.stateInputEqConstraint.f.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.f; - projectedModelData.stateInputEqConstraint.dfdx.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.dfdx; - projectedModelData.stateInputEqConstraint.dfdu.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.dfdu; - - // Change of variable matrices - const auto& Pu = constraintNullProjector; - const matrix_t Px = -projectedModelData.stateInputEqConstraint.dfdx; - const matrix_t u0 = -projectedModelData.stateInputEqConstraint.f; - - // dynamics - projectedModelData.dynamics = modelData.dynamics; - changeOfInputVariables(projectedModelData.dynamics, Pu, Px, u0); - - // dynamics bias - projectedModelData.dynamicsBias = modelData.dynamicsBias; - projectedModelData.dynamicsBias.noalias() += modelData.dynamics.dfdu * u0; - - // cost - projectedModelData.cost = modelData.cost; - changeOfInputVariables(projectedModelData.cost, Pu, Px, u0); - } -} - /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ From 8bc139e53bc66ccfff2635c8bb7186df41fc201d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 11 Aug 2022 10:51:51 +0200 Subject: [PATCH 284/512] some updates --- ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h | 15 ++- ocs2_ddp/src/GaussNewtonDDP.cpp | 101 ++++++++++----------- 2 files changed, 59 insertions(+), 57 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h index a508dbd79..85b3437de 100644 --- a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h +++ b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h @@ -170,7 +170,7 @@ class GaussNewtonDDP : public SolverBase { /** * Solves Riccati equations. * - * @param [in] finalValueFunction The final Sm(dfdxx), Sv(dfdx), s(f), for Riccati equation. + * @param [in] finalValueFunction: The final value of Sm (dfdxx), Sv (dfdx), s (f), for Riccati equation. * @return average time step */ virtual scalar_t solveSequentialRiccatiEquations(const ScalarFunctionQuadraticApproximation& finalValueFunction) = 0; @@ -292,11 +292,16 @@ class GaussNewtonDDP : public SolverBase { */ void updateConstraintPenalties(scalar_t equalityConstraintsSSE); - /** Initializes the nominal primal and dual solutions based on the optimized ones. */ - void initializeDualPrimal(); + /** Initializes the nominal primal based on the optimized ones. \ + * @return whether the rollout is a result of Initializer since the optimized controller was empty. + */ + bool initializePrimalSolution(); - /** Constructs and solves the LQ problem around the nominal trajectories. */ - void constructAndSolveLQ(); + /** + * Initializes the nominal dual solutions based on the optimized ones and nominal primal solution. + * Moreover, it updates ProblemMetrics. + */ + void initializeDualSolutionAndMetrics(); /** Based on the current LQ solution updates the optimized primal and dual solutions. */ void takePrimalDualStep(scalar_t lqModelExpectedCost); diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 698441651..3b105e73a 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -603,6 +603,11 @@ void GaussNewtonDDP::calculateController() { } } } + + // display + if (ddpSettings_.displayInfo_) { + std::cerr << "max feedforward norm: " << maxControllerUpdateNorm(unoptimizedController_) << "\n"; + } } /******************************************************************************************************/ @@ -793,7 +798,7 @@ void GaussNewtonDDP::updateConstraintPenalties(scalar_t equalityConstraintsSSE) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaussNewtonDDP::initializeDualPrimal() { +bool GaussNewtonDDP::initializePrimalSolution() { try { // adjust controller if (!optimizedPrimalSolution_.controllerPtr_->empty()) { @@ -812,56 +817,43 @@ void GaussNewtonDDP::initializeDualPrimal() { // perform a rollout rolloutInitialTrajectory(nominalPrimalData_.primalSolution); - // adjust dual solution - totalDualSolutionTimer_.startTimer(); - if (!optimizedDualSolution_.timeTrajectory.empty()) { - const auto status = - trajectorySpread(optimizedPrimalSolution_.modeSchedule_, nominalPrimalData_.primalSolution.modeSchedule_, optimizedDualSolution_); - } - - // initialize dual solution - ocs2::initializeDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, optimizedDualSolution_, - nominalDualData_.dualSolution); - totalDualSolutionTimer_.endTimer(); - - computeRolloutMetrics(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalDualData_.dualSolution, - nominalPrimalData_.problemMetrics); - - // update dual - // totalDualSolutionTimer_.startTimer(); - // ocs2::updateDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalPrimalData_.problemMetrics, - // nominalDualData_.dualSolution); - // totalDualSolutionTimer_.endTimer(); - - // calculates rollout merit - performanceIndex_ = - computeRolloutPerformanceIndex(nominalPrimalData_.primalSolution.timeTrajectory_, nominalPrimalData_.problemMetrics); - performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); - } catch (const std::exception& error) { const std::string msg = "[GaussNewtonDDP::initializeDualPrimal] the initial rollout is unstable!\n"; throw std::runtime_error(msg + error.what()); } + + // check if the rollout is a result of an empty controller + return nominalPrimalData_.primalSolution.controllerPtr_->empty(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaussNewtonDDP::constructAndSolveLQ() { - // linearizing the dynamics and quadratizing the cost function along nominal trajectories - linearQuadraticApproximationTimer_.startTimer(); - approximateOptimalControlProblem(); - linearQuadraticApproximationTimer_.endTimer(); +void GaussNewtonDDP::initializeDualSolutionAndMetrics() { + // adjust dual solution + totalDualSolutionTimer_.startTimer(); + if (!optimizedDualSolution_.timeTrajectory.empty()) { + const auto status = + trajectorySpread(optimizedPrimalSolution_.modeSchedule_, nominalPrimalData_.primalSolution.modeSchedule_, optimizedDualSolution_); + } - // solve Riccati equations - backwardPassTimer_.startTimer(); - avgTimeStepBP_ = solveSequentialRiccatiEquations(nominalPrimalData_.modelDataFinalTime.cost); - backwardPassTimer_.endTimer(); + // initialize dual solution + ocs2::initializeDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, optimizedDualSolution_, + nominalDualData_.dualSolution); + totalDualSolutionTimer_.endTimer(); + + computeRolloutMetrics(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalDualData_.dualSolution, + nominalPrimalData_.problemMetrics); - // calculate controller and store the result in unoptimizedController_ - computeControllerTimer_.startTimer(); - calculateController(); - computeControllerTimer_.endTimer(); + // update dual + // totalDualSolutionTimer_.startTimer(); + // ocs2::updateDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalPrimalData_.problemMetrics, + // nominalDualData_.dualSolution); + // totalDualSolutionTimer_.endTimer(); + + // calculates rollout merit + performanceIndex_ = computeRolloutPerformanceIndex(nominalPrimalData_.primalSolution.timeTrajectory_, nominalPrimalData_.problemMetrics); + performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); } /******************************************************************************************************/ @@ -958,17 +950,15 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala // optimized --> nominal: initializes the nominal primal and dual solutions based on the optimized ones initializationTimer_.startTimer(); - initializeDualPrimal(); + bool unreliableValueFunction = initializePrimalSolution(); // true if the rollout is purely from the Initializer + initializeDualSolutionAndMetrics(); performanceIndexHistory_.push_back(performanceIndex_); initializationTimer_.endTimer(); - // check if the rollout is a result of an empty controller - bool unreliableValueFunction = nominalPrimalData_.primalSolution.controllerPtr_->empty(); - // display if (ddpSettings_.displayInfo_) { if (unreliableValueFunction) { - std::cerr << "Note: system dynamics might have been violated in during initialization!\n"; + std::cerr << "Note: system dynamics might have been violated during the initialization!\n"; } std::cerr << performanceIndex_ << '\n'; } @@ -985,15 +975,22 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala std::cerr << "\n###################\n"; } - // nominal --> nominal: constructs and solves the LQ problem around the nominal trajectories - constructAndSolveLQ(); + // nominal --> nominal: constructs the LQ problem around the nominal trajectories + linearQuadraticApproximationTimer_.startTimer(); + approximateOptimalControlProblem(); + linearQuadraticApproximationTimer_.endTimer(); - // display - if (ddpSettings_.displayInfo_) { - std::cerr << "max feedforward norm: " << maxControllerUpdateNorm(unoptimizedController_) << "\n"; - } + // nominal --> nominal: solves the LQ problem + backwardPassTimer_.startTimer(); + avgTimeStepBP_ = solveSequentialRiccatiEquations(nominalPrimalData_.modelDataFinalTime.cost); + backwardPassTimer_.endTimer(); + + // calculate controller and store the result in unoptimizedController_ + computeControllerTimer_.startTimer(); + calculateController(); + computeControllerTimer_.endTimer(); - // if lqModelExpectedCost==true, the expected cost/merit calculated by the Riccati solution is not reliable + // the expected cost/merit calculated by the Riccati solution is not reliable const auto lqModelExpectedCost = unreliableValueFunction ? performanceIndex_.merit : nominalDualData_.valueFunctionTrajectory.front().f; // nominal --> optimized: based on the current LQ solution updates the optimized primal and dual solutions From 8e454dcdb4e626cd00150dc4cf2fbe9de8f8ebcd Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 11 Aug 2022 11:49:04 +0200 Subject: [PATCH 285/512] cherry-picking ocs2_perceptive_core from my ocs2_perceptive_mpc branch --- ocs2_perceptive/CMakeLists.txt | 107 ++++++++++++ .../ComputeDistanceTransform.h | 84 +++++++++ .../DistanceTransformInterface.h | 72 ++++++++ .../implementation/ComputeDistanceTransform.h | 99 +++++++++++ .../EndEffectorDistanceConstraint.h | 86 ++++++++++ .../EndEffectorDistanceConstraintCppAd.h | 95 +++++++++++ .../interpolation/BilinearInterpolation.h | 86 ++++++++++ ocs2_perceptive/package.xml | 16 ++ .../EndEffectorDistanceConstraint.cpp | 136 +++++++++++++++ .../EndEffectorDistanceConstraintCppAd.cpp | 160 ++++++++++++++++++ ocs2_perceptive/src/lintTarget.cpp | 41 +++++ .../testBilinearInterpolation.cpp | 157 +++++++++++++++++ 12 files changed, 1139 insertions(+) create mode 100644 ocs2_perceptive/CMakeLists.txt create mode 100644 ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h create mode 100644 ocs2_perceptive/include/ocs2_perceptive/distance_transform/DistanceTransformInterface.h create mode 100644 ocs2_perceptive/include/ocs2_perceptive/distance_transform/implementation/ComputeDistanceTransform.h create mode 100644 ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h create mode 100644 ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h create mode 100644 ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h create mode 100644 ocs2_perceptive/package.xml create mode 100644 ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraint.cpp create mode 100644 ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraintCppAd.cpp create mode 100644 ocs2_perceptive/src/lintTarget.cpp create mode 100644 ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp diff --git a/ocs2_perceptive/CMakeLists.txt b/ocs2_perceptive/CMakeLists.txt new file mode 100644 index 000000000..43d39c8a9 --- /dev/null +++ b/ocs2_perceptive/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.0) +project(ocs2_perceptive) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_core + ocs2_robotic_tools +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem +) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIRS} + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS + Boost +) + +############# +## Build ## +############# +include_directories( + include + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + src/end_effector/EndEffectorDistanceConstraint.cpp + src/end_effector/EndEffectorDistanceConstraintCppAd.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) + +add_executable(${PROJECT_NAME}_lintTarget + src/lintTarget.cpp +) + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + add_clang_tooling( + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/test + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR + ) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# + +install( + DIRECTORY + include/${PROJECT_NAME}/ + DESTINATION + ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +############# +## Testing ## +############# +## Info ============================== +## to run tests, cd package folder and run +## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this +## $ catkin run_tests --no-deps --this +## to see the summary of unit test results run +## $ catkin_test_results ../../../build/ocs2_perceptive + +catkin_add_gtest(test_bilinear_interpolation + test/interpolation/testBilinearInterpolation.cpp +) +target_link_libraries(test_bilinear_interpolation + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + gtest_main +) + diff --git a/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h new file mode 100644 index 000000000..aa3ccfbd3 --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h @@ -0,0 +1,84 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <vector> + +namespace ocs2 { + +/** + * Computes one-dimensional distance transform for a sampled function based on lower envelope of parabolas method introduced by: + * P. F. Felzenszwalb and D. P. Huttenlocher. "Distance transforms of sampled functions." (2012). + * + * @param numSamples: The size of the smapled function. + * @param getValue: Lambda function to get value from with signature: ScalarType(size_t index). + * @param setValue: Lambda function to set value to with signature: void(size_t index, ScalarType value). + * @param start: The start index for iteration over. + * @param end: The index of the element following the last element for iteration over. + * @param vBuffer: A buffered memory of unsigned integers with size "numSamples". This memory holds the information + * about the locations of parabolas in lower envelope. + * @param zBuffer: A buffered memory of Scalar type with size "numSamples + 1". This memory holds the information + * about the locations of boundaries between parabolas. + * + * @tparam GetValFunc: Template typename for inferring lambda expression with signature Scalar(size_t). + * @tparam SetValFunc: Template typename for inferring lambda expression with signature void(size_t, Scalar). + * @tpapram Scalar: The Scalar type + */ +template <typename GetValFunc, typename SetValFunc, typename Scalar = float> +void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, size_t start, size_t end, + std::vector<size_t>& vBuffer, std::vector<Scalar>& zBuffer); + +/** + * Computes one-dimensional distance transform for a sampled function based on lower envelope of parabolas method introduced by: + * P. F. Felzenszwalb and D. P. Huttenlocher. "Distance transforms of sampled functions." (2012). + * + * @param numSamples: The size of the smapled function. + * @param getValue: Lambda function to get value from with signature: ScalarType(size_t index). + * @param setValue: Lambda function to set value to with signature: void(size_t index, ScalarType value). + * @param setImageIndex: Lambda function to set mirror index with signature: void(size_t index, size_t mirrorIndex). + * @param start: The start index for iteration over. + * @param end: The index of the element following the last element for iteration over. + * @param vBuffer: A buffered memory of unsigned integers with size "numSamples". This memory holds the information + * about the locations of parabolas in lower envelope. + * @param zBuffer: A buffered memory of Scalar type with size "numSamples + 1". This memory holds the information + * about the locations of boundaries between parabolas. + * + * @tparam GetValFunc: Template typename for inferring lambda expression with signature Scalar(size_t). + * @tparam SetValFunc: Template typename for inferring lambda expression with signature void(size_t, Scalar). + * @tparam SetImageIndexFunc: Template typename for inferring lambda expression with signature void(size_t, size_t). + * @tpapram Scalar: The Scalar type + */ +template <typename GetValFunc, typename SetValFunc, typename SetImageIndexFunc, typename Scalar = float> +void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, SetImageIndexFunc&& setImageIndex, + size_t start, size_t end, std::vector<size_t>& vBuffer, std::vector<Scalar>& zBuffer); + +} // namespace ocs2 + +#include "implementation/ComputeDistanceTransform.h" diff --git a/ocs2_perceptive/include/ocs2_perceptive/distance_transform/DistanceTransformInterface.h b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/DistanceTransformInterface.h new file mode 100644 index 000000000..d49e9395d --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/DistanceTransformInterface.h @@ -0,0 +1,72 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <utility> + +#include <ocs2_core/Types.h> + +namespace ocs2 { + +/** Distance field which defines a 3D point distance to the "nearest" surface and the gradient of the field. */ +class DistanceTransformInterface { + public: + using vector3_t = Eigen::Matrix<scalar_t, 3, 1>; + + DistanceTransformInterface() = default; + virtual ~DistanceTransformInterface() = default; + + DistanceTransformInterface(const DistanceTransformInterface&) = delete; + DistanceTransformInterface& operator=(const DistanceTransformInterface&) = delete; + DistanceTransformInterface(DistanceTransformInterface&&) = default; + DistanceTransformInterface& operator=(DistanceTransformInterface&&) = default; + + /** Gets the distance to the given point. */ + virtual scalar_t getValue(const vector3_t& p) const = 0; + + /** Projects the given point to the nearest point on the surface. */ + virtual vector3_t getProjectedPoint(const vector3_t& p) const = 0; + + /** Gets the distance's value and its gradient at the given point. */ + virtual std::pair<scalar_t, vector3_t> getLinearApproximation(const vector3_t& p) const = 0; +}; + +/** Identity distance transform with constant zero value and zero gradients. */ +class IdentityDistanceTransform : public DistanceTransformInterface { + public: + IdentityDistanceTransform() = default; + ~IdentityDistanceTransform() override = default; + + scalar_t getValue(const vector3_t&) const override { return 0.0; } + vector3_t getProjectedPoint(const vector3_t& p) const override { return p; } + std::pair<scalar_t, vector3_t> getLinearApproximation(const vector3_t&) const override { return {0.0, vector3_t::Zero()}; } +}; + +} // namespace ocs2 diff --git a/ocs2_perceptive/include/ocs2_perceptive/distance_transform/implementation/ComputeDistanceTransform.h b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/implementation/ComputeDistanceTransform.h new file mode 100644 index 000000000..47d1459fa --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/implementation/ComputeDistanceTransform.h @@ -0,0 +1,99 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <limits> +#include <utility> + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <typename GetValFunc, typename SetValFunc, typename Scalar> +void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, size_t start, size_t end, + std::vector<size_t>& vBuffer, std::vector<Scalar>& zBuffer) { + computeDistanceTransform( + numSamples, std::forward<GetValFunc>(getValue), std::forward<SetValFunc>(setValue), [&](size_t x, size_t ind) {}, start, end, vBuffer, + zBuffer); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <typename GetValFunc, typename SetValFunc, typename SetImageIndexFunc, typename Scalar> +void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, SetImageIndexFunc&& setImageIndex, + size_t start, size_t end, std::vector<size_t>& vBuffer, std::vector<Scalar>& zBuffer) { + constexpr auto PlusInfinity = std::numeric_limits<Scalar>::max(); + constexpr auto MinusInfinity = std::numeric_limits<Scalar>::lowest(); + + // make sure the buffer memories has the right size + if (vBuffer.size() < numSamples) { + vBuffer.resize(numSamples); // locations of parabolas in lower envelope + } + if (zBuffer.size() < numSamples + 1) { + zBuffer.resize(numSamples + 1); // locations of boundaries between parabolas + } + + // initialization + vBuffer[start] = start; + zBuffer[start] = MinusInfinity; + zBuffer[start + 1] = PlusInfinity; + size_t k = start; // index of rightmost parabola in lower envelope + + // compute lower envelope + for (size_t q = start + 1; q < end; q++) { + k++; // compensates for first line of next do-while block + + Scalar s = 0; + do { + k--; + // compute horizontal position of intersection between the parabola from q & the current lowest parabola + s = ((getValue(q) + q * q) - (getValue(vBuffer[k]) + vBuffer[k] * vBuffer[k])) / (2 * q - 2 * vBuffer[k]); + } while (s <= zBuffer[k]); + + k++; + vBuffer[k] = q; + zBuffer[k] = s; + zBuffer[k + 1] = PlusInfinity; + } // end of for loop + + // fill in values of distance transform + k = start; + for (size_t q = start; q < end; q++) { + while (zBuffer[k + 1] < q) { + k++; + } + const size_t ind = vBuffer[k]; + const Scalar val = (q - ind) * (q - ind) + getValue(ind); + setValue(q, val); + setImageIndex(q, ind); + } // end of for loop +} + +} // namespace ocs2 diff --git a/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h b/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h new file mode 100644 index 000000000..ef595264d --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h @@ -0,0 +1,86 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <memory> +#include <string> +#include <utility> + +#include <ocs2_core/Types.h> +#include <ocs2_core/constraint/StateConstraint.h> +#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h> + +#include "ocs2_perceptive/distance_transform/DistanceTransformInterface.h" + +namespace ocs2 { + +/** + * End-effector distance constraint Function. + */ +class EndEffectorDistanceConstraint final : public ocs2::StateConstraint { + public: + /** Constructor */ + EndEffectorDistanceConstraint(size_t stateDim, scalar_t weight, std::unique_ptr<EndEffectorKinematics<scalar_t>> kinematicsPtr); + + /** Constructor */ + EndEffectorDistanceConstraint(size_t stateDim, scalar_t weight, std::unique_ptr<EndEffectorKinematics<scalar_t>> kinematicsPtr, + scalar_array_t clearances); + + /** Default destructor */ + ~EndEffectorDistanceConstraint() override = default; + + void set(const DistanceTransformInterface& distanceTransform); + void set(scalar_t clearance, const DistanceTransformInterface& distanceTransform); + void set(const scalar_array_t& clearances, const DistanceTransformInterface& distanceTransform); + + EndEffectorDistanceConstraint* clone() const override { return new EndEffectorDistanceConstraint(*this); } + size_t getNumConstraints(scalar_t time) const override { return kinematicsPtr_->getIds().size(); } + const std::vector<std::string>& getIDs() const { return kinematicsPtr_->getIds(); } + + vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const override; + + size_t getStateDimensions() const { return stateDim_; } + EndEffectorKinematics<scalar_t>& getKinematics() { return *kinematicsPtr_; } + + private: + EndEffectorDistanceConstraint(const EndEffectorDistanceConstraint& other); + + const size_t stateDim_; + const scalar_t weight_; + + std::unique_ptr<EndEffectorKinematics<scalar_t>> kinematicsPtr_; + + scalar_array_t clearances_; + const DistanceTransformInterface* distanceTransformPtr_ = nullptr; +}; + +} // namespace ocs2 diff --git a/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h b/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h new file mode 100644 index 000000000..c4b4c7e40 --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h @@ -0,0 +1,95 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <memory> +#include <string> +#include <utility> + +#include <ocs2_core/Types.h> +#include <ocs2_core/automatic_differentiation/CppAdInterface.h> +#include <ocs2_core/constraint/StateInputConstraint.h> +#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h> + +#include "ocs2_perceptive/distance_transform/DistanceTransformInterface.h" + +namespace ocs2 { + +/** + * End-effector distance constraint Function. + */ +class EndEffectorDistanceConstraintCppAd final : public ocs2::StateInputConstraint { + public: + struct Config { + Config(scalar_t weightParam = 1.0, bool generateModelParam = true, bool verboseParam = true) + : weight(weightParam), generateModel(generateModelParam), verbose(verboseParam) {} + scalar_t weight; + bool generateModel; + bool verbose; + }; + + /** Constructor */ + EndEffectorDistanceConstraintCppAd(size_t stateDim, size_t inputDim, Config config, + std::unique_ptr<EndEffectorKinematics<ad_scalar_t>> adKinematicsPtr); + + /** Default destructor */ + ~EndEffectorDistanceConstraintCppAd() override = default; + + void set(scalar_t clearance, const DistanceTransformInterface& distanceTransform); + void set(vector_t clearances, const DistanceTransformInterface& distanceTransform); + + EndEffectorDistanceConstraintCppAd* clone() const override { return new EndEffectorDistanceConstraintCppAd(*this); } + size_t getNumConstraints(scalar_t time) const override { return adKinematicsPtr_->getIds().size(); } + const std::vector<std::string>& getIDs() const { return adKinematicsPtr_->getIds(); } + + vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const override; + VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const override; + + EndEffectorKinematics<ad_scalar_t>& getKinematicsAD() { return *adKinematicsPtr_; } + std::pair<size_t, size_t> getStateInputDimensions() const { return {stateDim_, inputDim_}; } + + private: + EndEffectorDistanceConstraintCppAd(const EndEffectorDistanceConstraintCppAd& other); + + const size_t stateDim_; + const size_t inputDim_; + const Config config_; + + std::unique_ptr<EndEffectorKinematics<ad_scalar_t>> adKinematicsPtr_; + std::unique_ptr<CppAdInterface> kinematicsModelPtr_; + + vector_t clearances_; + const DistanceTransformInterface* distanceTransformPtr_ = nullptr; +}; + +} // namespace ocs2 diff --git a/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h b/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h new file mode 100644 index 000000000..19235f32c --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h @@ -0,0 +1,86 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <array> +#include <utility> + +#include <ocs2_core/Types.h> + +namespace ocs2 { +namespace bilinear_interpolation { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <typename Scalar> +Scalar getValue(Scalar resolution, const Eigen::Matrix<Scalar, 2, 1>& referenceCorner, const std::array<Scalar, 4>& cornerValues, + const Eigen::Matrix<Scalar, 2, 1>& position) { + // auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array<Scalar, 4> v; + v[0] = (position.x() - referenceCorner.x()) * r_inv; + v[1] = (position.y() - referenceCorner.y()) * r_inv; + v[2] = 1 - v[1]; + v[3] = 1 - v[0]; + + return cornerValues[1] * v[0] * v[2] + cornerValues[0] * v[3] * v[2] + cornerValues[2] * v[3] * v[1] + cornerValues[3] * v[0] * v[1]; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <typename Scalar> +std::pair<Scalar, Eigen::Matrix<Scalar, 2, 1>> getLinearApproximation(Scalar resolution, const Eigen::Matrix<Scalar, 2, 1>& referenceCorner, + const std::array<Scalar, 4>& cornerValues, + const Eigen::Matrix<Scalar, 2, 1>& position) { + // auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array<Scalar, 6> v; + v[0] = (position.y() - referenceCorner.y()) * r_inv; + v[1] = (position.x() - referenceCorner.x()) * r_inv; + v[2] = 1.0 - v[0]; + v[3] = 1.0 - v[1]; + v[4] = cornerValues[2] * v[3]; + v[5] = cornerValues[3] * v[1]; + v[3] = cornerValues[0] * v[3]; + v[1] = cornerValues[1] * v[1]; + + const Scalar value = v[1] * v[2] + v[3] * v[2] + v[4] * v[0] + v[5] * v[0]; + + Eigen::Matrix<Scalar, 2, 1> gradient; + gradient.x() = (v[2] * (cornerValues[1] - cornerValues[0]) + v[0] * (cornerValues[3] - cornerValues[2])) * r_inv; + gradient.y() = (v[4] + v[5] - (v[3] + v[1])) * r_inv; + + return {value, gradient}; +} + +} // namespace bilinear_interpolation +} // namespace ocs2 diff --git a/ocs2_perceptive/package.xml b/ocs2_perceptive/package.xml new file mode 100644 index 000000000..a85af33ff --- /dev/null +++ b/ocs2_perceptive/package.xml @@ -0,0 +1,16 @@ +<?xml version="1.0"?> +<package format="2"> + <name>ocs2_perceptive</name> + <version>0.0.0</version> + <description>The ocs2_perceptive package</description> + + <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer> + + <license>BSD</license> + + <buildtool_depend>catkin</buildtool_depend> + <build_depend>cmake_clang_tools</build_depend> + <depend>ocs2_core</depend> + <depend>ocs2_robotic_tools</depend> + +</package> diff --git a/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraint.cpp b/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraint.cpp new file mode 100644 index 000000000..f8a51ddb6 --- /dev/null +++ b/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraint.cpp @@ -0,0 +1,136 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include "ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraint::EndEffectorDistanceConstraint(size_t stateDim, scalar_t weight, + std::unique_ptr<EndEffectorKinematics<scalar_t>> kinematicsPtr) + : StateConstraint(ConstraintOrder::Linear), + stateDim_(stateDim), + weight_(weight), + kinematicsPtr_(std::move(kinematicsPtr)), + clearances_(kinematicsPtr_->getIds().size(), 0.0) {} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraint::EndEffectorDistanceConstraint(size_t stateDim, scalar_t weight, + std::unique_ptr<EndEffectorKinematics<scalar_t>> kinematicsPtr, + scalar_array_t clearances) + : StateConstraint(ConstraintOrder::Linear), + stateDim_(stateDim), + weight_(weight), + kinematicsPtr_(std::move(kinematicsPtr)), + clearances_(std::move(clearances)) { + if (clearances_.size() != kinematicsPtr_->getIds().size()) { + throw std::runtime_error("[EndEffectorDistanceConstraint] clearances.size() != kinematicsPtr->getIds().size()"); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraint::EndEffectorDistanceConstraint(const EndEffectorDistanceConstraint& other) + : StateConstraint(other), + stateDim_(other.stateDim_), + weight_(other.weight_), + kinematicsPtr_(other.kinematicsPtr_->clone()), + clearances_(other.clearances_) {} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraint::set(const DistanceTransformInterface& distanceTransform) { + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraint::set(scalar_t clearance, const DistanceTransformInterface& distanceTransform) { + clearances_.assign(kinematicsPtr_->getIds().size(), clearance); + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraint::set(const scalar_array_t& clearances, const DistanceTransformInterface& distanceTransform) { + assert(clearances.size() == kinematicsPtr_->getIds().size()); + clearances_ = clearances; + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t EndEffectorDistanceConstraint::getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const { + if (distanceTransformPtr_ == nullptr) { + throw std::runtime_error("[EndEffectorDistanceConstraint] First, set the distance-transform by calling set()!"); + } + + const auto numEEs = kinematicsPtr_->getIds().size(); + const auto eePositions = kinematicsPtr_->getPosition(state); + + vector_t g(numEEs); + for (size_t i = 0; i < numEEs; i++) { + g(i) = weight_ * (distanceTransformPtr_->getValue(eePositions[i]) - clearances_[i]); + } // end of i loop + + return g; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +VectorFunctionLinearApproximation EndEffectorDistanceConstraint::getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const { + if (distanceTransformPtr_ == nullptr) { + throw std::runtime_error("[EndEffectorDistanceConstraint] First, set the distance-transform by calling set()!"); + } + + const auto numEEs = kinematicsPtr_->getIds().size(); + const auto eePosLinApprox = kinematicsPtr_->getPositionLinearApproximation(state); + + VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero(numEEs, stateDim_, 0); + for (size_t i = 0; i < numEEs; i++) { + const auto distanceValueGradient = distanceTransformPtr_->getLinearApproximation(eePosLinApprox[i].f); + approx.f(i) = weight_ * (distanceValueGradient.first - clearances_[i]); + approx.dfdx.row(i).noalias() = weight_ * (distanceValueGradient.second.transpose() * eePosLinApprox[i].dfdx); + } // end of i loop + + return approx; +} + +} // namespace ocs2 diff --git a/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraintCppAd.cpp b/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraintCppAd.cpp new file mode 100644 index 000000000..e1cf49993 --- /dev/null +++ b/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraintCppAd.cpp @@ -0,0 +1,160 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include "ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraintCppAd::EndEffectorDistanceConstraintCppAd(size_t stateDim, size_t inputDim, Config config, + std::unique_ptr<EndEffectorKinematics<ad_scalar_t>> adKinematicsPtr) + : StateInputConstraint(ConstraintOrder::Linear), + stateDim_(stateDim), + inputDim_(inputDim), + config_(std::move(config)), + adKinematicsPtr_(std::move(adKinematicsPtr)) { + auto surrogateKinematicsAD = [this](const ad_vector_t& x, ad_vector_t& y) { + const size_t numEEs = adKinematicsPtr_->getIds().size(); + const auto eePositions = adKinematicsPtr_->getPosition(x); + y.resize(3 * numEEs); + for (size_t i = 0; i < numEEs; i++) { + y.segment<3>(3 * i) = eePositions[i]; + } // end of i loop + }; + + std::string libName = "ee_kinemtaics"; + for (const auto& id : adKinematicsPtr_->getIds()) { + libName += "_" + id; + } + kinematicsModelPtr_.reset(new CppAdInterface(surrogateKinematicsAD, stateDim_, libName)); + constexpr auto order = CppAdInterface::ApproximationOrder::First; + if (config_.generateModel) { + kinematicsModelPtr_->createModels(order, config_.verbose); + } else { + kinematicsModelPtr_->loadModelsIfAvailable(order, config_.verbose); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraintCppAd::EndEffectorDistanceConstraintCppAd(const EndEffectorDistanceConstraintCppAd& other) + : StateInputConstraint(other), + stateDim_(other.stateDim_), + inputDim_(other.inputDim_), + config_(other.config_), + adKinematicsPtr_(other.adKinematicsPtr_->clone()), + kinematicsModelPtr_(new CppAdInterface(*other.kinematicsModelPtr_)) {} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraintCppAd::set(scalar_t clearance, const DistanceTransformInterface& distanceTransform) { + const auto numEEs = adKinematicsPtr_->getIds().size(); + clearances_ = vector_t::Ones(numEEs) * clearance; + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraintCppAd::set(vector_t clearances, const DistanceTransformInterface& distanceTransform) { + const auto numEEs = adKinematicsPtr_->getIds().size(); + if (clearances.size() != numEEs) { + throw std::runtime_error( + "[EndEffectorDistanceConstraint] The size of the input clearance vector doesn't match the number of end-effectors!"); + } + + clearances_ = clearances; + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t EndEffectorDistanceConstraintCppAd::getValue(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const { + const auto numEEs = adKinematicsPtr_->getIds().size(); + const auto eePositions = kinematicsModelPtr_->getFunctionValue(state); + assert(eePositions.size() == 3 * numEEs); + + vector_t g(numEEs); + for (size_t i = 0; i < numEEs; i++) { + g(i) = config_.weight * (distanceTransformPtr_->getValue(eePositions.segment<3>(3 * i)) - clearances_[i]); + } // end of i loop + + return g; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +VectorFunctionLinearApproximation EndEffectorDistanceConstraintCppAd::getLinearApproximation(scalar_t time, const vector_t& state, + const vector_t& input, + const PreComputation& preComp) const { + const auto numEEs = adKinematicsPtr_->getIds().size(); + const auto eePositions = kinematicsModelPtr_->getFunctionValue(state); + const auto eeJacobians = kinematicsModelPtr_->getJacobian(state); + assert(eePositions.size() == 3 * numEEs); + assert(eeJacobians.rows() == 3 * numEEs); + assert(eeJacobians.cols() == stateDim_); + + VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero(numEEs, stateDim_, inputDim_); + for (size_t i = 0; i < numEEs; i++) { + const auto distanceValueGradient = distanceTransformPtr_->getLinearApproximation(eePositions.segment<3>(3 * i)); + approx.f(i) = config_.weight * (distanceValueGradient.first - clearances_[i]); + approx.dfdx.row(i).noalias() = config_.weight * (distanceValueGradient.second.transpose() * eeJacobians.middleRows<3>(3 * i)); + } // end of i loop + + return approx; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +VectorFunctionQuadraticApproximation EndEffectorDistanceConstraintCppAd::getQuadraticApproximation(scalar_t time, const vector_t& state, + const vector_t& input, + const PreComputation& preComp) const { + const auto numEEs = adKinematicsPtr_->getIds().size(); + auto linearApprox = getLinearApproximation(time, state, input, preComp); + + VectorFunctionQuadraticApproximation quadraticApproximation; + quadraticApproximation.f = std::move(linearApprox.f); + quadraticApproximation.dfdx = std::move(linearApprox.dfdx); + quadraticApproximation.dfdu = std::move(linearApprox.dfdu); + quadraticApproximation.dfdxx.assign(numEEs, matrix_t::Zero(stateDim_, stateDim_)); + quadraticApproximation.dfdux.assign(numEEs, matrix_t::Zero(inputDim_, stateDim_)); + quadraticApproximation.dfduu.assign(numEEs, matrix_t::Zero(inputDim_, inputDim_)); + + return quadraticApproximation; +} + +} // namespace ocs2 diff --git a/ocs2_perceptive/src/lintTarget.cpp b/ocs2_perceptive/src/lintTarget.cpp new file mode 100644 index 000000000..d142670b1 --- /dev/null +++ b/ocs2_perceptive/src/lintTarget.cpp @@ -0,0 +1,41 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <ocs2_perceptive/distance_transform/ComputeDistanceTransform.h> +#include <ocs2_perceptive/distance_transform/DistanceTransformInterface.h> + +#include <ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h> +#include <ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h> + +#include <ocs2_perceptive/interpolation/BilinearInterpolation.h> + +// dummy target for clang toolchain +int main() { + return 0; +} diff --git a/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp b/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp new file mode 100644 index 000000000..ca641f534 --- /dev/null +++ b/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp @@ -0,0 +1,157 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <array> +#include <cmath> +#include <utility> + +#include <gtest/gtest.h> + +#include <ocs2_core/automatic_differentiation/CppAdInterface.h> +#include <ocs2_core/misc/Numerics.h> + +#include "ocs2_perceptive/interpolation/BilinearInterpolation.h" + +namespace ocs2 { +namespace bilinear_interpolation { + +class TestBilinearInterpolation : public ::testing::Test { + protected: + using array4_t = std::array<scalar_t, 4>; + using vector2_t = Eigen::Matrix<scalar_t, 2, 1>; + + TestBilinearInterpolation() { + auto adBilinearInterpolation = [](const ad_vector_t& x, const ad_vector_t& p, ad_vector_t& out) { + const ad_scalar_t resolution = p(0); + const ad_vector_t referenceCorner = (ad_vector_t(2) << p(1), p(2)).finished(); + const std::array<ad_scalar_t, 4> cornerValues = {p(3), p(4), p(5), p(6)}; + + const ad_vector_t positionRed = (x.head(2) - referenceCorner) / resolution; + const ad_vector_t positionRedFlip = ad_vector_t::Ones(2) - positionRed; + + out.resize(1); + out(0) = cornerValues[0] * positionRedFlip(0) * positionRedFlip(1) + cornerValues[1] * positionRed(0) * positionRedFlip(1) + + cornerValues[2] * positionRedFlip(0) * positionRed(1) + cornerValues[3] * positionRed(0) * positionRed(1); + out(0) *= x(2); + }; + + cppadInterfacePtr.reset(new CppAdInterface(adBilinearInterpolation, variableDim, parameterDim, "TestBilinearInterpolation")); + cppadInterfacePtr->createModels(CppAdInterface::ApproximationOrder::First, verbose); + } + + scalar_t bilinearInterpolation(scalar_t resolution, const vector2_t& referenceCorner, const array4_t& cornerValues, + const vector2_t& position) { + const vector2_t positionRed = (position - referenceCorner) / resolution; + const vector2_t positionRedFlip = vector2_t::Ones() - positionRed; + const scalar_t value = cornerValues[0] * positionRedFlip.x() * positionRedFlip.y() + + cornerValues[1] * positionRed.x() * positionRedFlip.y() + + cornerValues[2] * positionRedFlip.x() * positionRed.y() + cornerValues[3] * positionRed.x() * positionRed.y(); + return value; + } + + /** parpam = (resolution, referenceCornerXY, cornerValues[0:3]) */ + vector_t toParameter(scalar_t resolution, const vector2_t& referenceCorner, const array4_t& cornerValues) { + vector_t p(7); + p(0) = resolution; + p(1) = referenceCorner.x(); + p(2) = referenceCorner.y(); + p(3) = cornerValues[0]; + p(4) = cornerValues[1]; + p(5) = cornerValues[2]; + p(6) = cornerValues[3]; + return p; + } + + static constexpr bool verbose = true; + static constexpr size_t numSamples = 100; + static constexpr size_t variableDim = 3; // (x, y, 1.0) + static constexpr size_t parameterDim = 7; // (resolution, referenceCornerXY, cornerValues[0:3]) + static constexpr scalar_t precision = 1e-6; + static constexpr scalar_t resolution = 0.05; + + std::unique_ptr<CppAdInterface> cppadInterfacePtr; +}; + +constexpr bool TestBilinearInterpolation::verbose; +constexpr size_t TestBilinearInterpolation::numSamples; +constexpr size_t TestBilinearInterpolation::variableDim; +constexpr size_t TestBilinearInterpolation::parameterDim; +constexpr scalar_t TestBilinearInterpolation::resolution; + +TEST_F(TestBilinearInterpolation, testAdFunction) { + for (size_t i = 0; i < numSamples; i++) { + const vector_t randValues = vector_t::Random(parameterDim - 1 + variableDim - 1); + const vector2_t position(randValues(0), randValues(1)); + const vector2_t referenceCorner(randValues(2), randValues(3)); + const array4_t cornerValues = {randValues(4), randValues(5), randValues(6), randValues(7)}; + + const scalar_t trueValue = bilinearInterpolation(resolution, referenceCorner, cornerValues, position); + + const vector_t input = (vector_t(3) << position, 1.0).finished(); + const vector_t param = toParameter(resolution, referenceCorner, cornerValues); + // true value + const scalar_t value = cppadInterfacePtr->getFunctionValue(input, param)(0); + // true linear approximation + const matrix_t jacAug = cppadInterfacePtr->getJacobian(input, param); + const std::pair<scalar_t, vector2_t> linApprox = {jacAug(0, 2), vector2_t(jacAug(0, 0), jacAug(0, 1))}; + + EXPECT_TRUE(std::abs(trueValue - value) < precision) << "the true value is " << trueValue << " while the value is " << value; + EXPECT_TRUE(std::abs(trueValue - linApprox.first) < precision) + << "the true value is " << trueValue << " while the linApprox.first is " << linApprox.first; + } // end of i loop +} + +TEST_F(TestBilinearInterpolation, testBilinearInterpolation) { + for (size_t i = 0; i < numSamples; i++) { + const vector_t randValues = vector_t::Random(parameterDim - 1 + variableDim - 1); + const vector2_t position(randValues(0), randValues(1)); + const vector2_t referenceCorner(randValues(2), randValues(3)); + const array4_t cornerValues = {randValues(4), randValues(5), randValues(6), randValues(7)}; + + const vector_t input = (vector_t(3) << position, 1.0).finished(); + const vector_t param = toParameter(resolution, referenceCorner, cornerValues); + // true value + const scalar_t trueValue = cppadInterfacePtr->getFunctionValue(input, param)(0); + // true linear approximation + const matrix_t jacAug = cppadInterfacePtr->getJacobian(input, param); + const std::pair<scalar_t, vector2_t> trueLinApprox = {jacAug(0, 2), vector2_t(jacAug(0, 0), jacAug(0, 1))}; + + // value and linear approximation + const auto value = bilinear_interpolation::getValue(resolution, referenceCorner, cornerValues, position); + const auto linApprox = bilinear_interpolation::getLinearApproximation(resolution, referenceCorner, cornerValues, position); + + EXPECT_TRUE(std::abs(trueValue - value) < precision) << "the true value is " << trueValue << " while the value is " << value; + EXPECT_TRUE(std::abs(trueValue - linApprox.first) < precision) + << "the true value is " << trueValue << " while linApprox.first is " << linApprox.first; + EXPECT_TRUE(trueLinApprox.second.isApprox(linApprox.second, precision)); + } // end of i loop +} + +} // namespace bilinear_interpolation +} // namespace ocs2 From 31b2bb7f8afe16d86182b0440f2272f7fd17d27a Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 11 Aug 2022 10:04:18 +0000 Subject: [PATCH 286/512] Merged in fix/remove_eigen_scalar (pull request #654) removing all eigen_scalar_t * removing all eigen_scalar_t --- ocs2_core/include/ocs2_core/Types.h | 9 --------- .../include/ocs2_ddp/unsupported/SLQ_Hamiltonian.h | 11 ++++------- .../ocs2_ddp/unsupported/bvp_solver/BVPEquations.h | 2 -- .../ocs2_ddp/unsupported/bvp_solver/SolveBVP.h | 2 -- .../unsupported/implementation/SLQ_Hamiltonian.h | 13 +++++-------- ocs2_ocs2/include/ocs2_ocs2/GSLQPSolver.h | 2 -- 6 files changed, 9 insertions(+), 30 deletions(-) diff --git a/ocs2_core/include/ocs2_core/Types.h b/ocs2_core/include/ocs2_core/Types.h index 699a9ecbc..3c161783f 100644 --- a/ocs2_core/include/ocs2_core/Types.h +++ b/ocs2_core/include/ocs2_core/Types.h @@ -71,15 +71,6 @@ using matrix_array2_t = std::vector<matrix_array_t>; /** Array of arrays of dynamic matrix trajectory type. */ using matrix_array3_t = std::vector<matrix_array2_t>; -/** Eigen scalar type. */ -using eigen_scalar_t = Eigen::Matrix<scalar_t, 1, 1>; -/** Eigen scalar trajectory type. */ -using eigen_scalar_array_t = std::vector<eigen_scalar_t>; -/** Array of eigen scalar trajectory type. */ -using eigen_scalar_array2_t = std::vector<eigen_scalar_array_t>; -/** Array of arrays of eigen scalar trajectory type. */ -using eigen_scalar_array3_t = std::vector<eigen_scalar_array2_t>; - /** * Defines the linear approximation of a scalar function * f(x,u) = dfdx' dx + dfdu' du + f diff --git a/ocs2_ddp/include/ocs2_ddp/unsupported/SLQ_Hamiltonian.h b/ocs2_ddp/include/ocs2_ddp/unsupported/SLQ_Hamiltonian.h index 73085e948..df32f839e 100644 --- a/ocs2_ddp/include/ocs2_ddp/unsupported/SLQ_Hamiltonian.h +++ b/ocs2_ddp/include/ocs2_ddp/unsupported/SLQ_Hamiltonian.h @@ -72,9 +72,6 @@ class SLQ_Hamiltonian : public DDP_BASE<STATE_DIM, INPUT_DIM> { using scalar_array_t = typename BASE::scalar_array_t; using scalar_array2_t = typename BASE::scalar_array2_t; using scalar_array3_t = typename BASE::scalar_array3_t; - using eigen_scalar_t = typename BASE::eigen_scalar_t; - using eigen_scalar_array_t = typename BASE::eigen_scalar_array_t; - using eigen_scalar_array2_t = typename BASE::eigen_scalar_array2_t; using state_vector_t = typename BASE::state_vector_t; using state_vector_array_t = typename BASE::state_vector_array_t; using state_vector_array2_t = typename BASE::state_vector_array2_t; @@ -275,7 +272,7 @@ class SLQ_Hamiltonian : public DDP_BASE<STATE_DIM, INPUT_DIM> { * @param [in] sFinal: The final s for Riccati equation. */ void solveRiccatiEquationsWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, const eigen_scalar_t& sFinal); + const state_vector_t& SvFinal, const scalar_t sFinal); /** * Solves a set of Riccati equations for the partition in the given index for nominal time trajectory stamp. @@ -288,7 +285,7 @@ class SLQ_Hamiltonian : public DDP_BASE<STATE_DIM, INPUT_DIM> { * @param [in] sFinal: The final s for the current Riccati equation. */ void solveRiccatiEquationsForNominalTimeWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, const eigen_scalar_t& sFinal); + const state_vector_t& SvFinal, const scalar_t sFinal); /** * Type_1 constraints error correction compensation which solves a set of error Riccati equations for the partition in the given index. @@ -310,7 +307,7 @@ class SLQ_Hamiltonian : public DDP_BASE<STATE_DIM, INPUT_DIM> { * @param [in] SveFinal: The final Sve for the current Riccati equation. */ void solveSlqRiccatiEquationsWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, const eigen_scalar_t& sFinal, const state_vector_t& SveFinal); + const state_vector_t& SvFinal, const scalar_t sFinal, const state_vector_t& SveFinal); /** * Full Backward Sweep method uses exponential method instead of ODE to solve Riccati equations. @@ -324,7 +321,7 @@ class SLQ_Hamiltonian : public DDP_BASE<STATE_DIM, INPUT_DIM> { * @param [in] constraintStepSize: type-1 constraint step-size */ void fullRiccatiBackwardSweepWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, const state_vector_t& SveFinal, const eigen_scalar_t& sFinal, + const state_vector_t& SvFinal, const state_vector_t& SveFinal, const scalar_t sFinal, const scalar_t& constraintStepSize); template <int DIM1, int DIM2 = 1> diff --git a/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/BVPEquations.h b/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/BVPEquations.h index 077623fb1..13f96a84b 100644 --- a/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/BVPEquations.h +++ b/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/BVPEquations.h @@ -60,7 +60,6 @@ class BVPEquations : public OdeBase<STATE_DIM * STATE_DIM + STATE_DIM> { using full_ode_vector_t = Eigen::Matrix<scalar_t, Full_ODE_VECTOR_DIM, 1>; using full_ode_vector_array_t = std::vector<full_ode_vector_t, Eigen::aligned_allocator<full_ode_vector_t> >; - using eigen_scalar_t = Eigen::Matrix<scalar_t, 1, 1>; using state_vector_t = Eigen::Matrix<scalar_t, STATE_DIM, 1>; using input_vector_t = Eigen::Matrix<scalar_t, INPUT_DIM, 1>; using state_state_matrix_t = Eigen::Matrix<scalar_t, STATE_DIM, STATE_DIM>; @@ -69,7 +68,6 @@ class BVPEquations : public OdeBase<STATE_DIM * STATE_DIM + STATE_DIM> { using state_input_matrix_t = Eigen::Matrix<scalar_t, STATE_DIM, INPUT_DIM>; using scalar_array_t = std::vector<scalar_t>; - using eigen_scalar_array_t = std::vector<eigen_scalar_t, Eigen::aligned_allocator<eigen_scalar_t> >; using state_vector_array_t = std::vector<state_vector_t, Eigen::aligned_allocator<state_vector_t> >; using input_vector_array_t = std::vector<input_vector_t, Eigen::aligned_allocator<input_vector_t> >; using state_state_matrix_array_t = std::vector<state_state_matrix_t, Eigen::aligned_allocator<state_state_matrix_t> >; diff --git a/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/SolveBVP.h b/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/SolveBVP.h index e52de8be5..1eed6f968 100644 --- a/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/SolveBVP.h +++ b/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/SolveBVP.h @@ -58,7 +58,6 @@ class SolveBVP { using full_ode_vector_t = typename bvp_equations_t::full_ode_vector_t; using full_ode_vector_array_t = typename bvp_equations_t::full_ode_vector_array_t; - using eigen_scalar_t = typename bvp_equations_t::eigen_scalar_t; using state_vector_t = typename bvp_equations_t::state_vector_t; using input_vector_t = typename bvp_equations_t::input_vector_t; using state_state_matrix_t = typename bvp_equations_t::state_state_matrix_t; @@ -68,7 +67,6 @@ class SolveBVP { using scalar_t = typename bvp_equations_t::scalar_t; using scalar_array_t = typename bvp_equations_t::scalar_array_t; - using eigen_scalar_array_t = typename bvp_equations_t::eigen_scalar_array_t; using state_vector_array_t = typename bvp_equations_t::state_vector_array_t; using input_vector_array_t = typename bvp_equations_t::input_vector_array_t; using state_state_matrix_array_t = typename bvp_equations_t::state_state_matrix_array_t; diff --git a/ocs2_ddp/include/ocs2_ddp/unsupported/implementation/SLQ_Hamiltonian.h b/ocs2_ddp/include/ocs2_ddp/unsupported/implementation/SLQ_Hamiltonian.h index afaa6384f..9a06a4771 100644 --- a/ocs2_ddp/include/ocs2_ddp/unsupported/implementation/SLQ_Hamiltonian.h +++ b/ocs2_ddp/include/ocs2_ddp/unsupported/implementation/SLQ_Hamiltonian.h @@ -403,7 +403,7 @@ template <size_t STATE_DIM, size_t INPUT_DIM, class LOGIC_RULES_T> void SLQ_Hamiltonian<STATE_DIM, INPUT_DIM, LOGIC_RULES_T>::solveRiccatiEquationsWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, const state_vector_t& SvFinal, - const eigen_scalar_t& sFinal) { + const scalar_t sFinal) { // set data for Riccati equations riccatiEquationsPtrStock_[workerIndex]->resetNumFunctionCalls(); riccatiEquationsPtrStock_[workerIndex]->setData( @@ -537,11 +537,8 @@ void SLQ_Hamiltonian<STATE_DIM, INPUT_DIM, LOGIC_RULES_T>::solveRiccatiEquations /******************************************************************************************************/ /******************************************************************************************************/ template <size_t STATE_DIM, size_t INPUT_DIM, class LOGIC_RULES_T> -void SLQ_Hamiltonian<STATE_DIM, INPUT_DIM, LOGIC_RULES_T>::solveRiccatiEquationsForNominalTimeWorker(size_t workerIndex, - const size_t& partitionIndex, - const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, - const eigen_scalar_t& sFinal) { +void SLQ_Hamiltonian<STATE_DIM, INPUT_DIM, LOGIC_RULES_T>::solveRiccatiEquationsForNominalTimeWorker( + size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, const state_vector_t& SvFinal, const scalar_t sFinal) { // set data for Riccati equations riccatiEquationsPtrStock_[workerIndex]->resetNumFunctionCalls(); riccatiEquationsPtrStock_[workerIndex]->setData( @@ -813,7 +810,7 @@ template <size_t STATE_DIM, size_t INPUT_DIM, class LOGIC_RULES_T> void SLQ_Hamiltonian<STATE_DIM, INPUT_DIM, LOGIC_RULES_T>::solveSlqRiccatiEquationsWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, const state_vector_t& SvFinal, - const eigen_scalar_t& sFinal, + const scalar_t sFinal, const state_vector_t& SveFinal) { if (settings_.useNominalTimeForBackwardPass_) { solveRiccatiEquationsForNominalTimeWorker(workerIndex, partitionIndex, SmFinal, SvFinal, sFinal); @@ -921,7 +918,7 @@ SLQ_Hamiltonian<STATE_DIM, INPUT_DIM, LOGIC_RULES_T>::integrateIncrement(size_t template <size_t STATE_DIM, size_t INPUT_DIM, class LOGIC_RULES_T> void SLQ_Hamiltonian<STATE_DIM, INPUT_DIM, LOGIC_RULES_T>::fullRiccatiBackwardSweepWorker( size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, const state_vector_t& SvFinal, - const state_vector_t& SveFinal, const eigen_scalar_t& sFinal, const scalar_t& constraintStepSize) { + const state_vector_t& SveFinal, const scalar_t sFinal, const scalar_t& constraintStepSize) { const size_t N = BASE::nominalTimeTrajectoriesStock_[partitionIndex].size(); // Riccati parameters diff --git a/ocs2_ocs2/include/ocs2_ocs2/GSLQPSolver.h b/ocs2_ocs2/include/ocs2_ocs2/GSLQPSolver.h index 01878f38c..978a22170 100644 --- a/ocs2_ocs2/include/ocs2_ocs2/GSLQPSolver.h +++ b/ocs2_ocs2/include/ocs2_ocs2/GSLQPSolver.h @@ -56,8 +56,6 @@ class GSLQSolver { typedef typename DIMENSIONS::MP_Options MP_Options_t; typedef typename DIMENSIONS::scalar_t scalar_t; typedef typename DIMENSIONS::scalar_array_t scalar_array_t; - typedef typename DIMENSIONS::eigen_scalar_t eigen_scalar_t; - typedef typename DIMENSIONS::eigen_scalar_array_t eigen_scalar_array_t; typedef typename DIMENSIONS::state_vector_t state_vector_t; typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t; typedef typename DIMENSIONS::input_vector_t input_vector_t; From 463099ed25ed8f4c24f1d25a77187dfd5b9eeb9a Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 11 Aug 2022 12:06:08 +0200 Subject: [PATCH 287/512] adding to the ocs2 metapackage --- ocs2/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ocs2/package.xml b/ocs2/package.xml index 834b4090e..14507f8d4 100644 --- a/ocs2/package.xml +++ b/ocs2/package.xml @@ -21,6 +21,7 @@ <exec_depend>ocs2_python_interface</exec_depend> <exec_depend>ocs2_pinocchio</exec_depend> <exec_depend>ocs2_robotic_tools</exec_depend> + <exec_depend>ocs2_perceptive</exec_depend> <exec_depend>ocs2_robotic_examples</exec_depend> <exec_depend>ocs2_thirdparty</exec_depend> <exec_depend>ocs2_raisim</exec_depend> From dfb559de886e4c32bb81a801810c8286e9304d78 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 11 Aug 2022 13:49:47 +0200 Subject: [PATCH 288/512] fix doc --- ocs2_ddp/src/GaussNewtonDDP.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 3b105e73a..dde23e961 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -818,7 +818,7 @@ bool GaussNewtonDDP::initializePrimalSolution() { rolloutInitialTrajectory(nominalPrimalData_.primalSolution); } catch (const std::exception& error) { - const std::string msg = "[GaussNewtonDDP::initializeDualPrimal] the initial rollout is unstable!\n"; + const std::string msg = "[GaussNewtonDDP::initializePrimalSolution] the initial rollout is unstable!\n"; throw std::runtime_error(msg + error.what()); } From 4b371984fe37f41438d419f37ff6fdf660c3a787 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 11 Aug 2022 14:07:00 +0200 Subject: [PATCH 289/512] few fixes from feature/sqp_with_metrics --- .../approximate_model/LinearQuadraticApproximator.h | 5 +++-- ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h | 8 +++++++- .../src/approximate_model/LinearQuadraticApproximator.cpp | 4 ++-- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h index 3a14543a0..1cc49908c 100644 --- a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h +++ b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h @@ -171,7 +171,7 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr * @param [in] state: The current state. * @param [in] input: The current input. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. - * @param [in] dynamicsViolation: The violation dynamics. It depends on the transcription method. + * @param [in] dynamicsViolation: The violation of dynamics. It depends on the transcription method. * @return The output Metrics. */ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, @@ -187,10 +187,11 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_ * @param [in] time: The current time. * @param [in] state: The current state. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. + * @param [in] dynamicsViolation: The violation of dynamics. It depends on the transcription method. * @return The output Metrics. */ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers); + const MultiplierCollection& multipliers, const vector_t& dynamicsViolation = vector_t()); /** * Compute the final-time Metrics (i.e. cost, softConstraints, and constraints). diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index 58ee34a8a..72184b59f 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -116,7 +116,13 @@ inline PerformanceIndex operator+(PerformanceIndex lhs, const PerformanceIndex& } template <typename SCALAR_T> -PerformanceIndex operator*(const SCALAR_T c, PerformanceIndex rhs) { +inline PerformanceIndex operator*(PerformanceIndex lhs, const SCALAR_T c) { + lhs *= c; // Copied lhs + return lhs; +} + +template <typename SCALAR_T> +inline PerformanceIndex operator*(const SCALAR_T c, PerformanceIndex rhs) { rhs *= c; // Copied rhs return rhs; } diff --git a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp index 8b0d874f8..fc2b984d8 100644 --- a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp +++ b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp @@ -299,7 +299,7 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_ /******************************************************************************************************/ /******************************************************************************************************/ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers) { + const MultiplierCollection& multipliers, const vector_t& dynamicsViolation) { auto& preComputation = *problem.preComputationPtr; Metrics metrics; @@ -308,7 +308,7 @@ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t tim metrics.cost = computeEventCost(problem, time, state); // Dynamics violation - // metrics.dynamicsViolation = vector_t(); + metrics.dynamicsViolation = dynamicsViolation; // Equality constraint metrics.stateEqConstraint = problem.preJumpEqualityConstraintPtr->getValue(time, state, preComputation); From 3e343be48233314c701979407a30d0a53d87a69d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 11 Aug 2022 14:13:28 +0200 Subject: [PATCH 290/512] clang format --- ocs2_core/src/model_data/Metrics.cpp | 2 +- ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 624385c8c..2ab367736 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -39,7 +39,7 @@ namespace ocs2 { /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ bool Metrics::isApprox(const Metrics& other, scalar_t prec) const { bool flag = std::abs(this->cost - other.cost) <= prec * std::min(std::abs(this->cost), std::abs(other.cost)) || - std::abs(this->cost - other.cost) < std::numeric_limits<scalar_t>::min(); + std::abs(this->cost - other.cost) < std::numeric_limits<scalar_t>::min(); flag = flag && this->dynamicsViolation.isApprox(other.dynamicsViolation, prec); flag = flag && toVector(this->stateEqConstraint).isApprox(toVector(other.stateEqConstraint), prec); flag = flag && toVector(this->stateInputEqConstraint).isApprox(toVector(other.stateInputEqConstraint), prec); diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index 72184b59f..dcbbe690b 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -160,7 +160,7 @@ inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& pe /** Computes the PerformanceIndex based on a given Metrics */ inline PerformanceIndex toPerformanceIndex(const Metrics& m) { PerformanceIndex performanceIndex; - performanceIndex.merit = 0.0; + performanceIndex.merit = 0.0; // left for the solver to fill performanceIndex.cost = m.cost; performanceIndex.dynamicsViolationSSE = m.dynamicsViolation.squaredNorm(); performanceIndex.equalityConstraintsSSE = constraintsSquaredNorm(m.stateEqConstraint) + constraintsSquaredNorm(m.stateInputEqConstraint); From 6cc894b73401d29ede619aab5666af916aca3adf Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 11 Aug 2022 14:33:37 +0200 Subject: [PATCH 291/512] remove inline --- ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index dcbbe690b..44b49dada 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -111,19 +111,19 @@ struct PerformanceIndex { }; inline PerformanceIndex operator+(PerformanceIndex lhs, const PerformanceIndex& rhs) { - lhs += rhs; // Copied lhs, add rhs to it. + lhs += rhs; // copied lhs, add rhs to it. return lhs; } template <typename SCALAR_T> -inline PerformanceIndex operator*(PerformanceIndex lhs, const SCALAR_T c) { - lhs *= c; // Copied lhs +PerformanceIndex operator*(PerformanceIndex lhs, const SCALAR_T c) { + lhs *= c; // copied lhs return lhs; } template <typename SCALAR_T> -inline PerformanceIndex operator*(const SCALAR_T c, PerformanceIndex rhs) { - rhs *= c; // Copied rhs +PerformanceIndex operator*(const SCALAR_T c, PerformanceIndex rhs) { + rhs *= c; // copied rhs return rhs; } From 98ba87ef8e07b7a61f043cda48e4281576f1de77 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 11 Aug 2022 18:51:12 +0200 Subject: [PATCH 292/512] enabling initialization from PrimalSolution trajectories --- ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h | 44 +++-- ocs2_ddp/src/GaussNewtonDDP.cpp | 189 +++++++++++++-------- 2 files changed, 148 insertions(+), 85 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h index 85b3437de..c21fc7e03 100644 --- a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h +++ b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h @@ -232,15 +232,33 @@ class GaussNewtonDDP : public SolverBase { } /** - * Forward integrate the system dynamics with given controller in primalSolution and operating trajectories. In general, it uses - * the given control policies and initial state, to integrate the system dynamics in the time period [initTime, finalTime]. - * However, if the provided controller does not cover the period [initTime, finalTime], it will use the controller till the - * final time of the controller and after it uses the operating trajectories. + * Forward integrate the system dynamics with the controller in inputPrimalSolution. In general, it uses the given + * control policies and the initial state, to integrate the system dynamics in the time period [initTime, finalTime]. + * However, if inputPrimalSolution's controller does not cover the period [initTime, finalTime], it will use the + * controller till the final time of the controller * - * @param [in, out] primalSolution: The resulting state-input trajectory. The primal solution is initialized with the controller - * and the modeSchedule. However, for StateTriggered Rollout the modeSchedule can be overwritten. + * @param [in] inputPrimalSolution: Its controller will be used for rollout. + * @param [out] outputPrimalSolution: The resulting PrimalSolution. + * @return True if the rollout was successful. */ - void rolloutInitialTrajectory(PrimalSolution& primalSolution); + bool rolloutInitialController(PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution); + + /** + * Extracts the PrimalSolution trajectories from inputPrimalSolution. In general, it will try to extract in time period + * [initTime, finalTime]. However, if inputPrimalSolution's timeTrajectory does not cover the period [initTime, finalTime], + * it will extract the solution until the last time of the timeTrajectory + * + * @param [in] inputPrimalSolution: Its controller will be used for rollout. + * @param [out] outputPrimalSolution: The resulting PrimalSolution. + * @return True if the extraction was successful. + */ + bool extractInitialTrajectories(PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution); + + /** + * It will check the content of the primalSolution, and if its final time is smaller than the current solver finalTime_, + * it will concatenate it with the result of Initializer. + */ + void rolloutInitializer(PrimalSolution& primalSolution); /** * Calculates the controller. This method uses the following variables. The method modifies unoptimizedController_. @@ -292,8 +310,8 @@ class GaussNewtonDDP : public SolverBase { */ void updateConstraintPenalties(scalar_t equalityConstraintsSSE); - /** Initializes the nominal primal based on the optimized ones. \ - * @return whether the rollout is a result of Initializer since the optimized controller was empty. + /** Initializes the nominal primal based on the optimized ones. + * @return True if the rollout is not purely from the Initializer. */ bool initializePrimalSolution(); @@ -321,13 +339,7 @@ class GaussNewtonDDP : public SolverBase { void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const ControllerBase* externalControllerPtr) override; - void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) override { - if (primalSolution.controllerPtr_ == nullptr) { - std::cerr - << "[GaussNewtonDDP] DDP cannot be warm started without a controller in the primal solution. Will revert to a cold start.\n"; - } - runImpl(initTime, initState, finalTime, primalSolution.controllerPtr_.get()); - } + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) override; protected: // nominal data diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index dde23e961..5c911ac4a 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -374,45 +374,84 @@ vector_t GaussNewtonDDP::getStateInputEqualityConstraintLagrangianImpl(scalar_t /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaussNewtonDDP::rolloutInitialTrajectory(PrimalSolution& primalSolution) { +bool GaussNewtonDDP::rolloutInitialController(PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution) { + if (inputPrimalSolution.controllerPtr_->empty()) { + return false; + } + + // cast to linear the controller + auto& inputLinearController = getLinearController(inputPrimalSolution); + + // adjust in-place the controller + std::ignore = trajectorySpread(inputPrimalSolution.modeSchedule_, getReferenceManager().getModeSchedule(), inputLinearController); + // after adjustment it might become empty + if (inputLinearController.empty()) { + return false; + } + + const auto finalTime = std::max(initTime_, std::min(inputLinearController.timeStamp_.back(), finalTime_)); + + if (initTime_ < finalTime) { + if (ddpSettings_.debugPrintRollout_) { + std::cerr << "[GaussNewtonDDP::rolloutInitialController] for t = [" << initTime_ << ", " << finalTime_ << "]\n"; + std::cerr << "\twill use controller for t = [" << initTime_ << ", " << finalTime << "]\n"; + } + outputPrimalSolution.controllerPtr_.swap(inputPrimalSolution.controllerPtr_); + std::ignore = rolloutTrajectory(*dynamicsForwardRolloutPtrStock_[0], initTime_, initState_, finalTime, outputPrimalSolution); + return true; + + } else { + return false; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool GaussNewtonDDP::extractInitialTrajectories(PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution) { + if (inputPrimalSolution.timeTrajectory_.empty()) { + return false; + } + + // adjust in-place the primalSolution + std::ignore = trajectorySpread(inputPrimalSolution.modeSchedule_, getReferenceManager().getModeSchedule(), inputPrimalSolution); + // after adjustment it might become empty + if (inputPrimalSolution.timeTrajectory_.empty()) { + return false; + } + + const auto finalTime = std::max(initTime_, std::min(inputPrimalSolution.timeTrajectory_.back(), finalTime_)); + + if (initTime_ < finalTime) { + if (ddpSettings_.debugPrintRollout_) { + std::cerr << "[GaussNewtonDDP::extractInitialTrajectories] for t = [" << initTime_ << ", " << finalTime_ << "]\n"; + std::cerr << "\twill use PrimalSolution trajectory for t = [" << initTime_ << ", " << finalTime << "]\n"; + } + extractPrimalSolution({initTime_, finalTime}, inputPrimalSolution, outputPrimalSolution); + return true; + + } else { + return false; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void GaussNewtonDDP::rolloutInitializer(PrimalSolution& primalSolution) { // create alias - auto* controllerPtr = primalSolution.controllerPtr_.get(); auto& modeSchedule = primalSolution.modeSchedule_; auto& timeTrajectory = primalSolution.timeTrajectory_; auto& stateTrajectory = primalSolution.stateTrajectory_; auto& inputTrajectory = primalSolution.inputTrajectory_; auto& postEventIndices = primalSolution.postEventIndices_; - // divide the rollout segment in controller rollout and operating points - const auto controllerAvailableTill = - controllerPtr->empty() ? initTime_ : static_cast<LinearController*>(controllerPtr)->timeStamp_.back(); - const auto controllerRolloutFromTo = std::make_pair(initTime_, std::max(initTime_, std::min(controllerAvailableTill, finalTime_))); - const auto operatingPointsFromTo = std::make_pair(controllerRolloutFromTo.second, finalTime_); - - // display - if (ddpSettings_.debugPrintRollout_) { - std::cerr << "[GaussNewtonDDP::rolloutInitialTrajectory] for t = [" << initTime_ << ", " << finalTime_ << "]\n"; - std::cerr << "\tcontroller available till t = " << controllerAvailableTill << "\n"; - if (controllerRolloutFromTo.first < controllerRolloutFromTo.second) { - std::cerr << "\twill use controller for t = [" << controllerRolloutFromTo.first << ", " << controllerRolloutFromTo.second << "]\n"; - } - if (operatingPointsFromTo.first < operatingPointsFromTo.second) { - std::cerr << "\twill use operating points for t = [" << operatingPointsFromTo.first << ", " << operatingPointsFromTo.second << "]\n"; - } - } + // finish rollout with Initializer + if (timeTrajectory.empty() || timeTrajectory.back() < finalTime_) { + scalar_t initTime = timeTrajectory.empty() ? initTime_ : timeTrajectory.back(); + const vector_t initState = stateTrajectory.empty() ? initState_ : stateTrajectory.back(); - // rollout with controller - vector_t xCurrent = initState_; - if (controllerRolloutFromTo.first < controllerRolloutFromTo.second) { - constexpr size_t workerIndex = 0; - xCurrent = dynamicsForwardRolloutPtrStock_[workerIndex]->run(controllerRolloutFromTo.first, initState_, controllerRolloutFromTo.second, - controllerPtr, modeSchedule, timeTrajectory, postEventIndices, - stateTrajectory, inputTrajectory); - } - - // finish rollout with operating points - if (operatingPointsFromTo.first < operatingPointsFromTo.second) { - // Remove last point of the controller rollout if it is directly past an event. Here where we want to use the initializer + // Remove last point of the rollout if it is directly past an event. Here where we want to use the Initializer // instead. However, we do start the integration at the state after the event. i.e. the jump map remains applied. if (!postEventIndices.empty() && postEventIndices.back() == (timeTrajectory.size() - 1)) { // Start new integration at the time point after the event @@ -420,14 +459,16 @@ void GaussNewtonDDP::rolloutInitialTrajectory(PrimalSolution& primalSolution) { stateTrajectory.pop_back(); inputTrajectory.pop_back(); // eventsPastTheEndIndeces is not removed because we need to mark the start of the operatingPointTrajectory as being after an event. + + initTime = std::min(initTime + numeric_traits::weakEpsilon<scalar_t>(), finalTime_); } scalar_array_t timeTrajectoryTail; size_array_t eventsPastTheEndIndecesTail; vector_array_t stateTrajectoryTail; vector_array_t inputTrajectoryTail; - xCurrent = initializerRolloutPtr_->run(operatingPointsFromTo.first, xCurrent, operatingPointsFromTo.second, nullptr, modeSchedule, - timeTrajectoryTail, eventsPastTheEndIndecesTail, stateTrajectoryTail, inputTrajectoryTail); + std::ignore = initializerRolloutPtr_->run(initTime, initState, finalTime_, nullptr, modeSchedule, timeTrajectoryTail, + eventsPastTheEndIndecesTail, stateTrajectoryTail, inputTrajectoryTail); // Add controller rollout length to event past the indeces for (auto& eventIndex : eventsPastTheEndIndecesTail) { @@ -440,18 +481,6 @@ void GaussNewtonDDP::rolloutInitialTrajectory(PrimalSolution& primalSolution) { stateTrajectory.insert(stateTrajectory.end(), stateTrajectoryTail.begin(), stateTrajectoryTail.end()); inputTrajectory.insert(inputTrajectory.end(), inputTrajectoryTail.begin(), inputTrajectoryTail.end()); } - - if (!xCurrent.allFinite()) { - throw std::runtime_error("[GaussNewtonDDP::rolloutInitialTrajectory] System became unstable during the initial rollout!"); - } - - // debug print - if (ddpSettings_.debugPrintRollout_) { - std::cerr << "\n++++++++++++++++++++++++++++++++++++++++\n"; - std::cerr << "[GaussNewtonDDP::rolloutInitialTrajectory] for t = [" << timeTrajectory.front() << ", " << timeTrajectory.back() << "]"; - std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++\n"; - RolloutBase::display(timeTrajectory, postEventIndices, stateTrajectory, &inputTrajectory); - } } /******************************************************************************************************/ @@ -800,30 +829,40 @@ void GaussNewtonDDP::updateConstraintPenalties(scalar_t equalityConstraintsSSE) /******************************************************************************************************/ bool GaussNewtonDDP::initializePrimalSolution() { try { - // adjust controller - if (!optimizedPrimalSolution_.controllerPtr_->empty()) { - std::ignore = trajectorySpread(optimizedPrimalSolution_.modeSchedule_, getReferenceManager().getModeSchedule(), - getLinearController(optimizedPrimalSolution_)); + // clear before starting to fill + nominalPrimalData_.clear(); + + // for non-StateTriggeredRollout case, set modeSchedule + nominalPrimalData_.primalSolution.modeSchedule_ = getReferenceManager().getModeSchedule(); + + // try to initialize with controller + bool initialSolutionExists = rolloutInitialController(optimizedPrimalSolution_, nominalPrimalData_.primalSolution); + + // try to initialize with PrimalSolution trajectories + if (!initialSolutionExists) { + // display + if (ddpSettings_.displayInfo_) { + std::cerr << "Initial controller is unavailable. Solver resorts to use PrimalSolution trajectories ...\n"; + } + + initialSolutionExists = extractInitialTrajectories(optimizedPrimalSolution_, nominalPrimalData_.primalSolution); } - // clear before starting to fill nominalPrimalData_ - nominalPrimalData_.clear(); + // display + if (!initialSolutionExists && ddpSettings_.displayInfo_) { + std::cerr << "Initial PrimalSolution trajectories are unavailable. Solver resorts to use Initializer ...\n"; + } + + // finish rollout with Initializer + rolloutInitializer(nominalPrimalData_.primalSolution); - // for non-StateTriggeredRollout initialize modeSchedule - nominalPrimalData_.primalSolution.modeSchedule_ = this->getReferenceManager().getModeSchedule(); - // nominal controller is stored in the optimized primal data as it is either the result of previous solve or it is provided by user and - // copied to optimized data container manually at the beginning of runImpl - nominalPrimalData_.primalSolution.controllerPtr_.swap(optimizedPrimalSolution_.controllerPtr_); - // perform a rollout - rolloutInitialTrajectory(nominalPrimalData_.primalSolution); + // true if the rollout is not purely from the Initializer + return initialSolutionExists; } catch (const std::exception& error) { const std::string msg = "[GaussNewtonDDP::initializePrimalSolution] the initial rollout is unstable!\n"; throw std::runtime_error(msg + error.what()); } - - // check if the rollout is a result of an empty controller - return nominalPrimalData_.primalSolution.controllerPtr_->empty(); } /******************************************************************************************************/ @@ -912,6 +951,21 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala runImpl(initTime, initState, finalTime); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) { + // use the given primalSolution + optimizedPrimalSolution_.controllerPtr_->clear(); + optimizedPrimalSolution_.modeSchedule_ = primalSolution.modeSchedule_; + optimizedPrimalSolution_.timeTrajectory_ = primalSolution.timeTrajectory_; + optimizedPrimalSolution_.postEventIndices_ = primalSolution.postEventIndices_; + optimizedPrimalSolution_.stateTrajectory_ = primalSolution.stateTrajectory_; + optimizedPrimalSolution_.inputTrajectory_ = primalSolution.inputTrajectory_; + + runImpl(initTime, initState, finalTime); +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -921,7 +975,7 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala std::cerr << "\n+++++++++++++ " + ddp::toAlgorithmName(ddpSettings_.algorithm_) + " solver is initialized ++++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; std::cerr << "\nSolver starts from initial time " << initTime << " to final time " << finalTime << ".\n"; - std::cerr << this->getReferenceManager().getModeSchedule() << "\n"; + std::cerr << getReferenceManager().getModeSchedule(); } // set cost desired trajectories @@ -950,16 +1004,13 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala // optimized --> nominal: initializes the nominal primal and dual solutions based on the optimized ones initializationTimer_.startTimer(); - bool unreliableValueFunction = initializePrimalSolution(); // true if the rollout is purely from the Initializer + bool initialSolutionExists = initializePrimalSolution(); // true if the rollout is not purely from the Initializer initializeDualSolutionAndMetrics(); performanceIndexHistory_.push_back(performanceIndex_); initializationTimer_.endTimer(); // display if (ddpSettings_.displayInfo_) { - if (unreliableValueFunction) { - std::cerr << "Note: system dynamics might have been violated during the initialization!\n"; - } std::cerr << performanceIndex_ << '\n'; } @@ -991,7 +1042,7 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala computeControllerTimer_.endTimer(); // the expected cost/merit calculated by the Riccati solution is not reliable - const auto lqModelExpectedCost = unreliableValueFunction ? performanceIndex_.merit : nominalDualData_.valueFunctionTrajectory.front().f; + const auto lqModelExpectedCost = initialSolutionExists ? nominalDualData_.valueFunctionTrajectory.front().f : performanceIndex_.merit; // nominal --> optimized: based on the current LQ solution updates the optimized primal and dual solutions takePrimalDualStep(lqModelExpectedCost); @@ -1007,8 +1058,8 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala // check convergence std::tie(isConverged, convergenceInfo) = searchStrategyPtr_->checkConvergence( - unreliableValueFunction, *std::prev(performanceIndexHistory_.end(), 2), performanceIndexHistory_.back()); - unreliableValueFunction = false; + !initialSolutionExists, *std::prev(performanceIndexHistory_.end(), 2), performanceIndexHistory_.back()); + initialSolutionExists = true; if (isConverged || (totalNumIterations_ - initIteration) == ddpSettings_.maxNumIterations_) { break; From 22434665b9bdd54f17d6c70643c49a33267c62ff Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 12 Aug 2022 10:08:27 +0200 Subject: [PATCH 293/512] minor changes --- ocs2_ddp/src/GaussNewtonDDP.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 5c911ac4a..6bb421928 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -460,24 +460,26 @@ void GaussNewtonDDP::rolloutInitializer(PrimalSolution& primalSolution) { inputTrajectory.pop_back(); // eventsPastTheEndIndeces is not removed because we need to mark the start of the operatingPointTrajectory as being after an event. - initTime = std::min(initTime + numeric_traits::weakEpsilon<scalar_t>(), finalTime_); + // adjusting the start time to correct for subsystem recognition + constexpr auto eps = numeric_traits::weakEpsilon<scalar_t>(); + initTime = std::min(initTime + eps, finalTime_); } scalar_array_t timeTrajectoryTail; - size_array_t eventsPastTheEndIndecesTail; + size_array_t postEventIndicesTail; vector_array_t stateTrajectoryTail; vector_array_t inputTrajectoryTail; std::ignore = initializerRolloutPtr_->run(initTime, initState, finalTime_, nullptr, modeSchedule, timeTrajectoryTail, - eventsPastTheEndIndecesTail, stateTrajectoryTail, inputTrajectoryTail); + postEventIndicesTail, stateTrajectoryTail, inputTrajectoryTail); // Add controller rollout length to event past the indeces - for (auto& eventIndex : eventsPastTheEndIndecesTail) { - eventIndex += stateTrajectory.size(); // This size of this trajectory part was missing when counting events in the tail + for (auto& eventIndex : postEventIndicesTail) { + eventIndex += stateTrajectory.size(); // the size of the old trajectory is missing when counting event's index } - // Concatenate the operating points to the rollout + // Concatenate timeTrajectory.insert(timeTrajectory.end(), timeTrajectoryTail.begin(), timeTrajectoryTail.end()); - postEventIndices.insert(postEventIndices.end(), eventsPastTheEndIndecesTail.begin(), eventsPastTheEndIndecesTail.end()); + postEventIndices.insert(postEventIndices.end(), postEventIndicesTail.begin(), postEventIndicesTail.end()); stateTrajectory.insert(stateTrajectory.end(), stateTrajectoryTail.begin(), stateTrajectoryTail.end()); inputTrajectory.insert(inputTrajectory.end(), inputTrajectoryTail.begin(), inputTrajectoryTail.end()); } From e42bc8e13e61d3d2196a63d693d31da2e22aaa78 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 12 Aug 2022 12:52:10 +0200 Subject: [PATCH 294/512] update GaussNewtonDDP::runImpl --- ocs2_ddp/src/GaussNewtonDDP.cpp | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 6bb421928..65ae1e8b6 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -840,7 +840,7 @@ bool GaussNewtonDDP::initializePrimalSolution() { // try to initialize with controller bool initialSolutionExists = rolloutInitialController(optimizedPrimalSolution_, nominalPrimalData_.primalSolution); - // try to initialize with PrimalSolution trajectories + // if rolloutInitialController failed, try to initialize with PrimalSolution's state-input trajectories if (!initialSolutionExists) { // display if (ddpSettings_.displayInfo_) { @@ -947,7 +947,7 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala if (linearControllerPtr == nullptr) { throw std::runtime_error("[GaussNewtonDDP::run] controller must be a LinearController type!"); } - *optimizedPrimalSolution_.controllerPtr_ = *linearControllerPtr; + optimizedPrimalSolution_.controllerPtr_.reset(linearControllerPtr->clone()); } runImpl(initTime, initState, finalTime); @@ -957,15 +957,21 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala /******************************************************************************************************/ /******************************************************************************************************/ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) { - // use the given primalSolution - optimizedPrimalSolution_.controllerPtr_->clear(); - optimizedPrimalSolution_.modeSchedule_ = primalSolution.modeSchedule_; - optimizedPrimalSolution_.timeTrajectory_ = primalSolution.timeTrajectory_; - optimizedPrimalSolution_.postEventIndices_ = primalSolution.postEventIndices_; - optimizedPrimalSolution_.stateTrajectory_ = primalSolution.stateTrajectory_; - optimizedPrimalSolution_.inputTrajectory_ = primalSolution.inputTrajectory_; + // if PrimalSolution's controller exists, use it for initialization + if (primalSolution.controllerPtr_ != nullptr && !primalSolution.controllerPtr_->empty()) { + optimizedPrimalSolution_.modeSchedule_ = primalSolution.modeSchedule_; + runImpl(initTime, initState, finalTime, primalSolution.controllerPtr_.get()); - runImpl(initTime, initState, finalTime); + } else { + // otherwise initialize with PrimalSolution's state-input trajectories + optimizedPrimalSolution_.controllerPtr_->clear(); + optimizedPrimalSolution_.modeSchedule_ = primalSolution.modeSchedule_; + optimizedPrimalSolution_.timeTrajectory_ = primalSolution.timeTrajectory_; + optimizedPrimalSolution_.postEventIndices_ = primalSolution.postEventIndices_; + optimizedPrimalSolution_.stateTrajectory_ = primalSolution.stateTrajectory_; + optimizedPrimalSolution_.inputTrajectory_ = primalSolution.inputTrajectory_; + runImpl(initTime, initState, finalTime); + } } /******************************************************************************************************/ From 5b5bb0eeb9b7c562eb11de2cadddecf1277e3d7d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 12 Aug 2022 13:28:55 +0200 Subject: [PATCH 295/512] moving PerformanceIndex to oc_data --- .../include/ocs2_ddp/DDP_HelperFunctions.h | 2 +- .../search_strategy/SearchStrategyBase.h | 3 +- ocs2_mpc/include/ocs2_mpc/MRT_BASE.h | 2 +- .../{oc_solver => oc_data}/PerformanceIndex.h | 0 .../include/ocs2_oc/oc_solver/SolverBase.h | 14 ++++---- ocs2_oc/src/lintTarget.cpp | 33 +++++++++++++++++-- .../common/RosMsgConversions.h | 2 +- .../ocs2_sqp/MultipleShootingSolverStatus.h | 4 +-- .../ocs2_sqp/MultipleShootingTranscription.h | 4 +-- 9 files changed, 46 insertions(+), 18 deletions(-) rename ocs2_oc/include/ocs2_oc/{oc_solver => oc_data}/PerformanceIndex.h (100%) diff --git a/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h b/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h index 371fa293c..2adca453e 100644 --- a/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h +++ b/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h @@ -34,8 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/model_data/Metrics.h> #include <ocs2_core/penalties/MultidimensionalPenalty.h> #include <ocs2_oc/oc_data/DualSolution.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> -#include <ocs2_oc/oc_solver/PerformanceIndex.h> #include <ocs2_oc/rollout/RolloutBase.h> #include "ocs2_ddp/DDP_Data.h" diff --git a/ocs2_ddp/include/ocs2_ddp/search_strategy/SearchStrategyBase.h b/ocs2_ddp/include/ocs2_ddp/search_strategy/SearchStrategyBase.h index 7192b3864..8d57eca51 100644 --- a/ocs2_ddp/include/ocs2_ddp/search_strategy/SearchStrategyBase.h +++ b/ocs2_ddp/include/ocs2_ddp/search_strategy/SearchStrategyBase.h @@ -38,11 +38,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/model_data/Metrics.h> #include <ocs2_core/model_data/ModelData.h> #include <ocs2_core/reference/ModeSchedule.h> - #include <ocs2_oc/oc_data/DualSolution.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/oc_data/PrimalSolution.h> #include <ocs2_oc/oc_data/ProblemMetrics.h> -#include <ocs2_oc/oc_solver/PerformanceIndex.h> #include "ocs2_ddp/search_strategy/StrategySettings.h" diff --git a/ocs2_mpc/include/ocs2_mpc/MRT_BASE.h b/ocs2_mpc/include/ocs2_mpc/MRT_BASE.h index 217f3c575..3d63a1ab8 100644 --- a/ocs2_mpc/include/ocs2_mpc/MRT_BASE.h +++ b/ocs2_mpc/include/ocs2_mpc/MRT_BASE.h @@ -41,8 +41,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/misc/LinearInterpolation.h> #include <ocs2_core/reference/ModeSchedule.h> #include <ocs2_core/reference/TargetTrajectories.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/oc_data/PrimalSolution.h> -#include <ocs2_oc/oc_solver/PerformanceIndex.h> #include <ocs2_oc/rollout/RolloutBase.h> #include "ocs2_mpc/CommandData.h" diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h similarity index 100% rename from ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h rename to ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h index 753aa9461..27dea8f33 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h @@ -37,14 +37,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/control/ControllerBase.h> -#include <ocs2_oc/oc_data/DualSolution.h> -#include <ocs2_oc/oc_data/PrimalSolution.h> -#include <ocs2_oc/oc_data/ProblemMetrics.h> -#include <ocs2_oc/oc_problem/OptimalControlProblem.h> -#include <ocs2_oc/oc_solver/PerformanceIndex.h> -#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h> -#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h> +#include "ocs2_oc/oc_data/DualSolution.h" +#include "ocs2_oc/oc_data/PerformanceIndex.h" +#include "ocs2_oc/oc_data/PrimalSolution.h" +#include "ocs2_oc/oc_data/ProblemMetrics.h" +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" #include "ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h" +#include "ocs2_oc/synchronized_module/ReferenceManagerInterface.h" +#include "ocs2_oc/synchronized_module/SolverSynchronizedModule.h" namespace ocs2 { diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 1f5887c14..af3c85ad6 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -1,9 +1,39 @@ -// Approximate model +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +// approximate model #include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> // oc_data #include <ocs2_oc/oc_data/DualSolution.h> #include <ocs2_oc/oc_data/LoopshapingPrimalSolution.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/oc_data/PrimalSolution.h> // oc_problem @@ -12,7 +42,6 @@ #include <ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h> // oc_solver -#include <ocs2_oc/oc_solver/PerformanceIndex.h> #include <ocs2_oc/oc_solver/SolverBase.h> // synchronized_module diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h index 435b2e70c..622c53caa 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h @@ -35,7 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/reference/ModeSchedule.h> #include <ocs2_core/reference/TargetTrajectories.h> #include <ocs2_mpc/SystemObservation.h> -#include <ocs2_oc/oc_solver/PerformanceIndex.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> // MPC messages #include <ocs2_msgs/lagrangian_metrics.h> diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h index 026a524dd..d2811de6e 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h @@ -30,7 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include <ocs2_core/Types.h> -#include <ocs2_oc/oc_solver/PerformanceIndex.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> namespace ocs2 { namespace multiple_shooting { @@ -60,4 +60,4 @@ enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; std::string toString(const Convergence& convergence); } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h index 92985b915..9e25e814d 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -31,8 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/integration/SensitivityIntegrator.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> -#include <ocs2_oc/oc_solver/PerformanceIndex.h> namespace ocs2 { namespace multiple_shooting { @@ -127,4 +127,4 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon const vector_t& x_next); } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 From 8af0d66908457a1faccb247c06463f2319ade388 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 12 Aug 2022 15:02:50 +0200 Subject: [PATCH 296/512] remove redefinition --- ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index ef5ce741a..44b49dada 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -121,12 +121,6 @@ PerformanceIndex operator*(PerformanceIndex lhs, const SCALAR_T c) { return lhs; } -template <typename SCALAR_T> -inline PerformanceIndex operator*(PerformanceIndex lhs, const SCALAR_T c) { - lhs *= c; // Copied lhs - return lhs; -} - template <typename SCALAR_T> PerformanceIndex operator*(const SCALAR_T c, PerformanceIndex rhs) { rhs *= c; // copied rhs From 16a1c3db556d9beff8ffb953a61f0628f8a52625 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 15 Aug 2022 15:44:28 +0200 Subject: [PATCH 297/512] unittests --- ocs2_ddp/test/CircularKinematicsTest.cpp | 12 +++++++----- ocs2_ddp/test/CorrectnessTest.cpp | 8 +++++--- ocs2_ddp/test/Exp0Test.cpp | 10 ++++++---- ocs2_ddp/test/Exp1Test.cpp | 11 ++++++----- ocs2_ddp/test/HybridSlqTest.cpp | 7 +++++-- ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp | 2 +- 6 files changed, 30 insertions(+), 20 deletions(-) diff --git a/ocs2_ddp/test/CircularKinematicsTest.cpp b/ocs2_ddp/test/CircularKinematicsTest.cpp index df37d25d7..aca231704 100644 --- a/ocs2_ddp/test/CircularKinematicsTest.cpp +++ b/ocs2_ddp/test/CircularKinematicsTest.cpp @@ -47,8 +47,9 @@ class CircularKinematicsTest : public testing::TestWithParam<std::tuple<ocs2::se static constexpr size_t STATE_DIM = 2; static constexpr size_t INPUT_DIM = 2; static constexpr ocs2::scalar_t timeStep = 0.01; + static constexpr ocs2::scalar_t minRelCost = 1e-3; + static constexpr ocs2::scalar_t constraintTolerance = 1e-5; static constexpr ocs2::scalar_t expectedCost = 0.1; - static constexpr ocs2::scalar_t expectedStateInputEqConstraintISE = 0.0; CircularKinematicsTest() { // optimal control problem @@ -109,8 +110,8 @@ class CircularKinematicsTest : public testing::TestWithParam<std::tuple<ocs2::se throw std::runtime_error("Not supported Algorithm!"); } ddpSettings.maxNumIterations_ = 150; - ddpSettings.minRelCost_ = 1e-3; - ddpSettings.constraintTolerance_ = 1e-5; + ddpSettings.minRelCost_ = minRelCost; + ddpSettings.constraintTolerance_ = constraintTolerance; ddpSettings.constraintPenaltyInitialValue_ = 2.0; ddpSettings.constraintPenaltyIncreaseRate_ = 1.5; ddpSettings.preComputeRiccatiTerms_ = false; @@ -133,7 +134,7 @@ class CircularKinematicsTest : public testing::TestWithParam<std::tuple<ocs2::se void performanceIndexTest(const ocs2::ddp::Settings& ddpSettings, const ocs2::PerformanceIndex& performanceIndex) const { const auto testName = getTestName(ddpSettings); EXPECT_LT(performanceIndex.cost - expectedCost, 0.0) << "MESSAGE: " << testName << ": failed in the cost test!"; - EXPECT_LT(fabs(performanceIndex.equalityConstraintsSSE - expectedStateInputEqConstraintISE), 10 * ddpSettings.constraintTolerance_) + EXPECT_NEAR(performanceIndex.equalityConstraintsSSE, 0.0, 10.0 * constraintTolerance) << "MESSAGE: " << testName << ": failed in state-input equality constraint ISE test!"; } @@ -148,8 +149,9 @@ class CircularKinematicsTest : public testing::TestWithParam<std::tuple<ocs2::se constexpr size_t CircularKinematicsTest::STATE_DIM; constexpr size_t CircularKinematicsTest::INPUT_DIM; constexpr ocs2::scalar_t CircularKinematicsTest::timeStep; +constexpr ocs2::scalar_t CircularKinematicsTest::minRelCost; +constexpr ocs2::scalar_t CircularKinematicsTest::constraintTolerance; constexpr ocs2::scalar_t CircularKinematicsTest::expectedCost; -constexpr ocs2::scalar_t CircularKinematicsTest::expectedStateInputEqConstraintISE; /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_ddp/test/CorrectnessTest.cpp b/ocs2_ddp/test/CorrectnessTest.cpp index c166adb59..81adcf112 100644 --- a/ocs2_ddp/test/CorrectnessTest.cpp +++ b/ocs2_ddp/test/CorrectnessTest.cpp @@ -53,6 +53,7 @@ class DDPCorrectness : public testing::TestWithParam<std::tuple<ocs2::search_str static constexpr size_t N = 50; static constexpr size_t STATE_DIM = 3; static constexpr size_t INPUT_DIM = 2; + static constexpr ocs2::scalar_t minRelCost = 1e-3; static constexpr ocs2::scalar_t solutionPrecision = 5e-3; static constexpr size_t numStateInputConstraints = 2; @@ -181,7 +182,7 @@ class DDPCorrectness : public testing::TestWithParam<std::tuple<ocs2::search_str ddpSettings.absTolODE_ = 1e-10; ddpSettings.relTolODE_ = 1e-7; ddpSettings.maxNumStepsPerSecond_ = 10000; - ddpSettings.minRelCost_ = 1e-3; + ddpSettings.minRelCost_ = minRelCost; ddpSettings.nThreads_ = numThreads; ddpSettings.maxNumIterations_ = 2 + (numThreads - 1); // need an extra iteration for each added time partition ddpSettings.strategy_ = strategy; @@ -214,7 +215,7 @@ class DDPCorrectness : public testing::TestWithParam<std::tuple<ocs2::search_str void correctnessTest(const ocs2::ddp::Settings& ddpSettings, const ocs2::PerformanceIndex& performanceIndex, const ocs2::PrimalSolution& ddpSolution) const { const auto testName = getTestName(ddpSettings); - EXPECT_LT(fabs(performanceIndex.cost - qpCost), 10 * ddpSettings.minRelCost_) + EXPECT_NEAR(performanceIndex.cost, qpCost, 10.0 * minRelCost) << "MESSAGE: " << testName << ": failed in the optimal cost test!"; EXPECT_LT(relError(ddpSolution.stateTrajectory_.back(), qpSolution.stateTrajectory.back()), solutionPrecision) << "MESSAGE: " << testName << ": failed in the optimal final state test!"; @@ -242,8 +243,9 @@ class DDPCorrectness : public testing::TestWithParam<std::tuple<ocs2::search_str constexpr size_t DDPCorrectness::N; constexpr size_t DDPCorrectness::STATE_DIM; constexpr size_t DDPCorrectness::INPUT_DIM; -constexpr size_t DDPCorrectness::numStateInputConstraints; +constexpr ocs2::scalar_t DDPCorrectness::minRelCost; constexpr ocs2::scalar_t DDPCorrectness::solutionPrecision; +constexpr size_t DDPCorrectness::numStateInputConstraints; /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_ddp/test/Exp0Test.cpp b/ocs2_ddp/test/Exp0Test.cpp index 8c21d558d..30d083b16 100644 --- a/ocs2_ddp/test/Exp0Test.cpp +++ b/ocs2_ddp/test/Exp0Test.cpp @@ -45,6 +45,7 @@ class Exp0 : public testing::Test { static constexpr size_t STATE_DIM = 2; static constexpr size_t INPUT_DIM = 1; static constexpr ocs2::scalar_t timeStep = 1e-2; + static constexpr ocs2::scalar_t minRelCost = 1e-3; static constexpr ocs2::scalar_t expectedCost = 9.766; static constexpr ocs2::scalar_t expectedStateInputEqConstraintISE = 0.0; @@ -86,7 +87,7 @@ class Exp0 : public testing::Test { ddpSettings.backwardPassIntegratorType_ = ocs2::IntegratorType::ODE45; ddpSettings.maxNumStepsPerSecond_ = 10000; ddpSettings.maxNumIterations_ = 30; - ddpSettings.minRelCost_ = 1e-3; + ddpSettings.minRelCost_ = minRelCost; ddpSettings.checkNumericalStability_ = true; ddpSettings.useFeedbackPolicy_ = true; ddpSettings.debugPrintRollout_ = false; @@ -106,9 +107,9 @@ class Exp0 : public testing::Test { void performanceIndexTest(const ocs2::ddp::Settings& ddpSettings, const ocs2::PerformanceIndex& performanceIndex) const { const auto testName = getTestName(ddpSettings); - EXPECT_LT(fabs(performanceIndex.cost - expectedCost), 10 * ddpSettings.minRelCost_) + EXPECT_NEAR(performanceIndex.cost, expectedCost, 10.0 * minRelCost) << "MESSAGE: " << testName << ": failed in the total cost test!"; - EXPECT_LT(fabs(performanceIndex.equalityConstraintsSSE - expectedStateInputEqConstraintISE), 10 * ddpSettings.constraintTolerance_) + EXPECT_NEAR(performanceIndex.equalityConstraintsSSE, expectedStateInputEqConstraintISE, 10.0 * ddpSettings.constraintTolerance_) << "MESSAGE: " << testName << ": failed in state-input equality constraint ISE test!"; } @@ -124,6 +125,7 @@ class Exp0 : public testing::Test { constexpr size_t Exp0::STATE_DIM; constexpr size_t Exp0::INPUT_DIM; constexpr ocs2::scalar_t Exp0::timeStep; +constexpr ocs2::scalar_t Exp0::minRelCost; constexpr ocs2::scalar_t Exp0::expectedCost; constexpr ocs2::scalar_t Exp0::expectedStateInputEqConstraintISE; @@ -231,8 +233,8 @@ TEST_F(Exp0, ddp_hamiltonian) { constexpr size_t numThreads = 2; auto ddpSettings = getSettings(ocs2::ddp::Algorithm::SLQ, numThreads, ocs2::search_strategy::Type::LINE_SEARCH); ddpSettings.useFeedbackPolicy_ = true; - ddpSettings.minRelCost_ = 1e-9; // to allow more iterations that the effect of final linesearch is negligible ddpSettings.maxNumIterations_ = 50; + ddpSettings.minRelCost_ = 1e-9; // to allow more iterations that the effect of final linesearch is negligible // dynamics and rollout ocs2::EXP0_System systemDynamics(referenceManagerPtr); diff --git a/ocs2_ddp/test/Exp1Test.cpp b/ocs2_ddp/test/Exp1Test.cpp index d82429fd8..5d411f2cd 100644 --- a/ocs2_ddp/test/Exp1Test.cpp +++ b/ocs2_ddp/test/Exp1Test.cpp @@ -45,8 +45,8 @@ class Exp1 : public testing::TestWithParam<std::tuple<ocs2::search_strategy::Typ static constexpr size_t STATE_DIM = 2; static constexpr size_t INPUT_DIM = 1; static constexpr ocs2::scalar_t timeStep = 1e-2; + static constexpr ocs2::scalar_t minRelCost = 1e-3; static constexpr ocs2::scalar_t expectedCost = 5.4399; - static constexpr ocs2::scalar_t expectedStateInputEqConstraintISE = 0.0; Exp1() { // event times @@ -84,6 +84,7 @@ class Exp1 : public testing::TestWithParam<std::tuple<ocs2::search_strategy::Typ ddpSettings.displayInfo_ = false; ddpSettings.displayShortSummary_ = display; ddpSettings.maxNumIterations_ = 30; + ddpSettings.minRelCost_ = minRelCost; ddpSettings.checkNumericalStability_ = true; ddpSettings.absTolODE_ = 1e-10; ddpSettings.relTolODE_ = 1e-7; @@ -108,9 +109,9 @@ class Exp1 : public testing::TestWithParam<std::tuple<ocs2::search_strategy::Typ void performanceIndexTest(const ocs2::ddp::Settings& ddpSettings, const ocs2::PerformanceIndex& performanceIndex) const { const auto testName = getTestName(ddpSettings); - EXPECT_LT(fabs(performanceIndex.cost - expectedCost), 10 * ddpSettings.minRelCost_) + EXPECT_NEAR(performanceIndex.cost, expectedCost, 10.0 * minRelCost) << "MESSAGE: " << testName << ": failed in the total cost test!"; - EXPECT_LT(fabs(performanceIndex.equalityConstraintsSSE - expectedStateInputEqConstraintISE), 10 * ddpSettings.constraintTolerance_) + EXPECT_NEAR(performanceIndex.equalityConstraintsSSE, 0.0, 10.0 * ddpSettings.constraintTolerance_) << "MESSAGE: " << testName << ": failed in state-input equality constraint ISE test!"; } @@ -126,8 +127,8 @@ class Exp1 : public testing::TestWithParam<std::tuple<ocs2::search_strategy::Typ constexpr size_t Exp1::STATE_DIM; constexpr size_t Exp1::INPUT_DIM; constexpr ocs2::scalar_t Exp1::timeStep; +constexpr ocs2::scalar_t Exp1::minRelCost; constexpr ocs2::scalar_t Exp1::expectedCost; -constexpr ocs2::scalar_t Exp1::expectedStateInputEqConstraintISE; /******************************************************************************************************/ /******************************************************************************************************/ @@ -136,8 +137,8 @@ TEST_F(Exp1, ddp_hamiltonian) { // ddp settings auto ddpSettings = getSettings(ocs2::ddp::Algorithm::SLQ, 2, ocs2::search_strategy::Type::LINE_SEARCH); ddpSettings.useFeedbackPolicy_ = true; - ddpSettings.minRelCost_ = 1e-9; // to allow more iterations that the effect of final linesearch is negligible ddpSettings.maxNumIterations_ = 50; + ddpSettings.minRelCost_ = 1e-9; // to allow more iterations that the effect of final linesearch is negligible // dynamics and rollout ocs2::EXP1_System systemDynamics(referenceManagerPtr); diff --git a/ocs2_ddp/test/HybridSlqTest.cpp b/ocs2_ddp/test/HybridSlqTest.cpp index 68606a90f..f4c3c7da2 100644 --- a/ocs2_ddp/test/HybridSlqTest.cpp +++ b/ocs2_ddp/test/HybridSlqTest.cpp @@ -70,6 +70,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. TEST(HybridSlqTest, state_rollout_slq) { using namespace ocs2; + constexpr scalar_t minRelCost = 1e-6; // to avoid early termination + constexpr scalar_t constraintTolerance = 1e-4; + const ddp::Settings ddpSettings = [&]() { ddp::Settings settings; @@ -78,7 +81,7 @@ TEST(HybridSlqTest, state_rollout_slq) { settings.displayShortSummary_ = true; settings.maxNumIterations_ = 100; settings.nThreads_ = 1; - settings.minRelCost_ = 1e-6; // to avoid early termination + settings.minRelCost_ = minRelCost; settings.checkNumericalStability_ = false; settings.absTolODE_ = 1e-10; settings.relTolODE_ = 1e-7; @@ -166,5 +169,5 @@ TEST(HybridSlqTest, state_rollout_slq) { // Test 3: Check of cost const auto performanceIndecesST = slq.getPerformanceIndeces(); - EXPECT_LT(performanceIndecesST.cost - 20.1, 10.0 * ddpSettings.minRelCost_); + EXPECT_LT(performanceIndecesST.cost - 20.1, 10.0 * minRelCost); } diff --git a/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp b/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp index bc3020220..59b4bef89 100644 --- a/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp +++ b/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp @@ -207,5 +207,5 @@ TEST(BouncingMassTest, DISABLED_state_rollout_slq) { // Test 2: Check of cost function auto performanceIndeces = slq.getPerformanceIndeces(); constexpr scalar_t expectedCost = 7.15; - EXPECT_LT(std::fabs(performanceIndeces.cost - expectedCost), 100 * ddpSettings.minRelCost_); + EXPECT_NEAR(performanceIndeces.cost, expectedCost, 100.0 * ddpSettings.minRelCost_); } From b2c3d447206074550d98fc15780841d1e90a83df Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 15 Aug 2022 15:53:21 +0200 Subject: [PATCH 298/512] lm --- .../LevenbergMarquardtStrategy.h | 14 +++- .../LevenbergMarquardtStrategy.cpp | 73 +++++++------------ 2 files changed, 40 insertions(+), 47 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h b/ocs2_ddp/include/ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h index 01221f3ce..f48c909fd 100644 --- a/ocs2_ddp/include/ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h +++ b/ocs2_ddp/include/ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h @@ -83,16 +83,26 @@ class LevenbergMarquardtStrategy final : public SearchStrategyBase { matrix_t augmentHamiltonianHessian(const ModelData& modelData, const matrix_t& Hm) const override; private: + /** computes the ratio between actual reduction and predicted reduction */ + scalar_t reductionToPredictedReduction(const scalar_t actualReduction, const scalar_t expectedReduction) const { + if (std::abs(actualReduction) < baseSettings_.minRelCost || expectedReduction <= baseSettings_.minRelCost) { + return 1.0; + } else if (actualReduction < 0.0) { + return 0.0; + } else { + return actualReduction / expectedReduction; + } + } + // Levenberg-Marquardt struct LevenbergMarquardtModule { - scalar_t pho = 1.0; // the ratio between actual reduction and predicted reduction scalar_t riccatiMultiple = 0.0; // the Riccati multiple for Tikhonov regularization. scalar_t riccatiMultipleAdaptiveRatio = 1.0; // the adaptive ratio of geometric progression for Riccati multiple. size_t numSuccessiveRejections = 0; // the number of successive rejections of solution. }; const levenberg_marquardt::Settings settings_; - LevenbergMarquardtModule levenbergMarquardtModule_; + LevenbergMarquardtModule lmModule_; RolloutBase& rolloutRef_; OptimalControlProblem& optimalControlProblemRef_; diff --git a/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp b/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp index 0e79a1b14..f25b46ea4 100644 --- a/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp +++ b/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp @@ -53,7 +53,7 @@ LevenbergMarquardtStrategy::LevenbergMarquardtStrategy(search_strategy::Settings /******************************************************************************************************/ /******************************************************************************************************/ void LevenbergMarquardtStrategy::reset() { - levenbergMarquardtModule_ = LevenbergMarquardtModule(); + lmModule_ = LevenbergMarquardtModule(); } /******************************************************************************************************/ @@ -119,14 +119,7 @@ bool LevenbergMarquardtStrategy::run(const std::pair<scalar_t, scalar_t>& timePe // compute pho (the ratio between actual reduction and predicted reduction) const auto actualReduction = prevMerit - solution.performanceIndex.merit; - - if (std::abs(actualReduction) < baseSettings_.minRelCost || expectedReduction <= baseSettings_.minRelCost) { - levenbergMarquardtModule_.pho = 1.0; - } else if (actualReduction < 0.0) { - levenbergMarquardtModule_.pho = 0.0; - } else { - levenbergMarquardtModule_.pho = actualReduction / expectedReduction; - } + const auto pho = reductionToPredictedReduction(actualReduction, expectedReduction); // display if (baseSettings_.displayInfo) { @@ -134,73 +127,63 @@ bool LevenbergMarquardtStrategy::run(const std::pair<scalar_t, scalar_t>& timePe } // adjust riccatiMultipleAdaptiveRatio and riccatiMultiple - if (levenbergMarquardtModule_.pho < 0.25) { + if (pho < 0.25) { // increase riccatiMultipleAdaptiveRatio - levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio = - std::max(1.0, levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio) * settings_.riccatiMultipleDefaultRatio; + lmModule_.riccatiMultipleAdaptiveRatio = std::max(1.0, lmModule_.riccatiMultipleAdaptiveRatio) * settings_.riccatiMultipleDefaultRatio; // increase riccatiMultiple - auto riccatiMultipleTemp = levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio * levenbergMarquardtModule_.riccatiMultiple; - if (riccatiMultipleTemp > settings_.riccatiMultipleDefaultFactor) { - levenbergMarquardtModule_.riccatiMultiple = riccatiMultipleTemp; - } else { - levenbergMarquardtModule_.riccatiMultiple = settings_.riccatiMultipleDefaultFactor; - } + const auto riccatiMultipleTemp = lmModule_.riccatiMultipleAdaptiveRatio * lmModule_.riccatiMultiple; + lmModule_.riccatiMultiple = std::max(riccatiMultipleTemp, settings_.riccatiMultipleDefaultFactor); - } else if (levenbergMarquardtModule_.pho > 0.75) { + } else if (pho > 0.75) { // decrease riccatiMultipleAdaptiveRatio - levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio = - std::min(1.0, levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio) / settings_.riccatiMultipleDefaultRatio; + lmModule_.riccatiMultipleAdaptiveRatio = std::min(1.0, lmModule_.riccatiMultipleAdaptiveRatio) / settings_.riccatiMultipleDefaultRatio; // decrease riccatiMultiple - auto riccatiMultipleTemp = levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio * levenbergMarquardtModule_.riccatiMultiple; - if (riccatiMultipleTemp > settings_.riccatiMultipleDefaultFactor) { - levenbergMarquardtModule_.riccatiMultiple = riccatiMultipleTemp; - } else { - levenbergMarquardtModule_.riccatiMultiple = 0.0; - } + const auto riccatiMultipleTemp = lmModule_.riccatiMultipleAdaptiveRatio * lmModule_.riccatiMultiple; + lmModule_.riccatiMultiple = (riccatiMultipleTemp > settings_.riccatiMultipleDefaultFactor) ? riccatiMultipleTemp : 0.0; + } else { - levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio = 1.0; - // levenbergMarquardtModule_.riccatiMultiple will not change. + lmModule_.riccatiMultipleAdaptiveRatio = 1.0; + // lmModule_.riccatiMultiple will not change. } // display if (baseSettings_.displayInfo) { std::stringstream displayInfo; - if (levenbergMarquardtModule_.numSuccessiveRejections == 0) { - displayInfo << "The step is accepted with pho: " << levenbergMarquardtModule_.pho << ". "; + if (lmModule_.numSuccessiveRejections == 0) { + displayInfo << "The step is accepted with pho: " << pho << ". "; } else { - displayInfo << "The step is rejected with pho: " << levenbergMarquardtModule_.pho << " (" - << levenbergMarquardtModule_.numSuccessiveRejections << " out of " << settings_.maxNumSuccessiveRejections << "). "; + displayInfo << "The step is rejected with pho: " << pho << " (" << lmModule_.numSuccessiveRejections << " out of " + << settings_.maxNumSuccessiveRejections << "). "; } - if (numerics::almost_eq(levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio, 1.0)) { + if (numerics::almost_eq(lmModule_.riccatiMultipleAdaptiveRatio, 1.0)) { displayInfo << "The Riccati multiple is kept constant: "; - } else if (levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio < 1.0) { + } else if (lmModule_.riccatiMultipleAdaptiveRatio < 1.0) { displayInfo << "The Riccati multiple is decreased to: "; } else { displayInfo << "The Riccati multiple is increased to: "; } - displayInfo << levenbergMarquardtModule_.riccatiMultiple << ", with ratio: " << levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio - << ".\n"; + displayInfo << lmModule_.riccatiMultiple << ", with ratio: " << lmModule_.riccatiMultipleAdaptiveRatio << ".\n"; std::cerr << displayInfo.str(); } // max accepted number of successive rejections - if (levenbergMarquardtModule_.numSuccessiveRejections > settings_.maxNumSuccessiveRejections) { + if (lmModule_.numSuccessiveRejections > settings_.maxNumSuccessiveRejections) { throw std::runtime_error("The maximum number of successive solution rejections has been reached!"); } // accept or reject the step and modify numSuccessiveRejections - if (levenbergMarquardtModule_.pho >= settings_.minAcceptedPho) { + if (pho >= settings_.minAcceptedPho) { // accept the solution - levenbergMarquardtModule_.numSuccessiveRejections = 0; + lmModule_.numSuccessiveRejections = 0; return true; } else { // reject the solution - ++levenbergMarquardtModule_.numSuccessiveRejections; + ++lmModule_.numSuccessiveRejections; return false; } } @@ -218,7 +201,7 @@ std::pair<bool, std::string> LevenbergMarquardtStrategy::checkConvergence(bool u const scalar_t previousTotalCost = previousPerformanceIndex.cost + previousPerformanceIndex.equalityLagrangian + previousPerformanceIndex.inequalityLagrangian; const scalar_t relCost = std::abs(currentTotalCost - previousTotalCost); - if (levenbergMarquardtModule_.numSuccessiveRejections == 0 && !unreliableControllerIncrement) { + if (lmModule_.numSuccessiveRejections == 0 && !unreliableControllerIncrement) { isCostFunctionConverged = relCost <= baseSettings_.minRelCost; } const bool isConstraintsSatisfied = currentPerformanceIndex.equalityConstraintsSSE <= baseSettings_.constraintTolerance; @@ -250,8 +233,8 @@ void LevenbergMarquardtStrategy::computeRiccatiModification(const ModelData& pro // deltaQm, deltaRm, deltaPm deltaQm.setZero(projectedModelData.stateDim, projectedModelData.stateDim); - deltaGv.noalias() = levenbergMarquardtModule_.riccatiMultiple * BmProjected.transpose() * HvProjected; - deltaGm.noalias() = levenbergMarquardtModule_.riccatiMultiple * BmProjected.transpose() * AmProjected; + deltaGv.noalias() = lmModule_.riccatiMultiple * BmProjected.transpose() * HvProjected; + deltaGm.noalias() = lmModule_.riccatiMultiple * BmProjected.transpose() * AmProjected; } /******************************************************************************************************/ @@ -259,7 +242,7 @@ void LevenbergMarquardtStrategy::computeRiccatiModification(const ModelData& pro /******************************************************************************************************/ matrix_t LevenbergMarquardtStrategy::augmentHamiltonianHessian(const ModelData& modelData, const matrix_t& Hm) const { matrix_t HmAug = Hm; - HmAug.noalias() += levenbergMarquardtModule_.riccatiMultiple * modelData.dynamics.dfdu.transpose() * modelData.dynamics.dfdu; + HmAug.noalias() += lmModule_.riccatiMultiple * modelData.dynamics.dfdu.transpose() * modelData.dynamics.dfdu; return HmAug; } From edf89c499a98bb1f38a6ee2896773ca65330f914 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 15 Aug 2022 17:14:26 +0200 Subject: [PATCH 299/512] improving ls --- .../src/search_strategy/LineSearchStrategy.cpp | 18 +++--------------- 1 file changed, 3 insertions(+), 15 deletions(-) diff --git a/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp b/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp index 4d218eb61..546eceee7 100644 --- a/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp +++ b/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp @@ -230,27 +230,15 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId) { * cost should be better than the baseline cost but learning rate should * be as high as possible. This is equivalent to a single core line search. */ - const bool progressCondition = workersSolution_[taskId].performanceIndex.merit < (baselineMerit_ * (1.0 - 1e-3 * stepLength)); const bool armijoCondition = workersSolution_[taskId].performanceIndex.merit < (baselineMerit_ - settings_.armijoCoefficient * stepLength * unoptimizedControllerUpdateIS_); - - if (armijoCondition && stepLength > bestStepSize_) { + if (armijoCondition && stepLength > bestStepSize_) { // save solution bestStepSize_ = stepLength; swap(*bestSolutionRef_, workersSolution_[taskId]); - - // whether to stop all other thread. - terminateLinesearchTasks = true; - for (size_t i = 0; i < alphaExp; i++) { - if (!alphaProcessed_[i]) { - terminateLinesearchTasks = false; - break; - } - } // end of i loop - - } // end of if + terminateLinesearchTasks = std::all_of(alphaProcessed_.cbegin(), alphaProcessed_.cbegin() + alphaExp, [](bool f) { return f; }); + } alphaProcessed_[alphaExp] = true; - } // end lock // kill other ongoing line search tasks From cadf2aa22e7062294280cafd47f0651ad7864649 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 15 Aug 2022 17:19:04 +0200 Subject: [PATCH 300/512] minor update --- ocs2_ddp/test/Exp0Test.cpp | 4 +--- ocs2_ddp/test/HybridSlqTest.cpp | 1 - 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/ocs2_ddp/test/Exp0Test.cpp b/ocs2_ddp/test/Exp0Test.cpp index 30d083b16..8280ac37b 100644 --- a/ocs2_ddp/test/Exp0Test.cpp +++ b/ocs2_ddp/test/Exp0Test.cpp @@ -47,7 +47,6 @@ class Exp0 : public testing::Test { static constexpr ocs2::scalar_t timeStep = 1e-2; static constexpr ocs2::scalar_t minRelCost = 1e-3; static constexpr ocs2::scalar_t expectedCost = 9.766; - static constexpr ocs2::scalar_t expectedStateInputEqConstraintISE = 0.0; Exp0() { // event times @@ -109,7 +108,7 @@ class Exp0 : public testing::Test { const auto testName = getTestName(ddpSettings); EXPECT_NEAR(performanceIndex.cost, expectedCost, 10.0 * minRelCost) << "MESSAGE: " << testName << ": failed in the total cost test!"; - EXPECT_NEAR(performanceIndex.equalityConstraintsSSE, expectedStateInputEqConstraintISE, 10.0 * ddpSettings.constraintTolerance_) + EXPECT_NEAR(performanceIndex.equalityConstraintsSSE, 0.0, 10.0 * ddpSettings.constraintTolerance_) << "MESSAGE: " << testName << ": failed in state-input equality constraint ISE test!"; } @@ -127,7 +126,6 @@ constexpr size_t Exp0::INPUT_DIM; constexpr ocs2::scalar_t Exp0::timeStep; constexpr ocs2::scalar_t Exp0::minRelCost; constexpr ocs2::scalar_t Exp0::expectedCost; -constexpr ocs2::scalar_t Exp0::expectedStateInputEqConstraintISE; /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_ddp/test/HybridSlqTest.cpp b/ocs2_ddp/test/HybridSlqTest.cpp index f4c3c7da2..3e3defe93 100644 --- a/ocs2_ddp/test/HybridSlqTest.cpp +++ b/ocs2_ddp/test/HybridSlqTest.cpp @@ -71,7 +71,6 @@ TEST(HybridSlqTest, state_rollout_slq) { using namespace ocs2; constexpr scalar_t minRelCost = 1e-6; // to avoid early termination - constexpr scalar_t constraintTolerance = 1e-4; const ddp::Settings ddpSettings = [&]() { ddp::Settings settings; From c32d1d17f37f227df058a96e8e6cc667184c410d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 15 Aug 2022 17:39:04 +0200 Subject: [PATCH 301/512] doc update --- ocs2_ddp/include/ocs2_ddp/DDP_Settings.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/DDP_Settings.h b/ocs2_ddp/include/ocs2_ddp/DDP_Settings.h index 9cb6c76d6..f6b41fd68 100644 --- a/ocs2_ddp/include/ocs2_ddp/DDP_Settings.h +++ b/ocs2_ddp/include/ocs2_ddp/DDP_Settings.h @@ -96,13 +96,12 @@ struct Settings { /** The backward pass integrator type: SLQ uses it for solving Riccati equation and ILQR uses it for discretizing LQ approximation. */ IntegratorType backwardPassIntegratorType_ = IntegratorType::ODE45; - /** The initial coefficient of the quadratic penalty function in augmented Lagrangian method. It should be greater than one. */ + /** The initial coefficient of the quadratic penalty function in the merit function. It should be greater than one. */ scalar_t constraintPenaltyInitialValue_ = 2.0; - /** The rate that the coefficient of the quadratic penalty function in augmented Lagrangian method grows. It should be greater than - * one. */ + /** The rate that the coefficient of the quadratic penalty function in the merit function grows. It should be greater than one. */ scalar_t constraintPenaltyIncreaseRate_ = 2.0; - /** If true, terms of the Riccati equation will be precomputed before interpolation in the flow-map */ + /** If true, terms of the Riccati equation will be pre-computed before interpolation in the flow-map */ bool preComputeRiccatiTerms_ = true; /** Use either the optimized control policy (true) or the optimized state-input trajectory (false). */ From b75421661a73c466d4c9b2f20fe904ab22d78e09 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 10:59:03 +0200 Subject: [PATCH 302/512] switch to cpp14 --- ocs2_core/cmake/ocs2_cxx_flags.cmake | 2 +- .../TrajectorySpreading.cpp | 26 +++---------------- 2 files changed, 5 insertions(+), 23 deletions(-) diff --git a/ocs2_core/cmake/ocs2_cxx_flags.cmake b/ocs2_core/cmake/ocs2_cxx_flags.cmake index 1735d016f..7f38eb333 100644 --- a/ocs2_core/cmake/ocs2_cxx_flags.cmake +++ b/ocs2_core/cmake/ocs2_cxx_flags.cmake @@ -22,5 +22,5 @@ list(APPEND OCS2_CXX_FLAGS ) # Cpp standard version -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp b/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp index 266eee23b..a89511a97 100644 --- a/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp +++ b/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp @@ -44,23 +44,6 @@ std::ostream& operator<<(std::ostream& os, const std::pair<int, int>& ind) { os << "(" << ind.first << ", " << ind.second << ")"; return os; } - -/** - * Returns the first mismatching pair of elements from two ranges: one defined by [first1, last1) and - * another defined by [first2,last2). - * - * TODO deprecate this function once switched to c++14 and use: std::mismatch(first1, last1, first2, last2) - */ -template <class InputIt1, class InputIt2> -std::pair<InputIt1, InputIt2> mismatch(InputIt1 first1, InputIt1 last1, InputIt2 first2, InputIt2 last2) { - auto mismatchedIndex = std::mismatch(first1, last1, first2); - while (std::distance(last2, mismatchedIndex.second) > 0) { - --mismatchedIndex.first; - --mismatchedIndex.second; - } - return mismatchedIndex; -} - } // anonymous namespace /******************************************************************************************************/ @@ -90,12 +73,11 @@ auto TrajectorySpreading::set(const ModeSchedule& oldModeSchedule, const ModeSch // this means: mode[firstMatchingModeIndex + w] != updatedMode[updatedFirstMatchingModeIndex + w] size_t w = 0; while (oldStartIndexOfMatchedSequence < oldModeSchedule.modeSequence.size()) { - // TODO: change to std::mismatch(first1, last1, first2, last2). It is supported since c++14 // +1 to include the last active mode - const auto mismatchedIndex = mismatch(oldModeSchedule.modeSequence.cbegin() + oldStartIndexOfMatchedSequence, - oldModeSchedule.modeSequence.cbegin() + oldLastActiveModeIndex + 1, - newModeSchedule.modeSequence.cbegin() + newStartIndexOfMatchedSequence, - newModeSchedule.modeSequence.cbegin() + newLastActiveModeIndex + 1); + const auto mismatchedIndex = std::mismatch(oldModeSchedule.modeSequence.cbegin() + oldStartIndexOfMatchedSequence, + oldModeSchedule.modeSequence.cbegin() + oldLastActiveModeIndex + 1, + newModeSchedule.modeSequence.cbegin() + newStartIndexOfMatchedSequence, + newModeSchedule.modeSequence.cbegin() + newLastActiveModeIndex + 1); w = std::distance(oldModeSchedule.modeSequence.begin() + oldStartIndexOfMatchedSequence, mismatchedIndex.first); From e63b08e216359b117c414b73760ae750ffcaab1a Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 15:28:37 +0200 Subject: [PATCH 303/512] core --- .../augmented/ModifiedRelaxedBarrierPenalty.h | 2 +- .../penalties/augmented/QuadraticPenalty.h | 4 +--- .../augmented/SlacknessSquaredHingePenalty.h | 2 +- .../augmented/SmoothAbsolutePenalty.h | 4 +--- .../ocs2_core/thread_support/BufferedValue.h | 6 ++--- .../ocs2_core/thread_support/ThreadPool.h | 2 +- .../AugmentedLagrangian.cpp | 9 ++++---- ocs2_core/src/integration/Integrator.cpp | 18 +++++++-------- .../LoopshapingAugmentedLagrangian.cpp | 9 +++----- .../constraint/LoopshapingConstraint.cpp | 8 +++---- .../src/loopshaping/cost/LoopshapingCost.cpp | 6 ++--- .../dynamics/LoopshapingDynamics.cpp | 6 ++--- .../LoopshapingSoftConstraint.cpp | 8 +++---- .../src/penalties/MultidimensionalPenalty.cpp | 2 +- .../constraint/testConstraintCollection.cpp | 20 ++++++++-------- ocs2_core/test/cost/testCostCollection.cpp | 6 ++--- .../test/integration/IntegrationTest.cpp | 16 ++++++------- .../integration/testSensitivityIntegrator.cpp | 6 ++--- .../testLoopshapingFilterDynamics.cpp | 2 +- .../loopshaping/testQuadraticConstraint.h | 6 ++--- .../soft_constraint/testSoftConstraint.cpp | 23 +++++++++---------- .../test/thread_support/testSynchronized.cpp | 4 ++-- 22 files changed, 77 insertions(+), 92 deletions(-) diff --git a/ocs2_core/include/ocs2_core/penalties/augmented/ModifiedRelaxedBarrierPenalty.h b/ocs2_core/include/ocs2_core/penalties/augmented/ModifiedRelaxedBarrierPenalty.h index 239e20b53..a5bb74645 100644 --- a/ocs2_core/include/ocs2_core/penalties/augmented/ModifiedRelaxedBarrierPenalty.h +++ b/ocs2_core/include/ocs2_core/penalties/augmented/ModifiedRelaxedBarrierPenalty.h @@ -76,7 +76,7 @@ class ModifiedRelaxedBarrierPenalty final : public AugmentedPenaltyBase { /** Factory function */ static std::unique_ptr<ModifiedRelaxedBarrierPenalty> create(Config config) { - return std::unique_ptr<ModifiedRelaxedBarrierPenalty>(new ModifiedRelaxedBarrierPenalty(std::move(config))); + return std::make_unique<ModifiedRelaxedBarrierPenalty>(std::move(config)); } ~ModifiedRelaxedBarrierPenalty() override = default; diff --git a/ocs2_core/include/ocs2_core/penalties/augmented/QuadraticPenalty.h b/ocs2_core/include/ocs2_core/penalties/augmented/QuadraticPenalty.h index 317516ab5..7a8b0578c 100644 --- a/ocs2_core/include/ocs2_core/penalties/augmented/QuadraticPenalty.h +++ b/ocs2_core/include/ocs2_core/penalties/augmented/QuadraticPenalty.h @@ -72,9 +72,7 @@ class QuadraticPenalty final : public AugmentedPenaltyBase { explicit QuadraticPenalty(Config config) : config_(std::move(config)) {} /** Factory function */ - static std::unique_ptr<QuadraticPenalty> create(Config config) { - return std::unique_ptr<QuadraticPenalty>(new QuadraticPenalty(std::move(config))); - } + static std::unique_ptr<QuadraticPenalty> create(Config config) { return std::make_unique<QuadraticPenalty>(std::move(config)); } ~QuadraticPenalty() override = default; QuadraticPenalty* clone() const override { return new QuadraticPenalty(*this); } diff --git a/ocs2_core/include/ocs2_core/penalties/augmented/SlacknessSquaredHingePenalty.h b/ocs2_core/include/ocs2_core/penalties/augmented/SlacknessSquaredHingePenalty.h index f4ca9da26..09cadba0f 100644 --- a/ocs2_core/include/ocs2_core/penalties/augmented/SlacknessSquaredHingePenalty.h +++ b/ocs2_core/include/ocs2_core/penalties/augmented/SlacknessSquaredHingePenalty.h @@ -75,7 +75,7 @@ class SlacknessSquaredHingePenalty final : public AugmentedPenaltyBase { /** Factory function */ static std::unique_ptr<SlacknessSquaredHingePenalty> create(Config config) { - return std::unique_ptr<SlacknessSquaredHingePenalty>(new SlacknessSquaredHingePenalty(std::move(config))); + return std::make_unique<SlacknessSquaredHingePenalty>(std::move(config)); } ~SlacknessSquaredHingePenalty() override = default; diff --git a/ocs2_core/include/ocs2_core/penalties/augmented/SmoothAbsolutePenalty.h b/ocs2_core/include/ocs2_core/penalties/augmented/SmoothAbsolutePenalty.h index 836ba28b5..d6f1e6b0f 100644 --- a/ocs2_core/include/ocs2_core/penalties/augmented/SmoothAbsolutePenalty.h +++ b/ocs2_core/include/ocs2_core/penalties/augmented/SmoothAbsolutePenalty.h @@ -76,9 +76,7 @@ class SmoothAbsolutePenalty final : public AugmentedPenaltyBase { explicit SmoothAbsolutePenalty(Config config) : config_(std::move(config)) {} /** Factory function */ - static std::unique_ptr<SmoothAbsolutePenalty> create(Config config) { - return std::unique_ptr<SmoothAbsolutePenalty>(new SmoothAbsolutePenalty(std::move(config))); - } + static std::unique_ptr<SmoothAbsolutePenalty> create(Config config) { return std::make_unique<SmoothAbsolutePenalty>(std::move(config)); } ~SmoothAbsolutePenalty() override = default; SmoothAbsolutePenalty* clone() const override { return new SmoothAbsolutePenalty(*this); } diff --git a/ocs2_core/include/ocs2_core/thread_support/BufferedValue.h b/ocs2_core/include/ocs2_core/thread_support/BufferedValue.h index 9fc28dca7..8c1464b78 100644 --- a/ocs2_core/include/ocs2_core/thread_support/BufferedValue.h +++ b/ocs2_core/include/ocs2_core/thread_support/BufferedValue.h @@ -58,10 +58,10 @@ class BufferedValue { T& get() { return activeValue_; } /** Copy a new value into the buffer. */ - void setBuffer(const T& value) { buffer_.reset(std::unique_ptr<T>(new T(value))); } + void setBuffer(const T& value) { buffer_.reset(std::make_unique<T>(value)); } /** Move a new value into the buffer. */ - void setBuffer(T&& value) { buffer_.reset(std::unique_ptr<T>(new T(std::move(value)))); } + void setBuffer(T&& value) { buffer_.reset(std::make_unique<T>(std::move(value))); } /** * Replaces the active value with the value in the buffer. @@ -87,4 +87,4 @@ class BufferedValue { Synchronized<T> buffer_; }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/thread_support/ThreadPool.h b/ocs2_core/include/ocs2_core/thread_support/ThreadPool.h index 1ae4324dd..106e9fc21 100644 --- a/ocs2_core/include/ocs2_core/thread_support/ThreadPool.h +++ b/ocs2_core/include/ocs2_core/thread_support/ThreadPool.h @@ -141,7 +141,7 @@ struct ThreadPool::Task final : public ThreadPool::TaskBase { /**************************************************************************************************/ template <typename Functor> std::future<typename std::result_of<Functor(int)>::type> ThreadPool::run(Functor taskFunction) { - std::unique_ptr<Task<Functor>> taskPtr(new Task<Functor>(std::move(taskFunction))); + auto taskPtr = std::make_unique<Task<Functor>>(std::move(taskFunction)); auto future = taskPtr->packagedTask.get_future(); if (workerThreads_.empty()) { diff --git a/ocs2_core/src/augmented_lagrangian/AugmentedLagrangian.cpp b/ocs2_core/src/augmented_lagrangian/AugmentedLagrangian.cpp index 01db3586e..d24cb8746 100644 --- a/ocs2_core/src/augmented_lagrangian/AugmentedLagrangian.cpp +++ b/ocs2_core/src/augmented_lagrangian/AugmentedLagrangian.cpp @@ -36,7 +36,7 @@ namespace ocs2 { /******************************************************************************************************/ std::unique_ptr<StateAugmentedLagrangian> create(std::unique_ptr<StateConstraint> constraintPtr, std::vector<std::unique_ptr<augmented::AugmentedPenaltyBase>> penaltyPtrArray) { - return std::unique_ptr<StateAugmentedLagrangian>(new StateAugmentedLagrangian(std::move(constraintPtr), std::move(penaltyPtrArray))); + return std::make_unique<StateAugmentedLagrangian>(std::move(constraintPtr), std::move(penaltyPtrArray)); } /******************************************************************************************************/ @@ -44,7 +44,7 @@ std::unique_ptr<StateAugmentedLagrangian> create(std::unique_ptr<StateConstraint /******************************************************************************************************/ std::unique_ptr<StateAugmentedLagrangian> create(std::unique_ptr<StateConstraint> constraintPtr, std::unique_ptr<augmented::AugmentedPenaltyBase> penaltyPtr) { - return std::unique_ptr<StateAugmentedLagrangian>(new StateAugmentedLagrangian(std::move(constraintPtr), std::move(penaltyPtr))); + return std::make_unique<StateAugmentedLagrangian>(std::move(constraintPtr), std::move(penaltyPtr)); } /******************************************************************************************************/ @@ -52,8 +52,7 @@ std::unique_ptr<StateAugmentedLagrangian> create(std::unique_ptr<StateConstraint /******************************************************************************************************/ std::unique_ptr<StateInputAugmentedLagrangian> create(std::unique_ptr<StateInputConstraint> constraintPtr, std::vector<std::unique_ptr<augmented::AugmentedPenaltyBase>> penaltyPtrArray) { - return std::unique_ptr<StateInputAugmentedLagrangian>( - new StateInputAugmentedLagrangian(std::move(constraintPtr), std::move(penaltyPtrArray))); + return std::make_unique<StateInputAugmentedLagrangian>(std::move(constraintPtr), std::move(penaltyPtrArray)); } /******************************************************************************************************/ @@ -61,7 +60,7 @@ std::unique_ptr<StateInputAugmentedLagrangian> create(std::unique_ptr<StateInput /******************************************************************************************************/ std::unique_ptr<StateInputAugmentedLagrangian> create(std::unique_ptr<StateInputConstraint> constraintPtr, std::unique_ptr<augmented::AugmentedPenaltyBase> penaltyPtr) { - return std::unique_ptr<StateInputAugmentedLagrangian>(new StateInputAugmentedLagrangian(std::move(constraintPtr), std::move(penaltyPtr))); + return std::make_unique<StateInputAugmentedLagrangian>(std::move(constraintPtr), std::move(penaltyPtr)); } } // namespace ocs2 diff --git a/ocs2_core/src/integration/Integrator.cpp b/ocs2_core/src/integration/Integrator.cpp index 01efb48d3..e7f1ab696 100644 --- a/ocs2_core/src/integration/Integrator.cpp +++ b/ocs2_core/src/integration/Integrator.cpp @@ -80,24 +80,24 @@ IntegratorType fromString(const std::string& name) { std::unique_ptr<IntegratorBase> newIntegrator(IntegratorType integratorType, const std::shared_ptr<SystemEventHandler>& eventHandlerPtr) { switch (integratorType) { case (IntegratorType::EULER): - return std::unique_ptr<IntegratorBase>(new IntegratorEuler(eventHandlerPtr)); + return std::make_unique<IntegratorEuler>(eventHandlerPtr); case (IntegratorType::ODE45): - return std::unique_ptr<IntegratorBase>(new ODE45(eventHandlerPtr)); + return std::make_unique<ODE45>(eventHandlerPtr); case (IntegratorType::ODE45_OCS2): - return std::unique_ptr<IntegratorBase>(new RungeKuttaDormandPrince5(eventHandlerPtr)); + return std::make_unique<RungeKuttaDormandPrince5>(eventHandlerPtr); case (IntegratorType::ADAMS_BASHFORTH): - return std::unique_ptr<IntegratorBase>(new IntegratorAdamsBashforth<1>(eventHandlerPtr)); + return std::make_unique<IntegratorAdamsBashforth<1>>(eventHandlerPtr); case (IntegratorType::BULIRSCH_STOER): - return std::unique_ptr<IntegratorBase>(new IntegratorBulirschStoer(eventHandlerPtr)); + return std::make_unique<IntegratorBulirschStoer>(eventHandlerPtr); case (IntegratorType::MODIFIED_MIDPOINT): - return std::unique_ptr<IntegratorBase>(new IntegratorModifiedMidpoint(eventHandlerPtr)); + return std::make_unique<IntegratorModifiedMidpoint>(eventHandlerPtr); case (IntegratorType::RK4): - return std::unique_ptr<IntegratorBase>(new IntegratorRK4(eventHandlerPtr)); + return std::make_unique<IntegratorRK4>(eventHandlerPtr); case (IntegratorType::RK5_VARIABLE): - return std::unique_ptr<IntegratorBase>(new IntegratorRK5Variable(eventHandlerPtr)); + return std::make_unique<IntegratorRK5Variable>(eventHandlerPtr); #if (BOOST_VERSION / 100000 == 1 && BOOST_VERSION / 100 % 1000 > 55) case (IntegratorType::ADAMS_BASHFORTH_MOULTON): - return std::unique_ptr<IntegratorBase>(new IntegratorAdamsBashforthMoulton<1>(eventHandlerPtr)); + return std::make_unique<IntegratorAdamsBashforthMoulton<1>>(eventHandlerPtr); #endif default: throw std::runtime_error("Integrator of type " + integrator_type::toString(integratorType) + " not supported."); diff --git a/ocs2_core/src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangian.cpp b/ocs2_core/src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangian.cpp index 223f8d7c3..9ec9286f5 100644 --- a/ocs2_core/src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangian.cpp +++ b/ocs2_core/src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangian.cpp @@ -41,8 +41,7 @@ namespace LoopshapingAugmentedLagrangian { /******************************************************************************************************/ std::unique_ptr<StateAugmentedLagrangianCollection> create(const StateAugmentedLagrangianCollection& systemAugmentedLagrangian, std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { - return std::unique_ptr<StateAugmentedLagrangianCollection>( - new LoopshapingStateAugmentedLagrangian(systemAugmentedLagrangian, std::move(loopshapingDefinition))); + return std::make_unique<LoopshapingStateAugmentedLagrangian>(systemAugmentedLagrangian, std::move(loopshapingDefinition)); } /******************************************************************************************************/ @@ -52,11 +51,9 @@ std::unique_ptr<StateInputAugmentedLagrangianCollection> create(const StateInput std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr<StateInputAugmentedLagrangianCollection>( - new LoopshapingAugmentedLagrangianOutputPattern(systemAugmentedLagrangian, std::move(loopshapingDefinition))); + return std::make_unique<LoopshapingAugmentedLagrangianOutputPattern>(systemAugmentedLagrangian, std::move(loopshapingDefinition)); case LoopshapingType::eliminatepattern: - return std::unique_ptr<StateInputAugmentedLagrangianCollection>( - new LoopshapingAugmentedLagrangianEliminatePattern(systemAugmentedLagrangian, std::move(loopshapingDefinition))); + return std::make_unique<LoopshapingAugmentedLagrangianEliminatePattern>(systemAugmentedLagrangian, std::move(loopshapingDefinition)); default: throw std::runtime_error("[LoopshapingAugmentedLagrangian::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp index 09dc48011..85dab7578 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp @@ -41,7 +41,7 @@ namespace LoopshapingConstraint { /******************************************************************************************************/ std::unique_ptr<StateConstraintCollection> create(const StateConstraintCollection& systemConstraint, std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { - return std::unique_ptr<StateConstraintCollection>(new LoopshapingStateConstraint(systemConstraint, loopshapingDefinition)); + return std::make_unique<LoopshapingStateConstraint>(systemConstraint, loopshapingDefinition); } /******************************************************************************************************/ @@ -51,11 +51,9 @@ std::unique_ptr<StateInputConstraintCollection> create(const StateInputConstrain std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr<StateInputConstraintCollection>( - new LoopshapingConstraintOutputPattern(systemConstraint, std::move(loopshapingDefinition))); + return std::make_unique<LoopshapingConstraintOutputPattern>(systemConstraint, std::move(loopshapingDefinition)); case LoopshapingType::eliminatepattern: - return std::unique_ptr<StateInputConstraintCollection>( - new LoopshapingConstraintEliminatePattern(systemConstraint, std::move(loopshapingDefinition))); + return std::make_unique<LoopshapingConstraintEliminatePattern>(systemConstraint, std::move(loopshapingDefinition)); default: throw std::runtime_error("[LoopshapingConstraint::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp b/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp index 44a10dc89..857e9d706 100644 --- a/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp +++ b/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp @@ -41,7 +41,7 @@ namespace LoopshapingCost { /******************************************************************************************************/ std::unique_ptr<StateCostCollection> create(const StateCostCollection& systemCost, std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { - return std::unique_ptr<StateCostCollection>(new LoopshapingStateCost(systemCost, loopshapingDefinition)); + return std::make_unique<LoopshapingStateCost>(systemCost, loopshapingDefinition); } /******************************************************************************************************/ @@ -51,9 +51,9 @@ std::unique_ptr<StateInputCostCollection> create(const StateInputCostCollection& std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr<LoopshapingStateInputCost>(new LoopshapingCostOutputPattern(systemCost, std::move(loopshapingDefinition))); + return std::make_unique<LoopshapingCostOutputPattern>(systemCost, std::move(loopshapingDefinition)); case LoopshapingType::eliminatepattern: - return std::unique_ptr<LoopshapingStateInputCost>(new LoopshapingCostEliminatePattern(systemCost, std::move(loopshapingDefinition))); + return std::make_unique<LoopshapingCostEliminatePattern>(systemCost, std::move(loopshapingDefinition)); default: throw std::runtime_error("[LoopshapingStateInputCost::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/loopshaping/dynamics/LoopshapingDynamics.cpp b/ocs2_core/src/loopshaping/dynamics/LoopshapingDynamics.cpp index d57cc1fe4..3ebb53890 100644 --- a/ocs2_core/src/loopshaping/dynamics/LoopshapingDynamics.cpp +++ b/ocs2_core/src/loopshaping/dynamics/LoopshapingDynamics.cpp @@ -125,11 +125,9 @@ std::unique_ptr<LoopshapingDynamics> LoopshapingDynamics::create(const SystemDyn switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr<LoopshapingDynamics>( - new LoopshapingDynamicsOutputPattern(systemDynamics, std::move(loopshapingDefinition), preComputation)); + return std::make_unique<LoopshapingDynamicsOutputPattern>(systemDynamics, std::move(loopshapingDefinition), preComputation); case LoopshapingType::eliminatepattern: - return std::unique_ptr<LoopshapingDynamics>( - new LoopshapingDynamicsEliminatePattern(systemDynamics, std::move(loopshapingDefinition), preComputation)); + return std::make_unique<LoopshapingDynamicsEliminatePattern>(systemDynamics, std::move(loopshapingDefinition), preComputation); default: throw std::runtime_error("[LoopshapingDynamics::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp b/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp index 31d97b7ca..5d405d34f 100644 --- a/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp +++ b/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp @@ -42,7 +42,7 @@ namespace LoopshapingSoftConstraint { std::unique_ptr<StateCostCollection> create(const StateCostCollection& systemSoftConstraint, std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { // State-only soft constraint wrapper is identical to LoopshapingStateCost wrapper. - return std::unique_ptr<StateCostCollection>(new LoopshapingStateCost(systemSoftConstraint, loopshapingDefinition)); + return std::make_unique<LoopshapingStateCost>(systemSoftConstraint, loopshapingDefinition); } /******************************************************************************************************/ @@ -52,11 +52,9 @@ std::unique_ptr<StateInputCostCollection> create(const StateInputCostCollection& std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr<StateInputCostCollection>( - new LoopshapingSoftConstraintOutputPattern(systemSoftConstraint, std::move(loopshapingDefinition))); + return std::make_unique<LoopshapingSoftConstraintOutputPattern>(systemSoftConstraint, std::move(loopshapingDefinition)); case LoopshapingType::eliminatepattern: - return std::unique_ptr<StateInputCostCollection>( - new LoopshapingSoftConstraintEliminatePattern(systemSoftConstraint, std::move(loopshapingDefinition))); + return std::make_unique<LoopshapingSoftConstraintEliminatePattern>(systemSoftConstraint, std::move(loopshapingDefinition)); default: throw std::runtime_error("[LoopshapingStateInputSoftConstraint::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/penalties/MultidimensionalPenalty.cpp b/ocs2_core/src/penalties/MultidimensionalPenalty.cpp index b15175abc..1eaf51b3c 100644 --- a/ocs2_core/src/penalties/MultidimensionalPenalty.cpp +++ b/ocs2_core/src/penalties/MultidimensionalPenalty.cpp @@ -63,7 +63,7 @@ class PenaltyBaseWrapper final : public augmented::AugmentedPenaltyBase { }; std::unique_ptr<PenaltyBaseWrapper> createWrapper(std::unique_ptr<PenaltyBase> penaltyPtr) { - return std::unique_ptr<PenaltyBaseWrapper>(new PenaltyBaseWrapper(std::move(penaltyPtr))); + return std::make_unique<PenaltyBaseWrapper>(std::move(penaltyPtr)); } scalar_t getMultiplier(const vector_t* l, size_t ind) { diff --git a/ocs2_core/test/constraint/testConstraintCollection.cpp b/ocs2_core/test/constraint/testConstraintCollection.cpp index 418275b98..8beb89126 100644 --- a/ocs2_core/test/constraint/testConstraintCollection.cpp +++ b/ocs2_core/test/constraint/testConstraintCollection.cpp @@ -37,7 +37,7 @@ TEST(TestConstraintCollection, add) { ocs2::StateInputConstraintCollection constraintCollection; // Add after construction - std::unique_ptr<TestEmptyConstraint> constraintTerm(new TestEmptyConstraint()); + auto constraintTerm = std::make_unique<TestEmptyConstraint>(); ASSERT_NO_THROW({ constraintCollection.add("Constraint1", std::move(constraintTerm)); }); } @@ -48,7 +48,7 @@ TEST(TestConstraintCollection, numberOfConstraints) { EXPECT_EQ(constraintCollection.getNumConstraints(0.0), 0); // Add Linear inequality constraint term, which has 2 constraints - std::unique_ptr<TestDummyConstraint> constraintTerm(new TestDummyConstraint()); + auto constraintTerm = std::make_unique<TestDummyConstraint>(); const size_t addedConstraints = constraintTerm->getNumConstraints(0.0); constraintCollection.add("Constraint1", std::move(constraintTerm)); @@ -63,7 +63,7 @@ TEST(TestConstraintCollection, activatingConstraints) { EXPECT_EQ(constraintCollection.getNumConstraints(0.0), 0); // Add Linear inequality constraint term, which has 2 constraints - std::unique_ptr<TestDummyConstraint> constraintTerm(new TestDummyConstraint()); + auto constraintTerm = std::make_unique<TestDummyConstraint>(); const size_t addedConstraints = constraintTerm->getNumConstraints(0.0); constraintCollection.add("Constraint1", std::move(constraintTerm)); @@ -78,7 +78,7 @@ TEST(TestConstraintCollection, activatingConstraints) { TEST(TestConstraintCollection, clone) { ocs2::StateInputConstraintCollection constraintCollection; - std::unique_ptr<TestDummyConstraint> constraintTerm(new TestDummyConstraint()); + auto constraintTerm = std::make_unique<TestDummyConstraint>(); const size_t addedConstraints = constraintTerm->getNumConstraints(0.0); constraintCollection.add("Constraint1", std::move(constraintTerm)); @@ -103,8 +103,8 @@ TEST(TestConstraintCollection, getValue) { EXPECT_EQ(constraintValues.size(), 0); // Add Linear inequality constraint term, which has 2 constraints, twice - std::unique_ptr<TestDummyConstraint> constraintTerm1(new TestDummyConstraint()); - std::unique_ptr<TestDummyConstraint> constraintTerm2(new TestDummyConstraint()); + auto constraintTerm1 = std::make_unique<TestDummyConstraint>(); + auto constraintTerm2 = std::make_unique<TestDummyConstraint>(); constraintCollection.add("Constraint1", std::move(constraintTerm1)); constraintCollection.add("Constraint2", std::move(constraintTerm2)); @@ -128,8 +128,8 @@ TEST(TestConstraintCollection, getLinearApproximation) { x.setZero(); // Add Linear inequality constraint term, which has 2 constraints, twice - std::unique_ptr<TestDummyConstraint> constraintTerm1(new TestDummyConstraint()); - std::unique_ptr<TestDummyConstraint> constraintTerm2(new TestDummyConstraint()); + auto constraintTerm1 = std::make_unique<TestDummyConstraint>(); + auto constraintTerm2 = std::make_unique<TestDummyConstraint>(); constraintCollection.add("Constraint1", std::move(constraintTerm1)); constraintCollection.add("Constraint2", std::move(constraintTerm2)); @@ -161,8 +161,8 @@ TEST(TestConstraintCollection, getQuadraticApproximation) { x.setZero(); // Add Linear inequality constraint term, which has 2 constraints, twice - std::unique_ptr<TestDummyConstraint> constraintTerm1(new TestDummyConstraint()); - std::unique_ptr<TestDummyConstraint> constraintTerm2(new TestDummyConstraint()); + auto constraintTerm1 = std::make_unique<TestDummyConstraint>(); + auto constraintTerm2 = std::make_unique<TestDummyConstraint>(); constraintCollection.add("Constraint1", std::move(constraintTerm1)); constraintCollection.add("Constraint2", std::move(constraintTerm2)); diff --git a/ocs2_core/test/cost/testCostCollection.cpp b/ocs2_core/test/cost/testCostCollection.cpp index 67661d598..76cf3702e 100644 --- a/ocs2_core/test/cost/testCostCollection.cpp +++ b/ocs2_core/test/cost/testCostCollection.cpp @@ -82,8 +82,8 @@ class StateInputCost_TestFixture : public ::testing::Test { u = ocs2::vector_t::Random(INPUT_DIM); t = 0.0; - auto cost1 = std::unique_ptr<SimpleQuadraticCost>(new SimpleQuadraticCost(Q, R)); - auto cost2 = std::unique_ptr<SimpleQuadraticCost>(new SimpleQuadraticCost(0.5 * Q, 2.0 * R)); + auto cost1 = std::make_unique<SimpleQuadraticCost>(Q, R); + auto cost2 = std::make_unique<SimpleQuadraticCost>(0.5 * Q, 2.0 * R); expectedCost = cost1->getValue(t, x, u, targetTrajectories, {}) + cost2->getValue(t, x, u, targetTrajectories, {}); expectedCostApproximation = cost1->getQuadraticApproximation(t, x, u, targetTrajectories, {}); expectedCostApproximation += cost2->getQuadraticApproximation(t, x, u, targetTrajectories, {}); @@ -193,7 +193,7 @@ class StateCost_TestFixture : public ::testing::Test { x = ocs2::vector_t::Random(STATE_DIM); t = 0.0; - auto cost = std::unique_ptr<SimpleQuadraticFinalCost>(new SimpleQuadraticFinalCost(std::move(Q))); + auto cost = std::make_unique<SimpleQuadraticFinalCost>(std::move(Q)); expectedCost = cost->getValue(t, x, targetTrajectories, {}); expectedCostApproximation = cost->getQuadraticApproximation(t, x, targetTrajectories, {}); diff --git a/ocs2_core/test/integration/IntegrationTest.cpp b/ocs2_core/test/integration/IntegrationTest.cpp index 354c29fb4..8c845aed7 100644 --- a/ocs2_core/test/integration/IntegrationTest.cpp +++ b/ocs2_core/test/integration/IntegrationTest.cpp @@ -37,19 +37,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace ocs2; -std::unique_ptr<OdeBase> getSystem() { +std::unique_ptr<OdeBase> getSystem(LinearController& controller) { matrix_t A(2, 2); A << -2, -1, // clang-format off 1, 0; // clang-format on matrix_t B(2, 1); B << 1, 0; - auto sys = std::unique_ptr<ControlledSystemBase>(new LinearSystemDynamics(A, B)); - scalar_array_t cntTimeStamp{0, 10}; - vector_array_t uff(2, vector_t::Ones(1)); - matrix_array_t k(2, matrix_t::Zero(1, 2)); - static std::shared_ptr<LinearController> controller = std::make_shared<LinearController>(cntTimeStamp, uff, k); - sys->setController(controller.get()); + auto sys = std::make_unique<LinearSystemDynamics>(A, B); + sys->setController(&controller); return std::move(sys); } @@ -60,7 +56,11 @@ void testSecondOrderSystem(IntegratorType integrator_type) { const scalar_t dt = 0.05; const vector_t x0 = vector_t::Zero(2); - auto sys = getSystem(); + const scalar_array_t cntTimeStamp{0, 10}; + const vector_array_t uff(2, vector_t::Ones(1)); + const matrix_array_t k(2, matrix_t::Zero(1, 2)); + LinearController controller(cntTimeStamp, uff, k); + auto sys = getSystem(controller); std::unique_ptr<IntegratorBase> integrator = newIntegrator(integrator_type); diff --git a/ocs2_core/test/integration/testSensitivityIntegrator.cpp b/ocs2_core/test/integration/testSensitivityIntegrator.cpp index a1280bf08..162dd5093 100644 --- a/ocs2_core/test/integration/testSensitivityIntegrator.cpp +++ b/ocs2_core/test/integration/testSensitivityIntegrator.cpp @@ -40,10 +40,10 @@ namespace { std::unique_ptr<ocs2::LinearSystemDynamics> getSystem() { ocs2::matrix_t A(2, 2); A << -2, -1, // clang-format off - 1, 0; // clang-format on + 1, 0; // clang-format on ocs2::matrix_t B(2, 1); B << 1, 0; - return std::unique_ptr<ocs2::LinearSystemDynamics>(new ocs2::LinearSystemDynamics(A, B)); + return std::make_unique<ocs2::LinearSystemDynamics>(A, B); } } // namespace @@ -209,4 +209,4 @@ TEST(test_sensitivity_integrator, vsBoostRK4) { // Check ASSERT_TRUE(rk4ForwardDynamics.isApprox(boostRk4ForwardDynamics)); -} \ No newline at end of file +} diff --git a/ocs2_core/test/loopshaping/testLoopshapingFilterDynamics.cpp b/ocs2_core/test/loopshaping/testLoopshapingFilterDynamics.cpp index 0c2b58756..8ba8c2210 100644 --- a/ocs2_core/test/loopshaping/testLoopshapingFilterDynamics.cpp +++ b/ocs2_core/test/loopshaping/testLoopshapingFilterDynamics.cpp @@ -43,7 +43,7 @@ TEST(testLoopshapingFilterDynamics, Integration) { C << 0.0, 0.0, 0.0; D << 1.0; Filter filter(A, B, C, D); - auto loopshapingDefinition = std::shared_ptr<LoopshapingDefinition>(new LoopshapingDefinition(LoopshapingType::outputpattern, filter)); + auto loopshapingDefinition = std::make_shared<LoopshapingDefinition>(LoopshapingType::outputpattern, filter); using TestLoopshapingFilterDynamics = LoopshapingFilterDynamics; TestLoopshapingFilterDynamics loopshapingFilterDynamics(loopshapingDefinition); diff --git a/ocs2_core/test/loopshaping/testQuadraticConstraint.h b/ocs2_core/test/loopshaping/testQuadraticConstraint.h index 94f81249d..f60726a39 100644 --- a/ocs2_core/test/loopshaping/testQuadraticConstraint.h +++ b/ocs2_core/test/loopshaping/testQuadraticConstraint.h @@ -44,7 +44,7 @@ class TestQuadraticStateInputConstraint : public StateInputConstraint { P.setRandom(inputDim, stateDim); Q = (0.5 * Q.transpose() + 0.5 * Q).eval(); R = (0.5 * R.transpose() + 0.5 * R).eval(); - return std::unique_ptr<TestQuadraticStateInputConstraint>(new TestQuadraticStateInputConstraint(Q, R, P)); + return std::make_unique<TestQuadraticStateInputConstraint>(Q, R, P); } ~TestQuadraticStateInputConstraint() override = default; @@ -94,7 +94,7 @@ class TestQuadraticStateConstraint : public StateConstraint { matrix_t Q; Q.setRandom(stateDim, stateDim); Q = (0.5 * Q.transpose() + 0.5 * Q).eval(); - return std::unique_ptr<TestQuadraticStateConstraint>(new TestQuadraticStateConstraint(Q)); + return std::make_unique<TestQuadraticStateConstraint>(std::move(Q)); } ~TestQuadraticStateConstraint() override = default; @@ -128,4 +128,4 @@ class TestQuadraticStateConstraint : public StateConstraint { matrix_t Q_; }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/test/soft_constraint/testSoftConstraint.cpp b/ocs2_core/test/soft_constraint/testSoftConstraint.cpp index daef4dac8..699e67388 100644 --- a/ocs2_core/test/soft_constraint/testSoftConstraint.cpp +++ b/ocs2_core/test/soft_constraint/testSoftConstraint.cpp @@ -36,7 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -class TestStateConstraint : public ocs2::StateConstraint { +class TestStateConstraint final : public ocs2::StateConstraint { public: TestStateConstraint(ocs2::ConstraintOrder constraintOrder, size_t numConstraints) : StateConstraint(constraintOrder), numConstraints_(numConstraints) {} @@ -96,19 +96,19 @@ template <class Constraint, class SoftConstraint> std::unique_ptr<SoftConstraint> softConstraintFactory(size_t numConstraints, ocs2::ConstraintOrder constraintOrder, bool useSimilarPenalty) { // constraint - std::unique_ptr<Constraint> constraintPtr(new Constraint(constraintOrder, numConstraints)); + auto constraintPtr = std::make_unique<Constraint>(constraintOrder, numConstraints); if (useSimilarPenalty) { // penalty function - std::unique_ptr<ocs2::RelaxedBarrierPenalty> penaltyFunctionPtr(new ocs2::RelaxedBarrierPenalty({10.0, 1.0})); - return std::unique_ptr<SoftConstraint>(new SoftConstraint(std::move(constraintPtr), std::move(penaltyFunctionPtr))); + auto penaltyFunctionPtr = std::make_unique<ocs2::RelaxedBarrierPenalty>(ocs2::RelaxedBarrierPenalty::Config{10.0, 1.0}); + return std::make_unique<SoftConstraint>(std::move(constraintPtr), std::move(penaltyFunctionPtr)); } else { std::vector<std::unique_ptr<ocs2::PenaltyBase>> penaltyFunctionPtrArry; for (size_t i = 0; i < numConstraints; i++) { penaltyFunctionPtrArry.emplace_back(new ocs2::RelaxedBarrierPenalty({10.0, 1.0})); } // end of i loop - return std::unique_ptr<SoftConstraint>(new SoftConstraint(std::move(constraintPtr), std::move(penaltyFunctionPtrArry))); + return std::make_unique<SoftConstraint>(std::move(constraintPtr), std::move(penaltyFunctionPtrArry)); } } @@ -184,9 +184,9 @@ class ActivityTestStateConstraint : public ocs2::StateConstraint { TEST(testSoftConstraint, checkActivityStateConstraint) { constexpr size_t numConstraints = 10; - std::unique_ptr<ActivityTestStateConstraint> constraint(new ActivityTestStateConstraint); - std::unique_ptr<ocs2::RelaxedBarrierPenalty> penalty(new ocs2::RelaxedBarrierPenalty({10.0, 1.0})); - std::unique_ptr<ocs2::StateSoftConstraint> softConstraint(new ocs2::StateSoftConstraint(std::move(constraint), std::move(penalty))); + auto constraint = std::make_unique<ActivityTestStateConstraint>(); + auto penalty = std::make_unique<ocs2::RelaxedBarrierPenalty>(ocs2::RelaxedBarrierPenalty::Config{10.0, 1.0}); + auto softConstraint = std::make_unique<ocs2::StateSoftConstraint>(std::move(constraint), std::move(penalty)); softConstraint->get<ActivityTestStateConstraint>().setActivity(true); EXPECT_TRUE(std::unique_ptr<ocs2::StateCost>(softConstraint->clone())->isActive(0.0)); @@ -216,10 +216,9 @@ class ActivityTestStateInputConstraint : public ocs2::StateInputConstraint { TEST(testSoftConstraint, checkActivityStateInputConstraint) { constexpr size_t numConstraints = 10; - std::unique_ptr<ActivityTestStateInputConstraint> constraint(new ActivityTestStateInputConstraint); - std::unique_ptr<ocs2::RelaxedBarrierPenalty> penalty(new ocs2::RelaxedBarrierPenalty({10.0, 1.0})); - std::unique_ptr<ocs2::StateInputSoftConstraint> softConstraint( - new ocs2::StateInputSoftConstraint(std::move(constraint), std::move(penalty))); + auto constraint = std::make_unique<ActivityTestStateInputConstraint>(); + auto penalty = std::make_unique<ocs2::RelaxedBarrierPenalty>(ocs2::RelaxedBarrierPenalty::Config{10.0, 1.0}); + auto softConstraint = std::make_unique<ocs2::StateInputSoftConstraint>(std::move(constraint), std::move(penalty)); softConstraint->get<ActivityTestStateInputConstraint>().setActivity(true); EXPECT_TRUE(std::unique_ptr<ocs2::StateInputCost>(softConstraint->clone())->isActive(0.0)); diff --git a/ocs2_core/test/thread_support/testSynchronized.cpp b/ocs2_core/test/thread_support/testSynchronized.cpp index 13425b934..ec3164ce9 100644 --- a/ocs2_core/test/thread_support/testSynchronized.cpp +++ b/ocs2_core/test/thread_support/testSynchronized.cpp @@ -35,7 +35,7 @@ TEST(testLockable, construction) { ASSERT_FALSE(defaultObj.lock()); // from pointer - std::unique_ptr<double> doublePtr(new double(1.0)); + auto doublePtr = std::make_unique<double>(1.0); ocs2::Synchronized<double> nonDefaultObj(std::move(doublePtr)); ASSERT_TRUE(nonDefaultObj.lock()); } @@ -176,4 +176,4 @@ TEST(testLockable, lockMultiple) { synchronizedA.getMutex().unlock(); synchronizedB.getMutex().unlock(); synchronizedDouble.getMutex().unlock(); -} \ No newline at end of file +} From b297d8b3d4055de46f3b333f139b6b9afe4e40a4 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 16:07:29 +0200 Subject: [PATCH 304/512] ddp, mpc, oc --- ocs2_ddp/test/HybridSlqTest.cpp | 10 +++++----- ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp | 6 +++--- ocs2_ddp/test/testContinuousTimeLqr.cpp | 2 +- ocs2_mpc/src/MPC_MRT_Interface.cpp | 6 +++--- ocs2_oc/test/include/ocs2_oc/test/EXP0.h | 4 ++-- ocs2_oc/test/include/ocs2_oc/test/EXP1.h | 4 ++-- .../test/include/ocs2_oc/test/circular_kinematics.h | 6 ++---- .../include/ocs2_oc/test/testProblemsGeneration.h | 12 ++++++------ ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp | 8 +++----- 9 files changed, 27 insertions(+), 31 deletions(-) diff --git a/ocs2_ddp/test/HybridSlqTest.cpp b/ocs2_ddp/test/HybridSlqTest.cpp index 3e3defe93..2fc603bd9 100644 --- a/ocs2_ddp/test/HybridSlqTest.cpp +++ b/ocs2_ddp/test/HybridSlqTest.cpp @@ -113,12 +113,12 @@ TEST(HybridSlqTest, state_rollout_slq) { // cost function const matrix_t Q = (matrix_t(STATE_DIM, STATE_DIM) << 50, 0, 0, 0, 50, 0, 0, 0, 0).finished(); const matrix_t R = (matrix_t(INPUT_DIM, INPUT_DIM) << 1).finished(); - std::unique_ptr<ocs2::StateInputCost> cost(new QuadraticStateInputCost(Q, R)); - std::unique_ptr<ocs2::StateCost> preJumpCost(new QuadraticStateCost(Q)); - std::unique_ptr<ocs2::StateCost> finalCost(new QuadraticStateCost(Q)); + auto cost = std::make_unique<QuadraticStateInputCost>(Q, R); + auto preJumpCost = std::make_unique<QuadraticStateCost>(Q); + auto finalCost = std::make_unique<QuadraticStateCost>(Q); // constraints - std::unique_ptr<StateInputConstraint> boundsConstraints(new HybridSysBounds); + auto boundsConstraints = std::make_unique<HybridSysBounds>(); OptimalControlProblem problem; problem.dynamicsPtr.reset(systemDynamics.clone()); @@ -138,7 +138,7 @@ TEST(HybridSlqTest, state_rollout_slq) { OperatingPoints operatingTrajectories(stateOperatingPoint, inputOperatingPoint); // Test 1: Check constraint compliance. It uses a solver observer to get metrics for the bounds constraints - std::unique_ptr<AugmentedLagrangianObserver> boundsConstraintsObserverPtr(new AugmentedLagrangianObserver("bounds")); + auto boundsConstraintsObserverPtr = std::make_unique<AugmentedLagrangianObserver>("bounds"); boundsConstraintsObserverPtr->setMetricsCallback([&](const scalar_array_t& timeTraj, const std::vector<LagrangianMetricsConstRef>& metricsTraj) { constexpr scalar_t constraintViolationTolerance = 1e-1; for (size_t i = 0; i < metricsTraj.size(); i++) { diff --git a/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp b/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp index 59b4bef89..4385caf80 100644 --- a/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp +++ b/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp @@ -125,12 +125,12 @@ TEST(BouncingMassTest, DISABLED_state_rollout_slq) { Q << 50.0, 0.0, 0.0, 0.0, 50.0, 0.0, 0.0, 0.0, 0.0; matrix_t R(INPUT_DIM, INPUT_DIM); R << 1.0; - std::unique_ptr<ocs2::StateInputCost> cost(new BouncingMassCost(reference, Q, R)); + auto cost = std::make_unique<BouncingMassCost>(reference, Q, R); matrix_t Qf(STATE_DIM, STATE_DIM); Qf << 56.63, 7.07, 0.0, 7.07, 8.01, 0.0, 0.0, 0.0, 0.0; - std::unique_ptr<ocs2::StateCost> finalCost(new BouncingMassFinalCost(reference, Qf, finalTime)); - std::unique_ptr<ocs2::StateCost> preJumpCost(new BouncingMassFinalCost(reference, Qf, finalTime)); + auto finalCost = std::make_unique<BouncingMassFinalCost>(reference, Qf, finalTime); + auto preJumpCost = std::make_unique<BouncingMassFinalCost>(reference, Qf, finalTime); ocs2::OptimalControlProblem problem; problem.dynamicsPtr.reset(systemDynamics.clone()); diff --git a/ocs2_ddp/test/testContinuousTimeLqr.cpp b/ocs2_ddp/test/testContinuousTimeLqr.cpp index 9eba70956..f88b05d3b 100644 --- a/ocs2_ddp/test/testContinuousTimeLqr.cpp +++ b/ocs2_ddp/test/testContinuousTimeLqr.cpp @@ -47,7 +47,7 @@ TEST(testContinousTimeLqr, compareWithMatlab) { const matrix_t Q = (matrix_t(2, 2) << 3.0, 2.0, 2.0, 4.0).finished(); const matrix_t R = (matrix_t(1, 1) << 5.0).finished(); const matrix_t P = (matrix_t(1, 2) << 0.1, 0.2).finished(); - std::unique_ptr<StateInputCost> cost(new QuadraticStateInputCost(Q, R, P)); + auto cost = std::make_unique<QuadraticStateInputCost>(Q, R, P); const scalar_t time = 0.0; const vector_t state = vector_t::Zero(2); diff --git a/ocs2_mpc/src/MPC_MRT_Interface.cpp b/ocs2_mpc/src/MPC_MRT_Interface.cpp index b15d90cbc..80b303015 100644 --- a/ocs2_mpc/src/MPC_MRT_Interface.cpp +++ b/ocs2_mpc/src/MPC_MRT_Interface.cpp @@ -117,19 +117,19 @@ void MPC_MRT_Interface::advanceMpc() { /******************************************************************************************************/ void MPC_MRT_Interface::copyToBuffer(const SystemObservation& mpcInitObservation) { // policy - std::unique_ptr<PrimalSolution> primalSolutionPtr(new PrimalSolution); + auto primalSolutionPtr = std::make_unique<PrimalSolution>(); const scalar_t startTime = mpcInitObservation.time; const scalar_t finalTime = (mpc_.settings().solutionTimeWindow_ < 0) ? mpc_.getSolverPtr()->getFinalTime() : startTime + mpc_.settings().solutionTimeWindow_; mpc_.getSolverPtr()->getPrimalSolution(finalTime, primalSolutionPtr.get()); // command - std::unique_ptr<CommandData> commandPtr(new CommandData); + auto commandPtr = std::make_unique<CommandData>(); commandPtr->mpcInitObservation_ = mpcInitObservation; commandPtr->mpcTargetTrajectories_ = mpc_.getSolverPtr()->getReferenceManager().getTargetTrajectories(); // performance indices - std::unique_ptr<PerformanceIndex> performanceIndicesPtr(new PerformanceIndex); + auto performanceIndicesPtr = std::make_unique<PerformanceIndex>(); *performanceIndicesPtr = mpc_.getSolverPtr()->getPerformanceIndeces(); this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr)); diff --git a/ocs2_oc/test/include/ocs2_oc/test/EXP0.h b/ocs2_oc/test/include/ocs2_oc/test/EXP0.h index 1e2cfb6a6..cdf3d1473 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/EXP0.h +++ b/ocs2_oc/test/include/ocs2_oc/test/EXP0.h @@ -165,8 +165,8 @@ inline OptimalControlProblem createExp0Problem(const std::shared_ptr<ocs2::Refer problem.dynamicsPtr.reset(new EXP0_System(referenceManagerPtr)); // cost function - problem.costPtr->add("cost", std::unique_ptr<ocs2::StateInputCost>(new ocs2::EXP0_Cost())); - problem.finalCostPtr->add("finalCost", std::unique_ptr<ocs2::StateCost>(new ocs2::EXP0_FinalCost())); + problem.costPtr->add("cost", std::make_unique<ocs2::EXP0_Cost>()); + problem.finalCostPtr->add("finalCost", std::make_unique<ocs2::EXP0_FinalCost>()); return problem; } diff --git a/ocs2_oc/test/include/ocs2_oc/test/EXP1.h b/ocs2_oc/test/include/ocs2_oc/test/EXP1.h index 86a8e837d..63904fa09 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/EXP1.h +++ b/ocs2_oc/test/include/ocs2_oc/test/EXP1.h @@ -222,8 +222,8 @@ inline OptimalControlProblem createExp1Problem(const std::shared_ptr<ocs2::Refer problem.dynamicsPtr.reset(new ocs2::EXP1_System(referenceManagerPtr)); // cost function - problem.costPtr->add("cost", std::unique_ptr<ocs2::StateInputCost>(new ocs2::EXP1_Cost())); - problem.finalCostPtr->add("finalCost", std::unique_ptr<ocs2::StateCost>(new ocs2::EXP1_FinalCost())); + problem.costPtr->add("cost", std::make_unique<ocs2::EXP1_Cost>()); + problem.finalCostPtr->add("finalCost", std::make_unique<ocs2::EXP1_FinalCost>()); return problem; } diff --git a/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h b/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h index e5f7e159b..01fd36f5e 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h +++ b/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h @@ -126,12 +126,10 @@ inline OptimalControlProblem createCircularKinematicsProblem(const std::string& problem.dynamicsPtr.reset(new CircularKinematicsSystem()); // cost function - std::unique_ptr<StateInputCost> cost(new CircularKinematicsCost(libraryFolder)); - problem.costPtr->add("cost", std::move(cost)); + problem.costPtr->add("cost", std::make_unique<CircularKinematicsCost>(libraryFolder)); // constraint - std::unique_ptr<StateInputConstraint> constraint(new CircularKinematicsConstraints()); - problem.equalityConstraintPtr->add("constraint", std::move(constraint)); + problem.equalityConstraintPtr->add("constraint", std::make_unique<CircularKinematicsConstraints>()); return problem; } diff --git a/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h b/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h index a4922459a..ccd8dc890 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h +++ b/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h @@ -58,11 +58,11 @@ inline ScalarFunctionQuadraticApproximation getRandomCost(int n, int m) { } inline std::unique_ptr<ocs2::StateInputCost> getOcs2Cost(const ScalarFunctionQuadraticApproximation& cost) { - return std::unique_ptr<ocs2::StateInputCost>(new ocs2::QuadraticStateInputCost(cost.dfdxx, cost.dfduu, cost.dfdux)); + return std::make_unique<ocs2::QuadraticStateInputCost>(cost.dfdxx, cost.dfduu, cost.dfdux); } inline std::unique_ptr<ocs2::StateCost> getOcs2StateCost(const ScalarFunctionQuadraticApproximation& costFinal) { - return std::unique_ptr<ocs2::StateCost>(new ocs2::QuadraticStateCost(costFinal.dfdxx)); + return std::make_unique<ocs2::QuadraticStateCost>(costFinal.dfdxx); } /** Get random linear dynamics of n states and m inputs */ @@ -77,7 +77,7 @@ inline VectorFunctionLinearApproximation getRandomDynamics(int n, int m) { } inline std::unique_ptr<ocs2::LinearSystemDynamics> getOcs2Dynamics(const VectorFunctionLinearApproximation& dynamics) { - return std::unique_ptr<ocs2::LinearSystemDynamics>(new ocs2::LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu)); + return std::make_unique<ocs2::LinearSystemDynamics>(dynamics.dfdx, dynamics.dfdu); } /** Get random nc linear constraints of n states, and m inputs */ @@ -92,12 +92,12 @@ inline VectorFunctionLinearApproximation getRandomConstraints(int n, int m, int } inline std::unique_ptr<ocs2::StateInputConstraint> getOcs2Constraints(const VectorFunctionLinearApproximation& stateInputConstraints) { - return std::unique_ptr<ocs2::StateInputConstraint>( - new ocs2::LinearStateInputConstraint(stateInputConstraints.f, stateInputConstraints.dfdx, stateInputConstraints.dfdu)); + return std::make_unique<ocs2::LinearStateInputConstraint>(stateInputConstraints.f, stateInputConstraints.dfdx, + stateInputConstraints.dfdu); } inline std::unique_ptr<ocs2::StateConstraint> getOcs2StateOnlyConstraints(const VectorFunctionLinearApproximation& stateOnlyConstraints) { - return std::unique_ptr<ocs2::StateConstraint>(new ocs2::LinearStateConstraint(stateOnlyConstraints.f, stateOnlyConstraints.dfdx)); + return std::make_unique<ocs2::LinearStateConstraint>(stateOnlyConstraints.f, stateOnlyConstraints.dfdx); } } // namespace ocs2 diff --git a/ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp b/ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp index 1d5dbbbee..344caf273 100644 --- a/ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp +++ b/ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp @@ -72,15 +72,13 @@ TEST(time_rollout_test, time_rollout_test) { }(); // rollout class - std::unique_ptr<RolloutBase> rolloutBasePtr(new TimeTriggeredRollout(systemDynamics, rolloutSettings)); + auto rolloutPtr = std::make_unique<TimeTriggeredRollout>(systemDynamics, rolloutSettings); scalar_array_t timeTrajectory; - size_array_t eventsPastTheEndIndeces; + size_array_t postEventIndices; vector_array_t stateTrajectory; vector_array_t inputTrajectory; - - rolloutBasePtr->run(initTime, initState, finalTime, &controller, modeSchedule, timeTrajectory, eventsPastTheEndIndeces, stateTrajectory, - inputTrajectory); + rolloutPtr->run(initTime, initState, finalTime, &controller, modeSchedule, timeTrajectory, postEventIndices, stateTrajectory, inputTrajectory); // check sizes const auto totalSize = timeTrajectory.size(); From 73b7235193bf24128c035b7ababf9b29b151f681 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 16:15:34 +0200 Subject: [PATCH 305/512] sqp --- ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp | 5 ++--- ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp | 2 +- ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp | 4 ++-- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp b/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp index 5ad47d6cd..6200ac992 100644 --- a/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp @@ -116,13 +116,12 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithEventTime(scal // Reference Manager const ocs2::ModeSchedule modeSchedule({eventTime}, {0, 1}); const ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Random(n)}, {ocs2::vector_t::Random(m)}); - std::shared_ptr<ocs2::ReferenceManager> referenceManagerPtr(new ocs2::ReferenceManager(targetTrajectories, modeSchedule)); + auto referenceManagerPtr = std::make_shared<ocs2::ReferenceManager>(targetTrajectories, modeSchedule); problem.targetTrajectoriesPtr = &targetTrajectories; // Constraint - problem.equalityConstraintPtr->add("switchedConstraint", - std::unique_ptr<StateInputConstraint>(new ocs2::SwitchedConstraint(referenceManagerPtr))); + problem.equalityConstraintPtr->add("switchedConstraint", std::make_unique<SwitchedConstraint>(referenceManagerPtr)); ocs2::DefaultInitializer zeroInitializer(m); diff --git a/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp b/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp index c68ffe0d9..5ad89b2c5 100644 --- a/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp @@ -56,7 +56,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithFeedbackSettin // Reference Managaer ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Ones(n)}, {ocs2::vector_t::Ones(m)}); - std::shared_ptr<ReferenceManager> referenceManagerPtr(new ReferenceManager(targetTrajectories)); + auto referenceManagerPtr = std::make_shared<ReferenceManager>(targetTrajectories); problem.targetTrajectoriesPtr = &referenceManagerPtr->getTargetTrajectories(); diff --git a/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp b/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp index d9e6fe2a5..fe40d3aee 100644 --- a/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp @@ -62,7 +62,7 @@ TEST(test_valuefunction, linear_quadratic_problem) { // Reference Manager const ocs2::ModeSchedule modeSchedule({eventTime}, {0, 1}); const ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Random(n)}, {ocs2::vector_t::Random(m)}); - std::shared_ptr<ocs2::ReferenceManager> referenceManagerPtr(new ocs2::ReferenceManager(targetTrajectories, modeSchedule)); + auto referenceManagerPtr = std::make_shared<ocs2::ReferenceManager>(targetTrajectories, modeSchedule); problem.targetTrajectoriesPtr = &targetTrajectories; @@ -103,4 +103,4 @@ TEST(test_valuefunction, linear_quadratic_problem) { EXPECT_NEAR(sampleCost, zeroCost + costToGo.dfdx.dot(dx) + 0.5 * dx.dot(costToGo.dfdxx * dx), tol); } -} \ No newline at end of file +} From e76e399adb1dadb4f52f0968929a891b6a63caee Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 16:26:59 +0200 Subject: [PATCH 306/512] ros, python --- ocs2_python_interface/test/testDummyPyBindings.cpp | 6 +++--- .../synchronized_module/RosReferenceManager.h | 4 ++-- ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ocs2_python_interface/test/testDummyPyBindings.cpp b/ocs2_python_interface/test/testDummyPyBindings.cpp index c858e00b1..e39954964 100644 --- a/ocs2_python_interface/test/testDummyPyBindings.cpp +++ b/ocs2_python_interface/test/testDummyPyBindings.cpp @@ -25,8 +25,8 @@ class DummyInterface final : public RobotInterface { const matrix_t Q = (matrix_t(2, 2) << 1, 0, 0, 1).finished(); const matrix_t R = (matrix_t(1, 1) << 1).finished(); const matrix_t Qf = (matrix_t(2, 2) << 2, 0, 0, 2).finished(); - problem_.costPtr->add("cost", std::unique_ptr<StateInputCost>(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr<StateCost>(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique<QuadraticStateInputCost>(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique<QuadraticStateCost>(Qf)); problem_.targetTrajectoriesPtr = &targetTrajectories_; @@ -41,7 +41,7 @@ class DummyInterface final : public RobotInterface { mpc::Settings mpcSettings; ddp::Settings ddpSettings; ddpSettings.algorithm_ = ddp::Algorithm::SLQ; - return std::unique_ptr<GaussNewtonDDP_MPC>(new GaussNewtonDDP_MPC(mpcSettings, ddpSettings, *rolloutPtr_, problem_, *initializerPtr_)); + return std::make_unique<GaussNewtonDDP_MPC>(mpcSettings, ddpSettings, *rolloutPtr_, problem_, *initializerPtr_); } const OptimalControlProblem& getOptimalControlProblem() const override { return problem_; } diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h index 9d46ea51f..cd3611392 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h @@ -85,8 +85,8 @@ class RosReferenceManager final : public ReferenceManagerDecorator { /******************************************************************************************************/ template <class ReferenceManagerType, class... Args> std::unique_ptr<RosReferenceManager> RosReferenceManager::create(const std::string& topicPrefix, Args&&... args) { - std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr(new ReferenceManagerType(std::forward<Args>(args)...)); - return std::unique_ptr<RosReferenceManager>(new RosReferenceManager(topicPrefix, std::move(referenceManagerPtr))); + auto referenceManagerPtr = std::make_shared<ReferenceManagerType>(std::forward<Args>(args)...); + return std::make_unique<RosReferenceManager>(topicPrefix, std::move(referenceManagerPtr)); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp index 60397deff..9d013071d 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp @@ -190,9 +190,9 @@ void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& /******************************************************************************************************/ void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& msg) { // read new policy and command from msg - std::unique_ptr<CommandData> commandPtr(new CommandData); - std::unique_ptr<PrimalSolution> primalSolutionPtr(new PrimalSolution); - std::unique_ptr<PerformanceIndex> performanceIndicesPtr(new PerformanceIndex); + auto commandPtr = std::make_unique<CommandData>(); + auto primalSolutionPtr = std::make_unique<PrimalSolution>(); + auto performanceIndicesPtr = std::make_unique<PerformanceIndex>(); readPolicyMsg(*msg, *commandPtr, *primalSolutionPtr, *performanceIndicesPtr); this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr)); From dbaf9aa158ff77369919ea01d5b261702b6b6a3d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 16:29:35 +0200 Subject: [PATCH 307/512] correcting ros_interface dependencies --- ocs2_ros_interfaces/CMakeLists.txt | 1 - ocs2_ros_interfaces/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/ocs2_ros_interfaces/CMakeLists.txt b/ocs2_ros_interfaces/CMakeLists.txt index 30895fc29..5b2f55be5 100644 --- a/ocs2_ros_interfaces/CMakeLists.txt +++ b/ocs2_ros_interfaces/CMakeLists.txt @@ -5,7 +5,6 @@ set(CATKIN_PACKAGE_DEPENDENCIES roscpp ocs2_msgs ocs2_core - ocs2_ddp ocs2_mpc std_msgs visualization_msgs diff --git a/ocs2_ros_interfaces/package.xml b/ocs2_ros_interfaces/package.xml index a18ee22b9..3398852f0 100644 --- a/ocs2_ros_interfaces/package.xml +++ b/ocs2_ros_interfaces/package.xml @@ -19,7 +19,6 @@ <depend>roscpp</depend> <depend>ocs2_msgs</depend> <depend>ocs2_core</depend> - <depend>ocs2_ddp</depend> <depend>ocs2_mpc</depend> <depend>std_msgs</depend> <depend>visualization_msgs</depend> From b3e104e908249daf4835d0cab9159cbad9a435f6 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 16:34:50 +0200 Subject: [PATCH 308/512] pinocchio --- .../implementation/PinocchioInterface.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/include/ocs2_pinocchio_interface/implementation/PinocchioInterface.h b/ocs2_pinocchio/ocs2_pinocchio_interface/include/ocs2_pinocchio_interface/implementation/PinocchioInterface.h index ef5d35668..3553b51f8 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/include/ocs2_pinocchio_interface/implementation/PinocchioInterface.h +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/include/ocs2_pinocchio_interface/implementation/PinocchioInterface.h @@ -41,7 +41,7 @@ namespace ocs2 { template <typename SCALAR> PinocchioInterfaceTpl<SCALAR>::PinocchioInterfaceTpl(const Model& model, const std::shared_ptr<const ::urdf::ModelInterface> urdfModelPtr) { robotModelPtr_ = std::make_shared<const Model>(model); - robotDataPtr_ = std::unique_ptr<Data>(new Data(*robotModelPtr_)); + robotDataPtr_ = std::make_unique<Data>(*robotModelPtr_); if (urdfModelPtr) { // This makes a copy of the urdfModelPtr, which guarantees constness of the urdf urdfModelPtr_ = std::make_shared<const ::urdf::ModelInterface>(*urdfModelPtr); From 962f16512615a6524788cf0f85b50b8b1ae44016 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 16:40:47 +0200 Subject: [PATCH 309/512] raisim --- .../src/LeggedRobotRaisimDummyNode.cpp | 6 +++--- .../ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp | 3 +-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ocs2_raisim/ocs2_legged_robot_raisim/src/LeggedRobotRaisimDummyNode.cpp b/ocs2_raisim/ocs2_legged_robot_raisim/src/LeggedRobotRaisimDummyNode.cpp index eef002bdd..cd96fab31 100644 --- a/ocs2_raisim/ocs2_legged_robot_raisim/src/LeggedRobotRaisimDummyNode.cpp +++ b/ocs2_raisim/ocs2_legged_robot_raisim/src/LeggedRobotRaisimDummyNode.cpp @@ -101,8 +101,8 @@ int main(int argc, char** argv) { CentroidalModelPinocchioMapping pinocchioMapping(interface.getCentroidalModelInfo()); PinocchioEndEffectorKinematics endEffectorKinematics(interface.getPinocchioInterface(), pinocchioMapping, interface.modelSettings().contactNames3DoF); - std::shared_ptr<LeggedRobotRaisimVisualizer> leggedRobotRaisimVisualizer(new LeggedRobotRaisimVisualizer( - interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + auto leggedRobotRaisimVisualizer = std::make_shared<LeggedRobotRaisimVisualizer>( + interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle); leggedRobotRaisimVisualizer->updateTerrain(); // legged robot dummy @@ -123,4 +123,4 @@ int main(int argc, char** argv) { leggedRobotDummy.run(initObservation, initTargetTrajectories); return 0; -} \ No newline at end of file +} diff --git a/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp b/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp index 35de3dcaf..04bbee0b8 100644 --- a/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp +++ b/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp @@ -83,8 +83,7 @@ std::unique_ptr<raisim::HeightMap> RaisimHeightmapRosConverter::convertGridmapTo std::vector<double> height(gridMap->data[0].data.rbegin(), gridMap->data[0].data.rend()); - std::unique_ptr<raisim::HeightMap> heightMap(new raisim::HeightMap(xSamples, ySamples, xSize, ySize, centerX, centerY, height)); - return heightMap; + return std::make_unique<raisim::HeightMap>(xSamples, ySamples, xSize, ySize, centerX, centerY, height); } void RaisimHeightmapRosConverter::publishGridmap(const raisim::HeightMap& heightMap, const std::string& frameId) { From 0bba91de83ce3e6312bea5f2aaea755c48624bdc Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 17:01:29 +0200 Subject: [PATCH 310/512] ballbot --- .../include/ocs2_ballbot/BallbotPyBindings.h | 6 +++--- .../ocs2_ballbot/src/BallbotInterface.cpp | 4 ++-- .../ocs2_ballbot/src/BallbotSimpleExample.cpp | 10 ++++------ .../ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp | 3 +-- .../ocs2_ballbot_ros/src/BallbotMpcNode.cpp | 3 +-- .../ocs2_ballbot_ros/src/DummyBallbotNode.cpp | 3 +-- .../src/MultipleShootingBallbotNode.cpp | 3 +-- 7 files changed, 13 insertions(+), 19 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotPyBindings.h b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotPyBindings.h index 9e733e466..099dea163 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotPyBindings.h +++ b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotPyBindings.h @@ -59,9 +59,9 @@ class BallbotPyBindings final : public PythonInterface { BallbotInterface ballbotInterface(taskFile, libraryFolder); // MPC - std::unique_ptr<GaussNewtonDDP_MPC> mpcPtr( - new GaussNewtonDDP_MPC(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer())); + auto mpcPtr = + std::make_unique<GaussNewtonDDP_MPC>(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); // Python interface diff --git a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp index 59dfeb88d..79aa2f96d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp @@ -88,8 +88,8 @@ BallbotInterface::BallbotInterface(const std::string& taskFile, const std::strin std::cerr << "R: \n" << R << "\n"; std::cerr << "Q_final:\n" << Qf << "\n"; - problem_.costPtr->add("cost", std::unique_ptr<StateInputCost>(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr<StateCost>(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique<QuadraticStateInputCost>(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique<QuadraticStateCost>(Qf)); // Dynamics bool recompileLibraries; // load the flag to generate library files from taskFile diff --git a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotSimpleExample.cpp b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotSimpleExample.cpp index c78e0180b..78421eb4f 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotSimpleExample.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotSimpleExample.cpp @@ -79,7 +79,7 @@ int main(int argc, char** argv) { * Rollout */ problem.dynamicsPtr.reset(new BallbotSystemDynamics(libraryFolder, true)); - std::unique_ptr<TimeTriggeredRollout> ballbotRolloutPtr(new TimeTriggeredRollout(*problem.dynamicsPtr, rolloutSettings)); + auto ballbotRolloutPtr = std::make_unique<TimeTriggeredRollout>(*problem.dynamicsPtr, rolloutSettings); /* * Cost function @@ -93,15 +93,13 @@ int main(int argc, char** argv) { vector_t xInit(STATE_DIM); loadData::loadEigenMatrix(taskFile, "initialState", xInit); - std::unique_ptr<QuadraticStateInputCost> L(new QuadraticStateInputCost(Q, R)); - std::unique_ptr<QuadraticStateCost> Phi(new QuadraticStateCost(QFinal)); - problem.costPtr->add("cost", std::move(L)); - problem.finalCostPtr->add("finalCost", std::move(Phi)); + problem.costPtr->add("cost", std::make_unique<QuadraticStateInputCost>(Q, R)); + problem.finalCostPtr->add("finalCost", std::make_unique<QuadraticStateCost>(QFinal)); /* * Initialization */ - std::unique_ptr<Initializer> ballbotInitializerPtr(new DefaultInitializer(INPUT_DIM)); + auto ballbotInitializerPtr = std::make_unique<DefaultInitializer>(INPUT_DIM); /* * Time partitioning which defines the time horizon and the number of data partitioning diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp index 4d558f2ff..0c9b5aec4 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp @@ -91,8 +91,7 @@ int main(int argc, char** argv) { ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); // ROS ReferenceManager. This gives us the command interface. Requires the observations to be published - std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, ballbotInterface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared<ocs2::RosReferenceManager>(robotName, ballbotInterface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); auto observationPublisher = nodeHandle.advertise<ocs2_msgs::mpc_observation>(robotName + "_mpc_observation", 1); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcNode.cpp index c0fc7cd63..627cf6d49 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcNode.cpp @@ -57,8 +57,7 @@ int main(int argc, char** argv) { ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager - std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, ballbotInterface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared<ocs2::RosReferenceManager>(robotName, ballbotInterface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp index 9d567ef9a..d969c468b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp @@ -65,8 +65,7 @@ int main(int argc, char** argv) { mrt.launchNodes(nodeHandle); // Visualization - std::shared_ptr<ocs2::ballbot::BallbotDummyVisualization> ballbotDummyVisualization( - new ocs2::ballbot::BallbotDummyVisualization(nodeHandle)); + auto ballbotDummyVisualization = std::make_shared<ocs2::ballbot::BallbotDummyVisualization>(nodeHandle); // Dummy ballbot ocs2::MRT_ROS_Dummy_Loop dummyBallbot(mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp index 090f3707d..2ce3abefa 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp @@ -57,8 +57,7 @@ int main(int argc, char** argv) { ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager - std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, ballbotInterface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared<ocs2::RosReferenceManager>(robotName, ballbotInterface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC From 40c908a0f9164a69cb218a89e1dcb8300bd4a776 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 17:04:36 +0200 Subject: [PATCH 311/512] cartpole --- .../ocs2_cartpole/src/CartPoleInterface.cpp | 6 +++--- .../ocs2_cartpole/test/testCartpole.cpp | 16 ++++++++-------- .../ocs2_cartpole_ros/src/CartpoleMpcNode.cpp | 3 +-- 3 files changed, 12 insertions(+), 13 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_cartpole/src/CartPoleInterface.cpp b/ocs2_robotic_examples/ocs2_cartpole/src/CartPoleInterface.cpp index 8e13f7faa..db0527118 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/src/CartPoleInterface.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole/src/CartPoleInterface.cpp @@ -92,8 +92,8 @@ CartPoleInterface::CartPoleInterface(const std::string& taskFile, const std::str std::cerr << "Q_final:\n" << Qf << "\n"; } - problem_.costPtr->add("cost", std::unique_ptr<StateInputCost>(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr<StateCost>(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique<QuadraticStateInputCost>(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique<QuadraticStateCost>(Qf)); // Dynamics CartPoleParameters cartPoleParameters; @@ -117,7 +117,7 @@ CartPoleInterface::CartPoleInterface(const std::string& taskFile, const std::str const vector_t e = (vector_t(numIneqConstraint) << cartPoleParameters.maxInput_, cartPoleParameters.maxInput_).finished(); const vector_t D = (vector_t(numIneqConstraint) << 1.0, -1.0).finished(); const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM); - return std::unique_ptr<StateInputConstraint>(new LinearStateInputConstraint(e, C, D)); + return std::make_unique<LinearStateInputConstraint>(e, C, D); }; problem_.inequalityLagrangianPtr->add("InputLimits", create(getConstraint(), getPenalty())); diff --git a/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp b/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp index 98ecb4aaa..ef27b41d9 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp @@ -74,7 +74,7 @@ class TestCartpole : public testing::TestWithParam<std::tuple<ddp::Algorithm, Pe matrix_t Qf(STATE_DIM, STATE_DIM); loadData::loadEigenMatrix(taskFile, "Q_final", Qf); Qf *= (timeHorizon / cartPoleInterfacePtr->mpcSettings().timeHorizon_); // scale cost - return std::unique_ptr<StateCost>(new QuadraticStateCost(Qf)); + return std::make_unique<QuadraticStateCost>(Qf); }; cartPoleInterfacePtr->optimalControlProblem().finalCostPtr->add(finalCostName, createFinalCost()); @@ -98,14 +98,14 @@ class TestCartpole : public testing::TestWithParam<std::tuple<ddp::Algorithm, Pe switch (algorithm) { case ddp::Algorithm::SLQ: - return std::unique_ptr<GaussNewtonDDP>(new SLQ(ddpSettings, cartPoleInterfacePtr->getRollout(), - createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), - cartPoleInterfacePtr->getInitializer())); + return std::make_unique<SLQ>(ddpSettings, cartPoleInterfacePtr->getRollout(), + createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), + cartPoleInterfacePtr->getInitializer()); case ddp::Algorithm::ILQR: - return std::unique_ptr<GaussNewtonDDP>(new ILQR(ddpSettings, cartPoleInterfacePtr->getRollout(), - createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), - cartPoleInterfacePtr->getInitializer())); + return std::make_unique<ILQR>(ddpSettings, cartPoleInterfacePtr->getRollout(), + createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), + cartPoleInterfacePtr->getInitializer()); default: throw std::runtime_error("[TestCartpole::getAlgorithm] undefined algorithm"); @@ -199,7 +199,7 @@ TEST_P(TestCartpole, testDDP) { ddpPtr->getReferenceManager().setTargetTrajectories(initTargetTrajectories); // observer for InputLimits violation - std::unique_ptr<AugmentedLagrangianObserver> inputLimitsObserverModulePtr(new AugmentedLagrangianObserver("InputLimits")); + auto inputLimitsObserverModulePtr = std::make_unique<AugmentedLagrangianObserver>("InputLimits"); inputLimitsObserverModulePtr->setMetricsCallback( [&](const scalar_array_t& timeTrajectory, const std::vector<LagrangianMetricsConstRef>& termMetrics) { testInputLimitsViolation(timeTrajectory, termMetrics); diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp index 8cb9e1fd1..cbd28ce46 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp @@ -71,8 +71,7 @@ int main(int argc, char** argv) { metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); } - std::unique_ptr<ocs2::AugmentedLagrangianObserver> stateInputBoundsObserverPtr( - new ocs2::AugmentedLagrangianObserver(observingLagrangianTerm)); + auto stateInputBoundsObserverPtr = std::make_unique<ocs2::AugmentedLagrangianObserver>(observingLagrangianTerm); stateInputBoundsObserverPtr->setMetricsCallback(ocs2::ros::createMetricsCallback( nodeHandle, observingTimePoints, metricsTopicNames, ocs2::ros::CallbackInterpolationStrategy::linear_interpolation)); stateInputBoundsObserverPtr->setMultiplierCallback(ocs2::ros::createMultiplierCallback( From 014ef82ff999681f0051ddbcea457eb28f9a802f Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 17:10:36 +0200 Subject: [PATCH 312/512] double_integrator --- .../ocs2_double_integrator/DoubleIntegratorPyBindings.h | 4 ++-- .../ocs2_double_integrator/src/DoubleIntegratorInterface.cpp | 4 ++-- .../test/DoubleIntegratorNoRosIntegrationTest.cpp | 4 ++-- .../src/DoubleIntegratorMpcNode.cpp | 3 +-- 4 files changed, 7 insertions(+), 8 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_double_integrator/include/ocs2_double_integrator/DoubleIntegratorPyBindings.h b/ocs2_robotic_examples/ocs2_double_integrator/include/ocs2_double_integrator/DoubleIntegratorPyBindings.h index f9f82726e..40ff2575e 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/include/ocs2_double_integrator/DoubleIntegratorPyBindings.h +++ b/ocs2_robotic_examples/ocs2_double_integrator/include/ocs2_double_integrator/DoubleIntegratorPyBindings.h @@ -30,9 +30,9 @@ class DoubleIntegratorPyBindings final : public PythonInterface { DoubleIntegratorInterface doubleIntegratorInterface(taskFile, libraryFolder); // MPC - std::unique_ptr<GaussNewtonDDP_MPC> mpcPtr(new GaussNewtonDDP_MPC( + auto mpcPtr = std::make_unique<GaussNewtonDDP_MPC>( doubleIntegratorInterface.mpcSettings(), doubleIntegratorInterface.ddpSettings(), doubleIntegratorInterface.getRollout(), - doubleIntegratorInterface.getOptimalControlProblem(), doubleIntegratorInterface.getInitializer())); + doubleIntegratorInterface.getOptimalControlProblem(), doubleIntegratorInterface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(doubleIntegratorInterface.getReferenceManagerPtr()); // Python interface diff --git a/ocs2_robotic_examples/ocs2_double_integrator/src/DoubleIntegratorInterface.cpp b/ocs2_robotic_examples/ocs2_double_integrator/src/DoubleIntegratorInterface.cpp index 25de87143..cf59812f5 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/src/DoubleIntegratorInterface.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator/src/DoubleIntegratorInterface.cpp @@ -88,8 +88,8 @@ DoubleIntegratorInterface::DoubleIntegratorInterface(const std::string& taskFile std::cerr << "R: \n" << R << "\n"; std::cerr << "Q_final:\n" << Qf << "\n"; - problem_.costPtr->add("cost", std::unique_ptr<StateInputCost>(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr<StateCost>(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique<QuadraticStateInputCost>(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique<QuadraticStateCost>(Qf)); // Dynamics const matrix_t A = (matrix_t(STATE_DIM, STATE_DIM) << 0.0, 1.0, 0.0, 0.0).finished(); diff --git a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp index aa0d941f9..c5d577fc8 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp @@ -66,8 +66,8 @@ class DoubleIntegratorIntegrationTest : public testing::Test { ddpSettings.maxNumIterations_ = 5; } - std::unique_ptr<GaussNewtonDDP_MPC> mpcPtr(new GaussNewtonDDP_MPC(mpcSettings, ddpSettings, interface.getRollout(), - interface.getOptimalControlProblem(), interface.getInitializer())); + auto mpcPtr = std::make_unique<GaussNewtonDDP_MPC>(mpcSettings, ddpSettings, interface.getRollout(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(interface.getReferenceManagerPtr()); return mpcPtr; diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp index 0963bf8a8..1e5d5c1f9 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp @@ -59,8 +59,7 @@ int main(int argc, char** argv) { interface_t doubleIntegratorInterface(taskFile, libFolder); // ROS ReferenceManager - std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, doubleIntegratorInterface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared<ocs2::RosReferenceManager>(robotName, doubleIntegratorInterface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC From ba61d6f313e89ad8c6e95bc7c23e7ce785820367 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 17:26:20 +0200 Subject: [PATCH 313/512] LeggedRobot --- .../src/LeggedRobotInterface.cpp | 22 +++++++++---------- .../test/AnymalFactoryFunctions.cpp | 12 +++++----- .../testEndEffectorLinearConstraint.cpp | 8 +++---- .../src/LeggedRobotDummyNode.cpp | 4 ++-- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 8f2ccd3ff..29fb5581f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -120,8 +120,8 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile modelSettings_.contactNames6DoF); // Swing trajectory planner - std::unique_ptr<SwingTrajectoryPlanner> swingTrajectoryPlanner( - new SwingTrajectoryPlanner(loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4)); + auto swingTrajectoryPlanner = + std::make_unique<SwingTrajectoryPlanner>(loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4); // Mode schedule manager referenceManagerPtr_ = @@ -279,7 +279,7 @@ std::unique_ptr<StateInputCost> LeggedRobotInterface::getBaseTrackingCost(const std::cerr << " #### =============================================================================\n"; } - return std::unique_ptr<StateInputCost>(new LeggedRobotStateInputQuadraticCost(std::move(Q), std::move(R), info, *referenceManagerPtr_)); + return std::make_unique<LeggedRobotStateInputQuadraticCost>(std::move(Q), std::move(R), info, *referenceManagerPtr_); } /******************************************************************************************************/ @@ -313,19 +313,19 @@ std::pair<scalar_t, RelaxedBarrierPenalty::Config> LeggedRobotInterface::loadFri std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); - std::unique_ptr<FrictionConeConstraint> frictionConeConstraintPtr( - new FrictionConeConstraint(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_)); + auto frictionConeConstraintPtr = std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, std::move(frictionConeConConfig), + contactPointIndex, centroidalModelInfo_); - std::unique_ptr<PenaltyBase> penalty(new RelaxedBarrierPenalty(barrierPenaltyConfig)); + auto penalty = std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig); - return std::unique_ptr<StateInputCost>(new StateInputSoftConstraint(std::move(frictionConeConstraintPtr), std::move(penalty))); + return std::make_unique<StateInputSoftConstraint>(std::move(frictionConeConstraintPtr), std::move(penalty)); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getZeroForceConstraint(size_t contactPointIndex) { - return std::unique_ptr<StateInputConstraint>(new ZeroForceConstraint(*referenceManagerPtr_, contactPointIndex, centroidalModelInfo_)); + return std::make_unique<ZeroForceConstraint>(*referenceManagerPtr_, contactPointIndex, centroidalModelInfo_); } /******************************************************************************************************/ @@ -349,8 +349,8 @@ std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getZeroVelocityConst throw std::runtime_error( "[LeggedRobotInterface::getZeroVelocityConstraint] The analytical end-effector zero velocity constraint is not implemented!"); } else { - return std::unique_ptr<StateInputConstraint>(new ZeroVelocityConstraintCppAd(*referenceManagerPtr_, eeKinematics, contactPointIndex, - eeZeroVelConConfig(modelSettings_.positionErrorGain))); + return std::make_unique<ZeroVelocityConstraintCppAd>(*referenceManagerPtr_, eeKinematics, contactPointIndex, + eeZeroVelConConfig(modelSettings_.positionErrorGain)); } } @@ -364,7 +364,7 @@ std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getNormalVelocityCon throw std::runtime_error( "[LeggedRobotInterface::getNormalVelocityConstraint] The analytical end-effector normal velocity constraint is not implemented!"); } else { - return std::unique_ptr<StateInputConstraint>(new NormalVelocityConstraintCppAd(*referenceManagerPtr_, eeKinematics, contactPointIndex)); + return std::make_unique<NormalVelocityConstraintCppAd>(*referenceManagerPtr_, eeKinematics, contactPointIndex); } } diff --git a/ocs2_robotic_examples/ocs2_legged_robot/test/AnymalFactoryFunctions.cpp b/ocs2_robotic_examples/ocs2_legged_robot/test/AnymalFactoryFunctions.cpp index b792e4027..b38aa7aa2 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/test/AnymalFactoryFunctions.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/test/AnymalFactoryFunctions.cpp @@ -50,7 +50,7 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr<PinocchioInterface> createAnymalPinocchioInterface() { - return std::unique_ptr<PinocchioInterface>(new PinocchioInterface(centroidal_model::createPinocchioInterface(URDF_FILE))); + return std::make_unique<PinocchioInterface>(centroidal_model::createPinocchioInterface(URDF_FILE)); } /******************************************************************************************************/ @@ -71,11 +71,11 @@ std::shared_ptr<SwitchedModelReferenceManager> createReferenceManager(size_t num const auto defaultModeSequenceTemplate = loadModeSequenceTemplate(REFERENCE_FILE, "defaultModeSequenceTemplate", false); const ModelSettings modelSettings; - std::shared_ptr<GaitSchedule> gaitSchedule( - new GaitSchedule(initModeSchedule, defaultModeSequenceTemplate, modelSettings.phaseTransitionStanceTime)); - std::unique_ptr<SwingTrajectoryPlanner> swingTrajectoryPlanner( - new SwingTrajectoryPlanner(loadSwingTrajectorySettings(TASK_FILE, "swing_trajectory_config", false), numFeet)); - return std::make_shared<SwitchedModelReferenceManager>(gaitSchedule, std::move(swingTrajectoryPlanner)); + auto gaitSchedule = + std::make_shared<GaitSchedule>(initModeSchedule, defaultModeSequenceTemplate, modelSettings.phaseTransitionStanceTime); + auto swingTrajectoryPlanner = + std::make_unique<SwingTrajectoryPlanner>(loadSwingTrajectorySettings(TASK_FILE, "swing_trajectory_config", false), numFeet); + return std::make_shared<SwitchedModelReferenceManager>(std::move(gaitSchedule), std::move(swingTrajectoryPlanner)); } } // namespace legged_robot diff --git a/ocs2_robotic_examples/ocs2_legged_robot/test/constraint/testEndEffectorLinearConstraint.cpp b/ocs2_robotic_examples/ocs2_legged_robot/test/constraint/testEndEffectorLinearConstraint.cpp index 7b03222f3..f79647ae4 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/test/constraint/testEndEffectorLinearConstraint.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/test/constraint/testEndEffectorLinearConstraint.cpp @@ -116,9 +116,9 @@ class testEndEffectorLinearConstraint : public ::testing::Test { }; TEST_F(testEndEffectorLinearConstraint, testValue) { - auto eeVelConstraintPtr = std::unique_ptr<EndEffectorLinearConstraint>(new EndEffectorLinearConstraint(*eeKinematicsPtr, 3)); + auto eeVelConstraintPtr = std::make_unique<EndEffectorLinearConstraint>(*eeKinematicsPtr, 3); eeVelConstraintPtr->configure(config); - auto eeVelConstraintAdPtr = std::unique_ptr<EndEffectorLinearConstraint>(new EndEffectorLinearConstraint(*eeKinematicsAdPtr, 3)); + auto eeVelConstraintAdPtr = std::make_unique<EndEffectorLinearConstraint>(*eeKinematicsAdPtr, 3); eeVelConstraintAdPtr->configure(config); dynamic_cast<PinocchioEndEffectorKinematics&>(eeVelConstraintPtr->getEndEffectorKinematics()) @@ -143,9 +143,9 @@ TEST_F(testEndEffectorLinearConstraint, testValue) { } TEST_F(testEndEffectorLinearConstraint, testLinearApproximation) { - auto eeVelConstraintPtr = std::unique_ptr<EndEffectorLinearConstraint>(new EndEffectorLinearConstraint(*eeKinematicsPtr, 3)); + auto eeVelConstraintPtr = std::make_unique<EndEffectorLinearConstraint>(*eeKinematicsPtr, 3); eeVelConstraintPtr->configure(config); - auto eeVelConstraintAdPtr = std::unique_ptr<EndEffectorLinearConstraint>(new EndEffectorLinearConstraint(*eeKinematicsAdPtr, 3)); + auto eeVelConstraintAdPtr = std::make_unique<EndEffectorLinearConstraint>(*eeKinematicsAdPtr, 3); eeVelConstraintAdPtr->configure(config); dynamic_cast<PinocchioEndEffectorKinematics&>(eeVelConstraintPtr->getEndEffectorKinematics()) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp index 4fc71d2de..85a47e4f3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp @@ -65,8 +65,8 @@ int main(int argc, char** argv) { CentroidalModelPinocchioMapping pinocchioMapping(interface.getCentroidalModelInfo()); PinocchioEndEffectorKinematics endEffectorKinematics(interface.getPinocchioInterface(), pinocchioMapping, interface.modelSettings().contactNames3DoF); - std::shared_ptr<LeggedRobotVisualizer> leggedRobotVisualizer( - new LeggedRobotVisualizer(interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + auto leggedRobotVisualizer = std::make_shared<LeggedRobotVisualizer>( + interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle); // Dummy legged robot MRT_ROS_Dummy_Loop leggedRobotDummySimulator(mrt, interface.mpcSettings().mrtDesiredFrequency_, From 4bdf749ef33654e7e34cf605d31e684524979731 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 17:39:09 +0200 Subject: [PATCH 314/512] MobileManipulator --- .../src/MobileManipulatorInterface.cpp | 22 +++++++++---------- .../test/testDummyMobileManipulator.cpp | 4 ++-- .../src/MobileManipulatorDummyMRT.cpp | 3 +-- .../src/MobileManipulatorMpcNode.cpp | 3 +-- 4 files changed, 15 insertions(+), 17 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp index f48a344b7..1fdee8f26 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp @@ -245,7 +245,7 @@ std::unique_ptr<StateInputCost> MobileManipulatorInterface::getQuadraticInputCos std::cerr << "inputCost.R: \n" << R << '\n'; std::cerr << " #### =============================================================================\n"; - return std::unique_ptr<StateInputCost>(new QuadraticInputCost(std::move(R), manipulatorModelInfo_.stateDim)); + return std::make_unique<QuadraticInputCost>(std::move(R), manipulatorModelInfo_.stateDim); } /******************************************************************************************************/ @@ -285,10 +285,10 @@ std::unique_ptr<StateCost> MobileManipulatorInterface::getEndEffectorConstraint( } std::vector<std::unique_ptr<PenaltyBase>> penaltyArray(6); - std::generate_n(penaltyArray.begin(), 3, [&] { return std::unique_ptr<PenaltyBase>(new QuadraticPenalty(muPosition)); }); - std::generate_n(penaltyArray.begin() + 3, 3, [&] { return std::unique_ptr<PenaltyBase>(new QuadraticPenalty(muOrientation)); }); + std::generate_n(penaltyArray.begin(), 3, [&] { return std::make_unique<QuadraticPenalty>(muPosition); }); + std::generate_n(penaltyArray.begin() + 3, 3, [&] { return std::make_unique<QuadraticPenalty>(muOrientation); }); - return std::unique_ptr<StateCost>(new StateSoftConstraint(std::move(constraint), std::move(penaltyArray))); + return std::make_unique<StateSoftConstraint>(std::move(constraint), std::move(penaltyArray)); } /******************************************************************************************************/ @@ -323,17 +323,17 @@ std::unique_ptr<StateCost> MobileManipulatorInterface::getSelfCollisionConstrain std::unique_ptr<StateConstraint> constraint; if (usePreComputation) { - constraint = std::unique_ptr<StateConstraint>(new MobileManipulatorSelfCollisionConstraint( - MobileManipulatorPinocchioMapping(manipulatorModelInfo_), std::move(geometryInterface), minimumDistance)); + constraint = std::make_unique<MobileManipulatorSelfCollisionConstraint>(MobileManipulatorPinocchioMapping(manipulatorModelInfo_), + std::move(geometryInterface), minimumDistance); } else { - constraint = std::unique_ptr<StateConstraint>(new SelfCollisionConstraintCppAd( + constraint = std::make_unique<SelfCollisionConstraintCppAd>( pinocchioInterface, MobileManipulatorPinocchioMapping(manipulatorModelInfo_), std::move(geometryInterface), minimumDistance, - "self_collision", libraryFolder, recompileLibraries, false)); + "self_collision", libraryFolder, recompileLibraries, false); } - std::unique_ptr<PenaltyBase> penalty(new RelaxedBarrierPenalty({mu, delta})); + auto penalty = std::make_unique<RelaxedBarrierPenalty>(RelaxedBarrierPenalty::Config{mu, delta}); - return std::unique_ptr<StateCost>(new StateSoftConstraint(std::move(constraint), std::move(penalty))); + return std::make_unique<StateSoftConstraint>(std::move(constraint), std::move(penalty)); } /******************************************************************************************************/ @@ -431,7 +431,7 @@ std::unique_ptr<StateInputCost> MobileManipulatorInterface::getJointLimitSoftCon } } - auto boxConstraints = std::unique_ptr<StateInputSoftBoxConstraint>(new StateInputSoftBoxConstraint(stateLimits, inputLimits)); + auto boxConstraints = std::make_unique<StateInputSoftBoxConstraint>(stateLimits, inputLimits); boxConstraints->initializeOffset(0.0, vector_t::Zero(manipulatorModelInfo_.stateDim), vector_t::Zero(manipulatorModelInfo_.inputDim)); return boxConstraints; } diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp index f2be4e0c8..b957e2def 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp @@ -113,10 +113,10 @@ class DummyMobileManipulatorParametersTests std::unique_ptr<GaussNewtonDDP_MPC> getMpc() { auto& interface = *mobileManipulatorInterfacePtr; - std::unique_ptr<GaussNewtonDDP_MPC> mpcPtr(new GaussNewtonDDP_MPC( + auto mpcPtr = std::make_unique<GaussNewtonDDP_MPC>( interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), interface.getOptimalControlProblem(), - interface.getInitializer())); + interface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager( mobileManipulatorInterfacePtr->getReferenceManagerPtr()); return mpcPtr; diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp index d354c643f..b472ae59b 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp @@ -64,8 +64,7 @@ int main(int argc, char** argv) { mrt.launchNodes(nodeHandle); // Visualization - std::shared_ptr<mobile_manipulator::MobileManipulatorDummyVisualization> dummyVisualization( - new mobile_manipulator::MobileManipulatorDummyVisualization(nodeHandle, interface)); + auto dummyVisualization = std::make_shared<mobile_manipulator::MobileManipulatorDummyVisualization>(nodeHandle, interface); // Dummy MRT MRT_ROS_Dummy_Loop dummy(mrt, interface.mpcSettings().mrtDesiredFrequency_, interface.mpcSettings().mpcDesiredFrequency_); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp index 4eaae89eb..64ebb951f 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp @@ -57,8 +57,7 @@ int main(int argc, char** argv) { MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); // ROS ReferenceManager - std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, interface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared<ocs2::RosReferenceManager>(robotName, interface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC From 902b5161a8e1c0d3181920dff35b911ef7e4a316 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 17 Aug 2022 17:49:40 +0200 Subject: [PATCH 315/512] Quadrotor --- .../include/ocs2_quadrotor/QuadrotorPyBindings.h | 6 +++--- .../ocs2_quadrotor/src/QuadrotorInterface.cpp | 4 ++-- .../ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp | 3 +-- .../ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp | 3 +-- 4 files changed, 7 insertions(+), 9 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_quadrotor/include/ocs2_quadrotor/QuadrotorPyBindings.h b/ocs2_robotic_examples/ocs2_quadrotor/include/ocs2_quadrotor/QuadrotorPyBindings.h index 5a3d617cf..964aeba96 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/include/ocs2_quadrotor/QuadrotorPyBindings.h +++ b/ocs2_robotic_examples/ocs2_quadrotor/include/ocs2_quadrotor/QuadrotorPyBindings.h @@ -59,9 +59,9 @@ class QuadrotorPyBindings final : public PythonInterface { QuadrotorInterface quadrotorInterface(taskFile, libraryFolder); // MPC - std::unique_ptr<GaussNewtonDDP_MPC> mpcPtr( - new GaussNewtonDDP_MPC(quadrotorInterface.mpcSettings(), quadrotorInterface.ddpSettings(), quadrotorInterface.getRollout(), - quadrotorInterface.getOptimalControlProblem(), quadrotorInterface.getInitializer())); + auto mpcPtr = std::make_unique<GaussNewtonDDP_MPC>(quadrotorInterface.mpcSettings(), quadrotorInterface.ddpSettings(), + quadrotorInterface.getRollout(), quadrotorInterface.getOptimalControlProblem(), + quadrotorInterface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(quadrotorInterface.getReferenceManagerPtr()); // Python interface diff --git a/ocs2_robotic_examples/ocs2_quadrotor/src/QuadrotorInterface.cpp b/ocs2_robotic_examples/ocs2_quadrotor/src/QuadrotorInterface.cpp index 17f02ccf5..a5b9f9f3a 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/src/QuadrotorInterface.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor/src/QuadrotorInterface.cpp @@ -88,8 +88,8 @@ QuadrotorInterface::QuadrotorInterface(const std::string& taskFile, const std::s std::cerr << "R: \n" << R << "\n"; std::cerr << "Q_final:\n" << Qf << "\n"; - problem_.costPtr->add("cost", std::unique_ptr<StateInputCost>(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr<StateCost>(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique<QuadraticStateInputCost>(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique<QuadraticStateCost>(Qf)); // Dynamics auto quadrotorParameters = quadrotor::loadSettings(taskFile, "QuadrotorParameters", true); diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp index bf8fcb160..bc8adf82c 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp @@ -64,8 +64,7 @@ int main(int argc, char** argv) { mrt.launchNodes(nodeHandle); // Visualization - std::shared_ptr<ocs2::quadrotor::QuadrotorDummyVisualization> quadrotorDummyVisualization( - new ocs2::quadrotor::QuadrotorDummyVisualization()); + auto quadrotorDummyVisualization = std::make_shared<ocs2::quadrotor::QuadrotorDummyVisualization>(); // Dummy loop ocs2::MRT_ROS_Dummy_Loop dummyQuadrotor(mrt, quadrotorInterface.mpcSettings().mrtDesiredFrequency_, diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp index a897bcbdb..485588d97 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp @@ -56,8 +56,7 @@ int main(int argc, char** argv) { ocs2::quadrotor::QuadrotorInterface quadrotorInterface(taskFile, libFolder); // ROS ReferenceManager - std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, quadrotorInterface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared<ocs2::RosReferenceManager>(robotName, quadrotorInterface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC From 098af78c8fdeee95682015c409ba654ee4a5c072 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 25 Aug 2022 12:04:33 +0200 Subject: [PATCH 316/512] updating ddp unittests --- ocs2_ddp/CMakeLists.txt | 2 +- ocs2_ddp/test/CircularKinematicsTest.cpp | 3 +- ocs2_ddp/test/CorrectnessTest.cpp | 3 +- ocs2_ddp/test/Exp0Test.cpp | 3 +- ocs2_ddp/test/Exp1Test.cpp | 3 +- ocs2_ddp/test/HybridSlqTest.cpp | 24 +++---- .../test/bouncingmass/OverallReference.cpp | 66 ++++++++----------- .../test/bouncingmass/OverallReference.h | 33 +--------- .../ocs2_ddp/test/bouncingmass/Reference.h | 9 +-- .../ocs2_ddp/test/bouncingmass/SystemModel.h | 53 +++++++-------- ocs2_ddp/test/testDdpHelperFunction.cpp | 2 +- .../config/mpc/task.info | 3 - .../ocs2_sqp/test/testCircularKinematics.cpp | 2 +- 13 files changed, 72 insertions(+), 134 deletions(-) diff --git a/ocs2_ddp/CMakeLists.txt b/ocs2_ddp/CMakeLists.txt index 5b133f45d..8ed125165 100644 --- a/ocs2_ddp/CMakeLists.txt +++ b/ocs2_ddp/CMakeLists.txt @@ -82,7 +82,7 @@ if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) add_clang_tooling( TARGETS ${PROJECT_NAME}_lintTarget ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR ) diff --git a/ocs2_ddp/test/CircularKinematicsTest.cpp b/ocs2_ddp/test/CircularKinematicsTest.cpp index aca231704..f51ebf070 100644 --- a/ocs2_ddp/test/CircularKinematicsTest.cpp +++ b/ocs2_ddp/test/CircularKinematicsTest.cpp @@ -54,8 +54,7 @@ class CircularKinematicsTest : public testing::TestWithParam<std::tuple<ocs2::se CircularKinematicsTest() { // optimal control problem boost::filesystem::path filePath(__FILE__); - const std::string libraryFolder = filePath.parent_path().generic_string() + "/ddp_test_generated"; - problem = ocs2::createCircularKinematicsProblem(libraryFolder); + problem = ocs2::createCircularKinematicsProblem("/tmp/ocs2/ddp_test_generated"); // initializer initializerPtr.reset(new ocs2::DefaultInitializer(INPUT_DIM)); diff --git a/ocs2_ddp/test/CorrectnessTest.cpp b/ocs2_ddp/test/CorrectnessTest.cpp index 81adcf112..3906ff921 100644 --- a/ocs2_ddp/test/CorrectnessTest.cpp +++ b/ocs2_ddp/test/CorrectnessTest.cpp @@ -215,8 +215,7 @@ class DDPCorrectness : public testing::TestWithParam<std::tuple<ocs2::search_str void correctnessTest(const ocs2::ddp::Settings& ddpSettings, const ocs2::PerformanceIndex& performanceIndex, const ocs2::PrimalSolution& ddpSolution) const { const auto testName = getTestName(ddpSettings); - EXPECT_NEAR(performanceIndex.cost, qpCost, 10.0 * minRelCost) - << "MESSAGE: " << testName << ": failed in the optimal cost test!"; + EXPECT_NEAR(performanceIndex.cost, qpCost, 10.0 * minRelCost) << "MESSAGE: " << testName << ": failed in the optimal cost test!"; EXPECT_LT(relError(ddpSolution.stateTrajectory_.back(), qpSolution.stateTrajectory.back()), solutionPrecision) << "MESSAGE: " << testName << ": failed in the optimal final state test!"; EXPECT_LT(relError(ddpSolution.inputTrajectory_.front(), qpSolution.inputTrajectory.front()), solutionPrecision) diff --git a/ocs2_ddp/test/Exp0Test.cpp b/ocs2_ddp/test/Exp0Test.cpp index 8280ac37b..92c3c22f7 100644 --- a/ocs2_ddp/test/Exp0Test.cpp +++ b/ocs2_ddp/test/Exp0Test.cpp @@ -106,8 +106,7 @@ class Exp0 : public testing::Test { void performanceIndexTest(const ocs2::ddp::Settings& ddpSettings, const ocs2::PerformanceIndex& performanceIndex) const { const auto testName = getTestName(ddpSettings); - EXPECT_NEAR(performanceIndex.cost, expectedCost, 10.0 * minRelCost) - << "MESSAGE: " << testName << ": failed in the total cost test!"; + EXPECT_NEAR(performanceIndex.cost, expectedCost, 10.0 * minRelCost) << "MESSAGE: " << testName << ": failed in the total cost test!"; EXPECT_NEAR(performanceIndex.equalityConstraintsSSE, 0.0, 10.0 * ddpSettings.constraintTolerance_) << "MESSAGE: " << testName << ": failed in state-input equality constraint ISE test!"; } diff --git a/ocs2_ddp/test/Exp1Test.cpp b/ocs2_ddp/test/Exp1Test.cpp index 5d411f2cd..4258379b8 100644 --- a/ocs2_ddp/test/Exp1Test.cpp +++ b/ocs2_ddp/test/Exp1Test.cpp @@ -109,8 +109,7 @@ class Exp1 : public testing::TestWithParam<std::tuple<ocs2::search_strategy::Typ void performanceIndexTest(const ocs2::ddp::Settings& ddpSettings, const ocs2::PerformanceIndex& performanceIndex) const { const auto testName = getTestName(ddpSettings); - EXPECT_NEAR(performanceIndex.cost, expectedCost, 10.0 * minRelCost) - << "MESSAGE: " << testName << ": failed in the total cost test!"; + EXPECT_NEAR(performanceIndex.cost, expectedCost, 10.0 * minRelCost) << "MESSAGE: " << testName << ": failed in the total cost test!"; EXPECT_NEAR(performanceIndex.equalityConstraintsSSE, 0.0, 10.0 * ddpSettings.constraintTolerance_) << "MESSAGE: " << testName << ": failed in state-input equality constraint ISE test!"; } diff --git a/ocs2_ddp/test/HybridSlqTest.cpp b/ocs2_ddp/test/HybridSlqTest.cpp index 3e3defe93..921e8e13c 100644 --- a/ocs2_ddp/test/HybridSlqTest.cpp +++ b/ocs2_ddp/test/HybridSlqTest.cpp @@ -125,7 +125,8 @@ TEST(HybridSlqTest, state_rollout_slq) { problem.costPtr->add("cost", std::move(cost)); problem.preJumpCostPtr->add("preJumpCost", std::move(preJumpCost)); problem.finalCostPtr->add("finalCost", std::move(finalCost)); - problem.inequalityLagrangianPtr->add("bounds", create(std::move(boundsConstraints), augmented::SlacknessSquaredHingePenalty::create({200.0, 0.1}))); + problem.inequalityLagrangianPtr->add("bounds", + create(std::move(boundsConstraints), augmented::SlacknessSquaredHingePenalty::create({200.0, 0.1}))); const vector_t xNominal = vector_t::Zero(STATE_DIM); const vector_t uNominal = vector_t::Zero(INPUT_DIM); @@ -139,16 +140,17 @@ TEST(HybridSlqTest, state_rollout_slq) { // Test 1: Check constraint compliance. It uses a solver observer to get metrics for the bounds constraints std::unique_ptr<AugmentedLagrangianObserver> boundsConstraintsObserverPtr(new AugmentedLagrangianObserver("bounds")); - boundsConstraintsObserverPtr->setMetricsCallback([&](const scalar_array_t& timeTraj, const std::vector<LagrangianMetricsConstRef>& metricsTraj) { - constexpr scalar_t constraintViolationTolerance = 1e-1; - for (size_t i = 0; i < metricsTraj.size(); i++) { - const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); - EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(1), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(2), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(3), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - } - }); + boundsConstraintsObserverPtr->setMetricsCallback( + [&](const scalar_array_t& timeTraj, const std::vector<LagrangianMetricsConstRef>& metricsTraj) { + constexpr scalar_t constraintViolationTolerance = 1e-1; + for (size_t i = 0; i < metricsTraj.size(); i++) { + const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); + EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(1), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(2), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(3), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + } + }); // setup SLQ SLQ slq(ddpSettings, stateTriggeredRollout, problem, operatingTrajectories); diff --git a/ocs2_ddp/test/bouncingmass/OverallReference.cpp b/ocs2_ddp/test/bouncingmass/OverallReference.cpp index c6ccd631a..19a70a51e 100644 --- a/ocs2_ddp/test/bouncingmass/OverallReference.cpp +++ b/ocs2_ddp/test/bouncingmass/OverallReference.cpp @@ -29,17 +29,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/test/bouncingmass/OverallReference.h" -OverallReference::OverallReference(const scalar_array_t trajTimes, const vector_array_t trajStates) { - References_.clear(); - References_.resize(trajTimes.size() - 1); - +OverallReference::OverallReference(const scalar_array_t& trajTimes, const vector_array_t& trajStates) { switchtimes_ = trajTimes; - vector_t x0 = trajStates[0]; + References_.reserve(trajTimes.size() - 1); for (int i = 0; i < trajTimes.size() - 1; i++) { - References_[i] = Reference(trajTimes[i], trajTimes[i + 1], x0, trajStates[i + 1]); - x0 = trajStates[i + 1]; - jumpMap(x0); + if (i == 0) { + References_.emplace_back(trajTimes[i], trajTimes[i + 1], trajStates[i], trajStates[i + 1]); + } else { + References_.emplace_back(trajTimes[i], trajTimes[i + 1], jumpMap(trajStates[i]), trajStates[i + 1]); + } } } @@ -53,51 +52,37 @@ int OverallReference::getIndex(scalar_t time) const { return -1; } -void OverallReference::getInput(scalar_t time, vector_t& input) const { - int idx = getIndex(time); +vector_t OverallReference::getInput(scalar_t time) const { + vector_t input = vector_t::Zero(INPUT_DIM); + const int idx = getIndex(time); if (idx >= 0 && idx < References_.size()) { References_[idx].getInput(time, input); - } else { - input.setZero(INPUT_DIM); } + return input; } -vector_t OverallReference::getInput(scalar_t time) const { - vector_t u; - getInput(time, u); - return u; -} - -void OverallReference::getState(int idx, scalar_t time, vector_t& x) const { +vector_t OverallReference::getState(int idx, scalar_t time) const { + vector_t state; if (idx >= 0 && idx < References_.size()) { if (time < 2) { - References_[idx].getState(time, x); + References_[idx].getState(time, state); } else { - getState(time, x); + state = getState(time); } } else { - References_[0].getState(0, x); + References_[0].getState(0, state); } -} - -vector_t OverallReference::getState(int idx, scalar_t time) const { - vector_t state; - getState(idx, time, state); return state; } -void OverallReference::getState(scalar_t time, vector_t& x) const { - int idx = getIndex(time); +vector_t OverallReference::getState(scalar_t time) const { + vector_t state; + const int idx = getIndex(time); if (idx >= 0 && idx < References_.size()) { - References_[idx].getState(time, x); + References_[idx].getState(time, state); } else { - References_[0].getState(0, x); + References_[0].getState(0, state); } -} - -vector_t OverallReference::getState(scalar_t time) const { - vector_t state; - getState(time, state); return state; } @@ -124,8 +109,9 @@ void OverallReference::display(int i) { References_[i].display(); } -void OverallReference::jumpMap(vector_t& x) const { - scalar_t e = 0.95; - x[1] = x[1] - (1 + e) * x[1]; - x[2] = x[2] + 1; +vector_t OverallReference::jumpMap(const vector_t& x) const { + constexpr scalar_t e = 0.95; + vector_t y(x.size()); + y << x(0), -e * x(1), x(2) + 1.0; + return y; } diff --git a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h index 2b95ebfda..3fa41cd91 100644 --- a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h +++ b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h @@ -59,15 +59,7 @@ class OverallReference { * @param [in] trajTimes: list of times at which the reference is defined * @param [in] trajState: list of waypoints at which the reference is defined */ - OverallReference(const scalar_array_t trajTimes, const vector_array_t trajState); - - /* - * Calculate the input at a certain time - * - * @param [in] time: time moment at which the input is calculated - * @param [out] input: input corresponding to time - */ - void getInput(scalar_t time, vector_t& input) const; + OverallReference(const scalar_array_t& trajTimes, const vector_array_t& trajState); /* * Calculate the input at a certain time @@ -77,14 +69,6 @@ class OverallReference { */ vector_t getInput(scalar_t time) const; - /* - * Calculate the reference state at a certain time - * - * @param [in] time: time moment at which the input is calculated - * @param [out] state: state corresponding to time - */ - void getState(scalar_t time, vector_t& x) const; - /* * Calculate the reference state at a certain time * @@ -102,15 +86,6 @@ class OverallReference { */ vector_t getState(int idx, scalar_t time) const; - /* - * Calculate the reference state at a certain time and mode - * - * @param [in] idx: mode at which the input is calculated - * @param [in] time: time moment at which the input is calculated - * @param [out] state: state corresponding to time and mode - */ - void getState(int idx, scalar_t time, vector_t& x) const; - /* * Extend the reference past the event times, by integrating the input signal * without applying the jump map @@ -138,12 +113,10 @@ class OverallReference { /* * Jump map of the system - * * @param [in] x: State before event - * - * @return currently active index + * @return state after event */ - void jumpMap(vector_t& x) const; + vector_t jumpMap(const vector_t& x) const; std::vector<Reference> References_; std::vector<scalar_t> switchtimes_; diff --git a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h index c486a68c6..e74845266 100644 --- a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h +++ b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h @@ -48,11 +48,6 @@ using vector_array_t = ocs2::vector_array_t; */ class Reference { public: - /* - * Constructor - */ - Reference() = default; - /* * Constructor * @@ -97,9 +92,7 @@ class Reference { */ void extendref(scalar_t delta, Reference* refPre, Reference* refPost); - /* - * Display the reference - */ + /* Display the reference */ void display(); scalar_t t0_; diff --git a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/SystemModel.h b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/SystemModel.h index b01246b45..6f8682932 100644 --- a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/SystemModel.h +++ b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/SystemModel.h @@ -51,10 +51,10 @@ class BouncingMassDynamics final : public ocs2::LinearSystemDynamics { public: BouncingMassDynamics() : LinearSystemDynamics(matrix_t(STATE_DIM, STATE_DIM), matrix_t(STATE_DIM, INPUT_DIM), matrix_t(STATE_DIM, STATE_DIM)) { - LinearSystemDynamics::A_ << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - LinearSystemDynamics::B_ << 0.0, 1.0, 0.0; - const scalar_t e = 0.95; - LinearSystemDynamics::G_ << 1.0, 0.0, 0.0, 0.0, -e, 0.0, 0.0, 0.0, 1.0; + constexpr scalar_t e = 0.95; + A_ << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + B_ << 0.0, 1.0, 0.0; + G_ << 1.0, 0.0, 0.0, 0.0, -e, 0.0, 0.0, 0.0, 1.0; } ~BouncingMassDynamics() override = default; @@ -86,7 +86,7 @@ class BouncingMassDynamics final : public ocs2::LinearSystemDynamics { class BouncingMassCost final : public ocs2::StateInputCost { public: - BouncingMassCost(OverallReference ref, matrix_t Q, matrix_t R) : ref_(ref), Q_(std::move(Q)), R_(std::move(R)) {} + BouncingMassCost(OverallReference ref, matrix_t Q, matrix_t R) : ref_(std::move(ref)), Q_(std::move(Q)), R_(std::move(R)) {} ~BouncingMassCost() override = default; @@ -95,16 +95,16 @@ class BouncingMassCost final : public ocs2::StateInputCost { scalar_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const ocs2::TargetTrajectories&, const PreComputation&) const override { const auto stateInput = getNominalStateInput(t, x, u); - vector_t xDeviation = x - stateInput.first; - vector_t uDeviation = u - stateInput.second; + const vector_t xDeviation = x - stateInput.first; + const vector_t uDeviation = u - stateInput.second; return 0.5 * xDeviation.dot(Q_ * xDeviation) + 0.5 * uDeviation.dot(R_ * uDeviation); } ScalarFunctionQuadraticApproximation getQuadraticApproximation(scalar_t t, const vector_t& x, const vector_t& u, const ocs2::TargetTrajectories&, const PreComputation&) const override { const auto stateInput = getNominalStateInput(t, x, u); - vector_t xDeviation = x - stateInput.first; - vector_t uDeviation = u - stateInput.second; + const vector_t xDeviation = x - stateInput.first; + const vector_t uDeviation = u - stateInput.second; ScalarFunctionQuadraticApproximation L; L.f = 0.5 * xDeviation.dot(Q_ * xDeviation) + 0.5 * uDeviation.dot(R_ * uDeviation); @@ -118,38 +118,32 @@ class BouncingMassCost final : public ocs2::StateInputCost { private: std::pair<vector_t, vector_t> getNominalStateInput(scalar_t t, const vector_t& x, const vector_t& u) const { - vector_t xRef; - vector_t uRef; const int currentMode = x.tail(1).value(); - ref_.getInput(t, uRef); - ref_.getState(currentMode, t, xRef); - return {xRef, uRef}; + return {ref_.getState(currentMode, t), ref_.getInput(t)}; } - private: - OverallReference ref_; - matrix_t Q_; - matrix_t R_; + const OverallReference ref_; + const matrix_t Q_; + const matrix_t R_; }; class BouncingMassFinalCost final : public ocs2::StateCost { public: BouncingMassFinalCost(OverallReference ref, matrix_t QFinal, scalar_t timeFinal) - : ref_(ref), timeFinal_(timeFinal), QFinal_(std::move(QFinal)) {} + : ref_(std::move(ref)), timeFinal_(timeFinal), QFinal_(std::move(QFinal)) {} ~BouncingMassFinalCost() override = default; BouncingMassFinalCost* clone() const override { return new BouncingMassFinalCost(*this); } scalar_t getValue(scalar_t t, const vector_t& x, const ocs2::TargetTrajectories&, const PreComputation&) const override { - vector_t xDeviation = x - getNominalFinalState(t, x); + const vector_t xDeviation = x - getNominalFinalState(t, x); return 0.5 * xDeviation.dot(QFinal_ * xDeviation); } ScalarFunctionQuadraticApproximation getQuadraticApproximation(scalar_t t, const vector_t& x, const ocs2::TargetTrajectories&, const PreComputation&) const override { - vector_t xDeviation = x - getNominalFinalState(t, x); - + const vector_t xDeviation = x - getNominalFinalState(t, x); ScalarFunctionQuadraticApproximation Phi; Phi.f = 0.5 * xDeviation.dot(QFinal_ * xDeviation); Phi.dfdx = QFinal_ * xDeviation; @@ -162,16 +156,13 @@ class BouncingMassFinalCost final : public ocs2::StateCost { // Terminal cost only calculated for final state, not for intermediate switch states if (std::fabs(t - timeFinal_) > ocs2::numeric_traits::weakEpsilon<scalar_t>()) { return x; + } else { + const int currentMode = x.tail(1).value(); + return ref_.getState(currentMode, t); } - - vector_t xRef; - const int currentMode = x.tail(1).value(); - ref_.getState(currentMode, t, xRef); - return xRef; } - private: - OverallReference ref_; - scalar_t timeFinal_; - matrix_t QFinal_; + const OverallReference ref_; + const scalar_t timeFinal_; + const matrix_t QFinal_; }; diff --git a/ocs2_ddp/test/testDdpHelperFunction.cpp b/ocs2_ddp/test/testDdpHelperFunction.cpp index a779c5352..06c7ed6b0 100644 --- a/ocs2_ddp/test/testDdpHelperFunction.cpp +++ b/ocs2_ddp/test/testDdpHelperFunction.cpp @@ -57,7 +57,7 @@ TEST(extractPrimalSolution, eventAtInitTime) { primalSolution.postEventIndices_.push_back(primalSolution.timeTrajectory_.size()); primalSolution.modeSchedule_.modeSequence.push_back(s + 1); primalSolution.modeSchedule_.eventTimes.push_back(primalSolution.timeTrajectory_.back()); - } // end of s + } // end of s // an extra event at final time primalSolution.timeTrajectory_.push_back(primalSolution.timeTrajectory_.back()); diff --git a/ocs2_robotic_examples/ocs2_double_integrator/config/mpc/task.info b/ocs2_robotic_examples/ocs2_double_integrator/config/mpc/task.info index 56b56c267..f9a26589f 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_double_integrator/config/mpc/task.info @@ -34,9 +34,6 @@ ddp maxNumStepsPerSecond 100000 backwardPassIntegratorType ODE45 - inequalityConstraintMu 100.0 - inequalityConstraintDelta 1.1 - preComputeRiccatiTerms true useFeedbackPolicy true diff --git a/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp b/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp index 05e1d9897..b3807fb6a 100644 --- a/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp @@ -37,7 +37,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. TEST(test_circular_kinematics, solve_projected_EqConstraints) { // optimal control problem - ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/sqp_test_generated"); + ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ocs2/sqp_test_generated"); // Initializer ocs2::DefaultInitializer zeroInitializer(2); From ec999f83705ef4db6eb3b1a4a4949f8c19ed5e21 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 25 Aug 2022 16:10:50 +0200 Subject: [PATCH 317/512] activating bouncingmass unittest --- .../test/bouncingmass/BouncingMassTest.cpp | 213 ++++++++---------- .../test/bouncingmass/OverallReference.cpp | 53 +---- ocs2_ddp/test/bouncingmass/Reference.cpp | 95 +++----- .../test/bouncingmass/OverallReference.h | 9 - .../ocs2_ddp/test/bouncingmass/Reference.h | 20 +- 5 files changed, 152 insertions(+), 238 deletions(-) diff --git a/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp b/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp index 59b4bef89..63a6f3c4a 100644 --- a/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp +++ b/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp @@ -40,7 +40,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/test/bouncingmass/OverallReference.h" #include "ocs2_ddp/test/bouncingmass/SystemModel.h" -/* Test for StateTriggeredRollout in combination with SLQ +/* + * Test for State-Triggered hybrid SLQ * * The system being tested is a system consisting of a mass and a wall * the mass bounces against the wall. The control objective is to follow a predefined @@ -65,147 +66,133 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * (1) No penetration of Guard Surfaces * (2) Check of cost function compared against cost calculated during trusted run of SLQ */ -TEST(BouncingMassTest, DISABLED_state_rollout_slq) { - using scalar_t = ocs2::scalar_t; - using vector_t = ocs2::vector_t; - using matrix_t = ocs2::matrix_t; - using scalar_array_t = ocs2::scalar_array_t; - using vector_array_t = ocs2::vector_array_t; - using matrix_array_t = ocs2::matrix_array_t; - - ocs2::ddp::Settings ddpSettings; - ddpSettings.algorithm_ = ocs2::ddp::Algorithm::SLQ; - ddpSettings.displayInfo_ = false; - ddpSettings.displayShortSummary_ = true; - ddpSettings.maxNumIterations_ = 30; - ddpSettings.minRelCost_ = 1e-4; - ddpSettings.nThreads_ = 1; - ddpSettings.checkNumericalStability_ = true; - ddpSettings.absTolODE_ = 1e-10; - ddpSettings.relTolODE_ = 1e-7; - ddpSettings.maxNumStepsPerSecond_ = 10000; - ddpSettings.useFeedbackPolicy_ = true; - ddpSettings.debugPrintRollout_ = false; - - ocs2::rollout::Settings rolloutSettings; - rolloutSettings.absTolODE = 1e-10; - rolloutSettings.relTolODE = 1e-7; - rolloutSettings.timeStep = 1e-3; - rolloutSettings.maxNumStepsPerSecond = 10000; - rolloutSettings.maxSingleEventIterations = 5; - rolloutSettings.useTrajectorySpreadingController = true; - +TEST(BouncingMassTest, state_triggered_hybrid_slq) { // Parameters - const scalar_t startTime = 0.0; - const scalar_t finalTime = 2.5; - const vector_t x0 = Eigen::Matrix<scalar_t, 3, 1>(0.7, 0.5, 0); - const vector_t u0 = vector_t::Zero(INPUT_DIM); - bool outputSolution = false; + constexpr bool outputSolution = false; + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 2.5; + const ocs2::vector_t x0 = Eigen::Matrix<ocs2::scalar_t, 3, 1>(0.7, 0.5, 0); + const ocs2::vector_t u0 = ocs2::vector_t::Zero(INPUT_DIM); + + const auto ddpSettings = [&]() { + ocs2::ddp::Settings s; + s.algorithm_ = ocs2::ddp::Algorithm::SLQ; + s.displayInfo_ = false; + s.displayShortSummary_ = true; + s.maxNumIterations_ = 30; + s.minRelCost_ = 1e-4; + s.nThreads_ = 1; + s.checkNumericalStability_ = true; + s.absTolODE_ = 1e-10; + s.relTolODE_ = 1e-7; + s.maxNumStepsPerSecond_ = 10000; + s.useFeedbackPolicy_ = true; + s.debugPrintRollout_ = false; + return s; + }(); + + const auto rolloutSettings = [&]() { + ocs2::rollout::Settings s; + s.absTolODE = 1e-10; + s.relTolODE = 1e-7; + s.timeStep = 1e-3; + s.maxNumStepsPerSecond = 10000; + s.maxSingleEventIterations = 5; + s.useTrajectorySpreadingController = true; + return s; + }(); // Generation of Reference Trajectory - const scalar_array_t trajTimes{0, 0.2, 0.8, 1.0, 1.2, 1.8, 2.0}; - - vector_t state0(STATE_DIM); // Initial and final state - state0 << 0.5, 0, 0; - vector_t state1(STATE_DIM); // Hard impact - state1 << 0, -5, 0; - vector_t state2(STATE_DIM); // Soft impact - state2 << 0, -1, 0; + const ocs2::scalar_array_t trajTimes{0, 0.2, 0.8, 1.0, 1.2, 1.8, 2.0}; + const ocs2::vector_t state0 = (ocs2::vector_t(STATE_DIM) << 0.5, 0, 0).finished(); // Initial and final state + const ocs2::vector_t state1 = (ocs2::vector_t(STATE_DIM) << 0, -5, 0).finished(); // Hard impact + const ocs2::vector_t state2 = (ocs2::vector_t(STATE_DIM) << 0, -1, 0).finished(); // Soft impact + const ocs2::vector_array_t trajStates{state0, state1, state2, state2, state1, state2, state0}; - const scalar_t delta = 0.5; - const vector_array_t trajStates{state0, state1, state2, state2, state1, state2, state0}; OverallReference reference(trajTimes, trajStates); + constexpr ocs2::scalar_t delta = 0.5; reference.extendref(delta); + // OCP + ocs2::OptimalControlProblem problem; + // Dynamics, Constraints and derivative classes BouncingMassDynamics systemDynamics; + problem.dynamicsPtr.reset(systemDynamics.clone()); // Cost Function - matrix_t Q(STATE_DIM, STATE_DIM); - Q << 50.0, 0.0, 0.0, 0.0, 50.0, 0.0, 0.0, 0.0, 0.0; - matrix_t R(INPUT_DIM, INPUT_DIM); - R << 1.0; - std::unique_ptr<ocs2::StateInputCost> cost(new BouncingMassCost(reference, Q, R)); - - matrix_t Qf(STATE_DIM, STATE_DIM); - Qf << 56.63, 7.07, 0.0, 7.07, 8.01, 0.0, 0.0, 0.0, 0.0; - std::unique_ptr<ocs2::StateCost> finalCost(new BouncingMassFinalCost(reference, Qf, finalTime)); - std::unique_ptr<ocs2::StateCost> preJumpCost(new BouncingMassFinalCost(reference, Qf, finalTime)); - - ocs2::OptimalControlProblem problem; - problem.dynamicsPtr.reset(systemDynamics.clone()); - problem.costPtr->add("cost", std::move(cost)); - problem.preJumpCostPtr->add("preJumpCost", std::move(preJumpCost)); - problem.finalCostPtr->add("finalCost", std::move(finalCost)); + const ocs2::matrix_t Q = (ocs2::matrix_t(STATE_DIM, STATE_DIM) << 50.0, 0.0, 0.0, 0.0, 50.0, 0.0, 0.0, 0.0, 0.0).finished(); + const ocs2::matrix_t R = (ocs2::matrix_t(INPUT_DIM, INPUT_DIM) << 1.0).finished(); + const ocs2::matrix_t Qf = (ocs2::matrix_t(STATE_DIM, STATE_DIM) << 56.63, 7.07, 0.0, 7.07, 8.01, 0.0, 0.0, 0.0, 0.0).finished(); + problem.costPtr->add("cost", std::unique_ptr<BouncingMassCost>(new BouncingMassCost(reference, Q, R))); + problem.preJumpCostPtr->add("preJumpCost", std::unique_ptr<BouncingMassFinalCost>(new BouncingMassFinalCost(reference, Qf, finalTime))); + problem.finalCostPtr->add("finalCost", std::unique_ptr<BouncingMassFinalCost>(new BouncingMassFinalCost(reference, Qf, finalTime))); // Rollout Class ocs2::StateTriggeredRollout stateTriggeredRollout(systemDynamics, rolloutSettings); // Initial Controller - matrix_t controllerGain(INPUT_DIM, STATE_DIM); - controllerGain << 25, 10, 0; - vector_t controllerBias; - - matrix_array_t controllerGainArray; - vector_array_t controllerBiasArray; - scalar_array_t timeStampArray; - - constexpr scalar_t controllerDeltaTime = 1e-3; // Time step for controller time array - constexpr scalar_t eps = ocs2::numeric_traits::weakEpsilon<scalar_t>(); - scalar_array_t controlTimes = trajTimes; - controlTimes.push_back(finalTime); - - for (int i = 0; i < controlTimes.size() - 1; i++) { - scalar_t timeSteps = (controlTimes[i + 1] + eps - controlTimes[i]) / controllerDeltaTime; - scalar_t timeStamp = 0; - vector_t refState; - - for (int j = 0; j <= timeSteps; j++) { - if (j == 0 && i < controlTimes.size() - 2) { - timeStamp = controlTimes[i] + eps; - } else { - timeStamp = controlTimes[i] + j * controllerDeltaTime; + const auto initController = [&]() { + ocs2::matrix_array_t controllerGainArray; + ocs2::vector_array_t controllerBiasArray; + ocs2::scalar_array_t timeStampArray; + + constexpr ocs2::scalar_t controllerDeltaTime = 1e-3; // Time step for controller time array + constexpr ocs2::scalar_t eps = ocs2::numeric_traits::weakEpsilon<ocs2::scalar_t>(); + ocs2::scalar_array_t controlTimes = trajTimes; + controlTimes.push_back(finalTime); + + const ocs2::matrix_t controllerGain = (ocs2::matrix_t(INPUT_DIM, STATE_DIM) << 25, 10, 0).finished(); + for (int i = 0; i < controlTimes.size() - 1; i++) { + const ocs2::scalar_t timeSteps = (controlTimes[i + 1] + eps - controlTimes[i]) / controllerDeltaTime; + ocs2::scalar_t timeStamp = 0; + for (int j = 0; j <= timeSteps; j++) { + if (j == 0 && i < controlTimes.size() - 2) { + timeStamp = controlTimes[i] + eps; + } else { + timeStamp = controlTimes[i] + j * controllerDeltaTime; + } + + ocs2::vector_t refState = reference.getState(timeStamp); + ocs2::vector_t refInput = reference.getInput(timeStamp); + ocs2::vector_t controllerBias = controllerGain * refState + refInput; + + timeStampArray.push_back(timeStamp); + controllerGainArray.push_back(-controllerGain); + controllerBiasArray.push_back(controllerBias); } - - vector_t refState = reference.getState(timeStamp); - vector_t refInput = reference.getInput(timeStamp); - controllerBias = controllerGain * refState + refInput; - - timeStampArray.push_back(timeStamp); - controllerGainArray.push_back(-controllerGain); - controllerBiasArray.push_back(controllerBias); } - } - ocs2::LinearController initController(timeStampArray, controllerBiasArray, controllerGainArray); + return ocs2::LinearController(timeStampArray, controllerBiasArray, controllerGainArray); + }(); + + // operating point + const ocs2::OperatingPoints operatingTrajectories(x0, u0); - ocs2::OperatingPoints operatingTrajectories(x0, u0); // SLQ ocs2::SLQ slq(ddpSettings, stateTriggeredRollout, problem, operatingTrajectories); slq.run(startTime, x0, finalTime, &initController); - auto solutionST = slq.primalSolution(finalTime); + const auto solutionST = slq.primalSolution(finalTime); + // Test 1: No penetration of Guard Surfaces for (int i = 0; i < solutionST.stateTrajectory_.size(); i++) { - // Test 1: No penetration of Guard Surfaces - EXPECT_GT(solutionST.stateTrajectory_[i][0], -ddpSettings.absTolODE_); - // Display output - // format: idx;time;x[0];xref[0];x[1];xref[1];u;uref - if (outputSolution) { - auto idx = static_cast<int>(solutionST.stateTrajectory_[i][2]); - - vector_t uRef = reference.getInput(solutionST.timeTrajectory_[i]); - vector_t xRef = reference.getState(idx, solutionST.timeTrajectory_[i]); + EXPECT_GT(solutionST.stateTrajectory_[i](0), -ddpSettings.absTolODE_); + // Display output: format: idx;time;x[0];xref[0];x[1];xref[1];u;uref + if (outputSolution) { + const auto idx = static_cast<int>(solutionST.stateTrajectory_[i][2]); + const auto uRef = reference.getInput(solutionST.timeTrajectory_[i]); + const auto xRef = reference.getState(idx, solutionST.timeTrajectory_[i]); std::cerr << i << ";" << idx << ";"; std::cerr << std::setprecision(25) << solutionST.timeTrajectory_[i]; - std::cerr << std::setprecision(6) << ";" << solutionST.stateTrajectory_[i][0] << ";" << xRef[0] << ";"; - std::cerr << solutionST.stateTrajectory_[i][1] << ";" << xRef[1] << ";"; - std::cerr << solutionST.inputTrajectory_[i][0] << ";" << uRef[0] << std::endl; + std::cerr << std::setprecision(6) << ";" << solutionST.stateTrajectory_[i](0) << ";" << xRef[0] << ";"; + std::cerr << solutionST.stateTrajectory_[i](1) << ";" << xRef[1] << ";"; + std::cerr << solutionST.inputTrajectory_[i](0) << ";" << uRef[0] << std::endl; } - } + } // end of i loop // Test 2: Check of cost function - auto performanceIndeces = slq.getPerformanceIndeces(); - constexpr scalar_t expectedCost = 7.15; - EXPECT_NEAR(performanceIndeces.cost, expectedCost, 100.0 * ddpSettings.minRelCost_); + constexpr ocs2::scalar_t expectedCost = 7.15; + const auto performanceIndeces = slq.getPerformanceIndeces(); + EXPECT_LE(performanceIndeces.cost, expectedCost); } diff --git a/ocs2_ddp/test/bouncingmass/OverallReference.cpp b/ocs2_ddp/test/bouncingmass/OverallReference.cpp index 19a70a51e..0a6628b9e 100644 --- a/ocs2_ddp/test/bouncingmass/OverallReference.cpp +++ b/ocs2_ddp/test/bouncingmass/OverallReference.cpp @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/test/bouncingmass/OverallReference.h" +#include <ocs2_core/misc/Lookup.h> + OverallReference::OverallReference(const scalar_array_t& trajTimes, const vector_array_t& trajStates) { switchtimes_ = trajTimes; @@ -42,65 +44,34 @@ OverallReference::OverallReference(const scalar_array_t& trajTimes, const vector } } -int OverallReference::getIndex(scalar_t time) const { - for (int i = 0; i < switchtimes_.size() - 1; i++) { - if (switchtimes_[i + 1] >= time) { - return i; - } - } - - return -1; +vector_t OverallReference::getInput(scalar_t time) const { + const int idx = ocs2::lookup::findIntervalInTimeArray(switchtimes_, time); + return (idx >= 0 && idx < References_.size()) ? References_[idx].getInput(time) : vector_t::Zero(INPUT_DIM); } -vector_t OverallReference::getInput(scalar_t time) const { - vector_t input = vector_t::Zero(INPUT_DIM); - const int idx = getIndex(time); - if (idx >= 0 && idx < References_.size()) { - References_[idx].getInput(time, input); - } - return input; +vector_t OverallReference::getState(scalar_t time) const { + const int idx = ocs2::lookup::findIntervalInTimeArray(switchtimes_, time); + return (idx >= 0 && idx < References_.size()) ? References_[idx].getState(time) : References_[0].getState(0); } vector_t OverallReference::getState(int idx, scalar_t time) const { vector_t state; if (idx >= 0 && idx < References_.size()) { if (time < 2) { - References_[idx].getState(time, state); + state = References_[idx].getState(time); } else { state = getState(time); } } else { - References_[0].getState(0, state); - } - return state; -} - -vector_t OverallReference::getState(scalar_t time) const { - vector_t state; - const int idx = getIndex(time); - if (idx >= 0 && idx < References_.size()) { - References_[idx].getState(time, state); - } else { - References_[0].getState(0, state); + state = References_[0].getState(0); } return state; } void OverallReference::extendref(scalar_t delta) { for (int i = 0; i < References_.size(); i++) { - Reference* pre; - Reference* post; - if (i > 0) { - pre = &References_[i - 1]; - } else { - pre = nullptr; - } - - if (i <= References_.size() - 1) { - post = &References_[i + 1]; - } else { - post = nullptr; - } + Reference* pre = i > 0 ? &References_[i - 1] : nullptr; + Reference* post = i < References_.size() - 1 ? &References_[i + 1] : nullptr; References_[i].extendref(delta, pre, post); } } diff --git a/ocs2_ddp/test/bouncingmass/Reference.cpp b/ocs2_ddp/test/bouncingmass/Reference.cpp index 2a1fc9656..17256c7d7 100644 --- a/ocs2_ddp/test/bouncingmass/Reference.cpp +++ b/ocs2_ddp/test/bouncingmass/Reference.cpp @@ -40,7 +40,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/test/bouncingmass/Reference.h" Reference::Reference(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) { - Create5thOrdPol(t0, t1, p0, p1); + polX_ = Create5thOrdPol(t0, t1, p0, p1); polV_ = polyder(polX_); polU_ = polyder(polV_); @@ -48,20 +48,16 @@ Reference::Reference(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_ t1_ = t1; } -void Reference::getInput(scalar_t time, vector_t& input) const { - input.setZero(1); +vector_t Reference::getInput(scalar_t time) const { + vector_t input = vector_t::Zero(1); for (int i = 0; i < polU_.size(); i++) { input[0] += polU_[i] * std::pow(time, i); } -} - -vector_t Reference::getInput(scalar_t time) const { - vector_t input; - getInput(time, input); return input; } -void Reference::getState(scalar_t time, vector_t& x) const { +vector_t Reference::getState(scalar_t time) const { + vector_t x; if (time <= t1_ && time >= t0_) { x.setZero(3); for (int i = 0; i < polU_.size(); i++) { @@ -71,42 +67,33 @@ void Reference::getState(scalar_t time, vector_t& x) const { } else { interpolate_ext(time, x); } + return x; } void Reference::extendref(scalar_t delta, Reference* refPre, Reference* refPost) { - delta_ = delta; + constexpr scalar_t dt = 1e-3; + boost::numeric::odeint::runge_kutta_dopri5<vector_t, scalar_t, vector_t, scalar_t, boost::numeric::odeint::vector_space_algebra> stepper; + // Lambda for general system dynamics, assuming that the reference input is available - auto model = [](const vector_t& x, vector_t& dxdt, const double t, vector_t uref) { - matrix_t A(3, 3); - A << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - matrix_t B(3, 1); - B << 0.0, 1.0, 0.0; - - dxdt = A * x + B * uref; - }; + const matrix_t A = (matrix_t(3, 3) << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); + const matrix_t B = (matrix_t(3, 1) << 0.0, 1.0, 0.0).finished(); + auto model = [&](const vector_t& x, const vector_t& uref, vector_t& dxdt, const scalar_t t) { dxdt = A * x + B * uref; }; + // pre-part of extension if (refPre != nullptr) { // Construct Lambda to represent System Dynamics with correct reference input - auto preModel = [&refPre, &model](const vector_t& x, vector_t& dxdt, const double t) { - vector_t uref = refPre->getInput(t); - model(x, dxdt, t, uref); - }; + auto preModel = [&](const vector_t& x, vector_t& dxdt, const scalar_t t) { model(x, refPre->getInput(t), dxdt, t); }; // Construct lambda to act as observer, which will store the time and state trajectories - scalar_array_t* timeStorePtr = &tPre_; - vector_array_t* stateStorePtr = &xPre_; - auto preObserver = [&timeStorePtr, &stateStorePtr](vector_t& x, scalar_t& t) { - timeStorePtr->push_back(t); - stateStorePtr->push_back(x); + auto preObserver = [&](const vector_t& x, scalar_t t) { + tPre_.push_back(t); + xPre_.push_back(x); }; - vector_t x0; - getState(t0_, x0); - scalar_t t0 = t0_; - scalar_t t1 = t0 - delta; - scalar_t dt = -1e-3; - - boost::numeric::odeint::integrate_adaptive(stepper, preModel, x0, t0, t1, dt, preObserver); + vector_t x0 = getState(t0_); + const scalar_t t0 = t0_; + const scalar_t t1 = t0 - delta; + boost::numeric::odeint::integrate_adaptive(stepper, preModel, x0, t0, t1, -dt, preObserver); std::reverse(std::begin(tPre_), std::end(tPre_)); std::reverse(std::begin(xPre_), std::end(xPre_)); } @@ -114,41 +101,31 @@ void Reference::extendref(scalar_t delta, Reference* refPre, Reference* refPost) // post-part of extension if (refPost != nullptr) { // Construct Lambda to represent System Dynamics with correct reference input - auto postModel = [&refPost, &model](const vector_t& x, vector_t& dxdt, const double t) { - vector_t uref = refPost->getInput(t); - model(x, dxdt, t, uref); - }; + auto postModel = [&](const vector_t& x, vector_t& dxdt, const scalar_t t) { model(x, refPost->getInput(t), dxdt, t); }; // Construct lambda to act as observer, which will store the time and state trajectories - scalar_array_t* timeStorePtr = &tPost_; - vector_array_t* stateStorePtr = &xPost_; - auto postObserver = [&timeStorePtr, &stateStorePtr](vector_t& x, scalar_t& t) { - timeStorePtr->push_back(t); - stateStorePtr->push_back(x); + auto postObserver = [&](const vector_t& x, scalar_t t) { + tPost_.push_back(t); + xPost_.push_back(x); }; - vector_t x0; - getState(t1_, x0); - scalar_t t0 = t1_; - scalar_t t1 = t0 + delta; - scalar_t dt = 1e-3; - boost::numeric::odeint::integrate_adaptive(stepper, postModel, x0, t0, t1, dt, postObserver); + vector_t x0 = getState(t1_); + const scalar_t t0 = t1_; + const scalar_t t1 = t0 + delta; + boost::numeric::odeint::integrate_adaptive(stepper, postModel, x0, t0, t1, -dt, postObserver); } } -void Reference::Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) { +Eigen::Matrix<scalar_t, 6, 1> Reference::Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) const { Eigen::Matrix<scalar_t, 6, 6> A; - Eigen::Matrix<scalar_t, 6, 6> Ainv; - A << 1, t0, std::pow(t0, 2), std::pow(t0, 3), std::pow(t0, 4), std::pow(t0, 5), 0, 1, 2 * t0, 3 * std::pow(t0, 2), 4 * std::pow(t0, 3), 5 * std::pow(t0, 4), 0, 0, 2, 6 * t0, 12 * std::pow(t0, 2), 20 * std::pow(t0, 3), 1, t1, std::pow(t1, 2), std::pow(t1, 3), std::pow(t1, 4), std::pow(t1, 5), 0, 1, 2 * t1, 3 * std::pow(t1, 2), 4 * std::pow(t1, 3), 5 * std::pow(t1, 4), 0, 0, 2, 6 * t1, 12 * std::pow(t1, 2), 20 * std::pow(t1, 3); - Ainv = A.inverse(); + Eigen::Matrix<scalar_t, 6, 1> b; + b << p0, p1; - Eigen::Matrix<scalar_t, 6, 1> x; - x << p0, p1; - polX_ = Ainv * x; + return A.colPivHouseholderQr().solve(b); } void Reference::interpolate_ext(scalar_t time, vector_t& x) const { @@ -187,12 +164,10 @@ void Reference::display() { std::cerr << "####Normal-Trajectory####" << std::endl; std::cerr << "#########################" << std::endl; - scalar_t dt = 0.01; + constexpr scalar_t dt = 0.01; for (int i = 0; i < (t1_ - t0_) / dt; i++) { scalar_t t = t0_ + dt * i; - vector_t x; - getState(t, x); - + vector_t x = getState(t); std::cerr << t << ";" << x[0] << ";" << x[1] << std::endl; } diff --git a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h index 3fa41cd91..ad4ea08e9 100644 --- a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h +++ b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h @@ -102,15 +102,6 @@ class OverallReference { void display(int i); private: - /* - * Find the index of the currently active reference - * - * @param [in] time: time moment at which the index is requested - * - * @return currently active index - */ - int getIndex(scalar_t time) const; - /* * Jump map of the system * @param [in] x: State before event diff --git a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h index e74845266..9acfffdf1 100644 --- a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h +++ b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h @@ -42,9 +42,8 @@ using vector_array_t = ocs2::vector_array_t; /* * This class constructs, contains a section of the reference, which is contained in * OverallReference and used in BouncingMass SLQ test. - * The reference is based on the reference presented - * in [On Optimal Trajectory Tracking for Mechanical Systems with Unilateral Constraints - * by M. Rijnen] + * The reference is based on the reference presented in + * "On Optimal Trajectory Tracking for Mechanical Systems with Unilateral Constraints" by M. Rijnen */ class Reference { public: @@ -58,14 +57,6 @@ class Reference { */ Reference(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1); - /* - * Obtain reference input at the current time - * - * @param [in] time: current time - * @param [out] input: current reference input - */ - void getInput(const scalar_t time, vector_t& input) const; - /* * Obtain reference input at the current time * @@ -78,9 +69,9 @@ class Reference { * Obtain reference state at current time * * @param [in] time: current time - * @param [out] x: current reference state + * @return current reference state */ - void getState(const scalar_t time, vector_t& x) const; + vector_t getState(const scalar_t time) const; /* * Extend the reference by integrating the input signal of the next @@ -97,7 +88,6 @@ class Reference { scalar_t t0_; scalar_t t1_; - scalar_t delta_ = 0; private: /* @@ -109,7 +99,7 @@ class Reference { * @param [in] p0: first waypoint * @param [in] p1: second waypoint */ - void Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1); + Eigen::Matrix<scalar_t, 6, 1> Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) const; /* * Find the derivative of the polynomial described by a From e2489fb38bd6cf22cce544038c60d31ca9e18653 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 26 Aug 2022 10:01:39 +0200 Subject: [PATCH 318/512] clang tidy --- ocs2_ddp/test/HybridSlqTest.cpp | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/ocs2_ddp/test/HybridSlqTest.cpp b/ocs2_ddp/test/HybridSlqTest.cpp index 744736146..b56a9204d 100644 --- a/ocs2_ddp/test/HybridSlqTest.cpp +++ b/ocs2_ddp/test/HybridSlqTest.cpp @@ -121,7 +121,8 @@ TEST(HybridSlqTest, state_rollout_slq) { problem.costPtr->add("cost", std::make_unique<QuadraticStateInputCost>(Q, R)); problem.preJumpCostPtr->add("preJumpCost", std::make_unique<QuadraticStateCost>(Q)); problem.finalCostPtr->add("finalCost", std::make_unique<QuadraticStateCost>(Q)); - problem.inequalityLagrangianPtr->add("bounds", create(std::move(boundsConstraints), augmented::SlacknessSquaredHingePenalty::create({200.0, 0.1}))); + problem.inequalityLagrangianPtr->add("bounds", + create(std::move(boundsConstraints), augmented::SlacknessSquaredHingePenalty::create({200.0, 0.1}))); const vector_t xNominal = vector_t::Zero(STATE_DIM); const vector_t uNominal = vector_t::Zero(INPUT_DIM); @@ -135,16 +136,17 @@ TEST(HybridSlqTest, state_rollout_slq) { // Test 1: Check constraint compliance. It uses a solver observer to get metrics for the bounds constraints auto boundsConstraintsObserverPtr = std::make_unique<AugmentedLagrangianObserver>("bounds"); - boundsConstraintsObserverPtr->setMetricsCallback([&](const scalar_array_t& timeTraj, const std::vector<LagrangianMetricsConstRef>& metricsTraj) { - constexpr scalar_t constraintViolationTolerance = 1e-1; - for (size_t i = 0; i < metricsTraj.size(); i++) { - const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); - EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(1), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(2), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(3), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - } - }); + boundsConstraintsObserverPtr->setMetricsCallback( + [&](const scalar_array_t& timeTraj, const std::vector<LagrangianMetricsConstRef>& metricsTraj) { + constexpr scalar_t constraintViolationTolerance = 1e-1; + for (size_t i = 0; i < metricsTraj.size(); i++) { + const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); + EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(1), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(2), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(3), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + } + }); // setup SLQ SLQ slq(ddpSettings, stateTriggeredRollout, problem, operatingTrajectories); From a521698b546931bc9e268549783d3da7142b6b22 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 26 Aug 2022 16:25:34 +0200 Subject: [PATCH 319/512] unittest for final and prejump equality Lagrangian --- ocs2_ddp/CMakeLists.txt | 10 + ocs2_ddp/test/testReachingTask.cpp | 192 ++++++++++++++++++ .../test/DoubleIntegratorReachingTask.h | 178 ++++++++++++++++ 3 files changed, 380 insertions(+) create mode 100644 ocs2_ddp/test/testReachingTask.cpp create mode 100644 ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h diff --git a/ocs2_ddp/CMakeLists.txt b/ocs2_ddp/CMakeLists.txt index 8ed125165..97606f278 100644 --- a/ocs2_ddp/CMakeLists.txt +++ b/ocs2_ddp/CMakeLists.txt @@ -201,3 +201,13 @@ target_link_libraries(testDdpHelperFunction ${PROJECT_NAME} gtest_main ) + +catkin_add_gtest(testReachingTask + test/testReachingTask.cpp +) +target_link_libraries(testReachingTask + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${PROJECT_NAME} + gtest_main +) diff --git a/ocs2_ddp/test/testReachingTask.cpp b/ocs2_ddp/test/testReachingTask.cpp new file mode 100644 index 000000000..f01317860 --- /dev/null +++ b/ocs2_ddp/test/testReachingTask.cpp @@ -0,0 +1,192 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <gtest/gtest.h> + +#include <ocs2_oc/rollout/TimeTriggeredRollout.h> +#include <ocs2_oc/test/DoubleIntegratorReachingTask.h> + +#include <ocs2_ddp/ILQR.h> +#include <ocs2_ddp/SLQ.h> + +namespace ocs2 { + +namespace { +// rollout settings +rollout::Settings getRolloutSettings(ddp::Algorithm alg) { + rollout::Settings s; + s.absTolODE = 1e-9; + s.relTolODE = 1e-6; + s.timeStep = DoubleIntegratorReachingTask::timeStep; + s.integratorType = (alg == ddp::Algorithm::SLQ) ? IntegratorType::ODE45 : IntegratorType::RK4; + s.maxNumStepsPerSecond = 100000; + s.checkNumericalStability = true; + return s; +} + +// DDP settings +ddp::Settings getDdpSettings(ddp::Algorithm alg, bool display) { + ddp::Settings s; + s.algorithm_ = alg; + s.nThreads_ = 2; + s.maxNumIterations_ = 50; + s.displayInfo_ = false; + s.displayShortSummary_ = display; + s.debugPrintRollout_ = false; + s.minRelCost_ = DoubleIntegratorReachingTask::minRelCost; + s.constraintTolerance_ = DoubleIntegratorReachingTask::constraintTolerance; + s.timeStep_ = DoubleIntegratorReachingTask::timeStep; + s.backwardPassIntegratorType_ = (alg == ddp::Algorithm::SLQ) ? IntegratorType::ODE45 : IntegratorType::RK4; + s.strategy_ = search_strategy::Type::LINE_SEARCH; + s.lineSearch_.minStepLength = 0.01; + s.lineSearch_.maxStepLength = 1.0; + return s; +} +} // unnamed namespace + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +/* Reaching task as event time at tGoal for a problem with horizon 2*tGoal. The solution input should be zero in [tGoal, 2*tGoal]. */ +class PreJumpDoubleIntegratorReachingTask : public DoubleIntegratorReachingTask, + public testing::Test, + public testing::WithParamInterface<DoubleIntegratorReachingTask::PenaltyType> { + protected: + PreJumpDoubleIntegratorReachingTask() { + // reference manager + referenceManagerPtr = getReferenceManagerPtr(); + // optimal control problem + ocp.dynamicsPtr = getDynamicsPtr(); + ocp.costPtr->add("cost", getCostPtr()); + ocp.equalityConstraintPtr->add("zero_force", std::unique_ptr<ZeroInputConstraint>(new ZeroInputConstraint(*referenceManagerPtr))); + ocp.preJumpEqualityLagrangianPtr->add("goal_reaching", getGoalReachingAugmentedLagrangian(xGoal, GetParam())); + } + + void test(const PrimalSolution& primalSolution) const { + for (size_t i = 0; i < primalSolution.timeTrajectory_.size(); ++i) { + // zero input after tGoal + if (primalSolution.timeTrajectory_[i] > tGoal) { + EXPECT_NEAR(primalSolution.inputTrajectory_[i](0), 0.0, constraintTolerance) << " at time " << primalSolution.timeTrajectory_[i]; + } + // reaching to the target + if (primalSolution.timeTrajectory_[i] > tGoal) { + EXPECT_TRUE(primalSolution.stateTrajectory_[i].isApprox(xGoal, constraintTolerance)) + << " at time " << primalSolution.timeTrajectory_[i]; + } + } // end of i loop + } + + OptimalControlProblem ocp; + std::shared_ptr<ReferenceManager> referenceManagerPtr; +}; + +TEST_P(PreJumpDoubleIntegratorReachingTask, SLQ) { + constexpr bool display = true; + constexpr auto alg = ddp::Algorithm::SLQ; + // rollout + TimeTriggeredRollout rollout(*ocp.dynamicsPtr, getRolloutSettings(alg)); + // solver + SLQ ddp(getDdpSettings(alg, display), rollout, ocp, *getInitializer()); + ddp.setReferenceManager(referenceManagerPtr); + // run solver + ddp.run(0.0, xInit, 2.0 * tGoal); + PrimalSolution primalSolution; + ddp.getPrimalSolution(2.0 * tGoal, &primalSolution); + // test + test(primalSolution); + printSolution(primalSolution, display); +} + +INSTANTIATE_TEST_CASE_P(PreJumpDoubleIntegratorReachingTaskCase, PreJumpDoubleIntegratorReachingTask, + testing::ValuesIn({DoubleIntegratorReachingTask::PenaltyType::QuadraticPenalty, + DoubleIntegratorReachingTask::PenaltyType::SmoothAbsolutePenalty}), + [](const testing::TestParamInfo<PreJumpDoubleIntegratorReachingTask::ParamType>& info) { + if (info.param == DoubleIntegratorReachingTask::PenaltyType::QuadraticPenalty) { + return std::string("QuadraticPenalty"); + } else if (info.param == DoubleIntegratorReachingTask::PenaltyType::SmoothAbsolutePenalty) { + return std::string("SmoothAbsolutePenalty"); + } + }); + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +/* Reaching task at final time for a problem with horizon tGoal. */ +class FinalDoubleIntegratorReachingTask : public DoubleIntegratorReachingTask, + public testing::Test, + public testing::WithParamInterface<DoubleIntegratorReachingTask::PenaltyType> { + protected: + FinalDoubleIntegratorReachingTask() { + // reference manager + referenceManagerPtr = getReferenceManagerPtr(); + // optimal control problem + ocp.dynamicsPtr = getDynamicsPtr(); + ocp.costPtr->add("cost", getCostPtr()); + ocp.finalEqualityLagrangianPtr->add("goal_reaching", getGoalReachingAugmentedLagrangian(xGoal, GetParam())); + } + + void test(const PrimalSolution& primalSolution) const { + // reaching to the target + EXPECT_TRUE(primalSolution.stateTrajectory_.back().isApprox(xGoal, constraintTolerance)) + << " at time " << primalSolution.timeTrajectory_.back(); + } + + OptimalControlProblem ocp; + std::shared_ptr<ReferenceManager> referenceManagerPtr; +}; + +TEST_P(FinalDoubleIntegratorReachingTask, SLQ) { + constexpr bool display = true; + constexpr auto alg = ddp::Algorithm::SLQ; + // rollout + TimeTriggeredRollout rollout(*ocp.dynamicsPtr, getRolloutSettings(alg)); + // solver + SLQ ddp(getDdpSettings(alg, display), rollout, ocp, *getInitializer()); + ddp.setReferenceManager(referenceManagerPtr); + // run solver + ddp.run(0.0, xInit, tGoal); + PrimalSolution primalSolution; + ddp.getPrimalSolution(tGoal, &primalSolution); + // test + test(primalSolution); + printSolution(primalSolution, display); +} + +INSTANTIATE_TEST_CASE_P(FinalDoubleIntegratorReachingTaskCase, FinalDoubleIntegratorReachingTask, + testing::ValuesIn({DoubleIntegratorReachingTask::PenaltyType::QuadraticPenalty, + DoubleIntegratorReachingTask::PenaltyType::SmoothAbsolutePenalty}), + [](const testing::TestParamInfo<PreJumpDoubleIntegratorReachingTask::ParamType>& info) { + if (info.param == DoubleIntegratorReachingTask::PenaltyType::QuadraticPenalty) { + return std::string("QuadraticPenalty"); + } else if (info.param == DoubleIntegratorReachingTask::PenaltyType::SmoothAbsolutePenalty) { + return std::string("SmoothAbsolutePenalty"); + } + }); + +} // namespace ocs2 diff --git a/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h b/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h new file mode 100644 index 000000000..69a3e6c84 --- /dev/null +++ b/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h @@ -0,0 +1,178 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <ocs2_core/Types.h> +#include <ocs2_core/augmented_lagrangian/AugmentedLagrangian.h> +#include <ocs2_core/constraint/LinearStateConstraint.h> +#include <ocs2_core/constraint/LinearStateInputConstraint.h> +#include <ocs2_core/cost/QuadraticStateInputCost.h> +#include <ocs2_core/dynamics/LinearSystemDynamics.h> +#include <ocs2_core/initialization/DefaultInitializer.h> +#include <ocs2_core/penalties/Penalties.h> +#include <ocs2_oc/synchronized_module/ReferenceManager.h> +#include <ocs2_oc/oc_data/PrimalSolution.h> + +namespace ocs2 { + +class DoubleIntegratorReachingTask { +public: + static constexpr size_t STATE_DIM = 2; + static constexpr size_t INPUT_DIM = 1; + static constexpr scalar_t timeStep = 1e-2; + static constexpr scalar_t minRelCost = 1e-12; // to avoid early termination + static constexpr scalar_t constraintTolerance = 1e-3; + + enum class PenaltyType { + QuadraticPenalty, + SmoothAbsolutePenalty, + }; + + DoubleIntegratorReachingTask() = default; + virtual ~DoubleIntegratorReachingTask() = default; + +protected: + const scalar_t tGoal = 1.0; + const vector_t xInit = vector_t::Zero(STATE_DIM); + const vector_t xGoal = (vector_t(STATE_DIM) << 2.0, 0.0).finished(); + + std::unique_ptr<DefaultInitializer> getInitializer() const { + return std::unique_ptr<DefaultInitializer>(new DefaultInitializer(INPUT_DIM)); + } + + std::shared_ptr<ReferenceManager> getReferenceManagerPtr() const { + const ModeSchedule modeSchedule({tGoal}, {0, 1}); + const TargetTrajectories targetTrajectories({tGoal}, {xGoal}, {vector_t::Zero(INPUT_DIM)}); + return std::make_shared<ReferenceManager>(targetTrajectories, modeSchedule); + } + + std::unique_ptr<StateInputCost> getCostPtr() const { + const matrix_t Q = matrix_t::Zero(STATE_DIM, STATE_DIM); + const matrix_t R = 0.1 * matrix_t::Identity(INPUT_DIM, INPUT_DIM); + return std::unique_ptr<StateInputCost>(new QuadraticStateInputCost(Q, R)); + } + + std::unique_ptr<SystemDynamicsBase> getDynamicsPtr() const { + const matrix_t A = (matrix_t(STATE_DIM, STATE_DIM) << 0.0, 1.0, 0.0, 0.0).finished(); + const matrix_t B = (matrix_t(STATE_DIM, INPUT_DIM) << 0.0, 1.0).finished(); + return std::unique_ptr<LinearSystemDynamics>(new LinearSystemDynamics(A, B)); + } + + std::unique_ptr<ocs2::StateAugmentedLagrangian> getGoalReachingAugmentedLagrangian(const vector_t& xGoal, PenaltyType penaltyType) { + constexpr scalar_t scale = 10.0; + constexpr scalar_t stepSize = 1.0; + + auto goalReachingConstraintPtr = + std::unique_ptr<LinearStateConstraint>(new ocs2::LinearStateConstraint(-xGoal, matrix_t::Identity(xGoal.size(), xGoal.size()))); + + switch (penaltyType) { + case PenaltyType::QuadraticPenalty: { + using penalty_type = augmented::QuadraticPenalty; + penalty_type::Config boundsConfig{scale, stepSize}; + return create(std::move(goalReachingConstraintPtr), penalty_type::create(boundsConfig)); + } + case PenaltyType::SmoothAbsolutePenalty: { + using penalty_type = augmented::SmoothAbsolutePenalty; + penalty_type::Config boundsConfig{scale, 0.1, stepSize}; + return create(std::move(goalReachingConstraintPtr), penalty_type::create(boundsConfig)); + } + default: + throw std::runtime_error("[TestCartpole::getPenalty] undefined penaltyType"); + } + } + + /** This class enforces zero force at the second mode (mode = 1)*/ + class ZeroInputConstraint final : public StateInputConstraint { + public: + ZeroInputConstraint(const ReferenceManager& referenceManager) + : StateInputConstraint(ConstraintOrder::Linear), referenceManagerPtr_(&referenceManager) {} + + ~ZeroInputConstraint() override = default; + ZeroInputConstraint* clone() const override { return new ZeroInputConstraint(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 1; } + + /** Only active after the fist mode */ + bool isActive(scalar_t time) const override { return referenceManagerPtr_->getModeSchedule().modeAtTime(time) > 0 ? true : false; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { return u; } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation&) const override { + VectorFunctionLinearApproximation approx; + approx.f = u; + approx.dfdx.setZero(getNumConstraints(t), x.size()); + approx.dfdu.setIdentity(getNumConstraints(t), u.size()); + return approx; + } + + private: + ZeroInputConstraint(const ZeroInputConstraint&) = default; + const ReferenceManager* referenceManagerPtr_; + }; + + + /* + * printout trajectory. Use the following commands for plotting in MATLAB: + * subplot(2, 1, 1); plot(timeTrajectory, stateTrajectory); + * xlabel("time [sec]"); legend("pos", "vel"); + * subplot(2, 1, 2); plot(timeTrajectory, inputTrajectory); + * xlabel("time [sec]"); legend("force"); + */ + void printSolution(const PrimalSolution& primalSolution, bool display) const { + if (display) { + std::cerr << "\n"; + // time + std::cerr << "timeTrajectory = ["; + for (const auto& t : primalSolution.timeTrajectory_) { + std::cerr << t << "; "; + } + std::cerr << "];\n"; + // state + std::cerr << "stateTrajectory = ["; + for (const auto& x : primalSolution.stateTrajectory_) { + std::cerr << x.transpose() << "; "; + } + std::cerr << "];\n"; + // input + std::cerr << "inputTrajectory = ["; + for (const auto& u : primalSolution.inputTrajectory_) { + std::cerr << u.transpose() << "; "; + } + std::cerr << "];\n"; + } + } +}; + +constexpr size_t DoubleIntegratorReachingTask::STATE_DIM; +constexpr size_t DoubleIntegratorReachingTask::INPUT_DIM; +constexpr scalar_t DoubleIntegratorReachingTask::timeStep; +constexpr scalar_t DoubleIntegratorReachingTask::minRelCost; +constexpr scalar_t DoubleIntegratorReachingTask::constraintTolerance; + +} // namespace ocs2 From d42d5f01025a99a22cc6355340c7eddde59a209d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 26 Aug 2022 16:28:28 +0200 Subject: [PATCH 320/512] doc update --- ocs2_ddp/test/testReachingTask.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_ddp/test/testReachingTask.cpp b/ocs2_ddp/test/testReachingTask.cpp index f01317860..63ea4ee9d 100644 --- a/ocs2_ddp/test/testReachingTask.cpp +++ b/ocs2_ddp/test/testReachingTask.cpp @@ -73,7 +73,7 @@ ddp::Settings getDdpSettings(ddp::Algorithm alg, bool display) { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -/* Reaching task as event time at tGoal for a problem with horizon 2*tGoal. The solution input should be zero in [tGoal, 2*tGoal]. */ +/* Reaching task at event time (tGoal) for a problem with horizon 2*tGoal. The solution input should be zero in [tGoal, 2*tGoal]. */ class PreJumpDoubleIntegratorReachingTask : public DoubleIntegratorReachingTask, public testing::Test, public testing::WithParamInterface<DoubleIntegratorReachingTask::PenaltyType> { From 1297e19b862935b263c032647c8e950b824a7708 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 26 Aug 2022 17:12:37 +0200 Subject: [PATCH 321/512] update --- ocs2_ddp/test/testReachingTask.cpp | 2 +- .../test/DoubleIntegratorReachingTask.h | 26 ++++++++----------- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/ocs2_ddp/test/testReachingTask.cpp b/ocs2_ddp/test/testReachingTask.cpp index 63ea4ee9d..486b83063 100644 --- a/ocs2_ddp/test/testReachingTask.cpp +++ b/ocs2_ddp/test/testReachingTask.cpp @@ -84,7 +84,7 @@ class PreJumpDoubleIntegratorReachingTask : public DoubleIntegratorReachingTask, // optimal control problem ocp.dynamicsPtr = getDynamicsPtr(); ocp.costPtr->add("cost", getCostPtr()); - ocp.equalityConstraintPtr->add("zero_force", std::unique_ptr<ZeroInputConstraint>(new ZeroInputConstraint(*referenceManagerPtr))); + ocp.equalityConstraintPtr->add("zero_force", std::make_unique<ZeroInputConstraint>(*referenceManagerPtr)); ocp.preJumpEqualityLagrangianPtr->add("goal_reaching", getGoalReachingAugmentedLagrangian(xGoal, GetParam())); } diff --git a/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h b/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h index 69a3e6c84..1101b8471 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h +++ b/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h @@ -35,13 +35,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/dynamics/LinearSystemDynamics.h> #include <ocs2_core/initialization/DefaultInitializer.h> #include <ocs2_core/penalties/Penalties.h> -#include <ocs2_oc/synchronized_module/ReferenceManager.h> #include <ocs2_oc/oc_data/PrimalSolution.h> +#include <ocs2_oc/synchronized_module/ReferenceManager.h> namespace ocs2 { class DoubleIntegratorReachingTask { -public: + public: static constexpr size_t STATE_DIM = 2; static constexpr size_t INPUT_DIM = 1; static constexpr scalar_t timeStep = 1e-2; @@ -56,14 +56,12 @@ class DoubleIntegratorReachingTask { DoubleIntegratorReachingTask() = default; virtual ~DoubleIntegratorReachingTask() = default; -protected: + protected: const scalar_t tGoal = 1.0; const vector_t xInit = vector_t::Zero(STATE_DIM); const vector_t xGoal = (vector_t(STATE_DIM) << 2.0, 0.0).finished(); - std::unique_ptr<DefaultInitializer> getInitializer() const { - return std::unique_ptr<DefaultInitializer>(new DefaultInitializer(INPUT_DIM)); - } + std::unique_ptr<DefaultInitializer> getInitializer() const { return std::make_unique<DefaultInitializer>(INPUT_DIM); } std::shared_ptr<ReferenceManager> getReferenceManagerPtr() const { const ModeSchedule modeSchedule({tGoal}, {0, 1}); @@ -72,23 +70,22 @@ class DoubleIntegratorReachingTask { } std::unique_ptr<StateInputCost> getCostPtr() const { - const matrix_t Q = matrix_t::Zero(STATE_DIM, STATE_DIM); - const matrix_t R = 0.1 * matrix_t::Identity(INPUT_DIM, INPUT_DIM); - return std::unique_ptr<StateInputCost>(new QuadraticStateInputCost(Q, R)); + matrix_t Q = matrix_t::Zero(STATE_DIM, STATE_DIM); + matrix_t R = 0.1 * matrix_t::Identity(INPUT_DIM, INPUT_DIM); + return std::make_unique<QuadraticStateInputCost>(std::move(Q), std::move(R)); } std::unique_ptr<SystemDynamicsBase> getDynamicsPtr() const { - const matrix_t A = (matrix_t(STATE_DIM, STATE_DIM) << 0.0, 1.0, 0.0, 0.0).finished(); - const matrix_t B = (matrix_t(STATE_DIM, INPUT_DIM) << 0.0, 1.0).finished(); - return std::unique_ptr<LinearSystemDynamics>(new LinearSystemDynamics(A, B)); + matrix_t A = (matrix_t(STATE_DIM, STATE_DIM) << 0.0, 1.0, 0.0, 0.0).finished(); + matrix_t B = (matrix_t(STATE_DIM, INPUT_DIM) << 0.0, 1.0).finished(); + return std::make_unique<LinearSystemDynamics>(std::move(A), std::move(B)); } std::unique_ptr<ocs2::StateAugmentedLagrangian> getGoalReachingAugmentedLagrangian(const vector_t& xGoal, PenaltyType penaltyType) { constexpr scalar_t scale = 10.0; constexpr scalar_t stepSize = 1.0; - auto goalReachingConstraintPtr = - std::unique_ptr<LinearStateConstraint>(new ocs2::LinearStateConstraint(-xGoal, matrix_t::Identity(xGoal.size(), xGoal.size()))); + auto goalReachingConstraintPtr = std::make_unique<LinearStateConstraint>(-xGoal, matrix_t::Identity(xGoal.size(), xGoal.size())); switch (penaltyType) { case PenaltyType::QuadraticPenalty: { @@ -136,7 +133,6 @@ class DoubleIntegratorReachingTask { const ReferenceManager* referenceManagerPtr_; }; - /* * printout trajectory. Use the following commands for plotting in MATLAB: * subplot(2, 1, 1); plot(timeTrajectory, stateTrajectory); From 152043535eae5f6d79918cd5a5108e9f4749726a Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 29 Aug 2022 11:10:21 +0200 Subject: [PATCH 322/512] clang format --- .../include/ocs2_oc/test/DoubleIntegratorReachingTask.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h b/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h index 69a3e6c84..1d438788f 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h +++ b/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h @@ -35,13 +35,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/dynamics/LinearSystemDynamics.h> #include <ocs2_core/initialization/DefaultInitializer.h> #include <ocs2_core/penalties/Penalties.h> -#include <ocs2_oc/synchronized_module/ReferenceManager.h> #include <ocs2_oc/oc_data/PrimalSolution.h> +#include <ocs2_oc/synchronized_module/ReferenceManager.h> namespace ocs2 { class DoubleIntegratorReachingTask { -public: + public: static constexpr size_t STATE_DIM = 2; static constexpr size_t INPUT_DIM = 1; static constexpr scalar_t timeStep = 1e-2; @@ -56,7 +56,7 @@ class DoubleIntegratorReachingTask { DoubleIntegratorReachingTask() = default; virtual ~DoubleIntegratorReachingTask() = default; -protected: + protected: const scalar_t tGoal = 1.0; const vector_t xInit = vector_t::Zero(STATE_DIM); const vector_t xGoal = (vector_t(STATE_DIM) << 2.0, 0.0).finished(); @@ -136,7 +136,6 @@ class DoubleIntegratorReachingTask { const ReferenceManager* referenceManagerPtr_; }; - /* * printout trajectory. Use the following commands for plotting in MATLAB: * subplot(2, 1, 1); plot(timeTrajectory, stateTrajectory); From 1a40d2dff754de0e7d0e2c431d2be30195f57600 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 29 Aug 2022 11:10:52 +0200 Subject: [PATCH 323/512] temp fix --- ocs2_oc/src/rollout/RolloutBase.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ocs2_oc/src/rollout/RolloutBase.cpp b/ocs2_oc/src/rollout/RolloutBase.cpp index bd79b6af8..e7cb1d3e2 100644 --- a/ocs2_oc/src/rollout/RolloutBase.cpp +++ b/ocs2_oc/src/rollout/RolloutBase.cpp @@ -60,10 +60,8 @@ std::vector<std::pair<scalar_t, scalar_t>> RolloutBase::findActiveModesTimeInter timeIntervalArray[i] = std::make_pair(beginTime, endTime); // adjusting the start time to correct for subsystem recognition - if (i > 0) { - constexpr auto eps = numeric_traits::weakEpsilon<scalar_t>(); - timeIntervalArray[i].first = std::min(beginTime + eps, endTime); - } + constexpr auto eps = numeric_traits::weakEpsilon<scalar_t>(); + timeIntervalArray[i].first = std::min(beginTime + eps, endTime); } // end of for loop return timeIntervalArray; From d92d097369ffbe37a90f7bbb5516dc412f8f9604 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 29 Aug 2022 17:11:48 +0200 Subject: [PATCH 324/512] review comments --- .../constraint/LoopshapingConstraint.cpp | 2 +- .../src/loopshaping/cost/LoopshapingCost.cpp | 2 +- .../LoopshapingSoftConstraint.cpp | 2 +- .../test/integration/IntegrationTest.cpp | 2 +- .../integration/testSensitivityIntegrator.cpp | 2 +- .../loopshaping/testQuadraticConstraint.h | 2 +- .../test/testDummyPyBindings.cpp | 30 ++++++++++++++++++- .../ocs2_cartpole/test/testCartpole.cpp | 4 +-- .../DoubleIntegratorNoRosIntegrationTest.cpp | 2 +- 9 files changed, 38 insertions(+), 10 deletions(-) diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp index 85dab7578..e99e8dc48 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp @@ -41,7 +41,7 @@ namespace LoopshapingConstraint { /******************************************************************************************************/ std::unique_ptr<StateConstraintCollection> create(const StateConstraintCollection& systemConstraint, std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { - return std::make_unique<LoopshapingStateConstraint>(systemConstraint, loopshapingDefinition); + return std::make_unique<LoopshapingStateConstraint>(systemConstraint, std::move(loopshapingDefinition)); } /******************************************************************************************************/ diff --git a/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp b/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp index 857e9d706..df5e301b7 100644 --- a/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp +++ b/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp @@ -41,7 +41,7 @@ namespace LoopshapingCost { /******************************************************************************************************/ std::unique_ptr<StateCostCollection> create(const StateCostCollection& systemCost, std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { - return std::make_unique<LoopshapingStateCost>(systemCost, loopshapingDefinition); + return std::make_unique<LoopshapingStateCost>(systemCost, std::move(loopshapingDefinition)); } /******************************************************************************************************/ diff --git a/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp b/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp index 5d405d34f..6755ad0f0 100644 --- a/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp +++ b/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp @@ -42,7 +42,7 @@ namespace LoopshapingSoftConstraint { std::unique_ptr<StateCostCollection> create(const StateCostCollection& systemSoftConstraint, std::shared_ptr<LoopshapingDefinition> loopshapingDefinition) { // State-only soft constraint wrapper is identical to LoopshapingStateCost wrapper. - return std::make_unique<LoopshapingStateCost>(systemSoftConstraint, loopshapingDefinition); + return std::make_unique<LoopshapingStateCost>(systemSoftConstraint, std::move(loopshapingDefinition)); } /******************************************************************************************************/ diff --git a/ocs2_core/test/integration/IntegrationTest.cpp b/ocs2_core/test/integration/IntegrationTest.cpp index 8c845aed7..9a111e927 100644 --- a/ocs2_core/test/integration/IntegrationTest.cpp +++ b/ocs2_core/test/integration/IntegrationTest.cpp @@ -44,7 +44,7 @@ std::unique_ptr<OdeBase> getSystem(LinearController& controller) { matrix_t B(2, 1); B << 1, 0; - auto sys = std::make_unique<LinearSystemDynamics>(A, B); + auto sys = std::make_unique<LinearSystemDynamics>(std::move(A), std::move(B)); sys->setController(&controller); return std::move(sys); diff --git a/ocs2_core/test/integration/testSensitivityIntegrator.cpp b/ocs2_core/test/integration/testSensitivityIntegrator.cpp index 162dd5093..974b663d0 100644 --- a/ocs2_core/test/integration/testSensitivityIntegrator.cpp +++ b/ocs2_core/test/integration/testSensitivityIntegrator.cpp @@ -43,7 +43,7 @@ std::unique_ptr<ocs2::LinearSystemDynamics> getSystem() { 1, 0; // clang-format on ocs2::matrix_t B(2, 1); B << 1, 0; - return std::make_unique<ocs2::LinearSystemDynamics>(A, B); + return std::make_unique<ocs2::LinearSystemDynamics>(std::move(A), std::move(B)); } } // namespace diff --git a/ocs2_core/test/loopshaping/testQuadraticConstraint.h b/ocs2_core/test/loopshaping/testQuadraticConstraint.h index f60726a39..b5c7237b7 100644 --- a/ocs2_core/test/loopshaping/testQuadraticConstraint.h +++ b/ocs2_core/test/loopshaping/testQuadraticConstraint.h @@ -44,7 +44,7 @@ class TestQuadraticStateInputConstraint : public StateInputConstraint { P.setRandom(inputDim, stateDim); Q = (0.5 * Q.transpose() + 0.5 * Q).eval(); R = (0.5 * R.transpose() + 0.5 * R).eval(); - return std::make_unique<TestQuadraticStateInputConstraint>(Q, R, P); + return std::make_unique<TestQuadraticStateInputConstraint>(std::move(Q), std::move(R), std::move(P)); } ~TestQuadraticStateInputConstraint() override = default; diff --git a/ocs2_python_interface/test/testDummyPyBindings.cpp b/ocs2_python_interface/test/testDummyPyBindings.cpp index e39954964..836a93b8b 100644 --- a/ocs2_python_interface/test/testDummyPyBindings.cpp +++ b/ocs2_python_interface/test/testDummyPyBindings.cpp @@ -1,3 +1,31 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ #include <gtest/gtest.h> @@ -41,7 +69,7 @@ class DummyInterface final : public RobotInterface { mpc::Settings mpcSettings; ddp::Settings ddpSettings; ddpSettings.algorithm_ = ddp::Algorithm::SLQ; - return std::make_unique<GaussNewtonDDP_MPC>(mpcSettings, ddpSettings, *rolloutPtr_, problem_, *initializerPtr_); + return std::make_unique<GaussNewtonDDP_MPC>(std::move(mpcSettings), std::move(ddpSettings), *rolloutPtr_, problem_, *initializerPtr_); } const OptimalControlProblem& getOptimalControlProblem() const override { return problem_; } diff --git a/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp b/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp index ef27b41d9..88ed93a55 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp @@ -98,12 +98,12 @@ class TestCartpole : public testing::TestWithParam<std::tuple<ddp::Algorithm, Pe switch (algorithm) { case ddp::Algorithm::SLQ: - return std::make_unique<SLQ>(ddpSettings, cartPoleInterfacePtr->getRollout(), + return std::make_unique<SLQ>(std::move(ddpSettings), cartPoleInterfacePtr->getRollout(), createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), cartPoleInterfacePtr->getInitializer()); case ddp::Algorithm::ILQR: - return std::make_unique<ILQR>(ddpSettings, cartPoleInterfacePtr->getRollout(), + return std::make_unique<ILQR>(std::move(ddpSettings), cartPoleInterfacePtr->getRollout(), createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), cartPoleInterfacePtr->getInitializer()); diff --git a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp index c5d577fc8..79401cac7 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp @@ -66,7 +66,7 @@ class DoubleIntegratorIntegrationTest : public testing::Test { ddpSettings.maxNumIterations_ = 5; } - auto mpcPtr = std::make_unique<GaussNewtonDDP_MPC>(mpcSettings, ddpSettings, interface.getRollout(), + auto mpcPtr = std::make_unique<GaussNewtonDDP_MPC>(std::move(mpcSettings), std::move(ddpSettings), interface.getRollout(), interface.getOptimalControlProblem(), interface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(interface.getReferenceManagerPtr()); From dc3da034f6de27794ecb0d8cbb42c8e973a90845 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 30 Aug 2022 10:14:50 +0200 Subject: [PATCH 325/512] bug in a static variable --- ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp b/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp index ca641f534..9e48d0f04 100644 --- a/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp +++ b/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp @@ -102,6 +102,7 @@ constexpr bool TestBilinearInterpolation::verbose; constexpr size_t TestBilinearInterpolation::numSamples; constexpr size_t TestBilinearInterpolation::variableDim; constexpr size_t TestBilinearInterpolation::parameterDim; +constexpr scalar_t TestBilinearInterpolation::precision; constexpr scalar_t TestBilinearInterpolation::resolution; TEST_F(TestBilinearInterpolation, testAdFunction) { From 090440b3d482bfd31ec48c185ab0c07ab7bb557e Mon Sep 17 00:00:00 2001 From: Mayank Mittal <mittalma@leggedrobotics.com> Date: Sun, 4 Sep 2022 15:26:39 +0200 Subject: [PATCH 326/512] fixes spelling in distance-transform function --- .../distance_transform/ComputeDistanceTransform.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h index aa3ccfbd3..4c419d822 100644 --- a/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h +++ b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h @@ -37,7 +37,7 @@ namespace ocs2 { * Computes one-dimensional distance transform for a sampled function based on lower envelope of parabolas method introduced by: * P. F. Felzenszwalb and D. P. Huttenlocher. "Distance transforms of sampled functions." (2012). * - * @param numSamples: The size of the smapled function. + * @param numSamples: The size of the sampled function. * @param getValue: Lambda function to get value from with signature: ScalarType(size_t index). * @param setValue: Lambda function to set value to with signature: void(size_t index, ScalarType value). * @param start: The start index for iteration over. @@ -49,7 +49,7 @@ namespace ocs2 { * * @tparam GetValFunc: Template typename for inferring lambda expression with signature Scalar(size_t). * @tparam SetValFunc: Template typename for inferring lambda expression with signature void(size_t, Scalar). - * @tpapram Scalar: The Scalar type + * @tparam Scalar: The Scalar type */ template <typename GetValFunc, typename SetValFunc, typename Scalar = float> void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, size_t start, size_t end, @@ -59,7 +59,7 @@ void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFu * Computes one-dimensional distance transform for a sampled function based on lower envelope of parabolas method introduced by: * P. F. Felzenszwalb and D. P. Huttenlocher. "Distance transforms of sampled functions." (2012). * - * @param numSamples: The size of the smapled function. + * @param numSamples: The size of the sampled function. * @param getValue: Lambda function to get value from with signature: ScalarType(size_t index). * @param setValue: Lambda function to set value to with signature: void(size_t index, ScalarType value). * @param setImageIndex: Lambda function to set mirror index with signature: void(size_t index, size_t mirrorIndex). @@ -73,7 +73,7 @@ void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFu * @tparam GetValFunc: Template typename for inferring lambda expression with signature Scalar(size_t). * @tparam SetValFunc: Template typename for inferring lambda expression with signature void(size_t, Scalar). * @tparam SetImageIndexFunc: Template typename for inferring lambda expression with signature void(size_t, size_t). - * @tpapram Scalar: The Scalar type + * @tparam Scalar: The Scalar type */ template <typename GetValFunc, typename SetValFunc, typename SetImageIndexFunc, typename Scalar = float> void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, SetImageIndexFunc&& setImageIndex, From 7bbb7ab5e6685270f03f52472774b2878d5b993e Mon Sep 17 00:00:00 2001 From: Mayank Mittal <mittalma@leggedrobotics.com> Date: Sun, 4 Sep 2022 15:27:12 +0200 Subject: [PATCH 327/512] splits bi-linear interpolation function to implementation file --- .../interpolation/BilinearInterpolation.h | 65 ++++++-------- .../implementation/BilinearInterpolation.h | 84 +++++++++++++++++++ 2 files changed, 109 insertions(+), 40 deletions(-) create mode 100644 ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/BilinearInterpolation.h diff --git a/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h b/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h index 19235f32c..27afa995b 100644 --- a/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h +++ b/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h @@ -30,57 +30,42 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include <array> -#include <utility> #include <ocs2_core/Types.h> namespace ocs2 { namespace bilinear_interpolation { -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ +/** + * Compute the value of a function at a queried position using bi-linear interpolation on a 2D-grid. + * + * @param resolution The resolution of the grid. + * @param referenceCorner The reference position on the 2-D grid closest to the point. + * @param cornerValues The values around the reference corner, in the order: (0, 0), (1, 0), (0, 1), (1, 1). + * @param position The queried position. + * @tparam Scalar : The Scalar type. + * @return Scalar : The interpolated function's value at the queried position. + */ template <typename Scalar> Scalar getValue(Scalar resolution, const Eigen::Matrix<Scalar, 2, 1>& referenceCorner, const std::array<Scalar, 4>& cornerValues, - const Eigen::Matrix<Scalar, 2, 1>& position) { - // auxiliary variables - const Scalar r_inv = 1.0 / resolution; - std::array<Scalar, 4> v; - v[0] = (position.x() - referenceCorner.x()) * r_inv; - v[1] = (position.y() - referenceCorner.y()) * r_inv; - v[2] = 1 - v[1]; - v[3] = 1 - v[0]; - - return cornerValues[1] * v[0] * v[2] + cornerValues[0] * v[3] * v[2] + cornerValues[2] * v[3] * v[1] + cornerValues[3] * v[0] * v[1]; -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ + const Eigen::Matrix<Scalar, 2, 1>& position); + +/** + * Computes a first-order approximation of the function at a queried position using bi-linear interpolation on a 2D-grid. + * + * @param resolution The resolution of the grid. + * @param referenceCorner The reference position on the 2-D grid closest to the point. + * @param cornerValues The values around the reference corner, in the order: (0, 0), (1, 0), (0, 1), (1, 1). + * @param position The queried position. + * @tparam Scalar : The Scalar type. + * @return std::pair<Scalar, Eigen::Matrix<Scalar, 2, 1>> : A tuple containing the value and jacobian. + */ template <typename Scalar> std::pair<Scalar, Eigen::Matrix<Scalar, 2, 1>> getLinearApproximation(Scalar resolution, const Eigen::Matrix<Scalar, 2, 1>& referenceCorner, const std::array<Scalar, 4>& cornerValues, - const Eigen::Matrix<Scalar, 2, 1>& position) { - // auxiliary variables - const Scalar r_inv = 1.0 / resolution; - std::array<Scalar, 6> v; - v[0] = (position.y() - referenceCorner.y()) * r_inv; - v[1] = (position.x() - referenceCorner.x()) * r_inv; - v[2] = 1.0 - v[0]; - v[3] = 1.0 - v[1]; - v[4] = cornerValues[2] * v[3]; - v[5] = cornerValues[3] * v[1]; - v[3] = cornerValues[0] * v[3]; - v[1] = cornerValues[1] * v[1]; - - const Scalar value = v[1] * v[2] + v[3] * v[2] + v[4] * v[0] + v[5] * v[0]; - - Eigen::Matrix<Scalar, 2, 1> gradient; - gradient.x() = (v[2] * (cornerValues[1] - cornerValues[0]) + v[0] * (cornerValues[3] - cornerValues[2])) * r_inv; - gradient.y() = (v[4] + v[5] - (v[3] + v[1])) * r_inv; - - return {value, gradient}; -} + const Eigen::Matrix<Scalar, 2, 1>& position); } // namespace bilinear_interpolation } // namespace ocs2 + +#include "implementation/BilinearInterpolation.h" diff --git a/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/BilinearInterpolation.h b/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/BilinearInterpolation.h new file mode 100644 index 000000000..194fb73af --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/BilinearInterpolation.h @@ -0,0 +1,84 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <utility> + +namespace ocs2 { +namespace bilinear_interpolation { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <typename Scalar> +Scalar getValue(Scalar resolution, const Eigen::Matrix<Scalar, 2, 1>& referenceCorner, const std::array<Scalar, 4>& cornerValues, + const Eigen::Matrix<Scalar, 2, 1>& position) { + // auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array<Scalar, 4> v; + v[0] = (position.x() - referenceCorner.x()) * r_inv; + v[1] = (position.y() - referenceCorner.y()) * r_inv; + v[2] = 1 - v[0]; + v[3] = 1 - v[1]; + // (1 - x)(1 - y) f_00 + x (1 - y) f_10 + (1 - x) y f_01 + x y f_11 + return cornerValues[0] * v[2] * v[3] + cornerValues[1] * v[0] * v[3] + cornerValues[2] * v[2] * v[1] + cornerValues[3] * v[0] * v[1]; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <typename Scalar> +std::pair<Scalar, Eigen::Matrix<Scalar, 2, 1>> getLinearApproximation(Scalar resolution, const Eigen::Matrix<Scalar, 2, 1>& referenceCorner, + const std::array<Scalar, 4>& cornerValues, + const Eigen::Matrix<Scalar, 2, 1>& position) { + // auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array<Scalar, 6> v; + v[0] = (position.y() - referenceCorner.y()) * r_inv; + v[1] = (position.x() - referenceCorner.x()) * r_inv; + v[2] = 1.0 - v[0]; + v[3] = 1.0 - v[1]; + v[4] = cornerValues[2] * v[3]; // (1 - x) f_01 + v[5] = cornerValues[3] * v[1]; // x f_11 + v[3] = cornerValues[0] * v[3]; // (1 - x) f_00 + v[1] = cornerValues[1] * v[1]; // x f_10 + + // x ( 1 - y) f_10 + (1 - x) (1 - y) f_00 + (1 - x) y f_01 + x y f_11 + const Scalar value = v[1] * v[2] + v[3] * v[2] + v[4] * v[0] + v[5] * v[0]; + + Eigen::Matrix<Scalar, 2, 1> gradient; + // (1 - y) (f_10 - f_00) + y (f_11 - f_01) + gradient.x() = (v[2] * (cornerValues[1] - cornerValues[0]) + v[0] * (cornerValues[3] - cornerValues[2])) * r_inv; + // (1 - x) (f_01 - f_00) + x (f_11 - f_10) + gradient.y() = (v[4] + v[5] - (v[3] + v[1])) * r_inv; + + return {value, gradient}; +} + +} // namespace bilinear_interpolation +} // namespace ocs2 From f464cdda9decc6067fb9ac79bd8c4d79a23e0577 Mon Sep 17 00:00:00 2001 From: Mayank Mittal <mittalma@leggedrobotics.com> Date: Sun, 4 Sep 2022 15:27:37 +0200 Subject: [PATCH 328/512] adds tri-linear interpolation function --- ocs2_perceptive/CMakeLists.txt | 9 + .../interpolation/TrilinearInterpolation.h | 73 +++++++ .../implementation/TrilinearInterpolation.h | 98 ++++++++++ ocs2_perceptive/src/lintTarget.cpp | 1 + .../testTrilinearInterpolation.cpp | 180 ++++++++++++++++++ 5 files changed, 361 insertions(+) create mode 100644 ocs2_perceptive/include/ocs2_perceptive/interpolation/TrilinearInterpolation.h create mode 100644 ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/TrilinearInterpolation.h create mode 100644 ocs2_perceptive/test/interpolation/testTrilinearInterpolation.cpp diff --git a/ocs2_perceptive/CMakeLists.txt b/ocs2_perceptive/CMakeLists.txt index 43d39c8a9..509406a35 100644 --- a/ocs2_perceptive/CMakeLists.txt +++ b/ocs2_perceptive/CMakeLists.txt @@ -105,3 +105,12 @@ target_link_libraries(test_bilinear_interpolation gtest_main ) +catkin_add_gtest(test_trilinear_interpolation + test/interpolation/testTrilinearInterpolation.cpp +) +target_link_libraries(test_trilinear_interpolation + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + gtest_main +) diff --git a/ocs2_perceptive/include/ocs2_perceptive/interpolation/TrilinearInterpolation.h b/ocs2_perceptive/include/ocs2_perceptive/interpolation/TrilinearInterpolation.h new file mode 100644 index 000000000..0d468fdbb --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/interpolation/TrilinearInterpolation.h @@ -0,0 +1,73 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <array> + +#include <ocs2_core/Types.h> + +namespace ocs2 { +namespace trilinear_interpolation { + +/** + * Compute the value of a function at a queried position using tri-linear interpolation on a 3D-grid. + * + * @param resolution The resolution of the grid. + * @param referenceCorner The reference position on the 3-D grid closest to the point. + * @param cornerValues The values around the reference corner, in the order: + * (0, 0, 0), (1, 0, 0), (0, 1, 0), (1, 1, 0), (0, 0, 1), (1, 0, 1), (0, 1, 1), (1, 1, 1). + * @param position The queried position. + * @tparam Scalar : The Scalar type. + * @return Scalar : The interpolated function's value at the queried position. + */ +template <typename Scalar> +Scalar getValue(Scalar resolution, const Eigen::Matrix<Scalar, 3, 1>& referenceCorner, const std::array<Scalar, 8>& cornerValues, + const Eigen::Matrix<Scalar, 3, 1>& position); + +/** + * Computes a first-order approximation of the function at a queried position using tri-linear interpolation on a 3D-grid. + * + * @param resolution The resolution of the grid. + * @param referenceCorner The reference position on the 3-D grid closest to the point. + * @param cornerValues The values around the reference corner, in the order: + * (0, 0, 0), (1, 0, 0), (0, 1, 0), (1, 1, 0), (0, 0, 1), (1, 0, 1), (0, 1, 1), (1, 1, 1). + * @param position The queried position. + * @tparam Scalar : The Scalar type. + * @return std::pair<Scalar, Eigen::Matrix<Scalar, 3, 1>> : A tuple containing the value and jacobian. + */ +template <typename Scalar> +std::pair<Scalar, Eigen::Matrix<Scalar, 3, 1>> getLinearApproximation(Scalar resolution, const Eigen::Matrix<Scalar, 3, 1>& referenceCorner, + const std::array<Scalar, 8>& cornerValues, + const Eigen::Matrix<Scalar, 3, 1>& position); + +} // namespace trilinear_interpolation +} // namespace ocs2 + +#include "implementation/TrilinearInterpolation.h" diff --git a/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/TrilinearInterpolation.h b/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/TrilinearInterpolation.h new file mode 100644 index 000000000..17df07b2d --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/TrilinearInterpolation.h @@ -0,0 +1,98 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <utility> + +namespace ocs2 { +namespace trilinear_interpolation { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <typename Scalar> +Scalar getValue(Scalar resolution, const Eigen::Matrix<Scalar, 3, 1>& referenceCorner, const std::array<Scalar, 8>& cornerValues, + const Eigen::Matrix<Scalar, 3, 1>& position) { + // auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array<Scalar, 10> v; + v[0] = (position.x() - referenceCorner.x()) * r_inv; + v[1] = (position.y() - referenceCorner.y()) * r_inv; + v[2] = (position.z() - referenceCorner.z()) * r_inv; + v[3] = 1 - v[0]; + v[4] = 1 - v[1]; + v[5] = 1 - v[2]; + v[6] = v[3] * cornerValues[0] + v[0] * cornerValues[1]; // f_00 = (1 - x) f_000 + x f_100 + v[7] = v[3] * cornerValues[2] + v[0] * cornerValues[3]; // f_10 = (1 - x) f_010 + x f_110 + v[8] = v[3] * cornerValues[4] + v[0] * cornerValues[5]; // f_01 = (1 - x) f_001 + x f_101 + v[9] = v[3] * cornerValues[6] + v[0] * cornerValues[7]; // f_11 = (1 - x) f_011 + x f_111 + // (1 - z) ((1 - y) f_00 + y f_10) + z ((1 - y) f_01 + y f_11) + return v[5] * (v[4] * v[6] + v[1] * v[7]) + v[2] * (v[4] * v[8] + v[1] * v[9]); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template <typename Scalar> +std::pair<Scalar, Eigen::Matrix<Scalar, 3, 1>> getLinearApproximation(Scalar resolution, const Eigen::Matrix<Scalar, 3, 1>& referenceCorner, + const std::array<Scalar, 8>& cornerValues, + const Eigen::Matrix<Scalar, 3, 1>& position) { + /// auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array<Scalar, 12> v; + v[0] = (position.x() - referenceCorner.x()) * r_inv; + v[1] = (position.y() - referenceCorner.y()) * r_inv; + v[2] = (position.z() - referenceCorner.z()) * r_inv; + v[3] = 1 - v[0]; + v[4] = 1 - v[1]; + v[5] = 1 - v[2]; + v[6] = v[3] * cornerValues[0] + v[0] * cornerValues[1]; // f_00 = (1 - x) f_000 + x f_100 + v[7] = v[3] * cornerValues[2] + v[0] * cornerValues[3]; // f_10 = (1 - x) f_010 + x f_110 + v[8] = v[3] * cornerValues[4] + v[0] * cornerValues[5]; // f_01 = (1 - x) f_001 + x f_101 + v[9] = v[3] * cornerValues[6] + v[0] * cornerValues[7]; // f_11 = (1 - x) f_011 + x f_111 + v[10] = v[4] * v[6] + v[1] * v[7]; // f_0 = (1 - y) f_00 + y f_10 + v[11] = v[4] * v[8] + v[1] * v[9]; // f_1 = (1 - y) f_01 + y f_11 + + // f = (1 - z) f_0 + z f_1 + const Scalar value = v[5] * v[10] + v[2] * v[11]; + + Eigen::Matrix<Scalar, 3, 1> gradient; + // df_z = f_11 - f_10 + gradient.z() = (v[11] - v[10]) * r_inv; + // df_y = (1 - z) (f_10 - f00) - z (f_11 - f_01) + gradient.y() = (v[5] * (v[7] - v[6]) + v[2] * (v[9] - v[8])) * r_inv; + // df_x = (1 - z) (1 - y) (f_100 - f_000) + (1 - z) y (f_110 - f_010) + z (1 - y) (f_101 - f_001) + z y (f_111 - f_011) + gradient.x() = (v[5] * v[4] * (cornerValues[1] - cornerValues[0]) + v[5] * v[1] * (cornerValues[3] - cornerValues[2]) + + v[2] * v[4] * (cornerValues[5] - cornerValues[4]) + v[2] * v[1] * (cornerValues[7] - cornerValues[6])) * + r_inv; + + return {value, gradient}; +} + +} // namespace trilinear_interpolation +} // namespace ocs2 diff --git a/ocs2_perceptive/src/lintTarget.cpp b/ocs2_perceptive/src/lintTarget.cpp index d142670b1..e78b475d7 100644 --- a/ocs2_perceptive/src/lintTarget.cpp +++ b/ocs2_perceptive/src/lintTarget.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h> #include <ocs2_perceptive/interpolation/BilinearInterpolation.h> +#include <ocs2_perceptive/interpolation/TrilinearInterpolation.h> // dummy target for clang toolchain int main() { diff --git a/ocs2_perceptive/test/interpolation/testTrilinearInterpolation.cpp b/ocs2_perceptive/test/interpolation/testTrilinearInterpolation.cpp new file mode 100644 index 000000000..88e832b23 --- /dev/null +++ b/ocs2_perceptive/test/interpolation/testTrilinearInterpolation.cpp @@ -0,0 +1,180 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <array> +#include <cmath> +#include <utility> + +#include <gtest/gtest.h> + +#include <ocs2_core/automatic_differentiation/CppAdInterface.h> +#include <ocs2_core/misc/Numerics.h> + +#include "ocs2_perceptive/interpolation/TrilinearInterpolation.h" + +namespace ocs2 { +namespace trilinear_interpolation { + +class TestTrilinearInterpolation : public ::testing::Test { + protected: + using array8_t = std::array<scalar_t, 8>; + using vector3_t = Eigen::Matrix<scalar_t, 3, 1>; + + TestTrilinearInterpolation() { + auto adTrilinearInterpolation = [](const ad_vector_t& x, const ad_vector_t& p, ad_vector_t& out) { + const ad_scalar_t resolution = p(0); + const ad_vector_t referenceCorner = (ad_vector_t(3) << p(1), p(2), p(3)).finished(); + const std::array<ad_scalar_t, 8> cornerValues = {p(4), p(5), p(6), p(7), p(8), p(9), p(10), p(11)}; + + const ad_vector_t positionRed = (x.head(3) - referenceCorner) / resolution; + const ad_vector_t positionRedFlip = ad_vector_t::Ones(3) - positionRed; + + const ad_scalar_t v00 = positionRedFlip(0) * cornerValues[0] + positionRed(0) * cornerValues[1]; + const ad_scalar_t v10 = positionRedFlip(0) * cornerValues[2] + positionRed(0) * cornerValues[3]; + const ad_scalar_t v01 = positionRedFlip(0) * cornerValues[4] + positionRed(0) * cornerValues[5]; + const ad_scalar_t v11 = positionRedFlip(0) * cornerValues[6] + positionRed(0) * cornerValues[7]; + const ad_scalar_t v0 = positionRedFlip(1) * v00 + positionRed(1) * v10; + const ad_scalar_t v1 = positionRedFlip(1) * v01 + positionRed(1) * v11; + + out.resize(1); + out(0) = positionRedFlip(2) * v0 + positionRed(2) * v1; + out(0) *= x(3); + }; + + cppadInterfacePtr.reset(new CppAdInterface(adTrilinearInterpolation, variableDim, parameterDim, "TestTrilinearInterpolation")); + cppadInterfacePtr->createModels(CppAdInterface::ApproximationOrder::First, verbose); + } + + scalar_t trilinearInterpolation(scalar_t resolution, const vector3_t& referenceCorner, const array8_t& cornerValues, + const vector3_t& position) { + const vector3_t positionRed = (position - referenceCorner) / resolution; + const vector3_t positionRedFlip = vector3_t::Ones() - positionRed; + + const scalar_t v00 = positionRedFlip(0) * cornerValues[0] + positionRed(0) * cornerValues[1]; + const scalar_t v10 = positionRedFlip(0) * cornerValues[2] + positionRed(0) * cornerValues[3]; + const scalar_t v01 = positionRedFlip(0) * cornerValues[4] + positionRed(0) * cornerValues[5]; + const scalar_t v11 = positionRedFlip(0) * cornerValues[6] + positionRed(0) * cornerValues[7]; + const scalar_t v0 = positionRedFlip(1) * v00 + positionRed(1) * v10; + const scalar_t v1 = positionRedFlip(1) * v01 + positionRed(1) * v11; + + const scalar_t value = positionRedFlip(2) * v0 + positionRed(2) * v1; + + return value; + } + + /** param = (resolution, referenceCornerXYZ, cornerValues[0:7]) */ + vector_t toParameter(scalar_t resolution, const vector3_t& referenceCorner, const array8_t& cornerValues) { + vector_t p(12); + p(0) = resolution; + p(1) = referenceCorner.x(); + p(2) = referenceCorner.y(); + p(3) = referenceCorner.z(); + p(4) = cornerValues[0]; // f_000 + p(5) = cornerValues[1]; // f_100 + p(6) = cornerValues[2]; // f_010 + p(7) = cornerValues[3]; // f_110 + p(8) = cornerValues[4]; // f_001 + p(9) = cornerValues[5]; // f_101 + p(10) = cornerValues[6]; // f_011 + p(11) = cornerValues[7]; // f_111 + return p; + } + + static constexpr bool verbose = true; + static constexpr size_t numSamples = 100; + static constexpr size_t variableDim = 4; // (x, y, z, 1.0) + static constexpr size_t parameterDim = 12; // (resolution, referenceCornerXYZ, cornerValues[0:7]) + static constexpr scalar_t precision = 1e-6; + static constexpr scalar_t resolution = 0.05; + + std::unique_ptr<CppAdInterface> cppadInterfacePtr; +}; + +constexpr bool TestTrilinearInterpolation::verbose; +constexpr size_t TestTrilinearInterpolation::numSamples; +constexpr size_t TestTrilinearInterpolation::variableDim; +constexpr size_t TestTrilinearInterpolation::parameterDim; +constexpr scalar_t TestTrilinearInterpolation::precision; +constexpr scalar_t TestTrilinearInterpolation::resolution; + +TEST_F(TestTrilinearInterpolation, testAdFunction) { + for (size_t i = 0; i < numSamples; i++) { + const vector_t randValues = vector_t::Random(parameterDim - 1 + variableDim - 1); + const vector3_t position(randValues(0), randValues(1), randValues(2)); + const vector3_t referenceCorner(randValues(3), randValues(4), randValues(5)); + const array8_t cornerValues = {randValues(6), randValues(7), randValues(8), randValues(9), + randValues(10), randValues(11), randValues(12), randValues(13)}; + + const scalar_t trueValue = trilinearInterpolation(resolution, referenceCorner, cornerValues, position); + + const vector_t input = (vector_t(4) << position, 1.0).finished(); + const vector_t param = toParameter(resolution, referenceCorner, cornerValues); + // true value + const scalar_t value = cppadInterfacePtr->getFunctionValue(input, param)(0); + // true linear approximation + const matrix_t jacAug = cppadInterfacePtr->getJacobian(input, param); + const std::pair<scalar_t, vector3_t> linApprox = {jacAug(0, 3), vector3_t(jacAug(0, 0), jacAug(0, 1), jacAug(0, 2))}; + + EXPECT_TRUE(std::abs(trueValue - value) < precision) << "the true value is " << trueValue << " while the value is " << value; + EXPECT_TRUE(std::abs(trueValue - linApprox.first) < precision) + << "the true value is " << trueValue << " while the linApprox.first is " << linApprox.first; + } // end of i loop +} + +TEST_F(TestTrilinearInterpolation, testTrilinearInterpolation) { + for (size_t i = 0; i < numSamples; i++) { + const vector_t randValues = vector_t::Random(parameterDim - 1 + variableDim - 1); + const vector3_t position(randValues(0), randValues(1), randValues(2)); + const vector3_t referenceCorner(randValues(3), randValues(4), randValues(5)); + const array8_t cornerValues = {randValues(6), randValues(7), randValues(8), randValues(9), + randValues(10), randValues(11), randValues(12), randValues(13)}; + + const vector_t input = (vector_t(4) << position, 1.0).finished(); + const vector_t param = toParameter(resolution, referenceCorner, cornerValues); + // true value + const scalar_t trueValue = cppadInterfacePtr->getFunctionValue(input, param)(0); + // true linear approximation + const matrix_t jacAug = cppadInterfacePtr->getJacobian(input, param); + const std::pair<scalar_t, vector3_t> trueLinApprox = {jacAug(0, 3), vector3_t(jacAug(0, 0), jacAug(0, 1), jacAug(0, 2))}; + + // value and linear approximation + const auto value = trilinear_interpolation::getValue(resolution, referenceCorner, cornerValues, position); + const auto linApprox = trilinear_interpolation::getLinearApproximation(resolution, referenceCorner, cornerValues, position); + + EXPECT_TRUE(std::abs(trueValue - value) < precision) << "the true value is " << trueValue << " while the value is " << value; + EXPECT_TRUE(std::abs(trueValue - linApprox.first) < precision) + << "the true value is " << trueValue << " while linApprox.first is " << linApprox.first; + EXPECT_TRUE(trueLinApprox.second.isApprox(linApprox.second, precision)) + << "the true value is (" << trueLinApprox.second.transpose() << ") while linApprox.second is (" << linApprox.second.transpose() + << ") "; + } // end of i loop +} + +} // namespace trilinear_interpolation +} // namespace ocs2 From 46d49338e30df072a538086e8f198104cd8086f1 Mon Sep 17 00:00:00 2001 From: mv_minniti <mariavittoria001.minniti@gmail.com> Date: Tue, 8 Nov 2022 11:10:15 +0100 Subject: [PATCH 329/512] fix number of rows in call to getOcs2Jacobian of PinocchioEndEffectorKinematics --- .../src/PinocchioEndEffectorKinematics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp index e2306a176..49a1a9ec8 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp @@ -219,7 +219,7 @@ std::vector<VectorFunctionLinearApproximation> PinocchioEndEffectorKinematics::g pinocchio::getFrameJacobian(model, data, frameId, rf, J); const matrix_t Jqdist = (quaternionDistanceJacobian(q, referenceOrientations[i]) * angularVelocityToQuaternionTimeDerivative(q)) * J.bottomRows<3>(); - std::tie(err.dfdx, std::ignore) = mappingPtr_->getOcs2Jacobian(state, Jqdist, matrix_t::Zero(0, model.nv)); + std::tie(err.dfdx, std::ignore) = mappingPtr_->getOcs2Jacobian(state, Jqdist, matrix_t::Zero(3, model.nv)); errors.emplace_back(std::move(err)); } return errors; From 776429261cc0552ed74586e57357d71322e1c845 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 16 Nov 2022 11:26:26 +0100 Subject: [PATCH 330/512] add inequality constraints --- .../ocs2_oc/oc_data/PerformanceIndex.h | 22 +++++++++++++++++++ .../oc_problem/OptimalControlProblem.h | 10 +++++++++ .../LoopshapingOptimalControlProblem.cpp | 9 ++++++++ .../src/oc_problem/OptimalControlProblem.cpp | 16 ++++++++++++++ 4 files changed, 57 insertions(+) diff --git a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index ee45978ee..8e06dd2f6 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -56,6 +56,20 @@ struct PerformanceIndex { */ scalar_t equalityConstraintsSSE = 0.0; + /** Sum of Squared Error (SSE) of inequality constraints: + * - Final: squared norm of violation in state inequality constraints + * - PreJumps: sum of squared norm of violation in state inequality constraints + * - Intermediates: Integral of squared norm violation in state/state-input inequality constraints + */ + scalar_t inequalityConstraintsSSE = 0.0; + + /** Sum of Squared Error (SSE) of the dual feasibilities: + * - Final: squared norm of violation in the dual feasibilities + * - PreJumps: sum of squared norm of violation in the dual feasibilities + * - Intermediates: sum of squared norm of violation in the dual feasibilities + */ + scalar_t dualFeasibilitiesSSE = 0.0; + /** Sum of equality Lagrangians: * - Final: penalty for violation in state equality constraints * - PreJumps: penalty for violation in state equality constraints @@ -76,6 +90,8 @@ struct PerformanceIndex { this->cost += rhs.cost; this->dynamicsViolationSSE += rhs.dynamicsViolationSSE; this->equalityConstraintsSSE += rhs.equalityConstraintsSSE; + this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE; + this->dualFeasibilitiesSSE += rhs.dualFeasibilitiesSSE; this->equalityLagrangian += rhs.equalityLagrangian; this->inequalityLagrangian += rhs.inequalityLagrangian; return *this; @@ -93,6 +109,8 @@ inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { std::swap(lhs.cost, rhs.cost); std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE); std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE); + std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE); + std::swap(lhs.dualFeasibilitiesSSE, rhs.dualFeasibilitiesSSE); std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian); std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian); } @@ -110,6 +128,10 @@ inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& pe stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE; stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE << '\n'; + stream << std::setw(indentation) << ""; + stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE; + stream << "Dual feasibilities SSE: " << std::setw(tabSpace) << performanceIndex.dualFeasibilitiesSSE << '\n'; + stream << std::setw(indentation) << ""; stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian; stream << "Inequality Lagrangian: " << std::setw(tabSpace) << performanceIndex.inequalityLagrangian; diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h index 64e53b6c1..b20fe2ec2 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h @@ -76,6 +76,16 @@ struct OptimalControlProblem { /** Final equality constraints */ std::unique_ptr<StateConstraintCollection> finalEqualityConstraintPtr; + /* Inequality Constraints */ + /** Intermediate inequality constraints */ + std::unique_ptr<StateInputConstraintCollection> inequalityConstraintPtr; + /** Intermediate state-only inequality constraints */ + std::unique_ptr<StateConstraintCollection> stateInequalityConstraintPtr; + /** Pre-jump inequality constraints */ + std::unique_ptr<StateConstraintCollection> preJumpInequalityConstraintPtr; + /** Final inequality constraints */ + std::unique_ptr<StateConstraintCollection> finalInequalityConstraintPtr; + /* Lagrangians */ /** Lagrangian for intermediate equality constraints */ std::unique_ptr<StateInputAugmentedLagrangianCollection> equalityLagrangianPtr; diff --git a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp index d21711fa1..159bec2ae 100644 --- a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp @@ -47,6 +47,15 @@ OptimalControlProblem create(const OptimalControlProblem& problem, std::shared_p LoopshapingConstraint::create(*problem.preJumpEqualityConstraintPtr, loopshapingDefinition); augmentedProblem.finalEqualityConstraintPtr = LoopshapingConstraint::create(*problem.finalEqualityConstraintPtr, loopshapingDefinition); + // Inequality constraints + augmentedProblem.inequalityConstraintPtr = LoopshapingConstraint::create(*problem.inequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.stateInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.stateInequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.preJumpInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.preJumpInequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.finalInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.finalInequalityConstraintPtr, loopshapingDefinition); + // Lagrangians augmentedProblem.equalityLagrangianPtr = LoopshapingAugmentedLagrangian::create(*problem.equalityLagrangianPtr, loopshapingDefinition); augmentedProblem.stateEqualityLagrangianPtr = diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp index 8ede3a4c4..53580c7e3 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp @@ -50,6 +50,11 @@ OptimalControlProblem::OptimalControlProblem() stateEqualityConstraintPtr(new StateConstraintCollection), preJumpEqualityConstraintPtr(new StateConstraintCollection), finalEqualityConstraintPtr(new StateConstraintCollection), + /* Inequality constraints */ + inequalityConstraintPtr(new StateInputConstraintCollection), + stateInequalityConstraintPtr(new StateConstraintCollection), + preJumpInequalityConstraintPtr(new StateConstraintCollection), + finalInequalityConstraintPtr(new StateConstraintCollection), /* Lagrangians */ equalityLagrangianPtr(new StateInputAugmentedLagrangianCollection), stateEqualityLagrangianPtr(new StateAugmentedLagrangianCollection), @@ -82,6 +87,11 @@ OptimalControlProblem::OptimalControlProblem(const OptimalControlProblem& other) stateEqualityConstraintPtr(other.stateEqualityConstraintPtr->clone()), preJumpEqualityConstraintPtr(other.preJumpEqualityConstraintPtr->clone()), finalEqualityConstraintPtr(other.finalEqualityConstraintPtr->clone()), + /* Inequality constraints */ + inequalityConstraintPtr(other.inequalityConstraintPtr->clone()), + stateInequalityConstraintPtr(other.stateInequalityConstraintPtr->clone()), + preJumpInequalityConstraintPtr(other.preJumpInequalityConstraintPtr->clone()), + finalInequalityConstraintPtr(other.finalInequalityConstraintPtr->clone()), /* Lagrangians */ equalityLagrangianPtr(other.equalityLagrangianPtr->clone()), stateEqualityLagrangianPtr(other.stateEqualityLagrangianPtr->clone()), @@ -130,6 +140,12 @@ void OptimalControlProblem::swap(OptimalControlProblem& other) noexcept { preJumpEqualityConstraintPtr.swap(other.preJumpEqualityConstraintPtr); finalEqualityConstraintPtr.swap(other.finalEqualityConstraintPtr); + /* Inequality constraints */ + inequalityConstraintPtr.swap(other.inequalityConstraintPtr); + stateInequalityConstraintPtr.swap(other.stateInequalityConstraintPtr); + preJumpInequalityConstraintPtr.swap(other.preJumpInequalityConstraintPtr); + finalInequalityConstraintPtr.swap(other.finalInequalityConstraintPtr); + /* Lagrangians */ equalityLagrangianPtr.swap(other.equalityLagrangianPtr); stateEqualityLagrangianPtr.swap(other.stateEqualityLagrangianPtr); From d23d3db5ab8a5c0fbd409107c2039aded7f558a4 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 16 Nov 2022 12:33:08 +0100 Subject: [PATCH 331/512] add inequality metrics --- ocs2_core/include/ocs2_core/model_data/Metrics.h | 10 ++++++++++ ocs2_core/src/model_data/Metrics.cpp | 8 ++++++++ ocs2_core/test/model_data/testMetrics.cpp | 13 +++++++++++++ 3 files changed, 31 insertions(+) diff --git a/ocs2_core/include/ocs2_core/model_data/Metrics.h b/ocs2_core/include/ocs2_core/model_data/Metrics.h index 2d205d377..3b5be5e4c 100644 --- a/ocs2_core/include/ocs2_core/model_data/Metrics.h +++ b/ocs2_core/include/ocs2_core/model_data/Metrics.h @@ -71,6 +71,10 @@ struct MetricsCollection { vector_t stateEqConstraint; vector_t stateInputEqConstraint; + // Inequality constraints + vector_t stateIneqConstraint; + vector_t stateInputIneqConstraint; + // Lagrangians std::vector<LagrangianMetrics> stateEqLagrangian; std::vector<LagrangianMetrics> stateIneqLagrangian; @@ -84,6 +88,9 @@ struct MetricsCollection { // Equality constraints stateEqConstraint.swap(other.stateEqConstraint); stateInputEqConstraint.swap(other.stateInputEqConstraint); + // Inequality constraints + stateIneqConstraint.swap(other.stateIneqConstraint); + stateInputIneqConstraint.swap(other.stateInputIneqConstraint); // Lagrangians stateEqLagrangian.swap(other.stateEqLagrangian); stateIneqLagrangian.swap(other.stateIneqLagrangian); @@ -98,6 +105,9 @@ struct MetricsCollection { // Equality constraints stateEqConstraint = vector_t(); stateInputEqConstraint = vector_t(); + // Inequality constraints + stateIneqConstraint = vector_t(); + stateInputIneqConstraint = vector_t(); // Lagrangians stateEqLagrangian.clear(); stateIneqLagrangian.clear(); diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 8d1066934..a50867fdc 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -121,6 +121,14 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector interpolate(indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); + // inequality constraints + out.stateIneqConstraint = interpolate(indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { + return array[t].stateIneqConstraint; + }); + out.stateInputIneqConstraint = + interpolate(indexAlpha, dataArray, + [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqConstraint; }); + // state equality Lagrangian out.stateEqLagrangian.reserve(mumStateEq); for (size_t i = 0; i < mumStateEq; i++) { diff --git a/ocs2_core/test/model_data/testMetrics.cpp b/ocs2_core/test/model_data/testMetrics.cpp index 40866541c..4bce431ae 100644 --- a/ocs2_core/test/model_data/testMetrics.cpp +++ b/ocs2_core/test/model_data/testMetrics.cpp @@ -83,6 +83,13 @@ inline MetricsCollection interpolateNew(const LinearInterpolation::index_alpha_t out.stateInputEqConstraint = LinearInterpolation::interpolate( indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); + // inequality constraints + out.stateIneqConstraint = LinearInterpolation::interpolate( + indexAlpha, dataArray, + [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateIneqConstraint; }); + out.stateInputIneqConstraint = LinearInterpolation::interpolate( + indexAlpha, dataArray, + [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqConstraint; }); out.stateEqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateEqLagrangian), f(lhs_stateEq, rhs_stateEq)); out.stateIneqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateIneqLagrangian), f(lhs_stateIneq, rhs_stateIneq)); out.stateInputEqLagrangian = @@ -110,6 +117,8 @@ bool isApprox(const MetricsCollection& lhs, const MetricsCollection& rhs, scalar bool flag = std::abs(lhs.cost - rhs.cost) < prec; flag = flag && lhs.stateEqConstraint.isApprox(rhs.stateEqConstraint, prec); flag = flag && lhs.stateInputEqConstraint.isApprox(rhs.stateInputEqConstraint, prec); + flag = flag && lhs.stateIneqConstraint.isApprox(rhs.stateIneqConstraint, prec); + flag = flag && lhs.stateInputIneqConstraint.isApprox(rhs.stateInputIneqConstraint, prec); flag = flag && toVector(lhs.stateEqLagrangian).isApprox(toVector(rhs.stateEqLagrangian), prec); flag = flag && toVector(lhs.stateIneqLagrangian).isApprox(toVector(rhs.stateIneqLagrangian), prec); flag = flag && toVector(lhs.stateInputEqLagrangian).isApprox(toVector(rhs.stateInputEqLagrangian), prec); @@ -141,6 +150,8 @@ TEST(TestMetrics, testSwap) { termsMetricsCollection.cost = ocs2::vector_t::Random(1)(0); termsMetricsCollection.stateEqConstraint = ocs2::vector_t::Random(2); termsMetricsCollection.stateInputEqConstraint = ocs2::vector_t::Random(3); + termsMetricsCollection.stateIneqConstraint = ocs2::vector_t::Random(2); + termsMetricsCollection.stateInputIneqConstraint = ocs2::vector_t::Random(3); ocs2::random(termsSize, termsMetricsCollection.stateEqLagrangian); ocs2::random(termsSize, termsMetricsCollection.stateIneqLagrangian); ocs2::random(termsSize, termsMetricsCollection.stateInputEqLagrangian); @@ -176,6 +187,8 @@ TEST(TestMetrics, testInterpolation) { metricsCollectionTrajectory[i].cost = ocs2::vector_t::Random(1)(0); metricsCollectionTrajectory[i].stateEqConstraint = ocs2::vector_t::Random(2); metricsCollectionTrajectory[i].stateInputEqConstraint = ocs2::vector_t::Random(3); + metricsCollectionTrajectory[i].stateIneqConstraint = ocs2::vector_t::Random(2); + metricsCollectionTrajectory[i].stateInputIneqConstraint = ocs2::vector_t::Random(3); ocs2::random(stateEqTermsSize, metricsCollectionTrajectory[i].stateEqLagrangian); ocs2::random(stateIneqTermsSize, metricsCollectionTrajectory[i].stateIneqLagrangian); ocs2::random(stateInputEqTermsSize, metricsCollectionTrajectory[i].stateInputEqLagrangian); From f76bf8215bae9ed5a2a574ae76744ac06e3097f9 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 16 Nov 2022 18:33:26 +0100 Subject: [PATCH 332/512] formatted code by clang-tools --- ocs2_core/src/model_data/Metrics.cpp | 10 +++++----- ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h | 6 +++--- .../oc_problem/LoopshapingOptimalControlProblem.cpp | 6 +++--- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index a50867fdc..980eb6ab1 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -122,12 +122,12 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); // inequality constraints - out.stateIneqConstraint = interpolate(indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { - return array[t].stateIneqConstraint; - }); - out.stateInputIneqConstraint = + out.stateIneqConstraint = interpolate(indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqConstraint; }); + [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateIneqConstraint; }); + out.stateInputIneqConstraint = interpolate( + indexAlpha, dataArray, + [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqConstraint; }); // state equality Lagrangian out.stateEqLagrangian.reserve(mumStateEq); diff --git a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index 8e06dd2f6..d51f121fb 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -57,9 +57,9 @@ struct PerformanceIndex { scalar_t equalityConstraintsSSE = 0.0; /** Sum of Squared Error (SSE) of inequality constraints: - * - Final: squared norm of violation in state inequality constraints - * - PreJumps: sum of squared norm of violation in state inequality constraints - * - Intermediates: Integral of squared norm violation in state/state-input inequality constraints + * - Final: squared norm of violation in state inequality constraints + * - PreJumps: sum of squared norm of violation in state inequality constraints + * - Intermediates: Integral of squared norm violation in state/state-input inequality constraints */ scalar_t inequalityConstraintsSSE = 0.0; diff --git a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp index 159bec2ae..6eb0cc2c9 100644 --- a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp @@ -49,11 +49,11 @@ OptimalControlProblem create(const OptimalControlProblem& problem, std::shared_p // Inequality constraints augmentedProblem.inequalityConstraintPtr = LoopshapingConstraint::create(*problem.inequalityConstraintPtr, loopshapingDefinition); - augmentedProblem.stateInequalityConstraintPtr = - LoopshapingConstraint::create(*problem.stateInequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.stateInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.stateInequalityConstraintPtr, loopshapingDefinition); augmentedProblem.preJumpInequalityConstraintPtr = LoopshapingConstraint::create(*problem.preJumpInequalityConstraintPtr, loopshapingDefinition); - augmentedProblem.finalInequalityConstraintPtr = + augmentedProblem.finalInequalityConstraintPtr = LoopshapingConstraint::create(*problem.finalInequalityConstraintPtr, loopshapingDefinition); // Lagrangians From 0458511b2e16538747565d4b74c4f61180058a05 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 22 Nov 2022 09:57:48 +0100 Subject: [PATCH 333/512] modify constraint projection for interior point method --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 12 +++---- .../include/ocs2_sqp/ConstraintProjection.h | 14 +++++--- .../ocs2_sqp/src/ConstraintProjection.cpp | 20 ++++++++--- .../src/MultipleShootingTranscription.cpp | 2 +- ocs2_sqp/ocs2_sqp/test/testProjection.cpp | 34 +++++++++++++++++-- 5 files changed, 65 insertions(+), 17 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 237b8a4c4..27111d9e5 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -100,13 +100,13 @@ install(DIRECTORY include/${PROJECT_NAME}/ ############# catkin_add_gtest(test_${PROJECT_NAME} - test/testCircularKinematics.cpp - test/testDiscretization.cpp + # test/testCircularKinematics.cpp + # test/testDiscretization.cpp test/testProjection.cpp - test/testSwitchedProblem.cpp - test/testTranscription.cpp - test/testUnconstrained.cpp - test/testValuefunction.cpp + # test/testSwitchedProblem.cpp + # test/testTranscription.cpp + # test/testUnconstrained.cpp + # test/testValuefunction.cpp ) add_dependencies(test_${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(test_${PROJECT_NAME} diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h index 0e7dfe93f..0e450d71e 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h @@ -42,9 +42,12 @@ namespace ocs2 { * Implementation based on the QR decomposition * * @param constraint : C = dfdx, D = dfdu, e = f; - * @return Px = dfdx, Pu = dfdu, Pe = f; + * @param extractLagrangeMultiplierCoefficient : If true, a term to compute the coefficients of the Lagrange multiplier associated with + * the state-input equality constraint is returned. If false, an empty matrix is returned. + * @return Px = dfdx, Pu = dfdu, Pe = f and the term for the Lagrange multiplier direction; */ -VectorFunctionLinearApproximation qrConstraintProjection(const VectorFunctionLinearApproximation& constraint); +std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint, + bool extractLagrangeMultiplierCoefficient = false); /** * Returns the linear projection @@ -55,8 +58,11 @@ VectorFunctionLinearApproximation qrConstraintProjection(const VectorFunctionLin * Implementation based on the LU decomposition * * @param constraint : C = dfdx, D = dfdu, e = f; - * @return Px = dfdx, Pu = dfdu, Pe = f; + * @param extractLagrangeMultiplierCoefficient : If true, a term to compute the coefficients of the Lagrange multiplier associated with + * the state-input equality constraint is returned. If false, an empty matrix is returned. + * @return Px = dfdx, Pu = dfdu, Pe = f and the term for the Lagrange multiplier direction; */ -VectorFunctionLinearApproximation luConstraintProjection(const VectorFunctionLinearApproximation& constraint); +std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, + bool extractLagrangeMultiplierCoefficient = false); } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp index 338c7cd10..c52d3095f 100644 --- a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp @@ -31,7 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { -VectorFunctionLinearApproximation qrConstraintProjection(const VectorFunctionLinearApproximation& constraint) { +std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint, + bool extractLagrangeMultiplierCoefficient) { // Constraint Projectors are based on the QR decomposition const auto numConstraints = constraint.dfdu.rows(); const auto numInputs = constraint.dfdu.cols(); @@ -49,10 +50,16 @@ VectorFunctionLinearApproximation qrConstraintProjection(const VectorFunctionLin projectionTerms.dfdx.noalias() = -Q1 * RTinvC; projectionTerms.f.noalias() = -Q1 * RTinve; - return projectionTerms; + matrix_t lagrangeMultiplierCoefficient; + if (extractLagrangeMultiplierCoefficient) { + lagrangeMultiplierCoefficient = RT.transpose().solve(Q1.transpose()); + } + + return std::make_pair(std::move(projectionTerms), std::move(lagrangeMultiplierCoefficient)); } -VectorFunctionLinearApproximation luConstraintProjection(const VectorFunctionLinearApproximation& constraint) { +std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, + bool extractLagrangeMultiplierCoefficient) { // Constraint Projectors are based on the LU decomposition const Eigen::FullPivLU<matrix_t> lu(constraint.dfdu); @@ -61,7 +68,12 @@ VectorFunctionLinearApproximation luConstraintProjection(const VectorFunctionLin projectionTerms.dfdx.noalias() = -lu.solve(constraint.dfdx); projectionTerms.f.noalias() = -lu.solve(constraint.f); - return projectionTerms; + matrix_t lagrangeMultiplierCoefficient; + if (extractLagrangeMultiplierCoefficient) { + lagrangeMultiplierCoefficient = lu.solve(matrix_t::Identity(constraint.f.size(), constraint.f.size())).transpose(); + } + + return std::make_pair(std::move(projectionTerms), std::move(lagrangeMultiplierCoefficient)); } } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index 9b999f07f..7f294ee1e 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -71,7 +71,7 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP performance.equalityConstraintsSSE = dt * constraints.f.squaredNorm(); if (projectStateInputEqualityConstraints) { // Handle equality constraints using projection. // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. - projection = luConstraintProjection(constraints); + projection = luConstraintProjection(constraints).first; constraints = VectorFunctionLinearApproximation(); // Adapt dynamics and cost diff --git a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp index f948839bc..03579a56c 100644 --- a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp @@ -36,7 +36,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. TEST(test_projection, testProjectionQR) { const auto constraint = ocs2::getRandomConstraints(30, 20, 10); - const auto projection = ocs2::qrConstraintProjection(constraint); + auto result = ocs2::qrConstraintProjection(constraint); + ASSERT_EQ(result.second.rows(), 0); + ASSERT_EQ(result.second.cols(), 0); + + const auto projection = std::move(result.first); // range of Pu is in null-space of D ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); @@ -46,12 +50,27 @@ TEST(test_projection, testProjectionQR) { // D * Pe cancels the e term ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); + + auto resultWithMultiplierCoefficient = ocs2::qrConstraintProjection(constraint, true); + const auto projectionWithMultiplierCoefficient = std::move(resultWithMultiplierCoefficient.first); + ASSERT_TRUE(projection.f.isApprox(projectionWithMultiplierCoefficient.f)); + ASSERT_TRUE(projection.dfdx.isApprox(projectionWithMultiplierCoefficient.dfdx)); + ASSERT_TRUE(projection.dfdu.isApprox(projectionWithMultiplierCoefficient.dfdu)); + + const auto lagrangeMultiplierCoefficient = std::move(resultWithMultiplierCoefficient.second); + ASSERT_EQ(lagrangeMultiplierCoefficient.rows(), constraint.dfdu.rows()); + ASSERT_EQ(lagrangeMultiplierCoefficient.cols(), constraint.dfdu.cols()); + ASSERT_TRUE((constraint.dfdx.transpose()*lagrangeMultiplierCoefficient).isApprox(- projection.dfdx.transpose())); } TEST(test_projection, testProjectionLU) { const auto constraint = ocs2::getRandomConstraints(30, 20, 10); - const auto projection = ocs2::luConstraintProjection(constraint); + auto result = ocs2::luConstraintProjection(constraint); + ASSERT_EQ(result.second.rows(), 0); + ASSERT_EQ(result.second.cols(), 0); + + const auto projection = std::move(result.first); // range of Pu is in null-space of D ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); @@ -61,4 +80,15 @@ TEST(test_projection, testProjectionLU) { // D * Pe cancels the e term ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); + + auto resultWithMultiplierCoefficient = ocs2::luConstraintProjection(constraint, true); + const auto projectionWithMultiplierCoefficient = std::move(resultWithMultiplierCoefficient.first); + ASSERT_TRUE(projection.f.isApprox(projectionWithMultiplierCoefficient.f)); + ASSERT_TRUE(projection.dfdx.isApprox(projectionWithMultiplierCoefficient.dfdx)); + ASSERT_TRUE(projection.dfdu.isApprox(projectionWithMultiplierCoefficient.dfdu)); + + const auto lagrangeMultiplierCoefficient = std::move(resultWithMultiplierCoefficient.second); + ASSERT_EQ(lagrangeMultiplierCoefficient.rows(), constraint.dfdu.rows()); + ASSERT_EQ(lagrangeMultiplierCoefficient.cols(), constraint.dfdu.cols()); + ASSERT_TRUE((constraint.dfdx.transpose()*lagrangeMultiplierCoefficient).isApprox(- projection.dfdx.transpose())); } \ No newline at end of file From 61e1fbe894fb4cef4309ead4c983e68330d40cdd Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 22 Nov 2022 09:59:01 +0100 Subject: [PATCH 334/512] turn on tests --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 27111d9e5..237b8a4c4 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -100,13 +100,13 @@ install(DIRECTORY include/${PROJECT_NAME}/ ############# catkin_add_gtest(test_${PROJECT_NAME} - # test/testCircularKinematics.cpp - # test/testDiscretization.cpp + test/testCircularKinematics.cpp + test/testDiscretization.cpp test/testProjection.cpp - # test/testSwitchedProblem.cpp - # test/testTranscription.cpp - # test/testUnconstrained.cpp - # test/testValuefunction.cpp + test/testSwitchedProblem.cpp + test/testTranscription.cpp + test/testUnconstrained.cpp + test/testValuefunction.cpp ) add_dependencies(test_${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(test_${PROJECT_NAME} From d428b82faf870e4c455c912deecf527aefc00b5c Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 22 Nov 2022 10:24:16 +0100 Subject: [PATCH 335/512] improve namings --- .../include/ocs2_sqp/ConstraintProjection.h | 14 +++---- .../ocs2_sqp/src/ConstraintProjection.cpp | 20 ++++----- ocs2_sqp/ocs2_sqp/test/testProjection.cpp | 42 ++++++++++--------- 3 files changed, 39 insertions(+), 37 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h index 0e450d71e..a3b31ee15 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h @@ -42,12 +42,11 @@ namespace ocs2 { * Implementation based on the QR decomposition * * @param constraint : C = dfdx, D = dfdu, e = f; - * @param extractLagrangeMultiplierCoefficient : If true, a term to compute the coefficients of the Lagrange multiplier associated with - * the state-input equality constraint is returned. If false, an empty matrix is returned. - * @return Px = dfdx, Pu = dfdu, Pe = f and the term for the Lagrange multiplier direction; + * @param extractPseudoInverse : If true, pseudo inverse of D is returned. If false, an empty matrix is returned. + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and pseudo inverse of D (second); */ std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint, - bool extractLagrangeMultiplierCoefficient = false); + bool extractPseudoInverse = false); /** * Returns the linear projection @@ -58,11 +57,10 @@ std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(co * Implementation based on the LU decomposition * * @param constraint : C = dfdx, D = dfdu, e = f; - * @param extractLagrangeMultiplierCoefficient : If true, a term to compute the coefficients of the Lagrange multiplier associated with - * the state-input equality constraint is returned. If false, an empty matrix is returned. - * @return Px = dfdx, Pu = dfdu, Pe = f and the term for the Lagrange multiplier direction; + * @param extractPseudoInverse : If true, pseudo inverse of D is returned. If false, an empty matrix is returned. + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and pseudo inverse of D (second); */ std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, - bool extractLagrangeMultiplierCoefficient = false); + bool extractPseudoInverse = false); } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp index c52d3095f..332e4e380 100644 --- a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint, - bool extractLagrangeMultiplierCoefficient) { + bool extractPseudoInverse) { // Constraint Projectors are based on the QR decomposition const auto numConstraints = constraint.dfdu.rows(); const auto numInputs = constraint.dfdu.cols(); @@ -50,16 +50,16 @@ std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(co projectionTerms.dfdx.noalias() = -Q1 * RTinvC; projectionTerms.f.noalias() = -Q1 * RTinve; - matrix_t lagrangeMultiplierCoefficient; - if (extractLagrangeMultiplierCoefficient) { - lagrangeMultiplierCoefficient = RT.transpose().solve(Q1.transpose()); + matrix_t pseudoInverse; + if (extractPseudoInverse) { + pseudoInverse = RT.transpose().solve(Q1.transpose()); } - return std::make_pair(std::move(projectionTerms), std::move(lagrangeMultiplierCoefficient)); + return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); } std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, - bool extractLagrangeMultiplierCoefficient) { + bool extractPseudoInverse) { // Constraint Projectors are based on the LU decomposition const Eigen::FullPivLU<matrix_t> lu(constraint.dfdu); @@ -68,12 +68,12 @@ std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(co projectionTerms.dfdx.noalias() = -lu.solve(constraint.dfdx); projectionTerms.f.noalias() = -lu.solve(constraint.f); - matrix_t lagrangeMultiplierCoefficient; - if (extractLagrangeMultiplierCoefficient) { - lagrangeMultiplierCoefficient = lu.solve(matrix_t::Identity(constraint.f.size(), constraint.f.size())).transpose(); + matrix_t pseudoInverse; + if (extractPseudoInverse) { + pseudoInverse = lu.solve(matrix_t::Identity(constraint.f.size(), constraint.f.size())).transpose(); } - return std::make_pair(std::move(projectionTerms), std::move(lagrangeMultiplierCoefficient)); + return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); } } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp index 03579a56c..452ed3baf 100644 --- a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp @@ -51,16 +51,18 @@ TEST(test_projection, testProjectionQR) { // D * Pe cancels the e term ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); - auto resultWithMultiplierCoefficient = ocs2::qrConstraintProjection(constraint, true); - const auto projectionWithMultiplierCoefficient = std::move(resultWithMultiplierCoefficient.first); - ASSERT_TRUE(projection.f.isApprox(projectionWithMultiplierCoefficient.f)); - ASSERT_TRUE(projection.dfdx.isApprox(projectionWithMultiplierCoefficient.dfdx)); - ASSERT_TRUE(projection.dfdu.isApprox(projectionWithMultiplierCoefficient.dfdu)); - - const auto lagrangeMultiplierCoefficient = std::move(resultWithMultiplierCoefficient.second); - ASSERT_EQ(lagrangeMultiplierCoefficient.rows(), constraint.dfdu.rows()); - ASSERT_EQ(lagrangeMultiplierCoefficient.cols(), constraint.dfdu.cols()); - ASSERT_TRUE((constraint.dfdx.transpose()*lagrangeMultiplierCoefficient).isApprox(- projection.dfdx.transpose())); + auto resultWithPseudoInverse = ocs2::qrConstraintProjection(constraint, true); + const auto projectionWithPseudoInverse = std::move(resultWithPseudoInverse.first); + ASSERT_TRUE(projection.f.isApprox(projectionWithPseudoInverse.f)); + ASSERT_TRUE(projection.dfdx.isApprox(projectionWithPseudoInverse.dfdx)); + ASSERT_TRUE(projection.dfdu.isApprox(projectionWithPseudoInverse.dfdu)); + + const auto pseudoInverse = std::move(resultWithPseudoInverse.second); + ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); + ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); + + ASSERT_TRUE((pseudoInverse*constraint.dfdu.transpose()).isIdentity()); + ASSERT_TRUE((constraint.dfdx.transpose()*pseudoInverse).isApprox(- projection.dfdx.transpose())); } TEST(test_projection, testProjectionLU) { @@ -81,14 +83,16 @@ TEST(test_projection, testProjectionLU) { // D * Pe cancels the e term ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); - auto resultWithMultiplierCoefficient = ocs2::luConstraintProjection(constraint, true); - const auto projectionWithMultiplierCoefficient = std::move(resultWithMultiplierCoefficient.first); - ASSERT_TRUE(projection.f.isApprox(projectionWithMultiplierCoefficient.f)); - ASSERT_TRUE(projection.dfdx.isApprox(projectionWithMultiplierCoefficient.dfdx)); - ASSERT_TRUE(projection.dfdu.isApprox(projectionWithMultiplierCoefficient.dfdu)); + auto resultWithPseudoInverse = ocs2::luConstraintProjection(constraint, true); + const auto projectionWithPseudoInverse = std::move(resultWithPseudoInverse.first); + ASSERT_TRUE(projection.f.isApprox(projectionWithPseudoInverse.f)); + ASSERT_TRUE(projection.dfdx.isApprox(projectionWithPseudoInverse.dfdx)); + ASSERT_TRUE(projection.dfdu.isApprox(projectionWithPseudoInverse.dfdu)); - const auto lagrangeMultiplierCoefficient = std::move(resultWithMultiplierCoefficient.second); - ASSERT_EQ(lagrangeMultiplierCoefficient.rows(), constraint.dfdu.rows()); - ASSERT_EQ(lagrangeMultiplierCoefficient.cols(), constraint.dfdu.cols()); - ASSERT_TRUE((constraint.dfdx.transpose()*lagrangeMultiplierCoefficient).isApprox(- projection.dfdx.transpose())); + const auto pseudoInverse = std::move(resultWithPseudoInverse.second); + ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); + ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); + + ASSERT_TRUE((pseudoInverse*constraint.dfdu.transpose()).isIdentity()); + ASSERT_TRUE((constraint.dfdx.transpose()*pseudoInverse).isApprox(- projection.dfdx.transpose())); } \ No newline at end of file From e355c5459439b6430ab511d2d821e7462f04b69c Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 22 Nov 2022 11:36:19 +0100 Subject: [PATCH 336/512] improve constraint projections and tests --- .../include/ocs2_sqp/ConstraintProjection.h | 4 +--- .../ocs2_sqp/src/ConstraintProjection.cpp | 19 ++++++------------- ocs2_sqp/ocs2_sqp/test/testProjection.cpp | 17 +++++------------ 3 files changed, 12 insertions(+), 28 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h index a3b31ee15..0e4049bc0 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h @@ -42,11 +42,9 @@ namespace ocs2 { * Implementation based on the QR decomposition * * @param constraint : C = dfdx, D = dfdu, e = f; - * @param extractPseudoInverse : If true, pseudo inverse of D is returned. If false, an empty matrix is returned. * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and pseudo inverse of D (second); */ -std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint, - bool extractPseudoInverse = false); +std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint); /** * Returns the linear projection diff --git a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp index 332e4e380..db3f117af 100644 --- a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp @@ -31,29 +31,22 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { -std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint, - bool extractPseudoInverse) { +std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint) { // Constraint Projectors are based on the QR decomposition const auto numConstraints = constraint.dfdu.rows(); const auto numInputs = constraint.dfdu.cols(); const Eigen::HouseholderQR<matrix_t> QRof_DT(constraint.dfdu.transpose()); - const auto RT = QRof_DT.matrixQR().topRows(numConstraints).triangularView<Eigen::Upper>().transpose(); - const matrix_t RTinvC = RT.solve(constraint.dfdx); // inv(R^T) * C - const matrix_t RTinve = RT.solve(constraint.f); // inv(R^T) * e - const matrix_t Q = QRof_DT.householderQ(); const auto Q1 = Q.leftCols(numConstraints); + const auto R = QRof_DT.matrixQR().topRows(numConstraints).triangularView<Eigen::Upper>(); + const matrix_t pseudoInverse = R.solve(Q1.transpose()); + VectorFunctionLinearApproximation projectionTerms; projectionTerms.dfdu = Q.rightCols(numInputs - numConstraints); - projectionTerms.dfdx.noalias() = -Q1 * RTinvC; - projectionTerms.f.noalias() = -Q1 * RTinve; - - matrix_t pseudoInverse; - if (extractPseudoInverse) { - pseudoInverse = RT.transpose().solve(Q1.transpose()); - } + projectionTerms.dfdx.noalias() = -pseudoInverse.transpose() * constraint.dfdx; + projectionTerms.f.noalias() = -pseudoInverse.transpose() * constraint.f; return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); } diff --git a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp index 452ed3baf..5845a54f7 100644 --- a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp @@ -37,10 +37,8 @@ TEST(test_projection, testProjectionQR) { const auto constraint = ocs2::getRandomConstraints(30, 20, 10); auto result = ocs2::qrConstraintProjection(constraint); - ASSERT_EQ(result.second.rows(), 0); - ASSERT_EQ(result.second.cols(), 0); - const auto projection = std::move(result.first); + const auto pseudoInverse = std::move(result.second); // range of Pu is in null-space of D ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); @@ -51,18 +49,12 @@ TEST(test_projection, testProjectionQR) { // D * Pe cancels the e term ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); - auto resultWithPseudoInverse = ocs2::qrConstraintProjection(constraint, true); - const auto projectionWithPseudoInverse = std::move(resultWithPseudoInverse.first); - ASSERT_TRUE(projection.f.isApprox(projectionWithPseudoInverse.f)); - ASSERT_TRUE(projection.dfdx.isApprox(projectionWithPseudoInverse.dfdx)); - ASSERT_TRUE(projection.dfdu.isApprox(projectionWithPseudoInverse.dfdu)); - - const auto pseudoInverse = std::move(resultWithPseudoInverse.second); ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); ASSERT_TRUE((pseudoInverse*constraint.dfdu.transpose()).isIdentity()); - ASSERT_TRUE((constraint.dfdx.transpose()*pseudoInverse).isApprox(- projection.dfdx.transpose())); + ASSERT_TRUE((pseudoInverse.transpose()*constraint.dfdx).isApprox(- projection.dfdx)); + ASSERT_TRUE((pseudoInverse.transpose()*constraint.f).isApprox(- projection.f)); } TEST(test_projection, testProjectionLU) { @@ -94,5 +86,6 @@ TEST(test_projection, testProjectionLU) { ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); ASSERT_TRUE((pseudoInverse*constraint.dfdu.transpose()).isIdentity()); - ASSERT_TRUE((constraint.dfdx.transpose()*pseudoInverse).isApprox(- projection.dfdx.transpose())); + ASSERT_TRUE((pseudoInverse.transpose()*constraint.dfdx).isApprox(- projection.dfdx)); + ASSERT_TRUE((pseudoInverse.transpose()*constraint.f).isApprox(- projection.f)); } \ No newline at end of file From 90cbe8fe63d114d3cadd323a056d047b3548f6a1 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 22 Nov 2022 11:42:54 +0100 Subject: [PATCH 337/512] revert changes --- .../include/ocs2_core/model_data/Metrics.h | 10 --------- ocs2_core/src/model_data/Metrics.cpp | 8 ------- ocs2_core/test/model_data/testMetrics.cpp | 13 ----------- .../ocs2_oc/oc_data/PerformanceIndex.h | 22 ------------------- .../oc_problem/OptimalControlProblem.h | 10 --------- .../LoopshapingOptimalControlProblem.cpp | 9 -------- .../src/oc_problem/OptimalControlProblem.cpp | 16 -------------- 7 files changed, 88 deletions(-) diff --git a/ocs2_core/include/ocs2_core/model_data/Metrics.h b/ocs2_core/include/ocs2_core/model_data/Metrics.h index 3b5be5e4c..2d205d377 100644 --- a/ocs2_core/include/ocs2_core/model_data/Metrics.h +++ b/ocs2_core/include/ocs2_core/model_data/Metrics.h @@ -71,10 +71,6 @@ struct MetricsCollection { vector_t stateEqConstraint; vector_t stateInputEqConstraint; - // Inequality constraints - vector_t stateIneqConstraint; - vector_t stateInputIneqConstraint; - // Lagrangians std::vector<LagrangianMetrics> stateEqLagrangian; std::vector<LagrangianMetrics> stateIneqLagrangian; @@ -88,9 +84,6 @@ struct MetricsCollection { // Equality constraints stateEqConstraint.swap(other.stateEqConstraint); stateInputEqConstraint.swap(other.stateInputEqConstraint); - // Inequality constraints - stateIneqConstraint.swap(other.stateIneqConstraint); - stateInputIneqConstraint.swap(other.stateInputIneqConstraint); // Lagrangians stateEqLagrangian.swap(other.stateEqLagrangian); stateIneqLagrangian.swap(other.stateIneqLagrangian); @@ -105,9 +98,6 @@ struct MetricsCollection { // Equality constraints stateEqConstraint = vector_t(); stateInputEqConstraint = vector_t(); - // Inequality constraints - stateIneqConstraint = vector_t(); - stateInputIneqConstraint = vector_t(); // Lagrangians stateEqLagrangian.clear(); stateIneqLagrangian.clear(); diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 980eb6ab1..8d1066934 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -121,14 +121,6 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector interpolate(indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); - // inequality constraints - out.stateIneqConstraint = - interpolate(indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateIneqConstraint; }); - out.stateInputIneqConstraint = interpolate( - indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqConstraint; }); - // state equality Lagrangian out.stateEqLagrangian.reserve(mumStateEq); for (size_t i = 0; i < mumStateEq; i++) { diff --git a/ocs2_core/test/model_data/testMetrics.cpp b/ocs2_core/test/model_data/testMetrics.cpp index 4bce431ae..40866541c 100644 --- a/ocs2_core/test/model_data/testMetrics.cpp +++ b/ocs2_core/test/model_data/testMetrics.cpp @@ -83,13 +83,6 @@ inline MetricsCollection interpolateNew(const LinearInterpolation::index_alpha_t out.stateInputEqConstraint = LinearInterpolation::interpolate( indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); - // inequality constraints - out.stateIneqConstraint = LinearInterpolation::interpolate( - indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateIneqConstraint; }); - out.stateInputIneqConstraint = LinearInterpolation::interpolate( - indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqConstraint; }); out.stateEqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateEqLagrangian), f(lhs_stateEq, rhs_stateEq)); out.stateIneqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateIneqLagrangian), f(lhs_stateIneq, rhs_stateIneq)); out.stateInputEqLagrangian = @@ -117,8 +110,6 @@ bool isApprox(const MetricsCollection& lhs, const MetricsCollection& rhs, scalar bool flag = std::abs(lhs.cost - rhs.cost) < prec; flag = flag && lhs.stateEqConstraint.isApprox(rhs.stateEqConstraint, prec); flag = flag && lhs.stateInputEqConstraint.isApprox(rhs.stateInputEqConstraint, prec); - flag = flag && lhs.stateIneqConstraint.isApprox(rhs.stateIneqConstraint, prec); - flag = flag && lhs.stateInputIneqConstraint.isApprox(rhs.stateInputIneqConstraint, prec); flag = flag && toVector(lhs.stateEqLagrangian).isApprox(toVector(rhs.stateEqLagrangian), prec); flag = flag && toVector(lhs.stateIneqLagrangian).isApprox(toVector(rhs.stateIneqLagrangian), prec); flag = flag && toVector(lhs.stateInputEqLagrangian).isApprox(toVector(rhs.stateInputEqLagrangian), prec); @@ -150,8 +141,6 @@ TEST(TestMetrics, testSwap) { termsMetricsCollection.cost = ocs2::vector_t::Random(1)(0); termsMetricsCollection.stateEqConstraint = ocs2::vector_t::Random(2); termsMetricsCollection.stateInputEqConstraint = ocs2::vector_t::Random(3); - termsMetricsCollection.stateIneqConstraint = ocs2::vector_t::Random(2); - termsMetricsCollection.stateInputIneqConstraint = ocs2::vector_t::Random(3); ocs2::random(termsSize, termsMetricsCollection.stateEqLagrangian); ocs2::random(termsSize, termsMetricsCollection.stateIneqLagrangian); ocs2::random(termsSize, termsMetricsCollection.stateInputEqLagrangian); @@ -187,8 +176,6 @@ TEST(TestMetrics, testInterpolation) { metricsCollectionTrajectory[i].cost = ocs2::vector_t::Random(1)(0); metricsCollectionTrajectory[i].stateEqConstraint = ocs2::vector_t::Random(2); metricsCollectionTrajectory[i].stateInputEqConstraint = ocs2::vector_t::Random(3); - metricsCollectionTrajectory[i].stateIneqConstraint = ocs2::vector_t::Random(2); - metricsCollectionTrajectory[i].stateInputIneqConstraint = ocs2::vector_t::Random(3); ocs2::random(stateEqTermsSize, metricsCollectionTrajectory[i].stateEqLagrangian); ocs2::random(stateIneqTermsSize, metricsCollectionTrajectory[i].stateIneqLagrangian); ocs2::random(stateInputEqTermsSize, metricsCollectionTrajectory[i].stateInputEqLagrangian); diff --git a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index d51f121fb..ee45978ee 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -56,20 +56,6 @@ struct PerformanceIndex { */ scalar_t equalityConstraintsSSE = 0.0; - /** Sum of Squared Error (SSE) of inequality constraints: - * - Final: squared norm of violation in state inequality constraints - * - PreJumps: sum of squared norm of violation in state inequality constraints - * - Intermediates: Integral of squared norm violation in state/state-input inequality constraints - */ - scalar_t inequalityConstraintsSSE = 0.0; - - /** Sum of Squared Error (SSE) of the dual feasibilities: - * - Final: squared norm of violation in the dual feasibilities - * - PreJumps: sum of squared norm of violation in the dual feasibilities - * - Intermediates: sum of squared norm of violation in the dual feasibilities - */ - scalar_t dualFeasibilitiesSSE = 0.0; - /** Sum of equality Lagrangians: * - Final: penalty for violation in state equality constraints * - PreJumps: penalty for violation in state equality constraints @@ -90,8 +76,6 @@ struct PerformanceIndex { this->cost += rhs.cost; this->dynamicsViolationSSE += rhs.dynamicsViolationSSE; this->equalityConstraintsSSE += rhs.equalityConstraintsSSE; - this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE; - this->dualFeasibilitiesSSE += rhs.dualFeasibilitiesSSE; this->equalityLagrangian += rhs.equalityLagrangian; this->inequalityLagrangian += rhs.inequalityLagrangian; return *this; @@ -109,8 +93,6 @@ inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { std::swap(lhs.cost, rhs.cost); std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE); std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE); - std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE); - std::swap(lhs.dualFeasibilitiesSSE, rhs.dualFeasibilitiesSSE); std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian); std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian); } @@ -128,10 +110,6 @@ inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& pe stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE; stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE << '\n'; - stream << std::setw(indentation) << ""; - stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE; - stream << "Dual feasibilities SSE: " << std::setw(tabSpace) << performanceIndex.dualFeasibilitiesSSE << '\n'; - stream << std::setw(indentation) << ""; stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian; stream << "Inequality Lagrangian: " << std::setw(tabSpace) << performanceIndex.inequalityLagrangian; diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h index b20fe2ec2..64e53b6c1 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h @@ -76,16 +76,6 @@ struct OptimalControlProblem { /** Final equality constraints */ std::unique_ptr<StateConstraintCollection> finalEqualityConstraintPtr; - /* Inequality Constraints */ - /** Intermediate inequality constraints */ - std::unique_ptr<StateInputConstraintCollection> inequalityConstraintPtr; - /** Intermediate state-only inequality constraints */ - std::unique_ptr<StateConstraintCollection> stateInequalityConstraintPtr; - /** Pre-jump inequality constraints */ - std::unique_ptr<StateConstraintCollection> preJumpInequalityConstraintPtr; - /** Final inequality constraints */ - std::unique_ptr<StateConstraintCollection> finalInequalityConstraintPtr; - /* Lagrangians */ /** Lagrangian for intermediate equality constraints */ std::unique_ptr<StateInputAugmentedLagrangianCollection> equalityLagrangianPtr; diff --git a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp index 6eb0cc2c9..d21711fa1 100644 --- a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp @@ -47,15 +47,6 @@ OptimalControlProblem create(const OptimalControlProblem& problem, std::shared_p LoopshapingConstraint::create(*problem.preJumpEqualityConstraintPtr, loopshapingDefinition); augmentedProblem.finalEqualityConstraintPtr = LoopshapingConstraint::create(*problem.finalEqualityConstraintPtr, loopshapingDefinition); - // Inequality constraints - augmentedProblem.inequalityConstraintPtr = LoopshapingConstraint::create(*problem.inequalityConstraintPtr, loopshapingDefinition); - augmentedProblem.stateInequalityConstraintPtr = - LoopshapingConstraint::create(*problem.stateInequalityConstraintPtr, loopshapingDefinition); - augmentedProblem.preJumpInequalityConstraintPtr = - LoopshapingConstraint::create(*problem.preJumpInequalityConstraintPtr, loopshapingDefinition); - augmentedProblem.finalInequalityConstraintPtr = - LoopshapingConstraint::create(*problem.finalInequalityConstraintPtr, loopshapingDefinition); - // Lagrangians augmentedProblem.equalityLagrangianPtr = LoopshapingAugmentedLagrangian::create(*problem.equalityLagrangianPtr, loopshapingDefinition); augmentedProblem.stateEqualityLagrangianPtr = diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp index 53580c7e3..8ede3a4c4 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp @@ -50,11 +50,6 @@ OptimalControlProblem::OptimalControlProblem() stateEqualityConstraintPtr(new StateConstraintCollection), preJumpEqualityConstraintPtr(new StateConstraintCollection), finalEqualityConstraintPtr(new StateConstraintCollection), - /* Inequality constraints */ - inequalityConstraintPtr(new StateInputConstraintCollection), - stateInequalityConstraintPtr(new StateConstraintCollection), - preJumpInequalityConstraintPtr(new StateConstraintCollection), - finalInequalityConstraintPtr(new StateConstraintCollection), /* Lagrangians */ equalityLagrangianPtr(new StateInputAugmentedLagrangianCollection), stateEqualityLagrangianPtr(new StateAugmentedLagrangianCollection), @@ -87,11 +82,6 @@ OptimalControlProblem::OptimalControlProblem(const OptimalControlProblem& other) stateEqualityConstraintPtr(other.stateEqualityConstraintPtr->clone()), preJumpEqualityConstraintPtr(other.preJumpEqualityConstraintPtr->clone()), finalEqualityConstraintPtr(other.finalEqualityConstraintPtr->clone()), - /* Inequality constraints */ - inequalityConstraintPtr(other.inequalityConstraintPtr->clone()), - stateInequalityConstraintPtr(other.stateInequalityConstraintPtr->clone()), - preJumpInequalityConstraintPtr(other.preJumpInequalityConstraintPtr->clone()), - finalInequalityConstraintPtr(other.finalInequalityConstraintPtr->clone()), /* Lagrangians */ equalityLagrangianPtr(other.equalityLagrangianPtr->clone()), stateEqualityLagrangianPtr(other.stateEqualityLagrangianPtr->clone()), @@ -140,12 +130,6 @@ void OptimalControlProblem::swap(OptimalControlProblem& other) noexcept { preJumpEqualityConstraintPtr.swap(other.preJumpEqualityConstraintPtr); finalEqualityConstraintPtr.swap(other.finalEqualityConstraintPtr); - /* Inequality constraints */ - inequalityConstraintPtr.swap(other.inequalityConstraintPtr); - stateInequalityConstraintPtr.swap(other.stateInequalityConstraintPtr); - preJumpInequalityConstraintPtr.swap(other.preJumpInequalityConstraintPtr); - finalInequalityConstraintPtr.swap(other.finalInequalityConstraintPtr); - /* Lagrangians */ equalityLagrangianPtr.swap(other.equalityLagrangianPtr); stateEqualityLagrangianPtr.swap(other.stateEqualityLagrangianPtr); From d30c7054fd21ce60b0456e9d8dd80100a44163af Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 22 Nov 2022 12:13:21 +0100 Subject: [PATCH 338/512] add ProjectionMultiplierCoefficients --- .../include/ocs2_sqp/ConstraintProjection.h | 30 +++++++++++++++++-- .../ocs2_sqp/src/ConstraintProjection.cpp | 16 ++++++++++ ocs2_sqp/ocs2_sqp/test/testProjection.cpp | 18 +++++++++++ 3 files changed, 61 insertions(+), 3 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h index 0e4049bc0..ef4d1119d 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h @@ -42,7 +42,7 @@ namespace ocs2 { * Implementation based on the QR decomposition * * @param constraint : C = dfdx, D = dfdu, e = f; - * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and pseudo inverse of D (second); + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and pseudo inverse of D^T (second); */ std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint); @@ -55,10 +55,34 @@ std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(co * Implementation based on the LU decomposition * * @param constraint : C = dfdx, D = dfdu, e = f; - * @param extractPseudoInverse : If true, pseudo inverse of D is returned. If false, an empty matrix is returned. - * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and pseudo inverse of D (second); + * @param extractPseudoInverse : If true, pseudo inverse of D^T is returned. If false, an empty matrix is returned. + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and pseudo inverse of D^T (second); */ std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, bool extractPseudoInverse = false); +/** + * Coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint such that + * dfdx*dx + dfdu*du + dfdcostate*dcostate + f + */ +struct ProjectionMultiplierCoefficients { + matrix_t dfdx; + matrix_t dfdu; + matrix_t dfdcostate; + vector_t f; +}; + +/** + * Extracts the coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint. + * + * @param dynamics : Dynamics + * @param cost : Cost + * @param constraintProjection : Constraint projection. + * @param pseudoInverse : Pseudo inverse of D^T of the state-input equality constraint. + */ +ProjectionMultiplierCoefficients extractProjectionMultiplierCoefficients(const VectorFunctionLinearApproximation& dynamics, + const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& constraintProjection, + const matrix_t& pseudoInverse); + } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp index db3f117af..af3b6d816 100644 --- a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp @@ -69,4 +69,20 @@ std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(co return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); } +ProjectionMultiplierCoefficients extractProjectionMultiplierCoefficients(const VectorFunctionLinearApproximation& dynamics, + const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& constraintProjection, + const matrix_t& pseudoInverse) { + const auto& Px = constraintProjection.dfdx; + const auto& Pu = constraintProjection.dfdu; + const auto& u0 = constraintProjection.f; + + ProjectionMultiplierCoefficients multiplierCoefficients; + multiplierCoefficients.dfdx.noalias() = -pseudoInverse * (cost.dfdux + cost.dfduu * Px); + multiplierCoefficients.dfdu.noalias() = -pseudoInverse * (cost.dfduu * Pu); + multiplierCoefficients.dfdcostate.noalias() = -pseudoInverse * dynamics.dfdu.transpose(); + multiplierCoefficients.f.noalias() = pseudoInverse * (cost.dfdu + cost.dfduu * u0); + return multiplierCoefficients; +} + } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp index 5845a54f7..0ecb7d097 100644 --- a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp @@ -88,4 +88,22 @@ TEST(test_projection, testProjectionLU) { ASSERT_TRUE((pseudoInverse*constraint.dfdu.transpose()).isIdentity()); ASSERT_TRUE((pseudoInverse.transpose()*constraint.dfdx).isApprox(- projection.dfdx)); ASSERT_TRUE((pseudoInverse.transpose()*constraint.f).isApprox(- projection.f)); +} + +TEST(test_projection, testProjectionMultiplierCoefficients) { + const size_t stateDim = 30; + const size_t inputDim = 20; + const size_t constraintDim = 10; + + const auto cost = ocs2::getRandomCost(stateDim, inputDim); + const auto dynamics = ocs2::getRandomDynamics(stateDim, inputDim); + const auto constraint = ocs2::getRandomConstraints(stateDim, inputDim, constraintDim); + + auto result = ocs2::qrConstraintProjection(constraint); + const auto projection = std::move(result.first); + const auto pseudoInverse = std::move(result.second); + + ASSERT_NO_THROW( + const auto projectionMultiplierCoefficients = extractProjectionMultiplierCoefficients(dynamics, cost, projection, pseudoInverse); + ); } \ No newline at end of file From 2b9a837315743d05b857d2b98b6200b623b26e3f Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 22 Nov 2022 15:05:08 +0100 Subject: [PATCH 339/512] modify constraint projection and its tests --- .../include/ocs2_sqp/ConstraintProjection.h | 8 ++++---- .../ocs2_sqp/src/ConstraintProjection.cpp | 20 +++++++++++-------- ocs2_sqp/ocs2_sqp/test/testProjection.cpp | 16 +++++++++++---- 3 files changed, 28 insertions(+), 16 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h index ef4d1119d..d959762c8 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h @@ -42,7 +42,7 @@ namespace ocs2 { * Implementation based on the QR decomposition * * @param constraint : C = dfdx, D = dfdu, e = f; - * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and pseudo inverse of D^T (second); + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second); */ std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint); @@ -55,8 +55,8 @@ std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(co * Implementation based on the LU decomposition * * @param constraint : C = dfdx, D = dfdu, e = f; - * @param extractPseudoInverse : If true, pseudo inverse of D^T is returned. If false, an empty matrix is returned. - * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and pseudo inverse of D^T (second); + * @param extractPseudoInverse : If true, left pseudo-inverse of D^T is returned. If false, an empty matrix is returned; + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second); */ std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, bool extractPseudoInverse = false); @@ -78,7 +78,7 @@ struct ProjectionMultiplierCoefficients { * @param dynamics : Dynamics * @param cost : Cost * @param constraintProjection : Constraint projection. - * @param pseudoInverse : Pseudo inverse of D^T of the state-input equality constraint. + * @param pseudoInverse : Left pseudo-inverse of D^T of the state-input equality constraint. */ ProjectionMultiplierCoefficients extractProjectionMultiplierCoefficients(const VectorFunctionLinearApproximation& dynamics, const ScalarFunctionQuadraticApproximation& cost, diff --git a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp index af3b6d816..d5764ee7c 100644 --- a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp @@ -41,7 +41,7 @@ std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(co const auto Q1 = Q.leftCols(numConstraints); const auto R = QRof_DT.matrixQR().topRows(numConstraints).triangularView<Eigen::Upper>(); - const matrix_t pseudoInverse = R.solve(Q1.transpose()); + const matrix_t pseudoInverse = R.solve(Q1.transpose()); // left pseudo-inverse of D^T VectorFunctionLinearApproximation projectionTerms; projectionTerms.dfdu = Q.rightCols(numInputs - numConstraints); @@ -63,7 +63,7 @@ std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(co matrix_t pseudoInverse; if (extractPseudoInverse) { - pseudoInverse = lu.solve(matrix_t::Identity(constraint.f.size(), constraint.f.size())).transpose(); + pseudoInverse = lu.solve(matrix_t::Identity(constraint.f.size(), constraint.f.size())).transpose(); // left pseudo-inverse of D^T } return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); @@ -73,15 +73,19 @@ ProjectionMultiplierCoefficients extractProjectionMultiplierCoefficients(const V const ScalarFunctionQuadraticApproximation& cost, const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse) { - const auto& Px = constraintProjection.dfdx; - const auto& Pu = constraintProjection.dfdu; - const auto& u0 = constraintProjection.f; + vector_t projectedCost_dfdu = cost.dfdu; + projectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f; + + matrix_t projectedCost_dfdux = cost.dfdux; + projectedCost_dfdux.noalias() += cost.dfduu * constraintProjection.dfdx; + + const matrix_t projectedCost_dfduu = cost.dfduu * constraintProjection.dfdu; ProjectionMultiplierCoefficients multiplierCoefficients; - multiplierCoefficients.dfdx.noalias() = -pseudoInverse * (cost.dfdux + cost.dfduu * Px); - multiplierCoefficients.dfdu.noalias() = -pseudoInverse * (cost.dfduu * Pu); + multiplierCoefficients.dfdx.noalias() = -pseudoInverse * projectedCost_dfdux; + multiplierCoefficients.dfdu.noalias() = -pseudoInverse * projectedCost_dfduu; multiplierCoefficients.dfdcostate.noalias() = -pseudoInverse * dynamics.dfdu.transpose(); - multiplierCoefficients.f.noalias() = pseudoInverse * (cost.dfdu + cost.dfduu * u0); + multiplierCoefficients.f.noalias() = -pseudoInverse * projectedCost_dfdu; return multiplierCoefficients; } diff --git a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp index 0ecb7d097..8ff923ce8 100644 --- a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp @@ -103,7 +103,15 @@ TEST(test_projection, testProjectionMultiplierCoefficients) { const auto projection = std::move(result.first); const auto pseudoInverse = std::move(result.second); - ASSERT_NO_THROW( - const auto projectionMultiplierCoefficients = extractProjectionMultiplierCoefficients(dynamics, cost, projection, pseudoInverse); - ); -} \ No newline at end of file + const auto projectionMultiplierCoefficients = extractProjectionMultiplierCoefficients(dynamics, cost, projection, pseudoInverse); + + const ocs2::matrix_t dfdx = -pseudoInverse * (cost.dfdux + cost.dfduu * projection.dfdx); + const ocs2::matrix_t dfdu = -pseudoInverse * (cost.dfduu * projection.dfdu); + const ocs2::matrix_t dfdcostate = -pseudoInverse * dynamics.dfdu.transpose(); + const ocs2::vector_t f = -pseudoInverse * (cost.dfdu + cost.dfduu * projection.f); + + ASSERT_TRUE(projectionMultiplierCoefficients.dfdx.isApprox(dfdx)); + ASSERT_TRUE(projectionMultiplierCoefficients.dfdu.isApprox(dfdu)); + ASSERT_TRUE(projectionMultiplierCoefficients.dfdcostate.isApprox(dfdcostate)); + ASSERT_TRUE(projectionMultiplierCoefficients.f.isApprox(f)); +} From 0886ad071edae69f1d78000ed692f628440459bc Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 23 Nov 2022 10:57:54 +0100 Subject: [PATCH 340/512] renamed semiprojected terms --- ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp index d5764ee7c..7af0652aa 100644 --- a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp @@ -73,19 +73,19 @@ ProjectionMultiplierCoefficients extractProjectionMultiplierCoefficients(const V const ScalarFunctionQuadraticApproximation& cost, const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse) { - vector_t projectedCost_dfdu = cost.dfdu; - projectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f; + vector_t semiprojectedCost_dfdu = cost.dfdu; + semiprojectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f; - matrix_t projectedCost_dfdux = cost.dfdux; - projectedCost_dfdux.noalias() += cost.dfduu * constraintProjection.dfdx; + matrix_t semiprojectedCost_dfdux = cost.dfdux; + semiprojectedCost_dfdux.noalias() += cost.dfduu * constraintProjection.dfdx; - const matrix_t projectedCost_dfduu = cost.dfduu * constraintProjection.dfdu; + const matrix_t semiprojectedCost_dfduu = cost.dfduu * constraintProjection.dfdu; ProjectionMultiplierCoefficients multiplierCoefficients; - multiplierCoefficients.dfdx.noalias() = -pseudoInverse * projectedCost_dfdux; - multiplierCoefficients.dfdu.noalias() = -pseudoInverse * projectedCost_dfduu; + multiplierCoefficients.dfdx.noalias() = -pseudoInverse * semiprojectedCost_dfdux; + multiplierCoefficients.dfdu.noalias() = -pseudoInverse * semiprojectedCost_dfduu; multiplierCoefficients.dfdcostate.noalias() = -pseudoInverse * dynamics.dfdu.transpose(); - multiplierCoefficients.f.noalias() = -pseudoInverse * projectedCost_dfdu; + multiplierCoefficients.f.noalias() = -pseudoInverse * semiprojectedCost_dfdu; return multiplierCoefficients; } From 50feabe3375f0113cf705ed2fb4a99ec5954ef95 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 23 Nov 2022 11:26:24 +0100 Subject: [PATCH 341/512] checkout from main --- .../include/ocs2_core/model_data/Metrics.h | 10 +++++++++ ocs2_core/src/model_data/Metrics.cpp | 8 +++++++ ocs2_core/test/model_data/testMetrics.cpp | 13 +++++++++++ .../ocs2_oc/oc_data/PerformanceIndex.h | 22 +++++++++++++++++++ .../oc_problem/OptimalControlProblem.h | 10 +++++++++ .../LoopshapingOptimalControlProblem.cpp | 9 ++++++++ .../src/oc_problem/OptimalControlProblem.cpp | 16 ++++++++++++++ 7 files changed, 88 insertions(+) diff --git a/ocs2_core/include/ocs2_core/model_data/Metrics.h b/ocs2_core/include/ocs2_core/model_data/Metrics.h index 2d205d377..3b5be5e4c 100644 --- a/ocs2_core/include/ocs2_core/model_data/Metrics.h +++ b/ocs2_core/include/ocs2_core/model_data/Metrics.h @@ -71,6 +71,10 @@ struct MetricsCollection { vector_t stateEqConstraint; vector_t stateInputEqConstraint; + // Inequality constraints + vector_t stateIneqConstraint; + vector_t stateInputIneqConstraint; + // Lagrangians std::vector<LagrangianMetrics> stateEqLagrangian; std::vector<LagrangianMetrics> stateIneqLagrangian; @@ -84,6 +88,9 @@ struct MetricsCollection { // Equality constraints stateEqConstraint.swap(other.stateEqConstraint); stateInputEqConstraint.swap(other.stateInputEqConstraint); + // Inequality constraints + stateIneqConstraint.swap(other.stateIneqConstraint); + stateInputIneqConstraint.swap(other.stateInputIneqConstraint); // Lagrangians stateEqLagrangian.swap(other.stateEqLagrangian); stateIneqLagrangian.swap(other.stateIneqLagrangian); @@ -98,6 +105,9 @@ struct MetricsCollection { // Equality constraints stateEqConstraint = vector_t(); stateInputEqConstraint = vector_t(); + // Inequality constraints + stateIneqConstraint = vector_t(); + stateInputIneqConstraint = vector_t(); // Lagrangians stateEqLagrangian.clear(); stateIneqLagrangian.clear(); diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 8d1066934..980eb6ab1 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -121,6 +121,14 @@ MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector interpolate(indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); + // inequality constraints + out.stateIneqConstraint = + interpolate(indexAlpha, dataArray, + [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateIneqConstraint; }); + out.stateInputIneqConstraint = interpolate( + indexAlpha, dataArray, + [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqConstraint; }); + // state equality Lagrangian out.stateEqLagrangian.reserve(mumStateEq); for (size_t i = 0; i < mumStateEq; i++) { diff --git a/ocs2_core/test/model_data/testMetrics.cpp b/ocs2_core/test/model_data/testMetrics.cpp index 40866541c..4bce431ae 100644 --- a/ocs2_core/test/model_data/testMetrics.cpp +++ b/ocs2_core/test/model_data/testMetrics.cpp @@ -83,6 +83,13 @@ inline MetricsCollection interpolateNew(const LinearInterpolation::index_alpha_t out.stateInputEqConstraint = LinearInterpolation::interpolate( indexAlpha, dataArray, [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); + // inequality constraints + out.stateIneqConstraint = LinearInterpolation::interpolate( + indexAlpha, dataArray, + [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateIneqConstraint; }); + out.stateInputIneqConstraint = LinearInterpolation::interpolate( + indexAlpha, dataArray, + [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqConstraint; }); out.stateEqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateEqLagrangian), f(lhs_stateEq, rhs_stateEq)); out.stateIneqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateIneqLagrangian), f(lhs_stateIneq, rhs_stateIneq)); out.stateInputEqLagrangian = @@ -110,6 +117,8 @@ bool isApprox(const MetricsCollection& lhs, const MetricsCollection& rhs, scalar bool flag = std::abs(lhs.cost - rhs.cost) < prec; flag = flag && lhs.stateEqConstraint.isApprox(rhs.stateEqConstraint, prec); flag = flag && lhs.stateInputEqConstraint.isApprox(rhs.stateInputEqConstraint, prec); + flag = flag && lhs.stateIneqConstraint.isApprox(rhs.stateIneqConstraint, prec); + flag = flag && lhs.stateInputIneqConstraint.isApprox(rhs.stateInputIneqConstraint, prec); flag = flag && toVector(lhs.stateEqLagrangian).isApprox(toVector(rhs.stateEqLagrangian), prec); flag = flag && toVector(lhs.stateIneqLagrangian).isApprox(toVector(rhs.stateIneqLagrangian), prec); flag = flag && toVector(lhs.stateInputEqLagrangian).isApprox(toVector(rhs.stateInputEqLagrangian), prec); @@ -141,6 +150,8 @@ TEST(TestMetrics, testSwap) { termsMetricsCollection.cost = ocs2::vector_t::Random(1)(0); termsMetricsCollection.stateEqConstraint = ocs2::vector_t::Random(2); termsMetricsCollection.stateInputEqConstraint = ocs2::vector_t::Random(3); + termsMetricsCollection.stateIneqConstraint = ocs2::vector_t::Random(2); + termsMetricsCollection.stateInputIneqConstraint = ocs2::vector_t::Random(3); ocs2::random(termsSize, termsMetricsCollection.stateEqLagrangian); ocs2::random(termsSize, termsMetricsCollection.stateIneqLagrangian); ocs2::random(termsSize, termsMetricsCollection.stateInputEqLagrangian); @@ -176,6 +187,8 @@ TEST(TestMetrics, testInterpolation) { metricsCollectionTrajectory[i].cost = ocs2::vector_t::Random(1)(0); metricsCollectionTrajectory[i].stateEqConstraint = ocs2::vector_t::Random(2); metricsCollectionTrajectory[i].stateInputEqConstraint = ocs2::vector_t::Random(3); + metricsCollectionTrajectory[i].stateIneqConstraint = ocs2::vector_t::Random(2); + metricsCollectionTrajectory[i].stateInputIneqConstraint = ocs2::vector_t::Random(3); ocs2::random(stateEqTermsSize, metricsCollectionTrajectory[i].stateEqLagrangian); ocs2::random(stateIneqTermsSize, metricsCollectionTrajectory[i].stateIneqLagrangian); ocs2::random(stateInputEqTermsSize, metricsCollectionTrajectory[i].stateInputEqLagrangian); diff --git a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index ee45978ee..d51f121fb 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -56,6 +56,20 @@ struct PerformanceIndex { */ scalar_t equalityConstraintsSSE = 0.0; + /** Sum of Squared Error (SSE) of inequality constraints: + * - Final: squared norm of violation in state inequality constraints + * - PreJumps: sum of squared norm of violation in state inequality constraints + * - Intermediates: Integral of squared norm violation in state/state-input inequality constraints + */ + scalar_t inequalityConstraintsSSE = 0.0; + + /** Sum of Squared Error (SSE) of the dual feasibilities: + * - Final: squared norm of violation in the dual feasibilities + * - PreJumps: sum of squared norm of violation in the dual feasibilities + * - Intermediates: sum of squared norm of violation in the dual feasibilities + */ + scalar_t dualFeasibilitiesSSE = 0.0; + /** Sum of equality Lagrangians: * - Final: penalty for violation in state equality constraints * - PreJumps: penalty for violation in state equality constraints @@ -76,6 +90,8 @@ struct PerformanceIndex { this->cost += rhs.cost; this->dynamicsViolationSSE += rhs.dynamicsViolationSSE; this->equalityConstraintsSSE += rhs.equalityConstraintsSSE; + this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE; + this->dualFeasibilitiesSSE += rhs.dualFeasibilitiesSSE; this->equalityLagrangian += rhs.equalityLagrangian; this->inequalityLagrangian += rhs.inequalityLagrangian; return *this; @@ -93,6 +109,8 @@ inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { std::swap(lhs.cost, rhs.cost); std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE); std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE); + std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE); + std::swap(lhs.dualFeasibilitiesSSE, rhs.dualFeasibilitiesSSE); std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian); std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian); } @@ -110,6 +128,10 @@ inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& pe stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE; stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE << '\n'; + stream << std::setw(indentation) << ""; + stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE; + stream << "Dual feasibilities SSE: " << std::setw(tabSpace) << performanceIndex.dualFeasibilitiesSSE << '\n'; + stream << std::setw(indentation) << ""; stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian; stream << "Inequality Lagrangian: " << std::setw(tabSpace) << performanceIndex.inequalityLagrangian; diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h index 64e53b6c1..b20fe2ec2 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h @@ -76,6 +76,16 @@ struct OptimalControlProblem { /** Final equality constraints */ std::unique_ptr<StateConstraintCollection> finalEqualityConstraintPtr; + /* Inequality Constraints */ + /** Intermediate inequality constraints */ + std::unique_ptr<StateInputConstraintCollection> inequalityConstraintPtr; + /** Intermediate state-only inequality constraints */ + std::unique_ptr<StateConstraintCollection> stateInequalityConstraintPtr; + /** Pre-jump inequality constraints */ + std::unique_ptr<StateConstraintCollection> preJumpInequalityConstraintPtr; + /** Final inequality constraints */ + std::unique_ptr<StateConstraintCollection> finalInequalityConstraintPtr; + /* Lagrangians */ /** Lagrangian for intermediate equality constraints */ std::unique_ptr<StateInputAugmentedLagrangianCollection> equalityLagrangianPtr; diff --git a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp index d21711fa1..6eb0cc2c9 100644 --- a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp @@ -47,6 +47,15 @@ OptimalControlProblem create(const OptimalControlProblem& problem, std::shared_p LoopshapingConstraint::create(*problem.preJumpEqualityConstraintPtr, loopshapingDefinition); augmentedProblem.finalEqualityConstraintPtr = LoopshapingConstraint::create(*problem.finalEqualityConstraintPtr, loopshapingDefinition); + // Inequality constraints + augmentedProblem.inequalityConstraintPtr = LoopshapingConstraint::create(*problem.inequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.stateInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.stateInequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.preJumpInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.preJumpInequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.finalInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.finalInequalityConstraintPtr, loopshapingDefinition); + // Lagrangians augmentedProblem.equalityLagrangianPtr = LoopshapingAugmentedLagrangian::create(*problem.equalityLagrangianPtr, loopshapingDefinition); augmentedProblem.stateEqualityLagrangianPtr = diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp index 8ede3a4c4..53580c7e3 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp @@ -50,6 +50,11 @@ OptimalControlProblem::OptimalControlProblem() stateEqualityConstraintPtr(new StateConstraintCollection), preJumpEqualityConstraintPtr(new StateConstraintCollection), finalEqualityConstraintPtr(new StateConstraintCollection), + /* Inequality constraints */ + inequalityConstraintPtr(new StateInputConstraintCollection), + stateInequalityConstraintPtr(new StateConstraintCollection), + preJumpInequalityConstraintPtr(new StateConstraintCollection), + finalInequalityConstraintPtr(new StateConstraintCollection), /* Lagrangians */ equalityLagrangianPtr(new StateInputAugmentedLagrangianCollection), stateEqualityLagrangianPtr(new StateAugmentedLagrangianCollection), @@ -82,6 +87,11 @@ OptimalControlProblem::OptimalControlProblem(const OptimalControlProblem& other) stateEqualityConstraintPtr(other.stateEqualityConstraintPtr->clone()), preJumpEqualityConstraintPtr(other.preJumpEqualityConstraintPtr->clone()), finalEqualityConstraintPtr(other.finalEqualityConstraintPtr->clone()), + /* Inequality constraints */ + inequalityConstraintPtr(other.inequalityConstraintPtr->clone()), + stateInequalityConstraintPtr(other.stateInequalityConstraintPtr->clone()), + preJumpInequalityConstraintPtr(other.preJumpInequalityConstraintPtr->clone()), + finalInequalityConstraintPtr(other.finalInequalityConstraintPtr->clone()), /* Lagrangians */ equalityLagrangianPtr(other.equalityLagrangianPtr->clone()), stateEqualityLagrangianPtr(other.stateEqualityLagrangianPtr->clone()), @@ -130,6 +140,12 @@ void OptimalControlProblem::swap(OptimalControlProblem& other) noexcept { preJumpEqualityConstraintPtr.swap(other.preJumpEqualityConstraintPtr); finalEqualityConstraintPtr.swap(other.finalEqualityConstraintPtr); + /* Inequality constraints */ + inequalityConstraintPtr.swap(other.inequalityConstraintPtr); + stateInequalityConstraintPtr.swap(other.stateInequalityConstraintPtr); + preJumpInequalityConstraintPtr.swap(other.preJumpInequalityConstraintPtr); + finalInequalityConstraintPtr.swap(other.finalInequalityConstraintPtr); + /* Lagrangians */ equalityLagrangianPtr.swap(other.equalityLagrangianPtr); stateEqualityLagrangianPtr.swap(other.stateEqualityLagrangianPtr); From 0ea286c79756b730b2175b3cf0b7dc9ec0e56fbc Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 23 Nov 2022 15:20:31 +0100 Subject: [PATCH 342/512] swap the initialization order --- ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 99e0a70c0..2eb1e1273 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -154,16 +154,16 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat const auto& eventTimes = this->getReferenceManager().getModeSchedule().eventTimes; const auto timeDiscretization = timeDiscretizationWithEvents(initTime, finalTime, settings_.dt, eventTimes); - // Initialize the state and input - vector_array_t x, u; - initializeStateInputTrajectories(initState, timeDiscretization, x, u); - // Initialize references for (auto& ocpDefinition : ocpDefinitions_) { const auto& targetTrajectories = this->getReferenceManager().getTargetTrajectories(); ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } + // Initialize the state and input + vector_array_t x, u; + initializeStateInputTrajectories(initState, timeDiscretization, x, u); + // Bookkeeping performanceIndeces_.clear(); From 339ca8cd318f558d88ff6694febd5c5fcf128f2d Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 23 Nov 2022 16:12:26 +0100 Subject: [PATCH 343/512] made acceptStep const member function instead of lambda function --- .../include/ocs2_sqp/MultipleShootingSolver.h | 5 ++ .../ocs2_sqp/src/MultipleShootingSolver.cpp | 47 +++++++++++-------- 2 files changed, 32 insertions(+), 20 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index 665fa58ee..fa8df73a0 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -160,6 +160,11 @@ class MultipleShootingSolver : public SolverBase { const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u); + /** Checks that the trial step is accepted */ + std::pair<bool, multiple_shooting::StepInfo::StepType> acceptStep(const PerformanceIndex& baselinePerformance, + const PerformanceIndex& stepPerformance, scalar_t armijoDescentMetric, + scalar_t alpha) const; + /** Determine convergence after a step */ multiple_shooting::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const multiple_shooting::StepInfo& stepInfo) const; diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 99e0a70c0..fffc7d936 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -551,26 +551,10 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn // Compute cost and constraints const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew); - const scalar_t newConstraintViolation = totalConstraintViolation(performanceNew); // Step acceptance and record step type - const bool stepAccepted = [&]() { - if (newConstraintViolation > settings_.g_max) { - // High constraint violation. Only accept decrease in constraints. - stepInfo.stepType = StepType::CONSTRAINT; - return newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); - } else if (newConstraintViolation < settings_.g_min && baselineConstraintViolation < settings_.g_min && - subproblemSolution.armijoDescentMetric < 0.0) { - // With low violation and having a descent direction, require the armijo condition. - stepInfo.stepType = StepType::COST; - return performanceNew.merit < (baseline.merit + settings_.armijoFactor * alpha * subproblemSolution.armijoDescentMetric); - } else { - // Medium violation: either merit or constraints decrease (with small gamma_c mixing of old constraints) - stepInfo.stepType = StepType::DUAL; - return performanceNew.merit < (baseline.merit - settings_.gamma_c * baselineConstraintViolation) || - newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); - } - }(); + bool stepAccepted; + std::tie(stepAccepted, stepInfo.stepType) = acceptStep(baseline, performanceNew, subproblemSolution.armijoDescentMetric, alpha); if (settings_.printLinesearch) { std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepInfo.stepType) @@ -587,7 +571,7 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn stepInfo.dx_norm = alpha * deltaXnorm; stepInfo.du_norm = alpha * deltaUnorm; stepInfo.performanceAfterStep = performanceNew; - stepInfo.totalConstraintViolationAfterStep = newConstraintViolation; + stepInfo.totalConstraintViolationAfterStep = totalConstraintViolation(performanceNew); return stepInfo; } else { // Try smaller step alpha *= settings_.alpha_decay; @@ -609,7 +593,7 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn stepInfo.dx_norm = 0.0; stepInfo.du_norm = 0.0; stepInfo.performanceAfterStep = baseline; - stepInfo.totalConstraintViolationAfterStep = baselineConstraintViolation; + stepInfo.totalConstraintViolationAfterStep = totalConstraintViolation(baseline); if (settings_.printLinesearch) { std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; @@ -618,6 +602,29 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn return stepInfo; } +std::pair<bool, multiple_shooting::StepInfo::StepType> MultipleShootingSolver::acceptStep(const PerformanceIndex& baselinePerformance, + const PerformanceIndex& stepPerformance, + scalar_t armijoDescentMetric, + scalar_t alpha) const { + const scalar_t baselineConstraintViolation = totalConstraintViolation(baselinePerformance); + const scalar_t newConstraintViolation = totalConstraintViolation(stepPerformance); + // Step acceptance and record step type + if (newConstraintViolation > settings_.g_max) { + // High constraint violation. Only accept decrease in constraints. + const bool accepted = newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); + return std::make_pair(accepted, multiple_shooting::StepInfo::StepType::CONSTRAINT); + } else if (newConstraintViolation < settings_.g_min && baselineConstraintViolation < settings_.g_min && armijoDescentMetric < 0.0) { + // With low violation and having a descent direction, require the armijo condition. + const bool accepted = stepPerformance.merit < (baselinePerformance.merit + settings_.armijoFactor * alpha * armijoDescentMetric); + return std::make_pair(accepted, multiple_shooting::StepInfo::StepType::COST); + } else { + // Medium violation: either merit or constraints decrease (with small gamma_c mixing of old constraints) + const bool accepted = stepPerformance.merit < (baselinePerformance.merit - settings_.gamma_c * baselineConstraintViolation) || + newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); + return std::make_pair(accepted, multiple_shooting::StepInfo::StepType::DUAL); + } +} + multiple_shooting::Convergence MultipleShootingSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const multiple_shooting::StepInfo& stepInfo) const { using Convergence = multiple_shooting::Convergence; From 85e196aa6660a61d19ca3689acf6fdc55887cd80 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Fri, 25 Nov 2022 17:41:27 +0100 Subject: [PATCH 344/512] add inequality constraints evaluations in transcriptions --- .../ocs2_sqp/MultipleShootingTranscription.h | 12 ++- .../src/MultipleShootingTranscription.cpp | 85 ++++++++++++++++++- 2 files changed, 92 insertions(+), 5 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h index 9e25e814d..aa32e4914 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -46,6 +46,8 @@ struct Transcription { ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; VectorFunctionLinearApproximation constraintsProjection; + VectorFunctionLinearApproximation stateIneqConstraints; + VectorFunctionLinearApproximation stateInputIneqConstraints; }; /** @@ -59,18 +61,22 @@ struct Transcription { * @param x : State at start of the interval * @param x_next : State at the end of the interval * @param u : Input, taken to be constant across the interval. + * @param enableStateInequalityConstraint : Flag to enalbe the state-only inequality constraint. The constraint must be disabled at the + * initial node. * @return multiple shooting transcription for this node. */ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + bool enableStateInequalityConstraint = true); /** * Compute only the performance index for a single intermediate node. * Corresponds to the performance index returned by "setupIntermediateNode" */ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + bool enableStateInequalityConstraint = true); /** * Results of the transcription at a terminal node @@ -79,6 +85,7 @@ struct TerminalTranscription { PerformanceIndex performance; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation stateIneqConstraints; }; /** @@ -105,6 +112,7 @@ struct EventTranscription { VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation stateIneqConstraints; }; /** diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index 7f294ee1e..37e958769 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -37,9 +37,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { +scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { + return ineqConstraint.cwiseMin(0.0).matrix().squaredNorm(); +} + Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + bool enableStateInequalityConstraint) { // Results and short-hand notation Transcription transcription; auto& dynamics = transcription.dynamics; @@ -47,6 +52,8 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP auto& cost = transcription.cost; auto& constraints = transcription.constraints; auto& projection = transcription.constraintsProjection; + auto& stateIneqConstraints = transcription.stateIneqConstraints; + auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; // Dynamics // Discretization returns x_{k+1} = A_{k} * dx_{k} + B_{k} * du_{k} + b_{k} @@ -63,6 +70,20 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP cost *= dt; performance.cost = cost.f; + // State inequality constraints. Should be disabled at the initial time of the horizon. + if (!optimalControlProblem.stateInequalityConstraintPtr->empty() && enableStateInequalityConstraint) { + stateIneqConstraints = + optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints.f); + } + + // State-input inequality constraints. + if (!optimalControlProblem.inequalityConstraintPtr->empty()) { + stateInputIneqConstraints = + optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); + performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints.f); + } + // Constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} = 0 @@ -74,9 +95,12 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP projection = luConstraintProjection(constraints).first; constraints = VectorFunctionLinearApproximation(); - // Adapt dynamics and cost + // Adapt dynamics, cost, and state-input inequality constraints changeOfInputVariables(dynamics, projection.dfdu, projection.dfdx, projection.f); changeOfInputVariables(cost, projection.dfdu, projection.dfdx, projection.f); + if (stateInputIneqConstraints.f.size() > 0) { + changeOfInputVariables(stateInputIneqConstraints, projection.dfdu, projection.dfdx, projection.f); + } } } } @@ -85,7 +109,8 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP } PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + bool enableStateInequalityConstraint) { PerformanceIndex performance; // Dynamics @@ -100,6 +125,24 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt // Costs performance.cost = dt * computeCost(optimalControlProblem, t, x, u); + // State inequality constraints. Should be disabled at the initial time of the horizon. + if (!optimalControlProblem.stateInequalityConstraintPtr->empty() && enableStateInequalityConstraint) { + const vector_t stateIneqConstraints = + optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + if (stateIneqConstraints.size() > 0) { + performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints); + } + } + + // State-input inequality constraints. + if (!optimalControlProblem.inequalityConstraintPtr->empty()) { + const vector_t stateInputIneqConstraints = + optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); + if (stateInputIneqConstraints.size() > 0) { + performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints); + } + } + // Constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { const vector_t constraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); @@ -117,6 +160,7 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont auto& performance = transcription.performance; auto& cost = transcription.cost; auto& constraints = transcription.constraints; + auto& stateIneqConstraints = transcription.stateIneqConstraints; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); @@ -124,6 +168,15 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont cost = approximateFinalCost(optimalControlProblem, t, x); performance.cost = cost.f; + // State inequality constraints. + if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { + stateIneqConstraints = + optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + if (stateIneqConstraints.f.size() > 0) { + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(stateIneqConstraints.f); + } + } + constraints = VectorFunctionLinearApproximation::Zero(0, x.size()); return transcription; @@ -137,6 +190,14 @@ PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimal performance.cost = computeFinalCost(optimalControlProblem, t, x); + if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { + const vector_t stateIneqConstraints = + optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + if (stateIneqConstraints.size() > 0) { + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(stateIneqConstraints); + } + } + return performance; } @@ -148,6 +209,7 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; auto& constraints = transcription.constraints; + auto& stateIneqConstraints = transcription.stateIneqConstraints; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation; optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); @@ -162,6 +224,15 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro cost = approximateEventCost(optimalControlProblem, t, x); performance.cost = cost.f; + // State inequality constraints. + if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { + stateIneqConstraints = + optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + if (stateIneqConstraints.f.size() > 0) { + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(stateIneqConstraints.f); + } + } + constraints = VectorFunctionLinearApproximation::Zero(0, x.size()); return transcription; } @@ -179,6 +250,14 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon performance.cost = computeEventCost(optimalControlProblem, t, x); + if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { + const vector_t stateIneqConstraints = + optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + if (stateIneqConstraints.size() > 0) { + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(stateIneqConstraints); + } + } + return performance; } From 519d28ee36ad086f8138bb77a83697ccd295c009 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Fri, 25 Nov 2022 17:41:45 +0100 Subject: [PATCH 345/512] add inequality constraints data in SQP solver --- .../ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h | 2 ++ ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index fa8df73a0..be3d12025 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -193,6 +193,8 @@ class MultipleShootingSolver : public SolverBase { std::vector<ScalarFunctionQuadraticApproximation> cost_; std::vector<VectorFunctionLinearApproximation> constraints_; std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + std::vector<VectorFunctionLinearApproximation> stateIneqConstraints_; + std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; // Iteration performance log std::vector<PerformanceIndex> performanceIndeces_; diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 4261ccf68..ac2cbd3d4 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -392,6 +392,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec cost_.resize(N + 1); constraints_.resize(N + 1); constraintsProjection_.resize(N); + stateIneqConstraints_.resize(N + 1); + stateInputIneqConstraints_.resize(N); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { @@ -410,6 +412,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec cost_[i] = std::move(result.cost); constraints_[i] = std::move(result.constraints); constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); + stateInputIneqConstraints_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); @@ -421,6 +425,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec cost_[i] = std::move(result.cost); constraints_[i] = std::move(result.constraints); constraintsProjection_[i] = std::move(result.constraintsProjection); + stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); + stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); } i = timeIndex++; @@ -432,6 +438,7 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec workerPerformance += result.performance; cost_[i] = std::move(result.cost); constraints_[i] = std::move(result.constraints); + stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); } // Accumulate! Same worker might run multiple tasks From 3a083bfdd1dda1f6da9b77631700258278317014 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 28 Nov 2022 10:55:07 +0100 Subject: [PATCH 346/512] add an option for the constraint pseudo inverse in transcription --- .../include/ocs2_sqp/MultipleShootingTranscription.h | 6 ++++-- ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp | 7 +++++-- .../ocs2_sqp/src/MultipleShootingTranscription.cpp | 12 +++++++++--- ocs2_sqp/ocs2_sqp/test/testTranscription.cpp | 2 +- 4 files changed, 19 insertions(+), 8 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h index aa32e4914..a22fd6714 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -46,6 +46,7 @@ struct Transcription { ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; VectorFunctionLinearApproximation constraintsProjection; + matrix_t constraintPseudoInverse; VectorFunctionLinearApproximation stateIneqConstraints; VectorFunctionLinearApproximation stateInputIneqConstraints; }; @@ -56,6 +57,7 @@ struct Transcription { * @param optimalControlProblem : Definition of the optimal control problem * @param sensitivityDiscretizer : Integrator to use for creating the discrete dynamics. * @param projectStateInputEqualityConstraints + * @param extractEqualityConstraintsPseudoInverse * @param t : Start of the discrete interval * @param dt : Duration of the interval * @param x : State at start of the interval @@ -67,8 +69,8 @@ struct Transcription { */ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, - bool enableStateInequalityConstraint = true); + bool extractEqualityConstraintsPseudoInverse, scalar_t t, scalar_t dt, const vector_t& x, + const vector_t& x_next, const vector_t& u, bool enableStateInequalityConstraint = true); /** * Compute only the performance index for a single intermediate node. diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index ac2cbd3d4..6815931be 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -418,8 +418,11 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - auto result = - multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, projection, ti, dt, x[i], x[i + 1], u[i]); + const bool extractEqualityConstraintsPseudoInverse = false; + const bool enableStateInequalityConstraint = (i > 0); + auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, projection, + extractEqualityConstraintsPseudoInverse, ti, dt, x[i], x[i + 1], u[i], + enableStateInequalityConstraint); workerPerformance += result.performance; dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index 37e958769..e88b3ac81 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -43,8 +43,8 @@ scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, - bool enableStateInequalityConstraint) { + bool extractEqualityConstraintsPseudoInverse, scalar_t t, scalar_t dt, const vector_t& x, + const vector_t& x_next, const vector_t& u, bool enableStateInequalityConstraint) { // Results and short-hand notation Transcription transcription; auto& dynamics = transcription.dynamics; @@ -52,6 +52,7 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP auto& cost = transcription.cost; auto& constraints = transcription.constraints; auto& projection = transcription.constraintsProjection; + auto& constraintPseudoInverse = transcription.constraintPseudoInverse; auto& stateIneqConstraints = transcription.stateIneqConstraints; auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; @@ -92,7 +93,12 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP performance.equalityConstraintsSSE = dt * constraints.f.squaredNorm(); if (projectStateInputEqualityConstraints) { // Handle equality constraints using projection. // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. - projection = luConstraintProjection(constraints).first; + if (extractEqualityConstraintsPseudoInverse) { + std::tie(projection, constraintPseudoInverse) = qrConstraintProjection(constraints); + } else { + projection = luConstraintProjection(constraints).first; + constraintPseudoInverse = matrix_t(); + } constraints = VectorFunctionLinearApproximation(); // Adapt dynamics, cost, and state-input inequality constraints diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp index f3c6fdce3..17f539f7e 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp @@ -58,7 +58,7 @@ TEST(test_transcription, intermediate_performance) { const vector_t x = (vector_t(2) << 1.0, 0.1).finished(); const vector_t x_next = (vector_t(2) << 1.1, 0.2).finished(); const vector_t u = (vector_t(2) << 0.1, 1.3).finished(); - const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, true, t, dt, x, x_next, u); + const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, true, true, t, dt, x, x_next, u); const auto performance = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); From bbe371cc54564b9eae1d01501e6fe532be141c97 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 28 Nov 2022 11:07:13 +0100 Subject: [PATCH 347/512] Add an argument of setupIntermediateNode to specify whether to compute the state-only inequality constraints --- ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 6815931be..39c888263 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -479,7 +479,9 @@ PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector<An // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + const bool enableStateInequalityConstraint = (i > 0); + workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i], + enableStateInequalityConstraint); } i = timeIndex++; From 4f052a8e42dc48273523b64ce7685f9f7aa6403b Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 28 Nov 2022 12:43:42 +0100 Subject: [PATCH 348/512] rename and reorder constraints --- .../include/ocs2_sqp/MultipleShootingSolver.h | 2 +- .../ocs2_sqp/MultipleShootingTranscription.h | 23 +++--- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 28 +++---- .../src/MultipleShootingTranscription.cpp | 77 ++++++++++--------- ocs2_sqp/ocs2_sqp/test/testTranscription.cpp | 4 +- 5 files changed, 66 insertions(+), 68 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index be3d12025..df8f55f67 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -191,8 +191,8 @@ class MultipleShootingSolver : public SolverBase { // LQ approximation std::vector<VectorFunctionLinearApproximation> dynamics_; std::vector<ScalarFunctionQuadraticApproximation> cost_; - std::vector<VectorFunctionLinearApproximation> constraints_; std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + std::vector<VectorFunctionLinearApproximation> stateInputEqConstraints_; std::vector<VectorFunctionLinearApproximation> stateIneqConstraints_; std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h index a22fd6714..524e5d7e1 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -44,9 +44,9 @@ struct Transcription { PerformanceIndex performance; VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; - VectorFunctionLinearApproximation constraints; - VectorFunctionLinearApproximation constraintsProjection; matrix_t constraintPseudoInverse; + VectorFunctionLinearApproximation constraintsProjection; + VectorFunctionLinearApproximation stateInputEqConstraints; VectorFunctionLinearApproximation stateIneqConstraints; VectorFunctionLinearApproximation stateInputIneqConstraints; }; @@ -57,28 +57,25 @@ struct Transcription { * @param optimalControlProblem : Definition of the optimal control problem * @param sensitivityDiscretizer : Integrator to use for creating the discrete dynamics. * @param projectStateInputEqualityConstraints - * @param extractEqualityConstraintsPseudoInverse * @param t : Start of the discrete interval * @param dt : Duration of the interval * @param x : State at start of the interval * @param x_next : State at the end of the interval * @param u : Input, taken to be constant across the interval. - * @param enableStateInequalityConstraint : Flag to enalbe the state-only inequality constraint. The constraint must be disabled at the - * initial node. + * @param extractEqualityConstraintsPseudoInverse * @return multiple shooting transcription for this node. */ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, - bool extractEqualityConstraintsPseudoInverse, scalar_t t, scalar_t dt, const vector_t& x, - const vector_t& x_next, const vector_t& u, bool enableStateInequalityConstraint = true); + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + bool extractEqualityConstraintsPseudoInverse = false); /** * Compute only the performance index for a single intermediate node. * Corresponds to the performance index returned by "setupIntermediateNode" */ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, - bool enableStateInequalityConstraint = true); + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); /** * Results of the transcription at a terminal node @@ -86,8 +83,8 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt struct TerminalTranscription { PerformanceIndex performance; ScalarFunctionQuadraticApproximation cost; - VectorFunctionLinearApproximation constraints; - VectorFunctionLinearApproximation stateIneqConstraints; + VectorFunctionLinearApproximation eqConstraints; + VectorFunctionLinearApproximation ineqConstraints; }; /** @@ -113,8 +110,8 @@ struct EventTranscription { PerformanceIndex performance; VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; - VectorFunctionLinearApproximation constraints; - VectorFunctionLinearApproximation stateIneqConstraints; + VectorFunctionLinearApproximation eqConstraints; + VectorFunctionLinearApproximation ineqConstraints; }; /** diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 39c888263..a4857429d 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -273,8 +273,9 @@ MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolu hpipm_status status; const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &constraints_)); - status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); + hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &stateInputEqConstraints_)); + status = + hpipmInterface_.solve(delta_x0, dynamics_, cost_, &stateInputEqConstraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); } else { // without constraints, or when using projection, we have an unconstrained QP. hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, nullptr)); status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); @@ -390,8 +391,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); dynamics_.resize(N); cost_.resize(N + 1); - constraints_.resize(N + 1); constraintsProjection_.resize(N); + stateInputEqConstraints_.resize(N + 1); stateIneqConstraints_.resize(N + 1); stateInputIneqConstraints_.resize(N); @@ -410,24 +411,21 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec workerPerformance += result.performance; dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); - constraints_[i] = std::move(result.constraints); constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); - stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); + stateInputEqConstraints_[i] = std::move(result.eqConstraints); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); stateInputIneqConstraints_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - const bool extractEqualityConstraintsPseudoInverse = false; - const bool enableStateInequalityConstraint = (i > 0); - auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, projection, - extractEqualityConstraintsPseudoInverse, ti, dt, x[i], x[i + 1], u[i], - enableStateInequalityConstraint); + auto result = + multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, projection, ti, dt, x[i], x[i + 1], u[i]); workerPerformance += result.performance; dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); - constraints_[i] = std::move(result.constraints); constraintsProjection_[i] = std::move(result.constraintsProjection); + stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); } @@ -440,8 +438,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); workerPerformance += result.performance; cost_[i] = std::move(result.cost); - constraints_[i] = std::move(result.constraints); - stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); + stateInputEqConstraints_[i] = std::move(result.eqConstraints); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); } // Accumulate! Same worker might run multiple tasks @@ -479,9 +477,7 @@ PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector<An // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - const bool enableStateInequalityConstraint = (i > 0); - workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i], - enableStateInequalityConstraint); + workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); } i = timeIndex++; diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index e88b3ac81..659dc1ebe 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -37,22 +37,24 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { -scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { +namespace { +inline scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { return ineqConstraint.cwiseMin(0.0).matrix().squaredNorm(); } +} // namespace Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, - bool extractEqualityConstraintsPseudoInverse, scalar_t t, scalar_t dt, const vector_t& x, - const vector_t& x_next, const vector_t& u, bool enableStateInequalityConstraint) { + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + bool extractEqualityConstraintsPseudoInverse) { // Results and short-hand notation Transcription transcription; auto& dynamics = transcription.dynamics; auto& performance = transcription.performance; auto& cost = transcription.cost; - auto& constraints = transcription.constraints; auto& projection = transcription.constraintsProjection; auto& constraintPseudoInverse = transcription.constraintPseudoInverse; + auto& stateInputEqConstraints = transcription.stateInputEqConstraints; auto& stateIneqConstraints = transcription.stateIneqConstraints; auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; @@ -71,8 +73,8 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP cost *= dt; performance.cost = cost.f; - // State inequality constraints. Should be disabled at the initial time of the horizon. - if (!optimalControlProblem.stateInequalityConstraintPtr->empty() && enableStateInequalityConstraint) { + // State inequality constraints. + if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints.f); @@ -88,18 +90,19 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP // Constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} = 0 - constraints = optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); - if (constraints.f.size() > 0) { - performance.equalityConstraintsSSE = dt * constraints.f.squaredNorm(); + stateInputEqConstraints = + optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); + if (stateInputEqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE = dt * stateInputEqConstraints.f.squaredNorm(); if (projectStateInputEqualityConstraints) { // Handle equality constraints using projection. // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. if (extractEqualityConstraintsPseudoInverse) { - std::tie(projection, constraintPseudoInverse) = qrConstraintProjection(constraints); + std::tie(projection, constraintPseudoInverse) = qrConstraintProjection(stateInputEqConstraints); } else { - projection = luConstraintProjection(constraints).first; + projection = luConstraintProjection(stateInputEqConstraints).first; constraintPseudoInverse = matrix_t(); } - constraints = VectorFunctionLinearApproximation(); + stateInputEqConstraints = VectorFunctionLinearApproximation(); // Adapt dynamics, cost, and state-input inequality constraints changeOfInputVariables(dynamics, projection.dfdu, projection.dfdx, projection.f); @@ -115,8 +118,7 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP } PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, - bool enableStateInequalityConstraint) { + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { PerformanceIndex performance; // Dynamics @@ -131,8 +133,8 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt // Costs performance.cost = dt * computeCost(optimalControlProblem, t, x, u); - // State inequality constraints. Should be disabled at the initial time of the horizon. - if (!optimalControlProblem.stateInequalityConstraintPtr->empty() && enableStateInequalityConstraint) { + // State inequality constraints. + if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { const vector_t stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); if (stateIneqConstraints.size() > 0) { @@ -151,9 +153,10 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt // Constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { - const vector_t constraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - if (constraints.size() > 0) { - performance.equalityConstraintsSSE = dt * constraints.squaredNorm(); + const vector_t stateIneqEqConstraints = + optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); + if (stateIneqEqConstraints.size() > 0) { + performance.equalityConstraintsSSE = dt * stateIneqEqConstraints.squaredNorm(); } } @@ -165,8 +168,8 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont TerminalTranscription transcription; auto& performance = transcription.performance; auto& cost = transcription.cost; - auto& constraints = transcription.constraints; - auto& stateIneqConstraints = transcription.stateIneqConstraints; + auto& eqConstraints = transcription.eqConstraints; + auto& ineqConstraints = transcription.ineqConstraints; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); @@ -176,14 +179,14 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont // State inequality constraints. if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { - stateIneqConstraints = + ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); - if (stateIneqConstraints.f.size() > 0) { - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(stateIneqConstraints.f); + if (ineqConstraints.f.size() > 0) { + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints.f); } } - constraints = VectorFunctionLinearApproximation::Zero(0, x.size()); + eqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size()); return transcription; } @@ -197,10 +200,10 @@ PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimal performance.cost = computeFinalCost(optimalControlProblem, t, x); if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { - const vector_t stateIneqConstraints = + const vector_t ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - if (stateIneqConstraints.size() > 0) { - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(stateIneqConstraints); + if (ineqConstraints.size() > 0) { + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints); } } @@ -214,8 +217,8 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro auto& performance = transcription.performance; auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; - auto& constraints = transcription.constraints; - auto& stateIneqConstraints = transcription.stateIneqConstraints; + auto& eqConstraints = transcription.eqConstraints; + auto& ineqConstraints = transcription.ineqConstraints; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation; optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); @@ -232,14 +235,14 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro // State inequality constraints. if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { - stateIneqConstraints = + ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); - if (stateIneqConstraints.f.size() > 0) { - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(stateIneqConstraints.f); + if (ineqConstraints.f.size() > 0) { + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints.f); } } - constraints = VectorFunctionLinearApproximation::Zero(0, x.size()); + eqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size()); return transcription; } @@ -257,10 +260,10 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon performance.cost = computeEventCost(optimalControlProblem, t, x); if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { - const vector_t stateIneqConstraints = + const vector_t ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - if (stateIneqConstraints.size() > 0) { - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(stateIneqConstraints); + if (ineqConstraints.size() > 0) { + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints); } } diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp index 17f539f7e..770645ead 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> +#include <iostream> + #include "ocs2_sqp/MultipleShootingTranscription.h" #include <ocs2_oc/test/circular_kinematics.h> @@ -58,7 +60,7 @@ TEST(test_transcription, intermediate_performance) { const vector_t x = (vector_t(2) << 1.0, 0.1).finished(); const vector_t x_next = (vector_t(2) << 1.1, 0.2).finished(); const vector_t u = (vector_t(2) << 0.1, 1.3).finished(); - const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, true, true, t, dt, x, x_next, u); + const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, true, t, dt, x, x_next, u); const auto performance = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); From 5a45806d5ad4ee47d164c86e22cdc9eed34dbffb Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 28 Nov 2022 12:55:12 +0100 Subject: [PATCH 349/512] update tests --- .../ocs2_sqp/src/MultipleShootingTranscription.cpp | 12 +++--------- ocs2_sqp/ocs2_sqp/test/testTranscription.cpp | 12 ++++++++++++ 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index 659dc1ebe..afc656074 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -137,18 +137,14 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { const vector_t stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - if (stateIneqConstraints.size() > 0) { - performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints); - } + performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints); } // State-input inequality constraints. if (!optimalControlProblem.inequalityConstraintPtr->empty()) { const vector_t stateInputIneqConstraints = optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - if (stateInputIneqConstraints.size() > 0) { - performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints); - } + performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints); } // Constraints @@ -262,9 +258,7 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { const vector_t ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - if (ineqConstraints.size() > 0) { - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints); - } + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints); } return performance; diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp index 770645ead..6166fbff1 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp @@ -52,6 +52,10 @@ TEST(test_transcription, intermediate_performance) { // optimal control problem OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/sqp_test_generated"); + // inequality constraints + problem.inequalityConstraintPtr->add("emptyInequalityConstraint", getOcs2Constraints(getRandomConstraints(2, 2, 0))); + problem.stateInequalityConstraintPtr->add("emptyStateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 0))); + auto discretizer = selectDynamicsDiscretization(SensitivityIntegratorType::RK4); auto sensitivityDiscretizer = selectDynamicsSensitivityDiscretization(SensitivityIntegratorType::RK4); @@ -65,6 +69,14 @@ TEST(test_transcription, intermediate_performance) { const auto performance = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_DOUBLE_EQ(performance.inequalityConstraintsSSE, 0.0); + + problem.inequalityConstraintPtr->add("inequalityConstraint", getOcs2Constraints(getRandomConstraints(2, 2, 3))); + problem.stateInequalityConstraintPtr->add("stateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 4))); + + const auto transcriptionWithIneq = setupIntermediateNode(problem, sensitivityDiscretizer, true, t, dt, x, x_next, u); + const auto performanceWithIneq = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); + ASSERT_TRUE(areIdentical(performance, transcription.performance)); } TEST(test_transcription, terminal_performance) { From 0ffb86a2a9307b1caf8785f2a67df0a89bc4ab8b Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 28 Nov 2022 13:22:50 +0100 Subject: [PATCH 350/512] fix tests --- ocs2_sqp/ocs2_sqp/test/testTranscription.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp index 6166fbff1..e87343af4 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp @@ -76,7 +76,7 @@ TEST(test_transcription, intermediate_performance) { const auto transcriptionWithIneq = setupIntermediateNode(problem, sensitivityDiscretizer, true, t, dt, x, x_next, u); const auto performanceWithIneq = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_TRUE(areIdentical(performanceWithIneq, transcriptionWithIneq.performance)); } TEST(test_transcription, terminal_performance) { From f5f15813f6b399096c2cfba6da873837b319ca49 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 28 Nov 2022 15:45:36 +0100 Subject: [PATCH 351/512] remove if, reorder evaluations, and modify comments --- .../src/MultipleShootingTranscription.cpp | 38 +++++++++---------- ocs2_sqp/ocs2_sqp/test/testTranscription.cpp | 2 - 2 files changed, 17 insertions(+), 23 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index afc656074..2881e4817 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -38,8 +38,12 @@ namespace ocs2 { namespace multiple_shooting { namespace { -inline scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { - return ineqConstraint.cwiseMin(0.0).matrix().squaredNorm(); +scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { + if (ineqConstraint.size() == 0) { + return 0.0; + } else { + return ineqConstraint.cwiseMin(0.0).squaredNorm(); + } } } // namespace @@ -87,7 +91,7 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints.f); } - // Constraints + // State-input equality constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} = 0 stateInputEqConstraints = @@ -133,6 +137,13 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt // Costs performance.cost = dt * computeCost(optimalControlProblem, t, x, u); + // State-input equality constraints + if (!optimalControlProblem.equalityConstraintPtr->empty()) { + const vector_t stateIneqEqConstraints = + optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); + performance.equalityConstraintsSSE = dt * stateIneqEqConstraints.squaredNorm(); + } + // State inequality constraints. if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { const vector_t stateIneqConstraints = @@ -147,15 +158,6 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints); } - // Constraints - if (!optimalControlProblem.equalityConstraintPtr->empty()) { - const vector_t stateIneqEqConstraints = - optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - if (stateIneqEqConstraints.size() > 0) { - performance.equalityConstraintsSSE = dt * stateIneqEqConstraints.squaredNorm(); - } - } - return performance; } @@ -177,9 +179,7 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); - if (ineqConstraints.f.size() > 0) { - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints.f); - } + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints.f); } eqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size()); @@ -198,9 +198,7 @@ PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimal if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { const vector_t ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - if (ineqConstraints.size() > 0) { - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints); - } + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints); } return performance; @@ -233,9 +231,7 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); - if (ineqConstraints.f.size() > 0) { - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints.f); - } + performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints.f); } eqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size()); diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp index e87343af4..d34647145 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp @@ -29,8 +29,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include <iostream> - #include "ocs2_sqp/MultipleShootingTranscription.h" #include <ocs2_oc/test/circular_kinematics.h> From eaecc9449a2c7f2fe310f2f4ad7eb2b60fb632d6 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 28 Nov 2022 15:49:06 +0100 Subject: [PATCH 352/512] fix names --- ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index 2881e4817..e4099753c 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -139,9 +139,9 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt // State-input equality constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { - const vector_t stateIneqEqConstraints = + const vector_t stateInputEqConstraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - performance.equalityConstraintsSSE = dt * stateIneqEqConstraints.squaredNorm(); + performance.equalityConstraintsSSE = dt * stateInputEqConstraints.squaredNorm(); } // State inequality constraints. From 9d96614576fcaca5a4a4d442b0da74194e0dcbce Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 28 Nov 2022 15:09:54 +0000 Subject: [PATCH 353/512] checking size for: performance.equalityConstraintsSSE = dt * stateInputEqConstraints.squaredNorm(); --- ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index e4099753c..462683a3b 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -141,7 +141,9 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt if (!optimalControlProblem.equalityConstraintPtr->empty()) { const vector_t stateInputEqConstraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - performance.equalityConstraintsSSE = dt * stateInputEqConstraints.squaredNorm(); + if (stateInputEqConstraints.size() > 0) { + performance.equalityConstraintsSSE = dt * stateInputEqConstraints.squaredNorm(); + } } // State inequality constraints. From 9edaa96bc1586d656a8da6c3cb36e4e7fe2c8300 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 29 Nov 2022 13:57:49 +0100 Subject: [PATCH 354/512] separate perfromance index computations and projection --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 1 + .../ocs2_sqp/MultipleShootingTranscription.h | 34 +--- .../ocs2_sqp/PerformanceIndexComputation.h | 97 ++++++++++ .../ocs2_sqp/src/MultipleShootingSolver.cpp | 19 +- .../src/MultipleShootingTranscription.cpp | 159 +++------------- .../src/PerformanceIndexComputation.cpp | 179 ++++++++++++++++++ ocs2_sqp/ocs2_sqp/test/testTranscription.cpp | 28 +-- 7 files changed, 341 insertions(+), 176 deletions(-) create mode 100644 ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h create mode 100644 ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 237b8a4c4..bf6c4eb1c 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -56,6 +56,7 @@ add_library(${PROJECT_NAME} src/MultipleShootingSolver.cpp src/MultipleShootingSolverStatus.cpp src/MultipleShootingTranscription.cpp + src/PerformanceIndexComputation.cpp src/TimeDiscretization.cpp ) add_dependencies(${PROJECT_NAME} diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h index 524e5d7e1..05a932c84 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -31,7 +31,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/integration/SensitivityIntegrator.h> -#include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> namespace ocs2 { @@ -41,7 +40,6 @@ namespace multiple_shooting { * Results of the transcription at an intermediate node */ struct Transcription { - PerformanceIndex performance; VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; matrix_t constraintPseudoInverse; @@ -56,32 +54,30 @@ struct Transcription { * * @param optimalControlProblem : Definition of the optimal control problem * @param sensitivityDiscretizer : Integrator to use for creating the discrete dynamics. - * @param projectStateInputEqualityConstraints * @param t : Start of the discrete interval * @param dt : Duration of the interval * @param x : State at start of the interval * @param x_next : State at the end of the interval * @param u : Input, taken to be constant across the interval. - * @param extractEqualityConstraintsPseudoInverse * @return multiple shooting transcription for this node. */ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, - DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, - bool extractEqualityConstraintsPseudoInverse = false); + DynamicsSensitivityDiscretizer& sensitivityDiscretizer, scalar_t t, scalar_t dt, const vector_t& x, + const vector_t& x_next, const vector_t& u); /** - * Compute only the performance index for a single intermediate node. - * Corresponds to the performance index returned by "setupIntermediateNode" + * Apply the state-input equality constraint projection for a single intermediate node transcription. + * + * @param transcription : Transcription for a single intermediate node + * @param extractEqualityConstraintsPseudoInverse + * @return multiple shooting transcription for this node. */ -PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); +void projectTranscription(Transcription& transcription, bool extractEqualityConstraintsPseudoInverse = false); /** * Results of the transcription at a terminal node */ struct TerminalTranscription { - PerformanceIndex performance; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation eqConstraints; VectorFunctionLinearApproximation ineqConstraints; @@ -97,17 +93,10 @@ struct TerminalTranscription { */ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); -/** - * Compute only the performance index for the terminal node. - * Corresponds to the performance index returned by "setTerminalNode" - */ -PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); - /** * Results of the transcription at an event */ struct EventTranscription { - PerformanceIndex performance; VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation eqConstraints; @@ -126,12 +115,5 @@ struct EventTranscription { EventTranscription setupEventNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); -/** - * Compute only the performance index for the event node. - * Corresponds to the performance index returned by "setupEventNode" - */ -PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next); - } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h new file mode 100644 index 000000000..401674fb7 --- /dev/null +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h @@ -0,0 +1,97 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_core/integration/SensitivityIntegrator.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> +#include <ocs2_oc/oc_problem/OptimalControlProblem.h> +#include "ocs2_sqp/MultipleShootingTranscription.h" + +namespace ocs2 { +namespace sqp { + +/** + * Compute the performance index from the transcription for a single intermediate node. + */ +PerformanceIndex computeIntermediatePerformance(const multiple_shooting::Transcription& transcription, scalar_t dt); + +/** + * Compute only the performance index for a single intermediate node. + * Corresponds to the performance index computed from the multiple_shooting::Transcription returned by + * "multiple_shooting::setupIntermediateNode" + * @param optimalControlProblem : Definition of the optimal control problem + * @param discretizer : Integrator to use for creating the discrete dynamics. + * @param t : Start of the discrete interval + * @param dt : Duration of the interval + * @param x : State at start of the interval + * @param x_next : State at the end of the interval + * @param u : Input, taken to be constant across the interval. + * @return Performance index for a single intermediate node. + */ +PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); + +/** + * Compute the performance index from the transcription for the terminal node. + */ +PerformanceIndex computeTerminalPerformance(const multiple_shooting::TerminalTranscription& transcription); + +/** + * Compute only the performance index for the terminal node. + * Corresponds to the performance index computed from multiple_shooting::TerminalTranscription returned by + * "multiple_shooting::setTerminalNode" + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the terminal node + * @param x : Terminal state + * @return Performance index for the terminal node. + */ +PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); + +/** + * Compute the performance index from the transcription for the event node. + */ +PerformanceIndex computeEventPerformance(const multiple_shooting::EventTranscription& transcription); + +/** + * Compute only the performance index for the event node. + * Corresponds to the performance index computed from multiple_shooting::EventTranscription returned by + * "multiple_shooting::setEventNode" + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the event node + * @param x : Pre-event state + * @param x_next : Post-event state + * @return Performance index for the event node. + */ +PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + const vector_t& x_next); + +} // namespace sqp +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index a4857429d..a3559fe8a 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -38,6 +38,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_sqp/MultipleShootingInitialization.h" #include "ocs2_sqp/MultipleShootingTranscription.h" +#include "ocs2_sqp/PerformanceIndexComputation.h" namespace ocs2 { @@ -408,7 +409,7 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); - workerPerformance += result.performance; + workerPerformance += sqp::computeEventPerformance(result); dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); @@ -419,9 +420,11 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - auto result = - multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, projection, ti, dt, x[i], x[i + 1], u[i]); - workerPerformance += result.performance; + auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + workerPerformance += sqp::computeIntermediatePerformance(result, dt); + if (projection) { + multiple_shooting::projectTranscription(result); + } dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); constraintsProjection_[i] = std::move(result.constraintsProjection); @@ -436,7 +439,7 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); - workerPerformance += result.performance; + workerPerformance += sqp::computeTerminalPerformance(result); cost_[i] = std::move(result.cost); stateInputEqConstraints_[i] = std::move(result.eqConstraints); stateIneqConstraints_[i] = std::move(result.ineqConstraints); @@ -472,12 +475,12 @@ PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector<An while (i < N) { if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node - workerPerformance += multiple_shooting::computeEventPerformance(ocpDefinition, time[i].time, x[i], x[i + 1]); + workerPerformance += sqp::computeEventPerformance(ocpDefinition, time[i].time, x[i], x[i + 1]); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + workerPerformance += sqp::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); } i = timeIndex++; @@ -485,7 +488,7 @@ PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector<An if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); - workerPerformance += multiple_shooting::computeTerminalPerformance(ocpDefinition, tN, x[N]); + workerPerformance += sqp::computeTerminalPerformance(ocpDefinition, tN, x[N]); } // Accumulate! Same worker might run multiple tasks diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index e4099753c..d9623988a 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -37,27 +37,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { -namespace { -scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { - if (ineqConstraint.size() == 0) { - return 0.0; - } else { - return ineqConstraint.cwiseMin(0.0).squaredNorm(); - } -} -} // namespace - Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, - DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, - bool extractEqualityConstraintsPseudoInverse) { + DynamicsSensitivityDiscretizer& sensitivityDiscretizer, scalar_t t, scalar_t dt, const vector_t& x, + const vector_t& x_next, const vector_t& u) { // Results and short-hand notation Transcription transcription; auto& dynamics = transcription.dynamics; - auto& performance = transcription.performance; auto& cost = transcription.cost; - auto& projection = transcription.constraintsProjection; - auto& constraintPseudoInverse = transcription.constraintPseudoInverse; auto& stateInputEqConstraints = transcription.stateInputEqConstraints; auto& stateIneqConstraints = transcription.stateIneqConstraints; auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; @@ -66,7 +52,6 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP // Discretization returns x_{k+1} = A_{k} * dx_{k} + B_{k} * du_{k} + b_{k} dynamics = sensitivityDiscretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); dynamics.f -= x_next; // make it dx_{k+1} = ... - performance.dynamicsViolationSSE = dt * dynamics.f.squaredNorm(); // Precomputation for other terms constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint + Request::Approximation; @@ -75,96 +60,59 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP // Costs: Approximate the integral with forward euler cost = approximateCost(optimalControlProblem, t, x, u); cost *= dt; - performance.cost = cost.f; + + // State-input equality constraints + if (!optimalControlProblem.equalityConstraintPtr->empty()) { + // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} = 0 + stateInputEqConstraints = + optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); + } // State inequality constraints. if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints.f); } // State-input inequality constraints. if (!optimalControlProblem.inequalityConstraintPtr->empty()) { stateInputIneqConstraints = optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints.f); - } - - // State-input equality constraints - if (!optimalControlProblem.equalityConstraintPtr->empty()) { - // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} = 0 - stateInputEqConstraints = - optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); - if (stateInputEqConstraints.f.size() > 0) { - performance.equalityConstraintsSSE = dt * stateInputEqConstraints.f.squaredNorm(); - if (projectStateInputEqualityConstraints) { // Handle equality constraints using projection. - // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. - if (extractEqualityConstraintsPseudoInverse) { - std::tie(projection, constraintPseudoInverse) = qrConstraintProjection(stateInputEqConstraints); - } else { - projection = luConstraintProjection(stateInputEqConstraints).first; - constraintPseudoInverse = matrix_t(); - } - stateInputEqConstraints = VectorFunctionLinearApproximation(); - - // Adapt dynamics, cost, and state-input inequality constraints - changeOfInputVariables(dynamics, projection.dfdu, projection.dfdx, projection.f); - changeOfInputVariables(cost, projection.dfdu, projection.dfdx, projection.f); - if (stateInputIneqConstraints.f.size() > 0) { - changeOfInputVariables(stateInputIneqConstraints, projection.dfdu, projection.dfdx, projection.f); - } - } - } } return transcription; } -PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { - PerformanceIndex performance; - - // Dynamics - vector_t dynamicsGap = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); - dynamicsGap -= x_next; - performance.dynamicsViolationSSE = dt * dynamicsGap.squaredNorm(); - - // Precomputation for other terms - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; - optimalControlProblem.preComputationPtr->request(request, t, x, u); - - // Costs - performance.cost = dt * computeCost(optimalControlProblem, t, x, u); - - // State-input equality constraints - if (!optimalControlProblem.equalityConstraintPtr->empty()) { - const vector_t stateInputEqConstraints = - optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - performance.equalityConstraintsSSE = dt * stateInputEqConstraints.squaredNorm(); - } +void projectTranscription(Transcription& transcription, bool extractEqualityConstraintsPseudoInverse) { + auto& dynamics = transcription.dynamics; + auto& cost = transcription.cost; + auto& projection = transcription.constraintsProjection; + auto& constraintPseudoInverse = transcription.constraintPseudoInverse; + auto& stateInputEqConstraints = transcription.stateInputEqConstraints; + auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; - // State inequality constraints. - if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { - const vector_t stateIneqConstraints = - optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints); - } + if (stateInputEqConstraints.f.size() > 0) { + // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. + if (extractEqualityConstraintsPseudoInverse) { + std::tie(projection, constraintPseudoInverse) = qrConstraintProjection(stateInputEqConstraints); + } else { + projection = luConstraintProjection(stateInputEqConstraints).first; + constraintPseudoInverse = matrix_t(); + } + stateInputEqConstraints = VectorFunctionLinearApproximation(); - // State-input inequality constraints. - if (!optimalControlProblem.inequalityConstraintPtr->empty()) { - const vector_t stateInputIneqConstraints = - optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints); + // Adapt dynamics, cost, and state-input inequality constraints + changeOfInputVariables(dynamics, projection.dfdu, projection.dfdx, projection.f); + changeOfInputVariables(cost, projection.dfdu, projection.dfdx, projection.f); + if (stateInputIneqConstraints.f.size() > 0) { + changeOfInputVariables(stateInputIneqConstraints, projection.dfdu, projection.dfdx, projection.f); + } } - - return performance; } TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { // Results and short-hand notation TerminalTranscription transcription; - auto& performance = transcription.performance; auto& cost = transcription.cost; auto& eqConstraints = transcription.eqConstraints; auto& ineqConstraints = transcription.ineqConstraints; @@ -173,13 +121,11 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont optimalControlProblem.preComputationPtr->requestFinal(request, t, x); cost = approximateFinalCost(optimalControlProblem, t, x); - performance.cost = cost.f; // State inequality constraints. if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints.f); } eqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size()); @@ -187,28 +133,10 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont return transcription; } -PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { - PerformanceIndex performance; - - constexpr auto request = Request::Cost + Request::SoftConstraint; - optimalControlProblem.preComputationPtr->requestFinal(request, t, x); - - performance.cost = computeFinalCost(optimalControlProblem, t, x); - - if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { - const vector_t ineqConstraints = - optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints); - } - - return performance; -} - EventTranscription setupEventNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { // Results and short-hand notation EventTranscription transcription; - auto& performance = transcription.performance; auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; auto& eqConstraints = transcription.eqConstraints; @@ -222,43 +150,18 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro dynamics = optimalControlProblem.dynamicsPtr->jumpMapLinearApproximation(t, x); dynamics.f -= x_next; // make it dx_{k+1} = ... dynamics.dfdu.setZero(x.size(), 0); // Overwrite derivative that shouldn't exist. - performance.dynamicsViolationSSE = dynamics.f.squaredNorm(); cost = approximateEventCost(optimalControlProblem, t, x); - performance.cost = cost.f; // State inequality constraints. if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints.f); } eqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size()); return transcription; } -PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next) { - PerformanceIndex performance; - - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics; - optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); - - // Dynamics - const vector_t dynamicsGap = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x) - x_next; - performance.dynamicsViolationSSE = dynamicsGap.squaredNorm(); - - performance.cost = computeEventCost(optimalControlProblem, t, x); - - if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { - const vector_t ineqConstraints = - optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += getIneqConstraintsSSE(ineqConstraints); - } - - return performance; -} - } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp b/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp new file mode 100644 index 000000000..deb67cbfd --- /dev/null +++ b/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp @@ -0,0 +1,179 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_sqp/PerformanceIndexComputation.h" + +#include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> + +namespace ocs2 { +namespace sqp { + +namespace { +scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { + if (ineqConstraint.size() == 0) { + return 0.0; + } else { + return ineqConstraint.cwiseMin(0.0).squaredNorm(); + } +} +} // namespace + +PerformanceIndex computeIntermediatePerformance(const multiple_shooting::Transcription& transcription, scalar_t dt) { + PerformanceIndex performance; + + // Dynamics + performance.dynamicsViolationSSE = dt * transcription.dynamics.f.squaredNorm(); + + // Costs + performance.cost = transcription.cost.f; + + // Inequality constraints. + performance.inequalityConstraintsSSE = + dt * getIneqConstraintsSSE(transcription.stateIneqConstraints.f) + dt * getIneqConstraintsSSE(transcription.stateIneqConstraints.f); + + // State-input equality constraints + if (transcription.stateInputEqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE = dt * transcription.stateInputEqConstraints.f.squaredNorm(); + } else { + performance.equalityConstraintsSSE = 0; + } + + return performance; +} + +PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { + PerformanceIndex performance; + + // Dynamics + vector_t dynamicsGap = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); + dynamicsGap -= x_next; + performance.dynamicsViolationSSE = dt * dynamicsGap.squaredNorm(); + + // Precomputation for other terms + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; + optimalControlProblem.preComputationPtr->request(request, t, x, u); + + // Costs + performance.cost = dt * computeCost(optimalControlProblem, t, x, u); + + // State-input equality constraints + if (!optimalControlProblem.equalityConstraintPtr->empty()) { + const vector_t stateInputEqConstraints = + optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); + if (stateInputEqConstraints.size() > 0) { + performance.equalityConstraintsSSE = dt * stateInputEqConstraints.squaredNorm(); + } else { + performance.equalityConstraintsSSE = 0; + } + } + + // State inequality constraints. + if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { + const vector_t stateIneqConstraints = + optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints); + } + + // State-input inequality constraints. + if (!optimalControlProblem.inequalityConstraintPtr->empty()) { + const vector_t stateInputIneqConstraints = + optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); + performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints); + } + + return performance; +} + +PerformanceIndex computeTerminalPerformance(const multiple_shooting::TerminalTranscription& transcription) { + PerformanceIndex performance; + + performance.cost = transcription.cost.f; + + // State inequality constraints. + performance.inequalityConstraintsSSE = getIneqConstraintsSSE(transcription.ineqConstraints.f); + + return performance; +} + +PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { + PerformanceIndex performance; + + constexpr auto request = Request::Cost + Request::SoftConstraint; + optimalControlProblem.preComputationPtr->requestFinal(request, t, x); + + performance.cost = computeFinalCost(optimalControlProblem, t, x); + + if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { + const vector_t ineqConstraints = + optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + performance.inequalityConstraintsSSE = getIneqConstraintsSSE(ineqConstraints); + } + + return performance; +} + +PerformanceIndex computeEventPerformance(const multiple_shooting::EventTranscription& transcription) { + PerformanceIndex performance; + + // Dynamics + performance.dynamicsViolationSSE = transcription.dynamics.f.squaredNorm(); + + performance.cost = transcription.cost.f; + + // State inequality constraints. + performance.inequalityConstraintsSSE = getIneqConstraintsSSE(transcription.ineqConstraints.f); + + return performance; +} + +PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + const vector_t& x_next) { + PerformanceIndex performance; + + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics; + optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); + + // Dynamics + const vector_t dynamicsGap = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x) - x_next; + performance.dynamicsViolationSSE = dynamicsGap.squaredNorm(); + + performance.cost = computeEventCost(optimalControlProblem, t, x); + + if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { + const vector_t ineqConstraints = + optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + performance.inequalityConstraintsSSE = getIneqConstraintsSSE(ineqConstraints); + } + + return performance; +} + +} // namespace sqp +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp index d34647145..29aafd6b1 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> #include "ocs2_sqp/MultipleShootingTranscription.h" +#include "ocs2_sqp/PerformanceIndexComputation.h" #include <ocs2_oc/test/circular_kinematics.h> #include <ocs2_oc/test/testProblemsGeneration.h> @@ -45,14 +46,15 @@ bool areIdentical(const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceInde using namespace ocs2; using namespace ocs2::multiple_shooting; +using namespace ocs2::sqp; TEST(test_transcription, intermediate_performance) { // optimal control problem OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/sqp_test_generated"); // inequality constraints - problem.inequalityConstraintPtr->add("emptyInequalityConstraint", getOcs2Constraints(getRandomConstraints(2, 2, 0))); - problem.stateInequalityConstraintPtr->add("emptyStateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 0))); + problem.inequalityConstraintPtr->add("inequalityConstraint", getOcs2Constraints(getRandomConstraints(2, 2, 3))); + problem.stateInequalityConstraintPtr->add("stateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 4))); auto discretizer = selectDynamicsDiscretization(SensitivityIntegratorType::RK4); auto sensitivityDiscretizer = selectDynamicsSensitivityDiscretization(SensitivityIntegratorType::RK4); @@ -62,19 +64,11 @@ TEST(test_transcription, intermediate_performance) { const vector_t x = (vector_t(2) << 1.0, 0.1).finished(); const vector_t x_next = (vector_t(2) << 1.1, 0.2).finished(); const vector_t u = (vector_t(2) << 0.1, 1.3).finished(); - const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, true, t, dt, x, x_next, u); + const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, t, dt, x, x_next, u); const auto performance = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); - ASSERT_DOUBLE_EQ(performance.inequalityConstraintsSSE, 0.0); - - problem.inequalityConstraintPtr->add("inequalityConstraint", getOcs2Constraints(getRandomConstraints(2, 2, 3))); - problem.stateInequalityConstraintPtr->add("stateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 4))); - - const auto transcriptionWithIneq = setupIntermediateNode(problem, sensitivityDiscretizer, true, t, dt, x, x_next, u); - const auto performanceWithIneq = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); - ASSERT_TRUE(areIdentical(performanceWithIneq, transcriptionWithIneq.performance)); + ASSERT_TRUE(areIdentical(performance, computeIntermediatePerformance(transcription, dt))); } TEST(test_transcription, terminal_performance) { @@ -86,6 +80,9 @@ TEST(test_transcription, terminal_performance) { problem.finalCostPtr->add("finalCost", getOcs2StateCost(getRandomCost(nx, 0))); problem.finalSoftConstraintPtr->add("finalSoftCost", getOcs2StateCost(getRandomCost(nx, 0))); + // inequality constraints + problem.finalInequalityConstraintPtr->add("finalInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 4))); + const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); problem.targetTrajectoriesPtr = &targetTrajectories; @@ -94,7 +91,7 @@ TEST(test_transcription, terminal_performance) { const auto transcription = setupTerminalNode(problem, t, x); const auto performance = computeTerminalPerformance(problem, t, x); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_TRUE(areIdentical(performance, computeTerminalPerformance(transcription))); } TEST(test_transcription, event_performance) { @@ -110,6 +107,9 @@ TEST(test_transcription, event_performance) { // cost problem.preJumpCostPtr->add("eventCost", getOcs2StateCost(getRandomCost(nx, 0))); + // inequality constraints + problem.preJumpInequalityConstraintPtr->add("preJumpInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 4))); + const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); problem.targetTrajectoriesPtr = &targetTrajectories; @@ -119,5 +119,5 @@ TEST(test_transcription, event_performance) { const auto transcription = setupEventNode(problem, t, x, x_next); const auto performance = computeEventPerformance(problem, t, x, x_next); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_TRUE(areIdentical(performance, computeEventPerformance(transcription))); } From eeb737ea98bd844a05a6293deed0c7f50ab21a0d Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 29 Nov 2022 16:31:14 +0100 Subject: [PATCH 355/512] fix docs, add eqConstraints evaluations, and fix test name --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 2 +- .../ocs2_sqp/MultipleShootingTranscription.h | 1 - .../ocs2_sqp/PerformanceIndexComputation.h | 9 +++- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 3 +- .../src/MultipleShootingTranscription.cpp | 14 ++++++ .../src/PerformanceIndexComputation.cpp | 50 ++++++++++++++++--- ... => testTranscriptionPerformanceIndex.cpp} | 0 7 files changed, 67 insertions(+), 12 deletions(-) rename ocs2_sqp/ocs2_sqp/test/{testTranscription.cpp => testTranscriptionPerformanceIndex.cpp} (100%) diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index bf6c4eb1c..f64a050c2 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -105,7 +105,7 @@ catkin_add_gtest(test_${PROJECT_NAME} test/testDiscretization.cpp test/testProjection.cpp test/testSwitchedProblem.cpp - test/testTranscription.cpp + test/testTranscriptionPerformanceIndex.cpp test/testUnconstrained.cpp test/testValuefunction.cpp ) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h index 05a932c84..1e7f9f0c8 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -70,7 +70,6 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP * * @param transcription : Transcription for a single intermediate node * @param extractEqualityConstraintsPseudoInverse - * @return multiple shooting transcription for this node. */ void projectTranscription(Transcription& transcription, bool extractEqualityConstraintsPseudoInverse = false); diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h index 401674fb7..d7671c058 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h @@ -39,7 +39,10 @@ namespace ocs2 { namespace sqp { /** - * Compute the performance index from the transcription for a single intermediate node. + * Compute the performance index from the transcription for this node. + * @param transcription: multiple shooting transcription for this node. + * @param dt : Duration of the interval + * @return Performance index for a single intermediate node. */ PerformanceIndex computeIntermediatePerformance(const multiple_shooting::Transcription& transcription, scalar_t dt); @@ -61,6 +64,8 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt /** * Compute the performance index from the transcription for the terminal node. + * @param transcription: multiple shooting transcription for this node. + * @return Performance index for the terminal node. */ PerformanceIndex computeTerminalPerformance(const multiple_shooting::TerminalTranscription& transcription); @@ -77,6 +82,8 @@ PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimal /** * Compute the performance index from the transcription for the event node. + * @param transcription: multiple shooting transcription for this node. + * @return Performance index for the event node. */ PerformanceIndex computeEventPerformance(const multiple_shooting::EventTranscription& transcription); diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index a3559fe8a..7c2320869 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -402,7 +402,6 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; PerformanceIndex workerPerformance; // Accumulate performance in local variable - const bool projection = settings_.projectStateInputEqualityConstraints; int i = timeIndex++; while (i < N) { @@ -422,7 +421,7 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); workerPerformance += sqp::computeIntermediatePerformance(result, dt); - if (projection) { + if (settings_.projectStateInputEqualityConstraints) { multiple_shooting::projectTranscription(result); } dynamics_[i] = std::move(result.dynamics); diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index d9623988a..d02037687 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -120,8 +120,15 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); + // Costs cost = approximateFinalCost(optimalControlProblem, t, x); + // State equality constraints. + if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { + eqConstraints = + optimalControlProblem.finalEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + } + // State inequality constraints. if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { ineqConstraints = @@ -151,8 +158,15 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro dynamics.f -= x_next; // make it dx_{k+1} = ... dynamics.dfdu.setZero(x.size(), 0); // Overwrite derivative that shouldn't exist. + // Costs cost = approximateEventCost(optimalControlProblem, t, x); + // State equality constraints. + if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { + eqConstraints = + optimalControlProblem.preJumpEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + } + // State inequality constraints. if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { ineqConstraints = diff --git a/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp b/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp index deb67cbfd..289f23765 100644 --- a/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp +++ b/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp @@ -53,17 +53,17 @@ PerformanceIndex computeIntermediatePerformance(const multiple_shooting::Transcr // Costs performance.cost = transcription.cost.f; - // Inequality constraints. - performance.inequalityConstraintsSSE = - dt * getIneqConstraintsSSE(transcription.stateIneqConstraints.f) + dt * getIneqConstraintsSSE(transcription.stateIneqConstraints.f); - // State-input equality constraints if (transcription.stateInputEqConstraints.f.size() > 0) { performance.equalityConstraintsSSE = dt * transcription.stateInputEqConstraints.f.squaredNorm(); } else { - performance.equalityConstraintsSSE = 0; + performance.equalityConstraintsSSE = 0.0; } + // Inequality constraints. + performance.inequalityConstraintsSSE = + dt * getIneqConstraintsSSE(transcription.stateIneqConstraints.f) + dt * getIneqConstraintsSSE(transcription.stateIneqConstraints.f); + return performance; } @@ -90,7 +90,7 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt if (stateInputEqConstraints.size() > 0) { performance.equalityConstraintsSSE = dt * stateInputEqConstraints.squaredNorm(); } else { - performance.equalityConstraintsSSE = 0; + performance.equalityConstraintsSSE = 0.0; } } @@ -98,7 +98,9 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { const vector_t stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints); + performance.inequalityConstraintsSSE = dt * getIneqConstraintsSSE(stateIneqConstraints); + } else { + performance.inequalityConstraintsSSE = 0.0; } // State-input inequality constraints. @@ -116,6 +118,13 @@ PerformanceIndex computeTerminalPerformance(const multiple_shooting::TerminalTra performance.cost = transcription.cost.f; + // Equality constraints + if (transcription.eqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE = transcription.eqConstraints.f.squaredNorm(); + } else { + performance.equalityConstraintsSSE = 0.0; + } + // State inequality constraints. performance.inequalityConstraintsSSE = getIneqConstraintsSSE(transcription.ineqConstraints.f); @@ -130,6 +139,16 @@ PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimal performance.cost = computeFinalCost(optimalControlProblem, t, x); + if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { + const vector_t eqConstraints = + optimalControlProblem.finalEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + if (eqConstraints.size() > 0) { + performance.equalityConstraintsSSE = eqConstraints.squaredNorm(); + } else { + performance.equalityConstraintsSSE = 0.0; + } + } + if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { const vector_t ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); @@ -147,6 +166,13 @@ PerformanceIndex computeEventPerformance(const multiple_shooting::EventTranscrip performance.cost = transcription.cost.f; + // Equality constraints + if (transcription.eqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE = transcription.eqConstraints.f.squaredNorm(); + } else { + performance.equalityConstraintsSSE = 0.0; + } + // State inequality constraints. performance.inequalityConstraintsSSE = getIneqConstraintsSSE(transcription.ineqConstraints.f); @@ -166,6 +192,16 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon performance.cost = computeEventCost(optimalControlProblem, t, x); + if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { + const vector_t eqConstraints = + optimalControlProblem.preJumpEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + if (eqConstraints.size() > 0) { + performance.equalityConstraintsSSE = eqConstraints.squaredNorm(); + } else { + performance.equalityConstraintsSSE = 0.0; + } + } + if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { const vector_t ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp similarity index 100% rename from ocs2_sqp/ocs2_sqp/test/testTranscription.cpp rename to ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp From a821cb213879df471329948bab4f028d346fffb9 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 29 Nov 2022 16:33:33 +0100 Subject: [PATCH 356/512] add eqConstraints evaluations --- ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index d02037687..c1f473dd4 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -135,8 +135,6 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); } - eqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size()); - return transcription; } @@ -173,7 +171,6 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); } - eqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size()); return transcription; } From 455bfecdfec4a2fd4200e4eb31a5a47d92af79af Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 29 Nov 2022 17:06:58 +0100 Subject: [PATCH 357/512] introducing getEqConstraintsSSE(f) --- .../src/PerformanceIndexComputation.cpp | 46 ++++++------------- 1 file changed, 14 insertions(+), 32 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp b/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp index 289f23765..fda5f6c3c 100644 --- a/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp +++ b/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp @@ -35,6 +35,14 @@ namespace ocs2 { namespace sqp { namespace { +scalar_t getEqConstraintsSSE(const vector_t& eqConstraint) { + if (eqConstraint.size() == 0) { + return 0.0; + } else { + return eqConstraint.squaredNorm(); + } +} + scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { if (ineqConstraint.size() == 0) { return 0.0; @@ -54,11 +62,7 @@ PerformanceIndex computeIntermediatePerformance(const multiple_shooting::Transcr performance.cost = transcription.cost.f; // State-input equality constraints - if (transcription.stateInputEqConstraints.f.size() > 0) { - performance.equalityConstraintsSSE = dt * transcription.stateInputEqConstraints.f.squaredNorm(); - } else { - performance.equalityConstraintsSSE = 0.0; - } + performance.equalityConstraintsSSE = dt * getEqConstraintsSSE(transcription.stateInputEqConstraints.f); // Inequality constraints. performance.inequalityConstraintsSSE = @@ -87,11 +91,7 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt if (!optimalControlProblem.equalityConstraintPtr->empty()) { const vector_t stateInputEqConstraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - if (stateInputEqConstraints.size() > 0) { - performance.equalityConstraintsSSE = dt * stateInputEqConstraints.squaredNorm(); - } else { - performance.equalityConstraintsSSE = 0.0; - } + performance.equalityConstraintsSSE = dt * getEqConstraintsSSE(stateInputEqConstraints); } // State inequality constraints. @@ -99,8 +99,6 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt const vector_t stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); performance.inequalityConstraintsSSE = dt * getIneqConstraintsSSE(stateIneqConstraints); - } else { - performance.inequalityConstraintsSSE = 0.0; } // State-input inequality constraints. @@ -119,11 +117,7 @@ PerformanceIndex computeTerminalPerformance(const multiple_shooting::TerminalTra performance.cost = transcription.cost.f; // Equality constraints - if (transcription.eqConstraints.f.size() > 0) { - performance.equalityConstraintsSSE = transcription.eqConstraints.f.squaredNorm(); - } else { - performance.equalityConstraintsSSE = 0.0; - } + performance.equalityConstraintsSSE = getEqConstraintsSSE(transcription.eqConstraints.f); // State inequality constraints. performance.inequalityConstraintsSSE = getIneqConstraintsSSE(transcription.ineqConstraints.f); @@ -142,11 +136,7 @@ PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimal if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { const vector_t eqConstraints = optimalControlProblem.finalEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - if (eqConstraints.size() > 0) { - performance.equalityConstraintsSSE = eqConstraints.squaredNorm(); - } else { - performance.equalityConstraintsSSE = 0.0; - } + performance.equalityConstraintsSSE = getEqConstraintsSSE(eqConstraints); } if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { @@ -167,11 +157,7 @@ PerformanceIndex computeEventPerformance(const multiple_shooting::EventTranscrip performance.cost = transcription.cost.f; // Equality constraints - if (transcription.eqConstraints.f.size() > 0) { - performance.equalityConstraintsSSE = transcription.eqConstraints.f.squaredNorm(); - } else { - performance.equalityConstraintsSSE = 0.0; - } + performance.equalityConstraintsSSE = getEqConstraintsSSE(transcription.eqConstraints.f); // State inequality constraints. performance.inequalityConstraintsSSE = getIneqConstraintsSSE(transcription.ineqConstraints.f); @@ -195,11 +181,7 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { const vector_t eqConstraints = optimalControlProblem.preJumpEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - if (eqConstraints.size() > 0) { - performance.equalityConstraintsSSE = eqConstraints.squaredNorm(); - } else { - performance.equalityConstraintsSSE = 0.0; - } + performance.equalityConstraintsSSE = getEqConstraintsSSE(eqConstraints); } if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { From 5b989d0de5d9e26928da647b8ee16d574615b2b8 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 29 Nov 2022 20:09:56 +0100 Subject: [PATCH 358/512] introduction of FilterLinesearch --- ocs2_oc/CMakeLists.txt | 1 + .../search_strategy/FilterLinesearch.h | 70 +++++++++++++++++ .../src/search_strategy/FilterLinesearch.cpp | 76 +++++++++++++++++++ 3 files changed, 147 insertions(+) create mode 100644 ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h create mode 100644 ocs2_oc/src/search_strategy/FilterLinesearch.cpp diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index db09a027a..16674447b 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -66,6 +66,7 @@ add_library(${PROJECT_NAME} src/synchronized_module/LoopshapingReferenceManager.cpp src/synchronized_module/LoopshapingSynchronizedModule.cpp src/synchronized_module/AugmentedLagrangianObserver.cpp + src/search_strategy/FilterLinesearch.cpp src/trajectory_adjustment/TrajectorySpreading.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h b/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h new file mode 100644 index 000000000..2952f8596 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h @@ -0,0 +1,70 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> + +namespace ocs2 { + +/* + * Filter linesearch based on: + * "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming" + * https://link.springer.com/article/10.1007/s10107-004-0559-y + * + * step acceptance criteria with c = costs, g = the norm of constraint violation, and w = [x; u] + */ +struct FilterLinesearch { + enum class StepType { UNKNOWN, CONSTRAINT, DUAL, COST, ZERO }; + + scalar_t g_max = 1e6; // (1): IF g{i+1} > g_max REQUIRE g{i+1} < (1-gamma_c) * g{i} + scalar_t g_min = 1e-6; // (2): ELSE IF (g{i} < g_min AND g{i+1} < g_min AND dc/dw'{i} * delta_w < 0) REQUIRE Armijo condition + scalar_t gamma_c = 1e-6; // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i} + scalar_t armijoFactor = 1e-4; // Armijo condition: c{i+1} < c{i} + armijoFactor * armijoDescentMetric{i} + + /** + * Checks that the step is accepted. + * + * @param [in] baselinePerformance : The zero step PerformanceIndex + * @param [in] stepPerformance : The step PerformanceIndex + * @param [in] armijoDescentMetric : The step Armijo descent metric defined as dc/dw' * delta_w + */ + std::pair<bool, StepType> acceptStep(const PerformanceIndex& baselinePerformance, const PerformanceIndex& stepPerformance, scalar_t armijoDescentMetric) const; + + /** Compute total constraint violation */ + static scalar_t totalConstraintViolation(const PerformanceIndex& performance) { + return std::sqrt(performance.dynamicsViolationSSE + performance.equalityConstraintsSSE); + } +}; + +/** Transforms the StepType to string */ +std::string toString(const FilterLinesearch::StepType& stepType); + +} // namespace ocs2 diff --git a/ocs2_oc/src/search_strategy/FilterLinesearch.cpp b/ocs2_oc/src/search_strategy/FilterLinesearch.cpp new file mode 100644 index 000000000..b9b39b54e --- /dev/null +++ b/ocs2_oc/src/search_strategy/FilterLinesearch.cpp @@ -0,0 +1,76 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_oc/search_strategy/FilterLinesearch.h" + +namespace ocs2 { + +std::pair<bool, FilterLinesearch::StepType> FilterLinesearch::acceptStep(const PerformanceIndex& baselinePerformance, + const PerformanceIndex& stepPerformance, + scalar_t armijoDescentMetric) const { + const scalar_t baselineConstraintViolation = totalConstraintViolation(baselinePerformance); + const scalar_t stepConstraintViolation = totalConstraintViolation(stepPerformance); + + // Step acceptance and record step type + if (stepConstraintViolation > g_max) { + // High constraint violation. Only accept decrease in constraints. + const bool accepted = stepConstraintViolation < ((1.0 - gamma_c) * baselineConstraintViolation); + return std::make_pair(accepted, StepType::CONSTRAINT); + + } else if (stepConstraintViolation < g_min && baselineConstraintViolation < g_min && armijoDescentMetric < 0.0) { + // With low violation and having a descent direction, require the armijo condition. + const bool accepted = stepPerformance.merit < (baselinePerformance.merit + armijoFactor * armijoDescentMetric); + return std::make_pair(accepted, StepType::COST); + + } else { + // Medium violation: either merit or constraints decrease (with small gamma_c mixing of old constraints) + const bool accepted = stepPerformance.merit < (baselinePerformance.merit - gamma_c * baselineConstraintViolation) || + stepConstraintViolation < ((1.0 - gamma_c) * baselineConstraintViolation); + return std::make_pair(accepted, StepType::DUAL); + } +} + +std::string toString(const FilterLinesearch::StepType& stepType) { + using StepType = FilterLinesearch::StepType; + switch (stepType) { + case StepType::COST: + return "Cost"; + case StepType::CONSTRAINT: + return "Constraint"; + case StepType::DUAL: + return "Dual"; + case StepType::ZERO: + return "Zero"; + case StepType::UNKNOWN: + default: + return "Unknown"; + } +} + +} // namespace ocs2 From 33ea8f24e637004a00a1223f53d97dc17c8acab8 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 29 Nov 2022 20:14:18 +0100 Subject: [PATCH 359/512] extracting general member methods as functions --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 1 + .../ocs2_sqp/MultipleShootingHelpers.h | 118 +++++++++++++++ .../ocs2_sqp/src/MultipleShootingHelpers.cpp | 134 ++++++++++++++++++ 3 files changed, 253 insertions(+) create mode 100644 ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h create mode 100644 ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 237b8a4c4..07b3ef398 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -51,6 +51,7 @@ include_directories( # Multiple shooting solver library add_library(${PROJECT_NAME} src/ConstraintProjection.cpp + src/MultipleShootingHelpers.cpp src/MultipleShootingInitialization.cpp src/MultipleShootingSettings.cpp src/MultipleShootingSolver.cpp diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h new file mode 100644 index 000000000..5ff614545 --- /dev/null +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h @@ -0,0 +1,118 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> +#include <ocs2_oc/oc_data/PrimalSolution.h> + +#include "ocs2_sqp/MultipleShootingSolverStatus.h" +#include "ocs2_sqp/TimeDiscretization.h" + +namespace ocs2 { +namespace multiple_shooting { + +/** Compute 2-norm of the trajectory: sqrt(sum_i v[i]^2) */ +inline scalar_t trajectoryNorm(const vector_array_t& v) { + scalar_t norm = 0.0; + for (const auto& vi : v) { + norm += vi.squaredNorm(); + } + return std::sqrt(norm); +} + +/** Increment the given trajectory as: vNew[i] = v[i] + alpha * dv[i]. It assumes that vNew is already resized to size of v. */ +template <typename Type> +void incrementTrajectory(const std::vector<Type>& v, const std::vector<Type>& dv, const scalar_t alpha, std::vector<Type>& vNew) { + assert(v.size() == dv.size()); + if (v.size() != vNew.size()) { + throw std::runtime_error("[incrementTrajectory] Resize vNew to the size of v!"); + } + + for (int i = 0; i < v.size(); i++) { + if (dv[i].size() > 0) { // account for absence of inputs at events. + vNew[i] = v[i] + alpha * dv[i]; + } else { + vNew[i] = Type(); + } + } +} + +/** + * Computes the Armijo descent metric that determines if the solution is a descent direction for the cost. It calculates sum of + * gradient(cost).dot([dx; du]) over the trajectory. + * + * @param [in] cost: The quadratic approximation of the cost. + * @param [in] deltaXSol: The state trajectory of the QP subproblem solution. + * @param [in] deltaUSol: The input trajectory of the QP subproblem solution. + * @return The Armijo descent metric. + */ +scalar_t armijoDescentMetric(const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& deltaXSol, + const vector_array_t& deltaUSol); + +/** + * Re-map the projected input back to the original space. + * + * @param [in] constraintsProjection: The constraints projection. + * @param [in] deltaXSol: The state trajectory of the QP subproblem solution. + * @param [in, out] deltaUSol: The input trajectory of the QP subproblem solution. + */ +void remapProjectedInput(const std::vector<VectorFunctionLinearApproximation>& constraintsProjection, const vector_array_t& deltaXSol, + vector_array_t& deltaUSol); + +void remapProjectedGain(const std::vector<VectorFunctionLinearApproximation>& constraintsProjection, matrix_array_t& KMatrices); + +/** + * Sets the primal solution (with a feedforward controller) based the LQ subproblem solution. + * + * @param [in] time : The annotated time trajectory + * @param [in] modeSchedule: The mode schedule. + * @param [in] x: The state trajectory of the QP subproblem solution. + * @param [in] u: The input trajectory of the QP subproblem solution. + * @param [out] the updated primal solution + */ +void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, + PrimalSolution& primalSolution); + +/** + * Sets the primal solution (with a linear controller) based the LQ subproblem solution. + * + * @param [in] time : The annotated time trajectory + * @param [in] modeSchedule: The mode schedule. + * @param [in] x: The state trajectory of the QP subproblem solution. + * @param [in] u: The input trajectory of the QP subproblem solution. + * @param [in] KMatrices: The LQR gain trajectory of the QP subproblem solution. + * @param [out] the updated primal solution + */ +void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, + matrix_array_t&& KMatrices, PrimalSolution& primalSolution); + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp new file mode 100644 index 000000000..7487249e4 --- /dev/null +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp @@ -0,0 +1,134 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_sqp/MultipleShootingHelpers.h" + +#include <ocs2_core/control/FeedforwardController.h> +#include <ocs2_core/control/LinearController.h> + +namespace ocs2 { +namespace multiple_shooting { + +scalar_t armijoDescentMetric(const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& deltaXSol, + const vector_array_t& deltaUSol) { + // To determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + scalar_t metric = 0.0; + for (int i = 0; i < cost.size(); i++) { + if (cost[i].dfdx.size() > 0) { + metric += cost[i].dfdx.dot(deltaXSol[i]); + } + if (cost[i].dfdu.size() > 0) { + metric += cost[i].dfdu.dot(deltaUSol[i]); + } + } + return metric; +} + +void remapProjectedInput(const std::vector<VectorFunctionLinearApproximation>& constraintsProjection, const vector_array_t& deltaXSol, + vector_array_t& deltaUSol) { + vector_t tmp; // 1 temporary for re-use. + for (int i = 0; i < deltaUSol.size(); ++i) { + if (constraintsProjection[i].f.size() > 0) { + tmp.noalias() = constraintsProjection[i].dfdu * deltaUSol[i]; + deltaUSol[i] = tmp + constraintsProjection[i].f; + deltaUSol[i].noalias() += constraintsProjection[i].dfdx * deltaXSol[i]; + } + } +} + +void remapProjectedGain(const std::vector<VectorFunctionLinearApproximation>& constraintsProjection, matrix_array_t& KMatrices) { + matrix_t tmp; // 1 temporary for re-use. + for (int i = 0; i < KMatrices.size(); ++i) { + if (constraintsProjection[i].f.size() > 0) { + tmp.noalias() = constraintsProjection[i].dfdu * KMatrices[i]; + KMatrices[i] = tmp + constraintsProjection[i].dfdx; + } + } +} + +void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, + PrimalSolution& primalSolution) { + // Correct for missing inputs at PreEvents and the terminal time + for (int i = 0; i < u.size(); ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { + u[i] = u[i - 1]; + } + } + // Repeat last input to make equal length vectors + u.push_back(u.back()); + + // Construct nominal time, state and input trajectories + primalSolution.clear(); + primalSolution.stateTrajectory_ = std::move(x); + primalSolution.inputTrajectory_ = std::move(u); + std::tie(primalSolution.timeTrajectory_, primalSolution.postEventIndices_) = toTime(time); + primalSolution.modeSchedule_ = std::move(modeSchedule); + primalSolution.controllerPtr_.reset(new FeedforwardController(primalSolution.timeTrajectory_, primalSolution.inputTrajectory_)); +} + +void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, + matrix_array_t&& KMatrices, PrimalSolution& primalSolution) { + // Compute feedback, before x and u are moved to primal solution + // see doc/LQR_full.pdf for detailed derivation for feedback terms + vector_array_t uff = u; // Copy and adapt in loop + for (int i = 0; i < KMatrices.size(); ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { + uff[i] = uff[i - 1]; + KMatrices[i] = KMatrices[i - 1]; + } else { + // Linear controller has convention u = uff + K * x; + // We computed u = u'(t) + K (x - x'(t)); + // >> uff = u'(t) - K x'(t) + uff[i].noalias() -= KMatrices[i] * x[i]; + } + } + // Copy last one to get correct length + uff.push_back(uff.back()); + KMatrices.push_back(KMatrices.back()); + + // Correct for missing inputs at PreEvents and the terminal time + for (int i = 0; i < u.size(); ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { + u[i] = u[i - 1]; + } + } + // Repeat last input to make equal length vectors + u.push_back(u.back()); + + // Construct nominal time, state and input trajectories + primalSolution.clear(); + primalSolution.stateTrajectory_ = std::move(x); + primalSolution.inputTrajectory_ = std::move(u); + std::tie(primalSolution.timeTrajectory_, primalSolution.postEventIndices_) = toTime(time); + primalSolution.modeSchedule_ = std::move(modeSchedule); + primalSolution.controllerPtr_.reset(new LinearController(primalSolution.timeTrajectory_, std::move(uff), std::move(KMatrices))); +} + +} // namespace multiple_shooting +} // namespace ocs2 From aa7d24b9e9eefa46b5897dc721e659a377f4e1a2 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 29 Nov 2022 20:16:52 +0100 Subject: [PATCH 360/512] moving state-input trajectory initialization to MultipleShootingInitialization --- .../ocs2_sqp/MultipleShootingInitialization.h | 26 ++++++++-- .../src/MultipleShootingInitialization.cpp | 51 ++++++++++++++++--- 2 files changed, 68 insertions(+), 9 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h index ad5137a4d..4f58e3ee6 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h @@ -31,8 +31,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/initialization/Initializer.h> +#include <ocs2_core/misc/LinearInterpolation.h> #include <ocs2_oc/oc_data/PrimalSolution.h> +#include "ocs2_sqp/TimeDiscretization.h" + namespace ocs2 { namespace multiple_shooting { @@ -57,10 +60,12 @@ inline std::pair<vector_t, vector_t> initializeIntermediateNode(Initializer& ini * @param primalSolution : previous solution * @param t : Start of the discrete interval * @param tNext : End time of te discrete interval - * @param x : Starting state of the discrete interval * @return {u(t), x(tNext)} : input and state transition */ -std::pair<vector_t, vector_t> initializeIntermediateNode(PrimalSolution& primalSolution, scalar_t t, scalar_t tNext, const vector_t& x); +inline std::pair<vector_t, vector_t> initializeIntermediateNode(const PrimalSolution& primalSolution, scalar_t t, scalar_t tNext) { + return {LinearInterpolation::interpolate(t, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_), + LinearInterpolation::interpolate(tNext, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_)}; +} /** * Initialize the state jump at an event node. @@ -74,5 +79,20 @@ inline vector_t initializeEventNode(scalar_t t, const vector_t& x) { return x; } +/** + * Initializes for the state-input trajectories. It interpolates the primalSolution for the starting intersecting time period and then uses + * initializer for the tail. + * + * @param [in] initState : Initial state + * @param [in] timeDiscretization : The annotated time trajectory + * @param [in] primalSolution : previous solution + * @param [in] initializer : System initializer + * @param [out] stateTrajectory : The initialized state trajectory + * @param [out] inputTrajectory : The initialized input trajectory + */ +void initializeStateInputTrajectories(const vector_t& initState, const std::vector<AnnotatedTime>& timeDiscretization, + const PrimalSolution& primalSolution, Initializer& initializer, vector_array_t& stateTrajectory, + vector_array_t& inputTrajectory); + } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp index ebce7ef06..3bdb9eb53 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp @@ -29,15 +29,54 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_sqp/MultipleShootingInitialization.h" -#include <ocs2_core/misc/LinearInterpolation.h> - namespace ocs2 { namespace multiple_shooting { -std::pair<vector_t, vector_t> initializeIntermediateNode(PrimalSolution& primalSolution, scalar_t t, scalar_t tNext, const vector_t& x) { - return {LinearInterpolation::interpolate(t, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_), - LinearInterpolation::interpolate(tNext, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_)}; +void initializeStateInputTrajectories(const vector_t& initState, const std::vector<AnnotatedTime>& timeDiscretization, + const PrimalSolution& primalSolution, Initializer& initializer, vector_array_t& stateTrajectory, + vector_array_t& inputTrajectory) { + const int N = static_cast<int>(timeDiscretization.size()) - 1; // // size of the input trajectory + stateTrajectory.clear(); + stateTrajectory.reserve(N + 1); + inputTrajectory.clear(); + inputTrajectory.reserve(N); + + // Determine till when to use the previous solution + scalar_t interpolateStateTill = timeDiscretization.front().time; + scalar_t interpolateInputTill = timeDiscretization.front().time; + if (primalSolution.timeTrajectory_.size() >= 2) { + interpolateStateTill = primalSolution.timeTrajectory_.back(); + interpolateInputTill = primalSolution.timeTrajectory_[primalSolution.timeTrajectory_.size() - 2]; + } + + // Initial state + const scalar_t initTime = getIntervalStart(timeDiscretization[0]); + if (initTime < interpolateStateTill) { + stateTrajectory.push_back(LinearInterpolation::interpolate(initTime, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_)); + } else { + stateTrajectory.push_back(initState); + } + + for (int i = 0; i < N; i++) { + if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { + // Event Node + inputTrajectory.push_back(vector_t()); // no input at event node + stateTrajectory.push_back(initializeEventNode(timeDiscretization[i].time, stateTrajectory.back())); + } else { + // Intermediate node + const scalar_t time = getIntervalStart(timeDiscretization[i]); + const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); + vector_t input, nextState; + if (time > interpolateInputTill || nextTime > interpolateStateTill) { // Using initializer + std::tie(input, nextState) = initializeIntermediateNode(initializer, time, nextTime, stateTrajectory.back()); + } else { // interpolate previous solution + std::tie(input, nextState) = initializeIntermediateNode(primalSolution, time, nextTime); + } + inputTrajectory.push_back(std::move(input)); + stateTrajectory.push_back(std::move(nextState)); + } + } } } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 From 3fa8d4b2a752261a68d725d863e09025f2b58781 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 29 Nov 2022 20:21:20 +0100 Subject: [PATCH 361/512] Transforming annotated time to regular time --- .../ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h | 10 +++++++++- ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp | 13 +++++++++++++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h index e235115a0..2f19dae53 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h @@ -61,7 +61,7 @@ scalar_t getIntervalEnd(const AnnotatedTime& end); scalar_t getIntervalDuration(const AnnotatedTime& start, const AnnotatedTime& end); /** - * Decides on time discretization along the horizon. Tries to makes step of dt, but will also ensure that eventtimes are part of the + * Decides on time discretization along the horizon. Tries to makes step of dt, but will also ensure that event times are part of the * discretization. * * @param initTime : start time. @@ -76,4 +76,12 @@ std::vector<AnnotatedTime> timeDiscretizationWithEvents(scalar_t initTime, scala const scalar_array_t& eventTimes, scalar_t dt_min = 10.0 * numeric_traits::limitEpsilon<scalar_t>()); +/** + * Transforms the annotated time trajectory to a regular time trajectory and an array of indices indicating the post-event times. + * + * @param annotatedTime : Annotated time trajectory. + * @return The pair of time and post-event indices. + */ +std::pair<scalar_array_t, size_array_t> toTime(const std::vector<AnnotatedTime>& annotatedTime); + } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp b/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp index 7f2605ab2..2ef9b6566 100644 --- a/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp +++ b/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp @@ -113,4 +113,17 @@ std::vector<AnnotatedTime> timeDiscretizationWithEvents(scalar_t initTime, scala return timeDiscretizationWithDoubleEvents; } +std::pair<scalar_array_t, size_array_t> toTime(const std::vector<AnnotatedTime>& annotatedTime) { + size_array_t postEventIndices; + scalar_array_t timeTrajectory; + timeTrajectory.reserve(annotatedTime.size()); + for (size_t i = 0; i < annotatedTime.size(); i++) { + timeTrajectory.push_back(annotatedTime[i].time); + if (annotatedTime[i].event == AnnotatedTime::Event::PreEvent) { + postEventIndices.push_back(i + 1); + } + } + return {timeTrajectory, postEventIndices}; +} + } // namespace ocs2 From 921a40697e6b533947abb26303f741a441ec3c09 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 29 Nov 2022 20:21:58 +0100 Subject: [PATCH 362/512] moved to FilterLinesearch --- .../ocs2_sqp/MultipleShootingSolverStatus.h | 7 ++----- .../src/MultipleShootingSolverStatus.cpp | 19 +------------------ 2 files changed, 3 insertions(+), 23 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h index d2811de6e..b441e61d2 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h @@ -31,17 +31,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_oc/oc_data/PerformanceIndex.h> +#include <ocs2_oc/search_strategy/FilterLinesearch.h> namespace ocs2 { namespace multiple_shooting { /** Struct to contain the result and logging data of the stepsize computation */ struct StepInfo { - enum class StepType { UNKNOWN, CONSTRAINT, DUAL, COST, ZERO }; - // Step size and type scalar_t stepSize = 0.0; - StepType stepType = StepType::UNKNOWN; + FilterLinesearch::StepType stepType = FilterLinesearch::StepType::UNKNOWN; // Step in primal variables scalar_t dx_norm = 0.0; // norm of the state trajectory update @@ -52,8 +51,6 @@ struct StepInfo { scalar_t totalConstraintViolationAfterStep; // constraint metric used in the line search }; -std::string toString(const StepInfo::StepType& stepType); - /** Different types of convergence */ enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp index 091a4b0e9..4708de46b 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp @@ -32,23 +32,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { -std::string toString(const StepInfo::StepType& stepType) { - using StepType = StepInfo::StepType; - switch (stepType) { - case StepType::COST: - return "Cost"; - case StepType::CONSTRAINT: - return "Constraint"; - case StepType::DUAL: - return "Dual"; - case StepType::ZERO: - return "Zero"; - case StepType::UNKNOWN: - default: - return "Unknown"; - } -} - std::string toString(const Convergence& convergence) { switch (convergence) { case Convergence::ITERATIONS: @@ -66,4 +49,4 @@ std::string toString(const Convergence& convergence) { } } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 From e951695f2f8010b1630410301b0e8b2bb6de11bd Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 29 Nov 2022 20:27:06 +0100 Subject: [PATCH 363/512] changes to sqp solver --- .../include/ocs2_sqp/MultipleShootingSolver.h | 17 +- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 219 ++++-------------- 2 files changed, 41 insertions(+), 195 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index fa8df73a0..3bebdb797 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/oc_problem/OptimalControlProblem.h> #include <ocs2_oc/oc_solver/SolverBase.h> +#include <ocs2_oc/search_strategy/FilterLinesearch.h> #include <hpipm_catkin/HpipmInterface.h> @@ -123,10 +124,6 @@ class MultipleShootingSolver : public SolverBase { /** Get profiling information as a string */ std::string getBenchmarkingInformation() const; - /** Initializes for the state-input trajectories */ - void initializeStateInputTrajectories(const vector_t& initState, const std::vector<AnnotatedTime>& timeDiscretization, - vector_array_t& stateTrajectory, vector_array_t& inputTrajectory); - /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, const vector_array_t& u); @@ -149,22 +146,11 @@ class MultipleShootingSolver : public SolverBase { /** Set up the primal solution based on the optimized state and input trajectories */ void setPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); - /** Compute 2-norm of the trajectory: sqrt(sum_i v[i]^2) */ - static scalar_t trajectoryNorm(const vector_array_t& v); - - /** Compute total constraint violation */ - scalar_t totalConstraintViolation(const PerformanceIndex& performance) const; - /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ multiple_shooting::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u); - /** Checks that the trial step is accepted */ - std::pair<bool, multiple_shooting::StepInfo::StepType> acceptStep(const PerformanceIndex& baselinePerformance, - const PerformanceIndex& stepPerformance, scalar_t armijoDescentMetric, - scalar_t alpha) const; - /** Determine convergence after a step */ multiple_shooting::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const multiple_shooting::StepInfo& stepInfo) const; @@ -175,6 +161,7 @@ class MultipleShootingSolver : public SolverBase { DynamicsSensitivityDiscretizer sensitivityDiscretizer_; std::vector<OptimalControlProblem> ocpDefinitions_; std::unique_ptr<Initializer> initializerPtr_; + FilterLinesearch filterLinesearch_; // Threading ThreadPool threadPool_; diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 4261ccf68..382524b0c 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -32,10 +32,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <iostream> #include <numeric> -#include <ocs2_core/control/FeedforwardController.h> -#include <ocs2_core/control/LinearController.h> #include <ocs2_core/penalties/penalties/RelaxedBarrierPenalty.h> +#include "ocs2_sqp/MultipleShootingHelpers.h" #include "ocs2_sqp/MultipleShootingInitialization.h" #include "ocs2_sqp/MultipleShootingTranscription.h" @@ -50,6 +49,10 @@ MultipleShootingSolver::MultipleShootingSolver(Settings settings, const OptimalC Eigen::setNbThreads(1); // No multithreading within Eigen. Eigen::initParallel(); + if (optimalControlProblem.equalityConstraintPtr->empty()) { + settings_.projectStateInputEqualityConstraints = false; // True does not make sense if there are no constraints. + } + // Dynamics discretization discretizer_ = selectDynamicsDiscretization(settings.integratorType); sensitivityDiscretizer_ = selectDynamicsSensitivityDiscretization(settings.integratorType); @@ -62,9 +65,11 @@ MultipleShootingSolver::MultipleShootingSolver(Settings settings, const OptimalC // Operating points initializerPtr_.reset(initializer.clone()); - if (optimalControlProblem.equalityConstraintPtr->empty()) { - settings_.projectStateInputEqualityConstraints = false; // True does not make sense if there are no constraints. - } + // Linesearch + filterLinesearch_.g_max = settings_.g_max; + filterLinesearch_.g_min = settings_.g_min; + filterLinesearch_.gamma_c = settings_.gamma_c; + filterLinesearch_.armijoFactor = settings_.armijoFactor; } MultipleShootingSolver::~MultipleShootingSolver() { @@ -162,7 +167,7 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat // Initialize the state and input vector_array_t x, u; - initializeStateInputTrajectories(initState, timeDiscretization, x, u); + multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); // Bookkeeping performanceIndeces_.clear(); @@ -217,54 +222,6 @@ void MultipleShootingSolver::runParallel(std::function<void(int)> taskFunction) threadPool_.runParallel(std::move(taskFunction), settings_.nThreads); } -void MultipleShootingSolver::initializeStateInputTrajectories(const vector_t& initState, - const std::vector<AnnotatedTime>& timeDiscretization, - vector_array_t& stateTrajectory, vector_array_t& inputTrajectory) { - const int N = static_cast<int>(timeDiscretization.size()) - 1; // // size of the input trajectory - stateTrajectory.clear(); - stateTrajectory.reserve(N + 1); - inputTrajectory.clear(); - inputTrajectory.reserve(N); - - // Determine till when to use the previous solution - scalar_t interpolateStateTill = timeDiscretization.front().time; - scalar_t interpolateInputTill = timeDiscretization.front().time; - if (primalSolution_.timeTrajectory_.size() >= 2) { - interpolateStateTill = primalSolution_.timeTrajectory_.back(); - interpolateInputTill = primalSolution_.timeTrajectory_[primalSolution_.timeTrajectory_.size() - 2]; - } - - // Initial state - const scalar_t initTime = getIntervalStart(timeDiscretization[0]); - if (initTime < interpolateStateTill) { - stateTrajectory.push_back( - LinearInterpolation::interpolate(initTime, primalSolution_.timeTrajectory_, primalSolution_.stateTrajectory_)); - } else { - stateTrajectory.push_back(initState); - } - - for (int i = 0; i < N; i++) { - if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { - // Event Node - inputTrajectory.push_back(vector_t()); // no input at event node - stateTrajectory.push_back(multiple_shooting::initializeEventNode(timeDiscretization[i].time, stateTrajectory.back())); - } else { - // Intermediate node - const scalar_t time = getIntervalStart(timeDiscretization[i]); - const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); - vector_t input, nextState; - if (time > interpolateInputTill || nextTime > interpolateStateTill) { // Using initializer - std::tie(input, nextState) = - multiple_shooting::initializeIntermediateNode(*initializerPtr_, time, nextTime, stateTrajectory.back()); - } else { // interpolate previous solution - std::tie(input, nextState) = multiple_shooting::initializeIntermediateNode(primalSolution_, time, nextTime, stateTrajectory.back()); - } - inputTrajectory.push_back(std::move(input)); - stateTrajectory.push_back(std::move(nextState)); - } - } -} - MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolution(const vector_t& delta_x0) { // Solve the QP OcpSubproblemSolution solution; @@ -284,27 +241,12 @@ MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolu throw std::runtime_error("[MultipleShootingSolver] Failed to solve QP"); } - // To determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] - solution.armijoDescentMetric = 0.0; - for (int i = 0; i < cost_.size(); i++) { - if (cost_[i].dfdx.size() > 0) { - solution.armijoDescentMetric += cost_[i].dfdx.dot(deltaXSol[i]); - } - if (cost_[i].dfdu.size() > 0) { - solution.armijoDescentMetric += cost_[i].dfdu.dot(deltaUSol[i]); - } - } + // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + solution.armijoDescentMetric = multiple_shooting::armijoDescentMetric(cost_, deltaXSol, deltaUSol); // remap the tilde delta u to real delta u if (settings_.projectStateInputEqualityConstraints) { - vector_t tmp; // 1 temporary for re-use. - for (int i = 0; i < deltaUSol.size(); i++) { - if (constraintsProjection_[i].f.size() > 0) { - tmp.noalias() = constraintsProjection_[i].dfdu * deltaUSol[i]; - deltaUSol[i] = tmp + constraintsProjection_[i].f; - deltaUSol[i].noalias() += constraintsProjection_[i].dfdx * deltaXSol[i]; - } - } + multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); } return solution; @@ -321,64 +263,18 @@ void MultipleShootingSolver::extractValueFunction(const std::vector<AnnotatedTim } void MultipleShootingSolver::setPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { - // Clear old solution - primalSolution_.clear(); - - // Correct for missing inputs at PreEvents - for (int i = 0; i < time.size(); ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { - u[i] = u[i - 1]; - } - } - - // Compute feedback, before x and u are moved to primal solution - vector_array_t uff; - matrix_array_t controllerGain; if (settings_.useFeedbackPolicy) { - // see doc/LQR_full.pdf for detailed derivation for feedback terms - uff = u; // Copy and adapt in loop - controllerGain.reserve(time.size()); + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); matrix_array_t KMatrices = hpipmInterface_.getRiccatiFeedback(dynamics_[0], cost_[0]); - for (int i = 0; (i + 1) < time.size(); i++) { - if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { - uff[i] = uff[i - 1]; - controllerGain.push_back(controllerGain.back()); - } else { - // Linear controller has convention u = uff + K * x; - // We computed u = u'(t) + K (x - x'(t)); - // >> uff = u'(t) - K x'(t) - if (constraintsProjection_[i].f.size() > 0) { - controllerGain.push_back(std::move(constraintsProjection_[i].dfdx)); // Steal! Don't use after this. - controllerGain.back().noalias() += constraintsProjection_[i].dfdu * KMatrices[i]; - } else { - controllerGain.push_back(std::move(KMatrices[i])); - } - uff[i].noalias() -= controllerGain.back() * x[i]; - } + // remap the tilde delta u to real delta u + if (settings_.projectStateInputEqualityConstraints) { + multiple_shooting::remapProjectedGain(constraintsProjection_, KMatrices); } - // Copy last one to get correct length - uff.push_back(uff.back()); - controllerGain.push_back(controllerGain.back()); - } + multiple_shooting::setPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), std::move(KMatrices), primalSolution_); - // Construct nominal time, state and input trajectories - primalSolution_.stateTrajectory_ = std::move(x); - u.push_back(u.back()); // Repeat last input to make equal length vectors - primalSolution_.inputTrajectory_ = std::move(u); - primalSolution_.timeTrajectory_.reserve(time.size()); - for (size_t i = 0; i < time.size(); i++) { - primalSolution_.timeTrajectory_.push_back(time[i].time); - if (time[i].event == AnnotatedTime::Event::PreEvent) { - primalSolution_.postEventIndices_.push_back(i + 1); - } - } - primalSolution_.modeSchedule_ = this->getReferenceManager().getModeSchedule(); - - // Assign controller - if (settings_.useFeedbackPolicy) { - primalSolution_.controllerPtr_.reset(new LinearController(primalSolution_.timeTrajectory_, std::move(uff), std::move(controllerGain))); } else { - primalSolution_.controllerPtr_.reset(new FeedforwardController(primalSolution_.timeTrajectory_, primalSolution_.inputTrajectory_)); + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); + multiple_shooting::setPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), primalSolution_); } } @@ -494,23 +390,11 @@ PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector<An return totalPerformance; } -scalar_t MultipleShootingSolver::trajectoryNorm(const vector_array_t& v) { - scalar_t norm = 0.0; - for (const auto& vi : v) { - norm += vi.squaredNorm(); - } - return std::sqrt(norm); -} - -scalar_t MultipleShootingSolver::totalConstraintViolation(const PerformanceIndex& performance) const { - return std::sqrt(performance.dynamicsViolationSSE + performance.equalityConstraintsSSE); -} - multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u) { - using StepType = multiple_shooting::StepInfo::StepType; + using StepType = FilterLinesearch::StepType; /* * Filter linesearch based on: @@ -524,40 +408,33 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn } // Baseline costs - const scalar_t baselineConstraintViolation = totalConstraintViolation(baseline); + const scalar_t baselineConstraintViolation = FilterLinesearch::totalConstraintViolation(baseline); // Update norm const auto& dx = subproblemSolution.deltaXSol; const auto& du = subproblemSolution.deltaUSol; - const scalar_t deltaUnorm = trajectoryNorm(du); - const scalar_t deltaXnorm = trajectoryNorm(dx); - - // Prepare step info - multiple_shooting::StepInfo stepInfo; + const auto deltaUnorm = multiple_shooting::trajectoryNorm(du); + const auto deltaXnorm = multiple_shooting::trajectoryNorm(dx); scalar_t alpha = 1.0; vector_array_t xNew(x.size()); vector_array_t uNew(u.size()); do { // Compute step - for (int i = 0; i < u.size(); i++) { - if (du[i].size() > 0) { // account for absence of inputs at events. - uNew[i] = u[i] + alpha * du[i]; - } - } - for (int i = 0; i < x.size(); i++) { - xNew[i] = x[i] + alpha * dx[i]; - } + multiple_shooting::incrementTrajectory(u, du, alpha, uNew); + multiple_shooting::incrementTrajectory(x, dx, alpha, xNew); // Compute cost and constraints const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew); // Step acceptance and record step type bool stepAccepted; - std::tie(stepAccepted, stepInfo.stepType) = acceptStep(baseline, performanceNew, subproblemSolution.armijoDescentMetric, alpha); + StepType stepType; + std::tie(stepAccepted, stepType) = + filterLinesearch_.acceptStep(baseline, performanceNew, alpha * subproblemSolution.armijoDescentMetric); if (settings_.printLinesearch) { - std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepInfo.stepType) + std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepType) << (stepAccepted ? std::string{" (Accepted)"} : std::string{" (Rejected)"}) << "\n"; std::cerr << "|dx| = " << alpha * deltaXnorm << "\t|du| = " << alpha * deltaUnorm << "\n"; std::cerr << performanceNew << "\n"; @@ -567,12 +444,16 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn x = std::move(xNew); u = std::move(uNew); + // Prepare step info + multiple_shooting::StepInfo stepInfo; stepInfo.stepSize = alpha; + stepInfo.stepType = stepType; stepInfo.dx_norm = alpha * deltaXnorm; stepInfo.du_norm = alpha * deltaUnorm; stepInfo.performanceAfterStep = performanceNew; - stepInfo.totalConstraintViolationAfterStep = totalConstraintViolation(performanceNew); + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(performanceNew); return stepInfo; + } else { // Try smaller step alpha *= settings_.alpha_decay; @@ -588,12 +469,13 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn } while (alpha >= settings_.alpha_min); // Alpha_min reached -> Don't take a step + multiple_shooting::StepInfo stepInfo; stepInfo.stepSize = 0.0; stepInfo.stepType = StepType::ZERO; stepInfo.dx_norm = 0.0; stepInfo.du_norm = 0.0; stepInfo.performanceAfterStep = baseline; - stepInfo.totalConstraintViolationAfterStep = totalConstraintViolation(baseline); + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline); if (settings_.printLinesearch) { std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; @@ -602,29 +484,6 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn return stepInfo; } -std::pair<bool, multiple_shooting::StepInfo::StepType> MultipleShootingSolver::acceptStep(const PerformanceIndex& baselinePerformance, - const PerformanceIndex& stepPerformance, - scalar_t armijoDescentMetric, - scalar_t alpha) const { - const scalar_t baselineConstraintViolation = totalConstraintViolation(baselinePerformance); - const scalar_t newConstraintViolation = totalConstraintViolation(stepPerformance); - // Step acceptance and record step type - if (newConstraintViolation > settings_.g_max) { - // High constraint violation. Only accept decrease in constraints. - const bool accepted = newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); - return std::make_pair(accepted, multiple_shooting::StepInfo::StepType::CONSTRAINT); - } else if (newConstraintViolation < settings_.g_min && baselineConstraintViolation < settings_.g_min && armijoDescentMetric < 0.0) { - // With low violation and having a descent direction, require the armijo condition. - const bool accepted = stepPerformance.merit < (baselinePerformance.merit + settings_.armijoFactor * alpha * armijoDescentMetric); - return std::make_pair(accepted, multiple_shooting::StepInfo::StepType::COST); - } else { - // Medium violation: either merit or constraints decrease (with small gamma_c mixing of old constraints) - const bool accepted = stepPerformance.merit < (baselinePerformance.merit - settings_.gamma_c * baselineConstraintViolation) || - newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); - return std::make_pair(accepted, multiple_shooting::StepInfo::StepType::DUAL); - } -} - multiple_shooting::Convergence MultipleShootingSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const multiple_shooting::StepInfo& stepInfo) const { using Convergence = multiple_shooting::Convergence; @@ -635,7 +494,7 @@ multiple_shooting::Convergence MultipleShootingSolver::checkConvergence(int iter // Converged because step size is below the specified minimum return Convergence::STEPSIZE; } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.costTol && - totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { + FilterLinesearch::totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { // Converged because the change in merit is below the specified tolerance while the constraint violation is below the minimum return Convergence::METRICS; } else if (stepInfo.dx_norm < settings_.deltaTol && stepInfo.du_norm < settings_.deltaTol) { From ef6dfd3333d104fa925b44825d56f32732aa08fb Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 29 Nov 2022 20:33:23 +0100 Subject: [PATCH 364/512] clang tidy --- .../ocs2_oc/search_strategy/FilterLinesearch.h | 11 ++++++----- ocs2_oc/src/lintTarget.cpp | 3 +++ 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h b/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h index 2952f8596..a11ba7c58 100644 --- a/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h +++ b/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h @@ -44,10 +44,10 @@ namespace ocs2 { struct FilterLinesearch { enum class StepType { UNKNOWN, CONSTRAINT, DUAL, COST, ZERO }; - scalar_t g_max = 1e6; // (1): IF g{i+1} > g_max REQUIRE g{i+1} < (1-gamma_c) * g{i} - scalar_t g_min = 1e-6; // (2): ELSE IF (g{i} < g_min AND g{i+1} < g_min AND dc/dw'{i} * delta_w < 0) REQUIRE Armijo condition - scalar_t gamma_c = 1e-6; // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i} - scalar_t armijoFactor = 1e-4; // Armijo condition: c{i+1} < c{i} + armijoFactor * armijoDescentMetric{i} + scalar_t g_max = 1e6; // (1): IF g{i+1} > g_max REQUIRE g{i+1} < (1-gamma_c) * g{i} + scalar_t g_min = 1e-6; // (2): ELSE IF (g{i} < g_min AND g{i+1} < g_min AND dc/dw'{i} * delta_w < 0) REQUIRE Armijo condition + scalar_t gamma_c = 1e-6; // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i} + scalar_t armijoFactor = 1e-4; // Armijo condition: c{i+1} < c{i} + armijoFactor * armijoDescentMetric{i} /** * Checks that the step is accepted. @@ -56,7 +56,8 @@ struct FilterLinesearch { * @param [in] stepPerformance : The step PerformanceIndex * @param [in] armijoDescentMetric : The step Armijo descent metric defined as dc/dw' * delta_w */ - std::pair<bool, StepType> acceptStep(const PerformanceIndex& baselinePerformance, const PerformanceIndex& stepPerformance, scalar_t armijoDescentMetric) const; + std::pair<bool, StepType> acceptStep(const PerformanceIndex& baselinePerformance, const PerformanceIndex& stepPerformance, + scalar_t armijoDescentMetric) const; /** Compute total constraint violation */ static scalar_t totalConstraintViolation(const PerformanceIndex& performance) { diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index af3c85ad6..a885a9499 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -59,6 +59,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/rollout/StateTriggeredRollout.h> #include <ocs2_oc/rollout/TimeTriggeredRollout.h> +// search_strategy +#include <ocs2_oc/search_strategy/FilterLinesearch.h> + // trajectory_adjustment #include <ocs2_oc/trajectory_adjustment/TrajectorySpreading.h> From 1c175196ec5174d9dea7308faa315ae38dc61720 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 30 Nov 2022 11:18:28 +0100 Subject: [PATCH 365/512] changes after the review --- .../ocs2_sqp/MultipleShootingHelpers.h | 16 +++++++-------- .../include/ocs2_sqp/MultipleShootingSolver.h | 4 ++-- .../include/ocs2_sqp/TimeDiscretization.h | 12 +++++++++-- .../ocs2_sqp/src/MultipleShootingHelpers.cpp | 20 +++++++++++-------- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 9 ++++----- ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp | 12 ++++++++--- 6 files changed, 45 insertions(+), 28 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h index 5ff614545..94a14a4c1 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h @@ -90,29 +90,29 @@ void remapProjectedInput(const std::vector<VectorFunctionLinearApproximation>& c void remapProjectedGain(const std::vector<VectorFunctionLinearApproximation>& constraintsProjection, matrix_array_t& KMatrices); /** - * Sets the primal solution (with a feedforward controller) based the LQ subproblem solution. + * Constructs a primal solution (with a feedforward controller) based the LQ subproblem solution. * * @param [in] time : The annotated time trajectory * @param [in] modeSchedule: The mode schedule. * @param [in] x: The state trajectory of the QP subproblem solution. * @param [in] u: The input trajectory of the QP subproblem solution. - * @param [out] the updated primal solution + * @return The primal solution. */ -void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, - PrimalSolution& primalSolution); +PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, + vector_array_t&& u); /** - * Sets the primal solution (with a linear controller) based the LQ subproblem solution. + * Constructs a primal solution (with a linear controller) based the LQ subproblem solution. * * @param [in] time : The annotated time trajectory * @param [in] modeSchedule: The mode schedule. * @param [in] x: The state trajectory of the QP subproblem solution. * @param [in] u: The input trajectory of the QP subproblem solution. * @param [in] KMatrices: The LQR gain trajectory of the QP subproblem solution. - * @param [out] the updated primal solution + * @return The primal solution. */ -void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, - matrix_array_t&& KMatrices, PrimalSolution& primalSolution); +PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, + matrix_array_t&& KMatrices); } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index bed86da3d..7a662673e 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -143,8 +143,8 @@ class MultipleShootingSolver : public SolverBase { /** Extract the value function based on the last solved QP */ void extractValueFunction(const std::vector<AnnotatedTime>& time, const vector_array_t& x); - /** Set up the primal solution based on the optimized state and input trajectories */ - void setPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); + /** Constructs the primal solution based on the optimized state and input trajectories */ + PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ multiple_shooting::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h index 2f19dae53..ca97cea8a 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h @@ -77,11 +77,19 @@ std::vector<AnnotatedTime> timeDiscretizationWithEvents(scalar_t initTime, scala scalar_t dt_min = 10.0 * numeric_traits::limitEpsilon<scalar_t>()); /** - * Transforms the annotated time trajectory to a regular time trajectory and an array of indices indicating the post-event times. + * Extracts the time trajectory from the annotated time trajectory. * * @param annotatedTime : Annotated time trajectory. * @return The pair of time and post-event indices. */ -std::pair<scalar_array_t, size_array_t> toTime(const std::vector<AnnotatedTime>& annotatedTime); +scalar_array_t toTime(const std::vector<AnnotatedTime>& annotatedTime); + +/** + * Extracts the array of indices indicating the post-event times from the annotated time trajectory. + * + * @param annotatedTime : Annotated time trajectory. + * @return The pair of time and post-event indices. + */ +size_array_t toPostEventIndices(const std::vector<AnnotatedTime>& annotatedTime); } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp index 7487249e4..2a565996a 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp @@ -72,8 +72,8 @@ void remapProjectedGain(const std::vector<VectorFunctionLinearApproximation>& co } } -void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, - PrimalSolution& primalSolution) { +PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, + vector_array_t&& u) { // Correct for missing inputs at PreEvents and the terminal time for (int i = 0; i < u.size(); ++i) { if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { @@ -84,16 +84,18 @@ void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& mo u.push_back(u.back()); // Construct nominal time, state and input trajectories - primalSolution.clear(); + PrimalSolution primalSolution; + primalSolution.timeTrajectory_ = toTime(time); + primalSolution.postEventIndices_ = toPostEventIndices(time); primalSolution.stateTrajectory_ = std::move(x); primalSolution.inputTrajectory_ = std::move(u); - std::tie(primalSolution.timeTrajectory_, primalSolution.postEventIndices_) = toTime(time); primalSolution.modeSchedule_ = std::move(modeSchedule); primalSolution.controllerPtr_.reset(new FeedforwardController(primalSolution.timeTrajectory_, primalSolution.inputTrajectory_)); + return primalSolution; } -void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, - matrix_array_t&& KMatrices, PrimalSolution& primalSolution) { +PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, + matrix_array_t&& KMatrices) { // Compute feedback, before x and u are moved to primal solution // see doc/LQR_full.pdf for detailed derivation for feedback terms vector_array_t uff = u; // Copy and adapt in loop @@ -122,12 +124,14 @@ void setPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& mo u.push_back(u.back()); // Construct nominal time, state and input trajectories - primalSolution.clear(); + PrimalSolution primalSolution; + primalSolution.timeTrajectory_ = toTime(time); + primalSolution.postEventIndices_ = toPostEventIndices(time); primalSolution.stateTrajectory_ = std::move(x); primalSolution.inputTrajectory_ = std::move(u); - std::tie(primalSolution.timeTrajectory_, primalSolution.postEventIndices_) = toTime(time); primalSolution.modeSchedule_ = std::move(modeSchedule); primalSolution.controllerPtr_.reset(new LinearController(primalSolution.timeTrajectory_, std::move(uff), std::move(KMatrices))); + return primalSolution; } } // namespace multiple_shooting diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index c536ba296..c0b293e9a 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -206,7 +206,7 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat } computeControllerTimer_.startTimer(); - setPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); computeControllerTimer_.endTimer(); ++numProblems_; @@ -264,19 +264,18 @@ void MultipleShootingSolver::extractValueFunction(const std::vector<AnnotatedTim } } -void MultipleShootingSolver::setPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { +PrimalSolution MultipleShootingSolver::toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { if (settings_.useFeedbackPolicy) { ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); matrix_array_t KMatrices = hpipmInterface_.getRiccatiFeedback(dynamics_[0], cost_[0]); - // remap the tilde delta u to real delta u if (settings_.projectStateInputEqualityConstraints) { multiple_shooting::remapProjectedGain(constraintsProjection_, KMatrices); } - multiple_shooting::setPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), std::move(KMatrices), primalSolution_); + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), std::move(KMatrices)); } else { ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); - multiple_shooting::setPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), primalSolution_); + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u)); } } diff --git a/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp b/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp index 2ef9b6566..a3672c848 100644 --- a/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp +++ b/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp @@ -113,17 +113,23 @@ std::vector<AnnotatedTime> timeDiscretizationWithEvents(scalar_t initTime, scala return timeDiscretizationWithDoubleEvents; } -std::pair<scalar_array_t, size_array_t> toTime(const std::vector<AnnotatedTime>& annotatedTime) { - size_array_t postEventIndices; +scalar_array_t toTime(const std::vector<AnnotatedTime>& annotatedTime) { scalar_array_t timeTrajectory; timeTrajectory.reserve(annotatedTime.size()); for (size_t i = 0; i < annotatedTime.size(); i++) { timeTrajectory.push_back(annotatedTime[i].time); + } + return timeTrajectory; +} + +size_array_t toPostEventIndices(const std::vector<AnnotatedTime>& annotatedTime) { + size_array_t postEventIndices; + for (size_t i = 0; i < annotatedTime.size(); i++) { if (annotatedTime[i].event == AnnotatedTime::Event::PreEvent) { postEventIndices.push_back(i + 1); } } - return {timeTrajectory, postEventIndices}; + return postEventIndices; } } // namespace ocs2 From e7fd5eb3cb2e10900c1dc5c1e8d1fbe43743f31c Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 30 Nov 2022 12:37:09 +0100 Subject: [PATCH 366/512] fixing the unittests --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 2 +- ocs2_sqp/ocs2_sqp/test/testProjection.cpp | 12 +++++----- .../testTranscriptionPerformanceIndex.cpp | 23 +++++++++++-------- ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp | 6 ++--- 4 files changed, 24 insertions(+), 19 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 2d3dd6e8e..e28279acc 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -78,7 +78,7 @@ if(cmake_clang_tools_FOUND) add_clang_tooling( TARGETS ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR ) diff --git a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp index 8ff923ce8..3518de974 100644 --- a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp @@ -52,9 +52,9 @@ TEST(test_projection, testProjectionQR) { ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); - ASSERT_TRUE((pseudoInverse*constraint.dfdu.transpose()).isIdentity()); - ASSERT_TRUE((pseudoInverse.transpose()*constraint.dfdx).isApprox(- projection.dfdx)); - ASSERT_TRUE((pseudoInverse.transpose()*constraint.f).isApprox(- projection.f)); + ASSERT_TRUE((pseudoInverse * constraint.dfdu.transpose()).isIdentity()); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.dfdx).isApprox(-projection.dfdx)); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.f).isApprox(-projection.f)); } TEST(test_projection, testProjectionLU) { @@ -85,9 +85,9 @@ TEST(test_projection, testProjectionLU) { ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); - ASSERT_TRUE((pseudoInverse*constraint.dfdu.transpose()).isIdentity()); - ASSERT_TRUE((pseudoInverse.transpose()*constraint.dfdx).isApprox(- projection.dfdx)); - ASSERT_TRUE((pseudoInverse.transpose()*constraint.f).isApprox(- projection.f)); + ASSERT_TRUE((pseudoInverse * constraint.dfdu.transpose()).isIdentity()); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.dfdx).isApprox(-projection.dfdx)); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.f).isApprox(-projection.f)); } TEST(test_projection, testProjectionMultiplierCoefficients) { diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp index 29aafd6b1..25067fdb2 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp @@ -49,21 +49,24 @@ using namespace ocs2::multiple_shooting; using namespace ocs2::sqp; TEST(test_transcription, intermediate_performance) { + constexpr int nx = 2; + constexpr int nu = 2; + // optimal control problem OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/sqp_test_generated"); // inequality constraints - problem.inequalityConstraintPtr->add("inequalityConstraint", getOcs2Constraints(getRandomConstraints(2, 2, 3))); - problem.stateInequalityConstraintPtr->add("stateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 4))); + problem.inequalityConstraintPtr->add("inequalityConstraint", getOcs2Constraints(getRandomConstraints(nx, nu, 3))); + problem.stateInequalityConstraintPtr->add("stateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); auto discretizer = selectDynamicsDiscretization(SensitivityIntegratorType::RK4); auto sensitivityDiscretizer = selectDynamicsSensitivityDiscretization(SensitivityIntegratorType::RK4); scalar_t t = 0.5; scalar_t dt = 0.1; - const vector_t x = (vector_t(2) << 1.0, 0.1).finished(); - const vector_t x_next = (vector_t(2) << 1.1, 0.2).finished(); - const vector_t u = (vector_t(2) << 0.1, 1.3).finished(); + const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); + const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); + const vector_t u = (vector_t(nu) << 0.1, 1.3).finished(); const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, t, dt, x, x_next, u); const auto performance = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); @@ -72,8 +75,9 @@ TEST(test_transcription, intermediate_performance) { } TEST(test_transcription, terminal_performance) { - int nx = 3; + constexpr int nx = 3; + // optimal control problem OptimalControlProblem problem; // cost @@ -81,7 +85,7 @@ TEST(test_transcription, terminal_performance) { problem.finalSoftConstraintPtr->add("finalSoftCost", getOcs2StateCost(getRandomCost(nx, 0))); // inequality constraints - problem.finalInequalityConstraintPtr->add("finalInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 4))); + problem.finalInequalityConstraintPtr->add("finalInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); problem.targetTrajectoriesPtr = &targetTrajectories; @@ -95,8 +99,9 @@ TEST(test_transcription, terminal_performance) { } TEST(test_transcription, event_performance) { - int nx = 2; + constexpr int nx = 2; + // optimal control problem OptimalControlProblem problem; // dynamics @@ -108,7 +113,7 @@ TEST(test_transcription, event_performance) { problem.preJumpCostPtr->add("eventCost", getOcs2StateCost(getRandomCost(nx, 0))); // inequality constraints - problem.preJumpInequalityConstraintPtr->add("preJumpInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(2, 2, 4))); + problem.preJumpInequalityConstraintPtr->add("preJumpInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); problem.targetTrajectoriesPtr = &targetTrajectories; diff --git a/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp b/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp index d9e6fe2a5..23e1e13e5 100644 --- a/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp @@ -44,7 +44,7 @@ TEST(test_valuefunction, linear_quadratic_problem) { constexpr int Nsample = 10; constexpr ocs2::scalar_t tol = 1e-9; const ocs2::scalar_t startTime = 0.0; - const ocs2::scalar_t eventTime = 1.0/3.0; + const ocs2::scalar_t eventTime = 1.0 / 3.0; const ocs2::scalar_t finalTime = 1.0; ocs2::OptimalControlProblem problem; @@ -67,7 +67,7 @@ TEST(test_valuefunction, linear_quadratic_problem) { problem.targetTrajectoriesPtr = &targetTrajectories; // Constraint - problem.equalityConstraintPtr->add("constraint", ocs2::getOcs2Constraints(ocs2::getRandomConstraints(n, m, nc))); + problem.equalityConstraintPtr->add("constraint", ocs2::getOcs2Constraints(ocs2::getRandomConstraints(n, m, nc))); ocs2::DefaultInitializer zeroInitializer(m); @@ -90,7 +90,7 @@ TEST(test_valuefunction, linear_quadratic_problem) { const ocs2::vector_t zeroState = ocs2::vector_t::Random(n); solver.reset(); solver.run(startTime, zeroState, finalTime); - const auto costToGo = solver.getValueFunction(startTime, zeroState); + const auto costToGo = solver.getValueFunction(startTime, zeroState); const ocs2::scalar_t zeroCost = solver.getPerformanceIndeces().cost; // Solve for random states and check consistency with value function From e5f56e8b5b52c86b35c96452ddd2e8d5223ab0ec Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 30 Nov 2022 12:38:39 +0100 Subject: [PATCH 367/512] fix doc --- ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h index ca97cea8a..304cc3daf 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h @@ -80,7 +80,7 @@ std::vector<AnnotatedTime> timeDiscretizationWithEvents(scalar_t initTime, scala * Extracts the time trajectory from the annotated time trajectory. * * @param annotatedTime : Annotated time trajectory. - * @return The pair of time and post-event indices. + * @return The time trajectory. */ scalar_array_t toTime(const std::vector<AnnotatedTime>& annotatedTime); @@ -88,7 +88,7 @@ scalar_array_t toTime(const std::vector<AnnotatedTime>& annotatedTime); * Extracts the array of indices indicating the post-event times from the annotated time trajectory. * * @param annotatedTime : Annotated time trajectory. - * @return The pair of time and post-event indices. + * @return The post-event indices. */ size_array_t toPostEventIndices(const std::vector<AnnotatedTime>& annotatedTime); From ab8d4d504010c4fdc447cbc45a2e4a215fc9cd68 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 1 Dec 2022 16:52:21 +0100 Subject: [PATCH 368/512] moving luConstraintProjection and qrConstraintProjection to LinearAlgebra --- .../include/ocs2_core/misc/LinearAlgebra.h | 97 ++++---- ocs2_core/src/misc/LinearAlgebra.cpp | 81 +++++++ ocs2_core/test/misc/testLinearAlgebra.cpp | 213 +++++++++++++----- .../src/MultipleShootingTranscription.cpp | 7 +- 4 files changed, 279 insertions(+), 119 deletions(-) diff --git a/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h b/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h index b16175052..29c23a8d4 100644 --- a/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h +++ b/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h @@ -38,41 +38,18 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace LinearAlgebra { -// forward declarations -void makePsdEigenvalue(matrix_t& squareMatrix, scalar_t minEigenvalue); - -void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue); - -void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, matrix_t& DmDagger, matrix_t& DmDaggerTRmDmDaggerUUT, - matrix_t& RmInvConstrainedUUT); - /** * Set the eigenvalues of a triangular matrix to a minimum magnitude (maintaining the sign). */ -inline void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue = numeric_traits::weakEpsilon<scalar_t>()) { - for (Eigen::Index i = 0; i < Lr.rows(); ++i) { - scalar_t& eigenValue = Lr(i, i); // diagonal element is the eigenvalue - if (eigenValue < 0.0) { - eigenValue = std::min(-minEigenValue, eigenValue); - } else { - eigenValue = std::max(minEigenValue, eigenValue); - } - } -} +void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue = numeric_traits::weakEpsilon<scalar_t>()); /** * Makes the input matrix PSD using a eigenvalue decomposition. * - * @tparam Derived type. - * @param squareMatrix: The matrix to become PSD. + * @param [in, out] squareMatrix: The matrix to become PSD. * @param [in] minEigenvalue: minimum eigenvalue. */ -template <typename Derived> -void makePsdEigenvalue(Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()) { - matrix_t mat = squareMatrix; - makePsdEigenvalue(mat, minEigenvalue); - squareMatrix = mat; -} +void makePsdEigenvalue(matrix_t& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()); /** * Makes the input matrix PSD based on Gershgorin circle theorem. If the input matrix is positive definite and diagonally dominant, @@ -89,20 +66,10 @@ void makePsdEigenvalue(Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEig * (1) Aii < minEigenvalue + Ri ==> minEigenvalue < lambda < minEigenvalue + 2 Ri * (2) Aii > minEigenvalue + Ri ==> minEigenvalue < Aii - Ri < lambda < Aii + Ri * - * @tparam Derived type. - * @param squareMatrix: The matrix to become PSD. + * @param [in, out] squareMatrix: The matrix to become PSD. * @param [in] minEigenvalue: minimum eigenvalue. */ -template <typename Derived> -void makePsdGershgorin(Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()) { - assert(squareMatrix.rows() == squareMatrix.cols()); - squareMatrix = 0.5 * (squareMatrix + squareMatrix.transpose()).eval(); - for (size_t i = 0; i < squareMatrix.rows(); i++) { - // Gershgorin radius: since the matrix is symmetric we use column sum instead of row sum - auto Ri = squareMatrix.col(i).cwiseAbs().sum() - std::abs(squareMatrix(i, i)); - squareMatrix(i, i) = std::max(squareMatrix(i, i), Ri + minEigenvalue); - } -} +void makePsdGershgorin(matrix_t& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()); /** * Makes the input matrix PSD based on modified Cholesky decomposition. @@ -116,32 +83,19 @@ void makePsdGershgorin(Eigen::MatrixBase<Derived>& squareMatrix, scalar_t minEig * References : C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with Limited memory, SIAM J. Sci. Comput. * 21(1), pp. 24-45, 1999 * - * @tparam Derived type. - * @param A: The matrix to become PSD. + * @param [in, out] A: The matrix to become PSD. * @param [in] minEigenvalue: minimum eigenvalue. */ -template <typename Derived> -void makePsdCholesky(Eigen::MatrixBase<Derived>& A, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()) { - matrix_t mat = A; - makePsdCholesky(mat, minEigenvalue); - A = mat; -} +void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()); /** * Computes the U*U^T decomposition associated to the inverse of the input matrix, where U is an upper triangular * matrix. Note that the U*U^T decomposition is different from the Cholesky decomposition (U^T*U). * - * @tparam Derived type. * @param [in] Am: A symmetric square positive definite matrix * @param [out] AmInvUmUmT: The upper-triangular matrix associated to the UUT decomposition of inv(Am) matrix. */ -template <typename Derived> -void computeInverseMatrixUUT(const Derived& Am, Derived& AmInvUmUmT) { - // Am = Lm Lm^T --> inv(Am) = inv(Lm^T) inv(Lm) where Lm^T is upper triangular - Eigen::LLT<Derived> lltOfA(Am); - AmInvUmUmT.setIdentity(Am.rows(), Am.cols()); // for dynamic size matrices - lltOfA.matrixU().solveInPlace(AmInvUmUmT); -} +void computeInverseMatrixUUT(const matrix_t& Am, matrix_t& AmInvUmUmT); /** * Computes constraint projection for linear constraints C*x + D*u - e = 0, with the weighting inv(Rm) @@ -154,11 +108,36 @@ void computeInverseMatrixUUT(const Derived& Am, Derived& AmInvUmUmT) { * @param [out] RmInvConstrainedUUT: The VVT decomposition of (I-DmDagger*Dm) * inv(Rm) * (I-DmDagger*Dm)^T where V is of * the dimension n_u*(n_u-n_c) with n_u = Rm.rows() and n_c = Dm.rows(). */ -template <typename DerivedInputMatrix> -void computeConstraintProjection(const matrix_t& Dm, const DerivedInputMatrix& RmInvUmUmT, matrix_t& DmDagger, - matrix_t& DmDaggerTRmDmDaggerUUT, matrix_t& RmInvConstrainedUUT) { - computeConstraintProjection(Dm, matrix_t(RmInvUmUmT), DmDagger, DmDaggerTRmDmDaggerUUT, RmInvConstrainedUUT); -} +void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, matrix_t& DmDagger, matrix_t& DmDaggerTRmDmDaggerUUT, + matrix_t& RmInvConstrainedUUT); + +/** + * Returns the linear projection + * u = Pu * \tilde{u} + Px * x + Pe + * + * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u} + * + * Implementation based on the QR decomposition + * + * @param [in] constraint : C = dfdx, D = dfdu, e = f; + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second); + */ +std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint); + +/** + * Returns the linear projection + * u = Pu * \tilde{u} + Px * x + Pe + * + * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u} + * + * Implementation based on the LU decomposition + * + * @param [in] constraint : C = dfdx, D = dfdu, e = f; + * @param [in] extractPseudoInverse : If true, left pseudo-inverse of D^T is returned. If false, an empty matrix is returned; + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second); + */ +std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, + bool extractPseudoInverse = false); /** Computes the rank of a matrix */ template <typename Derived> diff --git a/ocs2_core/src/misc/LinearAlgebra.cpp b/ocs2_core/src/misc/LinearAlgebra.cpp index 40a2fb6b0..a51a9031d 100644 --- a/ocs2_core/src/misc/LinearAlgebra.cpp +++ b/ocs2_core/src/misc/LinearAlgebra.cpp @@ -32,6 +32,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace LinearAlgebra { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue) { + for (Eigen::Index i = 0; i < Lr.rows(); ++i) { + scalar_t& eigenValue = Lr(i, i); // diagonal element is the eigenvalue + if (eigenValue < 0.0) { + eigenValue = std::min(-minEigenValue, eigenValue); + } else { + eigenValue = std::max(minEigenValue, eigenValue); + } + } +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -57,6 +71,19 @@ void makePsdEigenvalue(matrix_t& squareMatrix, scalar_t minEigenvalue) { } } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void makePsdGershgorin(matrix_t& squareMatrix, scalar_t minEigenvalue) { + assert(squareMatrix.rows() == squareMatrix.cols()); + squareMatrix = 0.5 * (squareMatrix + squareMatrix.transpose()).eval(); + for (size_t i = 0; i < squareMatrix.rows(); i++) { + // Gershgorin radius: since the matrix is symmetric we use column sum instead of row sum + auto Ri = squareMatrix.col(i).cwiseAbs().sum() - std::abs(squareMatrix(i, i)); + squareMatrix(i, i) = std::max(squareMatrix(i, i), Ri + minEigenvalue); + } +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -86,6 +113,16 @@ void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue) { A.diagonal().array() += minEigenvalue; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void computeInverseMatrixUUT(const matrix_t& Am, matrix_t& AmInvUmUmT) { + // Am = Lm Lm^T --> inv(Am) = inv(Lm^T) inv(Lm) where Lm^T is upper triangular + Eigen::LLT<matrix_t> lltOfA(Am); + AmInvUmUmT.setIdentity(Am.rows(), Am.cols()); // for dynamic size matrices + lltOfA.matrixU().solveInPlace(AmInvUmUmT); +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -117,6 +154,50 @@ void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, RmInvConstrainedUUT.noalias() = RmInvUmUmT * QRof_RmInvUmUmTT_DmT_Qu; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint) { + // Constraint Projectors are based on the QR decomposition + const auto numConstraints = constraint.dfdu.rows(); + const auto numInputs = constraint.dfdu.cols(); + const Eigen::HouseholderQR<matrix_t> QRof_DT(constraint.dfdu.transpose()); + + const matrix_t Q = QRof_DT.householderQ(); + const auto Q1 = Q.leftCols(numConstraints); + + const auto R = QRof_DT.matrixQR().topRows(numConstraints).triangularView<Eigen::Upper>(); + const matrix_t pseudoInverse = R.solve(Q1.transpose()); // left pseudo-inverse of D^T + + VectorFunctionLinearApproximation projectionTerms; + projectionTerms.dfdu = Q.rightCols(numInputs - numConstraints); + projectionTerms.dfdx.noalias() = -pseudoInverse.transpose() * constraint.dfdx; + projectionTerms.f.noalias() = -pseudoInverse.transpose() * constraint.f; + + return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, + bool extractPseudoInverse) { + // Constraint Projectors are based on the LU decomposition + const Eigen::FullPivLU<matrix_t> lu(constraint.dfdu); + + VectorFunctionLinearApproximation projectionTerms; + projectionTerms.dfdu = lu.kernel(); + projectionTerms.dfdx.noalias() = -lu.solve(constraint.dfdx); + projectionTerms.f.noalias() = -lu.solve(constraint.f); + + matrix_t pseudoInverse; + if (extractPseudoInverse) { + pseudoInverse = lu.solve(matrix_t::Identity(constraint.f.size(), constraint.f.size())).transpose(); // left pseudo-inverse of D^T + } + + return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); +} + // Explicit instantiations for dynamic sized matrices template int rank(const matrix_t& A); template Eigen::VectorXcd eigenvalues(const matrix_t& A); diff --git a/ocs2_core/test/misc/testLinearAlgebra.cpp b/ocs2_core/test/misc/testLinearAlgebra.cpp index 4aee1de9a..83057856a 100644 --- a/ocs2_core/test/misc/testLinearAlgebra.cpp +++ b/ocs2_core/test/misc/testLinearAlgebra.cpp @@ -1,86 +1,187 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <gtest/gtest.h> #include <ocs2_core/Types.h> #include <ocs2_core/misc/LinearAlgebra.h> #include <ocs2_core/misc/randomMatrices.h> -using namespace ocs2; -using namespace LinearAlgebra; +TEST(test_projection, testProjectionQR) { + constexpr ocs2::scalar_t nx = 30; + constexpr ocs2::scalar_t nu = 20; + constexpr ocs2::scalar_t nc = 10; + const auto constraint = [&]() { + ocs2::VectorFunctionLinearApproximation approx; + approx.dfdx = ocs2::matrix_t::Random(nc, nx); + approx.dfdu = ocs2::matrix_t::Random(nc, nu); + approx.f = ocs2::vector_t::Random(nc); + return approx; + }(); + + auto result = ocs2::LinearAlgebra::qrConstraintProjection(constraint); + const auto projection = std::move(result.first); + const auto pseudoInverse = std::move(result.second); + + // range of Pu is in null-space of D + ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); + + // D * Px cancels the C term + ASSERT_TRUE((constraint.dfdx + constraint.dfdu * projection.dfdx).isZero()); + + // D * Pe cancels the e term + ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); + + ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); + ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); + + ASSERT_TRUE((pseudoInverse * constraint.dfdu.transpose()).isIdentity()); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.dfdx).isApprox(-projection.dfdx)); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.f).isApprox(-projection.f)); +} + +TEST(test_projection, testProjectionLU) { + constexpr ocs2::scalar_t nx = 30; + constexpr ocs2::scalar_t nu = 20; + constexpr ocs2::scalar_t nc = 10; + const auto constraint = [&]() { + ocs2::VectorFunctionLinearApproximation approx; + approx.dfdx = ocs2::matrix_t::Random(nc, nx); + approx.dfdu = ocs2::matrix_t::Random(nc, nu); + approx.f = ocs2::vector_t::Random(nc); + return approx; + }(); + + auto result = ocs2::LinearAlgebra::luConstraintProjection(constraint); + ASSERT_EQ(result.second.rows(), 0); + ASSERT_EQ(result.second.cols(), 0); + + const auto projection = std::move(result.first); + + // range of Pu is in null-space of D + ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); + + // D * Px cancels the C term + ASSERT_TRUE((constraint.dfdx + constraint.dfdu * projection.dfdx).isZero()); + + // D * Pe cancels the e term + ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); + + auto resultWithPseudoInverse = ocs2::LinearAlgebra::luConstraintProjection(constraint, true); + const auto projectionWithPseudoInverse = std::move(resultWithPseudoInverse.first); + ASSERT_TRUE(projection.f.isApprox(projectionWithPseudoInverse.f)); + ASSERT_TRUE(projection.dfdx.isApprox(projectionWithPseudoInverse.dfdx)); + ASSERT_TRUE(projection.dfdu.isApprox(projectionWithPseudoInverse.dfdu)); + + const auto pseudoInverse = std::move(resultWithPseudoInverse.second); + ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); + ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); + + ASSERT_TRUE((pseudoInverse * constraint.dfdu.transpose()).isIdentity()); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.dfdx).isApprox(-projection.dfdx)); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.f).isApprox(-projection.f)); +} TEST(LLTofInverse, checkAgainstFullInverse) { - const size_t n = 10; // matrix size - const scalar_t tol = 1e-9; // Coefficient-wise tolerance + constexpr size_t n = 10; // matrix size + constexpr ocs2::scalar_t tol = 1e-9; // Coefficient-wise tolerance // Some random symmetric positive definite matrix - matrix_t A = generateSPDmatrix<matrix_t>(n); - matrix_t AmInvUmUmT; + ocs2::matrix_t A = ocs2::LinearAlgebra::generateSPDmatrix<ocs2::matrix_t>(n); + ocs2::matrix_t AmInvUmUmT; - computeInverseMatrixUUT(A, AmInvUmUmT); + ocs2::LinearAlgebra::computeInverseMatrixUUT(A, AmInvUmUmT); - matrix_t Ainv = A.inverse(); - matrix_t Ainv_constructed = AmInvUmUmT * AmInvUmUmT.transpose(); + ocs2::matrix_t Ainv = A.inverse(); + ocs2::matrix_t Ainv_constructed = AmInvUmUmT * AmInvUmUmT.transpose(); ASSERT_LT((Ainv - Ainv_constructed).array().abs().maxCoeff(), tol); } TEST(constraintProjection, checkAgainstFullComputations) { - const size_t m = 4; // num constraints - const size_t n = 15; // num inputs - const scalar_t tol = 1e-9; // Coefficient-wise tolerance + constexpr size_t m = 4; // num constraints + constexpr size_t n = 15; // num inputs + constexpr ocs2::scalar_t tol = 1e-9; // Coefficient-wise tolerance // Some random constraint matrix - matrix_t D = generateFullRowRankmatrix(m, n); + ocs2::matrix_t D = ocs2::LinearAlgebra::generateFullRowRankmatrix(m, n); // Random cost matrix - matrix_t R = generateSPDmatrix<matrix_t>(n); + ocs2::matrix_t R = ocs2::LinearAlgebra::generateSPDmatrix<ocs2::matrix_t>(n); // Inverse of R - matrix_t RmInvUmUmT; - computeInverseMatrixUUT(R, RmInvUmUmT); - matrix_t Rinv = RmInvUmUmT * RmInvUmUmT.transpose(); + ocs2::matrix_t RmInvUmUmT; + ocs2::LinearAlgebra::computeInverseMatrixUUT(R, RmInvUmUmT); + ocs2::matrix_t Rinv = RmInvUmUmT * RmInvUmUmT.transpose(); // Compute constraint projection terms, this is what we are testing in this unit test - matrix_t Ddagger, DdaggerT_R_Ddagger_Chol, RinvConstrainedChol; - computeConstraintProjection(D, RmInvUmUmT, Ddagger, DdaggerT_R_Ddagger_Chol, RinvConstrainedChol); + ocs2::matrix_t Ddagger, DdaggerT_R_Ddagger_Chol, RinvConstrainedChol; + ocs2::LinearAlgebra::computeConstraintProjection(D, RmInvUmUmT, Ddagger, DdaggerT_R_Ddagger_Chol, RinvConstrainedChol); // Reconstruct full matrices to compare - matrix_t RinvConstrained = RinvConstrainedChol * RinvConstrainedChol.transpose(); - matrix_t DdaggerT_R_Ddagger = DdaggerT_R_Ddagger_Chol * DdaggerT_R_Ddagger_Chol.transpose(); + ocs2::matrix_t RinvConstrained = RinvConstrainedChol * RinvConstrainedChol.transpose(); + ocs2::matrix_t DdaggerT_R_Ddagger = DdaggerT_R_Ddagger_Chol * DdaggerT_R_Ddagger_Chol.transpose(); // Alternative computation following Farshidian - An Efficient Optimal Planning and Control Framework For Quadrupedal Locomotion // Check Ddagger - matrix_t RmProjected = (D * Rinv * D.transpose()).ldlt().solve(matrix_t::Identity(m, m)); - matrix_t Ddagger_check = Rinv * D.transpose() * RmProjected; + ocs2::matrix_t RmProjected = (D * Rinv * D.transpose()).ldlt().solve(ocs2::matrix_t::Identity(m, m)); + ocs2::matrix_t Ddagger_check = Rinv * D.transpose() * RmProjected; ASSERT_LT((Ddagger - Ddagger_check).array().abs().maxCoeff(), tol); // Check Ddagger^T * R * Ddagger - matrix_t DdaggerT_R_Ddagger_check = Ddagger_check.transpose() * R * Ddagger_check; + ocs2::matrix_t DdaggerT_R_Ddagger_check = Ddagger_check.transpose() * R * Ddagger_check; ASSERT_LT((DdaggerT_R_Ddagger - DdaggerT_R_Ddagger_check).array().abs().maxCoeff(), tol); // Check Constrained cost matrix defined as defined in the paper - matrix_t nullspaceProjection = matrix_t::Identity(n, n) - Ddagger_check * D; - matrix_t RinvConstrained_check = Rinv.transpose() * nullspaceProjection.transpose() * R * nullspaceProjection * Rinv; + ocs2::matrix_t nullspaceProjection = ocs2::matrix_t::Identity(n, n) - Ddagger_check * D; + ocs2::matrix_t RinvConstrained_check = Rinv.transpose() * nullspaceProjection.transpose() * R * nullspaceProjection * Rinv; ASSERT_LT((RinvConstrained - RinvConstrained_check).array().abs().maxCoeff(), tol); } TEST(makePsdGershgorin, makePsdGershgorin) { - const size_t n = 10; // matrix size - const scalar_t tol = 1e-9; // Coefficient-wise tolerance + constexpr size_t n = 10; // matrix size + constexpr ocs2::scalar_t tol = 1e-9; // Coefficient-wise tolerance // a random, symmetric, and diagonally dominant matrix - matrix_t ddMat = generateSPDmatrix<matrix_t>(n); - matrix_t ddMatCorr = ddMat; - makePsdGershgorin(ddMatCorr); + ocs2::matrix_t ddMat = ocs2::LinearAlgebra::generateSPDmatrix<ocs2::matrix_t>(n); + ocs2::matrix_t ddMatCorr = ddMat; + ocs2::LinearAlgebra::makePsdGershgorin(ddMatCorr); ASSERT_TRUE(ddMat.isApprox(ddMatCorr, tol)); // non-definite matrix auto lambdaMin = ocs2::LinearAlgebra::symmetricEigenvalues(ddMat).minCoeff(); - matrix_t ndMat = ddMat - (lambdaMin + 1e-2) * matrix_t::Identity(n, n); - matrix_t ndMatCorr = ndMat; - const scalar_t minDesiredEigenvalue = 1e-3; - makePsdGershgorin(ndMatCorr, minDesiredEigenvalue); - vector_t lambda = ocs2::LinearAlgebra::symmetricEigenvalues(ndMat); - vector_t lambdaCorr = ocs2::LinearAlgebra::symmetricEigenvalues(ndMatCorr); + ocs2::matrix_t ndMat = ddMat - (lambdaMin + 1e-2) * ocs2::matrix_t::Identity(n, n); + ocs2::matrix_t ndMatCorr = ndMat; + const ocs2::scalar_t minDesiredEigenvalue = 1e-3; + ocs2::LinearAlgebra::makePsdGershgorin(ndMatCorr, minDesiredEigenvalue); + ocs2::vector_t lambda = ocs2::LinearAlgebra::symmetricEigenvalues(ndMat); + ocs2::vector_t lambdaCorr = ocs2::LinearAlgebra::symmetricEigenvalues(ndMatCorr); std::cerr << "MakePSD Gershgorin:" << std::endl; std::cerr << "eigenvalues " << lambda.transpose() << std::endl; std::cerr << "eigenvalues corrected: " << lambdaCorr.transpose() << std::endl; @@ -88,43 +189,43 @@ TEST(makePsdGershgorin, makePsdGershgorin) { } TEST(makePsdCholesky, makePsdCholesky) { - const size_t n = 10; // matrix size - const scalar_t tol = 1e-9; // Coefficient-wise tolerance + constexpr size_t n = 10; // matrix size + constexpr ocs2::scalar_t tol = 1e-9; // Coefficient-wise tolerance // PSD matrix check // some random symmetric matrix - matrix_t psdMat = generateSPDmatrix<matrix_t>(n); - matrix_t psdMatCorr = psdMat; - makePsdCholesky(psdMatCorr, 0.0); + ocs2::matrix_t psdMat = ocs2::LinearAlgebra::generateSPDmatrix<ocs2::matrix_t>(n); + ocs2::matrix_t psdMatCorr = psdMat; + ocs2::LinearAlgebra::makePsdCholesky(psdMatCorr, 0.0); ASSERT_TRUE(psdMat.isApprox(psdMatCorr, tol)); // non-definite matrix auto lambdaMin = ocs2::LinearAlgebra::symmetricEigenvalues(psdMat).minCoeff(); - matrix_t ndMat = psdMat - (lambdaMin + 1e-2) * matrix_t::Identity(n, n); - matrix_t ndMatCorr = ndMat; - const scalar_t minDesiredEigenvalue = 1e-1; - makePsdCholesky(ndMatCorr, minDesiredEigenvalue); - vector_t lambda = ocs2::LinearAlgebra::symmetricEigenvalues(ndMat); - vector_t lambdaCorr = ocs2::LinearAlgebra::symmetricEigenvalues(ndMatCorr); + ocs2::matrix_t ndMat = psdMat - (lambdaMin + 1e-2) * ocs2::matrix_t::Identity(n, n); + ocs2::matrix_t ndMatCorr = ndMat; + constexpr ocs2::scalar_t minDesiredEigenvalue = 1e-1; + ocs2::LinearAlgebra::makePsdCholesky(ndMatCorr, minDesiredEigenvalue); + ocs2::vector_t lambda = ocs2::LinearAlgebra::symmetricEigenvalues(ndMat); + ocs2::vector_t lambdaCorr = ocs2::LinearAlgebra::symmetricEigenvalues(ndMatCorr); std::cerr << "MakePSD Cholesky: " << std::endl; std::cerr << "eigenvalues " << lambda.transpose() << std::endl; std::cerr << "eigenvalues corrected: " << lambdaCorr.transpose() << std::endl; ASSERT_GE(lambdaCorr.minCoeff(), minDesiredEigenvalue); // zero matrix - matrix_t zeroMat = matrix_t::Zero(n, n); - makePsdCholesky(zeroMat, minDesiredEigenvalue); - vector_t lambdaZeroMat = ocs2::LinearAlgebra::symmetricEigenvalues(zeroMat); + ocs2::matrix_t zeroMat = ocs2::matrix_t::Zero(n, n); + ocs2::LinearAlgebra::makePsdCholesky(zeroMat, minDesiredEigenvalue); + ocs2::vector_t lambdaZeroMat = ocs2::LinearAlgebra::symmetricEigenvalues(zeroMat); ASSERT_GE(lambdaZeroMat.minCoeff(), minDesiredEigenvalue); // sparse matrix Eigen::VectorXd vec = Eigen::VectorXd::Random(n); - vec = vec.unaryExpr([](scalar_t x) { return std::max(x, 0.0); }); + vec = vec.unaryExpr([](ocs2::scalar_t x) { return std::max(x, 0.0); }); std::cerr << "Sparse vec:\n" << vec << std::endl; - matrix_t sparseMat = vec * vec.transpose() - minDesiredEigenvalue * matrix_t::Identity(n, n); - vector_t lambdaSparseMat = ocs2::LinearAlgebra::symmetricEigenvalues(sparseMat); - makePsdCholesky(sparseMat, minDesiredEigenvalue); - vector_t lambdaSparseMatCorr = ocs2::LinearAlgebra::symmetricEigenvalues(sparseMat); + ocs2::matrix_t sparseMat = vec * vec.transpose() - minDesiredEigenvalue * ocs2::matrix_t::Identity(n, n); + ocs2::vector_t lambdaSparseMat = ocs2::LinearAlgebra::symmetricEigenvalues(sparseMat); + ocs2::LinearAlgebra::makePsdCholesky(sparseMat, minDesiredEigenvalue); + ocs2::vector_t lambdaSparseMatCorr = ocs2::LinearAlgebra::symmetricEigenvalues(sparseMat); std::cerr << "MakePSD Cholesky Sparse Matrix: " << std::endl; std::cerr << "Sparse Matrix:\n" << sparseMat << std::endl; diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index c1f473dd4..eb7389614 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -29,11 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_sqp/MultipleShootingTranscription.h" +#include <ocs2_core/misc/LinearAlgebra.h> #include <ocs2_oc/approximate_model/ChangeOfInputVariables.h> #include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> -#include "ocs2_sqp/ConstraintProjection.h" - namespace ocs2 { namespace multiple_shooting { @@ -94,9 +93,9 @@ void projectTranscription(Transcription& transcription, bool extractEqualityCons if (stateInputEqConstraints.f.size() > 0) { // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. if (extractEqualityConstraintsPseudoInverse) { - std::tie(projection, constraintPseudoInverse) = qrConstraintProjection(stateInputEqConstraints); + std::tie(projection, constraintPseudoInverse) = LinearAlgebra::qrConstraintProjection(stateInputEqConstraints); } else { - projection = luConstraintProjection(stateInputEqConstraints).first; + projection = LinearAlgebra::luConstraintProjection(stateInputEqConstraints).first; constraintPseudoInverse = matrix_t(); } stateInputEqConstraints = VectorFunctionLinearApproximation(); From 8b4b658d374dfe4693636fd561181c3af7b86dda Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 1 Dec 2022 16:57:23 +0100 Subject: [PATCH 369/512] ProjectionMultiplierCoefficients moved to MultipleShootingHelpers --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 3 +- .../include/ocs2_sqp/ConstraintProjection.h | 88 ------------- .../ocs2_sqp/MultipleShootingHelpers.h | 24 ++++ .../ocs2_sqp/src/ConstraintProjection.cpp | 92 -------------- .../ocs2_sqp/src/MultipleShootingHelpers.cpp | 17 +++ .../test/testMultipleShootingHelpers.cpp | 62 ++++++++++ ocs2_sqp/ocs2_sqp/test/testProjection.cpp | 117 ------------------ 7 files changed, 104 insertions(+), 299 deletions(-) delete mode 100644 ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h delete mode 100644 ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp create mode 100644 ocs2_sqp/ocs2_sqp/test/testMultipleShootingHelpers.cpp delete mode 100644 ocs2_sqp/ocs2_sqp/test/testProjection.cpp diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index e28279acc..68a6a8a94 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -50,7 +50,6 @@ include_directories( # Multiple shooting solver library add_library(${PROJECT_NAME} - src/ConstraintProjection.cpp src/MultipleShootingHelpers.cpp src/MultipleShootingInitialization.cpp src/MultipleShootingSettings.cpp @@ -104,7 +103,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ catkin_add_gtest(test_${PROJECT_NAME} test/testCircularKinematics.cpp test/testDiscretization.cpp - test/testProjection.cpp + test/testMultipleShootingHelpers.cpp test/testSwitchedProblem.cpp test/testTranscriptionPerformanceIndex.cpp test/testUnconstrained.cpp diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h deleted file mode 100644 index d959762c8..000000000 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h +++ /dev/null @@ -1,88 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#pragma once - -#include <ocs2_core/Types.h> - -namespace ocs2 { - -/** - * Returns the linear projection - * u = Pu * \tilde{u} + Px * x + Pe - * - * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u} - * - * Implementation based on the QR decomposition - * - * @param constraint : C = dfdx, D = dfdu, e = f; - * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second); - */ -std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint); - -/** - * Returns the linear projection - * u = Pu * \tilde{u} + Px * x + Pe - * - * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u} - * - * Implementation based on the LU decomposition - * - * @param constraint : C = dfdx, D = dfdu, e = f; - * @param extractPseudoInverse : If true, left pseudo-inverse of D^T is returned. If false, an empty matrix is returned; - * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second); - */ -std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, - bool extractPseudoInverse = false); - -/** - * Coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint such that - * dfdx*dx + dfdu*du + dfdcostate*dcostate + f - */ -struct ProjectionMultiplierCoefficients { - matrix_t dfdx; - matrix_t dfdu; - matrix_t dfdcostate; - vector_t f; -}; - -/** - * Extracts the coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint. - * - * @param dynamics : Dynamics - * @param cost : Cost - * @param constraintProjection : Constraint projection. - * @param pseudoInverse : Left pseudo-inverse of D^T of the state-input equality constraint. - */ -ProjectionMultiplierCoefficients extractProjectionMultiplierCoefficients(const VectorFunctionLinearApproximation& dynamics, - const ScalarFunctionQuadraticApproximation& cost, - const VectorFunctionLinearApproximation& constraintProjection, - const matrix_t& pseudoInverse); - -} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h index 94a14a4c1..081f8faaf 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h @@ -114,5 +114,29 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, matrix_array_t&& KMatrices); +/** + * Coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint such that + * dfdx*dx + dfdu*du + dfdcostate*dcostate + f + */ +struct ProjectionMultiplierCoefficients { + matrix_t dfdx; + matrix_t dfdu; + matrix_t dfdcostate; + vector_t f; + + /** + * Extracts the coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint. + * + * @param dynamics : Dynamics + * @param cost : Cost + * @param constraintProjection : Constraint projection. + * @param pseudoInverse : Left pseudo-inverse of D^T of the state-input equality constraint. + */ + void extractProjectionMultiplierCoefficients(const VectorFunctionLinearApproximation& dynamics, + const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& constraintProjection, + const matrix_t& pseudoInverse); +}; + } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp deleted file mode 100644 index 7af0652aa..000000000 --- a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#include "ocs2_sqp/ConstraintProjection.h" - -namespace ocs2 { - -std::pair<VectorFunctionLinearApproximation, matrix_t> qrConstraintProjection(const VectorFunctionLinearApproximation& constraint) { - // Constraint Projectors are based on the QR decomposition - const auto numConstraints = constraint.dfdu.rows(); - const auto numInputs = constraint.dfdu.cols(); - const Eigen::HouseholderQR<matrix_t> QRof_DT(constraint.dfdu.transpose()); - - const matrix_t Q = QRof_DT.householderQ(); - const auto Q1 = Q.leftCols(numConstraints); - - const auto R = QRof_DT.matrixQR().topRows(numConstraints).triangularView<Eigen::Upper>(); - const matrix_t pseudoInverse = R.solve(Q1.transpose()); // left pseudo-inverse of D^T - - VectorFunctionLinearApproximation projectionTerms; - projectionTerms.dfdu = Q.rightCols(numInputs - numConstraints); - projectionTerms.dfdx.noalias() = -pseudoInverse.transpose() * constraint.dfdx; - projectionTerms.f.noalias() = -pseudoInverse.transpose() * constraint.f; - - return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); -} - -std::pair<VectorFunctionLinearApproximation, matrix_t> luConstraintProjection(const VectorFunctionLinearApproximation& constraint, - bool extractPseudoInverse) { - // Constraint Projectors are based on the LU decomposition - const Eigen::FullPivLU<matrix_t> lu(constraint.dfdu); - - VectorFunctionLinearApproximation projectionTerms; - projectionTerms.dfdu = lu.kernel(); - projectionTerms.dfdx.noalias() = -lu.solve(constraint.dfdx); - projectionTerms.f.noalias() = -lu.solve(constraint.f); - - matrix_t pseudoInverse; - if (extractPseudoInverse) { - pseudoInverse = lu.solve(matrix_t::Identity(constraint.f.size(), constraint.f.size())).transpose(); // left pseudo-inverse of D^T - } - - return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); -} - -ProjectionMultiplierCoefficients extractProjectionMultiplierCoefficients(const VectorFunctionLinearApproximation& dynamics, - const ScalarFunctionQuadraticApproximation& cost, - const VectorFunctionLinearApproximation& constraintProjection, - const matrix_t& pseudoInverse) { - vector_t semiprojectedCost_dfdu = cost.dfdu; - semiprojectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f; - - matrix_t semiprojectedCost_dfdux = cost.dfdux; - semiprojectedCost_dfdux.noalias() += cost.dfduu * constraintProjection.dfdx; - - const matrix_t semiprojectedCost_dfduu = cost.dfduu * constraintProjection.dfdu; - - ProjectionMultiplierCoefficients multiplierCoefficients; - multiplierCoefficients.dfdx.noalias() = -pseudoInverse * semiprojectedCost_dfdux; - multiplierCoefficients.dfdu.noalias() = -pseudoInverse * semiprojectedCost_dfduu; - multiplierCoefficients.dfdcostate.noalias() = -pseudoInverse * dynamics.dfdu.transpose(); - multiplierCoefficients.f.noalias() = -pseudoInverse * semiprojectedCost_dfdu; - return multiplierCoefficients; -} - -} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp index 2a565996a..a6c6ed9ac 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp @@ -134,5 +134,22 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche return primalSolution; } +void ProjectionMultiplierCoefficients::extractProjectionMultiplierCoefficients( + const VectorFunctionLinearApproximation& dynamics, const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse) { + vector_t semiprojectedCost_dfdu = cost.dfdu; + semiprojectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f; + + matrix_t semiprojectedCost_dfdux = cost.dfdux; + semiprojectedCost_dfdux.noalias() += cost.dfduu * constraintProjection.dfdx; + + const matrix_t semiprojectedCost_dfduu = cost.dfduu * constraintProjection.dfdu; + + this->dfdx.noalias() = -pseudoInverse * semiprojectedCost_dfdux; + this->dfdu.noalias() = -pseudoInverse * semiprojectedCost_dfduu; + this->dfdcostate.noalias() = -pseudoInverse * dynamics.dfdu.transpose(); + this->f.noalias() = -pseudoInverse * semiprojectedCost_dfdu; +} + } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/test/testMultipleShootingHelpers.cpp b/ocs2_sqp/ocs2_sqp/test/testMultipleShootingHelpers.cpp new file mode 100644 index 000000000..4e864ba30 --- /dev/null +++ b/ocs2_sqp/ocs2_sqp/test/testMultipleShootingHelpers.cpp @@ -0,0 +1,62 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <gtest/gtest.h> + +#include <ocs2_core/misc/LinearAlgebra.h> +#include <ocs2_sqp/MultipleShootingHelpers.h> + +#include <ocs2_oc/test/testProblemsGeneration.h> + +TEST(testMultipleShootingHelpers, testProjectionMultiplierCoefficients) { + constexpr size_t stateDim = 30; + constexpr size_t inputDim = 20; + constexpr size_t constraintDim = 10; + + const auto cost = ocs2::getRandomCost(stateDim, inputDim); + const auto dynamics = ocs2::getRandomDynamics(stateDim, inputDim); + const auto constraint = ocs2::getRandomConstraints(stateDim, inputDim, constraintDim); + + auto result = ocs2::LinearAlgebra::qrConstraintProjection(constraint); + const auto projection = std::move(result.first); + const auto pseudoInverse = std::move(result.second); + + ocs2::multiple_shooting::ProjectionMultiplierCoefficients projectionMultiplierCoefficients; + projectionMultiplierCoefficients.extractProjectionMultiplierCoefficients(dynamics, cost, projection, pseudoInverse); + + const ocs2::matrix_t dfdx = -pseudoInverse * (cost.dfdux + cost.dfduu * projection.dfdx); + const ocs2::matrix_t dfdu = -pseudoInverse * (cost.dfduu * projection.dfdu); + const ocs2::matrix_t dfdcostate = -pseudoInverse * dynamics.dfdu.transpose(); + const ocs2::vector_t f = -pseudoInverse * (cost.dfdu + cost.dfduu * projection.f); + + ASSERT_TRUE(projectionMultiplierCoefficients.dfdx.isApprox(dfdx)); + ASSERT_TRUE(projectionMultiplierCoefficients.dfdu.isApprox(dfdu)); + ASSERT_TRUE(projectionMultiplierCoefficients.dfdcostate.isApprox(dfdcostate)); + ASSERT_TRUE(projectionMultiplierCoefficients.f.isApprox(f)); +} diff --git a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp b/ocs2_sqp/ocs2_sqp/test/testProjection.cpp deleted file mode 100644 index 3518de974..000000000 --- a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#include <gtest/gtest.h> - -#include "ocs2_sqp/ConstraintProjection.h" - -#include <ocs2_oc/test/testProblemsGeneration.h> - -TEST(test_projection, testProjectionQR) { - const auto constraint = ocs2::getRandomConstraints(30, 20, 10); - - auto result = ocs2::qrConstraintProjection(constraint); - const auto projection = std::move(result.first); - const auto pseudoInverse = std::move(result.second); - - // range of Pu is in null-space of D - ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); - - // D * Px cancels the C term - ASSERT_TRUE((constraint.dfdx + constraint.dfdu * projection.dfdx).isZero()); - - // D * Pe cancels the e term - ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); - - ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); - ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); - - ASSERT_TRUE((pseudoInverse * constraint.dfdu.transpose()).isIdentity()); - ASSERT_TRUE((pseudoInverse.transpose() * constraint.dfdx).isApprox(-projection.dfdx)); - ASSERT_TRUE((pseudoInverse.transpose() * constraint.f).isApprox(-projection.f)); -} - -TEST(test_projection, testProjectionLU) { - const auto constraint = ocs2::getRandomConstraints(30, 20, 10); - - auto result = ocs2::luConstraintProjection(constraint); - ASSERT_EQ(result.second.rows(), 0); - ASSERT_EQ(result.second.cols(), 0); - - const auto projection = std::move(result.first); - - // range of Pu is in null-space of D - ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); - - // D * Px cancels the C term - ASSERT_TRUE((constraint.dfdx + constraint.dfdu * projection.dfdx).isZero()); - - // D * Pe cancels the e term - ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); - - auto resultWithPseudoInverse = ocs2::luConstraintProjection(constraint, true); - const auto projectionWithPseudoInverse = std::move(resultWithPseudoInverse.first); - ASSERT_TRUE(projection.f.isApprox(projectionWithPseudoInverse.f)); - ASSERT_TRUE(projection.dfdx.isApprox(projectionWithPseudoInverse.dfdx)); - ASSERT_TRUE(projection.dfdu.isApprox(projectionWithPseudoInverse.dfdu)); - - const auto pseudoInverse = std::move(resultWithPseudoInverse.second); - ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); - ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); - - ASSERT_TRUE((pseudoInverse * constraint.dfdu.transpose()).isIdentity()); - ASSERT_TRUE((pseudoInverse.transpose() * constraint.dfdx).isApprox(-projection.dfdx)); - ASSERT_TRUE((pseudoInverse.transpose() * constraint.f).isApprox(-projection.f)); -} - -TEST(test_projection, testProjectionMultiplierCoefficients) { - const size_t stateDim = 30; - const size_t inputDim = 20; - const size_t constraintDim = 10; - - const auto cost = ocs2::getRandomCost(stateDim, inputDim); - const auto dynamics = ocs2::getRandomDynamics(stateDim, inputDim); - const auto constraint = ocs2::getRandomConstraints(stateDim, inputDim, constraintDim); - - auto result = ocs2::qrConstraintProjection(constraint); - const auto projection = std::move(result.first); - const auto pseudoInverse = std::move(result.second); - - const auto projectionMultiplierCoefficients = extractProjectionMultiplierCoefficients(dynamics, cost, projection, pseudoInverse); - - const ocs2::matrix_t dfdx = -pseudoInverse * (cost.dfdux + cost.dfduu * projection.dfdx); - const ocs2::matrix_t dfdu = -pseudoInverse * (cost.dfduu * projection.dfdu); - const ocs2::matrix_t dfdcostate = -pseudoInverse * dynamics.dfdu.transpose(); - const ocs2::vector_t f = -pseudoInverse * (cost.dfdu + cost.dfduu * projection.f); - - ASSERT_TRUE(projectionMultiplierCoefficients.dfdx.isApprox(dfdx)); - ASSERT_TRUE(projectionMultiplierCoefficients.dfdu.isApprox(dfdu)); - ASSERT_TRUE(projectionMultiplierCoefficients.dfdcostate.isApprox(dfdcostate)); - ASSERT_TRUE(projectionMultiplierCoefficients.f.isApprox(f)); -} From 5a07d8940f39e8615d0d5024b523d463b217cc5c Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 1 Dec 2022 18:18:58 +0100 Subject: [PATCH 370/512] moving TimeDiscretization to ocs2_oc/oc_data --- ocs2_oc/CMakeLists.txt | 32 ++++++++++++------- .../ocs2_oc/oc_data}/TimeDiscretization.h | 0 .../src/oc_data}/TimeDiscretization.cpp | 2 +- .../test/oc_data/testTimeDiscretization.cpp | 10 +++--- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 2 -- .../ocs2_sqp/MultipleShootingHelpers.h | 2 +- .../ocs2_sqp/MultipleShootingInitialization.h | 3 +- .../include/ocs2_sqp/MultipleShootingSolver.h | 2 +- 8 files changed, 30 insertions(+), 23 deletions(-) rename {ocs2_sqp/ocs2_sqp/include/ocs2_sqp => ocs2_oc/include/ocs2_oc/oc_data}/TimeDiscretization.h (100%) rename {ocs2_sqp/ocs2_sqp/src => ocs2_oc/src/oc_data}/TimeDiscretization.cpp (99%) rename ocs2_sqp/ocs2_sqp/test/testDiscretization.cpp => ocs2_oc/test/oc_data/testTimeDiscretization.cpp (95%) diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 16674447b..a66f0679d 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -50,6 +50,7 @@ add_library(${PROJECT_NAME} src/approximate_model/ChangeOfInputVariables.cpp src/approximate_model/LinearQuadraticApproximator.cpp src/oc_data/LoopshapingPrimalSolution.cpp + src/oc_data/TimeDiscretization.cpp src/oc_problem/OptimalControlProblem.cpp src/oc_problem/LoopshapingOptimalControlProblem.cpp src/oc_problem/OptimalControlProblemHelperFunction.cpp @@ -122,42 +123,51 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_oc -catkin_add_gtest(test_time_triggered_rollout - test/rollout/testTimeTriggeredRollout.cpp +catkin_add_gtest(test_${PROJECT_NAME}_data + test/oc_data/testTimeDiscretization.cpp +) +add_dependencies(test_${PROJECT_NAME}_data + ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(test_time_triggered_rollout +target_link_libraries(test_${PROJECT_NAME}_data ${PROJECT_NAME} ${catkin_LIBRARIES} - ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(test_state_triggered_rollout - test/rollout/testStateTriggeredRollout.cpp +catkin_add_gtest(test_${PROJECT_NAME}_rollout + test/rollout/testTimeTriggeredRollout.cpp + test/rollout/testStateTriggeredRollout.cpp +) +add_dependencies(test_${PROJECT_NAME}_rollout + ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(test_state_triggered_rollout +target_link_libraries(test_${PROJECT_NAME}_rollout ${PROJECT_NAME} ${catkin_LIBRARIES} - ${Boost_LIBRARIES} gtest_main ) catkin_add_gtest(test_change_of_variables test/testChangeOfInputVariables.cpp ) +add_dependencies(test_change_of_variables + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(test_change_of_variables ${PROJECT_NAME} ${catkin_LIBRARIES} - ${Boost_LIBRARIES} gtest_main ) catkin_add_gtest(test_trajectory_spreading test/trajectory_adjustment/TrajectorySpreadingTest.cpp ) +add_dependencies(test_trajectory_spreading + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(test_trajectory_spreading ${PROJECT_NAME} ${catkin_LIBRARIES} - ${Boost_LIBRARIES} gtest_main -) +) \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h b/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h similarity index 100% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h rename to ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h diff --git a/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp b/ocs2_oc/src/oc_data/TimeDiscretization.cpp similarity index 99% rename from ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp rename to ocs2_oc/src/oc_data/TimeDiscretization.cpp index a3672c848..d4bbbb182 100644 --- a/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp +++ b/ocs2_oc/src/oc_data/TimeDiscretization.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_sqp/TimeDiscretization.h" +#include "ocs2_oc/oc_data/TimeDiscretization.h" #include <ocs2_core/misc/Lookup.h> diff --git a/ocs2_sqp/ocs2_sqp/test/testDiscretization.cpp b/ocs2_oc/test/oc_data/testTimeDiscretization.cpp similarity index 95% rename from ocs2_sqp/ocs2_sqp/test/testDiscretization.cpp rename to ocs2_oc/test/oc_data/testTimeDiscretization.cpp index 0ecdfda3d..b1b420ac1 100644 --- a/ocs2_sqp/ocs2_sqp/test/testDiscretization.cpp +++ b/ocs2_oc/test/oc_data/testTimeDiscretization.cpp @@ -29,11 +29,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include "ocs2_sqp/TimeDiscretization.h" +#include "ocs2_oc/oc_data/TimeDiscretization.h" using namespace ocs2; -TEST(test_discretization, noEvents_plusEps) { +TEST(test_time_discretization, noEvents_plusEps) { scalar_t initTime = 0.1; scalar_t finalTime = 0.3 + std::numeric_limits<scalar_t>::epsilon(); scalar_t dt = 0.1; @@ -51,7 +51,7 @@ TEST(test_discretization, noEvents_plusEps) { ASSERT_EQ(time.size(), 3); } -TEST(test_discretization, noEvents_minEps) { +TEST(test_time_discretization, noEvents_minEps) { scalar_t initTime = 0.1; scalar_t finalTime = 0.3 - std::numeric_limits<scalar_t>::epsilon(); scalar_t dt = 0.1; @@ -69,7 +69,7 @@ TEST(test_discretization, noEvents_minEps) { ASSERT_EQ(time.size(), 3); } -TEST(test_discretization, eventAtBeginning) { +TEST(test_time_discretization, eventAtBeginning) { scalar_t initTime = 0.1; scalar_t finalTime = 0.3; scalar_t dt = 0.1; @@ -87,7 +87,7 @@ TEST(test_discretization, eventAtBeginning) { ASSERT_EQ(time.size(), 3); } -TEST(test_discretization, withEvents) { +TEST(test_time_discretization, withEvents) { scalar_t initTime = 3.0; scalar_t finalTime = 4.0; scalar_t dt = 0.1; diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 68a6a8a94..943f01a38 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -57,7 +57,6 @@ add_library(${PROJECT_NAME} src/MultipleShootingSolverStatus.cpp src/MultipleShootingTranscription.cpp src/PerformanceIndexComputation.cpp - src/TimeDiscretization.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} @@ -102,7 +101,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ catkin_add_gtest(test_${PROJECT_NAME} test/testCircularKinematics.cpp - test/testDiscretization.cpp test/testMultipleShootingHelpers.cpp test/testSwitchedProblem.cpp test/testTranscriptionPerformanceIndex.cpp diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h index 081f8faaf..4c598ff76 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h @@ -32,9 +32,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/oc_data/PrimalSolution.h> +#include <ocs2_oc/oc_data/TimeDiscretization.h> #include "ocs2_sqp/MultipleShootingSolverStatus.h" -#include "ocs2_sqp/TimeDiscretization.h" namespace ocs2 { namespace multiple_shooting { diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h index 4f58e3ee6..bd2cf1945 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h @@ -33,8 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/initialization/Initializer.h> #include <ocs2_core/misc/LinearInterpolation.h> #include <ocs2_oc/oc_data/PrimalSolution.h> - -#include "ocs2_sqp/TimeDiscretization.h" +#include <ocs2_oc/oc_data/TimeDiscretization.h> namespace ocs2 { namespace multiple_shooting { diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index 7a662673e..293e63147 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/misc/Benchmark.h> #include <ocs2_core/thread_support/ThreadPool.h> +#include <ocs2_oc/oc_data/TimeDiscretization.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> #include <ocs2_oc/oc_solver/SolverBase.h> #include <ocs2_oc/search_strategy/FilterLinesearch.h> @@ -42,7 +43,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_sqp/MultipleShootingSettings.h" #include "ocs2_sqp/MultipleShootingSolverStatus.h" -#include "ocs2_sqp/TimeDiscretization.h" namespace ocs2 { From d14849bfa55c24cae1f84ce9220d63fa8cd5a048 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 1 Dec 2022 19:35:10 +0100 Subject: [PATCH 371/512] moving MultipleShooting code to ocs2_oc/multiple_shooting and changing multiple_shooting namespace to sqp --- ocs2_oc/CMakeLists.txt | 15 +++++ .../ocs2_oc/multiple_shooting/Helpers.h | 7 +- .../multiple_shooting/Initialization.h | 5 +- .../ocs2_oc/multiple_shooting/Transcription.h | 3 +- .../src/multiple_shooting/Helpers.cpp | 2 +- .../src/multiple_shooting/Initialization.cpp | 2 +- .../src/multiple_shooting/Transcription.cpp | 7 +- .../test/multiple_shooting/testHelpers.cpp | 24 ++++--- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 12 ++-- .../{MultipleShootingMpc.h => SqpMpc.h} | 22 +++--- ...ion.h => SqpPerformanceIndexComputation.h} | 2 +- ...ltipleShootingSettings.h => SqpSettings.h} | 4 +- .../{MultipleShootingSolver.h => SqpSolver.h} | 38 +++++------ ...ootingSolverStatus.h => SqpSolverStatus.h} | 4 +- ...cpp => SqpPerformanceIndexComputation.cpp} | 2 +- ...leShootingSettings.cpp => SqpSettings.cpp} | 6 +- ...ltipleShootingSolver.cpp => SqpSolver.cpp} | 67 +++++++++---------- ...ngSolverStatus.cpp => SqpSolverStatus.cpp} | 6 +- .../ocs2_sqp/test/testCircularKinematics.cpp | 10 +-- .../ocs2_sqp/test/testSwitchedProblem.cpp | 10 +-- .../testTranscriptionPerformanceIndex.cpp | 18 ++--- ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp | 6 +- ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp | 8 +-- 23 files changed, 144 insertions(+), 136 deletions(-) rename ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h => ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h (97%) rename ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h => ocs2_oc/include/ocs2_oc/multiple_shooting/Initialization.h (97%) rename ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h => ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h (98%) rename ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp => ocs2_oc/src/multiple_shooting/Helpers.cpp (99%) rename ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp => ocs2_oc/src/multiple_shooting/Initialization.cpp (98%) rename ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp => ocs2_oc/src/multiple_shooting/Transcription.cpp (97%) rename ocs2_sqp/ocs2_sqp/test/testMultipleShootingHelpers.cpp => ocs2_oc/test/multiple_shooting/testHelpers.cpp (74%) rename ocs2_sqp/ocs2_sqp/include/ocs2_sqp/{MultipleShootingMpc.h => SqpMpc.h} (79%) rename ocs2_sqp/ocs2_sqp/include/ocs2_sqp/{PerformanceIndexComputation.h => SqpPerformanceIndexComputation.h} (98%) rename ocs2_sqp/ocs2_sqp/include/ocs2_sqp/{MultipleShootingSettings.h => SqpSettings.h} (98%) rename ocs2_sqp/ocs2_sqp/include/ocs2_sqp/{MultipleShootingSolver.h => SqpSolver.h} (81%) rename ocs2_sqp/ocs2_sqp/include/ocs2_sqp/{MultipleShootingSolverStatus.h => SqpSolverStatus.h} (97%) rename ocs2_sqp/ocs2_sqp/src/{PerformanceIndexComputation.cpp => SqpPerformanceIndexComputation.cpp} (99%) rename ocs2_sqp/ocs2_sqp/src/{MultipleShootingSettings.cpp => SqpSettings.cpp} (97%) rename ocs2_sqp/ocs2_sqp/src/{MultipleShootingSolver.cpp => SqpSolver.cpp} (86%) rename ocs2_sqp/ocs2_sqp/src/{MultipleShootingSolverStatus.cpp => SqpSolverStatus.cpp} (95%) diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index a66f0679d..40a1df25b 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -49,6 +49,9 @@ include_directories( add_library(${PROJECT_NAME} src/approximate_model/ChangeOfInputVariables.cpp src/approximate_model/LinearQuadraticApproximator.cpp + src/multiple_shooting/Helpers.cpp + src/multiple_shooting/Initialization.cpp + src/multiple_shooting/Transcription.cpp src/oc_data/LoopshapingPrimalSolution.cpp src/oc_data/TimeDiscretization.cpp src/oc_problem/OptimalControlProblem.cpp @@ -123,6 +126,18 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_oc +catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting + test/multiple_shooting/testHelpers.cpp +) +add_dependencies(test_${PROJECT_NAME}_multiple_shooting + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(test_${PROJECT_NAME}_multiple_shooting + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main +) + catkin_add_gtest(test_${PROJECT_NAME}_data test/oc_data/testTimeDiscretization.cpp ) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h similarity index 97% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h rename to ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h index 4c598ff76..510f40b9e 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingHelpers.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h @@ -30,11 +30,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include <ocs2_core/Types.h> -#include <ocs2_oc/oc_data/PerformanceIndex.h> -#include <ocs2_oc/oc_data/PrimalSolution.h> -#include <ocs2_oc/oc_data/TimeDiscretization.h> -#include "ocs2_sqp/MultipleShootingSolverStatus.h" +#include "ocs2_oc/oc_data/PerformanceIndex.h" +#include "ocs2_oc/oc_data/PrimalSolution.h" +#include "ocs2_oc/oc_data/TimeDiscretization.h" namespace ocs2 { namespace multiple_shooting { diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Initialization.h similarity index 97% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h rename to ocs2_oc/include/ocs2_oc/multiple_shooting/Initialization.h index bd2cf1945..c13464e84 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Initialization.h @@ -32,8 +32,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/initialization/Initializer.h> #include <ocs2_core/misc/LinearInterpolation.h> -#include <ocs2_oc/oc_data/PrimalSolution.h> -#include <ocs2_oc/oc_data/TimeDiscretization.h> + +#include "ocs2_oc/oc_data/PrimalSolution.h" +#include "ocs2_oc/oc_data/TimeDiscretization.h" namespace ocs2 { namespace multiple_shooting { diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h similarity index 98% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h rename to ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h index 1e7f9f0c8..2527d0965 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h @@ -31,7 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/integration/SensitivityIntegrator.h> -#include <ocs2_oc/oc_problem/OptimalControlProblem.h> + +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" namespace ocs2 { namespace multiple_shooting { diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp b/ocs2_oc/src/multiple_shooting/Helpers.cpp similarity index 99% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp rename to ocs2_oc/src/multiple_shooting/Helpers.cpp index a6c6ed9ac..52d8f3e8f 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingHelpers.cpp +++ b/ocs2_oc/src/multiple_shooting/Helpers.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_sqp/MultipleShootingHelpers.h" +#include "ocs2_oc/multiple_shooting/Helpers.h" #include <ocs2_core/control/FeedforwardController.h> #include <ocs2_core/control/LinearController.h> diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp b/ocs2_oc/src/multiple_shooting/Initialization.cpp similarity index 98% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp rename to ocs2_oc/src/multiple_shooting/Initialization.cpp index 3bdb9eb53..436fe2fd0 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp +++ b/ocs2_oc/src/multiple_shooting/Initialization.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_sqp/MultipleShootingInitialization.h" +#include "ocs2_oc/multiple_shooting/Initialization.h" namespace ocs2 { namespace multiple_shooting { diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp similarity index 97% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp rename to ocs2_oc/src/multiple_shooting/Transcription.cpp index eb7389614..5fd28f022 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -27,11 +27,12 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_sqp/MultipleShootingTranscription.h" +#include "ocs2_oc/multiple_shooting/Transcription.h" #include <ocs2_core/misc/LinearAlgebra.h> -#include <ocs2_oc/approximate_model/ChangeOfInputVariables.h> -#include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> + +#include "ocs2_oc/approximate_model/ChangeOfInputVariables.h" +#include "ocs2_oc/approximate_model/LinearQuadraticApproximator.h" namespace ocs2 { namespace multiple_shooting { diff --git a/ocs2_sqp/ocs2_sqp/test/testMultipleShootingHelpers.cpp b/ocs2_oc/test/multiple_shooting/testHelpers.cpp similarity index 74% rename from ocs2_sqp/ocs2_sqp/test/testMultipleShootingHelpers.cpp rename to ocs2_oc/test/multiple_shooting/testHelpers.cpp index 4e864ba30..823382401 100644 --- a/ocs2_sqp/ocs2_sqp/test/testMultipleShootingHelpers.cpp +++ b/ocs2_oc/test/multiple_shooting/testHelpers.cpp @@ -30,30 +30,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> #include <ocs2_core/misc/LinearAlgebra.h> -#include <ocs2_sqp/MultipleShootingHelpers.h> +#include <ocs2_oc/multiple_shooting/Helpers.h> -#include <ocs2_oc/test/testProblemsGeneration.h> +#include "ocs2_oc/test/testProblemsGeneration.h" + +using namespace ocs2; TEST(testMultipleShootingHelpers, testProjectionMultiplierCoefficients) { constexpr size_t stateDim = 30; constexpr size_t inputDim = 20; constexpr size_t constraintDim = 10; - const auto cost = ocs2::getRandomCost(stateDim, inputDim); - const auto dynamics = ocs2::getRandomDynamics(stateDim, inputDim); - const auto constraint = ocs2::getRandomConstraints(stateDim, inputDim, constraintDim); + const auto cost = getRandomCost(stateDim, inputDim); + const auto dynamics = getRandomDynamics(stateDim, inputDim); + const auto constraint = getRandomConstraints(stateDim, inputDim, constraintDim); - auto result = ocs2::LinearAlgebra::qrConstraintProjection(constraint); + auto result = LinearAlgebra::qrConstraintProjection(constraint); const auto projection = std::move(result.first); const auto pseudoInverse = std::move(result.second); - ocs2::multiple_shooting::ProjectionMultiplierCoefficients projectionMultiplierCoefficients; + multiple_shooting::ProjectionMultiplierCoefficients projectionMultiplierCoefficients; projectionMultiplierCoefficients.extractProjectionMultiplierCoefficients(dynamics, cost, projection, pseudoInverse); - const ocs2::matrix_t dfdx = -pseudoInverse * (cost.dfdux + cost.dfduu * projection.dfdx); - const ocs2::matrix_t dfdu = -pseudoInverse * (cost.dfduu * projection.dfdu); - const ocs2::matrix_t dfdcostate = -pseudoInverse * dynamics.dfdu.transpose(); - const ocs2::vector_t f = -pseudoInverse * (cost.dfdu + cost.dfduu * projection.f); + const matrix_t dfdx = -pseudoInverse * (cost.dfdux + cost.dfduu * projection.dfdx); + const matrix_t dfdu = -pseudoInverse * (cost.dfduu * projection.dfdu); + const matrix_t dfdcostate = -pseudoInverse * dynamics.dfdu.transpose(); + const vector_t f = -pseudoInverse * (cost.dfdu + cost.dfduu * projection.f); ASSERT_TRUE(projectionMultiplierCoefficients.dfdx.isApprox(dfdx)); ASSERT_TRUE(projectionMultiplierCoefficients.dfdu.isApprox(dfdu)); diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 943f01a38..3f8781223 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -50,13 +50,10 @@ include_directories( # Multiple shooting solver library add_library(${PROJECT_NAME} - src/MultipleShootingHelpers.cpp - src/MultipleShootingInitialization.cpp - src/MultipleShootingSettings.cpp - src/MultipleShootingSolver.cpp - src/MultipleShootingSolverStatus.cpp - src/MultipleShootingTranscription.cpp - src/PerformanceIndexComputation.cpp + src/SqpSettings.cpp + src/SqpSolver.cpp + src/SqpSolverStatus.cpp + src/SqpPerformanceIndexComputation.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} @@ -101,7 +98,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ catkin_add_gtest(test_${PROJECT_NAME} test/testCircularKinematics.cpp - test/testMultipleShootingHelpers.cpp test/testSwitchedProblem.cpp test/testTranscriptionPerformanceIndex.cpp test/testUnconstrained.cpp diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingMpc.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h similarity index 79% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingMpc.h rename to ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h index 863786bec..25f40967b 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingMpc.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h @@ -31,27 +31,28 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_mpc/MPC_BASE.h> -#include <ocs2_sqp/MultipleShootingSolver.h> +#include "ocs2_sqp/SqpSolver.h" namespace ocs2 { -class MultipleShootingMpc final : public MPC_BASE { + +class SqpMpc final : public MPC_BASE { public: /** * Constructor * - * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use multiple shooting - * settings directly. - * @param settings : settings for the multiple shooting solver. + * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use + * multiple shooting SQP settings directly. + * @param settings : settings for the multiple shooting SQP solver. * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ - MultipleShootingMpc(mpc::Settings mpcSettings, multiple_shooting::Settings settings, const OptimalControlProblem& optimalControlProblem, - const Initializer& initializer) + SqpMpc(mpc::Settings mpcSettings, sqp::Settings settings, const OptimalControlProblem& optimalControlProblem, + const Initializer& initializer) : MPC_BASE(std::move(mpcSettings)) { - solverPtr_.reset(new MultipleShootingSolver(std::move(settings), optimalControlProblem, initializer)); + solverPtr_.reset(new SqpSolver(std::move(settings), optimalControlProblem, initializer)); }; - ~MultipleShootingMpc() override = default; + ~SqpMpc() override = default; MultipleShootingSolver* getSolverPtr() override { return solverPtr_.get(); } const MultipleShootingSolver* getSolverPtr() const override { return solverPtr_.get(); } @@ -65,6 +66,7 @@ class MultipleShootingMpc final : public MPC_BASE { } private: - std::unique_ptr<MultipleShootingSolver> solverPtr_; + std::unique_ptr<SqpSolver> solverPtr_; }; + } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpPerformanceIndexComputation.h similarity index 98% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h rename to ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpPerformanceIndexComputation.h index d7671c058..02d3f6642 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/PerformanceIndexComputation.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpPerformanceIndexComputation.h @@ -31,9 +31,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/integration/SensitivityIntegrator.h> +#include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> -#include "ocs2_sqp/MultipleShootingTranscription.h" namespace ocs2 { namespace sqp { diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSettings.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h similarity index 98% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSettings.h rename to ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h index 76ce32b44..4a142a2f7 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSettings.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h @@ -35,7 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <hpipm_catkin/HpipmInterfaceSettings.h> namespace ocs2 { -namespace multiple_shooting { +namespace sqp { struct Settings { // Sqp settings @@ -89,5 +89,5 @@ struct Settings { */ Settings loadSettings(const std::string& filename, const std::string& fieldName = "multiple_shooting", bool verbose = true); -} // namespace multiple_shooting +} // namespace sqp } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h similarity index 81% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h rename to ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index 293e63147..432ccc8f3 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -41,25 +41,23 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <hpipm_catkin/HpipmInterface.h> -#include "ocs2_sqp/MultipleShootingSettings.h" -#include "ocs2_sqp/MultipleShootingSolverStatus.h" +#include "ocs2_sqp/SqpSettings.h" +#include "ocs2_sqp/SqpSolverStatus.h" namespace ocs2 { -class MultipleShootingSolver : public SolverBase { +class SqpSolver : public SolverBase { public: - using Settings = multiple_shooting::Settings; - /** * Constructor * - * @param settings : settings for the multiple shooting solver. + * @param settings : settings for the multiple shooting SQP solver. * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ - MultipleShootingSolver(Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); + SqpSolver(sqp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); - ~MultipleShootingSolver() override; + ~SqpSolver() override; void reset() override; @@ -67,12 +65,10 @@ class MultipleShootingSolver : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } - const DualSolution& getDualSolution() const override { - throw std::runtime_error("[MultipleShootingSolver] getDualSolution() not available yet."); - } + const DualSolution& getDualSolution() const override { throw std::runtime_error("[SqpSolver] getDualSolution() not available yet."); } const ProblemMetrics& getSolutionMetrics() const override { - throw std::runtime_error("[MultipleShootingSolver] getSolutionMetrics() not available yet."); + throw std::runtime_error("[SqpSolver] getSolutionMetrics() not available yet."); } size_t getNumIterations() const override { return totalNumIterations_; } @@ -86,15 +82,15 @@ class MultipleShootingSolver : public SolverBase { ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const override; ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) override { - throw std::runtime_error("[MultipleShootingSolver] getHamiltonian() not available yet."); + throw std::runtime_error("[SqpSolver] getHamiltonian() not available yet."); } vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override { - throw std::runtime_error("[MultipleShootingSolver] getStateInputEqualityConstraintLagrangian() not available yet."); + throw std::runtime_error("[SqpSolver] getStateInputEqualityConstraintLagrangian() not available yet."); } MultiplierCollection getIntermediateDualSolution(scalar_t time) const override { - throw std::runtime_error("[MultipleShootingSolver] getIntermediateDualSolution() not available yet."); + throw std::runtime_error("[SqpSolver] getIntermediateDualSolution() not available yet."); } private: @@ -104,7 +100,7 @@ class MultipleShootingSolver : public SolverBase { if (externalControllerPtr == nullptr) { runImpl(initTime, initState, finalTime); } else { - throw std::runtime_error("[MultipleShootingSolver::run] This solver does not support external controller!"); + throw std::runtime_error("[SqpSolver::run] This solver does not support external controller!"); } } @@ -147,16 +143,14 @@ class MultipleShootingSolver : public SolverBase { PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ - multiple_shooting::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, - const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, - vector_array_t& u); + sqp::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, + const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u); /** Determine convergence after a step */ - multiple_shooting::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, - const multiple_shooting::StepInfo& stepInfo) const; + sqp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const; // Problem definition - Settings settings_; + sqp::Settings settings_; DynamicsDiscretizer discretizer_; DynamicsSensitivityDiscretizer sensitivityDiscretizer_; std::vector<OptimalControlProblem> ocpDefinitions_; diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h similarity index 97% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h rename to ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h index b441e61d2..4da41d2ea 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h @@ -34,7 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/search_strategy/FilterLinesearch.h> namespace ocs2 { -namespace multiple_shooting { +namespace sqp { /** Struct to contain the result and logging data of the stepsize computation */ struct StepInfo { @@ -56,5 +56,5 @@ enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; std::string toString(const Convergence& convergence); -} // namespace multiple_shooting +} // namespace sqp } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp b/ocs2_sqp/ocs2_sqp/src/SqpPerformanceIndexComputation.cpp similarity index 99% rename from ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp rename to ocs2_sqp/ocs2_sqp/src/SqpPerformanceIndexComputation.cpp index fda5f6c3c..07152787b 100644 --- a/ocs2_sqp/ocs2_sqp/src/PerformanceIndexComputation.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpPerformanceIndexComputation.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_sqp/PerformanceIndexComputation.h" +#include "ocs2_sqp/SqpPerformanceIndexComputation.h" #include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSettings.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp similarity index 97% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingSettings.cpp rename to ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp index d13880de6..ae56f3fa7 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSettings.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_sqp/MultipleShootingSettings.h" +#include "ocs2_sqp/SqpSettings.h" #include <boost/property_tree/info_parser.hpp> #include <boost/property_tree/ptree.hpp> @@ -35,7 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/misc/LoadData.h> namespace ocs2 { -namespace multiple_shooting { +namespace sqp { Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { boost::property_tree::ptree pt; @@ -79,5 +79,5 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, return settings; } -} // namespace multiple_shooting +} // namespace sqp } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp similarity index 86% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp rename to ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index c0b293e9a..bd61a6c59 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -27,22 +27,21 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_sqp/MultipleShootingSolver.h" +#include "ocs2_sqp/SqpSolver.h" #include <iostream> #include <numeric> #include <ocs2_core/penalties/penalties/RelaxedBarrierPenalty.h> +#include <ocs2_oc/multiple_shooting/Helpers.h> +#include <ocs2_oc/multiple_shooting/Initialization.h> +#include <ocs2_oc/multiple_shooting/Transcription.h> -#include "ocs2_sqp/MultipleShootingHelpers.h" -#include "ocs2_sqp/MultipleShootingInitialization.h" -#include "ocs2_sqp/MultipleShootingTranscription.h" -#include "ocs2_sqp/PerformanceIndexComputation.h" +#include "ocs2_sqp/SqpPerformanceIndexComputation.h" namespace ocs2 { -MultipleShootingSolver::MultipleShootingSolver(Settings settings, const OptimalControlProblem& optimalControlProblem, - const Initializer& initializer) +SqpSolver::SqpSolver(sqp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) : SolverBase(), settings_(std::move(settings)), hpipmInterface_(hpipm_interface::OcpSize(), settings.hpipmSettings), @@ -73,13 +72,13 @@ MultipleShootingSolver::MultipleShootingSolver(Settings settings, const OptimalC filterLinesearch_.armijoFactor = settings_.armijoFactor; } -MultipleShootingSolver::~MultipleShootingSolver() { +SqpSolver::~SqpSolver() { if (settings_.printSolverStatistics) { std::cerr << getBenchmarkingInformation() << std::endl; } } -void MultipleShootingSolver::reset() { +void SqpSolver::reset() { // Clear solution primalSolution_ = PrimalSolution(); valueFunction_.clear(); @@ -94,7 +93,7 @@ void MultipleShootingSolver::reset() { computeControllerTimer_.reset(); } -std::string MultipleShootingSolver::getBenchmarkingInformation() const { +std::string SqpSolver::getBenchmarkingInformation() const { const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds(); @@ -120,17 +119,17 @@ std::string MultipleShootingSolver::getBenchmarkingInformation() const { return infoStream.str(); } -const std::vector<PerformanceIndex>& MultipleShootingSolver::getIterationsLog() const { +const std::vector<PerformanceIndex>& SqpSolver::getIterationsLog() const { if (performanceIndeces_.empty()) { - throw std::runtime_error("[MultipleShootingSolver]: No performance log yet, no problem solved yet?"); + throw std::runtime_error("[SqpSolver]: No performance log yet, no problem solved yet?"); } else { return performanceIndeces_; } } -ScalarFunctionQuadraticApproximation MultipleShootingSolver::getValueFunction(scalar_t time, const vector_t& state) const { +ScalarFunctionQuadraticApproximation SqpSolver::getValueFunction(scalar_t time, const vector_t& state) const { if (valueFunction_.empty()) { - throw std::runtime_error("[MultipleShootingSolver] Value function is empty! Is createValueFunction true and did the solver run?"); + throw std::runtime_error("[SqpSolver] Value function is empty! Is createValueFunction true and did the solver run?"); } else { // Interpolation const auto indexAlpha = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_); @@ -149,7 +148,7 @@ ScalarFunctionQuadraticApproximation MultipleShootingSolver::getValueFunction(sc } } -void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { +void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; std::cerr << "\n+++++++++++++ SQP solver is initialized ++++++++++++++"; @@ -174,8 +173,8 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat performanceIndeces_.clear(); int iter = 0; - multiple_shooting::Convergence convergence = multiple_shooting::Convergence::FALSE; - while (convergence == multiple_shooting::Convergence::FALSE) { + sqp::Convergence convergence = sqp::Convergence::FALSE; + while (convergence == sqp::Convergence::FALSE) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\nSQP iteration: " << iter << "\n"; } @@ -219,11 +218,11 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat } } -void MultipleShootingSolver::runParallel(std::function<void(int)> taskFunction) { +void SqpSolver::runParallel(std::function<void(int)> taskFunction) { threadPool_.runParallel(std::move(taskFunction), settings_.nThreads); } -MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolution(const vector_t& delta_x0) { +SqpSolver::OcpSubproblemSolution SqpSolver::getOCPSolution(const vector_t& delta_x0) { // Solve the QP OcpSubproblemSolution solution; auto& deltaXSol = solution.deltaXSol; @@ -240,7 +239,7 @@ MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolu } if (status != hpipm_status::SUCCESS) { - throw std::runtime_error("[MultipleShootingSolver] Failed to solve QP"); + throw std::runtime_error("[SqpSolver] Failed to solve QP"); } // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] @@ -254,7 +253,7 @@ MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolu return solution; } -void MultipleShootingSolver::extractValueFunction(const std::vector<AnnotatedTime>& time, const vector_array_t& x) { +void SqpSolver::extractValueFunction(const std::vector<AnnotatedTime>& time, const vector_array_t& x) { if (settings_.createValueFunction) { valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], cost_[0]); // Correct for linearization state @@ -264,7 +263,7 @@ void MultipleShootingSolver::extractValueFunction(const std::vector<AnnotatedTim } } -PrimalSolution MultipleShootingSolver::toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { +PrimalSolution SqpSolver::toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { if (settings_.useFeedbackPolicy) { ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); matrix_array_t KMatrices = hpipmInterface_.getRiccatiFeedback(dynamics_[0], cost_[0]); @@ -279,8 +278,8 @@ PrimalSolution MultipleShootingSolver::toPrimalSolution(const std::vector<Annota } } -PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u) { +PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, + const vector_array_t& x, const vector_array_t& u) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -353,8 +352,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec return totalPerformance; } -PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u) { +PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -399,10 +398,9 @@ PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector<An return totalPerformance; } -multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIndex& baseline, - const std::vector<AnnotatedTime>& timeDiscretization, - const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, - vector_array_t& x, vector_array_t& u) { +sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u) { using StepType = FilterLinesearch::StepType; /* @@ -454,7 +452,7 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn u = std::move(uNew); // Prepare step info - multiple_shooting::StepInfo stepInfo; + sqp::StepInfo stepInfo; stepInfo.stepSize = alpha; stepInfo.stepType = stepType; stepInfo.dx_norm = alpha * deltaXnorm; @@ -478,7 +476,7 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn } while (alpha >= settings_.alpha_min); // Alpha_min reached -> Don't take a step - multiple_shooting::StepInfo stepInfo; + sqp::StepInfo stepInfo; stepInfo.stepSize = 0.0; stepInfo.stepType = StepType::ZERO; stepInfo.dx_norm = 0.0; @@ -493,9 +491,8 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn return stepInfo; } -multiple_shooting::Convergence MultipleShootingSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, - const multiple_shooting::StepInfo& stepInfo) const { - using Convergence = multiple_shooting::Convergence; +sqp::Convergence SqpSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const { + using Convergence = sqp::Convergence; if ((iteration + 1) >= settings_.sqpIteration) { // Converged because the next iteration would exceed the specified number of iterations return Convergence::ITERATIONS; diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolverStatus.cpp similarity index 95% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp rename to ocs2_sqp/ocs2_sqp/src/SqpSolverStatus.cpp index 4708de46b..0c2e4ebe7 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolverStatus.cpp @@ -27,10 +27,10 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_sqp/MultipleShootingSolverStatus.h" +#include "ocs2_sqp/SqpSolverStatus.h" namespace ocs2 { -namespace multiple_shooting { +namespace sqp { std::string toString(const Convergence& convergence) { switch (convergence) { @@ -48,5 +48,5 @@ std::string toString(const Convergence& convergence) { } } -} // namespace multiple_shooting +} // namespace sqp } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp b/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp index b3807fb6a..83024150f 100644 --- a/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include "ocs2_sqp/MultipleShootingSolver.h" +#include "ocs2_sqp/SqpSolver.h" #include <ocs2_core/initialization/DefaultInitializer.h> @@ -43,7 +43,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { ocs2::DefaultInitializer zeroInitializer(2); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.01; settings.sqpIteration = 20; settings.projectStateInputEqualityConstraints = true; @@ -59,7 +59,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 // Solve - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); // Inspect solution @@ -97,7 +97,7 @@ TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { ocs2::DefaultInitializer zeroInitializer(2); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.01; settings.sqpIteration = 20; settings.projectStateInputEqualityConstraints = false; // <- false to turn off projection of state-input equalities @@ -112,7 +112,7 @@ TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 // Solve - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); // Inspect solution diff --git a/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp b/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp index 5ad47d6cd..7463d6f2c 100644 --- a/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp @@ -29,15 +29,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include "ocs2_sqp/MultipleShootingSolver.h" -#include "ocs2_sqp/TimeDiscretization.h" +#include "ocs2_sqp/SqpSolver.h" #include <ocs2_core/constraint/LinearStateInputConstraint.h> #include <ocs2_core/constraint/StateInputConstraint.h> #include <ocs2_core/initialization/DefaultInitializer.h> #include <ocs2_core/misc/LinearInterpolation.h> - +#include <ocs2_oc/oc_data/TimeDiscretization.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> + #include <ocs2_oc/test/testProblemsGeneration.h> namespace ocs2 { @@ -127,7 +127,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithEventTime(scal ocs2::DefaultInitializer zeroInitializer(m); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.05; settings.sqpIteration = 20; settings.projectStateInputEqualityConstraints = true; @@ -141,7 +141,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithEventTime(scal const ocs2::vector_t initState = ocs2::vector_t::Random(n); // Set up solver - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); // Solve diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp index 25067fdb2..8ec7c909a 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp @@ -29,12 +29,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include "ocs2_sqp/MultipleShootingTranscription.h" -#include "ocs2_sqp/PerformanceIndexComputation.h" - +#include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/test/circular_kinematics.h> #include <ocs2_oc/test/testProblemsGeneration.h> +#include "ocs2_sqp/SqpPerformanceIndexComputation.h" + namespace { /** Helper to compare if two performance indices are identical */ bool areIdentical(const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) { @@ -69,9 +69,9 @@ TEST(test_transcription, intermediate_performance) { const vector_t u = (vector_t(nu) << 0.1, 1.3).finished(); const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, t, dt, x, x_next, u); - const auto performance = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); + const auto performance = sqp::computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); - ASSERT_TRUE(areIdentical(performance, computeIntermediatePerformance(transcription, dt))); + ASSERT_TRUE(areIdentical(performance, sqp::computeIntermediatePerformance(transcription, dt))); } TEST(test_transcription, terminal_performance) { @@ -93,9 +93,9 @@ TEST(test_transcription, terminal_performance) { scalar_t t = 0.5; const vector_t x = vector_t::Random(nx); const auto transcription = setupTerminalNode(problem, t, x); - const auto performance = computeTerminalPerformance(problem, t, x); + const auto performance = sqp::computeTerminalPerformance(problem, t, x); - ASSERT_TRUE(areIdentical(performance, computeTerminalPerformance(transcription))); + ASSERT_TRUE(areIdentical(performance, sqp::computeTerminalPerformance(transcription))); } TEST(test_transcription, event_performance) { @@ -122,7 +122,7 @@ TEST(test_transcription, event_performance) { const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); const auto transcription = setupEventNode(problem, t, x, x_next); - const auto performance = computeEventPerformance(problem, t, x, x_next); + const auto performance = sqp::computeEventPerformance(problem, t, x, x_next); - ASSERT_TRUE(areIdentical(performance, computeEventPerformance(transcription))); + ASSERT_TRUE(areIdentical(performance, sqp::computeEventPerformance(transcription))); } diff --git a/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp b/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp index c68ffe0d9..81a730f36 100644 --- a/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include "ocs2_sqp/MultipleShootingSolver.h" +#include "ocs2_sqp/SqpSolver.h" #include <ocs2_core/initialization/DefaultInitializer.h> @@ -67,7 +67,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithFeedbackSettin ocs2::DefaultInitializer zeroInitializer(m); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.05; settings.sqpIteration = 10; settings.projectStateInputEqualityConstraints = true; @@ -83,7 +83,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithFeedbackSettin const ocs2::vector_t initState = ocs2::vector_t::Ones(n); // Construct solver - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); // Solve diff --git a/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp b/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp index 23e1e13e5..65c2f7ad1 100644 --- a/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp @@ -29,11 +29,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include "ocs2_sqp/MultipleShootingSolver.h" -#include "ocs2_sqp/TimeDiscretization.h" +#include "ocs2_sqp/SqpSolver.h" #include <ocs2_core/initialization/DefaultInitializer.h> +#include <ocs2_oc/oc_data/TimeDiscretization.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> #include <ocs2_oc/test/testProblemsGeneration.h> @@ -72,7 +72,7 @@ TEST(test_valuefunction, linear_quadratic_problem) { ocs2::DefaultInitializer zeroInitializer(m); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.05; settings.sqpIteration = 1; settings.projectStateInputEqualityConstraints = true; @@ -83,7 +83,7 @@ TEST(test_valuefunction, linear_quadratic_problem) { settings.createValueFunction = true; // Set up solver - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); // Get value function From d254922ab5379fd91a462a0f61109b7f47335340 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 1 Dec 2022 19:40:58 +0100 Subject: [PATCH 372/512] fixing ddp --- ocs2_ddp/include/ocs2_ddp/HessianCorrection.h | 27 ++----------------- ocs2_ddp/src/HessianCorrection.cpp | 22 +++++++++++++++ 2 files changed, 24 insertions(+), 25 deletions(-) diff --git a/ocs2_ddp/include/ocs2_ddp/HessianCorrection.h b/ocs2_ddp/include/ocs2_ddp/HessianCorrection.h index 26ea4753a..deb3765b2 100644 --- a/ocs2_ddp/include/ocs2_ddp/HessianCorrection.h +++ b/ocs2_ddp/include/ocs2_ddp/HessianCorrection.h @@ -58,34 +58,11 @@ Strategy fromString(const std::string& name); /** * Shifts the Hessian based on the strategy defined by Line_Search::hessianCorrectionStrategy_. * - * @tparam Derived type. * @param [in] strategy: Hessian matrix correction strategy. - * @param matrix: The Hessian matrix. + * @param [in, out] matrix: The Hessian matrix. * @param [in] minEigenvalue: The minimum expected eigenvalue after correction. */ -template <typename Derived> -void shiftHessian(Strategy strategy, Eigen::MatrixBase<Derived>& matrix, - scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()) { - assert(matrix.rows() == matrix.cols()); - switch (strategy) { - case Strategy::DIAGONAL_SHIFT: { - matrix.diagonal().array() += minEigenvalue; - break; - } - case Strategy::CHOLESKY_MODIFICATION: { - LinearAlgebra::makePsdCholesky(matrix, minEigenvalue); - break; - } - case Strategy::EIGENVALUE_MODIFICATION: { - LinearAlgebra::makePsdEigenvalue(matrix, minEigenvalue); - break; - } - case Strategy::GERSHGORIN_MODIFICATION: { - LinearAlgebra::makePsdGershgorin(matrix, minEigenvalue); - break; - } - } -} +void shiftHessian(Strategy strategy, matrix_t& matrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon<scalar_t>()); } // namespace hessian_correction } // namespace ocs2 diff --git a/ocs2_ddp/src/HessianCorrection.cpp b/ocs2_ddp/src/HessianCorrection.cpp index 7b2dc331c..3a4f0424d 100644 --- a/ocs2_ddp/src/HessianCorrection.cpp +++ b/ocs2_ddp/src/HessianCorrection.cpp @@ -50,5 +50,27 @@ Strategy fromString(const std::string& name) { return strategyMap.at(name); } +void shiftHessian(Strategy strategy, matrix_t& matrix, scalar_t minEigenvalue) { + assert(matrix.rows() == matrix.cols()); + switch (strategy) { + case Strategy::DIAGONAL_SHIFT: { + matrix.diagonal().array() += minEigenvalue; + break; + } + case Strategy::CHOLESKY_MODIFICATION: { + LinearAlgebra::makePsdCholesky(matrix, minEigenvalue); + break; + } + case Strategy::EIGENVALUE_MODIFICATION: { + LinearAlgebra::makePsdEigenvalue(matrix, minEigenvalue); + break; + } + case Strategy::GERSHGORIN_MODIFICATION: { + LinearAlgebra::makePsdGershgorin(matrix, minEigenvalue); + break; + } + } +} + } // namespace hessian_correction } // namespace ocs2 From a431531a1fa2cf0642fe3b9846b9acb9f89b38ee Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 1 Dec 2022 20:00:04 +0100 Subject: [PATCH 373/512] fixing SqpMpc --- ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h index 25f40967b..3ff4d1f2a 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h @@ -54,8 +54,8 @@ class SqpMpc final : public MPC_BASE { ~SqpMpc() override = default; - MultipleShootingSolver* getSolverPtr() override { return solverPtr_.get(); } - const MultipleShootingSolver* getSolverPtr() const override { return solverPtr_.get(); } + SqpSolver* getSolverPtr() override { return solverPtr_.get(); } + const SqpSolver* getSolverPtr() const override { return solverPtr_.get(); } protected: void calculateController(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override { From c5d3f352a7cfc93feedb380fc4910c364064552f Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 1 Dec 2022 20:01:12 +0100 Subject: [PATCH 374/512] ballbot --- .../ocs2_ballbot/config/mpc/task.info | 4 ++-- .../include/ocs2_ballbot/BallbotInterface.h | 6 +++--- .../ocs2_ballbot/src/BallbotInterface.cpp | 2 +- .../ocs2_ballbot_ros/CMakeLists.txt | 17 +++++++++-------- .../{ballbot.launch => ballbot_ddp.launch} | 2 +- ...BallbotMpcNode.cpp => DdpBallbotMpcNode.cpp} | 0 ...ingBallbotNode.cpp => SqpBallbotMpcNode.cpp} | 6 +++--- 7 files changed, 19 insertions(+), 18 deletions(-) rename ocs2_robotic_examples/ocs2_ballbot_ros/launch/{ballbot.launch => ballbot_ddp.launch} (91%) rename ocs2_robotic_examples/ocs2_ballbot_ros/src/{BallbotMpcNode.cpp => DdpBallbotMpcNode.cpp} (100%) rename ocs2_robotic_examples/ocs2_ballbot_ros/src/{MultipleShootingBallbotNode.cpp => SqpBallbotMpcNode.cpp} (92%) diff --git a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info index 13865354a..3a7f860d4 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info @@ -12,8 +12,8 @@ templateSwitchingTimes { } -; multiple_shooting settings -multiple_shooting +; Multiple_Shooting SQP settings +sqp { dt 0.1 sqpIteration 5 diff --git a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h index a097f6bbd..9ecb270be 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h +++ b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h @@ -37,7 +37,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/rollout/TimeTriggeredRollout.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> #include <ocs2_robotic_tools/common/RobotInterface.h> -#include <ocs2_sqp/MultipleShootingSettings.h> +#include <ocs2_sqp/SqpSettings.h> // Ballbot #include "ocs2_ballbot/dynamics/BallbotSystemDynamics.h" @@ -69,7 +69,7 @@ class BallbotInterface final : public RobotInterface { const vector_t& getInitialState() { return initialState_; } - multiple_shooting::Settings& sqpSettings() { return sqpSettings_; } + sqp::Settings& sqpSettings() { return sqpSettings_; } ddp::Settings& ddpSettings() { return ddpSettings_; } @@ -86,7 +86,7 @@ class BallbotInterface final : public RobotInterface { private: ddp::Settings ddpSettings_; mpc::Settings mpcSettings_; - multiple_shooting::Settings sqpSettings_; + sqp::Settings sqpSettings_; OptimalControlProblem problem_; std::shared_ptr<ReferenceManager> referenceManagerPtr_; diff --git a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp index 59dfeb88d..a3b948e3d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp @@ -67,7 +67,7 @@ BallbotInterface::BallbotInterface(const std::string& taskFile, const std::strin // DDP SQP MPC settings ddpSettings_ = ddp::loadSettings(taskFile, "ddp"); mpcSettings_ = mpc::loadSettings(taskFile, "mpc"); - sqpSettings_ = multiple_shooting::loadSettings(taskFile, "multiple_shooting"); + sqpSettings_ = sqp::loadSettings(taskFile, "sqp"); /* * ReferenceManager & SolverSynchronizedModule diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt index 5121de843..6e2c638e0 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt @@ -58,16 +58,16 @@ include_directories( ) # Mpc node -add_executable(ballbot_mpc - src/BallbotMpcNode.cpp +add_executable(ballbot_ddp + src/DdpBallbotMpcNode.cpp ) -add_dependencies(ballbot_mpc +add_dependencies(ballbot_ddp ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(ballbot_mpc +target_link_libraries(ballbot_ddp ${catkin_LIBRARIES} ) -target_compile_options(ballbot_mpc PRIVATE ${OCS2_CXX_FLAGS}) +target_compile_options(ballbot_ddp PRIVATE ${OCS2_CXX_FLAGS}) # Dummy node add_executable(ballbot_dummy_test @@ -110,7 +110,7 @@ target_compile_options(ballbot_mpc_mrt PRIVATE ${OCS2_CXX_FLAGS}) ## SQP node for ballbot add_executable(ballbot_sqp - src/MultipleShootingBallbotNode.cpp + src/SqpBallbotMpcNode.cpp ) add_dependencies(ballbot_sqp ${catkin_EXPORTED_TARGETS} @@ -129,7 +129,8 @@ if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target ocs2_ballbot") add_clang_tooling( TARGETS - ballbot_mpc + ballbot_ddp + ballbot_sqp ballbot_dummy_test ballbot_target SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include @@ -145,7 +146,7 @@ endif(cmake_clang_tools_FOUND) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(TARGETS ballbot_mpc ballbot_dummy_test ballbot_target ballbot_sqp +install(TARGETS ballbot_ddp ballbot_dummy_test ballbot_target ballbot_sqp RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY launch rviz diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot.launch b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch similarity index 91% rename from ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot.launch rename to ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch index 1f91a76f3..0b43d96ad 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot.launch +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch @@ -11,7 +11,7 @@ <include file="$(find ocs2_ballbot_ros)/launch/multiplot.launch"/> </group> - <node pkg="ocs2_ballbot_ros" type="ballbot_mpc" name="ballbot_mpc" + <node pkg="ocs2_ballbot_ros" type="ballbot_ddp" name="ballbot_ddp" output="screen" args="$(arg task_name)" launch-prefix=""/> <node pkg="ocs2_ballbot_ros" type="ballbot_dummy_test" name="ballbot_dummy_test" diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DdpBallbotMpcNode.cpp similarity index 100% rename from ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcNode.cpp rename to ocs2_robotic_examples/ocs2_ballbot_ros/src/DdpBallbotMpcNode.cpp diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/SqpBallbotMpcNode.cpp similarity index 92% rename from ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp rename to ocs2_robotic_examples/ocs2_ballbot_ros/src/SqpBallbotMpcNode.cpp index 090f3707d..42fdc7cb5 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/SqpBallbotMpcNode.cpp @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> -#include <ocs2_sqp/MultipleShootingMpc.h> +#include <ocs2_sqp/SqpMpc.h> #include <ocs2_ballbot/BallbotInterface.h> @@ -62,8 +62,8 @@ int main(int argc, char** argv) { rosReferenceManagerPtr->subscribe(nodeHandle); // MPC - ocs2::MultipleShootingMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.sqpSettings(), ballbotInterface.getOptimalControlProblem(), - ballbotInterface.getInitializer()); + ocs2::SqpMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.sqpSettings(), ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node From 8153d80186017601a4b4b7f98801b13d3111f3d7 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 1 Dec 2022 20:08:57 +0100 Subject: [PATCH 375/512] LeggedRobot --- .../ocs2_legged_robot/config/mpc/task.info | 4 ++-- .../include/ocs2_legged_robot/LeggedRobotInterface.h | 6 +++--- .../ocs2_legged_robot/src/LeggedRobotInterface.cpp | 2 +- .../ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp | 5 ++--- 4 files changed, 8 insertions(+), 9 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index c1cf596d7..e175fe7cb 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -26,8 +26,8 @@ swing_trajectory_config swingTimeScale 0.15 } -; multiple_shooting settings -multiple_shooting +; Multiple_Shooting SQP settings +sqp { nThreads 3 dt 0.015 diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h index fa9c388e3..410d9e2d9 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h @@ -39,7 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_pinocchio_interface/PinocchioInterface.h> #include <ocs2_robotic_tools/common/RobotInterface.h> #include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h> -#include <ocs2_sqp/MultipleShootingSettings.h> +#include <ocs2_sqp/SqpSettings.h> #include "ocs2_legged_robot/common/ModelSettings.h" #include "ocs2_legged_robot/initialization/LeggedRobotInitializer.h" @@ -73,7 +73,7 @@ class LeggedRobotInterface final : public RobotInterface { const ddp::Settings& ddpSettings() const { return ddpSettings_; } const mpc::Settings& mpcSettings() const { return mpcSettings_; } const rollout::Settings& rolloutSettings() const { return rolloutSettings_; } - const multiple_shooting::Settings& sqpSettings() { return sqpSettings_; } + const sqp::Settings& sqpSettings() { return sqpSettings_; } const vector_t& getInitialState() const { return initialState_; } const RolloutBase& getRollout() const { return *rolloutPtr_; } @@ -104,7 +104,7 @@ class LeggedRobotInterface final : public RobotInterface { ModelSettings modelSettings_; ddp::Settings ddpSettings_; mpc::Settings mpcSettings_; - multiple_shooting::Settings sqpSettings_; + sqp::Settings sqpSettings_; std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_; CentroidalModelInfo centroidalModelInfo_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 8f2ccd3ff..c02c924aa 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -95,7 +95,7 @@ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const st ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose); mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose); rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); - sqpSettings_ = multiple_shooting::loadSettings(taskFile, "multiple_shooting", verbose); + sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose); // OptimalConrolProblem setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp index 13953b284..39c33b880 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp @@ -33,7 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_legged_robot/LeggedRobotInterface.h> #include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> -#include <ocs2_sqp/MultipleShootingMpc.h> +#include <ocs2_sqp/SqpMpc.h> #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" @@ -64,8 +64,7 @@ int main(int argc, char** argv) { rosReferenceManagerPtr->subscribe(nodeHandle); // MPC - MultipleShootingMpc mpc(interface.mpcSettings(), interface.sqpSettings(), interface.getOptimalControlProblem(), - interface.getInitializer()); + SqpMpc mpc(interface.mpcSettings(), interface.sqpSettings(), interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); From 0d3f6042534d4f81a0081d07ab4dd566f325a600 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 1 Dec 2022 20:20:56 +0100 Subject: [PATCH 376/512] small changes --- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index bd61a6c59..692b00f08 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -316,7 +316,8 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); workerPerformance += sqp::computeIntermediatePerformance(result, dt); if (settings_.projectStateInputEqualityConstraints) { - multiple_shooting::projectTranscription(result); + constexpr bool extractPseudoInverse = false; + multiple_shooting::projectTranscription(result, extractPseudoInverse); } dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); @@ -349,6 +350,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated // Sum performance of the threads PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + return totalPerformance; } From 38a2caceee1afa94feb07f782488bf908c33b368 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 2 Dec 2022 11:55:45 +0100 Subject: [PATCH 377/512] moving armijoDescentMetric to FilterLinesearch --- .../include/ocs2_oc/multiple_shooting/Helpers.h | 12 ------------ .../ocs2_oc/search_strategy/FilterLinesearch.h | 12 ++++++++++++ ocs2_oc/src/multiple_shooting/Helpers.cpp | 15 --------------- ocs2_oc/src/search_strategy/FilterLinesearch.cpp | 15 +++++++++++++++ ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 2 +- 5 files changed, 28 insertions(+), 28 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h index 510f40b9e..861531a59 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h @@ -64,18 +64,6 @@ void incrementTrajectory(const std::vector<Type>& v, const std::vector<Type>& dv } } -/** - * Computes the Armijo descent metric that determines if the solution is a descent direction for the cost. It calculates sum of - * gradient(cost).dot([dx; du]) over the trajectory. - * - * @param [in] cost: The quadratic approximation of the cost. - * @param [in] deltaXSol: The state trajectory of the QP subproblem solution. - * @param [in] deltaUSol: The input trajectory of the QP subproblem solution. - * @return The Armijo descent metric. - */ -scalar_t armijoDescentMetric(const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& deltaXSol, - const vector_array_t& deltaUSol); - /** * Re-map the projected input back to the original space. * diff --git a/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h b/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h index a11ba7c58..a89721d50 100644 --- a/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h +++ b/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h @@ -68,4 +68,16 @@ struct FilterLinesearch { /** Transforms the StepType to string */ std::string toString(const FilterLinesearch::StepType& stepType); +/** + * Computes the Armijo descent metric that determines if the solution is a descent direction for the cost. It calculates sum of + * gradient(cost).dot([dx; du]) over the trajectory. + * + * @param [in] cost: The quadratic approximation of the cost. + * @param [in] deltaXSol: The state trajectory of the QP subproblem solution. + * @param [in] deltaUSol: The input trajectory of the QP subproblem solution. + * @return The Armijo descent metric. + */ +scalar_t armijoDescentMetric(const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& deltaXSol, + const vector_array_t& deltaUSol); + } // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/Helpers.cpp b/ocs2_oc/src/multiple_shooting/Helpers.cpp index 52d8f3e8f..c2e59a219 100644 --- a/ocs2_oc/src/multiple_shooting/Helpers.cpp +++ b/ocs2_oc/src/multiple_shooting/Helpers.cpp @@ -35,21 +35,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { -scalar_t armijoDescentMetric(const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& deltaXSol, - const vector_array_t& deltaUSol) { - // To determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] - scalar_t metric = 0.0; - for (int i = 0; i < cost.size(); i++) { - if (cost[i].dfdx.size() > 0) { - metric += cost[i].dfdx.dot(deltaXSol[i]); - } - if (cost[i].dfdu.size() > 0) { - metric += cost[i].dfdu.dot(deltaUSol[i]); - } - } - return metric; -} - void remapProjectedInput(const std::vector<VectorFunctionLinearApproximation>& constraintsProjection, const vector_array_t& deltaXSol, vector_array_t& deltaUSol) { vector_t tmp; // 1 temporary for re-use. diff --git a/ocs2_oc/src/search_strategy/FilterLinesearch.cpp b/ocs2_oc/src/search_strategy/FilterLinesearch.cpp index b9b39b54e..80cfa47c4 100644 --- a/ocs2_oc/src/search_strategy/FilterLinesearch.cpp +++ b/ocs2_oc/src/search_strategy/FilterLinesearch.cpp @@ -73,4 +73,19 @@ std::string toString(const FilterLinesearch::StepType& stepType) { } } +scalar_t armijoDescentMetric(const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& deltaXSol, + const vector_array_t& deltaUSol) { + // To determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + scalar_t metric = 0.0; + for (int i = 0; i < cost.size(); i++) { + if (cost[i].dfdx.size() > 0) { + metric += cost[i].dfdx.dot(deltaXSol[i]); + } + if (cost[i].dfdu.size() > 0) { + metric += cost[i].dfdu.dot(deltaUSol[i]); + } + } + return metric; +} + } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 692b00f08..b5cd1c8f9 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -243,7 +243,7 @@ SqpSolver::OcpSubproblemSolution SqpSolver::getOCPSolution(const vector_t& delta } // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] - solution.armijoDescentMetric = multiple_shooting::armijoDescentMetric(cost_, deltaXSol, deltaUSol); + solution.armijoDescentMetric = armijoDescentMetric(cost_, deltaXSol, deltaUSol); // remap the tilde delta u to real delta u if (settings_.projectStateInputEqualityConstraints) { From 7a366b787372188638cd762186dd463322090858 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 2 Dec 2022 12:10:53 +0100 Subject: [PATCH 378/512] moving SqpPerformanceIndexComputation to ocs2_oc/multiple_shooting --- ocs2_oc/CMakeLists.txt | 2 + .../PerformanceIndexComputation.h | 26 ++++---- .../PerformanceIndexComputation.cpp | 12 ++-- .../testTranscriptionPerformanceIndex.cpp | 64 +++++++++---------- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 2 - ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 15 ++--- 6 files changed, 58 insertions(+), 63 deletions(-) rename ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpPerformanceIndexComputation.h => ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h (81%) rename ocs2_sqp/ocs2_sqp/src/SqpPerformanceIndexComputation.cpp => ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp (94%) rename {ocs2_sqp/ocs2_sqp/test => ocs2_oc/test/multiple_shooting}/testTranscriptionPerformanceIndex.cpp (81%) diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 40a1df25b..7d6a83b09 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -51,6 +51,7 @@ add_library(${PROJECT_NAME} src/approximate_model/LinearQuadraticApproximator.cpp src/multiple_shooting/Helpers.cpp src/multiple_shooting/Initialization.cpp + src/multiple_shooting/PerformanceIndexComputation.cpp src/multiple_shooting/Transcription.cpp src/oc_data/LoopshapingPrimalSolution.cpp src/oc_data/TimeDiscretization.cpp @@ -128,6 +129,7 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting test/multiple_shooting/testHelpers.cpp + test/multiple_shooting/testTranscriptionPerformanceIndex.cpp ) add_dependencies(test_${PROJECT_NAME}_multiple_shooting ${catkin_EXPORTED_TARGETS} diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpPerformanceIndexComputation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h similarity index 81% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpPerformanceIndexComputation.h rename to ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h index 02d3f6642..4cbd1b41d 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpPerformanceIndexComputation.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h @@ -31,12 +31,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/integration/SensitivityIntegrator.h> -#include <ocs2_oc/multiple_shooting/Transcription.h> -#include <ocs2_oc/oc_data/PerformanceIndex.h> -#include <ocs2_oc/oc_problem/OptimalControlProblem.h> + +#include "ocs2_oc/multiple_shooting/Transcription.h" +#include "ocs2_oc/oc_data/PerformanceIndex.h" +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" namespace ocs2 { -namespace sqp { +namespace multiple_shooting { /** * Compute the performance index from the transcription for this node. @@ -44,12 +45,11 @@ namespace sqp { * @param dt : Duration of the interval * @return Performance index for a single intermediate node. */ -PerformanceIndex computeIntermediatePerformance(const multiple_shooting::Transcription& transcription, scalar_t dt); +PerformanceIndex computeIntermediatePerformance(const Transcription& transcription, scalar_t dt); /** * Compute only the performance index for a single intermediate node. - * Corresponds to the performance index computed from the multiple_shooting::Transcription returned by - * "multiple_shooting::setupIntermediateNode" + * Corresponds to the performance index computed from the Transcription returned by "multiple_shooting::setupIntermediateNode". * @param optimalControlProblem : Definition of the optimal control problem * @param discretizer : Integrator to use for creating the discrete dynamics. * @param t : Start of the discrete interval @@ -67,12 +67,11 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt * @param transcription: multiple shooting transcription for this node. * @return Performance index for the terminal node. */ -PerformanceIndex computeTerminalPerformance(const multiple_shooting::TerminalTranscription& transcription); +PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcription); /** * Compute only the performance index for the terminal node. - * Corresponds to the performance index computed from multiple_shooting::TerminalTranscription returned by - * "multiple_shooting::setTerminalNode" + * Corresponds to the performance index computed from TerminalTranscription returned by "multiple_shooting::setTerminalNode". * @param optimalControlProblem : Definition of the optimal control problem * @param t : Time at the terminal node * @param x : Terminal state @@ -85,12 +84,11 @@ PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimal * @param transcription: multiple shooting transcription for this node. * @return Performance index for the event node. */ -PerformanceIndex computeEventPerformance(const multiple_shooting::EventTranscription& transcription); +PerformanceIndex computeEventPerformance(const EventTranscription& transcription); /** * Compute only the performance index for the event node. - * Corresponds to the performance index computed from multiple_shooting::EventTranscription returned by - * "multiple_shooting::setEventNode" + * Corresponds to the performance index computed from EventTranscription returned by "multiple_shooting::setEventNode". * @param optimalControlProblem : Definition of the optimal control problem * @param t : Time at the event node * @param x : Pre-event state @@ -100,5 +98,5 @@ PerformanceIndex computeEventPerformance(const multiple_shooting::EventTranscrip PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); -} // namespace sqp +} // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/SqpPerformanceIndexComputation.cpp b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp similarity index 94% rename from ocs2_sqp/ocs2_sqp/src/SqpPerformanceIndexComputation.cpp rename to ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp index 07152787b..5e8ce8e1b 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpPerformanceIndexComputation.cpp +++ b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp @@ -27,12 +27,12 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_sqp/SqpPerformanceIndexComputation.h" +#include "ocs2_oc/multiple_shooting/PerformanceIndexComputation.h" #include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> namespace ocs2 { -namespace sqp { +namespace multiple_shooting { namespace { scalar_t getEqConstraintsSSE(const vector_t& eqConstraint) { @@ -52,7 +52,7 @@ scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { } } // namespace -PerformanceIndex computeIntermediatePerformance(const multiple_shooting::Transcription& transcription, scalar_t dt) { +PerformanceIndex computeIntermediatePerformance(const Transcription& transcription, scalar_t dt) { PerformanceIndex performance; // Dynamics @@ -111,7 +111,7 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt return performance; } -PerformanceIndex computeTerminalPerformance(const multiple_shooting::TerminalTranscription& transcription) { +PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcription) { PerformanceIndex performance; performance.cost = transcription.cost.f; @@ -148,7 +148,7 @@ PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimal return performance; } -PerformanceIndex computeEventPerformance(const multiple_shooting::EventTranscription& transcription) { +PerformanceIndex computeEventPerformance(const EventTranscription& transcription) { PerformanceIndex performance; // Dynamics @@ -193,5 +193,5 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon return performance; } -} // namespace sqp +} // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp b/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp similarity index 81% rename from ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp rename to ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp index 8ec7c909a..58446e596 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscriptionPerformanceIndex.cpp +++ b/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp @@ -30,10 +30,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> #include <ocs2_oc/multiple_shooting/Transcription.h> -#include <ocs2_oc/test/circular_kinematics.h> -#include <ocs2_oc/test/testProblemsGeneration.h> +#include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> -#include "ocs2_sqp/SqpPerformanceIndexComputation.h" +#include "ocs2_oc/test/circular_kinematics.h" +#include "ocs2_oc/test/testProblemsGeneration.h" namespace { /** Helper to compare if two performance indices are identical */ @@ -45,8 +45,6 @@ bool areIdentical(const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceInde } // namespace using namespace ocs2; -using namespace ocs2::multiple_shooting; -using namespace ocs2::sqp; TEST(test_transcription, intermediate_performance) { constexpr int nx = 2; @@ -67,62 +65,62 @@ TEST(test_transcription, intermediate_performance) { const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); const vector_t u = (vector_t(nu) << 0.1, 1.3).finished(); - const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, t, dt, x, x_next, u); + const auto transcription = multiple_shooting::setupIntermediateNode(problem, sensitivityDiscretizer, t, dt, x, x_next, u); - const auto performance = sqp::computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); + const auto performance = multiple_shooting::computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); - ASSERT_TRUE(areIdentical(performance, sqp::computeIntermediatePerformance(transcription, dt))); + ASSERT_TRUE(areIdentical(performance, multiple_shooting::computeIntermediatePerformance(transcription, dt))); } -TEST(test_transcription, terminal_performance) { - constexpr int nx = 3; +TEST(test_transcription, event_performance) { + constexpr int nx = 2; // optimal control problem OptimalControlProblem problem; + // dynamics + const auto dynamics = getRandomDynamics(nx, 0); + const auto jumpMap = matrix_t::Random(nx, nx); + problem.dynamicsPtr.reset(new LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); + // cost - problem.finalCostPtr->add("finalCost", getOcs2StateCost(getRandomCost(nx, 0))); - problem.finalSoftConstraintPtr->add("finalSoftCost", getOcs2StateCost(getRandomCost(nx, 0))); + problem.preJumpCostPtr->add("eventCost", getOcs2StateCost(getRandomCost(nx, 0))); // inequality constraints - problem.finalInequalityConstraintPtr->add("finalInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); + problem.preJumpInequalityConstraintPtr->add("preJumpInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); problem.targetTrajectoriesPtr = &targetTrajectories; - scalar_t t = 0.5; - const vector_t x = vector_t::Random(nx); - const auto transcription = setupTerminalNode(problem, t, x); - const auto performance = sqp::computeTerminalPerformance(problem, t, x); + const scalar_t t = 0.5; + const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); + const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); + const auto transcription = multiple_shooting::setupEventNode(problem, t, x, x_next); + const auto performance = multiple_shooting::computeEventPerformance(problem, t, x, x_next); - ASSERT_TRUE(areIdentical(performance, sqp::computeTerminalPerformance(transcription))); + ASSERT_TRUE(areIdentical(performance, multiple_shooting::computeEventPerformance(transcription))); } -TEST(test_transcription, event_performance) { - constexpr int nx = 2; +TEST(test_transcription, terminal_performance) { + constexpr int nx = 3; // optimal control problem OptimalControlProblem problem; - // dynamics - const auto dynamics = getRandomDynamics(nx, 0); - const auto jumpMap = matrix_t::Random(nx, nx); - problem.dynamicsPtr.reset(new LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); - // cost - problem.preJumpCostPtr->add("eventCost", getOcs2StateCost(getRandomCost(nx, 0))); + problem.finalCostPtr->add("finalCost", getOcs2StateCost(getRandomCost(nx, 0))); + problem.finalSoftConstraintPtr->add("finalSoftCost", getOcs2StateCost(getRandomCost(nx, 0))); // inequality constraints - problem.preJumpInequalityConstraintPtr->add("preJumpInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); + problem.finalInequalityConstraintPtr->add("finalInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); problem.targetTrajectoriesPtr = &targetTrajectories; - const scalar_t t = 0.5; - const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); - const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); - const auto transcription = setupEventNode(problem, t, x, x_next); - const auto performance = sqp::computeEventPerformance(problem, t, x, x_next); + scalar_t t = 0.5; + const vector_t x = vector_t::Random(nx); + const auto transcription = multiple_shooting::setupTerminalNode(problem, t, x); + const auto performance = multiple_shooting::computeTerminalPerformance(problem, t, x); - ASSERT_TRUE(areIdentical(performance, sqp::computeEventPerformance(transcription))); + ASSERT_TRUE(areIdentical(performance, multiple_shooting::computeTerminalPerformance(transcription))); } diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 3f8781223..35829a9ed 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -53,7 +53,6 @@ add_library(${PROJECT_NAME} src/SqpSettings.cpp src/SqpSolver.cpp src/SqpSolverStatus.cpp - src/SqpPerformanceIndexComputation.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} @@ -99,7 +98,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ catkin_add_gtest(test_${PROJECT_NAME} test/testCircularKinematics.cpp test/testSwitchedProblem.cpp - test/testTranscriptionPerformanceIndex.cpp test/testUnconstrained.cpp test/testValuefunction.cpp ) diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index b5cd1c8f9..1b8435127 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -35,10 +35,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/penalties/penalties/RelaxedBarrierPenalty.h> #include <ocs2_oc/multiple_shooting/Helpers.h> #include <ocs2_oc/multiple_shooting/Initialization.h> +#include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> #include <ocs2_oc/multiple_shooting/Transcription.h> -#include "ocs2_sqp/SqpPerformanceIndexComputation.h" - namespace ocs2 { SqpSolver::SqpSolver(sqp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) @@ -302,7 +301,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); - workerPerformance += sqp::computeEventPerformance(result); + workerPerformance += multiple_shooting::computeEventPerformance(result); dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); @@ -314,7 +313,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); - workerPerformance += sqp::computeIntermediatePerformance(result, dt); + workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); if (settings_.projectStateInputEqualityConstraints) { constexpr bool extractPseudoInverse = false; multiple_shooting::projectTranscription(result, extractPseudoInverse); @@ -333,7 +332,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); - workerPerformance += sqp::computeTerminalPerformance(result); + workerPerformance += multiple_shooting::computeTerminalPerformance(result); cost_[i] = std::move(result.cost); stateInputEqConstraints_[i] = std::move(result.eqConstraints); stateIneqConstraints_[i] = std::move(result.ineqConstraints); @@ -370,12 +369,12 @@ PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>& while (i < N) { if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node - workerPerformance += sqp::computeEventPerformance(ocpDefinition, time[i].time, x[i], x[i + 1]); + workerPerformance += multiple_shooting::computeEventPerformance(ocpDefinition, time[i].time, x[i], x[i + 1]); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - workerPerformance += sqp::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); } i = timeIndex++; @@ -383,7 +382,7 @@ PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>& if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); - workerPerformance += sqp::computeTerminalPerformance(ocpDefinition, tN, x[N]); + workerPerformance += multiple_shooting::computeTerminalPerformance(ocpDefinition, tN, x[N]); } // Accumulate! Same worker might run multiple tasks From e636582734a04f1833785836aa7aa82b28f14ce4 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 2 Dec 2022 12:23:50 +0100 Subject: [PATCH 379/512] minor changes --- ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h | 8 +++----- ocs2_oc/src/lintTarget.cpp | 8 ++++++++ ocs2_oc/src/multiple_shooting/Helpers.cpp | 7 ++++--- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h index 861531a59..a7a5ebf53 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h @@ -112,17 +112,15 @@ struct ProjectionMultiplierCoefficients { vector_t f; /** - * Extracts the coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint. + * Extracts the coefficients of the Lagrange multiplier associated with the state-input equality constraint. * * @param dynamics : Dynamics * @param cost : Cost * @param constraintProjection : Constraint projection. * @param pseudoInverse : Left pseudo-inverse of D^T of the state-input equality constraint. */ - void extractProjectionMultiplierCoefficients(const VectorFunctionLinearApproximation& dynamics, - const ScalarFunctionQuadraticApproximation& cost, - const VectorFunctionLinearApproximation& constraintProjection, - const matrix_t& pseudoInverse); + void compute(const VectorFunctionLinearApproximation& dynamics, const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse); }; } // namespace multiple_shooting diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index a885a9499..149761750 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -28,13 +28,21 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ // approximate model +#include <ocs2_oc/approximate_model/ChangeOfInputVariables.h> #include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> +// multiple_shooting +#include <ocs2_oc/multiple_shooting/Helpers.h> +#include <ocs2_oc/multiple_shooting/Initialization.h> +#include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> +#include <ocs2_oc/multiple_shooting/Transcription.h> + // oc_data #include <ocs2_oc/oc_data/DualSolution.h> #include <ocs2_oc/oc_data/LoopshapingPrimalSolution.h> #include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/oc_data/PrimalSolution.h> +#include <ocs2_oc/oc_data/TimeDiscretization.h> // oc_problem #include <ocs2_oc/oc_problem/LoopshapingOptimalControlProblem.h> diff --git a/ocs2_oc/src/multiple_shooting/Helpers.cpp b/ocs2_oc/src/multiple_shooting/Helpers.cpp index c2e59a219..db33cca0f 100644 --- a/ocs2_oc/src/multiple_shooting/Helpers.cpp +++ b/ocs2_oc/src/multiple_shooting/Helpers.cpp @@ -119,9 +119,10 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche return primalSolution; } -void ProjectionMultiplierCoefficients::extractProjectionMultiplierCoefficients( - const VectorFunctionLinearApproximation& dynamics, const ScalarFunctionQuadraticApproximation& cost, - const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse) { +void ProjectionMultiplierCoefficients::compute(const VectorFunctionLinearApproximation& dynamics, + const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& constraintProjection, + const matrix_t& pseudoInverse) { vector_t semiprojectedCost_dfdu = cost.dfdu; semiprojectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f; From 05d19b78c07dba579b9746b5acbe4b8948bea6d1 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 2 Dec 2022 13:04:47 +0100 Subject: [PATCH 380/512] bug fix --- ocs2_ddp/test/testReachingTask.cpp | 2 ++ ocs2_oc/test/multiple_shooting/testHelpers.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/ocs2_ddp/test/testReachingTask.cpp b/ocs2_ddp/test/testReachingTask.cpp index 63ea4ee9d..43b1d5b41 100644 --- a/ocs2_ddp/test/testReachingTask.cpp +++ b/ocs2_ddp/test/testReachingTask.cpp @@ -186,6 +186,8 @@ INSTANTIATE_TEST_CASE_P(FinalDoubleIntegratorReachingTaskCase, FinalDoubleIntegr return std::string("QuadraticPenalty"); } else if (info.param == DoubleIntegratorReachingTask::PenaltyType::SmoothAbsolutePenalty) { return std::string("SmoothAbsolutePenalty"); + } else { + throw std::runtime_error("Undefined PenaltyTyp!"); } }); diff --git a/ocs2_oc/test/multiple_shooting/testHelpers.cpp b/ocs2_oc/test/multiple_shooting/testHelpers.cpp index 823382401..048ca3814 100644 --- a/ocs2_oc/test/multiple_shooting/testHelpers.cpp +++ b/ocs2_oc/test/multiple_shooting/testHelpers.cpp @@ -50,7 +50,7 @@ TEST(testMultipleShootingHelpers, testProjectionMultiplierCoefficients) { const auto pseudoInverse = std::move(result.second); multiple_shooting::ProjectionMultiplierCoefficients projectionMultiplierCoefficients; - projectionMultiplierCoefficients.extractProjectionMultiplierCoefficients(dynamics, cost, projection, pseudoInverse); + projectionMultiplierCoefficients.compute(dynamics, cost, projection, pseudoInverse); const matrix_t dfdx = -pseudoInverse * (cost.dfdux + cost.dfduu * projection.dfdx); const matrix_t dfdu = -pseudoInverse * (cost.dfduu * projection.dfdu); From 3671219f9a7c32b2b3b4293685d2a5a2ea7ed1ee Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 2 Dec 2022 14:38:07 +0100 Subject: [PATCH 381/512] mpcnet adaptation --- .../src/BallbotMpcnetDummyNode.cpp | 11 +++++------ .../src/BallbotMpcnetInterface.cpp | 10 +++++----- .../src/LeggedRobotMpcnetDummyNode.cpp | 9 ++++----- .../src/LeggedRobotMpcnetInterface.cpp | 18 +++++++++--------- .../src/rollout/MpcnetRolloutManager.cpp | 12 ++++++------ 5 files changed, 29 insertions(+), 31 deletions(-) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp index 010fc2a8b..012a178c0 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp @@ -69,20 +69,19 @@ int main(int argc, char** argv) { // policy (MPC-Net controller) auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); - std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition); - std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase> mpcnetControllerPtr( - new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); + auto mpcnetDefinitionPtr = std::make_shared<BallbotMpcnetDefinition>(); + auto mpcnetControllerPtr = + std::make_unique<ocs2::mpcnet::MpcnetOnnxController>(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr); mpcnetControllerPtr->loadPolicyModel(policyFilePath); // rollout std::unique_ptr<RolloutBase> rolloutPtr(ballbotInterface.getRollout().clone()); // observer - std::shared_ptr<ocs2::mpcnet::MpcnetDummyObserverRos> mpcnetDummyObserverRosPtr( - new ocs2::mpcnet::MpcnetDummyObserverRos(nodeHandle, robotName)); + auto mpcnetDummyObserverRosPtr = std::make_shared<ocs2::mpcnet::MpcnetDummyObserverRos>(nodeHandle, robotName); // visualization - std::shared_ptr<BallbotDummyVisualization> ballbotDummyVisualization(new BallbotDummyVisualization(nodeHandle)); + auto ballbotDummyVisualization = std::make_shared<BallbotDummyVisualization>(nodeHandle); // MPC-Net dummy loop ROS const scalar_t controlFrequency = ballbotInterface.mpcSettings().mrtDesiredFrequency_; diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp index dfe15a9a5..9a3da6748 100644 --- a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -61,10 +61,10 @@ BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, si referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { BallbotInterface ballbotInterface(taskFile, libraryFolder); - std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new BallbotMpcnetDefinition); + auto mpcnetDefinitionPtr = std::make_shared<BallbotMpcnetDefinition>(); mpcPtrs.push_back(getMpc(ballbotInterface)); - mpcnetPtrs.push_back(std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>( - new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, ballbotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr))); + mpcnetPtrs.push_back(std::make_unique<ocs2::mpcnet::MpcnetOnnxController>( + mpcnetDefinitionPtr, ballbotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr)); if (raisim) { throw std::runtime_error("[BallbotMpcnetInterface::BallbotMpcnetInterface] raisim rollout not yet implemented for ballbot."); } else { @@ -101,8 +101,8 @@ std::unique_ptr<MPC_BASE> BallbotMpcnetInterface::getMpc(BallbotInterface& ballb return settings; }(); // create one MPC instance - std::unique_ptr<MPC_BASE> mpcPtr(new GaussNewtonDDP_MPC(mpcSettings, ddpSettings, ballbotInterface.getRollout(), - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer())); + auto mpcPtr = std::make_unique<GaussNewtonDDP_MPC>(mpcSettings, ddpSettings, ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); return mpcPtr; } diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp index e16898bac..9788951b5 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -79,9 +79,9 @@ int main(int argc, char** argv) { // policy (MPC-Net controller) auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); - std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new LeggedRobotMpcnetDefinition(leggedRobotInterface)); - std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase> mpcnetControllerPtr( - new ocs2::mpcnet::MpcnetOnnxController(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr)); + auto mpcnetDefinitionPtr = std::make_shared<LeggedRobotMpcnetDefinition>(leggedRobotInterface); + auto mpcnetControllerPtr = + std::make_unique<ocs2::mpcnet::MpcnetOnnxController>(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr); mpcnetControllerPtr->loadPolicyModel(policyFile); // rollout @@ -121,8 +121,7 @@ int main(int argc, char** argv) { } // observer - std::shared_ptr<ocs2::mpcnet::MpcnetDummyObserverRos> mpcnetDummyObserverRosPtr( - new ocs2::mpcnet::MpcnetDummyObserverRos(nodeHandle, robotName)); + auto mpcnetDummyObserverRosPtr = std::make_shared<ocs2::mpcnet::MpcnetDummyObserverRos>(nodeHandle, robotName); // visualization CentroidalModelPinocchioMapping pinocchioMapping(leggedRobotInterface.getCentroidalModelInfo()); diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp index 6c068bca9..533141a45 100644 --- a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -64,19 +64,19 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr mpcnetDefinitionPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { - leggedRobotInterfacePtrs_.push_back(std::unique_ptr<LeggedRobotInterface>(new LeggedRobotInterface(taskFile, urdfFile, referenceFile))); - std::shared_ptr<ocs2::mpcnet::MpcnetDefinitionBase> mpcnetDefinitionPtr(new LeggedRobotMpcnetDefinition(*leggedRobotInterfacePtrs_[i])); + leggedRobotInterfacePtrs_.push_back(std::make_unique<LeggedRobotInterface>(taskFile, urdfFile, referenceFile)); + auto mpcnetDefinitionPtr = std::make_shared<LeggedRobotMpcnetDefinition>(*leggedRobotInterfacePtrs_[i]); mpcPtrs.push_back(getMpc(*leggedRobotInterfacePtrs_[i])); mpcnetPtrs.push_back(std::unique_ptr<ocs2::mpcnet::MpcnetControllerBase>(new ocs2::mpcnet::MpcnetOnnxController( mpcnetDefinitionPtr, leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr(), onnxEnvironmentPtr))); if (raisim) { RaisimRolloutSettings raisimRolloutSettings(raisimFile, "rollout"); raisimRolloutSettings.portNumber_ += i; - leggedRobotRaisimConversionsPtrs_.push_back(std::unique_ptr<LeggedRobotRaisimConversions>(new LeggedRobotRaisimConversions( + leggedRobotRaisimConversionsPtrs_.push_back(std::make_unique<LeggedRobotRaisimConversions>( leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(), - leggedRobotInterfacePtrs_[i]->getInitialState()))); + leggedRobotInterfacePtrs_[i]->getInitialState())); leggedRobotRaisimConversionsPtrs_[i]->loadSettings(raisimFile, "rollout", true); - rolloutPtrs.push_back(std::unique_ptr<RolloutBase>(new RaisimRollout( + rolloutPtrs.push_back(std::make_unique<RaisimRollout>( urdfFile, resourcePath, [&, i](const vector_t& state, const vector_t& input) { return leggedRobotRaisimConversionsPtrs_[i]->stateToRaisimGenCoordGenVel(state, input); @@ -90,7 +90,7 @@ LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThr nullptr, raisimRolloutSettings, [&, i](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { return leggedRobotRaisimConversionsPtrs_[i]->inputToRaisimPdTargets(time, input, state, q, dq); - }))); + })); if (raisimRolloutSettings.generateTerrain_) { raisim::TerrainProperties terrainProperties; terrainProperties.zScale = raisimRolloutSettings.terrainRoughness_; @@ -132,9 +132,9 @@ std::unique_ptr<MPC_BASE> LeggedRobotMpcnetInterface::getMpc(LeggedRobotInterfac return settings; }(); // create one MPC instance - std::unique_ptr<MPC_BASE> mpcPtr(new GaussNewtonDDP_MPC(mpcSettings, ddpSettings, leggedRobotInterface.getRollout(), - leggedRobotInterface.getOptimalControlProblem(), - leggedRobotInterface.getInitializer())); + auto mpcPtr = + std::make_unique<GaussNewtonDDP_MPC>(mpcSettings, ddpSettings, leggedRobotInterface.getRollout(), + leggedRobotInterface.getOptimalControlProblem(), leggedRobotInterface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(leggedRobotInterface.getReferenceManagerPtr()); return mpcPtr; } diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp index 9d3f43d2b..7c170fb33 100644 --- a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp @@ -47,9 +47,9 @@ MpcnetRolloutManager::MpcnetRolloutManager(size_t nDataGenerationThreads, size_t dataGenerationThreadPoolPtr_.reset(new ThreadPool(nDataGenerationThreads_)); dataGenerationPtrs_.reserve(nDataGenerationThreads); for (int i = 0; i < nDataGenerationThreads; i++) { - dataGenerationPtrs_.push_back(std::unique_ptr<MpcnetDataGeneration>( - new MpcnetDataGeneration(std::move(mpcPtrs.at(i)), std::move(mpcnetPtrs.at(i)), std::move(rolloutPtrs.at(i)), - std::move(mpcnetDefinitionPtrs.at(i)), referenceManagerPtrs.at(i)))); + dataGenerationPtrs_.push_back( + std::make_unique<MpcnetDataGeneration>(std::move(mpcPtrs.at(i)), std::move(mpcnetPtrs.at(i)), std::move(rolloutPtrs.at(i)), + std::move(mpcnetDefinitionPtrs.at(i)), referenceManagerPtrs.at(i))); } } @@ -59,9 +59,9 @@ MpcnetRolloutManager::MpcnetRolloutManager(size_t nDataGenerationThreads, size_t policyEvaluationThreadPoolPtr_.reset(new ThreadPool(nPolicyEvaluationThreads_)); policyEvaluationPtrs_.reserve(nPolicyEvaluationThreads_); for (int i = nDataGenerationThreads_; i < (nDataGenerationThreads_ + nPolicyEvaluationThreads_); i++) { - policyEvaluationPtrs_.push_back(std::unique_ptr<MpcnetPolicyEvaluation>( - new MpcnetPolicyEvaluation(std::move(mpcPtrs.at(i)), std::move(mpcnetPtrs.at(i)), std::move(rolloutPtrs.at(i)), - std::move(mpcnetDefinitionPtrs.at(i)), referenceManagerPtrs.at(i)))); + policyEvaluationPtrs_.push_back( + std::make_unique<MpcnetPolicyEvaluation>(std::move(mpcPtrs.at(i)), std::move(mpcnetPtrs.at(i)), std::move(rolloutPtrs.at(i)), + std::move(mpcnetDefinitionPtrs.at(i)), referenceManagerPtrs.at(i))); } } } From ca38d9be7d1ac1d5f24b810f0e5bd4ab9acccf3b Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 2 Dec 2022 16:10:46 +0100 Subject: [PATCH 382/512] update --- ocs2_pipg/CMakeLists.txt | 6 +- ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h | 29 --- ocs2_pipg/include/ocs2_pipg/PipgSettings.h | 58 ++++++ .../ocs2_pipg/{PIPG.h => PipgSolver.h} | 92 +++++----- .../include/ocs2_pipg/PipgSolverStatus.h | 55 ++++++ ocs2_pipg/src/HelperFunctions.cpp | 29 +++ ocs2_pipg/src/PIPG_Settings.cpp | 42 ----- ocs2_pipg/src/PipgSettings.cpp | 71 ++++++++ ocs2_pipg/src/{PIPG.cpp => PipgSolver.cpp} | 169 ++++++++++++------ ocs2_pipg/test/testHelper.cpp | 29 +++ ...{testPIPGSolver.cpp => testPipgSolver.cpp} | 73 +++++--- 11 files changed, 463 insertions(+), 190 deletions(-) delete mode 100644 ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h create mode 100644 ocs2_pipg/include/ocs2_pipg/PipgSettings.h rename ocs2_pipg/include/ocs2_pipg/{PIPG.h => PipgSolver.h} (53%) create mode 100644 ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h delete mode 100644 ocs2_pipg/src/PIPG_Settings.cpp create mode 100644 ocs2_pipg/src/PipgSettings.cpp rename ocs2_pipg/src/{PIPG.cpp => PipgSolver.cpp} (66%) rename ocs2_pipg/test/{testPIPGSolver.cpp => testPipgSolver.cpp} (67%) diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_pipg/CMakeLists.txt index 76253e25e..6ef99e922 100644 --- a/ocs2_pipg/CMakeLists.txt +++ b/ocs2_pipg/CMakeLists.txt @@ -41,9 +41,9 @@ include_directories( ) add_library(${PROJECT_NAME} - src/PIPG_Settings.cpp + src/PipgSettings.cpp + src/PipgSolver.cpp src/HelperFunctions.cpp - src/PIPG.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} @@ -84,7 +84,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ## Testing ## ############# catkin_add_gtest(test_${PROJECT_NAME} - test/testPIPGSolver.cpp + test/testPipgSolver.cpp test/testHelper.cpp ) target_link_libraries(test_${PROJECT_NAME} diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h b/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h deleted file mode 100644 index d1b028aab..000000000 --- a/ocs2_pipg/include/ocs2_pipg/PIPG_Settings.h +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#include <string> - -#include <ocs2_core/Types.h> - -namespace ocs2 { -namespace pipg { - -struct Settings { - /** Number of threads used in the multi-threading scheme. */ - size_t nThreads = 3; - /** Priority of threads used in the multi-threading scheme. */ - int threadPriority = 99; - /** Maximum number of iterations of PIPG. */ - size_t maxNumIterations = 3000; - /** Termination criteria. **/ - scalar_t absoluteTolerance = 1e-3; - scalar_t relativeTolerance = 1e-2; - /** Number of iterations between consecutive calculation of termination conditions. **/ - size_t checkTerminationInterval = 1; - /** This value determines to display the a summary log. */ - bool displayShortSummary = false; -}; - -Settings loadSettings(const std::string& filename, const std::string& fieldName = "pipg", bool verbose = true); - -} // namespace pipg -} // namespace ocs2 diff --git a/ocs2_pipg/include/ocs2_pipg/PipgSettings.h b/ocs2_pipg/include/ocs2_pipg/PipgSettings.h new file mode 100644 index 000000000..7497a0d1e --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/PipgSettings.h @@ -0,0 +1,58 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <string> + +#include <ocs2_core/Types.h> + +namespace ocs2 { +namespace pipg { + +struct Settings { + /** Number of threads used in the multi-threading scheme. */ + size_t nThreads = 3; + /** Priority of threads used in the multi-threading scheme. */ + int threadPriority = 99; + /** Maximum number of iterations of PIPG. */ + size_t maxNumIterations = 3000; + /** Termination criteria. **/ + scalar_t absoluteTolerance = 1e-3; + scalar_t relativeTolerance = 1e-2; + /** Number of iterations between consecutive calculation of termination conditions. **/ + size_t checkTerminationInterval = 1; + /** This value determines to display the a summary log. */ + bool displayShortSummary = false; +}; + +Settings loadSettings(const std::string& filename, const std::string& fieldName = "pipg", bool verbose = true); + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_pipg/include/ocs2_pipg/PIPG.h b/ocs2_pipg/include/ocs2_pipg/PipgSolver.h similarity index 53% rename from ocs2_pipg/include/ocs2_pipg/PIPG.h rename to ocs2_pipg/include/ocs2_pipg/PipgSolver.h index a33e8ed85..7aafe2d0e 100644 --- a/ocs2_pipg/include/ocs2_pipg/PIPG.h +++ b/ocs2_pipg/include/ocs2_pipg/PipgSolver.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <string> @@ -9,38 +38,22 @@ #include <ocs2_core/thread_support/ThreadPool.h> #include <ocs2_oc/oc_problem/OcpSize.h> -#include "ocs2_pipg/PIPG_Settings.h" +#include "ocs2_pipg/PipgSettings.h" +#include "ocs2_pipg/PipgSolverStatus.h" namespace ocs2 { -namespace pipg { -enum class SolverStatus { SUCCESS, MAX_ITER }; -inline std::string toString(SolverStatus s) { - switch (s) { - case SolverStatus::SUCCESS: - return std::string("SUCCESS"); - - case SolverStatus::MAX_ITER: - return std::string("MAX_ITER"); - - default: - throw std::invalid_argument("[pipg::toString] Invalid solver status."); - } -} -} // namespace pipg - -class Pipg { - public: - using OcpSize = ocs2::OcpSize; - using Settings = pipg::Settings; - using SolverStatus = ocs2::pipg::SolverStatus; +class PipgSolver { + public: /** - * @brief Construct a new Pipg with pipg setting object. + * @brief Construct a new PIPG with pipg setting object. * - * @param pipgSettings: pipg setting + * @param Settings: PIPG setting */ - explicit Pipg(pipg::Settings pipgSettings); - ~Pipg(); + explicit PipgSolver(pipg::Settings settings); + + /** Destructor */ + ~PipgSolver(); /** * @brief Solve generic QP problem. @@ -56,9 +69,9 @@ class Pipg { * @param result: Stacked result. * @return SolverStatus */ - SolverStatus solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, - const vector_t& g, const vector_t& EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, - vector_t& result); + pipg::SolverStatus solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, + const vector_t& g, const vector_t& EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, + vector_t& result); /** * @brief Solve Optimal Control type QP in parallel. @@ -77,11 +90,12 @@ class Pipg { * @param constraintsM For testing only. Can be removed. * @return SolverStatus */ - SolverStatus solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<ScalarFunctionQuadraticApproximation>& cost, - const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t& scalingVectors, - const vector_array_t* EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, - const ScalarFunctionQuadraticApproximation& costM, const VectorFunctionLinearApproximation& constraintsM); + pipg::SolverStatus solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints, + const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, + const scalar_t lambda, const scalar_t sigma, const ScalarFunctionQuadraticApproximation& costM, + const VectorFunctionLinearApproximation& constraintsM); void resize(const OcpSize& size); @@ -91,7 +105,7 @@ class Pipg { int getNumGeneralEqualityConstraints() const; - void getStackedSolution(vector_t& res) const; + void getStackedSolution(vector_t& res) const { packSolution(X_, U_, res); } void unpackSolution(const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const; @@ -105,7 +119,7 @@ class Pipg { scalar_t getTotalRunTimeInMilliseconds() const { return parallelizedQPTimer_.getTotalInMilliseconds(); } scalar_t getAverageRunTimeInMilliseconds() const { return parallelizedQPTimer_.getAverageInMilliseconds(); } - const Settings& settings() const { return pipgSettings_; } + const pipg::Settings& settings() const { return settings_; } const OcpSize& size() const { return ocpSize_; } ThreadPool& getThreadPool() { return threadPool_; } @@ -116,13 +130,11 @@ class Pipg { void verifyOcpSize(const OcpSize& ocpSize) const; - void runParallel(std::function<void(int)> taskFunction); + void runParallel(std::function<void(int)> taskFunction) { threadPool_.runParallel(std::move(taskFunction), settings().nThreads); } private: - const Settings pipgSettings_; - + const pipg::Settings settings_; ThreadPool threadPool_; - OcpSize ocpSize_; // Data buffer for parallelized QP diff --git a/ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h b/ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h new file mode 100644 index 000000000..02cf2ffce --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h @@ -0,0 +1,55 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <string> + +namespace ocs2 { +namespace pipg { + +enum class SolverStatus { + SUCCESS, + MAX_ITER, + UNDEFINED, +}; + +inline std::string toString(SolverStatus s) { + switch (s) { + case SolverStatus::SUCCESS: + return std::string("SUCCESS"); + case SolverStatus::MAX_ITER: + return std::string("MAX_ITER"); + default: + return std::string("UNDEFINED"); + } +} + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_pipg/src/HelperFunctions.cpp b/ocs2_pipg/src/HelperFunctions.cpp index 3ab596634..e158ee72a 100644 --- a/ocs2_pipg/src/HelperFunctions.cpp +++ b/ocs2_pipg/src/HelperFunctions.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_pipg/HelperFunctions.h" #include <atomic> diff --git a/ocs2_pipg/src/PIPG_Settings.cpp b/ocs2_pipg/src/PIPG_Settings.cpp deleted file mode 100644 index f5f46f203..000000000 --- a/ocs2_pipg/src/PIPG_Settings.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include "ocs2_pipg/PIPG_Settings.h" - -#include <iostream> - -#include <boost/property_tree/info_parser.hpp> -#include <boost/property_tree/ptree.hpp> - -#include <ocs2_core/misc/LoadData.h> - -namespace ocs2 { -namespace pipg { - -Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { - boost::property_tree::ptree pt; - boost::property_tree::read_info(filename, pt); - - Settings settings; - - if (verbose) { - std::cerr << "\n #### PIPG Settings: "; - std::cerr << "\n #### =============================================================================\n"; - } - - loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); - loadData::loadPtreeValue(pt, settings.threadPriority, fieldName + ".threadPriority", verbose); - - loadData::loadPtreeValue(pt, settings.maxNumIterations, fieldName + ".maxNumIterations", verbose); - loadData::loadPtreeValue(pt, settings.absoluteTolerance, fieldName + ".absoluteTolerance", verbose); - loadData::loadPtreeValue(pt, settings.relativeTolerance, fieldName + ".relativeTolerance", verbose); - - loadData::loadPtreeValue(pt, settings.checkTerminationInterval, fieldName + ".checkTerminationInterval", verbose); - loadData::loadPtreeValue(pt, settings.displayShortSummary, fieldName + ".displayShortSummary", verbose); - - if (verbose) { - std::cerr << " #### =============================================================================" << std::endl; - } - - return settings; -} - -} // namespace pipg -} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/src/PipgSettings.cpp b/ocs2_pipg/src/PipgSettings.cpp new file mode 100644 index 000000000..c162ec55d --- /dev/null +++ b/ocs2_pipg/src/PipgSettings.cpp @@ -0,0 +1,71 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_pipg/PipgSettings.h" + +#include <iostream> + +#include <boost/property_tree/info_parser.hpp> +#include <boost/property_tree/ptree.hpp> + +#include <ocs2_core/misc/LoadData.h> + +namespace ocs2 { +namespace pipg { + +Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { + boost::property_tree::ptree pt; + boost::property_tree::read_info(filename, pt); + + Settings settings; + + if (verbose) { + std::cerr << "\n #### PIPG Settings: "; + std::cerr << "\n #### =============================================================================\n"; + } + + loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); + loadData::loadPtreeValue(pt, settings.threadPriority, fieldName + ".threadPriority", verbose); + + loadData::loadPtreeValue(pt, settings.maxNumIterations, fieldName + ".maxNumIterations", verbose); + loadData::loadPtreeValue(pt, settings.absoluteTolerance, fieldName + ".absoluteTolerance", verbose); + loadData::loadPtreeValue(pt, settings.relativeTolerance, fieldName + ".relativeTolerance", verbose); + + loadData::loadPtreeValue(pt, settings.checkTerminationInterval, fieldName + ".checkTerminationInterval", verbose); + loadData::loadPtreeValue(pt, settings.displayShortSummary, fieldName + ".displayShortSummary", verbose); + + if (verbose) { + std::cerr << " #### =============================================================================" << std::endl; + } + + return settings; +} + +} // namespace pipg +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/src/PIPG.cpp b/ocs2_pipg/src/PipgSolver.cpp similarity index 66% rename from ocs2_pipg/src/PIPG.cpp rename to ocs2_pipg/src/PipgSolver.cpp index 0f3d6bca9..1a082c5d4 100644 --- a/ocs2_pipg/src/PIPG.cpp +++ b/ocs2_pipg/src/PipgSolver.cpp @@ -1,5 +1,33 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. -#include "ocs2_pipg/PIPG.h" +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_pipg/PipgSolver.h" #include <condition_variable> #include <iostream> @@ -8,16 +36,31 @@ namespace ocs2 { -Pipg::Pipg(pipg::Settings pipgSettings) - : pipgSettings_(pipgSettings), threadPool_(std::max(pipgSettings.nThreads, size_t(1)) - 1, pipgSettings.threadPriority) { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +PipgSolver::PipgSolver(pipg::Settings settings) + : settings_(std::move(settings)), threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { Eigen::initParallel(); -}; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +PipgSolver::~PipgSolver() { + if (settings().displayShortSummary) { + std::cerr << getBenchmarkingInformationDense() << std::endl; + } +} -Pipg::SolverStatus Pipg::solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, - const vector_t& g, const vector_t& EInv, const scalar_t mu, const scalar_t lambda, - const scalar_t sigma, vector_t& result) { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +pipg::SolverStatus PipgSolver::solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, + const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, + const scalar_t mu, const scalar_t lambda, const scalar_t sigma, vector_t& result) { if (const int N = ocpSize_.numStages < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + throw std::runtime_error("[PipgSolver::solveDenseQP] The number of stages cannot be less than 1."); } denseQPTimer_.startTimer(); @@ -83,7 +126,7 @@ Pipg::SolverStatus Pipg::solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, co } denseQPTimer_.endTimer(); - Pipg::SolverStatus status = isConverged ? Pipg::SolverStatus::SUCCESS : Pipg::SolverStatus::MAX_ITER; + pipg::SolverStatus status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; if (settings().displayShortSummary) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; std::cerr << "\n++++++++++++++ PIPG-DenseQP " << pipg::toString(status) << " +++++++++++++"; @@ -97,21 +140,25 @@ Pipg::SolverStatus Pipg::solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, co result.swap(z); return status; -}; +} -Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<ScalarFunctionQuadraticApproximation>& cost, - const std::vector<VectorFunctionLinearApproximation>* constraints, - const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma, const ScalarFunctionQuadraticApproximation& costM, - const VectorFunctionLinearApproximation& constraintsM) { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +pipg::SolverStatus PipgSolver::solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints, + const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, + const scalar_t lambda, const scalar_t sigma, + const ScalarFunctionQuadraticApproximation& costM, + const VectorFunctionLinearApproximation& constraintsM) { verifySizes(dynamics, cost, constraints); const int N = ocpSize_.numStages; if (N < 1) { - throw std::runtime_error("[PIPG: solveOCPInParallel] The number of stages cannot be less than 1."); + throw std::runtime_error("[PipgSolver::solveOCPInParallel] The number of stages cannot be less than 1."); } if (scalingVectors.size() != N) { - throw std::runtime_error("[PIPG: solveOCPInParallel] The size of scalingVectors doesn't match the number of stage."); + throw std::runtime_error("[PipgSolver::solveOCPInParallel] The size of scalingVectors doesn't match the number of stage."); } // Disable Eigen's internal multithreading @@ -373,7 +420,7 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect parallelizedQPTimer_.endTimer(); - Pipg::SolverStatus status = isConverged ? Pipg::SolverStatus::SUCCESS : Pipg::SolverStatus::MAX_ITER; + pipg::SolverStatus status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; if (settings().displayShortSummary) { scalar_t totalTasks = std::accumulate(threadsWorkloadCounter.cbegin(), threadsWorkloadCounter.cend(), 0.0); std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; @@ -395,7 +442,10 @@ Pipg::SolverStatus Pipg::solveOCPInParallel(const vector_t& x0, std::vector<Vect return status; }; -void Pipg::resize(const OcpSize& ocpSize) { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::resize(const OcpSize& ocpSize) { if (ocpSize_ == ocpSize) { return; } @@ -417,12 +467,11 @@ void Pipg::resize(const OcpSize& ocpSize) { WNew_.resize(N); } -void Pipg::getStackedSolution(vector_t& res) const { - packSolution(X_, U_, res); -} - -void Pipg::unpackSolution(const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, - vector_array_t& uTrajectory) const { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::unpackSolution(const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, + vector_array_t& uTrajectory) const { const int N = ocpSize_.numStages; xTrajectory.resize(N + 1); uTrajectory.resize(N); @@ -440,7 +489,10 @@ void Pipg::unpackSolution(const vector_t& stackedSolution, const vector_t x0, ve } } -void Pipg::packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) const { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) const { stackedSolution.resize(getNumDecisionVariables()); int curRow = 0; @@ -452,10 +504,13 @@ void Pipg::packSolution(const vector_array_t& xTrajectory, const vector_array_t& } } -void Pipg::descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { const int N = ocpSize_.numStages; if (D.size() != xTrajectory.size() + uTrajectory.size() - 1) { - throw std::runtime_error("[PIPG]::descaleSolution - Size doesn't match."); + throw std::runtime_error("[PipgSolver::descaleSolution] - Size doesn't match."); } for (int k = 0; k < N; k++) { @@ -464,7 +519,10 @@ void Pipg::descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, } } -void Pipg::getStateInputTrajectoriesSolution(vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::getStateInputTrajectoriesSolution(vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { xTrajectory.resize(X_.size()); uTrajectory.resize(U_.size()); @@ -472,56 +530,64 @@ void Pipg::getStateInputTrajectoriesSolution(vector_array_t& xTrajectory, vector std::copy(U_.begin(), U_.end(), uTrajectory.begin()); } -void Pipg::runParallel(std::function<void(int)> taskFunction) { - threadPool_.runParallel(std::move(taskFunction), settings().nThreads); -} - -void Pipg::verifySizes(const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<ScalarFunctionQuadraticApproximation>& cost, - const std::vector<VectorFunctionLinearApproximation>* constraints) const { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::verifySizes(const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints) const { if (dynamics.size() != ocpSize_.numStages) { - throw std::runtime_error("[PIPG] Inconsistent size of dynamics: " + std::to_string(dynamics.size()) + " with " + + throw std::runtime_error("[PipgSolver::verifySizes] Inconsistent size of dynamics: " + std::to_string(dynamics.size()) + " with " + std::to_string(ocpSize_.numStages) + " number of stages."); } if (cost.size() != ocpSize_.numStages + 1) { - throw std::runtime_error("[PIPG] Inconsistent size of cost: " + std::to_string(cost.size()) + " with " + + throw std::runtime_error("[PipgSolver::verifySizes] Inconsistent size of cost: " + std::to_string(cost.size()) + " with " + std::to_string(ocpSize_.numStages + 1) + " nodes."); } if (constraints != nullptr) { if (constraints->size() != ocpSize_.numStages + 1) { - throw std::runtime_error("[PIPG] Inconsistent size of constraints: " + std::to_string(constraints->size()) + " with " + - std::to_string(ocpSize_.numStages + 1) + " nodes."); + throw std::runtime_error("[PipgSolver::verifySizes] Inconsistent size of constraints: " + std::to_string(constraints->size()) + + " with " + std::to_string(ocpSize_.numStages + 1) + " nodes."); } } } -void Pipg::verifyOcpSize(const OcpSize& ocpSize) const { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::verifyOcpSize(const OcpSize& ocpSize) const { auto isNotEmpty = [](const std::vector<int>& v) { return std::any_of(v.cbegin(), v.cend(), [](int s) { return s != 0; }); }; if (isNotEmpty(ocpSize.numInputBoxConstraints)) { - throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support input box constraints."); + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support input box constraints."); } if (isNotEmpty(ocpSize.numStateBoxConstraints)) { - throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support state box constraints."); + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support state box constraints."); } if (isNotEmpty(ocpSize.numInputBoxSlack)) { - throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support input slack variables."); + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support input slack variables."); } if (isNotEmpty(ocpSize.numStateBoxSlack)) { - throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support state slack variables."); + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support state slack variables."); } if (isNotEmpty(ocpSize.numIneqSlack)) { - throw std::runtime_error("[Pipg::verifyOcpSize] PIPG solver does not support inequality slack variables."); + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support inequality slack variables."); } } -int Pipg::getNumGeneralEqualityConstraints() const { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +int PipgSolver::getNumGeneralEqualityConstraints() const { const auto totalNumberOfGeneralEqualityConstraints = std::accumulate(ocpSize_.numIneqConstraints.begin(), ocpSize_.numIneqConstraints.end(), 0); return totalNumberOfGeneralEqualityConstraints; } -std::string Pipg::getBenchmarkingInformationDense() const { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::string PipgSolver::getBenchmarkingInformationDense() const { const auto step1v = vComputationTimer_.getTotalInMilliseconds(); const auto step2z = zComputationTimer_.getTotalInMilliseconds(); const auto step3w = wComputationTimer_.getTotalInMilliseconds(); @@ -547,9 +613,4 @@ std::string Pipg::getBenchmarkingInformationDense() const { return infoStream.str(); } -Pipg::~Pipg() { - if (settings().displayShortSummary) { - std::cerr << getBenchmarkingInformationDense() << std::endl; - } -} } // namespace ocs2 diff --git a/ocs2_pipg/test/testHelper.cpp b/ocs2_pipg/test/testHelper.cpp index c931ca244..9d8ad3c74 100644 --- a/ocs2_pipg/test/testHelper.cpp +++ b/ocs2_pipg/test/testHelper.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <gtest/gtest.h> #include "ocs2_pipg/HelperFunctions.h" diff --git a/ocs2_pipg/test/testPIPGSolver.cpp b/ocs2_pipg/test/testPipgSolver.cpp similarity index 67% rename from ocs2_pipg/test/testPIPGSolver.cpp rename to ocs2_pipg/test/testPipgSolver.cpp index 4b892d0a9..fc13970be 100644 --- a/ocs2_pipg/test/testPIPGSolver.cpp +++ b/ocs2_pipg/test/testPipgSolver.cpp @@ -1,6 +1,35 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <gtest/gtest.h> -#include "ocs2_pipg/PIPG.h" +#include "ocs2_pipg/PipgSolver.h" #include <Eigen/Sparse> @@ -32,7 +61,7 @@ class PIPGSolverTest : public testing::Test { static constexpr size_t numConstraints = N_ * (nx_ + nc_); static constexpr bool verbose = true; - PIPGSolverTest() : pipgSolver(configurePipg(8, 30000, 1e-10, 1e-3, verbose)) { + PIPGSolverTest() : solver(configurePipg(8, 30000, 1e-10, 1e-3, verbose)) { srand(10); // Construct OCP problem @@ -46,9 +75,9 @@ class PIPGSolverTest : public testing::Test { costArray.push_back(ocs2::getRandomCost(nx_, nu_)); constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); - pipgSolver.resize(ocs2::extractSizesFromProblem(dynamicsArray, costArray, &constraintsArray)); - ocs2::getCostMatrix(pipgSolver.size(), x0, costArray, costApproximation); - ocs2::getConstraintMatrix(pipgSolver.size(), x0, dynamicsArray, nullptr, nullptr, constraintsApproximation); + solver.resize(ocs2::extractSizesFromProblem(dynamicsArray, costArray, &constraintsArray)); + ocs2::getCostMatrix(solver.size(), x0, costArray, costApproximation); + ocs2::getConstraintMatrix(solver.size(), x0, dynamicsArray, nullptr, nullptr, constraintsApproximation); } ocs2::vector_t x0; @@ -58,7 +87,7 @@ class PIPGSolverTest : public testing::Test { std::vector<ocs2::ScalarFunctionQuadraticApproximation> costArray; std::vector<ocs2::VectorFunctionLinearApproximation> constraintsArray; - ocs2::Pipg pipgSolver; + ocs2::PipgSolver solver; }; constexpr size_t PIPGSolverTest::numDecisionVariables; @@ -79,16 +108,16 @@ TEST_F(PIPGSolverTest, correctness) { ocs2::scalar_t sigma = svdGTG.singularValues()(0); ocs2::vector_t primalSolutionPIPG; - pipgSolver.solveDenseQP(costApproximation.dfdxx.sparseView(), costApproximation.dfdx, constraintsApproximation.dfdx.sparseView(), - constraintsApproximation.f, ocs2::vector_t::Ones(pipgSolver.getNumDynamicsConstraints()), mu, lambda, sigma, + solver.solveDenseQP(costApproximation.dfdxx.sparseView(), costApproximation.dfdx, constraintsApproximation.dfdx.sparseView(), + constraintsApproximation.f, ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, primalSolutionPIPG); ocs2::vector_t primalSolutionPIPGParallel; ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); - pipgSolver.solveOCPInParallel(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma, costApproximation, + solver.solveOCPInParallel(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma, costApproximation, constraintsApproximation); - pipgSolver.getStackedSolution(primalSolutionPIPGParallel); + solver.getStackedSolution(primalSolutionPIPGParallel); auto calculateConstraintViolation = [&](const ocs2::vector_t& sol) -> ocs2::scalar_t { return (constraintsApproximation.dfdx * sol - constraintsApproximation.f).cwiseAbs().maxCoeff(); @@ -128,24 +157,24 @@ TEST_F(PIPGSolverTest, correctness) { << "constraint-violation: " << PIPGParallelCConstraintViolation << "\n\n" << std::endl; } - EXPECT_TRUE(primalSolutionQP.isApprox(primalSolutionPIPG, pipgSolver.settings().absoluteTolerance * 10.0)) + EXPECT_TRUE(primalSolutionQP.isApprox(primalSolutionPIPG, solver.settings().absoluteTolerance * 10.0)) << "Inf-norm of (QP - PIPG): " << (primalSolutionQP - primalSolutionPIPG).cwiseAbs().maxCoeff(); - EXPECT_TRUE(primalSolutionPIPGParallel.isApprox(primalSolutionPIPG, pipgSolver.settings().absoluteTolerance * 10.0)) + EXPECT_TRUE(primalSolutionPIPGParallel.isApprox(primalSolutionPIPG, solver.settings().absoluteTolerance * 10.0)) << "Inf-norm of (PIPG - PIPGParallel): " << (primalSolutionPIPGParallel - primalSolutionPIPG).cwiseAbs().maxCoeff(); // Threshold is the (absoluteTolerance) * (2-Norm of the hessian H)[lambda] - EXPECT_TRUE(std::abs(QPCost - PIPGCost) < pipgSolver.settings().absoluteTolerance * lambda) + EXPECT_TRUE(std::abs(QPCost - PIPGCost) < solver.settings().absoluteTolerance * lambda) << "Absolute diff is [" << std::abs(QPCost - PIPGCost) << "] which is larger than [" - << pipgSolver.settings().absoluteTolerance * lambda << "]"; + << solver.settings().absoluteTolerance * lambda << "]"; - EXPECT_TRUE(std::abs(PIPGParallelCost - PIPGCost) < pipgSolver.settings().absoluteTolerance) + EXPECT_TRUE(std::abs(PIPGParallelCost - PIPGCost) < solver.settings().absoluteTolerance) << "Absolute diff is [" << std::abs(PIPGParallelCost - PIPGCost) << "] which is larger than [" - << pipgSolver.settings().absoluteTolerance * lambda << "]"; + << solver.settings().absoluteTolerance * lambda << "]"; - ASSERT_TRUE(std::abs(PIPGConstraintViolation) < pipgSolver.settings().absoluteTolerance); - EXPECT_TRUE(std::abs(QPConstraintViolation - PIPGConstraintViolation) < pipgSolver.settings().absoluteTolerance * 10.0); - EXPECT_TRUE(std::abs(PIPGParallelCConstraintViolation - PIPGConstraintViolation) < pipgSolver.settings().absoluteTolerance * 10.0); + ASSERT_TRUE(std::abs(PIPGConstraintViolation) < solver.settings().absoluteTolerance); + EXPECT_TRUE(std::abs(QPConstraintViolation - PIPGConstraintViolation) < solver.settings().absoluteTolerance * 10.0); + EXPECT_TRUE(std::abs(PIPGParallelCConstraintViolation - PIPGConstraintViolation) < solver.settings().absoluteTolerance * 10.0); } TEST_F(PIPGSolverTest, descaleSolution) { @@ -165,13 +194,13 @@ TEST_F(PIPGSolverTest, descaleSolution) { curRow += v.size(); } ocs2::vector_t packedSolution; - pipgSolver.packSolution(x, u, packedSolution); + solver.packSolution(x, u, packedSolution); packedSolution.array() *= DStacked.array(); ocs2::vector_t packedSolutionMy; - pipgSolver.descaleSolution(D, x, u); - pipgSolver.packSolution(x, u, packedSolutionMy); + solver.descaleSolution(D, x, u); + solver.packSolution(x, u, packedSolutionMy); EXPECT_TRUE(packedSolutionMy.isApprox(packedSolution)) << std::setprecision(6) << "DescaledSolution: \n" << packedSolutionMy.transpose() << "\nIt should be \n" << packedSolution.transpose(); From 043b4d84ad032c60fbb0718ff146c39d8bd14c8f Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 2 Dec 2022 16:23:12 +0100 Subject: [PATCH 383/512] fixing settings --- ocs2_pipg/include/ocs2_pipg/PipgSettings.h | 4 ++++ ocs2_pipg/src/PipgSettings.cpp | 3 +++ 2 files changed, 7 insertions(+) diff --git a/ocs2_pipg/include/ocs2_pipg/PipgSettings.h b/ocs2_pipg/include/ocs2_pipg/PipgSettings.h index 7497a0d1e..71401dc0c 100644 --- a/ocs2_pipg/include/ocs2_pipg/PipgSettings.h +++ b/ocs2_pipg/include/ocs2_pipg/PipgSettings.h @@ -48,6 +48,10 @@ struct Settings { scalar_t relativeTolerance = 1e-2; /** Number of iterations between consecutive calculation of termination conditions. **/ size_t checkTerminationInterval = 1; + /** Number of pre-conditioning run. **/ + int numScaling = 3; + /** The static lower bound of the cost hessian H. **/ + scalar_t lowerBoundH = 5e-6; /** This value determines to display the a summary log. */ bool displayShortSummary = false; }; diff --git a/ocs2_pipg/src/PipgSettings.cpp b/ocs2_pipg/src/PipgSettings.cpp index c162ec55d..85bdb2257 100644 --- a/ocs2_pipg/src/PipgSettings.cpp +++ b/ocs2_pipg/src/PipgSettings.cpp @@ -57,6 +57,9 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, loadData::loadPtreeValue(pt, settings.absoluteTolerance, fieldName + ".absoluteTolerance", verbose); loadData::loadPtreeValue(pt, settings.relativeTolerance, fieldName + ".relativeTolerance", verbose); + loadData::loadPtreeValue(pt, settings.numScaling, fieldName + ".numScaling", verbose); + loadData::loadPtreeValue(pt, settings.lowerBoundH, fieldName + ".lowerBoundH", verbose); + loadData::loadPtreeValue(pt, settings.checkTerminationInterval, fieldName + ".checkTerminationInterval", verbose); loadData::loadPtreeValue(pt, settings.displayShortSummary, fieldName + ".displayShortSummary", verbose); From 90ba32be2378e67f7ecc198c925254dbcf2a6b95 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 2 Dec 2022 16:25:07 +0100 Subject: [PATCH 384/512] clang tidy --- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 9c0eb3b84..3b52c2e86 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -33,11 +33,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <numeric> #include <ocs2_core/penalties/penalties/RelaxedBarrierPenalty.h> -#include <ocs2_oc/oc_problem/OcpSize.h> #include <ocs2_oc/multiple_shooting/Helpers.h> #include <ocs2_oc/multiple_shooting/Initialization.h> #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> #include <ocs2_oc/multiple_shooting/Transcription.h> +#include <ocs2_oc/oc_problem/OcpSize.h> namespace ocs2 { From c85412abf581e62804f93f5ed9bc227a6e708aa0 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Sun, 4 Dec 2022 14:39:23 +0100 Subject: [PATCH 385/512] fixes to sqp --- .../ocs2_sqp/include/ocs2_sqp/SqpSolver.h | 8 ++--- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 29 +++++++++++-------- 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index 432ccc8f3..98d160e61 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -150,13 +150,16 @@ class SqpSolver : public SolverBase { sqp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const; // Problem definition - sqp::Settings settings_; + const sqp::Settings settings_; DynamicsDiscretizer discretizer_; DynamicsSensitivityDiscretizer sensitivityDiscretizer_; std::vector<OptimalControlProblem> ocpDefinitions_; std::unique_ptr<Initializer> initializerPtr_; FilterLinesearch filterLinesearch_; + // Solver interface + HpipmInterface hpipmInterface_; + // Threading ThreadPool threadPool_; @@ -166,9 +169,6 @@ class SqpSolver : public SolverBase { // Value function in absolute state coordinates (without the constant value) std::vector<ScalarFunctionQuadraticApproximation> valueFunction_; - // Solver interface - HpipmInterface hpipmInterface_; - // LQ approximation std::vector<VectorFunctionLinearApproximation> dynamics_; std::vector<ScalarFunctionQuadraticApproximation> cost_; diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 3b52c2e86..9dd88cf45 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -32,7 +32,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <iostream> #include <numeric> -#include <ocs2_core/penalties/penalties/RelaxedBarrierPenalty.h> #include <ocs2_oc/multiple_shooting/Helpers.h> #include <ocs2_oc/multiple_shooting/Initialization.h> #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> @@ -41,24 +40,29 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +namespace { +sqp::Settings rectifySettings(const OptimalControlProblem& ocp, sqp::Settings&& settings) { + // True does not make sense if there are no constraints. + if (ocp.equalityConstraintPtr->empty()) { + settings.projectStateInputEqualityConstraints = false; + } + return settings; +} +} // anonymous namespace + SqpSolver::SqpSolver(sqp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) - : SolverBase(), - settings_(std::move(settings)), - hpipmInterface_(OcpSize(), settings.hpipmSettings), + : settings_(rectifySettings(optimalControlProblem, std::move(settings))), + hpipmInterface_(OcpSize(), settings_.hpipmSettings), threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { Eigen::setNbThreads(1); // No multithreading within Eigen. Eigen::initParallel(); - if (optimalControlProblem.equalityConstraintPtr->empty()) { - settings_.projectStateInputEqualityConstraints = false; // True does not make sense if there are no constraints. - } - // Dynamics discretization - discretizer_ = selectDynamicsDiscretization(settings.integratorType); - sensitivityDiscretizer_ = selectDynamicsSensitivityDiscretization(settings.integratorType); + discretizer_ = selectDynamicsDiscretization(settings_.integratorType); + sensitivityDiscretizer_ = selectDynamicsSensitivityDiscretization(settings_.integratorType); // Clone objects to have one for each worker - for (int w = 0; w < settings.nThreads; w++) { + for (int w = 0; w < settings_.nThreads; w++) { ocpDefinitions_.push_back(optimalControlProblem); } @@ -296,6 +300,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; PerformanceIndex workerPerformance; // Accumulate performance in local variable + const bool projectStateInputEqualityConstraints = settings_.projectStateInputEqualityConstraints; int i = timeIndex++; while (i < N) { @@ -315,7 +320,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); - if (settings_.projectStateInputEqualityConstraints) { + if (projectStateInputEqualityConstraints) { constexpr bool extractPseudoInverse = false; multiple_shooting::projectTranscription(result, extractPseudoInverse); } From 9e377b26a5abfdbbcd1e53cbbc8fabbb525e63df Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Sun, 4 Dec 2022 14:49:15 +0100 Subject: [PATCH 386/512] fixes based on sqp and oc --- ocs2_pipg/CMakeLists.txt | 35 +- .../{HelperFunctions.h => Helpers.h} | 0 ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h | 29 ++ .../include/ocs2_pipg/mpc/PipgMpcSolver.h | 99 ++++-- .../src/{HelperFunctions.cpp => Helpers.cpp} | 2 +- ocs2_pipg/src/mpc/PipgMpcSolver.cpp | 335 +++++++----------- .../test/{testHelper.cpp => testHelpers.cpp} | 4 +- ocs2_pipg/test/testPipgMpc.cpp | 2 +- ocs2_pipg/test/testPipgSolver.cpp | 10 +- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 7 +- 10 files changed, 272 insertions(+), 251 deletions(-) rename ocs2_pipg/include/ocs2_pipg/{HelperFunctions.h => Helpers.h} (100%) rename ocs2_pipg/src/{HelperFunctions.cpp => Helpers.cpp} (99%) rename ocs2_pipg/test/{testHelper.cpp => testHelpers.cpp} (99%) diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_pipg/CMakeLists.txt index 70b6e22b1..dfa42f41f 100644 --- a/ocs2_pipg/CMakeLists.txt +++ b/ocs2_pipg/CMakeLists.txt @@ -3,17 +3,20 @@ project(ocs2_pipg) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_core - ocs2_oc - ocs2_sqp ocs2_mpc + ocs2_oc ocs2_qp_solver + ocs2_sqp ) find_package(catkin REQUIRED COMPONENTS ${CATKIN_PACKAGE_DEPENDENCIES} ) -find_package(Boost REQUIRED) +find_package(Boost REQUIRED COMPONENTS + system + filesystem +) find_package(Eigen3 3.3 REQUIRED NO_MODULE) @@ -45,12 +48,17 @@ include_directories( add_library(${PROJECT_NAME} src/PipgSettings.cpp src/PipgSolver.cpp - src/HelperFunctions.cpp + src/Helpers.cpp src/mpc/PipgMpcSolver.cpp ) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} + ${Boost_LIBRARIES} ) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) ######################### ### CLANG TOOLING ### @@ -59,13 +67,9 @@ find_package(cmake_clang_tools QUIET) if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS - ${CMAKE_CURRENT_SOURCE_DIR}/src - ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS - ${CMAKE_CURRENT_SOURCE_DIR}/include + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR ) endif(cmake_clang_tools_FOUND) @@ -86,9 +90,13 @@ install(DIRECTORY include/${PROJECT_NAME}/ ############# ## Testing ## ############# + catkin_add_gtest(test_${PROJECT_NAME} + test/testHelpers.cpp test/testPipgSolver.cpp - test/testHelper.cpp +) +add_dependencies(test_${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} @@ -99,6 +107,9 @@ target_link_libraries(test_${PROJECT_NAME} catkin_add_gtest(test_pipg_mpc test/testPipgMpc.cpp ) +add_dependencies(test_pipg_mpc + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(test_pipg_mpc ${PROJECT_NAME} ${catkin_LIBRARIES} diff --git a/ocs2_pipg/include/ocs2_pipg/HelperFunctions.h b/ocs2_pipg/include/ocs2_pipg/Helpers.h similarity index 100% rename from ocs2_pipg/include/ocs2_pipg/HelperFunctions.h rename to ocs2_pipg/include/ocs2_pipg/Helpers.h diff --git a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h index 4e6917758..6c8c3a21e 100644 --- a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h +++ b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include "ocs2_pipg/mpc/PipgMpcSolver.h" diff --git a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h index 12a19f666..157769ed0 100644 --- a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h +++ b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h @@ -1,18 +1,48 @@ -#pragma once +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ -#include "ocs2_pipg/PIPG.h" +#pragma once #include <ocs2_core/initialization/Initializer.h> #include <ocs2_core/integration/SensitivityIntegrator.h> #include <ocs2_core/misc/Benchmark.h> #include <ocs2_core/thread_support/ThreadPool.h> +#include <ocs2_oc/oc_data/TimeDiscretization.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> #include <ocs2_oc/oc_solver/SolverBase.h> +#include <ocs2_oc/search_strategy/FilterLinesearch.h> + +#include <ocs2_sqp/SqpSettings.h> +#include <ocs2_sqp/SqpSolverStatus.h> -#include <ocs2_sqp/MultipleShootingSettings.h> -#include <ocs2_sqp/MultipleShootingSolverStatus.h> -#include <ocs2_sqp/TimeDiscretization.h> +#include "ocs2_pipg/PipgSolver.h" namespace ocs2 { @@ -25,7 +55,7 @@ class PipgMpcSolver : public SolverBase { * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ - PipgMpcSolver(multiple_shooting::Settings sqpSettings, pipg::Settings pipgSettings, const OptimalControlProblem& optimalControlProblem, + PipgMpcSolver(sqp::Settings sqpSettings, pipg::Settings pipgSettings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); ~PipgMpcSolver() override; @@ -36,8 +66,16 @@ class PipgMpcSolver : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } + const DualSolution& getDualSolution() const override { throw std::runtime_error("[SqpSolver] getDualSolution() not available yet."); } + + const ProblemMetrics& getSolutionMetrics() const override { + throw std::runtime_error("[SqpSolver] getSolutionMetrics() not available yet."); + } + size_t getNumIterations() const override { return totalNumIterations_; } + const OptimalControlProblem& getOptimalControlProblem() const override { return ocpDefinitions_.front(); } + const PerformanceIndex& getPerformanceIndeces() const override { return getIterationsLog().back(); }; const std::vector<PerformanceIndex>& getIterationsLog() const override; @@ -54,6 +92,10 @@ class PipgMpcSolver : public SolverBase { throw std::runtime_error("[PipgMpcSolver] getStateInputEqualityConstraintLagrangian() not available yet."); } + MultiplierCollection getIntermediateDualSolution(scalar_t time) const override { + throw std::runtime_error("[SqpSolver] getIntermediateDualSolution() not available yet."); + } + private: void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override; @@ -65,6 +107,16 @@ class PipgMpcSolver : public SolverBase { } } + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) override { + // Copy all except the controller + primalSolution_.timeTrajectory_ = primalSolution.timeTrajectory_; + primalSolution_.stateTrajectory_ = primalSolution.stateTrajectory_; + primalSolution_.inputTrajectory_ = primalSolution.inputTrajectory_; + primalSolution_.postEventIndices_ = primalSolution.postEventIndices_; + primalSolution_.modeSchedule_ = primalSolution.modeSchedule_; + runImpl(initTime, initState, finalTime); + } + /** Run a task in parallel with settings.nThreads */ void runParallel(std::function<void(int)> taskFunction); @@ -73,10 +125,6 @@ class PipgMpcSolver : public SolverBase { std::string getBenchmarkingInformationPIPG() const; - /** Initializes for the state-input trajectories */ - void initializeStateInputTrajectories(const vector_t& initState, const std::vector<AnnotatedTime>& timeDiscretization, - vector_array_t& stateTrajectory, vector_array_t& inputTrajectory); - /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, const vector_array_t& u); @@ -93,30 +141,26 @@ class PipgMpcSolver : public SolverBase { }; OcpSubproblemSolution getOCPSolution(const vector_t& delta_x0); - /** Set up the primal solution based on the optimized state and input trajectories */ - void setPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); - - /** Compute 2-norm of the trajectory: sqrt(sum_i v[i]^2) */ - static scalar_t trajectoryNorm(const vector_array_t& v); - - /** Compute total constraint violation */ - scalar_t totalConstraintViolation(const PerformanceIndex& performance) const; + /** Constructs the primal solution based on the optimized state and input trajectories */ + PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ - multiple_shooting::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, - const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, - vector_array_t& u); + sqp::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, + const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u); /** Determine convergence after a step */ - multiple_shooting::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, - const multiple_shooting::StepInfo& stepInfo) const; + sqp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const; // Problem definition - multiple_shooting::Settings settings_; + const sqp::Settings settings_; DynamicsDiscretizer discretizer_; DynamicsSensitivityDiscretizer sensitivityDiscretizer_; std::vector<OptimalControlProblem> ocpDefinitions_; std::unique_ptr<Initializer> initializerPtr_; + FilterLinesearch filterLinesearch_; + + // Solver interface + PipgSolver pipgSolver_; // Threading ThreadPool threadPool_; @@ -124,14 +168,13 @@ class PipgMpcSolver : public SolverBase { // Solution PrimalSolution primalSolution_; - // Solver interface - Pipg pipgSolver_; - // LQ approximation std::vector<VectorFunctionLinearApproximation> dynamics_; std::vector<ScalarFunctionQuadraticApproximation> cost_; - std::vector<VectorFunctionLinearApproximation> constraints_; std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + std::vector<VectorFunctionLinearApproximation> stateInputEqConstraints_; + std::vector<VectorFunctionLinearApproximation> stateIneqConstraints_; + std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; // Iteration performance log std::vector<PerformanceIndex> performanceIndeces_; diff --git a/ocs2_pipg/src/HelperFunctions.cpp b/ocs2_pipg/src/Helpers.cpp similarity index 99% rename from ocs2_pipg/src/HelperFunctions.cpp rename to ocs2_pipg/src/Helpers.cpp index e158ee72a..eee4c1cb1 100644 --- a/ocs2_pipg/src/HelperFunctions.cpp +++ b/ocs2_pipg/src/Helpers.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_pipg/HelperFunctions.h" +#include "ocs2_pipg/Helpers.h" #include <atomic> #include <numeric> diff --git a/ocs2_pipg/src/mpc/PipgMpcSolver.cpp b/ocs2_pipg/src/mpc/PipgMpcSolver.cpp index a52da7970..d86f7077a 100644 --- a/ocs2_pipg/src/mpc/PipgMpcSolver.cpp +++ b/ocs2_pipg/src/mpc/PipgMpcSolver.cpp @@ -1,23 +1,62 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_pipg/mpc/PipgMpcSolver.h" #include <iostream> #include <numeric> -#include <ocs2_core/control/FeedforwardController.h> +#include <ocs2_oc/multiple_shooting/Helpers.h> +#include <ocs2_oc/multiple_shooting/Initialization.h> +#include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> +#include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/pre_condition/Scaling.h> -#include <ocs2_sqp/MultipleShootingInitialization.h> -#include <ocs2_sqp/MultipleShootingTranscription.h> -#include "ocs2_pipg/HelperFunctions.h" +#include "ocs2_pipg/Helpers.h" namespace ocs2 { -PipgMpcSolver::PipgMpcSolver(multiple_shooting::Settings sqpSettings, pipg::Settings pipgSettings, - const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) - : SolverBase(), - settings_(std::move(sqpSettings)), - threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority), - pipgSolver_(pipgSettings) { +namespace { +sqp::Settings rectifySettings(const OptimalControlProblem& ocp, sqp::Settings&& settings) { + // True does not make sense if there are no constraints. + if (ocp.equalityConstraintPtr->empty()) { + settings.projectStateInputEqualityConstraints = false; + } + return settings; +} +} // anonymous namespace + +PipgMpcSolver::PipgMpcSolver(sqp::Settings settings, pipg::Settings pipgSettings, const OptimalControlProblem& optimalControlProblem, + const Initializer& initializer) + : settings_(rectifySettings(optimalControlProblem, std::move(settings))), + pipgSolver_(pipgSettings), + threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { Eigen::setNbThreads(1); // No multithreading within Eigen. Eigen::initParallel(); @@ -33,11 +72,33 @@ PipgMpcSolver::PipgMpcSolver(multiple_shooting::Settings sqpSettings, pipg::Sett // Operating points initializerPtr_.reset(initializer.clone()); - if (optimalControlProblem.equalityConstraintPtr->empty()) { - settings_.projectStateInputEqualityConstraints = false; // True does not make sense if there are no constraints. + // Linesearch + filterLinesearch_.g_max = settings_.g_max; + filterLinesearch_.g_min = settings_.g_min; + filterLinesearch_.gamma_c = settings_.gamma_c; + filterLinesearch_.armijoFactor = settings_.armijoFactor; +} + +PipgMpcSolver::~PipgMpcSolver() { + if (settings_.printSolverStatistics) { + std::cerr << getBenchmarkingInformationPIPG() << "\n" << getBenchmarkingInformation() << std::endl; } } +void PipgMpcSolver::reset() { + // Clear solution + primalSolution_ = PrimalSolution(); + performanceIndeces_.clear(); + + // reset timers + numProblems_ = 0; + totalNumIterations_ = 0; + linearQuadraticApproximationTimer_.reset(); + solveQpTimer_.reset(); + linesearchTimer_.reset(); + computeControllerTimer_.reset(); +} + std::string PipgMpcSolver::getBenchmarkingInformationPIPG() const { const auto GGTMultiplication = GGTMultiplication_.getTotalInMilliseconds(); const auto preConditioning = preConditioning_.getTotalInMilliseconds(); @@ -67,26 +128,6 @@ std::string PipgMpcSolver::getBenchmarkingInformationPIPG() const { return infoStream.str(); } -PipgMpcSolver::~PipgMpcSolver() { - if (settings_.printSolverStatistics) { - std::cerr << getBenchmarkingInformationPIPG() << "\n" << getBenchmarkingInformation() << std::endl; - } -} - -void PipgMpcSolver::reset() { - // Clear solution - primalSolution_ = PrimalSolution(); - performanceIndeces_.clear(); - - // reset timers - numProblems_ = 0; - totalNumIterations_ = 0; - linearQuadraticApproximationTimer_.reset(); - solveQpTimer_.reset(); - linesearchTimer_.reset(); - computeControllerTimer_.reset(); -} - std::string PipgMpcSolver::getBenchmarkingInformation() const { const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); @@ -124,7 +165,7 @@ const std::vector<PerformanceIndex>& PipgMpcSolver::getIterationsLog() const { void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; - std::cerr << "\n+++++++++++++ SQP solver is initialized ++++++++++++++"; + std::cerr << "\n+++++++++++++ PIPG solver is initialized +++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; } @@ -132,24 +173,24 @@ void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar const auto& eventTimes = this->getReferenceManager().getModeSchedule().eventTimes; const auto timeDiscretization = timeDiscretizationWithEvents(initTime, finalTime, settings_.dt, eventTimes); - // Initialize the state and input - vector_array_t x, u; - initializeStateInputTrajectories(initState, timeDiscretization, x, u); - // Initialize references for (auto& ocpDefinition : ocpDefinitions_) { const auto& targetTrajectories = this->getReferenceManager().getTargetTrajectories(); ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } + // Initialize the state and input + vector_array_t x, u; + multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); + // Bookkeeping performanceIndeces_.clear(); int iter = 0; - multiple_shooting::Convergence convergence = multiple_shooting::Convergence::FALSE; - while (convergence == multiple_shooting::Convergence::FALSE) { + sqp::Convergence convergence = sqp::Convergence::FALSE; + while (convergence == sqp::Convergence::FALSE) { if (settings_.printSolverStatus || settings_.printLinesearch) { - std::cerr << "\nSQP iteration: " << iter << "\n"; + std::cerr << "\nPIPG iteration: " << iter << "\n"; } // Make QP approximation linearQuadraticApproximationTimer_.startTimer(); @@ -177,7 +218,7 @@ void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar } computeControllerTimer_.startTimer(); - setPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); computeControllerTimer_.endTimer(); ++numProblems_; @@ -185,7 +226,7 @@ void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\nConvergence : " << toString(convergence) << "\n"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; - std::cerr << "\n+++++++++++++ SQP solver has terminated ++++++++++++++"; + std::cerr << "\n+++++++++++++ PIPG solver has terminated +++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; } } @@ -194,53 +235,6 @@ void PipgMpcSolver::runParallel(std::function<void(int)> taskFunction) { threadPool_.runParallel(std::move(taskFunction), settings_.nThreads); } -void PipgMpcSolver::initializeStateInputTrajectories(const vector_t& initState, const std::vector<AnnotatedTime>& timeDiscretization, - vector_array_t& stateTrajectory, vector_array_t& inputTrajectory) { - const int N = static_cast<int>(timeDiscretization.size()) - 1; // // size of the input trajectory - stateTrajectory.clear(); - stateTrajectory.reserve(N + 1); - inputTrajectory.clear(); - inputTrajectory.reserve(N); - - // Determine till when to use the previous solution - scalar_t interpolateStateTill = timeDiscretization.front().time; - scalar_t interpolateInputTill = timeDiscretization.front().time; - if (primalSolution_.timeTrajectory_.size() >= 2) { - interpolateStateTill = primalSolution_.timeTrajectory_.back(); - interpolateInputTill = primalSolution_.timeTrajectory_[primalSolution_.timeTrajectory_.size() - 2]; - } - - // Initial state - const scalar_t initTime = getIntervalStart(timeDiscretization[0]); - if (initTime < interpolateStateTill) { - stateTrajectory.push_back( - LinearInterpolation::interpolate(initTime, primalSolution_.timeTrajectory_, primalSolution_.stateTrajectory_)); - } else { - stateTrajectory.push_back(initState); - } - - for (int i = 0; i < N; i++) { - if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { - // Event Node - inputTrajectory.push_back(vector_t()); // no input at event node - stateTrajectory.push_back(multiple_shooting::initializeEventNode(timeDiscretization[i].time, stateTrajectory.back())); - } else { - // Intermediate node - const scalar_t time = getIntervalStart(timeDiscretization[i]); - const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); - vector_t input, nextState; - if (time > interpolateInputTill || nextTime > interpolateStateTill) { // Using initializer - std::tie(input, nextState) = - multiple_shooting::initializeIntermediateNode(*initializerPtr_, time, nextTime, stateTrajectory.back()); - } else { // interpolate previous solution - std::tie(input, nextState) = multiple_shooting::initializeIntermediateNode(primalSolution_, time, nextTime, stateTrajectory.back()); - } - inputTrajectory.push_back(std::move(input)); - stateTrajectory.push_back(std::move(nextState)); - } - } -} - PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_t& delta_x0) { // Solve the QP OcpSubproblemSolution solution; @@ -289,64 +283,27 @@ PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_ vector_array_t EInv(E.size()); std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); - Pipg::SolverStatus pipgStatus = + pipg::SolverStatus pipgStatus = pipgSolver_.solveOCPInParallel(delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled, ScalarFunctionQuadraticApproximation(), VectorFunctionLinearApproximation()); pipgSolver_.getStateInputTrajectoriesSolution(deltaXSol, deltaUSol); - // To determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] - solution.armijoDescentMetric = 0.0; - for (int i = 0; i < cost_.size(); i++) { - if (cost_[i].dfdx.size() > 0) { - solution.armijoDescentMetric += cost_[i].dfdx.dot(deltaXSol[i]); - } - if (cost_[i].dfdu.size() > 0) { - solution.armijoDescentMetric += cost_[i].dfdu.dot(deltaUSol[i]); - } - } - pipgSolver_.descaleSolution(D, deltaXSol, deltaUSol); + // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + solution.armijoDescentMetric = armijoDescentMetric(cost_, deltaXSol, deltaUSol); + // remap the tilde delta u to real delta u if (settings_.projectStateInputEqualityConstraints) { - vector_t tmp; // 1 temporary for re-use. - for (int i = 0; i < deltaUSol.size(); i++) { - if (constraintsProjection_[i].f.size() > 0) { - tmp.noalias() = constraintsProjection_[i].dfdu * deltaUSol[i]; - deltaUSol[i] = tmp + constraintsProjection_[i].f; - deltaUSol[i].noalias() += constraintsProjection_[i].dfdx * deltaXSol[i]; - } - } + multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); } return solution; } -void PipgMpcSolver::setPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { - // Clear old solution - primalSolution_.clear(); - - // Correct for missing inputs at PreEvents - for (int i = 0; i < time.size(); ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { - u[i] = u[i - 1]; - } - } - - // Construct nominal time, state and input trajectories - primalSolution_.stateTrajectory_ = std::move(x); - u.push_back(u.back()); // Repeat last input to make equal length vectors - primalSolution_.inputTrajectory_ = std::move(u); - primalSolution_.timeTrajectory_.reserve(time.size()); - for (size_t i = 0; i < time.size(); i++) { - primalSolution_.timeTrajectory_.push_back(time[i].time); - if (time[i].event == AnnotatedTime::Event::PreEvent) { - primalSolution_.postEventIndices_.push_back(i + 1); - } - } - primalSolution_.modeSchedule_ = this->getReferenceManager().getModeSchedule(); - - primalSolution_.controllerPtr_.reset(new FeedforwardController(primalSolution_.timeTrajectory_, primalSolution_.inputTrajectory_)); +PrimalSolution PipgMpcSolver::toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u)); } PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, @@ -357,37 +314,46 @@ PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<Annot std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); dynamics_.resize(N); cost_.resize(N + 1); - constraints_.resize(N + 1); constraintsProjection_.resize(N); + stateInputEqConstraints_.resize(N + 1); + stateIneqConstraints_.resize(N + 1); + stateInputIneqConstraints_.resize(N); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; PerformanceIndex workerPerformance; // Accumulate performance in local variable - const bool projection = settings_.projectStateInputEqualityConstraints; + const bool projectStateInputEqualityConstraints = settings_.projectStateInputEqualityConstraints; int i = timeIndex++; while (i < N) { if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); - workerPerformance += result.performance; + workerPerformance += multiple_shooting::computeEventPerformance(result); dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); - constraints_[i] = std::move(result.constraints); constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + stateInputEqConstraints_[i] = std::move(result.eqConstraints); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); + stateInputIneqConstraints_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - auto result = - multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, projection, ti, dt, x[i], x[i + 1], u[i]); - workerPerformance += result.performance; + auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); + if (projectStateInputEqualityConstraints) { + constexpr bool extractPseudoInverse = false; + multiple_shooting::projectTranscription(result, extractPseudoInverse); + } dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); - constraints_[i] = std::move(result.constraints); constraintsProjection_[i] = std::move(result.constraintsProjection); + stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); + stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); + stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); } i = timeIndex++; @@ -396,9 +362,10 @@ PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<Annot if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); - workerPerformance += result.performance; + workerPerformance += multiple_shooting::computeTerminalPerformance(result); cost_[i] = std::move(result.cost); - constraints_[i] = std::move(result.constraints); + stateInputEqConstraints_[i] = std::move(result.eqConstraints); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); } // Accumulate! Same worker might run multiple tasks @@ -412,6 +379,7 @@ PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<Annot // Sum performance of the threads PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + return totalPerformance; } @@ -461,22 +429,10 @@ PerformanceIndex PipgMpcSolver::computePerformance(const std::vector<AnnotatedTi return totalPerformance; } -scalar_t PipgMpcSolver::trajectoryNorm(const vector_array_t& v) { - scalar_t norm = 0.0; - for (const auto& vi : v) { - norm += vi.squaredNorm(); - } - return std::sqrt(norm); -} - -scalar_t PipgMpcSolver::totalConstraintViolation(const PerformanceIndex& performance) const { - return std::sqrt(performance.dynamicsViolationSSE + performance.equalityConstraintsSSE); -} - -multiple_shooting::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, - const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, - vector_array_t& x, vector_array_t& u) { - using StepType = multiple_shooting::StepInfo::StepType; +sqp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u) { + using StepType = FilterLinesearch::StepType; /* * Filter linesearch based on: @@ -490,56 +446,33 @@ multiple_shooting::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& base } // Baseline costs - const scalar_t baselineConstraintViolation = totalConstraintViolation(baseline); + const scalar_t baselineConstraintViolation = FilterLinesearch::totalConstraintViolation(baseline); // Update norm const auto& dx = subproblemSolution.deltaXSol; const auto& du = subproblemSolution.deltaUSol; - const scalar_t deltaUnorm = trajectoryNorm(du); - const scalar_t deltaXnorm = trajectoryNorm(dx); - - // Prepare step info - multiple_shooting::StepInfo stepInfo; + const auto deltaUnorm = multiple_shooting::trajectoryNorm(du); + const auto deltaXnorm = multiple_shooting::trajectoryNorm(dx); scalar_t alpha = 1.0; vector_array_t xNew(x.size()); vector_array_t uNew(u.size()); do { // Compute step - for (int i = 0; i < u.size(); i++) { - if (du[i].size() > 0) { // account for absence of inputs at events. - uNew[i] = u[i] + alpha * du[i]; - } - } - for (int i = 0; i < x.size(); i++) { - xNew[i] = x[i] + alpha * dx[i]; - } + multiple_shooting::incrementTrajectory(u, du, alpha, uNew); + multiple_shooting::incrementTrajectory(x, dx, alpha, xNew); // Compute cost and constraints const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew); - const scalar_t newConstraintViolation = totalConstraintViolation(performanceNew); // Step acceptance and record step type - const bool stepAccepted = [&]() { - if (newConstraintViolation > settings_.g_max) { - // High constraint violation. Only accept decrease in constraints. - stepInfo.stepType = StepType::CONSTRAINT; - return newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); - } else if (newConstraintViolation < settings_.g_min && baselineConstraintViolation < settings_.g_min && - subproblemSolution.armijoDescentMetric < 0.0) { - // With low violation and having a descent direction, require the armijo condition. - stepInfo.stepType = StepType::COST; - return performanceNew.merit < (baseline.merit + settings_.armijoFactor * alpha * subproblemSolution.armijoDescentMetric); - } else { - // Medium violation: either merit or constraints decrease (with small gamma_c mixing of old constraints) - stepInfo.stepType = StepType::DUAL; - return performanceNew.merit < (baseline.merit - settings_.gamma_c * baselineConstraintViolation) || - newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); - } - }(); + bool stepAccepted; + StepType stepType; + std::tie(stepAccepted, stepType) = + filterLinesearch_.acceptStep(baseline, performanceNew, alpha * subproblemSolution.armijoDescentMetric); if (settings_.printLinesearch) { - std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepInfo.stepType) + std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepType) << (stepAccepted ? std::string{" (Accepted)"} : std::string{" (Rejected)"}) << "\n"; std::cerr << "|dx| = " << alpha * deltaXnorm << "\t|du| = " << alpha * deltaUnorm << "\n"; std::cerr << performanceNew << "\n"; @@ -549,12 +482,16 @@ multiple_shooting::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& base x = std::move(xNew); u = std::move(uNew); + // Prepare step info + sqp::StepInfo stepInfo; stepInfo.stepSize = alpha; + stepInfo.stepType = stepType; stepInfo.dx_norm = alpha * deltaXnorm; stepInfo.du_norm = alpha * deltaUnorm; stepInfo.performanceAfterStep = performanceNew; - stepInfo.totalConstraintViolationAfterStep = newConstraintViolation; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(performanceNew); return stepInfo; + } else { // Try smaller step alpha *= settings_.alpha_decay; @@ -570,12 +507,13 @@ multiple_shooting::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& base } while (alpha >= settings_.alpha_min); // Alpha_min reached -> Don't take a step + sqp::StepInfo stepInfo; stepInfo.stepSize = 0.0; stepInfo.stepType = StepType::ZERO; stepInfo.dx_norm = 0.0; stepInfo.du_norm = 0.0; stepInfo.performanceAfterStep = baseline; - stepInfo.totalConstraintViolationAfterStep = baselineConstraintViolation; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline); if (settings_.printLinesearch) { std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; @@ -584,9 +522,8 @@ multiple_shooting::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& base return stepInfo; } -multiple_shooting::Convergence PipgMpcSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, - const multiple_shooting::StepInfo& stepInfo) const { - using Convergence = multiple_shooting::Convergence; +sqp::Convergence PipgMpcSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const { + using Convergence = sqp::Convergence; if ((iteration + 1) >= settings_.sqpIteration) { // Converged because the next iteration would exceed the specified number of iterations return Convergence::ITERATIONS; @@ -594,7 +531,7 @@ multiple_shooting::Convergence PipgMpcSolver::checkConvergence(int iteration, co // Converged because step size is below the specified minimum return Convergence::STEPSIZE; } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.costTol && - totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { + FilterLinesearch::totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { // Converged because the change in merit is below the specified tolerance while the constraint violation is below the minimum return Convergence::METRICS; } else if (stepInfo.dx_norm < settings_.deltaTol && stepInfo.du_norm < settings_.deltaTol) { diff --git a/ocs2_pipg/test/testHelper.cpp b/ocs2_pipg/test/testHelpers.cpp similarity index 99% rename from ocs2_pipg/test/testHelper.cpp rename to ocs2_pipg/test/testHelpers.cpp index 9d8ad3c74..2206ac473 100644 --- a/ocs2_pipg/test/testHelper.cpp +++ b/ocs2_pipg/test/testHelpers.cpp @@ -29,11 +29,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include "ocs2_pipg/HelperFunctions.h" - #include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> #include <ocs2_oc/test/testProblemsGeneration.h> +#include "ocs2_pipg/Helpers.h" + class HelperFunctionTest : public testing::Test { protected: // x_0, x_1, ... x_{N - 1}, X_{N} diff --git a/ocs2_pipg/test/testPipgMpc.cpp b/ocs2_pipg/test/testPipgMpc.cpp index c270522b3..4cafa0c96 100644 --- a/ocs2_pipg/test/testPipgMpc.cpp +++ b/ocs2_pipg/test/testPipgMpc.cpp @@ -66,7 +66,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct // Solver settings auto sqpSettings = []() { - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.05; settings.sqpIteration = 10; settings.projectStateInputEqualityConstraints = true; diff --git a/ocs2_pipg/test/testPipgSolver.cpp b/ocs2_pipg/test/testPipgSolver.cpp index fc13970be..4fdf1977b 100644 --- a/ocs2_pipg/test/testPipgSolver.cpp +++ b/ocs2_pipg/test/testPipgSolver.cpp @@ -109,14 +109,14 @@ TEST_F(PIPGSolverTest, correctness) { ocs2::vector_t primalSolutionPIPG; solver.solveDenseQP(costApproximation.dfdxx.sparseView(), costApproximation.dfdx, constraintsApproximation.dfdx.sparseView(), - constraintsApproximation.f, ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, - primalSolutionPIPG); + constraintsApproximation.f, ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, + primalSolutionPIPG); ocs2::vector_t primalSolutionPIPGParallel; ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); solver.solveOCPInParallel(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma, costApproximation, - constraintsApproximation); + constraintsApproximation); solver.getStackedSolution(primalSolutionPIPGParallel); auto calculateConstraintViolation = [&](const ocs2::vector_t& sol) -> ocs2::scalar_t { @@ -165,8 +165,8 @@ TEST_F(PIPGSolverTest, correctness) { // Threshold is the (absoluteTolerance) * (2-Norm of the hessian H)[lambda] EXPECT_TRUE(std::abs(QPCost - PIPGCost) < solver.settings().absoluteTolerance * lambda) - << "Absolute diff is [" << std::abs(QPCost - PIPGCost) << "] which is larger than [" - << solver.settings().absoluteTolerance * lambda << "]"; + << "Absolute diff is [" << std::abs(QPCost - PIPGCost) << "] which is larger than [" << solver.settings().absoluteTolerance * lambda + << "]"; EXPECT_TRUE(std::abs(PIPGParallelCost - PIPGCost) < solver.settings().absoluteTolerance) << "Absolute diff is [" << std::abs(PIPGParallelCost - PIPGCost) << "] which is larger than [" diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 35829a9ed..9ad186ded 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -70,8 +70,7 @@ find_package(cmake_clang_tools QUIET) if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) add_clang_tooling( - TARGETS - ${PROJECT_NAME} + TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR @@ -101,7 +100,9 @@ catkin_add_gtest(test_${PROJECT_NAME} test/testUnconstrained.cpp test/testValuefunction.cpp ) -add_dependencies(test_${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +add_dependencies(test_${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES} From e0c6aa5425c98a79542f5561e77ee109f4edce94 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Sun, 4 Dec 2022 16:25:09 +0100 Subject: [PATCH 387/512] updated to SQP --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 1 - .../ocs2_sqp/include/ocs2_sqp/SqpSettings.h | 2 +- .../include/ocs2_sqp/SqpSolverStatus.h | 25 +++++++-- ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp | 2 +- ocs2_sqp/ocs2_sqp/src/SqpSolverStatus.cpp | 52 ------------------- 5 files changed, 23 insertions(+), 59 deletions(-) delete mode 100644 ocs2_sqp/ocs2_sqp/src/SqpSolverStatus.cpp diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 9ad186ded..9bd34a16e 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -52,7 +52,6 @@ include_directories( add_library(${PROJECT_NAME} src/SqpSettings.cpp src/SqpSolver.cpp - src/SqpSolverStatus.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h index 4a142a2f7..a03fc023f 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h @@ -80,7 +80,7 @@ struct Settings { }; /** - * Loads the multiple shooting settings from a given file. + * Loads the multiple shooting SQP settings from a given file. * * @param [in] filename: File name which contains the configuration data. * @param [in] fieldName: Field name which contains the configuration data. diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h index 4da41d2ea..99df86b34 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include <string> + #include <ocs2_core/Types.h> #include <ocs2_oc/oc_data/PerformanceIndex.h> #include <ocs2_oc/search_strategy/FilterLinesearch.h> @@ -36,6 +38,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace sqp { +/** Different types of convergence */ +enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; + /** Struct to contain the result and logging data of the stepsize computation */ struct StepInfo { // Step size and type @@ -51,10 +56,22 @@ struct StepInfo { scalar_t totalConstraintViolationAfterStep; // constraint metric used in the line search }; -/** Different types of convergence */ -enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; - -std::string toString(const Convergence& convergence); +/** Transforms sqp::Convergence to string */ +inline std::string toString(const Convergence& convergence) { + switch (convergence) { + case Convergence::ITERATIONS: + return "Maximum number of iterations reached"; + case Convergence::STEPSIZE: + return "Step size below minimum"; + case Convergence::METRICS: + return "Cost decrease and constraint satisfaction below tolerance"; + case Convergence::PRIMAL: + return "Primal update below tolerance"; + case Convergence::FALSE: + default: + return "Not Converged"; + } +} } // namespace sqp } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp index ae56f3fa7..202d339dc 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp @@ -44,7 +44,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, Settings settings; if (verbose) { - std::cerr << "\n #### Multiple Shooting Settings:"; + std::cerr << "\n #### Multiple-Shooting SQP Settings:"; std::cerr << "\n #### =============================================================================\n"; } diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolverStatus.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolverStatus.cpp deleted file mode 100644 index 0c2e4ebe7..000000000 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolverStatus.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#include "ocs2_sqp/SqpSolverStatus.h" - -namespace ocs2 { -namespace sqp { - -std::string toString(const Convergence& convergence) { - switch (convergence) { - case Convergence::ITERATIONS: - return "Maximum number of iterations reached"; - case Convergence::STEPSIZE: - return "Step size below minimum"; - case Convergence::METRICS: - return "Cost decrease and constraint satisfaction below tolerance"; - case Convergence::PRIMAL: - return "Primal update below tolerance"; - case Convergence::FALSE: - default: - return "Not Converged"; - } -} - -} // namespace sqp -} // namespace ocs2 From 5ffa5d053bd7898a07e323cb2bc4cc3badb00108 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Sun, 4 Dec 2022 16:31:38 +0100 Subject: [PATCH 388/512] no SQP dependency --- ocs2_pipg/CMakeLists.txt | 17 +--- ocs2_pipg/include/ocs2_pipg/Helpers.h | 33 ++++++- ocs2_pipg/include/ocs2_pipg/PipgSettings.h | 10 ++- ocs2_pipg/include/ocs2_pipg/PipgSolver.h | 7 +- .../include/ocs2_pipg/PipgSolverStatus.h | 1 + ocs2_pipg/include/ocs2_pipg/SlpSettings.h | 88 +++++++++++++++++++ ocs2_pipg/include/ocs2_pipg/SlpSolverStatus.h | 77 ++++++++++++++++ .../include/ocs2_pipg/mpc/PipgMpcSolver.h | 16 ++-- ocs2_pipg/package.xml | 6 +- ocs2_pipg/src/Helpers.cpp | 16 ++-- ocs2_pipg/src/PipgSettings.cpp | 5 +- ocs2_pipg/src/SlpSettings.cpp | 83 +++++++++++++++++ ocs2_pipg/src/mpc/PipgMpcSolver.cpp | 55 ++++-------- ocs2_pipg/test/testHelpers.cpp | 4 +- ocs2_pipg/test/testPipgSolver.cpp | 5 +- .../{testPipgMpc.cpp => testSlpSolver.cpp} | 38 ++++---- 16 files changed, 356 insertions(+), 105 deletions(-) create mode 100644 ocs2_pipg/include/ocs2_pipg/SlpSettings.h create mode 100644 ocs2_pipg/include/ocs2_pipg/SlpSolverStatus.h create mode 100644 ocs2_pipg/src/SlpSettings.cpp rename ocs2_pipg/test/{testPipgMpc.cpp => testSlpSolver.cpp} (92%) diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_pipg/CMakeLists.txt index dfa42f41f..eec4f8d3d 100644 --- a/ocs2_pipg/CMakeLists.txt +++ b/ocs2_pipg/CMakeLists.txt @@ -6,7 +6,6 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_mpc ocs2_oc ocs2_qp_solver - ocs2_sqp ) find_package(catkin REQUIRED COMPONENTS @@ -46,9 +45,10 @@ include_directories( ) add_library(${PROJECT_NAME} + src/Helpers.cpp src/PipgSettings.cpp src/PipgSolver.cpp - src/Helpers.cpp + src/SlpSettings.cpp src/mpc/PipgMpcSolver.cpp ) add_dependencies(${PROJECT_NAME} @@ -94,6 +94,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ catkin_add_gtest(test_${PROJECT_NAME} test/testHelpers.cpp test/testPipgSolver.cpp + test/testSlpSolver.cpp ) add_dependencies(test_${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} @@ -103,15 +104,3 @@ target_link_libraries(test_${PROJECT_NAME} ${catkin_LIBRARIES} gtest_main ) - -catkin_add_gtest(test_pipg_mpc - test/testPipgMpc.cpp -) -add_dependencies(test_pipg_mpc - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(test_pipg_mpc - ${PROJECT_NAME} - ${catkin_LIBRARIES} - gtest_main -) diff --git a/ocs2_pipg/include/ocs2_pipg/Helpers.h b/ocs2_pipg/include/ocs2_pipg/Helpers.h index 26f593570..616e279c3 100644 --- a/ocs2_pipg/include/ocs2_pipg/Helpers.h +++ b/ocs2_pipg/include/ocs2_pipg/Helpers.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <ocs2_core/Types.h> @@ -5,12 +34,12 @@ #include <ocs2_oc/oc_problem/OcpSize.h> namespace ocs2 { -namespace pipg { +namespace slp { vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost); vector_t GGTAbsRowSumInParallel(const OcpSize& ocpSize, const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, ThreadPool& threadPool); -} // namespace pipg +} // namespace slp } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/include/ocs2_pipg/PipgSettings.h b/ocs2_pipg/include/ocs2_pipg/PipgSettings.h index 71401dc0c..66cc53d3f 100644 --- a/ocs2_pipg/include/ocs2_pipg/PipgSettings.h +++ b/ocs2_pipg/include/ocs2_pipg/PipgSettings.h @@ -29,8 +29,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include <string> - #include <ocs2_core/Types.h> namespace ocs2 { @@ -56,6 +54,14 @@ struct Settings { bool displayShortSummary = false; }; +/** + * Loads the PIPG settings from a given file. + * + * @param [in] filename: File name which contains the configuration data. + * @param [in] fieldName: Field name which contains the configuration data. + * @param [in] verbose: Flag to determine whether to print out the loaded settings or not. + * @return The settings + */ Settings loadSettings(const std::string& filename, const std::string& fieldName = "pipg", bool verbose = true); } // namespace pipg diff --git a/ocs2_pipg/include/ocs2_pipg/PipgSolver.h b/ocs2_pipg/include/ocs2_pipg/PipgSolver.h index 7aafe2d0e..304194a63 100644 --- a/ocs2_pipg/include/ocs2_pipg/PipgSolver.h +++ b/ocs2_pipg/include/ocs2_pipg/PipgSolver.h @@ -43,10 +43,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { +/* + * First order primal-dual method for solving optimal control problem based on: + * "Proportional-Integral Projected Gradient Method for Model Predictive Control" + * https://arxiv.org/abs/2009.06980 + */ class PipgSolver { public: /** - * @brief Construct a new PIPG with pipg setting object. + * @brief Constructor. * * @param Settings: PIPG setting */ diff --git a/ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h b/ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h index 02cf2ffce..b12f76600 100644 --- a/ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h +++ b/ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h @@ -40,6 +40,7 @@ enum class SolverStatus { UNDEFINED, }; +/** Transforms pipg::SolverStatus to string */ inline std::string toString(SolverStatus s) { switch (s) { case SolverStatus::SUCCESS: diff --git a/ocs2_pipg/include/ocs2_pipg/SlpSettings.h b/ocs2_pipg/include/ocs2_pipg/SlpSettings.h new file mode 100644 index 000000000..eac5012da --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/SlpSettings.h @@ -0,0 +1,88 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_core/integration/SensitivityIntegrator.h> + +#include "ocs2_pipg/PipgSettings.h" + +namespace ocs2 { +namespace slp { + +/** Multiple-shooting SLP (Successive Linear Programming) settings */ +struct Settings { + size_t slpIteration = 10; // Maximum number of SLP iterations + scalar_t deltaTol = 1e-6; // Termination condition : RMS update of x(t) and u(t) are both below this value + scalar_t costTol = 1e-4; // Termination condition : (cost{i+1} - (cost{i}) < costTol AND constraints{i+1} < g_min + + // Linesearch - step size rules + scalar_t alpha_decay = 0.5; // multiply the step size by this factor every time a linesearch step is rejected. + scalar_t alpha_min = 1e-4; // terminate linesearch if the attempted step size is below this threshold + + // Linesearch - step acceptance criteria with c = costs, g = the norm of constraint violation, and w = [x; u] + scalar_t g_max = 1e6; // (1): IF g{i+1} > g_max REQUIRE g{i+1} < (1-gamma_c) * g{i} + scalar_t g_min = 1e-6; // (2): ELSE IF (g{i} < g_min AND g{i+1} < g_min AND dc/dw'{i} * delta_w < 0) REQUIRE armijo condition + scalar_t armijoFactor = 1e-4; // Armijo condition: c{i+1} < c{i} + armijoFactor * dc/dw'{i} * delta_w + scalar_t gamma_c = 1e-6; // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i} + + // Discretization method + scalar_t dt = 0.01; // user-defined time discretization + SensitivityIntegratorType integratorType = SensitivityIntegratorType::RK2; + + // Inequality penalty relaxed barrier parameters + scalar_t inequalityConstraintMu = 0.0; + scalar_t inequalityConstraintDelta = 1e-6; + + // Printing + bool printSolverStatus = false; // Print HPIPM status after solving the QP subproblem + bool printSolverStatistics = false; // Print benchmarking of the multiple shooting method + bool printLinesearch = false; // Print linesearch information + + // Threading + size_t nThreads = 4; + int threadPriority = 50; + + // LP subproblem solver settings + pipg::Settings pipgSettings = pipg::Settings(); +}; + +/** + * Loads the multiple shooting SLP settings from a given file. + * + * @param [in] filename: File name which contains the configuration data. + * @param [in] fieldName: Field name which contains the configuration data. + * @param [in] verbose: Flag to determine whether to print out the loaded settings or not. + * @return The settings + */ +Settings loadSettings(const std::string& filename, const std::string& fieldName = "multiple_shooting", bool verbose = true); + +} // namespace slp +} // namespace ocs2 diff --git a/ocs2_pipg/include/ocs2_pipg/SlpSolverStatus.h b/ocs2_pipg/include/ocs2_pipg/SlpSolverStatus.h new file mode 100644 index 000000000..9d70d36cf --- /dev/null +++ b/ocs2_pipg/include/ocs2_pipg/SlpSolverStatus.h @@ -0,0 +1,77 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <string> + +#include <ocs2_core/Types.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> +#include <ocs2_oc/search_strategy/FilterLinesearch.h> + +namespace ocs2 { +namespace slp { + +/** Different types of convergence */ +enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; + +/** Struct to contain the result and logging data of the stepsize computation */ +struct StepInfo { + // Step size and type + scalar_t stepSize = 0.0; + FilterLinesearch::StepType stepType = FilterLinesearch::StepType::UNKNOWN; + + // Step in primal variables + scalar_t dx_norm = 0.0; // norm of the state trajectory update + scalar_t du_norm = 0.0; // norm of the input trajectory update + + // Performance result after the step + PerformanceIndex performanceAfterStep; + scalar_t totalConstraintViolationAfterStep; // constraint metric used in the line search +}; + +/** Transforms pipg::Convergence to string */ +inline std::string toString(const Convergence& convergence) { + switch (convergence) { + case Convergence::ITERATIONS: + return "Maximum number of iterations reached"; + case Convergence::STEPSIZE: + return "Step size below minimum"; + case Convergence::METRICS: + return "Cost decrease and constraint satisfaction below tolerance"; + case Convergence::PRIMAL: + return "Primal update below tolerance"; + case Convergence::FALSE: + default: + return "Not Converged"; + } +} + +} // namespace slp +} // namespace ocs2 diff --git a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h index 157769ed0..a520b1acd 100644 --- a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h +++ b/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h @@ -39,10 +39,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/oc_solver/SolverBase.h> #include <ocs2_oc/search_strategy/FilterLinesearch.h> -#include <ocs2_sqp/SqpSettings.h> -#include <ocs2_sqp/SqpSolverStatus.h> - #include "ocs2_pipg/PipgSolver.h" +#include "ocs2_pipg/SlpSettings.h" +#include "ocs2_pipg/SlpSolverStatus.h" namespace ocs2 { @@ -51,12 +50,11 @@ class PipgMpcSolver : public SolverBase { /** * Constructor * - * @param settings : settings for the multiple shooting solver. + * @param settings : settings for the multiple shooting SLP solver. * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ - PipgMpcSolver(sqp::Settings sqpSettings, pipg::Settings pipgSettings, const OptimalControlProblem& optimalControlProblem, - const Initializer& initializer); + PipgMpcSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); ~PipgMpcSolver() override; @@ -145,14 +143,14 @@ class PipgMpcSolver : public SolverBase { PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ - sqp::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, + slp::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u); /** Determine convergence after a step */ - sqp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const; + slp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const slp::StepInfo& stepInfo) const; // Problem definition - const sqp::Settings settings_; + const slp::Settings settings_; DynamicsDiscretizer discretizer_; DynamicsSensitivityDiscretizer sensitivityDiscretizer_; std::vector<OptimalControlProblem> ocpDefinitions_; diff --git a/ocs2_pipg/package.xml b/ocs2_pipg/package.xml index e03764a03..14c48548b 100644 --- a/ocs2_pipg/package.xml +++ b/ocs2_pipg/package.xml @@ -4,15 +4,15 @@ <version>0.0.0</version> <description>A numerical implementation of PIPG.</description> + <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer> <maintainer email="zhengfuaj@gmail.com">Zhengyu Fu</maintainer> - <license>TODO</license> + <license>BSD3</license> <buildtool_depend>catkin</buildtool_depend> <depend>ocs2_core</depend> - <depend>ocs2_oc</depend> - <depend>ocs2_sqp</depend> <depend>ocs2_mpc</depend> + <depend>ocs2_oc</depend> <!-- Test dependancy --> <depend>ocs2_qp_solver</depend> diff --git a/ocs2_pipg/src/Helpers.cpp b/ocs2_pipg/src/Helpers.cpp index eee4c1cb1..f90cd5f77 100644 --- a/ocs2_pipg/src/Helpers.cpp +++ b/ocs2_pipg/src/Helpers.cpp @@ -32,23 +32,23 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <atomic> #include <numeric> -namespace ocs2 { -namespace pipg { - namespace { -int getNumDecisionVariables(const OcpSize& ocpSize) { +int getNumDecisionVariables(const ocs2::OcpSize& ocpSize) { return std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0)); } -int getNumDynamicsConstraints(const OcpSize& ocpSize) { +int getNumDynamicsConstraints(const ocs2::OcpSize& ocpSize) { return std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0); } -int getNumGeneralEqualityConstraints(const OcpSize& ocpSize) { +int getNumGeneralEqualityConstraints(const ocs2::OcpSize& ocpSize) { return std::accumulate(ocpSize.numIneqConstraints.begin(), ocpSize.numIneqConstraints.end(), (int)0); } -} // namespace +} // anonymous namespace + +namespace ocs2 { +namespace slp { vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost) { const int N = ocpSize.numStages; @@ -179,5 +179,5 @@ vector_t GGTAbsRowSumInParallel(const OcpSize& ocpSize, const std::vector<Vector // */ } -} // namespace pipg +} // namespace slp } // namespace ocs2 diff --git a/ocs2_pipg/src/PipgSettings.cpp b/ocs2_pipg/src/PipgSettings.cpp index 85bdb2257..8ae0661e8 100644 --- a/ocs2_pipg/src/PipgSettings.cpp +++ b/ocs2_pipg/src/PipgSettings.cpp @@ -46,8 +46,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, Settings settings; if (verbose) { - std::cerr << "\n #### PIPG Settings: "; - std::cerr << "\n #### =============================================================================\n"; + std::cerr << "\n #### PIPG Settings: {"; } loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); @@ -64,7 +63,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, loadData::loadPtreeValue(pt, settings.displayShortSummary, fieldName + ".displayShortSummary", verbose); if (verbose) { - std::cerr << " #### =============================================================================" << std::endl; + std::cerr << "}\n"; } return settings; diff --git a/ocs2_pipg/src/SlpSettings.cpp b/ocs2_pipg/src/SlpSettings.cpp new file mode 100644 index 000000000..b8d8bc00b --- /dev/null +++ b/ocs2_pipg/src/SlpSettings.cpp @@ -0,0 +1,83 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_pipg/SlpSettings.h" + +#include <iostream> + +#include <boost/property_tree/info_parser.hpp> +#include <boost/property_tree/ptree.hpp> + +#include <ocs2_core/misc/LoadData.h> + +namespace ocs2 { +namespace slp { + +Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { + boost::property_tree::ptree pt; + boost::property_tree::read_info(filename, pt); + + Settings settings; + + if (verbose) { + std::cerr << "\n #### Multiple-Shooting SLP Settings:"; + std::cerr << "\n #### =============================================================================\n"; + } + + loadData::loadPtreeValue(pt, settings.slpIteration, fieldName + ".slpIteration", verbose); + loadData::loadPtreeValue(pt, settings.deltaTol, fieldName + ".deltaTol", verbose); + loadData::loadPtreeValue(pt, settings.alpha_decay, fieldName + ".alpha_decay", verbose); + loadData::loadPtreeValue(pt, settings.alpha_min, fieldName + ".alpha_min", verbose); + loadData::loadPtreeValue(pt, settings.gamma_c, fieldName + ".gamma_c", verbose); + loadData::loadPtreeValue(pt, settings.g_max, fieldName + ".g_max", verbose); + loadData::loadPtreeValue(pt, settings.g_min, fieldName + ".g_min", verbose); + loadData::loadPtreeValue(pt, settings.armijoFactor, fieldName + ".armijoFactor", verbose); + loadData::loadPtreeValue(pt, settings.costTol, fieldName + ".costTol", verbose); + loadData::loadPtreeValue(pt, settings.dt, fieldName + ".dt", verbose); + auto integratorName = sensitivity_integrator::toString(settings.integratorType); + loadData::loadPtreeValue(pt, integratorName, fieldName + ".integratorType", verbose); + settings.integratorType = sensitivity_integrator::fromString(integratorName); + loadData::loadPtreeValue(pt, settings.inequalityConstraintMu, fieldName + ".inequalityConstraintMu", verbose); + loadData::loadPtreeValue(pt, settings.inequalityConstraintDelta, fieldName + ".inequalityConstraintDelta", verbose); + loadData::loadPtreeValue(pt, settings.printSolverStatus, fieldName + ".printSolverStatus", verbose); + loadData::loadPtreeValue(pt, settings.printSolverStatistics, fieldName + ".printSolverStatistics", verbose); + loadData::loadPtreeValue(pt, settings.printLinesearch, fieldName + ".printLinesearch", verbose); + loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); + loadData::loadPtreeValue(pt, settings.threadPriority, fieldName + ".threadPriority", verbose); + settings.pipgSettings = pipg::loadSettings(filename, fieldName + ".pipg", verbose); + + if (verbose) { + std::cerr << " #### =============================================================================" << std::endl; + } + + return settings; +} + +} // namespace slp +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_pipg/src/mpc/PipgMpcSolver.cpp b/ocs2_pipg/src/mpc/PipgMpcSolver.cpp index d86f7077a..e6480e3c2 100644 --- a/ocs2_pipg/src/mpc/PipgMpcSolver.cpp +++ b/ocs2_pipg/src/mpc/PipgMpcSolver.cpp @@ -42,20 +42,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { -namespace { -sqp::Settings rectifySettings(const OptimalControlProblem& ocp, sqp::Settings&& settings) { - // True does not make sense if there are no constraints. - if (ocp.equalityConstraintPtr->empty()) { - settings.projectStateInputEqualityConstraints = false; - } - return settings; -} -} // anonymous namespace - -PipgMpcSolver::PipgMpcSolver(sqp::Settings settings, pipg::Settings pipgSettings, const OptimalControlProblem& optimalControlProblem, - const Initializer& initializer) - : settings_(rectifySettings(optimalControlProblem, std::move(settings))), - pipgSolver_(pipgSettings), +PipgMpcSolver::PipgMpcSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) + : settings_(std::move(settings)), + pipgSolver_(settings_.pipgSettings), threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { Eigen::setNbThreads(1); // No multithreading within Eigen. Eigen::initParallel(); @@ -187,8 +176,8 @@ void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar performanceIndeces_.clear(); int iter = 0; - sqp::Convergence convergence = sqp::Convergence::FALSE; - while (convergence == sqp::Convergence::FALSE) { + slp::Convergence convergence = slp::Convergence::FALSE; + while (convergence == slp::Convergence::FALSE) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\nPIPG iteration: " << iter << "\n"; } @@ -241,11 +230,6 @@ PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_ auto& deltaXSol = solution.deltaXSol; auto& deltaUSol = solution.deltaUSol; - const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); - if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { - throw std::runtime_error("[PipgMpcSolver] Please project State-Input equality constraints."); - } - // without constraints, or when using projection, we have an unconstrained QP. pipgSolver_.resize(extractSizesFromProblem(dynamics_, cost_, nullptr)); @@ -260,13 +244,13 @@ PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_ preConditioning_.endTimer(); lambdaEstimation_.startTimer(); - const vector_t rowwiseAbsSumH = pipg::hessianAbsRowSum(pipgSolver_.size(), cost_); + const vector_t rowwiseAbsSumH = slp::hessianAbsRowSum(pipgSolver_.size(), cost_); const scalar_t lambdaScaled = rowwiseAbsSumH.maxCoeff(); lambdaEstimation_.endTimer(); GGTMultiplication_.startTimer(); const vector_t rowwiseAbsSumGGT = - pipg::GGTAbsRowSumInParallel(pipgSolver_.size(), dynamics_, nullptr, &scalingVectors, pipgSolver_.getThreadPool()); + slp::GGTAbsRowSumInParallel(pipgSolver_.size(), dynamics_, nullptr, &scalingVectors, pipgSolver_.getThreadPool()); GGTMultiplication_.endTimer(); sigmaEstimation_.startTimer(); @@ -283,7 +267,7 @@ PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_ vector_array_t EInv(E.size()); std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); - pipg::SolverStatus pipgStatus = + const auto pipgStatus = pipgSolver_.solveOCPInParallel(delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled, ScalarFunctionQuadraticApproximation(), VectorFunctionLinearApproximation()); pipgSolver_.getStateInputTrajectoriesSolution(deltaXSol, deltaUSol); @@ -294,9 +278,7 @@ PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_ solution.armijoDescentMetric = armijoDescentMetric(cost_, deltaXSol, deltaUSol); // remap the tilde delta u to real delta u - if (settings_.projectStateInputEqualityConstraints) { - multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); - } + multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); return solution; } @@ -324,7 +306,6 @@ PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<Annot // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; PerformanceIndex workerPerformance; // Accumulate performance in local variable - const bool projectStateInputEqualityConstraints = settings_.projectStateInputEqualityConstraints; int i = timeIndex++; while (i < N) { @@ -344,10 +325,8 @@ PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<Annot const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); - if (projectStateInputEqualityConstraints) { - constexpr bool extractPseudoInverse = false; - multiple_shooting::projectTranscription(result, extractPseudoInverse); - } + constexpr bool extractPseudoInverse = false; + multiple_shooting::projectTranscription(result, extractPseudoInverse); dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); constraintsProjection_[i] = std::move(result.constraintsProjection); @@ -429,7 +408,7 @@ PerformanceIndex PipgMpcSolver::computePerformance(const std::vector<AnnotatedTi return totalPerformance; } -sqp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, +slp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u) { using StepType = FilterLinesearch::StepType; @@ -483,7 +462,7 @@ sqp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const st u = std::move(uNew); // Prepare step info - sqp::StepInfo stepInfo; + slp::StepInfo stepInfo; stepInfo.stepSize = alpha; stepInfo.stepType = stepType; stepInfo.dx_norm = alpha * deltaXnorm; @@ -507,7 +486,7 @@ sqp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const st } while (alpha >= settings_.alpha_min); // Alpha_min reached -> Don't take a step - sqp::StepInfo stepInfo; + slp::StepInfo stepInfo; stepInfo.stepSize = 0.0; stepInfo.stepType = StepType::ZERO; stepInfo.dx_norm = 0.0; @@ -522,9 +501,9 @@ sqp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const st return stepInfo; } -sqp::Convergence PipgMpcSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const { - using Convergence = sqp::Convergence; - if ((iteration + 1) >= settings_.sqpIteration) { +slp::Convergence PipgMpcSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const slp::StepInfo& stepInfo) const { + using Convergence = slp::Convergence; + if ((iteration + 1) >= settings_.slpIteration) { // Converged because the next iteration would exceed the specified number of iterations return Convergence::ITERATIONS; } else if (stepInfo.stepSize < settings_.alpha_min) { diff --git a/ocs2_pipg/test/testHelpers.cpp b/ocs2_pipg/test/testHelpers.cpp index 2206ac473..847aae5bc 100644 --- a/ocs2_pipg/test/testHelpers.cpp +++ b/ocs2_pipg/test/testHelpers.cpp @@ -75,7 +75,7 @@ class HelperFunctionTest : public testing::Test { }; TEST_F(HelperFunctionTest, hessianAbsRowSum) { - ocs2::vector_t rowwiseSum = ocs2::pipg::hessianAbsRowSum(ocpSize_, costArray); + ocs2::vector_t rowwiseSum = ocs2::slp::hessianAbsRowSum(ocpSize_, costArray); EXPECT_TRUE(rowwiseSum.isApprox(costApproximation.dfdxx.cwiseAbs().rowwise().sum())) << "rowSum:\n" << rowwiseSum.transpose(); } @@ -87,7 +87,7 @@ TEST_F(HelperFunctionTest, GGTAbsRowSumInParallel) { } ocs2::getConstraintMatrix(ocpSize_, x0, dynamicsArray, nullptr, &scalingVectors, constraintsApproximation); - ocs2::vector_t rowwiseSum = ocs2::pipg::GGTAbsRowSumInParallel(ocpSize_, dynamicsArray, nullptr, &scalingVectors, threadPool_); + ocs2::vector_t rowwiseSum = ocs2::slp::GGTAbsRowSumInParallel(ocpSize_, dynamicsArray, nullptr, &scalingVectors, threadPool_); ocs2::matrix_t GGT = constraintsApproximation.dfdx * constraintsApproximation.dfdx.transpose(); EXPECT_TRUE(rowwiseSum.isApprox(GGT.cwiseAbs().rowwise().sum())); } \ No newline at end of file diff --git a/ocs2_pipg/test/testPipgSolver.cpp b/ocs2_pipg/test/testPipgSolver.cpp index 4fdf1977b..1c60248ac 100644 --- a/ocs2_pipg/test/testPipgSolver.cpp +++ b/ocs2_pipg/test/testPipgSolver.cpp @@ -28,15 +28,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #include <gtest/gtest.h> - -#include "ocs2_pipg/PipgSolver.h" - #include <Eigen/Sparse> #include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> #include <ocs2_oc/test/testProblemsGeneration.h> #include <ocs2_qp_solver/QpSolver.h> +#include "ocs2_pipg/PipgSolver.h" + ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, ocs2::scalar_t relativeTolerance, bool verbose) { ocs2::pipg::Settings settings; diff --git a/ocs2_pipg/test/testPipgMpc.cpp b/ocs2_pipg/test/testSlpSolver.cpp similarity index 92% rename from ocs2_pipg/test/testPipgMpc.cpp rename to ocs2_pipg/test/testSlpSolver.cpp index 4cafa0c96..1838fd9fa 100644 --- a/ocs2_pipg/test/testPipgMpc.cpp +++ b/ocs2_pipg/test/testSlpSolver.cpp @@ -29,13 +29,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include "ocs2_pipg/mpc/PipgMpcSolver.h" - #include <ocs2_core/initialization/DefaultInitializer.h> - #include <ocs2_oc/synchronized_module/ReferenceManager.h> #include <ocs2_oc/test/testProblemsGeneration.h> +#include "ocs2_pipg/mpc/PipgMpcSolver.h" + namespace ocs2 { namespace { @@ -65,21 +64,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct ocs2::DefaultInitializer zeroInitializer(m); // Solver settings - auto sqpSettings = []() { - ocs2::sqp::Settings settings; - settings.dt = 0.05; - settings.sqpIteration = 10; - settings.projectStateInputEqualityConstraints = true; - settings.useFeedbackPolicy = false; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.nThreads = 100; - - return settings; - }(); - - auto pipgSettings = [&]() { + auto getPipgSettings = [&]() { ocs2::pipg::Settings settings; settings.nThreads = 100; settings.maxNumIterations = 30000; @@ -90,6 +75,19 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct settings.checkTerminationInterval = 1; settings.displayShortSummary = true; + return settings; + }; + + const auto slpSettings = [&]() { + ocs2::slp::Settings settings; + settings.dt = 0.05; + settings.slpIteration = 10; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 100; + settings.pipgSettings = getPipgSettings(); + return settings; }(); @@ -99,7 +97,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct const ocs2::vector_t initState = ocs2::vector_t::Ones(n); // Construct solver - ocs2::PipgMpcSolver solver(sqpSettings, pipgSettings, problem, zeroInitializer); + ocs2::PipgMpcSolver solver(slpSettings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); // Solve @@ -110,7 +108,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct } // namespace } // namespace ocs2 -TEST(test_unconstrained, correctness) { +TEST(testSlpSolver, test_unconstrained) { int n = 3; int m = 2; const double tol = 1e-9; From e66d832a877f008283c38e25dba0772b8604451a Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Sun, 4 Dec 2022 16:52:35 +0100 Subject: [PATCH 389/512] rename from ocs2_pipg to ocs2_slp --- ocs2/package.xml | 2 +- {ocs2_pipg => ocs2_slp}/CMakeLists.txt | 16 ++++--- .../include/ocs2_slp}/Helpers.h | 0 .../include/ocs2_slp/SlpMpc.h | 22 ++++----- .../include/ocs2_slp}/SlpSettings.h | 2 +- .../include/ocs2_slp/SlpSolver.h | 20 ++++---- .../include/ocs2_slp}/SlpSolverStatus.h | 0 .../include/ocs2_slp/pipg}/PipgSettings.h | 0 .../include/ocs2_slp/pipg}/PipgSolver.h | 4 +- .../include/ocs2_slp/pipg}/PipgSolverStatus.h | 0 {ocs2_pipg => ocs2_slp}/package.xml | 4 +- {ocs2_pipg => ocs2_slp}/src/Helpers.cpp | 2 +- {ocs2_pipg => ocs2_slp}/src/SlpSettings.cpp | 2 +- .../src/SlpSolver.cpp | 46 +++++++++---------- ocs2_slp/src/lintTarget.cpp | 43 +++++++++++++++++ .../src/pipg}/PipgSettings.cpp | 2 +- .../src => ocs2_slp/src/pipg}/PipgSolver.cpp | 2 +- {ocs2_pipg => ocs2_slp}/test/testHelpers.cpp | 2 +- .../test/testPipgSolver.cpp | 2 +- .../test/testSlpSolver.cpp | 4 +- 20 files changed, 111 insertions(+), 64 deletions(-) rename {ocs2_pipg => ocs2_slp}/CMakeLists.txt (90%) rename {ocs2_pipg/include/ocs2_pipg => ocs2_slp/include/ocs2_slp}/Helpers.h (100%) rename ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h => ocs2_slp/include/ocs2_slp/SlpMpc.h (77%) rename {ocs2_pipg/include/ocs2_pipg => ocs2_slp/include/ocs2_slp}/SlpSettings.h (99%) rename ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h => ocs2_slp/include/ocs2_slp/SlpSolver.h (92%) rename {ocs2_pipg/include/ocs2_pipg => ocs2_slp/include/ocs2_slp}/SlpSolverStatus.h (100%) rename {ocs2_pipg/include/ocs2_pipg => ocs2_slp/include/ocs2_slp/pipg}/PipgSettings.h (100%) rename {ocs2_pipg/include/ocs2_pipg => ocs2_slp/include/ocs2_slp/pipg}/PipgSolver.h (98%) rename {ocs2_pipg/include/ocs2_pipg => ocs2_slp/include/ocs2_slp/pipg}/PipgSolverStatus.h (100%) rename {ocs2_pipg => ocs2_slp}/package.xml (77%) rename {ocs2_pipg => ocs2_slp}/src/Helpers.cpp (99%) rename {ocs2_pipg => ocs2_slp}/src/SlpSettings.cpp (99%) rename ocs2_pipg/src/mpc/PipgMpcSolver.cpp => ocs2_slp/src/SlpSolver.cpp (91%) create mode 100644 ocs2_slp/src/lintTarget.cpp rename {ocs2_pipg/src => ocs2_slp/src/pipg}/PipgSettings.cpp (98%) rename {ocs2_pipg/src => ocs2_slp/src/pipg}/PipgSolver.cpp (99%) rename {ocs2_pipg => ocs2_slp}/test/testHelpers.cpp (99%) rename {ocs2_pipg => ocs2_slp}/test/testPipgSolver.cpp (99%) rename {ocs2_pipg => ocs2_slp}/test/testSlpSolver.cpp (97%) diff --git a/ocs2/package.xml b/ocs2/package.xml index 961296183..fcd5dd285 100644 --- a/ocs2/package.xml +++ b/ocs2/package.xml @@ -16,8 +16,8 @@ <exec_depend>ocs2_oc</exec_depend> <exec_depend>ocs2_qp_solver</exec_depend> <exec_depend>ocs2_ddp</exec_depend> + <exec_depend>ocs2_slp</exec_depend> <exec_depend>ocs2_sqp</exec_depend> - <exec_depend>ocs2_pipg</exec_depend> <exec_depend>ocs2_ros_interfaces</exec_depend> <exec_depend>ocs2_python_interface</exec_depend> <exec_depend>ocs2_pinocchio</exec_depend> diff --git a/ocs2_pipg/CMakeLists.txt b/ocs2_slp/CMakeLists.txt similarity index 90% rename from ocs2_pipg/CMakeLists.txt rename to ocs2_slp/CMakeLists.txt index eec4f8d3d..fa1515cf0 100644 --- a/ocs2_pipg/CMakeLists.txt +++ b/ocs2_slp/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(ocs2_pipg) +project(ocs2_slp) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_core @@ -45,11 +45,11 @@ include_directories( ) add_library(${PROJECT_NAME} + src/pipg/PipgSettings.cpp + src/pipg/PipgSolver.cpp src/Helpers.cpp - src/PipgSettings.cpp - src/PipgSolver.cpp src/SlpSettings.cpp - src/mpc/PipgMpcSolver.cpp + src/SlpSolver.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} @@ -60,14 +60,18 @@ target_link_libraries(${PROJECT_NAME} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) +add_executable(${PROJECT_NAME}_lintTarget + src/lintTarget.cpp +) + ######################### ### CLANG TOOLING ### ######################### find_package(cmake_clang_tools QUIET) if(cmake_clang_tools_FOUND) - message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) + message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) add_clang_tooling( - TARGETS ${PROJECT_NAME} + TARGETS ${PROJECT_NAME}_lintTarget ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR diff --git a/ocs2_pipg/include/ocs2_pipg/Helpers.h b/ocs2_slp/include/ocs2_slp/Helpers.h similarity index 100% rename from ocs2_pipg/include/ocs2_pipg/Helpers.h rename to ocs2_slp/include/ocs2_slp/Helpers.h diff --git a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h b/ocs2_slp/include/ocs2_slp/SlpMpc.h similarity index 77% rename from ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h rename to ocs2_slp/include/ocs2_slp/SlpMpc.h index 6c8c3a21e..d9c747893 100644 --- a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h +++ b/ocs2_slp/include/ocs2_slp/SlpMpc.h @@ -29,32 +29,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "ocs2_pipg/mpc/PipgMpcSolver.h" - #include <ocs2_mpc/MPC_BASE.h> +#include "ocs2_slp/SlpSolver.h" + namespace ocs2 { -class PipgMpc final : public MPC_BASE { +class SlpMpc final : public MPC_BASE { public: /** * Constructor * * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use multiple shooting * settings directly. - * @param settings : settings for the multiple shooting solver. + * @param settings : settings for the multiple shooting SLP solver. * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ - PipgMpc(mpc::Settings mpcSettings, multiple_shooting::Settings settings, pipg::Settings pipgSetting, - const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) + SlpMpc(mpc::Settings mpcSettings, slp::Settings slpSettings, const OptimalControlProblem& optimalControlProblem, + const Initializer& initializer) : MPC_BASE(std::move(mpcSettings)) { - solverPtr_.reset(new PipgMpcSolver(std::move(settings), std::move(pipgSetting), optimalControlProblem, initializer)); + solverPtr_.reset(new SlpSolver(std::move(slpSettings), optimalControlProblem, initializer)); }; - ~PipgMpc() override = default; + ~SlpMpc() override = default; - PipgMpcSolver* getSolverPtr() override { return solverPtr_.get(); } - const PipgMpcSolver* getSolverPtr() const override { return solverPtr_.get(); } + SlpSolver* getSolverPtr() override { return solverPtr_.get(); } + const SlpSolver* getSolverPtr() const override { return solverPtr_.get(); } protected: void calculateController(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override { @@ -65,6 +65,6 @@ class PipgMpc final : public MPC_BASE { } private: - std::unique_ptr<PipgMpcSolver> solverPtr_; + std::unique_ptr<SlpSolver> solverPtr_; }; } // namespace ocs2 diff --git a/ocs2_pipg/include/ocs2_pipg/SlpSettings.h b/ocs2_slp/include/ocs2_slp/SlpSettings.h similarity index 99% rename from ocs2_pipg/include/ocs2_pipg/SlpSettings.h rename to ocs2_slp/include/ocs2_slp/SlpSettings.h index eac5012da..bc268af98 100644 --- a/ocs2_pipg/include/ocs2_pipg/SlpSettings.h +++ b/ocs2_slp/include/ocs2_slp/SlpSettings.h @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/integration/SensitivityIntegrator.h> -#include "ocs2_pipg/PipgSettings.h" +#include "ocs2_slp/pipg/PipgSettings.h" namespace ocs2 { namespace slp { diff --git a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h similarity index 92% rename from ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h rename to ocs2_slp/include/ocs2_slp/SlpSolver.h index a520b1acd..3114fdb12 100644 --- a/ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -39,13 +39,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/oc_solver/SolverBase.h> #include <ocs2_oc/search_strategy/FilterLinesearch.h> -#include "ocs2_pipg/PipgSolver.h" -#include "ocs2_pipg/SlpSettings.h" -#include "ocs2_pipg/SlpSolverStatus.h" +#include "ocs2_slp/SlpSettings.h" +#include "ocs2_slp/SlpSolverStatus.h" +#include "ocs2_slp/pipg/PipgSolver.h" namespace ocs2 { -class PipgMpcSolver : public SolverBase { +class SlpSolver : public SolverBase { public: /** * Constructor @@ -54,9 +54,9 @@ class PipgMpcSolver : public SolverBase { * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ - PipgMpcSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); + SlpSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); - ~PipgMpcSolver() override; + ~SlpSolver() override; void reset() override; @@ -79,15 +79,15 @@ class PipgMpcSolver : public SolverBase { const std::vector<PerformanceIndex>& getIterationsLog() const override; ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const override { - throw std::runtime_error("[PipgMpcSolver] getValueFunction() not available yet."); + throw std::runtime_error("[SlpSolver] getValueFunction() not available yet."); }; ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) override { - throw std::runtime_error("[PipgMpcSolver] getHamiltonian() not available yet."); + throw std::runtime_error("[SlpSolver] getHamiltonian() not available yet."); } vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override { - throw std::runtime_error("[PipgMpcSolver] getStateInputEqualityConstraintLagrangian() not available yet."); + throw std::runtime_error("[SlpSolver] getStateInputEqualityConstraintLagrangian() not available yet."); } MultiplierCollection getIntermediateDualSolution(scalar_t time) const override { @@ -101,7 +101,7 @@ class PipgMpcSolver : public SolverBase { if (externalControllerPtr == nullptr) { runImpl(initTime, initState, finalTime); } else { - throw std::runtime_error("[PipgMpcSolver::run] This solver does not support external controller!"); + throw std::runtime_error("[SlpSolver::run] This solver does not support external controller!"); } } diff --git a/ocs2_pipg/include/ocs2_pipg/SlpSolverStatus.h b/ocs2_slp/include/ocs2_slp/SlpSolverStatus.h similarity index 100% rename from ocs2_pipg/include/ocs2_pipg/SlpSolverStatus.h rename to ocs2_slp/include/ocs2_slp/SlpSolverStatus.h diff --git a/ocs2_pipg/include/ocs2_pipg/PipgSettings.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h similarity index 100% rename from ocs2_pipg/include/ocs2_pipg/PipgSettings.h rename to ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h diff --git a/ocs2_pipg/include/ocs2_pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h similarity index 98% rename from ocs2_pipg/include/ocs2_pipg/PipgSolver.h rename to ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index 304194a63..4da997b4d 100644 --- a/ocs2_pipg/include/ocs2_pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -38,8 +38,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/thread_support/ThreadPool.h> #include <ocs2_oc/oc_problem/OcpSize.h> -#include "ocs2_pipg/PipgSettings.h" -#include "ocs2_pipg/PipgSolverStatus.h" +#include "ocs2_slp/pipg/PipgSettings.h" +#include "ocs2_slp/pipg/PipgSolverStatus.h" namespace ocs2 { diff --git a/ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolverStatus.h similarity index 100% rename from ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h rename to ocs2_slp/include/ocs2_slp/pipg/PipgSolverStatus.h diff --git a/ocs2_pipg/package.xml b/ocs2_slp/package.xml similarity index 77% rename from ocs2_pipg/package.xml rename to ocs2_slp/package.xml index 14c48548b..52ba012d7 100644 --- a/ocs2_pipg/package.xml +++ b/ocs2_slp/package.xml @@ -1,8 +1,8 @@ <?xml version="1.0"?> <package format="2"> - <name>ocs2_pipg</name> + <name>ocs2_slp</name> <version>0.0.0</version> - <description>A numerical implementation of PIPG.</description> + <description>A numerical implementation of a first order primal-dual MPC basee on PIPG.</description> <maintainer email="farbod.farshidian@gmail.com">Farbod Farshidian</maintainer> <maintainer email="zhengfuaj@gmail.com">Zhengyu Fu</maintainer> diff --git a/ocs2_pipg/src/Helpers.cpp b/ocs2_slp/src/Helpers.cpp similarity index 99% rename from ocs2_pipg/src/Helpers.cpp rename to ocs2_slp/src/Helpers.cpp index f90cd5f77..8e582eb26 100644 --- a/ocs2_pipg/src/Helpers.cpp +++ b/ocs2_slp/src/Helpers.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_pipg/Helpers.h" +#include "ocs2_slp/Helpers.h" #include <atomic> #include <numeric> diff --git a/ocs2_pipg/src/SlpSettings.cpp b/ocs2_slp/src/SlpSettings.cpp similarity index 99% rename from ocs2_pipg/src/SlpSettings.cpp rename to ocs2_slp/src/SlpSettings.cpp index b8d8bc00b..9676f38ea 100644 --- a/ocs2_pipg/src/SlpSettings.cpp +++ b/ocs2_slp/src/SlpSettings.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_pipg/SlpSettings.h" +#include "ocs2_slp/SlpSettings.h" #include <iostream> diff --git a/ocs2_pipg/src/mpc/PipgMpcSolver.cpp b/ocs2_slp/src/SlpSolver.cpp similarity index 91% rename from ocs2_pipg/src/mpc/PipgMpcSolver.cpp rename to ocs2_slp/src/SlpSolver.cpp index e6480e3c2..2631c3bd9 100644 --- a/ocs2_pipg/src/mpc/PipgMpcSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_pipg/mpc/PipgMpcSolver.h" +#include "ocs2_slp/SlpSolver.h" #include <iostream> #include <numeric> @@ -38,11 +38,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/pre_condition/Scaling.h> -#include "ocs2_pipg/Helpers.h" +#include "ocs2_slp/Helpers.h" namespace ocs2 { -PipgMpcSolver::PipgMpcSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) +SlpSolver::SlpSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) : settings_(std::move(settings)), pipgSolver_(settings_.pipgSettings), threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { @@ -68,13 +68,13 @@ PipgMpcSolver::PipgMpcSolver(slp::Settings settings, const OptimalControlProblem filterLinesearch_.armijoFactor = settings_.armijoFactor; } -PipgMpcSolver::~PipgMpcSolver() { +SlpSolver::~SlpSolver() { if (settings_.printSolverStatistics) { std::cerr << getBenchmarkingInformationPIPG() << "\n" << getBenchmarkingInformation() << std::endl; } } -void PipgMpcSolver::reset() { +void SlpSolver::reset() { // Clear solution primalSolution_ = PrimalSolution(); performanceIndeces_.clear(); @@ -88,7 +88,7 @@ void PipgMpcSolver::reset() { computeControllerTimer_.reset(); } -std::string PipgMpcSolver::getBenchmarkingInformationPIPG() const { +std::string SlpSolver::getBenchmarkingInformationPIPG() const { const auto GGTMultiplication = GGTMultiplication_.getTotalInMilliseconds(); const auto preConditioning = preConditioning_.getTotalInMilliseconds(); const auto lambdaEstimation = lambdaEstimation_.getTotalInMilliseconds(); @@ -117,7 +117,7 @@ std::string PipgMpcSolver::getBenchmarkingInformationPIPG() const { return infoStream.str(); } -std::string PipgMpcSolver::getBenchmarkingInformation() const { +std::string SlpSolver::getBenchmarkingInformation() const { const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds(); @@ -143,18 +143,18 @@ std::string PipgMpcSolver::getBenchmarkingInformation() const { return infoStream.str(); } -const std::vector<PerformanceIndex>& PipgMpcSolver::getIterationsLog() const { +const std::vector<PerformanceIndex>& SlpSolver::getIterationsLog() const { if (performanceIndeces_.empty()) { - throw std::runtime_error("[PipgMpcSolver]: No performance log yet, no problem solved yet?"); + throw std::runtime_error("[SlpSolver]: No performance log yet, no problem solved yet?"); } else { return performanceIndeces_; } } -void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { +void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; - std::cerr << "\n+++++++++++++ PIPG solver is initialized +++++++++++++"; + std::cerr << "\n+++++++++++++ SLP solver is initialized ++++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; } @@ -215,16 +215,16 @@ void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\nConvergence : " << toString(convergence) << "\n"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; - std::cerr << "\n+++++++++++++ PIPG solver has terminated +++++++++++++"; + std::cerr << "\n+++++++++++++ SLP solver has terminated ++++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; } } -void PipgMpcSolver::runParallel(std::function<void(int)> taskFunction) { +void SlpSolver::runParallel(std::function<void(int)> taskFunction) { threadPool_.runParallel(std::move(taskFunction), settings_.nThreads); } -PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_t& delta_x0) { +SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta_x0) { // Solve the QP OcpSubproblemSolution solution; auto& deltaXSol = solution.deltaXSol; @@ -283,13 +283,13 @@ PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_ return solution; } -PrimalSolution PipgMpcSolver::toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { +PrimalSolution SlpSolver::toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u)); } -PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u) { +PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, + const vector_array_t& x, const vector_array_t& u) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -362,8 +362,8 @@ PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<Annot return totalPerformance; } -PerformanceIndex PipgMpcSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u) { +PerformanceIndex SlpSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -408,9 +408,9 @@ PerformanceIndex PipgMpcSolver::computePerformance(const std::vector<AnnotatedTi return totalPerformance; } -slp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, - const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, - vector_array_t& u) { +slp::StepInfo SlpSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u) { using StepType = FilterLinesearch::StepType; /* @@ -501,7 +501,7 @@ slp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const st return stepInfo; } -slp::Convergence PipgMpcSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const slp::StepInfo& stepInfo) const { +slp::Convergence SlpSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const slp::StepInfo& stepInfo) const { using Convergence = slp::Convergence; if ((iteration + 1) >= settings_.slpIteration) { // Converged because the next iteration would exceed the specified number of iterations diff --git a/ocs2_slp/src/lintTarget.cpp b/ocs2_slp/src/lintTarget.cpp new file mode 100644 index 000000000..4eb308cc2 --- /dev/null +++ b/ocs2_slp/src/lintTarget.cpp @@ -0,0 +1,43 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <ocs2_slp/pipg/PipgSettings.h> +#include <ocs2_slp/pipg/PipgSolver.h> +#include <ocs2_slp/pipg/PipgSolverStatus.h> + +#include <ocs2_slp/Helpers.h> +#include <ocs2_slp/SlpMpc.h> +#include <ocs2_slp/SlpSettings.h> +#include <ocs2_slp/SlpSolver.h> +#include <ocs2_slp/SlpSolverStatus.h> + +// dummy target for clang toolchain +int main() { + return 0; +} diff --git a/ocs2_pipg/src/PipgSettings.cpp b/ocs2_slp/src/pipg/PipgSettings.cpp similarity index 98% rename from ocs2_pipg/src/PipgSettings.cpp rename to ocs2_slp/src/pipg/PipgSettings.cpp index 8ae0661e8..78c28ca3c 100644 --- a/ocs2_pipg/src/PipgSettings.cpp +++ b/ocs2_slp/src/pipg/PipgSettings.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_pipg/PipgSettings.h" +#include "ocs2_slp/pipg/PipgSettings.h" #include <iostream> diff --git a/ocs2_pipg/src/PipgSolver.cpp b/ocs2_slp/src/pipg/PipgSolver.cpp similarity index 99% rename from ocs2_pipg/src/PipgSolver.cpp rename to ocs2_slp/src/pipg/PipgSolver.cpp index 1a082c5d4..015914475 100644 --- a/ocs2_pipg/src/PipgSolver.cpp +++ b/ocs2_slp/src/pipg/PipgSolver.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_pipg/PipgSolver.h" +#include "ocs2_slp/pipg/PipgSolver.h" #include <condition_variable> #include <iostream> diff --git a/ocs2_pipg/test/testHelpers.cpp b/ocs2_slp/test/testHelpers.cpp similarity index 99% rename from ocs2_pipg/test/testHelpers.cpp rename to ocs2_slp/test/testHelpers.cpp index 847aae5bc..044e2738b 100644 --- a/ocs2_pipg/test/testHelpers.cpp +++ b/ocs2_slp/test/testHelpers.cpp @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> #include <ocs2_oc/test/testProblemsGeneration.h> -#include "ocs2_pipg/Helpers.h" +#include "ocs2_slp/Helpers.h" class HelperFunctionTest : public testing::Test { protected: diff --git a/ocs2_pipg/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp similarity index 99% rename from ocs2_pipg/test/testPipgSolver.cpp rename to ocs2_slp/test/testPipgSolver.cpp index 1c60248ac..3090746ce 100644 --- a/ocs2_pipg/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -34,7 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/test/testProblemsGeneration.h> #include <ocs2_qp_solver/QpSolver.h> -#include "ocs2_pipg/PipgSolver.h" +#include "ocs2_slp/pipg/PipgSolver.h" ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, ocs2::scalar_t relativeTolerance, bool verbose) { diff --git a/ocs2_pipg/test/testSlpSolver.cpp b/ocs2_slp/test/testSlpSolver.cpp similarity index 97% rename from ocs2_pipg/test/testSlpSolver.cpp rename to ocs2_slp/test/testSlpSolver.cpp index 1838fd9fa..da423dad0 100644 --- a/ocs2_pipg/test/testSlpSolver.cpp +++ b/ocs2_slp/test/testSlpSolver.cpp @@ -33,7 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/synchronized_module/ReferenceManager.h> #include <ocs2_oc/test/testProblemsGeneration.h> -#include "ocs2_pipg/mpc/PipgMpcSolver.h" +#include "ocs2_slp/SlpSolver.h" namespace ocs2 { namespace { @@ -97,7 +97,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct const ocs2::vector_t initState = ocs2::vector_t::Ones(n); // Construct solver - ocs2::PipgMpcSolver solver(slpSettings, problem, zeroInitializer); + ocs2::SlpSolver solver(slpSettings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); // Solve From 3e71f0e5163891e1c01c5f31916ecb6fd92f2963 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Sun, 4 Dec 2022 17:56:10 +0100 Subject: [PATCH 390/512] fixing ballbot --- .../ocs2_ballbot/CMakeLists.txt | 3 ++- .../ocs2_ballbot/config/mpc/pipg.info | 12 ---------- .../ocs2_ballbot/config/mpc/task.info | 24 +++++++++++++++++++ .../include/ocs2_ballbot/BallbotInterface.h | 4 ++++ .../ocs2_ballbot/package.xml | 3 ++- .../ocs2_ballbot/src/BallbotInterface.cpp | 1 + .../ocs2_ballbot_ros/CMakeLists.txt | 14 +++++------ ...ballbot_pipg.launch => ballbot_slp.launch} | 2 +- .../ocs2_ballbot_ros/package.xml | 2 +- ...gBallbotNode.cpp => BallbotSlpMpcNode.cpp} | 21 +++------------- ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h | 2 +- ocs2_slp/src/pipg/PipgSettings.cpp | 4 ++-- 12 files changed, 48 insertions(+), 44 deletions(-) delete mode 100644 ocs2_robotic_examples/ocs2_ballbot/config/mpc/pipg.info rename ocs2_robotic_examples/ocs2_ballbot_ros/launch/{ballbot_pipg.launch => ballbot_slp.launch} (91%) rename ocs2_robotic_examples/ocs2_ballbot_ros/src/{PipgBallbotNode.cpp => BallbotSlpMpcNode.cpp} (77%) diff --git a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt index c2dfdd74f..577c679ce 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt @@ -7,8 +7,9 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CATKIN_PACKAGE_DEPENDENCIES pybind11_catkin ocs2_core - ocs2_ddp ocs2_mpc + ocs2_ddp + ocs2_slp ocs2_sqp ocs2_robotic_tools ocs2_python_interface diff --git a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/pipg.info b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/pipg.info deleted file mode 100644 index 8fa92d821..000000000 --- a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/pipg.info +++ /dev/null @@ -1,12 +0,0 @@ -pipg -{ - nThreads 8 - threadPriority 50 - maxNumIterations 7000 - absoluteTolerance 1e-3 - relativeTolerance 1e-2 - numScaling 3 - lowerBoundH 0.2 - checkTerminationInterval 10 - displayShortSummary false -} \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info index 3a7f860d4..0a07f162d 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info @@ -12,6 +12,30 @@ templateSwitchingTimes { } +; Multiple_Shooting SLP settings +slp +{ + dt 0.1 + slpIteration 5 + deltaTol 1e-3 + printSolverStatistics true + printSolverStatus false + printLinesearch false + integratorType RK2 + nThreads 4 + pipg + { + nThreads 4 + maxNumIterations 7000 + absoluteTolerance 1e-3 + relativeTolerance 1e-2 + numScaling 3 + lowerBoundH 0.2 + checkTerminationInterval 10 + displayShortSummary false + } +} + ; Multiple_Shooting SQP settings sqp { diff --git a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h index 9ecb270be..f8a1388c7 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h +++ b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h @@ -37,6 +37,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/rollout/TimeTriggeredRollout.h> #include <ocs2_oc/synchronized_module/ReferenceManager.h> #include <ocs2_robotic_tools/common/RobotInterface.h> +#include <ocs2_slp/SlpSettings.h> #include <ocs2_sqp/SqpSettings.h> // Ballbot @@ -69,6 +70,8 @@ class BallbotInterface final : public RobotInterface { const vector_t& getInitialState() { return initialState_; } + slp::Settings& slpSettings() { return slpSettings_; } + sqp::Settings& sqpSettings() { return sqpSettings_; } ddp::Settings& ddpSettings() { return ddpSettings_; } @@ -87,6 +90,7 @@ class BallbotInterface final : public RobotInterface { ddp::Settings ddpSettings_; mpc::Settings mpcSettings_; sqp::Settings sqpSettings_; + slp::Settings slpSettings_; OptimalControlProblem problem_; std::shared_ptr<ReferenceManager> referenceManagerPtr_; diff --git a/ocs2_robotic_examples/ocs2_ballbot/package.xml b/ocs2_robotic_examples/ocs2_ballbot/package.xml index 64b1eb1b4..3c65f409c 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot/package.xml @@ -16,8 +16,9 @@ <depend>pybind11_catkin</depend> <depend>ocs2_core</depend> - <depend>ocs2_ddp</depend> <depend>ocs2_mpc</depend> + <depend>ocs2_ddp</depend> + <depend>ocs2_slp</depend> <depend>ocs2_sqp</depend> <depend>ocs2_python_interface</depend> <depend>ocs2_robotic_tools</depend> diff --git a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp index a3b948e3d..83bd2b715 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp @@ -68,6 +68,7 @@ BallbotInterface::BallbotInterface(const std::string& taskFile, const std::strin ddpSettings_ = ddp::loadSettings(taskFile, "ddp"); mpcSettings_ = mpc::loadSettings(taskFile, "mpc"); sqpSettings_ = sqp::loadSettings(taskFile, "sqp"); + slpSettings_ = slp::loadSettings(taskFile, "slp"); /* * ReferenceManager & SolverSynchronizedModule diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt index c64ddf736..99f6d33bf 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt @@ -14,7 +14,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp - ocs2_pipg + ocs2_slp ocs2_ros_interfaces ocs2_robotic_tools ocs2_ballbot @@ -136,17 +136,17 @@ target_link_libraries(ballbot_sqp ) target_compile_options(ballbot_sqp PRIVATE ${OCS2_CXX_FLAGS}) -## PIPG node for ballbot -add_executable(ballbot_pipg - src/PipgBallbotNode.cpp +## SLP node for ballbot +add_executable(ballbot_slp + src/BallbotSlpMpcNode.cpp ) -add_dependencies(ballbot_pipg +add_dependencies(ballbot_slp ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(ballbot_pipg +target_link_libraries(ballbot_slp ${catkin_LIBRARIES} ) -target_compile_options(ballbot_pipg PRIVATE ${OCS2_CXX_FLAGS}) +target_compile_options(ballbot_slp PRIVATE ${OCS2_CXX_FLAGS}) ######################### ### CLANG TOOLING ### diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_pipg.launch b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch similarity index 91% rename from ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_pipg.launch rename to ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch index dd293a1dc..5c8eae9f4 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_pipg.launch +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_slp.launch @@ -11,7 +11,7 @@ <include file="$(find ocs2_ballbot_ros)/launch/multiplot.launch"/> </group> - <node pkg="ocs2_ballbot_ros" type="ballbot_pipg" name="ballbot_pipg" + <node pkg="ocs2_ballbot_ros" type="ballbot_slp" name="ballbot_slp" output="screen" args="$(arg task_name)" launch-prefix=""/> <node pkg="ocs2_ballbot_ros" type="ballbot_dummy_test" name="ballbot_dummy_test" diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml index cb9459c26..00a7b00b6 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml @@ -24,7 +24,7 @@ <depend>ocs2_ddp</depend> <depend>ocs2_mpc</depend> <depend>ocs2_sqp</depend> - <depend>ocs2_pipg</depend> + <depend>ocs2_slp</depend> <depend>ocs2_ros_interfaces</depend> <depend>ocs2_ballbot</depend> <depend>ocs2_robotic_tools</depend> diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/PipgBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp similarity index 77% rename from ocs2_robotic_examples/ocs2_ballbot_ros/src/PipgBallbotNode.cpp rename to ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp index a08d8792d..606841c7f 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/PipgBallbotNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp @@ -30,16 +30,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ros/init.h> #include <ros/package.h> -#include <ocs2_pipg/mpc/PipgMpc.h> #include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> +#include <ocs2_slp/SlpMpc.h> #include <ocs2_ballbot/BallbotInterface.h> -// Boost -#include <boost/filesystem/operations.hpp> -#include <boost/filesystem/path.hpp> - int main(int argc, char** argv) { const std::string robotName = "ballbot"; @@ -57,28 +53,17 @@ int main(int argc, char** argv) { // Robot interface const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; - const std::string pipgConfigFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/pipg.info"; const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); - // Load PIPG settings - boost::filesystem::path pipgConfigPath(pipgConfigFile); - if (boost::filesystem::exists(pipgConfigPath)) { - std::cerr << "[pipgBallbotExample] Loading pipg config file: " << pipgConfigFile << std::endl; - } else { - throw std::invalid_argument("[pipgBallbotExample] Pipg config file not found: " + pipgConfigPath.string()); - } - ocs2::pipg::Settings pipgSettings = ocs2::pipg::loadSettings(pipgConfigFile, "pipg"); - // ROS ReferenceManager std::shared_ptr<ocs2::RosReferenceManager> rosReferenceManagerPtr( new ocs2::RosReferenceManager(robotName, ballbotInterface.getReferenceManagerPtr())); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC - ocs2::PipgMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.sqpSettings(), pipgSettings, - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); - + ocs2::SlpMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.slpSettings(), ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h index 66cc53d3f..cccc91521 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h @@ -38,7 +38,7 @@ struct Settings { /** Number of threads used in the multi-threading scheme. */ size_t nThreads = 3; /** Priority of threads used in the multi-threading scheme. */ - int threadPriority = 99; + int threadPriority = 50; /** Maximum number of iterations of PIPG. */ size_t maxNumIterations = 3000; /** Termination criteria. **/ diff --git a/ocs2_slp/src/pipg/PipgSettings.cpp b/ocs2_slp/src/pipg/PipgSettings.cpp index 78c28ca3c..27b20d77f 100644 --- a/ocs2_slp/src/pipg/PipgSettings.cpp +++ b/ocs2_slp/src/pipg/PipgSettings.cpp @@ -46,7 +46,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, Settings settings; if (verbose) { - std::cerr << "\n #### PIPG Settings: {"; + std::cerr << " #### PIPG Settings: {\n"; } loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); @@ -63,7 +63,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, loadData::loadPtreeValue(pt, settings.displayShortSummary, fieldName + ".displayShortSummary", verbose); if (verbose) { - std::cerr << "}\n"; + std::cerr << " #### }\n"; } return settings; From 6a9bf30cae30da6881ee2da3d33614f9831957ff Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Sun, 4 Dec 2022 18:28:05 +0100 Subject: [PATCH 391/512] reverting the changes to legged_robot --- .../ocs2_legged_robot/config/mpc/pipg.info | 12 --- .../ocs2_legged_robot_ros/CMakeLists.txt | 16 ---- .../launch/legged_robot_pipg.launch | 50 ---------- .../ocs2_legged_robot_ros/package.xml | 1 - .../src/LeggedRobotPipgMpcNode.cpp | 93 ------------------- 5 files changed, 172 deletions(-) delete mode 100644 ocs2_robotic_examples/ocs2_legged_robot/config/mpc/pipg.info delete mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_pipg.launch delete mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPipgMpcNode.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/pipg.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/pipg.info deleted file mode 100644 index 381ff95e3..000000000 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/pipg.info +++ /dev/null @@ -1,12 +0,0 @@ -pipg -{ - nThreads 8 - threadPriority 50 - maxNumIterations 7000 - absoluteTolerance 1e-3 - relativeTolerance 1e-2 - numScaling 3 - lowerBoundH 5e-6 - checkTerminationInterval 10 - displayShortSummary false -} \ No newline at end of file diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt index 619eb400e..d5c30446e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt @@ -16,7 +16,6 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp - ocs2_pipg ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model @@ -119,21 +118,6 @@ target_link_libraries(legged_robot_sqp_mpc ) target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS}) -## PIPG-MPC node for legged robot -add_executable(legged_robot_pipg_mpc - src/LeggedRobotPipgMpcNode.cpp -) -add_dependencies(legged_robot_pipg_mpc - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(legged_robot_pipg_mpc - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) -target_compile_options(legged_robot_pipg_mpc PRIVATE ${OCS2_CXX_FLAGS}) - # Dummy node add_executable(legged_robot_dummy src/LeggedRobotDummyNode.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_pipg.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_pipg.launch deleted file mode 100644 index 0cdefc30f..000000000 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_pipg.launch +++ /dev/null @@ -1,50 +0,0 @@ -<?xml version="1.0" ?> - -<launch> - <!-- visualization config --> - <arg name="rviz" default="true" /> - <arg name="description_name" default="legged_robot_description"/> - <arg name="multiplot" default="false"/> - - <!-- The task file for the mpc. --> - <arg name="taskFile" default="$(find ocs2_legged_robot)/config/mpc/task.info"/> - <!-- The reference related config file of the robot --> - <arg name="referenceFile" default="$(find ocs2_legged_robot)/config/command/reference.info"/> - <!-- The URDF model of the robot --> - <arg name="urdfFile" default="$(find ocs2_robotic_assets)/resources/anymal_c/urdf/anymal.urdf"/> - <!-- The file defining gait definition --> - <arg name="gaitCommandFile" default="$(find ocs2_legged_robot)/config/command/gait.info"/> - <!-- The pipg configuration file --> - <arg name="pipgConfigFile" default="$(find ocs2_legged_robot)/config/mpc/pipg.info" /> - - <!-- rviz --> - <group if="$(arg rviz)"> - <param name="$(arg description_name)" textfile="$(arg urdfFile)"/> - <arg name="rvizconfig" default="$(find ocs2_legged_robot_ros)/rviz/legged_robot.rviz" /> - <node pkg="rviz" type="rviz" name="rviz" args="-d $(arg rvizconfig)" output="screen" /> - </group> - - <!-- multiplot --> - <group if="$(arg multiplot)"> - <include file="$(find ocs2_legged_robot_ros)/launch/multiplot.launch"/> - </group> - - <!-- make the files into global parameters --> - <param name="taskFile" value="$(arg taskFile)" /> - <param name="referenceFile" value="$(arg referenceFile)" /> - <param name="urdfFile" value="$(arg urdfFile)" /> - <param name="gaitCommandFile" value="$(arg gaitCommandFile)"/> - <param name="pipgConfigFile" value="$(arg pipgConfigFile)" /> - - <node pkg="ocs2_legged_robot_ros" type="legged_robot_pipg_mpc" name="legged_robot_pipg_mpc" - output="screen" launch-prefix=""/> - - <node pkg="ocs2_legged_robot_ros" type="legged_robot_dummy" name="legged_robot_dummy" - output="screen" launch-prefix="gnome-terminal --"/> - - <node pkg="ocs2_legged_robot_ros" type="legged_robot_target" name="legged_robot_target" - output="screen" launch-prefix="gnome-terminal --"/> - - <node pkg="ocs2_legged_robot_ros" type="legged_robot_gait_command" name="legged_robot_gait_command" - output="screen" launch-prefix="gnome-terminal --"/> -</launch> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml index a23aab486..aa2943272 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml @@ -24,7 +24,6 @@ <depend>ocs2_ddp</depend> <depend>ocs2_mpc</depend> <depend>ocs2_sqp</depend> - <depend>ocs2_pipg</depend> <depend>ocs2_robotic_tools</depend> <depend>ocs2_pinocchio_interface</depend> <depend>ocs2_centroidal_model</depend> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPipgMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPipgMpcNode.cpp deleted file mode 100644 index 071ed54af..000000000 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotPipgMpcNode.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/****************************************************************************** -Copyright (c) 2017, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - ******************************************************************************/ - -#include <ros/init.h> -#include <ros/package.h> - -#include <ocs2_legged_robot/LeggedRobotInterface.h> -#include <ocs2_pipg/mpc/PipgMpc.h> - -#include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> -#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> - -#include "ocs2_legged_robot_ros/gait/GaitReceiver.h" - -// Boost -#include <boost/filesystem/operations.hpp> -#include <boost/filesystem/path.hpp> - -using namespace ocs2; -using namespace legged_robot; - -int main(int argc, char** argv) { - const std::string robotName = "legged_robot"; - - // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; - // Get node parameters - std::string taskFile, urdfFile, referenceFile, pipgConfigFile; - nodeHandle.getParam("/taskFile", taskFile); - nodeHandle.getParam("/urdfFile", urdfFile); - nodeHandle.getParam("/referenceFile", referenceFile); - - nodeHandle.getParam("/pipgConfigFile", pipgConfigFile); - boost::filesystem::path pipgConfigPath(pipgConfigFile); - if (boost::filesystem::exists(pipgConfigPath)) { - std::cerr << "[pipgLeggedExample] Loading pipg config file: " << pipgConfigFile << std::endl; - } else { - throw std::invalid_argument("[pipgLeggedExample] Pipg config file not found: " + pipgConfigPath.string()); - } - pipg::Settings pipgSettings = pipg::loadSettings(pipgConfigFile, "pipg"); - - // Robot interface - LeggedRobotInterface interface(taskFile, urdfFile, referenceFile); - - // Gait receiver - auto gaitReceiverPtr = - std::make_shared<GaitReceiver>(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); - - // ROS ReferenceManager - auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(robotName, interface.getReferenceManagerPtr()); - rosReferenceManagerPtr->subscribe(nodeHandle); - - // MPC - PipgMpc mpc(interface.mpcSettings(), interface.sqpSettings(), pipgSettings, interface.getOptimalControlProblem(), - interface.getInitializer()); - - mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); - mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - - // Launch MPC ROS node - MPC_ROS_Interface mpcNode(mpc, robotName); - mpcNode.launchNodes(nodeHandle); - - // Successful exit - return 0; -} From 59a5b0137d35f4ad3d73d1eeedc1eda22d2ee8cf Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 5 Dec 2022 11:46:49 +0100 Subject: [PATCH 392/512] minor changes --- .../include/ocs2_oc/pre_condition/Scaling.h | 75 +++-- ocs2_oc/src/pre_condition/Scaling.cpp | 302 ++++++------------ ocs2_slp/src/SlpSolver.cpp | 9 +- 3 files changed, 149 insertions(+), 237 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h index d0269ad07..19de2f6fd 100644 --- a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h +++ b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + #pragma once #include <Eigen/Sparse> @@ -9,6 +38,27 @@ namespace ocs2 { +/** + * Calculates the scaling factor D, E and c, and scale the input dynamics, cost data in place in parallel. + * + * @param[in] : threadPool External thread pool. + * @param[in] x0 : Initial state + * @param[in] ocpSize : The size of the oc problem. + * @param[in] iteration : Number of iteration. + * @param[in, out] : dynamics The dynamics array of all time points. + * @param[in, out] : cost The cost array of all time points. + * @param[out] : DOut Scaling factor D + * @param[out] : EOut Scaling factor E + * @param[out] scalingVectors : Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components of this type of matrix for every + * timestamp. + * @param[out] cOut : Scaling factor c + */ +void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, + std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, + vector_array_t& scalingVectors, scalar_t& cOut); + /** * Calculate the scaling factor D, E and c, and scale the input matrix(vector) H, h and G in place. The scaled matrix(vector) are defined as * \f[ @@ -28,31 +78,6 @@ namespace ocs2 { void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut); -/** - * @brief Calculate the scaling factor D, E and c, and scale the input dynamics, cost data in place in parallel. - * - * - * @param[in] x0 Initial state - * @param[in] ocpSize The size of the oc problem. - * @param[in] iteration Number of iteration. - * @param[in, out] dynamics The dynamics array od all time points. - * @param[in, out] cost The cost array of all time points. - * @param[out] DOut Scaling factor D - * @param[out] EOut Scaling factor E - * @param[out] scalingVectors Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. - * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components of this type of matrix for every - * timestamp. - * @param[out] cOut Scaling factor c - * @param[in] threadPool External thread pool. - * @param[in] H For test only. Can be removed. - * @param[in] h For test only. Can be removed. - * @param[in] G For test only. Can be removed. - */ -void preConditioningInPlaceInParallel(const vector_t& x0, const OcpSize& ocpSize, const int iteration, - std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, - vector_array_t& scalingVectors, scalar_t& cOut, ThreadPool& threadPool, - const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G); /** * @brief Scale the dynamics and cost array in place and construct scaling vector array from the given scaling factors E, D and c. * diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/pre_condition/Scaling.cpp index 7f5c47fa4..0ffef3228 100644 --- a/ocs2_oc/src/pre_condition/Scaling.cpp +++ b/ocs2_oc/src/pre_condition/Scaling.cpp @@ -4,6 +4,7 @@ #include <numeric> namespace ocs2 { + // Internal helper functions namespace { @@ -87,14 +88,17 @@ void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eige template <typename T> void scaleMatrixInPlace(const vector_t* rowScale, const vector_t* colScale, Eigen::MatrixBase<T>& mat) { - if (rowScale != nullptr) mat.array().colwise() *= rowScale->array(); - if (colScale != nullptr) mat *= colScale->asDiagonal(); + if (rowScale != nullptr) { + mat.array().colwise() *= rowScale->array(); + } + if (colScale != nullptr) { + mat *= colScale->asDiagonal(); + } } -void invSqrtInfNormInParallel(const std::vector<VectorFunctionLinearApproximation>& dynamics, +void invSqrtInfNormInParallel(ThreadPool& threadPool, const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& scalingVectors, - const int N, const std::function<void(vector_t&)>& limitScaling, vector_array_t& D, vector_array_t& E, - ThreadPool& threadPool) { + const int N, const std::function<void(vector_t&)>& limitScaling, vector_array_t& D, vector_array_t& E) { // Helper function auto procedure = [&limitScaling](vector_t& dst, const vector_t& norm) { dst = norm; @@ -121,41 +125,29 @@ void invSqrtInfNormInParallel(const std::vector<VectorFunctionLinearApproximatio procedure(D[2 * N - 1], matrixInfNormCols(cost[N].dfdxx, scalingVectors[N - 1].asDiagonal().toDenseMatrix())); } -void scaleDataOneStepInPlaceInParallel(const vector_array_t& D, const vector_array_t& E, const int N, +void scaleDataOneStepInPlaceInParallel(ThreadPool& threadPool, const vector_array_t& D, const vector_array_t& E, const int N, std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors, - ThreadPool& threadPool) { - scaleMatrixInPlace(&(D[0]), &(D[0]), cost.front().dfduu); + std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors) { scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdu); + scaleMatrixInPlace(&(D[0]), &(D[0]), cost.front().dfduu); scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdux); std::atomic_int timeStamp{1}; auto scaleCost = [&](int workerId) { int k; - while ((k = timeStamp++) < N) { - auto& Q = cost[k].dfdxx; - auto& R = cost[k].dfduu; - auto& P = cost[k].dfdux; - auto& q = cost[k].dfdx; - auto& r = cost[k].dfdu; - - scaleMatrixInPlace(&(D[2 * k - 1]), &(D[2 * k - 1]), Q); - scaleMatrixInPlace(&(D[2 * k - 1]), nullptr, q); - scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k - 1]), P); - - scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k]), R); - scaleMatrixInPlace(&(D[2 * k]), nullptr, r); + while ((k = timeStamp++) <= N) { + scaleMatrixInPlace(&(D[2 * k - 1]), nullptr, cost[k].dfdx); + scaleMatrixInPlace(&(D[2 * k - 1]), &(D[2 * k - 1]), cost[k].dfdxx); + if (cost[k].dfdu.size() > 0) { + scaleMatrixInPlace(&(D[2 * k]), nullptr, cost[k].dfdu); + scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k]), cost[k].dfduu); + scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k - 1]), cost[k].dfdux); + } } }; - threadPool.runParallel(std::move(scaleCost), threadPool.numThreads() + 1U); - scaleMatrixInPlace(&(D.back()), &(D.back()), cost[N].dfdxx); - scaleMatrixInPlace(&(D.back()), nullptr, cost[N].dfdx); - // Scale G & g - auto& B_0 = dynamics[0].dfdu; - auto& C_0 = scalingVectors[0]; /** * \f$ * \tilde{B} = E * B * D, @@ -163,95 +155,33 @@ void scaleDataOneStepInPlaceInParallel(const vector_array_t& D, const vector_arr * \tilde{g} = E * g, * \f$ */ - scaleMatrixInPlace(&(E[0]), &(D[0]), B_0); - C_0.array() *= E[0].array() * D[1].array(); - - scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].dfdx); scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].f); + scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].dfdx); + scalingVectors[0].array() *= E[0].array() * D[1].array(); + if (dynamics[0].dfdu.size() > 0) { + scaleMatrixInPlace(&(E[0]), &(D[0]), dynamics[0].dfdu); + } timeStamp = 1; auto scaleConstraints = [&](int workerId) { int k; while ((k = timeStamp++) < N) { - auto& A_k = dynamics[k].dfdx; - auto& B_k = dynamics[k].dfdu; - auto& b_k = dynamics[k].f; - auto& C_k = scalingVectors[k]; - - scaleMatrixInPlace(&(E[k]), &(D[2 * k - 1]), A_k); - scaleMatrixInPlace(&(E[k]), &(D[2 * k]), B_k); - C_k.array() *= E[k].array() * D[2 * k + 1].array(); - - scaleMatrixInPlace(&(E[k]), nullptr, b_k); + scaleMatrixInPlace(&(E[k]), nullptr, dynamics[k].f); + scaleMatrixInPlace(&(E[k]), &(D[2 * k - 1]), dynamics[k].dfdx); + scalingVectors[k].array() *= E[k].array() * D[2 * k + 1].array(); + if (dynamics[k].dfdu.size() > 0) { + scaleMatrixInPlace(&(E[k]), &(D[2 * k]), dynamics[k].dfdu); + } } }; - threadPool.runParallel(std::move(scaleConstraints), threadPool.numThreads() + 1U); } -} // namespace - -void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, - const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut) { - const int nz = H.rows(); - const int nc = G.rows(); - - // Init output - cOut = 1.0; - DOut.setOnes(nz); - EOut.setOnes(nc); - - for (int i = 0; i < iteration; i++) { - vector_t D, E; - const vector_t infNormOfHCols = matrixInfNormCols(H); - const vector_t infNormOfGCols = matrixInfNormCols(G); - - D = infNormOfHCols.array().max(infNormOfGCols.array()); - E = matrixInfNormRows(G); - - for (int i = 0; i < D.size(); i++) { - if (D(i) > 1e+4) D(i) = 1e+4; - if (D(i) < 1e-4) D(i) = 1.0; - } - - for (int i = 0; i < E.size(); i++) { - if (E(i) > 1e+4) E(i) = 1e+4; - if (E(i) < 1e-4) E(i) = 1.0; - } - - D = D.array().sqrt().inverse(); - E = E.array().sqrt().inverse(); - - scaleMatrixInPlace(D, D, H); - - scaleMatrixInPlace(E, D, G); - - h.array() *= D.array(); - - DOut = DOut.cwiseProduct(D); - EOut = EOut.cwiseProduct(E); - - scalar_t infNormOfh = h.lpNorm<Eigen::Infinity>(); - if (infNormOfh < 1e-4) infNormOfh = 1.0; - - const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); - scalar_t c_temp = std::max(infNormOfHColsUpdated.mean(), infNormOfh); - if (c_temp > 1e+4) c_temp = 1e+4; - if (c_temp < 1e-4) c_temp = 1.0; - - scalar_t gamma = 1.0 / c_temp; - - H *= gamma; - h *= gamma; - - cOut *= gamma; - } -} +} // anonymous namespace -void preConditioningInPlaceInParallel(const vector_t& x0, const OcpSize& ocpSize, const int iteration, +void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, - vector_array_t& scalingVectors, scalar_t& cOut, ThreadPool& threadPool, - const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G) { + vector_array_t& scalingVectors, scalar_t& cOut) { const int N = ocpSize.numStages; if (N < 1) { throw std::runtime_error("[preConditioningInPlaceInParallel] The number of stages cannot be less than 1."); @@ -276,20 +206,10 @@ void preConditioningInPlaceInParallel(const vector_t& x0, const OcpSize& ocpSize scalingVectors[i].setOnes(ocpSize.numStates[i + 1]); } - // /** - // * Test start - // */ - // Eigen::SparseMatrix<scalar_t> HTest = H; - // vector_t hTest = h; - // Eigen::SparseMatrix<scalar_t> GTest = G; - // /** - // * Test end - // */ - vector_array_t D(2 * N), E(N); for (int i = 0; i < iteration; i++) { - invSqrtInfNormInParallel(dynamics, cost, scalingVectors, N, limitScaling, D, E, threadPool); - scaleDataOneStepInPlaceInParallel(D, E, N, dynamics, cost, scalingVectors, threadPool); + invSqrtInfNormInParallel(threadPool, dynamics, cost, scalingVectors, N, limitScaling, D, E); + scaleDataOneStepInPlaceInParallel(threadPool, D, E, N, dynamics, cost, scalingVectors); for (int k = 0; k < DOut.size(); k++) { DOut[k].array() *= D[k].array(); @@ -312,14 +232,14 @@ void preConditioningInPlaceInParallel(const vector_t& x0, const OcpSize& ocpSize } sumOfInfNormOfH += matrixInfNormCols(cost[N].dfdxx).derived().sum(); - int numDecisionVariables = std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), - std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0)); + const int numDecisionVariables = std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), (int)0) + + std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0); scalar_t c_temp = std::max(sumOfInfNormOfH / static_cast<scalar_t>(numDecisionVariables), infNormOfh); if (c_temp > 1e+4) c_temp = 1e+4; if (c_temp < 1e-4) c_temp = 1.0; - scalar_t gamma = 1.0 / c_temp; + const scalar_t gamma = 1.0 / c_temp; for (int k = 0; k <= N; ++k) { cost[k].dfdxx *= gamma; @@ -330,95 +250,63 @@ void preConditioningInPlaceInParallel(const vector_t& x0, const OcpSize& ocpSize } cOut *= gamma; + } +} + +void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, + const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut) { + const int nz = H.rows(); + const int nc = G.rows(); - // /** - // * Test start - // */ - // auto isEqual = [](scalar_t a, scalar_t b) -> bool { - // return std::abs(a - b) < std::numeric_limits<scalar_t>::epsilon() * std::min(std::abs(a), std::abs(b)); - // }; - // vector_t DTest, ETest; - // vector_t infNormOfHCols = matrixInfNormCols(HTest); - // vector_t infNormOfGCols = matrixInfNormCols(GTest); - - // DTest = infNormOfHCols.array().max(infNormOfGCols.array()); - // ETest = matrixInfNormRows(GTest); - - // limitScaling(DTest); - // limitScaling(ETest); - - // std::cerr << "DTest: " << DTest.transpose() << "\n"; - // std::cerr << "ETest : " << ETest.transpose() << "\n"; - // std::cerr << "G :\n " << GTest.toDense() << "\n"; - - // DTest = DTest.array().sqrt().inverse(); - // ETest = ETest.array().sqrt().inverse(); - - // scaleMatrixInPlace(DTest, DTest, HTest); - - // scaleMatrixInPlace(ETest, DTest, GTest); - - // hTest.array() *= DTest.array(); - - // scalar_t infNormOfhTest = hTest.lpNorm<Eigen::Infinity>(); - // if (infNormOfhTest < 1e-4) infNormOfhTest = 1.0; - - // const vector_t infNormOfHColsUpdated = matrixInfNormCols(HTest); - // scalar_t c_tempTest = std::max(infNormOfHColsUpdated.mean(), infNormOfhTest); - // if (c_tempTest > 1e+4) c_tempTest = 1e+4; - // if (c_tempTest < 1e-4) c_tempTest = 1.0; - - // scalar_t gammaTest = 1.0 / c_tempTest; - - // HTest *= gamma; - // hTest *= gamma; - - // ocs2::vector_t DStacked(H.cols()), EStacked(G.rows()); - // int curRow = 0; - // for (auto& v : D) { - // DStacked.segment(curRow, v.size()) = v; - // curRow += v.size(); - // } - // curRow = 0; - // for (auto& v : E) { - // EStacked.segment(curRow, v.size()) = v; - // curRow += v.size(); - // } - // if (!DStacked.isApprox(DTest)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "DStacked: " << DStacked.transpose() << "\n"; - // std::cerr << "DTest : " << DTest.transpose() << "\n"; - // std::cerr << "diff : " << (DStacked - DTest).transpose() << std::endl; - // std::cerr << "R: \n" << cost[0].dfduu << std::endl; - // std::cerr << "B: \n" << dynamics[0].dfdu << std::endl; - // } - // if (!EStacked.isApprox(ETest)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "EStacked: " << EStacked.transpose() << "\n"; - // std::cerr << "ETest: " << ETest.transpose() << "\n"; - // std::cerr << "diff: " << (ETest - EStacked).transpose() << std::endl; - // } - // if (!isEqual(infNormOfhTest, infNormOfh)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "infNormOfhTest: " << infNormOfhTest << "\n"; - // std::cerr << "infNormOfh: " << infNormOfh << "\n"; - // std::cerr << "diff: " << (infNormOfhTest - infNormOfh) << std::endl; - // } - // if (!isEqual(infNormOfHColsUpdated.sum(), sumOfInfNormOfH)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "infNormOfHColsUpdated.sum(): " << infNormOfHColsUpdated.sum() << "\n"; - // std::cerr << "sumOfInfNormOfH: " << sumOfInfNormOfH << "\n"; - // std::cerr << "diff: " << (infNormOfHColsUpdated.sum() - sumOfInfNormOfH) << std::endl; - // } - // if (!isEqual(c_temp, c_tempTest)) { - // std::cerr << "i: " << i << "\n"; - // std::cerr << "c_temp: " << c_temp << "\n"; - // std::cerr << "c_tempTest: " << c_tempTest << "\n"; - // std::cerr << "diff: " << (c_temp - c_tempTest) << std::endl; - // } - // /** - // * Test end - // */ + // Init output + cOut = 1.0; + DOut.setOnes(nz); + EOut.setOnes(nc); + + for (int i = 0; i < iteration; i++) { + vector_t D, E; + const vector_t infNormOfHCols = matrixInfNormCols(H); + const vector_t infNormOfGCols = matrixInfNormCols(G); + + D = infNormOfHCols.array().max(infNormOfGCols.array()); + E = matrixInfNormRows(G); + + for (int i = 0; i < D.size(); i++) { + if (D(i) > 1e+4) D(i) = 1e+4; + if (D(i) < 1e-4) D(i) = 1.0; + } + + for (int i = 0; i < E.size(); i++) { + if (E(i) > 1e+4) E(i) = 1e+4; + if (E(i) < 1e-4) E(i) = 1.0; + } + + D = D.array().sqrt().inverse(); + E = E.array().sqrt().inverse(); + + scaleMatrixInPlace(D, D, H); + + scaleMatrixInPlace(E, D, G); + + h.array() *= D.array(); + + DOut = DOut.cwiseProduct(D); + EOut = EOut.cwiseProduct(E); + + scalar_t infNormOfh = h.lpNorm<Eigen::Infinity>(); + if (infNormOfh < 1e-4) infNormOfh = 1.0; + + const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); + scalar_t c_temp = std::max(infNormOfHColsUpdated.mean(), infNormOfh); + if (c_temp > 1e+4) c_temp = 1e+4; + if (c_temp < 1e-4) c_temp = 1.0; + + scalar_t gamma = 1.0 / c_temp; + + H *= gamma; + h *= gamma; + + cOut *= gamma; } } diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 2631c3bd9..647dedfbc 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -238,9 +238,8 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta scalar_t c; preConditioning_.startTimer(); - preConditioningInPlaceInParallel(delta_x0, pipgSolver_.size(), pipgSolver_.settings().numScaling, dynamics_, cost_, D, E, scalingVectors, - c, pipgSolver_.getThreadPool(), Eigen::SparseMatrix<scalar_t>(), vector_t(), - Eigen::SparseMatrix<scalar_t>()); + preConditioningInPlaceInParallel(pipgSolver_.getThreadPool(), delta_x0, pipgSolver_.size(), pipgSolver_.settings().numScaling, dynamics_, + cost_, D, E, scalingVectors, c); preConditioning_.endTimer(); lambdaEstimation_.startTimer(); @@ -272,11 +271,11 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta ScalarFunctionQuadraticApproximation(), VectorFunctionLinearApproximation()); pipgSolver_.getStateInputTrajectoriesSolution(deltaXSol, deltaUSol); - pipgSolver_.descaleSolution(D, deltaXSol, deltaUSol); - // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] solution.armijoDescentMetric = armijoDescentMetric(cost_, deltaXSol, deltaUSol); + pipgSolver_.descaleSolution(D, deltaXSol, deltaUSol); + // remap the tilde delta u to real delta u multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); From d739c039f99b3cbcd7743f0d8317d2ff2e3c38ec Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 5 Dec 2022 13:06:11 +0100 Subject: [PATCH 393/512] pre_condition/Scaling is simplified --- ocs2_oc/CMakeLists.txt | 9 +- .../oc_problem/OcpMatrixConstruction.h | 29 +++ .../include/ocs2_oc/pre_condition/Scaling.h | 35 --- ocs2_oc/src/lintTarget.cpp | 6 +- .../src/oc_problem/OcpMatrixConstruction.cpp | 29 +++ ocs2_oc/src/pre_condition/Scaling.cpp | 216 +++-------------- .../test/include/ocs2_oc/test/SparseScaling.h | 77 ++++++ .../testMatrixConstruction.cpp | 31 ++- ocs2_oc/test/pre_condition/SparseScaling.cpp | 228 ++++++++++++++++++ .../test/{ => pre_condition}/testScaling.cpp | 48 +++- 10 files changed, 465 insertions(+), 243 deletions(-) create mode 100644 ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h rename ocs2_oc/test/{ => oc_problem}/testMatrixConstruction.cpp (59%) create mode 100644 ocs2_oc/test/pre_condition/SparseScaling.cpp rename ocs2_oc/test/{ => pre_condition}/testScaling.cpp (76%) diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index f07aee083..7d6993de3 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -58,9 +58,9 @@ add_library(${PROJECT_NAME} src/oc_problem/OptimalControlProblem.cpp src/oc_problem/LoopshapingOptimalControlProblem.cpp src/oc_problem/OptimalControlProblemHelperFunction.cpp - src/oc_solver/SolverBase.cpp - src/oc_problem/OcpSize.cpp src/oc_problem/OcpMatrixConstruction.cpp + src/oc_problem/OcpSize.cpp + src/oc_solver/SolverBase.cpp src/pre_condition/Scaling.cpp src/rollout/PerformanceIndicesRollout.cpp src/rollout/RolloutBase.cpp @@ -192,7 +192,7 @@ target_link_libraries(test_trajectory_spreading ) catkin_add_gtest(test_matrix_construction - test/testMatrixConstruction.cpp + test/oc_problem/testMatrixConstruction.cpp ) target_link_libraries(test_matrix_construction ${PROJECT_NAME} @@ -201,7 +201,8 @@ target_link_libraries(test_matrix_construction ) catkin_add_gtest(test_scaling - test/testScaling.cpp + test/pre_condition/SparseScaling.cpp + test/pre_condition/testScaling.cpp ) target_link_libraries(test_scaling ${PROJECT_NAME} diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h b/ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h index eecde465b..e2b90f4ea 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #pragma once #include <Eigen/Sparse> diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h index 19de2f6fd..91adf1f25 100644 --- a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h +++ b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h @@ -59,39 +59,4 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, vector_array_t& scalingVectors, scalar_t& cOut); -/** - * Calculate the scaling factor D, E and c, and scale the input matrix(vector) H, h and G in place. The scaled matrix(vector) are defined as - * \f[ - * \tilde{H} = cDHD, \tilde{h} = cDh, - * \tilde{G} = EGD, \tilde{g} = Eg - * \f] - * - * - * @param[in, out] H Cost hessian. - * @param[in, out] h Linear cost term. - * @param[in, out] G Linear constraint matrix. - * @param[in] iteration Number of iteration. - * @param[out] DOut Scaling factor D - * @param[out] EOut Scaling factor E - * @param[out] cOut Scaling factor c - */ -void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, - const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut); - -/** - * @brief Scale the dynamics and cost array in place and construct scaling vector array from the given scaling factors E, D and c. - * - * @param[in] ocpSize The size of the oc problem. - * @param[in] D Scaling factor D - * @param[in] E Scaling factor E - * @param[in] c Scaling factor c - * @param[in, out] dynamics Dynamics array - * @param[in, out] cost Cost array - * @param[out] scalingVectors Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. - * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components of this type of matrix for every - * timestamp. - */ -void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, - std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, - std::vector<vector_t>& scalingVectors); } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 91738cca1..604295b32 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -46,12 +46,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // oc_problem #include <ocs2_oc/oc_problem/LoopshapingOptimalControlProblem.h> -#include <ocs2_oc/oc_problem/OptimalControlProblem.h> -#include <ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h> - -// oc_problem #include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> #include <ocs2_oc/oc_problem/OcpSize.h> +#include <ocs2_oc/oc_problem/OptimalControlProblem.h> +#include <ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h> // oc_solver #include <ocs2_oc/oc_solver/SolverBase.h> diff --git a/ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp b/ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp index cf05ca7ad..dc58ae2e5 100644 --- a/ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp +++ b/ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_oc/oc_problem/OcpMatrixConstruction.h" #include <numeric> diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/pre_condition/Scaling.cpp index 0ffef3228..a4885fbf2 100644 --- a/ocs2_oc/src/pre_condition/Scaling.cpp +++ b/ocs2_oc/src/pre_condition/Scaling.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include "ocs2_oc/pre_condition/Scaling.h" #include <atomic> @@ -8,19 +37,6 @@ namespace ocs2 { // Internal helper functions namespace { -vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat) { - vector_t infNorm; - infNorm.setZero(mat.rows()); - - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - int i = it.row(); - infNorm(i) = std::max(infNorm(i), std::abs(it.value())); - } - } - return infNorm; -} - template <typename T> vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat) { if (mat.rows() == 0 || mat.cols() == 0) { @@ -42,18 +58,6 @@ vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixB } } -vector_t matrixInfNormCols(const Eigen::SparseMatrix<scalar_t>& mat) { - vector_t infNorm; - infNorm.setZero(mat.cols()); - - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - infNorm(j) = std::max(infNorm(j), std::abs(it.value())); - } - } - return infNorm; -} - template <typename T> vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat) { if (mat.rows() == 0 || mat.cols() == 0) { @@ -75,17 +79,6 @@ vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixB } } -void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix<scalar_t>& mat) { - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - if (it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1) { - throw std::runtime_error("[scaleMatrixInPlace] it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1"); - } - it.valueRef() *= rowScale(it.row()) * colScale(j); - } - } -} - template <typename T> void scaleMatrixInPlace(const vector_t* rowScale, const vector_t* colScale, Eigen::MatrixBase<T>& mat) { if (rowScale != nullptr) { @@ -253,155 +246,4 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 } } -void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, - const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut) { - const int nz = H.rows(); - const int nc = G.rows(); - - // Init output - cOut = 1.0; - DOut.setOnes(nz); - EOut.setOnes(nc); - - for (int i = 0; i < iteration; i++) { - vector_t D, E; - const vector_t infNormOfHCols = matrixInfNormCols(H); - const vector_t infNormOfGCols = matrixInfNormCols(G); - - D = infNormOfHCols.array().max(infNormOfGCols.array()); - E = matrixInfNormRows(G); - - for (int i = 0; i < D.size(); i++) { - if (D(i) > 1e+4) D(i) = 1e+4; - if (D(i) < 1e-4) D(i) = 1.0; - } - - for (int i = 0; i < E.size(); i++) { - if (E(i) > 1e+4) E(i) = 1e+4; - if (E(i) < 1e-4) E(i) = 1.0; - } - - D = D.array().sqrt().inverse(); - E = E.array().sqrt().inverse(); - - scaleMatrixInPlace(D, D, H); - - scaleMatrixInPlace(E, D, G); - - h.array() *= D.array(); - - DOut = DOut.cwiseProduct(D); - EOut = EOut.cwiseProduct(E); - - scalar_t infNormOfh = h.lpNorm<Eigen::Infinity>(); - if (infNormOfh < 1e-4) infNormOfh = 1.0; - - const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); - scalar_t c_temp = std::max(infNormOfHColsUpdated.mean(), infNormOfh); - if (c_temp > 1e+4) c_temp = 1e+4; - if (c_temp < 1e-4) c_temp = 1.0; - - scalar_t gamma = 1.0 / c_temp; - - H *= gamma; - h *= gamma; - - cOut *= gamma; - } -} - -void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, - std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, - std::vector<vector_t>& scalingVectors) { - const int N = ocpSize.numStages; - if (N < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); - } - - scalingVectors.resize(N); - - // Scale H & h - const int nu_0 = ocpSize.numInputs.front(); - // Scale row - Pre multiply D - cost.front().dfduu.array().colwise() *= c * D.head(nu_0).array(); - // Scale col - Post multiply D - cost.front().dfduu *= D.head(nu_0).asDiagonal(); - - cost.front().dfdu.array() *= c * D.head(nu_0).array(); - - cost.front().dfdux.array().colwise() *= c * D.head(nu_0).array(); - - int currRow = nu_0; - for (int k = 1; k < N; ++k) { - const int nx_k = ocpSize.numStates[k]; - const int nu_k = ocpSize.numInputs[k]; - auto& Q = cost[k].dfdxx; - auto& R = cost[k].dfduu; - auto& P = cost[k].dfdux; - auto& q = cost[k].dfdx; - auto& r = cost[k].dfdu; - - Q.array().colwise() *= c * D.segment(currRow, nx_k).array(); - Q *= D.segment(currRow, nx_k).asDiagonal(); - q.array() *= c * D.segment(currRow, nx_k).array(); - - P *= D.segment(currRow, nx_k).asDiagonal(); - currRow += nx_k; - - R.array().colwise() *= c * D.segment(currRow, nu_k).array(); - R *= D.segment(currRow, nu_k).asDiagonal(); - r.array() *= c * D.segment(currRow, nu_k).array(); - - P.array().colwise() *= c * D.segment(currRow, nu_k).array(); - - currRow += nu_k; - } - - const int nx_N = ocpSize.numStates[N]; - cost[N].dfdxx.array().colwise() *= c * D.tail(nx_N).array(); - cost[N].dfdxx *= D.tail(nx_N).asDiagonal(); - cost[N].dfdx.array() *= c * D.tail(nx_N).array(); - - // Scale G & g - auto& B_0 = dynamics[0].dfdu; - auto& C_0 = scalingVectors[0]; - const int nx_1 = ocpSize.numStates[1]; - - // \tilde{B} = E * B * D, - // scaling = E * D, - // \tilde{g} = E * g - B_0.array().colwise() *= E.head(nx_1).array(); - B_0 *= D.head(nu_0).asDiagonal(); - - C_0 = E.head(nx_1).array() * D.segment(nu_0, nx_1).array(); - - dynamics[0].dfdx.array().colwise() *= E.head(nx_1).array(); - dynamics[0].f.array() *= E.head(nx_1).array(); - - currRow = nx_1; - int currCol = nu_0; - for (int k = 1; k < N; ++k) { - const int nu_k = ocpSize.numInputs[k]; - const int nx_k = ocpSize.numStates[k]; - const int nx_next = ocpSize.numStates[k + 1]; - auto& A_k = dynamics[k].dfdx; - auto& B_k = dynamics[k].dfdu; - auto& b_k = dynamics[k].f; - auto& C_k = scalingVectors[k]; - - A_k.array().colwise() *= E.segment(currRow, nx_next).array(); - A_k *= D.segment(currCol, nx_k).asDiagonal(); - - B_k.array().colwise() *= E.segment(currRow, nx_next).array(); - B_k *= D.segment(currCol + nx_k, nu_k).asDiagonal(); - - C_k = E.segment(currRow, nx_next).array() * D.segment(currCol + nx_k + nu_k, nx_next).array(); - - b_k.array() *= E.segment(currRow, nx_next).array(); - - currRow += nx_next; - currCol += nx_k + nu_k; - } -} - } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h b/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h new file mode 100644 index 000000000..5d5201393 --- /dev/null +++ b/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h @@ -0,0 +1,77 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#pragma once + +#include <Eigen/Sparse> + +#include <ocs2_core/Types.h> +#include <ocs2_core/thread_support/ThreadPool.h> + +#include "ocs2_oc/oc_problem/OcpSize.h" + +namespace ocs2 { + +/** + * Calculate the scaling factor D, E and c, and scale the input matrix(vector) H, h and G in place. The scaled matrix(vector) are defined as + * \f[ + * \tilde{H} = cDHD, \tilde{h} = cDh, + * \tilde{G} = EGD, \tilde{g} = Eg + * \f] + * + * + * @param[in, out] H Cost hessian. + * @param[in, out] h Linear cost term. + * @param[in, out] G Linear constraint matrix. + * @param[in] iteration Number of iteration. + * @param[out] DOut Scaling factor D + * @param[out] EOut Scaling factor E + * @param[out] cOut Scaling factor c + */ +void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, + const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut); + +/** + * @brief Scale the dynamics and cost array in place and construct scaling vector array from the given scaling factors E, D and c. + * + * @param[in] ocpSize The size of the oc problem. + * @param[in] D Scaling factor D + * @param[in] E Scaling factor E + * @param[in] c Scaling factor c + * @param[in, out] dynamics Dynamics array + * @param[in, out] cost Cost array + * @param[out] scalingVectors Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components of this type of matrix for every + * timestamp. + */ +void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, + std::vector<vector_t>& scalingVectors); + +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/test/testMatrixConstruction.cpp b/ocs2_oc/test/oc_problem/testMatrixConstruction.cpp similarity index 59% rename from ocs2_oc/test/testMatrixConstruction.cpp rename to ocs2_oc/test/oc_problem/testMatrixConstruction.cpp index 0911b3049..f78898330 100644 --- a/ocs2_oc/test/testMatrixConstruction.cpp +++ b/ocs2_oc/test/oc_problem/testMatrixConstruction.cpp @@ -1,8 +1,37 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <gtest/gtest.h> +#include "ocs2_oc/oc_problem/OcpSize.h" #include "ocs2_oc/oc_problem/OcpMatrixConstruction.h" -#include "ocs2_oc/oc_problem/OcpSize.h" #include "ocs2_oc/test/testProblemsGeneration.h" class MatrixConstructionTest : public testing::Test { diff --git a/ocs2_oc/test/pre_condition/SparseScaling.cpp b/ocs2_oc/test/pre_condition/SparseScaling.cpp new file mode 100644 index 000000000..79962654c --- /dev/null +++ b/ocs2_oc/test/pre_condition/SparseScaling.cpp @@ -0,0 +1,228 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_oc/test/SparseScaling.h" + +#include <atomic> +#include <numeric> + +namespace ocs2 { + +// Internal helper functions +namespace { + +vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat) { + vector_t infNorm; + infNorm.setZero(mat.rows()); + + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + int i = it.row(); + infNorm(i) = std::max(infNorm(i), std::abs(it.value())); + } + } + return infNorm; +} + +vector_t matrixInfNormCols(const Eigen::SparseMatrix<scalar_t>& mat) { + vector_t infNorm; + infNorm.setZero(mat.cols()); + + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + infNorm(j) = std::max(infNorm(j), std::abs(it.value())); + } + } + return infNorm; +} + +void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix<scalar_t>& mat) { + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + if (it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1) { + throw std::runtime_error("[scaleMatrixInPlace] it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1"); + } + it.valueRef() *= rowScale(it.row()) * colScale(j); + } + } +} +} // anonymous namespace + +void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, + const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut) { + const int nz = H.rows(); + const int nc = G.rows(); + + // Init output + cOut = 1.0; + DOut.setOnes(nz); + EOut.setOnes(nc); + + for (int i = 0; i < iteration; i++) { + vector_t D, E; + const vector_t infNormOfHCols = matrixInfNormCols(H); + const vector_t infNormOfGCols = matrixInfNormCols(G); + + D = infNormOfHCols.array().max(infNormOfGCols.array()); + E = matrixInfNormRows(G); + + for (int i = 0; i < D.size(); i++) { + if (D(i) > 1e+4) D(i) = 1e+4; + if (D(i) < 1e-4) D(i) = 1.0; + } + + for (int i = 0; i < E.size(); i++) { + if (E(i) > 1e+4) E(i) = 1e+4; + if (E(i) < 1e-4) E(i) = 1.0; + } + + D = D.array().sqrt().inverse(); + E = E.array().sqrt().inverse(); + + scaleMatrixInPlace(D, D, H); + + scaleMatrixInPlace(E, D, G); + + h.array() *= D.array(); + + DOut = DOut.cwiseProduct(D); + EOut = EOut.cwiseProduct(E); + + scalar_t infNormOfh = h.lpNorm<Eigen::Infinity>(); + if (infNormOfh < 1e-4) infNormOfh = 1.0; + + const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); + scalar_t c_temp = std::max(infNormOfHColsUpdated.mean(), infNormOfh); + if (c_temp > 1e+4) c_temp = 1e+4; + if (c_temp < 1e-4) c_temp = 1.0; + + scalar_t gamma = 1.0 / c_temp; + + H *= gamma; + h *= gamma; + + cOut *= gamma; + } +} + +void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, + std::vector<vector_t>& scalingVectors) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + + scalingVectors.resize(N); + + // Scale H & h + const int nu_0 = ocpSize.numInputs.front(); + // Scale row - Pre multiply D + cost.front().dfduu.array().colwise() *= c * D.head(nu_0).array(); + // Scale col - Post multiply D + cost.front().dfduu *= D.head(nu_0).asDiagonal(); + + cost.front().dfdu.array() *= c * D.head(nu_0).array(); + + cost.front().dfdux.array().colwise() *= c * D.head(nu_0).array(); + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + auto& Q = cost[k].dfdxx; + auto& R = cost[k].dfduu; + auto& P = cost[k].dfdux; + auto& q = cost[k].dfdx; + auto& r = cost[k].dfdu; + + Q.array().colwise() *= c * D.segment(currRow, nx_k).array(); + Q *= D.segment(currRow, nx_k).asDiagonal(); + q.array() *= c * D.segment(currRow, nx_k).array(); + + P *= D.segment(currRow, nx_k).asDiagonal(); + currRow += nx_k; + + R.array().colwise() *= c * D.segment(currRow, nu_k).array(); + R *= D.segment(currRow, nu_k).asDiagonal(); + r.array() *= c * D.segment(currRow, nu_k).array(); + + P.array().colwise() *= c * D.segment(currRow, nu_k).array(); + + currRow += nu_k; + } + + const int nx_N = ocpSize.numStates[N]; + cost[N].dfdxx.array().colwise() *= c * D.tail(nx_N).array(); + cost[N].dfdxx *= D.tail(nx_N).asDiagonal(); + cost[N].dfdx.array() *= c * D.tail(nx_N).array(); + + // Scale G & g + auto& B_0 = dynamics[0].dfdu; + auto& C_0 = scalingVectors[0]; + const int nx_1 = ocpSize.numStates[1]; + + // \tilde{B} = E * B * D, + // scaling = E * D, + // \tilde{g} = E * g + B_0.array().colwise() *= E.head(nx_1).array(); + B_0 *= D.head(nu_0).asDiagonal(); + + C_0 = E.head(nx_1).array() * D.segment(nu_0, nx_1).array(); + + dynamics[0].dfdx.array().colwise() *= E.head(nx_1).array(); + dynamics[0].f.array() *= E.head(nx_1).array(); + + currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + const int nx_next = ocpSize.numStates[k + 1]; + auto& A_k = dynamics[k].dfdx; + auto& B_k = dynamics[k].dfdu; + auto& b_k = dynamics[k].f; + auto& C_k = scalingVectors[k]; + + A_k.array().colwise() *= E.segment(currRow, nx_next).array(); + A_k *= D.segment(currCol, nx_k).asDiagonal(); + + B_k.array().colwise() *= E.segment(currRow, nx_next).array(); + B_k *= D.segment(currCol + nx_k, nu_k).asDiagonal(); + + C_k = E.segment(currRow, nx_next).array() * D.segment(currCol + nx_k + nu_k, nx_next).array(); + + b_k.array() *= E.segment(currRow, nx_next).array(); + + currRow += nx_next; + currCol += nx_k + nu_k; + } +} + +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/test/testScaling.cpp b/ocs2_oc/test/pre_condition/testScaling.cpp similarity index 76% rename from ocs2_oc/test/testScaling.cpp rename to ocs2_oc/test/pre_condition/testScaling.cpp index 288a7205a..92c1194ce 100644 --- a/ocs2_oc/test/testScaling.cpp +++ b/ocs2_oc/test/pre_condition/testScaling.cpp @@ -1,9 +1,40 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <gtest/gtest.h> #include <ocs2_core/thread_support/ThreadPool.h> #include "ocs2_oc/oc_problem/OcpMatrixConstruction.h" #include "ocs2_oc/pre_condition/Scaling.h" + +#include "ocs2_oc/test/SparseScaling.h" #include "ocs2_oc/test/testProblemsGeneration.h" class ScalingTest : public testing::Test { @@ -23,7 +54,7 @@ class ScalingTest : public testing::Test { dynamicsArray.push_back(ocs2::getRandomDynamics(nx_, nu_)); costArray.push_back(ocs2::getRandomCost(nx_, nu_)); } - costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + costArray.push_back(ocs2::getRandomCost(nx_, 0)); ocpSize_ = ocs2::extractSizesFromProblem(dynamicsArray, costArray, nullptr); } @@ -109,26 +140,19 @@ TEST_F(ScalingTest, preConditioningInPlaceInParallel) { Eigen::SparseMatrix<ocs2::scalar_t> H_ref; ocs2::vector_t h_ref; ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H_ref, h_ref); - // Copy of the original matrix/vector - const Eigen::SparseMatrix<ocs2::scalar_t> H_src = H_ref; - const ocs2::vector_t h_src = h_ref; - Eigen::SparseMatrix<ocs2::scalar_t> G_ref; + // Generate reference ocs2::vector_t g_ref; + Eigen::SparseMatrix<ocs2::scalar_t> G_ref; ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, nullptr, G_ref, g_ref); - // Copy of the original matrix/vector - const Eigen::SparseMatrix<ocs2::scalar_t> G_src = G_ref; - const ocs2::vector_t g_src = g_ref; - // Generate reference ocs2::preConditioningSparseMatrixInPlace(H_ref, h_ref, G_ref, 5, D_ref, E_ref, c_ref); - g_ref = E_ref.asDiagonal() * g_src; + g_ref = E_ref.asDiagonal() * g_ref; // Test start ocs2::vector_array_t D_array, E_array; ocs2::scalar_t c; ocs2::vector_array_t scalingVectors(N_); - ocs2::preConditioningInPlaceInParallel(x0, ocpSize_, 5, dynamicsArray, costArray, D_array, E_array, scalingVectors, c, threadPool, H_src, - h_src, G_src); + ocs2::preConditioningInPlaceInParallel(threadPool, x0, ocpSize_, 5, dynamicsArray, costArray, D_array, E_array, scalingVectors, c); ocs2::vector_t D_stacked(D_ref.rows()), E_stacked(E_ref.rows()); int curRow = 0; From 0d66da08dd075c71f6e1555fa55c0d8a41615669 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 5 Dec 2022 13:34:29 +0100 Subject: [PATCH 394/512] add Lagrangian evaluations and modify projection --- ocs2_oc/CMakeLists.txt | 2 + .../ocs2_oc/multiple_shooting/Helpers.h | 22 ----- .../multiple_shooting/LagrangianEvaluation.h | 85 +++++++++++++++++++ .../ProjectionMultiplierCoefficients.h | 60 +++++++++++++ .../ocs2_oc/multiple_shooting/Transcription.h | 6 +- ocs2_oc/src/lintTarget.cpp | 2 + ocs2_oc/src/multiple_shooting/Helpers.cpp | 18 ---- .../LagrangianEvaluation.cpp | 59 +++++++++++++ .../ProjectionMultiplierCoefficients.cpp | 54 ++++++++++++ .../src/multiple_shooting/Transcription.cpp | 3 + ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 4 +- 11 files changed, 271 insertions(+), 44 deletions(-) create mode 100644 ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h create mode 100644 ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h create mode 100644 ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp create mode 100644 ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 7d6a83b09..27855be1d 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -51,7 +51,9 @@ add_library(${PROJECT_NAME} src/approximate_model/LinearQuadraticApproximator.cpp src/multiple_shooting/Helpers.cpp src/multiple_shooting/Initialization.cpp + src/multiple_shooting/LagrangianEvaluation.cpp src/multiple_shooting/PerformanceIndexComputation.cpp + src/multiple_shooting/ProjectionMultiplierCoefficients.cpp src/multiple_shooting/Transcription.cpp src/oc_data/LoopshapingPrimalSolution.cpp src/oc_data/TimeDiscretization.cpp diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h index a7a5ebf53..478269ac7 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h @@ -101,27 +101,5 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, matrix_array_t&& KMatrices); -/** - * Coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint such that - * dfdx*dx + dfdu*du + dfdcostate*dcostate + f - */ -struct ProjectionMultiplierCoefficients { - matrix_t dfdx; - matrix_t dfdu; - matrix_t dfdcostate; - vector_t f; - - /** - * Extracts the coefficients of the Lagrange multiplier associated with the state-input equality constraint. - * - * @param dynamics : Dynamics - * @param cost : Cost - * @param constraintProjection : Constraint projection. - * @param pseudoInverse : Left pseudo-inverse of D^T of the state-input equality constraint. - */ - void compute(const VectorFunctionLinearApproximation& dynamics, const ScalarFunctionQuadraticApproximation& cost, - const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse); -}; - } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h new file mode 100644 index 000000000..7e13b1ea5 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h @@ -0,0 +1,85 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> + +namespace ocs2 { +namespace multiple_shooting { + +/** + * Computes the SSE of the residual in the dual feasibilities. + * + * @param[in] lagrangian : Quadratic approximation of the Lagrangian for a single node. + * @return SSE of the residual in the dual feasibilities + */ +inline scalar_t evaluateDualFeasibilities(const ScalarFunctionQuadraticApproximation& lagrangian) { + return lagrangian.dfdx.squaredNorm() + lagrangian.dfdu.squaredNorm(); +} + +/** + * Evaluates the quadratic approximation of the Lagrangian for a single intermediate node. + * + * @param[in] lmd : Costate at start of the interval + * @param[in] lmd_next : Costate at the end of the interval + * @param[in] nu : Lagrange multiplier of the projection constraint. + * @param[in] dynamics : Linear approximation of the dynamics + * @param[in] stateInputEqConstraints : Linear approximation of the state-input equality constraints + * @param[in, out] lagrangian : Should be initially filled with the quadratic approximation of the cost. Then this is overwritten into the + * quadratic approximation of the Lagrangian. + */ +void evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu, + const VectorFunctionLinearApproximation& dynamics, + const VectorFunctionLinearApproximation& stateInputEqConstraints, + ScalarFunctionQuadraticApproximation& lagrangian); + +/** + * Evaluates the quadratic approximation of the Lagrangian for the terminal node. + * + * @param[in] lmd : Costate at start of the interval + * @param[in, out] lagrangian : Should be initially filled with the quadratic approximation of the cost. Then this is overwritten into the + * quadratic approximation of the Lagrangian. + */ +void evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation& lagrangian); + +/** + * Evaluates the quadratic approximation of the Lagrangian for the event node. + * + * @param[in] lmd : Costate at start of the interval + * @param[in] lmd_next : Costate at the end of the the interval + * @param[in] dynamics : Dynamics + * @param[in, out] lagrangian : Should be initially filled with the quadratic approximation of the cost. Then this is overwritten into the + * quadratic approximation of the Lagrangian. + */ +void evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next, const VectorFunctionLinearApproximation& dynamics, + ScalarFunctionQuadraticApproximation& lagrangian); + +} // namespace multiple_shooting +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h new file mode 100644 index 000000000..2ebe20ff9 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h @@ -0,0 +1,60 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> + +namespace ocs2 { +namespace multiple_shooting { + +/** + * Coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint such that + * dfdx*dx + dfdu*du + dfdcostate*dcostate + f + */ +struct ProjectionMultiplierCoefficients { + matrix_t dfdx; + matrix_t dfdu; + matrix_t dfdcostate; + vector_t f; + + /** + * Extracts the coefficients of the Lagrange multiplier associated with the state-input equality constraint. + * + * @param dynamics : Dynamics + * @param cost : Cost + * @param constraintProjection : Constraint projection. + * @param pseudoInverse : Left pseudo-inverse of D^T of the state-input equality constraint. + */ + void compute(const VectorFunctionLinearApproximation& dynamics, const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse); +}; + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h index 2527d0965..875c202eb 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/integration/SensitivityIntegrator.h> +#include "ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h" #include "ocs2_oc/oc_problem/OptimalControlProblem.h" namespace ocs2 { @@ -44,6 +45,7 @@ struct Transcription { VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; matrix_t constraintPseudoInverse; + ProjectionMultiplierCoefficients projectionMultiplierCoefficients; VectorFunctionLinearApproximation constraintsProjection; VectorFunctionLinearApproximation stateInputEqConstraints; VectorFunctionLinearApproximation stateIneqConstraints; @@ -70,9 +72,9 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP * Apply the state-input equality constraint projection for a single intermediate node transcription. * * @param transcription : Transcription for a single intermediate node - * @param extractEqualityConstraintsPseudoInverse + * @param extractProjectionMultiplierCoefficients */ -void projectTranscription(Transcription& transcription, bool extractEqualityConstraintsPseudoInverse = false); +void projectTranscription(Transcription& transcription, bool extractProjectionMultiplierCoefficients = false); /** * Results of the transcription at a terminal node diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 149761750..72b1a14d4 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -34,7 +34,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // multiple_shooting #include <ocs2_oc/multiple_shooting/Helpers.h> #include <ocs2_oc/multiple_shooting/Initialization.h> +#include <ocs2_oc/multiple_shooting/LagrangianEvaluation.h> #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> +#include <ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h> #include <ocs2_oc/multiple_shooting/Transcription.h> // oc_data diff --git a/ocs2_oc/src/multiple_shooting/Helpers.cpp b/ocs2_oc/src/multiple_shooting/Helpers.cpp index db33cca0f..0f1328b34 100644 --- a/ocs2_oc/src/multiple_shooting/Helpers.cpp +++ b/ocs2_oc/src/multiple_shooting/Helpers.cpp @@ -119,23 +119,5 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche return primalSolution; } -void ProjectionMultiplierCoefficients::compute(const VectorFunctionLinearApproximation& dynamics, - const ScalarFunctionQuadraticApproximation& cost, - const VectorFunctionLinearApproximation& constraintProjection, - const matrix_t& pseudoInverse) { - vector_t semiprojectedCost_dfdu = cost.dfdu; - semiprojectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f; - - matrix_t semiprojectedCost_dfdux = cost.dfdux; - semiprojectedCost_dfdux.noalias() += cost.dfduu * constraintProjection.dfdx; - - const matrix_t semiprojectedCost_dfduu = cost.dfduu * constraintProjection.dfdu; - - this->dfdx.noalias() = -pseudoInverse * semiprojectedCost_dfdux; - this->dfdu.noalias() = -pseudoInverse * semiprojectedCost_dfduu; - this->dfdcostate.noalias() = -pseudoInverse * dynamics.dfdu.transpose(); - this->f.noalias() = -pseudoInverse * semiprojectedCost_dfdu; -} - } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp b/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp new file mode 100644 index 000000000..8afe2a015 --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp @@ -0,0 +1,59 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_oc/multiple_shooting/LagrangianEvaluation.h" + +namespace ocs2 { +namespace multiple_shooting { + +void evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu, + const VectorFunctionLinearApproximation& dynamics, + const VectorFunctionLinearApproximation& stateInputEqConstraints, + ScalarFunctionQuadraticApproximation& lagrangian) { + lagrangian.dfdx.noalias() += dynamics.dfdx.transpose() * lmd_next; + lagrangian.dfdx.noalias() -= lmd; + lagrangian.dfdu.noalias() += dynamics.dfdu.transpose() * lmd_next; + if (stateInputEqConstraints.f.size() > 0) { + lagrangian.dfdx.noalias() += stateInputEqConstraints.dfdx.transpose() * nu; + lagrangian.dfdu.noalias() += stateInputEqConstraints.dfdu.transpose() * nu; + } +} + +void evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation& lagrangian) { + lagrangian.dfdx.noalias() -= lmd; +} + +void evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next, const VectorFunctionLinearApproximation& dynamics, + ScalarFunctionQuadraticApproximation& lagrangian) { + lagrangian.dfdx.noalias() += dynamics.dfdx.transpose() * lmd_next; + lagrangian.dfdx.noalias() -= lmd; +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp b/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp new file mode 100644 index 000000000..2508bcc7a --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp @@ -0,0 +1,54 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h" + +namespace ocs2 { +namespace multiple_shooting { + +void ProjectionMultiplierCoefficients::compute(const VectorFunctionLinearApproximation& dynamics, + const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& constraintProjection, + const matrix_t& pseudoInverse) { + vector_t semiprojectedCost_dfdu = cost.dfdu; + semiprojectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f; + + matrix_t semiprojectedCost_dfdux = cost.dfdux; + semiprojectedCost_dfdux.noalias() += cost.dfduu * constraintProjection.dfdx; + + const matrix_t semiprojectedCost_dfduu = cost.dfduu * constraintProjection.dfdu; + + this->dfdx.noalias() = -pseudoInverse * semiprojectedCost_dfdux; + this->dfdu.noalias() = -pseudoInverse * semiprojectedCost_dfduu; + this->dfdcostate.noalias() = -pseudoInverse * dynamics.dfdu.transpose(); + this->f.noalias() = -pseudoInverse * semiprojectedCost_dfdu; +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/Transcription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp index 5fd28f022..2b4fb37ef 100644 --- a/ocs2_oc/src/multiple_shooting/Transcription.cpp +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -88,6 +88,7 @@ void projectTranscription(Transcription& transcription, bool extractEqualityCons auto& cost = transcription.cost; auto& projection = transcription.constraintsProjection; auto& constraintPseudoInverse = transcription.constraintPseudoInverse; + auto& projectionMultiplierCoefficients = transcription.projectionMultiplierCoefficients; auto& stateInputEqConstraints = transcription.stateInputEqConstraints; auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; @@ -95,9 +96,11 @@ void projectTranscription(Transcription& transcription, bool extractEqualityCons // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. if (extractEqualityConstraintsPseudoInverse) { std::tie(projection, constraintPseudoInverse) = LinearAlgebra::qrConstraintProjection(stateInputEqConstraints); + projectionMultiplierCoefficients.compute(dynamics, cost, projection, constraintPseudoInverse); } else { projection = LinearAlgebra::luConstraintProjection(stateInputEqConstraints).first; constraintPseudoInverse = matrix_t(); + projectionMultiplierCoefficients = ProjectionMultiplierCoefficients(); } stateInputEqConstraints = VectorFunctionLinearApproximation(); diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 1b8435127..516a4afab 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -315,8 +315,8 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); if (settings_.projectStateInputEqualityConstraints) { - constexpr bool extractPseudoInverse = false; - multiple_shooting::projectTranscription(result, extractPseudoInverse); + constexpr bool extractProjectionMultiplierCoefficients = false; + multiple_shooting::projectTranscription(result, extractProjectionMultiplierCoefficients); } dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); From 11eb40affecd138d09b7544b02046186b6e1da64 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 5 Dec 2022 18:52:23 +0100 Subject: [PATCH 395/512] introducing DensePipgSolver --- ocs2_slp/CMakeLists.txt | 1 + ocs2_slp/include/ocs2_slp/SlpSolver.h | 1 + .../include/ocs2_slp/pipg/DensePipgSolver.h | 58 ++++ ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h | 53 +--- ocs2_slp/src/SlpSolver.cpp | 15 +- ocs2_slp/src/pipg/DensePipgSolver.cpp | 188 ++++++++++++ ocs2_slp/src/pipg/PipgSolver.cpp | 269 +----------------- ocs2_slp/test/testPipgSolver.cpp | 20 +- 8 files changed, 277 insertions(+), 328 deletions(-) create mode 100644 ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h create mode 100644 ocs2_slp/src/pipg/DensePipgSolver.cpp diff --git a/ocs2_slp/CMakeLists.txt b/ocs2_slp/CMakeLists.txt index fa1515cf0..7abddb19a 100644 --- a/ocs2_slp/CMakeLists.txt +++ b/ocs2_slp/CMakeLists.txt @@ -47,6 +47,7 @@ include_directories( add_library(${PROJECT_NAME} src/pipg/PipgSettings.cpp src/pipg/PipgSolver.cpp + src/pipg/DensePipgSolver.cpp src/Helpers.cpp src/SlpSettings.cpp src/SlpSolver.cpp diff --git a/ocs2_slp/include/ocs2_slp/SlpSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h index 3114fdb12..a19ca3f0f 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSolver.h +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -191,6 +191,7 @@ class SlpSolver : public SolverBase { benchmark::RepeatedTimer lambdaEstimation_; benchmark::RepeatedTimer sigmaEstimation_; benchmark::RepeatedTimer preConditioning_; + benchmark::RepeatedTimer pipgSolverTimer_; }; } // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h new file mode 100644 index 000000000..bf7ae1a02 --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h @@ -0,0 +1,58 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <Eigen/Sparse> + +#include <ocs2_core/misc/Benchmark.h> +#include <ocs2_oc/oc_problem/OcpSize.h> + +#include "ocs2_slp/pipg/PipgSettings.h" +#include "ocs2_slp/pipg/PipgSolverStatus.h" + +namespace ocs2 { +namespace pipg { + +struct DensePipgBenchmark { + benchmark::RepeatedTimer vComputation; + benchmark::RepeatedTimer zComputation; + benchmark::RepeatedTimer wComputation; + benchmark::RepeatedTimer convergenceCheck; +}; + +SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, + const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, + const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution, DensePipgBenchmark* timerPtr = nullptr); + +void unpackSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, + vector_array_t& uTrajectory); + +void packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution); + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index 4da997b4d..0025a2a14 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -57,27 +57,6 @@ class PipgSolver { */ explicit PipgSolver(pipg::Settings settings); - /** Destructor */ - ~PipgSolver(); - - /** - * @brief Solve generic QP problem. - * - * @param H: Cost hessian. - * @param h: Cost linear component. - * @param G: Linear equality constraint matrix. - * @param g: Linear equality constraint values - * @param EInv: Inverse of the scaling matrix E. Used to calculate un-sacled termination criteria. - * @param mu: the lower bound of the cost hessian H. - * @param lambda: the upper bound of the cost hessian H. - * @param sigma: the upper bound of \f$ G^TG \f$ - * @param result: Stacked result. - * @return SolverStatus - */ - pipg::SolverStatus solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, - const vector_t& g, const vector_t& EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, - vector_t& result); - /** * @brief Solve Optimal Control type QP in parallel. * @@ -91,16 +70,13 @@ class PipgSolver { * @param mu: the lower bound of the cost hessian H. * @param lambda: the upper bound of the cost hessian H. * @param sigma: the upper bound of \f$ G^TG \f$ - * @param costM For testing only. Can be removed. - * @param constraintsM For testing only. Can be removed. * @return SolverStatus */ pipg::SolverStatus solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma, const ScalarFunctionQuadraticApproximation& costM, - const VectorFunctionLinearApproximation& constraintsM); + const scalar_t lambda, const scalar_t sigma); void resize(const OcpSize& size); @@ -110,20 +86,13 @@ class PipgSolver { int getNumGeneralEqualityConstraints() const; - void getStackedSolution(vector_t& res) const { packSolution(X_, U_, res); } - - void unpackSolution(const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const; - - void packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) const; - - void getStateInputTrajectoriesSolution(vector_array_t& xTrajectory, vector_array_t& uTrajectory) const; + void getStateInputTrajectoriesSolution(vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { + xTrajectory = X_; + uTrajectory = U_; + } void descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const; - std::string getBenchmarkingInformationDense() const; - scalar_t getTotalRunTimeInMilliseconds() const { return parallelizedQPTimer_.getTotalInMilliseconds(); } - scalar_t getAverageRunTimeInMilliseconds() const { return parallelizedQPTimer_.getAverageInMilliseconds(); } - const pipg::Settings& settings() const { return settings_; } const OcpSize& size() const { return ocpSize_; } ThreadPool& getThreadPool() { return threadPool_; } @@ -149,16 +118,6 @@ class PipgSolver { // Problem size int numDecisionVariables_; int numDynamicsConstraints_; - - // Profiling - // Dense PIPG - benchmark::RepeatedTimer denseQPTimer_; - benchmark::RepeatedTimer parallelizedQPTimer_; - - // Parallel PIPG - benchmark::RepeatedTimer vComputationTimer_; - benchmark::RepeatedTimer zComputationTimer_; - benchmark::RepeatedTimer wComputationTimer_; - benchmark::RepeatedTimer convergenceCheckTimer_; }; + } // namespace ocs2 diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 647dedfbc..22cb9a512 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -82,10 +82,16 @@ void SlpSolver::reset() { // reset timers numProblems_ = 0; totalNumIterations_ = 0; + initializationTimer_.reset(); linearQuadraticApproximationTimer_.reset(); solveQpTimer_.reset(); linesearchTimer_.reset(); computeControllerTimer_.reset(); + GGTMultiplication_.reset(); + lambdaEstimation_.reset(); + sigmaEstimation_.reset(); + preConditioning_.reset(); + pipgSolverTimer_.reset(); } std::string SlpSolver::getBenchmarkingInformationPIPG() const { @@ -93,7 +99,7 @@ std::string SlpSolver::getBenchmarkingInformationPIPG() const { const auto preConditioning = preConditioning_.getTotalInMilliseconds(); const auto lambdaEstimation = lambdaEstimation_.getTotalInMilliseconds(); const auto sigmaEstimation = sigmaEstimation_.getTotalInMilliseconds(); - const auto pipgRuntime = pipgSolver_.getTotalRunTimeInMilliseconds(); + const auto pipgRuntime = pipgSolverTimer_.getTotalInMilliseconds(); const auto benchmarkTotal = GGTMultiplication + preConditioning + lambdaEstimation + sigmaEstimation + pipgRuntime; @@ -111,7 +117,7 @@ std::string SlpSolver::getBenchmarkingInformationPIPG() const { << lambdaEstimation / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tsigmaEstimation :\t" << std::setw(10) << sigmaEstimation_.getAverageInMilliseconds() << " [ms] \t(" << sigmaEstimation / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\tPIPG runTime :\t" << std::setw(10) << pipgSolver_.getAverageRunTimeInMilliseconds() << " [ms] \t(" + infoStream << "\tPIPG runTime :\t" << std::setw(10) << pipgSolverTimer_.getAverageInMilliseconds() << " [ms] \t(" << pipgRuntime / benchmarkTotal * inPercent << "%)\n"; } return infoStream.str(); @@ -264,12 +270,13 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta } const scalar_t muEstimated = pipgSolver_.settings().lowerBoundH * c * maxScalingFactor * maxScalingFactor; + pipgSolverTimer_.startTimer(); vector_array_t EInv(E.size()); std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); const auto pipgStatus = - pipgSolver_.solveOCPInParallel(delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled, - ScalarFunctionQuadraticApproximation(), VectorFunctionLinearApproximation()); + pipgSolver_.solveOCPInParallel(delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled); pipgSolver_.getStateInputTrajectoriesSolution(deltaXSol, deltaUSol); + pipgSolverTimer_.endTimer(); // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] solution.armijoDescentMetric = armijoDescentMetric(cost_, deltaXSol, deltaUSol); diff --git a/ocs2_slp/src/pipg/DensePipgSolver.cpp b/ocs2_slp/src/pipg/DensePipgSolver.cpp new file mode 100644 index 000000000..295d66312 --- /dev/null +++ b/ocs2_slp/src/pipg/DensePipgSolver.cpp @@ -0,0 +1,188 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_slp/pipg/DensePipgSolver.h" + +#include <iostream> +#include <numeric> + +namespace ocs2 { +namespace pipg { + +SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, + const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, + const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution, DensePipgBenchmark* timerPtr) { + // Cold start + vector_t z = vector_t::Zero(H.cols()); + vector_t z_old = vector_t::Zero(H.cols()); + + vector_t v = vector_t::Zero(g.rows()); + vector_t w = vector_t::Zero(g.rows()); + vector_t constraintsViolation(g.rows()); + + // Iteration number + size_t k = 0; + bool isConverged = false; + scalar_t constraintsViolationInfNorm; + while (k < settings.maxNumIterations && !isConverged) { + const scalar_t alpha = 2.0 / ((k + 1.0) * mu + 2.0 * lambda); + const scalar_t beta = (k + 1) * mu / (2.0 * sigma); + + z_old.swap(z); + if (timerPtr != nullptr) { + timerPtr->vComputation.startTimer(); + } + // v = w + beta * (G * z - g); + v = -g; + v.noalias() += G * z; + v *= beta; + v += w; + if (timerPtr != nullptr) { + timerPtr->vComputation.endTimer(); + } + + if (timerPtr != nullptr) { + timerPtr->zComputation.startTimer(); + } + // z = z_old - alpha * (H * z_old + h + G.transpose() * v); + z = -h; + z.noalias() -= (H * z_old); + z.noalias() -= (G.transpose() * v); + z *= alpha; + z.noalias() += z_old; + if (timerPtr != nullptr) { + timerPtr->zComputation.endTimer(); + } + + if (timerPtr != nullptr) { + timerPtr->wComputation.startTimer(); + } + // w = w + beta * (G * z - g); + w -= beta * g; + w.noalias() += beta * (G * z); + if (timerPtr != nullptr) { + timerPtr->wComputation.endTimer(); + } + + if (timerPtr != nullptr) { + timerPtr->convergenceCheck.startTimer(); + } + if (k % settings.checkTerminationInterval == 0) { + const scalar_t zNorm = z.squaredNorm(); + + constraintsViolation.noalias() = G * z; + constraintsViolation -= g; + constraintsViolation.cwiseProduct(EInv); + constraintsViolationInfNorm = constraintsViolation.lpNorm<Eigen::Infinity>(); + + const vector_t z_delta = z - z_old; + const scalar_t z_deltaNorm = z_delta.squaredNorm(); + isConverged = + constraintsViolationInfNorm <= settings.absoluteTolerance && + (z_deltaNorm <= settings.relativeTolerance * settings.relativeTolerance * zNorm || z_deltaNorm <= settings.absoluteTolerance); + } + if (timerPtr != nullptr) { + timerPtr->convergenceCheck.endTimer(); + } + + ++k; + } // end of while loop + + stackedSolution.swap(z); + pipg::SolverStatus status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; + + if (settings.displayShortSummary) { + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n++++++++++++++ Dense PIPG ++++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++\n"; + std::cerr << "Solver status: " << pipg::toString(status) << "\n"; + std::cerr << "Number of Iterations: " << k << " out of " << settings.maxNumIterations << "\n"; + std::cerr << "Norm of delta primal solution: " << (stackedSolution - z_old).norm() << "\n"; + std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; + if (timerPtr != nullptr) { + const auto step1v = timerPtr->vComputation.getTotalInMilliseconds(); + const auto step2z = timerPtr->zComputation.getTotalInMilliseconds(); + const auto step3w = timerPtr->wComputation.getTotalInMilliseconds(); + const auto step4CheckConvergence = timerPtr->convergenceCheck.getTotalInMilliseconds(); + const auto benchmarkTotal = step1v + step2z + step3w + step4CheckConvergence; + std::cerr << "\n########################################################################\n"; + std::cerr << "The benchmarking is computed over " << k << " iterations. \n"; + std::cerr << "PIPG Dense Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + std::cerr << "\tvComputation :\t" << timerPtr->vComputation.getAverageInMilliseconds() << " [ms] \t\t(" + << step1v / benchmarkTotal * 100.0 << "%)\n"; + std::cerr << "\tzComputation :\t" << timerPtr->zComputation.getAverageInMilliseconds() << " [ms] \t\t(" + << step2z / benchmarkTotal * 100.0 << "%)\n"; + std::cerr << "\twComputation :\t" << timerPtr->wComputation.getAverageInMilliseconds() << " [ms] \t\t(" + << step3w / benchmarkTotal * 100.0 << "%)\n"; + std::cerr << "\tCheckConvergence :\t" << timerPtr->convergenceCheck.getAverageInMilliseconds() << " [ms] \t\t(" + << step4CheckConvergence / benchmarkTotal * 100.0 << "%)\n"; + } + } + + return status; +} + +void unpackSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, + vector_array_t& uTrajectory) { + const int N = ocpSize.numStages; + xTrajectory.resize(N + 1); + uTrajectory.resize(N); + + xTrajectory.front() = x0; + + int curRow = 0; + for (int i = 0; i < N; i++) { + const auto nx = ocpSize.numStates[i + 1]; + const auto nu = ocpSize.numInputs[i]; + xTrajectory[i + 1] = stackedSolution.segment(curRow + nu, nx); + uTrajectory[i] = stackedSolution.segment(curRow, nu); + + curRow += nx + nu; + } +} + +void packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) { + int numDecisionVariables = 0; + for (int i = 1; i < xTrajectory.size(); i++) { + numDecisionVariables += uTrajectory[i - 1].size() + xTrajectory[i].size(); + } + + stackedSolution.resize(numDecisionVariables); + + int curRow = 0; + for (int i = 1; i < xTrajectory.size(); i++) { + const auto nu = uTrajectory[i - 1].size(); + const auto nx = xTrajectory[i].size(); + stackedSolution.segment(curRow, nx + nu) << uTrajectory[i - 1], xTrajectory[i]; + curRow += nx + nu; + } +} + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_slp/src/pipg/PipgSolver.cpp b/ocs2_slp/src/pipg/PipgSolver.cpp index 015914475..0b1dfed51 100644 --- a/ocs2_slp/src/pipg/PipgSolver.cpp +++ b/ocs2_slp/src/pipg/PipgSolver.cpp @@ -44,104 +44,6 @@ PipgSolver::PipgSolver(pipg::Settings settings) Eigen::initParallel(); } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -PipgSolver::~PipgSolver() { - if (settings().displayShortSummary) { - std::cerr << getBenchmarkingInformationDense() << std::endl; - } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -pipg::SolverStatus PipgSolver::solveDenseQP(const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, - const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, - const scalar_t mu, const scalar_t lambda, const scalar_t sigma, vector_t& result) { - if (const int N = ocpSize_.numStages < 1) { - throw std::runtime_error("[PipgSolver::solveDenseQP] The number of stages cannot be less than 1."); - } - - denseQPTimer_.startTimer(); - - // Cold start - vector_t z = vector_t::Zero(H.cols()); - vector_t z_old = vector_t::Zero(H.cols()); - - vector_t v = vector_t::Zero(g.rows()); - vector_t w = vector_t::Zero(g.rows()); - vector_t constraintsViolation(g.rows()); - - // Iteration number - size_t k = 0; - bool isConverged = false; - scalar_t constraintsViolationInfNorm; - while (k < settings().maxNumIterations && !isConverged) { - const scalar_t alpha = 2.0 / ((k + 1.0) * mu + 2.0 * lambda); - const scalar_t beta = (k + 1) * mu / (2.0 * sigma); - - z_old.swap(z); - vComputationTimer_.startTimer(); - // v = w + beta * (G * z - g); - v = -g; - v.noalias() += G * z; - v *= beta; - v += w; - vComputationTimer_.endTimer(); - - zComputationTimer_.startTimer(); - // z = z_old - alpha * (H * z_old + h + G.transpose() * v); - z = -h; - z.noalias() -= (H * z_old); - z.noalias() -= (G.transpose() * v); - z *= alpha; - z.noalias() += z_old; - zComputationTimer_.endTimer(); - - wComputationTimer_.startTimer(); - // w = w + beta * (G * z - g); - w -= beta * g; - w.noalias() += beta * (G * z); - wComputationTimer_.endTimer(); - - if (k % settings().checkTerminationInterval == 0) { - convergenceCheckTimer_.startTimer(); - const scalar_t zNorm = z.squaredNorm(); - - constraintsViolation.noalias() = G * z; - constraintsViolation -= g; - constraintsViolation.cwiseProduct(EInv); - constraintsViolationInfNorm = constraintsViolation.lpNorm<Eigen::Infinity>(); - - const vector_t z_delta = z - z_old; - const scalar_t z_deltaNorm = z_delta.squaredNorm(); - isConverged = constraintsViolationInfNorm <= settings().absoluteTolerance && - (z_deltaNorm <= settings().relativeTolerance * settings().relativeTolerance * zNorm || - z_deltaNorm <= settings().absoluteTolerance); - convergenceCheckTimer_.endTimer(); - } - - ++k; - } - denseQPTimer_.endTimer(); - - pipg::SolverStatus status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; - if (settings().displayShortSummary) { - std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; - std::cerr << "\n++++++++++++++ PIPG-DenseQP " << pipg::toString(status) << " +++++++++++++"; - std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; - std::cerr << "Number of Iterations: " << k << " out of " << settings().maxNumIterations << "\n"; - std::cerr << "Norm of delta primal solution: " << (z - z_old).norm() << "\n"; - std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; - std::cerr << "Run time: " << denseQPTimer_.getLastIntervalInMilliseconds() << "\n"; - } - - result.swap(z); - - return status; -} - /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -149,9 +51,7 @@ pipg::SolverStatus PipgSolver::solveOCPInParallel(const vector_t& x0, std::vecto const std::vector<ScalarFunctionQuadraticApproximation>& cost, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma, - const ScalarFunctionQuadraticApproximation& costM, - const VectorFunctionLinearApproximation& constraintsM) { + const scalar_t lambda, const scalar_t sigma) { verifySizes(dynamics, cost, constraints); const int N = ocpSize_.numStages; if (N < 1) { @@ -164,8 +64,6 @@ pipg::SolverStatus PipgSolver::solveOCPInParallel(const vector_t& x0, std::vecto // Disable Eigen's internal multithreading Eigen::setNbThreads(1); - parallelizedQPTimer_.startTimer(); - vector_array_t primalResidualArray(N); scalar_array_t constraintsViolationInfNormArray(N); scalar_t constraintsViolationInfNorm; @@ -190,17 +88,6 @@ pipg::SolverStatus PipgSolver::solveOCPInParallel(const vector_t& x0, std::vecto scalar_t beta = mu / (2.0 * sigma); scalar_t betaLast = 0; - // // test - // vector_t v_test, w_test, z_test, old_z_test, old_old_z_test, old_w_test; - // scalar_t old_alpha = alpha, old_beta = beta; - // z_test.setZero(getNumDecisionVariables()); - // w_test.setZero(std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0)); - // auto& G = constraintsM.dfdx; - // auto& g = constraintsM.f; - // auto& H = costM.dfdxx; - // auto& h = costM.dfdx; - // // test end - size_t k = 0; std::atomic_int timeIndex{1}, finishedTaskCounter{0}; std::atomic_bool keepRunning{true}, shouldWait{true}; @@ -331,80 +218,6 @@ pipg::SolverStatus PipgSolver::solveOCPInParallel(const vector_t& x0, std::vecto UNew_.swap(U_); WNew_.swap(W_); - // /** - // * ********************************* - // * ************ Test begin ********* - // * ********************************* - // */ - // old_old_z_test = old_z_test; - // old_z_test = z_test; - // old_w_test = w_test; - - // v_test = w_test + old_beta * (G * z_test - g); - // z_test = z_test - old_alpha * (H * z_test + h + G.transpose() * v_test); - // w_test = w_test + old_beta * (G * z_test - g); - - // old_alpha = alpha; - // old_beta = beta; - - // vector_t V_all(V_.size() * ocpSize_.numStates.front()), W_all(W_.size() * ocpSize_.numStates.front()), - // X_all(X_.size() * ocpSize_.numStates.front()), U_all(U_.size() * ocpSize_.numInputs.front()), - // Z_all(getNumDecisionVariables()); - // int curRow = 0; - // for (int i = 0; i < N; i++) { - // const auto nx = ocpSize_.numStates[i + 1]; - // V_all.segment(curRow, nx) = V_[i]; - // W_all.segment(curRow, nx) = W_[i]; - // curRow += nx; - // } - // getStackedSolution(Z_all); - - // if (!V_all.isApprox(v_test)) { - // std::cerr << "k: " << k << "\n"; - // std::cerr << "v_test: " << v_test.transpose() << "\n"; - // std::cerr << "v_all: " << V_all.transpose() << "\n"; - // std::cerr << "diff:v " << (V_all - v_test).transpose() << std::endl; - // } - // if (!Z_all.isApprox(z_test)) { - // std::cerr << "k: " << k << "\n"; - // std::cerr << "z_test: " << z_test.transpose() << "\n"; - // std::cerr << "z_all: " << Z_all.transpose() << "\n"; - // for (auto& v : X_) std::cerr << v.transpose() << "\n"; - // std::cerr << "\n"; - // for (auto& v : U_) std::cerr << v.transpose() << "\n"; - // std::cerr << "diff:z " << (Z_all - z_test).transpose() << "\n" << std::endl; - // } - // if (k != 0 && !W_all.isApprox(old_w_test)) { - // std::cerr << "k: " << k << "\n"; - // std::cerr << "w_test: " << w_test.transpose() << "\n"; - // std::cerr << "w_all: " << W_all.transpose() << "\n"; - // std::cerr << "diff:w " << (W_all - w_test).transpose() << std::endl; - // } - // if (k != 0 && k % settings().checkTerminationInterval == 0 && - // (std ::abs((EInv.asDiagonal() * (G * old_z_test - g)).lpNorm<Eigen::Infinity>() - constraintsViolationInfNorm) > - // settings().absoluteTolerance)) { - // std::cerr << "k: " << k << "\n"; - // std::cerr << "base: " << (G * old_z_test - g).cwiseAbs().maxCoeff() << "\n"; - // std::cerr << "my: " << constraintsViolationInfNorm << "\n\n"; - // } - // if (k != 0 && k % settings().checkTerminationInterval == 0 && - // (std::abs((old_z_test - old_old_z_test).squaredNorm() - solutionSSE) > 1e-12 || - // std::abs(old_z_test.squaredNorm() - solutionSquaredNorm) > 1e-12)) { - // std::cerr << "k: " << k << "\n"; - // std::cerr << "solutionSSE diff: " << std::scientific << std::abs((old_z_test - old_old_z_test).squaredNorm() - solutionSSE) - // << "\n"; - // std::cerr << "solutionSSE: " << solutionSSE << "\n"; - // std::cerr << "base: " << (old_z_test - old_old_z_test).squaredNorm() << "\n"; - // std::cerr << "solutionSquaredNorm diff: " << std::scientific << std::abs(old_z_test.squaredNorm() - solutionSquaredNorm) << - // "\n"; std::cerr << "solutionSquaredNorm: " << solutionSquaredNorm << "\n"; std::cerr << "base: " << old_z_test.squaredNorm() << - // "\n\n"; - // } - // /** - // * ********************************* - // * ************ Test end *********** - // * ********************************* - // */ - ++k; finishedTaskCounter = 0; timeIndex = 1; @@ -418,8 +231,6 @@ pipg::SolverStatus PipgSolver::solveOCPInParallel(const vector_t& x0, std::vecto }; runParallel(std::move(updateVariablesTask)); - parallelizedQPTimer_.endTimer(); - pipg::SolverStatus status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; if (settings().displayShortSummary) { scalar_t totalTasks = std::accumulate(threadsWorkloadCounter.cbegin(), threadsWorkloadCounter.cend(), 0.0); @@ -434,7 +245,6 @@ pipg::SolverStatus PipgSolver::solveOCPInParallel(const vector_t& x0, std::vecto std::cerr << i << ": " << threadsWorkloadCounter[i] << "(" << static_cast<scalar_t>(threadsWorkloadCounter[i]) / totalTasks * 100.0 << "%) "; } - std::cerr << "\nRun time: " << parallelizedQPTimer_.getLastIntervalInMilliseconds() << "\n\n"; } Eigen::setNbThreads(0); // Restore default setup. @@ -467,43 +277,6 @@ void PipgSolver::resize(const OcpSize& ocpSize) { WNew_.resize(N); } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void PipgSolver::unpackSolution(const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, - vector_array_t& uTrajectory) const { - const int N = ocpSize_.numStages; - xTrajectory.resize(N + 1); - uTrajectory.resize(N); - - xTrajectory.front() = x0; - - int curRow = 0; - for (int i = 0; i < N; i++) { - const auto nx = ocpSize_.numStates[i + 1]; - const auto nu = ocpSize_.numInputs[i]; - xTrajectory[i + 1] = stackedSolution.segment(curRow + nu, nx); - uTrajectory[i] = stackedSolution.segment(curRow, nu); - - curRow += nx + nu; - } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void PipgSolver::packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) const { - stackedSolution.resize(getNumDecisionVariables()); - - int curRow = 0; - for (int i = 0; i < ocpSize_.numStages; i++) { - const auto nx = ocpSize_.numStates[i + 1]; - const auto nu = ocpSize_.numInputs[i]; - stackedSolution.segment(curRow, nx + nu) << uTrajectory[i], xTrajectory[i + 1]; - curRow += nx + nu; - } -} - /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -519,17 +292,6 @@ void PipgSolver::descaleSolution(const vector_array_t& D, vector_array_t& xTraje } } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void PipgSolver::getStateInputTrajectoriesSolution(vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { - xTrajectory.resize(X_.size()); - uTrajectory.resize(U_.size()); - - std::copy(X_.begin(), X_.end(), xTrajectory.begin()); - std::copy(U_.begin(), U_.end(), uTrajectory.begin()); -} - /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -584,33 +346,4 @@ int PipgSolver::getNumGeneralEqualityConstraints() const { return totalNumberOfGeneralEqualityConstraints; } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -std::string PipgSolver::getBenchmarkingInformationDense() const { - const auto step1v = vComputationTimer_.getTotalInMilliseconds(); - const auto step2z = zComputationTimer_.getTotalInMilliseconds(); - const auto step3w = wComputationTimer_.getTotalInMilliseconds(); - const auto step4CheckConvergence = convergenceCheckTimer_.getTotalInMilliseconds(); - - const auto benchmarkTotal = step1v + step2z + step3w + step4CheckConvergence; - - std::stringstream infoStream; - if (benchmarkTotal > 0.0) { - const scalar_t inPercent = 100.0; - infoStream << "\n########################################################################\n"; - infoStream << "The benchmarking is computed over " << vComputationTimer_.getNumTimedIntervals() << " iterations. \n"; - infoStream << "PIPG Dense Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; - infoStream << "\tvComputation :\t" << vComputationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" - << step1v / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\tzComputation :\t" << zComputationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" - << step2z / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\twComputation :\t" << wComputationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" - << step3w / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\tCheckConvergence :\t" << convergenceCheckTimer_.getAverageInMilliseconds() << " [ms] \t\t(" - << step4CheckConvergence / benchmarkTotal * inPercent << "%)\n"; - } - return infoStream.str(); -} - } // namespace ocs2 diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp index 3090746ce..1522c2245 100644 --- a/ocs2_slp/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/test/testProblemsGeneration.h> #include <ocs2_qp_solver/QpSolver.h> +#include "ocs2_slp/pipg/DensePipgSolver.h" #include "ocs2_slp/pipg/PipgSolver.h" ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, @@ -107,16 +108,17 @@ TEST_F(PIPGSolverTest, correctness) { ocs2::scalar_t sigma = svdGTG.singularValues()(0); ocs2::vector_t primalSolutionPIPG; - solver.solveDenseQP(costApproximation.dfdxx.sparseView(), costApproximation.dfdx, constraintsApproximation.dfdx.sparseView(), - constraintsApproximation.f, ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, - primalSolutionPIPG); + ocs2::pipg::densePipg(solver.settings(), costApproximation.dfdxx.sparseView(), costApproximation.dfdx, + constraintsApproximation.dfdx.sparseView(), constraintsApproximation.f, + ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, primalSolutionPIPG); - ocs2::vector_t primalSolutionPIPGParallel; ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); + solver.solveOCPInParallel(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma); - solver.solveOCPInParallel(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma, costApproximation, - constraintsApproximation); - solver.getStackedSolution(primalSolutionPIPGParallel); + ocs2::vector_array_t X, U; + solver.getStateInputTrajectoriesSolution(X, U); + ocs2::vector_t primalSolutionPIPGParallel; + ocs2::pipg::packSolution(X, U, primalSolutionPIPGParallel); auto calculateConstraintViolation = [&](const ocs2::vector_t& sol) -> ocs2::scalar_t { return (constraintsApproximation.dfdx * sol - constraintsApproximation.f).cwiseAbs().maxCoeff(); @@ -193,13 +195,13 @@ TEST_F(PIPGSolverTest, descaleSolution) { curRow += v.size(); } ocs2::vector_t packedSolution; - solver.packSolution(x, u, packedSolution); + ocs2::pipg::packSolution(x, u, packedSolution); packedSolution.array() *= DStacked.array(); ocs2::vector_t packedSolutionMy; solver.descaleSolution(D, x, u); - solver.packSolution(x, u, packedSolutionMy); + ocs2::pipg::packSolution(x, u, packedSolutionMy); EXPECT_TRUE(packedSolutionMy.isApprox(packedSolution)) << std::setprecision(6) << "DescaledSolution: \n" << packedSolutionMy.transpose() << "\nIt should be \n" << packedSolution.transpose(); From 985660c421eddbcd83752b283e62e2de698a076f Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 5 Dec 2022 19:13:54 +0100 Subject: [PATCH 396/512] renaming PipgSolver::solveOCPInParallel to PipgSolver::solve --- .../include/ocs2_slp/pipg/DensePipgSolver.h | 21 +++++++++++++++++++ ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h | 11 +++++----- ocs2_slp/src/SlpSolver.cpp | 2 +- ocs2_slp/src/pipg/PipgSolver.cpp | 14 ++++++------- ocs2_slp/test/testPipgSolver.cpp | 2 +- 5 files changed, 35 insertions(+), 15 deletions(-) diff --git a/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h index bf7ae1a02..f9343896f 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h @@ -45,13 +45,34 @@ struct DensePipgBenchmark { benchmark::RepeatedTimer convergenceCheck; }; +/* + * First order primal-dual method for solving optimal control problem based on: + * "Proportional-Integral Projected Gradient Method for Model Predictive Control" + * https://arxiv.org/abs/2009.06980 + */ SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution, DensePipgBenchmark* timerPtr = nullptr); +/** + * Deserializes the stacked solution to state-input trajecotries. + * + * @param[in] ocpSize : Optimal control problem sizes. + * @param[in] stackedSolution : Defined as [u[0], x[1], ..., u[N-1], x[N]]. + * @param[in] x0 : The initial state. + * @param[out] xTrajectory : State tarjectory. + * @param[out] UTrajectory : Input trajecotry. + */ void unpackSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, vector_array_t& uTrajectory); +/** + * Serializes the state-input trajecotries. + * + * @param[in] xTrajectory : State tarjectory + * @param[in] UTrajectory : Input trajecotry + * @param[out] stackedSolution : [u[0], x[1], ..., u[N-1], x[N]] + */ void packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution); } // namespace pipg diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index 0025a2a14..2774f7203 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -58,7 +58,7 @@ class PipgSolver { explicit PipgSolver(pipg::Settings settings); /** - * @brief Solve Optimal Control type QP in parallel. + * @brief Solve Optimal Control in parallel. * * @param x0 Initial state * @param dynamics: Dynamics array. @@ -72,11 +72,10 @@ class PipgSolver { * @param sigma: the upper bound of \f$ G^TG \f$ * @return SolverStatus */ - pipg::SolverStatus solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<ScalarFunctionQuadraticApproximation>& cost, - const std::vector<VectorFunctionLinearApproximation>* constraints, - const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma); + pipg::SolverStatus solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t& scalingVectors, + const vector_array_t* EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma); void resize(const OcpSize& size); diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 22cb9a512..f97e1016e 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -274,7 +274,7 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta vector_array_t EInv(E.size()); std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); const auto pipgStatus = - pipgSolver_.solveOCPInParallel(delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled); + pipgSolver_.solve(delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled); pipgSolver_.getStateInputTrajectoriesSolution(deltaXSol, deltaUSol); pipgSolverTimer_.endTimer(); diff --git a/ocs2_slp/src/pipg/PipgSolver.cpp b/ocs2_slp/src/pipg/PipgSolver.cpp index 0b1dfed51..543ad585d 100644 --- a/ocs2_slp/src/pipg/PipgSolver.cpp +++ b/ocs2_slp/src/pipg/PipgSolver.cpp @@ -47,18 +47,18 @@ PipgSolver::PipgSolver(pipg::Settings settings) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -pipg::SolverStatus PipgSolver::solveOCPInParallel(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<ScalarFunctionQuadraticApproximation>& cost, - const std::vector<VectorFunctionLinearApproximation>* constraints, - const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma) { +pipg::SolverStatus PipgSolver::solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<ScalarFunctionQuadraticApproximation>& cost, + const std::vector<VectorFunctionLinearApproximation>* constraints, + const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, + const scalar_t lambda, const scalar_t sigma) { verifySizes(dynamics, cost, constraints); const int N = ocpSize_.numStages; if (N < 1) { - throw std::runtime_error("[PipgSolver::solveOCPInParallel] The number of stages cannot be less than 1."); + throw std::runtime_error("[PipgSolver::solve] The number of stages cannot be less than 1."); } if (scalingVectors.size() != N) { - throw std::runtime_error("[PipgSolver::solveOCPInParallel] The size of scalingVectors doesn't match the number of stage."); + throw std::runtime_error("[PipgSolver::solve] The size of scalingVectors doesn't match the number of stage."); } // Disable Eigen's internal multithreading diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp index 1522c2245..cbc89b549 100644 --- a/ocs2_slp/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -113,7 +113,7 @@ TEST_F(PIPGSolverTest, correctness) { ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, primalSolutionPIPG); ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); - solver.solveOCPInParallel(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma); + solver.solve(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma); ocs2::vector_array_t X, U; solver.getStateInputTrajectoriesSolution(X, U); From 6dc2be6a0b247c3a7bfecb805f8dd0266bdc0f58 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 6 Dec 2022 10:46:54 +0100 Subject: [PATCH 397/512] add vector_trajectory_t for the trajectory spreading --- .../trajectory_adjustment/VectorTrajectory.h | 176 ++++++++++++++++++ 1 file changed, 176 insertions(+) create mode 100644 ocs2_oc/include/ocs2_oc/trajectory_adjustment/VectorTrajectory.h diff --git a/ocs2_oc/include/ocs2_oc/trajectory_adjustment/VectorTrajectory.h b/ocs2_oc/include/ocs2_oc/trajectory_adjustment/VectorTrajectory.h new file mode 100644 index 000000000..e94257c9d --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/trajectory_adjustment/VectorTrajectory.h @@ -0,0 +1,176 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_core/misc/LinearInterpolation.h> +#include <ocs2_core/reference/ModeSchedule.h> + +#include "ocs2_oc/oc_data/TimeDiscretization.h" +#include "ocs2_oc/trajectory_adjustment/TrajectorySpreading.h" + +namespace ocs2 { + +struct vector_trajectory_t { + scalar_array_t timeTrajectory; + size_array_t postEventIndices; + + vector_t final; + vector_array_t preJumps; + vector_array_t intermediates; + + /** Exchanges the content of trajectory */ + void swap(vector_trajectory_t& other) { + timeTrajectory.swap(other.timeTrajectory); + postEventIndices.swap(other.postEventIndices); + final.swap(other.final); + preJumps.swap(other.preJumps); + intermediates.swap(other.intermediates); + } + + /** Clears the content of the trajectory */ + void clear() { + timeTrajectory.clear(); + postEventIndices.clear(); + final = vector_t(); + preJumps.clear(); + intermediates.clear(); + } + + /** Convert a vector array to a vector trajectory */ + static vector_trajectory_t fromVectorArray(const std::vector<AnnotatedTime>& time, vector_array_t&& vector_array) { + vector_trajectory_t result; + + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + result.preJumps.push_back(std::move(vector_array[i])); + } else { + result.intermediates.push_back(std::move(vector_array[i])); + } + } + result.final = std::move(vector_array[N]); + + return result; + } + + /** Convert a vector trajectory to a vector array */ + static vector_array_t toVectorArray(const std::vector<AnnotatedTime>& time, vector_trajectory_t&& vector_trajectory) { + vector_array_t result; + + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + result.reserve(N + 1); + + int intermediateIdx = 0; + int preJumpIdx = 0; + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + result.push_back(std::move(vector_trajectory.preJumps[preJumpIdx])); + ++preJumpIdx; + } else { + result.push_back(std::move(vector_trajectory.intermediates[intermediateIdx])); + ++intermediateIdx; + } + } + result.push_back(std::move(vector_trajectory.final)); + + return result; + } +}; + +/** + * Calculates the intermediate vector value at the given time. + * + * @param [in] trajectory: The trajectory + * @param [in] time: The inquiry time + * @return The vector value at the given time. + */ +inline vector_t getIntermediateValueAtTime(const vector_trajectory_t& trajectory, scalar_t time) { + const auto indexAlpha = LinearInterpolation::timeSegment(time, trajectory.timeTrajectory); + return LinearInterpolation::interpolate(indexAlpha, trajectory.intermediates); +} + +/** + * Adjusts in-place a vector trajectory based on the last changes in mode schedule using a TrajectorySpreading strategy. + * + * @param [in] oldModeSchedule: The old mode schedule associated to the trajectories which should be adjusted. + * @param [in] newModeSchedule: The new mode schedule that should be adapted to. + * @param [in, out] trajectory: The dual solution that is associated with the old mode schedule. + * @returns the status of the devised trajectory spreading strategy. + */ +inline TrajectorySpreading::Status trajectorySpread(const ModeSchedule& oldModeSchedule, const ModeSchedule& newModeSchedule, + vector_trajectory_t& trajectory) { + // trajectory spreading + constexpr bool debugPrint = false; + TrajectorySpreading trajectorySpreading(debugPrint); + const auto status = trajectorySpreading.set(oldModeSchedule, newModeSchedule, trajectory.timeTrajectory); + + // adjust final, pre-jump, intermediate, time, postEventIndices + if (status.willTruncate) { + trajectory.final = vector_t(); + } + trajectory.preJumps = trajectorySpreading.extractEventsArray(trajectory.preJumps); + trajectorySpreading.adjustTrajectory(trajectory.intermediates); + trajectorySpreading.adjustTimeTrajectory(trajectory.timeTrajectory); + trajectory.postEventIndices = trajectorySpreading.getPostEventIndices(); + + return status; +} + +/** + * Adjusts a vector trajectory based on the last changes in mode schedule using a TrajectorySpreading strategy. + * + * @param [in] trajectorySpreading: An updated trajectorySpreading instance. In order to update trajectorySpreading + * call TrajectorySpreading::set. + * @param [in] oldDualSolution: The dual solution that is associated with the old mode schedule. + * @param [out] newDualSolution: The updated dual solution that is associated with the new mode schedule. + */ +inline void trajectorySpread(const TrajectorySpreading& trajectorySpreading, const vector_trajectory_t& oldTrajectory, + vector_trajectory_t& newTrajectory) { + newTrajectory.clear(); + + // adjust time and postEventIndices + newTrajectory.timeTrajectory = oldTrajectory.timeTrajectory; + trajectorySpreading.adjustTimeTrajectory(newTrajectory.timeTrajectory); + newTrajectory.postEventIndices = trajectorySpreading.getPostEventIndices(); + + // adjust final, pre-jump and intermediate + if (!trajectorySpreading.getStatus().willTruncate) { + newTrajectory.final = oldTrajectory.final; + } + newTrajectory.preJumps = trajectorySpreading.extractEventsArray(oldTrajectory.preJumps); + newTrajectory.intermediates = oldTrajectory.intermediates; + trajectorySpreading.adjustTrajectory(newTrajectory.intermediates); +} + +} // namespace ocs2 From f3c95b78736f30c5face1b974a53f884d8142eb2 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 6 Dec 2022 10:51:42 +0100 Subject: [PATCH 398/512] update lintTarget --- ocs2_oc/src/lintTarget.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 72b1a14d4..7de10530f 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -74,6 +74,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // trajectory_adjustment #include <ocs2_oc/trajectory_adjustment/TrajectorySpreading.h> +#include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h> +#include <ocs2_oc/trajectory_adjustment/VectorTrajectory.h> // dummy target for clang toolchain int main() { From 4ca0090407f64f07b75805ef0d01f3074797a736 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 11:05:03 +0100 Subject: [PATCH 399/512] OcpMatrixConstruction renamed to OcpToKkt --- ocs2_oc/CMakeLists.txt | 2 +- .../{OcpMatrixConstruction.h => OcpToKkt.h} | 50 ++++++++++++++++--- ocs2_oc/src/lintTarget.cpp | 2 +- ...OcpMatrixConstruction.cpp => OcpToKkt.cpp} | 49 +++++++++--------- .../test/include/ocs2_oc/test/SparseScaling.h | 1 - .../oc_problem/testMatrixConstruction.cpp | 2 +- ocs2_oc/test/pre_condition/testScaling.cpp | 2 +- ocs2_slp/test/testHelpers.cpp | 2 +- ocs2_slp/test/testPipgSolver.cpp | 2 +- 9 files changed, 72 insertions(+), 40 deletions(-) rename ocs2_oc/include/ocs2_oc/oc_problem/{OcpMatrixConstruction.h => OcpToKkt.h} (51%) rename ocs2_oc/src/oc_problem/{OcpMatrixConstruction.cpp => OcpToKkt.cpp} (91%) diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 7d6993de3..fb94c3894 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -58,8 +58,8 @@ add_library(${PROJECT_NAME} src/oc_problem/OptimalControlProblem.cpp src/oc_problem/LoopshapingOptimalControlProblem.cpp src/oc_problem/OptimalControlProblemHelperFunction.cpp - src/oc_problem/OcpMatrixConstruction.cpp src/oc_problem/OcpSize.cpp + src/oc_problem/OcpToKkt.cpp src/oc_solver/SolverBase.cpp src/pre_condition/Scaling.cpp src/rollout/PerformanceIndicesRollout.cpp diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h b/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h similarity index 51% rename from ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h rename to ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h index e2b90f4ea..49f55114a 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OcpMatrixConstruction.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h @@ -38,27 +38,61 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * @brief Get liner constraint matrix G from the dynamics, cost and constraint arrays. + * Constructs concatenated linear approximation of the constraints from the dynamics and constraints arrays w.r.t. + * X = [u_{0}, x{1}, ..., u_{i}, x{i+1}, ..., u_{N}, x{N+1}]. * - * @param ocpSize: The size of optimal control problem. - * @param x0: Initial state. - * @param dynamics: Dynamics array. - * @param constraints: Constraints array. - * @param scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. After - * scaling, they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. - * @param res: The resulting constraints approxmation. + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] x0: The initial state. + * @param[in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param[out] res: The resulting constraints approxmation. Here we misused dfdx field to store the jacobian w.r.t. X. */ void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, VectorFunctionLinearApproximation& res); +/** + * Constructs concatenated linear approximation of the constraints from the dynamics and constraints arrays w.r.t. + * X = [u_{0}, x{1}, ..., u_{i}, x{i+1}, ..., u_{N}, x{N+1}]. + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] x0: The initial state. + * @param[in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param[out] G: The jacobian of the concatenated constraints w.r.t. X. + * @param[out] g: The concatenated constraints value. + */ void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, Eigen::SparseMatrix<scalar_t>& G, vector_t& g); +/** + * Constructs concatenated quadratic approximation of the total cost w.r.t. X = [u_{0}, x{1}, ..., u_{i}, x{i+1}, ..., u_{N}, x{N+1}]. + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] x0: The initial state. + * @param[in] cost: Quadratic approximation of the cost over the time horizon. + * @param[out] res: The resulting cost approxmation. Here we misused dfdx and dfdxx fields to store the jacobian and the hessian + * matrices w.r.t. Xaug. + */ void getCostMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, ScalarFunctionQuadraticApproximation& res); +/** + * Constructs concatenated the jacobian and the hessian matrices of the total cost w.r.t. + * X = [u_{0}, x{1}, ..., u_{i}, x{i+1}, ..., u_{N}, x{N+1}]. + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] x0: The initial state. + * @param[in] cost: Quadratic approximation of the cost over the time horizon. + * @param[out] H: The concatenated hessian matrix w.r.t. X. + * @param[out] h: The concatenated jacobian vector w.r.t. X. + */ void getCostMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, Eigen::SparseMatrix<scalar_t>& H, vector_t& h); + } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 604295b32..9d05708f7 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -46,8 +46,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // oc_problem #include <ocs2_oc/oc_problem/LoopshapingOptimalControlProblem.h> -#include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> #include <ocs2_oc/oc_problem/OcpSize.h> +#include <ocs2_oc/oc_problem/OcpToKkt.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> #include <ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h> diff --git a/ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp b/ocs2_oc/src/oc_problem/OcpToKkt.cpp similarity index 91% rename from ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp rename to ocs2_oc/src/oc_problem/OcpToKkt.cpp index dc58ae2e5..9d20bf0ab 100644 --- a/ocs2_oc/src/oc_problem/OcpMatrixConstruction.cpp +++ b/ocs2_oc/src/oc_problem/OcpToKkt.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_oc/oc_problem/OcpMatrixConstruction.h" +#include "ocs2_oc/oc_problem/OcpToKkt.h" #include <numeric> @@ -49,7 +49,7 @@ int getNumGeneralEqualityConstraints(const OcpSize& ocpSize) { } // namespace void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, + const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, const vector_array_t* scalingVectorsPtr, VectorFunctionLinearApproximation& res) { const int N = ocpSize.numStages; if (N < 1) { @@ -60,15 +60,14 @@ void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std:: } // Preallocate full constraint matrix - auto& G = res.dfdx; auto& g = res.f; - if (constraints != nullptr) { - G.setZero(getNumDynamicsConstraints(ocpSize) + getNumGeneralEqualityConstraints(ocpSize), getNumDecisionVariables(ocpSize)); - + auto& G = res.dfdx; + if (constraintsPtr == nullptr) { + g.setZero(getNumDynamicsConstraints(ocpSize)); } else { - G.setZero(getNumDynamicsConstraints(ocpSize), getNumDecisionVariables(ocpSize)); + g.setZero(getNumDynamicsConstraints(ocpSize) + getNumGeneralEqualityConstraints(ocpSize)); } - g.setZero(G.rows()); + G.setZero(g.rows(), getNumDecisionVariables(ocpSize)); // Initial state constraint const int nu_0 = ocpSize.numInputs.front(); @@ -111,9 +110,9 @@ void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std:: currRow += nx_next; currCol += nx_k + nu_k; - } + } // end of i loop - if (constraints != nullptr) { + if (constraintsPtr != nullptr) { currCol = nu_0; // === Constraints === // for ocs2 --> C*dx + D*du + e = 0 @@ -121,7 +120,7 @@ void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std:: // Initial general constraints const int nc_0 = ocpSize.numIneqConstraints.front(); if (nc_0 > 0) { - const auto& constraint_0 = (*constraints).front(); + const auto& constraint_0 = (*constraintsPtr).front(); G.block(currRow, 0, nc_0, nu_0) = constraint_0.dfdu; g.segment(currRow, nc_0) = -constraint_0.f; g.segment(currRow, nc_0) -= constraint_0.dfdx * x0; @@ -133,7 +132,7 @@ void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std:: const int nu_k = ocpSize.numInputs[k]; const int nx_k = ocpSize.numStates[k]; if (nc_k > 0) { - const auto& constraints_k = (*constraints)[k]; + const auto& constraints_k = (*constraintsPtr)[k]; // Add [C, D, 0] G.block(currRow, currCol, nc_k, nx_k + nu_k) << constraints_k.dfdx, constraints_k.dfdu; @@ -148,16 +147,16 @@ void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std:: // Final general constraint const int nc_N = ocpSize.numIneqConstraints[N]; if (nc_N > 0) { - const auto& constraints_N = (*constraints)[N]; + const auto& constraints_N = (*constraintsPtr)[N]; G.bottomRightCorner(nc_N, constraints_N.dfdx.cols()) = constraints_N.dfdx; g.tail(nc_N) = -constraints_N.f; } - } + } // end of i loop } void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, - Eigen::SparseMatrix<scalar_t>& G, vector_t& g) { + const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, + const vector_array_t* scalingVectorsPtr, Eigen::SparseMatrix<scalar_t>& G, vector_t& g) { const int N = ocpSize.numStages; if (N < 1) { throw std::runtime_error("[getConstraintMatrixSparse] The number of stages cannot be less than 1."); @@ -178,7 +177,7 @@ void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const nnz += nx_next * (nx_k + nu_k + nx_next); } - if (constraints != nullptr) { + if (constraintsPtr != nullptr) { const int nc_0 = ocpSize.numIneqConstraints[0]; nnz += nc_0 * nu_0; for (int k = 1; k < N; ++k) { @@ -204,12 +203,12 @@ void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const } }; - if (constraints != nullptr) { - G.resize(getNumDynamicsConstraints(ocpSize) + getNumGeneralEqualityConstraints(ocpSize), getNumDecisionVariables(ocpSize)); + if (constraintsPtr == nullptr) { + g.setZero(getNumDynamicsConstraints(ocpSize)); } else { - G.resize(getNumDynamicsConstraints(ocpSize), getNumDecisionVariables(ocpSize)); + g.setZero(getNumDynamicsConstraints(ocpSize) + getNumGeneralEqualityConstraints(ocpSize)); } - g.setZero(G.rows()); + G.resize(g.rows(), getNumDecisionVariables(ocpSize)); // Initial state constraint emplaceBackMatrix(0, 0, -dynamics.front().dfdu); @@ -254,7 +253,7 @@ void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const currCol += nx_k + nu_k; } - if (constraints != nullptr) { + if (constraintsPtr != nullptr) { currCol = nu_0; // === Constraints === // for ocs2 --> C*dx + D*du + e = 0 @@ -262,7 +261,7 @@ void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const // Initial general constraints const int nc_0 = ocpSize.numIneqConstraints.front(); if (nc_0 > 0) { - const auto& constraint_0 = (*constraints).front(); + const auto& constraint_0 = (*constraintsPtr).front(); emplaceBackMatrix(currRow, 0, constraint_0.dfdu); g.segment(currRow, nc_0) = -constraint_0.f; @@ -275,7 +274,7 @@ void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const const int nu_k = ocpSize.numInputs[k]; const int nx_k = ocpSize.numStates[k]; if (nc_k > 0) { - const auto& constraints_k = (*constraints)[k]; + const auto& constraints_k = (*constraintsPtr)[k]; // Add [C, D, 0] emplaceBackMatrix(currRow, currCol, constraints_k.dfdx); @@ -291,7 +290,7 @@ void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const // Final general constraint const int nc_N = ocpSize.numIneqConstraints[N]; if (nc_N > 0) { - const auto& constraints_N = (*constraints)[N]; + const auto& constraints_N = (*constraintsPtr)[N]; emplaceBackMatrix(currRow, currCol, constraints_N.dfdx); g.segment(currRow, nc_N) = -constraints_N.f; } diff --git a/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h b/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h index 5d5201393..a0b1f1fbe 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h +++ b/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h @@ -32,7 +32,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <Eigen/Sparse> #include <ocs2_core/Types.h> -#include <ocs2_core/thread_support/ThreadPool.h> #include "ocs2_oc/oc_problem/OcpSize.h" diff --git a/ocs2_oc/test/oc_problem/testMatrixConstruction.cpp b/ocs2_oc/test/oc_problem/testMatrixConstruction.cpp index f78898330..66abea0da 100644 --- a/ocs2_oc/test/oc_problem/testMatrixConstruction.cpp +++ b/ocs2_oc/test/oc_problem/testMatrixConstruction.cpp @@ -30,7 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> #include "ocs2_oc/oc_problem/OcpSize.h" -#include "ocs2_oc/oc_problem/OcpMatrixConstruction.h" +#include "ocs2_oc/oc_problem/OcpToKkt.h" #include "ocs2_oc/test/testProblemsGeneration.h" diff --git a/ocs2_oc/test/pre_condition/testScaling.cpp b/ocs2_oc/test/pre_condition/testScaling.cpp index 92c1194ce..61cbc5865 100644 --- a/ocs2_oc/test/pre_condition/testScaling.cpp +++ b/ocs2_oc/test/pre_condition/testScaling.cpp @@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/thread_support/ThreadPool.h> -#include "ocs2_oc/oc_problem/OcpMatrixConstruction.h" +#include "ocs2_oc/oc_problem/OcpToKkt.h" #include "ocs2_oc/pre_condition/Scaling.h" #include "ocs2_oc/test/SparseScaling.h" diff --git a/ocs2_slp/test/testHelpers.cpp b/ocs2_slp/test/testHelpers.cpp index 044e2738b..090bfba41 100644 --- a/ocs2_slp/test/testHelpers.cpp +++ b/ocs2_slp/test/testHelpers.cpp @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> -#include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> +#include <ocs2_oc/oc_problem/OcpToKkt.h> #include <ocs2_oc/test/testProblemsGeneration.h> #include "ocs2_slp/Helpers.h" diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp index cbc89b549..311865b5e 100644 --- a/ocs2_slp/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -30,7 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> #include <Eigen/Sparse> -#include <ocs2_oc/oc_problem/OcpMatrixConstruction.h> +#include <ocs2_oc/oc_problem/OcpToKkt.h> #include <ocs2_oc/test/testProblemsGeneration.h> #include <ocs2_qp_solver/QpSolver.h> From a59f0a0362a30e7cb129e8d5a239919164dcff18 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 6 Dec 2022 11:26:39 +0100 Subject: [PATCH 400/512] fix test --- ocs2_oc/CMakeLists.txt | 2 +- ...testHelpers.cpp => testProjectionMultiplierCoefficients.cpp} | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) rename ocs2_oc/test/multiple_shooting/{testHelpers.cpp => testProjectionMultiplierCoefficients.cpp} (97%) diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 27855be1d..d93341bec 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -130,7 +130,7 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## $ catkin_test_results ../../../build/ocs2_oc catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting - test/multiple_shooting/testHelpers.cpp + test/multiple_shooting/testProjectionMultiplierCoefficients.cpp test/multiple_shooting/testTranscriptionPerformanceIndex.cpp ) add_dependencies(test_${PROJECT_NAME}_multiple_shooting diff --git a/ocs2_oc/test/multiple_shooting/testHelpers.cpp b/ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp similarity index 97% rename from ocs2_oc/test/multiple_shooting/testHelpers.cpp rename to ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp index 048ca3814..62f231ff6 100644 --- a/ocs2_oc/test/multiple_shooting/testHelpers.cpp +++ b/ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp @@ -30,7 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <gtest/gtest.h> #include <ocs2_core/misc/LinearAlgebra.h> -#include <ocs2_oc/multiple_shooting/Helpers.h> +#include <ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h> #include "ocs2_oc/test/testProblemsGeneration.h" From 9c6e5ece557de8dded16a7e7f895bfa37b0e63af Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 12:12:12 +0100 Subject: [PATCH 401/512] doc improved --- ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h | 67 ++++++++++++++++--- .../include/ocs2_oc/pre_condition/Scaling.h | 9 ++- ocs2_slp/include/ocs2_slp/Helpers.h | 52 +++++++++++++- ocs2_slp/src/Helpers.cpp | 7 +- ocs2_slp/src/SlpSolver.cpp | 2 +- ocs2_slp/test/testHelpers.cpp | 2 +- 6 files changed, 119 insertions(+), 20 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h b/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h index 49f55114a..aa696a8f9 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h @@ -39,7 +39,20 @@ namespace ocs2 { /** * Constructs concatenated linear approximation of the constraints from the dynamics and constraints arrays w.r.t. - * X = [u_{0}, x{1}, ..., u_{i}, x{i+1}, ..., u_{N}, x{N+1}]. + * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * G Z = g + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] * * @param[in] ocpSize: The size of optimal control problem. * @param[in] x0: The initial state. @@ -47,7 +60,7 @@ namespace ocs2 { * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. - * @param[out] res: The resulting constraints approxmation. Here we misused dfdx field to store the jacobian w.r.t. X. + * @param[out] res: The resulting constraints approxmation. Here we misused dfdx field to store the jacobian w.r.t. Z. */ void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, @@ -55,7 +68,20 @@ void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std:: /** * Constructs concatenated linear approximation of the constraints from the dynamics and constraints arrays w.r.t. - * X = [u_{0}, x{1}, ..., u_{i}, x{i+1}, ..., u_{N}, x{N+1}]. + * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * G Z = g + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] * * @param[in] ocpSize: The size of optimal control problem. * @param[in] x0: The initial state. @@ -63,7 +89,7 @@ void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std:: * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. - * @param[out] G: The jacobian of the concatenated constraints w.r.t. X. + * @param[out] G: The jacobian of the concatenated constraints w.r.t. Z. * @param[out] g: The concatenated constraints value. */ void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<VectorFunctionLinearApproximation>& dynamics, @@ -71,26 +97,47 @@ void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const Eigen::SparseMatrix<scalar_t>& G, vector_t& g); /** - * Constructs concatenated quadratic approximation of the total cost w.r.t. X = [u_{0}, x{1}, ..., u_{i}, x{i+1}, ..., u_{N}, x{N+1}]. + * Constructs concatenated quadratic approximation of the total cost w.r.t. Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * totalCost = 0.5 Z' H Z + Z' h + h0 + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] * * @param[in] ocpSize: The size of optimal control problem. * @param[in] x0: The initial state. * @param[in] cost: Quadratic approximation of the cost over the time horizon. * @param[out] res: The resulting cost approxmation. Here we misused dfdx and dfdxx fields to store the jacobian and the hessian - * matrices w.r.t. Xaug. + * matrices w.r.t. Z. */ void getCostMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, ScalarFunctionQuadraticApproximation& res); /** - * Constructs concatenated the jacobian and the hessian matrices of the total cost w.r.t. - * X = [u_{0}, x{1}, ..., u_{i}, x{i+1}, ..., u_{N}, x{N+1}]. + * Constructs concatenated the jacobian and the hessian matrices of the total cost w.r.t. Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * totalCost = 0.5 Z' H Z + Z' h + h0 + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] * * @param[in] ocpSize: The size of optimal control problem. * @param[in] x0: The initial state. * @param[in] cost: Quadratic approximation of the cost over the time horizon. - * @param[out] H: The concatenated hessian matrix w.r.t. X. - * @param[out] h: The concatenated jacobian vector w.r.t. X. + * @param[out] H: The concatenated hessian matrix w.r.t. Z. + * @param[out] h: The concatenated jacobian vector w.r.t. Z. */ void getCostMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, Eigen::SparseMatrix<scalar_t>& H, vector_t& h); diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h index 91adf1f25..82f3cbbe9 100644 --- a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h +++ b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h @@ -39,6 +39,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** + * There are a bunch of pre-conditioning methods aiming to shape different aspect of the problem. To balance the + * performance and computational effort, we choose a modified Ruzi equilibration summarized in Algorithm 2. + * Interested readers can find the original Ruiz equilibration in: + * "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices" + * * Calculates the scaling factor D, E and c, and scale the input dynamics, cost data in place in parallel. * * @param[in] : threadPool External thread pool. @@ -50,8 +55,8 @@ namespace ocs2 { * @param[out] : DOut Scaling factor D * @param[out] : EOut Scaling factor E * @param[out] scalingVectors : Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. - * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components of this type of matrix for every - * timestamp. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components + * of this type of matrix for every timestamp. * @param[out] cOut : Scaling factor c */ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, diff --git a/ocs2_slp/include/ocs2_slp/Helpers.h b/ocs2_slp/include/ocs2_slp/Helpers.h index 616e279c3..95f4f2e4d 100644 --- a/ocs2_slp/include/ocs2_slp/Helpers.h +++ b/ocs2_slp/include/ocs2_slp/Helpers.h @@ -36,10 +36,56 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace slp { +/** + * Computes the absolute sum of each row of hessian matrix H. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * totalCost = 0.5 Z' H Z + Z' h + h0 + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] cost: Quadratic approximation of the cost over the time horizon. + * @return: The absolute sum of rows of matrix H. + */ vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost); -vector_t GGTAbsRowSumInParallel(const OcpSize& ocpSize, const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, - ThreadPool& threadPool); +/** + * Computes the absolute sum of each row of matrix G G' in parallel. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * G Z = g + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * @param[in] threadPool: The thread pool. + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @return: The absolute sum of rows of matrix G G'. + */ +vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, + const vector_array_t* scalingVectorsPtr); + } // namespace slp } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_slp/src/Helpers.cpp b/ocs2_slp/src/Helpers.cpp index 8e582eb26..e8b1eebef 100644 --- a/ocs2_slp/src/Helpers.cpp +++ b/ocs2_slp/src/Helpers.cpp @@ -79,9 +79,10 @@ vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFuncti return res; } -vector_t GGTAbsRowSumInParallel(const OcpSize& ocpSize, const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr, - ThreadPool& threadPool) { +vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraints, + const vector_array_t* scalingVectorsPtr) { const int N = ocpSize.numStages; if (N < 1) { throw std::runtime_error("[GGTAbsRowSumInParallel] The number of stages cannot be less than 1."); diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index f97e1016e..24fd24524 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -255,7 +255,7 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta GGTMultiplication_.startTimer(); const vector_t rowwiseAbsSumGGT = - slp::GGTAbsRowSumInParallel(pipgSolver_.size(), dynamics_, nullptr, &scalingVectors, pipgSolver_.getThreadPool()); + slp::GGTAbsRowSumInParallel(pipgSolver_.getThreadPool(), pipgSolver_.size(), dynamics_, nullptr, &scalingVectors); GGTMultiplication_.endTimer(); sigmaEstimation_.startTimer(); diff --git a/ocs2_slp/test/testHelpers.cpp b/ocs2_slp/test/testHelpers.cpp index 090bfba41..eaf0fa0fc 100644 --- a/ocs2_slp/test/testHelpers.cpp +++ b/ocs2_slp/test/testHelpers.cpp @@ -87,7 +87,7 @@ TEST_F(HelperFunctionTest, GGTAbsRowSumInParallel) { } ocs2::getConstraintMatrix(ocpSize_, x0, dynamicsArray, nullptr, &scalingVectors, constraintsApproximation); - ocs2::vector_t rowwiseSum = ocs2::slp::GGTAbsRowSumInParallel(ocpSize_, dynamicsArray, nullptr, &scalingVectors, threadPool_); + ocs2::vector_t rowwiseSum = ocs2::slp::GGTAbsRowSumInParallel(threadPool_, ocpSize_, dynamicsArray, nullptr, &scalingVectors); ocs2::matrix_t GGT = constraintsApproximation.dfdx * constraintsApproximation.dfdx.transpose(); EXPECT_TRUE(rowwiseSum.isApprox(GGT.cwiseAbs().rowwise().sum())); } \ No newline at end of file From f611a23efbd78373b60a23dfebc174ce53137d24 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 13:42:51 +0100 Subject: [PATCH 402/512] new helper functions --- .../include/ocs2_oc/pre_condition/Scaling.h | 11 +-- ocs2_slp/include/ocs2_slp/Helpers.h | 77 ++++++++++++++++++- ocs2_slp/include/ocs2_slp/SlpSolver.h | 1 - ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h | 6 +- ocs2_slp/src/SlpSolver.cpp | 50 ++++++------ 5 files changed, 106 insertions(+), 39 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h index 82f3cbbe9..3cf29d1f2 100644 --- a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h +++ b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h @@ -39,13 +39,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * There are a bunch of pre-conditioning methods aiming to shape different aspect of the problem. To balance the - * performance and computational effort, we choose a modified Ruzi equilibration summarized in Algorithm 2. - * Interested readers can find the original Ruiz equilibration in: - * "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices" - * * Calculates the scaling factor D, E and c, and scale the input dynamics, cost data in place in parallel. * + * There are a few pre-conditioning methods aiming to shape different aspect of the problem. To balance the performance and + * computational effort, we choose a modified Ruzi equilibration summarized in Algorithm 2. Interested readers can find the + * original Ruiz equilibration in: "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices" + * + * + * * @param[in] : threadPool External thread pool. * @param[in] x0 : Initial state * @param[in] ocpSize : The size of the oc problem. diff --git a/ocs2_slp/include/ocs2_slp/Helpers.h b/ocs2_slp/include/ocs2_slp/Helpers.h index 95f4f2e4d..438abb10e 100644 --- a/ocs2_slp/include/ocs2_slp/Helpers.h +++ b/ocs2_slp/include/ocs2_slp/Helpers.h @@ -37,7 +37,7 @@ namespace ocs2 { namespace slp { /** - * Computes the absolute sum of each row of hessian matrix H. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * Computes the row-wise absolute sum of the cost hessian matrix, H. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". * * totalCost = 0.5 Z' H Z + Z' h + h0 * @@ -57,7 +57,39 @@ namespace slp { vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost); /** - * Computes the absolute sum of each row of matrix G G' in parallel. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * Computes the upper bound of eigenvalues for the total cost hessian matrix. The bound is computed based on the Gershgorin Circle Theorem. + * + * As the hessian matrix is a real symmetric matrix, all eigenvalues are real. Therefore, instead of estimating the + * upper bound on a complex plane, we can estimate the bound along a 1D real number axis. The estimated upper bound is + * defined as max_{i} [H_{ii} + R_{i}], where R_{i} is the radius of i'th Gershgorin discs. The Gershgorin discs radius is + * defined ad R_{i} = sum_{j \neq i} |H_{ij}|. Thus the upper bound of eigenvalues yields from max_{i}sum{j} |H_{ij}|. + * + * Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * totalCost = 0.5 Z' H Z + Z' h + h0 + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] cost: Quadratic approximation of the cost over the time horizon. + * @return: The upper bound of eigenvalues for H. + */ +inline scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost) { + const vector_t rowwiseAbsSumH = hessianAbsRowSum(ocpSize, cost); + return rowwiseAbsSumH.maxCoeff(); +} + +/** + * Computes the row-wise absolute sum of matrix G G' in parallel. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". * * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. * @@ -87,5 +119,46 @@ vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, const vector_array_t* scalingVectorsPtr); +/** + * Computes the upper bound of eigenvalues for the matrix G G' ( in parallel). The bound is computed based on the Gershgorin Circle Theorem. + * + * As the G G' matrix is a real symmetric matrix, all eigenvalues are real. Therefore, instead of estimating the + * upper bound on a complex plane, we can estimate the bound along a 1D real number axis. The estimated upper bound is + * defined as max_{i} [GG'_{ii} + R_{i}], where R_{i} is the radius of i'th Gershgorin discs. The Gershgorin discs radius is + * defined ad R_{i} = sum_{j \neq i} |GG'_{ij}|. Thus the upper bound of eigenvalues yields from max_{i}sum{j} |GG'_{ij}|. + * + * Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * G Z = g + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * @param[in] threadPool: The thread pool. + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @return: The upper bound of eigenvalues for G G'. + */ +inline scalar_t GGTEigenvaluesUpperBound(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, + const vector_array_t* scalingVectorsPtr) { + const vector_t rowwiseAbsSumGGT = GGTAbsRowSumInParallel(threadPool, ocpSize, dynamics, constraintsPtr, scalingVectorsPtr); + return rowwiseAbsSumGGT.maxCoeff(); +} + } // namespace slp } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_slp/include/ocs2_slp/SlpSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h index a19ca3f0f..0f54cb004 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSolver.h +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -187,7 +187,6 @@ class SlpSolver : public SolverBase { benchmark::RepeatedTimer computeControllerTimer_; // PIPG Solver - benchmark::RepeatedTimer GGTMultiplication_; benchmark::RepeatedTimer lambdaEstimation_; benchmark::RepeatedTimer sigmaEstimation_; benchmark::RepeatedTimer preConditioning_; diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index 2774f7203..385303aa7 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -64,13 +64,13 @@ class PipgSolver { * @param dynamics: Dynamics array. * @param cost: Cost array. * @param constraints: Constraints array. Pass nullptr for an unconstrained problem. - * @param scalingVectors Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. After - * scaling, they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param scalingVectors Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. * @param EInv Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. * @param mu: the lower bound of the cost hessian H. * @param lambda: the upper bound of the cost hessian H. * @param sigma: the upper bound of \f$ G^TG \f$ - * @return SolverStatus + * @return Solver satus */ pipg::SolverStatus solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 24fd24524..1801e46f0 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -87,7 +87,6 @@ void SlpSolver::reset() { solveQpTimer_.reset(); linesearchTimer_.reset(); computeControllerTimer_.reset(); - GGTMultiplication_.reset(); lambdaEstimation_.reset(); sigmaEstimation_.reset(); preConditioning_.reset(); @@ -95,22 +94,19 @@ void SlpSolver::reset() { } std::string SlpSolver::getBenchmarkingInformationPIPG() const { - const auto GGTMultiplication = GGTMultiplication_.getTotalInMilliseconds(); - const auto preConditioning = preConditioning_.getTotalInMilliseconds(); const auto lambdaEstimation = lambdaEstimation_.getTotalInMilliseconds(); const auto sigmaEstimation = sigmaEstimation_.getTotalInMilliseconds(); + const auto preConditioning = preConditioning_.getTotalInMilliseconds(); const auto pipgRuntime = pipgSolverTimer_.getTotalInMilliseconds(); - const auto benchmarkTotal = GGTMultiplication + preConditioning + lambdaEstimation + sigmaEstimation + pipgRuntime; + const auto benchmarkTotal = preConditioning + lambdaEstimation + sigmaEstimation + pipgRuntime; std::stringstream infoStream; if (benchmarkTotal > 0.0) { const scalar_t inPercent = 100.0; infoStream << "\n########################################################################\n"; - infoStream << "The benchmarking is computed over " << GGTMultiplication_.getNumTimedIntervals() << " iterations. \n"; + infoStream << "The benchmarking is computed over " << preConditioning_.getNumTimedIntervals() << " iterations. \n"; infoStream << "PIPG Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; - infoStream << "\tGGTMultiplication :\t" << std::setw(10) << GGTMultiplication_.getAverageInMilliseconds() << " [ms] \t(" - << GGTMultiplication / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tpreConditioning :\t" << std::setw(10) << preConditioning_.getAverageInMilliseconds() << " [ms] \t(" << preConditioning / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tlambdaEstimation :\t" << std::setw(10) << lambdaEstimation_.getAverageInMilliseconds() << " [ms] \t(" @@ -239,37 +235,35 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta // without constraints, or when using projection, we have an unconstrained QP. pipgSolver_.resize(extractSizesFromProblem(dynamics_, cost_, nullptr)); + // pre-condition the OCP + preConditioning_.startTimer(); + scalar_t c; vector_array_t D, E; vector_array_t scalingVectors; - scalar_t c; - - preConditioning_.startTimer(); - preConditioningInPlaceInParallel(pipgSolver_.getThreadPool(), delta_x0, pipgSolver_.size(), pipgSolver_.settings().numScaling, dynamics_, - cost_, D, E, scalingVectors, c); + preConditioningInPlaceInParallel(threadPool_, delta_x0, pipgSolver_.size(), pipgSolver_.settings().numScaling, dynamics_, cost_, D, E, + scalingVectors, c); preConditioning_.endTimer(); + // estimate mu and lambda: mu I < H < lambda I + const auto muEstimated = [&]() { + scalar_t maxScalingFactor = -1; + for (auto& v : D) { + if (v.size() != 0) { + maxScalingFactor = std::max(maxScalingFactor, v.maxCoeff()); + } + } + return c * pipgSolver_.settings().lowerBoundH * maxScalingFactor * maxScalingFactor; + }(); lambdaEstimation_.startTimer(); - const vector_t rowwiseAbsSumH = slp::hessianAbsRowSum(pipgSolver_.size(), cost_); - const scalar_t lambdaScaled = rowwiseAbsSumH.maxCoeff(); + const auto lambdaScaled = slp::hessianEigenvaluesUpperBound(pipgSolver_.size(), cost_); lambdaEstimation_.endTimer(); - GGTMultiplication_.startTimer(); - const vector_t rowwiseAbsSumGGT = - slp::GGTAbsRowSumInParallel(pipgSolver_.getThreadPool(), pipgSolver_.size(), dynamics_, nullptr, &scalingVectors); - GGTMultiplication_.endTimer(); - + // estimate sigma: G' G < sigma I + // However, since the G'G and GG' have exactly the same set of eigenvalues value: G G' < sigma I sigmaEstimation_.startTimer(); - const scalar_t sigmaScaled = rowwiseAbsSumGGT.maxCoeff(); + const auto sigmaScaled = slp::GGTEigenvaluesUpperBound(threadPool_, pipgSolver_.size(), dynamics_, nullptr, &scalingVectors); sigmaEstimation_.endTimer(); - scalar_t maxScalingFactor = -1; - for (auto& v : D) { - if (v.size() != 0) { - maxScalingFactor = std::max(maxScalingFactor, v.maxCoeff()); - } - } - const scalar_t muEstimated = pipgSolver_.settings().lowerBoundH * c * maxScalingFactor * maxScalingFactor; - pipgSolverTimer_.startTimer(); vector_array_t EInv(E.size()); std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); From 1e121a47f8aa54b290c2e25afc01ba4404e1018e Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 15:02:35 +0100 Subject: [PATCH 403/512] remvong timer from densePipg --- .../include/ocs2_slp/pipg/DensePipgSolver.h | 11 +---- ocs2_slp/src/pipg/DensePipgSolver.cpp | 45 +------------------ 2 files changed, 4 insertions(+), 52 deletions(-) diff --git a/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h index f9343896f..55f097604 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h @@ -38,13 +38,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace pipg { -struct DensePipgBenchmark { - benchmark::RepeatedTimer vComputation; - benchmark::RepeatedTimer zComputation; - benchmark::RepeatedTimer wComputation; - benchmark::RepeatedTimer convergenceCheck; -}; - /* * First order primal-dual method for solving optimal control problem based on: * "Proportional-Integral Projected Gradient Method for Model Predictive Control" @@ -52,7 +45,7 @@ struct DensePipgBenchmark { */ SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution, DensePipgBenchmark* timerPtr = nullptr); + const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution); /** * Deserializes the stacked solution to state-input trajecotries. @@ -61,7 +54,7 @@ SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix * @param[in] stackedSolution : Defined as [u[0], x[1], ..., u[N-1], x[N]]. * @param[in] x0 : The initial state. * @param[out] xTrajectory : State tarjectory. - * @param[out] UTrajectory : Input trajecotry. + * @param[out] uTrajectory : Input trajecotry. */ void unpackSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, vector_array_t& uTrajectory); diff --git a/ocs2_slp/src/pipg/DensePipgSolver.cpp b/ocs2_slp/src/pipg/DensePipgSolver.cpp index 295d66312..7bb7df8b5 100644 --- a/ocs2_slp/src/pipg/DensePipgSolver.cpp +++ b/ocs2_slp/src/pipg/DensePipgSolver.cpp @@ -37,7 +37,7 @@ namespace pipg { SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution, DensePipgBenchmark* timerPtr) { + const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution) { // Cold start vector_t z = vector_t::Zero(H.cols()); vector_t z_old = vector_t::Zero(H.cols()); @@ -55,44 +55,24 @@ SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix const scalar_t beta = (k + 1) * mu / (2.0 * sigma); z_old.swap(z); - if (timerPtr != nullptr) { - timerPtr->vComputation.startTimer(); - } + // v = w + beta * (G * z - g); v = -g; v.noalias() += G * z; v *= beta; v += w; - if (timerPtr != nullptr) { - timerPtr->vComputation.endTimer(); - } - if (timerPtr != nullptr) { - timerPtr->zComputation.startTimer(); - } // z = z_old - alpha * (H * z_old + h + G.transpose() * v); z = -h; z.noalias() -= (H * z_old); z.noalias() -= (G.transpose() * v); z *= alpha; z.noalias() += z_old; - if (timerPtr != nullptr) { - timerPtr->zComputation.endTimer(); - } - if (timerPtr != nullptr) { - timerPtr->wComputation.startTimer(); - } // w = w + beta * (G * z - g); w -= beta * g; w.noalias() += beta * (G * z); - if (timerPtr != nullptr) { - timerPtr->wComputation.endTimer(); - } - if (timerPtr != nullptr) { - timerPtr->convergenceCheck.startTimer(); - } if (k % settings.checkTerminationInterval == 0) { const scalar_t zNorm = z.squaredNorm(); @@ -107,9 +87,6 @@ SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix constraintsViolationInfNorm <= settings.absoluteTolerance && (z_deltaNorm <= settings.relativeTolerance * settings.relativeTolerance * zNorm || z_deltaNorm <= settings.absoluteTolerance); } - if (timerPtr != nullptr) { - timerPtr->convergenceCheck.endTimer(); - } ++k; } // end of while loop @@ -125,24 +102,6 @@ SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix std::cerr << "Number of Iterations: " << k << " out of " << settings.maxNumIterations << "\n"; std::cerr << "Norm of delta primal solution: " << (stackedSolution - z_old).norm() << "\n"; std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; - if (timerPtr != nullptr) { - const auto step1v = timerPtr->vComputation.getTotalInMilliseconds(); - const auto step2z = timerPtr->zComputation.getTotalInMilliseconds(); - const auto step3w = timerPtr->wComputation.getTotalInMilliseconds(); - const auto step4CheckConvergence = timerPtr->convergenceCheck.getTotalInMilliseconds(); - const auto benchmarkTotal = step1v + step2z + step3w + step4CheckConvergence; - std::cerr << "\n########################################################################\n"; - std::cerr << "The benchmarking is computed over " << k << " iterations. \n"; - std::cerr << "PIPG Dense Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; - std::cerr << "\tvComputation :\t" << timerPtr->vComputation.getAverageInMilliseconds() << " [ms] \t\t(" - << step1v / benchmarkTotal * 100.0 << "%)\n"; - std::cerr << "\tzComputation :\t" << timerPtr->zComputation.getAverageInMilliseconds() << " [ms] \t\t(" - << step2z / benchmarkTotal * 100.0 << "%)\n"; - std::cerr << "\twComputation :\t" << timerPtr->wComputation.getAverageInMilliseconds() << " [ms] \t\t(" - << step3w / benchmarkTotal * 100.0 << "%)\n"; - std::cerr << "\tCheckConvergence :\t" << timerPtr->convergenceCheck.getAverageInMilliseconds() << " [ms] \t\t(" - << step4CheckConvergence / benchmarkTotal * 100.0 << "%)\n"; - } } return status; From e245bd70fb50854bc27e47438d2fd5ea79f4e201 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 15:09:10 +0100 Subject: [PATCH 404/512] renaming --- .../pipg/{DensePipgSolver.h => SingleThreadPipg.h} | 6 +++--- .../pipg/{DensePipgSolver.cpp => SingleThreadPipg.cpp} | 8 ++++---- ocs2_slp/test/testPipgSolver.cpp | 8 ++++---- 3 files changed, 11 insertions(+), 11 deletions(-) rename ocs2_slp/include/ocs2_slp/pipg/{DensePipgSolver.h => SingleThreadPipg.h} (89%) rename ocs2_slp/src/pipg/{DensePipgSolver.cpp => SingleThreadPipg.cpp} (93%) diff --git a/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h similarity index 89% rename from ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h rename to ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h index 55f097604..2ff9dbfcf 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/DensePipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h @@ -43,9 +43,9 @@ namespace pipg { * "Proportional-Integral Projected Gradient Method for Model Predictive Control" * https://arxiv.org/abs/2009.06980 */ -SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, - const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution); +SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, + const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, + const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution); /** * Deserializes the stacked solution to state-input trajecotries. diff --git a/ocs2_slp/src/pipg/DensePipgSolver.cpp b/ocs2_slp/src/pipg/SingleThreadPipg.cpp similarity index 93% rename from ocs2_slp/src/pipg/DensePipgSolver.cpp rename to ocs2_slp/src/pipg/SingleThreadPipg.cpp index 7bb7df8b5..47c573793 100644 --- a/ocs2_slp/src/pipg/DensePipgSolver.cpp +++ b/ocs2_slp/src/pipg/SingleThreadPipg.cpp @@ -27,7 +27,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_slp/pipg/DensePipgSolver.h" +#include "ocs2_slp/pipg/SingleThreadPipg.h" #include <iostream> #include <numeric> @@ -35,9 +35,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace pipg { -SolverStatus densePipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, - const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution) { +SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, + const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, + const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution) { // Cold start vector_t z = vector_t::Zero(H.cols()); vector_t z_old = vector_t::Zero(H.cols()); diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp index 311865b5e..297991fe1 100644 --- a/ocs2_slp/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -34,8 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/test/testProblemsGeneration.h> #include <ocs2_qp_solver/QpSolver.h> -#include "ocs2_slp/pipg/DensePipgSolver.h" #include "ocs2_slp/pipg/PipgSolver.h" +#include "ocs2_slp/pipg/SingleThreadPipg.h" ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, ocs2::scalar_t relativeTolerance, bool verbose) { @@ -108,9 +108,9 @@ TEST_F(PIPGSolverTest, correctness) { ocs2::scalar_t sigma = svdGTG.singularValues()(0); ocs2::vector_t primalSolutionPIPG; - ocs2::pipg::densePipg(solver.settings(), costApproximation.dfdxx.sparseView(), costApproximation.dfdx, - constraintsApproximation.dfdx.sparseView(), constraintsApproximation.f, - ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, primalSolutionPIPG); + std::ignore = ocs2::pipg::singleThreadPipg( + solver.settings(), costApproximation.dfdxx.sparseView(), costApproximation.dfdx, constraintsApproximation.dfdx.sparseView(), + constraintsApproximation.f, ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, primalSolutionPIPG); ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); solver.solve(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma); From 9133a189c2b38671dd94be53729914326858f168 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 15:41:58 +0100 Subject: [PATCH 405/512] removing threadpool from pipg --- .../include/ocs2_oc/pre_condition/Scaling.h | 8 ++-- ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h | 4 -- ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h | 39 +++++++++---------- ocs2_slp/src/SlpSolver.cpp | 2 +- ocs2_slp/src/pipg/PipgSettings.cpp | 3 -- ocs2_slp/src/pipg/PipgSolver.cpp | 11 +++--- ocs2_slp/test/testPipgSolver.cpp | 11 +++--- ocs2_slp/test/testSlpSolver.cpp | 3 -- 8 files changed, 33 insertions(+), 48 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h index 3cf29d1f2..637ab9866 100644 --- a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h +++ b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h @@ -42,12 +42,10 @@ namespace ocs2 { * Calculates the scaling factor D, E and c, and scale the input dynamics, cost data in place in parallel. * * There are a few pre-conditioning methods aiming to shape different aspect of the problem. To balance the performance and - * computational effort, we choose a modified Ruzi equilibration summarized in Algorithm 2. Interested readers can find the - * original Ruiz equilibration in: "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices" + * computational effort, we choose a modified Ruzi equilibration Algorithm. Interested readers can find the original Ruiz + * equilibration in: "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices" * - * - * - * @param[in] : threadPool External thread pool. + * @param[in] : threadPool : The external thread pool. * @param[in] x0 : Initial state * @param[in] ocpSize : The size of the oc problem. * @param[in] iteration : Number of iteration. diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h index cccc91521..43ede260d 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h @@ -35,10 +35,6 @@ namespace ocs2 { namespace pipg { struct Settings { - /** Number of threads used in the multi-threading scheme. */ - size_t nThreads = 3; - /** Priority of threads used in the multi-threading scheme. */ - int threadPriority = 50; /** Maximum number of iterations of PIPG. */ size_t maxNumIterations = 3000; /** Termination criteria. **/ diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index 385303aa7..a5443870f 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -51,28 +51,28 @@ namespace ocs2 { class PipgSolver { public: /** - * @brief Constructor. - * - * @param Settings: PIPG setting + * Constructor. + * @param[in] Settings: PIPG setting */ explicit PipgSolver(pipg::Settings settings); /** - * @brief Solve Optimal Control in parallel. + * Solve the optimal control in parallel. * - * @param x0 Initial state - * @param dynamics: Dynamics array. - * @param cost: Cost array. - * @param constraints: Constraints array. Pass nullptr for an unconstrained problem. - * @param scalingVectors Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, - * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. - * @param EInv Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. - * @param mu: the lower bound of the cost hessian H. - * @param lambda: the upper bound of the cost hessian H. - * @param sigma: the upper bound of \f$ G^TG \f$ - * @return Solver satus + * @param[in] threadPool : The external thread pool. + * @param[in] x0 : Initial state + * @param[in] dynamics : Dynamics array. + * @param[in] cost : Cost array. + * @param[in] constraints : Constraints array. Pass nullptr for an unconstrained problem. + * @param[in] scalingVectors : Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param[in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. + * @param[in] mu : the lower bound of the cost hessian H. + * @param[in] lambda : the upper bound of the cost hessian H. + * @param[in] sigma : the upper bound of \f$ G^TG \f$. + * @return The solver status. */ - pipg::SolverStatus solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, + pipg::SolverStatus solve(ThreadPool& threadPool, const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma); @@ -94,7 +94,6 @@ class PipgSolver { const pipg::Settings& settings() const { return settings_; } const OcpSize& size() const { return ocpSize_; } - ThreadPool& getThreadPool() { return threadPool_; } private: void verifySizes(const std::vector<VectorFunctionLinearApproximation>& dynamics, @@ -103,14 +102,12 @@ class PipgSolver { void verifyOcpSize(const OcpSize& ocpSize) const; - void runParallel(std::function<void(int)> taskFunction) { threadPool_.runParallel(std::move(taskFunction), settings().nThreads); } - private: const pipg::Settings settings_; - ThreadPool threadPool_; + OcpSize ocpSize_; - // Data buffer for parallelized QP + // Data buffer for parallelized PIPG vector_array_t X_, W_, V_, U_; vector_array_t XNew_, UNew_, WNew_; diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 1801e46f0..161f7f159 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -268,7 +268,7 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta vector_array_t EInv(E.size()); std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); const auto pipgStatus = - pipgSolver_.solve(delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled); + pipgSolver_.solve(threadPool_, delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled); pipgSolver_.getStateInputTrajectoriesSolution(deltaXSol, deltaUSol); pipgSolverTimer_.endTimer(); diff --git a/ocs2_slp/src/pipg/PipgSettings.cpp b/ocs2_slp/src/pipg/PipgSettings.cpp index 27b20d77f..197932833 100644 --- a/ocs2_slp/src/pipg/PipgSettings.cpp +++ b/ocs2_slp/src/pipg/PipgSettings.cpp @@ -49,9 +49,6 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, std::cerr << " #### PIPG Settings: {\n"; } - loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); - loadData::loadPtreeValue(pt, settings.threadPriority, fieldName + ".threadPriority", verbose); - loadData::loadPtreeValue(pt, settings.maxNumIterations, fieldName + ".maxNumIterations", verbose); loadData::loadPtreeValue(pt, settings.absoluteTolerance, fieldName + ".absoluteTolerance", verbose); loadData::loadPtreeValue(pt, settings.relativeTolerance, fieldName + ".relativeTolerance", verbose); diff --git a/ocs2_slp/src/pipg/PipgSolver.cpp b/ocs2_slp/src/pipg/PipgSolver.cpp index 543ad585d..745a59afc 100644 --- a/ocs2_slp/src/pipg/PipgSolver.cpp +++ b/ocs2_slp/src/pipg/PipgSolver.cpp @@ -39,15 +39,14 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -PipgSolver::PipgSolver(pipg::Settings settings) - : settings_(std::move(settings)), threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { +PipgSolver::PipgSolver(pipg::Settings settings) : settings_(std::move(settings)) { Eigen::initParallel(); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -pipg::SolverStatus PipgSolver::solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, +pipg::SolverStatus PipgSolver::solve(ThreadPool& threadPool, const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, @@ -95,7 +94,7 @@ pipg::SolverStatus PipgSolver::solve(const vector_t& x0, std::vector<VectorFunct std::mutex mux; std::condition_variable iterationFinished; - std::vector<int> threadsWorkloadCounter(threadPool_.numThreads() + 1, 0); + std::vector<int> threadsWorkloadCounter(threadPool.numThreads() + 1U, 0); auto updateVariablesTask = [&](int workerId) { int t; @@ -111,7 +110,7 @@ pipg::SolverStatus PipgSolver::solve(const vector_t& x0, std::vector<VectorFunct shouldWait = true; } // Multi-thread performance analysis - threadsWorkloadCounter[workerId]++; + ++threadsWorkloadCounter[workerId]; // PIPG algorithm const auto& A = dynamics[t - 1].dfdx; @@ -229,7 +228,7 @@ pipg::SolverStatus PipgSolver::solve(const vector_t& x0, std::vector<VectorFunct } } }; - runParallel(std::move(updateVariablesTask)); + threadPool.runParallel(std::move(updateVariablesTask), threadPool.numThreads() + 1U); pipg::SolverStatus status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; if (settings().displayShortSummary) { diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp index 297991fe1..3ec955b54 100644 --- a/ocs2_slp/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -37,10 +37,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_slp/pipg/PipgSolver.h" #include "ocs2_slp/pipg/SingleThreadPipg.h" -ocs2::pipg::Settings configurePipg(size_t nThreads, size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, - ocs2::scalar_t relativeTolerance, bool verbose) { +ocs2::pipg::Settings configurePipg(size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, ocs2::scalar_t relativeTolerance, + bool verbose) { ocs2::pipg::Settings settings; - settings.nThreads = nThreads; settings.maxNumIterations = maxNumIterations; settings.absoluteTolerance = absoluteTolerance; settings.relativeTolerance = relativeTolerance; @@ -59,9 +58,10 @@ class PIPGSolverTest : public testing::Test { static constexpr size_t nc_ = 0; static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); static constexpr size_t numConstraints = N_ * (nx_ + nc_); + static constexpr size_t numThreads = 8; static constexpr bool verbose = true; - PIPGSolverTest() : solver(configurePipg(8, 30000, 1e-10, 1e-3, verbose)) { + PIPGSolverTest() : solver(configurePipg(30000, 1e-10, 1e-3, verbose)) { srand(10); // Construct OCP problem @@ -88,6 +88,7 @@ class PIPGSolverTest : public testing::Test { std::vector<ocs2::VectorFunctionLinearApproximation> constraintsArray; ocs2::PipgSolver solver; + ocs2::ThreadPool threadPool{numThreads - 1u, 50}; }; constexpr size_t PIPGSolverTest::numDecisionVariables; @@ -113,7 +114,7 @@ TEST_F(PIPGSolverTest, correctness) { constraintsApproximation.f, ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, primalSolutionPIPG); ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); - solver.solve(x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma); + solver.solve(threadPool, x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma); ocs2::vector_array_t X, U; solver.getStateInputTrajectoriesSolution(X, U); diff --git a/ocs2_slp/test/testSlpSolver.cpp b/ocs2_slp/test/testSlpSolver.cpp index da423dad0..8e0ecb67e 100644 --- a/ocs2_slp/test/testSlpSolver.cpp +++ b/ocs2_slp/test/testSlpSolver.cpp @@ -66,7 +66,6 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct // Solver settings auto getPipgSettings = [&]() { ocs2::pipg::Settings settings; - settings.nThreads = 100; settings.maxNumIterations = 30000; settings.absoluteTolerance = tol; settings.relativeTolerance = 1e-2; @@ -74,7 +73,6 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct settings.lowerBoundH = 1e-3; settings.checkTerminationInterval = 1; settings.displayShortSummary = true; - return settings; }; @@ -87,7 +85,6 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct settings.printLinesearch = true; settings.nThreads = 100; settings.pipgSettings = getPipgSettings(); - return settings; }(); From 5e64350ee0acf9c79f68e846aff10ddf4266823d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 16:02:45 +0100 Subject: [PATCH 406/512] moving scalingIteration to SlpSettings --- ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info | 3 +-- ocs2_slp/include/ocs2_slp/SlpSettings.h | 7 ++++--- ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h | 2 -- ocs2_slp/src/SlpSettings.cpp | 1 + ocs2_slp/src/SlpSolver.cpp | 8 ++++---- ocs2_slp/src/pipg/PipgSettings.cpp | 1 - ocs2_slp/test/testSlpSolver.cpp | 2 +- 7 files changed, 11 insertions(+), 13 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info index 0a07f162d..15295ac88 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info @@ -17,6 +17,7 @@ slp { dt 0.1 slpIteration 5 + scalingIteration 3 deltaTol 1e-3 printSolverStatistics true printSolverStatus false @@ -25,11 +26,9 @@ slp nThreads 4 pipg { - nThreads 4 maxNumIterations 7000 absoluteTolerance 1e-3 relativeTolerance 1e-2 - numScaling 3 lowerBoundH 0.2 checkTerminationInterval 10 displayShortSummary false diff --git a/ocs2_slp/include/ocs2_slp/SlpSettings.h b/ocs2_slp/include/ocs2_slp/SlpSettings.h index bc268af98..12c0429c2 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSettings.h +++ b/ocs2_slp/include/ocs2_slp/SlpSettings.h @@ -39,9 +39,10 @@ namespace slp { /** Multiple-shooting SLP (Successive Linear Programming) settings */ struct Settings { - size_t slpIteration = 10; // Maximum number of SLP iterations - scalar_t deltaTol = 1e-6; // Termination condition : RMS update of x(t) and u(t) are both below this value - scalar_t costTol = 1e-4; // Termination condition : (cost{i+1} - (cost{i}) < costTol AND constraints{i+1} < g_min + size_t slpIteration = 10; // Maximum number of SLP iterations + size_t scalingIteration = 3; // Number of pre-conditioning iterations + scalar_t deltaTol = 1e-6; // Termination condition : RMS update of x(t) and u(t) are both below this value + scalar_t costTol = 1e-4; // Termination condition : (cost{i+1} - (cost{i}) < costTol AND constraints{i+1} < g_min // Linesearch - step size rules scalar_t alpha_decay = 0.5; // multiply the step size by this factor every time a linesearch step is rejected. diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h index 43ede260d..1df95244a 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h @@ -42,8 +42,6 @@ struct Settings { scalar_t relativeTolerance = 1e-2; /** Number of iterations between consecutive calculation of termination conditions. **/ size_t checkTerminationInterval = 1; - /** Number of pre-conditioning run. **/ - int numScaling = 3; /** The static lower bound of the cost hessian H. **/ scalar_t lowerBoundH = 5e-6; /** This value determines to display the a summary log. */ diff --git a/ocs2_slp/src/SlpSettings.cpp b/ocs2_slp/src/SlpSettings.cpp index 9676f38ea..1980f4849 100644 --- a/ocs2_slp/src/SlpSettings.cpp +++ b/ocs2_slp/src/SlpSettings.cpp @@ -51,6 +51,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, } loadData::loadPtreeValue(pt, settings.slpIteration, fieldName + ".slpIteration", verbose); + loadData::loadPtreeValue(pt, settings.scalingIteration, fieldName + ".scalingIteration", verbose); loadData::loadPtreeValue(pt, settings.deltaTol, fieldName + ".deltaTol", verbose); loadData::loadPtreeValue(pt, settings.alpha_decay, fieldName + ".alpha_decay", verbose); loadData::loadPtreeValue(pt, settings.alpha_min, fieldName + ".alpha_min", verbose); diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 161f7f159..51f90eeac 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -45,7 +45,7 @@ namespace ocs2 { SlpSolver::SlpSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) : settings_(std::move(settings)), pipgSolver_(settings_.pipgSettings), - threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { + threadPool_(std::max(settings_.nThreads - 1, size_t(1)) - 1, settings_.threadPriority) { Eigen::setNbThreads(1); // No multithreading within Eigen. Eigen::initParallel(); @@ -132,10 +132,10 @@ std::string SlpSolver::getBenchmarkingInformation() const { const scalar_t inPercent = 100.0; infoStream << "\n########################################################################\n"; infoStream << "The benchmarking is computed over " << totalNumIterations_ << " iterations. \n"; - infoStream << "SQP Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "SLP Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; infoStream << "\tLQ Approximation :\t" << std::setw(10) << linearQuadraticApproximationTimer_.getAverageInMilliseconds() << " [ms] \t(" << linearQuadraticApproximationTotal / benchmarkTotal * inPercent << "%)\n"; - infoStream << "\tSolve QP :\t" << std::setw(10) << solveQpTimer_.getAverageInMilliseconds() << " [ms] \t(" + infoStream << "\tSolve LP :\t" << std::setw(10) << solveQpTimer_.getAverageInMilliseconds() << " [ms] \t(" << solveQpTotal / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tLinesearch :\t" << std::setw(10) << linesearchTimer_.getAverageInMilliseconds() << " [ms] \t(" << linesearchTotal / benchmarkTotal * inPercent << "%)\n"; @@ -240,7 +240,7 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta scalar_t c; vector_array_t D, E; vector_array_t scalingVectors; - preConditioningInPlaceInParallel(threadPool_, delta_x0, pipgSolver_.size(), pipgSolver_.settings().numScaling, dynamics_, cost_, D, E, + preConditioningInPlaceInParallel(threadPool_, delta_x0, pipgSolver_.size(), settings_.scalingIteration, dynamics_, cost_, D, E, scalingVectors, c); preConditioning_.endTimer(); diff --git a/ocs2_slp/src/pipg/PipgSettings.cpp b/ocs2_slp/src/pipg/PipgSettings.cpp index 197932833..123dc372b 100644 --- a/ocs2_slp/src/pipg/PipgSettings.cpp +++ b/ocs2_slp/src/pipg/PipgSettings.cpp @@ -53,7 +53,6 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, loadData::loadPtreeValue(pt, settings.absoluteTolerance, fieldName + ".absoluteTolerance", verbose); loadData::loadPtreeValue(pt, settings.relativeTolerance, fieldName + ".relativeTolerance", verbose); - loadData::loadPtreeValue(pt, settings.numScaling, fieldName + ".numScaling", verbose); loadData::loadPtreeValue(pt, settings.lowerBoundH, fieldName + ".lowerBoundH", verbose); loadData::loadPtreeValue(pt, settings.checkTerminationInterval, fieldName + ".checkTerminationInterval", verbose); diff --git a/ocs2_slp/test/testSlpSolver.cpp b/ocs2_slp/test/testSlpSolver.cpp index 8e0ecb67e..fd7cd8d9b 100644 --- a/ocs2_slp/test/testSlpSolver.cpp +++ b/ocs2_slp/test/testSlpSolver.cpp @@ -69,7 +69,6 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct settings.maxNumIterations = 30000; settings.absoluteTolerance = tol; settings.relativeTolerance = 1e-2; - settings.numScaling = 3; settings.lowerBoundH = 1e-3; settings.checkTerminationInterval = 1; settings.displayShortSummary = true; @@ -80,6 +79,7 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solve(const VectorFunct ocs2::slp::Settings settings; settings.dt = 0.05; settings.slpIteration = 10; + settings.scalingIteration = 3; settings.printSolverStatistics = true; settings.printSolverStatus = true; settings.printLinesearch = true; From 7406c990f61e17a03a35ebd775d6c12efb35fef8 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 17:55:29 +0100 Subject: [PATCH 407/512] introduction of PipgBounds --- ocs2_slp/include/ocs2_slp/Helpers.h | 82 +++++++++---------- ocs2_slp/include/ocs2_slp/pipg/PipgBounds.h | 76 +++++++++++++++++ ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h | 15 ++-- .../include/ocs2_slp/pipg/SingleThreadPipg.h | 48 +++++++++-- ocs2_slp/src/Helpers.cpp | 13 +++ ocs2_slp/src/SlpSolver.cpp | 6 +- ocs2_slp/src/pipg/PipgSolver.cpp | 24 +++--- ocs2_slp/src/pipg/SingleThreadPipg.cpp | 14 ++-- ocs2_slp/test/testPipgSolver.cpp | 18 ++-- 9 files changed, 208 insertions(+), 88 deletions(-) create mode 100644 ocs2_slp/include/ocs2_slp/pipg/PipgBounds.h diff --git a/ocs2_slp/include/ocs2_slp/Helpers.h b/ocs2_slp/include/ocs2_slp/Helpers.h index 438abb10e..08947e053 100644 --- a/ocs2_slp/include/ocs2_slp/Helpers.h +++ b/ocs2_slp/include/ocs2_slp/Helpers.h @@ -36,26 +36,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace slp { -/** - * Computes the row-wise absolute sum of the cost hessian matrix, H. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". - * - * totalCost = 0.5 Z' H Z + Z' h + h0 - * - * H = [ R0 - * * Q1 P1' - * * P1 R1 - * * * * Qn Pn' - * * * * Pn Rn - * * * * * * Q{n+1}] - * - * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] - * - * @param[in] ocpSize: The size of optimal control problem. - * @param[in] cost: Quadratic approximation of the cost over the time horizon. - * @return: The absolute sum of rows of matrix H. - */ -vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost); - /** * Computes the upper bound of eigenvalues for the total cost hessian matrix. The bound is computed based on the Gershgorin Circle Theorem. * @@ -83,13 +63,17 @@ vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFuncti * @param[in] cost: Quadratic approximation of the cost over the time horizon. * @return: The upper bound of eigenvalues for H. */ -inline scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost) { - const vector_t rowwiseAbsSumH = hessianAbsRowSum(ocpSize, cost); - return rowwiseAbsSumH.maxCoeff(); -} +scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost); /** - * Computes the row-wise absolute sum of matrix G G' in parallel. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * Computes the upper bound of eigenvalues for the matrix G G' ( in parallel). The bound is computed based on the Gershgorin Circle Theorem. + * + * As the G G' matrix is a real symmetric matrix, all eigenvalues are real. Therefore, instead of estimating the + * upper bound on a complex plane, we can estimate the bound along a 1D real number axis. The estimated upper bound is + * defined as max_{i} [GG'_{ii} + R_{i}], where R_{i} is the radius of i'th Gershgorin discs. The Gershgorin discs radius is + * defined ad R_{i} = sum_{j \neq i} |GG'_{ij}|. Thus the upper bound of eigenvalues yields from max_{i}sum{j} |GG'_{ij}|. + * + * Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". * * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. * @@ -112,22 +96,35 @@ inline scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std:: * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. - * @return: The absolute sum of rows of matrix G G'. + * @return: The upper bound of eigenvalues for G G'. */ -vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, - const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, - const vector_array_t* scalingVectorsPtr); +scalar_t GGTEigenvaluesUpperBound(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, + const vector_array_t* scalingVectorsPtr); /** - * Computes the upper bound of eigenvalues for the matrix G G' ( in parallel). The bound is computed based on the Gershgorin Circle Theorem. + * Computes the row-wise absolute sum of the cost hessian matrix, H. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". * - * As the G G' matrix is a real symmetric matrix, all eigenvalues are real. Therefore, instead of estimating the - * upper bound on a complex plane, we can estimate the bound along a 1D real number axis. The estimated upper bound is - * defined as max_{i} [GG'_{ii} + R_{i}], where R_{i} is the radius of i'th Gershgorin discs. The Gershgorin discs radius is - * defined ad R_{i} = sum_{j \neq i} |GG'_{ij}|. Thus the upper bound of eigenvalues yields from max_{i}sum{j} |GG'_{ij}|. + * totalCost = 0.5 Z' H Z + Z' h + h0 * - * Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] cost: Quadratic approximation of the cost over the time horizon. + * @return: The absolute sum of rows of matrix H. + */ +vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost); + +/** + * Computes the row-wise absolute sum of matrix G G' in parallel. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". * * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. * @@ -150,15 +147,12 @@ vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. - * @return: The upper bound of eigenvalues for G G'. + * @return: The absolute sum of rows of matrix G G'. */ -inline scalar_t GGTEigenvaluesUpperBound(ThreadPool& threadPool, const OcpSize& ocpSize, - const std::vector<VectorFunctionLinearApproximation>& dynamics, - const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, - const vector_array_t* scalingVectorsPtr) { - const vector_t rowwiseAbsSumGGT = GGTAbsRowSumInParallel(threadPool, ocpSize, dynamics, constraintsPtr, scalingVectorsPtr); - return rowwiseAbsSumGGT.maxCoeff(); -} +vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, + const vector_array_t* scalingVectorsPtr); } // namespace slp } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgBounds.h b/ocs2_slp/include/ocs2_slp/pipg/PipgBounds.h new file mode 100644 index 000000000..c0659a93d --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgBounds.h @@ -0,0 +1,76 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> + +namespace ocs2 { +namespace pipg { + +/** + * Defines the lower and upper bounds of the total cost hessian, H, and the upper bound of \f$ G^TG \f$. + * Refer to "Proportional-Integral Projected Gradient Method for Model Predictive Control" + * Link: https://arxiv.org/abs/2009.06980 + * + * z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * min 0.5 z' H z + z' h + * s.t. G z = g + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + */ +struct PipgBounds { + PipgBounds(scalar_t muArg, scalar_t lambdaArg, scalar_t sigmaArg) : mu(muArg), lambda(lambdaArg), sigma(sigmaArg) {} + + scalar_t dualStepSize(size_t iteration) const { return (static_cast<scalar_t>(iteration) + 1.0) * mu / (2.0 * sigma); } + + scalar_t primalStepSize(size_t iteration) const { return 2.0 / ((static_cast<scalar_t>(iteration) + 1.0) * mu + 2.0 * lambda); } + + scalar_t mu = 1e-5; // mu I <= H + scalar_t lambda = 1.0; // H <= lambda I + scalar_t sigma = 1.0; // G' G <= sigma I +}; + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index a5443870f..f8942af63 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -38,6 +38,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/thread_support/ThreadPool.h> #include <ocs2_oc/oc_problem/OcpSize.h> +#include "ocs2_slp/pipg/PipgBounds.h" #include "ocs2_slp/pipg/PipgSettings.h" #include "ocs2_slp/pipg/PipgSolverStatus.h" @@ -67,15 +68,16 @@ class PipgSolver { * @param[in] scalingVectors : Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. * @param[in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. - * @param[in] mu : the lower bound of the cost hessian H. - * @param[in] lambda : the upper bound of the cost hessian H. - * @param[in] sigma : the upper bound of \f$ G^TG \f$. + * @param[in] pipgBounds : The PipgBounds used to define the primal and dual stepsizes. + * @param[out] xTrajectory : The optimized state trajectory. + * @param[out] uTrajectory : The optimized input trajectory. * @return The solver status. */ pipg::SolverStatus solve(ThreadPool& threadPool, const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t& scalingVectors, - const vector_array_t* EInv, const scalar_t mu, const scalar_t lambda, const scalar_t sigma); + const vector_array_t* EInv, const pipg::PipgBounds& pipgBounds, vector_array_t& xTrajectory, + vector_array_t& uTrajectory); void resize(const OcpSize& size); @@ -85,11 +87,6 @@ class PipgSolver { int getNumGeneralEqualityConstraints() const; - void getStateInputTrajectoriesSolution(vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { - xTrajectory = X_; - uTrajectory = U_; - } - void descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const; const pipg::Settings& settings() const { return settings_; } diff --git a/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h index 2ff9dbfcf..dd66e378d 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h +++ b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h @@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/misc/Benchmark.h> #include <ocs2_oc/oc_problem/OcpSize.h> +#include "ocs2_slp/pipg/PipgBounds.h" #include "ocs2_slp/pipg/PipgSettings.h" #include "ocs2_slp/pipg/PipgSolverStatus.h" @@ -42,16 +43,51 @@ namespace pipg { * First order primal-dual method for solving optimal control problem based on: * "Proportional-Integral Projected Gradient Method for Model Predictive Control" * https://arxiv.org/abs/2009.06980 + * + * z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * min 0.5 z' H z + z' h + * s.t. G z = g + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * For constructing H, h, G, and g, refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * @param[in] settings : The PIPG settings. + * @param[in] H : The hessian matrix of the total cost. + * @param[in] h : The jacobian vector of the total cost. + * @param[in] G : The jacobian matrix of the constarinst. + * @param[in] g : The constraints vector. + * @param[in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. + * @param[in] pipgBounds : The PipgBounds used to define the primal and dual stepsizes. + * @param[out] stackedSolution : The concatenated state-input trajectories, z. + * @return The solver status. */ SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, - const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution); + const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const PipgBounds& pipgBounds, + vector_t& stackedSolution); /** * Deserializes the stacked solution to state-input trajecotries. * * @param[in] ocpSize : Optimal control problem sizes. - * @param[in] stackedSolution : Defined as [u[0], x[1], ..., u[N-1], x[N]]. + * @param[in] stackedSolution : Defined as [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. * @param[in] x0 : The initial state. * @param[out] xTrajectory : State tarjectory. * @param[out] uTrajectory : Input trajecotry. @@ -62,9 +98,9 @@ void unpackSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, con /** * Serializes the state-input trajecotries. * - * @param[in] xTrajectory : State tarjectory - * @param[in] UTrajectory : Input trajecotry - * @param[out] stackedSolution : [u[0], x[1], ..., u[N-1], x[N]] + * @param[in] xTrajectory : State tarjectory. + * @param[in] UTrajectory : Input trajecotry. + * @param[out] stackedSolution : [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. */ void packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution); diff --git a/ocs2_slp/src/Helpers.cpp b/ocs2_slp/src/Helpers.cpp index e8b1eebef..8b0173e49 100644 --- a/ocs2_slp/src/Helpers.cpp +++ b/ocs2_slp/src/Helpers.cpp @@ -50,6 +50,19 @@ int getNumGeneralEqualityConstraints(const ocs2::OcpSize& ocpSize) { namespace ocs2 { namespace slp { +scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost) { + const vector_t rowwiseAbsSumH = hessianAbsRowSum(ocpSize, cost); + return rowwiseAbsSumH.maxCoeff(); +} + +scalar_t GGTEigenvaluesUpperBound(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector<VectorFunctionLinearApproximation>& dynamics, + const std::vector<VectorFunctionLinearApproximation>* constraintsPtr, + const vector_array_t* scalingVectorsPtr) { + const vector_t rowwiseAbsSumGGT = GGTAbsRowSumInParallel(threadPool, ocpSize, dynamics, constraintsPtr, scalingVectorsPtr); + return rowwiseAbsSumGGT.maxCoeff(); +} + vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost) { const int N = ocpSize.numStages; const int nu_0 = ocpSize.numInputs[0]; diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 51f90eeac..5dc32bb3f 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -188,7 +188,7 @@ void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u); linearQuadraticApproximationTimer_.endTimer(); - // Solve QP + // Solve LP solveQpTimer_.startTimer(); const vector_t delta_x0 = initState - x[0]; const auto deltaSolution = getOCPSolution(delta_x0); @@ -267,9 +267,9 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta pipgSolverTimer_.startTimer(); vector_array_t EInv(E.size()); std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); + const pipg::PipgBounds pipgBounds{muEstimated, lambdaScaled, sigmaScaled}; const auto pipgStatus = - pipgSolver_.solve(threadPool_, delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, muEstimated, lambdaScaled, sigmaScaled); - pipgSolver_.getStateInputTrajectoriesSolution(deltaXSol, deltaUSol); + pipgSolver_.solve(threadPool_, delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, pipgBounds, deltaXSol, deltaUSol); pipgSolverTimer_.endTimer(); // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] diff --git a/ocs2_slp/src/pipg/PipgSolver.cpp b/ocs2_slp/src/pipg/PipgSolver.cpp index 745a59afc..199646f61 100644 --- a/ocs2_slp/src/pipg/PipgSolver.cpp +++ b/ocs2_slp/src/pipg/PipgSolver.cpp @@ -49,8 +49,8 @@ PipgSolver::PipgSolver(pipg::Settings settings) : settings_(std::move(settings)) pipg::SolverStatus PipgSolver::solve(ThreadPool& threadPool, const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const std::vector<VectorFunctionLinearApproximation>* constraints, - const vector_array_t& scalingVectors, const vector_array_t* EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma) { + const vector_array_t& scalingVectors, const vector_array_t* EInv, const pipg::PipgBounds& pipgBounds, + vector_array_t& xTrajectory, vector_array_t& uTrajectory) { verifySizes(dynamics, cost, constraints); const int N = ocpSize_.numStages; if (N < 1) { @@ -83,8 +83,8 @@ pipg::SolverStatus PipgSolver::solve(ThreadPool& threadPool, const vector_t& x0, WNew_[t].setZero(dynamics[t].dfdx.rows()); } - scalar_t alpha = 2.0 / (mu + 2.0 * lambda); - scalar_t beta = mu / (2.0 * sigma); + scalar_t alpha = pipgBounds.primalStepSize(0); + scalar_t beta = pipgBounds.primalStepSize(0); scalar_t betaLast = 0; size_t k = 0; @@ -196,8 +196,8 @@ pipg::SolverStatus PipgSolver::solve(ThreadPool& threadPool, const vector_t& x0, } else { betaLast = beta; // Adaptive step size - alpha = 2.0 / ((static_cast<scalar_t>(k) + 1.0) * mu + 2.0 * lambda); - beta = (static_cast<scalar_t>(k) + 1.0) * mu / (2.0 * sigma); + beta = pipgBounds.dualStepSize(k); + alpha = pipgBounds.primalStepSize(k); if (k != 0 && k % settings().checkTerminationInterval == 0) { constraintsViolationInfNorm = @@ -230,12 +230,16 @@ pipg::SolverStatus PipgSolver::solve(ThreadPool& threadPool, const vector_t& x0, }; threadPool.runParallel(std::move(updateVariablesTask), threadPool.numThreads() + 1U); - pipg::SolverStatus status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; + xTrajectory = X_; + uTrajectory = U_; + const auto status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; + if (settings().displayShortSummary) { scalar_t totalTasks = std::accumulate(threadsWorkloadCounter.cbegin(), threadsWorkloadCounter.cend(), 0.0); - std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; - std::cerr << "\n++++++++++++++ PIPG-Parallel " << pipg::toString(status) << " +++++++++++++"; - std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n++++++++++++++ PIPG +++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++++++\n"; + std::cerr << "Solver status: " << pipg::toString(status) << "\n"; std::cerr << "Number of Iterations: " << k << " out of " << settings().maxNumIterations << "\n"; std::cerr << "Norm of delta primal solution: " << std::sqrt(solutionSSE) << "\n"; std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; diff --git a/ocs2_slp/src/pipg/SingleThreadPipg.cpp b/ocs2_slp/src/pipg/SingleThreadPipg.cpp index 47c573793..cbdd2625c 100644 --- a/ocs2_slp/src/pipg/SingleThreadPipg.cpp +++ b/ocs2_slp/src/pipg/SingleThreadPipg.cpp @@ -36,8 +36,8 @@ namespace ocs2 { namespace pipg { SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, - const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const scalar_t mu, - const scalar_t lambda, const scalar_t sigma, vector_t& stackedSolution) { + const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const PipgBounds& pipgBounds, + vector_t& stackedSolution) { // Cold start vector_t z = vector_t::Zero(H.cols()); vector_t z_old = vector_t::Zero(H.cols()); @@ -51,8 +51,8 @@ SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::Spars bool isConverged = false; scalar_t constraintsViolationInfNorm; while (k < settings.maxNumIterations && !isConverged) { - const scalar_t alpha = 2.0 / ((k + 1.0) * mu + 2.0 * lambda); - const scalar_t beta = (k + 1) * mu / (2.0 * sigma); + const auto beta = pipgBounds.dualStepSize(k); + const auto alpha = pipgBounds.primalStepSize(k); z_old.swap(z); @@ -95,9 +95,9 @@ SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::Spars pipg::SolverStatus status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; if (settings.displayShortSummary) { - std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++"; - std::cerr << "\n++++++++++++++ Dense PIPG ++++++++++++++++"; - std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++\n"; + std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n++++++++++++++ PIPG +++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++++++\n"; std::cerr << "Solver status: " << pipg::toString(status) << "\n"; std::cerr << "Number of Iterations: " << k << " out of " << settings.maxNumIterations << "\n"; std::cerr << "Norm of delta primal solution: " << (stackedSolution - z_old).norm() << "\n"; diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp index 3ec955b54..8e39a8123 100644 --- a/ocs2_slp/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -103,21 +103,21 @@ TEST_F(PIPGSolverTest, correctness) { Eigen::JacobiSVD<ocs2::matrix_t> svd(costApproximation.dfdxx); ocs2::vector_t s = svd.singularValues(); - ocs2::scalar_t lambda = s(0); - ocs2::scalar_t mu = s(svd.rank() - 1); + const ocs2::scalar_t lambda = s(0); + const ocs2::scalar_t mu = s(svd.rank() - 1); Eigen::JacobiSVD<ocs2::matrix_t> svdGTG(constraintsApproximation.dfdx.transpose() * constraintsApproximation.dfdx); - ocs2::scalar_t sigma = svdGTG.singularValues()(0); + const ocs2::scalar_t sigma = svdGTG.singularValues()(0); + const ocs2::pipg::PipgBounds pipgBounds{mu, lambda, sigma}; ocs2::vector_t primalSolutionPIPG; - std::ignore = ocs2::pipg::singleThreadPipg( - solver.settings(), costApproximation.dfdxx.sparseView(), costApproximation.dfdx, constraintsApproximation.dfdx.sparseView(), - constraintsApproximation.f, ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), mu, lambda, sigma, primalSolutionPIPG); + std::ignore = ocs2::pipg::singleThreadPipg(solver.settings(), costApproximation.dfdxx.sparseView(), costApproximation.dfdx, + constraintsApproximation.dfdx.sparseView(), constraintsApproximation.f, + ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), pipgBounds, primalSolutionPIPG); ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); - solver.solve(threadPool, x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, mu, lambda, sigma); - ocs2::vector_array_t X, U; - solver.getStateInputTrajectoriesSolution(X, U); + std::ignore = osolver.solve(threadPool, x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, pipgBounds, X, U); + ocs2::vector_t primalSolutionPIPGParallel; ocs2::pipg::packSolution(X, U, primalSolutionPIPGParallel); From 8fffd2c71d75f3d59234fac70f55bfe17675812d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 18:04:50 +0100 Subject: [PATCH 408/512] style --- ocs2_slp/CMakeLists.txt | 2 +- ocs2_slp/include/ocs2_slp/Helpers.h | 48 +++++++++---------- ocs2_slp/include/ocs2_slp/SlpMpc.h | 6 +-- ocs2_slp/include/ocs2_slp/SlpSolver.h | 2 +- ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h | 22 ++++----- .../include/ocs2_slp/pipg/SingleThreadPipg.h | 34 ++++++------- ocs2_slp/test/testPipgSolver.cpp | 2 +- 7 files changed, 58 insertions(+), 58 deletions(-) diff --git a/ocs2_slp/CMakeLists.txt b/ocs2_slp/CMakeLists.txt index 7abddb19a..3ac73e7de 100644 --- a/ocs2_slp/CMakeLists.txt +++ b/ocs2_slp/CMakeLists.txt @@ -47,7 +47,7 @@ include_directories( add_library(${PROJECT_NAME} src/pipg/PipgSettings.cpp src/pipg/PipgSolver.cpp - src/pipg/DensePipgSolver.cpp + src/pipg/SingleThreadPipg.cpp src/Helpers.cpp src/SlpSettings.cpp src/SlpSolver.cpp diff --git a/ocs2_slp/include/ocs2_slp/Helpers.h b/ocs2_slp/include/ocs2_slp/Helpers.h index 08947e053..338ab2fd2 100644 --- a/ocs2_slp/include/ocs2_slp/Helpers.h +++ b/ocs2_slp/include/ocs2_slp/Helpers.h @@ -46,9 +46,9 @@ namespace slp { * * Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". * - * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. * - * totalCost = 0.5 Z' H Z + Z' h + h0 + * totalCost = 0.5 z' H z + z' h + h0 * * H = [ R0 * * Q1 P1' @@ -59,8 +59,8 @@ namespace slp { * * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] * - * @param[in] ocpSize: The size of optimal control problem. - * @param[in] cost: Quadratic approximation of the cost over the time horizon. + * @param [in] ocpSize: The size of optimal control problem. + * @param [in] cost: Quadratic approximation of the cost over the time horizon. * @return: The upper bound of eigenvalues for H. */ scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost); @@ -75,9 +75,9 @@ scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector< * * Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". * - * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. * - * G Z = g + * G z = g * * G = [-B0 I * * -A1 -B1 I @@ -90,12 +90,12 @@ scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector< * * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] * - * @param[in] threadPool: The thread pool. - * @param[in] ocpSize: The size of optimal control problem. - * @param[in] dynamics: Linear approximation of the dynamics over the time horizon. - * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. - * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, - * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param [in] threadPool: The thread pool. + * @param [in] ocpSize: The size of optimal control problem. + * @param [in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param [in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param [in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. * @return: The upper bound of eigenvalues for G G'. */ scalar_t GGTEigenvaluesUpperBound(ThreadPool& threadPool, const OcpSize& ocpSize, @@ -106,7 +106,7 @@ scalar_t GGTEigenvaluesUpperBound(ThreadPool& threadPool, const OcpSize& ocpSize /** * Computes the row-wise absolute sum of the cost hessian matrix, H. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". * - * totalCost = 0.5 Z' H Z + Z' h + h0 + * totalCost = 0.5 z' H z + z' h + h0 * * H = [ R0 * * Q1 P1' @@ -117,8 +117,8 @@ scalar_t GGTEigenvaluesUpperBound(ThreadPool& threadPool, const OcpSize& ocpSize * * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] * - * @param[in] ocpSize: The size of optimal control problem. - * @param[in] cost: Quadratic approximation of the cost over the time horizon. + * @param [in] ocpSize: The size of optimal control problem. + * @param [in] cost: Quadratic approximation of the cost over the time horizon. * @return: The absolute sum of rows of matrix H. */ vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost); @@ -126,9 +126,9 @@ vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFuncti /** * Computes the row-wise absolute sum of matrix G G' in parallel. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". * - * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. * - * G Z = g + * G z = g * * G = [-B0 I * * -A1 -B1 I @@ -141,13 +141,13 @@ vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFuncti * * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] * - * @param[in] threadPool: The thread pool. - * @param[in] ocpSize: The size of optimal control problem. - * @param[in] dynamics: Linear approximation of the dynamics over the time horizon. - * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. - * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, - * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. - * @return: The absolute sum of rows of matrix G G'. + * @param [in] threadPool: The thread pool. + * @param [in] ocpSize: The size of optimal control problem. + * @param [in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param [in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param [in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @return The absolute sum of rows of matrix G G'. */ vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, const std::vector<VectorFunctionLinearApproximation>& dynamics, diff --git a/ocs2_slp/include/ocs2_slp/SlpMpc.h b/ocs2_slp/include/ocs2_slp/SlpMpc.h index d9c747893..6f74a38f6 100644 --- a/ocs2_slp/include/ocs2_slp/SlpMpc.h +++ b/ocs2_slp/include/ocs2_slp/SlpMpc.h @@ -39,9 +39,9 @@ class SlpMpc final : public MPC_BASE { /** * Constructor * - * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use multiple shooting - * settings directly. - * @param settings : settings for the multiple shooting SLP solver. + * @param [in] mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use + * slp::Settings directly. + * @param [in] settings : settings for the multiple shooting SLP solver. * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ diff --git a/ocs2_slp/include/ocs2_slp/SlpSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h index 0f54cb004..f6bea863d 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSolver.h +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -50,7 +50,7 @@ class SlpSolver : public SolverBase { /** * Constructor * - * @param settings : settings for the multiple shooting SLP solver. + * @param [in] settings : settings for the multiple shooting SLP solver. * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index f8942af63..5d93579c4 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -60,17 +60,17 @@ class PipgSolver { /** * Solve the optimal control in parallel. * - * @param[in] threadPool : The external thread pool. - * @param[in] x0 : Initial state - * @param[in] dynamics : Dynamics array. - * @param[in] cost : Cost array. - * @param[in] constraints : Constraints array. Pass nullptr for an unconstrained problem. - * @param[in] scalingVectors : Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, - * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. - * @param[in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. - * @param[in] pipgBounds : The PipgBounds used to define the primal and dual stepsizes. - * @param[out] xTrajectory : The optimized state trajectory. - * @param[out] uTrajectory : The optimized input trajectory. + * @param [in] threadPool : The external thread pool. + * @param [in] x0 : Initial state + * @param [in] dynamics : Dynamics array. + * @param [in] cost : Cost array. + * @param [in] constraints : Constraints array. Pass nullptr for an unconstrained problem. + * @param [in] scalingVectors : Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param [in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. + * @param [in] pipgBounds : The PipgBounds used to define the primal and dual stepsizes. + * @param [out] xTrajectory : The optimized state trajectory. + * @param [out] uTrajectory : The optimized input trajectory. * @return The solver status. */ pipg::SolverStatus solve(ThreadPool& threadPool, const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics, diff --git a/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h index dd66e378d..2fa230309 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h +++ b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h @@ -39,7 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace pipg { -/* +/** * First order primal-dual method for solving optimal control problem based on: * "Proportional-Integral Projected Gradient Method for Model Predictive Control" * https://arxiv.org/abs/2009.06980 @@ -69,14 +69,14 @@ namespace pipg { * * For constructing H, h, G, and g, refer to "ocs2_oc/oc_problem/OcpToKkt.h". * - * @param[in] settings : The PIPG settings. - * @param[in] H : The hessian matrix of the total cost. - * @param[in] h : The jacobian vector of the total cost. - * @param[in] G : The jacobian matrix of the constarinst. - * @param[in] g : The constraints vector. - * @param[in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. - * @param[in] pipgBounds : The PipgBounds used to define the primal and dual stepsizes. - * @param[out] stackedSolution : The concatenated state-input trajectories, z. + * @param [in] settings : The PIPG settings. + * @param [in] H : The hessian matrix of the total cost. + * @param [in] h : The jacobian vector of the total cost. + * @param [in] G : The jacobian matrix of the constarinst. + * @param [in] g : The constraints vector. + * @param [in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. + * @param [in] pipgBounds : The PipgBounds used to define the primal and dual stepsizes. + * @param [out] stackedSolution : The concatenated state-input trajectories, z. * @return The solver status. */ SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::SparseMatrix<scalar_t>& H, const vector_t& h, @@ -86,11 +86,11 @@ SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::Spars /** * Deserializes the stacked solution to state-input trajecotries. * - * @param[in] ocpSize : Optimal control problem sizes. - * @param[in] stackedSolution : Defined as [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. - * @param[in] x0 : The initial state. - * @param[out] xTrajectory : State tarjectory. - * @param[out] uTrajectory : Input trajecotry. + * @param [in] ocpSize : Optimal control problem sizes. + * @param [in] stackedSolution : Defined as [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * @param [in] x0 : The initial state. + * @param [out] xTrajectory : State tarjectory. + * @param [out] uTrajectory : Input trajecotry. */ void unpackSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, vector_array_t& uTrajectory); @@ -98,9 +98,9 @@ void unpackSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, con /** * Serializes the state-input trajecotries. * - * @param[in] xTrajectory : State tarjectory. - * @param[in] UTrajectory : Input trajecotry. - * @param[out] stackedSolution : [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * @param [in] xTrajectory : State tarjectory. + * @param [in] UTrajectory : Input trajecotry. + * @param [out] stackedSolution : [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. */ void packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution); diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp index 8e39a8123..f0430b425 100644 --- a/ocs2_slp/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -116,7 +116,7 @@ TEST_F(PIPGSolverTest, correctness) { ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); ocs2::vector_array_t X, U; - std::ignore = osolver.solve(threadPool, x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, pipgBounds, X, U); + std::ignore = solver.solve(threadPool, x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, pipgBounds, X, U); ocs2::vector_t primalSolutionPIPGParallel; ocs2::pipg::packSolution(X, U, primalSolutionPIPGParallel); From 299e10048b373a8c8653a3bcc2bb79d04e7edf78 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 20:26:41 +0100 Subject: [PATCH 409/512] doc improve --- .../include/ocs2_oc/pre_condition/Scaling.h | 60 ++++++++++++++----- 1 file changed, 45 insertions(+), 15 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h index 637ab9866..490850d5f 100644 --- a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h +++ b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h @@ -39,24 +39,54 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { /** - * Calculates the scaling factor D, E and c, and scale the input dynamics, cost data in place in parallel. + * Calculates the scaling factors D, E, and c, and scale the input dynamics, and cost data in place in parallel. * - * There are a few pre-conditioning methods aiming to shape different aspect of the problem. To balance the performance and + * There are a few pre-conditioning methods aiming to shape different aspects of the problem. To balance the performance and * computational effort, we choose a modified Ruzi equilibration Algorithm. Interested readers can find the original Ruiz - * equilibration in: "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices" + * equilibration in: "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices". * - * @param[in] : threadPool : The external thread pool. - * @param[in] x0 : Initial state - * @param[in] ocpSize : The size of the oc problem. - * @param[in] iteration : Number of iteration. - * @param[in, out] : dynamics The dynamics array of all time points. - * @param[in, out] : cost The cost array of all time points. - * @param[out] : DOut Scaling factor D - * @param[out] : EOut Scaling factor E - * @param[out] scalingVectors : Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. - * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components - * of this type of matrix for every timestamp. - * @param[out] cOut : Scaling factor c + * This pre-conditioning transforms the following minimization with z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}] + * + * min_{z} 1/2 z' H y + y' h + * s.t. G z = g + * + * to the follwoing one with y := inv(D) z + * + * min_{y} c/2 y' (D H D) y + c y' (D h) + * s.t. (E G D) y = E g + * + * The KKT matrices H, h, G, and g are defined as + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * @param [in] threadPool : The external thread pool. + * @param [in] x0 : The initial state. + * @param [in] ocpSize : The size of the oc problem. + * @param [in] iteration : Number of iterations. + * @param [in, out] dynamics : The dynamics array of all time points. + * @param [in, out] cost : The cost array of all time points. + * @param [out] DOut : The matrix D decomposed for each time step. + * @param [out] EOut : The matrix E decomposed for each time step. + * @param [out] scalingVectors : Vector representation for the identity parts of the dynamics constraints inside the constraint matrix. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components + * of this type of matrix for every timestamp. + * @param [out] cOut : Scaling factor c. */ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, std::vector<VectorFunctionLinearApproximation>& dynamics, From db6d9d0aefe293fe18be307e36c9a813c3924912 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 20:27:24 +0100 Subject: [PATCH 410/512] improving pre_condition/Scaling implementation --- ocs2_oc/src/pre_condition/Scaling.cpp | 103 ++++++++++++++------------ 1 file changed, 55 insertions(+), 48 deletions(-) diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/pre_condition/Scaling.cpp index a4885fbf2..7698ab767 100644 --- a/ocs2_oc/src/pre_condition/Scaling.cpp +++ b/ocs2_oc/src/pre_condition/Scaling.cpp @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_oc/pre_condition/Scaling.h" #include <atomic> +#include <functional> #include <numeric> namespace ocs2 { @@ -42,7 +43,7 @@ vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat) { if (mat.rows() == 0 || mat.cols() == 0) { return vector_t(0); } else { - return mat.array().abs().matrix().rowwise().maxCoeff(); + return mat.rowwise().template lpNorm<Eigen::Infinity>(); } } @@ -52,9 +53,9 @@ vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixB if (mat.rows() == 0 || mat.cols() == 0) { return temp; } else if (temp.rows() != 0) { - return mat.array().abs().matrix().rowwise().maxCoeff().cwiseMax(temp); + return mat.rowwise().template lpNorm<Eigen::Infinity>().cwiseMax(temp); } else { - return mat.array().abs().matrix().rowwise().maxCoeff(); + return mat.rowwise().template lpNorm<Eigen::Infinity>(); } } @@ -63,7 +64,7 @@ vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat) { if (mat.rows() == 0 || mat.cols() == 0) { return vector_t(0); } else { - return mat.array().abs().matrix().colwise().maxCoeff().transpose(); + return mat.colwise().template lpNorm<Eigen::Infinity>().transpose(); } } @@ -73,9 +74,9 @@ vector_t matrixInfNormCols(const Eigen::MatrixBase<T>& mat, const Eigen::MatrixB if (mat.rows() == 0 || mat.cols() == 0) { return temp; } else if (temp.rows() != 0) { - return mat.array().abs().matrix().colwise().maxCoeff().transpose().cwiseMax(temp); + return mat.colwise().template lpNorm<Eigen::Infinity>().transpose().cwiseMax(temp); } else { - return mat.array().abs().matrix().colwise().maxCoeff().transpose(); + return mat.colwise().template lpNorm<Eigen::Infinity>().transpose(); } } @@ -89,33 +90,38 @@ void scaleMatrixInPlace(const vector_t* rowScale, const vector_t* colScale, Eige } } +scalar_t limitScaling(const scalar_t& v) { + if (v < 1e-4) { + return 1.0; + } else if (v > 1e+4) { + return 1e+4; + } else { + return v; + } +} + void invSqrtInfNormInParallel(ThreadPool& threadPool, const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& scalingVectors, - const int N, const std::function<void(vector_t&)>& limitScaling, vector_array_t& D, vector_array_t& E) { + const int N, vector_array_t& D, vector_array_t& E) { // Helper function - auto procedure = [&limitScaling](vector_t& dst, const vector_t& norm) { - dst = norm; - limitScaling(dst); - dst.array() = dst.array().sqrt().inverse(); - }; + auto invSqrt = [](const vector_t& v) -> vector_t { return v.unaryExpr(std::ref(limitScaling)).array().sqrt().inverse(); }; - procedure(D[0], matrixInfNormCols(cost[0].dfduu, dynamics[0].dfdu)); - procedure(E[0], matrixInfNormRows(dynamics[0].dfdu, scalingVectors[0].asDiagonal().toDenseMatrix())); + D[0] = invSqrt(matrixInfNormCols(cost[0].dfduu, dynamics[0].dfdu)); + E[0] = invSqrt(matrixInfNormRows(dynamics[0].dfdu, scalingVectors[0].asDiagonal().toDenseMatrix())); std::atomic_int timeStamp{1}; auto task = [&](int workerId) { int k; while ((k = timeStamp++) < N) { - procedure(D[2 * k - 1], - matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux, scalingVectors[k - 1].asDiagonal().toDenseMatrix(), dynamics[k].dfdx)); - procedure(D[2 * k], matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu, dynamics[k].dfdu)); - procedure(E[k], matrixInfNormRows(dynamics[k].dfdx, dynamics[k].dfdu, scalingVectors[k].asDiagonal().toDenseMatrix())); + D[2 * k - 1] = + invSqrt(matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux, scalingVectors[k - 1].asDiagonal().toDenseMatrix(), dynamics[k].dfdx)); + D[2 * k] = invSqrt(matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu, dynamics[k].dfdu)); + E[k] = invSqrt(matrixInfNormRows(dynamics[k].dfdx, dynamics[k].dfdu, scalingVectors[k].asDiagonal().toDenseMatrix())); } }; - threadPool.runParallel(std::move(task), threadPool.numThreads() + 1U); - procedure(D[2 * N - 1], matrixInfNormCols(cost[N].dfdxx, scalingVectors[N - 1].asDiagonal().toDenseMatrix())); + D[2 * N - 1] = invSqrt(matrixInfNormCols(cost[N].dfdxx, scalingVectors[N - 1].asDiagonal().toDenseMatrix())); } void scaleDataOneStepInPlaceInParallel(ThreadPool& threadPool, const vector_array_t& D, const vector_array_t& E, const int N, @@ -180,13 +186,6 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 throw std::runtime_error("[preConditioningInPlaceInParallel] The number of stages cannot be less than 1."); } - auto limitScaling = [](vector_t& vec) { - for (int i = 0; i < vec.size(); i++) { - if (vec(i) > 1e+4) vec(i) = 1e+4; - if (vec(i) < 1e-4) vec(i) = 1.0; - } - }; - // Init output cOut = 1.0; DOut.resize(2 * N); @@ -199,9 +198,12 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 scalingVectors[i].setOnes(ocpSize.numStates[i + 1]); } + const auto numDecisionVariables = std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), 0) + + std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), 0); + vector_array_t D(2 * N), E(N); for (int i = 0; i < iteration; i++) { - invSqrtInfNormInParallel(threadPool, dynamics, cost, scalingVectors, N, limitScaling, D, E); + invSqrtInfNormInParallel(threadPool, dynamics, cost, scalingVectors, N, D, E); scaleDataOneStepInPlaceInParallel(threadPool, D, E, N, dynamics, cost, scalingVectors); for (int k = 0; k < DOut.size(); k++) { @@ -211,28 +213,33 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 EOut[k].array() *= E[k].array(); } - scalar_t infNormOfh = (cost.front().dfdu + cost.front().dfdux * x0).lpNorm<Eigen::Infinity>(); - for (int k = 1; k < N; k++) { - infNormOfh = std::max(infNormOfh, std::max(cost[k].dfdx.lpNorm<Eigen::Infinity>(), cost[k].dfdu.lpNorm<Eigen::Infinity>())); - } - infNormOfh = std::max(infNormOfh, cost[N].dfdx.lpNorm<Eigen::Infinity>()); - if (infNormOfh < 1e-4) infNormOfh = 1.0; - - scalar_t sumOfInfNormOfH = matrixInfNormCols(cost[0].dfduu).derived().sum(); - for (int k = 1; k < N; k++) { - sumOfInfNormOfH += matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux).derived().sum(); - sumOfInfNormOfH += matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu).derived().sum(); - } - sumOfInfNormOfH += matrixInfNormCols(cost[N].dfdxx).derived().sum(); - - const int numDecisionVariables = std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), (int)0) + - std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0); + const auto infNormOfh = [&cost, &x0]() { + scalar_t out = (cost.front().dfdu + cost.front().dfdux * x0).lpNorm<Eigen::Infinity>(); + for (int k = 1; k < cost.size(); ++k) { + out = std::max(out, cost[k].dfdx.lpNorm<Eigen::Infinity>()); + if (cost[k].dfdu.size() > 0) { + out = std::max(out, cost[k].dfdu.lpNorm<Eigen::Infinity>()); + } + } + return out; + }(); + + const auto sumOfInfNormOfH = [&cost]() { + scalar_t out = matrixInfNormCols(cost[0].dfduu).derived().sum(); + for (int k = 1; k < cost.size(); k++) { + if (cost[k].dfduu.size() == 0) { + out += matrixInfNormCols(cost[k].dfdxx).derived().sum(); + } else { + out += matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux).derived().sum(); + out += matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu).derived().sum(); + } + } + return out; + }(); - scalar_t c_temp = std::max(sumOfInfNormOfH / static_cast<scalar_t>(numDecisionVariables), infNormOfh); - if (c_temp > 1e+4) c_temp = 1e+4; - if (c_temp < 1e-4) c_temp = 1.0; + const auto averageOfInfNormOfH = sumOfInfNormOfH / static_cast<scalar_t>(numDecisionVariables); - const scalar_t gamma = 1.0 / c_temp; + const auto gamma = 1.0 / limitScaling(std::max(averageOfInfNormOfH, infNormOfh)); for (int k = 0; k <= N; ++k) { cost[k].dfdxx *= gamma; From fd3343e43e70523dd5b8b5c1ca5d79912a52ca52 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 6 Dec 2022 20:56:38 +0100 Subject: [PATCH 411/512] removing SparseScaling --- ocs2_oc/CMakeLists.txt | 1 - .../include/ocs2_oc/pre_condition/Scaling.h | 68 ++++++ ocs2_oc/src/pre_condition/Scaling.cpp | 199 ++++++++++++++- .../test/include/ocs2_oc/test/SparseScaling.h | 76 ------ ocs2_oc/test/pre_condition/SparseScaling.cpp | 228 ------------------ ocs2_oc/test/pre_condition/testScaling.cpp | 10 +- ocs2_slp/include/ocs2_slp/Helpers.h | 4 +- ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h | 2 +- 8 files changed, 264 insertions(+), 324 deletions(-) delete mode 100644 ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h delete mode 100644 ocs2_oc/test/pre_condition/SparseScaling.cpp diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index fb94c3894..96b8e5a51 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -201,7 +201,6 @@ target_link_libraries(test_matrix_construction ) catkin_add_gtest(test_scaling - test/pre_condition/SparseScaling.cpp test/pre_condition/testScaling.cpp ) target_link_libraries(test_scaling diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h index 490850d5f..295898fad 100644 --- a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h +++ b/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h @@ -93,4 +93,72 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, vector_array_t& scalingVectors, scalar_t& cOut); +/** + * Calculates the scaling factors D, E, and c, and scale the input dynamics, and cost data in place in parallel. + * + * There are a few pre-conditioning methods aiming to shape different aspects of the problem. To balance the performance and + * computational effort, we choose a modified Ruzi equilibration Algorithm. Interested readers can find the original Ruiz + * equilibration in: "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices". + * + * This pre-conditioning transforms the following minimization with z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}] + * + * min_{z} 1/2 z' H y + y' h + * s.t. G z = g + * + * to the follwoing one with y := inv(D) z + * + * min_{y} c/2 y' (D H D) y + c y' (D h) + * s.t. (E G D) y = E g + * + * The KKT matrices H, h, G, and g are defined as + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * For constructing H, h, G, and g, refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * @param [in] iteration : Number of iterations. + * @param [in, out] H : The hessian matrix of the total cost. + * @param [in, out] h : The jacobian vector of the total cost. + * @param [in, out] G : The jacobian matrix of the constarinst. + * @param [in, out] g : The constraints vector. + * @param [out] DOut : The matrix D decomposed for each time step. + * @param [out] EOut : The matrix E decomposed for each time step. + * @param [out] cOut : Scaling factor c. + */ +void preConditioningSparseMatrixInPlace(int iteration, Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, + vector_t& g, vector_t& DOut, vector_t& EOut, scalar_t& cOut); + +/** + * Scales the dynamics and cost array in place and construct scaling vector array from the given scaling factors E, D and c. + * + * @param[in] ocpSize : The size of the oc problem. + * @param[in] D : Scaling factor D + * @param[in] E : Scaling factor E + * @param[in] c : Scaling factor c + * @param[in, out] dynamics : The dynamics array of all time points. + * @param[in, out] cost : The cost array of all time points. + * @param[out] scalingVectors : Vector representation for the identity parts of the dynamics inside the constraint matrix. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal + * components of this type of matrix for every timestamp. + */ +void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, + std::vector<vector_t>& scalingVectors); + } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/pre_condition/Scaling.cpp index 7698ab767..2b6aacc05 100644 --- a/ocs2_oc/src/pre_condition/Scaling.cpp +++ b/ocs2_oc/src/pre_condition/Scaling.cpp @@ -38,6 +38,16 @@ namespace ocs2 { // Internal helper functions namespace { +scalar_t limitScaling(const scalar_t& v) { + if (v < 1e-4) { + return 1.0; + } else if (v > 1e+4) { + return 1e+4; + } else { + return v; + } +} + template <typename T> vector_t matrixInfNormRows(const Eigen::MatrixBase<T>& mat) { if (mat.rows() == 0 || mat.cols() == 0) { @@ -90,16 +100,6 @@ void scaleMatrixInPlace(const vector_t* rowScale, const vector_t* colScale, Eige } } -scalar_t limitScaling(const scalar_t& v) { - if (v < 1e-4) { - return 1.0; - } else if (v > 1e+4) { - return 1e+4; - } else { - return v; - } -} - void invSqrtInfNormInParallel(ThreadPool& threadPool, const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& scalingVectors, const int N, vector_array_t& D, vector_array_t& E) { @@ -175,6 +175,41 @@ void scaleDataOneStepInPlaceInParallel(ThreadPool& threadPool, const vector_arra }; threadPool.runParallel(std::move(scaleConstraints), threadPool.numThreads() + 1U); } + +vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat) { + vector_t infNorm; + infNorm.setZero(mat.rows()); + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + int i = it.row(); + infNorm(i) = std::max(infNorm(i), std::abs(it.value())); + } + } + return infNorm; +} + +vector_t matrixInfNormCols(const Eigen::SparseMatrix<scalar_t>& mat) { + vector_t infNorm; + infNorm.setZero(mat.cols()); + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + infNorm(j) = std::max(infNorm(j), std::abs(it.value())); + } + } + return infNorm; +} + +void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix<scalar_t>& mat) { + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { + if (it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1) { + throw std::runtime_error("[scaleMatrixInPlace] it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1"); + } + it.valueRef() *= rowScale(it.row()) * colScale(j); + } + } +} + } // anonymous namespace void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, @@ -253,4 +288,148 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 } } +void preConditioningSparseMatrixInPlace(int iteration, Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, + vector_t& g, vector_t& DOut, vector_t& EOut, scalar_t& cOut) { + const int nz = H.rows(); + const int nc = G.rows(); + + // Init output + cOut = 1.0; + DOut.setOnes(nz); + EOut.setOnes(nc); + + for (int i = 0; i < iteration; i++) { + vector_t D, E; + const vector_t infNormOfHCols = matrixInfNormCols(H); + const vector_t infNormOfGCols = matrixInfNormCols(G); + + D = infNormOfHCols.array().max(infNormOfGCols.array()); + E = matrixInfNormRows(G); + + for (int i = 0; i < D.size(); i++) { + if (D(i) > 1e+4) D(i) = 1e+4; + if (D(i) < 1e-4) D(i) = 1.0; + } + + for (int i = 0; i < E.size(); i++) { + if (E(i) > 1e+4) E(i) = 1e+4; + if (E(i) < 1e-4) E(i) = 1.0; + } + + D = D.array().sqrt().inverse(); + E = E.array().sqrt().inverse(); + + scaleMatrixInPlace(D, D, H); + scaleMatrixInPlace(E, D, G); + h.array() *= D.array(); + g.array() *= E.array(); + + DOut = DOut.cwiseProduct(D); + EOut = EOut.cwiseProduct(E); + + const scalar_t infNormOfh = h.lpNorm<Eigen::Infinity>(); + const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); + const scalar_t gamma = 1.0 / limitScaling(std::max(infNormOfHColsUpdated.mean(), infNormOfh)); + + H *= gamma; + h *= gamma; + + cOut *= gamma; + } +} + +void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, + std::vector<vector_t>& scalingVectors) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + } + + scalingVectors.resize(N); + + // Scale H & h + const int nu_0 = ocpSize.numInputs.front(); + // Scale row - Pre multiply D + cost.front().dfduu.array().colwise() *= c * D.head(nu_0).array(); + // Scale col - Post multiply D + cost.front().dfduu *= D.head(nu_0).asDiagonal(); + + cost.front().dfdu.array() *= c * D.head(nu_0).array(); + + cost.front().dfdux.array().colwise() *= c * D.head(nu_0).array(); + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + auto& Q = cost[k].dfdxx; + auto& R = cost[k].dfduu; + auto& P = cost[k].dfdux; + auto& q = cost[k].dfdx; + auto& r = cost[k].dfdu; + + Q.array().colwise() *= c * D.segment(currRow, nx_k).array(); + Q *= D.segment(currRow, nx_k).asDiagonal(); + q.array() *= c * D.segment(currRow, nx_k).array(); + + P *= D.segment(currRow, nx_k).asDiagonal(); + currRow += nx_k; + + R.array().colwise() *= c * D.segment(currRow, nu_k).array(); + R *= D.segment(currRow, nu_k).asDiagonal(); + r.array() *= c * D.segment(currRow, nu_k).array(); + + P.array().colwise() *= c * D.segment(currRow, nu_k).array(); + + currRow += nu_k; + } + + const int nx_N = ocpSize.numStates[N]; + cost[N].dfdxx.array().colwise() *= c * D.tail(nx_N).array(); + cost[N].dfdxx *= D.tail(nx_N).asDiagonal(); + cost[N].dfdx.array() *= c * D.tail(nx_N).array(); + + // Scale G & g + auto& B_0 = dynamics[0].dfdu; + auto& C_0 = scalingVectors[0]; + const int nx_1 = ocpSize.numStates[1]; + + // \tilde{B} = E * B * D, + // scaling = E * D, + // \tilde{g} = E * g + B_0.array().colwise() *= E.head(nx_1).array(); + B_0 *= D.head(nu_0).asDiagonal(); + + C_0 = E.head(nx_1).array() * D.segment(nu_0, nx_1).array(); + + dynamics[0].dfdx.array().colwise() *= E.head(nx_1).array(); + dynamics[0].f.array() *= E.head(nx_1).array(); + + currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + const int nx_next = ocpSize.numStates[k + 1]; + auto& A_k = dynamics[k].dfdx; + auto& B_k = dynamics[k].dfdu; + auto& b_k = dynamics[k].f; + auto& C_k = scalingVectors[k]; + + A_k.array().colwise() *= E.segment(currRow, nx_next).array(); + A_k *= D.segment(currCol, nx_k).asDiagonal(); + + B_k.array().colwise() *= E.segment(currRow, nx_next).array(); + B_k *= D.segment(currCol + nx_k, nu_k).asDiagonal(); + + C_k = E.segment(currRow, nx_next).array() * D.segment(currCol + nx_k + nu_k, nx_next).array(); + + b_k.array() *= E.segment(currRow, nx_next).array(); + + currRow += nx_next; + currCol += nx_k + nu_k; + } +} + } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h b/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h deleted file mode 100644 index a0b1f1fbe..000000000 --- a/ocs2_oc/test/include/ocs2_oc/test/SparseScaling.h +++ /dev/null @@ -1,76 +0,0 @@ -/****************************************************************************** -Copyright (c) 2017, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - ******************************************************************************/ - -#pragma once - -#include <Eigen/Sparse> - -#include <ocs2_core/Types.h> - -#include "ocs2_oc/oc_problem/OcpSize.h" - -namespace ocs2 { - -/** - * Calculate the scaling factor D, E and c, and scale the input matrix(vector) H, h and G in place. The scaled matrix(vector) are defined as - * \f[ - * \tilde{H} = cDHD, \tilde{h} = cDh, - * \tilde{G} = EGD, \tilde{g} = Eg - * \f] - * - * - * @param[in, out] H Cost hessian. - * @param[in, out] h Linear cost term. - * @param[in, out] G Linear constraint matrix. - * @param[in] iteration Number of iteration. - * @param[out] DOut Scaling factor D - * @param[out] EOut Scaling factor E - * @param[out] cOut Scaling factor c - */ -void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, - const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut); - -/** - * @brief Scale the dynamics and cost array in place and construct scaling vector array from the given scaling factors E, D and c. - * - * @param[in] ocpSize The size of the oc problem. - * @param[in] D Scaling factor D - * @param[in] E Scaling factor E - * @param[in] c Scaling factor c - * @param[in, out] dynamics Dynamics array - * @param[in, out] cost Cost array - * @param[out] scalingVectors Vector representatoin for the identity parts of the dynamics constraints inside the constraint matrix. - * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components of this type of matrix for every - * timestamp. - */ -void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, - std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, - std::vector<vector_t>& scalingVectors); - -} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/test/pre_condition/SparseScaling.cpp b/ocs2_oc/test/pre_condition/SparseScaling.cpp deleted file mode 100644 index 79962654c..000000000 --- a/ocs2_oc/test/pre_condition/SparseScaling.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#include "ocs2_oc/test/SparseScaling.h" - -#include <atomic> -#include <numeric> - -namespace ocs2 { - -// Internal helper functions -namespace { - -vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat) { - vector_t infNorm; - infNorm.setZero(mat.rows()); - - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - int i = it.row(); - infNorm(i) = std::max(infNorm(i), std::abs(it.value())); - } - } - return infNorm; -} - -vector_t matrixInfNormCols(const Eigen::SparseMatrix<scalar_t>& mat) { - vector_t infNorm; - infNorm.setZero(mat.cols()); - - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - infNorm(j) = std::max(infNorm(j), std::abs(it.value())); - } - } - return infNorm; -} - -void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix<scalar_t>& mat) { - for (int j = 0; j < mat.outerSize(); ++j) { - for (Eigen::SparseMatrix<scalar_t>::InnerIterator it(mat, j); it; ++it) { - if (it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1) { - throw std::runtime_error("[scaleMatrixInPlace] it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1"); - } - it.valueRef() *= rowScale(it.row()) * colScale(j); - } - } -} -} // anonymous namespace - -void preConditioningSparseMatrixInPlace(Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, - const int iteration, vector_t& DOut, vector_t& EOut, scalar_t& cOut) { - const int nz = H.rows(); - const int nc = G.rows(); - - // Init output - cOut = 1.0; - DOut.setOnes(nz); - EOut.setOnes(nc); - - for (int i = 0; i < iteration; i++) { - vector_t D, E; - const vector_t infNormOfHCols = matrixInfNormCols(H); - const vector_t infNormOfGCols = matrixInfNormCols(G); - - D = infNormOfHCols.array().max(infNormOfGCols.array()); - E = matrixInfNormRows(G); - - for (int i = 0; i < D.size(); i++) { - if (D(i) > 1e+4) D(i) = 1e+4; - if (D(i) < 1e-4) D(i) = 1.0; - } - - for (int i = 0; i < E.size(); i++) { - if (E(i) > 1e+4) E(i) = 1e+4; - if (E(i) < 1e-4) E(i) = 1.0; - } - - D = D.array().sqrt().inverse(); - E = E.array().sqrt().inverse(); - - scaleMatrixInPlace(D, D, H); - - scaleMatrixInPlace(E, D, G); - - h.array() *= D.array(); - - DOut = DOut.cwiseProduct(D); - EOut = EOut.cwiseProduct(E); - - scalar_t infNormOfh = h.lpNorm<Eigen::Infinity>(); - if (infNormOfh < 1e-4) infNormOfh = 1.0; - - const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); - scalar_t c_temp = std::max(infNormOfHColsUpdated.mean(), infNormOfh); - if (c_temp > 1e+4) c_temp = 1e+4; - if (c_temp < 1e-4) c_temp = 1.0; - - scalar_t gamma = 1.0 / c_temp; - - H *= gamma; - h *= gamma; - - cOut *= gamma; - } -} - -void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, - std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, - std::vector<vector_t>& scalingVectors) { - const int N = ocpSize.numStages; - if (N < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); - } - - scalingVectors.resize(N); - - // Scale H & h - const int nu_0 = ocpSize.numInputs.front(); - // Scale row - Pre multiply D - cost.front().dfduu.array().colwise() *= c * D.head(nu_0).array(); - // Scale col - Post multiply D - cost.front().dfduu *= D.head(nu_0).asDiagonal(); - - cost.front().dfdu.array() *= c * D.head(nu_0).array(); - - cost.front().dfdux.array().colwise() *= c * D.head(nu_0).array(); - - int currRow = nu_0; - for (int k = 1; k < N; ++k) { - const int nx_k = ocpSize.numStates[k]; - const int nu_k = ocpSize.numInputs[k]; - auto& Q = cost[k].dfdxx; - auto& R = cost[k].dfduu; - auto& P = cost[k].dfdux; - auto& q = cost[k].dfdx; - auto& r = cost[k].dfdu; - - Q.array().colwise() *= c * D.segment(currRow, nx_k).array(); - Q *= D.segment(currRow, nx_k).asDiagonal(); - q.array() *= c * D.segment(currRow, nx_k).array(); - - P *= D.segment(currRow, nx_k).asDiagonal(); - currRow += nx_k; - - R.array().colwise() *= c * D.segment(currRow, nu_k).array(); - R *= D.segment(currRow, nu_k).asDiagonal(); - r.array() *= c * D.segment(currRow, nu_k).array(); - - P.array().colwise() *= c * D.segment(currRow, nu_k).array(); - - currRow += nu_k; - } - - const int nx_N = ocpSize.numStates[N]; - cost[N].dfdxx.array().colwise() *= c * D.tail(nx_N).array(); - cost[N].dfdxx *= D.tail(nx_N).asDiagonal(); - cost[N].dfdx.array() *= c * D.tail(nx_N).array(); - - // Scale G & g - auto& B_0 = dynamics[0].dfdu; - auto& C_0 = scalingVectors[0]; - const int nx_1 = ocpSize.numStates[1]; - - // \tilde{B} = E * B * D, - // scaling = E * D, - // \tilde{g} = E * g - B_0.array().colwise() *= E.head(nx_1).array(); - B_0 *= D.head(nu_0).asDiagonal(); - - C_0 = E.head(nx_1).array() * D.segment(nu_0, nx_1).array(); - - dynamics[0].dfdx.array().colwise() *= E.head(nx_1).array(); - dynamics[0].f.array() *= E.head(nx_1).array(); - - currRow = nx_1; - int currCol = nu_0; - for (int k = 1; k < N; ++k) { - const int nu_k = ocpSize.numInputs[k]; - const int nx_k = ocpSize.numStates[k]; - const int nx_next = ocpSize.numStates[k + 1]; - auto& A_k = dynamics[k].dfdx; - auto& B_k = dynamics[k].dfdu; - auto& b_k = dynamics[k].f; - auto& C_k = scalingVectors[k]; - - A_k.array().colwise() *= E.segment(currRow, nx_next).array(); - A_k *= D.segment(currCol, nx_k).asDiagonal(); - - B_k.array().colwise() *= E.segment(currRow, nx_next).array(); - B_k *= D.segment(currCol + nx_k, nu_k).asDiagonal(); - - C_k = E.segment(currRow, nx_next).array() * D.segment(currCol + nx_k + nu_k, nx_next).array(); - - b_k.array() *= E.segment(currRow, nx_next).array(); - - currRow += nx_next; - currCol += nx_k + nu_k; - } -} - -} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/test/pre_condition/testScaling.cpp b/ocs2_oc/test/pre_condition/testScaling.cpp index 61cbc5865..9799555a9 100644 --- a/ocs2_oc/test/pre_condition/testScaling.cpp +++ b/ocs2_oc/test/pre_condition/testScaling.cpp @@ -33,8 +33,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_oc/oc_problem/OcpToKkt.h" #include "ocs2_oc/pre_condition/Scaling.h" - -#include "ocs2_oc/test/SparseScaling.h" #include "ocs2_oc/test/testProblemsGeneration.h" class ScalingTest : public testing::Test { @@ -84,9 +82,9 @@ TEST_F(ScalingTest, preConditioningSparseMatrix) { const ocs2::vector_t g_src = g; // Test 1: Construct the stacked cost and constraints matrices first and scale next. - ocs2::preConditioningSparseMatrixInPlace(H, h, G, 5, D, E, c); + ocs2::preConditioningSparseMatrixInPlace(5, H, h, G, g, D, E, c); - // After pre-conditioning, H, h, G will be scaled in place. g remains the same. + // After pre-conditioning, H, h, G, and g will be scaled in place.. const Eigen::SparseMatrix<ocs2::scalar_t> H_ref = c * D.asDiagonal() * H_src * D.asDiagonal(); const ocs2::vector_t h_ref = c * D.asDiagonal() * h_src; const Eigen::SparseMatrix<ocs2::scalar_t> G_ref = E.asDiagonal() * G_src * D.asDiagonal(); @@ -96,6 +94,7 @@ TEST_F(ScalingTest, preConditioningSparseMatrix) { ASSERT_TRUE(H_ref.isApprox(H)); // H ASSERT_TRUE(h_ref.isApprox(h)); // h ASSERT_TRUE(G_ref.isApprox(G)) << "G_ref:\n" << G_ref << "\nG:\n" << G.toDense(); // G + ASSERT_TRUE(g_ref.isApprox(g)); // g // Normalized the inf-Norm of both rows and cols of KKT matrix should be closer to 1 const ocs2::matrix_t KKT = (ocs2::matrix_t(H_src.rows() + G_src.rows(), H_src.rows() + G_src.rows()) << H_src.toDense(), @@ -145,8 +144,7 @@ TEST_F(ScalingTest, preConditioningInPlaceInParallel) { ocs2::vector_t g_ref; Eigen::SparseMatrix<ocs2::scalar_t> G_ref; ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, nullptr, G_ref, g_ref); - ocs2::preConditioningSparseMatrixInPlace(H_ref, h_ref, G_ref, 5, D_ref, E_ref, c_ref); - g_ref = E_ref.asDiagonal() * g_ref; + ocs2::preConditioningSparseMatrixInPlace(5, H_ref, h_ref, G_ref, g_ref, D_ref, E_ref, c_ref); // Test start ocs2::vector_array_t D_array, E_array; diff --git a/ocs2_slp/include/ocs2_slp/Helpers.h b/ocs2_slp/include/ocs2_slp/Helpers.h index 338ab2fd2..769ae24a6 100644 --- a/ocs2_slp/include/ocs2_slp/Helpers.h +++ b/ocs2_slp/include/ocs2_slp/Helpers.h @@ -94,7 +94,7 @@ scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector< * @param [in] ocpSize: The size of optimal control problem. * @param [in] dynamics: Linear approximation of the dynamics over the time horizon. * @param [in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. - * @param [in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * @param [in] scalingVectorsPtr: Vector representation for the identity parts of the dynamics inside the constraint matrix. After scaling, * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. * @return: The upper bound of eigenvalues for G G'. */ @@ -145,7 +145,7 @@ vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFuncti * @param [in] ocpSize: The size of optimal control problem. * @param [in] dynamics: Linear approximation of the dynamics over the time horizon. * @param [in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. - * @param [in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * @param [in] scalingVectorsPtr: Vector representation for the identity parts of the dynamics inside the constraint matrix. After scaling, * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. * @return The absolute sum of rows of matrix G G'. */ diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index 5d93579c4..69db40786 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -65,7 +65,7 @@ class PipgSolver { * @param [in] dynamics : Dynamics array. * @param [in] cost : Cost array. * @param [in] constraints : Constraints array. Pass nullptr for an unconstrained problem. - * @param [in] scalingVectors : Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * @param [in] scalingVectors : Vector representation for the identity parts of the dynamics inside the constraint matrix. After scaling, * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. * @param [in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. * @param [in] pipgBounds : The PipgBounds used to define the primal and dual stepsizes. From 3e9538a5ad0dc863246d5963aeb59a9e0388caba Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 7 Dec 2022 14:49:02 +0100 Subject: [PATCH 412/512] small changes --- ocs2_oc/src/pre_condition/Scaling.cpp | 64 ++++++++++++--------------- 1 file changed, 29 insertions(+), 35 deletions(-) diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/pre_condition/Scaling.cpp index 2b6aacc05..4157fd7f3 100644 --- a/ocs2_oc/src/pre_condition/Scaling.cpp +++ b/ocs2_oc/src/pre_condition/Scaling.cpp @@ -102,39 +102,53 @@ void scaleMatrixInPlace(const vector_t* rowScale, const vector_t* colScale, Eige void invSqrtInfNormInParallel(ThreadPool& threadPool, const std::vector<VectorFunctionLinearApproximation>& dynamics, const std::vector<ScalarFunctionQuadraticApproximation>& cost, const vector_array_t& scalingVectors, - const int N, vector_array_t& D, vector_array_t& E) { + vector_array_t& D, vector_array_t& E) { // Helper function auto invSqrt = [](const vector_t& v) -> vector_t { return v.unaryExpr(std::ref(limitScaling)).array().sqrt().inverse(); }; + // resize + const int N = static_cast<int>(cost.size()) - 1; + E.resize(N); + D.resize(2 * N); + D[0] = invSqrt(matrixInfNormCols(cost[0].dfduu, dynamics[0].dfdu)); - E[0] = invSqrt(matrixInfNormRows(dynamics[0].dfdu, scalingVectors[0].asDiagonal().toDenseMatrix())); + E[0] = invSqrt(matrixInfNormRows(dynamics[0].dfdu, scalingVectors[0])); std::atomic_int timeStamp{1}; auto task = [&](int workerId) { int k; while ((k = timeStamp++) < N) { - D[2 * k - 1] = - invSqrt(matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux, scalingVectors[k - 1].asDiagonal().toDenseMatrix(), dynamics[k].dfdx)); + D[2 * k - 1] = invSqrt(matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux, scalingVectors[k - 1].transpose().eval(), dynamics[k].dfdx)); D[2 * k] = invSqrt(matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu, dynamics[k].dfdu)); - E[k] = invSqrt(matrixInfNormRows(dynamics[k].dfdx, dynamics[k].dfdu, scalingVectors[k].asDiagonal().toDenseMatrix())); + E[k] = invSqrt(matrixInfNormRows(dynamics[k].dfdx, dynamics[k].dfdu, scalingVectors[k])); } }; threadPool.runParallel(std::move(task), threadPool.numThreads() + 1U); - D[2 * N - 1] = invSqrt(matrixInfNormCols(cost[N].dfdxx, scalingVectors[N - 1].asDiagonal().toDenseMatrix())); + D[2 * N - 1] = invSqrt(matrixInfNormCols(cost[N].dfdxx, scalingVectors[N - 1].transpose().eval())); } -void scaleDataOneStepInPlaceInParallel(ThreadPool& threadPool, const vector_array_t& D, const vector_array_t& E, const int N, +void scaleDataOneStepInPlaceInParallel(ThreadPool& threadPool, const vector_array_t& D, const vector_array_t& E, std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<vector_t>& scalingVectors) { + // cost at 0 scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdu); scaleMatrixInPlace(&(D[0]), &(D[0]), cost.front().dfduu); scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdux); + // constraints at 0 + scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].f); + scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].dfdx); + scalingVectors[0].array() *= E[0].array() * D[1].array(); + if (dynamics[0].dfdu.size() > 0) { + scaleMatrixInPlace(&(E[0]), &(D[0]), dynamics[0].dfdu); + } - std::atomic_int timeStamp{1}; - auto scaleCost = [&](int workerId) { + std::atomic_int timeStamp1{1}, timeStamp2{1}; + auto scaleCostConstraints = [&](int workerId) { int k; - while ((k = timeStamp++) <= N) { + const int N = static_cast<int>(cost.size()) - 1; + // cost + while ((k = timeStamp1++) <= N) { scaleMatrixInPlace(&(D[2 * k - 1]), nullptr, cost[k].dfdx); scaleMatrixInPlace(&(D[2 * k - 1]), &(D[2 * k - 1]), cost[k].dfdxx); if (cost[k].dfdu.size() > 0) { @@ -143,28 +157,8 @@ void scaleDataOneStepInPlaceInParallel(ThreadPool& threadPool, const vector_arra scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k - 1]), cost[k].dfdux); } } - }; - threadPool.runParallel(std::move(scaleCost), threadPool.numThreads() + 1U); - - // Scale G & g - /** - * \f$ - * \tilde{B} = E * B * D, - * scaling = E * I * D, - * \tilde{g} = E * g, - * \f$ - */ - scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].f); - scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].dfdx); - scalingVectors[0].array() *= E[0].array() * D[1].array(); - if (dynamics[0].dfdu.size() > 0) { - scaleMatrixInPlace(&(E[0]), &(D[0]), dynamics[0].dfdu); - } - - timeStamp = 1; - auto scaleConstraints = [&](int workerId) { - int k; - while ((k = timeStamp++) < N) { + // constraints + while ((k = timeStamp2++) < N) { scaleMatrixInPlace(&(E[k]), nullptr, dynamics[k].f); scaleMatrixInPlace(&(E[k]), &(D[2 * k - 1]), dynamics[k].dfdx); scalingVectors[k].array() *= E[k].array() * D[2 * k + 1].array(); @@ -173,7 +167,7 @@ void scaleDataOneStepInPlaceInParallel(ThreadPool& threadPool, const vector_arra } } }; - threadPool.runParallel(std::move(scaleConstraints), threadPool.numThreads() + 1U); + threadPool.runParallel(std::move(scaleCostConstraints), threadPool.numThreads() + 1U); } vector_t matrixInfNormRows(const Eigen::SparseMatrix<scalar_t>& mat) { @@ -238,8 +232,8 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 vector_array_t D(2 * N), E(N); for (int i = 0; i < iteration; i++) { - invSqrtInfNormInParallel(threadPool, dynamics, cost, scalingVectors, N, D, E); - scaleDataOneStepInPlaceInParallel(threadPool, D, E, N, dynamics, cost, scalingVectors); + invSqrtInfNormInParallel(threadPool, dynamics, cost, scalingVectors, D, E); + scaleDataOneStepInPlaceInParallel(threadPool, D, E, dynamics, cost, scalingVectors); for (int k = 0; k < DOut.size(); k++) { DOut[k].array() *= D[k].array(); From 7e887f0fe7c2b4931f1ca36b0b6fd58ae2de027f Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 7 Dec 2022 14:54:42 +0100 Subject: [PATCH 413/512] changing the order of operations --- ocs2_oc/src/pre_condition/Scaling.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/pre_condition/Scaling.cpp index 4157fd7f3..fc05c9239 100644 --- a/ocs2_oc/src/pre_condition/Scaling.cpp +++ b/ocs2_oc/src/pre_condition/Scaling.cpp @@ -235,13 +235,6 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 invSqrtInfNormInParallel(threadPool, dynamics, cost, scalingVectors, D, E); scaleDataOneStepInPlaceInParallel(threadPool, D, E, dynamics, cost, scalingVectors); - for (int k = 0; k < DOut.size(); k++) { - DOut[k].array() *= D[k].array(); - } - for (int k = 0; k < EOut.size(); k++) { - EOut[k].array() *= E[k].array(); - } - const auto infNormOfh = [&cost, &x0]() { scalar_t out = (cost.front().dfdu + cost.front().dfdux * x0).lpNorm<Eigen::Infinity>(); for (int k = 1; k < cost.size(); ++k) { @@ -267,9 +260,15 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 }(); const auto averageOfInfNormOfH = sumOfInfNormOfH / static_cast<scalar_t>(numDecisionVariables); - const auto gamma = 1.0 / limitScaling(std::max(averageOfInfNormOfH, infNormOfh)); + for (int k = 0; k < DOut.size(); k++) { + DOut[k].array() *= D[k].array(); + } + for (int k = 0; k < EOut.size(); k++) { + EOut[k].array() *= E[k].array(); + } + for (int k = 0; k <= N; ++k) { cost[k].dfdxx *= gamma; cost[k].dfduu *= gamma; From ab8b344b2768d3fc7975d10033b535b4727e0281 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 7 Dec 2022 15:01:07 +0100 Subject: [PATCH 414/512] In-parallel E, D, c compute --- ocs2_oc/src/pre_condition/Scaling.cpp | 43 ++++++++++++++++++--------- 1 file changed, 29 insertions(+), 14 deletions(-) diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/pre_condition/Scaling.cpp index fc05c9239..f760bcb85 100644 --- a/ocs2_oc/src/pre_condition/Scaling.cpp +++ b/ocs2_oc/src/pre_condition/Scaling.cpp @@ -231,6 +231,8 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), 0); vector_array_t D(2 * N), E(N); + std::atomic_int timeIndex{0}; + const size_t numThreads = threadPool.numThreads() + 1U; for (int i = 0; i < iteration; i++) { invSqrtInfNormInParallel(threadPool, dynamics, cost, scalingVectors, D, E); scaleDataOneStepInPlaceInParallel(threadPool, D, E, dynamics, cost, scalingVectors); @@ -262,21 +264,34 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 const auto averageOfInfNormOfH = sumOfInfNormOfH / static_cast<scalar_t>(numDecisionVariables); const auto gamma = 1.0 / limitScaling(std::max(averageOfInfNormOfH, infNormOfh)); - for (int k = 0; k < DOut.size(); k++) { - DOut[k].array() *= D[k].array(); - } - for (int k = 0; k < EOut.size(); k++) { - EOut[k].array() *= E[k].array(); - } - - for (int k = 0; k <= N; ++k) { - cost[k].dfdxx *= gamma; - cost[k].dfduu *= gamma; - cost[k].dfdux *= gamma; - cost[k].dfdx *= gamma; - cost[k].dfdu *= gamma; - } + // compute EOut, DOut, and scale cost + timeIndex = 0; + auto computeDOutEOutScaleCost = [&](int workerId) { + int k = timeIndex++; + while (k < N) { + EOut[k].array() *= E[k].array(); + DOut[2 * k].array() *= D[2 * k].array(); + DOut[2 * k + 1].array() *= D[2 * k + 1].array(); + // cost + cost[k].dfdxx *= gamma; + cost[k].dfduu *= gamma; + cost[k].dfdux *= gamma; + cost[k].dfdx *= gamma; + cost[k].dfdu *= gamma; + + k = timeIndex++; + } + if (k == N) { // Only one worker will execute this + cost[N].dfdxx *= gamma; + cost[N].dfduu *= gamma; + cost[N].dfdux *= gamma; + cost[N].dfdx *= gamma; + cost[N].dfdu *= gamma; + } + }; + threadPool.runParallel(std::move(computeDOutEOutScaleCost), numThreads); + // compute cOut cOut *= gamma; } } From 33833dd99b42583f33e36fe7e3b6f42bade2708b Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 7 Dec 2022 17:27:27 +0100 Subject: [PATCH 415/512] all in parallel --- ocs2_oc/src/pre_condition/Scaling.cpp | 55 +++++++++++++++------------ 1 file changed, 30 insertions(+), 25 deletions(-) diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/pre_condition/Scaling.cpp index f760bcb85..fa15ae426 100644 --- a/ocs2_oc/src/pre_condition/Scaling.cpp +++ b/ocs2_oc/src/pre_condition/Scaling.cpp @@ -228,39 +228,44 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 } const auto numDecisionVariables = std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), 0) + - std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), 0); + std::accumulate(std::next(ocpSize.numStates.begin()), ocpSize.numStates.end(), 0); vector_array_t D(2 * N), E(N); std::atomic_int timeIndex{0}; - const size_t numThreads = threadPool.numThreads() + 1U; + const size_t numWorkers = threadPool.numThreads() + 1U; for (int i = 0; i < iteration; i++) { invSqrtInfNormInParallel(threadPool, dynamics, cost, scalingVectors, D, E); scaleDataOneStepInPlaceInParallel(threadPool, D, E, dynamics, cost, scalingVectors); - const auto infNormOfh = [&cost, &x0]() { - scalar_t out = (cost.front().dfdu + cost.front().dfdux * x0).lpNorm<Eigen::Infinity>(); - for (int k = 1; k < cost.size(); ++k) { - out = std::max(out, cost[k].dfdx.lpNorm<Eigen::Infinity>()); - if (cost[k].dfdu.size() > 0) { - out = std::max(out, cost[k].dfdu.lpNorm<Eigen::Infinity>()); - } + timeIndex = 0; + scalar_array_t infNormOfhArray(numWorkers, 0.0); + scalar_array_t sumOfInfNormOfHArray(numWorkers, 0.0); + auto infNormOfh_sumOfInfNormOfH = [&](int workerId) { + scalar_t workerInfNormOfh = 0.0; + scalar_t workerSumOfInfNormOfH = 0.0; + + int k = timeIndex++; + if (k == 0) { // Only one worker will execute this + workerInfNormOfh = (cost[0].dfdu + cost[0].dfdux * x0).lpNorm<Eigen::Infinity>(); + workerSumOfInfNormOfH = matrixInfNormCols(cost[0].dfduu).derived().sum(); + k = timeIndex++; } - return out; - }(); - - const auto sumOfInfNormOfH = [&cost]() { - scalar_t out = matrixInfNormCols(cost[0].dfduu).derived().sum(); - for (int k = 1; k < cost.size(); k++) { - if (cost[k].dfduu.size() == 0) { - out += matrixInfNormCols(cost[k].dfdxx).derived().sum(); - } else { - out += matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux).derived().sum(); - out += matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu).derived().sum(); - } + + while (k <= N) { + workerInfNormOfh = std::max(workerInfNormOfh, cost[k].dfdx.lpNorm<Eigen::Infinity>()); + workerInfNormOfh = std::max(workerInfNormOfh, cost[k].dfdu.lpNorm<Eigen::Infinity>()); + workerSumOfInfNormOfH += matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux).derived().sum(); + workerSumOfInfNormOfH += matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu).derived().sum(); + k = timeIndex++; } - return out; - }(); + infNormOfhArray[workerId] = std::max(infNormOfhArray[workerId], workerInfNormOfh); + sumOfInfNormOfHArray[workerId] += workerSumOfInfNormOfH; + }; + threadPool.runParallel(std::move(infNormOfh_sumOfInfNormOfH), numWorkers); + + const auto infNormOfh = *std::max_element(infNormOfhArray.cbegin(), infNormOfhArray.cend()); + const auto sumOfInfNormOfH = std::accumulate(sumOfInfNormOfHArray.cbegin(), sumOfInfNormOfHArray.cend(), 0.0); const auto averageOfInfNormOfH = sumOfInfNormOfH / static_cast<scalar_t>(numDecisionVariables); const auto gamma = 1.0 / limitScaling(std::max(averageOfInfNormOfH, infNormOfh)); @@ -289,7 +294,7 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 cost[N].dfdu *= gamma; } }; - threadPool.runParallel(std::move(computeDOutEOutScaleCost), numThreads); + threadPool.runParallel(std::move(computeDOutEOutScaleCost), numWorkers); // compute cOut cOut *= gamma; @@ -440,4 +445,4 @@ void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& } } -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 From 96ed00aaaabd9944dc9c9982d7ce4ac62809a736 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 7 Dec 2022 18:14:00 +0100 Subject: [PATCH 416/512] oc --- ocs2_oc/CMakeLists.txt | 14 ++--- .../Scaling.h => precondition/Ruzi.h} | 52 ++++++++++++------- ocs2_oc/src/lintTarget.cpp | 4 +- .../Scaling.cpp => precondition/Ruzi.cpp} | 37 ++++++++----- ...atrixConstruction.cpp => testOcpToKkt.cpp} | 8 +-- .../testPrecondition.cpp} | 18 +++---- ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h | 2 - ocs2_slp/src/pipg/PipgSolver.cpp | 15 ------ 8 files changed, 79 insertions(+), 71 deletions(-) rename ocs2_oc/include/ocs2_oc/{pre_condition/Scaling.h => precondition/Ruzi.h} (70%) rename ocs2_oc/src/{pre_condition/Scaling.cpp => precondition/Ruzi.cpp} (90%) rename ocs2_oc/test/oc_problem/{testMatrixConstruction.cpp => testOcpToKkt.cpp} (94%) rename ocs2_oc/test/{pre_condition/testScaling.cpp => precondition/testPrecondition.cpp} (92%) diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 96b8e5a51..a8f2ef99f 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -61,7 +61,7 @@ add_library(${PROJECT_NAME} src/oc_problem/OcpSize.cpp src/oc_problem/OcpToKkt.cpp src/oc_solver/SolverBase.cpp - src/pre_condition/Scaling.cpp + src/precondition/Ruzi.cpp src/rollout/PerformanceIndicesRollout.cpp src/rollout/RolloutBase.cpp src/rollout/RootFinder.cpp @@ -191,19 +191,19 @@ target_link_libraries(test_trajectory_spreading gtest_main ) -catkin_add_gtest(test_matrix_construction - test/oc_problem/testMatrixConstruction.cpp +catkin_add_gtest(test_ocp_to_kkt + test/oc_problem/testOcpToKkt.cpp ) -target_link_libraries(test_matrix_construction +target_link_libraries(test_ocp_to_kkt ${PROJECT_NAME} ${catkin_LIBRARIES} gtest_main ) -catkin_add_gtest(test_scaling - test/pre_condition/testScaling.cpp +catkin_add_gtest(test_precondition + test/precondition/testPrecondition.cpp ) -target_link_libraries(test_scaling +target_link_libraries(test_precondition ${PROJECT_NAME} ${catkin_LIBRARIES} gtest_main diff --git a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h b/ocs2_oc/include/ocs2_oc/precondition/Ruzi.h similarity index 70% rename from ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h rename to ocs2_oc/include/ocs2_oc/precondition/Ruzi.h index 295898fad..7f37a3a3f 100644 --- a/ocs2_oc/include/ocs2_oc/pre_condition/Scaling.h +++ b/ocs2_oc/include/ocs2_oc/precondition/Ruzi.h @@ -37,9 +37,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_oc/oc_problem/OcpSize.h" namespace ocs2 { +namespace precondition { /** - * Calculates the scaling factors D, E, and c, and scale the input dynamics, and cost data in place in parallel. + * Calculates the pre-conditioning factors D, E, and c, and scale the input dynamics, and cost data in place and parallel. * * There are a few pre-conditioning methods aiming to shape different aspects of the problem. To balance the performance and * computational effort, we choose a modified Ruzi equilibration Algorithm. Interested readers can find the original Ruiz @@ -88,13 +89,13 @@ namespace ocs2 { * of this type of matrix for every timestamp. * @param [out] cOut : Scaling factor c. */ -void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, - std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, - vector_array_t& scalingVectors, scalar_t& cOut); +void ocpDataInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, + std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, + vector_array_t& scalingVectors, scalar_t& cOut); /** - * Calculates the scaling factors D, E, and c, and scale the input dynamics, and cost data in place in parallel. + * Calculates the pre-conditioning factors D, E, and c, and scale the input dynamics, and cost data in place in place. * * There are a few pre-conditioning methods aiming to shape different aspects of the problem. To balance the performance and * computational effort, we choose a modified Ruzi equilibration Algorithm. Interested readers can find the original Ruiz @@ -141,24 +142,35 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 * @param [out] EOut : The matrix E decomposed for each time step. * @param [out] cOut : Scaling factor c. */ -void preConditioningSparseMatrixInPlace(int iteration, Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, - vector_t& g, vector_t& DOut, vector_t& EOut, scalar_t& cOut); +void kktMatrixInPlace(int iteration, Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, vector_t& g, + vector_t& DOut, vector_t& EOut, scalar_t& cOut); /** * Scales the dynamics and cost array in place and construct scaling vector array from the given scaling factors E, D and c. * - * @param[in] ocpSize : The size of the oc problem. - * @param[in] D : Scaling factor D - * @param[in] E : Scaling factor E - * @param[in] c : Scaling factor c - * @param[in, out] dynamics : The dynamics array of all time points. - * @param[in, out] cost : The cost array of all time points. - * @param[out] scalingVectors : Vector representation for the identity parts of the dynamics inside the constraint matrix. - * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal - * components of this type of matrix for every timestamp. + * @param [in] ocpSize : The size of the oc problem. + * @param [in] D : Scaling factor D + * @param [in] E : Scaling factor E + * @param [in] c : Scaling factor c + * @param [in, out] dynamics : The dynamics array of all time points. + * @param [in, out] cost : The cost array of all time points. + * @param [out] scalingVectors : Vector representation for the identity parts of the dynamics inside the constraint matrix. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal + * components of this type of matrix for every timestamp. + */ +void scaleOcpData(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, + std::vector<vector_t>& scalingVectors); + +/** + * Descales the solution. Note that the initial state is not considered a decision variable; therefore, it is not scaled. + * This pre-conditioning transforms the following decision vector z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}] to y := inv(D) z. + * + * @param [in] D : Scaling factor D + * @param [in, out] xTrajectory : The state trajectory of the length (N + 1). + * @param [in, out] uTrajectory : The input trajectory of the length N. */ -void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, - std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, - std::vector<vector_t>& scalingVectors); +void descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory); +} // namespace precondition } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 9d05708f7..982fa4c77 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -54,8 +54,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // oc_solver #include <ocs2_oc/oc_solver/SolverBase.h> -// pre_condition -#include <ocs2_oc/pre_condition/Scaling.h> +// precondition +#include <ocs2_oc/precondition/Ruzi.h> // synchronized_module #include <ocs2_oc/synchronized_module/LoopshapingReferenceManager.h> diff --git a/ocs2_oc/src/pre_condition/Scaling.cpp b/ocs2_oc/src/precondition/Ruzi.cpp similarity index 90% rename from ocs2_oc/src/pre_condition/Scaling.cpp rename to ocs2_oc/src/precondition/Ruzi.cpp index fa15ae426..3f7cb6e59 100644 --- a/ocs2_oc/src/pre_condition/Scaling.cpp +++ b/ocs2_oc/src/precondition/Ruzi.cpp @@ -27,13 +27,14 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include "ocs2_oc/pre_condition/Scaling.h" +#include "ocs2_oc/precondition/Ruzi.h" #include <atomic> #include <functional> #include <numeric> namespace ocs2 { +namespace precondition { // Internal helper functions namespace { @@ -206,13 +207,13 @@ void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eige } // anonymous namespace -void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, - std::vector<VectorFunctionLinearApproximation>& dynamics, - std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, - vector_array_t& scalingVectors, scalar_t& cOut) { +void ocpDataInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, + std::vector<VectorFunctionLinearApproximation>& dynamics, + std::vector<ScalarFunctionQuadraticApproximation>& cost, vector_array_t& DOut, vector_array_t& EOut, + vector_array_t& scalingVectors, scalar_t& cOut) { const int N = ocpSize.numStages; if (N < 1) { - throw std::runtime_error("[preConditioningInPlaceInParallel] The number of stages cannot be less than 1."); + throw std::runtime_error("[precondition::ocpDataInPlaceInParallel] The number of stages cannot be less than 1."); } // Init output @@ -301,8 +302,8 @@ void preConditioningInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0 } } -void preConditioningSparseMatrixInPlace(int iteration, Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, - vector_t& g, vector_t& DOut, vector_t& EOut, scalar_t& cOut) { +void kktMatrixInPlace(int iteration, Eigen::SparseMatrix<scalar_t>& H, vector_t& h, Eigen::SparseMatrix<scalar_t>& G, vector_t& g, + vector_t& DOut, vector_t& EOut, scalar_t& cOut) { const int nz = H.rows(); const int nc = G.rows(); @@ -351,12 +352,12 @@ void preConditioningSparseMatrixInPlace(int iteration, Eigen::SparseMatrix<scala } } -void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, - std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, - std::vector<vector_t>& scalingVectors) { +void scaleOcpData(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector<VectorFunctionLinearApproximation>& dynamics, std::vector<ScalarFunctionQuadraticApproximation>& cost, + std::vector<vector_t>& scalingVectors) { const int N = ocpSize.numStages; if (N < 1) { - throw std::runtime_error("[PIPG] The number of stages cannot be less than 1."); + throw std::runtime_error("[precondition::scaleOcpData] The number of stages cannot be less than 1."); } scalingVectors.resize(N); @@ -445,4 +446,16 @@ void scaleDataInPlace(const OcpSize& ocpSize, const vector_t& D, const vector_t& } } +void descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) { + if (D.size() != xTrajectory.size() + uTrajectory.size() - 1) { + throw std::runtime_error("[precondition::descaleSolution] - Size doesn't match."); + } + + for (int k = 0; k < uTrajectory.size(); k++) { + uTrajectory[k].array() *= D[2 * k].array(); + xTrajectory[k + 1].array() *= D[2 * k + 1].array(); + } +} + +} // namespace precondition } // namespace ocs2 diff --git a/ocs2_oc/test/oc_problem/testMatrixConstruction.cpp b/ocs2_oc/test/oc_problem/testOcpToKkt.cpp similarity index 94% rename from ocs2_oc/test/oc_problem/testMatrixConstruction.cpp rename to ocs2_oc/test/oc_problem/testOcpToKkt.cpp index 66abea0da..10e490ec8 100644 --- a/ocs2_oc/test/oc_problem/testMatrixConstruction.cpp +++ b/ocs2_oc/test/oc_problem/testOcpToKkt.cpp @@ -34,7 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_oc/test/testProblemsGeneration.h" -class MatrixConstructionTest : public testing::Test { +class OcpToKktTest : public testing::Test { protected: // x_0, x_1, ... x_{N - 1}, X_{N} static constexpr size_t N_ = 10; // numStages @@ -44,7 +44,7 @@ class MatrixConstructionTest : public testing::Test { static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); static constexpr size_t numConstraints = N_ * (nx_ + nc_); - MatrixConstructionTest() { + OcpToKktTest() { srand(0); x0 = ocs2::vector_t::Random(nx_); @@ -66,7 +66,7 @@ class MatrixConstructionTest : public testing::Test { std::vector<ocs2::VectorFunctionLinearApproximation> constraintsArray; }; -TEST_F(MatrixConstructionTest, sparseConstraintsApproximation) { +TEST_F(OcpToKktTest, sparseConstraintsApproximation) { Eigen::SparseMatrix<ocs2::scalar_t> G; ocs2::vector_t g; ocs2::vector_array_t scalingVectors(N_); @@ -81,7 +81,7 @@ TEST_F(MatrixConstructionTest, sparseConstraintsApproximation) { EXPECT_TRUE(constraintsApproximation.f.isApprox(g)); } -TEST_F(MatrixConstructionTest, sparseCostApproximation) { +TEST_F(OcpToKktTest, sparseCostApproximation) { Eigen::SparseMatrix<ocs2::scalar_t> H; ocs2::vector_t h; diff --git a/ocs2_oc/test/pre_condition/testScaling.cpp b/ocs2_oc/test/precondition/testPrecondition.cpp similarity index 92% rename from ocs2_oc/test/pre_condition/testScaling.cpp rename to ocs2_oc/test/precondition/testPrecondition.cpp index 9799555a9..ce509aec2 100644 --- a/ocs2_oc/test/pre_condition/testScaling.cpp +++ b/ocs2_oc/test/precondition/testPrecondition.cpp @@ -32,10 +32,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/thread_support/ThreadPool.h> #include "ocs2_oc/oc_problem/OcpToKkt.h" -#include "ocs2_oc/pre_condition/Scaling.h" +#include "ocs2_oc/precondition/Ruzi.h" #include "ocs2_oc/test/testProblemsGeneration.h" -class ScalingTest : public testing::Test { +class PreconditionTest : public testing::Test { protected: // x_0, x_1, ... x_{N - 1}, X_{N} static constexpr size_t N_ = 10; // numStages @@ -44,7 +44,7 @@ class ScalingTest : public testing::Test { static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); static constexpr size_t numConstraints = N_ * nx_; - ScalingTest() { + PreconditionTest() { srand(0); x0 = ocs2::vector_t::Random(nx_); @@ -63,7 +63,7 @@ class ScalingTest : public testing::Test { std::vector<ocs2::ScalarFunctionQuadraticApproximation> costArray; }; -TEST_F(ScalingTest, preConditioningSparseMatrix) { +TEST_F(PreconditionTest, kktMatrixInPlace) { ocs2::vector_t D, E; ocs2::scalar_t c; @@ -82,7 +82,7 @@ TEST_F(ScalingTest, preConditioningSparseMatrix) { const ocs2::vector_t g_src = g; // Test 1: Construct the stacked cost and constraints matrices first and scale next. - ocs2::preConditioningSparseMatrixInPlace(5, H, h, G, g, D, E, c); + ocs2::precondition::kktMatrixInPlace(5, H, h, G, g, D, E, c); // After pre-conditioning, H, h, G, and g will be scaled in place.. const Eigen::SparseMatrix<ocs2::scalar_t> H_ref = c * D.asDiagonal() * H_src * D.asDiagonal(); @@ -116,7 +116,7 @@ TEST_F(ScalingTest, preConditioningSparseMatrix) { // Test 2: Scale const and dynamics data of each time step first and then construct the stacked matrices from the scaled data. std::vector<ocs2::vector_t> scalingVectors; - ocs2::scaleDataInPlace(ocpSize_, D, E, c, dynamicsArray, costArray, scalingVectors); + ocs2::precondition::scaleOcpData(ocpSize_, D, E, c, dynamicsArray, costArray, scalingVectors); Eigen::SparseMatrix<ocs2::scalar_t> H_scaledData; ocs2::vector_t h_scaledData; @@ -130,7 +130,7 @@ TEST_F(ScalingTest, preConditioningSparseMatrix) { EXPECT_TRUE(g_ref.isApprox(g_scaledData)); // g } -TEST_F(ScalingTest, preConditioningInPlaceInParallel) { +TEST_F(PreconditionTest, ocpDataInPlaceInParallel) { ocs2::ThreadPool threadPool(5, 99); ocs2::vector_t D_ref, E_ref; @@ -144,13 +144,13 @@ TEST_F(ScalingTest, preConditioningInPlaceInParallel) { ocs2::vector_t g_ref; Eigen::SparseMatrix<ocs2::scalar_t> G_ref; ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, nullptr, G_ref, g_ref); - ocs2::preConditioningSparseMatrixInPlace(5, H_ref, h_ref, G_ref, g_ref, D_ref, E_ref, c_ref); + ocs2::precondition::kktMatrixInPlace(5, H_ref, h_ref, G_ref, g_ref, D_ref, E_ref, c_ref); // Test start ocs2::vector_array_t D_array, E_array; ocs2::scalar_t c; ocs2::vector_array_t scalingVectors(N_); - ocs2::preConditioningInPlaceInParallel(threadPool, x0, ocpSize_, 5, dynamicsArray, costArray, D_array, E_array, scalingVectors, c); + ocs2::precondition::ocpDataInPlaceInParallel(threadPool, x0, ocpSize_, 5, dynamicsArray, costArray, D_array, E_array, scalingVectors, c); ocs2::vector_t D_stacked(D_ref.rows()), E_stacked(E_ref.rows()); int curRow = 0; diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index 69db40786..13cdfe9bb 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -87,8 +87,6 @@ class PipgSolver { int getNumGeneralEqualityConstraints() const; - void descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const; - const pipg::Settings& settings() const { return settings_; } const OcpSize& size() const { return ocpSize_; } diff --git a/ocs2_slp/src/pipg/PipgSolver.cpp b/ocs2_slp/src/pipg/PipgSolver.cpp index 199646f61..1ab91d700 100644 --- a/ocs2_slp/src/pipg/PipgSolver.cpp +++ b/ocs2_slp/src/pipg/PipgSolver.cpp @@ -280,21 +280,6 @@ void PipgSolver::resize(const OcpSize& ocpSize) { WNew_.resize(N); } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void PipgSolver::descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) const { - const int N = ocpSize_.numStages; - if (D.size() != xTrajectory.size() + uTrajectory.size() - 1) { - throw std::runtime_error("[PipgSolver::descaleSolution] - Size doesn't match."); - } - - for (int k = 0; k < N; k++) { - uTrajectory[k].array() *= D[2 * k].array(); - xTrajectory[k + 1].array() *= D[2 * k + 1].array(); - } -} - /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ From 9225bb0ca49bb51f30a6e56c95e5a901bc3ba4d6 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 7 Dec 2022 18:35:22 +0100 Subject: [PATCH 417/512] slp --- ocs2_oc/CMakeLists.txt | 2 - ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h | 21 ++++++++++ ocs2_oc/src/oc_problem/OcpToKkt.cpp | 39 ++++++++++++++++++- .../test/precondition/testPrecondition.cpp | 29 ++++++++++++++ .../include/ocs2_slp/pipg/SingleThreadPipg.h | 21 ---------- ocs2_slp/src/SlpSolver.cpp | 8 ++-- ocs2_slp/src/pipg/SingleThreadPipg.cpp | 36 ----------------- ocs2_slp/test/testPipgSolver.cpp | 33 +--------------- 8 files changed, 94 insertions(+), 95 deletions(-) diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index a8f2ef99f..58c0aad74 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -110,11 +110,9 @@ install( LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) - install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) - install(DIRECTORY test/include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h b/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h index aa696a8f9..2fa79343e 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h @@ -142,4 +142,25 @@ void getCostMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector void getCostMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector<ScalarFunctionQuadraticApproximation>& cost, Eigen::SparseMatrix<scalar_t>& H, vector_t& h); +/** + * Deserializes the stacked solution to state-input trajecotries. Note that the initial state is not part of the stacked solution. + * + * @param [in] ocpSize : Optimal control problem sizes. + * @param [in] stackedSolution : Defined as [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * @param [in] x0 : The initial state. + * @param [out] xTrajectory : State tarjectory. + * @param [out] uTrajectory : Input trajecotry. + */ +void toOcpSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, + vector_array_t& uTrajectory); + +/** + * Serializes the state-input trajecotries. Note that the initial state is not considered as a decision variable. + * + * @param [in] xTrajectory : State tarjectory. + * @param [in] UTrajectory : Input trajecotry. + * @param [out] stackedSolution : [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + */ +void toKktSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution); + } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/src/oc_problem/OcpToKkt.cpp b/ocs2_oc/src/oc_problem/OcpToKkt.cpp index 9d20bf0ab..c068212e0 100644 --- a/ocs2_oc/src/oc_problem/OcpToKkt.cpp +++ b/ocs2_oc/src/oc_problem/OcpToKkt.cpp @@ -406,4 +406,41 @@ void getCostMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std:: H.setFromTriplets(tripletList.begin(), tripletList.end()); assert(H.nonZeros() <= nnz); } -} // namespace ocs2 + +void toOcpSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, + vector_array_t& uTrajectory) { + const int N = ocpSize.numStages; + xTrajectory.resize(N + 1); + uTrajectory.resize(N); + + xTrajectory.front() = x0; + + int curRow = 0; + for (int i = 0; i < N; i++) { + const auto nx = ocpSize.numStates[i + 1]; + const auto nu = ocpSize.numInputs[i]; + xTrajectory[i + 1] = stackedSolution.segment(curRow + nu, nx); + uTrajectory[i] = stackedSolution.segment(curRow, nu); + + curRow += nx + nu; + } +} + +void toKktSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) { + int numDecisionVariables = 0; + for (int i = 1; i < xTrajectory.size(); i++) { + numDecisionVariables += uTrajectory[i - 1].size() + xTrajectory[i].size(); + } + + stackedSolution.resize(numDecisionVariables); + + int curRow = 0; + for (int i = 1; i < xTrajectory.size(); i++) { + const auto nu = uTrajectory[i - 1].size(); + const auto nx = xTrajectory[i].size(); + stackedSolution.segment(curRow, nx + nu) << uTrajectory[i - 1], xTrajectory[i]; + curRow += nx + nu; + } +} + +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/test/precondition/testPrecondition.cpp b/ocs2_oc/test/precondition/testPrecondition.cpp index ce509aec2..74d002149 100644 --- a/ocs2_oc/test/precondition/testPrecondition.cpp +++ b/ocs2_oc/test/precondition/testPrecondition.cpp @@ -180,3 +180,32 @@ TEST_F(PreconditionTest, ocpDataInPlaceInParallel) { EXPECT_TRUE(G_ref.isApprox(G_scaledData)); // G EXPECT_TRUE(g_ref.isApprox(g_scaledData)); // g } + +TEST_F(PreconditionTest, descaleSolution) { + ocs2::vector_array_t D(2 * N_); + ocs2::vector_t DStacked(numDecisionVariables); + ocs2::vector_array_t x(N_ + 1), u(N_); + x[0].setRandom(nx_); + for (int i = 0; i < N_; i++) { + D[2 * i].setRandom(nu_); + D[2 * i + 1].setRandom(nx_); + u[i].setRandom(nu_); + x[i + 1].setRandom(nx_); + } + int curRow = 0; + for (auto& v : D) { + DStacked.segment(curRow, v.size()) = v; + curRow += v.size(); + } + ocs2::vector_t packedSolution; + ocs2::toKktSolution(x, u, packedSolution); + + packedSolution.array() *= DStacked.array(); + + ocs2::vector_t packedSolutionNew; + ocs2::precondition::descaleSolution(D, x, u); + ocs2::toKktSolution(x, u, packedSolutionNew); + EXPECT_TRUE(packedSolutionNew.isApprox(packedSolution)) << std::setprecision(6) << "DescaledSolution: \n" + << packedSolutionNew.transpose() << "\nIt should be \n" + << packedSolution.transpose(); +} \ No newline at end of file diff --git a/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h index 2fa230309..9274bfe20 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h +++ b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h @@ -83,26 +83,5 @@ SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::Spars const Eigen::SparseMatrix<scalar_t>& G, const vector_t& g, const vector_t& EInv, const PipgBounds& pipgBounds, vector_t& stackedSolution); -/** - * Deserializes the stacked solution to state-input trajecotries. - * - * @param [in] ocpSize : Optimal control problem sizes. - * @param [in] stackedSolution : Defined as [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. - * @param [in] x0 : The initial state. - * @param [out] xTrajectory : State tarjectory. - * @param [out] uTrajectory : Input trajecotry. - */ -void unpackSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, - vector_array_t& uTrajectory); - -/** - * Serializes the state-input trajecotries. - * - * @param [in] xTrajectory : State tarjectory. - * @param [in] UTrajectory : Input trajecotry. - * @param [out] stackedSolution : [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. - */ -void packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution); - } // namespace pipg } // namespace ocs2 diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 5dc32bb3f..8131af966 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -36,7 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/multiple_shooting/Initialization.h> #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> #include <ocs2_oc/multiple_shooting/Transcription.h> -#include <ocs2_oc/pre_condition/Scaling.h> +#include <ocs2_oc/precondition/Ruzi.h> #include "ocs2_slp/Helpers.h" @@ -240,8 +240,8 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta scalar_t c; vector_array_t D, E; vector_array_t scalingVectors; - preConditioningInPlaceInParallel(threadPool_, delta_x0, pipgSolver_.size(), settings_.scalingIteration, dynamics_, cost_, D, E, - scalingVectors, c); + precondition::ocpDataInPlaceInParallel(threadPool_, delta_x0, pipgSolver_.size(), settings_.scalingIteration, dynamics_, cost_, D, E, + scalingVectors, c); preConditioning_.endTimer(); // estimate mu and lambda: mu I < H < lambda I @@ -275,7 +275,7 @@ SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] solution.armijoDescentMetric = armijoDescentMetric(cost_, deltaXSol, deltaUSol); - pipgSolver_.descaleSolution(D, deltaXSol, deltaUSol); + precondition::descaleSolution(D, deltaXSol, deltaUSol); // remap the tilde delta u to real delta u multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); diff --git a/ocs2_slp/src/pipg/SingleThreadPipg.cpp b/ocs2_slp/src/pipg/SingleThreadPipg.cpp index cbdd2625c..c0dfb1c63 100644 --- a/ocs2_slp/src/pipg/SingleThreadPipg.cpp +++ b/ocs2_slp/src/pipg/SingleThreadPipg.cpp @@ -107,41 +107,5 @@ SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::Spars return status; } -void unpackSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, - vector_array_t& uTrajectory) { - const int N = ocpSize.numStages; - xTrajectory.resize(N + 1); - uTrajectory.resize(N); - - xTrajectory.front() = x0; - - int curRow = 0; - for (int i = 0; i < N; i++) { - const auto nx = ocpSize.numStates[i + 1]; - const auto nu = ocpSize.numInputs[i]; - xTrajectory[i + 1] = stackedSolution.segment(curRow + nu, nx); - uTrajectory[i] = stackedSolution.segment(curRow, nu); - - curRow += nx + nu; - } -} - -void packSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) { - int numDecisionVariables = 0; - for (int i = 1; i < xTrajectory.size(); i++) { - numDecisionVariables += uTrajectory[i - 1].size() + xTrajectory[i].size(); - } - - stackedSolution.resize(numDecisionVariables); - - int curRow = 0; - for (int i = 1; i < xTrajectory.size(); i++) { - const auto nu = uTrajectory[i - 1].size(); - const auto nx = xTrajectory[i].size(); - stackedSolution.segment(curRow, nx + nu) << uTrajectory[i - 1], xTrajectory[i]; - curRow += nx + nu; - } -} - } // namespace pipg } // namespace ocs2 diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp index f0430b425..c80c9f8f6 100644 --- a/ocs2_slp/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -119,7 +119,7 @@ TEST_F(PIPGSolverTest, correctness) { std::ignore = solver.solve(threadPool, x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, pipgBounds, X, U); ocs2::vector_t primalSolutionPIPGParallel; - ocs2::pipg::packSolution(X, U, primalSolutionPIPGParallel); + ocs2::toKktSolution(X, U, primalSolutionPIPGParallel); auto calculateConstraintViolation = [&](const ocs2::vector_t& sol) -> ocs2::scalar_t { return (constraintsApproximation.dfdx * sol - constraintsApproximation.f).cwiseAbs().maxCoeff(); @@ -177,33 +177,4 @@ TEST_F(PIPGSolverTest, correctness) { ASSERT_TRUE(std::abs(PIPGConstraintViolation) < solver.settings().absoluteTolerance); EXPECT_TRUE(std::abs(QPConstraintViolation - PIPGConstraintViolation) < solver.settings().absoluteTolerance * 10.0); EXPECT_TRUE(std::abs(PIPGParallelCConstraintViolation - PIPGConstraintViolation) < solver.settings().absoluteTolerance * 10.0); -} - -TEST_F(PIPGSolverTest, descaleSolution) { - ocs2::vector_array_t D(2 * N_); - ocs2::vector_t DStacked(numDecisionVariables); - ocs2::vector_array_t x(N_ + 1), u(N_); - x[0].setRandom(nx_); - for (int i = 0; i < N_; i++) { - D[2 * i].setRandom(nu_); - D[2 * i + 1].setRandom(nx_); - u[i].setRandom(nu_); - x[i + 1].setRandom(nx_); - } - int curRow = 0; - for (auto& v : D) { - DStacked.segment(curRow, v.size()) = v; - curRow += v.size(); - } - ocs2::vector_t packedSolution; - ocs2::pipg::packSolution(x, u, packedSolution); - - packedSolution.array() *= DStacked.array(); - - ocs2::vector_t packedSolutionMy; - solver.descaleSolution(D, x, u); - ocs2::pipg::packSolution(x, u, packedSolutionMy); - EXPECT_TRUE(packedSolutionMy.isApprox(packedSolution)) << std::setprecision(6) << "DescaledSolution: \n" - << packedSolutionMy.transpose() << "\nIt should be \n" - << packedSolution.transpose(); -} +} \ No newline at end of file From ec7c9d7b4abdbdc67ea7c67f4a732e063f89845e Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 7 Dec 2022 18:49:57 +0100 Subject: [PATCH 418/512] static variable definition --- .../test/precondition/testPrecondition.cpp | 12 +++++++--- ocs2_slp/test/testPipgSolver.cpp | 24 ++++++++++++------- 2 files changed, 24 insertions(+), 12 deletions(-) diff --git a/ocs2_oc/test/precondition/testPrecondition.cpp b/ocs2_oc/test/precondition/testPrecondition.cpp index 74d002149..a0243f710 100644 --- a/ocs2_oc/test/precondition/testPrecondition.cpp +++ b/ocs2_oc/test/precondition/testPrecondition.cpp @@ -41,8 +41,8 @@ class PreconditionTest : public testing::Test { static constexpr size_t N_ = 10; // numStages static constexpr size_t nx_ = 4; static constexpr size_t nu_ = 3; - static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); - static constexpr size_t numConstraints = N_ * nx_; + static constexpr size_t numDecisionVariables_ = N_ * (nx_ + nu_); + static constexpr size_t numConstraints_ = N_ * nx_; PreconditionTest() { srand(0); @@ -63,6 +63,12 @@ class PreconditionTest : public testing::Test { std::vector<ocs2::ScalarFunctionQuadraticApproximation> costArray; }; +constexpr size_t PreconditionTest::N_; +constexpr size_t PreconditionTest::nx_; +constexpr size_t PreconditionTest::nu_; +constexpr size_t PreconditionTest::numDecisionVariables_; +constexpr size_t PreconditionTest::numConstraints_; + TEST_F(PreconditionTest, kktMatrixInPlace) { ocs2::vector_t D, E; ocs2::scalar_t c; @@ -183,7 +189,7 @@ TEST_F(PreconditionTest, ocpDataInPlaceInParallel) { TEST_F(PreconditionTest, descaleSolution) { ocs2::vector_array_t D(2 * N_); - ocs2::vector_t DStacked(numDecisionVariables); + ocs2::vector_t DStacked(numDecisionVariables_); ocs2::vector_array_t x(N_ + 1), u(N_); x[0].setRandom(nx_); for (int i = 0; i < N_; i++) { diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp index c80c9f8f6..09d907e77 100644 --- a/ocs2_slp/test/testPipgSolver.cpp +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -56,12 +56,12 @@ class PIPGSolverTest : public testing::Test { static constexpr size_t nx_ = 4; static constexpr size_t nu_ = 3; static constexpr size_t nc_ = 0; - static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); - static constexpr size_t numConstraints = N_ * (nx_ + nc_); - static constexpr size_t numThreads = 8; - static constexpr bool verbose = true; + static constexpr size_t numDecisionVariables_ = N_ * (nx_ + nu_); + static constexpr size_t numConstraints_ = N_ * (nx_ + nc_); + static constexpr size_t numThreads_ = 8; + static constexpr bool verbose_ = true; - PIPGSolverTest() : solver(configurePipg(30000, 1e-10, 1e-3, verbose)) { + PIPGSolverTest() : solver(configurePipg(30000, 1e-10, 1e-3, verbose_)) { srand(10); // Construct OCP problem @@ -88,11 +88,17 @@ class PIPGSolverTest : public testing::Test { std::vector<ocs2::VectorFunctionLinearApproximation> constraintsArray; ocs2::PipgSolver solver; - ocs2::ThreadPool threadPool{numThreads - 1u, 50}; + ocs2::ThreadPool threadPool{numThreads_ - 1u, 50}; }; -constexpr size_t PIPGSolverTest::numDecisionVariables; -constexpr size_t PIPGSolverTest::numConstraints; +constexpr size_t PIPGSolverTest::N_; +constexpr size_t PIPGSolverTest::nx_; +constexpr size_t PIPGSolverTest::nu_; +constexpr size_t PIPGSolverTest::nc_; +constexpr size_t PIPGSolverTest::numDecisionVariables_; +constexpr size_t PIPGSolverTest::numConstraints_; +constexpr size_t PIPGSolverTest::numThreads_; +constexpr bool PIPGSolverTest::verbose_; TEST_F(PIPGSolverTest, correctness) { // ocs2::qp_solver::SolveDenseQP use Gz + g = 0 for constraints @@ -136,7 +142,7 @@ TEST_F(PIPGSolverTest, correctness) { ocs2::scalar_t PIPGParallelCost = calculateCost(primalSolutionPIPGParallel); ocs2::scalar_t PIPGParallelCConstraintViolation = calculateConstraintViolation(primalSolutionPIPGParallel); - if (verbose) { + if (verbose_) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; std::cerr << "\n++++++++++++++ [TestPIPG] Correctness ++++++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; From 274c9ca01df41bdb1000d904379b60785d27e39d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 7 Dec 2022 19:12:22 +0100 Subject: [PATCH 419/512] small fixes --- ocs2_slp/include/ocs2_slp/SlpMpc.h | 2 ++ ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h | 14 +++----- ocs2_slp/src/Helpers.cpp | 34 ------------------- ocs2_slp/src/lintTarget.cpp | 2 ++ ocs2_slp/src/pipg/PipgSolver.cpp | 13 ++----- .../include/hpipm_catkin/HpipmInterface.h | 1 - 6 files changed, 11 insertions(+), 55 deletions(-) diff --git a/ocs2_slp/include/ocs2_slp/SlpMpc.h b/ocs2_slp/include/ocs2_slp/SlpMpc.h index 6f74a38f6..517ad094c 100644 --- a/ocs2_slp/include/ocs2_slp/SlpMpc.h +++ b/ocs2_slp/include/ocs2_slp/SlpMpc.h @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_slp/SlpSolver.h" namespace ocs2 { + class SlpMpc final : public MPC_BASE { public: /** @@ -67,4 +68,5 @@ class SlpMpc final : public MPC_BASE { private: std::unique_ptr<SlpSolver> solverPtr_; }; + } // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h index 13cdfe9bb..5a631e0cd 100644 --- a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -82,13 +82,10 @@ class PipgSolver { void resize(const OcpSize& size); int getNumDecisionVariables() const { return numDecisionVariables_; } - int getNumDynamicsConstraints() const { return numDynamicsConstraints_; } - int getNumGeneralEqualityConstraints() const; - - const pipg::Settings& settings() const { return settings_; } const OcpSize& size() const { return ocpSize_; } + const pipg::Settings& settings() const { return settings_; } private: void verifySizes(const std::vector<VectorFunctionLinearApproximation>& dynamics, @@ -97,18 +94,17 @@ class PipgSolver { void verifyOcpSize(const OcpSize& ocpSize) const; - private: + // Settings const pipg::Settings settings_; + // Problem size OcpSize ocpSize_; + int numDecisionVariables_; + int numDynamicsConstraints_; // Data buffer for parallelized PIPG vector_array_t X_, W_, V_, U_; vector_array_t XNew_, UNew_, WNew_; - - // Problem size - int numDecisionVariables_; - int numDynamicsConstraints_; }; } // namespace ocs2 diff --git a/ocs2_slp/src/Helpers.cpp b/ocs2_slp/src/Helpers.cpp index 8b0173e49..8c3c397c3 100644 --- a/ocs2_slp/src/Helpers.cpp +++ b/ocs2_slp/src/Helpers.cpp @@ -157,40 +157,6 @@ vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, Eigen::setNbThreads(0); // Restore default setup. return res; - - // /** - // * ********************************* - // * ************ Test begin ********* - // * ********************************* - // */ - // matrix_t m(getNumDynamicsConstraints(), getNumDynamicsConstraints()); - // const int nx_1 = ocpSize.numStates[1]; - // const int nx_2 = ocpSize.numStates[2]; - // const auto& A1 = dynamics[1].dfdx; - // m.topLeftCorner(nx_1, nx_1 + nx_2) << tempMatrixArray[0], - // -(A1 * (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_1, nx_1) : (*scalingVectorsPtr)[0].asDiagonal().toDenseMatrix())) - // .transpose(); - - // curRow = nx_1; - // for (int i = 1; i < N - 1; i++) { - // const int nx_i1 = ocpSize.numStates[i + 1]; - // const int nx_i2 = ocpSize.numStates[i + 2]; - // const auto& ANext = dynamics[i + 1].dfdx; - - // m.block(curRow, curRow, nx_i1, nx_i1 + nx_i2) << tempMatrixArray[i], - // -(ANext * (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_i1, nx_i1) : - // (*scalingVectorsPtr)[i].asDiagonal().toDenseMatrix())) - // .transpose(); - // curRow += nx_i1; - // } - // const int nx_N = ocpSize.numStates[N]; - // m.block(curRow, curRow, nx_N, nx_N) = tempMatrixArray[N - 1]; - // std::cerr << "GGT: \n" << m.selfadjointView<Eigen::Upper>().toDenseMatrix() << "\n\n"; - // /** - // * ********************************* - // * ************ Test end ********* - // * ********************************* - // */ } } // namespace slp diff --git a/ocs2_slp/src/lintTarget.cpp b/ocs2_slp/src/lintTarget.cpp index 4eb308cc2..80dba9fcb 100644 --- a/ocs2_slp/src/lintTarget.cpp +++ b/ocs2_slp/src/lintTarget.cpp @@ -27,9 +27,11 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ +#include <ocs2_slp/pipg/PipgBounds.h> #include <ocs2_slp/pipg/PipgSettings.h> #include <ocs2_slp/pipg/PipgSolver.h> #include <ocs2_slp/pipg/PipgSolverStatus.h> +#include <ocs2_slp/pipg/SingleThreadPipg.h> #include <ocs2_slp/Helpers.h> #include <ocs2_slp/SlpMpc.h> diff --git a/ocs2_slp/src/pipg/PipgSolver.cpp b/ocs2_slp/src/pipg/PipgSolver.cpp index 1ab91d700..f3da51a19 100644 --- a/ocs2_slp/src/pipg/PipgSolver.cpp +++ b/ocs2_slp/src/pipg/PipgSolver.cpp @@ -267,9 +267,9 @@ void PipgSolver::resize(const OcpSize& ocpSize) { ocpSize_ = ocpSize; const int N = ocpSize_.numStages; - numDecisionVariables_ = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); + numDecisionVariables_ = std::accumulate(std::next(ocpSize_.numStates.begin()), ocpSize_.numStates.end(), 0); numDecisionVariables_ += std::accumulate(ocpSize_.numInputs.begin(), ocpSize_.numInputs.end(), 0); - numDynamicsConstraints_ = std::accumulate(ocpSize_.numStates.begin() + 1, ocpSize_.numStates.end(), 0); + numDynamicsConstraints_ = std::accumulate(std::next(ocpSize_.numStates.begin()), ocpSize_.numStates.end(), 0); X_.resize(N + 1); W_.resize(N); @@ -325,13 +325,4 @@ void PipgSolver::verifyOcpSize(const OcpSize& ocpSize) const { } } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -int PipgSolver::getNumGeneralEqualityConstraints() const { - const auto totalNumberOfGeneralEqualityConstraints = - std::accumulate(ocpSize_.numIneqConstraints.begin(), ocpSize_.numIneqConstraints.end(), 0); - return totalNumberOfGeneralEqualityConstraints; -} - } // namespace ocs2 diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h index bd3786c70..2a62314d5 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h @@ -48,7 +48,6 @@ namespace ocs2 { */ class HpipmInterface { public: - using OcpSize = ::ocs2::OcpSize; using Settings = hpipm_interface::Settings; /** From e2ecea24b47a48b4118357738e5d6bf6221b4ee2 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 7 Dec 2022 19:52:47 +0100 Subject: [PATCH 420/512] bugfix --- ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp b/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp index df63bab24..1d3f7a6df 100644 --- a/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp +++ b/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp @@ -50,7 +50,7 @@ TEST(test_hpiphm_interface, solve_and_check_dynamic) { cost.emplace_back(ocs2::getRandomCost(nx, 0)); // Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); ocs2::HpipmInterface hpipmInterface(ocpSize); // Solve! @@ -87,7 +87,7 @@ TEST(test_hpiphm_interface, solve_after_resize) { cost.emplace_back(ocs2::getRandomCost(nx, 0)); // Resize Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); hpipmInterface.resize(ocpSize); // Solve! @@ -137,7 +137,7 @@ TEST(test_hpiphm_interface, knownSolution) { cost[N].dfdx = -cost[N].dfdxx * xSolGiven[N]; // Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); ocs2::HpipmInterface hpipmInterface(ocpSize); // Solve! @@ -174,7 +174,7 @@ TEST(test_hpiphm_interface, with_constraints) { constraints.emplace_back(ocs2::getRandomConstraints(nx, 0, nc)); // Resize Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); std::fill(ocpSize.numIneqConstraints.begin(), ocpSize.numIneqConstraints.end(), nc); // Set one of the constraints to empty @@ -304,7 +304,7 @@ TEST(test_hpiphm_interface, retrieveRiccati) { } // Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); ocs2::HpipmInterface hpipmInterface(ocpSize); // Solve! From ee01a91a32fe76703f1a1daab0517ae52fe1b623 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 8 Dec 2022 10:03:19 +0100 Subject: [PATCH 421/512] resolve tasks --- .../multiple_shooting/LagrangianEvaluation.h | 47 ++++++++++--------- .../ocs2_oc/multiple_shooting/Transcription.h | 1 - .../LagrangianEvaluation.cpp | 21 ++++++--- .../src/multiple_shooting/Transcription.cpp | 3 +- .../ocs2_sqp/include/ocs2_sqp/SqpSettings.h | 3 +- .../ocs2_sqp/include/ocs2_sqp/SqpSolver.h | 2 + ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 5 +- 7 files changed, 46 insertions(+), 36 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h index 7e13b1ea5..27bdce4c3 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h @@ -37,7 +37,7 @@ namespace multiple_shooting { /** * Computes the SSE of the residual in the dual feasibilities. * - * @param[in] lagrangian : Quadratic approximation of the Lagrangian for a single node. + * @param lagrangian : Quadratic approximation of the Lagrangian for a single node. * @return SSE of the residual in the dual feasibilities */ inline scalar_t evaluateDualFeasibilities(const ScalarFunctionQuadraticApproximation& lagrangian) { @@ -47,39 +47,40 @@ inline scalar_t evaluateDualFeasibilities(const ScalarFunctionQuadraticApproxima /** * Evaluates the quadratic approximation of the Lagrangian for a single intermediate node. * - * @param[in] lmd : Costate at start of the interval - * @param[in] lmd_next : Costate at the end of the interval - * @param[in] nu : Lagrange multiplier of the projection constraint. - * @param[in] dynamics : Linear approximation of the dynamics - * @param[in] stateInputEqConstraints : Linear approximation of the state-input equality constraints - * @param[in, out] lagrangian : Should be initially filled with the quadratic approximation of the cost. Then this is overwritten into the - * quadratic approximation of the Lagrangian. + * @param lmd : Costate at start of the interval + * @param lmd_next : Costate at the end of the interval + * @param nu : Lagrange multiplier of the projection constraint. + * @param cost : Quadratic approximation of the cost. + * @param dynamics : Linear approximation of the dynamics + * @param stateInputEqConstraints : Linear approximation of the state-input equality constraints + * @return Quadratic approximation of the Lagrangian. */ -void evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu, - const VectorFunctionLinearApproximation& dynamics, - const VectorFunctionLinearApproximation& stateInputEqConstraints, - ScalarFunctionQuadraticApproximation& lagrangian); +ScalarFunctionQuadraticApproximation evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu, + ScalarFunctionQuadraticApproximation&& cost, + const VectorFunctionLinearApproximation& dynamics, + const VectorFunctionLinearApproximation& stateInputEqConstraints); /** * Evaluates the quadratic approximation of the Lagrangian for the terminal node. * - * @param[in] lmd : Costate at start of the interval - * @param[in, out] lagrangian : Should be initially filled with the quadratic approximation of the cost. Then this is overwritten into the - * quadratic approximation of the Lagrangian. + * @param lmd : Costate at start of the interval + * @param cost : Quadratic approximation of the cost. + * @return Quadratic approximation of the Lagrangian. */ -void evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation& lagrangian); +ScalarFunctionQuadraticApproximation evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation&& cost); /** * Evaluates the quadratic approximation of the Lagrangian for the event node. * - * @param[in] lmd : Costate at start of the interval - * @param[in] lmd_next : Costate at the end of the the interval - * @param[in] dynamics : Dynamics - * @param[in, out] lagrangian : Should be initially filled with the quadratic approximation of the cost. Then this is overwritten into the - * quadratic approximation of the Lagrangian. + * @param lmd : Costate at start of the interval + * @param lmd_next : Costate at the end of the the interval + * @param cost : Quadratic approximation of the cost. + * @param dynamics : Dynamics + * @return Quadratic approximation of the Lagrangian. */ -void evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next, const VectorFunctionLinearApproximation& dynamics, - ScalarFunctionQuadraticApproximation& lagrangian); +ScalarFunctionQuadraticApproximation evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next, + ScalarFunctionQuadraticApproximation&& cost, + const VectorFunctionLinearApproximation& dynamics); } // namespace multiple_shooting } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h index 875c202eb..9ea808566 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h @@ -44,7 +44,6 @@ namespace multiple_shooting { struct Transcription { VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; - matrix_t constraintPseudoInverse; ProjectionMultiplierCoefficients projectionMultiplierCoefficients; VectorFunctionLinearApproximation constraintsProjection; VectorFunctionLinearApproximation stateInputEqConstraints; diff --git a/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp b/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp index 8afe2a015..7cff7663f 100644 --- a/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp +++ b/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp @@ -32,10 +32,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { -void evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu, - const VectorFunctionLinearApproximation& dynamics, - const VectorFunctionLinearApproximation& stateInputEqConstraints, - ScalarFunctionQuadraticApproximation& lagrangian) { +ScalarFunctionQuadraticApproximation evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu, + ScalarFunctionQuadraticApproximation&& cost, + const VectorFunctionLinearApproximation& dynamics, + const VectorFunctionLinearApproximation& stateInputEqConstraints) { + ScalarFunctionQuadraticApproximation lagrangian = std::move(cost); lagrangian.dfdx.noalias() += dynamics.dfdx.transpose() * lmd_next; lagrangian.dfdx.noalias() -= lmd; lagrangian.dfdu.noalias() += dynamics.dfdu.transpose() * lmd_next; @@ -43,16 +44,22 @@ void evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd lagrangian.dfdx.noalias() += stateInputEqConstraints.dfdx.transpose() * nu; lagrangian.dfdu.noalias() += stateInputEqConstraints.dfdu.transpose() * nu; } + return lagrangian; } -void evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation& lagrangian) { +ScalarFunctionQuadraticApproximation evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation&& cost) { + ScalarFunctionQuadraticApproximation lagrangian = std::move(cost); lagrangian.dfdx.noalias() -= lmd; + return lagrangian; } -void evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next, const VectorFunctionLinearApproximation& dynamics, - ScalarFunctionQuadraticApproximation& lagrangian) { +ScalarFunctionQuadraticApproximation evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next, + ScalarFunctionQuadraticApproximation&& cost, + const VectorFunctionLinearApproximation& dynamics) { + ScalarFunctionQuadraticApproximation lagrangian = std::move(cost); lagrangian.dfdx.noalias() += dynamics.dfdx.transpose() * lmd_next; lagrangian.dfdx.noalias() -= lmd; + return lagrangian; } } // namespace multiple_shooting diff --git a/ocs2_oc/src/multiple_shooting/Transcription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp index 2b4fb37ef..85d1215e3 100644 --- a/ocs2_oc/src/multiple_shooting/Transcription.cpp +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -87,7 +87,6 @@ void projectTranscription(Transcription& transcription, bool extractEqualityCons auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; auto& projection = transcription.constraintsProjection; - auto& constraintPseudoInverse = transcription.constraintPseudoInverse; auto& projectionMultiplierCoefficients = transcription.projectionMultiplierCoefficients; auto& stateInputEqConstraints = transcription.stateInputEqConstraints; auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; @@ -95,11 +94,11 @@ void projectTranscription(Transcription& transcription, bool extractEqualityCons if (stateInputEqConstraints.f.size() > 0) { // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. if (extractEqualityConstraintsPseudoInverse) { + matrix_t constraintPseudoInverse; std::tie(projection, constraintPseudoInverse) = LinearAlgebra::qrConstraintProjection(stateInputEqConstraints); projectionMultiplierCoefficients.compute(dynamics, cost, projection, constraintPseudoInverse); } else { projection = LinearAlgebra::luConstraintProjection(stateInputEqConstraints).first; - constraintPseudoInverse = matrix_t(); projectionMultiplierCoefficients = ProjectionMultiplierCoefficients(); } stateInputEqConstraints = VectorFunctionLinearApproximation(); diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h index a03fc023f..75d29508f 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h @@ -67,7 +67,8 @@ struct Settings { // Inequality penalty relaxed barrier parameters scalar_t inequalityConstraintMu = 0.0; scalar_t inequalityConstraintDelta = 1e-6; - bool projectStateInputEqualityConstraints = true; // Use a projection method to resolve the state-input constraint Cx+Du+e + bool projectStateInputEqualityConstraints = true; // Use a projection method to resolve the state-input constraint Cx+Du+e + bool extractProjectionMultiplierCoefficients = false; // Extract the Lagrange multiplier of the projected state-input constraint Cx+Du+e // Printing bool printSolverStatus = false; // Print HPIPM status after solving the QP subproblem diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index 98d160e61..db3e3b54a 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/misc/Benchmark.h> #include <ocs2_core/thread_support/ThreadPool.h> +#include <ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h> #include <ocs2_oc/oc_data/TimeDiscretization.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> #include <ocs2_oc/oc_solver/SolverBase.h> @@ -173,6 +174,7 @@ class SqpSolver : public SolverBase { std::vector<VectorFunctionLinearApproximation> dynamics_; std::vector<ScalarFunctionQuadraticApproximation> cost_; std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_; std::vector<VectorFunctionLinearApproximation> stateInputEqConstraints_; std::vector<VectorFunctionLinearApproximation> stateIneqConstraints_; std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 411cddec3..f6f88947b 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -291,6 +291,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated dynamics_.resize(N); cost_.resize(N + 1); constraintsProjection_.resize(N); + projectionMultiplierCoefficients_.resize(N); stateInputEqConstraints_.resize(N + 1); stateIneqConstraints_.resize(N + 1); stateInputIneqConstraints_.resize(N); @@ -321,12 +322,12 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); if (settings_.projectStateInputEqualityConstraints) { - constexpr bool extractProjectionMultiplierCoefficients = false; - multiple_shooting::projectTranscription(result, extractProjectionMultiplierCoefficients); + multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplierCoefficients); } dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); constraintsProjection_[i] = std::move(result.constraintsProjection); + projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients); stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); From faa2a10ebc57dc47c87ed02684c37402199f289f Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 8 Dec 2022 10:30:44 +0100 Subject: [PATCH 422/512] add option to extract the Lagrange multiplier --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 2 +- .../ocs2_sqp/include/ocs2_sqp/SqpSettings.h | 5 +++-- ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h | 5 ++++- ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp | 1 + ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 17 ++++++++++++++++- 5 files changed, 25 insertions(+), 5 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 9bd34a16e..1835dfcc8 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -72,7 +72,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR + CF_FIX ) endif(cmake_clang_tools_FOUND) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h index 75d29508f..b6f84e6c7 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h @@ -67,8 +67,9 @@ struct Settings { // Inequality penalty relaxed barrier parameters scalar_t inequalityConstraintMu = 0.0; scalar_t inequalityConstraintDelta = 1e-6; - bool projectStateInputEqualityConstraints = true; // Use a projection method to resolve the state-input constraint Cx+Du+e - bool extractProjectionMultiplierCoefficients = false; // Extract the Lagrange multiplier of the projected state-input constraint Cx+Du+e + + bool projectStateInputEqualityConstraints = true; // Use a projection method to resolve the state-input constraint Cx+Du+e + bool extractProjectionMultiplier = false; // Extract the Lagrange multiplier of the projected state-input constraint Cx+Du+e // Printing bool printSolverStatus = false; // Print HPIPM status after solving the QP subproblem diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index db3e3b54a..265a1e698 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -174,11 +174,14 @@ class SqpSolver : public SolverBase { std::vector<VectorFunctionLinearApproximation> dynamics_; std::vector<ScalarFunctionQuadraticApproximation> cost_; std::vector<VectorFunctionLinearApproximation> constraintsProjection_; - std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_; std::vector<VectorFunctionLinearApproximation> stateInputEqConstraints_; std::vector<VectorFunctionLinearApproximation> stateIneqConstraints_; std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; + // Lagrange multipliers + vector_array_t projectionMultiplier_; + std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_; + // Iteration performance log std::vector<PerformanceIndex> performanceIndeces_; diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp index 202d339dc..0160c921b 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp @@ -66,6 +66,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, loadData::loadPtreeValue(pt, settings.inequalityConstraintMu, fieldName + ".inequalityConstraintMu", verbose); loadData::loadPtreeValue(pt, settings.inequalityConstraintDelta, fieldName + ".inequalityConstraintDelta", verbose); loadData::loadPtreeValue(pt, settings.projectStateInputEqualityConstraints, fieldName + ".projectStateInputEqualityConstraints", verbose); + loadData::loadPtreeValue(pt, settings.extractProjectionMultiplier, fieldName + ".extractProjectionMultiplier", verbose); loadData::loadPtreeValue(pt, settings.printSolverStatus, fieldName + ".printSolverStatus", verbose); loadData::loadPtreeValue(pt, settings.printSolverStatistics, fieldName + ".printSolverStatistics", verbose); loadData::loadPtreeValue(pt, settings.printLinesearch, fieldName + ".printLinesearch", verbose); diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index f6f88947b..e631c400c 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -251,6 +251,20 @@ SqpSolver::OcpSubproblemSolution SqpSolver::getOCPSolution(const vector_t& delta // remap the tilde delta u to real delta u if (settings_.projectStateInputEqualityConstraints) { + // compute Lagrange multipliers before remap + projectionMultiplier_.resize(projectionMultiplierCoefficients_.size()); + if (settings_.extractProjectionMultiplier) { + for (int i = 0; i < projectionMultiplierCoefficients_.size(); ++i) { + if (projectionMultiplierCoefficients_[i].f.size() > 0) { + const auto& dfdx = projectionMultiplierCoefficients_[i].dfdx; + const auto& dfdu = projectionMultiplierCoefficients_[i].dfdu; + const auto& f = projectionMultiplierCoefficients_[i].f; + projectionMultiplier_[i].noalias() = dfdx * deltaXSol[i] + dfdu * deltaUSol[i] + f; + } else { + projectionMultiplier_[i].resize(0); + } + } + } multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); } @@ -312,6 +326,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); stateInputEqConstraints_[i] = std::move(result.eqConstraints); stateIneqConstraints_[i] = std::move(result.ineqConstraints); stateInputIneqConstraints_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); @@ -322,7 +337,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); if (settings_.projectStateInputEqualityConstraints) { - multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplierCoefficients); + multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplier); } dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); From a648baffe8e1768e8e2e0f3484c8c4aa669fe50f Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 8 Dec 2022 10:53:46 +0100 Subject: [PATCH 423/512] fix CMakeLists and arg name --- ocs2_sqp/ocs2_sqp/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 1835dfcc8..9bd34a16e 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -72,7 +72,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) From 107c76f1b08b7aa1264a55b03ed9b7bbc23a0b56 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 8 Dec 2022 12:39:53 +0100 Subject: [PATCH 424/512] remove VectorTrajectory and modify names --- .../ocs2_oc/multiple_shooting/Transcription.h | 4 +- .../trajectory_adjustment/VectorTrajectory.h | 176 ------------------ .../src/multiple_shooting/Transcription.cpp | 4 +- 3 files changed, 4 insertions(+), 180 deletions(-) delete mode 100644 ocs2_oc/include/ocs2_oc/trajectory_adjustment/VectorTrajectory.h diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h index 9ea808566..d93d900c5 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h @@ -71,9 +71,9 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP * Apply the state-input equality constraint projection for a single intermediate node transcription. * * @param transcription : Transcription for a single intermediate node - * @param extractProjectionMultiplierCoefficients + * @param extractProjectionMultiplier */ -void projectTranscription(Transcription& transcription, bool extractProjectionMultiplierCoefficients = false); +void projectTranscription(Transcription& transcription, bool extractProjectionMultiplier = false); /** * Results of the transcription at a terminal node diff --git a/ocs2_oc/include/ocs2_oc/trajectory_adjustment/VectorTrajectory.h b/ocs2_oc/include/ocs2_oc/trajectory_adjustment/VectorTrajectory.h deleted file mode 100644 index e94257c9d..000000000 --- a/ocs2_oc/include/ocs2_oc/trajectory_adjustment/VectorTrajectory.h +++ /dev/null @@ -1,176 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#pragma once - -#include <ocs2_core/Types.h> -#include <ocs2_core/misc/LinearInterpolation.h> -#include <ocs2_core/reference/ModeSchedule.h> - -#include "ocs2_oc/oc_data/TimeDiscretization.h" -#include "ocs2_oc/trajectory_adjustment/TrajectorySpreading.h" - -namespace ocs2 { - -struct vector_trajectory_t { - scalar_array_t timeTrajectory; - size_array_t postEventIndices; - - vector_t final; - vector_array_t preJumps; - vector_array_t intermediates; - - /** Exchanges the content of trajectory */ - void swap(vector_trajectory_t& other) { - timeTrajectory.swap(other.timeTrajectory); - postEventIndices.swap(other.postEventIndices); - final.swap(other.final); - preJumps.swap(other.preJumps); - intermediates.swap(other.intermediates); - } - - /** Clears the content of the trajectory */ - void clear() { - timeTrajectory.clear(); - postEventIndices.clear(); - final = vector_t(); - preJumps.clear(); - intermediates.clear(); - } - - /** Convert a vector array to a vector trajectory */ - static vector_trajectory_t fromVectorArray(const std::vector<AnnotatedTime>& time, vector_array_t&& vector_array) { - vector_trajectory_t result; - - // Problem horizon - const int N = static_cast<int>(time.size()) - 1; - - for (int i = 0; i < N; ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent) { - result.preJumps.push_back(std::move(vector_array[i])); - } else { - result.intermediates.push_back(std::move(vector_array[i])); - } - } - result.final = std::move(vector_array[N]); - - return result; - } - - /** Convert a vector trajectory to a vector array */ - static vector_array_t toVectorArray(const std::vector<AnnotatedTime>& time, vector_trajectory_t&& vector_trajectory) { - vector_array_t result; - - // Problem horizon - const int N = static_cast<int>(time.size()) - 1; - result.reserve(N + 1); - - int intermediateIdx = 0; - int preJumpIdx = 0; - for (int i = 0; i < N; ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent) { - result.push_back(std::move(vector_trajectory.preJumps[preJumpIdx])); - ++preJumpIdx; - } else { - result.push_back(std::move(vector_trajectory.intermediates[intermediateIdx])); - ++intermediateIdx; - } - } - result.push_back(std::move(vector_trajectory.final)); - - return result; - } -}; - -/** - * Calculates the intermediate vector value at the given time. - * - * @param [in] trajectory: The trajectory - * @param [in] time: The inquiry time - * @return The vector value at the given time. - */ -inline vector_t getIntermediateValueAtTime(const vector_trajectory_t& trajectory, scalar_t time) { - const auto indexAlpha = LinearInterpolation::timeSegment(time, trajectory.timeTrajectory); - return LinearInterpolation::interpolate(indexAlpha, trajectory.intermediates); -} - -/** - * Adjusts in-place a vector trajectory based on the last changes in mode schedule using a TrajectorySpreading strategy. - * - * @param [in] oldModeSchedule: The old mode schedule associated to the trajectories which should be adjusted. - * @param [in] newModeSchedule: The new mode schedule that should be adapted to. - * @param [in, out] trajectory: The dual solution that is associated with the old mode schedule. - * @returns the status of the devised trajectory spreading strategy. - */ -inline TrajectorySpreading::Status trajectorySpread(const ModeSchedule& oldModeSchedule, const ModeSchedule& newModeSchedule, - vector_trajectory_t& trajectory) { - // trajectory spreading - constexpr bool debugPrint = false; - TrajectorySpreading trajectorySpreading(debugPrint); - const auto status = trajectorySpreading.set(oldModeSchedule, newModeSchedule, trajectory.timeTrajectory); - - // adjust final, pre-jump, intermediate, time, postEventIndices - if (status.willTruncate) { - trajectory.final = vector_t(); - } - trajectory.preJumps = trajectorySpreading.extractEventsArray(trajectory.preJumps); - trajectorySpreading.adjustTrajectory(trajectory.intermediates); - trajectorySpreading.adjustTimeTrajectory(trajectory.timeTrajectory); - trajectory.postEventIndices = trajectorySpreading.getPostEventIndices(); - - return status; -} - -/** - * Adjusts a vector trajectory based on the last changes in mode schedule using a TrajectorySpreading strategy. - * - * @param [in] trajectorySpreading: An updated trajectorySpreading instance. In order to update trajectorySpreading - * call TrajectorySpreading::set. - * @param [in] oldDualSolution: The dual solution that is associated with the old mode schedule. - * @param [out] newDualSolution: The updated dual solution that is associated with the new mode schedule. - */ -inline void trajectorySpread(const TrajectorySpreading& trajectorySpreading, const vector_trajectory_t& oldTrajectory, - vector_trajectory_t& newTrajectory) { - newTrajectory.clear(); - - // adjust time and postEventIndices - newTrajectory.timeTrajectory = oldTrajectory.timeTrajectory; - trajectorySpreading.adjustTimeTrajectory(newTrajectory.timeTrajectory); - newTrajectory.postEventIndices = trajectorySpreading.getPostEventIndices(); - - // adjust final, pre-jump and intermediate - if (!trajectorySpreading.getStatus().willTruncate) { - newTrajectory.final = oldTrajectory.final; - } - newTrajectory.preJumps = trajectorySpreading.extractEventsArray(oldTrajectory.preJumps); - newTrajectory.intermediates = oldTrajectory.intermediates; - trajectorySpreading.adjustTrajectory(newTrajectory.intermediates); -} - -} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/Transcription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp index 85d1215e3..3e8489c89 100644 --- a/ocs2_oc/src/multiple_shooting/Transcription.cpp +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -83,7 +83,7 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP return transcription; } -void projectTranscription(Transcription& transcription, bool extractEqualityConstraintsPseudoInverse) { +void projectTranscription(Transcription& transcription, bool extractProjectionMultiplier) { auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; auto& projection = transcription.constraintsProjection; @@ -93,7 +93,7 @@ void projectTranscription(Transcription& transcription, bool extractEqualityCons if (stateInputEqConstraints.f.size() > 0) { // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. - if (extractEqualityConstraintsPseudoInverse) { + if (extractProjectionMultiplier) { matrix_t constraintPseudoInverse; std::tie(projection, constraintPseudoInverse) = LinearAlgebra::qrConstraintProjection(stateInputEqConstraints); projectionMultiplierCoefficients.compute(dynamics, cost, projection, constraintPseudoInverse); From 8c77ab2590c4b895f827fb7b3e490099ccfed29a Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 8 Dec 2022 12:41:19 +0100 Subject: [PATCH 425/512] remove projection multiplier computation --- ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h | 1 - ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 14 -------------- 2 files changed, 15 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index 265a1e698..991ec9b28 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -179,7 +179,6 @@ class SqpSolver : public SolverBase { std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; // Lagrange multipliers - vector_array_t projectionMultiplier_; std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_; // Iteration performance log diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index e631c400c..3113e7b90 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -251,20 +251,6 @@ SqpSolver::OcpSubproblemSolution SqpSolver::getOCPSolution(const vector_t& delta // remap the tilde delta u to real delta u if (settings_.projectStateInputEqualityConstraints) { - // compute Lagrange multipliers before remap - projectionMultiplier_.resize(projectionMultiplierCoefficients_.size()); - if (settings_.extractProjectionMultiplier) { - for (int i = 0; i < projectionMultiplierCoefficients_.size(); ++i) { - if (projectionMultiplierCoefficients_[i].f.size() > 0) { - const auto& dfdx = projectionMultiplierCoefficients_[i].dfdx; - const auto& dfdu = projectionMultiplierCoefficients_[i].dfdu; - const auto& f = projectionMultiplierCoefficients_[i].f; - projectionMultiplier_[i].noalias() = dfdx * deltaXSol[i] + dfdu * deltaUSol[i] + f; - } else { - projectionMultiplier_[i].resize(0); - } - } - } multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); } From 4f85fce760c3ca1abaf584c53ca431d34d9600e4 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 8 Dec 2022 13:33:18 +0100 Subject: [PATCH 426/512] fix linTraget --- ocs2_oc/src/lintTarget.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 0f7752891..f3485a8b7 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -80,7 +80,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. // trajectory_adjustment #include <ocs2_oc/trajectory_adjustment/TrajectorySpreading.h> #include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h> -#include <ocs2_oc/trajectory_adjustment/VectorTrajectory.h> // dummy target for clang toolchain int main() { From 6e8bd2dc94f2877bdbf1257e8f17a5fe17fe3df4 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 8 Dec 2022 16:23:10 +0100 Subject: [PATCH 427/512] do not setting stateInputEqConstraints_ for event times. Changing the order of input arguments in ProjectionMultiplierCoefficients::compute. removing unused projectStateInputEqualityConstraints --- .../ProjectionMultiplierCoefficients.h | 14 +++++------ .../ocs2_oc/multiple_shooting/Transcription.h | 10 ++++---- .../ProjectionMultiplierCoefficients.cpp | 4 ++-- .../src/multiple_shooting/Transcription.cpp | 10 ++++---- ocs2_slp/include/ocs2_slp/SlpSettings.h | 3 +++ ocs2_slp/include/ocs2_slp/SlpSolver.h | 8 +++++-- ocs2_slp/src/SlpSettings.cpp | 1 + ocs2_slp/src/SlpSolver.cpp | 23 +++++++++--------- .../ocs2_sqp/include/ocs2_sqp/SqpSolver.h | 4 ++-- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 24 +++++++++---------- 10 files changed, 54 insertions(+), 47 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h index 2ebe20ff9..10e378c78 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h @@ -36,7 +36,7 @@ namespace multiple_shooting { /** * Coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint such that - * dfdx*dx + dfdu*du + dfdcostate*dcostate + f + * dfdx * dx + dfdu * du + dfdcostate * dcostate + f */ struct ProjectionMultiplierCoefficients { matrix_t dfdx; @@ -45,14 +45,14 @@ struct ProjectionMultiplierCoefficients { vector_t f; /** - * Extracts the coefficients of the Lagrange multiplier associated with the state-input equality constraint. + * Computes the coefficients of the Lagrange multiplier associated with the state-input equality constraint. * - * @param dynamics : Dynamics - * @param cost : Cost - * @param constraintProjection : Constraint projection. - * @param pseudoInverse : Left pseudo-inverse of D^T of the state-input equality constraint. + * @param cost : The cost quadratic approximation. + * @param dynamics : The dynamics linear approximation. + * @param constraintProjection : The constraint projection. + * @param pseudoInverse : Left pseudo-inverse of D^T of the state-input linearized equality constraint (C dx + D du + e = 0). */ - void compute(const VectorFunctionLinearApproximation& dynamics, const ScalarFunctionQuadraticApproximation& cost, + void compute(const ScalarFunctionQuadraticApproximation& cost, const VectorFunctionLinearApproximation& dynamics, const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse); }; diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h index d93d900c5..ad461532f 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h @@ -42,13 +42,13 @@ namespace multiple_shooting { * Results of the transcription at an intermediate node */ struct Transcription { - VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; - ProjectionMultiplierCoefficients projectionMultiplierCoefficients; - VectorFunctionLinearApproximation constraintsProjection; + VectorFunctionLinearApproximation dynamics; VectorFunctionLinearApproximation stateInputEqConstraints; VectorFunctionLinearApproximation stateIneqConstraints; VectorFunctionLinearApproximation stateInputIneqConstraints; + VectorFunctionLinearApproximation constraintsProjection; + ProjectionMultiplierCoefficients projectionMultiplierCoefficients; }; /** @@ -71,7 +71,7 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP * Apply the state-input equality constraint projection for a single intermediate node transcription. * * @param transcription : Transcription for a single intermediate node - * @param extractProjectionMultiplier + * @param extractProjectionMultiplier : Whether to extract the projection multiplier. */ void projectTranscription(Transcription& transcription, bool extractProjectionMultiplier = false); @@ -98,8 +98,8 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont * Results of the transcription at an event */ struct EventTranscription { - VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; + VectorFunctionLinearApproximation dynamics; VectorFunctionLinearApproximation eqConstraints; VectorFunctionLinearApproximation ineqConstraints; }; diff --git a/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp b/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp index 2508bcc7a..d158a4ea2 100644 --- a/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp +++ b/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp @@ -32,8 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { -void ProjectionMultiplierCoefficients::compute(const VectorFunctionLinearApproximation& dynamics, - const ScalarFunctionQuadraticApproximation& cost, +void ProjectionMultiplierCoefficients::compute(const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& dynamics, const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse) { vector_t semiprojectedCost_dfdu = cost.dfdu; diff --git a/ocs2_oc/src/multiple_shooting/Transcription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp index 3e8489c89..f24ed46fb 100644 --- a/ocs2_oc/src/multiple_shooting/Transcription.cpp +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -84,19 +84,19 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP } void projectTranscription(Transcription& transcription, bool extractProjectionMultiplier) { - auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; - auto& projection = transcription.constraintsProjection; - auto& projectionMultiplierCoefficients = transcription.projectionMultiplierCoefficients; + auto& dynamics = transcription.dynamics; auto& stateInputEqConstraints = transcription.stateInputEqConstraints; auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; + auto& projection = transcription.constraintsProjection; + auto& projectionMultiplierCoefficients = transcription.projectionMultiplierCoefficients; if (stateInputEqConstraints.f.size() > 0) { // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. if (extractProjectionMultiplier) { matrix_t constraintPseudoInverse; std::tie(projection, constraintPseudoInverse) = LinearAlgebra::qrConstraintProjection(stateInputEqConstraints); - projectionMultiplierCoefficients.compute(dynamics, cost, projection, constraintPseudoInverse); + projectionMultiplierCoefficients.compute(cost, dynamics, projection, constraintPseudoInverse); } else { projection = LinearAlgebra::luConstraintProjection(stateInputEqConstraints).first; projectionMultiplierCoefficients = ProjectionMultiplierCoefficients(); @@ -144,8 +144,8 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro const vector_t& x_next) { // Results and short-hand notation EventTranscription transcription; - auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; + auto& dynamics = transcription.dynamics; auto& eqConstraints = transcription.eqConstraints; auto& ineqConstraints = transcription.ineqConstraints; diff --git a/ocs2_slp/include/ocs2_slp/SlpSettings.h b/ocs2_slp/include/ocs2_slp/SlpSettings.h index 12c0429c2..766505100 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSettings.h +++ b/ocs2_slp/include/ocs2_slp/SlpSettings.h @@ -62,6 +62,9 @@ struct Settings { scalar_t inequalityConstraintMu = 0.0; scalar_t inequalityConstraintDelta = 1e-6; + // Extract the Lagrange multiplier of the projected state-input constraint Cx+Du+e + bool extractProjectionMultiplier = false; + // Printing bool printSolverStatus = false; // Print HPIPM status after solving the QP subproblem bool printSolverStatistics = false; // Print benchmarking of the multiple shooting method diff --git a/ocs2_slp/include/ocs2_slp/SlpSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h index f6bea863d..8d91488b4 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSolver.h +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/misc/Benchmark.h> #include <ocs2_core/thread_support/ThreadPool.h> +#include <ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h> #include <ocs2_oc/oc_data/TimeDiscretization.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> #include <ocs2_oc/oc_solver/SolverBase.h> @@ -167,12 +168,15 @@ class SlpSolver : public SolverBase { PrimalSolution primalSolution_; // LQ approximation - std::vector<VectorFunctionLinearApproximation> dynamics_; std::vector<ScalarFunctionQuadraticApproximation> cost_; - std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + std::vector<VectorFunctionLinearApproximation> dynamics_; std::vector<VectorFunctionLinearApproximation> stateInputEqConstraints_; std::vector<VectorFunctionLinearApproximation> stateIneqConstraints_; std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; + std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + + // Lagrange multipliers + std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_; // Iteration performance log std::vector<PerformanceIndex> performanceIndeces_; diff --git a/ocs2_slp/src/SlpSettings.cpp b/ocs2_slp/src/SlpSettings.cpp index 1980f4849..8e0f4a550 100644 --- a/ocs2_slp/src/SlpSettings.cpp +++ b/ocs2_slp/src/SlpSettings.cpp @@ -66,6 +66,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, settings.integratorType = sensitivity_integrator::fromString(integratorName); loadData::loadPtreeValue(pt, settings.inequalityConstraintMu, fieldName + ".inequalityConstraintMu", verbose); loadData::loadPtreeValue(pt, settings.inequalityConstraintDelta, fieldName + ".inequalityConstraintDelta", verbose); + loadData::loadPtreeValue(pt, settings.extractProjectionMultiplier, fieldName + ".extractProjectionMultiplier", verbose); loadData::loadPtreeValue(pt, settings.printSolverStatus, fieldName + ".printSolverStatus", verbose); loadData::loadPtreeValue(pt, settings.printSolverStatistics, fieldName + ".printSolverStatistics", verbose); loadData::loadPtreeValue(pt, settings.printLinesearch, fieldName + ".printLinesearch", verbose); diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 8131af966..aedeb38b8 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -294,12 +294,13 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated const int N = static_cast<int>(time.size()) - 1; std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); - dynamics_.resize(N); cost_.resize(N + 1); - constraintsProjection_.resize(N); - stateInputEqConstraints_.resize(N + 1); + dynamics_.resize(N); + stateInputEqConstraints_.resize(N); stateIneqConstraints_.resize(N + 1); stateInputIneqConstraints_.resize(N); + constraintsProjection_.resize(N); + projectionMultiplierCoefficients_.resize(N); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { @@ -313,26 +314,27 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); workerPerformance += multiple_shooting::computeEventPerformance(result); - dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); - constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); - stateInputEqConstraints_[i] = std::move(result.eqConstraints); + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i] = VectorFunctionLinearApproximation(0, x[i].size(), 0); stateIneqConstraints_[i] = std::move(result.ineqConstraints); stateInputIneqConstraints_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); - constexpr bool extractPseudoInverse = false; - multiple_shooting::projectTranscription(result, extractPseudoInverse); - dynamics_[i] = std::move(result.dynamics); + multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplier); cost_[i] = std::move(result.cost); - constraintsProjection_[i] = std::move(result.constraintsProjection); + dynamics_[i] = std::move(result.dynamics); stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); + constraintsProjection_[i] = std::move(result.constraintsProjection); + projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients); } i = timeIndex++; @@ -343,7 +345,6 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); workerPerformance += multiple_shooting::computeTerminalPerformance(result); cost_[i] = std::move(result.cost); - stateInputEqConstraints_[i] = std::move(result.eqConstraints); stateIneqConstraints_[i] = std::move(result.ineqConstraints); } diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index 991ec9b28..1169239df 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -171,12 +171,12 @@ class SqpSolver : public SolverBase { std::vector<ScalarFunctionQuadraticApproximation> valueFunction_; // LQ approximation - std::vector<VectorFunctionLinearApproximation> dynamics_; std::vector<ScalarFunctionQuadraticApproximation> cost_; - std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + std::vector<VectorFunctionLinearApproximation> dynamics_; std::vector<VectorFunctionLinearApproximation> stateInputEqConstraints_; std::vector<VectorFunctionLinearApproximation> stateIneqConstraints_; std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; + std::vector<VectorFunctionLinearApproximation> constraintsProjection_; // Lagrange multipliers std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_; diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 3113e7b90..b36851fca 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -288,20 +288,19 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated const int N = static_cast<int>(time.size()) - 1; std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); - dynamics_.resize(N); cost_.resize(N + 1); - constraintsProjection_.resize(N); - projectionMultiplierCoefficients_.resize(N); - stateInputEqConstraints_.resize(N + 1); + dynamics_.resize(N); + stateInputEqConstraints_.resize(N); stateIneqConstraints_.resize(N + 1); stateInputIneqConstraints_.resize(N); + constraintsProjection_.resize(N); + projectionMultiplierCoefficients_.resize(N); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; PerformanceIndex workerPerformance; // Accumulate performance in local variable - const bool projectStateInputEqualityConstraints = settings_.projectStateInputEqualityConstraints; int i = timeIndex++; while (i < N) { @@ -309,13 +308,13 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); workerPerformance += multiple_shooting::computeEventPerformance(result); - dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); - constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); - projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); - stateInputEqConstraints_[i] = std::move(result.eqConstraints); + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i] = VectorFunctionLinearApproximation(0, x[i].size(), 0); stateIneqConstraints_[i] = std::move(result.ineqConstraints); stateInputIneqConstraints_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); @@ -325,13 +324,13 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated if (settings_.projectStateInputEqualityConstraints) { multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplier); } - dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); - constraintsProjection_[i] = std::move(result.constraintsProjection); - projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients); + dynamics_[i] = std::move(result.dynamics); stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); + constraintsProjection_[i] = std::move(result.constraintsProjection); + projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients); } i = timeIndex++; @@ -342,7 +341,6 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); workerPerformance += multiple_shooting::computeTerminalPerformance(result); cost_[i] = std::move(result.cost); - stateInputEqConstraints_[i] = std::move(result.eqConstraints); stateIneqConstraints_[i] = std::move(result.ineqConstraints); } From a55bf9c8d65cbea2cd1267fa73d3b4d3106c4933 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 8 Dec 2022 16:31:52 +0100 Subject: [PATCH 428/512] clang tidy --- ocs2_slp/include/ocs2_slp/SlpSolver.h | 2 +- ocs2_slp/src/SlpSolver.cpp | 6 +++--- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 9 +++++---- 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/ocs2_slp/include/ocs2_slp/SlpSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h index 8d91488b4..2661fd14d 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSolver.h +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -174,7 +174,7 @@ class SlpSolver : public SolverBase { std::vector<VectorFunctionLinearApproximation> stateIneqConstraints_; std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; std::vector<VectorFunctionLinearApproximation> constraintsProjection_; - + // Lagrange multipliers std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_; diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index aedeb38b8..5cce3844c 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -316,10 +316,10 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated workerPerformance += multiple_shooting::computeEventPerformance(result); cost_[i] = std::move(result.cost); dynamics_[i] = std::move(result.dynamics); - stateInputEqConstraints_[i] = VectorFunctionLinearApproximation(0, x[i].size(), 0); + stateInputEqConstraints_[i].resize(0, x[i].size()); stateIneqConstraints_[i] = std::move(result.ineqConstraints); - stateInputIneqConstraints_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); - constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + stateInputIneqConstraints_[i].resize(0, x[i].size()); + constraintsProjection_[i].resize(0, x[i].size()); projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); } else { // Normal, intermediate node diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index b36851fca..5dd92c998 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -290,7 +290,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); cost_.resize(N + 1); dynamics_.resize(N); - stateInputEqConstraints_.resize(N); + stateInputEqConstraints_.resize(N + 1); // +1 because of HpipmInterface size check stateIneqConstraints_.resize(N + 1); stateInputIneqConstraints_.resize(N); constraintsProjection_.resize(N); @@ -310,10 +310,10 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated workerPerformance += multiple_shooting::computeEventPerformance(result); cost_[i] = std::move(result.cost); dynamics_[i] = std::move(result.dynamics); - stateInputEqConstraints_[i] = VectorFunctionLinearApproximation(0, x[i].size(), 0); + stateInputEqConstraints_[i].resize(0, x[i].size()); stateIneqConstraints_[i] = std::move(result.ineqConstraints); - stateInputIneqConstraints_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); - constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + stateInputIneqConstraints_[i].resize(0, x[i].size()); + constraintsProjection_[i].resize(0, x[i].size()); projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); } else { // Normal, intermediate node @@ -341,6 +341,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); workerPerformance += multiple_shooting::computeTerminalPerformance(result); cost_[i] = std::move(result.cost); + stateInputEqConstraints_[i].resize(0, x[i].size()); stateIneqConstraints_[i] = std::move(result.ineqConstraints); } From d4d0b35064987aaaa01c58f3bded035db5b3e1bf Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Thu, 8 Dec 2022 17:29:13 +0100 Subject: [PATCH 429/512] bugfix --- .../multiple_shooting/testProjectionMultiplierCoefficients.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp b/ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp index 62f231ff6..f7f573caa 100644 --- a/ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp +++ b/ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp @@ -50,7 +50,7 @@ TEST(testMultipleShootingHelpers, testProjectionMultiplierCoefficients) { const auto pseudoInverse = std::move(result.second); multiple_shooting::ProjectionMultiplierCoefficients projectionMultiplierCoefficients; - projectionMultiplierCoefficients.compute(dynamics, cost, projection, pseudoInverse); + projectionMultiplierCoefficients.compute(cost, dynamics, projection, pseudoInverse); const matrix_t dfdx = -pseudoInverse * (cost.dfdux + cost.dfduu * projection.dfdx); const matrix_t dfdu = -pseudoInverse * (cost.dfduu * projection.dfdu); From 3ce74d9e45fb34766ac8f8918a156b3c7ee24dd3 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 9 Dec 2022 11:54:12 +0100 Subject: [PATCH 430/512] bugfix --- ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp index 149f503b3..a653501e3 100644 --- a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp +++ b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp @@ -95,10 +95,10 @@ void approximatePreJumpLQ(OptimalControlProblem& problem, const scalar_t& time, modelData.time = time; modelData.stateDim = state.rows(); modelData.inputDim = 0; - modelData.dynamicsBias.setZero(state.rows()); // Jump map modelData.dynamics = problem.dynamicsPtr->jumpMapLinearApproximation(time, state, preComputation); + modelData.dynamicsBias.setZero(modelData.dynamics.dfdx.rows()); // Pre-jump cost modelData.cost = approximateEventCost(problem, time, state); From 666bc6e0ccd54f84c7f12eb09e04de9218818c5c Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 9 Dec 2022 11:56:46 +0100 Subject: [PATCH 431/512] making similar --- ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp index a653501e3..0be20fa38 100644 --- a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp +++ b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp @@ -47,11 +47,11 @@ void approximateIntermediateLQ(OptimalControlProblem& problem, const scalar_t ti modelData.time = time; modelData.stateDim = state.rows(); modelData.inputDim = input.rows(); - modelData.dynamicsBias.setZero(state.rows()); // Dynamics modelData.dynamicsCovariance = problem.dynamicsPtr->dynamicsCovariance(time, state, input); modelData.dynamics = problem.dynamicsPtr->linearApproximation(time, state, input, preComputation); + modelData.dynamicsBias.setZero(modelData.dynamics.dfdx.rows()); // Cost modelData.cost = ocs2::approximateCost(problem, time, state, input); From 7a01056be9f83523c831c7306c69aee3bd175064 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 9 Dec 2022 16:36:27 +0100 Subject: [PATCH 432/512] bugfix --- .../include/ocs2_core/model_data/Metrics.h | 63 +++++++---- ocs2_core/src/model_data/Metrics.cpp | 102 ++++++++++-------- ocs2_core/test/model_data/testMetrics.cpp | 17 ++- ocs2_oc/CMakeLists.txt | 1 - .../ocs2_oc/multiple_shooting/Transcription.h | 1 + .../ocs2_oc/oc_data/PerformanceIndex.h | 37 ++++--- .../PerformanceIndexComputation.cpp | 51 ++++----- .../src/multiple_shooting/Transcription.cpp | 8 +- .../testTranscriptionPerformanceIndex.cpp | 4 +- 9 files changed, 169 insertions(+), 115 deletions(-) diff --git a/ocs2_core/include/ocs2_core/model_data/Metrics.h b/ocs2_core/include/ocs2_core/model_data/Metrics.h index 69872b14a..bba21fb9c 100644 --- a/ocs2_core/include/ocs2_core/model_data/Metrics.h +++ b/ocs2_core/include/ocs2_core/model_data/Metrics.h @@ -128,24 +128,49 @@ struct Metrics { bool isApprox(const Metrics& other, scalar_t prec = 1e-8) const; }; -/** Sums penalties of an array of LagrangianMetrics */ +/** Sums penalties of an array of LagrangianMetrics. */ inline scalar_t sumPenalties(const std::vector<LagrangianMetrics>& metricsArray) { scalar_t s = 0.0; std::for_each(metricsArray.begin(), metricsArray.end(), [&s](const LagrangianMetrics& m) { s += m.penalty; }); return s; } -/** Computes the sum of squared norm of constraints of an array of LagrangianMetrics */ +/** Computes the sum of squared norm of constraints of an array of LagrangianMetrics. */ inline scalar_t constraintsSquaredNorm(const std::vector<LagrangianMetrics>& metricsArray) { scalar_t s = 0.0; std::for_each(metricsArray.begin(), metricsArray.end(), [&s](const LagrangianMetrics& m) { s += m.constraint.squaredNorm(); }); return s; } -/** Computes the sum of squared norm of an array of constraint terms */ -inline scalar_t constraintsSquaredNorm(const vector_array_t& constraintArray) { +/** Computes the sum of squared norm of a vector of equality constraints violation. */ +inline scalar_t getEqConstraintsSSE(const vector_t& eqConstraint) { + if (eqConstraint.size() == 0) { + return 0.0; + } else { + return eqConstraint.squaredNorm(); + } +} + +/** Computes the sum of squared norm of a vector of inequality constraints violation. */ +inline scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { + if (ineqConstraint.size() == 0) { + return 0.0; + } else { + return ineqConstraint.cwiseMin(0.0).squaredNorm(); + } +} + +/** Computes the sum of squared norm of an array of equality constraints violation. */ +inline scalar_t getEqConstraintsSSE(const vector_array_t& eqConstraint) { + scalar_t s = 0.0; + std::for_each(eqConstraint.begin(), eqConstraint.end(), [&s](const vector_t& v) { s += getEqConstraintsSSE(v); }); + return s; +} + +/** Computes the sum of squared norm of an array of inequality constraints violation. */ +inline scalar_t getIneqConstraintsSSE(const vector_array_t& ineqConstraint) { scalar_t s = 0.0; - std::for_each(constraintArray.begin(), constraintArray.end(), [&s](const vector_t& v) { s += v.squaredNorm(); }); + std::for_each(ineqConstraint.begin(), ineqConstraint.end(), [&s](const vector_t& v) { s += getIneqConstraintsSSE(v); }); return s; } @@ -165,14 +190,6 @@ vector_t toVector(const std::vector<LagrangianMetrics>& termsLagrangianMetrics); */ vector_t toVector(const vector_array_t& constraintArray); -/** - * Gets the size of constraint terms. - * - * @ param [in] termsLagrangianMetrics : LagrangianMetrics associated to an array of constraint terms. - * @return An array of constraint terms size. It has the same size as the input array. - */ -size_array_t getSizes(const std::vector<LagrangianMetrics>& termsLagrangianMetrics); - /** * Gets the size of constraint terms. * @@ -182,14 +199,12 @@ size_array_t getSizes(const std::vector<LagrangianMetrics>& termsLagrangianMetri size_array_t getSizes(const vector_array_t& constraintArray); /** - * Deserializes the vector to an array of LagrangianMetrics structures based on size of constraint terms. + * Gets the size of constraint Lagrangian terms. * - * @param [in] termsSize : An array of constraint terms size. It as the same size as the output array. - * @param [in] vec : Serialized array of LagrangianMetrics structures of the format : - * (..., termsMultiplier[i].penalty, termsMultiplier[i].constraint, ...) - * @return An array of LagrangianMetrics structures associated to an array of constraint terms + * @ param [in] termsLagrangianMetrics : LagrangianMetrics associated to an array of constraint terms. + * @return An array of constraint terms size. It has the same size as the input array. */ -std::vector<LagrangianMetrics> toLagrangianMetrics(const size_array_t& termsSize, const vector_t& vec); +size_array_t getSizes(const std::vector<LagrangianMetrics>& termsLagrangianMetrics); /** * Deserializes the vector to an array of constraint terms. @@ -201,6 +216,16 @@ std::vector<LagrangianMetrics> toLagrangianMetrics(const size_array_t& termsSize */ vector_array_t toConstraintArray(const size_array_t& termsSize, const vector_t& vec); +/** + * Deserializes the vector to an array of LagrangianMetrics structures based on size of constraint terms. + * + * @param [in] termsSize : An array of constraint terms size. It as the same size as the output array. + * @param [in] vec : Serialized array of LagrangianMetrics structures of the format : + * (..., termsMultiplier[i].penalty, termsMultiplier[i].constraint, ...) + * @return An array of LagrangianMetrics structures associated to an array of constraint terms + */ +std::vector<LagrangianMetrics> toLagrangianMetrics(const size_array_t& termsSize, const vector_t& vec); + } // namespace ocs2 namespace ocs2 { diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 90a526a02..4b37b5260 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -43,6 +43,8 @@ bool Metrics::isApprox(const Metrics& other, scalar_t prec) const { flag = flag && this->dynamicsViolation.isApprox(other.dynamicsViolation, prec); flag = flag && toVector(this->stateEqConstraint).isApprox(toVector(other.stateEqConstraint), prec); flag = flag && toVector(this->stateInputEqConstraint).isApprox(toVector(other.stateInputEqConstraint), prec); + flag = flag && toVector(this->stateIneqConstraint).isApprox(toVector(other.stateIneqConstraint), prec); + flag = flag && toVector(this->stateInputIneqConstraint).isApprox(toVector(other.stateInputIneqConstraint), prec); flag = flag && toVector(this->stateEqLagrangian).isApprox(toVector(other.stateEqLagrangian), prec); flag = flag && toVector(this->stateIneqLagrangian).isApprox(toVector(other.stateIneqLagrangian), prec); flag = flag && toVector(this->stateInputEqLagrangian).isApprox(toVector(other.stateInputEqLagrangian), prec); @@ -89,52 +91,52 @@ vector_t toVector(const vector_array_t& constraintArray) { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -std::vector<LagrangianMetrics> toLagrangianMetrics(const size_array_t& termsSize, const vector_t& vec) { - std::vector<LagrangianMetrics> lagrangianMetrics; - lagrangianMetrics.reserve(termsSize.size()); +vector_array_t toConstraintArray(const size_array_t& termsSize, const vector_t& vec) { + vector_array_t constraintArray; + constraintArray.reserve(termsSize.size()); size_t head = 0; for (const auto& l : termsSize) { - lagrangianMetrics.emplace_back(vec(head), vec.segment(head + 1, l)); - head += 1 + l; + constraintArray.emplace_back(vec.segment(head, l)); + head += l; } // end of i loop - return lagrangianMetrics; + return constraintArray; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -vector_array_t toConstraintArray(const size_array_t& termsSize, const vector_t& vec) { - vector_array_t constraintArray; - constraintArray.reserve(termsSize.size()); +std::vector<LagrangianMetrics> toLagrangianMetrics(const size_array_t& termsSize, const vector_t& vec) { + std::vector<LagrangianMetrics> lagrangianMetrics; + lagrangianMetrics.reserve(termsSize.size()); size_t head = 0; for (const auto& l : termsSize) { - constraintArray.emplace_back(vec.segment(head, l)); - head += l; + lagrangianMetrics.emplace_back(vec(head), vec.segment(head + 1, l)); + head += 1 + l; } // end of i loop - return constraintArray; + return lagrangianMetrics; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -size_array_t getSizes(const std::vector<LagrangianMetrics>& termsLagrangianMetrics) { - size_array_t s(termsLagrangianMetrics.size()); - std::transform(termsLagrangianMetrics.begin(), termsLagrangianMetrics.end(), s.begin(), - [](const LagrangianMetrics& m) { return static_cast<size_t>(m.constraint.size()); }); +size_array_t getSizes(const vector_array_t& constraintArray) { + size_array_t s(constraintArray.size()); + std::transform(constraintArray.begin(), constraintArray.end(), s.begin(), + [](const vector_t& v) { return static_cast<size_t>(v.size()); }); return s; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -size_array_t getSizes(const vector_array_t& constraintArray) { - size_array_t s(constraintArray.size()); - std::transform(constraintArray.begin(), constraintArray.end(), s.begin(), - [](const vector_t& v) { return static_cast<size_t>(v.size()); }); +size_array_t getSizes(const std::vector<LagrangianMetrics>& termsLagrangianMetrics) { + size_array_t s(termsLagrangianMetrics.size()); + std::transform(termsLagrangianMetrics.begin(), termsLagrangianMetrics.end(), s.begin(), + [](const LagrangianMetrics& m) { return static_cast<size_t>(m.constraint.size()); }); return s; } @@ -164,12 +166,14 @@ LagrangianMetrics interpolate(const index_alpha_t& indexAlpha, const std::vector Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector<Metrics>& dataArray) { // number of terms const auto ind = indexAlpha.second > 0.5 ? indexAlpha.first : indexAlpha.first + 1; - const size_t mumStateEqConst = dataArray[ind].stateEqConstraint.size(); - const size_t mumStateInputEqCost = dataArray[ind].stateInputEqConstraint.size(); - const size_t mumStateEqLag = dataArray[ind].stateEqLagrangian.size(); - const size_t mumStateIneqLag = dataArray[ind].stateIneqLagrangian.size(); - const size_t mumStateInputEqLag = dataArray[ind].stateInputEqLagrangian.size(); - const size_t mumStateInputIneqLag = dataArray[ind].stateInputIneqLagrangian.size(); + const size_t numStateEqConst = dataArray[ind].stateEqConstraint.size(); + const size_t numStateInputEqCost = dataArray[ind].stateInputEqConstraint.size(); + const size_t numStateIneqConst = dataArray[ind].stateIneqConstraint.size(); + const size_t numStateInputIneqCost = dataArray[ind].stateInputIneqConstraint.size(); + const size_t numStateEqLag = dataArray[ind].stateEqLagrangian.size(); + const size_t numStateIneqLag = dataArray[ind].stateIneqLagrangian.size(); + const size_t numStateInputEqLag = dataArray[ind].stateInputEqLagrangian.size(); + const size_t numStateInputIneqLag = dataArray[ind].stateInputIneqLagrangian.size(); Metrics out; @@ -181,16 +185,16 @@ Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector<Metrics>& out.dynamicsViolation = interpolate( indexAlpha, dataArray, [](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].dynamicsViolation; }); - // constraints - out.stateEqConstraint.reserve(mumStateEqConst); - for (size_t i = 0; i < mumStateEqConst; i++) { + // equality constraints + out.stateEqConstraint.reserve(numStateEqConst); + for (size_t i = 0; i < numStateEqConst; ++i) { auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].stateEqConstraint[i]; }); out.stateEqConstraint.emplace_back(std::move(constraint)); } - out.stateInputEqConstraint.reserve(mumStateInputEqCost); - for (size_t i = 0; i < mumStateInputEqCost; i++) { + out.stateInputEqConstraint.reserve(numStateInputEqCost); + for (size_t i = 0; i < numStateInputEqCost; ++i) { auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint[i]; }); @@ -198,16 +202,24 @@ Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector<Metrics>& } // inequality constraints - out.stateIneqConstraint = - interpolate(indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateIneqConstraint; }); - out.stateInputIneqConstraint = interpolate( - indexAlpha, dataArray, - [](const std::vector<MetricsCollection>& array, size_t t) -> const vector_t& { return array[t].stateInputIneqConstraint; }); + out.stateIneqConstraint.reserve(numStateIneqConst); + for (size_t i = 0; i < numStateIneqConst; ++i) { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { + return array[t].stateIneqConstraint[i]; + }); + out.stateIneqConstraint.emplace_back(std::move(constraint)); + } + out.stateInputIneqConstraint.reserve(numStateInputIneqCost); + for (size_t i = 0; i < numStateInputIneqCost; ++i) { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const vector_t& { + return array[t].stateInputIneqConstraint[i]; + }); + out.stateInputIneqConstraint.emplace_back(std::move(constraint)); + } // state equality Lagrangian - out.stateEqLagrangian.reserve(mumStateEqLag); - for (size_t i = 0; i < mumStateEqLag; i++) { + out.stateEqLagrangian.reserve(numStateEqLag); + for (size_t i = 0; i < numStateEqLag; ++i) { auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].stateEqLagrangian[i].penalty; }); @@ -218,8 +230,8 @@ Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector<Metrics>& } // end of i loop // state inequality Lagrangian - out.stateIneqLagrangian.reserve(mumStateIneqLag); - for (size_t i = 0; i < mumStateIneqLag; i++) { + out.stateIneqLagrangian.reserve(numStateIneqLag); + for (size_t i = 0; i < numStateIneqLag; ++i) { auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].stateIneqLagrangian[i].penalty; }); @@ -230,8 +242,8 @@ Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector<Metrics>& } // end of i loop // state-input equality Lagrangian - out.stateInputEqLagrangian.reserve(mumStateInputEqLag); - for (size_t i = 0; i < mumStateInputEqLag; i++) { + out.stateInputEqLagrangian.reserve(numStateInputEqLag); + for (size_t i = 0; i < numStateInputEqLag; ++i) { auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].stateInputEqLagrangian[i].penalty; }); @@ -242,8 +254,8 @@ Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector<Metrics>& } // end of i loop // state-input inequality Lagrangian - out.stateInputIneqLagrangian.reserve(mumStateInputIneqLag); - for (size_t i = 0; i < mumStateInputIneqLag; i++) { + out.stateInputIneqLagrangian.reserve(numStateInputIneqLag); + for (size_t i = 0; i < numStateInputIneqLag; ++i) { auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector<Metrics>& array, size_t t) -> const scalar_t& { return array[t].stateInputIneqLagrangian[i].penalty; }); diff --git a/ocs2_core/test/model_data/testMetrics.cpp b/ocs2_core/test/model_data/testMetrics.cpp index 3c71576b5..052f31f9e 100644 --- a/ocs2_core/test/model_data/testMetrics.cpp +++ b/ocs2_core/test/model_data/testMetrics.cpp @@ -62,6 +62,14 @@ inline Metrics interpolateNew(const LinearInterpolation::index_alpha_t& indexAlp const auto rhs_stateInputEqConst = toVector(dataArray[index + 1].stateInputEqConstraint); areSameSize = areSameSize && (lhs_stateInputEqConst.size() == rhs_stateInputEqConst.size()); + const auto lhs_stateIneqConst = toVector(dataArray[index].stateIneqConstraint); + const auto rhs_stateIneqConst = toVector(dataArray[index + 1].stateIneqConstraint); + areSameSize = areSameSize && (lhs_stateIneqConst.size() == rhs_stateIneqConst.size()); + + const auto lhs_stateInputIneqConst = toVector(dataArray[index].stateInputIneqConstraint); + const auto rhs_stateInputIneqConst = toVector(dataArray[index + 1].stateInputIneqConstraint); + areSameSize = areSameSize && (lhs_stateInputIneqConst.size() == rhs_stateInputIneqConst.size()); + const auto lhs_stateEqLag = toVector(dataArray[index].stateEqLagrangian); const auto rhs_stateEqLag = toVector(dataArray[index + 1].stateEqLagrangian); areSameSize = areSameSize && (lhs_stateEqLag.size() == rhs_stateEqLag.size()); @@ -87,9 +95,12 @@ inline Metrics interpolateNew(const LinearInterpolation::index_alpha_t& indexAlp // dynamics violation out.dynamicsViolation = LinearInterpolation::interpolate( indexAlpha, dataArray, [](const std::vector<Metrics>& array, size_t t) -> const vector_t& { return array[t].dynamicsViolation; }); - // constraints + // equality constraints out.stateEqConstraint = toConstraintArray(getSizes(dataArray[index].stateEqConstraint), f(lhs_stateEqConst, rhs_stateEqConst)); out.stateInputEqConstraint = toConstraintArray(getSizes(dataArray[index].stateInputEqConstraint), f(lhs_stateInputEqConst, rhs_stateInputEqConst)); + // inequality constraints + out.stateIneqConstraint = toConstraintArray(getSizes(dataArray[index].stateIneqConstraint), f(lhs_stateIneqConst, rhs_stateIneqConst)); + out.stateInputIneqConstraint = toConstraintArray(getSizes(dataArray[index].stateInputIneqConstraint), f(lhs_stateInputIneqConst, rhs_stateInputIneqConst)); // lagrangian out.stateEqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateEqLagrangian), f(lhs_stateEqLag, rhs_stateEqLag)); out.stateIneqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateIneqLagrangian), f(lhs_stateIneqLag, rhs_stateIneqLag)); @@ -145,6 +156,8 @@ TEST(TestMetrics, testSwap) { termsMetrics.dynamicsViolation.setRandom(2); ocs2::random(termsSize, termsMetrics.stateEqConstraint); ocs2::random(termsSize, termsMetrics.stateInputEqConstraint); + ocs2::random(termsSize, termsMetrics.stateIneqConstraint); + ocs2::random(termsSize, termsMetrics.stateInputIneqConstraint); ocs2::random(termsSize, termsMetrics.stateEqLagrangian); ocs2::random(termsSize, termsMetrics.stateIneqLagrangian); ocs2::random(termsSize, termsMetrics.stateInputEqLagrangian); @@ -181,6 +194,8 @@ TEST(TestMetrics, testInterpolation) { metricsTrajectory[i].dynamicsViolation.setRandom(2); ocs2::random(stateEqTermsSize, metricsTrajectory[i].stateEqConstraint); ocs2::random(stateInputEqTermsSize, metricsTrajectory[i].stateInputEqConstraint); + ocs2::random(stateIneqTermsSize, metricsTrajectory[i].stateIneqConstraint); + ocs2::random(stateInputIneqTermsSize, metricsTrajectory[i].stateInputIneqConstraint); ocs2::random(stateEqTermsSize, metricsTrajectory[i].stateEqLagrangian); ocs2::random(stateIneqTermsSize, metricsTrajectory[i].stateIneqLagrangian); ocs2::random(stateInputEqTermsSize, metricsTrajectory[i].stateInputEqLagrangian); diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 36ef894bd..68a2eefcf 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -83,7 +83,6 @@ target_link_libraries(${PROJECT_NAME} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h index ad461532f..168322fca 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h @@ -44,6 +44,7 @@ namespace multiple_shooting { struct Transcription { ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation dynamics; + VectorFunctionLinearApproximation stateEqConstraints; VectorFunctionLinearApproximation stateInputEqConstraints; VectorFunctionLinearApproximation stateIneqConstraints; VectorFunctionLinearApproximation stateInputIneqConstraints; diff --git a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index 83719e755..3e0e65dc5 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -48,6 +48,13 @@ struct PerformanceIndex { /** The total cost of a rollout. */ scalar_t cost = 0.0; + /** Sum of Squared Error (SSE) of the dual feasibilities: + * - Final: squared norm of violation in the dual feasibilities + * - PreJumps: sum of squared norm of violation in the dual feasibilities + * - Intermediates: sum of squared norm of violation in the dual feasibilities + */ + scalar_t dualFeasibilitiesSSE = 0.0; + /** Sum of Squared Error (SSE) of system dynamics violation */ scalar_t dynamicsViolationSSE = 0.0; @@ -65,13 +72,6 @@ struct PerformanceIndex { */ scalar_t inequalityConstraintsSSE = 0.0; - /** Sum of Squared Error (SSE) of the dual feasibilities: - * - Final: squared norm of violation in the dual feasibilities - * - PreJumps: sum of squared norm of violation in the dual feasibilities - * - Intermediates: sum of squared norm of violation in the dual feasibilities - */ - scalar_t dualFeasibilitiesSSE = 0.0; - /** Sum of equality Lagrangians: * - Final: penalty for violation in state equality constraints * - PreJumps: penalty for violation in state equality constraints @@ -90,10 +90,10 @@ struct PerformanceIndex { PerformanceIndex& operator+=(const PerformanceIndex& rhs) { this->merit += rhs.merit; this->cost += rhs.cost; + this->dualFeasibilitiesSSE += rhs.dualFeasibilitiesSSE; this->dynamicsViolationSSE += rhs.dynamicsViolationSSE; this->equalityConstraintsSSE += rhs.equalityConstraintsSSE; this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE; - this->dualFeasibilitiesSSE += rhs.dualFeasibilitiesSSE; this->equalityLagrangian += rhs.equalityLagrangian; this->inequalityLagrangian += rhs.inequalityLagrangian; return *this; @@ -104,8 +104,10 @@ struct PerformanceIndex { PerformanceIndex& operator*=(const SCALAR_T c) { this->merit *= static_cast<scalar_t>(c); this->cost *= static_cast<scalar_t>(c); + this->dualFeasibilitiesSSE *= static_cast<scalar_t>(c); this->dynamicsViolationSSE *= static_cast<scalar_t>(c); this->equalityConstraintsSSE *= static_cast<scalar_t>(c); + this->inequalityConstraintsSSE *= static_cast<scalar_t>(c); this->equalityLagrangian *= static_cast<scalar_t>(c); this->inequalityLagrangian *= static_cast<scalar_t>(c); return *this; @@ -118,8 +120,10 @@ struct PerformanceIndex { }; bool isEqual = fuzzyCompares(this->merit, other.merit); isEqual = isEqual && fuzzyCompares(this->cost, other.cost); + isEqual = isEqual && fuzzyCompares(this->dualFeasibilitiesSSE, other.dualFeasibilitiesSSE); isEqual = isEqual && fuzzyCompares(this->dynamicsViolationSSE, other.dynamicsViolationSSE); isEqual = isEqual && fuzzyCompares(this->equalityConstraintsSSE, other.equalityConstraintsSSE); + isEqual = isEqual && fuzzyCompares(this->inequalityConstraintsSSE, other.inequalityConstraintsSSE); isEqual = isEqual && fuzzyCompares(this->equalityLagrangian, other.equalityLagrangian); isEqual = isEqual && fuzzyCompares(this->inequalityLagrangian, other.inequalityLagrangian); return isEqual; @@ -147,10 +151,10 @@ PerformanceIndex operator*(const SCALAR_T c, PerformanceIndex rhs) { inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { std::swap(lhs.merit, rhs.merit); std::swap(lhs.cost, rhs.cost); + std::swap(lhs.dualFeasibilitiesSSE, rhs.dualFeasibilitiesSSE); std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE); std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE); std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE); - std::swap(lhs.dualFeasibilitiesSSE, rhs.dualFeasibilitiesSSE); std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian); std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian); } @@ -165,12 +169,12 @@ inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& pe stream << "Rollout Cost: " << std::setw(tabSpace) << performanceIndex.cost << '\n'; stream << std::setw(indentation) << ""; - stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE; - stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE << '\n'; + stream << "Dual feasibilities SSE: " << std::setw(tabSpace) << performanceIndex.dualFeasibilitiesSSE; + stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE << '\n'; stream << std::setw(indentation) << ""; - stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE; - stream << "Dual feasibilities SSE: " << std::setw(tabSpace) << performanceIndex.dualFeasibilitiesSSE << '\n'; + stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE; + stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE << '\n'; stream << std::setw(indentation) << ""; stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian; @@ -184,8 +188,11 @@ inline PerformanceIndex toPerformanceIndex(const Metrics& m) { PerformanceIndex performanceIndex; performanceIndex.merit = 0.0; // left for the solver to fill performanceIndex.cost = m.cost; - performanceIndex.dynamicsViolationSSE = m.dynamicsViolation.squaredNorm(); - performanceIndex.equalityConstraintsSSE = constraintsSquaredNorm(m.stateEqConstraint) + constraintsSquaredNorm(m.stateInputEqConstraint); + performanceIndex.dualFeasibilitiesSSE = 0.0; // left for the solver to fill + performanceIndex.dynamicsViolationSSE = getEqConstraintsSSE(m.dynamicsViolation); + performanceIndex.equalityConstraintsSSE = getEqConstraintsSSE(m.stateEqConstraint) + getEqConstraintsSSE(m.stateInputEqConstraint); + performanceIndex.inequalityConstraintsSSE = + getIneqConstraintsSSE(m.stateIneqConstraint) + getIneqConstraintsSSE(m.stateInputIneqConstraint); performanceIndex.equalityLagrangian = sumPenalties(m.stateEqLagrangian) + sumPenalties(m.stateInputEqLagrangian); performanceIndex.inequalityLagrangian = sumPenalties(m.stateIneqLagrangian) + sumPenalties(m.stateInputIneqLagrangian); return performanceIndex; diff --git a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp index 5e8ce8e1b..3891b2fdc 100644 --- a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp +++ b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp @@ -30,28 +30,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_oc/multiple_shooting/PerformanceIndexComputation.h" #include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> +#include "ocs2_core/model_data/Metrics.h" namespace ocs2 { namespace multiple_shooting { -namespace { -scalar_t getEqConstraintsSSE(const vector_t& eqConstraint) { - if (eqConstraint.size() == 0) { - return 0.0; - } else { - return eqConstraint.squaredNorm(); - } -} - -scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { - if (ineqConstraint.size() == 0) { - return 0.0; - } else { - return ineqConstraint.cwiseMin(0.0).squaredNorm(); - } -} -} // namespace - PerformanceIndex computeIntermediatePerformance(const Transcription& transcription, scalar_t dt) { PerformanceIndex performance; @@ -62,11 +45,12 @@ PerformanceIndex computeIntermediatePerformance(const Transcription& transcripti performance.cost = transcription.cost.f; // State-input equality constraints - performance.equalityConstraintsSSE = dt * getEqConstraintsSSE(transcription.stateInputEqConstraints.f); + performance.equalityConstraintsSSE = + dt * (getEqConstraintsSSE(transcription.stateEqConstraints.f) + getEqConstraintsSSE(transcription.stateInputEqConstraints.f)); // Inequality constraints. performance.inequalityConstraintsSSE = - dt * getIneqConstraintsSSE(transcription.stateIneqConstraints.f) + dt * getIneqConstraintsSSE(transcription.stateIneqConstraints.f); + dt * (getIneqConstraintsSSE(transcription.stateIneqConstraints.f) + getIneqConstraintsSSE(transcription.stateInputIneqConstraints.f)); return performance; } @@ -87,23 +71,30 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt // Costs performance.cost = dt * computeCost(optimalControlProblem, t, x, u); + // State equality constraints + if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { + const auto stateEqConstraints = + optimalControlProblem.stateEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + performance.equalityConstraintsSSE += dt * getEqConstraintsSSE(stateEqConstraints); + } + // State-input equality constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { - const vector_t stateInputEqConstraints = + const auto stateInputEqConstraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - performance.equalityConstraintsSSE = dt * getEqConstraintsSSE(stateInputEqConstraints); + performance.equalityConstraintsSSE += dt * getEqConstraintsSSE(stateInputEqConstraints); } // State inequality constraints. if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { - const vector_t stateIneqConstraints = + const auto stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE = dt * getIneqConstraintsSSE(stateIneqConstraints); + performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints); } // State-input inequality constraints. if (!optimalControlProblem.inequalityConstraintPtr->empty()) { - const vector_t stateInputIneqConstraints = + const auto stateInputIneqConstraints = optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints); } @@ -134,13 +125,12 @@ PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimal performance.cost = computeFinalCost(optimalControlProblem, t, x); if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { - const vector_t eqConstraints = - optimalControlProblem.finalEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + const auto eqConstraints = optimalControlProblem.finalEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); performance.equalityConstraintsSSE = getEqConstraintsSSE(eqConstraints); } if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { - const vector_t ineqConstraints = + const auto ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); performance.inequalityConstraintsSSE = getIneqConstraintsSSE(ineqConstraints); } @@ -179,13 +169,12 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon performance.cost = computeEventCost(optimalControlProblem, t, x); if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { - const vector_t eqConstraints = - optimalControlProblem.preJumpEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + const auto eqConstraints = optimalControlProblem.preJumpEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); performance.equalityConstraintsSSE = getEqConstraintsSSE(eqConstraints); } if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { - const vector_t ineqConstraints = + const auto ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); performance.inequalityConstraintsSSE = getIneqConstraintsSSE(ineqConstraints); } diff --git a/ocs2_oc/src/multiple_shooting/Transcription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp index f24ed46fb..8b95289c5 100644 --- a/ocs2_oc/src/multiple_shooting/Transcription.cpp +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -44,6 +44,7 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP Transcription transcription; auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; + auto& stateEqConstraints = transcription.stateEqConstraints; auto& stateInputEqConstraints = transcription.stateInputEqConstraints; auto& stateIneqConstraints = transcription.stateIneqConstraints; auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; @@ -61,9 +62,14 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP cost = approximateCost(optimalControlProblem, t, x, u); cost *= dt; + // State equality constraints + if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { + stateEqConstraints = + optimalControlProblem.stateEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + } + // State-input equality constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { - // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} = 0 stateInputEqConstraints = optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); } diff --git a/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp b/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp index b9b9f6a48..73b2472e8 100644 --- a/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp +++ b/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp @@ -60,7 +60,7 @@ TEST(test_transcription, intermediate_performance) { const auto performance = multiple_shooting::computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); - ASSERT_TRUE(areIdentical(performance, multiple_shooting::computeIntermediatePerformance(transcription, dt))); + ASSERT_TRUE(performance.isApprox(multiple_shooting::computeIntermediatePerformance(transcription, dt), 1e-12)); } TEST(test_transcription, event_performance) { @@ -89,7 +89,7 @@ TEST(test_transcription, event_performance) { const auto transcription = multiple_shooting::setupEventNode(problem, t, x, x_next); const auto performance = multiple_shooting::computeEventPerformance(problem, t, x, x_next); - ASSERT_TRUE(performance.isApprox(multiple_shooting::computeEventPerformance(transcription). 1e-12)); + ASSERT_TRUE(performance.isApprox(multiple_shooting::computeEventPerformance(transcription), 1e-12)); } TEST(test_transcription, terminal_performance) { From 03fc4dac0f53d02ccf7e1093bccaa9f30746ace0 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 9 Dec 2022 17:19:08 +0100 Subject: [PATCH 433/512] adding multiple_shooting::MetricsComputation --- ocs2_oc/CMakeLists.txt | 1 + .../multiple_shooting/MetricsComputation.h | 75 +++++++++++ .../multiple_shooting/MetricsComputation.cpp | 123 ++++++++++++++++++ .../PerformanceIndexComputation.cpp | 102 +++------------ 4 files changed, 214 insertions(+), 87 deletions(-) create mode 100644 ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h create mode 100644 ocs2_oc/src/multiple_shooting/MetricsComputation.cpp diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 68a2eefcf..75de1f056 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -52,6 +52,7 @@ add_library(${PROJECT_NAME} src/multiple_shooting/Helpers.cpp src/multiple_shooting/Initialization.cpp src/multiple_shooting/LagrangianEvaluation.cpp + src/multiple_shooting/MetricsComputation.cpp src/multiple_shooting/PerformanceIndexComputation.cpp src/multiple_shooting/ProjectionMultiplierCoefficients.cpp src/multiple_shooting/Transcription.cpp diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h new file mode 100644 index 000000000..80da49816 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h @@ -0,0 +1,75 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_core/integration/SensitivityIntegrator.h> +#include <ocs2_core/model_data/Metrics.h> + +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" + +namespace ocs2 { +namespace multiple_shooting { + +/** + * Compute only the Metrics for a single intermediate node. + * @param optimalControlProblem : Definition of the optimal control problem + * @param discretizer : Integrator to use for creating the discrete dynamics. + * @param t : Start of the discrete interval + * @param dt : Duration of the interval + * @param x : State at start of the interval + * @param x_next : State at the end of the interval + * @param u : Input, taken to be constant across the interval. + * @return Metrics for a single intermediate node. + */ +Metrics computeIntermediateMetrics(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); + +/** + * Compute only the Metrics for the terminal node. + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the terminal node + * @param x : Terminal state + * @return Metrics for the terminal node. + */ +Metrics computeTerminalMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); + +/** + * Compute only the Metrics for the event node. + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the event node + * @param x : Pre-event state + * @param x_next : Post-event state + * @return Metrics for the event node. + */ +Metrics computeEventMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp b/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp new file mode 100644 index 000000000..41e1e99aa --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp @@ -0,0 +1,123 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_oc/multiple_shooting/MetricsComputation.h" + +#include "ocs2_oc/approximate_model/LinearQuadraticApproximator.h" + +namespace ocs2 { +namespace multiple_shooting { + +Metrics computeIntermediateMetrics(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { + Metrics metrics; + + // Dynamics + metrics.dynamicsViolation = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); + metrics.dynamicsViolation -= x_next; + + // Precomputation for other terms + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; + optimalControlProblem.preComputationPtr->request(request, t, x, u); + + // Costs + metrics.cost = dt * computeCost(optimalControlProblem, t, x, u); + + // State equality constraints + if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { + metrics.stateEqConstraint = optimalControlProblem.stateEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + } + + // State-input equality constraints + if (!optimalControlProblem.equalityConstraintPtr->empty()) { + metrics.stateInputEqConstraint = + optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); + } + + // State inequality constraints. + if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { + metrics.stateIneqConstraint = + optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + } + + // State-input inequality constraints. + if (!optimalControlProblem.inequalityConstraintPtr->empty()) { + metrics.stateInputIneqConstraint = + optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); + } + + return metrics; +} + +Metrics computeTerminalMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { + Metrics metrics; + + constexpr auto request = Request::Cost + Request::SoftConstraint; + optimalControlProblem.preComputationPtr->requestFinal(request, t, x); + + metrics.cost = computeFinalCost(optimalControlProblem, t, x); + + if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { + metrics.stateEqConstraint = optimalControlProblem.finalEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + } + + if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { + metrics.stateIneqConstraint = + optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + } + + return metrics; +} + +Metrics computeEventMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { + Metrics metrics; + + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics; + optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); + + // Dynamics + metrics.dynamicsViolation = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x) - x_next; + + metrics.cost = computeEventCost(optimalControlProblem, t, x); + + if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { + metrics.stateEqConstraint = + optimalControlProblem.preJumpEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + } + + if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { + metrics.stateIneqConstraint = + optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); + } + + return metrics; +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp index 3891b2fdc..c897c1c03 100644 --- a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp +++ b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp @@ -29,8 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_oc/multiple_shooting/PerformanceIndexComputation.h" -#include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> -#include "ocs2_core/model_data/Metrics.h" +#include <ocs2_core/model_data/Metrics.h> + +#include "ocs2_oc/approximate_model/LinearQuadraticApproximator.h" +#include "ocs2_oc/multiple_shooting/MetricsComputation.h" namespace ocs2 { namespace multiple_shooting { @@ -57,49 +59,13 @@ PerformanceIndex computeIntermediatePerformance(const Transcription& transcripti PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { - PerformanceIndex performance; - - // Dynamics - vector_t dynamicsGap = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); - dynamicsGap -= x_next; - performance.dynamicsViolationSSE = dt * dynamicsGap.squaredNorm(); - - // Precomputation for other terms - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; - optimalControlProblem.preComputationPtr->request(request, t, x, u); - - // Costs - performance.cost = dt * computeCost(optimalControlProblem, t, x, u); - - // State equality constraints - if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { - const auto stateEqConstraints = - optimalControlProblem.stateEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.equalityConstraintsSSE += dt * getEqConstraintsSSE(stateEqConstraints); - } - - // State-input equality constraints - if (!optimalControlProblem.equalityConstraintPtr->empty()) { - const auto stateInputEqConstraints = - optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - performance.equalityConstraintsSSE += dt * getEqConstraintsSSE(stateInputEqConstraints); - } - - // State inequality constraints. - if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { - const auto stateIneqConstraints = - optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateIneqConstraints); - } - - // State-input inequality constraints. - if (!optimalControlProblem.inequalityConstraintPtr->empty()) { - const auto stateInputIneqConstraints = - optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE += dt * getIneqConstraintsSSE(stateInputIneqConstraints); - } - - return performance; + const auto metrics = computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); + auto performanceIndex = toPerformanceIndex(metrics); + // performanceIndex.cost *= dt no need since it is already considered in computeIntermediateMetrics() + performanceIndex.dynamicsViolationSSE *= dt; + performanceIndex.equalityConstraintsSSE *= dt; + performanceIndex.inequalityConstraintsSSE *= dt; + return performanceIndex; } PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcription) { @@ -117,25 +83,8 @@ PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcr } PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { - PerformanceIndex performance; - - constexpr auto request = Request::Cost + Request::SoftConstraint; - optimalControlProblem.preComputationPtr->requestFinal(request, t, x); - - performance.cost = computeFinalCost(optimalControlProblem, t, x); - - if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { - const auto eqConstraints = optimalControlProblem.finalEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.equalityConstraintsSSE = getEqConstraintsSSE(eqConstraints); - } - - if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { - const auto ineqConstraints = - optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE = getIneqConstraintsSSE(ineqConstraints); - } - - return performance; + const auto metrics = computeTerminalMetrics(optimalControlProblem, t, x); + return toPerformanceIndex(metrics); } PerformanceIndex computeEventPerformance(const EventTranscription& transcription) { @@ -157,29 +106,8 @@ PerformanceIndex computeEventPerformance(const EventTranscription& transcription PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { - PerformanceIndex performance; - - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics; - optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); - - // Dynamics - const vector_t dynamicsGap = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x) - x_next; - performance.dynamicsViolationSSE = dynamicsGap.squaredNorm(); - - performance.cost = computeEventCost(optimalControlProblem, t, x); - - if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { - const auto eqConstraints = optimalControlProblem.preJumpEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.equalityConstraintsSSE = getEqConstraintsSSE(eqConstraints); - } - - if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { - const auto ineqConstraints = - optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - performance.inequalityConstraintsSSE = getIneqConstraintsSSE(ineqConstraints); - } - - return performance; + const auto metrics = computeEventMetrics(optimalControlProblem, t, x, x_next); + return toPerformanceIndex(metrics); } } // namespace multiple_shooting From 63240c57384acf67ced37dad950a2bc53d45ff0b Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 12 Dec 2022 14:04:02 +0100 Subject: [PATCH 434/512] using LinearQuadraticApproximator functions in multiple_shooting::MetricsComputation --- .../LinearQuadraticApproximator.h | 48 ++++++++- .../multiple_shooting/MetricsComputation.h | 24 ++--- .../PerformanceIndexComputation.h | 8 +- .../LinearQuadraticApproximator.cpp | 99 ++++++++++++++++--- .../multiple_shooting/MetricsComputation.cpp | 84 ++++------------ .../PerformanceIndexComputation.cpp | 10 +- 6 files changed, 169 insertions(+), 104 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h index 1cc49908c..9c4074231 100644 --- a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h +++ b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h @@ -160,6 +160,22 @@ scalar_t computeFinalCost(const OptimalControlProblem& problem, const scalar_t& ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlProblem& problem, const scalar_t& time, const vector_t& state); +/** + * Compute the intermediate-time Metrics (i.e. cost, softConstraints, and constraints). + * + * @note It is assumed that the precomputation request is already made. + * problem.preComputationPtr->request(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x, u) + * + * @param [in] problem: The optimal control probelm + * @param [in] time: The current time. + * @param [in] state: The current state. + * @param [in] input: The current input. + * @param [in] dynamicsViolation: The violation of dynamics. It depends on the transcription method. + * @return The output Metrics. + */ +Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, + vector_t&& dynamicsViolation = vector_t()); + /** * Compute the intermediate-time Metrics (i.e. cost, softConstraints, and constraints). * @@ -175,7 +191,22 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr * @return The output Metrics. */ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, - const MultiplierCollection& multipliers, const vector_t& dynamicsViolation = vector_t()); + const MultiplierCollection& multipliers, vector_t&& dynamicsViolation = vector_t()); + +/** + * Compute the event-time Metrics based on pre-jump state value (i.e. cost, softConstraints, and constraints). + * + * @note It is assumed that the precomputation request is already made. + * problem.preComputationPtr->requestPreJump(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x) + * + * @param [in] problem: The optimal control probelm + * @param [in] time: The current time. + * @param [in] state: The current state. + * @param [in] dynamicsViolation: The violation of dynamics. It depends on the transcription method. + * @return The output Metrics. + */ +Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + vector_t&& dynamicsViolation = vector_t()); /** * Compute the event-time Metrics based on pre-jump state value (i.e. cost, softConstraints, and constraints). @@ -191,7 +222,20 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_ * @return The output Metrics. */ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers, const vector_t& dynamicsViolation = vector_t()); + const MultiplierCollection& multipliers, vector_t&& dynamicsViolation = vector_t()); + +/** + * Compute the final-time Metrics (i.e. cost, softConstraints, and constraints). + * + * @note It is assumed that the precomputation request is already made. + * problem.preComputationPtr->requestFinal(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x) + * + * @param [in] problem: The optimal control probelm + * @param [in] time: The current time. + * @param [in] state: The current state. + * @return The output Metrics. + */ +Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state); /** * Compute the final-time Metrics (i.e. cost, softConstraints, and constraints). diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h index 80da49816..b9c2afc50 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h @@ -49,17 +49,8 @@ namespace multiple_shooting { * @param u : Input, taken to be constant across the interval. * @return Metrics for a single intermediate node. */ -Metrics computeIntermediateMetrics(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, - scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); - -/** - * Compute only the Metrics for the terminal node. - * @param optimalControlProblem : Definition of the optimal control problem - * @param t : Time at the terminal node - * @param x : Terminal state - * @return Metrics for the terminal node. - */ -Metrics computeTerminalMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); +Metrics computeIntermediateMetrics(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, scalar_t dt, + const vector_t& x, const vector_t& x_next, const vector_t& u); /** * Compute only the Metrics for the event node. @@ -69,7 +60,16 @@ Metrics computeTerminalMetrics(const OptimalControlProblem& optimalControlProble * @param x_next : Post-event state * @return Metrics for the event node. */ -Metrics computeEventMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); +Metrics computeEventMetrics(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); + +/** + * Compute only the Metrics for the terminal node. + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the terminal node + * @param x : Terminal state + * @return Metrics for the terminal node. + */ +Metrics computeTerminalMetrics(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h index 4cbd1b41d..d54192b62 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h @@ -59,8 +59,8 @@ PerformanceIndex computeIntermediatePerformance(const Transcription& transcripti * @param u : Input, taken to be constant across the interval. * @return Performance index for a single intermediate node. */ -PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); +PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); /** * Compute the performance index from the transcription for the terminal node. @@ -77,7 +77,7 @@ PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcr * @param x : Terminal state * @return Performance index for the terminal node. */ -PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); +PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); /** * Compute the performance index from the transcription for the event node. @@ -95,7 +95,7 @@ PerformanceIndex computeEventPerformance(const EventTranscription& transcription * @param x_next : Post-event state * @return Performance index for the event node. */ -PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, +PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); } // namespace multiple_shooting diff --git a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp index fc2b984d8..fbe21dd8c 100644 --- a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp +++ b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp @@ -270,7 +270,7 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr /******************************************************************************************************/ /******************************************************************************************************/ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, - const MultiplierCollection& multipliers, const vector_t& dynamicsViolation) { + vector_t&& dynamicsViolation) { auto& preComputation = *problem.preComputationPtr; Metrics metrics; @@ -279,16 +279,43 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_ metrics.cost = computeCost(problem, time, state, input); // Dynamics violation - metrics.dynamicsViolation = dynamicsViolation; + metrics.dynamicsViolation = std::move(dynamicsViolation); // Equality constraints - metrics.stateEqConstraint = problem.stateEqualityConstraintPtr->getValue(time, state, preComputation); - metrics.stateInputEqConstraint = problem.equalityConstraintPtr->getValue(time, state, input, preComputation); + if (!problem.stateEqualityConstraintPtr->empty()) { + metrics.stateEqConstraint = problem.stateEqualityConstraintPtr->getValue(time, state, preComputation); + } + if (!problem.equalityConstraintPtr->empty()) { + metrics.stateInputEqConstraint = problem.equalityConstraintPtr->getValue(time, state, input, preComputation); + } - // Lagrangians + // Inequality constraints + if (!problem.stateInequalityConstraintPtr->empty()) { + metrics.stateIneqConstraint = problem.stateInequalityConstraintPtr->getValue(time, state, preComputation); + } + if (!problem.inequalityConstraintPtr->empty()) { + metrics.stateInputIneqConstraint = problem.inequalityConstraintPtr->getValue(time, state, input, preComputation); + } + + return metrics; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, + const MultiplierCollection& multipliers, vector_t&& dynamicsViolation) { + auto& preComputation = *problem.preComputationPtr; + + // cost, dynamics violation, equlaity constraints, inequlaity constraints + auto metrics = computeIntermediateMetrics(problem, time, state, input, std::move(dynamicsViolation)); + + // Equality Lagrangians metrics.stateEqLagrangian = problem.stateEqualityLagrangianPtr->getValue(time, state, multipliers.stateEq, preComputation); - metrics.stateIneqLagrangian = problem.stateInequalityLagrangianPtr->getValue(time, state, multipliers.stateIneq, preComputation); metrics.stateInputEqLagrangian = problem.equalityLagrangianPtr->getValue(time, state, input, multipliers.stateInputEq, preComputation); + + // Inequality Lagrangians + metrics.stateIneqLagrangian = problem.stateInequalityLagrangianPtr->getValue(time, state, multipliers.stateIneq, preComputation); metrics.stateInputIneqLagrangian = problem.inequalityLagrangianPtr->getValue(time, state, input, multipliers.stateInputIneq, preComputation); @@ -298,8 +325,7 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_ /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers, const vector_t& dynamicsViolation) { +Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, vector_t&& dynamicsViolation) { auto& preComputation = *problem.preComputationPtr; Metrics metrics; @@ -308,13 +334,35 @@ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t tim metrics.cost = computeEventCost(problem, time, state); // Dynamics violation - metrics.dynamicsViolation = dynamicsViolation; + metrics.dynamicsViolation = std::move(dynamicsViolation); // Equality constraint - metrics.stateEqConstraint = problem.preJumpEqualityConstraintPtr->getValue(time, state, preComputation); + if (!problem.preJumpEqualityConstraintPtr->empty()) { + metrics.stateEqConstraint = problem.preJumpEqualityConstraintPtr->getValue(time, state, preComputation); + } - // Lagrangians + // Inequality constraint + if (!problem.preJumpInequalityConstraintPtr->empty()) { + metrics.stateIneqConstraint = problem.preJumpInequalityConstraintPtr->getValue(time, state, preComputation); + } + + return metrics; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers, vector_t&& dynamicsViolation) { + auto& preComputation = *problem.preComputationPtr; + + // cost, dynamics violation, equlaity constraints, inequlaity constraints + auto metrics = computePreJumpMetrics(problem, time, state, std::move(dynamicsViolation)); + + // Equality Lagrangians metrics.stateEqLagrangian = problem.preJumpEqualityLagrangianPtr->getValue(time, state, multipliers.stateEq, preComputation); + + // Inequality Lagrangians metrics.stateIneqLagrangian = problem.preJumpInequalityLagrangianPtr->getValue(time, state, multipliers.stateIneq, preComputation); return metrics; @@ -323,8 +371,7 @@ Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t tim /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers) { +Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state) { auto& preComputation = *problem.preComputationPtr; Metrics metrics; @@ -336,10 +383,32 @@ Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, // metrics.dynamicsViolation = vector_t(); // Equality constraint - metrics.stateEqConstraint = problem.finalEqualityConstraintPtr->getValue(time, state, preComputation); + if (!problem.finalEqualityConstraintPtr->empty()) { + metrics.stateEqConstraint = problem.finalEqualityConstraintPtr->getValue(time, state, preComputation); + } - // Lagrangians + // Inequality constraint + if (!problem.finalInequalityConstraintPtr->empty()) { + metrics.stateIneqConstraint = problem.finalInequalityConstraintPtr->getValue(time, state, preComputation); + } + + return metrics; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers) { + auto& preComputation = *problem.preComputationPtr; + + // cost, equlaity constraints, inequlaity constraints + auto metrics = computeFinalMetrics(problem, time, state); + + // Equality Lagrangians metrics.stateEqLagrangian = problem.finalEqualityLagrangianPtr->getValue(time, state, multipliers.stateEq, preComputation); + + // Inequality Lagrangians metrics.stateIneqLagrangian = problem.finalInequalityLagrangianPtr->getValue(time, state, multipliers.stateIneq, preComputation); return metrics; diff --git a/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp b/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp index 41e1e99aa..ce045d294 100644 --- a/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp +++ b/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp @@ -34,89 +34,41 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { -Metrics computeIntermediateMetrics(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, - scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { - Metrics metrics; - +Metrics computeIntermediateMetrics(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, scalar_t dt, + const vector_t& x, const vector_t& x_next, const vector_t& u) { // Dynamics - metrics.dynamicsViolation = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); - metrics.dynamicsViolation -= x_next; + auto dynamicsViolation = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); + dynamicsViolation -= x_next; - // Precomputation for other terms + // Precomputation constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; optimalControlProblem.preComputationPtr->request(request, t, x, u); - // Costs - metrics.cost = dt * computeCost(optimalControlProblem, t, x, u); - - // State equality constraints - if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { - metrics.stateEqConstraint = optimalControlProblem.stateEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - } - - // State-input equality constraints - if (!optimalControlProblem.equalityConstraintPtr->empty()) { - metrics.stateInputEqConstraint = - optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - } - - // State inequality constraints. - if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { - metrics.stateIneqConstraint = - optimalControlProblem.stateInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - } - - // State-input inequality constraints. - if (!optimalControlProblem.inequalityConstraintPtr->empty()) { - metrics.stateInputIneqConstraint = - optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - } + // Compute metrics + auto metrics = computeIntermediateMetrics(optimalControlProblem, t, x, u, std::move(dynamicsViolation)); + metrics.cost *= dt; // consider dt return metrics; } -Metrics computeTerminalMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { - Metrics metrics; - - constexpr auto request = Request::Cost + Request::SoftConstraint; +Metrics computeTerminalMetrics(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { + // Precomputation + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); - metrics.cost = computeFinalCost(optimalControlProblem, t, x); - - if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { - metrics.stateEqConstraint = optimalControlProblem.finalEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - } - - if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { - metrics.stateIneqConstraint = - optimalControlProblem.finalInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - } - - return metrics; + return computeFinalMetrics(optimalControlProblem, t, x); } -Metrics computeEventMetrics(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { - Metrics metrics; - - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics; +Metrics computeEventMetrics(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { + // Precomputation + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint + Request::Dynamics; optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); // Dynamics - metrics.dynamicsViolation = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x) - x_next; + auto dynamicsViolation = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x); + dynamicsViolation -= x_next; - metrics.cost = computeEventCost(optimalControlProblem, t, x); - - if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { - metrics.stateEqConstraint = - optimalControlProblem.preJumpEqualityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - } - - if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { - metrics.stateIneqConstraint = - optimalControlProblem.preJumpInequalityConstraintPtr->getValue(t, x, *optimalControlProblem.preComputationPtr); - } - - return metrics; + return computePreJumpMetrics(optimalControlProblem, t, x, std::move(dynamicsViolation)); } } // namespace multiple_shooting diff --git a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp index c897c1c03..fab6505dc 100644 --- a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp +++ b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp @@ -57,11 +57,11 @@ PerformanceIndex computeIntermediatePerformance(const Transcription& transcripti return performance; } -PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { +PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { const auto metrics = computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); auto performanceIndex = toPerformanceIndex(metrics); - // performanceIndex.cost *= dt no need since it is already considered in computeIntermediateMetrics() + // performanceIndex.cost *= dt no need since it is already considered in multiple_shooting::computeIntermediateMetrics() performanceIndex.dynamicsViolationSSE *= dt; performanceIndex.equalityConstraintsSSE *= dt; performanceIndex.inequalityConstraintsSSE *= dt; @@ -82,7 +82,7 @@ PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcr return performance; } -PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { +PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { const auto metrics = computeTerminalMetrics(optimalControlProblem, t, x); return toPerformanceIndex(metrics); } @@ -104,7 +104,7 @@ PerformanceIndex computeEventPerformance(const EventTranscription& transcription return performance; } -PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, +PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { const auto metrics = computeEventMetrics(optimalControlProblem, t, x, x_next); return toPerformanceIndex(metrics); From 16e3d9af5e2b64af15051a9e877d48884994d2b5 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 12 Dec 2022 15:04:39 +0100 Subject: [PATCH 435/512] moving defintion of PerformanceIndex to src file --- ocs2_oc/CMakeLists.txt | 1 + .../ocs2_oc/oc_data/PerformanceIndex.h | 111 +++------------- ocs2_oc/src/oc_data/PerformanceIndex.cpp | 125 ++++++++++++++++++ ocs2_slp/src/SlpSolver.cpp | 1 + ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 1 + 5 files changed, 145 insertions(+), 94 deletions(-) create mode 100644 ocs2_oc/src/oc_data/PerformanceIndex.cpp diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index 75de1f056..f109d89bc 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -57,6 +57,7 @@ add_library(${PROJECT_NAME} src/multiple_shooting/ProjectionMultiplierCoefficients.cpp src/multiple_shooting/Transcription.cpp src/oc_data/LoopshapingPrimalSolution.cpp + src/oc_data/PerformanceIndex.cpp src/oc_data/TimeDiscretization.cpp src/oc_problem/OptimalControlProblem.cpp src/oc_problem/LoopshapingOptimalControlProblem.cpp diff --git a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index 3e0e65dc5..8bbce1ba7 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -29,8 +29,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include <iomanip> -#include <limits> #include <ostream> #include <ocs2_core/Types.h> @@ -86,116 +84,41 @@ struct PerformanceIndex { */ scalar_t inequalityLagrangian = 0.0; - /** Add performance indices */ - PerformanceIndex& operator+=(const PerformanceIndex& rhs) { - this->merit += rhs.merit; - this->cost += rhs.cost; - this->dualFeasibilitiesSSE += rhs.dualFeasibilitiesSSE; - this->dynamicsViolationSSE += rhs.dynamicsViolationSSE; - this->equalityConstraintsSSE += rhs.equalityConstraintsSSE; - this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE; - this->equalityLagrangian += rhs.equalityLagrangian; - this->inequalityLagrangian += rhs.inequalityLagrangian; - return *this; - } - - /** Multiply by a scalar */ - template <typename SCALAR_T> - PerformanceIndex& operator*=(const SCALAR_T c) { - this->merit *= static_cast<scalar_t>(c); - this->cost *= static_cast<scalar_t>(c); - this->dualFeasibilitiesSSE *= static_cast<scalar_t>(c); - this->dynamicsViolationSSE *= static_cast<scalar_t>(c); - this->equalityConstraintsSSE *= static_cast<scalar_t>(c); - this->inequalityConstraintsSSE *= static_cast<scalar_t>(c); - this->equalityLagrangian *= static_cast<scalar_t>(c); - this->inequalityLagrangian *= static_cast<scalar_t>(c); - return *this; - } + /** Add performance indices. */ + PerformanceIndex& operator+=(const PerformanceIndex& rhs); + + /** Multiply by a scalar. */ + PerformanceIndex& operator*=(const scalar_t c); /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ - bool isApprox(const PerformanceIndex other, const scalar_t prec = 1e-8) const { - auto fuzzyCompares = [&](const scalar_t a, const scalar_t b) { - return std::abs(a - b) <= prec * std::min(std::abs(a), std::abs(b)) || std::abs(a - b) < std::numeric_limits<scalar_t>::min(); - }; - bool isEqual = fuzzyCompares(this->merit, other.merit); - isEqual = isEqual && fuzzyCompares(this->cost, other.cost); - isEqual = isEqual && fuzzyCompares(this->dualFeasibilitiesSSE, other.dualFeasibilitiesSSE); - isEqual = isEqual && fuzzyCompares(this->dynamicsViolationSSE, other.dynamicsViolationSSE); - isEqual = isEqual && fuzzyCompares(this->equalityConstraintsSSE, other.equalityConstraintsSSE); - isEqual = isEqual && fuzzyCompares(this->inequalityConstraintsSSE, other.inequalityConstraintsSSE); - isEqual = isEqual && fuzzyCompares(this->equalityLagrangian, other.equalityLagrangian); - isEqual = isEqual && fuzzyCompares(this->inequalityLagrangian, other.inequalityLagrangian); - return isEqual; - } + bool isApprox(const PerformanceIndex other, const scalar_t prec = 1e-8) const; }; +/** Add performance indices. */ inline PerformanceIndex operator+(PerformanceIndex lhs, const PerformanceIndex& rhs) { lhs += rhs; // copied lhs, add rhs to it. return lhs; } -template <typename SCALAR_T> -PerformanceIndex operator*(PerformanceIndex lhs, const SCALAR_T c) { +/** Multiply by a scalar. */ +inline PerformanceIndex operator*(PerformanceIndex lhs, const scalar_t c) { lhs *= c; // copied lhs return lhs; } -template <typename SCALAR_T> -PerformanceIndex operator*(const SCALAR_T c, PerformanceIndex rhs) { +/** Multiply by a scalar. */ +inline PerformanceIndex operator*(const scalar_t c, PerformanceIndex rhs) { rhs *= c; // copied rhs return rhs; } -/** Swaps performance indices */ -inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { - std::swap(lhs.merit, rhs.merit); - std::swap(lhs.cost, rhs.cost); - std::swap(lhs.dualFeasibilitiesSSE, rhs.dualFeasibilitiesSSE); - std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE); - std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE); - std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE); - std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian); - std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian); -} - -inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex) { - const size_t tabSpace = 12; - const auto indentation = stream.width(); - stream << std::left; // fill from left - - stream << std::setw(indentation) << ""; - stream << "Rollout Merit: " << std::setw(tabSpace) << performanceIndex.merit; - stream << "Rollout Cost: " << std::setw(tabSpace) << performanceIndex.cost << '\n'; - - stream << std::setw(indentation) << ""; - stream << "Dual feasibilities SSE: " << std::setw(tabSpace) << performanceIndex.dualFeasibilitiesSSE; - stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE << '\n'; +/** Swaps performance indices. */ +void swap(PerformanceIndex& lhs, PerformanceIndex& rhs); - stream << std::setw(indentation) << ""; - stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE; - stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE << '\n'; +/** Computes the PerformanceIndex based on a given Metrics. */ +PerformanceIndex toPerformanceIndex(const Metrics& m); - stream << std::setw(indentation) << ""; - stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian; - stream << "Inequality Lagrangian: " << std::setw(tabSpace) << performanceIndex.inequalityLagrangian; - - return stream; -} - -/** Computes the PerformanceIndex based on a given Metrics */ -inline PerformanceIndex toPerformanceIndex(const Metrics& m) { - PerformanceIndex performanceIndex; - performanceIndex.merit = 0.0; // left for the solver to fill - performanceIndex.cost = m.cost; - performanceIndex.dualFeasibilitiesSSE = 0.0; // left for the solver to fill - performanceIndex.dynamicsViolationSSE = getEqConstraintsSSE(m.dynamicsViolation); - performanceIndex.equalityConstraintsSSE = getEqConstraintsSSE(m.stateEqConstraint) + getEqConstraintsSSE(m.stateInputEqConstraint); - performanceIndex.inequalityConstraintsSSE = - getIneqConstraintsSSE(m.stateIneqConstraint) + getIneqConstraintsSSE(m.stateInputIneqConstraint); - performanceIndex.equalityLagrangian = sumPenalties(m.stateEqLagrangian) + sumPenalties(m.stateInputEqLagrangian); - performanceIndex.inequalityLagrangian = sumPenalties(m.stateIneqLagrangian) + sumPenalties(m.stateInputIneqLagrangian); - return performanceIndex; -} +/** Overloads the stream insertion operator. */ +std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex); } // namespace ocs2 diff --git a/ocs2_oc/src/oc_data/PerformanceIndex.cpp b/ocs2_oc/src/oc_data/PerformanceIndex.cpp new file mode 100644 index 000000000..f4e01044c --- /dev/null +++ b/ocs2_oc/src/oc_data/PerformanceIndex.cpp @@ -0,0 +1,125 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_oc/oc_data/PerformanceIndex.h" + +#include <iomanip> +#include <limits> + +namespace ocs2 { + +PerformanceIndex& PerformanceIndex::operator+=(const PerformanceIndex& rhs) { + this->merit += rhs.merit; + this->cost += rhs.cost; + this->dualFeasibilitiesSSE += rhs.dualFeasibilitiesSSE; + this->dynamicsViolationSSE += rhs.dynamicsViolationSSE; + this->equalityConstraintsSSE += rhs.equalityConstraintsSSE; + this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE; + this->equalityLagrangian += rhs.equalityLagrangian; + this->inequalityLagrangian += rhs.inequalityLagrangian; + return *this; +} + +PerformanceIndex& PerformanceIndex::operator*=(const scalar_t c) { + this->merit *= c; + this->cost *= c; + this->dualFeasibilitiesSSE *= c; + this->dynamicsViolationSSE *= c; + this->equalityConstraintsSSE *= c; + this->inequalityConstraintsSSE *= c; + this->equalityLagrangian *= c; + this->inequalityLagrangian *= c; + return *this; +} + +bool PerformanceIndex::isApprox(const PerformanceIndex other, const scalar_t prec) const { + auto fuzzyCompares = [&](const scalar_t a, const scalar_t b) { + return std::abs(a - b) <= prec * std::min(std::abs(a), std::abs(b)) || std::abs(a - b) < std::numeric_limits<scalar_t>::min(); + }; + bool isEqual = fuzzyCompares(this->merit, other.merit); + isEqual = isEqual && fuzzyCompares(this->cost, other.cost); + isEqual = isEqual && fuzzyCompares(this->dualFeasibilitiesSSE, other.dualFeasibilitiesSSE); + isEqual = isEqual && fuzzyCompares(this->dynamicsViolationSSE, other.dynamicsViolationSSE); + isEqual = isEqual && fuzzyCompares(this->equalityConstraintsSSE, other.equalityConstraintsSSE); + isEqual = isEqual && fuzzyCompares(this->inequalityConstraintsSSE, other.inequalityConstraintsSSE); + isEqual = isEqual && fuzzyCompares(this->equalityLagrangian, other.equalityLagrangian); + isEqual = isEqual && fuzzyCompares(this->inequalityLagrangian, other.inequalityLagrangian); + return isEqual; +} + +void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { + std::swap(lhs.merit, rhs.merit); + std::swap(lhs.cost, rhs.cost); + std::swap(lhs.dualFeasibilitiesSSE, rhs.dualFeasibilitiesSSE); + std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE); + std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE); + std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE); + std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian); + std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian); +} + +PerformanceIndex toPerformanceIndex(const Metrics& m) { + PerformanceIndex performanceIndex; + performanceIndex.merit = 0.0; // left for the solver to fill + performanceIndex.cost = m.cost; + performanceIndex.dualFeasibilitiesSSE = 0.0; // left for the solver to fill + performanceIndex.dynamicsViolationSSE = getEqConstraintsSSE(m.dynamicsViolation); + performanceIndex.equalityConstraintsSSE = getEqConstraintsSSE(m.stateEqConstraint) + getEqConstraintsSSE(m.stateInputEqConstraint); + performanceIndex.inequalityConstraintsSSE = + getIneqConstraintsSSE(m.stateIneqConstraint) + getIneqConstraintsSSE(m.stateInputIneqConstraint); + performanceIndex.equalityLagrangian = sumPenalties(m.stateEqLagrangian) + sumPenalties(m.stateInputEqLagrangian); + performanceIndex.inequalityLagrangian = sumPenalties(m.stateIneqLagrangian) + sumPenalties(m.stateInputIneqLagrangian); + return performanceIndex; +} + +std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex) { + const size_t tabSpace = 12; + const auto indentation = stream.width(); + stream << std::left; // fill from left + + stream << std::setw(indentation) << ""; + stream << "Rollout Merit: " << std::setw(tabSpace) << performanceIndex.merit; + stream << "Rollout Cost: " << std::setw(tabSpace) << performanceIndex.cost << '\n'; + + stream << std::setw(indentation) << ""; + stream << "Dual feasibilities SSE: " << std::setw(tabSpace) << performanceIndex.dualFeasibilitiesSSE; + stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE << '\n'; + + stream << std::setw(indentation) << ""; + stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE; + stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE << '\n'; + + stream << std::setw(indentation) << ""; + stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian; + stream << "Inequality Lagrangian: " << std::setw(tabSpace) << performanceIndex.inequalityLagrangian; + + return stream; +} + +} // namespace ocs2 diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 5cce3844c..35789f742 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -29,6 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_slp/SlpSolver.h" +#include <iomanip> #include <iostream> #include <numeric> diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 5dd92c998..95d29a9b9 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -29,6 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_sqp/SqpSolver.h" +#include <iomanip> #include <iostream> #include <numeric> From dfc325dae5744fca00963e01a66e8bd74c195852 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 12 Dec 2022 15:08:34 +0100 Subject: [PATCH 436/512] adding missing header --- ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp | 2 ++ ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp b/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp index f25b46ea4..4f2122eb9 100644 --- a/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp +++ b/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h" +#include <iomanip> + #include "ocs2_ddp/DDP_HelperFunctions.h" #include "ocs2_ddp/HessianCorrection.h" diff --git a/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp b/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp index 546eceee7..4a6e3b7e1 100644 --- a/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp +++ b/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/search_strategy/LineSearchStrategy.h" +#include <iomanip> + #include "ocs2_ddp/DDP_HelperFunctions.h" #include "ocs2_ddp/HessianCorrection.h" From dfdf20dc18bc7da6cd82c55b26943ffc273fa5e3 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 12 Dec 2022 16:58:12 +0100 Subject: [PATCH 437/512] update after merge --- ocs2_ddp/src/DDP_HelperFunctions.cpp | 2 +- .../ocs2_oc/multiple_shooting/Helpers.h | 10 ++++ .../ocs2_oc/oc_data/PerformanceIndex.h | 5 +- ocs2_oc/src/multiple_shooting/Helpers.cpp | 24 +++++++++ .../PerformanceIndexComputation.cpp | 7 +-- ocs2_oc/src/oc_data/PerformanceIndex.cpp | 10 ++++ .../ocs2_sqp/include/ocs2_sqp/SqpSolver.h | 7 ++- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 50 ++++++++----------- 8 files changed, 74 insertions(+), 41 deletions(-) diff --git a/ocs2_ddp/src/DDP_HelperFunctions.cpp b/ocs2_ddp/src/DDP_HelperFunctions.cpp index af3dc2e26..8e0290bf3 100644 --- a/ocs2_ddp/src/DDP_HelperFunctions.cpp +++ b/ocs2_ddp/src/DDP_HelperFunctions.cpp @@ -73,7 +73,7 @@ void computeRolloutMetrics(OptimalControlProblem& problem, const PrimalSolution& problemMetrics.intermediates.reserve(tTrajectory.size()); auto nextPostEventIndexItr = postEventIndices.begin(); - const auto request = Request::Cost + Request::Constraint + Request::SoftConstraint; + constexpr auto request = Request::Cost + Request::Constraint + Request::SoftConstraint; for (size_t k = 0; k < tTrajectory.size(); k++) { // intermediate time cost and constraints problem.preComputationPtr->request(request, tTrajectory[k], xTrajectory[k], uTrajectory[k]); diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h index 478269ac7..1da9b9d70 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h @@ -33,6 +33,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_oc/oc_data/PerformanceIndex.h" #include "ocs2_oc/oc_data/PrimalSolution.h" +#include "ocs2_oc/oc_data/ProblemMetrics.h" #include "ocs2_oc/oc_data/TimeDiscretization.h" namespace ocs2 { @@ -101,5 +102,14 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, matrix_array_t&& KMatrices); +/** + * Constructs a ProblemMetrics from an array of metrics. + * + * @param [in] time : The annotated time trajectory + * @param [in] metrics: The metrics array. + * @return The ProblemMetrics. + */ +ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, const std::vector<Metrics>& metrics); + } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index 8bbce1ba7..252b5551b 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -115,9 +115,12 @@ inline PerformanceIndex operator*(const scalar_t c, PerformanceIndex rhs) { /** Swaps performance indices. */ void swap(PerformanceIndex& lhs, PerformanceIndex& rhs); -/** Computes the PerformanceIndex based on a given Metrics. */ +/** Computes the PerformanceIndex based on a given continuous-time Metrics. */ PerformanceIndex toPerformanceIndex(const Metrics& m); +/** Computes the PerformanceIndex based on a given discrete-time Metrics. */ +PerformanceIndex toPerformanceIndex(const Metrics& m, const scalar_t dt); + /** Overloads the stream insertion operator. */ std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex); diff --git a/ocs2_oc/src/multiple_shooting/Helpers.cpp b/ocs2_oc/src/multiple_shooting/Helpers.cpp index 0f1328b34..373b5545a 100644 --- a/ocs2_oc/src/multiple_shooting/Helpers.cpp +++ b/ocs2_oc/src/multiple_shooting/Helpers.cpp @@ -119,5 +119,29 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche return primalSolution; } +ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, const std::vector<Metrics>& metrics) { + assert(time.size() > 1); + assert(metrics.size() == time.size()); + + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + + // resize + ProblemMetrics problemMetrics; + problemMetrics.intermediates.reserve(N); + problemMetrics.preJumps.reserve(8); // the size is just a guess + problemMetrics.final = metrics.back(); + + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + problemMetrics.preJumps.push_back(metrics[i]); + } else { + problemMetrics.intermediates.push_back(metrics[i]); + } + } + + return problemMetrics; +} + } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp index fab6505dc..d78d5d3d2 100644 --- a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp +++ b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp @@ -60,12 +60,7 @@ PerformanceIndex computeIntermediatePerformance(const Transcription& transcripti PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { const auto metrics = computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); - auto performanceIndex = toPerformanceIndex(metrics); - // performanceIndex.cost *= dt no need since it is already considered in multiple_shooting::computeIntermediateMetrics() - performanceIndex.dynamicsViolationSSE *= dt; - performanceIndex.equalityConstraintsSSE *= dt; - performanceIndex.inequalityConstraintsSSE *= dt; - return performanceIndex; + return toPerformanceIndex(metrics, dt); } PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcription) { diff --git a/ocs2_oc/src/oc_data/PerformanceIndex.cpp b/ocs2_oc/src/oc_data/PerformanceIndex.cpp index f4e01044c..46f4a8bd1 100644 --- a/ocs2_oc/src/oc_data/PerformanceIndex.cpp +++ b/ocs2_oc/src/oc_data/PerformanceIndex.cpp @@ -98,6 +98,16 @@ PerformanceIndex toPerformanceIndex(const Metrics& m) { return performanceIndex; } +PerformanceIndex toPerformanceIndex(const Metrics& m, const scalar_t dt) { + auto performanceIndex = toPerformanceIndex(m); + // performanceIndex.cost *= dt no need since it is already considered in multiple_shooting::computeIntermediateMetrics() + performanceIndex.dualFeasibilitiesSSE *= dt; + performanceIndex.dynamicsViolationSSE *= dt; + performanceIndex.equalityConstraintsSSE *= dt; + performanceIndex.inequalityConstraintsSSE *= dt; + return performanceIndex; +} + std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex) { const size_t tabSpace = 12; const auto indentation = stream.width(); diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index 1c67f26a3..a14af3e21 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -125,7 +125,7 @@ class SqpSolver : public SolverBase { /** Computes only the performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u, ProblemMetrics& problemMetrics); + const vector_array_t& u, std::vector<Metrics>& metrics); /** Returns solution of the QP subproblem in delta coordinates: */ struct OcpSubproblemSolution { @@ -143,7 +143,8 @@ class SqpSolver : public SolverBase { /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ sqp::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, - const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u); + const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u, + std::vector<Metrics>& metrics); /** Determine convergence after a step */ sqp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const; @@ -184,8 +185,6 @@ class SqpSolver : public SolverBase { // The ProblemMetrics associated to primalSolution_ ProblemMetrics problemMetrics_; - // Memory used within the search strategy - ProblemMetrics problemMetricsNew_; // Benchmarking size_t numProblems_{0}; diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 08a959092..23a7cc63d 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/multiple_shooting/Helpers.h> #include <ocs2_oc/multiple_shooting/Initialization.h> +#include <ocs2_oc/multiple_shooting/MetricsComputation.h> #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> #include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/oc_problem/OcpSize.h> @@ -176,6 +177,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Bookkeeping performanceIndeces_.clear(); + std::vector<Metrics> metrics(timeDiscretization.size()); int iter = 0; sqp::Convergence convergence = sqp::Convergence::FALSE; @@ -197,7 +199,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Apply step linesearchTimer_.startTimer(); - const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u); + const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, metrics); performanceIndeces_.push_back(stepInfo.performanceAfterStep); linesearchTimer_.endTimer(); @@ -211,6 +213,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f computeControllerTimer_.startTimer(); primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, metrics); computeControllerTimer_.endTimer(); ++numProblems_; @@ -362,21 +365,11 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated } PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u, ProblemMetrics& problemMetrics) { - // Problem horizon + const vector_array_t& u, std::vector<Metrics>& metrics) { + // Problem size const int N = static_cast<int>(time.size()) - 1; - - // extract indices for setting up problemMetrics - int numEvents = 0, numIntermediates = 0; - std::vector<int> indices(N); - for (int i = 0; i < N; ++i) { - indices[i] = (time[i].event == AnnotatedTime::Event::PreEvent) ? numEvents++ : numIntermediates++; - } - - // resize - problemMetrics.clear(); - problemMetrics.preJumps.resize(numEvents); - problemMetrics.intermediates.resize(numIntermediates); + metrics.clear(); + metrics.resize(N + 1); std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); std::atomic_int timeIndex{0}; @@ -386,18 +379,16 @@ PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>& int i = timeIndex++; while (i < N) { - const auto problemMetricsIndex = indices[i]; if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node - problemMetrics.preJumps[problemMetricsIndex] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]); - performance[workerId] += toPerformanceIndex(problemMetrics.preJumps[problemMetricsIndex]); + metrics[i] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]); + performance[workerId] += toPerformanceIndex(metrics[i]); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - problemMetrics.intermediates[problemMetricsIndex] = - multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); - performance[workerId] += toPerformanceIndex(problemMetrics.intermediates[problemMetricsIndex], dt); + metrics[i] = multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + performance[workerId] += toPerformanceIndex(metrics[i], dt); } i = timeIndex++; @@ -405,15 +396,15 @@ PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>& if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); - problemMetrics.final = multiple_shooting::computeTerminalMetrics(ocpDefinition, tN, x[N]); - performance[workerId] += toPerformanceIndex(problemMetrics.final); + metrics[N] = multiple_shooting::computeTerminalMetrics(ocpDefinition, tN, x[N]); + performance[workerId] += toPerformanceIndex(metrics[N]); } }; runParallel(std::move(parallelTask)); // Account for initial state in performance const vector_t initDynamicsViolation = initState - x.front(); - problemMetrics.intermediates.front().dynamicsViolation += initDynamicsViolation; + metrics.front().dynamicsViolation += initDynamicsViolation; performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm(); // Sum performance of the threads @@ -424,7 +415,7 @@ PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>& sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, - vector_array_t& u) { + vector_array_t& u, std::vector<Metrics>& metrics) { using StepType = FilterLinesearch::StepType; /* @@ -450,13 +441,14 @@ sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::v scalar_t alpha = 1.0; vector_array_t xNew(x.size()); vector_array_t uNew(u.size()); + std::vector<Metrics> metricsNew(x.size()); do { // Compute step multiple_shooting::incrementTrajectory(u, du, alpha, uNew); multiple_shooting::incrementTrajectory(x, dx, alpha, xNew); // Compute cost and constraints - const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew); + const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew, metricsNew); // Step acceptance and record step type bool stepAccepted; @@ -474,7 +466,7 @@ sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::v if (stepAccepted) { // Return if step accepted x = std::move(xNew); u = std::move(uNew); - problemMetrics.swap(problemMetricsNew_); + metrics = std::move(metricsNew); // Prepare step info sqp::StepInfo stepInfo; @@ -508,8 +500,8 @@ sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::v stepInfo.du_norm = 0.0; stepInfo.performanceAfterStep = baseline; stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline); - - std::ignore = computePerformance(timeDiscretization, initState, xNew, uNew, problemMetrics); + // compute metrics for the baseline rollout + std::ignore = computePerformance(timeDiscretization, initState, x, u, metrics); if (settings_.printLinesearch) { std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; From bc8073eb76f9e46ef2d0b75a37ea131ba63e398b Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 12 Dec 2022 18:21:28 +0100 Subject: [PATCH 438/512] bugfix --- ocs2_slp/include/ocs2_slp/SlpSolver.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_slp/include/ocs2_slp/SlpSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h index 2661fd14d..05c8083bd 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSolver.h +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -65,7 +65,7 @@ class SlpSolver : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } - const DualSolution& getDualSolution() const override { throw std::runtime_error("[SqpSolver] getDualSolution() not available yet."); } + const DualSolution* getDualSolution() const override { return nullptr; } const ProblemMetrics& getSolutionMetrics() const override { throw std::runtime_error("[SqpSolver] getSolutionMetrics() not available yet."); From c79157451b7da8571319713d894583d8fb35c853 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Mon, 12 Dec 2022 18:52:58 +0100 Subject: [PATCH 439/512] getDualSolution --- ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h | 2 +- ocs2_slp/include/ocs2_slp/SlpSolver.h | 4 +--- ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h | 2 -- 3 files changed, 2 insertions(+), 6 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h index c15a0c2fb..136c973fe 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h @@ -195,7 +195,7 @@ class SolverBase { * * @return: The dual problem's solution. */ - virtual const DualSolution* getDualSolution() const = 0; + virtual const DualSolution* getDualSolution() const { return nullptr; } /** * @brief Returns the optimized value of the Metrics. diff --git a/ocs2_slp/include/ocs2_slp/SlpSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h index 05c8083bd..37303982e 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSolver.h +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -65,10 +65,8 @@ class SlpSolver : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } - const DualSolution* getDualSolution() const override { return nullptr; } - const ProblemMetrics& getSolutionMetrics() const override { - throw std::runtime_error("[SqpSolver] getSolutionMetrics() not available yet."); + throw std::runtime_error("[SlpSolver] getSolutionMetrics() not available yet."); } size_t getNumIterations() const override { return totalNumIterations_; } diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index a14af3e21..184548f1f 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -66,8 +66,6 @@ class SqpSolver : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } - const DualSolution* getDualSolution() const override { return nullptr; } - const ProblemMetrics& getSolutionMetrics() const override { return problemMetrics_; } size_t getNumIterations() const override { return totalNumIterations_; } From d80dd5d76e9d92b912829d19c64fee2ae545123f Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 09:23:59 +0100 Subject: [PATCH 440/512] ConstraintCollection has an optional output for constraint size --- .../constraint/StateConstraintCollection.h | 7 +++-- .../StateInputConstraintCollection.h | 6 ++-- .../LoopshapingConstraintEliminatePattern.h | 6 ++-- .../LoopshapingConstraintOutputPattern.h | 6 ++-- .../constraint/LoopshapingStateConstraint.h | 10 ++++--- .../constraint/StateConstraintCollection.cpp | 28 ++++++++++++++++--- .../StateInputConstraintCollection.cpp | 28 ++++++++++++++++--- .../LoopshapingConstraintEliminatePattern.cpp | 10 ++++--- .../LoopshapingConstraintOutputPattern.cpp | 10 ++++--- .../constraint/LoopshapingStateConstraint.cpp | 10 ++++--- 10 files changed, 88 insertions(+), 33 deletions(-) diff --git a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h index a070aa46c..f149f2ab0 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h @@ -56,12 +56,13 @@ class StateConstraintCollection : public Collection<StateConstraint> { virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const; /** Get the constraint linear approximation */ - virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, - const PreComputation& preComp) const; + virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const; /** Get the constraint quadratic approximation */ virtual VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, - const PreComputation& preComp) const; + const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const; protected: /** Copy constructor */ diff --git a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h index 0eaafb700..fbb86cde4 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h @@ -57,11 +57,13 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> { /** Get the constraint linear approximation */ virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const; + const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const; /** Get the constraint quadratic approximation */ virtual VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const; + const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const; protected: /** Copy constructor */ diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintEliminatePattern.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintEliminatePattern.h index 3e3364f1e..4a261a9c5 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintEliminatePattern.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintEliminatePattern.h @@ -50,10 +50,12 @@ class LoopshapingConstraintEliminatePattern final : public LoopshapingStateInput LoopshapingConstraintEliminatePattern* clone() const override { return new LoopshapingConstraintEliminatePattern(*this); }; VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const override; + const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const override; VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const override; + const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const override; protected: using BASE::loopshapingDefinition_; diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintOutputPattern.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintOutputPattern.h index e0a641a18..29e4db8c1 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintOutputPattern.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintOutputPattern.h @@ -50,10 +50,12 @@ class LoopshapingConstraintOutputPattern final : public LoopshapingStateInputCon LoopshapingConstraintOutputPattern* clone() const override { return new LoopshapingConstraintOutputPattern(*this); }; VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const override; + const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const override; VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const override; + const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const override; protected: using BASE::loopshapingDefinition_; diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h index 150e29ff6..1d44f8470 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h @@ -48,10 +48,12 @@ class LoopshapingStateConstraint final : public StateConstraintCollection { LoopshapingStateConstraint* clone() const override { return new LoopshapingStateConstraint(*this); } vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; - VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, - const PreComputation& preComp) const override; - VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, - const PreComputation& preComp) const override; + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const override; + + VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp, + size_array_t* termsSizePtr = nullptr) const override; private: LoopshapingStateConstraint(const LoopshapingStateConstraint& other) = default; diff --git a/ocs2_core/src/constraint/StateConstraintCollection.cpp b/ocs2_core/src/constraint/StateConstraintCollection.cpp index 4fbcda78f..b8d96aabc 100644 --- a/ocs2_core/src/constraint/StateConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateConstraintCollection.cpp @@ -76,19 +76,29 @@ vector_array_t StateConstraintCollection::getValue(scalar_t time, const vector_t /******************************************************************************************************/ /******************************************************************************************************/ VectorFunctionLinearApproximation StateConstraintCollection::getLinearApproximation(scalar_t time, const vector_t& state, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { VectorFunctionLinearApproximation linearApproximation(getNumConstraints(time), state.rows()); + if (termsSizePtr != nullptr) { + termsSizePtr->clear(); + termsSizePtr->reserve(this->terms_.size()); + } + // append linearApproximation of each constraintTerm size_t i = 0; for (const auto& constraintTerm : this->terms_) { + size_t nc = 0; if (constraintTerm->isActive(time)) { const auto constraintTermApproximation = constraintTerm->getLinearApproximation(time, state, preComp); - const size_t nc = constraintTermApproximation.f.rows(); + nc = constraintTermApproximation.f.rows(); linearApproximation.f.segment(i, nc) = constraintTermApproximation.f; linearApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx; i += nc; } + if (termsSizePtr != nullptr) { + termsSizePtr->push_back(nc); + } } return linearApproximation; @@ -98,7 +108,8 @@ VectorFunctionLinearApproximation StateConstraintCollection::getLinearApproximat /******************************************************************************************************/ /******************************************************************************************************/ VectorFunctionQuadraticApproximation StateConstraintCollection::getQuadraticApproximation(scalar_t time, const vector_t& state, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { const auto numConstraints = getNumConstraints(time); VectorFunctionQuadraticApproximation quadraticApproximation; @@ -106,17 +117,26 @@ VectorFunctionQuadraticApproximation StateConstraintCollection::getQuadraticAppr quadraticApproximation.dfdx.resize(numConstraints, state.rows()); quadraticApproximation.dfdxx.reserve(numConstraints); // Use reserve instead of resize to avoid unnecessary allocations. + if (termsSizePtr != nullptr) { + termsSizePtr->clear(); + termsSizePtr->reserve(this->terms_.size()); + } + // append quadraticApproximation of each constraintTerm size_t i = 0; for (const auto& constraintTerm : this->terms_) { + size_t nc = 0; if (constraintTerm->isActive(time)) { auto constraintTermApproximation = constraintTerm->getQuadraticApproximation(time, state, preComp); - const size_t nc = constraintTermApproximation.f.rows(); + nc = constraintTermApproximation.f.rows(); quadraticApproximation.f.segment(i, nc) = constraintTermApproximation.f; quadraticApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx; appendVectorToVectorByMoving(quadraticApproximation.dfdxx, std::move(constraintTermApproximation.dfdxx)); i += nc; } + if (termsSizePtr != nullptr) { + termsSizePtr->push_back(nc); + } } return quadraticApproximation; diff --git a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp index a6e1c8192..94976265b 100644 --- a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp @@ -79,20 +79,30 @@ vector_array_t StateInputConstraintCollection::getValue(scalar_t time, const vec /******************************************************************************************************/ VectorFunctionLinearApproximation StateInputConstraintCollection::getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { VectorFunctionLinearApproximation linearApproximation(getNumConstraints(time), state.rows(), input.rows()); + if (termsSizePtr != nullptr) { + termsSizePtr->clear(); + termsSizePtr->reserve(this->terms_.size()); + } + // append linearApproximation of each constraintTerm size_t i = 0; for (const auto& constraintTerm : this->terms_) { + size_t nc = 0; if (constraintTerm->isActive(time)) { const auto constraintTermApproximation = constraintTerm->getLinearApproximation(time, state, input, preComp); - const size_t nc = constraintTermApproximation.f.rows(); + nc = constraintTermApproximation.f.rows(); linearApproximation.f.segment(i, nc) = constraintTermApproximation.f; linearApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx; linearApproximation.dfdu.middleRows(i, nc) = constraintTermApproximation.dfdu; i += nc; } + if (termsSizePtr != nullptr) { + termsSizePtr->push_back(nc); + } } return linearApproximation; @@ -103,7 +113,8 @@ VectorFunctionLinearApproximation StateInputConstraintCollection::getLinearAppro /******************************************************************************************************/ VectorFunctionQuadraticApproximation StateInputConstraintCollection::getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { const auto numConstraints = getNumConstraints(time); VectorFunctionQuadraticApproximation quadraticApproximation; @@ -114,12 +125,18 @@ VectorFunctionQuadraticApproximation StateInputConstraintCollection::getQuadrati quadraticApproximation.dfdux.reserve(numConstraints); quadraticApproximation.dfduu.reserve(numConstraints); + if (termsSizePtr != nullptr) { + termsSizePtr->clear(); + termsSizePtr->reserve(this->terms_.size()); + } + // append quadraticApproximation of each constraintTerm size_t i = 0; for (const auto& constraintTerm : this->terms_) { + size_t nc = 0; if (constraintTerm->isActive(time)) { auto constraintTermApproximation = constraintTerm->getQuadraticApproximation(time, state, input, preComp); - const size_t nc = constraintTermApproximation.f.rows(); + nc = constraintTermApproximation.f.rows(); quadraticApproximation.f.segment(i, nc) = constraintTermApproximation.f; quadraticApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx; quadraticApproximation.dfdu.middleRows(i, nc) = constraintTermApproximation.dfdu; @@ -128,6 +145,9 @@ VectorFunctionQuadraticApproximation StateInputConstraintCollection::getQuadrati appendVectorToVectorByMoving(quadraticApproximation.dfduu, std::move(constraintTermApproximation.dfduu)); i += nc; } + if (termsSizePtr != nullptr) { + termsSizePtr->push_back(nc); + } } return quadraticApproximation; diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintEliminatePattern.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintEliminatePattern.cpp index 3cb42f21b..6686c6176 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintEliminatePattern.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintEliminatePattern.cpp @@ -37,7 +37,8 @@ namespace ocs2 { /******************************************************************************************************/ VectorFunctionLinearApproximation LoopshapingConstraintEliminatePattern::getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { if (this->empty()) { return VectorFunctionLinearApproximation::Zero(0, x.rows(), u.rows()); } @@ -53,7 +54,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintEliminatePattern::getLine const auto filtStateDim = s_filter.getNumStates(); // Not const so we can move - auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system); + auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system, termsSizePtr); const auto numConstraints = g_system.f.rows(); VectorFunctionLinearApproximation g; @@ -83,7 +84,8 @@ VectorFunctionLinearApproximation LoopshapingConstraintEliminatePattern::getLine /******************************************************************************************************/ VectorFunctionQuadraticApproximation LoopshapingConstraintEliminatePattern::getQuadraticApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { if (this->empty()) { return VectorFunctionQuadraticApproximation::Zero(0, x.rows(), u.rows()); } @@ -101,7 +103,7 @@ VectorFunctionQuadraticApproximation LoopshapingConstraintEliminatePattern::getQ const auto filtStateDim = x_filter.rows(); // Not const so we can move - auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system); + auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system, termsSizePtr); const auto numConstraints = h_system.f.rows(); VectorFunctionQuadraticApproximation h; diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintOutputPattern.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintOutputPattern.cpp index d77e95a77..b07c44c81 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintOutputPattern.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintOutputPattern.cpp @@ -37,7 +37,8 @@ namespace ocs2 { /******************************************************************************************************/ VectorFunctionLinearApproximation LoopshapingConstraintOutputPattern::getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { if (this->empty()) { return VectorFunctionLinearApproximation::Zero(0, x.rows(), u.rows()); } @@ -51,7 +52,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintOutputPattern::getLinearA const auto filtStateDim = x.rows() - sysStateDim; // Not const so we can move - auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system); + auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system, termsSizePtr); const auto numConstraints = g_system.f.rows(); VectorFunctionLinearApproximation g; @@ -71,7 +72,8 @@ VectorFunctionLinearApproximation LoopshapingConstraintOutputPattern::getLinearA /******************************************************************************************************/ VectorFunctionQuadraticApproximation LoopshapingConstraintOutputPattern::getQuadraticApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { if (this->empty()) { return VectorFunctionQuadraticApproximation::Zero(0, x.rows(), u.rows()); } @@ -87,7 +89,7 @@ VectorFunctionQuadraticApproximation LoopshapingConstraintOutputPattern::getQuad const auto filtStateDim = x_filter.rows(); // Not const so we can move - auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system); + auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system, termsSizePtr); const auto numConstraints = h_system.f.rows(); VectorFunctionQuadraticApproximation h; diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp index af67c3761..a6cac38c7 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp @@ -54,7 +54,8 @@ vector_array_t LoopshapingStateConstraint::getValue(scalar_t t, const vector_t& /******************************************************************************************************/ /******************************************************************************************************/ VectorFunctionLinearApproximation LoopshapingStateConstraint::getLinearApproximation(scalar_t t, const vector_t& x, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { if (this->empty()) { return VectorFunctionLinearApproximation::Zero(0, x.rows()); } @@ -67,7 +68,7 @@ VectorFunctionLinearApproximation LoopshapingStateConstraint::getLinearApproxima const auto filtStateDim = x.rows() - sysStateDim; // Not const so we can move - auto c_system = StateConstraintCollection::getLinearApproximation(t, x_system, preComp_system); + auto c_system = StateConstraintCollection::getLinearApproximation(t, x_system, preComp_system, termsSizePtr); const auto numConstraints = c_system.f.rows(); VectorFunctionLinearApproximation c; @@ -85,7 +86,8 @@ VectorFunctionLinearApproximation LoopshapingStateConstraint::getLinearApproxima /******************************************************************************************************/ /******************************************************************************************************/ VectorFunctionQuadraticApproximation LoopshapingStateConstraint::getQuadraticApproximation(scalar_t t, const vector_t& x, - const PreComputation& preComp) const { + const PreComputation& preComp, + size_array_t* termsSizePtr) const { if (this->empty()) { return VectorFunctionQuadraticApproximation::Zero(0, x.rows()); } @@ -98,7 +100,7 @@ VectorFunctionQuadraticApproximation LoopshapingStateConstraint::getQuadraticApp const auto filtStateDim = x.rows() - sysStateDim; // Not const so we can move - const auto c_system = StateConstraintCollection::getQuadraticApproximation(t, x_system, preComp_system); + const auto c_system = StateConstraintCollection::getQuadraticApproximation(t, x_system, preComp_system, termsSizePtr); const auto numConstraints = c_system.f.rows(); VectorFunctionQuadraticApproximation c; From ddbcfea8476a9d014f4fe2fbf5fbccf51ed1d3b2 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 09:37:59 +0100 Subject: [PATCH 441/512] Transcription extracts onstraints size --- .../ocs2_oc/multiple_shooting/Transcription.h | 13 +++++++ .../src/multiple_shooting/Transcription.cpp | 37 ++++++++++--------- 2 files changed, 33 insertions(+), 17 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h index 168322fca..7687380a2 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h @@ -38,10 +38,21 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { +/** + * The size of each constraint terms for each type. + */ +struct ConstraintsSize { + size_array_t stateEq; + size_array_t stateInputEq; + size_array_t stateIneq; + size_array_t stateInputIneq; +}; + /** * Results of the transcription at an intermediate node */ struct Transcription { + ConstraintsSize constraintsSize; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation dynamics; VectorFunctionLinearApproximation stateEqConstraints; @@ -80,6 +91,7 @@ void projectTranscription(Transcription& transcription, bool extractProjectionMu * Results of the transcription at a terminal node */ struct TerminalTranscription { + ConstraintsSize constraintsSize; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation eqConstraints; VectorFunctionLinearApproximation ineqConstraints; @@ -99,6 +111,7 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont * Results of the transcription at an event */ struct EventTranscription { + ConstraintsSize constraintsSize; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation dynamics; VectorFunctionLinearApproximation eqConstraints; diff --git a/ocs2_oc/src/multiple_shooting/Transcription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp index 8b95289c5..0daba99af 100644 --- a/ocs2_oc/src/multiple_shooting/Transcription.cpp +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -42,12 +42,13 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP const vector_t& x_next, const vector_t& u) { // Results and short-hand notation Transcription transcription; - auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; + auto& dynamics = transcription.dynamics; auto& stateEqConstraints = transcription.stateEqConstraints; auto& stateInputEqConstraints = transcription.stateInputEqConstraints; auto& stateIneqConstraints = transcription.stateIneqConstraints; auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; + auto& constraintsSize = transcription.constraintsSize; // Dynamics // Discretization returns x_{k+1} = A_{k} * dx_{k} + B_{k} * du_{k} + b_{k} @@ -64,26 +65,26 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP // State equality constraints if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { - stateEqConstraints = - optimalControlProblem.stateEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + stateEqConstraints = optimalControlProblem.stateEqualityConstraintPtr->getLinearApproximation( + t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateEq); } // State-input equality constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { - stateInputEqConstraints = - optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); + stateInputEqConstraints = optimalControlProblem.equalityConstraintPtr->getLinearApproximation( + t, x, u, *optimalControlProblem.preComputationPtr, &constraintsSize.stateInputEq); } // State inequality constraints. if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { - stateIneqConstraints = - optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation( + t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateIneq); } // State-input inequality constraints. if (!optimalControlProblem.inequalityConstraintPtr->empty()) { - stateInputIneqConstraints = - optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); + stateInputIneqConstraints = optimalControlProblem.inequalityConstraintPtr->getLinearApproximation( + t, x, u, *optimalControlProblem.preComputationPtr, &constraintsSize.stateInputIneq); } return transcription; @@ -124,6 +125,7 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont auto& cost = transcription.cost; auto& eqConstraints = transcription.eqConstraints; auto& ineqConstraints = transcription.ineqConstraints; + auto& constraintsSize = transcription.constraintsSize; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); @@ -133,14 +135,14 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont // State equality constraints. if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { - eqConstraints = - optimalControlProblem.finalEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + eqConstraints = optimalControlProblem.finalEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr, + &constraintsSize.stateEq); } // State inequality constraints. if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { - ineqConstraints = - optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation( + t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateIneq); } return transcription; @@ -154,6 +156,7 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro auto& dynamics = transcription.dynamics; auto& eqConstraints = transcription.eqConstraints; auto& ineqConstraints = transcription.ineqConstraints; + auto& constraintsSize = transcription.constraintsSize; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation; optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); @@ -169,14 +172,14 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro // State equality constraints. if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { - eqConstraints = - optimalControlProblem.preJumpEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + eqConstraints = optimalControlProblem.preJumpEqualityConstraintPtr->getLinearApproximation( + t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateEq); } // State inequality constraints. if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { - ineqConstraints = - optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation( + t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateIneq); } return transcription; From b1273f5545908121d74707369df5c278334f8379 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 09:43:25 +0100 Subject: [PATCH 442/512] computing metrics from Transcription using function overload. --- ocs2_oc/CMakeLists.txt | 1 + .../ocs2_oc/multiple_shooting/Helpers.h | 2 +- .../multiple_shooting/MetricsComputation.h | 28 +++- ocs2_oc/src/lintTarget.cpp | 1 + ocs2_oc/src/multiple_shooting/Helpers.cpp | 8 +- .../multiple_shooting/MetricsComputation.cpp | 59 +++++++++ .../testTranscriptionMetrics.cpp | 121 ++++++++++++++++++ .../ocs2_sqp/include/ocs2_sqp/SqpSolver.h | 2 +- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 12 +- 9 files changed, 220 insertions(+), 14 deletions(-) create mode 100644 ocs2_oc/test/multiple_shooting/testTranscriptionMetrics.cpp diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index f109d89bc..4ba703f79 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -132,6 +132,7 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting test/multiple_shooting/testProjectionMultiplierCoefficients.cpp + test/multiple_shooting/testTranscriptionMetrics.cpp test/multiple_shooting/testTranscriptionPerformanceIndex.cpp ) add_dependencies(test_${PROJECT_NAME}_multiple_shooting diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h index 1da9b9d70..529b198f7 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h @@ -109,7 +109,7 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche * @param [in] metrics: The metrics array. * @return The ProblemMetrics. */ -ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, const std::vector<Metrics>& metrics); +ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, std::vector<Metrics>&& metrics); } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h index b9c2afc50..fe1203406 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h @@ -33,13 +33,35 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/integration/SensitivityIntegrator.h> #include <ocs2_core/model_data/Metrics.h> +#include "ocs2_oc/multiple_shooting/Transcription.h" #include "ocs2_oc/oc_problem/OptimalControlProblem.h" namespace ocs2 { namespace multiple_shooting { /** - * Compute only the Metrics for a single intermediate node. + * Compute the Metrics for a single intermediate node. + * @param transcription: multiple shooting transcription for an intermediate node. + * @return Metrics for a single intermediate node. + */ +Metrics computeMetrics(const Transcription& transcription); + +/** + * Compute the Metrics for the event node. + * @param transcription: multiple shooting transcription for event node. + * @return Metrics for a event node. + */ +Metrics computeMetrics(const EventTranscription& transcription); + +/** + * Compute the Metrics for the terminal node. + * @param transcription: multiple shooting transcription for terminal node. + * @return Metrics for a terminal node. + */ +Metrics computeMetrics(const TerminalTranscription& transcription); + +/** + * Compute the Metrics for a single intermediate node. * @param optimalControlProblem : Definition of the optimal control problem * @param discretizer : Integrator to use for creating the discrete dynamics. * @param t : Start of the discrete interval @@ -53,7 +75,7 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& optimalControlProblem, const vector_t& x, const vector_t& x_next, const vector_t& u); /** - * Compute only the Metrics for the event node. + * Compute the Metrics for the event node. * @param optimalControlProblem : Definition of the optimal control problem * @param t : Time at the event node * @param x : Pre-event state @@ -63,7 +85,7 @@ Metrics computeIntermediateMetrics(OptimalControlProblem& optimalControlProblem, Metrics computeEventMetrics(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); /** - * Compute only the Metrics for the terminal node. + * Compute the Metrics for the terminal node. * @param optimalControlProblem : Definition of the optimal control problem * @param t : Time at the terminal node * @param x : Terminal state diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index c0352c9b2..ce296fdcc 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/multiple_shooting/Helpers.h> #include <ocs2_oc/multiple_shooting/Initialization.h> #include <ocs2_oc/multiple_shooting/LagrangianEvaluation.h> +#include <ocs2_oc/multiple_shooting/MetricsComputation.h> #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> #include <ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h> #include <ocs2_oc/multiple_shooting/Transcription.h> diff --git a/ocs2_oc/src/multiple_shooting/Helpers.cpp b/ocs2_oc/src/multiple_shooting/Helpers.cpp index 373b5545a..c474b1d09 100644 --- a/ocs2_oc/src/multiple_shooting/Helpers.cpp +++ b/ocs2_oc/src/multiple_shooting/Helpers.cpp @@ -119,7 +119,7 @@ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, ModeSche return primalSolution; } -ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, const std::vector<Metrics>& metrics) { +ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, std::vector<Metrics>&& metrics) { assert(time.size() > 1); assert(metrics.size() == time.size()); @@ -130,13 +130,13 @@ ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, const st ProblemMetrics problemMetrics; problemMetrics.intermediates.reserve(N); problemMetrics.preJumps.reserve(8); // the size is just a guess - problemMetrics.final = metrics.back(); + problemMetrics.final = std::move(metrics.back()); for (int i = 0; i < N; ++i) { if (time[i].event == AnnotatedTime::Event::PreEvent) { - problemMetrics.preJumps.push_back(metrics[i]); + problemMetrics.preJumps.push_back(std::move(metrics[i])); } else { - problemMetrics.intermediates.push_back(metrics[i]); + problemMetrics.intermediates.push_back(std::move(metrics[i])); } } diff --git a/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp b/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp index ce045d294..5cb0ee8f8 100644 --- a/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp +++ b/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp @@ -34,6 +34,65 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { +Metrics computeMetrics(const Transcription& transcription) { + const auto& constraintsSize = transcription.constraintsSize; + + Metrics metrics; + + // Cost + metrics.cost = transcription.cost.f; + + // Dynamics + metrics.dynamicsViolation = transcription.dynamics.f; + + // Equality constraints + metrics.stateEqConstraint = toConstraintArray(constraintsSize.stateEq, transcription.stateEqConstraints.f); + metrics.stateInputEqConstraint = toConstraintArray(constraintsSize.stateInputEq, transcription.stateInputEqConstraints.f); + + // Inequality constraints. + metrics.stateIneqConstraint = toConstraintArray(constraintsSize.stateIneq, transcription.stateIneqConstraints.f); + metrics.stateInputIneqConstraint = toConstraintArray(constraintsSize.stateInputIneq, transcription.stateInputIneqConstraints.f); + + return metrics; +} + +Metrics computeMetrics(const EventTranscription& transcription) { + const auto& constraintsSize = transcription.constraintsSize; + + Metrics metrics; + + // Cost + metrics.cost = transcription.cost.f; + + // Dynamics + metrics.dynamicsViolation = transcription.dynamics.f; + + // Equality constraints + metrics.stateEqConstraint = toConstraintArray(constraintsSize.stateEq, transcription.eqConstraints.f); + + // Inequality constraints. + metrics.stateIneqConstraint = toConstraintArray(constraintsSize.stateIneq, transcription.ineqConstraints.f); + + return metrics; +} + +Metrics computeMetrics(const TerminalTranscription& transcription) { + const auto& constraintsSize = transcription.constraintsSize; + + Metrics metrics; + + // Cost + metrics.cost = transcription.cost.f; + + // Equality constraints + metrics.stateEqConstraint = toConstraintArray(constraintsSize.stateEq, transcription.eqConstraints.f); + + // Inequality constraints. + metrics.stateIneqConstraint = toConstraintArray(constraintsSize.stateIneq, transcription.ineqConstraints.f); + + return metrics; +} + Metrics computeIntermediateMetrics(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { // Dynamics diff --git a/ocs2_oc/test/multiple_shooting/testTranscriptionMetrics.cpp b/ocs2_oc/test/multiple_shooting/testTranscriptionMetrics.cpp new file mode 100644 index 000000000..147d68cb1 --- /dev/null +++ b/ocs2_oc/test/multiple_shooting/testTranscriptionMetrics.cpp @@ -0,0 +1,121 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <gtest/gtest.h> + +#include <ocs2_oc/multiple_shooting/MetricsComputation.h> +#include <ocs2_oc/multiple_shooting/Transcription.h> + +#include "ocs2_oc/test/circular_kinematics.h" +#include "ocs2_oc/test/testProblemsGeneration.h" + +using namespace ocs2; + +TEST(test_transcription_metrics, intermediate) { + constexpr int nx = 2; + constexpr int nu = 2; + + // optimal control problem + OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/sqp_test_generated"); + + // equality constraints + problem.equalityConstraintPtr->add("equalityConstraint", getOcs2Constraints(getRandomConstraints(nx, nu, 2))); + problem.stateEqualityConstraintPtr->add("stateEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 1))); + // inequality constraints + problem.inequalityConstraintPtr->add("inequalityConstraint", getOcs2Constraints(getRandomConstraints(nx, nu, 3))); + problem.stateInequalityConstraintPtr->add("stateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); + + auto discretizer = selectDynamicsDiscretization(SensitivityIntegratorType::RK4); + auto sensitivityDiscretizer = selectDynamicsSensitivityDiscretization(SensitivityIntegratorType::RK4); + + const scalar_t t = 0.5; + const scalar_t dt = 0.1; + const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); + const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); + const vector_t u = (vector_t(nu) << 0.1, 1.3).finished(); + const auto transcription = multiple_shooting::setupIntermediateNode(problem, sensitivityDiscretizer, t, dt, x, x_next, u); + const auto metrics = multiple_shooting::computeIntermediateMetrics(problem, discretizer, t, dt, x, x_next, u); + + ASSERT_TRUE(metrics.isApprox(multiple_shooting::computeMetrics(transcription), 1e-12)); +} + +TEST(test_transcription_metrics, event) { + constexpr int nx = 2; + + // optimal control problem + OptimalControlProblem problem; + + // dynamics + const auto dynamics = getRandomDynamics(nx, 0); + const auto jumpMap = matrix_t::Random(nx, nx); + problem.dynamicsPtr.reset(new LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); + + // cost + problem.preJumpCostPtr->add("eventCost", getOcs2StateCost(getRandomCost(nx, 0))); + + // constraints + problem.preJumpEqualityConstraintPtr->add("preJumpEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 3))); + problem.preJumpInequalityConstraintPtr->add("preJumpInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); + + const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); + problem.targetTrajectoriesPtr = &targetTrajectories; + + const scalar_t t = 0.5; + const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); + const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); + const auto transcription = multiple_shooting::setupEventNode(problem, t, x, x_next); + const auto metrics = multiple_shooting::computeEventMetrics(problem, t, x, x_next); + + ASSERT_TRUE(metrics.isApprox(multiple_shooting::computeMetrics(transcription), 1e-12)); +} + +TEST(test_transcription_metrics, terminal) { + constexpr int nx = 3; + + // optimal control problem + OptimalControlProblem problem; + + // cost + problem.finalCostPtr->add("finalCost", getOcs2StateCost(getRandomCost(nx, 0))); + problem.finalSoftConstraintPtr->add("finalSoftCost", getOcs2StateCost(getRandomCost(nx, 0))); + + // constraints + problem.finalEqualityConstraintPtr->add("finalEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 3))); + problem.finalInequalityConstraintPtr->add("finalInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); + + const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); + problem.targetTrajectoriesPtr = &targetTrajectories; + + const scalar_t t = 0.5; + const vector_t x = vector_t::Random(nx); + const auto transcription = multiple_shooting::setupTerminalNode(problem, t, x); + const auto metrics = multiple_shooting::computeTerminalMetrics(problem, t, x); + + ASSERT_TRUE(metrics.isApprox(multiple_shooting::computeMetrics(transcription), 1e-12)); +} diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index 184548f1f..da2d9cd16 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -119,7 +119,7 @@ class SqpSolver : public SolverBase { /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u); + const vector_array_t& u, std::vector<Metrics>& metrics); /** Computes only the performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 23a7cc63d..11acf942b 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -187,7 +187,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f } // Make QP approximation linearQuadraticApproximationTimer_.startTimer(); - const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u); + const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u, metrics); linearQuadraticApproximationTimer_.endTimer(); // Solve QP @@ -213,7 +213,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f computeControllerTimer_.startTimer(); primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); - problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, metrics); + problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); ++numProblems_; @@ -287,7 +287,7 @@ PrimalSolution SqpSolver::toPrimalSolution(const std::vector<AnnotatedTime>& tim } PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u) { + const vector_array_t& x, const vector_array_t& u, std::vector<Metrics>& metrics) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -299,6 +299,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated stateInputIneqConstraints_.resize(N); constraintsProjection_.resize(N); projectionMultiplierCoefficients_.resize(N); + metrics.resize(N + 1); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { @@ -311,6 +312,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); + metrics[i] = multiple_shooting::computeMetrics(result); workerPerformance += multiple_shooting::computeEventPerformance(result); cost_[i] = std::move(result.cost); dynamics_[i] = std::move(result.dynamics); @@ -324,6 +326,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + metrics[i] = multiple_shooting::computeMetrics(result); workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); if (settings_.projectStateInputEqualityConstraints) { multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplier); @@ -343,6 +346,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); + metrics[i] = multiple_shooting::computeMetrics(result); workerPerformance += multiple_shooting::computeTerminalPerformance(result); cost_[i] = std::move(result.cost); stateInputEqConstraints_[i].resize(0, x[i].size()); @@ -500,8 +504,6 @@ sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::v stepInfo.du_norm = 0.0; stepInfo.performanceAfterStep = baseline; stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline); - // compute metrics for the baseline rollout - std::ignore = computePerformance(timeDiscretization, initState, x, u, metrics); if (settings_.printLinesearch) { std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; From 25649bad7185f7db2cf9ddb792610605024a036a Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 09:46:13 +0100 Subject: [PATCH 443/512] using function overloads in PerformanceIndexComputation --- .../PerformanceIndexComputation.h | 52 +++++++++---------- .../PerformanceIndexComputation.cpp | 34 ++++++------ .../testTranscriptionPerformanceIndex.cpp | 28 +++++----- ocs2_slp/src/SlpSolver.cpp | 6 +-- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 6 +-- 5 files changed, 65 insertions(+), 61 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h index d54192b62..50456edd8 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h @@ -40,12 +40,26 @@ namespace ocs2 { namespace multiple_shooting { /** - * Compute the performance index from the transcription for this node. - * @param transcription: multiple shooting transcription for this node. + * Compute the performance index from the transcription for an intermediate node. + * @param transcription: multiple shooting transcription for an intermediate node. * @param dt : Duration of the interval * @return Performance index for a single intermediate node. */ -PerformanceIndex computeIntermediatePerformance(const Transcription& transcription, scalar_t dt); +PerformanceIndex computePerformanceIndex(const Transcription& transcription, scalar_t dt); + +/** + * Compute the performance index from the transcription for the event node. + * @param transcription: multiple shooting transcription for this node. + * @return Performance index for the event node. + */ +PerformanceIndex computePerformanceIndex(const EventTranscription& transcription); + +/** + * Compute the performance index from the transcription for the terminal node. + * @param transcription: multiple shooting transcription for this node. + * @return Performance index for the terminal node. + */ +PerformanceIndex computePerformanceIndex(const TerminalTranscription& transcription); /** * Compute only the performance index for a single intermediate node. @@ -63,11 +77,16 @@ PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalCo scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); /** - * Compute the performance index from the transcription for the terminal node. - * @param transcription: multiple shooting transcription for this node. - * @return Performance index for the terminal node. + * Compute only the performance index for the event node. + * Corresponds to the performance index computed from EventTranscription returned by "multiple_shooting::setEventNode". + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the event node + * @param x : Pre-event state + * @param x_next : Post-event state + * @return Performance index for the event node. */ -PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcription); +PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + const vector_t& x_next); /** * Compute only the performance index for the terminal node. @@ -79,24 +98,5 @@ PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcr */ PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); -/** - * Compute the performance index from the transcription for the event node. - * @param transcription: multiple shooting transcription for this node. - * @return Performance index for the event node. - */ -PerformanceIndex computeEventPerformance(const EventTranscription& transcription); - -/** - * Compute only the performance index for the event node. - * Corresponds to the performance index computed from EventTranscription returned by "multiple_shooting::setEventNode". - * @param optimalControlProblem : Definition of the optimal control problem - * @param t : Time at the event node - * @param x : Pre-event state - * @param x_next : Post-event state - * @return Performance index for the event node. - */ -PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next); - } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp index d78d5d3d2..278442fa4 100644 --- a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp +++ b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp @@ -37,7 +37,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { -PerformanceIndex computeIntermediatePerformance(const Transcription& transcription, scalar_t dt) { +PerformanceIndex computePerformanceIndex(const Transcription& transcription, scalar_t dt) { PerformanceIndex performance; // Dynamics @@ -57,15 +57,12 @@ PerformanceIndex computeIntermediatePerformance(const Transcription& transcripti return performance; } -PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, - scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { - const auto metrics = computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); - return toPerformanceIndex(metrics, dt); -} - -PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcription) { +PerformanceIndex computePerformanceIndex(const EventTranscription& transcription) { PerformanceIndex performance; + // Dynamics + performance.dynamicsViolationSSE = transcription.dynamics.f.squaredNorm(); + performance.cost = transcription.cost.f; // Equality constraints @@ -77,17 +74,9 @@ PerformanceIndex computeTerminalPerformance(const TerminalTranscription& transcr return performance; } -PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { - const auto metrics = computeTerminalMetrics(optimalControlProblem, t, x); - return toPerformanceIndex(metrics); -} - -PerformanceIndex computeEventPerformance(const EventTranscription& transcription) { +PerformanceIndex computePerformanceIndex(const TerminalTranscription& transcription) { PerformanceIndex performance; - // Dynamics - performance.dynamicsViolationSSE = transcription.dynamics.f.squaredNorm(); - performance.cost = transcription.cost.f; // Equality constraints @@ -99,11 +88,22 @@ PerformanceIndex computeEventPerformance(const EventTranscription& transcription return performance; } +PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { + const auto metrics = computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); + return toPerformanceIndex(metrics, dt); +} + PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { const auto metrics = computeEventMetrics(optimalControlProblem, t, x, x_next); return toPerformanceIndex(metrics); } +PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { + const auto metrics = computeTerminalMetrics(optimalControlProblem, t, x); + return toPerformanceIndex(metrics); +} + } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp b/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp index 73b2472e8..7fb21e8e5 100644 --- a/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp +++ b/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp @@ -37,13 +37,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace ocs2; -TEST(test_transcription, intermediate_performance) { +TEST(test_transcription_performance, intermediate) { constexpr int nx = 2; constexpr int nu = 2; // optimal control problem OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/sqp_test_generated"); + // equality constraints + problem.equalityConstraintPtr->add("equalityConstraint", getOcs2Constraints(getRandomConstraints(nx, nu, 2))); + problem.stateEqualityConstraintPtr->add("stateEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 1))); // inequality constraints problem.inequalityConstraintPtr->add("inequalityConstraint", getOcs2Constraints(getRandomConstraints(nx, nu, 3))); problem.stateInequalityConstraintPtr->add("stateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); @@ -51,19 +54,18 @@ TEST(test_transcription, intermediate_performance) { auto discretizer = selectDynamicsDiscretization(SensitivityIntegratorType::RK4); auto sensitivityDiscretizer = selectDynamicsSensitivityDiscretization(SensitivityIntegratorType::RK4); - scalar_t t = 0.5; - scalar_t dt = 0.1; + const scalar_t t = 0.5; + const scalar_t dt = 0.1; const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); const vector_t u = (vector_t(nu) << 0.1, 1.3).finished(); const auto transcription = multiple_shooting::setupIntermediateNode(problem, sensitivityDiscretizer, t, dt, x, x_next, u); - const auto performance = multiple_shooting::computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); - ASSERT_TRUE(performance.isApprox(multiple_shooting::computeIntermediatePerformance(transcription, dt), 1e-12)); + ASSERT_TRUE(performance.isApprox(multiple_shooting::computePerformanceIndex(transcription, dt), 1e-12)); } -TEST(test_transcription, event_performance) { +TEST(test_transcription_performance, event) { constexpr int nx = 2; // optimal control problem @@ -77,7 +79,8 @@ TEST(test_transcription, event_performance) { // cost problem.preJumpCostPtr->add("eventCost", getOcs2StateCost(getRandomCost(nx, 0))); - // inequality constraints + // constraints + problem.preJumpEqualityConstraintPtr->add("preJumpEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 3))); problem.preJumpInequalityConstraintPtr->add("preJumpInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); @@ -89,10 +92,10 @@ TEST(test_transcription, event_performance) { const auto transcription = multiple_shooting::setupEventNode(problem, t, x, x_next); const auto performance = multiple_shooting::computeEventPerformance(problem, t, x, x_next); - ASSERT_TRUE(performance.isApprox(multiple_shooting::computeEventPerformance(transcription), 1e-12)); + ASSERT_TRUE(performance.isApprox(multiple_shooting::computePerformanceIndex(transcription), 1e-12)); } -TEST(test_transcription, terminal_performance) { +TEST(test_transcription_performance, terminal) { constexpr int nx = 3; // optimal control problem @@ -102,16 +105,17 @@ TEST(test_transcription, terminal_performance) { problem.finalCostPtr->add("finalCost", getOcs2StateCost(getRandomCost(nx, 0))); problem.finalSoftConstraintPtr->add("finalSoftCost", getOcs2StateCost(getRandomCost(nx, 0))); - // inequality constraints + // constraints + problem.finalEqualityConstraintPtr->add("finalEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 3))); problem.finalInequalityConstraintPtr->add("finalInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); problem.targetTrajectoriesPtr = &targetTrajectories; - scalar_t t = 0.5; + const scalar_t t = 0.5; const vector_t x = vector_t::Random(nx); const auto transcription = multiple_shooting::setupTerminalNode(problem, t, x); const auto performance = multiple_shooting::computeTerminalPerformance(problem, t, x); - ASSERT_TRUE(performance.isApprox(multiple_shooting::computeTerminalPerformance(transcription), 1e-12)); + ASSERT_TRUE(performance.isApprox(multiple_shooting::computePerformanceIndex(transcription), 1e-12)); } diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 35789f742..87121b30a 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -314,7 +314,7 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); - workerPerformance += multiple_shooting::computeEventPerformance(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result); cost_[i] = std::move(result.cost); dynamics_[i] = std::move(result.dynamics); stateInputEqConstraints_[i].resize(0, x[i].size()); @@ -327,7 +327,7 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); - workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); + workerPerformance += multiple_shooting::computePerformanceIndex(result, dt); multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplier); cost_[i] = std::move(result.cost); dynamics_[i] = std::move(result.dynamics); @@ -344,7 +344,7 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); - workerPerformance += multiple_shooting::computeTerminalPerformance(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result); cost_[i] = std::move(result.cost); stateIneqConstraints_[i] = std::move(result.ineqConstraints); } diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 11acf942b..aa241a1ac 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -313,7 +313,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); metrics[i] = multiple_shooting::computeMetrics(result); - workerPerformance += multiple_shooting::computeEventPerformance(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result); cost_[i] = std::move(result.cost); dynamics_[i] = std::move(result.dynamics); stateInputEqConstraints_[i].resize(0, x[i].size()); @@ -327,7 +327,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); metrics[i] = multiple_shooting::computeMetrics(result); - workerPerformance += multiple_shooting::computeIntermediatePerformance(result, dt); + workerPerformance += multiple_shooting::computePerformanceIndex(result, dt); if (settings_.projectStateInputEqualityConstraints) { multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplier); } @@ -347,7 +347,7 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); metrics[i] = multiple_shooting::computeMetrics(result); - workerPerformance += multiple_shooting::computeTerminalPerformance(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result); cost_[i] = std::move(result.cost); stateInputEqConstraints_[i].resize(0, x[i].size()); stateIneqConstraints_[i] = std::move(result.ineqConstraints); From 85a030c506844db06c4ee73a1ad68cc5015e1cc3 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 09:57:58 +0100 Subject: [PATCH 444/512] minor change --- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index aa241a1ac..cc771dd55 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -177,7 +177,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Bookkeeping performanceIndeces_.clear(); - std::vector<Metrics> metrics(timeDiscretization.size()); + std::vector<Metrics> metrics; int iter = 0; sqp::Convergence convergence = sqp::Convergence::FALSE; @@ -445,7 +445,7 @@ sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::v scalar_t alpha = 1.0; vector_array_t xNew(x.size()); vector_array_t uNew(u.size()); - std::vector<Metrics> metricsNew(x.size()); + std::vector<Metrics> metricsNew(metrics.size()); do { // Compute step multiple_shooting::incrementTrajectory(u, du, alpha, uNew); From f363c8994fca922e5da33cb3d8684fbdd701f2d3 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 11:50:42 +0100 Subject: [PATCH 445/512] extending fussy comparison --- ocs2_core/include/ocs2_core/misc/Numerics.h | 95 +++++++++++++++------ 1 file changed, 69 insertions(+), 26 deletions(-) diff --git a/ocs2_core/include/ocs2_core/misc/Numerics.h b/ocs2_core/include/ocs2_core/misc/Numerics.h index 5003791ce..d2e45d2f4 100644 --- a/ocs2_core/include/ocs2_core/misc/Numerics.h +++ b/ocs2_core/include/ocs2_core/misc/Numerics.h @@ -42,22 +42,39 @@ namespace numerics { * * @tparam T1: data type of x. * @tparam T2: data type of y. - * @param [in] x: a floating-point number. - * @param [in] y: a floating-point number. + * @tparam T3: data type of prec. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @param [in] prec: The comparison precision. * @return bool: true if x=y. */ -template <class T1, class T2> -typename std::enable_if<std::is_floating_point<typename std::remove_reference<T1>::type>::value || - std::is_floating_point<typename std::remove_reference<T2>::type>::value, - bool>::type -almost_eq(T1&& x, T2&& y) { - using TypeResult = typename std::conditional<std::is_floating_point<typename std::remove_reference<T1>::type>::value, - typename std::remove_reference<T1>::type, typename std::remove_reference<T2>::type>::type; +template <class T1, class T2, class T3> +bool almost_eq(T1&& x, T2&& y, T3&& prec) { + static_assert(std::is_floating_point<typename std::remove_reference<T1>::type>::value, "First argument is not floating point!"); + static_assert(std::is_floating_point<typename std::remove_reference<T2>::type>::value, "Second argument is not floating point!"); + static_assert(std::is_floating_point<typename std::remove_reference<T3>::type>::value, "prec is not floating point!"); // the machine epsilon has to be scaled to the magnitude of the values used - // and multiplied by the desired precision in ULPs (units in the last place) - return std::abs(x - y) <= std::numeric_limits<TypeResult>::epsilon() * std::abs(x + y) - // unless the result is subnormal - || std::abs(x - y) < std::numeric_limits<TypeResult>::min(); + // and multiplied by the desired precision unless the result is subnormal + using Type = const std::remove_reference_t<T1>; + const auto absDiff = std::abs(x - static_cast<Type>(y)); + const auto magnitude = std::min(std::abs(x), std::abs(static_cast<Type>(y))); + return absDiff <= static_cast<Type>(prec) * magnitude || absDiff < std::numeric_limits<Type>::min(); +} + +/** + * Almost equal which uses machine epsilon to compare floating-point values for equality. + * refer to: https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon + * + * @tparam T1: data type of x. + * @tparam T2: data type of y. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @return bool: true if x=y. + */ +template <class T1, class T2> +bool almost_eq(T1&& x, T2&& y) { + const auto prec = std::numeric_limits<std::remove_reference_t<T1>>::epsilon(); + return almost_eq(x, y, prec); } /** @@ -65,15 +82,28 @@ almost_eq(T1&& x, T2&& y) { * * @tparam T1: data type of x. * @tparam T2: data type of y. - * @param [in] x: a floating-point number. - * @param [in] y: a floating-point number. + * @tparam T3: data type of prec. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @param [in] prec: The comparison precision. * @return bool: true if x<=y. */ -template <class T1, class T2> -typename std::enable_if<std::is_floating_point<typename std::remove_reference<T1>::type>::value || - std::is_floating_point<typename std::remove_reference<T2>::type>::value, - bool>::type -almost_le(T1&& x, T2&& y) { +template <class T1, class T2, class T3> +bool almost_le(T1&& x, T2&& y, T3&& prec) { + return x < y || almost_eq(x, y, prec); +} + +/** + * Almost less-equal which uses machine epsilon to compare floating-point values for equality. + * + * @tparam T1: data type of x. + * @tparam T2: data type of y. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @return bool: true if x<=y. + */ +template <class T1, class T2, class T3> +bool almost_le(T1&& x, T2&& y) { return x < y || almost_eq(x, y); } @@ -82,15 +112,28 @@ almost_le(T1&& x, T2&& y) { * * @tparam T1: data type of x. * @tparam T2: data type of y. - * @param [in] x: a floating-point number. - * @param [in] y: a floating-point number. + * @tparam T3: data type of prec. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @param [in] prec: The comparison precision. + * @return bool: true if x>=y. + */ +template <class T1, class T2, class T3> +bool almost_ge(T1&& x, T2&& y, T3&& prec) { + return x > y || almost_eq(x, y, prec); +} + +/** + * Almost greater-equal which uses machine epsilon to compare floating-point values for equality. + * + * @tparam T1: data type of x. + * @tparam T2: data type of y. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. * @return bool: true if x>=y. */ template <class T1, class T2> -typename std::enable_if<std::is_floating_point<typename std::remove_reference<T1>::type>::value || - std::is_floating_point<typename std::remove_reference<T2>::type>::value, - bool>::type -almost_ge(T1&& x, T2&& y) { +bool almost_ge(T1&& x, T2&& y) { return x > y || almost_eq(x, y); } From 861666ac760f39d2c9e990acd194d5e082401fce Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 12:43:47 +0100 Subject: [PATCH 446/512] fixing const issue of Transcription. extracting inequality constraints --- .../ocs2_oc/multiple_shooting/Transcription.h | 10 ++++---- .../OptimalControlProblemHelperFunction.cpp | 24 +++++++++++++++++++ 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h index 168322fca..7809b4222 100644 --- a/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h @@ -64,9 +64,8 @@ struct Transcription { * @param u : Input, taken to be constant across the interval. * @return multiple shooting transcription for this node. */ -Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, - DynamicsSensitivityDiscretizer& sensitivityDiscretizer, scalar_t t, scalar_t dt, const vector_t& x, - const vector_t& x_next, const vector_t& u); +Transcription setupIntermediateNode(OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); /** * Apply the state-input equality constraint projection for a single intermediate node transcription. @@ -93,7 +92,7 @@ struct TerminalTranscription { * @param x : Terminal state * @return multiple shooting transcription for the terminal node. */ -TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); +TerminalTranscription setupTerminalNode(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); /** * Results of the transcription at an event @@ -114,8 +113,7 @@ struct EventTranscription { * @param x_next : Post-event state * @return multiple shooting transcription for the event node. */ -EventTranscription setupEventNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next); +EventTranscription setupEventNode(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); } // namespace multiple_shooting } // namespace ocs2 diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp index 7d61d273a..75e44d649 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp @@ -188,6 +188,9 @@ const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, con if (ocp.finalEqualityConstraintPtr->getTermIndex(name, index)) { return &metrics.stateEqConstraint[index]; + } else if (ocp.finalInequalityConstraintPtr->getTermIndex(name, index)) { + return &metrics.stateIneqConstraint[index]; + } else { return nullptr; } @@ -225,6 +228,13 @@ bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::s } return true; + } else if (ocp.preJumpInequalityConstraintPtr->getTermIndex(name, index)) { + constraintArray.reserve(metricsArray.size()); + for (const auto& m : metricsArray) { + constraintArray.emplace_back(m.stateIneqConstraint[index]); + } + return true; + } else { return false; } @@ -280,6 +290,20 @@ bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const s } return true; + } else if (ocp.inequalityConstraintPtr->getTermIndex(name, index)) { + constraintTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + constraintTraj.emplace_back(m.stateInputIneqConstraint[index]); + } + return true; + + } else if (ocp.stateInequalityConstraintPtr->getTermIndex(name, index)) { + constraintTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + constraintTraj.emplace_back(m.stateIneqConstraint[index]); + } + return true; + } else { return false; } From a1e3449854a6cd937b514fb919468d575a199c67 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 12:59:08 +0100 Subject: [PATCH 447/512] using numerics::almost_eq instead of fuzzyCompares --- .../include/ocs2_core/model_data/Metrics.h | 59 ++++------------- ocs2_core/src/model_data/Metrics.cpp | 66 +++++++++++++++---- .../ocs2_oc/oc_data/PerformanceIndex.h | 2 +- ocs2_oc/src/oc_data/PerformanceIndex.cpp | 24 +++---- 4 files changed, 77 insertions(+), 74 deletions(-) diff --git a/ocs2_core/include/ocs2_core/model_data/Metrics.h b/ocs2_core/include/ocs2_core/model_data/Metrics.h index bba21fb9c..dc01f54d5 100644 --- a/ocs2_core/include/ocs2_core/model_data/Metrics.h +++ b/ocs2_core/include/ocs2_core/model_data/Metrics.h @@ -53,17 +53,18 @@ struct LagrangianMetricsConstRef { }; /** - * The collection of cost, equality constraints, and LagrangianMetrics structure for all possible constraint terms (handled by - * Lagrangian method) in a particular time point. - * cost : The total cost in a particular time point. - * stateEqConstraint : An array of all state equality constraints. - * stateInputEqConstraint : An array of all state-input equality constraints. - * stateIneqConstraint : An array of all state inequality constraints. - * stateInputIneqConstraint : An array of all state-input inequality constraints. - * stateEqLagrangian : An array of state equality constraint terms handled by Lagrangian method. - * stateIneqLagrangian : An array of state inequality constraint terms handled by Lagrangian method. - * stateInputEqLagrangian : An array of state-input equality constraint terms handled by Lagrangian method. - * stateInputIneqLagrangian : An array of state-input inequality constraint terms handled by Lagrangian method. + * The collection of cost, dynamics violation, constraints, and LagrangianMetrics structure for all possible constraint + * terms (handled by Lagrangian method) in a point of time. + * cost : The total cost in a particular time point. + * dynamicsViolation : The vector of dynamics violation. + * stateEqConstraint : An array of all state equality constraints. + * stateInputEqConstraint : An array of all state-input equality constraints. + * stateIneqConstraint : An array of all state inequality constraints. + * stateInputIneqConstraint : An array of all state-input inequality constraints. + * stateEqLagrangian : An array of state equality constraint terms handled by Lagrangian method. + * stateIneqLagrangian : An array of state inequality constraint terms handled by Lagrangian method. + * stateInputEqLagrangian : An array of state-input equality constraint terms handled by Lagrangian method. + * stateInputIneqLagrangian : An array of state-input inequality constraint terms handled by Lagrangian method. */ struct Metrics { // Cost @@ -87,42 +88,10 @@ struct Metrics { std::vector<LagrangianMetrics> stateInputIneqLagrangian; /** Exchanges the values of Metrics */ - void swap(Metrics& other) { - // Cost - std::swap(cost, other.cost); - // Dynamics violation - dynamicsViolation.swap(other.dynamicsViolation); - // Equality constraints - stateEqConstraint.swap(other.stateEqConstraint); - stateInputEqConstraint.swap(other.stateInputEqConstraint); - // Inequality constraints - stateIneqConstraint.swap(other.stateIneqConstraint); - stateInputIneqConstraint.swap(other.stateInputIneqConstraint); - // Lagrangians - stateEqLagrangian.swap(other.stateEqLagrangian); - stateIneqLagrangian.swap(other.stateIneqLagrangian); - stateInputEqLagrangian.swap(other.stateInputEqLagrangian); - stateInputIneqLagrangian.swap(other.stateInputIneqLagrangian); - } + void swap(Metrics& other); /** Clears the value of the Metrics */ - void clear() { - // Cost - cost = 0.0; - // Dynamics violation - dynamicsViolation = vector_t(); - // Equality constraints - stateEqConstraint.clear(); - stateInputEqConstraint.clear(); - // Inequality constraints - stateIneqConstraint.clear(); - stateInputIneqConstraint.clear(); - // Lagrangians - stateEqLagrangian.clear(); - stateIneqLagrangian.clear(); - stateInputEqLagrangian.clear(); - stateInputIneqLagrangian.clear(); - } + void clear(); /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ bool isApprox(const Metrics& other, scalar_t prec = 1e-8) const; diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 4b37b5260..39e440aa7 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -29,27 +29,65 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_core/model_data/Metrics.h" -#include <limits> +#include <ocs2_core/misc/Numerics.h> namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -/** Returns true if *this is approximately equal to other, within the precision determined by prec. */ +void Metrics::swap(Metrics& other) { + // Cost + std::swap(cost, other.cost); + // Dynamics violation + dynamicsViolation.swap(other.dynamicsViolation); + // Equality constraints + stateEqConstraint.swap(other.stateEqConstraint); + stateInputEqConstraint.swap(other.stateInputEqConstraint); + // Inequality constraints + stateIneqConstraint.swap(other.stateIneqConstraint); + stateInputIneqConstraint.swap(other.stateInputIneqConstraint); + // Lagrangians + stateEqLagrangian.swap(other.stateEqLagrangian); + stateIneqLagrangian.swap(other.stateIneqLagrangian); + stateInputEqLagrangian.swap(other.stateInputEqLagrangian); + stateInputIneqLagrangian.swap(other.stateInputIneqLagrangian); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void Metrics::clear() { + // Cost + cost = 0.0; + // Dynamics violation + dynamicsViolation = vector_t(); + // Equality constraints + stateEqConstraint.clear(); + stateInputEqConstraint.clear(); + // Inequality constraints + stateIneqConstraint.clear(); + stateInputIneqConstraint.clear(); + // Lagrangians + stateEqLagrangian.clear(); + stateIneqLagrangian.clear(); + stateInputEqLagrangian.clear(); + stateInputIneqLagrangian.clear(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ bool Metrics::isApprox(const Metrics& other, scalar_t prec) const { - bool flag = std::abs(this->cost - other.cost) <= prec * std::min(std::abs(this->cost), std::abs(other.cost)) || - std::abs(this->cost - other.cost) < std::numeric_limits<scalar_t>::min(); - flag = flag && this->dynamicsViolation.isApprox(other.dynamicsViolation, prec); - flag = flag && toVector(this->stateEqConstraint).isApprox(toVector(other.stateEqConstraint), prec); - flag = flag && toVector(this->stateInputEqConstraint).isApprox(toVector(other.stateInputEqConstraint), prec); - flag = flag && toVector(this->stateIneqConstraint).isApprox(toVector(other.stateIneqConstraint), prec); - flag = flag && toVector(this->stateInputIneqConstraint).isApprox(toVector(other.stateInputIneqConstraint), prec); - flag = flag && toVector(this->stateEqLagrangian).isApprox(toVector(other.stateEqLagrangian), prec); - flag = flag && toVector(this->stateIneqLagrangian).isApprox(toVector(other.stateIneqLagrangian), prec); - flag = flag && toVector(this->stateInputEqLagrangian).isApprox(toVector(other.stateInputEqLagrangian), prec); - flag = flag && toVector(this->stateInputIneqLagrangian).isApprox(toVector(other.stateInputIneqLagrangian), prec); - return flag; + return numerics::almost_eq(this->cost, other.cost, prec) && this->dynamicsViolation.isApprox(other.dynamicsViolation, prec) && + toVector(this->stateEqConstraint).isApprox(toVector(other.stateEqConstraint), prec) && + toVector(this->stateInputEqConstraint).isApprox(toVector(other.stateInputEqConstraint), prec) && + toVector(this->stateIneqConstraint).isApprox(toVector(other.stateIneqConstraint), prec) && + toVector(this->stateInputIneqConstraint).isApprox(toVector(other.stateInputIneqConstraint), prec) && + toVector(this->stateEqLagrangian).isApprox(toVector(other.stateEqLagrangian), prec) && + toVector(this->stateIneqLagrangian).isApprox(toVector(other.stateIneqLagrangian), prec) && + toVector(this->stateInputEqLagrangian).isApprox(toVector(other.stateInputEqLagrangian), prec) && + toVector(this->stateInputIneqLagrangian).isApprox(toVector(other.stateInputIneqLagrangian), prec); } /******************************************************************************************************/ diff --git a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index 8bbce1ba7..7979cbe80 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -91,7 +91,7 @@ struct PerformanceIndex { PerformanceIndex& operator*=(const scalar_t c); /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ - bool isApprox(const PerformanceIndex other, const scalar_t prec = 1e-8) const; + bool isApprox(const PerformanceIndex& other, const scalar_t prec = 1e-8) const; }; /** Add performance indices. */ diff --git a/ocs2_oc/src/oc_data/PerformanceIndex.cpp b/ocs2_oc/src/oc_data/PerformanceIndex.cpp index f4e01044c..962c4622d 100644 --- a/ocs2_oc/src/oc_data/PerformanceIndex.cpp +++ b/ocs2_oc/src/oc_data/PerformanceIndex.cpp @@ -30,7 +30,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_oc/oc_data/PerformanceIndex.h" #include <iomanip> -#include <limits> + +#include <ocs2_core/misc/Numerics.h> namespace ocs2 { @@ -58,19 +59,14 @@ PerformanceIndex& PerformanceIndex::operator*=(const scalar_t c) { return *this; } -bool PerformanceIndex::isApprox(const PerformanceIndex other, const scalar_t prec) const { - auto fuzzyCompares = [&](const scalar_t a, const scalar_t b) { - return std::abs(a - b) <= prec * std::min(std::abs(a), std::abs(b)) || std::abs(a - b) < std::numeric_limits<scalar_t>::min(); - }; - bool isEqual = fuzzyCompares(this->merit, other.merit); - isEqual = isEqual && fuzzyCompares(this->cost, other.cost); - isEqual = isEqual && fuzzyCompares(this->dualFeasibilitiesSSE, other.dualFeasibilitiesSSE); - isEqual = isEqual && fuzzyCompares(this->dynamicsViolationSSE, other.dynamicsViolationSSE); - isEqual = isEqual && fuzzyCompares(this->equalityConstraintsSSE, other.equalityConstraintsSSE); - isEqual = isEqual && fuzzyCompares(this->inequalityConstraintsSSE, other.inequalityConstraintsSSE); - isEqual = isEqual && fuzzyCompares(this->equalityLagrangian, other.equalityLagrangian); - isEqual = isEqual && fuzzyCompares(this->inequalityLagrangian, other.inequalityLagrangian); - return isEqual; +bool PerformanceIndex::isApprox(const PerformanceIndex& other, const scalar_t prec) const { + return numerics::almost_eq(this->merit, other.merit, prec) && numerics::almost_eq(this->cost, other.cost, prec) && + numerics::almost_eq(this->dualFeasibilitiesSSE, other.dualFeasibilitiesSSE, prec) && + numerics::almost_eq(this->dynamicsViolationSSE, other.dynamicsViolationSSE, prec) && + numerics::almost_eq(this->equalityConstraintsSSE, other.equalityConstraintsSSE, prec) && + numerics::almost_eq(this->inequalityConstraintsSSE, other.inequalityConstraintsSSE, prec) && + numerics::almost_eq(this->equalityLagrangian, other.equalityLagrangian, prec) && + numerics::almost_eq(this->inequalityLagrangian, other.inequalityLagrangian, prec); } void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { From f75950b51086f7785d6207c4c30753ef5557757b Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 13:29:24 +0100 Subject: [PATCH 448/512] bugfix --- ocs2_oc/src/multiple_shooting/Transcription.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/ocs2_oc/src/multiple_shooting/Transcription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp index 8b95289c5..aacb1520f 100644 --- a/ocs2_oc/src/multiple_shooting/Transcription.cpp +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -37,9 +37,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace multiple_shooting { -Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, - DynamicsSensitivityDiscretizer& sensitivityDiscretizer, scalar_t t, scalar_t dt, const vector_t& x, - const vector_t& x_next, const vector_t& u) { +Transcription setupIntermediateNode(OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { // Results and short-hand notation Transcription transcription; auto& dynamics = transcription.dynamics; @@ -118,7 +117,7 @@ void projectTranscription(Transcription& transcription, bool extractProjectionMu } } -TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { +TerminalTranscription setupTerminalNode(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { // Results and short-hand notation TerminalTranscription transcription; auto& cost = transcription.cost; @@ -146,8 +145,7 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont return transcription; } -EventTranscription setupEventNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next) { +EventTranscription setupEventNode(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { // Results and short-hand notation EventTranscription transcription; auto& cost = transcription.cost; From 348ded27de0b19a4fe1544bfe5766f9e3b17dd2d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 13 Dec 2022 15:26:39 +0100 Subject: [PATCH 449/512] doc update --- ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h index e457fda46..567bba40b 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h @@ -134,8 +134,8 @@ class SolverBase { } /** - * Adds one observer to the vector of modules that observe solver's dual solution and optimized metrics. - * @note: These observer can slow down the MPC. Only employ them during debugging and remove them for deployment. + * Adds an observer to probe the dual solution or optimized metrics. + * @note: Observers will slow down the MPC. Only employ them during debugging and remove them for deployment. */ void addSolverObserver(std::unique_ptr<SolverObserver> observerModule) { solverObservers_.push_back(std::move(observerModule)); } From 1f684c87f1e65c49f6a89917d3d35a7edac157db Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 14 Dec 2022 11:32:52 +0100 Subject: [PATCH 450/512] removing termsSizePtr from constraint collection --- .../constraint/StateConstraintCollection.h | 7 ++--- .../StateInputConstraintCollection.h | 6 ++-- .../LoopshapingConstraintEliminatePattern.h | 6 ++-- .../LoopshapingConstraintOutputPattern.h | 6 ++-- .../constraint/LoopshapingStateConstraint.h | 9 +++--- .../constraint/StateConstraintCollection.cpp | 28 +++---------------- .../StateInputConstraintCollection.cpp | 28 +++---------------- .../LoopshapingConstraintEliminatePattern.cpp | 10 +++---- .../LoopshapingConstraintOutputPattern.cpp | 10 +++---- .../constraint/LoopshapingStateConstraint.cpp | 10 +++---- 10 files changed, 33 insertions(+), 87 deletions(-) diff --git a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h index f149f2ab0..a070aa46c 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h @@ -56,13 +56,12 @@ class StateConstraintCollection : public Collection<StateConstraint> { virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const; /** Get the constraint linear approximation */ - virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const; + virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const; /** Get the constraint quadratic approximation */ virtual VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, - const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const; + const PreComputation& preComp) const; protected: /** Copy constructor */ diff --git a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h index fbb86cde4..0eaafb700 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h @@ -57,13 +57,11 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> { /** Get the constraint linear approximation */ virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const; + const PreComputation& preComp) const; /** Get the constraint quadratic approximation */ virtual VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const; + const PreComputation& preComp) const; protected: /** Copy constructor */ diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintEliminatePattern.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintEliminatePattern.h index 4a261a9c5..3e3364f1e 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintEliminatePattern.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintEliminatePattern.h @@ -50,12 +50,10 @@ class LoopshapingConstraintEliminatePattern final : public LoopshapingStateInput LoopshapingConstraintEliminatePattern* clone() const override { return new LoopshapingConstraintEliminatePattern(*this); }; VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const override; + const PreComputation& preComp) const override; VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const override; + const PreComputation& preComp) const override; protected: using BASE::loopshapingDefinition_; diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintOutputPattern.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintOutputPattern.h index 29e4db8c1..e0a641a18 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintOutputPattern.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingConstraintOutputPattern.h @@ -50,12 +50,10 @@ class LoopshapingConstraintOutputPattern final : public LoopshapingStateInputCon LoopshapingConstraintOutputPattern* clone() const override { return new LoopshapingConstraintOutputPattern(*this); }; VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const override; + const PreComputation& preComp) const override; VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const override; + const PreComputation& preComp) const override; protected: using BASE::loopshapingDefinition_; diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h index 1d44f8470..7e96eb387 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h @@ -48,12 +48,11 @@ class LoopshapingStateConstraint final : public StateConstraintCollection { LoopshapingStateConstraint* clone() const override { return new LoopshapingStateConstraint(*this); } vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const override; - VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const override; - - VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp, - size_array_t* termsSizePtr = nullptr) const override; + VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const override; private: LoopshapingStateConstraint(const LoopshapingStateConstraint& other) = default; diff --git a/ocs2_core/src/constraint/StateConstraintCollection.cpp b/ocs2_core/src/constraint/StateConstraintCollection.cpp index b8d96aabc..4fbcda78f 100644 --- a/ocs2_core/src/constraint/StateConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateConstraintCollection.cpp @@ -76,29 +76,19 @@ vector_array_t StateConstraintCollection::getValue(scalar_t time, const vector_t /******************************************************************************************************/ /******************************************************************************************************/ VectorFunctionLinearApproximation StateConstraintCollection::getLinearApproximation(scalar_t time, const vector_t& state, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { VectorFunctionLinearApproximation linearApproximation(getNumConstraints(time), state.rows()); - if (termsSizePtr != nullptr) { - termsSizePtr->clear(); - termsSizePtr->reserve(this->terms_.size()); - } - // append linearApproximation of each constraintTerm size_t i = 0; for (const auto& constraintTerm : this->terms_) { - size_t nc = 0; if (constraintTerm->isActive(time)) { const auto constraintTermApproximation = constraintTerm->getLinearApproximation(time, state, preComp); - nc = constraintTermApproximation.f.rows(); + const size_t nc = constraintTermApproximation.f.rows(); linearApproximation.f.segment(i, nc) = constraintTermApproximation.f; linearApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx; i += nc; } - if (termsSizePtr != nullptr) { - termsSizePtr->push_back(nc); - } } return linearApproximation; @@ -108,8 +98,7 @@ VectorFunctionLinearApproximation StateConstraintCollection::getLinearApproximat /******************************************************************************************************/ /******************************************************************************************************/ VectorFunctionQuadraticApproximation StateConstraintCollection::getQuadraticApproximation(scalar_t time, const vector_t& state, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { const auto numConstraints = getNumConstraints(time); VectorFunctionQuadraticApproximation quadraticApproximation; @@ -117,26 +106,17 @@ VectorFunctionQuadraticApproximation StateConstraintCollection::getQuadraticAppr quadraticApproximation.dfdx.resize(numConstraints, state.rows()); quadraticApproximation.dfdxx.reserve(numConstraints); // Use reserve instead of resize to avoid unnecessary allocations. - if (termsSizePtr != nullptr) { - termsSizePtr->clear(); - termsSizePtr->reserve(this->terms_.size()); - } - // append quadraticApproximation of each constraintTerm size_t i = 0; for (const auto& constraintTerm : this->terms_) { - size_t nc = 0; if (constraintTerm->isActive(time)) { auto constraintTermApproximation = constraintTerm->getQuadraticApproximation(time, state, preComp); - nc = constraintTermApproximation.f.rows(); + const size_t nc = constraintTermApproximation.f.rows(); quadraticApproximation.f.segment(i, nc) = constraintTermApproximation.f; quadraticApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx; appendVectorToVectorByMoving(quadraticApproximation.dfdxx, std::move(constraintTermApproximation.dfdxx)); i += nc; } - if (termsSizePtr != nullptr) { - termsSizePtr->push_back(nc); - } } return quadraticApproximation; diff --git a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp index 94976265b..a6e1c8192 100644 --- a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp @@ -79,30 +79,20 @@ vector_array_t StateInputConstraintCollection::getValue(scalar_t time, const vec /******************************************************************************************************/ VectorFunctionLinearApproximation StateInputConstraintCollection::getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { VectorFunctionLinearApproximation linearApproximation(getNumConstraints(time), state.rows(), input.rows()); - if (termsSizePtr != nullptr) { - termsSizePtr->clear(); - termsSizePtr->reserve(this->terms_.size()); - } - // append linearApproximation of each constraintTerm size_t i = 0; for (const auto& constraintTerm : this->terms_) { - size_t nc = 0; if (constraintTerm->isActive(time)) { const auto constraintTermApproximation = constraintTerm->getLinearApproximation(time, state, input, preComp); - nc = constraintTermApproximation.f.rows(); + const size_t nc = constraintTermApproximation.f.rows(); linearApproximation.f.segment(i, nc) = constraintTermApproximation.f; linearApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx; linearApproximation.dfdu.middleRows(i, nc) = constraintTermApproximation.dfdu; i += nc; } - if (termsSizePtr != nullptr) { - termsSizePtr->push_back(nc); - } } return linearApproximation; @@ -113,8 +103,7 @@ VectorFunctionLinearApproximation StateInputConstraintCollection::getLinearAppro /******************************************************************************************************/ VectorFunctionQuadraticApproximation StateInputConstraintCollection::getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { const auto numConstraints = getNumConstraints(time); VectorFunctionQuadraticApproximation quadraticApproximation; @@ -125,18 +114,12 @@ VectorFunctionQuadraticApproximation StateInputConstraintCollection::getQuadrati quadraticApproximation.dfdux.reserve(numConstraints); quadraticApproximation.dfduu.reserve(numConstraints); - if (termsSizePtr != nullptr) { - termsSizePtr->clear(); - termsSizePtr->reserve(this->terms_.size()); - } - // append quadraticApproximation of each constraintTerm size_t i = 0; for (const auto& constraintTerm : this->terms_) { - size_t nc = 0; if (constraintTerm->isActive(time)) { auto constraintTermApproximation = constraintTerm->getQuadraticApproximation(time, state, input, preComp); - nc = constraintTermApproximation.f.rows(); + const size_t nc = constraintTermApproximation.f.rows(); quadraticApproximation.f.segment(i, nc) = constraintTermApproximation.f; quadraticApproximation.dfdx.middleRows(i, nc) = constraintTermApproximation.dfdx; quadraticApproximation.dfdu.middleRows(i, nc) = constraintTermApproximation.dfdu; @@ -145,9 +128,6 @@ VectorFunctionQuadraticApproximation StateInputConstraintCollection::getQuadrati appendVectorToVectorByMoving(quadraticApproximation.dfduu, std::move(constraintTermApproximation.dfduu)); i += nc; } - if (termsSizePtr != nullptr) { - termsSizePtr->push_back(nc); - } } return quadraticApproximation; diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintEliminatePattern.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintEliminatePattern.cpp index 6686c6176..3cb42f21b 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintEliminatePattern.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintEliminatePattern.cpp @@ -37,8 +37,7 @@ namespace ocs2 { /******************************************************************************************************/ VectorFunctionLinearApproximation LoopshapingConstraintEliminatePattern::getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { if (this->empty()) { return VectorFunctionLinearApproximation::Zero(0, x.rows(), u.rows()); } @@ -54,7 +53,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintEliminatePattern::getLine const auto filtStateDim = s_filter.getNumStates(); // Not const so we can move - auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system, termsSizePtr); + auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system); const auto numConstraints = g_system.f.rows(); VectorFunctionLinearApproximation g; @@ -84,8 +83,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintEliminatePattern::getLine /******************************************************************************************************/ VectorFunctionQuadraticApproximation LoopshapingConstraintEliminatePattern::getQuadraticApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { if (this->empty()) { return VectorFunctionQuadraticApproximation::Zero(0, x.rows(), u.rows()); } @@ -103,7 +101,7 @@ VectorFunctionQuadraticApproximation LoopshapingConstraintEliminatePattern::getQ const auto filtStateDim = x_filter.rows(); // Not const so we can move - auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system, termsSizePtr); + auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system); const auto numConstraints = h_system.f.rows(); VectorFunctionQuadraticApproximation h; diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintOutputPattern.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintOutputPattern.cpp index b07c44c81..d77e95a77 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintOutputPattern.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraintOutputPattern.cpp @@ -37,8 +37,7 @@ namespace ocs2 { /******************************************************************************************************/ VectorFunctionLinearApproximation LoopshapingConstraintOutputPattern::getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { if (this->empty()) { return VectorFunctionLinearApproximation::Zero(0, x.rows(), u.rows()); } @@ -52,7 +51,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintOutputPattern::getLinearA const auto filtStateDim = x.rows() - sysStateDim; // Not const so we can move - auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system, termsSizePtr); + auto g_system = StateInputConstraintCollection::getLinearApproximation(t, x_system, u_system, preComp_system); const auto numConstraints = g_system.f.rows(); VectorFunctionLinearApproximation g; @@ -72,8 +71,7 @@ VectorFunctionLinearApproximation LoopshapingConstraintOutputPattern::getLinearA /******************************************************************************************************/ VectorFunctionQuadraticApproximation LoopshapingConstraintOutputPattern::getQuadraticApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { if (this->empty()) { return VectorFunctionQuadraticApproximation::Zero(0, x.rows(), u.rows()); } @@ -89,7 +87,7 @@ VectorFunctionQuadraticApproximation LoopshapingConstraintOutputPattern::getQuad const auto filtStateDim = x_filter.rows(); // Not const so we can move - auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system, termsSizePtr); + auto h_system = StateInputConstraintCollection::getQuadraticApproximation(t, x_system, u_system, preComp_system); const auto numConstraints = h_system.f.rows(); VectorFunctionQuadraticApproximation h; diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp index a6cac38c7..af67c3761 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp @@ -54,8 +54,7 @@ vector_array_t LoopshapingStateConstraint::getValue(scalar_t t, const vector_t& /******************************************************************************************************/ /******************************************************************************************************/ VectorFunctionLinearApproximation LoopshapingStateConstraint::getLinearApproximation(scalar_t t, const vector_t& x, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { if (this->empty()) { return VectorFunctionLinearApproximation::Zero(0, x.rows()); } @@ -68,7 +67,7 @@ VectorFunctionLinearApproximation LoopshapingStateConstraint::getLinearApproxima const auto filtStateDim = x.rows() - sysStateDim; // Not const so we can move - auto c_system = StateConstraintCollection::getLinearApproximation(t, x_system, preComp_system, termsSizePtr); + auto c_system = StateConstraintCollection::getLinearApproximation(t, x_system, preComp_system); const auto numConstraints = c_system.f.rows(); VectorFunctionLinearApproximation c; @@ -86,8 +85,7 @@ VectorFunctionLinearApproximation LoopshapingStateConstraint::getLinearApproxima /******************************************************************************************************/ /******************************************************************************************************/ VectorFunctionQuadraticApproximation LoopshapingStateConstraint::getQuadraticApproximation(scalar_t t, const vector_t& x, - const PreComputation& preComp, - size_array_t* termsSizePtr) const { + const PreComputation& preComp) const { if (this->empty()) { return VectorFunctionQuadraticApproximation::Zero(0, x.rows()); } @@ -100,7 +98,7 @@ VectorFunctionQuadraticApproximation LoopshapingStateConstraint::getQuadraticApp const auto filtStateDim = x.rows() - sysStateDim; // Not const so we can move - const auto c_system = StateConstraintCollection::getQuadraticApproximation(t, x_system, preComp_system, termsSizePtr); + const auto c_system = StateConstraintCollection::getQuadraticApproximation(t, x_system, preComp_system); const auto numConstraints = c_system.f.rows(); VectorFunctionQuadraticApproximation c; From 50a9df8a0e658a2003878294fbe317e1ddda74e0 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 14 Dec 2022 12:10:24 +0100 Subject: [PATCH 451/512] adding getTermsSize to constraintCollection --- .../constraint/StateConstraintCollection.h | 7 ++-- .../StateInputConstraintCollection.h | 7 ++-- .../constraint/StateConstraintCollection.cpp | 16 +++++++-- .../StateInputConstraintCollection.cpp | 16 +++++++-- .../constraint/testConstraintCollection.cpp | 33 +++++++++++++++++++ 5 files changed, 69 insertions(+), 10 deletions(-) diff --git a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h index a070aa46c..f9daa7d35 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h @@ -49,8 +49,11 @@ class StateConstraintCollection : public Collection<StateConstraint> { ~StateConstraintCollection() override = default; StateConstraintCollection* clone() const override; - /** Returns the number of active constraints at given time. */ - virtual size_t getNumConstraints(scalar_t time) const; + /** Returns the number of active constraints at a given time. */ + size_t getNumConstraints(scalar_t time) const; + + /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */ + size_array_t getTermsSize(scalar_t time) const; /** Get an array of all constraints */ virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const; diff --git a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h index 0eaafb700..efd6fc69d 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h @@ -49,8 +49,11 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> { ~StateInputConstraintCollection() override = default; StateInputConstraintCollection* clone() const override; - /** Returns the number of active constraints at given time. */ - virtual size_t getNumConstraints(scalar_t time) const; + /** Returns the number of active constraints at a given time. */ + size_t getNumConstraints(scalar_t time) const; + + /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */ + size_array_t getTermsSize(scalar_t time) const; /** Get an array of all constraints */ virtual vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const; diff --git a/ocs2_core/src/constraint/StateConstraintCollection.cpp b/ocs2_core/src/constraint/StateConstraintCollection.cpp index 4fbcda78f..94dc0634a 100644 --- a/ocs2_core/src/constraint/StateConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateConstraintCollection.cpp @@ -48,17 +48,27 @@ StateConstraintCollection* StateConstraintCollection::clone() const { /******************************************************************************************************/ size_t StateConstraintCollection::getNumConstraints(scalar_t time) const { size_t numConstraints = 0; - - // accumulate number of constraints for each constraintTerm for (const auto& constraintTerm : this->terms_) { if (constraintTerm->isActive(time)) { numConstraints += constraintTerm->getNumConstraints(time); } } - return numConstraints; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +size_array_t StateConstraintCollection::getTermsSize(scalar_t time) const { + size_array_t termsSize(this->terms_.size(), 0); + for (size_t i = 0; i < this->terms_.size(); ++i) { + if (this->terms_[i]->isActive(time)) { + termsSize[i] = this->terms_[i]->getNumConstraints(time); + } + } + return termsSize; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp index a6e1c8192..ae3daa870 100644 --- a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp @@ -49,17 +49,27 @@ StateInputConstraintCollection* StateInputConstraintCollection::clone() const { /******************************************************************************************************/ size_t StateInputConstraintCollection::getNumConstraints(scalar_t time) const { size_t numConstraints = 0; - - // accumulate number of constraints for each constraintTerm for (const auto& constraintTerm : this->terms_) { if (constraintTerm->isActive(time)) { numConstraints += constraintTerm->getNumConstraints(time); } } - return numConstraints; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +size_array_t StateInputConstraintCollection::getTermsSize(scalar_t time) const { + size_array_t termsSize(this->terms_.size(), 0); + for (size_t i = 0; i < this->terms_.size(); ++i) { + if (this->terms_[i]->isActive(time)) { + termsSize[i] = this->terms_[i]->getNumConstraints(time); + } + } + return termsSize; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_core/test/constraint/testConstraintCollection.cpp b/ocs2_core/test/constraint/testConstraintCollection.cpp index 3eb641fa4..aef7c3fe5 100644 --- a/ocs2_core/test/constraint/testConstraintCollection.cpp +++ b/ocs2_core/test/constraint/testConstraintCollection.cpp @@ -27,6 +27,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ +#include <numeric> + #include <gtest/gtest.h> #include <ocs2_core/constraint/StateConstraintCollection.h> @@ -56,6 +58,37 @@ TEST(TestConstraintCollection, numberOfConstraints) { EXPECT_EQ(constraintCollection.getNumConstraints(0.0), addedConstraints); } +TEST(TestConstraintCollection, termsSize) { + ocs2::StateInputConstraintCollection constraintCollection; + + // Initially we have zero constraints for all types + auto termsSize = constraintCollection.getTermsSize(0.0); + EXPECT_EQ(termsSize.size(), 0); + EXPECT_EQ(std::accumulate(termsSize.begin(), termsSize.end(), 0), 0); + + // Add 2 Linear inequality constraint term, which has 2 constraints + constraintCollection.add("Constraint1", std::make_unique<TestDummyConstraint>()); + constraintCollection.add("Constraint2", std::make_unique<TestDummyConstraint>()); + constraintCollection.add("Constraint3", std::make_unique<TestDummyConstraint>()); + auto& constraint1 = constraintCollection.get<TestDummyConstraint>("Constraint1"); + const size_t constraint1Size = constraint1.getNumConstraints(0.0); + + // Check the right constraint size + termsSize = constraintCollection.getTermsSize(0.0); + EXPECT_EQ(termsSize.size(), 3); + if (termsSize.size() == 3) { + EXPECT_EQ(termsSize[0], constraint1Size); + EXPECT_EQ(termsSize[1], constraint1Size); + EXPECT_EQ(termsSize[2], constraint1Size); + } + + // Deactivate constraint1 + constraint1.setActivity(false); + termsSize = constraintCollection.getTermsSize(0.0); + EXPECT_EQ(termsSize.size(), 3); + EXPECT_EQ(std::accumulate(termsSize.begin(), termsSize.end(), 0), 2 * constraint1Size); +} + TEST(TestConstraintCollection, activatingConstraints) { ocs2::StateInputConstraintCollection constraintCollection; From c03cba4a7a84678b6b421b0a17e8ecb5cb6adb37 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 14 Dec 2022 12:38:07 +0100 Subject: [PATCH 452/512] fixing Transcription --- .../src/multiple_shooting/Transcription.cpp | 46 +++++++++++-------- 1 file changed, 27 insertions(+), 19 deletions(-) diff --git a/ocs2_oc/src/multiple_shooting/Transcription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp index 0daba99af..b6bd7569e 100644 --- a/ocs2_oc/src/multiple_shooting/Transcription.cpp +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -44,11 +44,11 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP Transcription transcription; auto& cost = transcription.cost; auto& dynamics = transcription.dynamics; + auto& constraintsSize = transcription.constraintsSize; auto& stateEqConstraints = transcription.stateEqConstraints; auto& stateInputEqConstraints = transcription.stateInputEqConstraints; auto& stateIneqConstraints = transcription.stateIneqConstraints; auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; - auto& constraintsSize = transcription.constraintsSize; // Dynamics // Discretization returns x_{k+1} = A_{k} * dx_{k} + B_{k} * du_{k} + b_{k} @@ -65,26 +65,30 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP // State equality constraints if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { - stateEqConstraints = optimalControlProblem.stateEqualityConstraintPtr->getLinearApproximation( - t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateEq); + constraintsSize.stateEq = optimalControlProblem.stateEqualityConstraintPtr->getTermsSize(t); + stateEqConstraints = + optimalControlProblem.stateEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); } // State-input equality constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { - stateInputEqConstraints = optimalControlProblem.equalityConstraintPtr->getLinearApproximation( - t, x, u, *optimalControlProblem.preComputationPtr, &constraintsSize.stateInputEq); + constraintsSize.stateInputEq = optimalControlProblem.equalityConstraintPtr->getTermsSize(t); + stateInputEqConstraints = + optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); } // State inequality constraints. if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { - stateIneqConstraints = optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation( - t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateIneq); + constraintsSize.stateIneq = optimalControlProblem.stateInequalityConstraintPtr->getTermsSize(t); + stateIneqConstraints = + optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); } // State-input inequality constraints. if (!optimalControlProblem.inequalityConstraintPtr->empty()) { - stateInputIneqConstraints = optimalControlProblem.inequalityConstraintPtr->getLinearApproximation( - t, x, u, *optimalControlProblem.preComputationPtr, &constraintsSize.stateInputIneq); + constraintsSize.stateInputIneq = optimalControlProblem.inequalityConstraintPtr->getTermsSize(t); + stateInputIneqConstraints = + optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); } return transcription; @@ -123,9 +127,9 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont // Results and short-hand notation TerminalTranscription transcription; auto& cost = transcription.cost; + auto& constraintsSize = transcription.constraintsSize; auto& eqConstraints = transcription.eqConstraints; auto& ineqConstraints = transcription.ineqConstraints; - auto& constraintsSize = transcription.constraintsSize; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); @@ -135,14 +139,16 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont // State equality constraints. if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { - eqConstraints = optimalControlProblem.finalEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr, - &constraintsSize.stateEq); + constraintsSize.stateEq = optimalControlProblem.finalEqualityConstraintPtr->getTermsSize(t); + eqConstraints = + optimalControlProblem.finalEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); } // State inequality constraints. if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { - ineqConstraints = optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation( - t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateIneq); + constraintsSize.stateIneq = optimalControlProblem.finalInequalityConstraintPtr->getTermsSize(t); + ineqConstraints = + optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); } return transcription; @@ -154,9 +160,9 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro EventTranscription transcription; auto& cost = transcription.cost; auto& dynamics = transcription.dynamics; + auto& constraintsSize = transcription.constraintsSize; auto& eqConstraints = transcription.eqConstraints; auto& ineqConstraints = transcription.ineqConstraints; - auto& constraintsSize = transcription.constraintsSize; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation; optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); @@ -172,14 +178,16 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro // State equality constraints. if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { - eqConstraints = optimalControlProblem.preJumpEqualityConstraintPtr->getLinearApproximation( - t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateEq); + constraintsSize.stateEq = optimalControlProblem.preJumpEqualityConstraintPtr->getTermsSize(t); + eqConstraints = + optimalControlProblem.preJumpEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); } // State inequality constraints. if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { - ineqConstraints = optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation( - t, x, *optimalControlProblem.preComputationPtr, &constraintsSize.stateIneq); + constraintsSize.stateIneq = optimalControlProblem.preJumpInequalityConstraintPtr->getTermsSize(t); + ineqConstraints = + optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); } return transcription; From 25942baae204a9ea5ef0ad918d2e2dbde1d5afd8 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 14 Dec 2022 12:41:37 +0100 Subject: [PATCH 453/512] 8 --> N/10 --- ocs2_oc/src/multiple_shooting/Helpers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_oc/src/multiple_shooting/Helpers.cpp b/ocs2_oc/src/multiple_shooting/Helpers.cpp index c474b1d09..5df5f400d 100644 --- a/ocs2_oc/src/multiple_shooting/Helpers.cpp +++ b/ocs2_oc/src/multiple_shooting/Helpers.cpp @@ -129,7 +129,7 @@ ProblemMetrics toProblemMetrics(const std::vector<AnnotatedTime>& time, std::vec // resize ProblemMetrics problemMetrics; problemMetrics.intermediates.reserve(N); - problemMetrics.preJumps.reserve(8); // the size is just a guess + problemMetrics.preJumps.reserve(N / 10); // the size is just a guess problemMetrics.final = std::move(metrics.back()); for (int i = 0; i < N; ++i) { From 6b43b13e97737533d057f70fe7950ff52a74b49d Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 14 Dec 2022 14:45:33 +0100 Subject: [PATCH 454/512] Extracting problemMetrics for SLP --- ocs2_slp/include/ocs2_slp/SlpSolver.h | 14 +++++---- ocs2_slp/src/SlpSolver.cpp | 43 +++++++++++++++++---------- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 1 - 3 files changed, 35 insertions(+), 23 deletions(-) diff --git a/ocs2_slp/include/ocs2_slp/SlpSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h index 37303982e..7d0623f0e 100644 --- a/ocs2_slp/include/ocs2_slp/SlpSolver.h +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -65,9 +65,7 @@ class SlpSolver : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } - const ProblemMetrics& getSolutionMetrics() const override { - throw std::runtime_error("[SlpSolver] getSolutionMetrics() not available yet."); - } + const ProblemMetrics& getSolutionMetrics() const override { return problemMetrics_; } size_t getNumIterations() const override { return totalNumIterations_; } @@ -124,11 +122,11 @@ class SlpSolver : public SolverBase { /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u); + const vector_array_t& u, std::vector<Metrics>& metrics); /** Computes only the performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u); + const vector_array_t& u, std::vector<Metrics>& metrics); /** Returns solution of the QP subproblem in delta coordinates: */ struct OcpSubproblemSolution { @@ -143,7 +141,8 @@ class SlpSolver : public SolverBase { /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ slp::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, - const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u); + const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u, + std::vector<Metrics>& metrics); /** Determine convergence after a step */ slp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const slp::StepInfo& stepInfo) const; @@ -179,6 +178,9 @@ class SlpSolver : public SolverBase { // Iteration performance log std::vector<PerformanceIndex> performanceIndeces_; + // The ProblemMetrics associated to primalSolution_ + ProblemMetrics problemMetrics_; + // Benchmarking size_t numProblems_{0}; size_t totalNumIterations_{0}; diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 87121b30a..428bf9bad 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/multiple_shooting/Helpers.h> #include <ocs2_oc/multiple_shooting/Initialization.h> +#include <ocs2_oc/multiple_shooting/MetricsComputation.h> #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> #include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/precondition/Ruzi.h> @@ -177,6 +178,7 @@ void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Bookkeeping performanceIndeces_.clear(); + std::vector<Metrics> metrics; int iter = 0; slp::Convergence convergence = slp::Convergence::FALSE; @@ -186,7 +188,7 @@ void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f } // Make QP approximation linearQuadraticApproximationTimer_.startTimer(); - const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u); + const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u, metrics); linearQuadraticApproximationTimer_.endTimer(); // Solve LP @@ -197,7 +199,7 @@ void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Apply step linesearchTimer_.startTimer(); - const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u); + const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, metrics); performanceIndeces_.push_back(stepInfo.performanceAfterStep); linesearchTimer_.endTimer(); @@ -211,6 +213,7 @@ void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f computeControllerTimer_.startTimer(); primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); ++numProblems_; @@ -290,7 +293,7 @@ PrimalSolution SlpSolver::toPrimalSolution(const std::vector<AnnotatedTime>& tim } PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u) { + const vector_array_t& x, const vector_array_t& u, std::vector<Metrics>& metrics) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -302,6 +305,7 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated stateInputIneqConstraints_.resize(N); constraintsProjection_.resize(N); projectionMultiplierCoefficients_.resize(N); + metrics.resize(N + 1); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { @@ -314,6 +318,7 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); + metrics[i] = multiple_shooting::computeMetrics(result); workerPerformance += multiple_shooting::computePerformanceIndex(result); cost_[i] = std::move(result.cost); dynamics_[i] = std::move(result.dynamics); @@ -327,6 +332,7 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + metrics[i] = multiple_shooting::computeMetrics(result); workerPerformance += multiple_shooting::computePerformanceIndex(result, dt); multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplier); cost_[i] = std::move(result.cost); @@ -344,6 +350,7 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); + metrics[i] = multiple_shooting::computeMetrics(result); workerPerformance += multiple_shooting::computePerformanceIndex(result); cost_[i] = std::move(result.cost); stateIneqConstraints_[i] = std::move(result.ineqConstraints); @@ -365,27 +372,29 @@ PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<Annotated } PerformanceIndex SlpSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u) { - // Problem horizon + const vector_array_t& u, std::vector<Metrics>& metrics) { + // Problem size const int N = static_cast<int>(time.size()) - 1; + metrics.resize(N + 1); std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; - PerformanceIndex workerPerformance; // Accumulate performance in local variable int i = timeIndex++; while (i < N) { if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node - workerPerformance += multiple_shooting::computeEventPerformance(ocpDefinition, time[i].time, x[i], x[i + 1]); + metrics[i] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]); + performance[workerId] += toPerformanceIndex(metrics[i]); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + metrics[i] = multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + performance[workerId] += toPerformanceIndex(metrics[i], dt); } i = timeIndex++; @@ -393,16 +402,16 @@ PerformanceIndex SlpSolver::computePerformance(const std::vector<AnnotatedTime>& if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); - workerPerformance += multiple_shooting::computeTerminalPerformance(ocpDefinition, tN, x[N]); + metrics[N] = multiple_shooting::computeTerminalMetrics(ocpDefinition, tN, x[N]); + performance[workerId] += toPerformanceIndex(metrics[N]); } - - // Accumulate! Same worker might run multiple tasks - performance[workerId] += workerPerformance; }; runParallel(std::move(parallelTask)); - // Account for init state in performance - performance.front().dynamicsViolationSSE += (initState - x.front()).squaredNorm(); + // Account for initial state in performance + const vector_t initDynamicsViolation = initState - x.front(); + metrics.front().dynamicsViolation += initDynamicsViolation; + performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm(); // Sum performance of the threads PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); @@ -412,7 +421,7 @@ PerformanceIndex SlpSolver::computePerformance(const std::vector<AnnotatedTime>& slp::StepInfo SlpSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, - vector_array_t& u) { + vector_array_t& u, std::vector<Metrics>& metrics) { using StepType = FilterLinesearch::StepType; /* @@ -438,13 +447,14 @@ slp::StepInfo SlpSolver::takeStep(const PerformanceIndex& baseline, const std::v scalar_t alpha = 1.0; vector_array_t xNew(x.size()); vector_array_t uNew(u.size()); + std::vector<Metrics> metricsNew(metrics.size()); do { // Compute step multiple_shooting::incrementTrajectory(u, du, alpha, uNew); multiple_shooting::incrementTrajectory(x, dx, alpha, xNew); // Compute cost and constraints - const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew); + const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew, metricsNew); // Step acceptance and record step type bool stepAccepted; @@ -462,6 +472,7 @@ slp::StepInfo SlpSolver::takeStep(const PerformanceIndex& baseline, const std::v if (stepAccepted) { // Return if step accepted x = std::move(xNew); u = std::move(uNew); + metrics = std::move(metricsNew); // Prepare step info slp::StepInfo stepInfo; diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index cc771dd55..33a921e54 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -372,7 +372,6 @@ PerformanceIndex SqpSolver::computePerformance(const std::vector<AnnotatedTime>& const vector_array_t& u, std::vector<Metrics>& metrics) { // Problem size const int N = static_cast<int>(time.size()) - 1; - metrics.clear(); metrics.resize(N + 1); std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); From ea9ac8b060b8d75ffa7fd501cb137e6d0c489da4 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Wed, 14 Dec 2022 14:49:01 +0100 Subject: [PATCH 455/512] update doc --- .../include/ocs2_core/constraint/StateConstraintCollection.h | 2 +- .../ocs2_core/constraint/StateInputConstraintCollection.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h index f9daa7d35..79b112fce 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h @@ -55,7 +55,7 @@ class StateConstraintCollection : public Collection<StateConstraint> { /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */ size_array_t getTermsSize(scalar_t time) const; - /** Get an array of all constraints */ + /** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */ virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const; /** Get the constraint linear approximation */ diff --git a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h index efd6fc69d..aecee543f 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h @@ -55,7 +55,7 @@ class StateInputConstraintCollection : public Collection<StateInputConstraint> { /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */ size_array_t getTermsSize(scalar_t time) const; - /** Get an array of all constraints */ + /** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */ virtual vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const; /** Get the constraint linear approximation */ From 8f085d956706e30f02d17bf1e96df2e0802f0d6f Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 14 Dec 2022 18:55:41 +0100 Subject: [PATCH 456/512] add ocs2_ipm package --- ocs2_ipm/CMakeLists.txt | 114 +++ ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 111 +++ ocs2_ipm/include/ocs2_ipm/IpmInitialization.h | 62 ++ ocs2_ipm/include/ocs2_ipm/IpmMpc.h | 72 ++ .../ocs2_ipm/IpmPerformanceIndexComputation.h | 135 +++ ocs2_ipm/include/ocs2_ipm/IpmSettings.h | 116 +++ ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 230 +++++ ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h | 78 ++ ocs2_ipm/package.xml | 19 + ocs2_ipm/src/IpmHelpers.cpp | 123 +++ ocs2_ipm/src/IpmInitialization.cpp | 45 + .../src/IpmPerformanceIndexComputation.cpp | 141 +++ ocs2_ipm/src/IpmSettings.cpp | 114 +++ ocs2_ipm/src/IpmSolver.cpp | 859 ++++++++++++++++++ ocs2_ipm/test/Exp0Test.cpp | 200 ++++ ocs2_ipm/test/Exp1Test.cpp | 297 ++++++ ocs2_ipm/test/testCircularKinematics.cpp | 426 +++++++++ ocs2_ipm/test/testSwitchedProblem.cpp | 269 ++++++ ocs2_ipm/test/testUnconstrained.cpp | 162 ++++ ocs2_ipm/test/testValuefunction.cpp | 106 +++ 20 files changed, 3679 insertions(+) create mode 100644 ocs2_ipm/CMakeLists.txt create mode 100644 ocs2_ipm/include/ocs2_ipm/IpmHelpers.h create mode 100644 ocs2_ipm/include/ocs2_ipm/IpmInitialization.h create mode 100644 ocs2_ipm/include/ocs2_ipm/IpmMpc.h create mode 100644 ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h create mode 100644 ocs2_ipm/include/ocs2_ipm/IpmSettings.h create mode 100644 ocs2_ipm/include/ocs2_ipm/IpmSolver.h create mode 100644 ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h create mode 100644 ocs2_ipm/package.xml create mode 100644 ocs2_ipm/src/IpmHelpers.cpp create mode 100644 ocs2_ipm/src/IpmInitialization.cpp create mode 100644 ocs2_ipm/src/IpmPerformanceIndexComputation.cpp create mode 100644 ocs2_ipm/src/IpmSettings.cpp create mode 100644 ocs2_ipm/src/IpmSolver.cpp create mode 100644 ocs2_ipm/test/Exp0Test.cpp create mode 100644 ocs2_ipm/test/Exp1Test.cpp create mode 100644 ocs2_ipm/test/testCircularKinematics.cpp create mode 100644 ocs2_ipm/test/testSwitchedProblem.cpp create mode 100644 ocs2_ipm/test/testUnconstrained.cpp create mode 100644 ocs2_ipm/test/testValuefunction.cpp diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt new file mode 100644 index 000000000..c100a0833 --- /dev/null +++ b/ocs2_ipm/CMakeLists.txt @@ -0,0 +1,114 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_ipm) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver + blasfeo_catkin + hpipm_catkin +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem +) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIRS} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + LIBRARIES + ${PROJECT_NAME} + DEPENDS + Boost +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# Multiple shooting solver library +add_library(${PROJECT_NAME} + src/IpmInitialization.cpp + src/IpmHelpers.cpp + src/IpmPerformanceIndexComputation.cpp + src/IpmSettings.cpp + src/IpmSolver.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) + +# ######################### +# ### CLANG TOOLING ### +# ######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) + add_clang_tooling( + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR + ) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +############# +## Testing ## +############# + +catkin_add_gtest(test_${PROJECT_NAME} + test/Exp0Test.cpp + test/Exp1Test.cpp + test/testCircularKinematics.cpp + test/testSwitchedProblem.cpp + test/testUnconstrained.cpp + test/testValuefunction.cpp +) +add_dependencies(test_${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main +) \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h new file mode 100644 index 000000000..3e4feddbd --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -0,0 +1,111 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_oc/oc_problem/OptimalControlProblem.h> + +namespace ocs2 { +namespace ipm { + +/** + * Removes the Newton directions of slack and dual variables and the linearized inequality constraints are removed from the linear system + * equations for the Newton step computation. These terms are considered in the quadratic approximation of the Lagrangian. + * + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slack : The slack variable associated with the inequality constraints. + * @param[in] dual : The dual variable associated with the inequality constraints. + * @param[in] ineqConstraints : Linear approximation of the inequality constraints. + * @param[in, out] lagrangian : Quadratic approximation of the Lagrangian. + */ +void condenseIneqConstraints(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, + const VectorFunctionLinearApproximation& ineqConstraints, ScalarFunctionQuadraticApproximation& lagrangian); + +/** + * Computes the SSE of the residual in the perturbed complementary slackness. + * + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slack : The slack variable associated with the inequality constraints. + * @param[in] dual : The dual variable associated with the inequality constraints. + * @return SSE of the residual in the perturbed complementary slackness + */ +inline scalar_t evaluateComplementarySlackness(scalar_t barrierParam, const vector_t& slack, const vector_t& dual) { + return (slack.array() * dual.array() - barrierParam).matrix().squaredNorm(); +} + +/** + * Retrieves the Newton directions of the slack variable. + * + * @param[in] stateInputIneqConstraints : State-input inequality constraints + * @param[in] dx : Newton direction of the state + * @param[in] du : Newton direction of the input + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slackStateInputIneq : The slack variable associated with the state-input inequality constraints. + * @return Newton directions of the slack variable. + */ +vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateInputIneqConstraints, const vector_t& dx, const vector_t& du, + scalar_t barrierParam, const vector_t& slackStateInputIneq); + +/** + * Retrieves the Newton directions of the slack variable. + * + * @param[in] stateIneqConstraints : State-only inequality constraints + * @param[in] dx : Newton direction of the state + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slackStateIneq : The slack variable associated with the state-only inequality constraints. + * @return Newton directions of the slack variable. + */ +vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateIneqConstraints, const vector_t& dx, scalar_t barrierParam, + const vector_t& slackStateIneq); + +/** + * Retrieves the Newton directions of the dual variable. + * + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slack : The slack variable associated with the inequality constraints. + * @param[in] dual : The dual variable associated with the inequality constraints. + * @param[in] slackDirection : The Newton direction of the slack variable. + * @return Newton directions of the dual variable. + */ +vector_t retrieveDualDirection(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, const vector_t& slackDirection); + +/** + * Computes the step size via fraction-to-boundary-rule, which is introduced in the IPOPT's implementaion paper, + * "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming" + * https://link.springer.com/article/10.1007/s10107-004-0559-y + * + * @param [in] v: A variable (slack or dual). + * @param [in] dv: The serach direction of the variable (slack dirction or dual dirction). + * @param [in] marginRate: Margin rate to avoid the slack or dual variables becoming too close to 0. Typical values are 0.95 or 0.995. + */ +scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scalar_t marginRate = 0.995); + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h new file mode 100644 index 000000000..c5c518cd9 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h @@ -0,0 +1,62 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> + +namespace ocs2 { +namespace ipm { + +/** + * Initializes the slack variable. The initialization of the slack variables follows IPOPT + * (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Initialization). + * + * @param ineqConstraint: The value of the inequality constraint. + * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. + * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + * @return Initialized slack variable. + */ +vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); + +/** + * Initializes the dual variable. The initialization of dual variables follows IPOPT option `bound_mult_init_method`: `mu-based` + * (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Initialization) with additoinal options. + * + * @param slack: The slack variable. + * @param barrierParam: The barrier parameter of the IPM. Must be positive. + * @param initialDualLowerBound : Lower bound of the initial dual variables. + * @param initialDualMarginRate : Additional margin rate of the initial dual variables. + * @return Initialized dual variable. + */ +vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialSlackLowerBound, + scalar_t initialSlackMarginRate); + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmMpc.h b/ocs2_ipm/include/ocs2_ipm/IpmMpc.h new file mode 100644 index 000000000..1eda8be58 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmMpc.h @@ -0,0 +1,72 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_mpc/MPC_BASE.h> + +#include "ocs2_ipm/IpmSolver.h" + +namespace ocs2 { + +class IpmMpc final : public MPC_BASE { + public: + /** + * Constructor + * + * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use + * multiple shooting IPM settings directly. + * @param settings : settings for the multiple shooting IPM solver. + * @param [in] optimalControlProblem: The optimal control problem formulation. + * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. + */ + IpmMpc(mpc::Settings mpcSettings, ipm::Settings settings, const OptimalControlProblem& optimalControlProblem, + const Initializer& initializer) + : MPC_BASE(std::move(mpcSettings)) { + solverPtr_.reset(new IpmSolver(std::move(settings), optimalControlProblem, initializer)); + }; + + ~IpmMpc() override = default; + + IpmSolver* getSolverPtr() override { return solverPtr_.get(); } + const IpmSolver* getSolverPtr() const override { return solverPtr_.get(); } + + protected: + void calculateController(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override { + if (settings().coldStart_) { + solverPtr_->reset(); + } + solverPtr_->run(initTime, initState, finalTime); + } + + private: + std::unique_ptr<IpmSolver> solverPtr_; +}; + +} // namespace ocs2 diff --git a/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h b/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h new file mode 100644 index 000000000..2dbb10b96 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h @@ -0,0 +1,135 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_core/model_data/Metrics.h> +#include <ocs2_oc/multiple_shooting/Transcription.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> + +namespace ocs2 { +namespace ipm { + +/** + * Compute the performance index from the transcription for a single intermediate node. + * + * @param transcription : transcription computed by "multiple_shooting::setupIntermediateNode" + * @param dt : Duration of the interval + * @param barrierParam : Barrier parameter of the interior point method. + * @param slackStateIneq : Slack variable of the state inequality constraints. + * @param slackStateInputIneq : Slack variable of the state-input inequality constraints. + * @return Performance index for a single intermediate node. + */ +PerformanceIndex computePerformanceIndex(const multiple_shooting::Transcription& transcription, scalar_t dt, scalar_t barrierParam, + const vector_t& slackStateIneq, const vector_t& slackStateInputIneq); + +/** + * Compute the performance index from the transcription for the terminal node. + * + * @param transcription : transcription computed by "multiple_shooting::setupTerminalNode" + * @param barrierParam : Barrier parameter of the interior point method. + * @param slackIneq : Slack variable of the inequality constraints. + * @return Performance index for the terminal node. + */ +PerformanceIndex computePerformanceIndex(const multiple_shooting::TerminalTranscription& transcription, scalar_t barrierParam, + const vector_t& slackIneq); + +/** + * Compute the performance index from the transcription for the event node. + * + * @param transcription : transcription computed by "multiple_shooting::setupEventNode" + * @param barrierParam : Barrier parameter of the interior point method. + * @param slackIneq : Slack variable of the inequality constraints. + * @return Performance index for the event node. + */ +PerformanceIndex computePerformanceIndex(const multiple_shooting::EventTranscription& transcription, scalar_t barrierParam, + const vector_t& slackIneq); + +/** + * Compute only the performance index for a single intermediate node. + * Corresponds to the performance index computed from the multiple_shooting::Transcription returned by + * "multiple_shooting::setupIntermediateNode" + * + * @param optimalControlProblem : Definition of the optimal control problem + * @param discretizer : Integrator to use for creating the discrete dynamics. + * @param t : Start of the discrete interval + * @param dt : Duration of the interval + * @param x : State at start of the interval + * @param x_next : State at the end of the interval + * @param u : Input, taken to be constant across the interval. + * @param barrierParam : Barrier parameter of the interior point method. + * @param slackStateIneq : Slack variable of the state inequality constraints. + * @param slackStateInputIneq : Slack variable of the state-input inequality constraints. + * @param enableStateInequalityConstraints : Should be disabled at the initial node (i = 0). + * @return Performance index for a single intermediate node. + */ +PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + scalar_t barrierParam, const vector_t& slackStateIneq, const vector_t& slackStateInputIneq, + bool enableStateInequalityConstraints); + +/** + * Compute only the performance index for the terminal node. + * Corresponds to the performance index computed from multiple_shooting::TerminalTranscription returned by + * "multiple_shooting::setTerminalNode" + * + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the terminal node + * @param x : Terminal state + * @param barrierParam : Barrier parameter of the interior point method. + * @param slackIneq : Slack variable of the inequality constraints. + * @return Performance index for the terminal node. + */ +PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + scalar_t barrierParam, const vector_t& slackIneq); + +/** + * Compute only the performance index for the event node. + * Corresponds to the performance index computed from multiple_shooting::EventTranscription returned by + * "multiple_shooting::setEventNode" + * + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the event node + * @param x : Pre-event state + * @param x_next : Post-event state + * @param barrierParam : Barrier parameter of the interior point method. + * @param slackIneq : Slack variable of the inequality constraints. + * @return Performance index for the event node. + */ +PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + const vector_t& x_next, scalar_t barrierParam, const vector_t& slackIneq); + +PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_t barrierParam, const vector_t& slackStateIneq, + const vector_t& slackStateInputIneq, bool enableStateInequalityConstraints); + +PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t barrierParam, const vector_t& slackIneq); + +} // namespace ipm +} // namespace ocs2 diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h new file mode 100644 index 000000000..9d787e49f --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h @@ -0,0 +1,116 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> +#include <ocs2_core/integration/SensitivityIntegrator.h> + +#include <hpipm_catkin/HpipmInterfaceSettings.h> + +namespace ocs2 { +namespace ipm { + +struct Settings { + // Ipm settings + size_t ipmIteration = 10; // Maximum number of IPM iterations + scalar_t deltaTol = 1e-6; // Termination condition : RMS update of x(t) and u(t) are both below this value + scalar_t costTol = 1e-4; // Termination condition : (cost{i+1} - (cost{i}) < costTol AND constraints{i+1} < g_min + + // Linesearch - step size rules + scalar_t alpha_decay = 0.5; // multiply the step size by this factor every time a linesearch step is rejected. + scalar_t alpha_min = 1e-4; // terminate linesearch if the attempted step size is below this threshold + + // Linesearch - step acceptance criteria with c = costs, g = the norm of constraint violation, and w = [x; u] + scalar_t g_max = 1e6; // (1): IF g{i+1} > g_max REQUIRE g{i+1} < (1-gamma_c) * g{i} + scalar_t g_min = 1e-6; // (2): ELSE IF (g{i} < g_min AND g{i+1} < g_min AND dc/dw'{i} * delta_w < 0) REQUIRE armijo condition + scalar_t armijoFactor = 1e-4; // Armijo condition: c{i+1} < c{i} + armijoFactor * dc/dw'{i} * delta_w + scalar_t gamma_c = 1e-6; // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i} + + // controller type + bool useFeedbackPolicy = true; // true to use feedback, false to use feedforward + bool createValueFunction = false; // true to store the value function, false to ignore it + + // QP subproblem solver settings + hpipm_interface::Settings hpipmSettings = hpipm_interface::Settings(); + + // Discretization method + scalar_t dt = 0.01; // user-defined time discretization + SensitivityIntegratorType integratorType = SensitivityIntegratorType::RK2; + + // Inequality penalty relaxed barrier parameters + scalar_t inequalityConstraintMu = 0.0; + scalar_t inequalityConstraintDelta = 1e-6; + + bool projectStateInputEqualityConstraints = true; // Use a projection method to resolve the state-input constraint Cx+Du+e + bool computeLagrangeMultipliers = true; // compute the Lagrange multipliers to evaluate the SSE of the dual feasibilities + + // Barrier strategy of the primal-dual interior point method. Conventions follows Ipopt. + scalar_t initialBarrierParameter = 1.0e-02; // Initial value of the barrier parameter + scalar_t targetBarrierParameter = 1.0e-04; // Targer value of the barrier parameter. The barreir will decrease until reaches this value. + scalar_t barrierReductionCostTol = 1.0e-02; // Barrier reduction condition : (cost{i+1} - (cost{i}) < costTol + scalar_t barrierReductionConstraintTol = 1.0e-02; // Barrier reduction condition : Constraint violations below this value + scalar_t barrierLinearDecreaseFactor = 0.2; // Linear decrease factor of the barrier parameter, i.e., mu <- mu * factor. + scalar_t barrierSuperlinearDecreasePower = 1.5; // Superlinear decrease factor of the barrier parameter, i.e., mu <- mu ^ factor + + // Initialization of the interior point method. Follows the initialization method of IPOPT + // (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Initialization). + scalar_t initialSlackLowerBound = + 1.0e-04; // Minimum value of the initial slack variable. Corresponds to `slack_bound_push` option of IPOPT. + scalar_t initialDualLowerBound = 1.0e-04; // Minimum value of the initial dual variable. + scalar_t initialSlackMarginRate = 0.01; // Margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + scalar_t initialDualMarginRate = 0.01; // Margin rate of the initial dual variables. + + // Linesearch for the interior point method. + scalar_t fractionToBoundaryMargin = + 0.995; // Margin of the fraction-to-boundary-rule for the step size selection. Correcponds to `tau_min` option of IPOPT. + bool usePrimalStepSizeForDual = true; // If true, the primal step size is also used as the dual step size. + + // Printing + bool printSolverStatus = false; // Print HPIPM status after solving the QP subproblem + bool printSolverStatistics = false; // Print benchmarking of the multiple shooting method + bool printLinesearch = false; // Print linesearch information + + // Threading + size_t nThreads = 4; + int threadPriority = 50; +}; + +/** + * Loads the multiple shooting IPM settings from a given file. + * + * @param [in] filename: File name which contains the configuration data. + * @param [in] fieldName: Field name which contains the configuration data. + * @param [in] verbose: Flag to determine whether to print out the loaded settings or not. + * @return The settings + */ +Settings loadSettings(const std::string& filename, const std::string& fieldName = "multiple_shooting", bool verbose = true); + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h new file mode 100644 index 000000000..1526cd815 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -0,0 +1,230 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/initialization/Initializer.h> +#include <ocs2_core/integration/SensitivityIntegrator.h> +#include <ocs2_core/misc/Benchmark.h> +#include <ocs2_core/thread_support/ThreadPool.h> + +#include <ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h> +#include <ocs2_oc/oc_data/TimeDiscretization.h> +#include <ocs2_oc/oc_problem/OptimalControlProblem.h> +#include <ocs2_oc/oc_solver/SolverBase.h> +#include <ocs2_oc/search_strategy/FilterLinesearch.h> + +#include <hpipm_catkin/HpipmInterface.h> + +#include "ocs2_ipm/IpmSettings.h" +#include "ocs2_ipm/IpmSolverStatus.h" + +namespace ocs2 { + +class IpmSolver : public SolverBase { + public: + /** + * Constructor + * + * @param settings : settings for the multiple shooting IPM solver. + * @param [in] optimalControlProblem: The optimal control problem formulation. + * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. + */ + IpmSolver(ipm::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); + + ~IpmSolver() override; + + void reset() override; + + scalar_t getFinalTime() const override { return primalSolution_.timeTrajectory_.back(); }; + + void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } + + const ProblemMetrics& getSolutionMetrics() const override { return problemMetrics_; } + + size_t getNumIterations() const override { return totalNumIterations_; } + + const OptimalControlProblem& getOptimalControlProblem() const override { return ocpDefinitions_.front(); } + + const PerformanceIndex& getPerformanceIndeces() const override { return getIterationsLog().back(); }; + + const std::vector<PerformanceIndex>& getIterationsLog() const override; + + ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const override; + + ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) override { + throw std::runtime_error("[IpmSolver] getHamiltonian() not available yet."); + } + + vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override { + throw std::runtime_error("[IpmSolver] getStateInputEqualityConstraintLagrangian() not available yet."); + } + + MultiplierCollection getIntermediateDualSolution(scalar_t time) const override { + throw std::runtime_error("[IpmSolver] getIntermediateDualSolution() not available yet."); + } + + private: + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override; + + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const ControllerBase* externalControllerPtr) override { + if (externalControllerPtr == nullptr) { + runImpl(initTime, initState, finalTime); + } else { + throw std::runtime_error("[IpmSolver::run] This solver does not support external controller!"); + } + } + + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) override { + // Copy all except the controller + primalSolution_.timeTrajectory_ = primalSolution.timeTrajectory_; + primalSolution_.stateTrajectory_ = primalSolution.stateTrajectory_; + primalSolution_.inputTrajectory_ = primalSolution.inputTrajectory_; + primalSolution_.postEventIndices_ = primalSolution.postEventIndices_; + primalSolution_.modeSchedule_ = primalSolution.modeSchedule_; + runImpl(initTime, initState, finalTime); + } + + /** Run a task in parallel with settings.nThreads */ + void runParallel(std::function<void(int)> taskFunction); + + /** Get profiling information as a string */ + std::string getBenchmarkingInformation() const; + + /** Initializes for the costate trajectories */ + void initializeCostateTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& stateTrajectory, + vector_array_t& costateTrajectory); + + /** Initializes for the Lagrange multiplier trajectories of the constraint projection */ + void initializeProjectionMultiplierTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, + vector_array_t& projectionMultiplierTrajectory); + + /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ + PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, std::vector<Metrics>& metrics, const vector_array_t& lmd, + const vector_array_t& nu, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& slackStateInputIneq, vector_array_t& dualStateIneq, + vector_array_t& dualStateInputIneq, bool initializeSlackAndDualVariables); + + /** Computes only the performance metrics at the current {t, x(t), u(t)} */ + PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, std::vector<Metrics>& metrics, scalar_t barrierParam, + const vector_array_t& slackStateIneq, const vector_array_t& slackStateInputIneq); + + /** Returns solution of the QP subproblem in delta coordinates: */ + struct OcpSubproblemSolution { + vector_array_t deltaXSol; // delta_x(t) + vector_array_t deltaUSol; // delta_u(t) + vector_array_t deltaNuSol; // delta_nu(t) + vector_array_t deltaSlackStateIneq; + vector_array_t deltaSlackStateInputIneq; + vector_array_t deltaDualStateIneq; + vector_array_t deltaDualStateInputIneq; + scalar_t armijoDescentMetric; // inner product of the cost gradient and decision variable step + scalar_t maxPrimalStepSize; + scalar_t maxDualStepSize; + }; + OcpSubproblemSolution getOCPSolution(const vector_t& delta_x0, scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateIneq, + const vector_array_t& dualStateInputIneq); + + /** Extract the value function based on the last solved QP */ + void extractValueFunction(const std::vector<AnnotatedTime>& time, const vector_array_t& x, const vector_array_t& lmd, + const vector_array_t& deltaXSol, vector_array_t& deltaLmdSol); + + /** Constructs the primal solution based on the optimized state and input trajectories */ + PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); + + /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ + ipm::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, + const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u, + std::vector<Metrics>& metrics, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& slackStateInputIneq); + + /** Updates the barrier parameter */ + scalar_t updateBarrierParameter(scalar_t currentBarrierParameter, const PerformanceIndex& baseline, const ipm::StepInfo& stepInfo) const; + + /** Determine convergence after a step */ + ipm::Convergence checkConvergence(int iteration, scalar_t barrierParam, const PerformanceIndex& baseline, + const ipm::StepInfo& stepInfo) const; + + // Problem definition + const ipm::Settings settings_; + DynamicsDiscretizer discretizer_; + DynamicsSensitivityDiscretizer sensitivityDiscretizer_; + std::vector<OptimalControlProblem> ocpDefinitions_; + std::unique_ptr<Initializer> initializerPtr_; + FilterLinesearch filterLinesearch_; + + // Solver interface + HpipmInterface hpipmInterface_; + + // Threading + ThreadPool threadPool_; + + // Solution + PrimalSolution primalSolution_; + vector_array_t costateTrajectory_; + vector_array_t projectionMultiplierTrajectory_; + vector_array_t slackStateIneqTrajectory_; + vector_array_t dualStateIneqTrajectory_; + vector_array_t slackStateInputIneqTrajectory_; + vector_array_t dualStateInputIneqTrajectory_; + + // Value function in absolute state coordinates (without the constant value) + std::vector<ScalarFunctionQuadraticApproximation> valueFunction_; + + // LQ approximation + std::vector<ScalarFunctionQuadraticApproximation> lagrangian_; + std::vector<VectorFunctionLinearApproximation> dynamics_; + std::vector<VectorFunctionLinearApproximation> stateInputEqConstraints_; + std::vector<VectorFunctionLinearApproximation> stateIneqConstraints_; + std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; + std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + + // Lagrange multipliers + std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_; + + // Iteration performance log + std::vector<PerformanceIndex> performanceIndeces_; + + // The ProblemMetrics associated to primalSolution_ + ProblemMetrics problemMetrics_; + + // Benchmarking + size_t numProblems_{0}; + size_t totalNumIterations_{0}; + benchmark::RepeatedTimer initializationTimer_; + benchmark::RepeatedTimer linearQuadraticApproximationTimer_; + benchmark::RepeatedTimer solveQpTimer_; + benchmark::RepeatedTimer linesearchTimer_; + benchmark::RepeatedTimer computeControllerTimer_; +}; + +} // namespace ocs2 diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h b/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h new file mode 100644 index 000000000..d64412d37 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h @@ -0,0 +1,78 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <string> + +#include <ocs2_core/Types.h> +#include <ocs2_oc/oc_data/PerformanceIndex.h> +#include <ocs2_oc/search_strategy/FilterLinesearch.h> + +namespace ocs2 { +namespace ipm { + +/** Different types of convergence */ +enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; + +/** Struct to contain the result and logging data of the stepsize computation */ +struct StepInfo { + // Step size and type + scalar_t stepSize = 0.0; + scalar_t dualStepSize = 0.0; + FilterLinesearch::StepType stepType = FilterLinesearch::StepType::UNKNOWN; + + // Step in primal variables + scalar_t dx_norm = 0.0; // norm of the state trajectory update + scalar_t du_norm = 0.0; // norm of the input trajectory update + + // Performance result after the step + PerformanceIndex performanceAfterStep; + scalar_t totalConstraintViolationAfterStep; // constraint metric used in the line search +}; + +/** Transforms ipm::Convergence to string */ +inline std::string toString(const Convergence& convergence) { + switch (convergence) { + case Convergence::ITERATIONS: + return "Maximum number of iterations reached"; + case Convergence::STEPSIZE: + return "Step size below minimum"; + case Convergence::METRICS: + return "Cost decrease and constraint satisfaction below tolerance"; + case Convergence::PRIMAL: + return "Primal update below tolerance"; + case Convergence::FALSE: + default: + return "Not Converged"; + } +} + +} // namespace ipm +} // namespace ocs2 diff --git a/ocs2_ipm/package.xml b/ocs2_ipm/package.xml new file mode 100644 index 000000000..60e87ddb4 --- /dev/null +++ b/ocs2_ipm/package.xml @@ -0,0 +1,19 @@ +<?xml version="1.0"?> +<package format="2"> + <name>ocs2_ipm</name> + <version>0.0.0</version> + <description>The ocs2_hpipm package</description> + + <maintainer email="huier@todo.todo">huier</maintainer> + + <license>TODO</license> + + <buildtool_depend>catkin</buildtool_depend> + <depend>ocs2_core</depend> + <depend>ocs2_mpc</depend> + <depend>ocs2_oc</depend> + <depend>ocs2_qp_solver</depend> + <depend>blasfeo_catkin</depend> + <depend>hpipm_catkin</depend> + +</package> diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp new file mode 100644 index 000000000..e1d3e5675 --- /dev/null +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -0,0 +1,123 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_ipm/IpmHelpers.h" + +#include <cassert> + +namespace ocs2 { +namespace ipm { + +void condenseIneqConstraints(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, + const VectorFunctionLinearApproximation& ineqConstraint, ScalarFunctionQuadraticApproximation& lagrangian) { + assert(barrierParam > 0.0); + + const size_t nc = ineqConstraint.f.size(); + const size_t nu = ineqConstraint.dfdu.cols(); + + if (nc == 0) { + return; + } + + // dual feasibilities + lagrangian.dfdx.noalias() -= ineqConstraint.dfdx.transpose() * dual; + if (ineqConstraint.dfdu.cols() > 0) { + lagrangian.dfdu.noalias() -= ineqConstraint.dfdu.transpose() * dual; + } + + // coefficients for condensing + const vector_t condensingLinearCoeff = ((dual.array() * ineqConstraint.f.array() - barrierParam) / slack.array()).matrix(); + const vector_t condensingQuadraticCoeff = dual.cwiseQuotient(slack); + matrix_t condensingQuadraticCoeff_dfdx, condensingQuadraticCoeff_dfdu; + + // condensing + lagrangian.dfdx.noalias() += ineqConstraint.dfdx.transpose() * condensingLinearCoeff; + condensingQuadraticCoeff_dfdx.noalias() = condensingQuadraticCoeff.asDiagonal() * ineqConstraint.dfdx; + lagrangian.dfdxx.noalias() += ineqConstraint.dfdx.transpose() * condensingQuadraticCoeff_dfdx; + + if (nu > 0) { + lagrangian.dfdu.noalias() += ineqConstraint.dfdu.transpose() * condensingLinearCoeff; + condensingQuadraticCoeff_dfdu.noalias() = condensingQuadraticCoeff.asDiagonal() * ineqConstraint.dfdu; + lagrangian.dfduu.noalias() += ineqConstraint.dfdu.transpose() * condensingQuadraticCoeff_dfdu; + lagrangian.dfdux.noalias() += ineqConstraint.dfdu.transpose() * condensingQuadraticCoeff_dfdx; + } +} + +vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateInputIneqConstraints, const vector_t& dx, const vector_t& du, + scalar_t barrierParam, const vector_t& slackStateInputIneq) { + assert(barrierParam > 0.0); + if (stateInputIneqConstraints.f.size() == 0) { + return vector_t(); + } + + vector_t slackDirection = stateInputIneqConstraints.f - slackStateInputIneq; + slackDirection.noalias() += stateInputIneqConstraints.dfdx * dx; + slackDirection.noalias() += stateInputIneqConstraints.dfdu * du; + return slackDirection; +} + +vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateIneqConstraints, const vector_t& dx, scalar_t barrierParam, + const vector_t& slackStateIneq) { + assert(barrierParam > 0.0); + if (stateIneqConstraints.f.size() == 0) { + return vector_t(); + } + + vector_t slackDirection = stateIneqConstraints.f - slackStateIneq; + slackDirection.noalias() += stateIneqConstraints.dfdx * dx; + return slackDirection; +} + +vector_t retrieveDualDirection(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, const vector_t& slackDirection) { + assert(barrierParam > 0.0); + vector_t dualDirection(slackDirection.size()); + dualDirection.array() = -(dual.array() * slackDirection.array() + (slack.array() * dual.array() - barrierParam)) / slack.array(); + return dualDirection; +} + +scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scalar_t marginRate) { + assert(marginRate > 0.0); + assert(marginRate <= 1.0); + + if (v.size() == 0) { + return 1.0; + } + + scalar_t minFractionToBoundary = 1.0; + vector_t fractionToBoundary = -marginRate * v.cwiseQuotient(dv); + for (int i = 0; i < fractionToBoundary.size(); ++i) { + if (fractionToBoundary[i] <= 0.0) { + fractionToBoundary[i] = 1.0; + } + } + return std::min(1.0, fractionToBoundary.minCoeff()); +} + +} // namespace ipm +} // namespace ocs2 diff --git a/ocs2_ipm/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp new file mode 100644 index 000000000..5c72292c1 --- /dev/null +++ b/ocs2_ipm/src/IpmInitialization.cpp @@ -0,0 +1,45 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_ipm/IpmInitialization.h" + +namespace ocs2 { +namespace ipm { + +vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound); +} + +vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialDualLowerBound, + scalar_t initialDualMarginRate) { + return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound); +} + +} // namespace ipm +} // namespace ocs2 diff --git a/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp b/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp new file mode 100644 index 000000000..171d12d30 --- /dev/null +++ b/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp @@ -0,0 +1,141 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_ipm/IpmPerformanceIndexComputation.h" + +#include <ocs2_core/model_data/Metrics.h> + +#include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> +#include <ocs2_oc/multiple_shooting/MetricsComputation.h> +#include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> + +namespace ocs2 { +namespace ipm { + +PerformanceIndex computePerformanceIndex(const multiple_shooting::Transcription& transcription, scalar_t dt, scalar_t barrierParam, + const vector_t& slackStateIneq, const vector_t& slackStateInputIneq) { + auto performance = multiple_shooting::computePerformanceIndex(transcription, dt); + + if (slackStateIneq.size() > 0) { + performance.cost += (-dt * barrierParam * slackStateIneq.array().log().sum()); + } + if (slackStateInputIneq.size() > 0) { + performance.cost += (-dt * barrierParam * slackStateInputIneq.array().log().sum()); + } + + if (transcription.stateIneqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE += dt * (transcription.stateIneqConstraints.f - slackStateIneq).squaredNorm(); + } + if (transcription.stateInputIneqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE += dt * (transcription.stateInputIneqConstraints.f - slackStateInputIneq).squaredNorm(); + } + + return performance; +} + +PerformanceIndex computePerformanceIndex(const multiple_shooting::TerminalTranscription& transcription, scalar_t barrierParam, + const vector_t& slackIneq) { + auto performance = multiple_shooting::computePerformanceIndex(transcription); + + if (slackIneq.size() > 0) { + performance.cost += (-barrierParam * slackIneq.array().log().sum()); + } + + if (transcription.ineqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE += (transcription.ineqConstraints.f - slackIneq).squaredNorm(); + } + + return performance; +} + +PerformanceIndex computePerformanceIndex(const multiple_shooting::EventTranscription& transcription, scalar_t barrierParam, + const vector_t& slackIneq) { + auto performance = multiple_shooting::computePerformanceIndex(transcription); + + if (slackIneq.size() > 0) { + performance.cost += (-barrierParam * slackIneq.array().log().sum()); + } + + if (transcription.ineqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE += (transcription.ineqConstraints.f - slackIneq).squaredNorm(); + } + + return performance; +} + +PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + scalar_t barrierParam, const vector_t& slackStateIneq, const vector_t& slackStateInputIneq, + bool enableStateInequalityConstraints) { + const auto metrics = multiple_shooting::computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); + return toPerformanceIndex(metrics, dt, barrierParam, slackStateIneq, slackStateInputIneq, enableStateInequalityConstraints); +} + +PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + const vector_t& x_next, scalar_t barrierParam, const vector_t& slackIneq) { + const auto metrics = multiple_shooting::computeEventMetrics(optimalControlProblem, t, x, x_next); + return toPerformanceIndex(metrics, barrierParam, slackIneq); +} + +PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + scalar_t barrierParam, const vector_t& slackIneq) { + const auto metrics = multiple_shooting::computeTerminalMetrics(optimalControlProblem, t, x); + return toPerformanceIndex(metrics, barrierParam, slackIneq); +} + +PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_t barrierParam, const vector_t& slackStateIneq, + const vector_t& slackStateInputIneq, bool enableStateInequalityConstraints) { + PerformanceIndex performance = toPerformanceIndex(metrics, dt); + + if (slackStateIneq.size() > 0) { + performance.cost += (-dt * barrierParam * slackStateIneq.array().log().sum()); + performance.equalityConstraintsSSE += dt * (toVector(metrics.stateIneqConstraint) - slackStateIneq).squaredNorm(); + } + + if (slackStateInputIneq.size() > 0 && enableStateInequalityConstraints) { + performance.cost += (-dt * barrierParam * slackStateInputIneq.array().log().sum()); + performance.equalityConstraintsSSE += dt * (toVector(metrics.stateInputIneqConstraint) - slackStateInputIneq).squaredNorm(); + } + + return performance; +} + +PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t barrierParam, const vector_t& slackIneq) { + PerformanceIndex performance = toPerformanceIndex(metrics); + + if (slackIneq.size() > 0) { + performance.cost += (-barrierParam * slackIneq.array().log().sum()); + performance.equalityConstraintsSSE += (toVector(metrics.stateIneqConstraint) - slackIneq).squaredNorm(); + } + + return performance; +} + +} // namespace ipm +} // namespace ocs2 diff --git a/ocs2_ipm/src/IpmSettings.cpp b/ocs2_ipm/src/IpmSettings.cpp new file mode 100644 index 000000000..737b38396 --- /dev/null +++ b/ocs2_ipm/src/IpmSettings.cpp @@ -0,0 +1,114 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_ipm/IpmSettings.h" + +#include <boost/property_tree/info_parser.hpp> +#include <boost/property_tree/ptree.hpp> + +#include <ocs2_core/misc/LoadData.h> + +#include <stdexcept> + +namespace ocs2 { +namespace ipm { + +Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { + boost::property_tree::ptree pt; + boost::property_tree::read_info(filename, pt); + + Settings settings; + + if (verbose) { + std::cerr << "\n #### Multiple-Shooting IPM Settings:"; + std::cerr << "\n #### =============================================================================\n"; + } + + loadData::loadPtreeValue(pt, settings.ipmIteration, fieldName + ".ipmIteration", verbose); + loadData::loadPtreeValue(pt, settings.deltaTol, fieldName + ".deltaTol", verbose); + loadData::loadPtreeValue(pt, settings.alpha_decay, fieldName + ".alpha_decay", verbose); + loadData::loadPtreeValue(pt, settings.alpha_min, fieldName + ".alpha_min", verbose); + loadData::loadPtreeValue(pt, settings.gamma_c, fieldName + ".gamma_c", verbose); + loadData::loadPtreeValue(pt, settings.g_max, fieldName + ".g_max", verbose); + loadData::loadPtreeValue(pt, settings.g_min, fieldName + ".g_min", verbose); + loadData::loadPtreeValue(pt, settings.armijoFactor, fieldName + ".armijoFactor", verbose); + loadData::loadPtreeValue(pt, settings.costTol, fieldName + ".costTol", verbose); + loadData::loadPtreeValue(pt, settings.dt, fieldName + ".dt", verbose); + loadData::loadPtreeValue(pt, settings.useFeedbackPolicy, fieldName + ".useFeedbackPolicy", verbose); + loadData::loadPtreeValue(pt, settings.createValueFunction, fieldName + ".createValueFunction", verbose); + auto integratorName = sensitivity_integrator::toString(settings.integratorType); + loadData::loadPtreeValue(pt, integratorName, fieldName + ".integratorType", verbose); + settings.integratorType = sensitivity_integrator::fromString(integratorName); + loadData::loadPtreeValue(pt, settings.inequalityConstraintMu, fieldName + ".inequalityConstraintMu", verbose); + loadData::loadPtreeValue(pt, settings.inequalityConstraintDelta, fieldName + ".inequalityConstraintDelta", verbose); + loadData::loadPtreeValue(pt, settings.initialBarrierParameter, fieldName + ".initialBarrierParameter", verbose); + loadData::loadPtreeValue(pt, settings.targetBarrierParameter, fieldName + ".targetBarrierParameter", verbose); + loadData::loadPtreeValue(pt, settings.barrierReductionCostTol, fieldName + ".barrierReductionCostTol ", verbose); + loadData::loadPtreeValue(pt, settings.barrierReductionConstraintTol, fieldName + ".barrierReductionConstraintTol", verbose); + loadData::loadPtreeValue(pt, settings.barrierLinearDecreaseFactor, fieldName + ".barrierLinearDecreaseFactor", verbose); + loadData::loadPtreeValue(pt, settings.barrierSuperlinearDecreasePower, fieldName + ".barrierSuperlinearDecreasePower", verbose); + loadData::loadPtreeValue(pt, settings.fractionToBoundaryMargin, fieldName + ".fractionToBoundaryMargin", verbose); + loadData::loadPtreeValue(pt, settings.usePrimalStepSizeForDual, fieldName + ".usePrimalStepSizeForDual", verbose); + loadData::loadPtreeValue(pt, settings.projectStateInputEqualityConstraints, fieldName + ".projectStateInputEqualityConstraints", verbose); + loadData::loadPtreeValue(pt, settings.computeLagrangeMultipliers, fieldName + ".computeLagrangeMultipliers", verbose); + loadData::loadPtreeValue(pt, settings.initialSlackLowerBound, fieldName + ".initialSlackLowerBound", verbose); + loadData::loadPtreeValue(pt, settings.initialDualLowerBound, fieldName + ".initialDualLowerBound", verbose); + loadData::loadPtreeValue(pt, settings.initialSlackMarginRate, fieldName + ".initialSlackMarginRate", verbose); + loadData::loadPtreeValue(pt, settings.initialDualMarginRate, fieldName + ".initialDualMarginRate", verbose); + loadData::loadPtreeValue(pt, settings.printSolverStatus, fieldName + ".printSolverStatus", verbose); + loadData::loadPtreeValue(pt, settings.printSolverStatistics, fieldName + ".printSolverStatistics", verbose); + loadData::loadPtreeValue(pt, settings.printLinesearch, fieldName + ".printLinesearch", verbose); + loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); + loadData::loadPtreeValue(pt, settings.threadPriority, fieldName + ".threadPriority", verbose); + + if (settings.initialSlackLowerBound <= 0.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] initialSlackLowerBound must be positive!"); + } + if (settings.initialDualLowerBound <= 0.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] initialDualLowerBound must be positive!"); + } + if (settings.initialSlackMarginRate < 0.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] initialSlackMarginRate must be non-negative!"); + } + if (settings.initialDualMarginRate < 0.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] initialDualMarginRate must be non-negative!"); + } + if (settings.fractionToBoundaryMargin <= 0.0 || settings.fractionToBoundaryMargin > 1.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] fractionToBoundaryMargin must be positive and no more than 1.0!"); + } + + if (verbose) { + std::cerr << settings.hpipmSettings; + std::cerr << " #### =============================================================================" << std::endl; + } + + return settings; +} +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp new file mode 100644 index 000000000..a4998a070 --- /dev/null +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -0,0 +1,859 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_ipm/IpmSolver.h" + +#include <iomanip> +#include <iostream> +#include <numeric> + +#include <ocs2_oc/approximate_model/LinearQuadraticApproximator.h> +#include <ocs2_oc/multiple_shooting/Helpers.h> +#include <ocs2_oc/multiple_shooting/Initialization.h> +#include <ocs2_oc/multiple_shooting/LagrangianEvaluation.h> +#include <ocs2_oc/multiple_shooting/MetricsComputation.h> +#include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> +#include <ocs2_oc/multiple_shooting/Transcription.h> +#include <ocs2_oc/oc_problem/OcpSize.h> + +#include "ocs2_ipm/IpmHelpers.h" +#include "ocs2_ipm/IpmInitialization.h" +#include "ocs2_ipm/IpmPerformanceIndexComputation.h" + +namespace ocs2 { + +namespace { +ipm::Settings rectifySettings(const OptimalControlProblem& ocp, ipm::Settings&& settings) { + // True does not make sense if there are no constraints. + if (ocp.equalityConstraintPtr->empty()) { + settings.projectStateInputEqualityConstraints = false; + } + // Turn off the barrier update strategy if there are no inequality constraints. + if (ocp.inequalityConstraintPtr->empty() && ocp.stateInequalityConstraintPtr->empty() && ocp.preJumpInequalityConstraintPtr->empty() && + ocp.finalInequalityConstraintPtr->empty()) { + settings.targetBarrierParameter = settings.initialBarrierParameter; + } + return settings; +} +} // anonymous namespace + +IpmSolver::IpmSolver(ipm::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) + : settings_(rectifySettings(optimalControlProblem, std::move(settings))), + hpipmInterface_(OcpSize(), settings_.hpipmSettings), + threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { + Eigen::setNbThreads(1); // No multithreading within Eigen. + Eigen::initParallel(); + + // Dynamics discretization + discretizer_ = selectDynamicsDiscretization(settings_.integratorType); + sensitivityDiscretizer_ = selectDynamicsSensitivityDiscretization(settings_.integratorType); + + // Clone objects to have one for each worker + for (int w = 0; w < settings_.nThreads; w++) { + ocpDefinitions_.push_back(optimalControlProblem); + } + + // Operating points + initializerPtr_.reset(initializer.clone()); + + // Linesearch + filterLinesearch_.g_max = settings_.g_max; + filterLinesearch_.g_min = settings_.g_min; + filterLinesearch_.gamma_c = settings_.gamma_c; + filterLinesearch_.armijoFactor = settings_.armijoFactor; +} + +IpmSolver::~IpmSolver() { + if (settings_.printSolverStatistics) { + std::cerr << getBenchmarkingInformation() << std::endl; + } +} + +void IpmSolver::reset() { + // Clear solution + primalSolution_ = PrimalSolution(); + costateTrajectory_.clear(); + projectionMultiplierTrajectory_.clear(); + slackStateIneqTrajectory_.clear(); + dualStateIneqTrajectory_.clear(); + slackStateInputIneqTrajectory_.clear(); + dualStateInputIneqTrajectory_.clear(); + valueFunction_.clear(); + performanceIndeces_.clear(); + + // reset timers + numProblems_ = 0; + totalNumIterations_ = 0; + linearQuadraticApproximationTimer_.reset(); + solveQpTimer_.reset(); + linesearchTimer_.reset(); + computeControllerTimer_.reset(); +} + +std::string IpmSolver::getBenchmarkingInformation() const { + const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); + const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); + const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds(); + const auto computeControllerTotal = computeControllerTimer_.getTotalInMilliseconds(); + + const auto benchmarkTotal = linearQuadraticApproximationTotal + solveQpTotal + linesearchTotal + computeControllerTotal; + + std::stringstream infoStream; + if (benchmarkTotal > 0.0) { + const scalar_t inPercent = 100.0; + infoStream << "\n########################################################################\n"; + infoStream << "The benchmarking is computed over " << totalNumIterations_ << " iterations. \n"; + infoStream << "IPM Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "\tLQ Approximation :\t" << linearQuadraticApproximationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << linearQuadraticApproximationTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tSolve QP :\t" << solveQpTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << solveQpTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tLinesearch :\t" << linesearchTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << linesearchTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tCompute Controller :\t" << computeControllerTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << computeControllerTotal / benchmarkTotal * inPercent << "%)\n"; + } + return infoStream.str(); +} + +const std::vector<PerformanceIndex>& IpmSolver::getIterationsLog() const { + if (performanceIndeces_.empty()) { + throw std::runtime_error("[IpmSolver]: No performance log yet, no problem solved yet?"); + } else { + return performanceIndeces_; + } +} + +ScalarFunctionQuadraticApproximation IpmSolver::getValueFunction(scalar_t time, const vector_t& state) const { + if (valueFunction_.empty()) { + throw std::runtime_error("[IpmSolver] Value function is empty! Is createValueFunction true and did the solver run?"); + } else { + // Interpolation + const auto indexAlpha = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_); + + ScalarFunctionQuadraticApproximation valueFunction; + using T = std::vector<ocs2::ScalarFunctionQuadraticApproximation>; + using LinearInterpolation::interpolate; + valueFunction.f = 0.0; + valueFunction.dfdx = interpolate(indexAlpha, valueFunction_, [](const T& v, size_t ind) -> const vector_t& { return v[ind].dfdx; }); + valueFunction.dfdxx = interpolate(indexAlpha, valueFunction_, [](const T& v, size_t ind) -> const matrix_t& { return v[ind].dfdxx; }); + + // Re-center around query state + valueFunction.dfdx.noalias() += valueFunction.dfdxx * state; + + return valueFunction; + } +} + +void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++ IPM solver is initialized ++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + } + + // Determine time discretization, taking into account event times. + const auto& eventTimes = this->getReferenceManager().getModeSchedule().eventTimes; + const auto timeDiscretization = timeDiscretizationWithEvents(initTime, finalTime, settings_.dt, eventTimes); + + // Initialize references + for (auto& ocpDefinition : ocpDefinitions_) { + const auto& targetTrajectories = this->getReferenceManager().getTargetTrajectories(); + ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; + } + + // Initialize the state and input + vector_array_t x, u; + multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); + + // Initialize the costate and projection multiplier + vector_array_t lmd, nu; + if (settings_.computeLagrangeMultipliers) { + initializeCostateTrajectory(timeDiscretization, x, lmd); + initializeProjectionMultiplierTrajectory(timeDiscretization, nu); + } + + // Interior point variables + vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; + + scalar_t barrierParam = settings_.initialBarrierParameter; + + // Bookkeeping + performanceIndeces_.clear(); + std::vector<Metrics> metrics; + + int iter = 0; + ipm::Convergence convergence = ipm::Convergence::FALSE; + while (convergence == ipm::Convergence::FALSE) { + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\nIPM iteration: " << iter << " (barrier parameter: " << barrierParam << ")\n"; + } + + // Make QP approximation + linearQuadraticApproximationTimer_.startTimer(); + const bool initializeSlackAndDualVariables = (iter == 0); + const auto baselinePerformance = + setupQuadraticSubproblem(timeDiscretization, initState, x, u, metrics, lmd, nu, barrierParam, slackStateIneq, slackStateInputIneq, + dualStateIneq, dualStateInputIneq, initializeSlackAndDualVariables); + linearQuadraticApproximationTimer_.endTimer(); + + // Solve QP + solveQpTimer_.startTimer(); + const vector_t delta_x0 = initState - x[0]; + const auto deltaSolution = + getOCPSolution(delta_x0, barrierParam, slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq); + vector_array_t deltaLmdSol; + extractValueFunction(timeDiscretization, x, lmd, deltaSolution.deltaXSol, deltaLmdSol); + + solveQpTimer_.endTimer(); + + // Apply step + linesearchTimer_.startTimer(); + const scalar_t maxPrimalStepSize = settings_.usePrimalStepSizeForDual + ? std::min(deltaSolution.maxDualStepSize, deltaSolution.maxPrimalStepSize) + : deltaSolution.maxPrimalStepSize; + const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, metrics, barrierParam, + slackStateIneq, slackStateInputIneq); + const scalar_t dualStepSize = + settings_.usePrimalStepSizeForDual ? std::min(stepInfo.stepSize, deltaSolution.maxDualStepSize) : deltaSolution.maxDualStepSize; + performanceIndeces_.push_back(stepInfo.performanceAfterStep); + if (settings_.computeLagrangeMultipliers) { + for (int i = 0; i < lmd.size(); i++) { + lmd[i].noalias() += stepInfo.stepSize * deltaLmdSol[i]; + } + if (settings_.projectStateInputEqualityConstraints) { + for (int i = 0; i < nu.size(); i++) { + if (nu[i].size() > 0) { + nu[i].noalias() += stepInfo.stepSize * deltaSolution.deltaNuSol[i]; + // remaining term that depends on the costate + nu[i].noalias() += stepInfo.stepSize * projectionMultiplierCoefficients_[i].dfdcostate * deltaLmdSol[i + 1]; + } + } + } + } + for (int i = 1; i < dualStateIneq.size(); i++) { + dualStateIneq[i].noalias() += dualStepSize * deltaSolution.deltaDualStateIneq[i]; + } + for (int i = 0; i < dualStateInputIneq.size(); i++) { + dualStateInputIneq[i].noalias() += dualStepSize * deltaSolution.deltaDualStateInputIneq[i]; + } + linesearchTimer_.endTimer(); + + // Check convergence + convergence = checkConvergence(iter, barrierParam, baselinePerformance, stepInfo); + + // Update the barrier parameter + barrierParam = updateBarrierParameter(barrierParam, baselinePerformance, stepInfo); + + // Next iteration + ++iter; + ++totalNumIterations_; + } + + computeControllerTimer_.startTimer(); + primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + costateTrajectory_ = std::move(lmd); + projectionMultiplierTrajectory_ = std::move(nu); + slackStateIneqTrajectory_ = std::move(slackStateIneq); + dualStateIneqTrajectory_ = std::move(dualStateIneq); + slackStateInputIneqTrajectory_ = std::move(slackStateInputIneq); + dualStateInputIneqTrajectory_ = std::move(dualStateInputIneq); + problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); + computeControllerTimer_.endTimer(); + + ++numProblems_; + + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\nConvergence : " << toString(convergence) << "\n"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++ IPM solver has terminated ++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + } +} + +void IpmSolver::runParallel(std::function<void(int)> taskFunction) { + threadPool_.runParallel(std::move(taskFunction), settings_.nThreads); +} + +void IpmSolver::initializeCostateTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& stateTrajectory, + vector_array_t& costateTrajectory) { + const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // // size of the input trajectory + costateTrajectory.clear(); + costateTrajectory.reserve(N + 1); + const auto& ocpDefinition = ocpDefinitions_[0]; + + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; + ocpDefinition.preComputationPtr->requestFinal(request, timeDiscretization[N].time, stateTrajectory[N]); + const vector_t lmdN = -approximateFinalCost(ocpDefinition, timeDiscretization[N].time, stateTrajectory[N]).dfdx; + + // Determine till when to use the previous solution + scalar_t interpolateCostateTill = timeDiscretization.front().time; + if (primalSolution_.timeTrajectory_.size() >= 2) { + interpolateCostateTill = primalSolution_.timeTrajectory_.back(); + } + + const scalar_t initTime = getIntervalStart(timeDiscretization[0]); + if (initTime < interpolateCostateTill) { + costateTrajectory.push_back(LinearInterpolation::interpolate(initTime, primalSolution_.timeTrajectory_, costateTrajectory_)); + } else { + costateTrajectory.push_back(lmdN); + // costateTrajectory.push_back(vector_t::Zero(stateTrajectory[0].size())); + } + + for (int i = 0; i < N; i++) { + const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); + if (nextTime > interpolateCostateTill) { // Initialize with zero + // costateTrajectory.push_back(vector_t::Zero(stateTrajectory[i + 1].size())); + costateTrajectory.push_back(lmdN); + } else { // interpolate previous solution + costateTrajectory.push_back(LinearInterpolation::interpolate(nextTime, primalSolution_.timeTrajectory_, costateTrajectory_)); + } + } +} + +void IpmSolver::initializeProjectionMultiplierTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, + vector_array_t& projectionMultiplierTrajectory) { + const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // // size of the input trajectory + projectionMultiplierTrajectory.clear(); + projectionMultiplierTrajectory.reserve(N); + const auto& ocpDefinition = ocpDefinitions_[0]; + + // Determine till when to use the previous solution + scalar_t interpolateInputTill = timeDiscretization.front().time; + if (primalSolution_.timeTrajectory_.size() >= 2) { + interpolateInputTill = primalSolution_.timeTrajectory_[primalSolution_.timeTrajectory_.size() - 2]; + } + + auto interpolateProjectionMultiplierTrajectory = [&](scalar_t time) -> vector_t { + const size_t numConstraints = ocpDefinition.equalityConstraintPtr->getNumConstraints(time); + const size_t index = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_).first; + if (projectionMultiplierTrajectory_.size() > index + 1) { + if (projectionMultiplierTrajectory_[index].size() == numConstraints && + projectionMultiplierTrajectory_[index].size() == projectionMultiplierTrajectory_[index + 1].size()) { + return LinearInterpolation::interpolate(time, primalSolution_.timeTrajectory_, projectionMultiplierTrajectory_); + } + } + if (projectionMultiplierTrajectory_.size() > index) { + if (projectionMultiplierTrajectory_[index].size() == numConstraints) { + return projectionMultiplierTrajectory_[index]; + } + } + return vector_t::Zero(numConstraints); + }; + + for (int i = 0; i < N; i++) { + if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { + // Event Node + projectionMultiplierTrajectory.push_back(vector_t()); // no input at event node + } else { + // Intermediate node + const scalar_t time = getIntervalStart(timeDiscretization[i]); + const size_t numConstraints = ocpDefinition.equalityConstraintPtr->getNumConstraints(time); + if (time > interpolateInputTill) { // Initialize with zero + projectionMultiplierTrajectory.push_back(vector_t::Zero(numConstraints)); + } else { // interpolate previous solution + projectionMultiplierTrajectory.push_back(interpolateProjectionMultiplierTrajectory(time)); + } + } + } +} + +IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta_x0, scalar_t barrierParam, + const vector_array_t& slackStateIneq, const vector_array_t& slackStateInputIneq, + const vector_array_t& dualStateIneq, const vector_array_t& dualStateInputIneq) { + // Solve the QP + OcpSubproblemSolution solution; + auto& deltaXSol = solution.deltaXSol; + auto& deltaUSol = solution.deltaUSol; + hpipm_status status; + const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); + if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { + hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, &stateInputEqConstraints_)); + status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, &stateInputEqConstraints_, deltaXSol, deltaUSol, + settings_.printSolverStatus); + } else { // without constraints, or when using projection, we have an unconstrained QP. + hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, nullptr)); + status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); + } + + if (status != hpipm_status::SUCCESS) { + throw std::runtime_error("[IpmSolver] Failed to solve QP"); + } + + // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + solution.armijoDescentMetric = armijoDescentMetric(lagrangian_, deltaXSol, deltaUSol); + + // Problem horizon + const int N = static_cast<int>(deltaXSol.size()) - 1; + + auto& deltaNuSol = solution.deltaNuSol; + auto& deltaSlackStateIneq = solution.deltaSlackStateIneq; + auto& deltaSlackStateInputIneq = solution.deltaSlackStateInputIneq; + auto& deltaDualStateIneq = solution.deltaDualStateIneq; + auto& deltaDualStateInputIneq = solution.deltaDualStateInputIneq; + deltaNuSol.resize(N); + deltaSlackStateIneq.resize(N + 1); + deltaSlackStateInputIneq.resize(N + 1); + deltaDualStateIneq.resize(N + 1); + deltaDualStateInputIneq.resize(N + 1); + + scalar_array_t primalStepSizes(N + 1, 1.0); + scalar_array_t dualStepSizes(N + 1, 1.0); + + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + vector_t tmp; // 1 temporary for re-use for projection. + + int i = timeIndex++; + while (i < N) { + deltaSlackStateIneq[i] = ipm::retrieveSlackDirection(stateIneqConstraints_[i], deltaXSol[i], barrierParam, slackStateIneq[i]); + deltaDualStateIneq[i] = ipm::retrieveDualDirection(barrierParam, slackStateIneq[i], dualStateIneq[i], deltaSlackStateIneq[i]); + deltaSlackStateInputIneq[i] = + ipm::retrieveSlackDirection(stateInputIneqConstraints_[i], deltaXSol[i], deltaUSol[i], barrierParam, slackStateInputIneq[i]); + deltaDualStateInputIneq[i] = + ipm::retrieveDualDirection(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], deltaSlackStateInputIneq[i]); + primalStepSizes[i] = std::min( + ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin), + ipm::fractionToBoundaryStepSize(slackStateInputIneq[i], deltaSlackStateInputIneq[i], settings_.fractionToBoundaryMargin)); + dualStepSizes[i] = + std::min(ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin), + ipm::fractionToBoundaryStepSize(dualStateInputIneq[i], deltaDualStateInputIneq[i], settings_.fractionToBoundaryMargin)); + // Re-map the projected input back to the original space. + if (constraintsProjection_[i].f.size() > 0) { + if (settings_.computeLagrangeMultipliers) { + const auto& dfdx = projectionMultiplierCoefficients_[i].dfdx; + const auto& dfdu = projectionMultiplierCoefficients_[i].dfdu; + const auto& dfdcostate = projectionMultiplierCoefficients_[i].dfdcostate; + const auto& f = projectionMultiplierCoefficients_[i].f; + deltaNuSol[i].noalias() = dfdx * deltaXSol[i] + dfdu * deltaUSol[i] + f; + } + tmp.noalias() = constraintsProjection_[i].dfdu * deltaUSol[i]; + deltaUSol[i] = tmp + constraintsProjection_[i].f; + deltaUSol[i].noalias() += constraintsProjection_[i].dfdx * deltaXSol[i]; + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + deltaSlackStateIneq[i] = ipm::retrieveSlackDirection(stateIneqConstraints_[i], deltaXSol[i], barrierParam, slackStateIneq[i]); + deltaDualStateIneq[i] = ipm::retrieveDualDirection(barrierParam, slackStateIneq[i], dualStateIneq[i], deltaSlackStateIneq[i]); + primalStepSizes[i] = ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin); + dualStepSizes[i] = ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin); + } + }; + runParallel(std::move(parallelTask)); + + solution.maxPrimalStepSize = *std::min_element(primalStepSizes.begin(), primalStepSizes.end()); + solution.maxDualStepSize = *std::min_element(dualStepSizes.begin(), dualStepSizes.end()); + + return solution; +} + +void IpmSolver::extractValueFunction(const std::vector<AnnotatedTime>& time, const vector_array_t& x, const vector_array_t& lmd, + const vector_array_t& deltaXSol, vector_array_t& deltaLmdSol) { + valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], lagrangian_[0]); + // Compute costate directions + deltaLmdSol.resize(deltaXSol.size()); + for (int i = 0; i < time.size(); ++i) { + deltaLmdSol[i] = valueFunction_[i].dfdx; + deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * x[i]; + } + // Correct for linearization state + for (int i = 0; i < time.size(); ++i) { + valueFunction_[i].dfdx.noalias() -= valueFunction_[i].dfdxx * x[i]; + if (settings_.computeLagrangeMultipliers) { + valueFunction_[i].dfdx.noalias() += lmd[i]; + } + } +} + +PrimalSolution IpmSolver::toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) { + if (settings_.useFeedbackPolicy) { + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); + matrix_array_t KMatrices = hpipmInterface_.getRiccatiFeedback(dynamics_[0], lagrangian_[0]); + if (settings_.projectStateInputEqualityConstraints) { + multiple_shooting::remapProjectedGain(constraintsProjection_, KMatrices); + } + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), std::move(KMatrices)); + + } else { + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u)); + } +} + +PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, + const vector_array_t& x, const vector_array_t& u, std::vector<Metrics>& metrics, + const vector_array_t& lmd, const vector_array_t& nu, scalar_t barrierParam, + vector_array_t& slackStateIneq, vector_array_t& slackStateInputIneq, + vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq, + bool initializeSlackAndDualVariables) { + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + + std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); + lagrangian_.resize(N + 1); + dynamics_.resize(N); + stateInputEqConstraints_.resize(N + 1); + stateIneqConstraints_.resize(N + 1); + stateInputIneqConstraints_.resize(N + 1); + constraintsProjection_.resize(N); + projectionMultiplierCoefficients_.resize(N); + metrics.resize(N + 1); + + if (initializeSlackAndDualVariables) { + slackStateIneq.resize(N + 1); + slackStateInputIneq.resize(N + 1); + dualStateIneq.resize(N + 1); + dualStateInputIneq.resize(N + 1); + } + + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; + PerformanceIndex workerPerformance; // Accumulate performance in local variable + + int i = timeIndex++; + while (i < N) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + // Event node + auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); + if (initializeSlackAndDualVariables) { + slackStateIneq[i] = + ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } + metrics[i] = multiple_shooting::computeMetrics(result); + workerPerformance += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]); + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i].resize(0, x[i].size()); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); + stateInputIneqConstraints_[i].resize(0, x[i].size()); + constraintsProjection_[i].resize(0, x[i].size()); + projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); + if (settings_.computeLagrangeMultipliers) { + lagrangian_[i] = multiple_shooting::evaluateLagrangianEventNode(lmd[i], lmd[i + 1], std::move(result.cost), dynamics_[i]); + } else { + lagrangian_[i] = std::move(result.cost); + } + + ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]); + if (settings_.computeLagrangeMultipliers) { + workerPerformance.dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); + } + workerPerformance.dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); + } else { + // Normal, intermediate node + const scalar_t ti = getIntervalStart(time[i]); + const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); + auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + if (i == 0) { + // Disable the state-only inequality constraints at the initial node + result.stateIneqConstraints.setZero(0, x[i].size()); + std::fill(result.constraintsSize.stateIneq.begin(), result.constraintsSize.stateIneq.end(), 0); + } + if (initializeSlackAndDualVariables) { + slackStateIneq[i] = ipm::initializeSlackVariable(result.stateIneqConstraints.f, settings_.initialSlackLowerBound, + settings_.initialSlackMarginRate); + slackStateInputIneq[i] = ipm::initializeSlackVariable(result.stateInputIneqConstraints.f, settings_.initialSlackLowerBound, + settings_.initialSlackMarginRate); + dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } + metrics[i] = multiple_shooting::computeMetrics(result); + workerPerformance += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); + if (settings_.projectStateInputEqualityConstraints) { + multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers); + } + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); + stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); + stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); + constraintsProjection_[i] = std::move(result.constraintsProjection); + projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients); + if (settings_.computeLagrangeMultipliers) { + lagrangian_[i] = multiple_shooting::evaluateLagrangianIntermediateNode(lmd[i], lmd[i + 1], nu[i], std::move(result.cost), + dynamics_[i], stateInputEqConstraints_[i]); + } else { + lagrangian_[i] = std::move(result.cost); + } + + ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]); + ipm::condenseIneqConstraints(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], stateInputIneqConstraints_[i], + lagrangian_[i]); + if (settings_.computeLagrangeMultipliers) { + workerPerformance.dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); + } + workerPerformance.dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); + workerPerformance.dualFeasibilitiesSSE += + ipm::evaluateComplementarySlackness(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i]); + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + const scalar_t tN = getIntervalStart(time[N]); + auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); + if (initializeSlackAndDualVariables) { + slackStateIneq[N] = + ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[N] = + ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + } + metrics[i] = multiple_shooting::computeMetrics(result); + workerPerformance += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]); + stateInputEqConstraints_[i].resize(0, x[i].size()); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); + if (settings_.computeLagrangeMultipliers) { + lagrangian_[i] = multiple_shooting::evaluateLagrangianTerminalNode(lmd[i], std::move(result.cost)); + } else { + lagrangian_[i] = std::move(result.cost); + } + ipm::condenseIneqConstraints(barrierParam, slackStateIneq[N], dualStateIneq[N], stateIneqConstraints_[N], lagrangian_[N]); + if (settings_.computeLagrangeMultipliers) { + workerPerformance.dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]); + } + workerPerformance.dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[N], dualStateIneq[N]); + } + + // Accumulate! Same worker might run multiple tasks + performance[workerId] += workerPerformance; + }; + runParallel(std::move(parallelTask)); + + // Account for init state in performance + performance.front().dynamicsViolationSSE += (initState - x.front()).squaredNorm(); + + // Sum performance of the threads + PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); + totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + + return totalPerformance; +} + +PerformanceIndex IpmSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, std::vector<Metrics>& metrics, scalar_t barrierParam, + const vector_array_t& slackStateIneq, const vector_array_t& slackStateInputIneq) { + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + metrics.resize(N + 1); + + std::vector<PerformanceIndex> performance(settings_.nThreads, PerformanceIndex()); + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; + + int i = timeIndex++; + while (i < N) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + // Event node + metrics[i] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]); + performance[workerId] += ipm::toPerformanceIndex(metrics[i], barrierParam, slackStateIneq[i]); + } else { + // Normal, intermediate node + const scalar_t ti = getIntervalStart(time[i]); + const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); + const bool enableStateInequalityConstraints = (i > 0); + metrics[i] = multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + performance[workerId] += ipm::toPerformanceIndex(metrics[i], dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i], + enableStateInequalityConstraints); + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + const scalar_t tN = getIntervalStart(time[N]); + metrics[N] = multiple_shooting::computeTerminalMetrics(ocpDefinition, tN, x[N]); + performance[workerId] += ipm::toPerformanceIndex(metrics[N], barrierParam, slackStateIneq[N]); + } + }; + runParallel(std::move(parallelTask)); + + // Account for initial state in performance + const vector_t initDynamicsViolation = initState - x.front(); + metrics.front().dynamicsViolation += initDynamicsViolation; + performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm(); + + // Sum performance of the threads + PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); + totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + return totalPerformance; +} + +ipm::StepInfo IpmSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u, std::vector<Metrics>& metrics, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& slackStateInputIneq) { + using StepType = FilterLinesearch::StepType; + + /* + * Filter linesearch based on: + * "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming" + * https://link.springer.com/article/10.1007/s10107-004-0559-y + */ + if (settings_.printLinesearch) { + std::cerr << std::setprecision(9) << std::fixed; + std::cerr << "\n=== Linesearch ===\n"; + std::cerr << "Baseline:\n" << baseline << "\n"; + } + + // Baseline costs + const scalar_t baselineConstraintViolation = FilterLinesearch::totalConstraintViolation(baseline); + + // Update norm + const auto& dx = subproblemSolution.deltaXSol; + const auto& du = subproblemSolution.deltaUSol; + const auto& deltaSlackStateIneq = subproblemSolution.deltaSlackStateIneq; + const auto& deltaSlackStateInputIneq = subproblemSolution.deltaSlackStateInputIneq; + const auto deltaUnorm = multiple_shooting::trajectoryNorm(du); + const auto deltaXnorm = multiple_shooting::trajectoryNorm(dx); + + scalar_t alpha = subproblemSolution.maxPrimalStepSize; + vector_array_t xNew(x.size()); + vector_array_t uNew(u.size()); + vector_array_t slackStateIneqNew(slackStateIneq.size()); + vector_array_t slackStateInputIneqNew(slackStateInputIneq.size()); + std::vector<Metrics> metricsNew(metrics.size()); + do { + // Compute step + multiple_shooting::incrementTrajectory(u, du, alpha, uNew); + multiple_shooting::incrementTrajectory(x, dx, alpha, xNew); + multiple_shooting::incrementTrajectory(slackStateIneq, deltaSlackStateIneq, alpha, slackStateIneqNew); + multiple_shooting::incrementTrajectory(slackStateInputIneq, deltaSlackStateInputIneq, alpha, slackStateInputIneqNew); + + // Compute cost and constraints + const PerformanceIndex performanceNew = + computePerformance(timeDiscretization, initState, xNew, uNew, metricsNew, barrierParam, slackStateIneqNew, slackStateInputIneqNew); + + // Step acceptance and record step type + bool stepAccepted; + StepType stepType; + std::tie(stepAccepted, stepType) = + filterLinesearch_.acceptStep(baseline, performanceNew, alpha * subproblemSolution.armijoDescentMetric); + + if (settings_.printLinesearch) { + std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepType) + << (stepAccepted ? std::string{" (Accepted)"} : std::string{" (Rejected)"}) << "\n"; + std::cerr << "|dx| = " << alpha * deltaXnorm << "\t|du| = " << alpha * deltaUnorm << "\n"; + std::cerr << performanceNew << "\n"; + } + + if (stepAccepted) { // Return if step accepted + x = std::move(xNew); + u = std::move(uNew); + slackStateIneq = std::move(slackStateIneqNew); + slackStateInputIneq = std::move(slackStateInputIneqNew); + metrics = std::move(metricsNew); + + // Prepare step info + ipm::StepInfo stepInfo; + stepInfo.stepSize = alpha; + stepInfo.stepType = stepType; + stepInfo.dx_norm = alpha * deltaXnorm; + stepInfo.du_norm = alpha * deltaUnorm; + stepInfo.performanceAfterStep = performanceNew; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(performanceNew); + return stepInfo; + + } else { // Try smaller step + alpha *= settings_.alpha_decay; + + // Detect too small step size during back-tracking to escape early. Prevents going all the way to alpha_min + if (alpha * deltaXnorm < settings_.deltaTol && alpha * deltaUnorm < settings_.deltaTol) { + if (settings_.printLinesearch) { + std::cerr << "Exiting linesearch early due to too small primal steps |dx|: " << alpha * deltaXnorm + << ", and or |du|: " << alpha * deltaUnorm << " are below deltaTol: " << settings_.deltaTol << "\n"; + } + break; + } + } + } while (alpha >= settings_.alpha_min); + + // Alpha_min reached -> Don't take a step + ipm::StepInfo stepInfo; + stepInfo.stepSize = 0.0; + stepInfo.stepType = StepType::ZERO; + stepInfo.dx_norm = 0.0; + stepInfo.du_norm = 0.0; + stepInfo.performanceAfterStep = baseline; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline); + + if (settings_.printLinesearch) { + std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; + } + + return stepInfo; +} + +scalar_t IpmSolver::updateBarrierParameter(scalar_t currentBarrierParameter, const PerformanceIndex& baseline, + const ipm::StepInfo& stepInfo) const { + if (currentBarrierParameter <= settings_.targetBarrierParameter) { + return currentBarrierParameter; + } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.barrierReductionCostTol && + FilterLinesearch::totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.barrierReductionConstraintTol) { + return std::min((currentBarrierParameter * settings_.barrierLinearDecreaseFactor), + std::pow(currentBarrierParameter, settings_.barrierSuperlinearDecreasePower)); + } else { + return currentBarrierParameter; + } +} + +ipm::Convergence IpmSolver::checkConvergence(int iteration, scalar_t barrierParam, const PerformanceIndex& baseline, + const ipm::StepInfo& stepInfo) const { + using Convergence = ipm::Convergence; + if ((iteration + 1) >= settings_.ipmIteration) { + // Converged because the next iteration would exceed the specified number of iterations + return Convergence::ITERATIONS; + } else if (stepInfo.stepSize < settings_.alpha_min) { + // Converged because step size is below the specified minimum + return Convergence::STEPSIZE; + } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.costTol && + FilterLinesearch::totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { + // Converged because the change in merit is below the specified tolerance while the constraint violation is below the minimum + return Convergence::METRICS; + } else if (stepInfo.dx_norm < settings_.deltaTol && stepInfo.du_norm < settings_.deltaTol && + barrierParam <= settings_.targetBarrierParameter) { + // Converged because the change in primal variables is below the specified tolerance + return Convergence::PRIMAL; + } else { + // None of the above convergence criteria were met -> not converged. + return Convergence::FALSE; + } +} + +} // namespace ocs2 diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp new file mode 100644 index 000000000..79a8a9069 --- /dev/null +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -0,0 +1,200 @@ +#include <gtest/gtest.h> +#include <cstdlib> +#include <ctime> +#include <iostream> +#include <memory> + +#include "ocs2_ipm/IpmSolver.h" + +#include <ocs2_core/initialization/DefaultInitializer.h> + +#include <ocs2_oc/test/EXP0.h> + +using namespace ocs2; + +class EXP0_StateIneqConstraints final : public StateConstraint { + public: + EXP0_StateIneqConstraints(const vector_t& xmin, const vector_t& xmax) + : StateConstraint(ConstraintOrder::Linear), xmin_(xmin), xmax_(xmax) {} + ~EXP0_StateIneqConstraints() override = default; + + EXP0_StateIneqConstraints* clone() const override { return new EXP0_StateIneqConstraints(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 4; } + + vector_t getValue(scalar_t t, const vector_t& x, const PreComputation&) const override { + vector_t e(4); + e.head(2) = x - xmin_; + e.tail(2) = xmax_ - x; + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e; + e.f = getValue(t, x, preComp); + e.dfdx = matrix_t::Zero(4, x.size()); + e.dfdx.topLeftCorner(2, 2) = matrix_t::Identity(2, 2); + e.dfdx.bottomRightCorner(2, 2) = -matrix_t::Identity(2, 2); + return e; + } + + private: + vector_t xmin_, xmax_; +}; + +class EXP0_StateInputIneqConstraints final : public StateInputConstraint { + public: + EXP0_StateInputIneqConstraints(scalar_t umin, scalar_t umax) : StateInputConstraint(ConstraintOrder::Linear), umin_(umin), umax_(umax) {} + ~EXP0_StateInputIneqConstraints() override = default; + + EXP0_StateInputIneqConstraints* clone() const override { return new EXP0_StateInputIneqConstraints(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 2; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { + vector_t e(2); + e << (u.coeff(0) - umin_), (umax_ - u.coeff(0)); + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e; + e.f = getValue(t, x, u, preComp); + e.dfdx = matrix_t::Zero(2, x.size()); + e.dfdu = (matrix_t(2, 1) << 1, -1).finished(); + return e; + } + + private: + scalar_t umin_, umax_; +}; + +TEST(Exp0Test, Unconstrained) { + static constexpr size_t STATE_DIM = 2; + static constexpr size_t INPUT_DIM = 1; + + // Solver settings + ipm::Settings settings; + settings.dt = 0.01; + settings.ipmIteration = 20; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 1; + + settings.initialBarrierParameter = 1.0e-02; + settings.targetBarrierParameter = 1.0e-04; + settings.barrierLinearDecreaseFactor = 0.2; + settings.barrierSuperlinearDecreasePower = 1.5; + settings.fractionToBoundaryMargin = 0.995; + + const scalar_array_t initEventTimes{0.1897}; + const size_array_t modeSequence{0, 1}; + auto referenceManagerPtr = getExp0ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp0Problem(referenceManagerPtr); + + const scalar_t startTime = 0.0; + const scalar_t finalTime = 2.0; + const vector_t initState = (vector_t(STATE_DIM) << 0.0, 2.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); + solver.run(startTime, initState, finalTime); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + std::cout << "Optimal unconstrained trajectory" << std::endl; + for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { + std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] + << "\t state: " << primalSolution.stateTrajectory_[i].transpose() + << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; + } +} + +TEST(Exp0Test, Constrained) { + static constexpr size_t STATE_DIM = 2; + static constexpr size_t INPUT_DIM = 1; + + // Solver settings + ipm::Settings settings; + settings.dt = 0.01; + settings.ipmIteration = 20; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 1; + + settings.initialBarrierParameter = 1.0e-02; + settings.targetBarrierParameter = 1.0e-04; + settings.barrierLinearDecreaseFactor = 0.2; + settings.barrierSuperlinearDecreasePower = 1.5; + settings.fractionToBoundaryMargin = 0.995; + + const scalar_array_t initEventTimes{0.1897}; + const size_array_t modeSequence{0, 1}; + auto referenceManagerPtr = getExp0ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp0Problem(referenceManagerPtr); + + // add inequality constraints + const scalar_t umin = -7.5; + const scalar_t umax = 7.5; + std::unique_ptr<StateInputConstraint> stateInputIneqConstraint(new EXP0_StateInputIneqConstraints(umin, umax)); + problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint)); + const vector_t xmin = (vector_t(2) << -7.5, -7.5).finished(); + const vector_t xmax = (vector_t(2) << 7.5, 7.5).finished(); + std::unique_ptr<StateConstraint> stateIneqConstraint(new EXP0_StateIneqConstraints(xmin, xmax)); + std::unique_ptr<StateConstraint> finalStateIneqConstraint(new EXP0_StateIneqConstraints(xmin, xmax)); + problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint)); + problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint)); + + const scalar_t startTime = 0.0; + const scalar_t finalTime = 2.0; + const vector_t initState = (vector_t(STATE_DIM) << 0.0, 2.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); + // std::cout << solver.getBenchmarkingInformation() << std::endl; + + const auto primalSolution = solver.primalSolution(finalTime); + std::cout << "Optimal unconstrained trajectory" << std::endl; + for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { + std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] + << "\t state: " << primalSolution.stateTrajectory_[i].transpose() + << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; + } + + // check constraint satisfaction + for (const auto& e : primalSolution.stateTrajectory_) { + if (e.size() > 0) { + EXPECT_TRUE(e.coeff(0) >= xmin.coeff(0)); + EXPECT_TRUE(e.coeff(1) >= xmin.coeff(1)); + EXPECT_TRUE(e.coeff(0) <= xmax.coeff(0)); + EXPECT_TRUE(e.coeff(1) <= xmax.coeff(1)); + } + } + for (const auto& e : primalSolution.inputTrajectory_) { + if (e.size() > 0) { + EXPECT_TRUE(e.coeff(0) >= umin); + EXPECT_TRUE(e.coeff(0) <= umax); + } + } +} \ No newline at end of file diff --git a/ocs2_ipm/test/Exp1Test.cpp b/ocs2_ipm/test/Exp1Test.cpp new file mode 100644 index 000000000..9c2a9a99e --- /dev/null +++ b/ocs2_ipm/test/Exp1Test.cpp @@ -0,0 +1,297 @@ +#include <gtest/gtest.h> +#include <cstdlib> +#include <ctime> +#include <iostream> +#include <memory> + +#include "ocs2_ipm/IpmSolver.h" + +#include <ocs2_core/initialization/DefaultInitializer.h> + +#include <ocs2_oc/test/EXP1.h> + +using namespace ocs2; + +class EXP1_StateIneqConstraints final : public StateConstraint { + public: + EXP1_StateIneqConstraints(const vector_t& xmin, const vector_t& xmax) + : StateConstraint(ConstraintOrder::Linear), xmin_(xmin), xmax_(xmax) {} + ~EXP1_StateIneqConstraints() override = default; + + EXP1_StateIneqConstraints* clone() const override { return new EXP1_StateIneqConstraints(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 4; } + + vector_t getValue(scalar_t t, const vector_t& x, const PreComputation&) const override { + vector_t e(4); + e.head(2) = x - xmin_; + e.tail(2) = xmax_ - x; + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e; + e.f = getValue(t, x, preComp); + e.dfdx = matrix_t::Zero(4, x.size()); + e.dfdx.topLeftCorner(2, 2) = matrix_t::Identity(2, 2); + e.dfdx.bottomRightCorner(2, 2) = -matrix_t::Identity(2, 2); + return e; + } + + private: + vector_t xmin_, xmax_; +}; + +class EXP1_StateInputIneqConstraints final : public StateInputConstraint { + public: + EXP1_StateInputIneqConstraints(scalar_t umin, scalar_t umax) : StateInputConstraint(ConstraintOrder::Linear), umin_(umin), umax_(umax) {} + ~EXP1_StateInputIneqConstraints() override = default; + + EXP1_StateInputIneqConstraints* clone() const override { return new EXP1_StateInputIneqConstraints(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 2; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { + vector_t e(2); + e << (u.coeff(0) - umin_), (umax_ - u.coeff(0)); + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e; + e.f = getValue(t, x, u, preComp); + e.dfdx = matrix_t::Zero(2, x.size()); + e.dfdu = (matrix_t(2, 1) << 1, -1).finished(); + return e; + } + + private: + scalar_t umin_, umax_; +}; + +class EXP1_MixedStateInputIneqConstraints final : public StateInputConstraint { + public: + EXP1_MixedStateInputIneqConstraints(scalar_t xumin, scalar_t xumax) + : StateInputConstraint(ConstraintOrder::Linear), xumin_(xumin), xumax_(xumax) {} + ~EXP1_MixedStateInputIneqConstraints() override = default; + + EXP1_MixedStateInputIneqConstraints* clone() const override { return new EXP1_MixedStateInputIneqConstraints(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 2; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { + vector_t e(2); + e << (x.coeff(0) * u.coeff(0) - xumin_), (xumax_ - x.coeff(1) * u.coeff(0)); + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e; + e.f = getValue(t, x, u, preComp); + e.dfdx = (matrix_t(2, 2) << u.coeff(0), 0, 0, -u.coeff(0)).finished(); + e.dfdu = (matrix_t(2, 1) << x.coeff(0), -x.coeff(1)).finished(); + return e; + } + + private: + scalar_t xumin_, xumax_; +}; + +TEST(Exp1Test, Unconstrained) { + static constexpr size_t STATE_DIM = 2; + static constexpr size_t INPUT_DIM = 1; + + // Solver settings + ipm::Settings settings; + settings.dt = 0.01; + settings.ipmIteration = 20; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 1; + + settings.initialBarrierParameter = 1.0e-02; + settings.targetBarrierParameter = 1.0e-04; + settings.barrierLinearDecreaseFactor = 0.2; + settings.barrierSuperlinearDecreasePower = 1.5; + settings.fractionToBoundaryMargin = 0.995; + + const scalar_array_t initEventTimes{0.2262, 1.0176}; + const size_array_t modeSequence{0, 1, 2}; + auto referenceManagerPtr = getExp1ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp1Problem(referenceManagerPtr); + + const scalar_t startTime = 0.0; + const scalar_t finalTime = 3.0; + const vector_t initState = (vector_t(STATE_DIM) << 2.0, 3.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + auto initializerPtr = std::unique_ptr<Initializer>(new DefaultInitializer(INPUT_DIM)); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + std::cout << "Optimal unconstrained trajectory" << std::endl; + for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { + std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] + << "\t state: " << primalSolution.stateTrajectory_[i].transpose() + << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; + } +} + +TEST(Exp1Test, Constrained) { + static constexpr size_t STATE_DIM = 2; + static constexpr size_t INPUT_DIM = 1; + + // Solver settings + ipm::Settings settings; + settings.dt = 0.01; + settings.ipmIteration = 100; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 1; + + settings.initialBarrierParameter = 1.0e-02; + settings.targetBarrierParameter = 1.0e-04; + settings.barrierLinearDecreaseFactor = 0.2; + settings.barrierSuperlinearDecreasePower = 1.5; + settings.fractionToBoundaryMargin = 0.995; + + const scalar_array_t initEventTimes{0.2262, 1.0176}; + const size_array_t modeSequence{0, 1, 2}; + auto referenceManagerPtr = getExp1ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp1Problem(referenceManagerPtr); + + // add inequality constraints + const scalar_t umin = -1.0; + const scalar_t umax = 1.0; + std::unique_ptr<StateInputConstraint> stateInputIneqConstraint(new EXP1_StateInputIneqConstraints(umin, umax)); + problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint)); + const vector_t xmin = (vector_t(2) << -0.0, -0.0).finished(); + const vector_t xmax = (vector_t(2) << 3.0, 4.0).finished(); + std::unique_ptr<StateConstraint> stateIneqConstraint(new EXP1_StateIneqConstraints(xmin, xmax)); + std::unique_ptr<StateConstraint> finalStateIneqConstraint(new EXP1_StateIneqConstraints(xmin, xmax)); + problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint)); + problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint)); + + const scalar_t startTime = 0.0; + const scalar_t finalTime = 3.0; + const vector_t initState = (vector_t(STATE_DIM) << 2.0, 3.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + auto initializerPtr = std::unique_ptr<Initializer>(new DefaultInitializer(INPUT_DIM)); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + std::cout << "Optimal unconstrained trajectory" << std::endl; + for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { + std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] + << "\t state: " << primalSolution.stateTrajectory_[i].transpose() + << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; + } + + // check constraint satisfaction + for (const auto& e : primalSolution.stateTrajectory_) { + if (e.size() > 0) { + EXPECT_TRUE(e.coeff(0) >= xmin.coeff(0)); + EXPECT_TRUE(e.coeff(1) >= xmin.coeff(1)); + EXPECT_TRUE(e.coeff(0) <= xmax.coeff(0)); + EXPECT_TRUE(e.coeff(1) <= xmax.coeff(1)); + } + } + for (const auto& e : primalSolution.inputTrajectory_) { + if (e.size() > 0) { + EXPECT_TRUE(e.coeff(0) >= umin); + EXPECT_TRUE(e.coeff(0) <= umax); + } + } +} + +TEST(Exp1Test, MixedConstrained) { + static constexpr size_t STATE_DIM = 2; + static constexpr size_t INPUT_DIM = 1; + + // Solver settings + ipm::Settings settings; + settings.dt = 0.01; + settings.ipmIteration = 100; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 1; + + settings.initialBarrierParameter = 1.0e-02; + settings.targetBarrierParameter = 1.0e-04; + settings.barrierLinearDecreaseFactor = 0.2; + settings.barrierSuperlinearDecreasePower = 1.5; + settings.fractionToBoundaryMargin = 0.995; + + const scalar_array_t initEventTimes{0.2262, 1.0176}; + const size_array_t modeSequence{0, 1, 2}; + auto referenceManagerPtr = getExp1ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp1Problem(referenceManagerPtr); + + // add inequality constraints + const scalar_t xumin = -1.0; + const scalar_t xumax = 1.0; + std::unique_ptr<StateInputConstraint> stateInputIneqConstraint(new EXP1_MixedStateInputIneqConstraints(xumin, xumax)); + auto stateInputIneqConstraintCloned = stateInputIneqConstraint->clone(); + problem.inequalityConstraintPtr->add("bound", std::move(stateInputIneqConstraint)); + const scalar_t startTime = 0.0; + const scalar_t finalTime = 3.0; + const vector_t initState = (vector_t(STATE_DIM) << 2.0, 3.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + auto initializerPtr = std::unique_ptr<Initializer>(new DefaultInitializer(INPUT_DIM)); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + std::cout << "Optimal unconstrained trajectory" << std::endl; + for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { + std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] + << "\t state: " << primalSolution.stateTrajectory_[i].transpose() + << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; + } + + // check constraint satisfaction + const size_t N = primalSolution.inputTrajectory_.size(); + for (size_t i = 0; i < N; ++i) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, PreComputation()); + EXPECT_TRUE(constraintValue.minCoeff() >= 0.0); + } +} diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp new file mode 100644 index 000000000..3449d6c47 --- /dev/null +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -0,0 +1,426 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <gtest/gtest.h> + +#include "ocs2_ipm/IpmSolver.h" + +#include <ocs2_core/initialization/DefaultInitializer.h> + +#include <ocs2_oc/test/circular_kinematics.h> + +namespace ocs2 { + +class CircleKinematics_StateIneqConstraints final : public StateConstraint { + public: + CircleKinematics_StateIneqConstraints(const vector_t& xmin, const vector_t& xmax) + : StateConstraint(ConstraintOrder::Linear), xmin_(xmin), xmax_(xmax) {} + ~CircleKinematics_StateIneqConstraints() override = default; + + CircleKinematics_StateIneqConstraints* clone() const override { return new CircleKinematics_StateIneqConstraints(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 4; } + + vector_t getValue(scalar_t t, const vector_t& x, const PreComputation&) const override { + vector_t e(4); + e.head(2) = x - xmin_; + e.tail(2) = xmax_ - x; + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e; + e.f = getValue(t, x, preComp); + e.dfdx = matrix_t::Zero(4, x.size()); + e.dfdx.topLeftCorner(2, 2) = matrix_t::Identity(2, 2); + e.dfdx.bottomRightCorner(2, 2) = -matrix_t::Identity(2, 2); + return e; + } + + private: + vector_t xmin_, xmax_; +}; + +class CircleKinematics_StateInputIneqConstraints final : public StateInputConstraint { + public: + CircleKinematics_StateInputIneqConstraints(const vector_t& umin, const vector_t& umax) + : StateInputConstraint(ConstraintOrder::Linear), umin_(umin), umax_(umax) {} + ~CircleKinematics_StateInputIneqConstraints() override = default; + + CircleKinematics_StateInputIneqConstraints* clone() const override { return new CircleKinematics_StateInputIneqConstraints(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 4; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { + vector_t e(4); + e.head(2) = u - umin_; + e.tail(2) = umax_ - u; + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e; + e.f = getValue(t, x, u, preComp); + e.dfdx = matrix_t::Zero(4, x.size()); + e.dfdu = matrix_t::Zero(4, u.size()); + e.dfdu.topLeftCorner(2, 2) = matrix_t::Identity(2, 2); + e.dfdu.bottomRightCorner(2, 2) = -matrix_t::Identity(2, 2); + return e; + } + + private: + vector_t umin_, umax_; +}; + +class CircleKinematics_MixedStateInputIneqConstraints final : public StateInputConstraint { + public: + CircleKinematics_MixedStateInputIneqConstraints(scalar_t xumin, scalar_t xumax) + : StateInputConstraint(ConstraintOrder::Linear), xumin_(xumin), xumax_(xumax) {} + ~CircleKinematics_MixedStateInputIneqConstraints() override = default; + + CircleKinematics_MixedStateInputIneqConstraints* clone() const override { + return new CircleKinematics_MixedStateInputIneqConstraints(*this); + } + + size_t getNumConstraints(scalar_t time) const override { return 2; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { + vector_t e(2); + e << (x.coeff(0) * u.coeff(0) - xumin_), (xumax_ - x.coeff(1) * u.coeff(1)); + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e; + e.f = getValue(t, x, u, preComp); + e.dfdx = (matrix_t(2, 2) << u.coeff(0), 0, 0, -u.coeff(1)).finished(); + e.dfdu = (matrix_t(2, 2) << x.coeff(0), 0, 0, -x.coeff(1)).finished(); + return e; + } + + private: + scalar_t xumin_, xumax_; +}; + +} // namespace ocs2 + +TEST(test_circular_kinematics, solve_projected_EqConstraints) { + // optimal control problem + ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + + // Initializer + ocs2::DefaultInitializer zeroInitializer(2); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.01; + settings.ipmIteration = 20; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 1; + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + + // Solve + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.run(startTime, initState, finalTime); + + // Inspect solution + const auto primalSolution = solver.primalSolution(finalTime); + for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { + std::cout << "time: " << primalSolution.timeTrajectory_[i] << "\t state: " << primalSolution.stateTrajectory_[i].transpose() + << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; + } + + // Check initial condition + ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime); + + // Check constraint satisfaction. + const auto performance = solver.getPerformanceIndeces(); + ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); + ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); + + // Check feedback controller + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + // Feed forward part + ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); + } +} + +TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { + // optimal control problem + ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ipm_test_generated"); + + // Initializer + ocs2::DefaultInitializer zeroInitializer(2); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.01; + settings.ipmIteration = 20; + settings.projectStateInputEqualityConstraints = false; // <- false to turn off projection of state-input equalities + settings.useFeedbackPolicy = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + + // Solve + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.run(startTime, initState, finalTime); + + // Inspect solution + const auto primalSolution = solver.primalSolution(finalTime); + for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { + std::cout << "time: " << primalSolution.timeTrajectory_[i] << "\t state: " << primalSolution.stateTrajectory_[i].transpose() + << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; + } + + // Check initial condition + ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime); + + // Check constraint satisfaction. + const auto performance = solver.getPerformanceIndeces(); + ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); + ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); + + // Check feedback controller + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + // Feed forward part + ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); + } +} + +TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { + // optimal control problem + ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + + // inequality constraints + const ocs2::vector_t umin = (ocs2::vector_t(2) << -0.5, -0.5).finished(); + const ocs2::vector_t umax = (ocs2::vector_t(2) << 0.5, 0.5).finished(); + std::unique_ptr<ocs2::StateInputConstraint> stateInputIneqConstraint(new ocs2::CircleKinematics_StateInputIneqConstraints(umin, umax)); + problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint)); + const ocs2::vector_t xmin = (ocs2::vector_t(2) << -0.5, -0.5).finished(); + const ocs2::vector_t xmax = (ocs2::vector_t(2) << 1.0e03, 1.0e03).finished(); // no upper bound + std::unique_ptr<ocs2::StateConstraint> stateIneqConstraint(new ocs2::CircleKinematics_StateIneqConstraints(xmin, xmax)); + std::unique_ptr<ocs2::StateConstraint> finalStateIneqConstraint(new ocs2::CircleKinematics_StateIneqConstraints(xmin, xmax)); + problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint)); + problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint)); + + // Initializer + ocs2::DefaultInitializer zeroInitializer(2); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.01; + settings.ipmIteration = 40; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 1; + + settings.initialBarrierParameter = 1.0e-02; + settings.targetBarrierParameter = 1.0e-04; + settings.barrierLinearDecreaseFactor = 0.2; + settings.barrierSuperlinearDecreasePower = 1.5; + settings.fractionToBoundaryMargin = 0.995; + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + + // Solve + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.run(startTime, initState, finalTime); + + // Inspect solution + const auto primalSolution = solver.primalSolution(finalTime); + for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { + std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] + << "\t state: " << primalSolution.stateTrajectory_[i].transpose() + << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; + } + + // check constraint satisfaction + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + if (primalSolution.inputTrajectory_[i].size() > 0) { + const ocs2::scalar_t projectionConstraintViolation = primalSolution.stateTrajectory_[i].dot(primalSolution.inputTrajectory_[i]); + EXPECT_LT(std::abs(projectionConstraintViolation), 1.0e-06); + } + } + + // check constraint satisfaction + for (const auto& e : primalSolution.stateTrajectory_) { + if (e.size() > 0) { + EXPECT_TRUE(e.coeff(0) >= xmin.coeff(0)); + EXPECT_TRUE(e.coeff(1) >= xmin.coeff(1)); + EXPECT_TRUE(e.coeff(0) <= xmax.coeff(0)); + EXPECT_TRUE(e.coeff(1) <= xmax.coeff(1)); + } + } + for (const auto& e : primalSolution.inputTrajectory_) { + if (e.size() > 0) { + EXPECT_TRUE(e.coeff(0) >= umin.coeff(0)); + EXPECT_TRUE(e.coeff(1) >= umin.coeff(1)); + EXPECT_TRUE(e.coeff(0) <= umax.coeff(0)); + EXPECT_TRUE(e.coeff(1) <= umax.coeff(1)); + } + } + + // Check initial condition + ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime); + + // Check constraint satisfaction. + const auto performance = solver.getPerformanceIndeces(); + ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); + ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); + + // Check feedback controller + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + // Feed forward part + ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); + } +} + +TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraints) { + // optimal control problem + ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + + // inequality constraints + const ocs2::scalar_t xumin = -2.0; + const ocs2::scalar_t xumax = 2.0; + std::unique_ptr<ocs2::StateInputConstraint> stateInputIneqConstraint( + new ocs2::CircleKinematics_MixedStateInputIneqConstraints(xumin, xumax)); + auto stateInputIneqConstraintCloned = stateInputIneqConstraint->clone(); + problem.inequalityConstraintPtr->add("xubound", std::move(stateInputIneqConstraint)); + + // Initializer + ocs2::DefaultInitializer zeroInitializer(2); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.01; + settings.ipmIteration = 100; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 1; + + settings.initialBarrierParameter = 1.0e-02; + settings.targetBarrierParameter = 1.0e-04; + settings.barrierLinearDecreaseFactor = 0.2; + settings.barrierSuperlinearDecreasePower = 1.5; + settings.fractionToBoundaryMargin = 0.995; + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + + // Solve + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.run(startTime, initState, finalTime); + + // Inspect solution + const auto primalSolution = solver.primalSolution(finalTime); + for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { + std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] + << "\t state: " << primalSolution.stateTrajectory_[i].transpose() + << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; + } + + // check constraint satisfaction + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + if (primalSolution.inputTrajectory_[i].size() > 0) { + const ocs2::scalar_t projectionConstraintViolation = primalSolution.stateTrajectory_[i].dot(primalSolution.inputTrajectory_[i]); + EXPECT_LT(std::abs(projectionConstraintViolation), 1.0e-06); + } + } + + // check constraint satisfaction + const size_t N = primalSolution.inputTrajectory_.size(); + for (size_t i = 0; i < N; ++i) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, ocs2::PreComputation()); + EXPECT_TRUE(constraintValue.minCoeff() >= 0.0); + } + + // Check initial condition + ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime); + + // Check constraint satisfaction. + const auto performance = solver.getPerformanceIndeces(); + ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); + ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); + + // Check feedback controller + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + // Feed forward part + ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); + } +} \ No newline at end of file diff --git a/ocs2_ipm/test/testSwitchedProblem.cpp b/ocs2_ipm/test/testSwitchedProblem.cpp new file mode 100644 index 000000000..ebcb42f80 --- /dev/null +++ b/ocs2_ipm/test/testSwitchedProblem.cpp @@ -0,0 +1,269 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <gtest/gtest.h> + +#include "ocs2_ipm/IpmSolver.h" + +#include <ocs2_core/constraint/LinearStateInputConstraint.h> +#include <ocs2_core/constraint/StateInputConstraint.h> +#include <ocs2_core/initialization/DefaultInitializer.h> +#include <ocs2_core/misc/LinearInterpolation.h> +#include <ocs2_oc/oc_data/TimeDiscretization.h> +#include <ocs2_oc/synchronized_module/ReferenceManager.h> + +#include <ocs2_oc/test/testProblemsGeneration.h> + +namespace ocs2 { +namespace { +/** + * A constraint that in mode 0 sets u[0] = 0 and in mode 1 sets u[1] = 1 + */ +class SwitchedConstraint : public StateInputConstraint { + public: + explicit SwitchedConstraint(std::shared_ptr<ReferenceManager> referenceManagerPtr) + : StateInputConstraint(ConstraintOrder::Linear), referenceManagerPtr_(std::move(referenceManagerPtr)) { + constexpr int n = 3; + constexpr int m = 2; + constexpr int nc = 1; + + auto stateInputConstraints0 = VectorFunctionLinearApproximation::Zero(nc, n, m); + stateInputConstraints0.dfdu << 1.0, 0.0; + + auto stateInputConstraints1 = VectorFunctionLinearApproximation::Zero(nc, n, m); + stateInputConstraints1.dfdu << 0.0, 1.0; + + subsystemConstraintsPtr_.emplace_back(getOcs2Constraints(stateInputConstraints0)); + subsystemConstraintsPtr_.emplace_back(getOcs2Constraints(stateInputConstraints1)); + } + + ~SwitchedConstraint() override = default; + SwitchedConstraint* clone() const override { return new SwitchedConstraint(*this); } + + size_t getNumConstraints(scalar_t time) const override { + const auto activeMode = referenceManagerPtr_->getModeSchedule().modeAtTime(time); + return subsystemConstraintsPtr_[activeMode]->getNumConstraints(time); + } + + vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override { + const auto activeMode = referenceManagerPtr_->getModeSchedule().modeAtTime(time); + return subsystemConstraintsPtr_[activeMode]->getValue(time, state, input, preComp); + }; + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const { + const auto activeMode = referenceManagerPtr_->getModeSchedule().modeAtTime(time); + return subsystemConstraintsPtr_[activeMode]->getLinearApproximation(time, state, input, preComp); + } + + private: + SwitchedConstraint(const SwitchedConstraint& other) + : ocs2::StateInputConstraint(other), referenceManagerPtr_(other.referenceManagerPtr_) { + for (const auto& constraint : other.subsystemConstraintsPtr_) { + subsystemConstraintsPtr_.emplace_back(constraint->clone()); + } + } + + std::shared_ptr<ReferenceManager> referenceManagerPtr_; + std::vector<std::unique_ptr<ocs2::StateInputConstraint>> subsystemConstraintsPtr_; +}; + +std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithEventTime(scalar_t eventTime) { + constexpr int n = 3; + constexpr int m = 2; + + ocs2::OptimalControlProblem problem; + + // System + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto jumpMap = matrix_t::Random(n, n); + problem.dynamicsPtr.reset(new ocs2::LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); + + // Cost + problem.costPtr->add("intermediateCost", ocs2::getOcs2Cost(ocs2::getRandomCost(n, m))); + problem.softConstraintPtr->add("intermediateCost", ocs2::getOcs2Cost(ocs2::getRandomCost(n, m))); + problem.preJumpCostPtr->add("eventCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + problem.preJumpSoftConstraintPtr->add("eventCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + problem.finalCostPtr->add("finalCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + problem.finalSoftConstraintPtr->add("finalCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + + // Reference Manager + const ocs2::ModeSchedule modeSchedule({eventTime}, {0, 1}); + const ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Random(n)}, {ocs2::vector_t::Random(m)}); + auto referenceManagerPtr = std::make_shared<ocs2::ReferenceManager>(targetTrajectories, modeSchedule); + + problem.targetTrajectoriesPtr = &targetTrajectories; + + // Constraint + problem.equalityConstraintPtr->add("switchedConstraint", std::make_unique<SwitchedConstraint>(referenceManagerPtr)); + + ocs2::DefaultInitializer zeroInitializer(m); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.05; + settings.ipmIteration = 20; + settings.projectStateInputEqualityConstraints = true; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = ocs2::vector_t::Random(n); + + // Set up solver + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + + // Solve + solver.run(startTime, initState, finalTime); + return {solver.primalSolution(finalTime), solver.getIterationsLog()}; +} + +} // namespace +} // namespace ocs2 + +TEST(test_switched_problem, switched_constraint) { + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::scalar_t eventTime = 0.1875; + const double tol = 1e-9; + const auto solution = ocs2::solveWithEventTime(eventTime); + const auto& primalSolution = solution.first; + const auto& performanceLog = solution.second; + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(performanceLog.size(), 2); + ASSERT_LT(performanceLog.back().dynamicsViolationSSE, tol); + + // Should have correct node pre and post event time, with corresponding inputs + ASSERT_EQ(primalSolution.timeTrajectory_[4], eventTime); + ASSERT_EQ(primalSolution.timeTrajectory_[5], eventTime); + ASSERT_LT(std::abs(primalSolution.inputTrajectory_[4][0]), tol); + ASSERT_LT(std::abs(primalSolution.inputTrajectory_[5][1]), tol); + + // Inspect solution, check at a dt smaller than solution to check interpolation around the switch. + ocs2::scalar_t t_check = startTime; + ocs2::scalar_t dt_check = 1e-5; + while (t_check < finalTime) { + ocs2::vector_t xNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_); + ocs2::vector_t uNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_); + ocs2::vector_t uControl = primalSolution.controllerPtr_->computeInput(t_check, xNominal); + if (t_check < eventTime) { + ASSERT_LT(std::abs(uNominal[0]), tol); + ASSERT_LT(std::abs(uControl[0]), tol); + } else if (t_check > eventTime) { + ASSERT_LT(std::abs(uNominal[1]), tol); + ASSERT_LT(std::abs(uControl[1]), tol); + } + t_check += dt_check; + } +} + +TEST(test_switched_problem, event_at_beginning) { + // The event should replace the start time, all inputs should be after the event. + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::scalar_t eventTime = 1e-8; + const double tol = 1e-9; + const auto solution = ocs2::solveWithEventTime(eventTime); + const auto& primalSolution = solution.first; + const auto& performanceLog = solution.second; + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(performanceLog.size(), 2); + ASSERT_LT(performanceLog.back().dynamicsViolationSSE, tol); + + // Should have correct post event time start + ASSERT_EQ(primalSolution.timeTrajectory_[0], eventTime); + ASSERT_NE(primalSolution.timeTrajectory_[1], eventTime); + + // Inspect solution, check at a dt smaller than solution to check interpolation around the switch. + ocs2::scalar_t t_check = eventTime; + ocs2::scalar_t dt_check = 1e-5; + while (t_check < finalTime) { + ocs2::vector_t xNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_); + ocs2::vector_t uNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_); + ocs2::vector_t uControl = primalSolution.controllerPtr_->computeInput(t_check, xNominal); + ASSERT_LT(std::abs(uNominal[1]), tol); + ASSERT_LT(std::abs(uControl[1]), tol); + t_check += dt_check; + } +} + +TEST(test_switched_problem, event_at_end) { + // The event should be ignored because its too close to the final time, all inputs should be before the event. + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::scalar_t eventTime = 1.0 - 1e-8; + const double tol = 1e-9; + const auto solution = ocs2::solveWithEventTime(eventTime); + const auto& primalSolution = solution.first; + const auto& performanceLog = solution.second; + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(performanceLog.size(), 2); + ASSERT_LT(performanceLog.back().dynamicsViolationSSE, tol); + + // Should have correct node pre and post event time + ASSERT_TRUE(std::none_of(primalSolution.timeTrajectory_.begin(), primalSolution.timeTrajectory_.end(), + [=](ocs2::scalar_t t) { return t == eventTime; })); + + // Inspect solution, check at a dt smaller than solution to check interpolation around the switch. + ocs2::scalar_t t_check = startTime; + ocs2::scalar_t dt_check = 1e-5; + while (t_check < finalTime) { + ocs2::vector_t xNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_); + ocs2::vector_t uNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_); + ocs2::vector_t uControl = primalSolution.controllerPtr_->computeInput(t_check, xNominal); + ASSERT_LT(std::abs(uNominal[0]), tol); + ASSERT_LT(std::abs(uControl[0]), tol); + t_check += dt_check; + } +} diff --git a/ocs2_ipm/test/testUnconstrained.cpp b/ocs2_ipm/test/testUnconstrained.cpp new file mode 100644 index 000000000..8485a31ce --- /dev/null +++ b/ocs2_ipm/test/testUnconstrained.cpp @@ -0,0 +1,162 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <gtest/gtest.h> + +#include "ocs2_ipm/IpmSolver.h" + +#include <ocs2_core/initialization/DefaultInitializer.h> + +#include <ocs2_oc/synchronized_module/ReferenceManager.h> +#include <ocs2_oc/test/testProblemsGeneration.h> + +namespace ocs2 { +namespace { + +std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithFeedbackSetting( + bool feedback, bool emptyConstraint, const VectorFunctionLinearApproximation& dynamicsMatrices, + const ScalarFunctionQuadraticApproximation& costMatrices) { + int n = dynamicsMatrices.dfdu.rows(); + int m = dynamicsMatrices.dfdu.cols(); + + ocs2::OptimalControlProblem problem; + + // System + problem.dynamicsPtr = getOcs2Dynamics(dynamicsMatrices); + + // Cost + problem.costPtr->add("intermediateCost", ocs2::getOcs2Cost(costMatrices)); + problem.finalCostPtr->add("finalCost", ocs2::getOcs2StateCost(costMatrices)); + + // Reference Managaer + ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Ones(n)}, {ocs2::vector_t::Ones(m)}); + auto referenceManagerPtr = std::make_shared<ReferenceManager>(targetTrajectories); + + problem.targetTrajectoriesPtr = &referenceManagerPtr->getTargetTrajectories(); + + if (emptyConstraint) { + problem.equalityConstraintPtr->add("intermediateCost", ocs2::getOcs2Constraints(getRandomConstraints(n, m, 0))); + } + + ocs2::DefaultInitializer zeroInitializer(m); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.05; + settings.ipmIteration = 10; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = feedback; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 100; + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = ocs2::vector_t::Ones(n); + + // Construct solver + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + + // Solve + solver.run(startTime, initState, finalTime); + return {solver.primalSolution(finalTime), solver.getIterationsLog()}; +} + +} // namespace +} // namespace ocs2 + +TEST(test_unconstrained, withFeedback) { + int n = 3; + int m = 2; + const double tol = 1e-9; + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto costs = ocs2::getRandomCost(n, m); + const auto solWithEmptyConstraint = ocs2::solveWithFeedbackSetting(true, true, dynamics, costs); + const auto solWithNullConstraint = ocs2::solveWithFeedbackSetting(true, false, dynamics, costs); + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(solWithEmptyConstraint.second.size(), 2); + ASSERT_LE(solWithNullConstraint.second.size(), 2); + ASSERT_LT(solWithEmptyConstraint.second.back().dynamicsViolationSSE, tol); + ASSERT_LT(solWithNullConstraint.second.back().dynamicsViolationSSE, tol); + + // Compare + const auto& withEmptyConstraint = solWithEmptyConstraint.first; + const auto& withNullConstraint = solWithNullConstraint.first; + for (int i = 0; i < withEmptyConstraint.timeTrajectory_.size(); i++) { + ASSERT_DOUBLE_EQ(withEmptyConstraint.timeTrajectory_[i], withNullConstraint.timeTrajectory_[i]); + ASSERT_TRUE(withEmptyConstraint.stateTrajectory_[i].isApprox(withNullConstraint.stateTrajectory_[i], tol)); + ASSERT_TRUE(withEmptyConstraint.inputTrajectory_[i].isApprox(withNullConstraint.inputTrajectory_[i], tol)); + const auto t = withEmptyConstraint.timeTrajectory_[i]; + const auto& x = withEmptyConstraint.stateTrajectory_[i]; + ASSERT_TRUE( + withEmptyConstraint.controllerPtr_->computeInput(t, x).isApprox(withNullConstraint.controllerPtr_->computeInput(t, x), tol)); + } +} + +TEST(test_unconstrained, noFeedback) { + int n = 3; + int m = 2; + const double tol = 1e-9; + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto costs = ocs2::getRandomCost(n, m); + const auto solWithEmptyConstraint = ocs2::solveWithFeedbackSetting(false, true, dynamics, costs); + const auto solWithNullConstraint = ocs2::solveWithFeedbackSetting(false, false, dynamics, costs); + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(solWithEmptyConstraint.second.size(), 2); + ASSERT_LE(solWithNullConstraint.second.size(), 2); + ASSERT_LT(solWithEmptyConstraint.second.back().dynamicsViolationSSE, tol); + ASSERT_LT(solWithNullConstraint.second.back().dynamicsViolationSSE, tol); + + // Compare + const auto& withEmptyConstraint = solWithEmptyConstraint.first; + const auto& withNullConstraint = solWithNullConstraint.first; + for (int i = 0; i < withEmptyConstraint.timeTrajectory_.size(); i++) { + ASSERT_DOUBLE_EQ(withEmptyConstraint.timeTrajectory_[i], withNullConstraint.timeTrajectory_[i]); + ASSERT_TRUE(withEmptyConstraint.stateTrajectory_[i].isApprox(withNullConstraint.stateTrajectory_[i], tol)); + ASSERT_TRUE(withEmptyConstraint.inputTrajectory_[i].isApprox(withNullConstraint.inputTrajectory_[i], tol)); + + const auto t = withEmptyConstraint.timeTrajectory_[i]; + const auto& x = withEmptyConstraint.stateTrajectory_[i]; + ASSERT_TRUE( + withEmptyConstraint.controllerPtr_->computeInput(t, x).isApprox(withNullConstraint.controllerPtr_->computeInput(t, x), tol)); + } +} diff --git a/ocs2_ipm/test/testValuefunction.cpp b/ocs2_ipm/test/testValuefunction.cpp new file mode 100644 index 000000000..15812c129 --- /dev/null +++ b/ocs2_ipm/test/testValuefunction.cpp @@ -0,0 +1,106 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include <gtest/gtest.h> + +#include "ocs2_ipm/IpmSolver.h" + +#include <ocs2_core/initialization/DefaultInitializer.h> + +#include <ocs2_oc/oc_data/TimeDiscretization.h> +#include <ocs2_oc/synchronized_module/ReferenceManager.h> +#include <ocs2_oc/test/testProblemsGeneration.h> + +TEST(test_valuefunction, linear_quadratic_problem) { + constexpr int n = 3; + constexpr int m = 2; + constexpr int nc = 1; + constexpr int Nsample = 10; + constexpr ocs2::scalar_t tol = 1e-9; + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t eventTime = 1.0 / 3.0; + const ocs2::scalar_t finalTime = 1.0; + + ocs2::OptimalControlProblem problem; + + // System + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto jumpMap = ocs2::matrix_t::Random(n, n); + problem.dynamicsPtr.reset(new ocs2::LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); + + // Cost + problem.costPtr->add("intermediateCost", ocs2::getOcs2Cost(ocs2::getRandomCost(n, m))); + problem.preJumpCostPtr->add("eventCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + problem.finalCostPtr->add("finalCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + + // Reference Manager + const ocs2::ModeSchedule modeSchedule({eventTime}, {0, 1}); + const ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Random(n)}, {ocs2::vector_t::Random(m)}); + auto referenceManagerPtr = std::make_shared<ocs2::ReferenceManager>(targetTrajectories, modeSchedule); + + problem.targetTrajectoriesPtr = &targetTrajectories; + + // Constraint + problem.equalityConstraintPtr->add("constraint", ocs2::getOcs2Constraints(ocs2::getRandomConstraints(n, m, nc))); + + ocs2::DefaultInitializer zeroInitializer(m); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.05; + settings.ipmIteration = 1; + settings.projectStateInputEqualityConstraints = true; + settings.printSolverStatistics = false; + settings.printSolverStatus = false; + settings.printLinesearch = false; + settings.useFeedbackPolicy = true; + settings.createValueFunction = true; + + // Set up solver + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + + // Get value function + const ocs2::vector_t zeroState = ocs2::vector_t::Random(n); + solver.reset(); + solver.run(startTime, zeroState, finalTime); + const auto costToGo = solver.getValueFunction(startTime, zeroState); + const ocs2::scalar_t zeroCost = solver.getPerformanceIndeces().cost; + + // Solve for random states and check consistency with value function + for (int i = 0; i < Nsample; ++i) { + const ocs2::vector_t sampleState = ocs2::vector_t::Random(n); + solver.reset(); + solver.run(startTime, sampleState, finalTime); + const ocs2::scalar_t sampleCost = solver.getPerformanceIndeces().cost; + const ocs2::vector_t dx = sampleState - zeroState; + + EXPECT_NEAR(sampleCost, zeroCost + costToGo.dfdx.dot(dx) + 0.5 * dx.dot(costToGo.dfdxx * dx), tol); + } +} From 3526f950e9b80478073c803101888ada7363389b Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 14 Dec 2022 19:41:57 +0100 Subject: [PATCH 457/512] add IPM legged robot example --- .../ocs2_legged_robot/CMakeLists.txt | 1 + .../ocs2_legged_robot/config/mpc/task.info | 36 +++++++ .../ocs2_legged_robot/LeggedRobotInterface.h | 13 ++- .../ocs2_legged_robot/package.xml | 1 + .../src/LeggedRobotInterface.cpp | 27 +++++- .../ocs2_legged_robot_ros/CMakeLists.txt | 18 ++++ .../launch/legged_robot_ipm.launch | 48 ++++++++++ .../ocs2_legged_robot_ros/package.xml | 1 + .../src/LeggedRobotIpmMpcNode.cpp | 96 +++++++++++++++++++ 9 files changed, 233 insertions(+), 8 deletions(-) create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch create mode 100644 ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index 8a19765f3..c6d07ed02 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -10,6 +10,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index a6047bd97..40317d367 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -46,6 +46,42 @@ sqp threadPriority 50 } +; Multiple_Shooting IPM settings +ipm +{ + nThreads 3 + dt 0.015 + sqpIteration 1 + deltaTol 1e-4 + g_max 10.0 + g_min 1e-4 + inequalityConstraintMu 0.1 + inequalityConstraintDelta 5.0 + projectStateInputEqualityConstraints true + computeLagrangeMultipliers false + printSolverStatistics true + printSolverStatus false + printLinesearch false + useFeedbackPolicy true + integratorType RK2 + threadPriority 50 + + initialBarrierParameter 1e-4 + targetBarrierParameter 1e-4 + barrierLinearDecreaseFactor 0.2 + barrierSuperlinearDecreasePower 1.5 + barrierReductionCostTol 1e-3 + barrierReductionConstraintTol 1e-3 + + fractionToBoundaryMargin 0.995 + usePrimalStepSizeForDual false + + initialSlackLowerBound 1e-4 + initialDualLowerBound 1e-4 + initialSlackMarginRate 1e-2 + initialDualMarginRate 1e-2 +} + ; DDP settings ddp { diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h index 410d9e2d9..cc9ed6f18 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/Types.h> #include <ocs2_core/penalties/Penalties.h> #include <ocs2_ddp/DDP_Settings.h> +#include <ocs2_ipm/IpmSettings.h> #include <ocs2_mpc/MPC_Settings.h> #include <ocs2_oc/rollout/TimeTriggeredRollout.h> #include <ocs2_pinocchio_interface/PinocchioInterface.h> @@ -62,8 +63,10 @@ class LeggedRobotInterface final : public RobotInterface { * @param [in] taskFile: The absolute path to the configuration file for the MPC. * @param [in] urdfFile: The absolute path to the URDF file for the robot. * @param [in] referenceFile: The absolute path to the reference configuration file. + * @param [in] useHardFrictionConeConstraint: Which to use hard or soft friction cone constraints. */ - LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile); + LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, + bool useHardFrictionConeConstraint = false); ~LeggedRobotInterface() override = default; @@ -74,6 +77,7 @@ class LeggedRobotInterface final : public RobotInterface { const mpc::Settings& mpcSettings() const { return mpcSettings_; } const rollout::Settings& rolloutSettings() const { return rolloutSettings_; } const sqp::Settings& sqpSettings() { return sqpSettings_; } + const ipm::Settings& ipmSettings() { return ipmSettings_; } const vector_t& getInitialState() const { return initialState_; } const RolloutBase& getRollout() const { return *rolloutPtr_; } @@ -93,8 +97,9 @@ class LeggedRobotInterface final : public RobotInterface { matrix_t initializeInputCostWeight(const std::string& taskFile, const CentroidalModelInfo& info); std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(const std::string& taskFile, bool verbose) const; - std::unique_ptr<StateInputCost> getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); + std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient); + std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, + const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); std::unique_ptr<StateInputConstraint> getZeroForceConstraint(size_t contactPointIndex); std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(const EndEffectorKinematics<scalar_t>& eeKinematics, size_t contactPointIndex, bool useAnalyticalGradients); @@ -105,6 +110,8 @@ class LeggedRobotInterface final : public RobotInterface { ddp::Settings ddpSettings_; mpc::Settings mpcSettings_; sqp::Settings sqpSettings_; + ipm::Settings ipmSettings_; + bool useHardFrictionConeConstraint_; std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_; CentroidalModelInfo centroidalModelInfo_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot/package.xml b/ocs2_robotic_examples/ocs2_legged_robot/package.xml index bb62d1e9d..a9f76f9dd 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot/package.xml @@ -17,6 +17,7 @@ <depend>ocs2_ddp</depend> <depend>ocs2_mpc</depend> <depend>ocs2_sqp</depend> + <depend>ocs2_ipm</depend> <depend>ocs2_robotic_assets</depend> <depend>ocs2_robotic_tools</depend> <depend>ocs2_pinocchio_interface</depend> diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 2e0a6a389..14c3fcc43 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -64,7 +64,8 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile) { +LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, + bool useHardFrictionConeConstraint) { // check that task file exists boost::filesystem::path taskFilePath(taskFile); if (boost::filesystem::exists(taskFilePath)) { @@ -96,6 +97,8 @@ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const st mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose); rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose); + ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose); + useHardFrictionConeConstraint_ = useHardFrictionConeConstraint; // OptimalConrolProblem setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose); @@ -174,8 +177,12 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd)); } - problemPtr_->softConstraintPtr->add(footName + "_frictionCone", - getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig)); + if (useHardFrictionConeConstraint_) { + problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", getFrictionConeConstraint(i, frictionCoefficient)); + } else { + problemPtr_->softConstraintPtr->add(footName + "_frictionCone", + getFrictionConeSoftConstraint(i, frictionCoefficient, barrierPenaltyConfig)); + } problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i)); problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints)); @@ -310,8 +317,18 @@ std::pair<scalar_t, RelaxedBarrierPenalty::Config> LeggedRobotInterface::loadFri /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { +std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex, + scalar_t frictionCoefficient) { + FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); + return std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, + centroidalModelInfo_); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeSoftConstraint( + size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); auto frictionConeConstraintPtr = std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt index d5c30446e..a8819c8f8 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt @@ -16,6 +16,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model @@ -118,6 +119,21 @@ target_link_libraries(legged_robot_sqp_mpc ) target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS}) +## IPM-MPC node for legged robot +add_executable(legged_robot_ipm_mpc + src/LeggedRobotIpmMpcNode.cpp +) +add_dependencies(legged_robot_ipm_mpc + ${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(legged_robot_ipm_mpc + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(legged_robot_ipm_mpc PRIVATE ${OCS2_CXX_FLAGS}) + # Dummy node add_executable(legged_robot_dummy src/LeggedRobotDummyNode.cpp @@ -174,6 +190,7 @@ if(cmake_clang_tools_FOUND) ${PROJECT_NAME} legged_robot_ddp_mpc legged_robot_sqp_mpc + legged_robot_ipm_mpc legged_robot_dummy legged_robot_target legged_robot_gait_command @@ -198,6 +215,7 @@ install( TARGETS legged_robot_ddp_mpc legged_robot_sqp_mpc + legged_robot_ipm_mpc legged_robot_dummy legged_robot_target legged_robot_gait_command diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch new file mode 100644 index 000000000..9744aee4f --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch @@ -0,0 +1,48 @@ +<?xml version="1.0" ?> + +<launch> + <!-- visualization config --> + <arg name="rviz" default="true" /> + <arg name="description_name" default="legged_robot_description"/> + <arg name="multiplot" default="false"/> + + <!-- The task file for the mpc. --> + <arg name="taskFile" default="$(find ocs2_legged_robot)/config/mpc/task.info"/> + <!-- The reference related config file of the robot --> + <arg name="referenceFile" default="$(find ocs2_legged_robot)/config/command/reference.info"/> + <!-- The URDF model of the robot --> + <arg name="urdfFile" default="$(find ocs2_robotic_assets)/resources/anymal_c/urdf/anymal.urdf"/> + <!-- The file defining gait definition --> + <arg name="gaitCommandFile" default="$(find ocs2_legged_robot)/config/command/gait.info"/> + + <!-- rviz --> + <group if="$(arg rviz)"> + <param name="$(arg description_name)" textfile="$(arg urdfFile)"/> + <arg name="rvizconfig" default="$(find ocs2_legged_robot_ros)/rviz/legged_robot.rviz" /> + <node pkg="rviz" type="rviz" name="rviz" args="-d $(arg rvizconfig)" output="screen" /> + </group> + + <!-- multiplot --> + <group if="$(arg multiplot)"> + <include file="$(find ocs2_legged_robot_ros)/launch/multiplot.launch"/> + </group> + + <!-- make the files into global parameters --> + <param name="multiplot" value="$(arg multiplot)"/> + <param name="taskFile" value="$(arg taskFile)" /> + <param name="referenceFile" value="$(arg referenceFile)" /> + <param name="urdfFile" value="$(arg urdfFile)" /> + <param name="gaitCommandFile" value="$(arg gaitCommandFile)"/> + + <node pkg="ocs2_legged_robot_ros" type="legged_robot_ipm_mpc" name="legged_robot_ipm_mpc" + output="screen" launch-prefix=""/> + + <node pkg="ocs2_legged_robot_ros" type="legged_robot_dummy" name="legged_robot_dummy" + output="screen" launch-prefix="gnome-terminal --"/> + + <node pkg="ocs2_legged_robot_ros" type="legged_robot_target" name="legged_robot_target" + output="screen" launch-prefix="gnome-terminal --"/> + + <node pkg="ocs2_legged_robot_ros" type="legged_robot_gait_command" name="legged_robot_gait_command" + output="screen" launch-prefix="gnome-terminal --"/> +</launch> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml index aa2943272..7bc71c34e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml @@ -24,6 +24,7 @@ <depend>ocs2_ddp</depend> <depend>ocs2_mpc</depend> <depend>ocs2_sqp</depend> + <depend>ocs2_ipm</depend> <depend>ocs2_robotic_tools</depend> <depend>ocs2_pinocchio_interface</depend> <depend>ocs2_centroidal_model</depend> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp new file mode 100644 index 000000000..30a061779 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp @@ -0,0 +1,96 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ******************************************************************************/ + +#include <ros/init.h> +#include <ros/package.h> + +#include <ocs2_ipm/IpmMpc.h> +#include <ocs2_legged_robot/LeggedRobotInterface.h> +#include <ocs2_ros_interfaces/mpc/MPC_ROS_Interface.h> +#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h> +#include <ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h> + +#include "ocs2_legged_robot_ros/gait/GaitReceiver.h" + +using namespace ocs2; +using namespace legged_robot; + +int main(int argc, char** argv) { + const std::string robotName = "legged_robot"; + + // Initialize ros node + ::ros::init(argc, argv, robotName + "_mpc"); + ::ros::NodeHandle nodeHandle; + // Get node parameters + bool multiplot = false; + std::string taskFile, urdfFile, referenceFile; + nodeHandle.getParam("/multiplot", multiplot); + nodeHandle.getParam("/taskFile", taskFile); + nodeHandle.getParam("/urdfFile", urdfFile); + nodeHandle.getParam("/referenceFile", referenceFile); + + // Robot interface + constexpr bool useHardFrictionConeConstraint = true; + LeggedRobotInterface interface(taskFile, urdfFile, referenceFile, useHardFrictionConeConstraint); + + // Gait receiver + auto gaitReceiverPtr = + std::make_shared<GaitReceiver>(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + + // ROS ReferenceManager + auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // MPC + IpmMpc mpc(interface.mpcSettings(), interface.ipmSettings(), interface.getOptimalControlProblem(), interface.getInitializer()); + mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + + // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + if (multiplot) { + auto createStateInputBoundsObserver = [&](const std::string& termName) { + const ocs2::scalar_array_t observingTimePoints{0.0}; + const std::vector<std::string> topicNames{"metrics/" + termName + "/0MsLookAhead"}; + auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + }; + for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_frictionCone")); + } + } + + // Launch MPC ROS node + MPC_ROS_Interface mpcNode(mpc, robotName); + mpcNode.launchNodes(nodeHandle); + + // Successful exit + return 0; +} From 1a5b44e35dc3e95d9ac13b2e15fb0956635ac1c3 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 14 Dec 2022 20:41:52 +0100 Subject: [PATCH 458/512] add friction cone metric plotting --- .../config/multiplot/friction_cone.xml | 720 ++++++++++++++++++ .../launch/legged_robot_ipm.launch | 4 +- 2 files changed, 723 insertions(+), 1 deletion(-) create mode 100644 ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml new file mode 100644 index 000000000..e658acd29 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml @@ -0,0 +1,720 @@ +<?xml version="1.0" encoding="UTF-8"?> +<rqt_multiplot> + <table> + <background_color>#ffffff</background_color> + <foreground_color>#000000</foreground_color> + <link_cursor>false</link_cursor> + <link_scale>false</link_scale> + <plots> + <row_0> + <column_0> + <axes> + <axes> + <x_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </x_axis> + <y_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </y_axis> + </axes> + </axes> + <curves> + <curve_0> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/0</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>X</title> + </curve_0> + <curve_1> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/1</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Y</title> + </curve_1> + <curve_2> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/2</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Z</title> + </curve_2> + </curves> + <legend> + <visible>true</visible> + </legend> + <plot_rate>30</plot_rate> + <title>LF_FOOT_frictionCone</title> + </column_0> + <column_1> + <axes> + <axes> + <x_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </x_axis> + <y_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </y_axis> + </axes> + </axes> + <curves> + <curve_0> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/0</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>X</title> + </curve_0> + <curve_1> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/1</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Y</title> + </curve_1> + <curve_2> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/2</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Z</title> + </curve_2> + </curves> + <legend> + <visible>true</visible> + </legend> + <plot_rate>30</plot_rate> + <title>RF_FOOT_frictionCone</title> + </column_1> + </row_0> + <row_1> + <column_0> + <axes> + <axes> + <x_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </x_axis> + <y_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </y_axis> + </axes> + </axes> + <curves> + <curve_0> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/0</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>X</title> + </curve_0> + <curve_1> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/1</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Y</title> + </curve_1> + <curve_2> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/2</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Z</title> + </curve_2> + </curves> + <legend> + <visible>true</visible> + </legend> + <plot_rate>30</plot_rate> + <title>LH_FOOT_frictionCone</title> + </column_0> + <column_1> + <axes> + <axes> + <x_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </x_axis> + <y_axis> + <custom_title>Untitled Axis</custom_title> + <title_type>0</title_type> + <title_visible>true</title_visible> + </y_axis> + </axes> + </axes> + <curves> + <curve_0> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/0</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>X</title> + </curve_0> + <curve_1> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/1</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Y</title> + </curve_1> + <curve_2> + <axes> + <x_axis> + <field>time</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </x_axis> + <y_axis> + <field>value/2</field> + <field_type>0</field_type> + <scale> + <absolute_maximum>1000</absolute_maximum> + <absolute_minimum>0</absolute_minimum> + <relative_maximum>0</relative_maximum> + <relative_minimum>-1000</relative_minimum> + <type>0</type> + </scale> + <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> + <type>ocs2_msgs/constraint</type> + </y_axis> + </axes> + <color> + <custom_color>#000000</custom_color> + <type>0</type> + </color> + <data> + <circular_buffer_capacity>1000</circular_buffer_capacity> + <time_frame_length>10</time_frame_length> + <type>0</type> + </data> + <style> + <lines_interpolate>false</lines_interpolate> + <pen_style>1</pen_style> + <pen_width>1</pen_width> + <render_antialias>false</render_antialias> + <steps_invert>false</steps_invert> + <sticks_baseline>0</sticks_baseline> + <sticks_orientation>2</sticks_orientation> + <type>0</type> + </style> + <subscriber_queue_size>100</subscriber_queue_size> + <title>Z</title> + </curve_2> + </curves> + <legend> + <visible>true</visible> + </legend> + <plot_rate>30</plot_rate> + <title>RH_FOOT_frictionCone</title> + </column_1> + </row_1> + </plots> + <track_points>false</track_points> + </table> +</rqt_multiplot> diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch index 9744aee4f..085a29175 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch @@ -24,7 +24,9 @@ <!-- multiplot --> <group if="$(arg multiplot)"> - <include file="$(find ocs2_legged_robot_ros)/launch/multiplot.launch"/> + <include file="$(find ocs2_legged_robot_ros)/launch/multiplot.launch"> + <arg name="metrics_config" value="$(find ocs2_legged_robot)/config/multiplot/friction_cone.xml" /> + </include> </group> <!-- make the files into global parameters --> From e7fbd444c67ffe5f53b013443301db9ce0886f7d Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Wed, 14 Dec 2022 20:46:45 +0100 Subject: [PATCH 459/512] add friction cone metric plotting --- .../ocs2_cartpole_ros/launch/multiplot.launch | 2 +- .../config/multiplot/friction_cone.xml | 416 +----------------- .../{mpc_metrics.xml => zero_velocity.xml} | 0 .../launch/multiplot.launch | 2 +- 4 files changed, 6 insertions(+), 414 deletions(-) rename ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/{mpc_metrics.xml => zero_velocity.xml} (100%) diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch index 7194cb22b..9a1f5061a 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch @@ -1,6 +1,6 @@ <launch> <arg name="observation_config" default="$(find ocs2_cartpole)/config/multiplot/mpc_observation.xml" /> - <arg name="metrics_config" default="$(find ocs2_cartpole)/config/multiplot/mpc_metrics.xml" /> + <arg name="metrics_config" default="$(find ocs2_cartpole)/config/multiplot/zero_velocity.xml" /> <!-- Launch RQT Multi-plot --> <node name="mpc_observation" pkg="rqt_multiplot" type="rqt_multiplot" diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml index e658acd29..163176caf 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml @@ -72,110 +72,8 @@ <type>0</type> </style> <subscriber_queue_size>100</subscriber_queue_size> - <title>X</title> + <title>Constraint value</title> </curve_0> - <curve_1> - <axes> - <x_axis> - <field>time</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </x_axis> - <y_axis> - <field>value/1</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </y_axis> - </axes> - <color> - <custom_color>#000000</custom_color> - <type>0</type> - </color> - <data> - <circular_buffer_capacity>1000</circular_buffer_capacity> - <time_frame_length>10</time_frame_length> - <type>0</type> - </data> - <style> - <lines_interpolate>false</lines_interpolate> - <pen_style>1</pen_style> - <pen_width>1</pen_width> - <render_antialias>false</render_antialias> - <steps_invert>false</steps_invert> - <sticks_baseline>0</sticks_baseline> - <sticks_orientation>2</sticks_orientation> - <type>0</type> - </style> - <subscriber_queue_size>100</subscriber_queue_size> - <title>Y</title> - </curve_1> - <curve_2> - <axes> - <x_axis> - <field>time</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </x_axis> - <y_axis> - <field>value/2</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/LF_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </y_axis> - </axes> - <color> - <custom_color>#000000</custom_color> - <type>0</type> - </color> - <data> - <circular_buffer_capacity>1000</circular_buffer_capacity> - <time_frame_length>10</time_frame_length> - <type>0</type> - </data> - <style> - <lines_interpolate>false</lines_interpolate> - <pen_style>1</pen_style> - <pen_width>1</pen_width> - <render_antialias>false</render_antialias> - <steps_invert>false</steps_invert> - <sticks_baseline>0</sticks_baseline> - <sticks_orientation>2</sticks_orientation> - <type>0</type> - </style> - <subscriber_queue_size>100</subscriber_queue_size> - <title>Z</title> - </curve_2> </curves> <legend> <visible>true</visible> @@ -248,110 +146,8 @@ <type>0</type> </style> <subscriber_queue_size>100</subscriber_queue_size> - <title>X</title> + <title>Constraint value</title> </curve_0> - <curve_1> - <axes> - <x_axis> - <field>time</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </x_axis> - <y_axis> - <field>value/1</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </y_axis> - </axes> - <color> - <custom_color>#000000</custom_color> - <type>0</type> - </color> - <data> - <circular_buffer_capacity>1000</circular_buffer_capacity> - <time_frame_length>10</time_frame_length> - <type>0</type> - </data> - <style> - <lines_interpolate>false</lines_interpolate> - <pen_style>1</pen_style> - <pen_width>1</pen_width> - <render_antialias>false</render_antialias> - <steps_invert>false</steps_invert> - <sticks_baseline>0</sticks_baseline> - <sticks_orientation>2</sticks_orientation> - <type>0</type> - </style> - <subscriber_queue_size>100</subscriber_queue_size> - <title>Y</title> - </curve_1> - <curve_2> - <axes> - <x_axis> - <field>time</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </x_axis> - <y_axis> - <field>value/2</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/RF_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </y_axis> - </axes> - <color> - <custom_color>#000000</custom_color> - <type>0</type> - </color> - <data> - <circular_buffer_capacity>1000</circular_buffer_capacity> - <time_frame_length>10</time_frame_length> - <type>0</type> - </data> - <style> - <lines_interpolate>false</lines_interpolate> - <pen_style>1</pen_style> - <pen_width>1</pen_width> - <render_antialias>false</render_antialias> - <steps_invert>false</steps_invert> - <sticks_baseline>0</sticks_baseline> - <sticks_orientation>2</sticks_orientation> - <type>0</type> - </style> - <subscriber_queue_size>100</subscriber_queue_size> - <title>Z</title> - </curve_2> </curves> <legend> <visible>true</visible> @@ -426,110 +222,8 @@ <type>0</type> </style> <subscriber_queue_size>100</subscriber_queue_size> - <title>X</title> + <title>Constraint value</title> </curve_0> - <curve_1> - <axes> - <x_axis> - <field>time</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </x_axis> - <y_axis> - <field>value/1</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </y_axis> - </axes> - <color> - <custom_color>#000000</custom_color> - <type>0</type> - </color> - <data> - <circular_buffer_capacity>1000</circular_buffer_capacity> - <time_frame_length>10</time_frame_length> - <type>0</type> - </data> - <style> - <lines_interpolate>false</lines_interpolate> - <pen_style>1</pen_style> - <pen_width>1</pen_width> - <render_antialias>false</render_antialias> - <steps_invert>false</steps_invert> - <sticks_baseline>0</sticks_baseline> - <sticks_orientation>2</sticks_orientation> - <type>0</type> - </style> - <subscriber_queue_size>100</subscriber_queue_size> - <title>Y</title> - </curve_1> - <curve_2> - <axes> - <x_axis> - <field>time</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </x_axis> - <y_axis> - <field>value/2</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/LH_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </y_axis> - </axes> - <color> - <custom_color>#000000</custom_color> - <type>0</type> - </color> - <data> - <circular_buffer_capacity>1000</circular_buffer_capacity> - <time_frame_length>10</time_frame_length> - <type>0</type> - </data> - <style> - <lines_interpolate>false</lines_interpolate> - <pen_style>1</pen_style> - <pen_width>1</pen_width> - <render_antialias>false</render_antialias> - <steps_invert>false</steps_invert> - <sticks_baseline>0</sticks_baseline> - <sticks_orientation>2</sticks_orientation> - <type>0</type> - </style> - <subscriber_queue_size>100</subscriber_queue_size> - <title>Z</title> - </curve_2> </curves> <legend> <visible>true</visible> @@ -602,110 +296,8 @@ <type>0</type> </style> <subscriber_queue_size>100</subscriber_queue_size> - <title>X</title> + <title>Constraint value</title> </curve_0> - <curve_1> - <axes> - <x_axis> - <field>time</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </x_axis> - <y_axis> - <field>value/1</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </y_axis> - </axes> - <color> - <custom_color>#000000</custom_color> - <type>0</type> - </color> - <data> - <circular_buffer_capacity>1000</circular_buffer_capacity> - <time_frame_length>10</time_frame_length> - <type>0</type> - </data> - <style> - <lines_interpolate>false</lines_interpolate> - <pen_style>1</pen_style> - <pen_width>1</pen_width> - <render_antialias>false</render_antialias> - <steps_invert>false</steps_invert> - <sticks_baseline>0</sticks_baseline> - <sticks_orientation>2</sticks_orientation> - <type>0</type> - </style> - <subscriber_queue_size>100</subscriber_queue_size> - <title>Y</title> - </curve_1> - <curve_2> - <axes> - <x_axis> - <field>time</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </x_axis> - <y_axis> - <field>value/2</field> - <field_type>0</field_type> - <scale> - <absolute_maximum>1000</absolute_maximum> - <absolute_minimum>0</absolute_minimum> - <relative_maximum>0</relative_maximum> - <relative_minimum>-1000</relative_minimum> - <type>0</type> - </scale> - <topic>/metrics/RH_FOOT_frictionCone/0MsLookAhead</topic> - <type>ocs2_msgs/constraint</type> - </y_axis> - </axes> - <color> - <custom_color>#000000</custom_color> - <type>0</type> - </color> - <data> - <circular_buffer_capacity>1000</circular_buffer_capacity> - <time_frame_length>10</time_frame_length> - <type>0</type> - </data> - <style> - <lines_interpolate>false</lines_interpolate> - <pen_style>1</pen_style> - <pen_width>1</pen_width> - <render_antialias>false</render_antialias> - <steps_invert>false</steps_invert> - <sticks_baseline>0</sticks_baseline> - <sticks_orientation>2</sticks_orientation> - <type>0</type> - </style> - <subscriber_queue_size>100</subscriber_queue_size> - <title>Z</title> - </curve_2> </curves> <legend> <visible>true</visible> diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/mpc_metrics.xml b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/zero_velocity.xml similarity index 100% rename from ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/mpc_metrics.xml rename to ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/zero_velocity.xml diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch index 26190f062..e08f5659f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch @@ -1,7 +1,7 @@ <?xml version="1.0" ?> <launch> - <arg name="metrics_config" default="$(find ocs2_legged_robot)/config/multiplot/mpc_metrics.xml" /> + <arg name="metrics_config" default="$(find ocs2_legged_robot)/config/multiplot/zero_velocity.xml" /> <node name="mpc_metrics" pkg="rqt_multiplot" type="rqt_multiplot" args="--multiplot-run-all --multiplot-config $(arg metrics_config)" From 17f3b135b4f265b084298cd4e7dcbeb456805488 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 09:58:16 +0100 Subject: [PATCH 460/512] update pull requests --- ocs2_ipm/CMakeLists.txt | 6 +- ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 4 +- ocs2_ipm/include/ocs2_ipm/IpmMpc.h | 5 +- .../ocs2_ipm/IpmPerformanceIndexComputation.h | 65 ++++++++++++------- ocs2_ipm/include/ocs2_ipm/IpmSettings.h | 5 +- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 18 +++-- ocs2_ipm/package.xml | 8 +-- ocs2_ipm/src/IpmHelpers.cpp | 7 +- ocs2_ipm/src/IpmSettings.cpp | 2 - ocs2_ipm/src/IpmSolver.cpp | 57 +++++++--------- ocs2_ipm/test/testCircularKinematics.cpp | 4 ++ 11 files changed, 94 insertions(+), 87 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index c100a0833..5b95add44 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -5,7 +5,6 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_core ocs2_mpc ocs2_oc - ocs2_qp_solver blasfeo_catkin hpipm_catkin ) @@ -75,7 +74,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR + CF_FIX ) endif(cmake_clang_tools_FOUND) @@ -90,7 +89,8 @@ install( ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) ############# ## Testing ## diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 3e4feddbd..9531582a9 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -61,7 +61,7 @@ inline scalar_t evaluateComplementarySlackness(scalar_t barrierParam, const vect } /** - * Retrieves the Newton directions of the slack variable. + * Retrieves the Newton directions of the slack variable associated with state-input inequality constraints. * * @param[in] stateInputIneqConstraints : State-input inequality constraints * @param[in] dx : Newton direction of the state @@ -74,7 +74,7 @@ vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateIn scalar_t barrierParam, const vector_t& slackStateInputIneq); /** - * Retrieves the Newton directions of the slack variable. + * Retrieves the Newton directions of the slack variable associated with state-only inequality constraints. * * @param[in] stateIneqConstraints : State-only inequality constraints * @param[in] dx : Newton direction of the state diff --git a/ocs2_ipm/include/ocs2_ipm/IpmMpc.h b/ocs2_ipm/include/ocs2_ipm/IpmMpc.h index 1eda8be58..f5a5ada10 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmMpc.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmMpc.h @@ -48,9 +48,8 @@ class IpmMpc final : public MPC_BASE { */ IpmMpc(mpc::Settings mpcSettings, ipm::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) - : MPC_BASE(std::move(mpcSettings)) { - solverPtr_.reset(new IpmSolver(std::move(settings), optimalControlProblem, initializer)); - }; + : MPC_BASE(std::move(mpcSettings)), + solverPtr_(std::make_unique<IpmSolver>(std::move(settings), optimalControlProblem, initializer)) {} ~IpmMpc() override = default; diff --git a/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h b/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h index 2dbb10b96..b1609724f 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h @@ -42,10 +42,10 @@ namespace ipm { * * @param transcription : transcription computed by "multiple_shooting::setupIntermediateNode" * @param dt : Duration of the interval - * @param barrierParam : Barrier parameter of the interior point method. - * @param slackStateIneq : Slack variable of the state inequality constraints. - * @param slackStateInputIneq : Slack variable of the state-input inequality constraints. - * @return Performance index for a single intermediate node. + * @param barrierParam : Barrier parameter of the interior point method + * @param slackStateIneq : Slack variable of the state inequality constraints + * @param slackStateInputIneq : Slack variable of the state-input inequality constraints + * @return Performance index for a single intermediate node */ PerformanceIndex computePerformanceIndex(const multiple_shooting::Transcription& transcription, scalar_t dt, scalar_t barrierParam, const vector_t& slackStateIneq, const vector_t& slackStateInputIneq); @@ -54,9 +54,9 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::Transcription& * Compute the performance index from the transcription for the terminal node. * * @param transcription : transcription computed by "multiple_shooting::setupTerminalNode" - * @param barrierParam : Barrier parameter of the interior point method. - * @param slackIneq : Slack variable of the inequality constraints. - * @return Performance index for the terminal node. + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index for the terminal node */ PerformanceIndex computePerformanceIndex(const multiple_shooting::TerminalTranscription& transcription, scalar_t barrierParam, const vector_t& slackIneq); @@ -65,9 +65,9 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::TerminalTransc * Compute the performance index from the transcription for the event node. * * @param transcription : transcription computed by "multiple_shooting::setupEventNode" - * @param barrierParam : Barrier parameter of the interior point method. - * @param slackIneq : Slack variable of the inequality constraints. - * @return Performance index for the event node. + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index for the event node */ PerformanceIndex computePerformanceIndex(const multiple_shooting::EventTranscription& transcription, scalar_t barrierParam, const vector_t& slackIneq); @@ -78,17 +78,17 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::EventTranscrip * "multiple_shooting::setupIntermediateNode" * * @param optimalControlProblem : Definition of the optimal control problem - * @param discretizer : Integrator to use for creating the discrete dynamics. + * @param discretizer : Integrator to use for creating the discrete dynamics * @param t : Start of the discrete interval * @param dt : Duration of the interval * @param x : State at start of the interval * @param x_next : State at the end of the interval - * @param u : Input, taken to be constant across the interval. - * @param barrierParam : Barrier parameter of the interior point method. - * @param slackStateIneq : Slack variable of the state inequality constraints. - * @param slackStateInputIneq : Slack variable of the state-input inequality constraints. - * @param enableStateInequalityConstraints : Should be disabled at the initial node (i = 0). - * @return Performance index for a single intermediate node. + * @param u : Input, taken to be constant across the interval + * @param barrierParam : Barrier parameter of the interior point method + * @param slackStateIneq : Slack variable of the state inequality constraints + * @param slackStateInputIneq : Slack variable of the state-input inequality constraints + * @param enableStateInequalityConstraints : Should be disabled at the initial node (i = 0) + * @return Performance index for a single intermediate node */ PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, @@ -103,9 +103,9 @@ PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalCo * @param optimalControlProblem : Definition of the optimal control problem * @param t : Time at the terminal node * @param x : Terminal state - * @param barrierParam : Barrier parameter of the interior point method. - * @param slackIneq : Slack variable of the inequality constraints. - * @return Performance index for the terminal node. + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index for the terminal node */ PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, scalar_t barrierParam, const vector_t& slackIneq); @@ -119,16 +119,35 @@ PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalContro * @param t : Time at the event node * @param x : Pre-event state * @param x_next : Post-event state - * @param barrierParam : Barrier parameter of the interior point method. - * @param slackIneq : Slack variable of the inequality constraints. - * @return Performance index for the event node. + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index for the event node */ PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next, scalar_t barrierParam, const vector_t& slackIneq); +/** + * Computes the PerformanceIndex of the interior point method based on a given Metrics at an intermediate node. + * + * @param metrics : Metrics at an intermediate node + * @param dt : Duration of the interval + * @param barrierParam : Barrier parameter of the interior point method + * @param slackStateIneq : Slack variable of the state inequality constraints + * @param slackStateInputIneq : Slack variable of the state-input inequality constraints + * @param enableStateInequalityConstraints : Should be disabled at the initial node (i = 0) + * @return Performance index of the interior point method + */ PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_t barrierParam, const vector_t& slackStateIneq, const vector_t& slackStateInputIneq, bool enableStateInequalityConstraints); +/** + * Computes the PerformanceIndex of the interior point method based on a given Metrics at the event or terminal node. + * + * @param metrics : Metrics at the event or terminal node + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index of the interior point method + */ PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t barrierParam, const vector_t& slackIneq); } // namespace ipm diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h index 9d787e49f..aa141ebb3 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h @@ -64,10 +64,7 @@ struct Settings { scalar_t dt = 0.01; // user-defined time discretization SensitivityIntegratorType integratorType = SensitivityIntegratorType::RK2; - // Inequality penalty relaxed barrier parameters - scalar_t inequalityConstraintMu = 0.0; - scalar_t inequalityConstraintDelta = 1e-6; - + // Equality constraints bool projectStateInputEqualityConstraints = true; // Use a projection method to resolve the state-input constraint Cx+Du+e bool computeLagrangeMultipliers = true; // compute the Lagrange multipliers to evaluate the SSE of the dual feasibilities diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 1526cd815..30f15befe 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -127,15 +127,15 @@ class IpmSolver : public SolverBase { /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u, std::vector<Metrics>& metrics, const vector_array_t& lmd, - const vector_array_t& nu, scalar_t barrierParam, vector_array_t& slackStateIneq, - vector_array_t& slackStateInputIneq, vector_array_t& dualStateIneq, - vector_array_t& dualStateInputIneq, bool initializeSlackAndDualVariables); + const vector_array_t& u, const vector_array_t& lmd, const vector_array_t& nu, + scalar_t barrierParam, vector_array_t& slackStateIneq, vector_array_t& slackStateInputIneq, + vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq, + bool initializeSlackAndDualVariables, std::vector<Metrics>& metrics); /** Computes only the performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u, std::vector<Metrics>& metrics, scalar_t barrierParam, - const vector_array_t& slackStateIneq, const vector_array_t& slackStateInputIneq); + const vector_array_t& u, scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& slackStateInputIneq, std::vector<Metrics>& metrics); /** Returns solution of the QP subproblem in delta coordinates: */ struct OcpSubproblemSolution { @@ -163,9 +163,8 @@ class IpmSolver : public SolverBase { /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ ipm::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, - const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u, - std::vector<Metrics>& metrics, scalar_t barrierParam, vector_array_t& slackStateIneq, - vector_array_t& slackStateInputIneq); + const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u, scalar_t barrierParam, + vector_array_t& slackStateIneq, vector_array_t& slackStateInputIneq, std::vector<Metrics>& metrics); /** Updates the barrier parameter */ scalar_t updateBarrierParameter(scalar_t currentBarrierParameter, const PerformanceIndex& baseline, const ipm::StepInfo& stepInfo) const; @@ -218,7 +217,6 @@ class IpmSolver : public SolverBase { ProblemMetrics problemMetrics_; // Benchmarking - size_t numProblems_{0}; size_t totalNumIterations_{0}; benchmark::RepeatedTimer initializationTimer_; benchmark::RepeatedTimer linearQuadraticApproximationTimer_; diff --git a/ocs2_ipm/package.xml b/ocs2_ipm/package.xml index 60e87ddb4..9cb5787b9 100644 --- a/ocs2_ipm/package.xml +++ b/ocs2_ipm/package.xml @@ -2,18 +2,18 @@ <package format="2"> <name>ocs2_ipm</name> <version>0.0.0</version> - <description>The ocs2_hpipm package</description> + <description>The ocs2_ipm package</description> - <maintainer email="huier@todo.todo">huier</maintainer> + <maintainer email="sotaro.katayama@gmail.com">Sotaro Katayama</maintainer> - <license>TODO</license> + <license>BSD-3</license> <buildtool_depend>catkin</buildtool_depend> <depend>ocs2_core</depend> <depend>ocs2_mpc</depend> <depend>ocs2_oc</depend> - <depend>ocs2_qp_solver</depend> <depend>blasfeo_catkin</depend> <depend>hpipm_catkin</depend> + <test_depend>ocs2_qp_solver</test_depend> </package> diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index e1d3e5675..eaf1e614f 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -52,18 +52,17 @@ void condenseIneqConstraints(scalar_t barrierParam, const vector_t& slack, const } // coefficients for condensing - const vector_t condensingLinearCoeff = ((dual.array() * ineqConstraint.f.array() - barrierParam) / slack.array()).matrix(); + const vector_t condensingLinearCoeff = (dual.array() * ineqConstraint.f.array() - barrierParam) / slack.array(); const vector_t condensingQuadraticCoeff = dual.cwiseQuotient(slack); - matrix_t condensingQuadraticCoeff_dfdx, condensingQuadraticCoeff_dfdu; // condensing lagrangian.dfdx.noalias() += ineqConstraint.dfdx.transpose() * condensingLinearCoeff; - condensingQuadraticCoeff_dfdx.noalias() = condensingQuadraticCoeff.asDiagonal() * ineqConstraint.dfdx; + const matrix_t condensingQuadraticCoeff_dfdx = condensingQuadraticCoeff.asDiagonal() * ineqConstraint.dfdx; lagrangian.dfdxx.noalias() += ineqConstraint.dfdx.transpose() * condensingQuadraticCoeff_dfdx; if (nu > 0) { lagrangian.dfdu.noalias() += ineqConstraint.dfdu.transpose() * condensingLinearCoeff; - condensingQuadraticCoeff_dfdu.noalias() = condensingQuadraticCoeff.asDiagonal() * ineqConstraint.dfdu; + const matrix_t condensingQuadraticCoeff_dfdu = condensingQuadraticCoeff.asDiagonal() * ineqConstraint.dfdu; lagrangian.dfduu.noalias() += ineqConstraint.dfdu.transpose() * condensingQuadraticCoeff_dfdu; lagrangian.dfdux.noalias() += ineqConstraint.dfdu.transpose() * condensingQuadraticCoeff_dfdx; } diff --git a/ocs2_ipm/src/IpmSettings.cpp b/ocs2_ipm/src/IpmSettings.cpp index 737b38396..5a8cd8dc6 100644 --- a/ocs2_ipm/src/IpmSettings.cpp +++ b/ocs2_ipm/src/IpmSettings.cpp @@ -65,8 +65,6 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, auto integratorName = sensitivity_integrator::toString(settings.integratorType); loadData::loadPtreeValue(pt, integratorName, fieldName + ".integratorType", verbose); settings.integratorType = sensitivity_integrator::fromString(integratorName); - loadData::loadPtreeValue(pt, settings.inequalityConstraintMu, fieldName + ".inequalityConstraintMu", verbose); - loadData::loadPtreeValue(pt, settings.inequalityConstraintDelta, fieldName + ".inequalityConstraintDelta", verbose); loadData::loadPtreeValue(pt, settings.initialBarrierParameter, fieldName + ".initialBarrierParameter", verbose); loadData::loadPtreeValue(pt, settings.targetBarrierParameter, fieldName + ".targetBarrierParameter", verbose); loadData::loadPtreeValue(pt, settings.barrierReductionCostTol, fieldName + ".barrierReductionCostTol ", verbose); diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index a4998a070..5b13d783b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -108,7 +108,6 @@ void IpmSolver::reset() { performanceIndeces_.clear(); // reset timers - numProblems_ = 0; totalNumIterations_ = 0; linearQuadraticApproximationTimer_.reset(); solveQpTimer_.reset(); @@ -219,8 +218,8 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f linearQuadraticApproximationTimer_.startTimer(); const bool initializeSlackAndDualVariables = (iter == 0); const auto baselinePerformance = - setupQuadraticSubproblem(timeDiscretization, initState, x, u, metrics, lmd, nu, barrierParam, slackStateIneq, slackStateInputIneq, - dualStateIneq, dualStateInputIneq, initializeSlackAndDualVariables); + setupQuadraticSubproblem(timeDiscretization, initState, x, u, lmd, nu, barrierParam, slackStateIneq, slackStateInputIneq, + dualStateIneq, dualStateInputIneq, initializeSlackAndDualVariables, metrics); linearQuadraticApproximationTimer_.endTimer(); // Solve QP @@ -230,7 +229,6 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f getOCPSolution(delta_x0, barrierParam, slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq); vector_array_t deltaLmdSol; extractValueFunction(timeDiscretization, x, lmd, deltaSolution.deltaXSol, deltaLmdSol); - solveQpTimer_.endTimer(); // Apply step @@ -238,31 +236,28 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f const scalar_t maxPrimalStepSize = settings_.usePrimalStepSizeForDual ? std::min(deltaSolution.maxDualStepSize, deltaSolution.maxPrimalStepSize) : deltaSolution.maxPrimalStepSize; - const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, metrics, barrierParam, - slackStateIneq, slackStateInputIneq); + const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, barrierParam, slackStateIneq, + slackStateInputIneq, metrics); const scalar_t dualStepSize = settings_.usePrimalStepSizeForDual ? std::min(stepInfo.stepSize, deltaSolution.maxDualStepSize) : deltaSolution.maxDualStepSize; performanceIndeces_.push_back(stepInfo.performanceAfterStep); if (settings_.computeLagrangeMultipliers) { - for (int i = 0; i < lmd.size(); i++) { - lmd[i].noalias() += stepInfo.stepSize * deltaLmdSol[i]; - } + multiple_shooting::incrementTrajectory(lmd, deltaLmdSol, stepInfo.stepSize, lmd); if (settings_.projectStateInputEqualityConstraints) { + // Computes the deltaNuSol wirht r + // remaining term that depends on the costate + auto deltaNuSol = std::move(deltaSolution.deltaNuSol); for (int i = 0; i < nu.size(); i++) { if (nu[i].size() > 0) { - nu[i].noalias() += stepInfo.stepSize * deltaSolution.deltaNuSol[i]; - // remaining term that depends on the costate - nu[i].noalias() += stepInfo.stepSize * projectionMultiplierCoefficients_[i].dfdcostate * deltaLmdSol[i + 1]; + deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdcostate * deltaLmdSol[i + 1]; } } + multiple_shooting::incrementTrajectory(nu, deltaNuSol, stepInfo.stepSize, nu); } } - for (int i = 1; i < dualStateIneq.size(); i++) { - dualStateIneq[i].noalias() += dualStepSize * deltaSolution.deltaDualStateIneq[i]; - } - for (int i = 0; i < dualStateInputIneq.size(); i++) { - dualStateInputIneq[i].noalias() += dualStepSize * deltaSolution.deltaDualStateInputIneq[i]; - } + multiple_shooting::incrementTrajectory(dualStateIneq, deltaSolution.deltaDualStateIneq, stepInfo.stepSize, dualStateIneq); + multiple_shooting::incrementTrajectory(dualStateInputIneq, deltaSolution.deltaDualStateInputIneq, stepInfo.stepSize, + dualStateInputIneq); linesearchTimer_.endTimer(); // Check convergence @@ -287,8 +282,6 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); - ++numProblems_; - if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\nConvergence : " << toString(convergence) << "\n"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; @@ -303,7 +296,7 @@ void IpmSolver::runParallel(std::function<void(int)> taskFunction) { void IpmSolver::initializeCostateTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& stateTrajectory, vector_array_t& costateTrajectory) { - const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // // size of the input trajectory + const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // size of the input trajectory costateTrajectory.clear(); costateTrajectory.reserve(N + 1); const auto& ocpDefinition = ocpDefinitions_[0]; @@ -339,7 +332,7 @@ void IpmSolver::initializeCostateTrajectory(const std::vector<AnnotatedTime>& ti void IpmSolver::initializeProjectionMultiplierTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, vector_array_t& projectionMultiplierTrajectory) { - const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // // size of the input trajectory + const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // size of the input trajectory projectionMultiplierTrajectory.clear(); projectionMultiplierTrajectory.reserve(N); const auto& ocpDefinition = ocpDefinitions_[0]; @@ -511,11 +504,11 @@ PrimalSolution IpmSolver::toPrimalSolution(const std::vector<AnnotatedTime>& tim } PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u, std::vector<Metrics>& metrics, - const vector_array_t& lmd, const vector_array_t& nu, scalar_t barrierParam, - vector_array_t& slackStateIneq, vector_array_t& slackStateInputIneq, - vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq, - bool initializeSlackAndDualVariables) { + const vector_array_t& x, const vector_array_t& u, const vector_array_t& lmd, + const vector_array_t& nu, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& slackStateInputIneq, vector_array_t& dualStateIneq, + vector_array_t& dualStateInputIneq, bool initializeSlackAndDualVariables, + std::vector<Metrics>& metrics) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -665,8 +658,8 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated } PerformanceIndex IpmSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u, std::vector<Metrics>& metrics, scalar_t barrierParam, - const vector_array_t& slackStateIneq, const vector_array_t& slackStateInputIneq) { + const vector_array_t& u, scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& slackStateInputIneq, std::vector<Metrics>& metrics) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; metrics.resize(N + 1); @@ -717,8 +710,8 @@ PerformanceIndex IpmSolver::computePerformance(const std::vector<AnnotatedTime>& ipm::StepInfo IpmSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, - vector_array_t& u, std::vector<Metrics>& metrics, scalar_t barrierParam, vector_array_t& slackStateIneq, - vector_array_t& slackStateInputIneq) { + vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& slackStateInputIneq, std::vector<Metrics>& metrics) { using StepType = FilterLinesearch::StepType; /* @@ -758,7 +751,7 @@ ipm::StepInfo IpmSolver::takeStep(const PerformanceIndex& baseline, const std::v // Compute cost and constraints const PerformanceIndex performanceNew = - computePerformance(timeDiscretization, initState, xNew, uNew, metricsNew, barrierParam, slackStateIneqNew, slackStateInputIneqNew); + computePerformance(timeDiscretization, initState, xNew, uNew, barrierParam, slackStateIneqNew, slackStateInputIneqNew, metricsNew); // Step acceptance and record step type bool stepAccepted; diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp index 3449d6c47..349ff1d11 100644 --- a/ocs2_ipm/test/testCircularKinematics.cpp +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -144,6 +144,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { settings.dt = 0.01; settings.ipmIteration = 20; settings.projectStateInputEqualityConstraints = true; + settings.computeLagrangeMultipliers = true; settings.useFeedbackPolicy = true; settings.printSolverStatistics = true; settings.printSolverStatus = true; @@ -198,6 +199,7 @@ TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { settings.dt = 0.01; settings.ipmIteration = 20; settings.projectStateInputEqualityConstraints = false; // <- false to turn off projection of state-input equalities + settings.computeLagrangeMultipliers = true; settings.useFeedbackPolicy = true; settings.printSolverStatistics = true; settings.printSolverStatus = true; @@ -263,6 +265,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { settings.dt = 0.01; settings.ipmIteration = 40; settings.projectStateInputEqualityConstraints = true; + settings.computeLagrangeMultipliers = true; settings.useFeedbackPolicy = true; settings.printSolverStatistics = true; settings.printSolverStatus = true; @@ -358,6 +361,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint settings.dt = 0.01; settings.ipmIteration = 100; settings.projectStateInputEqualityConstraints = true; + settings.computeLagrangeMultipliers = true; settings.useFeedbackPolicy = true; settings.printSolverStatistics = true; settings.printSolverStatus = true; From c63fc83a0ce8551ca20e5f304a1cf61b187d7a2e Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 10:00:12 +0100 Subject: [PATCH 461/512] fix CMake --- ocs2_ipm/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index 5b95add44..9822ddb8b 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -74,7 +74,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) From 6f3ad252155d9bb5596c901831b36d293df5dc22 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 10:04:25 +0100 Subject: [PATCH 462/512] fix doc --- ocs2_ipm/src/IpmSolver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 5b13d783b..c88eecf1c 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -244,8 +244,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f if (settings_.computeLagrangeMultipliers) { multiple_shooting::incrementTrajectory(lmd, deltaLmdSol, stepInfo.stepSize, lmd); if (settings_.projectStateInputEqualityConstraints) { - // Computes the deltaNuSol wirht r - // remaining term that depends on the costate + // Remaining term that depends on the costate auto deltaNuSol = std::move(deltaSolution.deltaNuSol); for (int i = 0; i < nu.size(); i++) { if (nu[i].size() > 0) { From 27ccaaa8ef34c97eb4d04c64d729155c42cb3523 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 11:04:38 +0100 Subject: [PATCH 463/512] add IpmTrajectoryAdjustment --- ocs2_ipm/CMakeLists.txt | 1 + ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 8 +- .../ocs2_ipm/IpmTrajectoryAdjustment.h | 73 +++++++ ocs2_ipm/src/IpmSolver.cpp | 45 ++--- ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 180 ++++++++++++++++++ 5 files changed, 276 insertions(+), 31 deletions(-) create mode 100644 ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h create mode 100644 ocs2_ipm/src/IpmTrajectoryAdjustment.cpp diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index 9822ddb8b..e8e6021bf 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -54,6 +54,7 @@ add_library(${PROJECT_NAME} src/IpmPerformanceIndexComputation.cpp src/IpmSettings.cpp src/IpmSolver.cpp + src/IpmTrajectoryAdjustment.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 30f15befe..2d7e28be1 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -130,7 +130,7 @@ class IpmSolver : public SolverBase { const vector_array_t& u, const vector_array_t& lmd, const vector_array_t& nu, scalar_t barrierParam, vector_array_t& slackStateIneq, vector_array_t& slackStateInputIneq, vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq, - bool initializeSlackAndDualVariables, std::vector<Metrics>& metrics); + std::vector<Metrics>& metrics); /** Computes only the performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, @@ -191,10 +191,8 @@ class IpmSolver : public SolverBase { PrimalSolution primalSolution_; vector_array_t costateTrajectory_; vector_array_t projectionMultiplierTrajectory_; - vector_array_t slackStateIneqTrajectory_; - vector_array_t dualStateIneqTrajectory_; - vector_array_t slackStateInputIneqTrajectory_; - vector_array_t dualStateInputIneqTrajectory_; + DualSolution slackIneqTrajectory_; + DualSolution dualIneqTrajectory_; // Value function in absolute state coordinates (without the constant value) std::vector<ScalarFunctionQuadraticApproximation> valueFunction_; diff --git a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h new file mode 100644 index 000000000..3097800e0 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h @@ -0,0 +1,73 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#pragma once + +#include <ocs2_core/Types.h> + +#include <ocs2_oc/oc_data/DualSolution.h> +#include <ocs2_oc/oc_data/TimeDiscretization.h> + +namespace ocs2 { +namespace ipm { + +/** + * Convert the optimized slack or dual trajectories as a DualSolution. + * + * @param time: The time discretization. + * @param stateIneq: The slack/dual of the state inequality constraints. + * @param stateInputIneq: The slack/dual of the state-input inequality constraints. + * @return A dual solution. + */ +DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq); + +/** + * Convert the optimized slack or dual trajectories as a DualSolution. + * + * @param time: The time discretization. + * @param dualSolution: The dual solution. + * @return The slack/dual of the state inequality constraints (first) and state-input inequality constraints (second). + */ +std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution); + +/** + * Initializes the interior point trajectory. + * + * @param time: The time discretization. + * @param stateIneq: The slack/dual of the state inequality constraints. + * @param stateInputIneq: The slack/dual of the state-input inequality constraints. + * @return A dual solution. + */ +std::pair<vector_array_t, vector_array_t> initializeInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector<AnnotatedTime>& time, + DualSolution&& oldDualSolution); + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index c88eecf1c..4cdff7dd4 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -45,6 +45,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ipm/IpmHelpers.h" #include "ocs2_ipm/IpmInitialization.h" #include "ocs2_ipm/IpmPerformanceIndexComputation.h" +#include "ocs2_ipm/IpmTrajectoryAdjustment.h" namespace ocs2 { @@ -100,10 +101,8 @@ void IpmSolver::reset() { primalSolution_ = PrimalSolution(); costateTrajectory_.clear(); projectionMultiplierTrajectory_.clear(); - slackStateIneqTrajectory_.clear(); - dualStateIneqTrajectory_.clear(); - slackStateInputIneqTrajectory_.clear(); - dualStateInputIneqTrajectory_.clear(); + slackIneqTrajectory_.clear(); + dualIneqTrajectory_.clear(); valueFunction_.clear(); performanceIndeces_.clear(); @@ -200,6 +199,10 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Interior point variables vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; + std::tie(slackStateIneq, slackStateInputIneq) = ipm::initializeInteriorPointTrajectory( + primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(slackIneqTrajectory_)); + std::tie(dualStateIneq, dualStateInputIneq) = ipm::initializeInteriorPointTrajectory( + primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(dualIneqTrajectory_)); scalar_t barrierParam = settings_.initialBarrierParameter; @@ -216,10 +219,8 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Make QP approximation linearQuadraticApproximationTimer_.startTimer(); - const bool initializeSlackAndDualVariables = (iter == 0); - const auto baselinePerformance = - setupQuadraticSubproblem(timeDiscretization, initState, x, u, lmd, nu, barrierParam, slackStateIneq, slackStateInputIneq, - dualStateIneq, dualStateInputIneq, initializeSlackAndDualVariables, metrics); + const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u, lmd, nu, barrierParam, slackStateIneq, + slackStateInputIneq, dualStateIneq, dualStateInputIneq, metrics); linearQuadraticApproximationTimer_.endTimer(); // Solve QP @@ -274,10 +275,8 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); costateTrajectory_ = std::move(lmd); projectionMultiplierTrajectory_ = std::move(nu); - slackStateIneqTrajectory_ = std::move(slackStateIneq); - dualStateIneqTrajectory_ = std::move(dualStateIneq); - slackStateInputIneqTrajectory_ = std::move(slackStateInputIneq); - dualStateInputIneqTrajectory_ = std::move(dualStateInputIneq); + slackIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(slackStateIneq), std::move(slackStateInputIneq)); + dualIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(dualStateIneq), std::move(dualStateInputIneq)); problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); @@ -506,8 +505,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated const vector_array_t& x, const vector_array_t& u, const vector_array_t& lmd, const vector_array_t& nu, scalar_t barrierParam, vector_array_t& slackStateIneq, vector_array_t& slackStateInputIneq, vector_array_t& dualStateIneq, - vector_array_t& dualStateInputIneq, bool initializeSlackAndDualVariables, - std::vector<Metrics>& metrics) { + vector_array_t& dualStateInputIneq, std::vector<Metrics>& metrics) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -521,13 +519,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated projectionMultiplierCoefficients_.resize(N); metrics.resize(N + 1); - if (initializeSlackAndDualVariables) { - slackStateIneq.resize(N + 1); - slackStateInputIneq.resize(N + 1); - dualStateIneq.resize(N + 1); - dualStateInputIneq.resize(N + 1); - } - std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { // Get worker specific resources @@ -539,7 +530,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); - if (initializeSlackAndDualVariables) { + if (slackStateIneq[i].size() != result.ineqConstraints.f.size()) { slackStateIneq[i] = ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, @@ -574,13 +565,15 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated result.stateIneqConstraints.setZero(0, x[i].size()); std::fill(result.constraintsSize.stateIneq.begin(), result.constraintsSize.stateIneq.end(), 0); } - if (initializeSlackAndDualVariables) { + if (slackStateIneq[i].size() != result.stateIneqConstraints.f.size()) { slackStateIneq[i] = ipm::initializeSlackVariable(result.stateIneqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - slackStateInputIneq[i] = ipm::initializeSlackVariable(result.stateInputIneqConstraints.f, settings_.initialSlackLowerBound, - settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + } + if (slackStateInputIneq[i].size() != result.stateInputIneqConstraints.f.size()) { + slackStateInputIneq[i] = ipm::initializeSlackVariable(result.stateInputIneqConstraints.f, settings_.initialSlackLowerBound, + settings_.initialSlackMarginRate); dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } @@ -619,7 +612,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); - if (initializeSlackAndDualVariables) { + if (slackStateIneq[i].size() != result.ineqConstraints.f.size()) { slackStateIneq[N] = ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[N] = diff --git a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp new file mode 100644 index 000000000..eaf2645cc --- /dev/null +++ b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp @@ -0,0 +1,180 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_ipm/IpmTrajectoryAdjustment.h" + +#include <ocs2_oc/trajectory_adjustment/TrajectorySpreading.h> +#include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h> + +namespace ocs2 { +namespace { + +MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { + MultiplierCollection multiplierCollection; + multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); + if (stateInputIneq.size() > 0) { + multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); + } + return multiplierCollection; +} + +std::pair<vector_t, vector_t> fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { + if (multiplierCollection.stateInputIneq.empty()) { + return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), vector_t()); + } else { + return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), + std::move(multiplierCollection.stateInputIneq.front().lagrangian)); + } +} +} // namespace +} // namespace ocs2 + +namespace ocs2 { +namespace ipm { + +DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq) { + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + + DualSolution dualSolution; + dualSolution.timeTrajectory = toTime(time); + dualSolution.postEventIndices = toPostEventIndices(time); + + dualSolution.preJumps.reserve(dualSolution.postEventIndices.size()); + dualSolution.intermediates.reserve(time.size()); + + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(stateIneq[i]))); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node + } else { + dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(stateIneq[i]), std::move(stateInputIneq[i]))); + } + } + dualSolution.final = toMultiplierCollection(std::move(stateIneq[N])); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); + + return dualSolution; +} + +std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution) { + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + + vector_array_t stateIneq; + vector_array_t stateInputIneq; + stateIneq.reserve(N + 1); + stateInputIneq.reserve(N + 1); + + int preJumpIdx = 0; + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + auto result = fromMultiplierCollection(std::move(dualSolution.preJumps[preJumpIdx])); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + ++preJumpIdx; + } else { + auto result = fromMultiplierCollection(std::move(dualSolution.intermediates[i])); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + } + } + + auto result = fromMultiplierCollection(std::move(dualSolution.final)); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + + return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); +} + +std::pair<vector_array_t, vector_array_t> initializeInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector<AnnotatedTime>& time, + DualSolution&& oldDualSolution) { + const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; + const auto oldPostEventIndices = oldDualSolution.postEventIndices; + + if (!oldTimeTrajectory.empty()) { + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, oldDualSolution); + } + + const auto newTimeTrajectory = toTime(time); + const auto newPostEventIndices = toPostEventIndices(time); + + // find the time period that we can interpolate the cached solution + const auto timePeriod = std::make_pair(newTimeTrajectory.front(), newTimeTrajectory.back()); + const auto interpolatableTimePeriod = findIntersectionToExtendableInterval(oldTimeTrajectory, newModeSchedule.eventTimes, timePeriod); + const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); + + DualSolution newDualSolution; + + // set time and post-event indices + newDualSolution.timeTrajectory = std::move(newTimeTrajectory); + newDualSolution.postEventIndices = std::move(newPostEventIndices); + + // final + if (interpolateTillFinalTime) { + newDualSolution.final = std::move(oldDualSolution.final); + } else { + newDualSolution.final = toMultiplierCollection(vector_t()); + } + + // pre-jumps + newDualSolution.preJumps.resize(newDualSolution.postEventIndices.size()); + if (!newDualSolution.postEventIndices.empty()) { + const auto firstEventTime = newDualSolution.timeTrajectory[newDualSolution.postEventIndices[0] - 1]; + const auto cacheEventIndexBias = getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); + + for (size_t i = 0; i < newDualSolution.postEventIndices.size(); i++) { + const auto cachedTimeIndex = cacheEventIndexBias + i; + if (cachedTimeIndex < oldDualSolution.preJumps.size()) { + newDualSolution.preJumps[i] = std::move(oldDualSolution.preJumps[cachedTimeIndex]); + } else { + newDualSolution.preJumps[i] = toMultiplierCollection(vector_t()); + } + } + } + + // intermediates + newDualSolution.intermediates.resize(newDualSolution.timeTrajectory.size() - 1); + for (size_t i = 0; i < newDualSolution.timeTrajectory.size() - 1; i++) { + const auto time = newDualSolution.timeTrajectory[i]; + + if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { + newDualSolution.intermediates[i] = getIntermediateDualSolutionAtTime(oldDualSolution, time); + } else { + newDualSolution.intermediates[i] = toMultiplierCollection(vector_t(), vector_t()); + } + } + + return fromDualSolution(time, std::move(newDualSolution)); +} + +} // namespace ipm +} // namespace ocs2 From 496142c372bb9e3b41076fac102be98bd0c65113 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 11:15:51 +0100 Subject: [PATCH 464/512] fix doc --- ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h index 3097800e0..a44b93f71 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h @@ -57,7 +57,8 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution); /** - * Initializes the interior point trajectory. + * Initializes the interior point trajectory based on the stored trajectory. The part of the new trajectory that is uncovered by the stored + * trajectory is set by zero-sized vectors. * * @param time: The time discretization. * @param stateIneq: The slack/dual of the state inequality constraints. From 3bfd82024cdafd9918b21346bf0e19856f23703f Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 12:00:47 +0100 Subject: [PATCH 465/512] fix initializations --- ocs2_ipm/CMakeLists.txt | 2 +- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 4 +-- ocs2_ipm/src/IpmSolver.cpp | 40 ++++++++++----------------- 3 files changed, 18 insertions(+), 28 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index 9822ddb8b..5b95add44 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -74,7 +74,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR + CF_FIX ) endif(cmake_clang_tools_FOUND) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 30f15befe..00265d668 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -119,11 +119,11 @@ class IpmSolver : public SolverBase { /** Initializes for the costate trajectories */ void initializeCostateTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& stateTrajectory, - vector_array_t& costateTrajectory); + vector_array_t& costateTrajectory) const; /** Initializes for the Lagrange multiplier trajectories of the constraint projection */ void initializeProjectionMultiplierTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, - vector_array_t& projectionMultiplierTrajectory); + vector_array_t& projectionMultiplierTrajectory) const; /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index c88eecf1c..bdf863d06 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -294,54 +294,44 @@ void IpmSolver::runParallel(std::function<void(int)> taskFunction) { } void IpmSolver::initializeCostateTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& stateTrajectory, - vector_array_t& costateTrajectory) { + vector_array_t& costateTrajectory) const { const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // size of the input trajectory costateTrajectory.clear(); costateTrajectory.reserve(N + 1); - const auto& ocpDefinition = ocpDefinitions_[0]; - - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; - ocpDefinition.preComputationPtr->requestFinal(request, timeDiscretization[N].time, stateTrajectory[N]); - const vector_t lmdN = -approximateFinalCost(ocpDefinition, timeDiscretization[N].time, stateTrajectory[N]).dfdx; // Determine till when to use the previous solution - scalar_t interpolateCostateTill = timeDiscretization.front().time; - if (primalSolution_.timeTrajectory_.size() >= 2) { - interpolateCostateTill = primalSolution_.timeTrajectory_.back(); - } + const auto interpolateTill = + primalSolution_.timeTrajectory_.size() < 2 ? timeDiscretization.front().time : primalSolution_.timeTrajectory_.back(); const scalar_t initTime = getIntervalStart(timeDiscretization[0]); - if (initTime < interpolateCostateTill) { + if (initTime < interpolateTill) { costateTrajectory.push_back(LinearInterpolation::interpolate(initTime, primalSolution_.timeTrajectory_, costateTrajectory_)); } else { - costateTrajectory.push_back(lmdN); - // costateTrajectory.push_back(vector_t::Zero(stateTrajectory[0].size())); + costateTrajectory.push_back(vector_t::Zero(stateTrajectory[0].size())); } for (int i = 0; i < N; i++) { const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); - if (nextTime > interpolateCostateTill) { // Initialize with zero - // costateTrajectory.push_back(vector_t::Zero(stateTrajectory[i + 1].size())); - costateTrajectory.push_back(lmdN); - } else { // interpolate previous solution + if (nextTime < interpolateTill) { // interpolate previous solution costateTrajectory.push_back(LinearInterpolation::interpolate(nextTime, primalSolution_.timeTrajectory_, costateTrajectory_)); + } else { // Initialize with zero + costateTrajectory.push_back(vector_t::Zero(stateTrajectory[i + 1].size())); } } } void IpmSolver::initializeProjectionMultiplierTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, - vector_array_t& projectionMultiplierTrajectory) { + vector_array_t& projectionMultiplierTrajectory) const { const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // size of the input trajectory projectionMultiplierTrajectory.clear(); projectionMultiplierTrajectory.reserve(N); const auto& ocpDefinition = ocpDefinitions_[0]; // Determine till when to use the previous solution - scalar_t interpolateInputTill = timeDiscretization.front().time; - if (primalSolution_.timeTrajectory_.size() >= 2) { - interpolateInputTill = primalSolution_.timeTrajectory_[primalSolution_.timeTrajectory_.size() - 2]; - } + const auto interpolateTill = + primalSolution_.timeTrajectory_.size() < 2 ? timeDiscretization.front().time : *std::prev(primalSolution_.timeTrajectory_.end(), 2); + // @todo Fix this using trajectory spreading auto interpolateProjectionMultiplierTrajectory = [&](scalar_t time) -> vector_t { const size_t numConstraints = ocpDefinition.equalityConstraintPtr->getNumConstraints(time); const size_t index = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_).first; @@ -367,10 +357,10 @@ void IpmSolver::initializeProjectionMultiplierTrajectory(const std::vector<Annot // Intermediate node const scalar_t time = getIntervalStart(timeDiscretization[i]); const size_t numConstraints = ocpDefinition.equalityConstraintPtr->getNumConstraints(time); - if (time > interpolateInputTill) { // Initialize with zero - projectionMultiplierTrajectory.push_back(vector_t::Zero(numConstraints)); - } else { // interpolate previous solution + if (time < interpolateTill) { // interpolate previous solution projectionMultiplierTrajectory.push_back(interpolateProjectionMultiplierTrajectory(time)); + } else { // Initialize with zero + projectionMultiplierTrajectory.push_back(vector_t::Zero(numConstraints)); } } } From 092e122191dbc45b33480823b3d12e9b9a27259c Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 12:04:12 +0100 Subject: [PATCH 466/512] fix metrics for initial state --- ocs2_ipm/src/IpmSolver.cpp | 6 ++++-- ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index bdf863d06..500926583 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -636,8 +636,10 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated }; runParallel(std::move(parallelTask)); - // Account for init state in performance - performance.front().dynamicsViolationSSE += (initState - x.front()).squaredNorm(); + // Account for initial state in performance + const vector_t initDynamicsViolation = initState - x.front(); + metrics.front().dynamicsViolation += initDynamicsViolation; + performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm(); // Sum performance of the threads PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 33a921e54..0408cbc2d 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -358,8 +358,10 @@ PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector<Annotated }; runParallel(std::move(parallelTask)); - // Account for init state in performance - performance.front().dynamicsViolationSSE += (initState - x.front()).squaredNorm(); + // Account for initial state in performance + const vector_t initDynamicsViolation = initState - x.front(); + metrics.front().dynamicsViolation += initDynamicsViolation; + performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm(); // Sum performance of the threads PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); From c7034d5cf64ed10c7a13a112042732eb5f28ccb4 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 12:34:01 +0100 Subject: [PATCH 467/512] refactor IpmSolver --- ocs2_ipm/src/IpmSolver.cpp | 64 +++++++++++++++++++------------------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 500926583..c41f2e02b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -374,8 +374,7 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta auto& deltaXSol = solution.deltaXSol; auto& deltaUSol = solution.deltaUSol; hpipm_status status; - const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); - if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { + if (!settings_.projectStateInputEqualityConstraints) { hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, &stateInputEqConstraints_)); status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, &stateInputEqConstraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); @@ -405,8 +404,8 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta deltaDualStateIneq.resize(N + 1); deltaDualStateInputIneq.resize(N + 1); - scalar_array_t primalStepSizes(N + 1, 1.0); - scalar_array_t dualStepSizes(N + 1, 1.0); + scalar_array_t primalStepSizes(settings_.nThreads, 1.0); + scalar_array_t dualStepSizes(settings_.nThreads, 1.0); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { @@ -421,21 +420,21 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta ipm::retrieveSlackDirection(stateInputIneqConstraints_[i], deltaXSol[i], deltaUSol[i], barrierParam, slackStateInputIneq[i]); deltaDualStateInputIneq[i] = ipm::retrieveDualDirection(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], deltaSlackStateInputIneq[i]); - primalStepSizes[i] = std::min( - ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin), - ipm::fractionToBoundaryStepSize(slackStateInputIneq[i], deltaSlackStateInputIneq[i], settings_.fractionToBoundaryMargin)); - dualStepSizes[i] = - std::min(ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin), - ipm::fractionToBoundaryStepSize(dualStateInputIneq[i], deltaDualStateInputIneq[i], settings_.fractionToBoundaryMargin)); - // Re-map the projected input back to the original space. + primalStepSizes[workerId] = std::min( + {primalStepSizes[workerId], + ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin), + ipm::fractionToBoundaryStepSize(slackStateInputIneq[i], deltaSlackStateInputIneq[i], settings_.fractionToBoundaryMargin)}); + dualStepSizes[workerId] = std::min( + {dualStepSizes[workerId], + ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin), + ipm::fractionToBoundaryStepSize(dualStateInputIneq[i], deltaDualStateInputIneq[i], settings_.fractionToBoundaryMargin)}); if (constraintsProjection_[i].f.size() > 0) { if (settings_.computeLagrangeMultipliers) { - const auto& dfdx = projectionMultiplierCoefficients_[i].dfdx; - const auto& dfdu = projectionMultiplierCoefficients_[i].dfdu; - const auto& dfdcostate = projectionMultiplierCoefficients_[i].dfdcostate; - const auto& f = projectionMultiplierCoefficients_[i].f; - deltaNuSol[i].noalias() = dfdx * deltaXSol[i] + dfdu * deltaUSol[i] + f; + deltaNuSol[i] = projectionMultiplierCoefficients_[i].f; + deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdx * deltaXSol[i]; + deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdu * deltaUSol[i]; } + // Re-map the projected input back to the original space. tmp.noalias() = constraintsProjection_[i].dfdu * deltaUSol[i]; deltaUSol[i] = tmp + constraintsProjection_[i].f; deltaUSol[i].noalias() += constraintsProjection_[i].dfdx * deltaXSol[i]; @@ -447,8 +446,11 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta if (i == N) { // Only one worker will execute this deltaSlackStateIneq[i] = ipm::retrieveSlackDirection(stateIneqConstraints_[i], deltaXSol[i], barrierParam, slackStateIneq[i]); deltaDualStateIneq[i] = ipm::retrieveDualDirection(barrierParam, slackStateIneq[i], dualStateIneq[i], deltaSlackStateIneq[i]); - primalStepSizes[i] = ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin); - dualStepSizes[i] = ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin); + primalStepSizes[workerId] = + std::min(primalStepSizes[workerId], + ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin)); + dualStepSizes[workerId] = std::min(dualStepSizes[workerId], ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], + settings_.fractionToBoundaryMargin)); } }; runParallel(std::move(parallelTask)); @@ -522,7 +524,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated auto parallelTask = [&](int workerId) { // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; - PerformanceIndex workerPerformance; // Accumulate performance in local variable int i = timeIndex++; while (i < N) { @@ -536,7 +537,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated settings_.initialDualMarginRate); } metrics[i] = multiple_shooting::computeMetrics(result); - workerPerformance += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]); + performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]); dynamics_[i] = std::move(result.dynamics); stateInputEqConstraints_[i].resize(0, x[i].size()); stateIneqConstraints_[i] = std::move(result.ineqConstraints); @@ -551,9 +552,10 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]); if (settings_.computeLagrangeMultipliers) { - workerPerformance.dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); + performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); } - workerPerformance.dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); + performance[workerId].dualFeasibilitiesSSE += + ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); @@ -575,7 +577,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated settings_.initialDualMarginRate); } metrics[i] = multiple_shooting::computeMetrics(result); - workerPerformance += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); + performance[workerId] += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); if (settings_.projectStateInputEqualityConstraints) { multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers); } @@ -596,10 +598,11 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated ipm::condenseIneqConstraints(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], stateInputIneqConstraints_[i], lagrangian_[i]); if (settings_.computeLagrangeMultipliers) { - workerPerformance.dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); + performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); } - workerPerformance.dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); - workerPerformance.dualFeasibilitiesSSE += + performance[workerId].dualFeasibilitiesSSE += + ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); + performance[workerId].dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i]); } @@ -616,7 +619,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } metrics[i] = multiple_shooting::computeMetrics(result); - workerPerformance += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]); + performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]); stateInputEqConstraints_[i].resize(0, x[i].size()); stateIneqConstraints_[i] = std::move(result.ineqConstraints); if (settings_.computeLagrangeMultipliers) { @@ -626,13 +629,10 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated } ipm::condenseIneqConstraints(barrierParam, slackStateIneq[N], dualStateIneq[N], stateIneqConstraints_[N], lagrangian_[N]); if (settings_.computeLagrangeMultipliers) { - workerPerformance.dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]); + performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]); } - workerPerformance.dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[N], dualStateIneq[N]); + performance[workerId].dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[N], dualStateIneq[N]); } - - // Accumulate! Same worker might run multiple tasks - performance[workerId] += workerPerformance; }; runParallel(std::move(parallelTask)); From 3ca37d83f554a095b3dea03f58c82ba3bf583684 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 13:27:45 +0100 Subject: [PATCH 468/512] modify around computeLagrangeMultiplier option --- ocs2_ipm/include/ocs2_ipm/IpmSettings.h | 8 ++--- ocs2_ipm/src/IpmSettings.cpp | 2 +- ocs2_ipm/src/IpmSolver.cpp | 42 ++++++++++++------------- 3 files changed, 26 insertions(+), 26 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h index aa141ebb3..11d32f8e9 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h @@ -54,8 +54,9 @@ struct Settings { scalar_t gamma_c = 1e-6; // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i} // controller type - bool useFeedbackPolicy = true; // true to use feedback, false to use feedforward - bool createValueFunction = false; // true to store the value function, false to ignore it + bool useFeedbackPolicy = true; // true to use feedback, false to use feedforward + bool createValueFunction = false; // true to store the value function, false to ignore it + bool computeLagrangeMultipliers = false; // true to compute the Lagrange multipliers // QP subproblem solver settings hpipm_interface::Settings hpipmSettings = hpipm_interface::Settings(); @@ -66,7 +67,6 @@ struct Settings { // Equality constraints bool projectStateInputEqualityConstraints = true; // Use a projection method to resolve the state-input constraint Cx+Du+e - bool computeLagrangeMultipliers = true; // compute the Lagrange multipliers to evaluate the SSE of the dual feasibilities // Barrier strategy of the primal-dual interior point method. Conventions follows Ipopt. scalar_t initialBarrierParameter = 1.0e-02; // Initial value of the barrier parameter @@ -107,7 +107,7 @@ struct Settings { * @param [in] verbose: Flag to determine whether to print out the loaded settings or not. * @return The settings */ -Settings loadSettings(const std::string& filename, const std::string& fieldName = "multiple_shooting", bool verbose = true); +Settings loadSettings(const std::string& filename, const std::string& fieldName = "ipm", bool verbose = true); } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmSettings.cpp b/ocs2_ipm/src/IpmSettings.cpp index 5a8cd8dc6..d2447f77b 100644 --- a/ocs2_ipm/src/IpmSettings.cpp +++ b/ocs2_ipm/src/IpmSettings.cpp @@ -62,6 +62,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, loadData::loadPtreeValue(pt, settings.dt, fieldName + ".dt", verbose); loadData::loadPtreeValue(pt, settings.useFeedbackPolicy, fieldName + ".useFeedbackPolicy", verbose); loadData::loadPtreeValue(pt, settings.createValueFunction, fieldName + ".createValueFunction", verbose); + loadData::loadPtreeValue(pt, settings.computeLagrangeMultipliers, fieldName + ".computeLagrangeMultipliers", verbose); auto integratorName = sensitivity_integrator::toString(settings.integratorType); loadData::loadPtreeValue(pt, integratorName, fieldName + ".integratorType", verbose); settings.integratorType = sensitivity_integrator::fromString(integratorName); @@ -74,7 +75,6 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, loadData::loadPtreeValue(pt, settings.fractionToBoundaryMargin, fieldName + ".fractionToBoundaryMargin", verbose); loadData::loadPtreeValue(pt, settings.usePrimalStepSizeForDual, fieldName + ".usePrimalStepSizeForDual", verbose); loadData::loadPtreeValue(pt, settings.projectStateInputEqualityConstraints, fieldName + ".projectStateInputEqualityConstraints", verbose); - loadData::loadPtreeValue(pt, settings.computeLagrangeMultipliers, fieldName + ".computeLagrangeMultipliers", verbose); loadData::loadPtreeValue(pt, settings.initialSlackLowerBound, fieldName + ".initialSlackLowerBound", verbose); loadData::loadPtreeValue(pt, settings.initialDualLowerBound, fieldName + ".initialDualLowerBound", verbose); loadData::loadPtreeValue(pt, settings.initialSlackMarginRate, fieldName + ".initialSlackMarginRate", verbose); diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index c41f2e02b..eff62ded3 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -50,6 +50,10 @@ namespace ocs2 { namespace { ipm::Settings rectifySettings(const OptimalControlProblem& ocp, ipm::Settings&& settings) { + // We have to create the value function if we want to compute the Lagrange multipliers. + if (settings.computeLagrangeMultipliers) { + settings.createValueFunction = true; + } // True does not make sense if there are no constraints. if (ocp.equalityConstraintPtr->empty()) { settings.projectStateInputEqualityConstraints = false; @@ -463,18 +467,20 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta void IpmSolver::extractValueFunction(const std::vector<AnnotatedTime>& time, const vector_array_t& x, const vector_array_t& lmd, const vector_array_t& deltaXSol, vector_array_t& deltaLmdSol) { - valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], lagrangian_[0]); - // Compute costate directions - deltaLmdSol.resize(deltaXSol.size()); - for (int i = 0; i < time.size(); ++i) { - deltaLmdSol[i] = valueFunction_[i].dfdx; - deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * x[i]; - } - // Correct for linearization state - for (int i = 0; i < time.size(); ++i) { - valueFunction_[i].dfdx.noalias() -= valueFunction_[i].dfdxx * x[i]; - if (settings_.computeLagrangeMultipliers) { - valueFunction_[i].dfdx.noalias() += lmd[i]; + if (settings_.createValueFunction) { + valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], lagrangian_[0]); + // Compute costate directions + deltaLmdSol.resize(deltaXSol.size()); + for (int i = 0; i < time.size(); ++i) { + deltaLmdSol[i] = valueFunction_[i].dfdx; + deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * x[i]; + } + // Correct for linearization state + for (int i = 0; i < time.size(); ++i) { + valueFunction_[i].dfdx.noalias() -= valueFunction_[i].dfdxx * x[i]; + if (settings_.computeLagrangeMultipliers) { + valueFunction_[i].dfdx.noalias() += lmd[i]; + } } } } @@ -551,9 +557,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated } ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]); - if (settings_.computeLagrangeMultipliers) { - performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); - } + performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); performance[workerId].dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); } else { @@ -597,9 +601,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]); ipm::condenseIneqConstraints(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], stateInputIneqConstraints_[i], lagrangian_[i]); - if (settings_.computeLagrangeMultipliers) { - performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); - } + performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); performance[workerId].dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); performance[workerId].dualFeasibilitiesSSE += @@ -628,9 +630,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated lagrangian_[i] = std::move(result.cost); } ipm::condenseIneqConstraints(barrierParam, slackStateIneq[N], dualStateIneq[N], stateIneqConstraints_[N], lagrangian_[N]); - if (settings_.computeLagrangeMultipliers) { - performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]); - } + performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]); performance[workerId].dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[N], dualStateIneq[N]); } }; From ced25bc4fef386f3ecbce36635a8022dff1192e5 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 13:47:23 +0100 Subject: [PATCH 469/512] refactored tests --- ocs2_ipm/src/IpmSolver.cpp | 2 +- ocs2_ipm/test/Exp0Test.cpp | 141 +++++++++-------- ocs2_ipm/test/Exp1Test.cpp | 192 ++++++++++++----------- ocs2_ipm/test/testCircularKinematics.cpp | 158 +++++++++++-------- 4 files changed, 272 insertions(+), 221 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index eff62ded3..f887874a3 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -565,8 +565,8 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + // Disable the state-only inequality constraints at the initial node if (i == 0) { - // Disable the state-only inequality constraints at the initial node result.stateIneqConstraints.setZero(0, x[i].size()); std::fill(result.constraintsSize.stateIneq.begin(), result.constraintsSize.stateIneq.end(), 0); } diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp index 79a8a9069..1995ef5d6 100644 --- a/ocs2_ipm/test/Exp0Test.cpp +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <gtest/gtest.h> #include <cstdlib> #include <ctime> @@ -7,7 +36,6 @@ #include "ocs2_ipm/IpmSolver.h" #include <ocs2_core/initialization/DefaultInitializer.h> - #include <ocs2_oc/test/EXP0.h> using namespace ocs2; @@ -75,24 +103,26 @@ TEST(Exp0Test, Unconstrained) { static constexpr size_t INPUT_DIM = 1; // Solver settings - ipm::Settings settings; - settings.dt = 0.01; - settings.ipmIteration = 20; - settings.projectStateInputEqualityConstraints = true; - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.nThreads = 1; - - settings.initialBarrierParameter = 1.0e-02; - settings.targetBarrierParameter = 1.0e-04; - settings.barrierLinearDecreaseFactor = 0.2; - settings.barrierSuperlinearDecreasePower = 1.5; - settings.fractionToBoundaryMargin = 0.995; + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.projectStateInputEqualityConstraints = true; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); const scalar_array_t initEventTimes{0.1897}; const size_array_t modeSequence{0, 1}; @@ -109,16 +139,6 @@ TEST(Exp0Test, Unconstrained) { IpmSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); solver.run(startTime, initState, finalTime); - solver.run(startTime, initState, finalTime); - solver.run(startTime, initState, finalTime); - - const auto primalSolution = solver.primalSolution(finalTime); - std::cout << "Optimal unconstrained trajectory" << std::endl; - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] - << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } } TEST(Exp0Test, Constrained) { @@ -126,24 +146,26 @@ TEST(Exp0Test, Constrained) { static constexpr size_t INPUT_DIM = 1; // Solver settings - ipm::Settings settings; - settings.dt = 0.01; - settings.ipmIteration = 20; - settings.projectStateInputEqualityConstraints = true; - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.nThreads = 1; - - settings.initialBarrierParameter = 1.0e-02; - settings.targetBarrierParameter = 1.0e-04; - settings.barrierLinearDecreaseFactor = 0.2; - settings.barrierSuperlinearDecreasePower = 1.5; - settings.fractionToBoundaryMargin = 0.995; + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.projectStateInputEqualityConstraints = true; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); const scalar_array_t initEventTimes{0.1897}; const size_array_t modeSequence{0, 1}; @@ -153,12 +175,12 @@ TEST(Exp0Test, Constrained) { // add inequality constraints const scalar_t umin = -7.5; const scalar_t umax = 7.5; - std::unique_ptr<StateInputConstraint> stateInputIneqConstraint(new EXP0_StateInputIneqConstraints(umin, umax)); + auto stateInputIneqConstraint = std::make_unique<EXP0_StateInputIneqConstraints>(umin, umax); problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint)); const vector_t xmin = (vector_t(2) << -7.5, -7.5).finished(); const vector_t xmax = (vector_t(2) << 7.5, 7.5).finished(); - std::unique_ptr<StateConstraint> stateIneqConstraint(new EXP0_StateIneqConstraints(xmin, xmax)); - std::unique_ptr<StateConstraint> finalStateIneqConstraint(new EXP0_StateIneqConstraints(xmin, xmax)); + auto stateIneqConstraint = std::make_unique<EXP0_StateIneqConstraints>(xmin, xmax); + auto finalStateIneqConstraint = std::make_unique<EXP0_StateIneqConstraints>(xmin, xmax); problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint)); problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint)); @@ -172,29 +194,22 @@ TEST(Exp0Test, Constrained) { IpmSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); solver.run(startTime, initState, finalTime); - // std::cout << solver.getBenchmarkingInformation() << std::endl; const auto primalSolution = solver.primalSolution(finalTime); - std::cout << "Optimal unconstrained trajectory" << std::endl; - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] - << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } // check constraint satisfaction for (const auto& e : primalSolution.stateTrajectory_) { if (e.size() > 0) { - EXPECT_TRUE(e.coeff(0) >= xmin.coeff(0)); - EXPECT_TRUE(e.coeff(1) >= xmin.coeff(1)); - EXPECT_TRUE(e.coeff(0) <= xmax.coeff(0)); - EXPECT_TRUE(e.coeff(1) <= xmax.coeff(1)); + ASSERT_TRUE(e.coeff(0) >= xmin.coeff(0)); + ASSERT_TRUE(e.coeff(1) >= xmin.coeff(1)); + ASSERT_TRUE(e.coeff(0) <= xmax.coeff(0)); + ASSERT_TRUE(e.coeff(1) <= xmax.coeff(1)); } } for (const auto& e : primalSolution.inputTrajectory_) { if (e.size() > 0) { - EXPECT_TRUE(e.coeff(0) >= umin); - EXPECT_TRUE(e.coeff(0) <= umax); + ASSERT_TRUE(e.coeff(0) >= umin); + ASSERT_TRUE(e.coeff(0) <= umax); } } } \ No newline at end of file diff --git a/ocs2_ipm/test/Exp1Test.cpp b/ocs2_ipm/test/Exp1Test.cpp index 9c2a9a99e..11651229f 100644 --- a/ocs2_ipm/test/Exp1Test.cpp +++ b/ocs2_ipm/test/Exp1Test.cpp @@ -1,3 +1,32 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + #include <gtest/gtest.h> #include <cstdlib> #include <ctime> @@ -7,7 +36,6 @@ #include "ocs2_ipm/IpmSolver.h" #include <ocs2_core/initialization/DefaultInitializer.h> - #include <ocs2_oc/test/EXP1.h> using namespace ocs2; @@ -104,24 +132,26 @@ TEST(Exp1Test, Unconstrained) { static constexpr size_t INPUT_DIM = 1; // Solver settings - ipm::Settings settings; - settings.dt = 0.01; - settings.ipmIteration = 20; - settings.projectStateInputEqualityConstraints = true; - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.nThreads = 1; - - settings.initialBarrierParameter = 1.0e-02; - settings.targetBarrierParameter = 1.0e-04; - settings.barrierLinearDecreaseFactor = 0.2; - settings.barrierSuperlinearDecreasePower = 1.5; - settings.fractionToBoundaryMargin = 0.995; + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.projectStateInputEqualityConstraints = true; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); const scalar_array_t initEventTimes{0.2262, 1.0176}; const size_array_t modeSequence{0, 1, 2}; @@ -134,20 +164,10 @@ TEST(Exp1Test, Unconstrained) { DefaultInitializer zeroInitializer(INPUT_DIM); - auto initializerPtr = std::unique_ptr<Initializer>(new DefaultInitializer(INPUT_DIM)); - // Solve IpmSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); solver.run(startTime, initState, finalTime); - - const auto primalSolution = solver.primalSolution(finalTime); - std::cout << "Optimal unconstrained trajectory" << std::endl; - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] - << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } } TEST(Exp1Test, Constrained) { @@ -155,24 +175,26 @@ TEST(Exp1Test, Constrained) { static constexpr size_t INPUT_DIM = 1; // Solver settings - ipm::Settings settings; - settings.dt = 0.01; - settings.ipmIteration = 100; - settings.projectStateInputEqualityConstraints = true; - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.nThreads = 1; - - settings.initialBarrierParameter = 1.0e-02; - settings.targetBarrierParameter = 1.0e-04; - settings.barrierLinearDecreaseFactor = 0.2; - settings.barrierSuperlinearDecreasePower = 1.5; - settings.fractionToBoundaryMargin = 0.995; + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.projectStateInputEqualityConstraints = true; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); const scalar_array_t initEventTimes{0.2262, 1.0176}; const size_array_t modeSequence{0, 1, 2}; @@ -182,12 +204,12 @@ TEST(Exp1Test, Constrained) { // add inequality constraints const scalar_t umin = -1.0; const scalar_t umax = 1.0; - std::unique_ptr<StateInputConstraint> stateInputIneqConstraint(new EXP1_StateInputIneqConstraints(umin, umax)); + auto stateInputIneqConstraint = std::make_unique<EXP1_StateInputIneqConstraints>(umin, umax); problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint)); const vector_t xmin = (vector_t(2) << -0.0, -0.0).finished(); const vector_t xmax = (vector_t(2) << 3.0, 4.0).finished(); - std::unique_ptr<StateConstraint> stateIneqConstraint(new EXP1_StateIneqConstraints(xmin, xmax)); - std::unique_ptr<StateConstraint> finalStateIneqConstraint(new EXP1_StateIneqConstraints(xmin, xmax)); + auto stateIneqConstraint = std::make_unique<EXP1_StateIneqConstraints>(xmin, xmax); + auto finalStateIneqConstraint = std::make_unique<EXP1_StateIneqConstraints>(xmin, xmax); problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint)); problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint)); @@ -197,34 +219,26 @@ TEST(Exp1Test, Constrained) { DefaultInitializer zeroInitializer(INPUT_DIM); - auto initializerPtr = std::unique_ptr<Initializer>(new DefaultInitializer(INPUT_DIM)); - // Solve IpmSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); solver.run(startTime, initState, finalTime); const auto primalSolution = solver.primalSolution(finalTime); - std::cout << "Optimal unconstrained trajectory" << std::endl; - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] - << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } // check constraint satisfaction for (const auto& e : primalSolution.stateTrajectory_) { if (e.size() > 0) { - EXPECT_TRUE(e.coeff(0) >= xmin.coeff(0)); - EXPECT_TRUE(e.coeff(1) >= xmin.coeff(1)); - EXPECT_TRUE(e.coeff(0) <= xmax.coeff(0)); - EXPECT_TRUE(e.coeff(1) <= xmax.coeff(1)); + ASSERT_TRUE(e.coeff(0) >= xmin.coeff(0)); + ASSERT_TRUE(e.coeff(1) >= xmin.coeff(1)); + ASSERT_TRUE(e.coeff(0) <= xmax.coeff(0)); + ASSERT_TRUE(e.coeff(1) <= xmax.coeff(1)); } } for (const auto& e : primalSolution.inputTrajectory_) { if (e.size() > 0) { - EXPECT_TRUE(e.coeff(0) >= umin); - EXPECT_TRUE(e.coeff(0) <= umax); + ASSERT_TRUE(e.coeff(0) >= umin); + ASSERT_TRUE(e.coeff(0) <= umax); } } } @@ -234,24 +248,26 @@ TEST(Exp1Test, MixedConstrained) { static constexpr size_t INPUT_DIM = 1; // Solver settings - ipm::Settings settings; - settings.dt = 0.01; - settings.ipmIteration = 100; - settings.projectStateInputEqualityConstraints = true; - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.nThreads = 1; - - settings.initialBarrierParameter = 1.0e-02; - settings.targetBarrierParameter = 1.0e-04; - settings.barrierLinearDecreaseFactor = 0.2; - settings.barrierSuperlinearDecreasePower = 1.5; - settings.fractionToBoundaryMargin = 0.995; + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.projectStateInputEqualityConstraints = true; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); const scalar_array_t initEventTimes{0.2262, 1.0176}; const size_array_t modeSequence{0, 1, 2}; @@ -261,7 +277,7 @@ TEST(Exp1Test, MixedConstrained) { // add inequality constraints const scalar_t xumin = -1.0; const scalar_t xumax = 1.0; - std::unique_ptr<StateInputConstraint> stateInputIneqConstraint(new EXP1_MixedStateInputIneqConstraints(xumin, xumax)); + auto stateInputIneqConstraint = std::make_unique<EXP1_MixedStateInputIneqConstraints>(xumin, xumax); auto stateInputIneqConstraintCloned = stateInputIneqConstraint->clone(); problem.inequalityConstraintPtr->add("bound", std::move(stateInputIneqConstraint)); const scalar_t startTime = 0.0; @@ -270,20 +286,12 @@ TEST(Exp1Test, MixedConstrained) { DefaultInitializer zeroInitializer(INPUT_DIM); - auto initializerPtr = std::unique_ptr<Initializer>(new DefaultInitializer(INPUT_DIM)); - // Solve IpmSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); solver.run(startTime, initState, finalTime); const auto primalSolution = solver.primalSolution(finalTime); - std::cout << "Optimal unconstrained trajectory" << std::endl; - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] - << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } // check constraint satisfaction const size_t N = primalSolution.inputTrajectory_.size(); @@ -292,6 +300,6 @@ TEST(Exp1Test, MixedConstrained) { const auto& x = primalSolution.stateTrajectory_[i]; const auto& u = primalSolution.inputTrajectory_[i]; const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, PreComputation()); - EXPECT_TRUE(constraintValue.minCoeff() >= 0.0); + ASSERT_TRUE(constraintValue.minCoeff() >= 0.0); } } diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp index 349ff1d11..9532f4f5f 100644 --- a/ocs2_ipm/test/testCircularKinematics.cpp +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -140,16 +140,26 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { ocs2::DefaultInitializer zeroInitializer(2); // Solver settings - ocs2::ipm::Settings settings; - settings.dt = 0.01; - settings.ipmIteration = 20; - settings.projectStateInputEqualityConstraints = true; - settings.computeLagrangeMultipliers = true; - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.nThreads = 1; + const auto settings = []() { + ocs2::ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.projectStateInputEqualityConstraints = true; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); // Additional problem definitions const ocs2::scalar_t startTime = 0.0; @@ -195,15 +205,26 @@ TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { ocs2::DefaultInitializer zeroInitializer(2); // Solver settings - ocs2::ipm::Settings settings; - settings.dt = 0.01; - settings.ipmIteration = 20; - settings.projectStateInputEqualityConstraints = false; // <- false to turn off projection of state-input equalities - settings.computeLagrangeMultipliers = true; - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; + const auto settings = []() { + ocs2::ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.projectStateInputEqualityConstraints = false; // <- false to turn off projection of state-input equalities + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); // Additional problem definitions const ocs2::scalar_t startTime = 0.0; @@ -248,12 +269,12 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { // inequality constraints const ocs2::vector_t umin = (ocs2::vector_t(2) << -0.5, -0.5).finished(); const ocs2::vector_t umax = (ocs2::vector_t(2) << 0.5, 0.5).finished(); - std::unique_ptr<ocs2::StateInputConstraint> stateInputIneqConstraint(new ocs2::CircleKinematics_StateInputIneqConstraints(umin, umax)); + auto stateInputIneqConstraint = std::make_unique<ocs2::CircleKinematics_StateInputIneqConstraints>(umin, umax); problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint)); const ocs2::vector_t xmin = (ocs2::vector_t(2) << -0.5, -0.5).finished(); const ocs2::vector_t xmax = (ocs2::vector_t(2) << 1.0e03, 1.0e03).finished(); // no upper bound - std::unique_ptr<ocs2::StateConstraint> stateIneqConstraint(new ocs2::CircleKinematics_StateIneqConstraints(xmin, xmax)); - std::unique_ptr<ocs2::StateConstraint> finalStateIneqConstraint(new ocs2::CircleKinematics_StateIneqConstraints(xmin, xmax)); + auto stateIneqConstraint = std::make_unique<ocs2::CircleKinematics_StateIneqConstraints>(xmin, xmax); + auto finalStateIneqConstraint = std::make_unique<ocs2::CircleKinematics_StateIneqConstraints>(xmin, xmax); problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint)); problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint)); @@ -261,22 +282,26 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { ocs2::DefaultInitializer zeroInitializer(2); // Solver settings - ocs2::ipm::Settings settings; - settings.dt = 0.01; - settings.ipmIteration = 40; - settings.projectStateInputEqualityConstraints = true; - settings.computeLagrangeMultipliers = true; - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.nThreads = 1; - - settings.initialBarrierParameter = 1.0e-02; - settings.targetBarrierParameter = 1.0e-04; - settings.barrierLinearDecreaseFactor = 0.2; - settings.barrierSuperlinearDecreasePower = 1.5; - settings.fractionToBoundaryMargin = 0.995; + const auto settings = []() { + ocs2::ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.projectStateInputEqualityConstraints = true; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); // Additional problem definitions const ocs2::scalar_t startTime = 0.0; @@ -306,18 +331,18 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { // check constraint satisfaction for (const auto& e : primalSolution.stateTrajectory_) { if (e.size() > 0) { - EXPECT_TRUE(e.coeff(0) >= xmin.coeff(0)); - EXPECT_TRUE(e.coeff(1) >= xmin.coeff(1)); - EXPECT_TRUE(e.coeff(0) <= xmax.coeff(0)); - EXPECT_TRUE(e.coeff(1) <= xmax.coeff(1)); + ASSERT_TRUE(e.coeff(0) >= xmin.coeff(0)); + ASSERT_TRUE(e.coeff(1) >= xmin.coeff(1)); + ASSERT_TRUE(e.coeff(0) <= xmax.coeff(0)); + ASSERT_TRUE(e.coeff(1) <= xmax.coeff(1)); } } for (const auto& e : primalSolution.inputTrajectory_) { if (e.size() > 0) { - EXPECT_TRUE(e.coeff(0) >= umin.coeff(0)); - EXPECT_TRUE(e.coeff(1) >= umin.coeff(1)); - EXPECT_TRUE(e.coeff(0) <= umax.coeff(0)); - EXPECT_TRUE(e.coeff(1) <= umax.coeff(1)); + ASSERT_TRUE(e.coeff(0) >= umin.coeff(0)); + ASSERT_TRUE(e.coeff(1) >= umin.coeff(1)); + ASSERT_TRUE(e.coeff(0) <= umax.coeff(0)); + ASSERT_TRUE(e.coeff(1) <= umax.coeff(1)); } } @@ -348,8 +373,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint // inequality constraints const ocs2::scalar_t xumin = -2.0; const ocs2::scalar_t xumax = 2.0; - std::unique_ptr<ocs2::StateInputConstraint> stateInputIneqConstraint( - new ocs2::CircleKinematics_MixedStateInputIneqConstraints(xumin, xumax)); + auto stateInputIneqConstraint = std::make_unique<ocs2::CircleKinematics_MixedStateInputIneqConstraints>(xumin, xumax); auto stateInputIneqConstraintCloned = stateInputIneqConstraint->clone(); problem.inequalityConstraintPtr->add("xubound", std::move(stateInputIneqConstraint)); @@ -357,22 +381,26 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint ocs2::DefaultInitializer zeroInitializer(2); // Solver settings - ocs2::ipm::Settings settings; - settings.dt = 0.01; - settings.ipmIteration = 100; - settings.projectStateInputEqualityConstraints = true; - settings.computeLagrangeMultipliers = true; - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - settings.nThreads = 1; - - settings.initialBarrierParameter = 1.0e-02; - settings.targetBarrierParameter = 1.0e-04; - settings.barrierLinearDecreaseFactor = 0.2; - settings.barrierSuperlinearDecreasePower = 1.5; - settings.fractionToBoundaryMargin = 0.995; + const auto settings = []() { + ocs2::ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.projectStateInputEqualityConstraints = true; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); // Additional problem definitions const ocs2::scalar_t startTime = 0.0; @@ -406,7 +434,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint const auto& x = primalSolution.stateTrajectory_[i]; const auto& u = primalSolution.inputTrajectory_[i]; const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, ocs2::PreComputation()); - EXPECT_TRUE(constraintValue.minCoeff() >= 0.0); + ASSERT_TRUE(constraintValue.minCoeff() >= 0.0); } // Check initial condition From 4f24fdac18222dc55ee683d5a1d29b4f2bf4073f Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 13:58:49 +0100 Subject: [PATCH 470/512] refactor Ipm --- ocs2_ipm/CMakeLists.txt | 1 - ocs2_ipm/include/ocs2_ipm/IpmInitialization.h | 10 +++-- ocs2_ipm/src/IpmInitialization.cpp | 45 ------------------- .../src/IpmPerformanceIndexComputation.cpp | 14 +++--- 4 files changed, 14 insertions(+), 56 deletions(-) delete mode 100644 ocs2_ipm/src/IpmInitialization.cpp diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index 5b95add44..d5e27aae7 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -49,7 +49,6 @@ include_directories( # Multiple shooting solver library add_library(${PROJECT_NAME} - src/IpmInitialization.cpp src/IpmHelpers.cpp src/IpmPerformanceIndexComputation.cpp src/IpmSettings.cpp diff --git a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h index c5c518cd9..7e318323c 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h @@ -43,7 +43,9 @@ namespace ipm { * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. * @return Initialized slack variable. */ -vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); +inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound); +} /** * Initializes the dual variable. The initialization of dual variables follows IPOPT option `bound_mult_init_method`: `mu-based` @@ -55,8 +57,10 @@ vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initia * @param initialDualMarginRate : Additional margin rate of the initial dual variables. * @return Initialized dual variable. */ -vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialSlackLowerBound, - scalar_t initialSlackMarginRate); +inline vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialDualLowerBound, + scalar_t initialDualMarginRate) { + return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound); +} } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp deleted file mode 100644 index 5c72292c1..000000000 --- a/ocs2_ipm/src/IpmInitialization.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#include "ocs2_ipm/IpmInitialization.h" - -namespace ocs2 { -namespace ipm { - -vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { - return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound); -} - -vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialDualLowerBound, - scalar_t initialDualMarginRate) { - return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound); -} - -} // namespace ipm -} // namespace ocs2 diff --git a/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp b/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp index 171d12d30..bccb25413 100644 --- a/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp +++ b/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp @@ -43,10 +43,10 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::Transcription& auto performance = multiple_shooting::computePerformanceIndex(transcription, dt); if (slackStateIneq.size() > 0) { - performance.cost += (-dt * barrierParam * slackStateIneq.array().log().sum()); + performance.cost -= dt * barrierParam * slackStateIneq.array().log().sum(); } if (slackStateInputIneq.size() > 0) { - performance.cost += (-dt * barrierParam * slackStateInputIneq.array().log().sum()); + performance.cost -= dt * barrierParam * slackStateInputIneq.array().log().sum(); } if (transcription.stateIneqConstraints.f.size() > 0) { @@ -64,7 +64,7 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::TerminalTransc auto performance = multiple_shooting::computePerformanceIndex(transcription); if (slackIneq.size() > 0) { - performance.cost += (-barrierParam * slackIneq.array().log().sum()); + performance.cost -= barrierParam * slackIneq.array().log().sum(); } if (transcription.ineqConstraints.f.size() > 0) { @@ -79,7 +79,7 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::EventTranscrip auto performance = multiple_shooting::computePerformanceIndex(transcription); if (slackIneq.size() > 0) { - performance.cost += (-barrierParam * slackIneq.array().log().sum()); + performance.cost -= barrierParam * slackIneq.array().log().sum(); } if (transcription.ineqConstraints.f.size() > 0) { @@ -114,12 +114,12 @@ PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_ PerformanceIndex performance = toPerformanceIndex(metrics, dt); if (slackStateIneq.size() > 0) { - performance.cost += (-dt * barrierParam * slackStateIneq.array().log().sum()); + performance.cost -= dt * barrierParam * slackStateIneq.array().log().sum(); performance.equalityConstraintsSSE += dt * (toVector(metrics.stateIneqConstraint) - slackStateIneq).squaredNorm(); } if (slackStateInputIneq.size() > 0 && enableStateInequalityConstraints) { - performance.cost += (-dt * barrierParam * slackStateInputIneq.array().log().sum()); + performance.cost -= dt * barrierParam * slackStateInputIneq.array().log().sum(); performance.equalityConstraintsSSE += dt * (toVector(metrics.stateInputIneqConstraint) - slackStateInputIneq).squaredNorm(); } @@ -130,7 +130,7 @@ PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t barrierPara PerformanceIndex performance = toPerformanceIndex(metrics); if (slackIneq.size() > 0) { - performance.cost += (-barrierParam * slackIneq.array().log().sum()); + performance.cost -= barrierParam * slackIneq.array().log().sum(); performance.equalityConstraintsSSE += (toVector(metrics.stateIneqConstraint) - slackIneq).squaredNorm(); } From ec559e46ed5d14ffc211c0fad5f5c39d44f7ad77 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 13:59:14 +0100 Subject: [PATCH 471/512] refactor Ipm --- ocs2_ipm/src/IpmSolver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index f887874a3..1d2e18296 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -203,9 +203,8 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f } // Interior point variables - vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; - scalar_t barrierParam = settings_.initialBarrierParameter; + vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; // Bookkeeping performanceIndeces_.clear(); From 3825614e7b1fae4f38b6ac2a56fd91194466c39e Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 14:20:41 +0100 Subject: [PATCH 472/512] refactore Ipm --- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 9 +++--- ocs2_ipm/src/IpmSolver.cpp | 42 +++++++++++++-------------- 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 00265d668..13a5e066d 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -139,9 +139,10 @@ class IpmSolver : public SolverBase { /** Returns solution of the QP subproblem in delta coordinates: */ struct OcpSubproblemSolution { - vector_array_t deltaXSol; // delta_x(t) - vector_array_t deltaUSol; // delta_u(t) - vector_array_t deltaNuSol; // delta_nu(t) + vector_array_t deltaXSol; // delta_x(t) + vector_array_t deltaUSol; // delta_u(t) + vector_array_t deltaLmdSol; // delta_lmd(t) + vector_array_t deltaNuSol; // delta_nu(t) vector_array_t deltaSlackStateIneq; vector_array_t deltaSlackStateInputIneq; vector_array_t deltaDualStateIneq; @@ -156,7 +157,7 @@ class IpmSolver : public SolverBase { /** Extract the value function based on the last solved QP */ void extractValueFunction(const std::vector<AnnotatedTime>& time, const vector_array_t& x, const vector_array_t& lmd, - const vector_array_t& deltaXSol, vector_array_t& deltaLmdSol); + const vector_array_t& deltaXSol); /** Constructs the primal solution based on the optimized state and input trajectories */ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 1d2e18296..8d4907aa8 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -230,8 +230,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f const vector_t delta_x0 = initState - x[0]; const auto deltaSolution = getOCPSolution(delta_x0, barrierParam, slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq); - vector_array_t deltaLmdSol; - extractValueFunction(timeDiscretization, x, lmd, deltaSolution.deltaXSol, deltaLmdSol); + extractValueFunction(timeDiscretization, x, lmd, deltaSolution.deltaXSol); solveQpTimer_.endTimer(); // Apply step @@ -243,23 +242,16 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f slackStateInputIneq, metrics); const scalar_t dualStepSize = settings_.usePrimalStepSizeForDual ? std::min(stepInfo.stepSize, deltaSolution.maxDualStepSize) : deltaSolution.maxDualStepSize; - performanceIndeces_.push_back(stepInfo.performanceAfterStep); if (settings_.computeLagrangeMultipliers) { - multiple_shooting::incrementTrajectory(lmd, deltaLmdSol, stepInfo.stepSize, lmd); + multiple_shooting::incrementTrajectory(lmd, deltaSolution.deltaLmdSol, stepInfo.stepSize, lmd); if (settings_.projectStateInputEqualityConstraints) { - // Remaining term that depends on the costate - auto deltaNuSol = std::move(deltaSolution.deltaNuSol); - for (int i = 0; i < nu.size(); i++) { - if (nu[i].size() > 0) { - deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdcostate * deltaLmdSol[i + 1]; - } - } - multiple_shooting::incrementTrajectory(nu, deltaNuSol, stepInfo.stepSize, nu); + multiple_shooting::incrementTrajectory(nu, deltaSolution.deltaNuSol, stepInfo.stepSize, nu); } } multiple_shooting::incrementTrajectory(dualStateIneq, deltaSolution.deltaDualStateIneq, stepInfo.stepSize, dualStateIneq); multiple_shooting::incrementTrajectory(dualStateInputIneq, deltaSolution.deltaDualStateInputIneq, stepInfo.stepSize, dualStateInputIneq); + performanceIndeces_.push_back(stepInfo.performanceAfterStep); linesearchTimer_.endTimer(); // Check convergence @@ -393,6 +385,20 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] solution.armijoDescentMetric = armijoDescentMetric(lagrangian_, deltaXSol, deltaUSol); + // Extract value function + auto& deltaLmdSol = solution.deltaLmdSol; + if (settings_.createValueFunction) { + valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], lagrangian_[0]); + // Extract costate directions + if (settings_.computeLagrangeMultipliers) { + deltaLmdSol.resize(deltaXSol.size()); + for (int i = 0; i < deltaXSol.size(); ++i) { + deltaLmdSol[i] = valueFunction_[i].dfdx; + deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * deltaXSol[i]; + } + } + } + // Problem horizon const int N = static_cast<int>(deltaXSol.size()) - 1; @@ -436,6 +442,7 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta deltaNuSol[i] = projectionMultiplierCoefficients_[i].f; deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdx * deltaXSol[i]; deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdu * deltaUSol[i]; + deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdcostate * deltaLmdSol[i + 1]; } // Re-map the projected input back to the original space. tmp.noalias() = constraintsProjection_[i].dfdu * deltaUSol[i]; @@ -465,16 +472,9 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta } void IpmSolver::extractValueFunction(const std::vector<AnnotatedTime>& time, const vector_array_t& x, const vector_array_t& lmd, - const vector_array_t& deltaXSol, vector_array_t& deltaLmdSol) { + const vector_array_t& deltaXSol) { if (settings_.createValueFunction) { - valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], lagrangian_[0]); - // Compute costate directions - deltaLmdSol.resize(deltaXSol.size()); - for (int i = 0; i < time.size(); ++i) { - deltaLmdSol[i] = valueFunction_[i].dfdx; - deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * x[i]; - } - // Correct for linearization state + // Correct for linearization state. Naive value function of hpipm is already extracted and stored in valueFunction_ in getOCPSolution(). for (int i = 0; i < time.size(); ++i) { valueFunction_[i].dfdx.noalias() -= valueFunction_[i].dfdxx * x[i]; if (settings_.computeLagrangeMultipliers) { From 1f18f07c4b339e66035cee3ea396b1f40a3d5421 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 14:26:08 +0100 Subject: [PATCH 473/512] fix task.info --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 2 -- 1 file changed, 2 deletions(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index 970f37fc8..1edc2801f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -55,8 +55,6 @@ ipm deltaTol 1e-4 g_max 10.0 g_min 1e-4 - inequalityConstraintMu 0.1 - inequalityConstraintDelta 5.0 projectStateInputEqualityConstraints true computeLagrangeMultipliers true printSolverStatistics true From da5f8b397e0d40e54a38905d938a594a2dacb3f1 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 14:49:52 +0100 Subject: [PATCH 474/512] reorder metrics computation --- ocs2_ipm/src/IpmSolver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 8d4907aa8..a2a12717d 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -535,13 +535,13 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); + metrics[i] = multiple_shooting::computeMetrics(result); if (initializeSlackAndDualVariables) { slackStateIneq[i] = ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } - metrics[i] = multiple_shooting::computeMetrics(result); performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]); dynamics_[i] = std::move(result.dynamics); stateInputEqConstraints_[i].resize(0, x[i].size()); @@ -564,6 +564,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + metrics[i] = multiple_shooting::computeMetrics(result); // Disable the state-only inequality constraints at the initial node if (i == 0) { result.stateIneqConstraints.setZero(0, x[i].size()); @@ -579,7 +580,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } - metrics[i] = multiple_shooting::computeMetrics(result); performance[workerId] += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); if (settings_.projectStateInputEqualityConstraints) { multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers); @@ -613,13 +613,13 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); + metrics[i] = multiple_shooting::computeMetrics(result); if (initializeSlackAndDualVariables) { slackStateIneq[N] = ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[N] = ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } - metrics[i] = multiple_shooting::computeMetrics(result); performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]); stateInputEqConstraints_[i].resize(0, x[i].size()); stateIneqConstraints_[i] = std::move(result.ineqConstraints); From a5446cbacb488a951c1d509c1434185be22710ef Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 15:44:50 +0100 Subject: [PATCH 475/512] parallelize the costate direction computation --- ocs2_ipm/src/IpmSolver.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index a2a12717d..9cbdbefb4 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -386,27 +386,20 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta solution.armijoDescentMetric = armijoDescentMetric(lagrangian_, deltaXSol, deltaUSol); // Extract value function - auto& deltaLmdSol = solution.deltaLmdSol; if (settings_.createValueFunction) { valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], lagrangian_[0]); - // Extract costate directions - if (settings_.computeLagrangeMultipliers) { - deltaLmdSol.resize(deltaXSol.size()); - for (int i = 0; i < deltaXSol.size(); ++i) { - deltaLmdSol[i] = valueFunction_[i].dfdx; - deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * deltaXSol[i]; - } - } } // Problem horizon const int N = static_cast<int>(deltaXSol.size()) - 1; + auto& deltaLmdSol = solution.deltaLmdSol; auto& deltaNuSol = solution.deltaNuSol; auto& deltaSlackStateIneq = solution.deltaSlackStateIneq; auto& deltaSlackStateInputIneq = solution.deltaSlackStateInputIneq; auto& deltaDualStateIneq = solution.deltaDualStateIneq; auto& deltaDualStateInputIneq = solution.deltaDualStateInputIneq; + deltaLmdSol.resize(N + 1); deltaNuSol.resize(N); deltaSlackStateIneq.resize(N + 1); deltaSlackStateInputIneq.resize(N + 1); @@ -437,7 +430,14 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta {dualStepSizes[workerId], ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin), ipm::fractionToBoundaryStepSize(dualStateInputIneq[i], deltaDualStateInputIneq[i], settings_.fractionToBoundaryMargin)}); + + // Extract Newton directions of the costate + if (settings_.computeLagrangeMultipliers) { + deltaLmdSol[i] = valueFunction_[i].dfdx; + deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * deltaXSol[i]; + } if (constraintsProjection_[i].f.size() > 0) { + // Extract Newton directions of the Lagrange multiplier associated with the state-input equality constraints if (settings_.computeLagrangeMultipliers) { deltaNuSol[i] = projectionMultiplierCoefficients_[i].f; deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdx * deltaXSol[i]; @@ -461,6 +461,11 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin)); dualStepSizes[workerId] = std::min(dualStepSizes[workerId], ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin)); + // Extract Newton directions of the costate + if (settings_.computeLagrangeMultipliers) { + deltaLmdSol[i] = valueFunction_[i].dfdx; + deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * deltaXSol[i]; + } } }; runParallel(std::move(parallelTask)); From 895e2c3fecdbeed1e125cd459e36a890813e5e55 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 15:51:11 +0100 Subject: [PATCH 476/512] remove projectStateInputEqualityConstraints option --- ocs2_ipm/include/ocs2_ipm/IpmSettings.h | 3 -- ocs2_ipm/src/IpmSettings.cpp | 1 - ocs2_ipm/src/IpmSolver.cpp | 26 ++------- ocs2_ipm/test/Exp0Test.cpp | 2 - ocs2_ipm/test/Exp1Test.cpp | 3 -- ocs2_ipm/test/testCircularKinematics.cpp | 68 ------------------------ ocs2_ipm/test/testSwitchedProblem.cpp | 1 - ocs2_ipm/test/testUnconstrained.cpp | 1 - ocs2_ipm/test/testValuefunction.cpp | 1 - 9 files changed, 5 insertions(+), 101 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h index 11d32f8e9..d9e865c5b 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h @@ -65,9 +65,6 @@ struct Settings { scalar_t dt = 0.01; // user-defined time discretization SensitivityIntegratorType integratorType = SensitivityIntegratorType::RK2; - // Equality constraints - bool projectStateInputEqualityConstraints = true; // Use a projection method to resolve the state-input constraint Cx+Du+e - // Barrier strategy of the primal-dual interior point method. Conventions follows Ipopt. scalar_t initialBarrierParameter = 1.0e-02; // Initial value of the barrier parameter scalar_t targetBarrierParameter = 1.0e-04; // Targer value of the barrier parameter. The barreir will decrease until reaches this value. diff --git a/ocs2_ipm/src/IpmSettings.cpp b/ocs2_ipm/src/IpmSettings.cpp index d2447f77b..f99636ea0 100644 --- a/ocs2_ipm/src/IpmSettings.cpp +++ b/ocs2_ipm/src/IpmSettings.cpp @@ -74,7 +74,6 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, loadData::loadPtreeValue(pt, settings.barrierSuperlinearDecreasePower, fieldName + ".barrierSuperlinearDecreasePower", verbose); loadData::loadPtreeValue(pt, settings.fractionToBoundaryMargin, fieldName + ".fractionToBoundaryMargin", verbose); loadData::loadPtreeValue(pt, settings.usePrimalStepSizeForDual, fieldName + ".usePrimalStepSizeForDual", verbose); - loadData::loadPtreeValue(pt, settings.projectStateInputEqualityConstraints, fieldName + ".projectStateInputEqualityConstraints", verbose); loadData::loadPtreeValue(pt, settings.initialSlackLowerBound, fieldName + ".initialSlackLowerBound", verbose); loadData::loadPtreeValue(pt, settings.initialDualLowerBound, fieldName + ".initialDualLowerBound", verbose); loadData::loadPtreeValue(pt, settings.initialSlackMarginRate, fieldName + ".initialSlackMarginRate", verbose); diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 9cbdbefb4..b37818a13 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -54,10 +54,6 @@ ipm::Settings rectifySettings(const OptimalControlProblem& ocp, ipm::Settings&& if (settings.computeLagrangeMultipliers) { settings.createValueFunction = true; } - // True does not make sense if there are no constraints. - if (ocp.equalityConstraintPtr->empty()) { - settings.projectStateInputEqualityConstraints = false; - } // Turn off the barrier update strategy if there are no inequality constraints. if (ocp.inequalityConstraintPtr->empty() && ocp.stateInequalityConstraintPtr->empty() && ocp.preJumpInequalityConstraintPtr->empty() && ocp.finalInequalityConstraintPtr->empty()) { @@ -244,9 +240,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f settings_.usePrimalStepSizeForDual ? std::min(stepInfo.stepSize, deltaSolution.maxDualStepSize) : deltaSolution.maxDualStepSize; if (settings_.computeLagrangeMultipliers) { multiple_shooting::incrementTrajectory(lmd, deltaSolution.deltaLmdSol, stepInfo.stepSize, lmd); - if (settings_.projectStateInputEqualityConstraints) { - multiple_shooting::incrementTrajectory(nu, deltaSolution.deltaNuSol, stepInfo.stepSize, nu); - } + multiple_shooting::incrementTrajectory(nu, deltaSolution.deltaNuSol, stepInfo.stepSize, nu); } multiple_shooting::incrementTrajectory(dualStateIneq, deltaSolution.deltaDualStateIneq, stepInfo.stepSize, dualStateIneq); multiple_shooting::incrementTrajectory(dualStateInputIneq, deltaSolution.deltaDualStateInputIneq, stepInfo.stepSize, @@ -369,14 +363,8 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta auto& deltaXSol = solution.deltaXSol; auto& deltaUSol = solution.deltaUSol; hpipm_status status; - if (!settings_.projectStateInputEqualityConstraints) { - hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, &stateInputEqConstraints_)); - status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, &stateInputEqConstraints_, deltaXSol, deltaUSol, - settings_.printSolverStatus); - } else { // without constraints, or when using projection, we have an unconstrained QP. - hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, nullptr)); - status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); - } + hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, nullptr)); + status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); if (status != hpipm_status::SUCCESS) { throw std::runtime_error("[IpmSolver] Failed to solve QP"); @@ -493,9 +481,7 @@ PrimalSolution IpmSolver::toPrimalSolution(const std::vector<AnnotatedTime>& tim if (settings_.useFeedbackPolicy) { ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); matrix_array_t KMatrices = hpipmInterface_.getRiccatiFeedback(dynamics_[0], lagrangian_[0]); - if (settings_.projectStateInputEqualityConstraints) { - multiple_shooting::remapProjectedGain(constraintsProjection_, KMatrices); - } + multiple_shooting::remapProjectedGain(constraintsProjection_, KMatrices); return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), std::move(KMatrices)); } else { @@ -586,9 +572,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated settings_.initialDualMarginRate); } performance[workerId] += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); - if (settings_.projectStateInputEqualityConstraints) { - multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers); - } + multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers); dynamics_[i] = std::move(result.dynamics); stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp index 1995ef5d6..de974443b 100644 --- a/ocs2_ipm/test/Exp0Test.cpp +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -107,7 +107,6 @@ TEST(Exp0Test, Unconstrained) { ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; - s.projectStateInputEqualityConstraints = true; s.useFeedbackPolicy = true; s.printSolverStatistics = true; s.printSolverStatus = true; @@ -150,7 +149,6 @@ TEST(Exp0Test, Constrained) { ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; - s.projectStateInputEqualityConstraints = true; s.useFeedbackPolicy = true; s.printSolverStatistics = true; s.printSolverStatus = true; diff --git a/ocs2_ipm/test/Exp1Test.cpp b/ocs2_ipm/test/Exp1Test.cpp index 11651229f..fc04f9dde 100644 --- a/ocs2_ipm/test/Exp1Test.cpp +++ b/ocs2_ipm/test/Exp1Test.cpp @@ -136,7 +136,6 @@ TEST(Exp1Test, Unconstrained) { ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; - s.projectStateInputEqualityConstraints = true; s.useFeedbackPolicy = true; s.printSolverStatistics = true; s.printSolverStatus = true; @@ -179,7 +178,6 @@ TEST(Exp1Test, Constrained) { ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; - s.projectStateInputEqualityConstraints = true; s.useFeedbackPolicy = true; s.printSolverStatistics = true; s.printSolverStatus = true; @@ -252,7 +250,6 @@ TEST(Exp1Test, MixedConstrained) { ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; - s.projectStateInputEqualityConstraints = true; s.useFeedbackPolicy = true; s.printSolverStatistics = true; s.printSolverStatus = true; diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp index 9532f4f5f..fb5ab41ae 100644 --- a/ocs2_ipm/test/testCircularKinematics.cpp +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -144,72 +144,6 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { ocs2::ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; - s.projectStateInputEqualityConstraints = true; - s.useFeedbackPolicy = true; - s.printSolverStatistics = true; - s.printSolverStatus = true; - s.printLinesearch = true; - s.printSolverStatistics = true; - s.printSolverStatus = true; - s.printLinesearch = true; - s.nThreads = 1; - s.initialBarrierParameter = 1.0e-02; - s.targetBarrierParameter = 1.0e-04; - s.barrierLinearDecreaseFactor = 0.2; - s.barrierSuperlinearDecreasePower = 1.5; - s.fractionToBoundaryMargin = 0.995; - return s; - }(); - - // Additional problem definitions - const ocs2::scalar_t startTime = 0.0; - const ocs2::scalar_t finalTime = 1.0; - const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 - - // Solve - ocs2::IpmSolver solver(settings, problem, zeroInitializer); - solver.run(startTime, initState, finalTime); - - // Inspect solution - const auto primalSolution = solver.primalSolution(finalTime); - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << primalSolution.timeTrajectory_[i] << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } - - // Check initial condition - ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); - ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime); - ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime); - - // Check constraint satisfaction. - const auto performance = solver.getPerformanceIndeces(); - ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); - ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); - - // Check feedback controller - for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { - const auto t = primalSolution.timeTrajectory_[i]; - const auto& x = primalSolution.stateTrajectory_[i]; - const auto& u = primalSolution.inputTrajectory_[i]; - // Feed forward part - ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); - } -} - -TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { - // optimal control problem - ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ipm_test_generated"); - - // Initializer - ocs2::DefaultInitializer zeroInitializer(2); - - // Solver settings - const auto settings = []() { - ocs2::ipm::Settings s; - s.dt = 0.01; - s.ipmIteration = 20; - s.projectStateInputEqualityConstraints = false; // <- false to turn off projection of state-input equalities s.useFeedbackPolicy = true; s.printSolverStatistics = true; s.printSolverStatus = true; @@ -286,7 +220,6 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { ocs2::ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; - s.projectStateInputEqualityConstraints = true; s.useFeedbackPolicy = true; s.printSolverStatistics = true; s.printSolverStatus = true; @@ -385,7 +318,6 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint ocs2::ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; - s.projectStateInputEqualityConstraints = true; s.useFeedbackPolicy = true; s.printSolverStatistics = true; s.printSolverStatus = true; diff --git a/ocs2_ipm/test/testSwitchedProblem.cpp b/ocs2_ipm/test/testSwitchedProblem.cpp index ebcb42f80..dbc0f0813 100644 --- a/ocs2_ipm/test/testSwitchedProblem.cpp +++ b/ocs2_ipm/test/testSwitchedProblem.cpp @@ -129,7 +129,6 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithEventTime(scal ocs2::ipm::Settings settings; settings.dt = 0.05; settings.ipmIteration = 20; - settings.projectStateInputEqualityConstraints = true; settings.printSolverStatistics = true; settings.printSolverStatus = true; settings.printLinesearch = true; diff --git a/ocs2_ipm/test/testUnconstrained.cpp b/ocs2_ipm/test/testUnconstrained.cpp index 8485a31ce..517252735 100644 --- a/ocs2_ipm/test/testUnconstrained.cpp +++ b/ocs2_ipm/test/testUnconstrained.cpp @@ -70,7 +70,6 @@ std::pair<PrimalSolution, std::vector<PerformanceIndex>> solveWithFeedbackSettin ocs2::ipm::Settings settings; settings.dt = 0.05; settings.ipmIteration = 10; - settings.projectStateInputEqualityConstraints = true; settings.useFeedbackPolicy = feedback; settings.printSolverStatistics = true; settings.printSolverStatus = true; diff --git a/ocs2_ipm/test/testValuefunction.cpp b/ocs2_ipm/test/testValuefunction.cpp index 15812c129..e121e6505 100644 --- a/ocs2_ipm/test/testValuefunction.cpp +++ b/ocs2_ipm/test/testValuefunction.cpp @@ -75,7 +75,6 @@ TEST(test_valuefunction, linear_quadratic_problem) { ocs2::ipm::Settings settings; settings.dt = 0.05; settings.ipmIteration = 1; - settings.projectStateInputEqualityConstraints = true; settings.printSolverStatistics = false; settings.printSolverStatus = false; settings.printLinesearch = false; From 502c3426727feeb1128e116b84c670bb9278e33f Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 16:05:56 +0100 Subject: [PATCH 477/512] fix costate parallel computation --- ocs2_ipm/src/IpmSolver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index b37818a13..5334d4c04 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -421,8 +421,8 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta // Extract Newton directions of the costate if (settings_.computeLagrangeMultipliers) { - deltaLmdSol[i] = valueFunction_[i].dfdx; - deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * deltaXSol[i]; + deltaLmdSol[i + 1] = valueFunction_[i + 1].dfdx; + deltaLmdSol[i + 1].noalias() += valueFunction_[i + 1].dfdxx * deltaXSol[i + 1]; } if (constraintsProjection_[i].f.size() > 0) { // Extract Newton directions of the Lagrange multiplier associated with the state-input equality constraints @@ -451,8 +451,8 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta settings_.fractionToBoundaryMargin)); // Extract Newton directions of the costate if (settings_.computeLagrangeMultipliers) { - deltaLmdSol[i] = valueFunction_[i].dfdx; - deltaLmdSol[i].noalias() += valueFunction_[i].dfdxx * deltaXSol[i]; + deltaLmdSol[0] = valueFunction_[0].dfdx; + deltaLmdSol[0].noalias() += valueFunction_[i].dfdxx * deltaXSol[0]; } } }; From 17fdad8569bc4c6ebe0c9b6f578d851727121c33 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 16:09:01 +0100 Subject: [PATCH 478/512] remove projectStateInputEqualityConstraints --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 1 - 1 file changed, 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index 1edc2801f..2a130e969 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -55,7 +55,6 @@ ipm deltaTol 1e-4 g_max 10.0 g_min 1e-4 - projectStateInputEqualityConstraints true computeLagrangeMultipliers true printSolverStatistics true printSolverStatus false From 5c587761f30a81c39927c60e337de83180fec238 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Thu, 15 Dec 2022 17:14:58 +0100 Subject: [PATCH 479/512] fix bugs --- ocs2_ipm/CMakeLists.txt | 2 +- .../ocs2_ipm/IpmTrajectoryAdjustment.h | 21 ++++++++++--------- ocs2_ipm/src/IpmSolver.cpp | 4 ++-- ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 12 +++++------ .../ocs2_oc/oc_data/TimeDiscretization.h | 8 +++++++ ocs2_oc/src/oc_data/TimeDiscretization.cpp | 13 ++++++++++++ 6 files changed, 41 insertions(+), 19 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index beec07e7f..655a7a8ac 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -74,7 +74,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h index a44b93f71..cea8a41ae 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h @@ -57,18 +57,19 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution); /** - * Initializes the interior point trajectory based on the stored trajectory. The part of the new trajectory that is uncovered by the stored - * trajectory is set by zero-sized vectors. + * Interpolates the interior point trajectory using the stored interior point trajectory. The part of the new trajectory that is not + * covered by the stored trajectory is set by vectors of zero size. * - * @param time: The time discretization. - * @param stateIneq: The slack/dual of the state inequality constraints. - * @param stateInputIneq: The slack/dual of the state-input inequality constraints. - * @return A dual solution. + * @param oldModeSchedule: Mode schedule of the previous optimization. + * @param newModeSchedule: Mode schedule of the new optimization. + * @param time: Time discretization. + * @param oldDualSolution: The old dual solution that stored the previous interior point trajectory. + * @return Interpolated interior point trajectory. */ -std::pair<vector_array_t, vector_array_t> initializeInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, - const std::vector<AnnotatedTime>& time, - DualSolution&& oldDualSolution); +std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector<AnnotatedTime>& time, + DualSolution&& oldDualSolution); } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 5365e9266..c8cad176e 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -200,9 +200,9 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f // Interior point variables scalar_t barrierParam = settings_.initialBarrierParameter; vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; - std::tie(slackStateIneq, slackStateInputIneq) = ipm::initializeInteriorPointTrajectory( + std::tie(slackStateIneq, slackStateInputIneq) = ipm::interpolateInteriorPointTrajectory( primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(slackIneqTrajectory_)); - std::tie(dualStateIneq, dualStateInputIneq) = ipm::initializeInteriorPointTrajectory( + std::tie(dualStateIneq, dualStateInputIneq) = ipm::interpolateInteriorPointTrajectory( primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(dualIneqTrajectory_)); // Bookkeeping diff --git a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp index eaf2645cc..7225965e5 100644 --- a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp +++ b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp @@ -63,7 +63,7 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array const int N = static_cast<int>(time.size()) - 1; DualSolution dualSolution; - dualSolution.timeTrajectory = toTime(time); + const auto newTimeTrajectory = toInterpolationTime(time); dualSolution.postEventIndices = toPostEventIndices(time); dualSolution.preJumps.reserve(dualSolution.postEventIndices.size()); @@ -113,10 +113,10 @@ std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<Ann return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); } -std::pair<vector_array_t, vector_array_t> initializeInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, - const std::vector<AnnotatedTime>& time, - DualSolution&& oldDualSolution) { +std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector<AnnotatedTime>& time, + DualSolution&& oldDualSolution) { const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; const auto oldPostEventIndices = oldDualSolution.postEventIndices; @@ -124,7 +124,7 @@ std::pair<vector_array_t, vector_array_t> initializeInteriorPointTrajectory(cons std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, oldDualSolution); } - const auto newTimeTrajectory = toTime(time); + const auto newTimeTrajectory = toInterpolationTime(time); const auto newPostEventIndices = toPostEventIndices(time); // find the time period that we can interpolate the cached solution diff --git a/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h b/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h index 304cc3daf..dc0a70422 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h @@ -84,6 +84,14 @@ std::vector<AnnotatedTime> timeDiscretizationWithEvents(scalar_t initTime, scala */ scalar_array_t toTime(const std::vector<AnnotatedTime>& annotatedTime); +/** + * Extracts the time trajectory from the annotated time trajectory respecting interpolation rules around event times. + * + * @param annotatedTime : Annotated time trajectory. + * @return The time trajectory. + */ +scalar_array_t toInterpolationTime(const std::vector<AnnotatedTime>& annotatedTime); + /** * Extracts the array of indices indicating the post-event times from the annotated time trajectory. * diff --git a/ocs2_oc/src/oc_data/TimeDiscretization.cpp b/ocs2_oc/src/oc_data/TimeDiscretization.cpp index d4bbbb182..466f68b48 100644 --- a/ocs2_oc/src/oc_data/TimeDiscretization.cpp +++ b/ocs2_oc/src/oc_data/TimeDiscretization.cpp @@ -122,6 +122,19 @@ scalar_array_t toTime(const std::vector<AnnotatedTime>& annotatedTime) { return timeTrajectory; } +scalar_array_t toInterpolationTime(const std::vector<AnnotatedTime>& annotatedTime) { + scalar_array_t timeTrajectory; + timeTrajectory.reserve(annotatedTime.size()); + for (size_t i = 0; i < annotatedTime.size(); i++) { + if (annotatedTime[i].event == AnnotatedTime::Event::PostEvent) { + timeTrajectory.push_back(getInterpolationTime(annotatedTime[i])); + } else { + timeTrajectory.push_back(annotatedTime[i].time); + } + } + return timeTrajectory; +} + size_array_t toPostEventIndices(const std::vector<AnnotatedTime>& annotatedTime) { size_array_t postEventIndices; for (size_t i = 0; i < annotatedTime.size(); i++) { From e1759543f7f643460f5dbbc9cf0fc08b9d8e04ea Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Fri, 16 Dec 2022 11:32:12 +0100 Subject: [PATCH 480/512] add doc and getStateInputEqualityConstraintLagrangian --- ocs2_ipm/include/ocs2_ipm/IpmSettings.h | 3 ++- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 4 +--- ocs2_ipm/src/IpmSolver.cpp | 19 +++++++++++++++++++ ocs2_ipm/test/testCircularKinematics.cpp | 10 ++++++++++ 4 files changed, 32 insertions(+), 4 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h index d9e865c5b..6a917cf50 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h @@ -56,7 +56,8 @@ struct Settings { // controller type bool useFeedbackPolicy = true; // true to use feedback, false to use feedforward bool createValueFunction = false; // true to store the value function, false to ignore it - bool computeLagrangeMultipliers = false; // true to compute the Lagrange multipliers + bool computeLagrangeMultipliers = false; // If set to true to compute the Lagrange multipliers. If set to false the dualFeasibilitiesSSE + // in the PerformanceIndex log is incorrect but it will not affect algorithm correctness. // QP subproblem solver settings hpipm_interface::Settings hpipmSettings = hpipm_interface::Settings(); diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 13a5e066d..8e12650f0 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -82,9 +82,7 @@ class IpmSolver : public SolverBase { throw std::runtime_error("[IpmSolver] getHamiltonian() not available yet."); } - vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override { - throw std::runtime_error("[IpmSolver] getStateInputEqualityConstraintLagrangian() not available yet."); - } + vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override; MultiplierCollection getIntermediateDualSolution(scalar_t time) const override { throw std::runtime_error("[IpmSolver] getIntermediateDualSolution() not available yet."); diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 5334d4c04..998f8ace3 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -170,6 +170,25 @@ ScalarFunctionQuadraticApproximation IpmSolver::getValueFunction(scalar_t time, } } +vector_t IpmSolver::getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const { + if (settings_.computeLagrangeMultipliers && !projectionMultiplierTrajectory_.empty()) { + // Interpolation + using LinearInterpolation::interpolate; + const auto nominalMultiplier = interpolate(time, primalSolution_.timeTrajectory_, projectionMultiplierTrajectory_); + const auto nominalState = interpolate(time, primalSolution_.timeTrajectory_, primalSolution_.stateTrajectory_); + + const auto indexAlpha = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_); + using T = std::vector<multiple_shooting::ProjectionMultiplierCoefficients>; + const auto sensitivityWrtState = + interpolate(indexAlpha, projectionMultiplierCoefficients_, [](const T& v, size_t ind) -> const matrix_t& { return v[ind].dfdx; }); + + return nominalMultiplier + sensitivityWrtState * (state - nominalState); + + } else { + throw std::runtime_error("[IpmSolver] getStateInputEqualityConstraintLagrangian() not available yet."); + } +} + void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp index fb5ab41ae..9d484d3b0 100644 --- a/ocs2_ipm/test/testCircularKinematics.cpp +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -319,6 +319,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint s.dt = 0.01; s.ipmIteration = 20; s.useFeedbackPolicy = true; + s.computeLagrangeMultipliers = true; s.printSolverStatistics = true; s.printSolverStatus = true; s.printLinesearch = true; @@ -387,4 +388,13 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint // Feed forward part ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); } + + // Check Lagrange multipliers + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + std::cerr << "i: " << i << std::endl; + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + ASSERT_NO_THROW(const auto multiplier = solver.getStateInputEqualityConstraintLagrangian(t, x);); + } } \ No newline at end of file From b390927c418c1944e0b0c451451e417b499f539f Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Fri, 16 Dec 2022 14:07:55 +0100 Subject: [PATCH 481/512] refactor IpmSolver --- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 11 +++- ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h | 4 +- ocs2_ipm/src/IpmSolver.cpp | 66 +++++++++++---------- 3 files changed, 45 insertions(+), 36 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 8e12650f0..efcfad64f 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -161,9 +161,14 @@ class IpmSolver : public SolverBase { PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ - ipm::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, - const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u, scalar_t barrierParam, - vector_array_t& slackStateIneq, vector_array_t& slackStateInputIneq, std::vector<Metrics>& metrics); + ipm::StepInfo takePrimalStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& slackStateInputIneq, std::vector<Metrics>& metrics); + + /** Updates the Lagrange multipliers */ + void takeDualStep(const OcpSubproblemSolution& subproblemSolution, const ipm::StepInfo& stepInfo, vector_array_t& lmd, vector_array_t& nu, + vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq) const; /** Updates the barrier parameter */ scalar_t updateBarrierParameter(scalar_t currentBarrierParameter, const PerformanceIndex& baseline, const ipm::StepInfo& stepInfo) const; diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h b/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h index d64412d37..42a5f8a47 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h @@ -43,8 +43,8 @@ enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; /** Struct to contain the result and logging data of the stepsize computation */ struct StepInfo { - // Step size and type - scalar_t stepSize = 0.0; + // Step sizes and type + scalar_t primalStepSize = 0.0; scalar_t dualStepSize = 0.0; FilterLinesearch::StepType stepType = FilterLinesearch::StepType::UNKNOWN; diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 998f8ace3..e181137ee 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -172,17 +172,17 @@ ScalarFunctionQuadraticApproximation IpmSolver::getValueFunction(scalar_t time, vector_t IpmSolver::getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const { if (settings_.computeLagrangeMultipliers && !projectionMultiplierTrajectory_.empty()) { - // Interpolation - using LinearInterpolation::interpolate; - const auto nominalMultiplier = interpolate(time, primalSolution_.timeTrajectory_, projectionMultiplierTrajectory_); - const auto nominalState = interpolate(time, primalSolution_.timeTrajectory_, primalSolution_.stateTrajectory_); - - const auto indexAlpha = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_); using T = std::vector<multiple_shooting::ProjectionMultiplierCoefficients>; - const auto sensitivityWrtState = - interpolate(indexAlpha, projectionMultiplierCoefficients_, [](const T& v, size_t ind) -> const matrix_t& { return v[ind].dfdx; }); + const auto indexAlpha = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_); + + const auto nominalState = LinearInterpolation::interpolate(indexAlpha, primalSolution_.stateTrajectory_); + const auto sensitivityWrtState = LinearInterpolation::interpolate( + indexAlpha, projectionMultiplierCoefficients_, [](const T& v, size_t ind) -> const matrix_t& { return v[ind].dfdx; }); + + auto multiplier = LinearInterpolation::interpolate(indexAlpha, projectionMultiplierTrajectory_); + multiplier.noalias() += sensitivityWrtState * (state - nominalState); - return nominalMultiplier + sensitivityWrtState * (state - nominalState); + return multiplier; } else { throw std::runtime_error("[IpmSolver] getStateInputEqualityConstraintLagrangian() not available yet."); @@ -253,17 +253,9 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f const scalar_t maxPrimalStepSize = settings_.usePrimalStepSizeForDual ? std::min(deltaSolution.maxDualStepSize, deltaSolution.maxPrimalStepSize) : deltaSolution.maxPrimalStepSize; - const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, barrierParam, slackStateIneq, - slackStateInputIneq, metrics); - const scalar_t dualStepSize = - settings_.usePrimalStepSizeForDual ? std::min(stepInfo.stepSize, deltaSolution.maxDualStepSize) : deltaSolution.maxDualStepSize; - if (settings_.computeLagrangeMultipliers) { - multiple_shooting::incrementTrajectory(lmd, deltaSolution.deltaLmdSol, stepInfo.stepSize, lmd); - multiple_shooting::incrementTrajectory(nu, deltaSolution.deltaNuSol, stepInfo.stepSize, nu); - } - multiple_shooting::incrementTrajectory(dualStateIneq, deltaSolution.deltaDualStateIneq, stepInfo.stepSize, dualStateIneq); - multiple_shooting::incrementTrajectory(dualStateInputIneq, deltaSolution.deltaDualStateInputIneq, stepInfo.stepSize, - dualStateInputIneq); + const auto stepInfo = takePrimalStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, barrierParam, + slackStateIneq, slackStateInputIneq, metrics); + takeDualStep(deltaSolution, stepInfo, lmd, nu, dualStateIneq, dualStateInputIneq); performanceIndeces_.push_back(stepInfo.performanceAfterStep); linesearchTimer_.endTimer(); @@ -303,9 +295,8 @@ void IpmSolver::runParallel(std::function<void(int)> taskFunction) { void IpmSolver::initializeCostateTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& stateTrajectory, vector_array_t& costateTrajectory) const { - const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // size of the input trajectory costateTrajectory.clear(); - costateTrajectory.reserve(N + 1); + costateTrajectory.reserve(timeDiscretization.size()); // Determine till when to use the previous solution const auto interpolateTill = @@ -318,7 +309,7 @@ void IpmSolver::initializeCostateTrajectory(const std::vector<AnnotatedTime>& ti costateTrajectory.push_back(vector_t::Zero(stateTrajectory[0].size())); } - for (int i = 0; i < N; i++) { + for (int i = 0; i < timeDiscretization.size() - 1; i++) { const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); if (nextTime < interpolateTill) { // interpolate previous solution costateTrajectory.push_back(LinearInterpolation::interpolate(nextTime, primalSolution_.timeTrajectory_, costateTrajectory_)); @@ -706,10 +697,10 @@ PerformanceIndex IpmSolver::computePerformance(const std::vector<AnnotatedTime>& return totalPerformance; } -ipm::StepInfo IpmSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, - const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, - vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, - vector_array_t& slackStateInputIneq, std::vector<Metrics>& metrics) { +ipm::StepInfo IpmSolver::takePrimalStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& slackStateInputIneq, std::vector<Metrics>& metrics) { using StepType = FilterLinesearch::StepType; /* @@ -773,7 +764,7 @@ ipm::StepInfo IpmSolver::takeStep(const PerformanceIndex& baseline, const std::v // Prepare step info ipm::StepInfo stepInfo; - stepInfo.stepSize = alpha; + stepInfo.primalStepSize = alpha; stepInfo.stepType = stepType; stepInfo.dx_norm = alpha * deltaXnorm; stepInfo.du_norm = alpha * deltaUnorm; @@ -797,7 +788,7 @@ ipm::StepInfo IpmSolver::takeStep(const PerformanceIndex& baseline, const std::v // Alpha_min reached -> Don't take a step ipm::StepInfo stepInfo; - stepInfo.stepSize = 0.0; + stepInfo.primalStepSize = 0.0; stepInfo.stepType = StepType::ZERO; stepInfo.dx_norm = 0.0; stepInfo.du_norm = 0.0; @@ -805,12 +796,25 @@ ipm::StepInfo IpmSolver::takeStep(const PerformanceIndex& baseline, const std::v stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline); if (settings_.printLinesearch) { - std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; + std::cerr << "[Linesearch terminated] Primal Step size: " << stepInfo.primalStepSize << ", Step Type: " << toString(stepInfo.stepType) + << "\n"; } return stepInfo; } +void IpmSolver::takeDualStep(const OcpSubproblemSolution& subproblemSolution, const ipm::StepInfo& stepInfo, vector_array_t& lmd, + vector_array_t& nu, vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq) const { + if (settings_.computeLagrangeMultipliers) { + multiple_shooting::incrementTrajectory(lmd, subproblemSolution.deltaLmdSol, stepInfo.primalStepSize, lmd); + multiple_shooting::incrementTrajectory(nu, subproblemSolution.deltaNuSol, stepInfo.primalStepSize, nu); + } + const scalar_t dualStepSize = settings_.usePrimalStepSizeForDual ? std::min(stepInfo.primalStepSize, subproblemSolution.maxDualStepSize) + : subproblemSolution.maxDualStepSize; + multiple_shooting::incrementTrajectory(dualStateIneq, subproblemSolution.deltaDualStateIneq, dualStepSize, dualStateIneq); + multiple_shooting::incrementTrajectory(dualStateInputIneq, subproblemSolution.deltaDualStateInputIneq, dualStepSize, dualStateInputIneq); +} + scalar_t IpmSolver::updateBarrierParameter(scalar_t currentBarrierParameter, const PerformanceIndex& baseline, const ipm::StepInfo& stepInfo) const { if (currentBarrierParameter <= settings_.targetBarrierParameter) { @@ -830,7 +834,7 @@ ipm::Convergence IpmSolver::checkConvergence(int iteration, scalar_t barrierPara if ((iteration + 1) >= settings_.ipmIteration) { // Converged because the next iteration would exceed the specified number of iterations return Convergence::ITERATIONS; - } else if (stepInfo.stepSize < settings_.alpha_min) { + } else if (stepInfo.primalStepSize < settings_.alpha_min) { // Converged because step size is below the specified minimum return Convergence::STEPSIZE; } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.costTol && From 4a2d390444c7a604697166b055e083fe27607b99 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Fri, 16 Dec 2022 14:53:27 +0100 Subject: [PATCH 482/512] refactored IpmSolver --- ocs2_ipm/src/IpmSolver.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index e181137ee..ced4312bf 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -296,7 +296,7 @@ void IpmSolver::runParallel(std::function<void(int)> taskFunction) { void IpmSolver::initializeCostateTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& stateTrajectory, vector_array_t& costateTrajectory) const { costateTrajectory.clear(); - costateTrajectory.reserve(timeDiscretization.size()); + costateTrajectory.reserve(stateTrajectory.size()); // Determine till when to use the previous solution const auto interpolateTill = @@ -309,12 +309,12 @@ void IpmSolver::initializeCostateTrajectory(const std::vector<AnnotatedTime>& ti costateTrajectory.push_back(vector_t::Zero(stateTrajectory[0].size())); } - for (int i = 0; i < timeDiscretization.size() - 1; i++) { - const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); - if (nextTime < interpolateTill) { // interpolate previous solution - costateTrajectory.push_back(LinearInterpolation::interpolate(nextTime, primalSolution_.timeTrajectory_, costateTrajectory_)); + for (int i = 1; i < stateTrajectory.size(); i++) { + const auto time = getIntervalEnd(timeDiscretization[i]); + if (time < interpolateTill) { // interpolate previous solution + costateTrajectory.push_back(LinearInterpolation::interpolate(time, primalSolution_.timeTrajectory_, costateTrajectory_)); } else { // Initialize with zero - costateTrajectory.push_back(vector_t::Zero(stateTrajectory[i + 1].size())); + costateTrajectory.push_back(vector_t::Zero(stateTrajectory[i].size())); } } } @@ -400,9 +400,9 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta deltaLmdSol.resize(N + 1); deltaNuSol.resize(N); deltaSlackStateIneq.resize(N + 1); - deltaSlackStateInputIneq.resize(N + 1); + deltaSlackStateInputIneq.resize(N); deltaDualStateIneq.resize(N + 1); - deltaDualStateInputIneq.resize(N + 1); + deltaDualStateInputIneq.resize(N); scalar_array_t primalStepSizes(settings_.nThreads, 1.0); scalar_array_t dualStepSizes(settings_.nThreads, 1.0); @@ -521,9 +521,9 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated if (initializeSlackAndDualVariables) { slackStateIneq.resize(N + 1); - slackStateInputIneq.resize(N + 1); + slackStateInputIneq.resize(N); dualStateIneq.resize(N + 1); - dualStateInputIneq.resize(N + 1); + dualStateInputIneq.resize(N); } std::atomic_int timeIndex{0}; From 84202829de088fb440e2132be94142a3257584f8 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 16 Dec 2022 14:58:34 +0100 Subject: [PATCH 483/512] small changes --- ocs2_ipm/src/IpmHelpers.cpp | 14 +++++--------- ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h | 1 - ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 3 --- 3 files changed, 5 insertions(+), 13 deletions(-) diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index eaf1e614f..d60b0c3d6 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -37,7 +37,6 @@ namespace ipm { void condenseIneqConstraints(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, const VectorFunctionLinearApproximation& ineqConstraint, ScalarFunctionQuadraticApproximation& lagrangian) { assert(barrierParam > 0.0); - const size_t nc = ineqConstraint.f.size(); const size_t nu = ineqConstraint.dfdu.cols(); @@ -95,8 +94,9 @@ vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateIn vector_t retrieveDualDirection(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, const vector_t& slackDirection) { assert(barrierParam > 0.0); - vector_t dualDirection(slackDirection.size()); - dualDirection.array() = -(dual.array() * slackDirection.array() + (slack.array() * dual.array() - barrierParam)) / slack.array(); + vector_t dualDirection = dual.cwiseProduct(slack + slackDirection); + dualDirection.array() -= barrierParam; + dualDirection.array() /= -slack.array(); return dualDirection; } @@ -108,13 +108,9 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala return 1.0; } - scalar_t minFractionToBoundary = 1.0; vector_t fractionToBoundary = -marginRate * v.cwiseQuotient(dv); - for (int i = 0; i < fractionToBoundary.size(); ++i) { - if (fractionToBoundary[i] <= 0.0) { - fractionToBoundary[i] = 1.0; - } - } + fractionToBoundary.unaryExpr([](scalar_t ftb) { return ftb > 0.0 ? ftb : 1.0; }); + return std::min(1.0, fractionToBoundary.minCoeff()); } diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index da2d9cd16..0247a5473 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -185,7 +185,6 @@ class SqpSolver : public SolverBase { ProblemMetrics problemMetrics_; // Benchmarking - size_t numProblems_{0}; size_t totalNumIterations_{0}; benchmark::RepeatedTimer initializationTimer_; benchmark::RepeatedTimer linearQuadraticApproximationTimer_; diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index 0408cbc2d..ff2f658e2 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -91,7 +91,6 @@ void SqpSolver::reset() { performanceIndeces_.clear(); // reset timers - numProblems_ = 0; totalNumIterations_ = 0; linearQuadraticApproximationTimer_.reset(); solveQpTimer_.reset(); @@ -216,8 +215,6 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); - ++numProblems_; - if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\nConvergence : " << toString(convergence) << "\n"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; From 549ed1972b6326e0216645bdeb302b4a4caa93a9 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Fri, 16 Dec 2022 15:15:33 +0100 Subject: [PATCH 484/512] fix doc and add resize --- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 3 ++- ocs2_ipm/src/IpmSolver.cpp | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index efcfad64f..582bc77a3 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -160,7 +160,8 @@ class IpmSolver : public SolverBase { /** Constructs the primal solution based on the optimized state and input trajectories */ PrimalSolution toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u); - /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ + /** Decides on the step to take and overrides given trajectories {x(t), u(t), slackStateIneq(t), slackStateInputIneq(t)} + * <- {x(t) + a*dx(t), u(t) + a*du(t), slackStateIneq(t) + a*dslackStateIneq(t), slackStateInputIneq(t) + a*dslackStateInputIneq(t)} */ ipm::StepInfo takePrimalStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization, const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index ced4312bf..723075146 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -542,6 +542,8 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + slackStateInputIneq[i].resize(0); + dualStateInputIneq[i].resize(0); } performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]); dynamics_[i] = std::move(result.dynamics); From 88efbd92c4071b7ac9770722ef0a88d55718181b Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Fri, 16 Dec 2022 16:07:33 +0100 Subject: [PATCH 485/512] refactored tests --- ocs2_ipm/test/Exp0Test.cpp | 114 +++++---------- ocs2_ipm/test/Exp1Test.cpp | 124 ++++++---------- ocs2_ipm/test/testCircularKinematics.cpp | 172 +++++++++-------------- 3 files changed, 142 insertions(+), 268 deletions(-) diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp index de974443b..3521e8d90 100644 --- a/ocs2_ipm/test/Exp0Test.cpp +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -35,72 +35,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ipm/IpmSolver.h" +#include <ocs2_core/constraint/LinearStateConstraint.h> +#include <ocs2_core/constraint/LinearStateInputConstraint.h> #include <ocs2_core/initialization/DefaultInitializer.h> #include <ocs2_oc/test/EXP0.h> using namespace ocs2; -class EXP0_StateIneqConstraints final : public StateConstraint { - public: - EXP0_StateIneqConstraints(const vector_t& xmin, const vector_t& xmax) - : StateConstraint(ConstraintOrder::Linear), xmin_(xmin), xmax_(xmax) {} - ~EXP0_StateIneqConstraints() override = default; - - EXP0_StateIneqConstraints* clone() const override { return new EXP0_StateIneqConstraints(*this); } - - size_t getNumConstraints(scalar_t time) const override { return 4; } - - vector_t getValue(scalar_t t, const vector_t& x, const PreComputation&) const override { - vector_t e(4); - e.head(2) = x - xmin_; - e.tail(2) = xmax_ - x; - return e; - } - - VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const PreComputation& preComp) const override { - VectorFunctionLinearApproximation e; - e.f = getValue(t, x, preComp); - e.dfdx = matrix_t::Zero(4, x.size()); - e.dfdx.topLeftCorner(2, 2) = matrix_t::Identity(2, 2); - e.dfdx.bottomRightCorner(2, 2) = -matrix_t::Identity(2, 2); - return e; - } - - private: - vector_t xmin_, xmax_; -}; - -class EXP0_StateInputIneqConstraints final : public StateInputConstraint { - public: - EXP0_StateInputIneqConstraints(scalar_t umin, scalar_t umax) : StateInputConstraint(ConstraintOrder::Linear), umin_(umin), umax_(umax) {} - ~EXP0_StateInputIneqConstraints() override = default; - - EXP0_StateInputIneqConstraints* clone() const override { return new EXP0_StateInputIneqConstraints(*this); } - - size_t getNumConstraints(scalar_t time) const override { return 2; } - - vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { - vector_t e(2); - e << (u.coeff(0) - umin_), (umax_ - u.coeff(0)); - return e; - } - - VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp) const override { - VectorFunctionLinearApproximation e; - e.f = getValue(t, x, u, preComp); - e.dfdx = matrix_t::Zero(2, x.size()); - e.dfdu = (matrix_t(2, 1) << 1, -1).finished(); - return e; - } - - private: - scalar_t umin_, umax_; -}; - TEST(Exp0Test, Unconstrained) { - static constexpr size_t STATE_DIM = 2; - static constexpr size_t INPUT_DIM = 1; + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; // Solver settings const auto settings = []() { @@ -141,8 +85,27 @@ TEST(Exp0Test, Unconstrained) { } TEST(Exp0Test, Constrained) { - static constexpr size_t STATE_DIM = 2; - static constexpr size_t INPUT_DIM = 1; + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; + + auto getStateBoxConstraint = [&](const vector_t& minState, const vector_t& maxState) { + constexpr size_t numIneqConstraint = 2 * STATE_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minState, maxState).finished(); + const matrix_t C = + (matrix_t(numIneqConstraint, STATE_DIM) << matrix_t::Identity(STATE_DIM, STATE_DIM), -matrix_t::Identity(STATE_DIM, STATE_DIM)) + .finished(); + return std::make_unique<LinearStateConstraint>(std::move(e), std::move(C)); + }; + + auto getInputBoxConstraint = [&](scalar_t minInput, scalar_t maxInput) { + constexpr size_t numIneqConstraint = 2 * INPUT_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minInput, maxInput).finished(); + const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM); + const matrix_t D = + (matrix_t(numIneqConstraint, INPUT_DIM) << matrix_t::Identity(INPUT_DIM, INPUT_DIM), -matrix_t::Identity(INPUT_DIM, INPUT_DIM)) + .finished(); + return std::make_unique<LinearStateInputConstraint>(std::move(e), std::move(C), std::move(D)); + }; // Solver settings const auto settings = []() { @@ -173,14 +136,11 @@ TEST(Exp0Test, Constrained) { // add inequality constraints const scalar_t umin = -7.5; const scalar_t umax = 7.5; - auto stateInputIneqConstraint = std::make_unique<EXP0_StateInputIneqConstraints>(umin, umax); - problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint)); + problem.inequalityConstraintPtr->add("ubound", getInputBoxConstraint(umin, umax)); const vector_t xmin = (vector_t(2) << -7.5, -7.5).finished(); const vector_t xmax = (vector_t(2) << 7.5, 7.5).finished(); - auto stateIneqConstraint = std::make_unique<EXP0_StateIneqConstraints>(xmin, xmax); - auto finalStateIneqConstraint = std::make_unique<EXP0_StateIneqConstraints>(xmin, xmax); - problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint)); - problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint)); + problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); const scalar_t startTime = 0.0; const scalar_t finalTime = 2.0; @@ -196,18 +156,16 @@ TEST(Exp0Test, Constrained) { const auto primalSolution = solver.primalSolution(finalTime); // check constraint satisfaction - for (const auto& e : primalSolution.stateTrajectory_) { - if (e.size() > 0) { - ASSERT_TRUE(e.coeff(0) >= xmin.coeff(0)); - ASSERT_TRUE(e.coeff(1) >= xmin.coeff(1)); - ASSERT_TRUE(e.coeff(0) <= xmax.coeff(0)); - ASSERT_TRUE(e.coeff(1) <= xmax.coeff(1)); + for (const auto& x : primalSolution.stateTrajectory_) { + if (x.size() > 0) { + ASSERT_TRUE((x - xmin).minCoeff() >= 0); + ASSERT_TRUE((xmax - x).minCoeff() >= 0); } } - for (const auto& e : primalSolution.inputTrajectory_) { - if (e.size() > 0) { - ASSERT_TRUE(e.coeff(0) >= umin); - ASSERT_TRUE(e.coeff(0) <= umax); + for (const auto& u : primalSolution.inputTrajectory_) { + if (u.size() > 0) { + ASSERT_TRUE(u(0) - umin >= 0); + ASSERT_TRUE(umax - u(0) >= 0); } } } \ No newline at end of file diff --git a/ocs2_ipm/test/Exp1Test.cpp b/ocs2_ipm/test/Exp1Test.cpp index fc04f9dde..fa22ff8db 100644 --- a/ocs2_ipm/test/Exp1Test.cpp +++ b/ocs2_ipm/test/Exp1Test.cpp @@ -35,69 +35,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ipm/IpmSolver.h" +#include <ocs2_core/constraint/LinearStateConstraint.h> +#include <ocs2_core/constraint/LinearStateInputConstraint.h> #include <ocs2_core/initialization/DefaultInitializer.h> #include <ocs2_oc/test/EXP1.h> using namespace ocs2; -class EXP1_StateIneqConstraints final : public StateConstraint { - public: - EXP1_StateIneqConstraints(const vector_t& xmin, const vector_t& xmax) - : StateConstraint(ConstraintOrder::Linear), xmin_(xmin), xmax_(xmax) {} - ~EXP1_StateIneqConstraints() override = default; - - EXP1_StateIneqConstraints* clone() const override { return new EXP1_StateIneqConstraints(*this); } - - size_t getNumConstraints(scalar_t time) const override { return 4; } - - vector_t getValue(scalar_t t, const vector_t& x, const PreComputation&) const override { - vector_t e(4); - e.head(2) = x - xmin_; - e.tail(2) = xmax_ - x; - return e; - } - - VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const PreComputation& preComp) const override { - VectorFunctionLinearApproximation e; - e.f = getValue(t, x, preComp); - e.dfdx = matrix_t::Zero(4, x.size()); - e.dfdx.topLeftCorner(2, 2) = matrix_t::Identity(2, 2); - e.dfdx.bottomRightCorner(2, 2) = -matrix_t::Identity(2, 2); - return e; - } - - private: - vector_t xmin_, xmax_; -}; - -class EXP1_StateInputIneqConstraints final : public StateInputConstraint { - public: - EXP1_StateInputIneqConstraints(scalar_t umin, scalar_t umax) : StateInputConstraint(ConstraintOrder::Linear), umin_(umin), umax_(umax) {} - ~EXP1_StateInputIneqConstraints() override = default; - - EXP1_StateInputIneqConstraints* clone() const override { return new EXP1_StateInputIneqConstraints(*this); } - - size_t getNumConstraints(scalar_t time) const override { return 2; } - - vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { - vector_t e(2); - e << (u.coeff(0) - umin_), (umax_ - u.coeff(0)); - return e; - } - - VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp) const override { - VectorFunctionLinearApproximation e; - e.f = getValue(t, x, u, preComp); - e.dfdx = matrix_t::Zero(2, x.size()); - e.dfdu = (matrix_t(2, 1) << 1, -1).finished(); - return e; - } - - private: - scalar_t umin_, umax_; -}; - class EXP1_MixedStateInputIneqConstraints final : public StateInputConstraint { public: EXP1_MixedStateInputIneqConstraints(scalar_t xumin, scalar_t xumax) @@ -110,26 +54,26 @@ class EXP1_MixedStateInputIneqConstraints final : public StateInputConstraint { vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { vector_t e(2); - e << (x.coeff(0) * u.coeff(0) - xumin_), (xumax_ - x.coeff(1) * u.coeff(0)); + e << (x(0) * u(0) - xumin_), (xumax_ - x(1) * u(0)); return e; } VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation& preComp) const override { - VectorFunctionLinearApproximation e; + VectorFunctionLinearApproximation e(2, x.size(), u.size()); e.f = getValue(t, x, u, preComp); - e.dfdx = (matrix_t(2, 2) << u.coeff(0), 0, 0, -u.coeff(0)).finished(); - e.dfdu = (matrix_t(2, 1) << x.coeff(0), -x.coeff(1)).finished(); + e.dfdx << u(0), 0, 0, -u(0); + e.dfdu << x(0), -x(1); return e; } private: - scalar_t xumin_, xumax_; + const scalar_t xumin_, xumax_; }; TEST(Exp1Test, Unconstrained) { - static constexpr size_t STATE_DIM = 2; - static constexpr size_t INPUT_DIM = 1; + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; // Solver settings const auto settings = []() { @@ -170,8 +114,27 @@ TEST(Exp1Test, Unconstrained) { } TEST(Exp1Test, Constrained) { - static constexpr size_t STATE_DIM = 2; - static constexpr size_t INPUT_DIM = 1; + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; + + auto getStateBoxConstraint = [&](const vector_t& minState, const vector_t& maxState) { + constexpr size_t numIneqConstraint = 2 * STATE_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minState, maxState).finished(); + const matrix_t C = + (matrix_t(numIneqConstraint, STATE_DIM) << matrix_t::Identity(STATE_DIM, STATE_DIM), -matrix_t::Identity(STATE_DIM, STATE_DIM)) + .finished(); + return std::make_unique<LinearStateConstraint>(std::move(e), std::move(C)); + }; + + auto getInputBoxConstraint = [&](scalar_t minInput, scalar_t maxInput) { + constexpr size_t numIneqConstraint = 2 * INPUT_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minInput, maxInput).finished(); + const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM); + const matrix_t D = + (matrix_t(numIneqConstraint, INPUT_DIM) << matrix_t::Identity(INPUT_DIM, INPUT_DIM), -matrix_t::Identity(INPUT_DIM, INPUT_DIM)) + .finished(); + return std::make_unique<LinearStateInputConstraint>(std::move(e), std::move(C), std::move(D)); + }; // Solver settings const auto settings = []() { @@ -202,14 +165,11 @@ TEST(Exp1Test, Constrained) { // add inequality constraints const scalar_t umin = -1.0; const scalar_t umax = 1.0; - auto stateInputIneqConstraint = std::make_unique<EXP1_StateInputIneqConstraints>(umin, umax); - problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint)); + problem.inequalityConstraintPtr->add("ubound", getInputBoxConstraint(umin, umax)); const vector_t xmin = (vector_t(2) << -0.0, -0.0).finished(); const vector_t xmax = (vector_t(2) << 3.0, 4.0).finished(); - auto stateIneqConstraint = std::make_unique<EXP1_StateIneqConstraints>(xmin, xmax); - auto finalStateIneqConstraint = std::make_unique<EXP1_StateIneqConstraints>(xmin, xmax); - problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint)); - problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint)); + problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); const scalar_t startTime = 0.0; const scalar_t finalTime = 3.0; @@ -225,18 +185,16 @@ TEST(Exp1Test, Constrained) { const auto primalSolution = solver.primalSolution(finalTime); // check constraint satisfaction - for (const auto& e : primalSolution.stateTrajectory_) { - if (e.size() > 0) { - ASSERT_TRUE(e.coeff(0) >= xmin.coeff(0)); - ASSERT_TRUE(e.coeff(1) >= xmin.coeff(1)); - ASSERT_TRUE(e.coeff(0) <= xmax.coeff(0)); - ASSERT_TRUE(e.coeff(1) <= xmax.coeff(1)); + for (const auto& x : primalSolution.stateTrajectory_) { + if (x.size() > 0) { + ASSERT_TRUE((x - xmin).minCoeff() >= 0); + ASSERT_TRUE((xmax - x).minCoeff() >= 0); } } - for (const auto& e : primalSolution.inputTrajectory_) { - if (e.size() > 0) { - ASSERT_TRUE(e.coeff(0) >= umin); - ASSERT_TRUE(e.coeff(0) <= umax); + for (const auto& u : primalSolution.inputTrajectory_) { + if (u.size() > 0) { + ASSERT_TRUE(u(0) - umin >= 0); + ASSERT_TRUE(umax - u(0) >= 0); } } } diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp index 9d484d3b0..6db18bc64 100644 --- a/ocs2_ipm/test/testCircularKinematics.cpp +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -31,73 +31,17 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ipm/IpmSolver.h" +#include <ocs2_core/constraint/LinearStateConstraint.h> +#include <ocs2_core/constraint/LinearStateInputConstraint.h> #include <ocs2_core/initialization/DefaultInitializer.h> - #include <ocs2_oc/test/circular_kinematics.h> -namespace ocs2 { - -class CircleKinematics_StateIneqConstraints final : public StateConstraint { - public: - CircleKinematics_StateIneqConstraints(const vector_t& xmin, const vector_t& xmax) - : StateConstraint(ConstraintOrder::Linear), xmin_(xmin), xmax_(xmax) {} - ~CircleKinematics_StateIneqConstraints() override = default; - - CircleKinematics_StateIneqConstraints* clone() const override { return new CircleKinematics_StateIneqConstraints(*this); } - - size_t getNumConstraints(scalar_t time) const override { return 4; } - - vector_t getValue(scalar_t t, const vector_t& x, const PreComputation&) const override { - vector_t e(4); - e.head(2) = x - xmin_; - e.tail(2) = xmax_ - x; - return e; - } - - VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const PreComputation& preComp) const override { - VectorFunctionLinearApproximation e; - e.f = getValue(t, x, preComp); - e.dfdx = matrix_t::Zero(4, x.size()); - e.dfdx.topLeftCorner(2, 2) = matrix_t::Identity(2, 2); - e.dfdx.bottomRightCorner(2, 2) = -matrix_t::Identity(2, 2); - return e; - } - - private: - vector_t xmin_, xmax_; -}; - -class CircleKinematics_StateInputIneqConstraints final : public StateInputConstraint { - public: - CircleKinematics_StateInputIneqConstraints(const vector_t& umin, const vector_t& umax) - : StateInputConstraint(ConstraintOrder::Linear), umin_(umin), umax_(umax) {} - ~CircleKinematics_StateInputIneqConstraints() override = default; +using namespace ocs2; - CircleKinematics_StateInputIneqConstraints* clone() const override { return new CircleKinematics_StateInputIneqConstraints(*this); } - - size_t getNumConstraints(scalar_t time) const override { return 4; } - - vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { - vector_t e(4); - e.head(2) = u - umin_; - e.tail(2) = umax_ - u; - return e; - } - - VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, - const PreComputation& preComp) const override { - VectorFunctionLinearApproximation e; - e.f = getValue(t, x, u, preComp); - e.dfdx = matrix_t::Zero(4, x.size()); - e.dfdu = matrix_t::Zero(4, u.size()); - e.dfdu.topLeftCorner(2, 2) = matrix_t::Identity(2, 2); - e.dfdu.bottomRightCorner(2, 2) = -matrix_t::Identity(2, 2); - return e; - } - - private: - vector_t umin_, umax_; -}; +namespace { +constexpr size_t STATE_DIM = 2; +constexpr size_t INPUT_DIM = 2; +} // namespace class CircleKinematics_MixedStateInputIneqConstraints final : public StateInputConstraint { public: @@ -113,35 +57,33 @@ class CircleKinematics_MixedStateInputIneqConstraints final : public StateInputC vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { vector_t e(2); - e << (x.coeff(0) * u.coeff(0) - xumin_), (xumax_ - x.coeff(1) * u.coeff(1)); + e << (x(0) * u(0) - xumin_), (xumax_ - x(1) * u(1)); return e; } VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation& preComp) const override { - VectorFunctionLinearApproximation e; + VectorFunctionLinearApproximation e(2, x.size(), u.size()); e.f = getValue(t, x, u, preComp); - e.dfdx = (matrix_t(2, 2) << u.coeff(0), 0, 0, -u.coeff(1)).finished(); - e.dfdu = (matrix_t(2, 2) << x.coeff(0), 0, 0, -x.coeff(1)).finished(); + e.dfdx << u(0), 0, 0, -u(1); + e.dfdu << x(0), 0, 0, -x(1); return e; } private: - scalar_t xumin_, xumax_; + const scalar_t xumin_, xumax_; }; -} // namespace ocs2 - TEST(test_circular_kinematics, solve_projected_EqConstraints) { // optimal control problem - ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); // Initializer - ocs2::DefaultInitializer zeroInitializer(2); + DefaultInitializer zeroInitializer(2); // Solver settings const auto settings = []() { - ocs2::ipm::Settings s; + ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; s.useFeedbackPolicy = true; @@ -161,12 +103,12 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { }(); // Additional problem definitions - const ocs2::scalar_t startTime = 0.0; - const ocs2::scalar_t finalTime = 1.0; - const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + const scalar_t startTime = 0.0; + const scalar_t finalTime = 1.0; + const vector_t initState = (vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 // Solve - ocs2::IpmSolver solver(settings, problem, zeroInitializer); + IpmSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); // Inspect solution @@ -198,26 +140,42 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { // optimal control problem - ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + + auto getStateBoxConstraint = [&](const vector_t& minState, const vector_t& maxState) { + constexpr size_t numIneqConstraint = 2 * STATE_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minState, maxState).finished(); + const matrix_t C = + (matrix_t(numIneqConstraint, STATE_DIM) << matrix_t::Identity(STATE_DIM, STATE_DIM), -matrix_t::Identity(STATE_DIM, STATE_DIM)) + .finished(); + return std::make_unique<LinearStateConstraint>(std::move(e), std::move(C)); + }; + + auto getInputBoxConstraint = [&](const vector_t& minInput, const vector_t& maxInput) { + constexpr size_t numIneqConstraint = 2 * INPUT_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minInput, maxInput).finished(); + const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM); + const matrix_t D = + (matrix_t(numIneqConstraint, INPUT_DIM) << matrix_t::Identity(INPUT_DIM, INPUT_DIM), -matrix_t::Identity(INPUT_DIM, INPUT_DIM)) + .finished(); + return std::make_unique<LinearStateInputConstraint>(std::move(e), std::move(C), std::move(D)); + }; // inequality constraints - const ocs2::vector_t umin = (ocs2::vector_t(2) << -0.5, -0.5).finished(); - const ocs2::vector_t umax = (ocs2::vector_t(2) << 0.5, 0.5).finished(); - auto stateInputIneqConstraint = std::make_unique<ocs2::CircleKinematics_StateInputIneqConstraints>(umin, umax); - problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint)); - const ocs2::vector_t xmin = (ocs2::vector_t(2) << -0.5, -0.5).finished(); - const ocs2::vector_t xmax = (ocs2::vector_t(2) << 1.0e03, 1.0e03).finished(); // no upper bound - auto stateIneqConstraint = std::make_unique<ocs2::CircleKinematics_StateIneqConstraints>(xmin, xmax); - auto finalStateIneqConstraint = std::make_unique<ocs2::CircleKinematics_StateIneqConstraints>(xmin, xmax); - problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint)); - problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint)); + const vector_t umin = (vector_t(2) << -0.5, -0.5).finished(); + const vector_t umax = (vector_t(2) << 0.5, 0.5).finished(); + problem.inequalityConstraintPtr->add("ubound", getInputBoxConstraint(umin, umax)); + const vector_t xmin = (vector_t(2) << -0.5, -0.5).finished(); + const vector_t xmax = (vector_t(2) << 1.0e03, 1.0e03).finished(); // no upper bound + problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); // Initializer - ocs2::DefaultInitializer zeroInitializer(2); + DefaultInitializer zeroInitializer(2); // Solver settings const auto settings = []() { - ocs2::ipm::Settings s; + ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; s.useFeedbackPolicy = true; @@ -237,12 +195,12 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { }(); // Additional problem definitions - const ocs2::scalar_t startTime = 0.0; - const ocs2::scalar_t finalTime = 1.0; - const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + const scalar_t startTime = 0.0; + const scalar_t finalTime = 1.0; + const vector_t initState = (vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 // Solve - ocs2::IpmSolver solver(settings, problem, zeroInitializer); + IpmSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); // Inspect solution @@ -256,7 +214,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { // check constraint satisfaction for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { if (primalSolution.inputTrajectory_[i].size() > 0) { - const ocs2::scalar_t projectionConstraintViolation = primalSolution.stateTrajectory_[i].dot(primalSolution.inputTrajectory_[i]); + const scalar_t projectionConstraintViolation = primalSolution.stateTrajectory_[i].dot(primalSolution.inputTrajectory_[i]); EXPECT_LT(std::abs(projectionConstraintViolation), 1.0e-06); } } @@ -301,21 +259,21 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraints) { // optimal control problem - ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); // inequality constraints - const ocs2::scalar_t xumin = -2.0; - const ocs2::scalar_t xumax = 2.0; - auto stateInputIneqConstraint = std::make_unique<ocs2::CircleKinematics_MixedStateInputIneqConstraints>(xumin, xumax); + const scalar_t xumin = -2.0; + const scalar_t xumax = 2.0; + auto stateInputIneqConstraint = std::make_unique<CircleKinematics_MixedStateInputIneqConstraints>(xumin, xumax); auto stateInputIneqConstraintCloned = stateInputIneqConstraint->clone(); problem.inequalityConstraintPtr->add("xubound", std::move(stateInputIneqConstraint)); // Initializer - ocs2::DefaultInitializer zeroInitializer(2); + DefaultInitializer zeroInitializer(2); // Solver settings const auto settings = []() { - ocs2::ipm::Settings s; + ipm::Settings s; s.dt = 0.01; s.ipmIteration = 20; s.useFeedbackPolicy = true; @@ -336,12 +294,12 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint }(); // Additional problem definitions - const ocs2::scalar_t startTime = 0.0; - const ocs2::scalar_t finalTime = 1.0; - const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + const scalar_t startTime = 0.0; + const scalar_t finalTime = 1.0; + const vector_t initState = (vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 // Solve - ocs2::IpmSolver solver(settings, problem, zeroInitializer); + IpmSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); // Inspect solution @@ -355,7 +313,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint // check constraint satisfaction for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { if (primalSolution.inputTrajectory_[i].size() > 0) { - const ocs2::scalar_t projectionConstraintViolation = primalSolution.stateTrajectory_[i].dot(primalSolution.inputTrajectory_[i]); + const scalar_t projectionConstraintViolation = primalSolution.stateTrajectory_[i].dot(primalSolution.inputTrajectory_[i]); EXPECT_LT(std::abs(projectionConstraintViolation), 1.0e-06); } } @@ -366,7 +324,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint const auto t = primalSolution.timeTrajectory_[i]; const auto& x = primalSolution.stateTrajectory_[i]; const auto& u = primalSolution.inputTrajectory_[i]; - const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, ocs2::PreComputation()); + const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, PreComputation()); ASSERT_TRUE(constraintValue.minCoeff() >= 0.0); } From a364458f63826e7c4f308e94f6021f64b07a99b8 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 16 Dec 2022 17:16:20 +0100 Subject: [PATCH 486/512] Fixing fractionToBoundaryStepSize --- ocs2_ipm/src/IpmHelpers.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index d60b0c3d6..50d1e9564 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -108,10 +108,9 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala return 1.0; } - vector_t fractionToBoundary = -marginRate * v.cwiseQuotient(dv); - fractionToBoundary.unaryExpr([](scalar_t ftb) { return ftb > 0.0 ? ftb : 1.0; }); - - return std::min(1.0, fractionToBoundary.minCoeff()); + const vector_t invFractionToBoundary = (-1.0 / marginRate) * dv.cwiseQuotient(v); + const auto alpha = invFractionToBoundary.maxCoeff(); + return alpha > 0.0? std::min(1.0 / alpha, 1.0): 1.0; } } // namespace ipm From 98ea50f26bafe514e1789d66d1aae9d30cc2c2a8 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Fri, 16 Dec 2022 17:44:49 +0100 Subject: [PATCH 487/512] fix state-only inequality constraints at initial node and tests --- .../ocs2_ipm/IpmPerformanceIndexComputation.h | 4 +- .../src/IpmPerformanceIndexComputation.cpp | 11 +++-- ocs2_ipm/src/IpmSolver.cpp | 13 +++--- ocs2_ipm/test/Exp1Test.cpp | 4 +- ocs2_ipm/test/testCircularKinematics.cpp | 45 +++++-------------- 5 files changed, 29 insertions(+), 48 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h b/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h index b1609724f..e36d6d6b5 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h @@ -87,7 +87,6 @@ PerformanceIndex computePerformanceIndex(const multiple_shooting::EventTranscrip * @param barrierParam : Barrier parameter of the interior point method * @param slackStateIneq : Slack variable of the state inequality constraints * @param slackStateInputIneq : Slack variable of the state-input inequality constraints - * @param enableStateInequalityConstraints : Should be disabled at the initial node (i = 0) * @return Performance index for a single intermediate node */ PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, @@ -134,11 +133,10 @@ PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlPr * @param barrierParam : Barrier parameter of the interior point method * @param slackStateIneq : Slack variable of the state inequality constraints * @param slackStateInputIneq : Slack variable of the state-input inequality constraints - * @param enableStateInequalityConstraints : Should be disabled at the initial node (i = 0) * @return Performance index of the interior point method */ PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_t barrierParam, const vector_t& slackStateIneq, - const vector_t& slackStateInputIneq, bool enableStateInequalityConstraints); + const vector_t& slackStateInputIneq); /** * Computes the PerformanceIndex of the interior point method based on a given Metrics at the event or terminal node. diff --git a/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp b/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp index bccb25413..0e727c56a 100644 --- a/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp +++ b/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp @@ -93,8 +93,11 @@ PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalCo scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, scalar_t barrierParam, const vector_t& slackStateIneq, const vector_t& slackStateInputIneq, bool enableStateInequalityConstraints) { - const auto metrics = multiple_shooting::computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); - return toPerformanceIndex(metrics, dt, barrierParam, slackStateIneq, slackStateInputIneq, enableStateInequalityConstraints); + auto metrics = multiple_shooting::computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); + if (!enableStateInequalityConstraints) { + metrics.stateIneqConstraint.clear(); + } + return toPerformanceIndex(metrics, dt, barrierParam, slackStateIneq, slackStateInputIneq); } PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, @@ -110,7 +113,7 @@ PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalContro } PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_t barrierParam, const vector_t& slackStateIneq, - const vector_t& slackStateInputIneq, bool enableStateInequalityConstraints) { + const vector_t& slackStateInputIneq) { PerformanceIndex performance = toPerformanceIndex(metrics, dt); if (slackStateIneq.size() > 0) { @@ -118,7 +121,7 @@ PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_ performance.equalityConstraintsSSE += dt * (toVector(metrics.stateIneqConstraint) - slackStateIneq).squaredNorm(); } - if (slackStateInputIneq.size() > 0 && enableStateInequalityConstraints) { + if (slackStateInputIneq.size() > 0) { performance.cost -= dt * barrierParam * slackStateInputIneq.array().log().sum(); performance.equalityConstraintsSSE += dt * (toVector(metrics.stateInputIneqConstraint) - slackStateInputIneq).squaredNorm(); } diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 723075146..55dc83b05 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -536,7 +536,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); - metrics[i] = multiple_shooting::computeMetrics(result); if (initializeSlackAndDualVariables) { slackStateIneq[i] = ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); @@ -545,6 +544,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated slackStateInputIneq[i].resize(0); dualStateInputIneq[i].resize(0); } + metrics[i] = multiple_shooting::computeMetrics(result); performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]); dynamics_[i] = std::move(result.dynamics); stateInputEqConstraints_[i].resize(0, x[i].size()); @@ -567,7 +567,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); - metrics[i] = multiple_shooting::computeMetrics(result); // Disable the state-only inequality constraints at the initial node if (i == 0) { result.stateIneqConstraints.setZero(0, x[i].size()); @@ -583,6 +582,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } + metrics[i] = multiple_shooting::computeMetrics(result); performance[workerId] += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers); dynamics_[i] = std::move(result.dynamics); @@ -614,13 +614,13 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); - metrics[i] = multiple_shooting::computeMetrics(result); if (initializeSlackAndDualVariables) { slackStateIneq[N] = ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[N] = ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } + metrics[i] = multiple_shooting::computeMetrics(result); performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]); stateInputEqConstraints_[i].resize(0, x[i].size()); stateIneqConstraints_[i] = std::move(result.ineqConstraints); @@ -673,8 +673,11 @@ PerformanceIndex IpmSolver::computePerformance(const std::vector<AnnotatedTime>& const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); const bool enableStateInequalityConstraints = (i > 0); metrics[i] = multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); - performance[workerId] += ipm::toPerformanceIndex(metrics[i], dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i], - enableStateInequalityConstraints); + // Disable the state-only inequality constraints at the initial node + if (i == 0) { + metrics[i].stateIneqConstraint.clear(); + } + performance[workerId] += ipm::toPerformanceIndex(metrics[i], dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); } i = timeIndex++; diff --git a/ocs2_ipm/test/Exp1Test.cpp b/ocs2_ipm/test/Exp1Test.cpp index fa22ff8db..5a4cd0cb1 100644 --- a/ocs2_ipm/test/Exp1Test.cpp +++ b/ocs2_ipm/test/Exp1Test.cpp @@ -200,8 +200,8 @@ TEST(Exp1Test, Constrained) { } TEST(Exp1Test, MixedConstrained) { - static constexpr size_t STATE_DIM = 2; - static constexpr size_t INPUT_DIM = 1; + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; // Solver settings const auto settings = []() { diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp index 6db18bc64..a054ed01d 100644 --- a/ocs2_ipm/test/testCircularKinematics.cpp +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -38,11 +38,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace ocs2; -namespace { -constexpr size_t STATE_DIM = 2; -constexpr size_t INPUT_DIM = 2; -} // namespace - class CircleKinematics_MixedStateInputIneqConstraints final : public StateInputConstraint { public: CircleKinematics_MixedStateInputIneqConstraints(scalar_t xumin, scalar_t xumax) @@ -111,12 +106,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { IpmSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); - // Inspect solution const auto primalSolution = solver.primalSolution(finalTime); - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << primalSolution.timeTrajectory_[i] << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } // Check initial condition ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); @@ -139,6 +129,9 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { } TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 2; + // optimal control problem OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); @@ -203,13 +196,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { IpmSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); - // Inspect solution const auto primalSolution = solver.primalSolution(finalTime); - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] - << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } // check constraint satisfaction for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { @@ -220,20 +207,16 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { } // check constraint satisfaction - for (const auto& e : primalSolution.stateTrajectory_) { - if (e.size() > 0) { - ASSERT_TRUE(e.coeff(0) >= xmin.coeff(0)); - ASSERT_TRUE(e.coeff(1) >= xmin.coeff(1)); - ASSERT_TRUE(e.coeff(0) <= xmax.coeff(0)); - ASSERT_TRUE(e.coeff(1) <= xmax.coeff(1)); + for (const auto& x : primalSolution.stateTrajectory_) { + if (x.size() > 0) { + ASSERT_TRUE((x - xmin).minCoeff() >= 0); + ASSERT_TRUE((xmax - x).minCoeff() >= 0); } } - for (const auto& e : primalSolution.inputTrajectory_) { - if (e.size() > 0) { - ASSERT_TRUE(e.coeff(0) >= umin.coeff(0)); - ASSERT_TRUE(e.coeff(1) >= umin.coeff(1)); - ASSERT_TRUE(e.coeff(0) <= umax.coeff(0)); - ASSERT_TRUE(e.coeff(1) <= umax.coeff(1)); + for (const auto& u : primalSolution.inputTrajectory_) { + if (u.size() > 0) { + ASSERT_TRUE((u - umin).minCoeff() >= 0); + ASSERT_TRUE((umax - u).minCoeff() >= 0); } } @@ -302,13 +285,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint IpmSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); - // Inspect solution const auto primalSolution = solver.primalSolution(finalTime); - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << std::setprecision(4) << primalSolution.timeTrajectory_[i] - << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } // check constraint satisfaction for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { From 52b0d0e203df6cd6742f10b27c08edb7609886e0 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Fri, 16 Dec 2022 23:06:49 +0100 Subject: [PATCH 488/512] changing to CF_WERROR --- ocs2_ipm/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index d5e27aae7..bd02a70cc 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -73,7 +73,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) From 551ef2069ac1b93621f3bb3714a9ad540273ed2c Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Sun, 18 Dec 2022 00:29:48 +0100 Subject: [PATCH 489/512] update pull request --- ocs2_ipm/src/IpmHelpers.cpp | 2 +- .../ocs2_cartpole_ros/launch/multiplot.launch | 2 +- .../ocs2_legged_robot/CMakeLists.txt | 2 +- .../ocs2_legged_robot/config/mpc/task.info | 2 +- .../ocs2_legged_robot/LeggedRobotInterface.h | 2 +- .../src/LeggedRobotInterface.cpp | 16 ++++++---------- .../src/LeggedRobotIpmMpcNode.cpp | 2 +- 7 files changed, 12 insertions(+), 16 deletions(-) diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index 50d1e9564..ea7188006 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -110,7 +110,7 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala const vector_t invFractionToBoundary = (-1.0 / marginRate) * dv.cwiseQuotient(v); const auto alpha = invFractionToBoundary.maxCoeff(); - return alpha > 0.0? std::min(1.0 / alpha, 1.0): 1.0; + return alpha > 0.0 ? std::min(1.0 / alpha, 1.0) : 1.0; } } // namespace ipm diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch index 9a1f5061a..7194cb22b 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch @@ -1,6 +1,6 @@ <launch> <arg name="observation_config" default="$(find ocs2_cartpole)/config/multiplot/mpc_observation.xml" /> - <arg name="metrics_config" default="$(find ocs2_cartpole)/config/multiplot/zero_velocity.xml" /> + <arg name="metrics_config" default="$(find ocs2_cartpole)/config/multiplot/mpc_metrics.xml" /> <!-- Launch RQT Multi-plot --> <node name="mpc_observation" pkg="rqt_multiplot" type="rqt_multiplot" diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index c6d07ed02..a27bb3f62 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -122,7 +122,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR + CF_FIX ) endif(cmake_clang_tools_FOUND) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index 2a130e969..d936d79e3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -54,7 +54,7 @@ ipm ipmIteration 1 deltaTol 1e-4 g_max 10.0 - g_min 1e-4 + g_min 1e-6 computeLagrangeMultipliers true printSolverStatistics true printSolverStatus false diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h index cc9ed6f18..22a950a72 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h @@ -111,7 +111,7 @@ class LeggedRobotInterface final : public RobotInterface { mpc::Settings mpcSettings_; sqp::Settings sqpSettings_; ipm::Settings ipmSettings_; - bool useHardFrictionConeConstraint_; + const bool useHardFrictionConeConstraint_; std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_; CentroidalModelInfo centroidalModelInfo_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 14c3fcc43..36cb5669f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -65,7 +65,8 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, - bool useHardFrictionConeConstraint) { + bool useHardFrictionConeConstraint) + : useHardFrictionConeConstraint_(useHardFrictionConeConstraint) { // check that task file exists boost::filesystem::path taskFilePath(taskFile); if (boost::filesystem::exists(taskFilePath)) { @@ -93,12 +94,11 @@ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const st // load setting from loading file modelSettings_ = loadModelSettings(taskFile, "model_settings", verbose); - ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose); mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose); - rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); + ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose); sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose); ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose); - useHardFrictionConeConstraint_ = useHardFrictionConeConstraint; + rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); // OptimalConrolProblem setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose); @@ -329,13 +329,9 @@ std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getFrictionConeConst /******************************************************************************************************/ std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeSoftConstraint( size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { - FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); - auto frictionConeConstraintPtr = std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, std::move(frictionConeConConfig), - contactPointIndex, centroidalModelInfo_); - auto penalty = std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig); - - return std::make_unique<StateInputSoftConstraint>(std::move(frictionConeConstraintPtr), std::move(penalty)); + return std::make_unique<StateInputSoftConstraint>(getFrictionConeConstraint(contactPointIndex, frictionCoefficient), + std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig)); } /******************************************************************************************************/ diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp index 30a061779..e754012b9 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp @@ -72,7 +72,7 @@ int main(int argc, char** argv) { mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); - // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + // observer for friction cone constraints (only add this for debugging as it slows down the solver) if (multiplot) { auto createStateInputBoundsObserver = [&](const std::string& termName) { const ocs2::scalar_array_t observingTimePoints{0.0}; From 0a3f5ad157aada592678393908c0b6f509ff983b Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Sun, 18 Dec 2022 00:30:55 +0100 Subject: [PATCH 490/512] change printLineSearch false --- ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index d936d79e3..017dc40c3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -58,7 +58,7 @@ ipm computeLagrangeMultipliers true printSolverStatistics true printSolverStatus false - printLinesearch true + printLinesearch false useFeedbackPolicy true integratorType RK2 threadPriority 50 From a6e97e6cd4d3781d8cf70def21d08498f7acdb2a Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Sun, 18 Dec 2022 00:37:05 +0100 Subject: [PATCH 491/512] change printLineSearch false --- .../ocs2_legged_robot/src/LeggedRobotInterface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 36cb5669f..5bb70a26f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -329,7 +329,6 @@ std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getFrictionConeConst /******************************************************************************************************/ std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeSoftConstraint( size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { - auto penalty = std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig); return std::make_unique<StateInputSoftConstraint>(getFrictionConeConstraint(contactPointIndex, frictionCoefficient), std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig)); } From 7be7c496fbb570b51e4fab98556ec8025105b6a5 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Sun, 18 Dec 2022 09:38:27 +0100 Subject: [PATCH 492/512] modify trajectory spreading --- ocs2_ipm/src/IpmSolver.cpp | 32 +++++++++++++++++------- ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 12 +++------ 2 files changed, 26 insertions(+), 18 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 6566cd09e..45170db0b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -41,6 +41,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> #include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/oc_problem/OcpSize.h> +#include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h> #include "ocs2_ipm/IpmHelpers.h" #include "ocs2_ipm/IpmInitialization.h" @@ -205,7 +206,28 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } - // Initialize the state and input + // Interior point variables + scalar_t barrierParam = settings_.initialBarrierParameter; + vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; + if (!slackIneqTrajectory_.timeTrajectory.empty()) { + const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); + std::tie(slackStateIneq, slackStateInputIneq) = ipm::interpolateInteriorPointTrajectory( + primalSolution_.modeSchedule_, newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); + std::tie(dualStateIneq, dualStateInputIneq) = ipm::interpolateInteriorPointTrajectory( + primalSolution_.modeSchedule_, newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); + } else { + slackStateIneq.resize(timeDiscretization.size()); + slackStateInputIneq.resize(timeDiscretization.size() - 1); + dualStateIneq.resize(timeDiscretization.size()); + dualStateInputIneq.resize(timeDiscretization.size() - 1); + } + + // Trajectory spread of primalSolution_ after trajectory spread of the interior point variables + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + } + + // Initialize the state and input after trajectory spread of primalSolution_ vector_array_t x, u; multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); @@ -216,14 +238,6 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f initializeProjectionMultiplierTrajectory(timeDiscretization, nu); } - // Interior point variables - scalar_t barrierParam = settings_.initialBarrierParameter; - vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; - std::tie(slackStateIneq, slackStateInputIneq) = ipm::interpolateInteriorPointTrajectory( - primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(slackIneqTrajectory_)); - std::tie(dualStateIneq, dualStateInputIneq) = ipm::interpolateInteriorPointTrajectory( - primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), timeDiscretization, std::move(dualIneqTrajectory_)); - // Bookkeeping performanceIndeces_.clear(); std::vector<Metrics> metrics; diff --git a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp index 3a9a2cdb3..9176ac3d7 100644 --- a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp +++ b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp @@ -38,19 +38,13 @@ namespace { MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { MultiplierCollection multiplierCollection; multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); - if (stateInputIneq.size() > 0) { - multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); - } + multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); return multiplierCollection; } std::pair<vector_t, vector_t> fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { - if (multiplierCollection.stateInputIneq.empty()) { - return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), vector_t()); - } else { - return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), - std::move(multiplierCollection.stateInputIneq.front().lagrangian)); - } + return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), + std::move(multiplierCollection.stateInputIneq.front().lagrangian)); } } // namespace } // namespace ocs2 From 1eac5b65a03e9a31fadfa325a86be8aaf6172b79 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Sun, 18 Dec 2022 10:35:30 +0000 Subject: [PATCH 493/512] changing to CF_WERROR --- ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index a27bb3f62..c6d07ed02 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -122,7 +122,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) From f42ec7fb3184e2f312e5087802b45a2376dc6f1c Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Sun, 18 Dec 2022 12:19:03 +0100 Subject: [PATCH 494/512] refactored trajectory spread of Ipm variables --- ocs2_ipm/src/IpmSolver.cpp | 11 +++++++---- ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 5 ----- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 45170db0b..9b2651f01 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -210,11 +210,14 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f scalar_t barrierParam = settings_.initialBarrierParameter; vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; if (!slackIneqTrajectory_.timeTrajectory.empty()) { + const auto& oldModeSchedule = primalSolution_.modeSchedule_; const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); - std::tie(slackStateIneq, slackStateInputIneq) = ipm::interpolateInteriorPointTrajectory( - primalSolution_.modeSchedule_, newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); - std::tie(dualStateIneq, dualStateInputIneq) = ipm::interpolateInteriorPointTrajectory( - primalSolution_.modeSchedule_, newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackIneqTrajectory_); + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, dualIneqTrajectory_); + std::tie(slackStateIneq, slackStateInputIneq) = + ipm::interpolateInteriorPointTrajectory(oldModeSchedule, newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); + std::tie(dualStateIneq, dualStateInputIneq) = + ipm::interpolateInteriorPointTrajectory(oldModeSchedule, newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); } else { slackStateIneq.resize(timeDiscretization.size()); slackStateInputIneq.resize(timeDiscretization.size() - 1); diff --git a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp index 9176ac3d7..646c3c2dc 100644 --- a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp +++ b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp @@ -112,11 +112,6 @@ std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(con DualSolution&& oldDualSolution) { const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; const auto oldPostEventIndices = oldDualSolution.postEventIndices; - - if (!oldTimeTrajectory.empty()) { - std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, oldDualSolution); - } - const auto newTimeTrajectory = toInterpolationTime(time); const auto newPostEventIndices = toPostEventIndices(time); From 95e0aacd3959aad31d50a852c9e4e56e556bbc32 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Sun, 18 Dec 2022 12:23:19 +0100 Subject: [PATCH 495/512] add trajectorySpread of primalSolution --- ocs2_slp/src/SlpSolver.cpp | 6 ++++++ ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp index 428bf9bad..81c2a6c9c 100644 --- a/ocs2_slp/src/SlpSolver.cpp +++ b/ocs2_slp/src/SlpSolver.cpp @@ -39,6 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> #include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/precondition/Ruzi.h> +#include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h> #include "ocs2_slp/Helpers.h" @@ -172,6 +173,11 @@ void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } + // Trajectory spread of primalSolution_ + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + } + // Initialize the state and input vector_array_t x, u; multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index ff2f658e2..5e0e38379 100644 --- a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -39,6 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> #include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/oc_problem/OcpSize.h> +#include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h> namespace ocs2 { @@ -170,6 +171,11 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } + // Trajectory spread of primalSolution_ + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + } + // Initialize the state and input vector_array_t x, u; multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); From 4a81c9e1b78104884282569d290999c747981031 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Sun, 18 Dec 2022 12:27:50 +0100 Subject: [PATCH 496/512] move functions to IpmHelpers --- ocs2_ipm/CMakeLists.txt | 1 - ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 36 ++++ .../ocs2_ipm/IpmTrajectoryAdjustment.h | 75 -------- ocs2_ipm/src/IpmHelpers.cpp | 129 ++++++++++++++ ocs2_ipm/src/IpmSolver.cpp | 1 - ocs2_ipm/src/IpmTrajectoryAdjustment.cpp | 168 ------------------ 6 files changed, 165 insertions(+), 245 deletions(-) delete mode 100644 ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h delete mode 100644 ocs2_ipm/src/IpmTrajectoryAdjustment.cpp diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index 655a7a8ac..bd02a70cc 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -53,7 +53,6 @@ add_library(${PROJECT_NAME} src/IpmPerformanceIndexComputation.cpp src/IpmSettings.cpp src/IpmSolver.cpp - src/IpmTrajectoryAdjustment.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 9531582a9..24d2dad39 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -30,6 +30,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include <ocs2_core/Types.h> +#include <ocs2_oc/oc_data/DualSolution.h> +#include <ocs2_oc/oc_data/TimeDiscretization.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> namespace ocs2 { @@ -107,5 +109,39 @@ vector_t retrieveDualDirection(scalar_t barrierParam, const vector_t& slack, con */ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scalar_t marginRate = 0.995); +/** + * Convert the optimized slack or dual trajectories as a DualSolution. + * + * @param time: The time discretization. + * @param stateIneq: The slack/dual of the state inequality constraints. + * @param stateInputIneq: The slack/dual of the state-input inequality constraints. + * @return A dual solution. + */ +DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq); + +/** + * Convert the optimized slack or dual trajectories as a DualSolution. + * + * @param time: The time discretization. + * @param dualSolution: The dual solution. + * @return The slack/dual of the state inequality constraints (first) and state-input inequality constraints (second). + */ +std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution); + +/** + * Interpolates the interior point trajectory using the stored interior point trajectory. The part of the new trajectory that is not + * covered by the stored trajectory is set by vectors of zero size. + * + * @param oldModeSchedule: Mode schedule of the previous optimization. + * @param newModeSchedule: Mode schedule of the new optimization. + * @param time: Time discretization. + * @param oldDualSolution: The old dual solution that stored the previous interior point trajectory. + * @return Interpolated interior point trajectory. + */ +std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector<AnnotatedTime>& time, + DualSolution&& oldDualSolution); + } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h b/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h deleted file mode 100644 index cea8a41ae..000000000 --- a/ocs2_ipm/include/ocs2_ipm/IpmTrajectoryAdjustment.h +++ /dev/null @@ -1,75 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#pragma once - -#include <ocs2_core/Types.h> - -#include <ocs2_oc/oc_data/DualSolution.h> -#include <ocs2_oc/oc_data/TimeDiscretization.h> - -namespace ocs2 { -namespace ipm { - -/** - * Convert the optimized slack or dual trajectories as a DualSolution. - * - * @param time: The time discretization. - * @param stateIneq: The slack/dual of the state inequality constraints. - * @param stateInputIneq: The slack/dual of the state-input inequality constraints. - * @return A dual solution. - */ -DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq); - -/** - * Convert the optimized slack or dual trajectories as a DualSolution. - * - * @param time: The time discretization. - * @param dualSolution: The dual solution. - * @return The slack/dual of the state inequality constraints (first) and state-input inequality constraints (second). - */ -std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution); - -/** - * Interpolates the interior point trajectory using the stored interior point trajectory. The part of the new trajectory that is not - * covered by the stored trajectory is set by vectors of zero size. - * - * @param oldModeSchedule: Mode schedule of the previous optimization. - * @param newModeSchedule: Mode schedule of the new optimization. - * @param time: Time discretization. - * @param oldDualSolution: The old dual solution that stored the previous interior point trajectory. - * @return Interpolated interior point trajectory. - */ -std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, - const std::vector<AnnotatedTime>& time, - DualSolution&& oldDualSolution); - -} // namespace ipm -} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index ea7188006..1d6e3e2b5 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -31,6 +31,23 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <cassert> +namespace ocs2 { +namespace { + +MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { + MultiplierCollection multiplierCollection; + multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); + multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); + return multiplierCollection; +} + +std::pair<vector_t, vector_t> fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { + return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), + std::move(multiplierCollection.stateInputIneq.front().lagrangian)); +} +} // namespace +} // namespace ocs2 + namespace ocs2 { namespace ipm { @@ -113,5 +130,117 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala return alpha > 0.0 ? std::min(1.0 / alpha, 1.0) : 1.0; } +DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq) { + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + + DualSolution dualSolution; + const auto newTimeTrajectory = toInterpolationTime(time); + dualSolution.postEventIndices = toPostEventIndices(time); + + dualSolution.preJumps.reserve(dualSolution.postEventIndices.size()); + dualSolution.intermediates.reserve(time.size()); + + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(stateIneq[i]))); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node + } else { + dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(stateIneq[i]), std::move(stateInputIneq[i]))); + } + } + dualSolution.final = toMultiplierCollection(std::move(stateIneq[N])); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); + + return dualSolution; +} + +std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution) { + // Problem horizon + const int N = static_cast<int>(time.size()) - 1; + + vector_array_t stateIneq; + vector_array_t stateInputIneq; + stateIneq.reserve(N + 1); + stateInputIneq.reserve(N); + + int preJumpIdx = 0; + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + auto result = fromMultiplierCollection(std::move(dualSolution.preJumps[preJumpIdx])); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + ++preJumpIdx; + } else { + auto result = fromMultiplierCollection(std::move(dualSolution.intermediates[i])); + stateIneq.emplace_back(std::move(result.first)); + stateInputIneq.emplace_back(std::move(result.second)); + } + } + + auto result = fromMultiplierCollection(std::move(dualSolution.final)); + stateIneq.emplace_back(std::move(result.first)); + + return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); +} + +std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, + const ModeSchedule& newModeSchedule, + const std::vector<AnnotatedTime>& time, + DualSolution&& oldDualSolution) { + const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; + const auto oldPostEventIndices = oldDualSolution.postEventIndices; + const auto newTimeTrajectory = toInterpolationTime(time); + const auto newPostEventIndices = toPostEventIndices(time); + + // find the time period that we can interpolate the cached solution + const auto timePeriod = std::make_pair(newTimeTrajectory.front(), newTimeTrajectory.back()); + const auto interpolatableTimePeriod = findIntersectionToExtendableInterval(oldTimeTrajectory, newModeSchedule.eventTimes, timePeriod); + const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); + + DualSolution newDualSolution; + + // set time and post-event indices + newDualSolution.timeTrajectory = std::move(newTimeTrajectory); + newDualSolution.postEventIndices = std::move(newPostEventIndices); + + // final + if (interpolateTillFinalTime) { + newDualSolution.final = std::move(oldDualSolution.final); + } else { + newDualSolution.final = toMultiplierCollection(vector_t()); + } + + // pre-jumps + newDualSolution.preJumps.resize(newDualSolution.postEventIndices.size()); + if (!newDualSolution.postEventIndices.empty()) { + const auto firstEventTime = newDualSolution.timeTrajectory[newDualSolution.postEventIndices[0] - 1]; + const auto cacheEventIndexBias = getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); + + for (size_t i = 0; i < newDualSolution.postEventIndices.size(); i++) { + const auto cachedTimeIndex = cacheEventIndexBias + i; + if (cachedTimeIndex < oldDualSolution.preJumps.size()) { + newDualSolution.preJumps[i] = std::move(oldDualSolution.preJumps[cachedTimeIndex]); + } else { + newDualSolution.preJumps[i] = toMultiplierCollection(vector_t()); + } + } + } + + // intermediates + newDualSolution.intermediates.resize(newDualSolution.timeTrajectory.size() - 1); + for (size_t i = 0; i < newDualSolution.timeTrajectory.size() - 1; i++) { + const auto time = newDualSolution.timeTrajectory[i]; + + if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { + newDualSolution.intermediates[i] = getIntermediateDualSolutionAtTime(oldDualSolution, time); + } else { + newDualSolution.intermediates[i] = toMultiplierCollection(vector_t(), vector_t()); + } + } + + return fromDualSolution(time, std::move(newDualSolution)); +} + } // namespace ipm } // namespace ocs2 diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 9b2651f01..2595e1f5b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -46,7 +46,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ipm/IpmHelpers.h" #include "ocs2_ipm/IpmInitialization.h" #include "ocs2_ipm/IpmPerformanceIndexComputation.h" -#include "ocs2_ipm/IpmTrajectoryAdjustment.h" namespace ocs2 { diff --git a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp b/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp deleted file mode 100644 index 646c3c2dc..000000000 --- a/ocs2_ipm/src/IpmTrajectoryAdjustment.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -******************************************************************************/ - -#include "ocs2_ipm/IpmTrajectoryAdjustment.h" - -#include <ocs2_oc/trajectory_adjustment/TrajectorySpreading.h> -#include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h> - -namespace ocs2 { -namespace { - -MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { - MultiplierCollection multiplierCollection; - multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); - multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); - return multiplierCollection; -} - -std::pair<vector_t, vector_t> fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { - return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), - std::move(multiplierCollection.stateInputIneq.front().lagrangian)); -} -} // namespace -} // namespace ocs2 - -namespace ocs2 { -namespace ipm { - -DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq) { - // Problem horizon - const int N = static_cast<int>(time.size()) - 1; - - DualSolution dualSolution; - const auto newTimeTrajectory = toInterpolationTime(time); - dualSolution.postEventIndices = toPostEventIndices(time); - - dualSolution.preJumps.reserve(dualSolution.postEventIndices.size()); - dualSolution.intermediates.reserve(time.size()); - - for (int i = 0; i < N; ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent) { - dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(stateIneq[i]))); - dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node - } else { - dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(stateIneq[i]), std::move(stateInputIneq[i]))); - } - } - dualSolution.final = toMultiplierCollection(std::move(stateIneq[N])); - dualSolution.intermediates.push_back(dualSolution.intermediates.back()); - - return dualSolution; -} - -std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution) { - // Problem horizon - const int N = static_cast<int>(time.size()) - 1; - - vector_array_t stateIneq; - vector_array_t stateInputIneq; - stateIneq.reserve(N + 1); - stateInputIneq.reserve(N); - - int preJumpIdx = 0; - for (int i = 0; i < N; ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent) { - auto result = fromMultiplierCollection(std::move(dualSolution.preJumps[preJumpIdx])); - stateIneq.emplace_back(std::move(result.first)); - stateInputIneq.emplace_back(std::move(result.second)); - ++preJumpIdx; - } else { - auto result = fromMultiplierCollection(std::move(dualSolution.intermediates[i])); - stateIneq.emplace_back(std::move(result.first)); - stateInputIneq.emplace_back(std::move(result.second)); - } - } - - auto result = fromMultiplierCollection(std::move(dualSolution.final)); - stateIneq.emplace_back(std::move(result.first)); - - return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); -} - -std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, - const std::vector<AnnotatedTime>& time, - DualSolution&& oldDualSolution) { - const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; - const auto oldPostEventIndices = oldDualSolution.postEventIndices; - const auto newTimeTrajectory = toInterpolationTime(time); - const auto newPostEventIndices = toPostEventIndices(time); - - // find the time period that we can interpolate the cached solution - const auto timePeriod = std::make_pair(newTimeTrajectory.front(), newTimeTrajectory.back()); - const auto interpolatableTimePeriod = findIntersectionToExtendableInterval(oldTimeTrajectory, newModeSchedule.eventTimes, timePeriod); - const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); - - DualSolution newDualSolution; - - // set time and post-event indices - newDualSolution.timeTrajectory = std::move(newTimeTrajectory); - newDualSolution.postEventIndices = std::move(newPostEventIndices); - - // final - if (interpolateTillFinalTime) { - newDualSolution.final = std::move(oldDualSolution.final); - } else { - newDualSolution.final = toMultiplierCollection(vector_t()); - } - - // pre-jumps - newDualSolution.preJumps.resize(newDualSolution.postEventIndices.size()); - if (!newDualSolution.postEventIndices.empty()) { - const auto firstEventTime = newDualSolution.timeTrajectory[newDualSolution.postEventIndices[0] - 1]; - const auto cacheEventIndexBias = getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); - - for (size_t i = 0; i < newDualSolution.postEventIndices.size(); i++) { - const auto cachedTimeIndex = cacheEventIndexBias + i; - if (cachedTimeIndex < oldDualSolution.preJumps.size()) { - newDualSolution.preJumps[i] = std::move(oldDualSolution.preJumps[cachedTimeIndex]); - } else { - newDualSolution.preJumps[i] = toMultiplierCollection(vector_t()); - } - } - } - - // intermediates - newDualSolution.intermediates.resize(newDualSolution.timeTrajectory.size() - 1); - for (size_t i = 0; i < newDualSolution.timeTrajectory.size() - 1; i++) { - const auto time = newDualSolution.timeTrajectory[i]; - - if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { - newDualSolution.intermediates[i] = getIntermediateDualSolutionAtTime(oldDualSolution, time); - } else { - newDualSolution.intermediates[i] = toMultiplierCollection(vector_t(), vector_t()); - } - } - - return fromDualSolution(time, std::move(newDualSolution)); -} - -} // namespace ipm -} // namespace ocs2 From 02ebd11d5be34bb6e6f3feb125bf3f2ef6d18bed Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Sun, 18 Dec 2022 15:48:15 +0100 Subject: [PATCH 497/512] remove unused argment --- ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 6 ++---- ocs2_ipm/src/IpmHelpers.cpp | 5 ++--- ocs2_ipm/src/IpmSolver.cpp | 4 ++-- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 24d2dad39..041994304 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -132,14 +132,12 @@ std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<Ann * Interpolates the interior point trajectory using the stored interior point trajectory. The part of the new trajectory that is not * covered by the stored trajectory is set by vectors of zero size. * - * @param oldModeSchedule: Mode schedule of the previous optimization. - * @param newModeSchedule: Mode schedule of the new optimization. + * @param modeSchedule: Mode schedule. * @param time: Time discretization. * @param oldDualSolution: The old dual solution that stored the previous interior point trajectory. * @return Interpolated interior point trajectory. */ -std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, +std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& modeSchedule, const std::vector<AnnotatedTime>& time, DualSolution&& oldDualSolution); diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index 1d6e3e2b5..cc0c8ba8a 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -184,8 +184,7 @@ std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<Ann return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); } -std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& oldModeSchedule, - const ModeSchedule& newModeSchedule, +std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& modeSchedule, const std::vector<AnnotatedTime>& time, DualSolution&& oldDualSolution) { const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; @@ -195,7 +194,7 @@ std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(con // find the time period that we can interpolate the cached solution const auto timePeriod = std::make_pair(newTimeTrajectory.front(), newTimeTrajectory.back()); - const auto interpolatableTimePeriod = findIntersectionToExtendableInterval(oldTimeTrajectory, newModeSchedule.eventTimes, timePeriod); + const auto interpolatableTimePeriod = findIntersectionToExtendableInterval(oldTimeTrajectory, modeSchedule.eventTimes, timePeriod); const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); DualSolution newDualSolution; diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 2595e1f5b..79bb24bc2 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -214,9 +214,9 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackIneqTrajectory_); std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, dualIneqTrajectory_); std::tie(slackStateIneq, slackStateInputIneq) = - ipm::interpolateInteriorPointTrajectory(oldModeSchedule, newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); + ipm::interpolateInteriorPointTrajectory(newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); std::tie(dualStateIneq, dualStateInputIneq) = - ipm::interpolateInteriorPointTrajectory(oldModeSchedule, newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); + ipm::interpolateInteriorPointTrajectory(newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); } else { slackStateIneq.resize(timeDiscretization.size()); slackStateInputIneq.resize(timeDiscretization.size() - 1); From 047f5dcddc9271313658edc48101a93b7c4f2f29 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 19 Dec 2022 16:18:57 +0100 Subject: [PATCH 498/512] modify initialization of the slack and dual trajectory --- ocs2_ipm/CMakeLists.txt | 2 +- ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 36 ++-- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 18 +- ocs2_ipm/src/IpmHelpers.cpp | 140 ++++----------- ocs2_ipm/src/IpmSolver.cpp | 198 ++++++++++++++------- ocs2_oc/src/oc_data/TimeDiscretization.cpp | 3 +- 6 files changed, 203 insertions(+), 194 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index bd02a70cc..d5e27aae7 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -73,7 +73,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR + CF_FIX ) endif(cmake_clang_tools_FOUND) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 041994304..23e97b716 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -113,33 +113,35 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala * Convert the optimized slack or dual trajectories as a DualSolution. * * @param time: The time discretization. - * @param stateIneq: The slack/dual of the state inequality constraints. - * @param stateInputIneq: The slack/dual of the state-input inequality constraints. + * @param slackStateIneq: The slack variable trajectory of the state inequality constraints. + * @param dualStateIneq: The dual variable trajectory of the state inequality constraints. + * @param slackStateInputIneq: The slack variable trajectory of the state-input inequality constraints. + * @param dualStateInputIneq: The dual variable trajectory of the state-input inequality constraints. * @return A dual solution. */ -DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq); +DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq, + vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq); /** - * Convert the optimized slack or dual trajectories as a DualSolution. + * Extracts a slack variable of the state-only and state-input constraints from a MultiplierCollection * - * @param time: The time discretization. - * @param dualSolution: The dual solution. - * @return The slack/dual of the state inequality constraints (first) and state-input inequality constraints (second). + * @param[in] multiplierCollection: The MultiplierCollection. + * @param[out] slackStateIneq: The slack variable of the state inequality constraints. + * @param[out] dualStateIneq: The dual variable of the state inequality constraints. + * @param[out] slackStateInputIneq: The slack variable of the state-input inequality constraints. + * @param[out] dualStateInputIneq: The dual variable of the state-input inequality constraints. */ -std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution); +void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, + vector_t& slackStateInputIneq, vector_t& dualStateInputIneq); /** - * Interpolates the interior point trajectory using the stored interior point trajectory. The part of the new trajectory that is not - * covered by the stored trajectory is set by vectors of zero size. + * Extracts a slack variable of the state-only constraints from a MultiplierCollection * - * @param modeSchedule: Mode schedule. - * @param time: Time discretization. - * @param oldDualSolution: The old dual solution that stored the previous interior point trajectory. - * @return Interpolated interior point trajectory. + * @param[in] multiplierCollection: The MultiplierCollection. + * @param[out] slackStateIneq: The slack variable of the state inequality constraints. + * @param[out] dualStateIneq: The dual variable of the state inequality constraints. */ -std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& modeSchedule, - const std::vector<AnnotatedTime>& time, - DualSolution&& oldDualSolution); +void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq); } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 410bd45c2..073eae008 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -123,12 +123,17 @@ class IpmSolver : public SolverBase { void initializeProjectionMultiplierTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, vector_array_t& projectionMultiplierTrajectory) const; + /** Initializes for the slack and dual trajectories of the hard inequality constraints */ + void initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& x, const vector_array_t& u, + scalar_t barrierParam, vector_array_t& slackStateIneq, vector_array_t& dualStateIneq, + vector_array_t& slackStateInputIneq, vector_array_t& dualStateInputIneq); + /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, const vector_array_t& u, const vector_array_t& lmd, const vector_array_t& nu, - scalar_t barrierParam, vector_array_t& slackStateIneq, vector_array_t& slackStateInputIneq, - vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq, - std::vector<Metrics>& metrics); + scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateIneq, + const vector_array_t& dualStateInputIneq, std::vector<Metrics>& metrics); /** Computes only the performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, @@ -142,15 +147,15 @@ class IpmSolver : public SolverBase { vector_array_t deltaLmdSol; // delta_lmd(t) vector_array_t deltaNuSol; // delta_nu(t) vector_array_t deltaSlackStateIneq; - vector_array_t deltaSlackStateInputIneq; vector_array_t deltaDualStateIneq; + vector_array_t deltaSlackStateInputIneq; vector_array_t deltaDualStateInputIneq; scalar_t armijoDescentMetric; // inner product of the cost gradient and decision variable step scalar_t maxPrimalStepSize; scalar_t maxDualStepSize; }; OcpSubproblemSolution getOCPSolution(const vector_t& delta_x0, scalar_t barrierParam, const vector_array_t& slackStateIneq, - const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateIneq, + const vector_array_t& dualStateIneq, const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateInputIneq); /** Extract the value function based on the last solved QP */ @@ -196,8 +201,7 @@ class IpmSolver : public SolverBase { PrimalSolution primalSolution_; vector_array_t costateTrajectory_; vector_array_t projectionMultiplierTrajectory_; - DualSolution slackIneqTrajectory_; - DualSolution dualIneqTrajectory_; + DualSolution slackDualTrajectory_; // Value function in absolute state coordinates (without the constant value) std::vector<ScalarFunctionQuadraticApproximation> valueFunction_; diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index cc0c8ba8a..d962cf1c9 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -31,23 +31,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <cassert> -namespace ocs2 { -namespace { - -MultiplierCollection toMultiplierCollection(vector_t&& stateIneq, vector_t&& stateInputIneq = vector_t()) { - MultiplierCollection multiplierCollection; - multiplierCollection.stateIneq.emplace_back(0.0, std::move(stateIneq)); - multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(stateInputIneq)); - return multiplierCollection; -} - -std::pair<vector_t, vector_t> fromMultiplierCollection(MultiplierCollection&& multiplierCollection) { - return std::make_pair(std::move(multiplierCollection.stateIneq.front().lagrangian), - std::move(multiplierCollection.stateInputIneq.front().lagrangian)); -} -} // namespace -} // namespace ocs2 - namespace ocs2 { namespace ipm { @@ -130,12 +113,38 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala return alpha > 0.0 ? std::min(1.0 / alpha, 1.0) : 1.0; } -DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& stateIneq, vector_array_t&& stateInputIneq) { +namespace { +MultiplierCollection toMultiplierCollection(vector_t&& slackStateIneq, vector_t&& dualStateIneq, + vector_t&& slackStateInputIneq = vector_t(), vector_t&& dualStateInputIneq = vector_t()) { + MultiplierCollection multiplierCollection; + multiplierCollection.stateIneq.emplace_back(0.0, std::move(slackStateIneq)); + multiplierCollection.stateEq.emplace_back(0.0, std::move(dualStateIneq)); + multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(slackStateInputIneq)); + multiplierCollection.stateInputEq.emplace_back(0.0, std::move(dualStateInputIneq)); + return multiplierCollection; +} +} // namespace + +void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, + vector_t& slackStateInputIneq, vector_t& dualStateInputIneq) { + slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian); + dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian); + slackStateInputIneq = std::move(multiplierCollection.stateInputIneq.front().lagrangian); + dualStateInputIneq = std::move(multiplierCollection.stateInputEq.front().lagrangian); +} + +void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq) { + slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian); + dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian); +} + +DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq, + vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; DualSolution dualSolution; - const auto newTimeTrajectory = toInterpolationTime(time); + dualSolution.timeTrajectory = toInterpolationTime(time); dualSolution.postEventIndices = toPostEventIndices(time); dualSolution.preJumps.reserve(dualSolution.postEventIndices.size()); @@ -143,103 +152,18 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array for (int i = 0; i < N; ++i) { if (time[i].event == AnnotatedTime::Event::PreEvent) { - dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(stateIneq[i]))); + dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i]))); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node } else { - dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(stateIneq[i]), std::move(stateInputIneq[i]))); + dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i]), + std::move(slackStateInputIneq[i]), std::move(dualStateInputIneq[i]))); } } - dualSolution.final = toMultiplierCollection(std::move(stateIneq[N])); + dualSolution.final = toMultiplierCollection(std::move(slackStateIneq[N]), std::move(dualStateIneq[N])); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); return dualSolution; } -std::pair<vector_array_t, vector_array_t> fromDualSolution(const std::vector<AnnotatedTime>& time, DualSolution&& dualSolution) { - // Problem horizon - const int N = static_cast<int>(time.size()) - 1; - - vector_array_t stateIneq; - vector_array_t stateInputIneq; - stateIneq.reserve(N + 1); - stateInputIneq.reserve(N); - - int preJumpIdx = 0; - for (int i = 0; i < N; ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent) { - auto result = fromMultiplierCollection(std::move(dualSolution.preJumps[preJumpIdx])); - stateIneq.emplace_back(std::move(result.first)); - stateInputIneq.emplace_back(std::move(result.second)); - ++preJumpIdx; - } else { - auto result = fromMultiplierCollection(std::move(dualSolution.intermediates[i])); - stateIneq.emplace_back(std::move(result.first)); - stateInputIneq.emplace_back(std::move(result.second)); - } - } - - auto result = fromMultiplierCollection(std::move(dualSolution.final)); - stateIneq.emplace_back(std::move(result.first)); - - return std::make_pair(std::move(stateIneq), std::move(stateInputIneq)); -} - -std::pair<vector_array_t, vector_array_t> interpolateInteriorPointTrajectory(const ModeSchedule& modeSchedule, - const std::vector<AnnotatedTime>& time, - DualSolution&& oldDualSolution) { - const auto oldTimeTrajectory = oldDualSolution.timeTrajectory; - const auto oldPostEventIndices = oldDualSolution.postEventIndices; - const auto newTimeTrajectory = toInterpolationTime(time); - const auto newPostEventIndices = toPostEventIndices(time); - - // find the time period that we can interpolate the cached solution - const auto timePeriod = std::make_pair(newTimeTrajectory.front(), newTimeTrajectory.back()); - const auto interpolatableTimePeriod = findIntersectionToExtendableInterval(oldTimeTrajectory, modeSchedule.eventTimes, timePeriod); - const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); - - DualSolution newDualSolution; - - // set time and post-event indices - newDualSolution.timeTrajectory = std::move(newTimeTrajectory); - newDualSolution.postEventIndices = std::move(newPostEventIndices); - - // final - if (interpolateTillFinalTime) { - newDualSolution.final = std::move(oldDualSolution.final); - } else { - newDualSolution.final = toMultiplierCollection(vector_t()); - } - - // pre-jumps - newDualSolution.preJumps.resize(newDualSolution.postEventIndices.size()); - if (!newDualSolution.postEventIndices.empty()) { - const auto firstEventTime = newDualSolution.timeTrajectory[newDualSolution.postEventIndices[0] - 1]; - const auto cacheEventIndexBias = getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); - - for (size_t i = 0; i < newDualSolution.postEventIndices.size(); i++) { - const auto cachedTimeIndex = cacheEventIndexBias + i; - if (cachedTimeIndex < oldDualSolution.preJumps.size()) { - newDualSolution.preJumps[i] = std::move(oldDualSolution.preJumps[cachedTimeIndex]); - } else { - newDualSolution.preJumps[i] = toMultiplierCollection(vector_t()); - } - } - } - - // intermediates - newDualSolution.intermediates.resize(newDualSolution.timeTrajectory.size() - 1); - for (size_t i = 0; i < newDualSolution.timeTrajectory.size() - 1; i++) { - const auto time = newDualSolution.timeTrajectory[i]; - - if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { - newDualSolution.intermediates[i] = getIntermediateDualSolutionAtTime(oldDualSolution, time); - } else { - newDualSolution.intermediates[i] = toMultiplierCollection(vector_t(), vector_t()); - } - } - - return fromDualSolution(time, std::move(newDualSolution)); -} - } // namespace ipm } // namespace ocs2 diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 79bb24bc2..e0bf26c7f 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -101,8 +101,7 @@ void IpmSolver::reset() { primalSolution_ = PrimalSolution(); costateTrajectory_.clear(); projectionMultiplierTrajectory_.clear(); - slackIneqTrajectory_.clear(); - dualIneqTrajectory_.clear(); + slackDualTrajectory_.clear(); valueFunction_.clear(); performanceIndeces_.clear(); @@ -205,34 +204,26 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } - // Interior point variables - scalar_t barrierParam = settings_.initialBarrierParameter; - vector_array_t slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq; - if (!slackIneqTrajectory_.timeTrajectory.empty()) { - const auto& oldModeSchedule = primalSolution_.modeSchedule_; - const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); - std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackIneqTrajectory_); - std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, dualIneqTrajectory_); - std::tie(slackStateIneq, slackStateInputIneq) = - ipm::interpolateInteriorPointTrajectory(newModeSchedule, timeDiscretization, std::move(slackIneqTrajectory_)); - std::tie(dualStateIneq, dualStateInputIneq) = - ipm::interpolateInteriorPointTrajectory(newModeSchedule, timeDiscretization, std::move(dualIneqTrajectory_)); - } else { - slackStateIneq.resize(timeDiscretization.size()); - slackStateInputIneq.resize(timeDiscretization.size() - 1); - dualStateIneq.resize(timeDiscretization.size()); - dualStateInputIneq.resize(timeDiscretization.size() - 1); - } + // old and new mode schedules for the trajectory spreading + const auto oldModeSchedule = primalSolution_.modeSchedule_; + const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); - // Trajectory spread of primalSolution_ after trajectory spread of the interior point variables + // Initialize the state and input if (!primalSolution_.timeTrajectory_.empty()) { - std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, primalSolution_); } - - // Initialize the state and input after trajectory spread of primalSolution_ vector_array_t x, u; multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); + // Initialize the slack and dual variables of the interior point method + if (!slackDualTrajectory_.timeTrajectory.empty()) { + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackDualTrajectory_); + } + scalar_t barrierParam = settings_.initialBarrierParameter; + vector_array_t slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq; + initializeSlackDualTrajectory(timeDiscretization, x, u, barrierParam, slackStateIneq, dualStateIneq, slackStateInputIneq, + dualStateInputIneq); + // Initialize the costate and projection multiplier vector_array_t lmd, nu; if (settings_.computeLagrangeMultipliers) { @@ -261,7 +252,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f solveQpTimer_.startTimer(); const vector_t delta_x0 = initState - x[0]; const auto deltaSolution = - getOCPSolution(delta_x0, barrierParam, slackStateIneq, slackStateInputIneq, dualStateIneq, dualStateInputIneq); + getOCPSolution(delta_x0, barrierParam, slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq); extractValueFunction(timeDiscretization, x, lmd, deltaSolution.deltaXSol); solveQpTimer_.endTimer(); @@ -291,8 +282,8 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); costateTrajectory_ = std::move(lmd); projectionMultiplierTrajectory_ = std::move(nu); - slackIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(slackStateIneq), std::move(slackStateInputIneq)); - dualIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(dualStateIneq), std::move(dualStateInputIneq)); + slackDualTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(slackStateIneq), std::move(dualStateIneq), + std::move(slackStateInputIneq), std::move(dualStateInputIneq)); problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); @@ -380,9 +371,122 @@ void IpmSolver::initializeProjectionMultiplierTrajectory(const std::vector<Annot } } +void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& timeDiscretization, const vector_array_t& x, + const vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& dualStateIneq, vector_array_t& slackStateInputIneq, + vector_array_t& dualStateInputIneq) { + const auto& oldTimeTrajectory = slackDualTrajectory_.timeTrajectory; + const auto& oldPostEventIndices = slackDualTrajectory_.postEventIndices; + const auto newTimeTrajectory = toInterpolationTime(timeDiscretization); + const auto newPostEventIndices = toPostEventIndices(timeDiscretization); + + // find the time period that we can interpolate the cached solution + const auto timePeriod = std::make_pair(newTimeTrajectory.front(), newTimeTrajectory.back()); + const auto interpolatableTimePeriod = + findIntersectionToExtendableInterval(oldTimeTrajectory, this->getReferenceManager().getModeSchedule().eventTimes, timePeriod); + const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); + const auto cacheEventIndexBias = [&]() -> size_t { + if (!newPostEventIndices.empty()) { + const auto firstEventTime = newTimeTrajectory[newPostEventIndices[0] - 1]; + return getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); + } else { + return 0; + } + }(); + + auto& ocpDefinition = ocpDefinitions_.front(); + const size_t N = static_cast<int>(timeDiscretization.size()) - 1; // size of the input trajectory + slackStateIneq.resize(N + 1); + dualStateIneq.resize(N + 1); + slackStateInputIneq.resize(N); + dualStateInputIneq.resize(N); + + int eventIdx = 0; + for (size_t i = 0; i < N; i++) { + if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { + const auto cachedEventIndex = cacheEventIndexBias + eventIdx; + if (cachedEventIndex < slackDualTrajectory_.preJumps.size()) { + ipm::toSlackDual(std::move(slackDualTrajectory_.preJumps[cachedEventIndex]), slackStateIneq[i], dualStateIneq[i]); + } else { + const auto time = newTimeTrajectory[i]; + const auto& state = x[i]; + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestPreJump(request, time, state); + auto& preComputation = *ocpDefinition.preComputationPtr; + if (!ocpDefinition.preJumpInequalityConstraintPtr->empty()) { + const auto ineqConstraint = toVector(ocpDefinition.preJumpInequalityConstraintPtr->getValue(time, state, preComputation)); + slackStateIneq[i] = + ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } else { + slackStateIneq[i].resize(0); + dualStateIneq[i].resize(0); + } + } + slackStateInputIneq[i].resize(0); + dualStateInputIneq[i].resize(0); + ++eventIdx; + } else { + const auto time = newTimeTrajectory[i]; + if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { + ipm::toSlackDual(getIntermediateDualSolutionAtTime(slackDualTrajectory_, time), slackStateIneq[i], dualStateIneq[i], + slackStateInputIneq[i], dualStateInputIneq[i]); + } else { + constexpr auto request = Request::Constraint; + const auto& state = x[i]; + const auto& input = u[i]; + ocpDefinition.preComputationPtr->request(request, time, state, input); + if (!ocpDefinition.stateInequalityConstraintPtr->empty() && i > 0) { + const auto ineqConstraint = + toVector(ocpDefinition.stateInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + slackStateIneq[i] = + ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } else { + slackStateIneq[i].resize(0); + dualStateIneq[i].resize(0); + } + if (!ocpDefinition.inequalityConstraintPtr->empty()) { + const auto ineqConstraint = + toVector(ocpDefinition.inequalityConstraintPtr->getValue(time, state, input, *ocpDefinition.preComputationPtr)); + slackStateInputIneq[i] = + ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } else { + slackStateInputIneq[i].resize(0); + dualStateInputIneq[i].resize(0); + } + } + } + } + + if (interpolateTillFinalTime) { + ipm::toSlackDual(std::move(slackDualTrajectory_.final), slackStateIneq[N], dualStateIneq[N]); + } else { + const auto time = newTimeTrajectory[N]; + const auto& state = x[N]; + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestFinal(request, time, state); + auto& preComputation = *ocpDefinition.preComputationPtr; + if (!ocpDefinition.finalInequalityConstraintPtr->empty()) { + const auto ineqConstraint = toVector(ocpDefinition.finalInequalityConstraintPtr->getValue(time, state, preComputation)); + slackStateIneq[N] = ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[N] = + ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + } else { + slackStateIneq[N].resize(0); + dualStateIneq[N].resize(0); + } + } +} + IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta_x0, scalar_t barrierParam, - const vector_array_t& slackStateIneq, const vector_array_t& slackStateInputIneq, - const vector_array_t& dualStateIneq, const vector_array_t& dualStateInputIneq) { + const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq, + const vector_array_t& slackStateInputIneq, + const vector_array_t& dualStateInputIneq) { // Solve the QP OcpSubproblemSolution solution; auto& deltaXSol = solution.deltaXSol; @@ -409,14 +513,14 @@ IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta auto& deltaLmdSol = solution.deltaLmdSol; auto& deltaNuSol = solution.deltaNuSol; auto& deltaSlackStateIneq = solution.deltaSlackStateIneq; - auto& deltaSlackStateInputIneq = solution.deltaSlackStateInputIneq; auto& deltaDualStateIneq = solution.deltaDualStateIneq; + auto& deltaSlackStateInputIneq = solution.deltaSlackStateInputIneq; auto& deltaDualStateInputIneq = solution.deltaDualStateInputIneq; deltaLmdSol.resize(N + 1); deltaNuSol.resize(N); deltaSlackStateIneq.resize(N + 1); - deltaSlackStateInputIneq.resize(N); deltaDualStateIneq.resize(N + 1); + deltaSlackStateInputIneq.resize(N); deltaDualStateInputIneq.resize(N); scalar_array_t primalStepSizes(settings_.nThreads, 1.0); @@ -517,9 +621,9 @@ PrimalSolution IpmSolver::toPrimalSolution(const std::vector<AnnotatedTime>& tim PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x, const vector_array_t& u, const vector_array_t& lmd, - const vector_array_t& nu, scalar_t barrierParam, vector_array_t& slackStateIneq, - vector_array_t& slackStateInputIneq, vector_array_t& dualStateIneq, - vector_array_t& dualStateInputIneq, std::vector<Metrics>& metrics) { + const vector_array_t& nu, scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateIneq, + const vector_array_t& dualStateInputIneq, std::vector<Metrics>& metrics) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -543,14 +647,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); - if (slackStateIneq[i].size() != result.ineqConstraints.f.size()) { - slackStateIneq[i] = - ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, - settings_.initialDualMarginRate); - slackStateInputIneq[i].resize(0); - dualStateInputIneq[i].resize(0); - } metrics[i] = multiple_shooting::computeMetrics(result); performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]); dynamics_[i] = std::move(result.dynamics); @@ -579,18 +675,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated result.stateIneqConstraints.setZero(0, x[i].size()); std::fill(result.constraintsSize.stateIneq.begin(), result.constraintsSize.stateIneq.end(), 0); } - if (slackStateIneq[i].size() != result.stateIneqConstraints.f.size()) { - slackStateIneq[i] = ipm::initializeSlackVariable(result.stateIneqConstraints.f, settings_.initialSlackLowerBound, - settings_.initialSlackMarginRate); - dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, - settings_.initialDualMarginRate); - } - if (slackStateInputIneq[i].size() != result.stateInputIneqConstraints.f.size()) { - slackStateInputIneq[i] = ipm::initializeSlackVariable(result.stateInputIneqConstraints.f, settings_.initialSlackLowerBound, - settings_.initialSlackMarginRate); - dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, - settings_.initialDualMarginRate); - } metrics[i] = multiple_shooting::computeMetrics(result); performance[workerId] += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers); @@ -623,12 +707,6 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); - if (slackStateIneq[N].size() != result.ineqConstraints.f.size()) { - slackStateIneq[N] = - ipm::initializeSlackVariable(result.ineqConstraints.f, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateIneq[N] = - ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); - } metrics[i] = multiple_shooting::computeMetrics(result); performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]); stateInputEqConstraints_[i].resize(0, x[i].size()); diff --git a/ocs2_oc/src/oc_data/TimeDiscretization.cpp b/ocs2_oc/src/oc_data/TimeDiscretization.cpp index 466f68b48..18eb53170 100644 --- a/ocs2_oc/src/oc_data/TimeDiscretization.cpp +++ b/ocs2_oc/src/oc_data/TimeDiscretization.cpp @@ -125,13 +125,14 @@ scalar_array_t toTime(const std::vector<AnnotatedTime>& annotatedTime) { scalar_array_t toInterpolationTime(const std::vector<AnnotatedTime>& annotatedTime) { scalar_array_t timeTrajectory; timeTrajectory.reserve(annotatedTime.size()); - for (size_t i = 0; i < annotatedTime.size(); i++) { + for (size_t i = 0; i < annotatedTime.size() - 1; i++) { if (annotatedTime[i].event == AnnotatedTime::Event::PostEvent) { timeTrajectory.push_back(getInterpolationTime(annotatedTime[i])); } else { timeTrajectory.push_back(annotatedTime[i].time); } } + timeTrajectory.push_back(annotatedTime.back().time - numeric_traits::limitEpsilon<scalar_t>()); return timeTrajectory; } From 67237dd5c5f0d60524fb809b092c1009bb34cc8d Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 19 Dec 2022 16:36:14 +0100 Subject: [PATCH 499/512] fix time trajectory for interpolation --- ocs2_ipm/src/IpmSolver.cpp | 4 ++-- ocs2_oc/src/oc_data/TimeDiscretization.cpp | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index e0bf26c7f..1f541437b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -408,7 +408,7 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& if (cachedEventIndex < slackDualTrajectory_.preJumps.size()) { ipm::toSlackDual(std::move(slackDualTrajectory_.preJumps[cachedEventIndex]), slackStateIneq[i], dualStateIneq[i]); } else { - const auto time = newTimeTrajectory[i]; + const auto time = getIntervalStart(timeDiscretization[i]); const auto& state = x[i]; constexpr auto request = Request::Constraint; ocpDefinition.preComputationPtr->requestPreJump(request, time, state); @@ -428,7 +428,7 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& dualStateInputIneq[i].resize(0); ++eventIdx; } else { - const auto time = newTimeTrajectory[i]; + const auto time = getIntervalStart(timeDiscretization[i]); if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { ipm::toSlackDual(getIntermediateDualSolutionAtTime(slackDualTrajectory_, time), slackStateIneq[i], dualStateIneq[i], slackStateInputIneq[i], dualStateInputIneq[i]); diff --git a/ocs2_oc/src/oc_data/TimeDiscretization.cpp b/ocs2_oc/src/oc_data/TimeDiscretization.cpp index 18eb53170..5b9f86ab3 100644 --- a/ocs2_oc/src/oc_data/TimeDiscretization.cpp +++ b/ocs2_oc/src/oc_data/TimeDiscretization.cpp @@ -123,9 +123,13 @@ scalar_array_t toTime(const std::vector<AnnotatedTime>& annotatedTime) { } scalar_array_t toInterpolationTime(const std::vector<AnnotatedTime>& annotatedTime) { + if (annotatedTime.empty()) { + return scalar_array_t(); + } scalar_array_t timeTrajectory; timeTrajectory.reserve(annotatedTime.size()); - for (size_t i = 0; i < annotatedTime.size() - 1; i++) { + timeTrajectory.push_back(annotatedTime.back().time - numeric_traits::limitEpsilon<scalar_t>()); + for (int i = 1; i < annotatedTime.size() - 1; i++) { if (annotatedTime[i].event == AnnotatedTime::Event::PostEvent) { timeTrajectory.push_back(getInterpolationTime(annotatedTime[i])); } else { From 666b81da996e044cf4d194b8f5f4ab0cdf4157a6 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 19 Dec 2022 16:57:02 +0100 Subject: [PATCH 500/512] fix CMakeLists and add tests --- ocs2_ipm/CMakeLists.txt | 2 +- ocs2_ipm/test/Exp0Test.cpp | 6 ++++++ ocs2_ipm/test/Exp1Test.cpp | 12 ++++++++++++ ocs2_ipm/test/testCircularKinematics.cpp | 13 ++++++++++++- 4 files changed, 31 insertions(+), 2 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index d5e27aae7..bd02a70cc 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -73,7 +73,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp index 3521e8d90..ba7a8b25e 100644 --- a/ocs2_ipm/test/Exp0Test.cpp +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -168,4 +168,10 @@ TEST(Exp0Test, Constrained) { ASSERT_TRUE(umax - u(0) >= 0); } } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } \ No newline at end of file diff --git a/ocs2_ipm/test/Exp1Test.cpp b/ocs2_ipm/test/Exp1Test.cpp index 5a4cd0cb1..15cceb2d3 100644 --- a/ocs2_ipm/test/Exp1Test.cpp +++ b/ocs2_ipm/test/Exp1Test.cpp @@ -197,6 +197,12 @@ TEST(Exp1Test, Constrained) { ASSERT_TRUE(umax - u(0) >= 0); } } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } TEST(Exp1Test, MixedConstrained) { @@ -257,4 +263,10 @@ TEST(Exp1Test, MixedConstrained) { const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, PreComputation()); ASSERT_TRUE(constraintValue.minCoeff() >= 0.0); } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp index a054ed01d..e05dc269b 100644 --- a/ocs2_ipm/test/testCircularKinematics.cpp +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -238,6 +238,12 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { // Feed forward part ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraints) { @@ -326,10 +332,15 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint // Check Lagrange multipliers for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { - std::cerr << "i: " << i << std::endl; const auto t = primalSolution.timeTrajectory_[i]; const auto& x = primalSolution.stateTrajectory_[i]; const auto& u = primalSolution.inputTrajectory_[i]; ASSERT_NO_THROW(const auto multiplier = solver.getStateInputEqualityConstraintLagrangian(t, x);); } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } } \ No newline at end of file From 6a3d7ce0263e54dc3e15815b3e713865ed6c4922 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 19 Dec 2022 17:42:02 +0100 Subject: [PATCH 501/512] modify the dual solution that stores the slack and dual trajectory --- ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 11 ++-- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 4 ++ ocs2_ipm/src/IpmHelpers.cpp | 69 ++++++++++++++++++-------- ocs2_ipm/src/IpmSolver.cpp | 9 ++-- 4 files changed, 66 insertions(+), 27 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 23e97b716..4b4d1f224 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include <ocs2_core/Types.h> +#include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/oc_data/DualSolution.h> #include <ocs2_oc/oc_data/TimeDiscretization.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> @@ -113,14 +114,16 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala * Convert the optimized slack or dual trajectories as a DualSolution. * * @param time: The time discretization. + * @param constraintsSize: The constraint tems size. * @param slackStateIneq: The slack variable trajectory of the state inequality constraints. * @param dualStateIneq: The dual variable trajectory of the state inequality constraints. * @param slackStateInputIneq: The slack variable trajectory of the state-input inequality constraints. * @param dualStateInputIneq: The dual variable trajectory of the state-input inequality constraints. * @return A dual solution. */ -DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq, - vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq); +DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::vector<multiple_shooting::ConstraintsSize>& constraintsSize, + const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq, + const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateInputIneq); /** * Extracts a slack variable of the state-only and state-input constraints from a MultiplierCollection @@ -131,7 +134,7 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array * @param[out] slackStateInputIneq: The slack variable of the state-input inequality constraints. * @param[out] dualStateInputIneq: The dual variable of the state-input inequality constraints. */ -void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, +void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, vector_t& slackStateInputIneq, vector_t& dualStateInputIneq); /** @@ -141,7 +144,7 @@ void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackSta * @param[out] slackStateIneq: The slack variable of the state inequality constraints. * @param[out] dualStateIneq: The dual variable of the state inequality constraints. */ -void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq); +void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq); } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 073eae008..f77a8efea 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_core/thread_support/ThreadPool.h> #include <ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h> +#include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/oc_data/TimeDiscretization.h> #include <ocs2_oc/oc_problem/OptimalControlProblem.h> #include <ocs2_oc/oc_solver/SolverBase.h> @@ -214,6 +215,9 @@ class IpmSolver : public SolverBase { std::vector<VectorFunctionLinearApproximation> stateInputIneqConstraints_; std::vector<VectorFunctionLinearApproximation> constraintsProjection_; + // Constraint terms size + std::vector<multiple_shooting::ConstraintsSize> constraintsSize_; + // Lagrange multipliers std::vector<multiple_shooting::ProjectionMultiplierCoefficients> projectionMultiplierCoefficients_; diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index d962cf1c9..3159d6f74 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -114,32 +114,61 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala } namespace { -MultiplierCollection toMultiplierCollection(vector_t&& slackStateIneq, vector_t&& dualStateIneq, - vector_t&& slackStateInputIneq = vector_t(), vector_t&& dualStateInputIneq = vector_t()) { +MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& slackStateIneq, + const vector_t& dualStateIneq) { MultiplierCollection multiplierCollection; - multiplierCollection.stateIneq.emplace_back(0.0, std::move(slackStateIneq)); - multiplierCollection.stateEq.emplace_back(0.0, std::move(dualStateIneq)); - multiplierCollection.stateInputIneq.emplace_back(0.0, std::move(slackStateInputIneq)); - multiplierCollection.stateInputEq.emplace_back(0.0, std::move(dualStateInputIneq)); + size_t head = 0; + for (const size_t size : constraintsSize.stateIneq) { + multiplierCollection.stateIneq.emplace_back(0.0, slackStateIneq.segment(head, size)); + multiplierCollection.stateEq.emplace_back(0.0, dualStateIneq.segment(head, size)); + head += size; + } + return multiplierCollection; +} + +MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& slackStateIneq, + const vector_t& dualStateIneq, const vector_t& slackStateInputIneq, + const vector_t& dualStateInputIneq) { + MultiplierCollection multiplierCollection = toMultiplierCollection(constraintsSize, slackStateIneq, dualStateIneq); + size_t head = 0; + for (const size_t size : constraintsSize.stateInputIneq) { + multiplierCollection.stateInputIneq.emplace_back(0.0, slackStateInputIneq.segment(head, size)); + multiplierCollection.stateInputEq.emplace_back(0.0, dualStateInputIneq.segment(head, size)); + } return multiplierCollection; } + +vector_t extractLagrangian(const std::vector<Multiplier>& termsMultiplier) { + size_t n = 0; + std::for_each(termsMultiplier.begin(), termsMultiplier.end(), [&](const Multiplier& m) { n += m.lagrangian.size(); }); + + vector_t vec(n); + size_t head = 0; + for (const auto& m : termsMultiplier) { + vec.segment(head, m.lagrangian.size()) = m.lagrangian; + head += m.lagrangian.size(); + } // end of i loop + + return vec; +} } // namespace -void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, +void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, vector_t& slackStateInputIneq, vector_t& dualStateInputIneq) { - slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian); - dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian); - slackStateInputIneq = std::move(multiplierCollection.stateInputIneq.front().lagrangian); - dualStateInputIneq = std::move(multiplierCollection.stateInputEq.front().lagrangian); + slackStateIneq = extractLagrangian(multiplierCollection.stateIneq); + dualStateIneq = extractLagrangian(multiplierCollection.stateEq); + slackStateInputIneq = extractLagrangian(multiplierCollection.stateInputIneq); + dualStateInputIneq = extractLagrangian(multiplierCollection.stateInputEq); } -void toSlackDual(MultiplierCollection&& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq) { - slackStateIneq = std::move(multiplierCollection.stateIneq.front().lagrangian); - dualStateIneq = std::move(multiplierCollection.stateEq.front().lagrangian); +void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq) { + slackStateIneq = extractLagrangian(multiplierCollection.stateIneq); + dualStateIneq = extractLagrangian(multiplierCollection.stateEq); } -DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& slackStateIneq, vector_array_t&& dualStateIneq, - vector_array_t&& slackStateInputIneq, vector_array_t&& dualStateInputIneq) { +DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::vector<multiple_shooting::ConstraintsSize>& constraintsSize, + const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq, + const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateInputIneq) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -152,14 +181,14 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, vector_array for (int i = 0; i < N; ++i) { if (time[i].event == AnnotatedTime::Event::PreEvent) { - dualSolution.preJumps.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i]))); + dualSolution.preJumps.emplace_back(toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i])); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node } else { - dualSolution.intermediates.emplace_back(toMultiplierCollection(std::move(slackStateIneq[i]), std::move(dualStateIneq[i]), - std::move(slackStateInputIneq[i]), std::move(dualStateInputIneq[i]))); + dualSolution.intermediates.emplace_back( + toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i], slackStateInputIneq[i], dualStateInputIneq[i])); } } - dualSolution.final = toMultiplierCollection(std::move(slackStateIneq[N]), std::move(dualStateIneq[N])); + dualSolution.final = toMultiplierCollection(constraintsSize[N], slackStateIneq[N], dualStateIneq[N]); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); return dualSolution; diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 1f541437b..94986ee5b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -39,7 +39,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include <ocs2_oc/multiple_shooting/LagrangianEvaluation.h> #include <ocs2_oc/multiple_shooting/MetricsComputation.h> #include <ocs2_oc/multiple_shooting/PerformanceIndexComputation.h> -#include <ocs2_oc/multiple_shooting/Transcription.h> #include <ocs2_oc/oc_problem/OcpSize.h> #include <ocs2_oc/trajectory_adjustment/TrajectorySpreadingHelperFunctions.h> @@ -282,8 +281,8 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); costateTrajectory_ = std::move(lmd); projectionMultiplierTrajectory_ = std::move(nu); - slackDualTrajectory_ = ipm::toDualSolution(timeDiscretization, std::move(slackStateIneq), std::move(dualStateIneq), - std::move(slackStateInputIneq), std::move(dualStateInputIneq)); + slackDualTrajectory_ = + ipm::toDualSolution(timeDiscretization, constraintsSize_, slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq); problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); @@ -635,6 +634,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated stateInputIneqConstraints_.resize(N + 1); constraintsProjection_.resize(N); projectionMultiplierCoefficients_.resize(N); + constraintsSize_.resize(N + 1); metrics.resize(N + 1); std::atomic_int timeIndex{0}; @@ -655,6 +655,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated stateInputIneqConstraints_[i].resize(0, x[i].size()); constraintsProjection_[i].resize(0, x[i].size()); projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); + constraintsSize_[i] = std::move(result.constraintsSize); if (settings_.computeLagrangeMultipliers) { lagrangian_[i] = multiple_shooting::evaluateLagrangianEventNode(lmd[i], lmd[i + 1], std::move(result.cost), dynamics_[i]); } else { @@ -684,6 +685,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); constraintsProjection_[i] = std::move(result.constraintsProjection); projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients); + constraintsSize_[i] = std::move(result.constraintsSize); if (settings_.computeLagrangeMultipliers) { lagrangian_[i] = multiple_shooting::evaluateLagrangianIntermediateNode(lmd[i], lmd[i + 1], nu[i], std::move(result.cost), dynamics_[i], stateInputEqConstraints_[i]); @@ -711,6 +713,7 @@ PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector<Annotated performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]); stateInputEqConstraints_[i].resize(0, x[i].size()); stateIneqConstraints_[i] = std::move(result.ineqConstraints); + constraintsSize_[i] = std::move(result.constraintsSize); if (settings_.computeLagrangeMultipliers) { lagrangian_[i] = multiple_shooting::evaluateLagrangianTerminalNode(lmd[i], std::move(result.cost)); } else { From 5cc9bba5cf72b6c155f838c480c07b7c1ca25f1b Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 19 Dec 2022 17:59:06 +0100 Subject: [PATCH 502/512] refactored slack and dual initialization --- ocs2_ipm/CMakeLists.txt | 2 +- ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 30 ++++-------------- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 3 +- ocs2_ipm/src/IpmHelpers.cpp | 44 +++++++++----------------- ocs2_ipm/src/IpmSolver.cpp | 30 +++++++++++------- 5 files changed, 43 insertions(+), 66 deletions(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index bd02a70cc..d5e27aae7 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -73,7 +73,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_WERROR + CF_FIX ) endif(cmake_clang_tools_FOUND) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 4b4d1f224..5eae47eb2 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -115,36 +115,20 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala * * @param time: The time discretization. * @param constraintsSize: The constraint tems size. - * @param slackStateIneq: The slack variable trajectory of the state inequality constraints. - * @param dualStateIneq: The dual variable trajectory of the state inequality constraints. - * @param slackStateInputIneq: The slack variable trajectory of the state-input inequality constraints. - * @param dualStateInputIneq: The dual variable trajectory of the state-input inequality constraints. + * @param stateIneq: The slack/dual variable trajectory of the state inequality constraints. + * @param stateInputIneq: The slack/dual variable trajectory of the state-input inequality constraints. * @return A dual solution. */ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::vector<multiple_shooting::ConstraintsSize>& constraintsSize, - const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq, - const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateInputIneq); + const vector_array_t& stateIneq, const vector_array_t& stateInputIneq); /** - * Extracts a slack variable of the state-only and state-input constraints from a MultiplierCollection + * Extracts slack/dual variables of the state-only and state-input constraints from a MultiplierCollection * - * @param[in] multiplierCollection: The MultiplierCollection. - * @param[out] slackStateIneq: The slack variable of the state inequality constraints. - * @param[out] dualStateIneq: The dual variable of the state inequality constraints. - * @param[out] slackStateInputIneq: The slack variable of the state-input inequality constraints. - * @param[out] dualStateInputIneq: The dual variable of the state-input inequality constraints. + * @param multiplierCollection: The MultiplierCollection. + * @return slack/dual variables of the state-only (first) and state-input constraints (second). */ -void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, - vector_t& slackStateInputIneq, vector_t& dualStateInputIneq); - -/** - * Extracts a slack variable of the state-only constraints from a MultiplierCollection - * - * @param[in] multiplierCollection: The MultiplierCollection. - * @param[out] slackStateIneq: The slack variable of the state inequality constraints. - * @param[out] dualStateIneq: The dual variable of the state inequality constraints. - */ -void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq); +std::pair<vector_t, vector_t> fromDualSolution(const MultiplierCollection& multiplierCollection); } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index f77a8efea..b9d488cf5 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -202,7 +202,8 @@ class IpmSolver : public SolverBase { PrimalSolution primalSolution_; vector_array_t costateTrajectory_; vector_array_t projectionMultiplierTrajectory_; - DualSolution slackDualTrajectory_; + DualSolution slackIneqTrajectory_; + DualSolution dualIneqTrajectory_; // Value function in absolute state coordinates (without the constant value) std::vector<ScalarFunctionQuadraticApproximation> valueFunction_; diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index 3159d6f74..ce349c401 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -114,26 +114,23 @@ scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scala } namespace { -MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& slackStateIneq, - const vector_t& dualStateIneq) { +MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& stateIneq) { MultiplierCollection multiplierCollection; size_t head = 0; for (const size_t size : constraintsSize.stateIneq) { - multiplierCollection.stateIneq.emplace_back(0.0, slackStateIneq.segment(head, size)); - multiplierCollection.stateEq.emplace_back(0.0, dualStateIneq.segment(head, size)); + multiplierCollection.stateIneq.emplace_back(0.0, stateIneq.segment(head, size)); head += size; } return multiplierCollection; } -MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& slackStateIneq, - const vector_t& dualStateIneq, const vector_t& slackStateInputIneq, - const vector_t& dualStateInputIneq) { - MultiplierCollection multiplierCollection = toMultiplierCollection(constraintsSize, slackStateIneq, dualStateIneq); +MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& stateIneq, + const vector_t& stateInputIneq) { + MultiplierCollection multiplierCollection = toMultiplierCollection(constraintsSize, stateIneq); size_t head = 0; for (const size_t size : constraintsSize.stateInputIneq) { - multiplierCollection.stateInputIneq.emplace_back(0.0, slackStateInputIneq.segment(head, size)); - multiplierCollection.stateInputEq.emplace_back(0.0, dualStateInputIneq.segment(head, size)); + multiplierCollection.stateInputIneq.emplace_back(0.0, stateInputIneq.segment(head, size)); + head += size; } return multiplierCollection; } @@ -153,22 +150,8 @@ vector_t extractLagrangian(const std::vector<Multiplier>& termsMultiplier) { } } // namespace -void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq, - vector_t& slackStateInputIneq, vector_t& dualStateInputIneq) { - slackStateIneq = extractLagrangian(multiplierCollection.stateIneq); - dualStateIneq = extractLagrangian(multiplierCollection.stateEq); - slackStateInputIneq = extractLagrangian(multiplierCollection.stateInputIneq); - dualStateInputIneq = extractLagrangian(multiplierCollection.stateInputEq); -} - -void toSlackDual(const MultiplierCollection& multiplierCollection, vector_t& slackStateIneq, vector_t& dualStateIneq) { - slackStateIneq = extractLagrangian(multiplierCollection.stateIneq); - dualStateIneq = extractLagrangian(multiplierCollection.stateEq); -} - DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::vector<multiple_shooting::ConstraintsSize>& constraintsSize, - const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq, - const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateInputIneq) { + const vector_array_t& stateIneq, const vector_array_t& stateInputIneq) { // Problem horizon const int N = static_cast<int>(time.size()) - 1; @@ -181,18 +164,21 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::v for (int i = 0; i < N; ++i) { if (time[i].event == AnnotatedTime::Event::PreEvent) { - dualSolution.preJumps.emplace_back(toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i])); + dualSolution.preJumps.emplace_back(toMultiplierCollection(constraintsSize[i], stateIneq[i])); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node } else { - dualSolution.intermediates.emplace_back( - toMultiplierCollection(constraintsSize[i], slackStateIneq[i], dualStateIneq[i], slackStateInputIneq[i], dualStateInputIneq[i])); + dualSolution.intermediates.emplace_back(toMultiplierCollection(constraintsSize[i], stateIneq[i], stateInputIneq[i])); } } - dualSolution.final = toMultiplierCollection(constraintsSize[N], slackStateIneq[N], dualStateIneq[N]); + dualSolution.final = toMultiplierCollection(constraintsSize[N], stateIneq[N]); dualSolution.intermediates.push_back(dualSolution.intermediates.back()); return dualSolution; } +std::pair<vector_t, vector_t> fromDualSolution(const MultiplierCollection& multiplierCollection) { + return std::make_pair(extractLagrangian(multiplierCollection.stateIneq), extractLagrangian(multiplierCollection.stateInputIneq)); +} + } // namespace ipm } // namespace ocs2 diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 94986ee5b..97656ddea 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -100,7 +100,8 @@ void IpmSolver::reset() { primalSolution_ = PrimalSolution(); costateTrajectory_.clear(); projectionMultiplierTrajectory_.clear(); - slackDualTrajectory_.clear(); + slackIneqTrajectory_.clear(); + dualIneqTrajectory_.clear(); valueFunction_.clear(); performanceIndeces_.clear(); @@ -215,8 +216,9 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); // Initialize the slack and dual variables of the interior point method - if (!slackDualTrajectory_.timeTrajectory.empty()) { - std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackDualTrajectory_); + if (!slackIneqTrajectory_.timeTrajectory.empty()) { + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackIneqTrajectory_); + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, dualIneqTrajectory_); } scalar_t barrierParam = settings_.initialBarrierParameter; vector_array_t slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq; @@ -281,8 +283,8 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); costateTrajectory_ = std::move(lmd); projectionMultiplierTrajectory_ = std::move(nu); - slackDualTrajectory_ = - ipm::toDualSolution(timeDiscretization, constraintsSize_, slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq); + slackIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, constraintsSize_, slackStateIneq, slackStateInputIneq); + dualIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, constraintsSize_, dualStateIneq, dualStateInputIneq); problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); @@ -374,8 +376,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& const vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, vector_array_t& dualStateIneq, vector_array_t& slackStateInputIneq, vector_array_t& dualStateInputIneq) { - const auto& oldTimeTrajectory = slackDualTrajectory_.timeTrajectory; - const auto& oldPostEventIndices = slackDualTrajectory_.postEventIndices; + const auto& oldTimeTrajectory = slackIneqTrajectory_.timeTrajectory; + const auto& oldPostEventIndices = slackIneqTrajectory_.postEventIndices; const auto newTimeTrajectory = toInterpolationTime(timeDiscretization); const auto newPostEventIndices = toPostEventIndices(timeDiscretization); @@ -404,8 +406,9 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& for (size_t i = 0; i < N; i++) { if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { const auto cachedEventIndex = cacheEventIndexBias + eventIdx; - if (cachedEventIndex < slackDualTrajectory_.preJumps.size()) { - ipm::toSlackDual(std::move(slackDualTrajectory_.preJumps[cachedEventIndex]), slackStateIneq[i], dualStateIneq[i]); + if (cachedEventIndex < slackIneqTrajectory_.preJumps.size()) { + std::tie(slackStateIneq[i], std::ignore) = ipm::fromDualSolution(slackIneqTrajectory_.preJumps[cachedEventIndex]); + std::tie(dualStateIneq[i], std::ignore) = ipm::fromDualSolution(dualIneqTrajectory_.preJumps[cachedEventIndex]); } else { const auto time = getIntervalStart(timeDiscretization[i]); const auto& state = x[i]; @@ -429,8 +432,10 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& } else { const auto time = getIntervalStart(timeDiscretization[i]); if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { - ipm::toSlackDual(getIntermediateDualSolutionAtTime(slackDualTrajectory_, time), slackStateIneq[i], dualStateIneq[i], - slackStateInputIneq[i], dualStateInputIneq[i]); + std::tie(slackStateIneq[i], slackStateInputIneq[i]) = + ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); + std::tie(dualStateIneq[i], dualStateInputIneq[i]) = + ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); } else { constexpr auto request = Request::Constraint; const auto& state = x[i]; @@ -463,7 +468,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& } if (interpolateTillFinalTime) { - ipm::toSlackDual(std::move(slackDualTrajectory_.final), slackStateIneq[N], dualStateIneq[N]); + std::tie(slackStateIneq[N], std::ignore) = ipm::fromDualSolution(slackIneqTrajectory_.final); + std::tie(dualStateIneq[N], std::ignore) = ipm::fromDualSolution(dualIneqTrajectory_.final); } else { const auto time = newTimeTrajectory[N]; const auto& state = x[N]; From 2a307e58c7a93ee7c5b14d936a1771d7992d096c Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 19 Dec 2022 17:59:45 +0100 Subject: [PATCH 503/512] fix CMake --- ocs2_ipm/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index d5e27aae7..bd02a70cc 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -73,7 +73,7 @@ if(cmake_clang_tools_FOUND) TARGETS ${PROJECT_NAME} SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX + CF_WERROR ) endif(cmake_clang_tools_FOUND) From 68e864b18cdec95d68fdb8faec7dd00b0408b297 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 19 Dec 2022 22:23:28 +0100 Subject: [PATCH 504/512] refactor initialization --- ocs2_ipm/CMakeLists.txt | 1 + ocs2_ipm/include/ocs2_ipm/IpmHelpers.h | 2 +- ocs2_ipm/include/ocs2_ipm/IpmInitialization.h | 54 +++++++++++- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 4 +- ocs2_ipm/src/IpmHelpers.cpp | 2 +- ocs2_ipm/src/IpmInitialization.cpp | 86 +++++++++++++++++++ ocs2_ipm/src/IpmSolver.cpp | 85 +++++++----------- ocs2_ipm/test/Exp0Test.cpp | 2 +- 8 files changed, 174 insertions(+), 62 deletions(-) create mode 100644 ocs2_ipm/src/IpmInitialization.cpp diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt index bd02a70cc..798f35478 100644 --- a/ocs2_ipm/CMakeLists.txt +++ b/ocs2_ipm/CMakeLists.txt @@ -50,6 +50,7 @@ include_directories( # Multiple shooting solver library add_library(${PROJECT_NAME} src/IpmHelpers.cpp + src/IpmInitialization.cpp src/IpmPerformanceIndexComputation.cpp src/IpmSettings.cpp src/IpmSolver.cpp diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h index 5eae47eb2..d9f27dac5 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -128,7 +128,7 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::v * @param multiplierCollection: The MultiplierCollection. * @return slack/dual variables of the state-only (first) and state-input constraints (second). */ -std::pair<vector_t, vector_t> fromDualSolution(const MultiplierCollection& multiplierCollection); +std::pair<vector_t, vector_t> fromMultiplierCollection(const MultiplierCollection& multiplierCollection); } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h index 7e318323c..9aade2d5a 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h @@ -30,6 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include <ocs2_core/Types.h> +#include <ocs2_oc/oc_problem/OptimalControlProblem.h> namespace ocs2 { namespace ipm { @@ -44,7 +45,11 @@ namespace ipm { * @return Initialized slack variable. */ inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { - return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound); + if (ineqConstraint.size() > 0) { + return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound); + } else { + return vector_t(); + } } /** @@ -59,8 +64,53 @@ inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t */ inline vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialDualLowerBound, scalar_t initialDualMarginRate) { - return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound); + if (slack.size() > 0) { + return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound); + } else { + return vector_t(); + } } +/** + * Initializes the slack variable at a single intermediate node. + * + * @param ocpDefinition: Definition of the optimal control problem. + * @param time : Time of this node + * @param state : State + * @param input : Input + * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. + * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + * @return Initialized slack variable. + */ +std::pair<vector_t, vector_t> initializeIntermediateSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, + const vector_t& state, const vector_t& input, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); + +/** + * Initializes the slack variable at the terminal node. + * + * @param ocpDefinition: Definition of the optimal control problem. + * @param time : Time at the terminal node + * @param state : Terminal state + * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. + * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + * @return Initialized slack variable. + */ +vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); + +/** + * Initializes the slack variable at an event node. + * + * @param ocpDefinition: Definition of the optimal control problem. + * @param time : Time at the event node + * @param state : Pre-event state + * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. + * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + * @return Initialized slack variable. + */ +vector_t initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); + } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index b9d488cf5..99f316f17 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -85,9 +85,7 @@ class IpmSolver : public SolverBase { vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override; - MultiplierCollection getIntermediateDualSolution(scalar_t time) const override { - throw std::runtime_error("[IpmSolver] getIntermediateDualSolution() not available yet."); - } + MultiplierCollection getIntermediateDualSolution(scalar_t time) const override; private: void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override; diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp index ce349c401..f7ebcc812 100644 --- a/ocs2_ipm/src/IpmHelpers.cpp +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -176,7 +176,7 @@ DualSolution toDualSolution(const std::vector<AnnotatedTime>& time, const std::v return dualSolution; } -std::pair<vector_t, vector_t> fromDualSolution(const MultiplierCollection& multiplierCollection) { +std::pair<vector_t, vector_t> fromMultiplierCollection(const MultiplierCollection& multiplierCollection) { return std::make_pair(extractLagrangian(multiplierCollection.stateIneq), extractLagrangian(multiplierCollection.stateInputIneq)); } diff --git a/ocs2_ipm/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp new file mode 100644 index 000000000..139b40b44 --- /dev/null +++ b/ocs2_ipm/src/IpmInitialization.cpp @@ -0,0 +1,86 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include "ocs2_ipm/IpmInitialization.h" + +namespace ocs2 { +namespace ipm { + +std::pair<vector_t, vector_t> initializeIntermediateSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, + const vector_t& state, const vector_t& input, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + vector_t slackStateIneq, slackStateInputIneq; + + if (!ocpDefinition.stateInequalityConstraintPtr->empty() || !ocpDefinition.inequalityConstraintPtr->empty()) { + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestPreJump(request, time, state); + } + + if (!ocpDefinition.stateInequalityConstraintPtr->empty()) { + const auto ineqConstraint = + toVector(ocpDefinition.stateInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + slackStateIneq = initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); + } + + if (!ocpDefinition.inequalityConstraintPtr->empty()) { + const auto ineqConstraint = + toVector(ocpDefinition.inequalityConstraintPtr->getValue(time, state, input, *ocpDefinition.preComputationPtr)); + slackStateInputIneq = initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); + } + + return std::make_pair(std::move(slackStateIneq), std::move(slackStateInputIneq)); +} + +vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + if (ocpDefinition.finalInequalityConstraintPtr->empty()) { + return vector_t(); + } + + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestFinal(request, time, state); + const auto ineqConstraint = toVector(ocpDefinition.finalInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); +} + +vector_t initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + if (ocpDefinition.preJumpInequalityConstraintPtr->empty()) { + return vector_t(); + } + + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestPreJump(request, time, state); + const auto ineqConstraint = + toVector(ocpDefinition.preJumpInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); +} + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 97656ddea..98949f28b 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -187,6 +187,14 @@ vector_t IpmSolver::getStateInputEqualityConstraintLagrangian(scalar_t time, con } } +MultiplierCollection IpmSolver::getIntermediateDualSolution(scalar_t time) const { + if (!dualIneqTrajectory_.timeTrajectory.empty()) { + return getIntermediateDualSolutionAtTime(dualIneqTrajectory_, time); + } else { + throw std::runtime_error("[IpmSolver] getIntermediateDualSolution() not available yet."); + } +} + void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; @@ -407,24 +415,15 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { const auto cachedEventIndex = cacheEventIndexBias + eventIdx; if (cachedEventIndex < slackIneqTrajectory_.preJumps.size()) { - std::tie(slackStateIneq[i], std::ignore) = ipm::fromDualSolution(slackIneqTrajectory_.preJumps[cachedEventIndex]); - std::tie(dualStateIneq[i], std::ignore) = ipm::fromDualSolution(dualIneqTrajectory_.preJumps[cachedEventIndex]); + std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); + std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); } else { const auto time = getIntervalStart(timeDiscretization[i]); const auto& state = x[i]; - constexpr auto request = Request::Constraint; - ocpDefinition.preComputationPtr->requestPreJump(request, time, state); - auto& preComputation = *ocpDefinition.preComputationPtr; - if (!ocpDefinition.preJumpInequalityConstraintPtr->empty()) { - const auto ineqConstraint = toVector(ocpDefinition.preJumpInequalityConstraintPtr->getValue(time, state, preComputation)); - slackStateIneq[i] = - ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, - settings_.initialDualMarginRate); - } else { - slackStateIneq[i].resize(0); - dualStateIneq[i].resize(0); - } + slackStateIneq[i] = ipm::initializePreJumpSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound, + settings_.initialSlackMarginRate); + dualStateIneq[i] = + ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } slackStateInputIneq[i].resize(0); dualStateInputIneq[i].resize(0); @@ -433,58 +432,36 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& const auto time = getIntervalStart(timeDiscretization[i]); if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { std::tie(slackStateIneq[i], slackStateInputIneq[i]) = - ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); + ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); std::tie(dualStateIneq[i], dualStateInputIneq[i]) = - ipm::fromDualSolution(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); + ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); } else { - constexpr auto request = Request::Constraint; const auto& state = x[i]; const auto& input = u[i]; - ocpDefinition.preComputationPtr->request(request, time, state, input); - if (!ocpDefinition.stateInequalityConstraintPtr->empty() && i > 0) { - const auto ineqConstraint = - toVector(ocpDefinition.stateInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); - slackStateIneq[i] = - ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, - settings_.initialDualMarginRate); - } else { + std::tie(slackStateIneq[i], slackStateInputIneq[i]) = ipm::initializeIntermediateSlackVariable( + ocpDefinition, time, state, input, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + // Disable the state-only inequality constraints at the initial node + if (i == 0) { slackStateIneq[i].resize(0); - dualStateIneq[i].resize(0); - } - if (!ocpDefinition.inequalityConstraintPtr->empty()) { - const auto ineqConstraint = - toVector(ocpDefinition.inequalityConstraintPtr->getValue(time, state, input, *ocpDefinition.preComputationPtr)); - slackStateInputIneq[i] = - ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, - settings_.initialDualMarginRate); - } else { - slackStateInputIneq[i].resize(0); - dualStateInputIneq[i].resize(0); } + dualStateIneq[i] = + ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); } } } if (interpolateTillFinalTime) { - std::tie(slackStateIneq[N], std::ignore) = ipm::fromDualSolution(slackIneqTrajectory_.final); - std::tie(dualStateIneq[N], std::ignore) = ipm::fromDualSolution(dualIneqTrajectory_.final); + std::tie(slackStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.final); + std::tie(dualStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.final); } else { - const auto time = newTimeTrajectory[N]; + const auto time = getIntervalStart(timeDiscretization[N]); const auto& state = x[N]; - constexpr auto request = Request::Constraint; - ocpDefinition.preComputationPtr->requestFinal(request, time, state); - auto& preComputation = *ocpDefinition.preComputationPtr; - if (!ocpDefinition.finalInequalityConstraintPtr->empty()) { - const auto ineqConstraint = toVector(ocpDefinition.finalInequalityConstraintPtr->getValue(time, state, preComputation)); - slackStateIneq[N] = ipm::initializeSlackVariable(ineqConstraint, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - dualStateIneq[N] = - ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); - } else { - slackStateIneq[N].resize(0); - dualStateIneq[N].resize(0); - } + slackStateIneq[N] = ipm::initializeTerminalSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound, + settings_.initialSlackMarginRate); + dualStateIneq[N] = + ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } } diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp index ba7a8b25e..2864b59db 100644 --- a/ocs2_ipm/test/Exp0Test.cpp +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -140,7 +140,7 @@ TEST(Exp0Test, Constrained) { const vector_t xmin = (vector_t(2) << -7.5, -7.5).finished(); const vector_t xmax = (vector_t(2) << 7.5, 7.5).finished(); problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); - problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + // problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); const scalar_t startTime = 0.0; const scalar_t finalTime = 2.0; From 72c0be8a72afc5a19572b8ee65565912e37d71d9 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 19 Dec 2022 22:51:31 +0100 Subject: [PATCH 505/512] update doc --- ocs2_ipm/include/ocs2_ipm/IpmInitialization.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h index 9aade2d5a..e18cf2e75 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h @@ -80,7 +80,7 @@ inline vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierPa * @param input : Input * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. - * @return Initialized slack variable. + * @return Initialized slack variables of the intermediate state-only (first) and state-input (second) constraints. */ std::pair<vector_t, vector_t> initializeIntermediateSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, const vector_t& input, @@ -94,7 +94,7 @@ std::pair<vector_t, vector_t> initializeIntermediateSlackVariable(OptimalControl * @param state : Terminal state * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. - * @return Initialized slack variable. + * @return Initialized slack variable of the terminal state-only constraints. */ vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); @@ -107,7 +107,7 @@ vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, s * @param state : Pre-event state * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. - * @return Initialized slack variable. + * @return Initialized slack variable of the pre-jump state-only constraints. */ vector_t initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); From 750f665142a428a46048a610bb577c554debe2e4 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Mon, 19 Dec 2022 22:54:35 +0100 Subject: [PATCH 506/512] add getDualSolution() implementation --- ocs2_ipm/include/ocs2_ipm/IpmSolver.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h index 99f316f17..56e7e282b 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -67,6 +67,8 @@ class IpmSolver : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } + const DualSolution* getDualSolution() const override { return &dualIneqTrajectory_; } + const ProblemMetrics& getSolutionMetrics() const override { return problemMetrics_; } size_t getNumIterations() const override { return totalNumIterations_; } From 668ccaec4d2db740ebb2c443adab4f80ecd3ec9a Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 20 Dec 2022 13:00:44 +0100 Subject: [PATCH 507/512] doc update --- ocs2_doc/docs/intro.rst | 7 ++++--- ocs2_doc/docs/overview.rst | 40 +++++++++++++++++++++++++++++--------- 2 files changed, 35 insertions(+), 12 deletions(-) diff --git a/ocs2_doc/docs/intro.rst b/ocs2_doc/docs/intro.rst index 2cab1805b..ffac40b6e 100644 --- a/ocs2_doc/docs/intro.rst +++ b/ocs2_doc/docs/intro.rst @@ -7,10 +7,11 @@ Introduction **S**\ witched **S**\ ystems (OCS2). The toolbox provides an efficient implementation of the following algorithms: -* **SLQ**\: Continuous-time domain DDP -* **iLQR**\: Discrete-time domain DDP +* **SLQ**\: Continuous-time domain constrained DDP +* **iLQR**\: Discrete-time domain constrained DDP * **SQP**\: Multiple-shooting algorithm based on `HPIPM <href="https://github.com/giaf/hpipm"/>`__ -* **PISOC**\: Path integral stochastic optimal control +* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method +* **SLP**\: Sequential Linear Programming based on `PIPG <href="https://arxiv.org/abs/2009.06980"/>`__ OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic diff --git a/ocs2_doc/docs/overview.rst b/ocs2_doc/docs/overview.rst index 6141cc135..8ce91d113 100644 --- a/ocs2_doc/docs/overview.rst +++ b/ocs2_doc/docs/overview.rst @@ -9,10 +9,11 @@ Overview for **S**\ witched **S**\ ystems (OCS2). The toolbox provides an efficient implementation of the following algorithms: -* **SLQ**\: Continuous-time domain DDP -* **iLQR**\: Discrete-time domain DDP +* **SLQ**\: Continuous-time domain constrained DDP +* **iLQR**\: Discrete-time domain constrained DDP * **SQP**\: Multiple-shooting algorithm based on `HPIPM <href="https://github.com/giaf/hpipm"/>`__ -* **PISOC**\: Path integral stochastic optimal control +* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method +* **SLP**\: Sequential Linear Programming based on `PIPG <href="https://arxiv.org/abs/2009.06980"/>`__ OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic @@ -51,18 +52,20 @@ Credits The following people have been involved in the development of OCS2: **Project Manager**: -Farbod Farshidian (ETHZ). +Farbod Farshidian. **Main Developers**: -Farbod Farshidian (ETHZ), -Ruben Grandia (ETHZ), -Michael Spieler (ETHZ), -Jan Carius (ETHZ), -Jean-Pierre Sleiman (ETHZ). +Farbod Farshidian, +Ruben Grandia, +Michael Spieler, +Jan Carius, +Jean-Pierre Sleiman. **Other Developers**: Alexander Reske, +Sotaro Katayama, Mayank Mittal, +Jia-​Ruei Chiu, Johannes Pankert, Perry Franklin, Tom Lankhorst, @@ -76,6 +79,25 @@ Edo Jelavic. project has continued to evolve at RSL, ETH Zurich. The RSL team now actively supports the development of OCS2. +Citing OCS2 +~~~~~~~~~~~ +To cite the toolbox in your academic research, please use the following: + +.. code-block:: + + @misc{OCS2, + title = {{OCS2}: An open source library for Optimal Control of Switched Systems}, + note = {[Online]. Available: \url{https://github.com/leggedrobotics/ocs2}}, + author = {Farbod Farshidian and others} + } + +Tutorials +~~~~~~~~~ +* Tutorial on OCS2 Toolbox, Farbod Farshidian, +MPC Workshop at RSS 2021 `link <href="https://youtu.be/RYmQN9GbFYg"/>`__ +* Real-time optimal control for legged locomotion & manipulation, Marco Hutter, +MPC Workshop at RSS 2021 `link <href="https://youtu.be/sjAENmtO4bA"/>`__ + Related publications ~~~~~~~~~~~~~~~~~~~~ From dfabca4f20b86a65073b9c2b0e30ede1d77facbb Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 20 Dec 2022 15:00:02 +0100 Subject: [PATCH 508/512] fix bugs and add initialization timer --- ocs2_ipm/src/IpmInitialization.cpp | 2 +- ocs2_ipm/src/IpmSolver.cpp | 33 +++++++++++++++--------------- ocs2_ipm/test/Exp0Test.cpp | 2 +- 3 files changed, 19 insertions(+), 18 deletions(-) diff --git a/ocs2_ipm/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp index 139b40b44..d2a2a9ccd 100644 --- a/ocs2_ipm/src/IpmInitialization.cpp +++ b/ocs2_ipm/src/IpmInitialization.cpp @@ -39,7 +39,7 @@ std::pair<vector_t, vector_t> initializeIntermediateSlackVariable(OptimalControl if (!ocpDefinition.stateInequalityConstraintPtr->empty() || !ocpDefinition.inequalityConstraintPtr->empty()) { constexpr auto request = Request::Constraint; - ocpDefinition.preComputationPtr->requestPreJump(request, time, state); + ocpDefinition.preComputationPtr->request(request, time, state, input); } if (!ocpDefinition.stateInequalityConstraintPtr->empty()) { diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 98949f28b..150a6543a 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -107,6 +107,7 @@ void IpmSolver::reset() { // reset timers totalNumIterations_ = 0; + initializationTimer_.reset(); linearQuadraticApproximationTimer_.reset(); solveQpTimer_.reset(); linesearchTimer_.reset(); @@ -114,12 +115,14 @@ void IpmSolver::reset() { } std::string IpmSolver::getBenchmarkingInformation() const { + const auto initializationTotal = initializationTimer_.getTotalInMilliseconds(); const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds(); const auto computeControllerTotal = computeControllerTimer_.getTotalInMilliseconds(); - const auto benchmarkTotal = linearQuadraticApproximationTotal + solveQpTotal + linesearchTotal + computeControllerTotal; + const auto benchmarkTotal = + initializationTotal + linearQuadraticApproximationTotal + solveQpTotal + linesearchTotal + computeControllerTotal; std::stringstream infoStream; if (benchmarkTotal > 0.0) { @@ -127,6 +130,8 @@ std::string IpmSolver::getBenchmarkingInformation() const { infoStream << "\n########################################################################\n"; infoStream << "The benchmarking is computed over " << totalNumIterations_ << " iterations. \n"; infoStream << "IPM Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "\tInitialization :\t" << initializationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << initializationTotal / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tLQ Approximation :\t" << linearQuadraticApproximationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" << linearQuadraticApproximationTotal / benchmarkTotal * inPercent << "%)\n"; infoStream << "\tSolve QP :\t" << solveQpTimer_.getAverageInMilliseconds() << " [ms] \t\t(" @@ -216,6 +221,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f const auto oldModeSchedule = primalSolution_.modeSchedule_; const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); + initializationTimer_.startTimer(); // Initialize the state and input if (!primalSolution_.timeTrajectory_.empty()) { std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, primalSolution_); @@ -239,6 +245,7 @@ void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f initializeCostateTrajectory(timeDiscretization, x, lmd); initializeProjectionMultiplierTrajectory(timeDiscretization, nu); } + initializationTimer_.endTimer(); // Bookkeeping performanceIndeces_.clear(); @@ -418,10 +425,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); } else { - const auto time = getIntervalStart(timeDiscretization[i]); - const auto& state = x[i]; - slackStateIneq[i] = ipm::initializePreJumpSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound, - settings_.initialSlackMarginRate); + slackStateIneq[i] = ipm::initializePreJumpSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[i]), x[i], + settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } @@ -436,14 +441,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& std::tie(dualStateIneq[i], dualStateInputIneq[i]) = ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); } else { - const auto& state = x[i]; - const auto& input = u[i]; std::tie(slackStateIneq[i], slackStateInputIneq[i]) = ipm::initializeIntermediateSlackVariable( - ocpDefinition, time, state, input, settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); - // Disable the state-only inequality constraints at the initial node - if (i == 0) { - slackStateIneq[i].resize(0); - } + ocpDefinition, time, x[i], u[i], settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, @@ -452,14 +451,16 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& } } + // Disable the state-only inequality constraints at the initial node + slackStateIneq[0].resize(0); + dualStateIneq[0].resize(0); + if (interpolateTillFinalTime) { std::tie(slackStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.final); std::tie(dualStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.final); } else { - const auto time = getIntervalStart(timeDiscretization[N]); - const auto& state = x[N]; - slackStateIneq[N] = ipm::initializeTerminalSlackVariable(ocpDefinition, time, state, settings_.initialSlackLowerBound, - settings_.initialSlackMarginRate); + slackStateIneq[N] = ipm::initializeTerminalSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[N]), x[N], + settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[N] = ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp index 2864b59db..ba7a8b25e 100644 --- a/ocs2_ipm/test/Exp0Test.cpp +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -140,7 +140,7 @@ TEST(Exp0Test, Constrained) { const vector_t xmin = (vector_t(2) << -7.5, -7.5).finished(); const vector_t xmax = (vector_t(2) << 7.5, 7.5).finished(); problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); - // problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); const scalar_t startTime = 0.0; const scalar_t finalTime = 2.0; From f4174ec5cd7c4f30942168b2dca6aa88b985f3e5 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 20 Dec 2022 15:02:37 +0100 Subject: [PATCH 509/512] improve function name --- ocs2_ipm/include/ocs2_ipm/IpmInitialization.h | 4 ++-- ocs2_ipm/src/IpmInitialization.cpp | 4 ++-- ocs2_ipm/src/IpmSolver.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h index e18cf2e75..cdc50a4ec 100644 --- a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h +++ b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h @@ -109,8 +109,8 @@ vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, s * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. * @return Initialized slack variable of the pre-jump state-only constraints. */ -vector_t initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, - scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); +vector_t initializeEventSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); } // namespace ipm } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp index d2a2a9ccd..33f8b7aa1 100644 --- a/ocs2_ipm/src/IpmInitialization.cpp +++ b/ocs2_ipm/src/IpmInitialization.cpp @@ -69,8 +69,8 @@ vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, s return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); } -vector_t initializePreJumpSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, - scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { +vector_t initializeEventSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { if (ocpDefinition.preJumpInequalityConstraintPtr->empty()) { return vector_t(); } diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 150a6543a..9f156c4cb 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -425,8 +425,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); } else { - slackStateIneq[i] = ipm::initializePreJumpSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[i]), x[i], - settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + slackStateIneq[i] = ipm::initializeEventSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[i]), x[i], + settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); } From c2ce8398f70cc53febe9ac66dfe76415cc153fa3 Mon Sep 17 00:00:00 2001 From: Sotaro Katayama <ihtf4ta7f@gmail.com> Date: Tue, 20 Dec 2022 15:30:42 +0100 Subject: [PATCH 510/512] fix time at preJump node of initialization --- ocs2_ipm/src/IpmSolver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp index 9f156c4cb..46ea304b7 100644 --- a/ocs2_ipm/src/IpmSolver.cpp +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -425,7 +425,8 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); } else { - slackStateIneq[i] = ipm::initializeEventSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[i]), x[i], + scalar_t time = timeDiscretization[i].time; + slackStateIneq[i] = ipm::initializeEventSlackVariable(ocpDefinition, timeDiscretization[i].time, x[i], settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); dualStateIneq[i] = ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); @@ -434,7 +435,7 @@ void IpmSolver::initializeSlackDualTrajectory(const std::vector<AnnotatedTime>& dualStateInputIneq[i].resize(0); ++eventIdx; } else { - const auto time = getIntervalStart(timeDiscretization[i]); + const scalar_t time = getIntervalStart(timeDiscretization[i]); if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { std::tie(slackStateIneq[i], slackStateInputIneq[i]) = ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); From ebde452b10d0eceaac45364f7bb8f0ac1038b637 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian <farbod.farshidian@gmail.com> Date: Tue, 20 Dec 2022 18:07:07 +0100 Subject: [PATCH 511/512] fix doc --- ocs2_doc/docs/overview.rst | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/ocs2_doc/docs/overview.rst b/ocs2_doc/docs/overview.rst index 8ce91d113..0f55770c7 100644 --- a/ocs2_doc/docs/overview.rst +++ b/ocs2_doc/docs/overview.rst @@ -9,11 +9,11 @@ Overview for **S**\ witched **S**\ ystems (OCS2). The toolbox provides an efficient implementation of the following algorithms: -* **SLQ**\: Continuous-time domain constrained DDP -* **iLQR**\: Discrete-time domain constrained DDP -* **SQP**\: Multiple-shooting algorithm based on `HPIPM <href="https://github.com/giaf/hpipm"/>`__ -* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method -* **SLP**\: Sequential Linear Programming based on `PIPG <href="https://arxiv.org/abs/2009.06980"/>`__ +* **SLQ**\: Continuous-time domain constrained DDP. +* **iLQR**\: Discrete-time domain constrained DDP. +* **SQP**\: Multiple-shooting algorithm based on `HPIPM <https://github.com/giaf/hpipm>`__. +* **SLP**\: Sequential Linear Programming based on `PIPG <https://arxiv.org/abs/2009.06980>`__. +* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method. OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic @@ -81,9 +81,10 @@ supports the development of OCS2. Citing OCS2 ~~~~~~~~~~~ + To cite the toolbox in your academic research, please use the following: -.. code-block:: +.. code-block:: latex @misc{OCS2, title = {{OCS2}: An open source library for Optimal Control of Switched Systems}, @@ -93,10 +94,10 @@ To cite the toolbox in your academic research, please use the following: Tutorials ~~~~~~~~~ -* Tutorial on OCS2 Toolbox, Farbod Farshidian, -MPC Workshop at RSS 2021 `link <href="https://youtu.be/RYmQN9GbFYg"/>`__ -* Real-time optimal control for legged locomotion & manipulation, Marco Hutter, -MPC Workshop at RSS 2021 `link <href="https://youtu.be/sjAENmtO4bA"/>`__ + +* Tutorial on OCS2 toolbox, Farbod Farshidian, MPC Workshop, RSS 2021 (`link <https://youtu.be/RYmQN9GbFYg>`__). + +* Real-time optimal control for legged locomotion and manipulation, Marco Hutter, MPC Workshop, RSS 2021 (`link <https://youtu.be/sjAENmtO4bA>`__). Related publications From 122529e8a4d7ea603a5eaa835e7fbf36b52ad5e7 Mon Sep 17 00:00:00 2001 From: Manuel Yves Galliker <manuel.galliker@gmx.ch> Date: Tue, 3 Jan 2023 12:21:06 +0100 Subject: [PATCH 512/512] Fix segment indices for orientation error in PinocchioEndEffectorKinematicsCppAd --- .../src/PinocchioEndEffectorKinematicsCppAd.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp index 0adabf6b4..a76a22329 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp @@ -253,7 +253,7 @@ auto PinocchioEndEffectorKinematicsCppAd::getOrientationError(const vector_t& st -> std::vector<vector3_t> { vector_t params(4 * endEffectorIds_.size()); for (int i = 0; i < endEffectorIds_.size(); i++) { - params.segment<4>(i) = referenceOrientations[i].coeffs(); + params.segment<4>(4 * i) = referenceOrientations[i].coeffs(); } const vector_t errorValues = orientationErrorCppAdInterfacePtr_->getFunctionValue(state, params); @@ -272,7 +272,7 @@ std::vector<VectorFunctionLinearApproximation> PinocchioEndEffectorKinematicsCpp const vector_t& state, const std::vector<quaternion_t>& referenceOrientations) const { vector_t params(4 * endEffectorIds_.size()); for (int i = 0; i < endEffectorIds_.size(); i++) { - params.segment<4>(i) = referenceOrientations[i].coeffs(); + params.segment<4>(4 * i) = referenceOrientations[i].coeffs(); } const vector_t errorValues = orientationErrorCppAdInterfacePtr_->getFunctionValue(state, params);