Skip to content

Commit 50c3e3a

Browse files
mayatakafarbod-farshidian
mayataka
authored and
farbod-farshidian
committed
Merged in feature/interior_point_solver (pull request #685)
add ocs2_ipm package Approved-by: Farbod Farshidian
2 parents 6895abe + 36d0883 commit 50c3e3a

21 files changed

+3489
-6
lines changed

‎ocs2_ipm/CMakeLists.txt

Lines changed: 113 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
cmake_minimum_required(VERSION 3.0.2)
2+
project(ocs2_ipm)
3+
4+
set(CATKIN_PACKAGE_DEPENDENCIES
5+
ocs2_core
6+
ocs2_mpc
7+
ocs2_oc
8+
blasfeo_catkin
9+
hpipm_catkin
10+
)
11+
12+
find_package(catkin REQUIRED COMPONENTS
13+
${CATKIN_PACKAGE_DEPENDENCIES}
14+
)
15+
16+
find_package(Boost REQUIRED COMPONENTS
17+
system
18+
filesystem
19+
)
20+
21+
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
22+
23+
###################################
24+
## catkin specific configuration ##
25+
###################################
26+
27+
catkin_package(
28+
INCLUDE_DIRS
29+
include
30+
${EIGEN3_INCLUDE_DIRS}
31+
CATKIN_DEPENDS
32+
${CATKIN_PACKAGE_DEPENDENCIES}
33+
LIBRARIES
34+
${PROJECT_NAME}
35+
DEPENDS
36+
Boost
37+
)
38+
39+
###########
40+
## Build ##
41+
###########
42+
43+
include_directories(
44+
include
45+
${catkin_INCLUDE_DIRS}
46+
${EIGEN3_INCLUDE_DIRS}
47+
${Boost_INCLUDE_DIRS}
48+
)
49+
50+
# Multiple shooting solver library
51+
add_library(${PROJECT_NAME}
52+
src/IpmHelpers.cpp
53+
src/IpmPerformanceIndexComputation.cpp
54+
src/IpmSettings.cpp
55+
src/IpmSolver.cpp
56+
)
57+
add_dependencies(${PROJECT_NAME}
58+
${catkin_EXPORTED_TARGETS}
59+
)
60+
target_link_libraries(${PROJECT_NAME}
61+
${catkin_LIBRARIES}
62+
${Boost_LIBRARIES}
63+
)
64+
target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS})
65+
66+
# #########################
67+
# ### CLANG TOOLING ###
68+
# #########################
69+
find_package(cmake_clang_tools QUIET)
70+
if(cmake_clang_tools_FOUND)
71+
message(STATUS "Run clang tooling for target " ${PROJECT_NAME})
72+
add_clang_tooling(
73+
TARGETS ${PROJECT_NAME}
74+
SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test
75+
CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
76+
CF_FIX
77+
)
78+
endif(cmake_clang_tools_FOUND)
79+
80+
#############
81+
## Install ##
82+
#############
83+
install(
84+
TARGETS ${PROJECT_NAME}
85+
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
86+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
87+
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
88+
)
89+
90+
install(DIRECTORY include/${PROJECT_NAME}/
91+
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
92+
)
93+
94+
#############
95+
## Testing ##
96+
#############
97+
98+
catkin_add_gtest(test_${PROJECT_NAME}
99+
test/Exp0Test.cpp
100+
test/Exp1Test.cpp
101+
test/testCircularKinematics.cpp
102+
test/testSwitchedProblem.cpp
103+
test/testUnconstrained.cpp
104+
test/testValuefunction.cpp
105+
)
106+
add_dependencies(test_${PROJECT_NAME}
107+
${catkin_EXPORTED_TARGETS}
108+
)
109+
target_link_libraries(test_${PROJECT_NAME}
110+
${PROJECT_NAME}
111+
${catkin_LIBRARIES}
112+
gtest_main
113+
)

