Skip to content

Commit 9acd52e

Browse files
jtordejtorde
committed
gurobi + nlopt option
1 parent 243ccab commit 9acd52e

File tree

9 files changed

+128
-114
lines changed

9 files changed

+128
-114
lines changed

‎mader/CMakeLists.txt

Lines changed: 46 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -9,22 +9,20 @@ add_definitions(-std=c++11)
99
find_package( Eigen3 REQUIRED )
1010
include_directories(${EIGEN3_INCLUDE_DIR})
1111

12-
list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}")
13-
find_package(GUROBI REQUIRED)
12+
list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake_finders")
1413

15-
if(GUROBI_FOUND)
16-
message(STATUS "GUROBI FOUND")
17-
else(GUROBI_FOUND)
18-
message(FATAL_ERROR "GUROBI NOT FOUND!")
19-
endif(GUROBI_FOUND)
2014

21-
find_package(NLOPT REQUIRED)
2215
find_package(CGAL REQUIRED)
16+
17+
18+
option(USE_GUROBI "Use GUROBI as the solver" ON) #if OFF, then it will use NLOPT as the solver
19+
20+
2321
include(${CGAL_USE_FILE})
2422

2523
set(CMAKE_BUILD_TYPE Release) #From terminal: catkin config -DCMAKE_BUILD_TYPE=Release (or RelWithDebInfo) or Debug
2624

27-
include_directories(${GUROBI_INCLUDE_DIRS})
25+
2826

