Skip to content

Commit 8b59b1b

Browse files
Huoleitfarbod-farshidian
Huoleit
authored and
farbod-farshidian
committedDec 4, 2022
Merged in feature/PIPG-SQP (pull request #627)
Add multiple shooting MPC for PIPG Approved-by: Farbod Farshidian
·
12.0
2 parents ca38d9b + 5ffa5d0 commit 8b59b1b

35 files changed

+1608
-103
lines changed
 

‎ocs2/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
<exec_depend>ocs2_qp_solver</exec_depend>
1818
<exec_depend>ocs2_ddp</exec_depend>
1919
<exec_depend>ocs2_sqp</exec_depend>
20+
<exec_depend>ocs2_pipg</exec_depend>
2021
<exec_depend>ocs2_ros_interfaces</exec_depend>
2122
<exec_depend>ocs2_python_interface</exec_depend>
2223
<exec_depend>ocs2_pinocchio</exec_depend>

‎ocs2_pipg/CMakeLists.txt

+23-11
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@ project(ocs2_pipg)
33

44
set(CATKIN_PACKAGE_DEPENDENCIES
55
ocs2_core
6+
ocs2_mpc
67
ocs2_oc
78
ocs2_qp_solver
89
)
@@ -11,7 +12,10 @@ find_package(catkin REQUIRED COMPONENTS
1112
${CATKIN_PACKAGE_DEPENDENCIES}
1213
)
1314

14-
find_package(Boost REQUIRED)
15+
find_package(Boost REQUIRED COMPONENTS
16+
system
17+
filesystem
18+
)
1519

1620
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
1721

@@ -41,13 +45,20 @@ include_directories(
4145
)
4246

4347
add_library(${PROJECT_NAME}
48+
src/Helpers.cpp
4449
src/PipgSettings.cpp
4550
src/PipgSolver.cpp
46-
src/HelperFunctions.cpp
51+
src/SlpSettings.cpp
52+
src/mpc/PipgMpcSolver.cpp
53+
)
54+
add_dependencies(${PROJECT_NAME}
55+
${catkin_EXPORTED_TARGETS}
4756
)
4857
target_link_libraries(${PROJECT_NAME}
4958
${catkin_LIBRARIES}
59+
${Boost_LIBRARIES}
5060
)
61+
target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS})
5162

5263
#########################
5364
### CLANG TOOLING ###
@@ -56,13 +67,9 @@ find_package(cmake_clang_tools QUIET)
5667
if(cmake_clang_tools_FOUND)
5768
message(STATUS "Run clang tooling for target " ${PROJECT_NAME})
5869
add_clang_tooling(
59-
TARGETS
60-
${PROJECT_NAME}
61-
SOURCE_DIRS
62-
${CMAKE_CURRENT_SOURCE_DIR}/src
63-
${CMAKE_CURRENT_SOURCE_DIR}/include
64-
CT_HEADER_DIRS
65-
${CMAKE_CURRENT_SOURCE_DIR}/include
70+
TARGETS ${PROJECT_NAME}
71+
SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test
72+
CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
6673
CF_WERROR
6774
)
6875
endif(cmake_clang_tools_FOUND)
@@ -83,12 +90,17 @@ install(DIRECTORY include/${PROJECT_NAME}/
8390
#############
8491
## Testing ##
8592
#############
93+
8694
catkin_add_gtest(test_${PROJECT_NAME}
95+
test/testHelpers.cpp
8796
test/testPipgSolver.cpp
88-
test/testHelper.cpp
97+
test/testSlpSolver.cpp
98+
)
99+
add_dependencies(test_${PROJECT_NAME}
100+
${catkin_EXPORTED_TARGETS}
89101
)
90102
target_link_libraries(test_${PROJECT_NAME}
91103
${PROJECT_NAME}
92104
${catkin_LIBRARIES}
93105
gtest_main
94-
)
106+
)

‎ocs2_pipg/include/ocs2_pipg/HelperFunctions.h

-16
This file was deleted.

‎ocs2_sqp/ocs2_sqp/src/SqpSolverStatus.cpp renamed to ‎ocs2_pipg/include/ocs2_pipg/Helpers.h

+14-21
Original file line numberDiff line numberDiff line change
@@ -27,26 +27,19 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
2727
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
2828
******************************************************************************/
2929

30-
#include "ocs2_sqp/SqpSolverStatus.h"
30+
#pragma once
31+
32+
#include <ocs2_core/Types.h>
33+
#include <ocs2_core/thread_support/ThreadPool.h>
34+
#include <ocs2_oc/oc_problem/OcpSize.h>
3135

