概要
ロボティクスで扱われるアルゴリズムにはよく最適化計算を行うものがあります。ROS1にはifoptというパッケージが公開されていたのでapt install一発で最適化計算ができましたがROS2でも使いたいのでどうにかする記事になります。(ifoptはIPOPTをEigenを使って扱えるようにしたインターフェイスライブラリです。)
解決方法
ifoptはpure cmakeプロジェクトでもあるのでROS2用のvendorパッケージを作ることでifoptをROS2でも使えるようにします
実験環境
- Ubuntu 22.04
- ROS2 Humble
依存関係のインストール
ROS2環境はすでに整っているものとします
sudo apt-get install libmumps-dev coinor-libipopt-dev
巨大な疎行列の線形代数計算ライブラリであるMUMPSと連続最適化問題を解くライブラリであるIPOPTをインストールします。
ifoptのexampleを動かしてみる
ifoptのexampleを計算するROS2ノードを作成してみます。
問題設定は以下のとおりです
\begin{align}
\underset{x_1, x_2}{\text{min}} \space & -(x_2-2)^2 \\
\text{s.t} \space & x_1^2 + x_2 -1 = 0 \\
& -1 < x_1 < 1
\end{align}
適当なwsを作って実験用にifopt_exampleというパッケージを作成して以下のファイルを追加します。
main.cpp
#include <rclcpp/rclcpp.hpp>
#include <ifopt/problem.h>
#include <ifopt/ipopt_solver.h>
#include <ifopt/test_vars_constr_cost.h>
class ExampleNode: public rclcpp::Node
{
public:
ExampleNode(
const std::string& name_space="",
const rclcpp::NodeOptions& options = rclcpp::NodeOptions()
): Node("example_node", name_space, options)
{
using namespace ifopt;
// 1. define the problem
Problem nlp;
nlp.AddVariableSet (std::make_shared<ExVariables>());
nlp.AddConstraintSet(std::make_shared<ExConstraint>());
nlp.AddCostSet (std::make_shared<ExCost>());
nlp.PrintCurrent();
// 2. choose solver and options
IpoptSolver ipopt;
// ipopt.SetOption("sb", "yes"); // スタートアップメッセージを消したい時はコメントアウトを外す
// ipopt.SetOption("print_level", 0); // 最適化中の詳細なログを出力させない時はコメントアウトを外す
ipopt.SetOption("linear_solver", "mumps");
ipopt.SetOption("jacobian_approximation", "exact");
// 3 . solve
ipopt.Solve(nlp);
Eigen::VectorXd x = nlp.GetOptVariables()->GetValues();
RCLCPP_INFO_STREAM(get_logger(), x.transpose());
}
};
int main(int argc, char * argv[]){
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ExampleNode>());
rclcpp::shutdown();
return 0;
}
packages.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ifopt_example</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="kotakkucu@gmail.com">uowner</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>ifopt_vendor</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(ifopt_example)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(ifopt_vendor REQUIRED)
find_package(Eigen3 REQUIRED)
add_executable(${PROJECT_NAME}_node src/main.cpp)
target_include_directories(${PROJECT_NAME}_node
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
$<INSTALL_INTERFACE:include>
)
ament_target_dependencies(${PROJECT_NAME}_node
rclcpp
ifopt_vendor
)
install(TARGETS
${PROJECT_NAME}_node
DESTINATION lib/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
vendorパッケージをwsに追加してcolcon buildします。
実行結果
ros2 run ifopt_example ifopt_example_node
************************************************************
IFOPT - Interface to Nonlinear Optimizers (v2.0)
© Alexander W. Winkler
https://github.com/ethz-adrl/ifopt
************************************************************
Legend:
c - number of variables, constraints or cost terms
i - indices of this set in overall problem
v - number of [violated variable- or constraint-bounds] or [cost term value]
c i v
variable-sets:
var_set1 2 0......1 1
constraint-sets:
constraint1 1 0......0 1
cost-terms:
cost_term1 1 0......0 -0.25
******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
Ipopt is released as open source code under the Eclipse Public License (EPL).
For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************
Total number of variables............................: 2
variables with only lower bounds: 0
variables with lower and upper bounds: 1
variables with only upper bounds: 0
Total number of equality constraints.................: 1
Total number of inequality constraints...............: 0
inequality constraints with only lower bounds: 0
inequality constraints with lower and upper bounds: 0
inequality constraints with only upper bounds: 0
Number of Iterations....: 5
(scaled) (unscaled)
Objective...............: -3.9999774633100951e+00 -3.9999774633100951e+00
Dual infeasibility......: 3.1836494894710299e-04 3.1836494894710299e-04
Constraint violation....: 3.6263436697936413e-11 3.6263436697936413e-11
Complementarity.........: 2.3763462994088299e-05 2.3763462994088299e-05
Overall NLP error.......: 3.1836494894710299e-04 3.1836494894710299e-04
Number of objective function evaluations = 9
Number of objective gradient evaluations = 6
Number of equality constraint evaluations = 9
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 6
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 0
Total CPU secs in IPOPT (w/o function evaluations) = 0.003
Total CPU secs in NLP function evaluations = 0.000
EXIT: Optimal Solution Found.
[INFO] [1688565222.484472301] [example_node]: 0.999997 5.63418e-06
↑最後の行で(1,0)に収束したことが分かります