‎ocs2_ipm/include/ocs2_ipm/IpmHelpers.h

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
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_oc/oc_problem/OptimalControlProblem.h>
34+
35+
namespace ocs2 {
36+
namespace ipm {
37+
38+
/**
39+
* Removes the Newton directions of slack and dual variables and the linearized inequality constraints are removed from the linear system
40+
* equations for the Newton step computation. These terms are considered in the quadratic approximation of the Lagrangian.
41+
*
42+
* @param[in] barrierParam : The barrier parameter of the interior point method.
43+
* @param[in] slack : The slack variable associated with the inequality constraints.
44+
* @param[in] dual : The dual variable associated with the inequality constraints.
45+
* @param[in] ineqConstraints : Linear approximation of the inequality constraints.
46+
* @param[in, out] lagrangian : Quadratic approximation of the Lagrangian.
47+
*/
48+
void condenseIneqConstraints(scalar_t barrierParam, const vector_t& slack, const vector_t& dual,
49+
const VectorFunctionLinearApproximation& ineqConstraints, ScalarFunctionQuadraticApproximation& lagrangian);
50+
51+
/**
52+
* Computes the SSE of the residual in the perturbed complementary slackness.
53+
*
54+
* @param[in] barrierParam : The barrier parameter of the interior point method.
55+
* @param[in] slack : The slack variable associated with the inequality constraints.
56+
* @param[in] dual : The dual variable associated with the inequality constraints.
57+
* @return SSE of the residual in the perturbed complementary slackness
58+
*/
59+
inline scalar_t evaluateComplementarySlackness(scalar_t barrierParam, const vector_t& slack, const vector_t& dual) {
60+
return (slack.array() * dual.array() - barrierParam).matrix().squaredNorm();
61+
}
62+
63+
/**
64+
* Retrieves the Newton directions of the slack variable associated with state-input inequality constraints.
65+
*
66+
* @param[in] stateInputIneqConstraints : State-input inequality constraints
67+
* @param[in] dx : Newton direction of the state
68+
* @param[in] du : Newton direction of the input
69+
* @param[in] barrierParam : The barrier parameter of the interior point method.
70+
* @param[in] slackStateInputIneq : The slack variable associated with the state-input inequality constraints.
71+
* @return Newton directions of the slack variable.
72+
*/
73+
vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateInputIneqConstraints, const vector_t& dx, const vector_t& du,
74+
scalar_t barrierParam, const vector_t& slackStateInputIneq);
75+
76+
/**
77+
* Retrieves the Newton directions of the slack variable associated with state-only inequality constraints.
78+
*
79+
* @param[in] stateIneqConstraints : State-only inequality constraints
80+
* @param[in] dx : Newton direction of the state
81+
* @param[in] barrierParam : The barrier parameter of the interior point method.
82+
* @param[in] slackStateIneq : The slack variable associated with the state-only inequality constraints.
83+
* @return Newton directions of the slack variable.
84+
*/
85+
vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateIneqConstraints, const vector_t& dx, scalar_t barrierParam,
86+
const vector_t& slackStateIneq);
87+
88+
/**
89+
* Retrieves the Newton directions of the dual variable.
90+
*
91+
* @param[in] barrierParam : The barrier parameter of the interior point method.
92+
* @param[in] slack : The slack variable associated with the inequality constraints.
93+
* @param[in] dual : The dual variable associated with the inequality constraints.
94+
* @param[in] slackDirection : The Newton direction of the slack variable.
95+
* @return Newton directions of the dual variable.
96+
*/
97+
vector_t retrieveDualDirection(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, const vector_t& slackDirection);
98+
99+
/**
100+
* Computes the step size via fraction-to-boundary-rule, which is introduced in the IPOPT's implementaion paper,
101+
* "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming"
102+
* https://link.springer.com/article/10.1007/s10107-004-0559-y
103+
*
104+
* @param [in] v: A variable (slack or dual).
105+
* @param [in] dv: The serach direction of the variable (slack dirction or dual dirction).
106+
* @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.
107+
*/
108+
scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scalar_t marginRate = 0.995);
109+
110+
} // namespace ipm
111+
} // namespace ocs2

‎ocs2_ipm/include/ocs2_ipm/IpmInitialization.h

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
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+
34+
namespace ocs2 {
35+
namespace ipm {
36+
37+
/**
38+
* Initializes the slack variable. The initialization of the slack variables follows IPOPT
39+
* (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Initialization).
40+
*
41+
* @param ineqConstraint: The value of the inequality constraint.
42+
* @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT.
43+
* @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT.
44+
* @return Initialized slack variable.
45+
*/
46+
inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) {
47+
return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound);
48+
}
49+
50+
/**
51+
* Initializes the dual variable. The initialization of dual variables follows IPOPT option `bound_mult_init_method`: `mu-based`
52+
* (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Initialization) with additoinal options.
53+
*
54+
* @param slack: The slack variable.
55+
* @param barrierParam: The barrier parameter of the IPM. Must be positive.
56+
* @param initialDualLowerBound : Lower bound of the initial dual variables.
57+
* @param initialDualMarginRate : Additional margin rate of the initial dual variables.
58+
* @return Initialized dual variable.
59+
*/
60+
inline vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialDualLowerBound,
61+
scalar_t initialDualMarginRate) {
62+
return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound);
63+
}
64+
65+
} // namespace ipm
66+
} // namespace ocs2

‎ocs2_ipm/include/ocs2_ipm/IpmMpc.h

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
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_mpc/MPC_BASE.h>
33+
34+
#include "ocs2_ipm/IpmSolver.h"
35+
36+
namespace ocs2 {
37+
38+
class IpmMpc final : public MPC_BASE {
39+
public:
40+
/**
41+
* Constructor
42+
*
43+
* @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use
44+
* multiple shooting IPM settings directly.
45+
* @param settings : settings for the multiple shooting IPM solver.
46+
* @param [in] optimalControlProblem: The optimal control problem formulation.
47+
* @param [in] initializer: This class initializes the state-input for the time steps that no controller is available.
48+
*/
49+
IpmMpc(mpc::Settings mpcSettings, ipm::Settings settings, const OptimalControlProblem& optimalControlProblem,
50+
const Initializer& initializer)
51+
: MPC_BASE(std::move(mpcSettings)),
52+
solverPtr_(std::make_unique<IpmSolver>(std::move(settings), optimalControlProblem, initializer)) {}
53+
54+
~IpmMpc() override = default;
55+
56+
IpmSolver* getSolverPtr() override { return solverPtr_.get(); }
57+
const IpmSolver* getSolverPtr() const override { return solverPtr_.get(); }
58+
59+
protected:
60+
void calculateController(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override {
61+
if (settings().coldStart_) {
62+
solverPtr_->reset();
63+
}
64+
solverPtr_->run(initTime, initState, finalTime);
65+
}
66+
67+
private:
68+
std::unique_ptr<IpmSolver> solverPtr_;
69+
};
70+
71+
} // namespace ocs2

0 commit comments

Comments
 (0)