3236
namespace ocs2 {
33-
namespace sqp {
34-
35-
std::string toString(const Convergence& convergence) {
36-
switch (convergence) {
37-
case Convergence::ITERATIONS:
38-
return "Maximum number of iterations reached";
39-
case Convergence::STEPSIZE:
40-
return "Step size below minimum";
41-
case Convergence::METRICS:
42-
return "Cost decrease and constraint satisfaction below tolerance";
43-
case Convergence::PRIMAL:
44-
return "Primal update below tolerance";
45-
case Convergence::FALSE:
46-
default:
47-
return "Not Converged";
48-
}
49-
}
50-
51-
} // namespace sqp
52-
} // namespace ocs2
37+
namespace slp {
38+
39+
vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector<ScalarFunctionQuadraticApproximation>& cost);
40+
41+
vector_t GGTAbsRowSumInParallel(const OcpSize& ocpSize, const std::vector<VectorFunctionLinearApproximation>& dynamics,
42+
const std::vector<VectorFunctionLinearApproximation>* constraints, const vector_array_t* scalingVectorsPtr,
43+
ThreadPool& threadPool);
44+
} // namespace slp
45+
} // namespace ocs2

‎ocs2_pipg/include/ocs2_pipg/PipgSettings.h

+12-2
Original file line numberDiff line numberDiff line change
@@ -29,8 +29,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
2929

3030
#pragma once
3131

32-
#include <string>
33-
3432
#include <ocs2_core/Types.h>
3533

3634
namespace ocs2 {
@@ -48,10 +46,22 @@ struct Settings {
4846
scalar_t relativeTolerance = 1e-2;
4947
/** Number of iterations between consecutive calculation of termination conditions. **/
5048
size_t checkTerminationInterval = 1;
49+
/** Number of pre-conditioning run. **/
50+
int numScaling = 3;
51+
/** The static lower bound of the cost hessian H. **/
52+
scalar_t lowerBoundH = 5e-6;
5153
/** This value determines to display the a summary log. */
5254
bool displayShortSummary = false;
5355
};
5456

57+
/**
58+
* Loads the PIPG settings from a given file.
59+
*
60+
* @param [in] filename: File name which contains the configuration data.
61+
* @param [in] fieldName: Field name which contains the configuration data.
62+
* @param [in] verbose: Flag to determine whether to print out the loaded settings or not.
63+
* @return The settings
64+
*/
5565
Settings loadSettings(const std::string& filename, const std::string& fieldName = "pipg", bool verbose = true);
5666

5767
} // namespace pipg

‎ocs2_pipg/include/ocs2_pipg/PipgSolver.h

+6-1
Original file line numberDiff line numberDiff line change
@@ -43,10 +43,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
4343

4444
namespace ocs2 {
4545

46+
/*
47+
* First order primal-dual method for solving optimal control problem based on:
48+
* "Proportional-Integral Projected Gradient Method for Model Predictive Control"
49+
* https://arxiv.org/abs/2009.06980
50+
*/
4651
class PipgSolver {
4752
public:
4853
/**
49-
* @brief Construct a new PIPG with pipg setting object.
54+
* @brief Constructor.
5055
*
5156
* @param Settings: PIPG setting
5257
*/

‎ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h

+1
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ enum class SolverStatus {
4040
UNDEFINED,
4141
};
4242

43+
/** Transforms pipg::SolverStatus to string */
4344
inline std::string toString(SolverStatus s) {
4445
switch (s) {
4546
case SolverStatus::SUCCESS:

‎ocs2_pipg/include/ocs2_pipg/SlpSettings.h

+88
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
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

‎ocs2_pipg/include/ocs2_pipg/SlpSolverStatus.h

+77
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
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 <string>
33+
34+
#include <ocs2_core/Types.h>
35+
#include <ocs2_oc/oc_data/PerformanceIndex.h>
36+
#include <ocs2_oc/search_strategy/FilterLinesearch.h>
37+
38+
namespace ocs2 {
39+
namespace slp {
40+
41+
/** Different types of convergence */
42+
enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL };
43+
44+
/** Struct to contain the result and logging data of the stepsize computation */
45+
struct StepInfo {
46+
// Step size and type
47+
scalar_t stepSize = 0.0;
48+
FilterLinesearch::StepType stepType = FilterLinesearch::StepType::UNKNOWN;
49+
50+
// Step in primal variables
51+
scalar_t dx_norm = 0.0; // norm of the state trajectory update
52+
scalar_t du_norm = 0.0; // norm of the input trajectory update
53+
54+
// Performance result after the step
55+
PerformanceIndex performanceAfterStep;
56+
scalar_t totalConstraintViolationAfterStep; // constraint metric used in the line search
57+
};
58+
59+
/** Transforms pipg::Convergence to string */
60+
inline std::string toString(const Convergence& convergence) {
61+
switch (convergence) {
62+
case Convergence::ITERATIONS:
63+
return "Maximum number of iterations reached";
64+
case Convergence::STEPSIZE:
65+
return "Step size below minimum";
66+
case Convergence::METRICS:
67+
return "Cost decrease and constraint satisfaction below tolerance";
68+
case Convergence::PRIMAL:
69+
return "Primal update below tolerance";
70+
case Convergence::FALSE:
71+
default:
72+
return "Not Converged";
73+
}
74+
}
75+
76+
} // namespace slp
77+
} // namespace ocs2

0 commit comments

Comments
 (0)
Please sign in to comment.