|
| 1 | +/****************************************************************************** |
| 2 | +Copyright (c) 2020, Farbod Farshidian. All rights reserved. |
| 3 | +
|
| 4 | +Redistribution and use in source and binary forms, with or without |
| 5 | +modification, are permitted provided that the following conditions are met: |
| 6 | +
|
| 7 | +* Redistributions of source code must retain the above copyright notice, this |
| 8 | + list of conditions and the following disclaimer. |
| 9 | +
|
| 10 | +* Redistributions in binary form must reproduce the above copyright notice, |
| 11 | + this list of conditions and the following disclaimer in the documentation |
| 12 | + and/or other materials provided with the distribution. |
| 13 | +
|
| 14 | +* Neither the name of the copyright holder nor the names of its |
| 15 | + contributors may be used to endorse or promote products derived from |
| 16 | + this software without specific prior written permission. |
| 17 | +
|
| 18 | +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| 19 | +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 20 | +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
| 21 | +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE |
| 22 | +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
| 23 | +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR |
| 24 | +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 25 | +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, |
| 26 | +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
| 27 | +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| 28 | +******************************************************************************/ |
| 29 | + |
| 30 | +#pragma once |
| 31 | + |
| 32 | +#include <ocs2_core/Types.h> |
| 33 | +#include <ocs2_core/integration/SensitivityIntegrator.h> |
| 34 | + |
| 35 | +#include "ocs2_pipg/PipgSettings.h" |
| 36 | + |
| 37 | +namespace ocs2 { |
| 38 | +namespace slp { |
| 39 | + |
| 40 | +/** Multiple-shooting SLP (Successive Linear Programming) settings */ |
| 41 | +struct Settings { |
| 42 | + size_t slpIteration = 10; // Maximum number of SLP iterations |
| 43 | + scalar_t deltaTol = 1e-6; // Termination condition : RMS update of x(t) and u(t) are both below this value |
| 44 | + scalar_t costTol = 1e-4; // Termination condition : (cost{i+1} - (cost{i}) < costTol AND constraints{i+1} < g_min |
| 45 | + |
| 46 | + // Linesearch - step size rules |
| 47 | + scalar_t alpha_decay = 0.5; // multiply the step size by this factor every time a linesearch step is rejected. |
| 48 | + scalar_t alpha_min = 1e-4; // terminate linesearch if the attempted step size is below this threshold |
| 49 | + |
| 50 | + // Linesearch - step acceptance criteria with c = costs, g = the norm of constraint violation, and w = [x; u] |
| 51 | + scalar_t g_max = 1e6; // (1): IF g{i+1} > g_max REQUIRE g{i+1} < (1-gamma_c) * g{i} |
| 52 | + 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 |
| 53 | + scalar_t armijoFactor = 1e-4; // Armijo condition: c{i+1} < c{i} + armijoFactor * dc/dw'{i} * delta_w |
| 54 | + 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} |
| 55 | + |
| 56 | + // Discretization method |
| 57 | + scalar_t dt = 0.01; // user-defined time discretization |
| 58 | + SensitivityIntegratorType integratorType = SensitivityIntegratorType::RK2; |
| 59 | + |
| 60 | + // Inequality penalty relaxed barrier parameters |
| 61 | + scalar_t inequalityConstraintMu = 0.0; |
| 62 | + scalar_t inequalityConstraintDelta = 1e-6; |
| 63 | + |
| 64 | + // Printing |
| 65 | + bool printSolverStatus = false; // Print HPIPM status after solving the QP subproblem |
| 66 | + bool printSolverStatistics = false; // Print benchmarking of the multiple shooting method |
| 67 | + bool printLinesearch = false; // Print linesearch information |
| 68 | + |
| 69 | + // Threading |
| 70 | + size_t nThreads = 4; |
| 71 | + int threadPriority = 50; |
| 72 | + |
| 73 | + // LP subproblem solver settings |
| 74 | + pipg::Settings pipgSettings = pipg::Settings(); |
| 75 | +}; |
| 76 | + |
| 77 | +/** |
| 78 | + * Loads the multiple shooting SLP settings from a given file. |
| 79 | + * |
| 80 | + * @param [in] filename: File name which contains the configuration data. |
| 81 | + * @param [in] fieldName: Field name which contains the configuration data. |
| 82 | + * @param [in] verbose: Flag to determine whether to print out the loaded settings or not. |
| 83 | + * @return The settings |
| 84 | + */ |
| 85 | +Settings loadSettings(const std::string& filename, const std::string& fieldName = "multiple_shooting", bool verbose = true); |
| 86 | + |
| 87 | +} // namespace slp |
| 88 | +} // namespace ocs2 |
0 commit comments