2927
find_package(catkin REQUIRED COMPONENTS
3028
roscpp
@@ -36,7 +34,7 @@ find_package(catkin REQUIRED COMPONENTS
3634
decomp_ros_utils
3735
rviz_visual_tools
3836
separator
39-
jsk_rviz_plugins
37+
jsk_rviz_plugins
4038
)
4139
find_package(decomp_util REQUIRED)
4240
include_directories(${DECOMP_UTIL_INCLUDE_DIRS})
@@ -45,8 +43,6 @@ catkin_package()
4543

4644
include_directories(${catkin_INCLUDE_DIRS} include)
4745

48-
set(GUROBI_LIBRARIES "$ENV{GUROBI_HOME}/lib/libgurobi_c++.a;$ENV{GUROBI_HOME}/lib/libgurobi91.so;$ENV{GUROBI_HOME}/lib/" )
49-
5046
#For precompile headers see example here
5147
#https://gitlab.kitware.com/cmake/cmake/tree/375d01c6808713a0cfeef9ea092c8236ba063525/Tests/PrecompileHeaders
5248

@@ -62,18 +58,46 @@ add_library(octopus_search STATIC src/octopus_search.cpp src/bspline_utils.cpp s
6258
target_include_directories (octopus_search PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
6359
target_link_libraries(octopus_search PUBLIC ${catkin_LIBRARIES})
6460

61+
if(USE_GUROBI)
62+
63+
find_package(GUROBI REQUIRED)
64+
65+
if(GUROBI_FOUND)
66+
message(STATUS "GUROBI FOUND")
67+
else(GUROBI_FOUND)
68+
message(FATAL_ERROR "GUROBI NOT FOUND!")
69+
endif(GUROBI_FOUND)
70+
71+
include_directories(${GUROBI_INCLUDE_DIRS})
72+
file(GLOB GurobiSOFiles $ENV{GUROBI_HOME}/lib/libgurobi*[0-9].so) #files that are start with libgurobi and end with number.so
73+
set(GUROBI_LIBRARIES "$ENV{GUROBI_HOME}/lib/libgurobi_c++.a;${GurobiSOFiles};$ENV{GUROBI_HOME}/lib/" )
6574

66-
add_library(solver_nlopt STATIC src/solver_nlopt.cpp src/bspline_utils.cpp src/nlopt_utils.cpp )
67-
target_include_directories (solver_nlopt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${DECOMP_UTIL_INCLUDE_DIRS})
68-
target_link_libraries(solver_nlopt PUBLIC ${NLOPT_LIBRARIES} ${DECOMP_UTIL_LIBRARIES} utils octopus_search ${catkin_LIBRARIES} cgal_utils)
75+
add_library(solver STATIC src/solver_gurobi.cpp src/bspline_utils.cpp src/solver_gurobi_utils.cpp src/solver_gurobi_guess.cpp)
76+
target_compile_definitions(solver PUBLIC USE_GUROBI_FLAG=1)
77+
target_include_directories (solver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${DECOMP_UTIL_INCLUDE_DIRS})
78+
target_link_libraries(solver PUBLIC ${GUROBI_LIBRARIES} ${DECOMP_UTIL_LIBRARIES} utils octopus_search ${catkin_LIBRARIES} cgal_utils)
79+
80+
add_executable(test_gurobi src/examples/test_gurobi.cpp)
81+
add_dependencies(test_gurobi ${catkin_EXPORTED_TARGETS})
82+
target_link_libraries(test_gurobi ${catkin_LIBRARIES} utils ${GUROBI_LIBRARIES} solver)
83+
84+
else()
85+
find_package(NLOPT REQUIRED)
86+
87+
add_library(solver STATIC src/solver_nlopt.cpp src/bspline_utils.cpp src/nlopt_utils.cpp )
88+
target_compile_definitions(solver PUBLIC USE_GUROBI_FLAG=0)
89+
target_include_directories (solver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${DECOMP_UTIL_INCLUDE_DIRS})
90+
target_link_libraries(solver PUBLIC ${NLOPT_LIBRARIES} ${DECOMP_UTIL_LIBRARIES} utils octopus_search ${catkin_LIBRARIES} cgal_utils)
91+
92+
add_executable(test_nlopt src/examples/test_nlopt.cpp)
93+
add_dependencies(test_nlopt ${catkin_EXPORTED_TARGETS})
94+
target_link_libraries(test_nlopt ${catkin_LIBRARIES} utils solver)
95+
96+
endif()
6997

70-
add_library(solver_gurobi STATIC src/solver_gurobi.cpp src/bspline_utils.cpp src/nlopt_utils.cpp src/solver_gurobi_utils.cpp src/solver_gurobi_guess.cpp)
71-
target_include_directories (solver_gurobi PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${DECOMP_UTIL_INCLUDE_DIRS})
72-
target_link_libraries(solver_gurobi PUBLIC ${NLOPT_LIBRARIES} ${GUROBI_LIBRARIES} ${DECOMP_UTIL_LIBRARIES} utils octopus_search ${catkin_LIBRARIES} cgal_utils)
7398

74-
# enable_unity_build(UNITY_BUILD_NAME src/mader_node.cpp)
7599
add_executable(${PROJECT_NAME}_node src/mader_node.cpp src/mader_ros.cpp src/mader.cpp )
76-
target_link_libraries(${PROJECT_NAME}_node PUBLIC ${catkin_LIBRARIES} solver_nlopt solver_gurobi utils) #${CGAL_LIBS}
100+
target_link_libraries(${PROJECT_NAME}_node PUBLIC ${catkin_LIBRARIES} solver utils) #${CGAL_LIBS}
77101
add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS} )
78102

79103
add_executable(test_octopus_search src/examples/test_octopus_search.cpp src/octopus_search.cpp src/bspline_utils.cpp)
@@ -84,18 +108,13 @@ add_executable(test_utils src/examples/test_utils.cpp)
84108
add_dependencies(test_utils ${catkin_EXPORTED_TARGETS})
85109
target_link_libraries(test_utils ${catkin_LIBRARIES} utils)
86110

87-
add_executable(test_nlopt src/examples/test_nlopt.cpp)
88-
add_dependencies(test_nlopt ${catkin_EXPORTED_TARGETS})
89-
target_link_libraries(test_nlopt ${catkin_LIBRARIES} utils solver_nlopt)
90-
91-
add_executable(test_gurobi src/examples/test_gurobi.cpp)
92-
add_dependencies(test_gurobi ${catkin_EXPORTED_TARGETS})
93-
target_link_libraries(test_gurobi ${catkin_LIBRARIES} utils ${GUROBI_LIBRARIES} solver_gurobi)
94111

95112
add_executable(test_bspline_utils src/examples/test_bspline_utils.cpp src/bspline_utils.cpp)
96113
add_dependencies(test_bspline_utils ${catkin_EXPORTED_TARGETS})
97114
target_link_libraries(test_bspline_utils ${catkin_LIBRARIES})
98115

116+
unset(USE_GUROBI CACHE) # To avoid https://stackoverflow.com/questions/22481647/cmake-if-else-with-option
117+
99118

100119
# add_library(mader STATIC src/mader.cpp src/mader.hpp)
101120
# target_include_directories (mader PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -115,8 +134,3 @@ target_link_libraries(test_bspline_utils ${catkin_LIBRARIES})
115134
# add_executable(test_predictor_node src/test_predictor.cpp src/predictor.cpp src/bspline_utils.cpp)
116135
# add_dependencies(test_predictor_node ${catkin_EXPORTED_TARGETS})
117136
# target_link_libraries(test_predictor_node ${catkin_LIBRARIES} utils)
118-
119-
120-
# add_executable(test_cvxgen src/solvers/cvxgen/test_cvxgen.cpp src/solvers/cvxgen/solver_cvxgen.cpp)
121-
# add_dependencies(test_cvxgen ${catkin_EXPORTED_TARGETS})
122-
# target_link_libraries(test_cvxgen ${catkin_LIBRARIES} ${catkin_LIBRARIES} ${CMAKE_CURRENT_LIST_DIR}/src/solvers/cvxgen/cvxgen/libcvx.so)

‎mader/FindGUROBI.cmake

Lines changed: 0 additions & 72 deletions
This file was deleted.

‎mader/cmake_finders/FindGUROBI.cmake

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
#Taken (and modified) from https://support.gurobi.com/hc/en-us/articles/360039499751-How-do-I-use-CMake-to-build-Gurobi-C-C-projects-
2+
3+
find_path(GUROBI_INCLUDE_DIRS
4+
NAMES gurobi_c.h
5+
HINTS ${GUROBI_DIR} $ENV{GUROBI_HOME}
6+
PATH_SUFFIXES include)
7+
8+
set(gurobi_library_names)
9+
foreach(ver RANGE 00 99)
10+
list(APPEND gurobi_library_names gurobi${ver}) #Different versions of Gurobi
11+
endforeach()
12+
13+
find_library(GUROBI_LIBRARY
14+
NAMES gurobi ${gurobi_library_names}
15+
HINTS ${GUROBI_DIR} $ENV{GUROBI_HOME}
16+
PATH_SUFFIXES lib)
17+
18+
if(CXX)
19+
if(MSVC)
20+
# determine Visual Studio year
21+
if(MSVC_TOOLSET_VERSION EQUAL 142)
22+
set(MSVC_YEAR "2019")
23+
elseif(MSVC_TOOLSET_VERSION EQUAL 141)
24+
set(MSVC_YEAR "2017")
25+
elseif(MSVC_TOOLSET_VERSION EQUAL 140)
26+
set(MSVC_YEAR "2015")
27+
endif()
28+
29+
if(MT)
30+
set(M_FLAG "mt")
31+
else()
32+
set(M_FLAG "md")
33+
endif()
34+
35+
find_library(GUROBI_CXX_LIBRARY
36+
NAMES gurobi_c++${M_FLAG}${MSVC_YEAR}
37+
HINTS ${GUROBI_DIR} $ENV{GUROBI_HOME}
38+
PATH_SUFFIXES lib)
39+
find_library(GUROBI_CXX_DEBUG_LIBRARY
40+
NAMES gurobi_c++${M_FLAG}d${MSVC_YEAR}
41+
HINTS ${GUROBI_DIR} $ENV{GUROBI_HOME}
42+
PATH_SUFFIXES lib)
43+
else()
44+
find_library(GUROBI_CXX_LIBRARY
45+
NAMES gurobi_c++
46+
HINTS ${GUROBI_DIR} $ENV{GUROBI_HOME}
47+
PATH_SUFFIXES lib)
48+
set(GUROBI_CXX_DEBUG_LIBRARY ${GUROBI_CXX_LIBRARY})
49+
endif()
50+
endif()
51+
52+
include(FindPackageHandleStandardArgs)
53+
find_package_handle_standard_args(GUROBI DEFAULT_MSG GUROBI_LIBRARY)

‎mader/FindNLOPT.cmake renamed to ‎mader/cmake_finders/FindNLOPT.cmake

File renamed without changes.

‎mader/include/mader.hpp

Lines changed: 11 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,12 @@
1616
#include <mutex>
1717

1818
#include "mader_types.hpp"
19-
#include "solver_nlopt.hpp"
19+
20+
#if USE_GUROBI_FLAG
2021
#include "solver_gurobi.hpp"
22+
#else
23+
#include "solver_nlopt.hpp"
24+
#endif
2125

2226
// status_ : YAWING-->TRAVELING-->GOAL_SEEN-->GOAL_REACHED-->YAWING-->TRAVELING-->...
2327

@@ -150,10 +154,13 @@ class Mader
150154

151155
double time_init_opt_;
152156

153-
double av_improvement_nlopt_ = 0.0;
157+
// double av_improvement_nlopt_ = 0.0;
154158

155-
// SolverNlopt* solver_; // pointer to the optimization solver
156-
SolverGurobi* solver_; // pointer to the optimization solver
159+
#if USE_GUROBI_FLAG
160+
std::unique_ptr<SolverGurobi> solver_; // pointer to the optimization solver
161+
#else
162+
std::unique_ptr<SolverNlopt> solver_; // pointer to the optimization solver
163+
#endif
157164

158165
Eigen::Matrix<double, 4, 4> A_rest_pos_basis_;
159166
Eigen::Matrix<double, 4, 4> A_rest_pos_basis_inverse_;

‎mader/include/mader_types.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -589,7 +589,7 @@ struct parameters
589589
double y_min;
590590
double y_max;
591591

592-
double z_ground;
592+
double z_min;
593593
double z_max;
594594

595595
Eigen::Vector3d v_max;

‎mader/src/mader.cpp

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,10 @@
1717
#include "timer.hpp"
1818
#include "termcolor.hpp"
1919

20+
#if USE_GUROBI_FLAG
21+
#else
2022
#include "nlopt_utils.hpp"
23+
#endif
2124

2225
using namespace termcolor;
2326

@@ -36,6 +39,8 @@ Mader::Mader(mt::parameters par) : par_(par)
3639
stateA_.setZero();
3740
mtx_initial_cond.unlock();
3841

42+
#if USE_GUROBI_FLAG
43+
#else
3944
// Check that the gradients are right
4045
if (nlopt_utils::checkGradientsNlopt(par_.basis) == false)
4146
{
@@ -51,6 +56,7 @@ Mader::Mader(mt::parameters par) : par_(par)
5156
std::cout << bold << "Gradient check was " << green << " OK " << reset << std::endl;
5257
std::cout << "==============================================" << std::endl;
5358
}
59+
#endif
5460

5561
changeDroneStatus(DroneStatus::GOAL_REACHED);
5662
resetInitialization();
@@ -63,7 +69,7 @@ Mader::Mader(mt::parameters par) : par_(par)
6369
par_for_solver.y_min = par_.y_min;
6470
par_for_solver.y_max = par_.y_max;
6571

66-
par_for_solver.z_min = par_.z_ground;
72+
par_for_solver.z_min = par_.z_min;
6773
par_for_solver.z_max = par_.z_max;
6874

6975
par_for_solver.Ra = par_.Ra;
@@ -110,8 +116,14 @@ Mader::Mader(mt::parameters par) : par_(par)
110116

111117
A_rest_pos_basis_inverse_ = A_rest_pos_basis_.inverse();
112118

119+
#if USE_GUROBI_FLAG
120+
solver_ = std::unique_ptr<SolverGurobi>(new SolverGurobi(par_for_solver));
121+
#else
122+
solver_ = std::unique_ptr<SolverNlopt>(new SolverNlopt(par_for_solver));
123+
#endif
124+
113125
// solver_ = new SolverNlopt(par_for_solver);
114-
solver_ = new SolverGurobi(par_for_solver);
126+
// solver_ = new SolverGurobi(par_for_solver);
115127

116128
separator_solver_ = new separator::Separator();
117129
}

