Draft: Nominal MPC support package

gnc/ctl_mpc/.gitmodules

@@ -0,0 +1,3 @@
+[submodule "acados"]
+	path = acados
+	url = https://github.com/acados/acados.git

gnc/ctl_mpc/CMakeLists.txt

@@ -0,0 +1,130 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(ctl_mpc)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11 -DACADOS_WITH_DAQP -DACADOS_WITH_OSQP)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  nodelet
+  std_msgs
+  ff_msgs
+  ff_util
+  config_reader
+)
+
+find_package(Eigen3 REQUIRED)
+
+if (EXISTS "${PROJECT_SOURCE_DIR}/acados/include" AND EXISTS "${PROJECT_SOURCE_DIR}/acados/lib" AND EXISTS "${PROJECT_SOURCE_DIR}/acados/include/daqp/")
+  message("Found acados!")
+else()
+  message( FATAL_ERROR "acados not found. Please compile the acados submodule in this repository first.")
+endif()
+
+set(acados_include ${PROJECT_SOURCE_DIR}/acados/include)
+set(acados_lib ${PROJECT_SOURCE_DIR}/acados/lib)
+set(solver_code ${PROJECT_SOURCE_DIR}/solver/c_generated_code)
+
+# This exports a file that can be used in VSCode so that it also finds the #includes (e.g. for intellisense) 
+set( CMAKE_EXPORT_COMPILE_COMMANDS ON )
+
+catkin_package(
+  INCLUDE_DIRS
+    include   
+    ${acados_include}/blasfeo/include/
+    ${acados_include}/hpipm/include/
+    ${acados_include}/
+    ${acados_include}/daqp/
+    ${acados_include}/qpOASES_e/
+    ${acados_include}/osqp/
+    ${acados_include}/../
+    # "solver/"
+    ${solver_code}
+    ${catkin_INCLUDE_DIRS}
+  LIBRARIES
+    ctl_mpc
+  CATKIN_DEPENDS
+    roscpp
+    nodelet
+    ff_msgs
+    ff_util
+    config_reader
+)
+
+include_directories(
+# include
+  ${acados_include}/blasfeo/include/
+  ${acados_include}/hpipm/include/
+  ${acados_include}/
+  ${acados_include}/daqp/
+  ${acados_include}/qpOASES_e/
+  ${acados_include}/osqp/
+  ${acados_include}/../
+  ${solver_code}
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+link_directories(
+  ${acados_lib}
+)
+
+## Declare a C++ library
+add_library(${PROJECT_NAME}
+  ${solver_code}/astrobee_constraints/astrobee_constr_h_e_fun_jac_uxt_zt_hess.c
+  ${solver_code}/astrobee_constraints/astrobee_constr_h_e_fun_jac_uxt_zt.c
+  ${solver_code}/astrobee_constraints/astrobee_constr_h_e_fun.c
+  ${solver_code}/astrobee_constraints/astrobee_constr_h_fun_jac_uxt_zt_hess.c 
+  ${solver_code}/astrobee_constraints/astrobee_constr_h_fun_jac_uxt_zt.c 
+  ${solver_code}/astrobee_constraints/astrobee_constr_h_fun.c 
+  ${solver_code}/astrobee_cost/astrobee_cost_ext_cost_0_fun_jac_hess.c
+  ${solver_code}/astrobee_cost/astrobee_cost_ext_cost_0_fun_jac.c 
+  ${solver_code}/astrobee_cost/astrobee_cost_ext_cost_0_fun.c 
+  ${solver_code}/astrobee_cost/astrobee_cost_ext_cost_e_fun_jac_hess.c
+  ${solver_code}/astrobee_cost/astrobee_cost_ext_cost_e_fun_jac.c 
+  ${solver_code}/astrobee_cost/astrobee_cost_ext_cost_e_fun.c 
+  ${solver_code}/astrobee_cost/astrobee_cost_ext_cost_fun_jac_hess.c
+  ${solver_code}/astrobee_cost/astrobee_cost_ext_cost_fun_jac.c 
+  ${solver_code}/astrobee_cost/astrobee_cost_ext_cost_fun.c 
+  ${solver_code}/astrobee_model/astrobee_expl_ode_fun.c
+  ${solver_code}/astrobee_model/astrobee_expl_ode_hess.c 
+  ${solver_code}/astrobee_model/astrobee_expl_vde_adj.c 
+  ${solver_code}/astrobee_model/astrobee_expl_vde_forw.c
+
+  ${solver_code}/acados_sim_solver_astrobee.c
+  ${solver_code}/acados_solver_astrobee.c
+
+  src/ctl_mpc.cpp
+  src/ctl_mpc_nodelet.cpp
+)
+
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} acados hpipm blasfeo m daqp ${catkin_LIBRARIES})
+
+# Mark libraries for installation
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+# Mark nodelet_plugin for installation
+install(FILES nodelet_plugins.xml
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+# Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+# Mark launch files for installation
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  PATTERN ".svn" EXCLUDE)

gnc/ctl_mpc/README.md

@@ -0,0 +1,49 @@
+# ctl_mpc Package
+This is an extension package for the [NASA Astrobee project](https://github.com/nasa/astrobee) which allows MPC with Acados.
+
+## Installation
+This assumes you have the astrobee software already installed and built.
+
+1. Clone this repo in a suitable directory, e.g.
+```
+cd $ASTROBEE_WS/src/gnc
+git clone https://github.com/DISCOWER/ctl_acados_mpc.git
+cd ctl_acados_mpc
+```
+
+2. Now wee need to install acados. First run `git submodule update --recursive --init`
+
+3. Make a build directory and install acados. You can choose which solvers you would like to use with optional flags
+```
+cd acados
+mkdir -p build
+cd build
+cmake .. # with optional arguments e.g. -DACADOS_WITH_DAQP=OFF/ON -DACADOS_INSTALL_DIR=<path_to_acados_installation_folder>
+make install
+```
+4. In case you want to rebuild the c-code for the MPC (e.g. with a different QP solver, different horizon):
+Install the python interface with pip: `pip install -e ./interfaces/acados_template`
+Note that some parameters can be changed online (costs, state tolerances, inertia and mass)
+
+5. Add the compiled shared library paths to the following variables:
+```
+export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"<acados_root>/lib"
+export ACADOS_SOURCE_DIR="<acados_root>"
+```
+
+6. Build this package by running `catkin build --this --no-deps`
+
+## Usage
+1. Open 2 terminals, go to your `$ASTROBEE_WS` and run `source devel/setup.bash` on each
+
+2. In the first terminal, launch the astrobee. E.g. for a simulation, do `roslaunch astrobee astrobee_sim.launch`. Check out the launch files for available settings
+
+3. In the second terminal, you can send the astrobee on a mission! E.g. run: `rosrun executive teleop_tool -move -relative -pos "0 -1 0"`
+
+### (OPTIONAL in case you want to tune params/settings that are unavailable for change in the C++ code)
+5. Open `/solver/generate_solver.py` and tune the settings to your liking. Then run `python generate_solver.py`
+
+6. Re-build the package. You can do this by making sure you are in the package root folder (e.g. `$ASTROBEE_WS/gnc/ctl_acados_mpc`) and running `catkin build --this --no-deps`
+
+## Acknowledgements
+A special thanks to Pedro Roque for instights, bug-fixing help and code contribution in the CMakeLists.txt.

gnc/ctl_mpc/acados/.appveyor.yml

@@ -0,0 +1,39 @@
+branches:
+  only:
+    - master
+
+cache:
+  - c:\dev\Eigen3\
+
+environment:
+  matrix:
+    - BUILD_TYPE: Release
+      COMPILER: MinGW-w64
+      PLATFORM: x64
+
+install:
+  - cmd: if not exist c:\dev\Eigen3\include\eigen3\Eigen\Core (
+            curl -L -o eigen-3.2.10.tar.gz https://gitlab.com/libeigen/eigen/-/archive/3.2.10/eigen-3.2.10.tar.gz &&
+            cmake -E tar zxf eigen-3.2.10.tar.gz &&
+            cd eigen-3.2.10 &&
+            mkdir build &&
+            cd build &&
+            cmake -G "Visual Studio 12" -DCMAKE_BUILD_TYPE=Debug -DCMAKE_INSTALL_PREFIX="c:\dev\Eigen3" .. &&
+            cmake --build . --target install --config Release &&
+            cd ..\..
+         ) else (echo Using cached Eigen3)
+
+# NOTE: only static linking, i.e. via .lib file supported
+#  -D BUILD_SHARED_LIBS=OFF;
+# extern "C" {  would have to be added to subprojects
+
+build_script:
+  - git submodule update --init --recursive
+  # MinGW files need to be at the beginning of the PATH
+  - set PATH=C:\mingw-w64\x86_64-7.3.0-posix-seh-rt_v5-rev0\mingw64\bin;%PATH%
+  # Workaround for CMake not wanting sh.exe on PATH for MinGW
+  - rm "C:\Program Files\Git\usr\bin\sh.exe"
+  - cmake -E make_directory build
+  - cmake -E chdir build cmake -DCMAKE_PREFIX_PATH=C:\mingw-w64\x86_64-7.3.0-posix-seh-rt_v5-rev0 -D ACADOS_WITH_QPOASES=ON -D BLASFEO_TARGET=GENERIC -D HPIPM_TARGET=GENERIC -D BUILD_SHARED_LIBS=OFF -D ACADOS_EXAMPLES=ON -D ACADOS_UNIT_TESTS=ON -DEIGEN3_INCLUDE_DIR="C:\dev\Eigen3\include\eigen3" -G "MinGW Makefiles" ..
+  - cmake --build build --config Release
+  - cmake -E chdir build ctest --output-on-failure

gnc/ctl_mpc/acados/.clang-format

@@ -0,0 +1,65 @@
+---
+Language:        Cpp
+BasedOnStyle:  Google
+AccessModifierOffset: -3
+AlignAfterOpenBracket: true
+AlignEscapedNewlinesLeft: true
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: All
+AlwaysBreakAfterDefinitionReturnType: false
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: true
+BreakBeforeBinaryOperators: None
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: false
+BinPackParameters: true
+BinPackArguments: true
+ColumnLimit:     100
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+DerivePointerAlignment: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+IndentWrappedFunctionNames: false
+IndentFunctionDeclarationAfterType: false
+MaxEmptyLinesToKeep: 3
+KeepEmptyLinesAtTheStartOfBlocks: false
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: false
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakString: 1000
+PenaltyBreakFirstLessLess: 120
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: true
+Standard:        Auto
+IndentWidth:     4
+TabWidth:        4
+UseTab:          Never
+BreakBeforeBraces: Allman
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+SpacesInAngles:  false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterCStyleCast: true
+SpacesInContainerLiterals: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+CommentPragmas:  '^ IWYU pragma:'
+ForEachMacros:   [ foreach, Q_FOREACH, BOOST_FOREACH ]
+SpaceBeforeParens: ControlStatements
+DisableFormat:   false
+...
+

gnc/ctl_mpc/acados/.gitattributes

@@ -0,0 +1 @@
+qpOASES/doc/html/*.html linguist-documentation

gnc/ctl_mpc/acados/.github/linux/install_casadi_matlab.sh

@@ -0,0 +1,45 @@
+#!/bin/bash
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+# CASADI_VERSION='3.5.3'; # Latest version with Octave 4.2.2 binaries
+CASADI_VERSION='3.5.5';
+MATLAB_VERSION='R2014b';
+
+_CASADI_GITHUB_RELEASES="https://github.com/casadi/casadi/releases/download/${CASADI_VERSION}";
+
+CASADI_MATLAB_URL="${_CASADI_GITHUB_RELEASES}/casadi-linux-matlab${MATLAB_VERSION}-v${CASADI_VERSION}.tar.gz";
+
+wget -O casadi-linux-matlab.tar.gz "${CASADI_MATLAB_URL}";
+mkdir -p casadi-matlab;
+tar -xf casadi-linux-matlab.tar.gz -C casadi-matlab;

gnc/ctl_mpc/acados/.github/linux/install_casadi_octave.sh

@@ -0,0 +1,45 @@
+#!/bin/bash
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+# CASADI_VERSION='3.5.3'; # Latest version with Octave 4.2.2 binaries
+CASADI_VERSION='3.5.5';
+OCTAVE_VERSION='5.2.0';
+
+_CASADI_GITHUB_RELEASES="https://github.com/casadi/casadi/releases/download/${CASADI_VERSION}";
+
+CASADI_OCTAVE_URL="${_CASADI_GITHUB_RELEASES}/casadi-linux-octave-${OCTAVE_VERSION}-v${CASADI_VERSION}.tar.gz";
+
+wget -O casadi-linux-octave.tar.gz "${CASADI_OCTAVE_URL}";
+mkdir -p casadi-octave;
+tar -xf casadi-linux-octave.tar.gz -C casadi-octave;