‎mader/src/mader_ros.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ MaderRos::MaderRos(ros::NodeHandle nh1, ros::NodeHandle nh2, ros::NodeHandle nh3
4444
mu::safeGetParam(nh1_, "y_min", par_.y_min);
4545
mu::safeGetParam(nh1_, "y_max", par_.y_max);
4646

47-
mu::safeGetParam(nh1_, "z_ground", par_.z_ground);
47+
mu::safeGetParam(nh1_, "z_min", par_.z_min);
4848
mu::safeGetParam(nh1_, "z_max", par_.z_max);
4949

5050
std::vector<double> v_max_tmp;

‎mader/src/nlopt_utils.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ bool nlopt_utils::checkGradientsNlopt(std::string basis)
2929
double y_min = -10.0;
3030
double y_max = 10.0;
3131

32-
double z_ground = -4.0;
32+
double z_min = -4.0;
3333
double z_max = 4.0;
3434
double dc = 0.01;
3535
double Ra = 4.0;
@@ -68,7 +68,7 @@ bool nlopt_utils::checkGradientsNlopt(std::string basis)
6868
param.y_min = y_min;
6969
param.y_max = y_max;
7070

71-
param.z_min = z_ground;
71+
param.z_min = z_min;
7272
param.z_max = z_max;
7373
param.v_max = Eigen::Vector3d(v_max, v_max, v_max);
7474
param.a_max = Eigen::Vector3d(a_max, a_max, a_max);

0 commit comments

Comments
 (0)