├── .clang-format ├── .clang-format-check.sh ├── .clang-format-common.sh ├── .clang-format-fix.sh ├── .github └── workflows │ ├── ci-colcon.yaml │ ├── ci-standalone.yaml │ └── package.yaml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── debian ├── changelog ├── compat ├── control ├── copyright └── rules ├── doc └── Install.md ├── nmpc ├── CMakeLists.txt └── package.xml ├── nmpc_cgmres ├── CMakeLists.txt ├── README.md ├── doc │ ├── CMakeLists.txt │ ├── Doxyfile.extra.in │ └── images │ │ ├── TestCartPoleProblem.png │ │ └── TestSemiactiveDamperProblem.png ├── include │ └── nmpc_cgmres │ │ ├── CgmresProblem.h │ │ ├── CgmresSolver.h │ │ ├── Gmres.h │ │ └── OdeSolver.h ├── package.xml ├── scripts │ └── plotCgmresData.py ├── src │ └── CgmresSolver.cpp └── tests │ ├── CMakeLists.txt │ └── src │ ├── CartPoleProblem.h │ ├── SemiactiveDamperProblem.h │ ├── TestCgmresSolver.cpp │ └── TestGmres.cpp ├── nmpc_ddp ├── CMakeLists.txt ├── README.md ├── doc │ ├── CMakeLists.txt │ ├── Doxyfile.extra.in │ └── images │ │ ├── TestDDPBipedal.png │ │ ├── TestDDPCartPole.gif │ │ └── TestDDPVerticalMotion.png ├── include │ └── nmpc_ddp │ │ ├── BoxQP.h │ │ ├── DDPProblem.h │ │ ├── DDPSolver.h │ │ └── DDPSolver.hpp ├── package.xml ├── scripts │ └── plotDDPTraceData.py └── tests │ ├── CMakeLists.txt │ ├── rqt │ └── TestDDPCartPole.perspective │ ├── rviz │ └── TestDDPCartPole.rviz │ ├── scripts │ ├── plotTestDDPBipedal.py │ └── plotTestDDPVerticalMotion.py │ ├── src │ ├── TestBoxQP.cpp │ ├── TestDDPBipedal.cpp │ ├── TestDDPCartPole.cpp │ ├── TestDDPCentroidalMotion.cpp │ └── TestDDPVerticalMotion.cpp │ └── test │ └── TestDDPCartPole.test └── nmpc_fmpc ├── CMakeLists.txt ├── README.md ├── doc ├── CMakeLists.txt ├── Doxyfile.extra.in └── images │ ├── TestFmpcCartPole.png │ └── TestFmpcOscillator.png ├── include └── nmpc_fmpc │ ├── FmpcProblem.h │ ├── FmpcSolver.h │ ├── FmpcSolver.hpp │ └── MathUtils.h ├── package.xml └── tests ├── CMakeLists.txt ├── rqt └── TestFmpcCartPole.perspective ├── rviz └── TestFmpcCartPole.rviz ├── src ├── TestFmpcCartPole.cpp ├── TestFmpcOscillator.cpp └── TestMathUtils.cpp └── test └── TestFmpcCartPole.test /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | AccessModifierOffset: -2 4 | AlignAfterOpenBracket: Align 5 | AlignConsecutiveAssignments: false 6 | AlignConsecutiveDeclarations: false 7 | AlignEscapedNewlinesLeft: true 8 | AlignOperands: true 9 | AlignTrailingComments: false 10 | AllowAllParametersOfDeclarationOnNextLine: false 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: Empty 14 | AllowShortIfStatementsOnASingleLine: true 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: false 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: true 21 | BinPackParameters: false 22 | BreakBeforeBinaryOperators: NonAssignment 23 | BreakBeforeBraces: Allman 24 | BreakBeforeInheritanceComma: false 25 | BreakBeforeTernaryOperators: true 26 | BreakConstructorInitializersBeforeComma: false 27 | BreakConstructorInitializers: BeforeColon 28 | BreakAfterJavaFieldAnnotations: false 29 | BreakStringLiterals: true 30 | ColumnLimit: 120 31 | CommentPragmas: '^ IWYU pragma:' 32 | CompactNamespaces: false 33 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 34 | ConstructorInitializerIndentWidth: 0 35 | ContinuationIndentWidth: 4 36 | Cpp11BracedListStyle: true 37 | DerivePointerAlignment: false 38 | DisableFormat: false 39 | ExperimentalAutoDetectBinPacking: false 40 | FixNamespaceComments: true 41 | ForEachMacros: 42 | - foreach 43 | - Q_FOREACH 44 | - BOOST_FOREACH 45 | IncludeBlocks: Preserve 46 | IncludeCategories: 47 | - Regex: '^( $tmpfile 11 | if ! [[ -z `diff $tmpfile $f` ]]; then 12 | echo "Wrong formatting in $f" 13 | out=1 14 | fi 15 | done 16 | rm -f $tmpfile 17 | if [[ $out -eq 1 ]]; then 18 | echo "You can run ./.clang-format-fix.sh to fix the issues locally, then commit/push again" 19 | fi 20 | exit $out 21 | -------------------------------------------------------------------------------- /.clang-format-common.sh: -------------------------------------------------------------------------------- 1 | # This script is meant to be sourced from other scripts 2 | 3 | # Check for clang-format, prefer 10 if available 4 | if [[ -x "$(command -v clang-format-10)" ]]; then 5 | clang_format=clang-format-10 6 | elif [[ -x "$(command -v clang-format)" ]]; then 7 | clang_format=clang-format 8 | else 9 | echo "clang-format or clang-format-10 must be installed" 10 | exit 1 11 | fi 12 | 13 | # Find all source files in the project minus those that are auto-generated or we do not maintain 14 | src_files=`find ./ -name '*.cpp' -or -name '*.h' -or -name '*.hpp'` 15 | -------------------------------------------------------------------------------- /.clang-format-fix.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | readonly this_dir=`cd $(dirname $0); pwd` 4 | cd $this_dir 5 | source .clang-format-common.sh 6 | 7 | for f in ${src_files}; do 8 | $clang_format -style=file -i $f 9 | done 10 | -------------------------------------------------------------------------------- /.github/workflows/ci-colcon.yaml: -------------------------------------------------------------------------------- 1 | name: CI of NMPC (colcon) 2 | 3 | on: 4 | push: 5 | branches: 6 | - '**' 7 | pull_request: 8 | branches: 9 | - '**' 10 | schedule: 11 | - cron: '0 0 * * 0' 12 | 13 | jobs: 14 | 15 | clang-format: 16 | runs-on: ubuntu-22.04 17 | steps: 18 | - name: Check out repository code 19 | uses: actions/checkout@v3 20 | - name: Install clang-format-14 21 | run: | 22 | sudo apt-get -qq update 23 | sudo apt-get -qq install clang-format-14 24 | - name: Run clang-format-check 25 | run: | 26 | ./.clang-format-check.sh 27 | 28 | build-and-test: 29 | strategy: 30 | fail-fast: false 31 | matrix: 32 | os: [Ubuntu-22.04, Ubuntu-24.04] 33 | build-type: [Debug, RelWithDebInfo, Release] 34 | include: 35 | - build-type: Debug 36 | optimize-for-native: OFF 37 | - build-type: RelWithDebInfo 38 | optimize-for-native: OFF 39 | - build-type: Release 40 | optimize-for-native: ON 41 | runs-on: ${{ matrix.os }} 42 | steps: 43 | - name: Setup environment variables 44 | run: | 45 | set -e 46 | set -x 47 | if [ "${{ matrix.os }}" == "ubuntu-22.04" ] && \ 48 | [ "${{ matrix.build-type }}" == "RelWithDebInfo" ] && \ 49 | [ "${{ github.repository_owner }}" == "isri-aist" ] && \ 50 | [ "${{ github.ref }}" == "refs/heads/master" ] 51 | then 52 | echo "UPLOAD_DOCUMENTATION=true" >> $GITHUB_ENV 53 | sudo apt-get install -qq doxygen graphviz 54 | else 55 | echo "UPLOAD_DOCUMENTATION=false" >> $GITHUB_ENV 56 | fi 57 | - name: Install ROS 58 | uses: jrl-umi3218/github-actions/install-dependencies@master 59 | with: 60 | build-type: ${{ matrix.build-type }} 61 | ros: | 62 | apt: ros-base 63 | - name: Colcon build 64 | uses: jrl-umi3218/github-actions/build-colcon-project@master 65 | with: 66 | build-type: ${{ matrix.build-type }} 67 | cmake-args: -DUSE_ROS2=ON -DOPTIMIZE_FOR_NATIVE=${{ matrix.optimize-for-native }} -DINSTALL_DOCUMENTATION=${{ env.UPLOAD_DOCUMENTATION }} 68 | build-packages: NMPC 69 | test-packages: NMPC 70 | 71 | - name: Upload documentation 72 | # Only run for one configuration and on origin master branch 73 | if: env.UPLOAD_DOCUMENTATION == 'true' 74 | run: | 75 | set -e 76 | set -x 77 | cd ${GITHUB_WORKSPACE}/colcon_ws/src/${{ github.repository }} 78 | git config --global user.name "Masaki Murooka" 79 | git config --global user.email "m-murooka@aist.go.jp" 80 | git remote set-url origin "https://mmurooka:${{ secrets.CI_TOKEN }}@github.com/isri-aist/NMPC" 81 | git fetch --depth=1 origin gh-pages:gh-pages 82 | git checkout --quiet gh-pages 83 | rm -rf nmpc_ddp/ nmpc_fmpc/ nmpc_cgmres/ cmake/ 84 | cp -r ${GITHUB_WORKSPACE}/colcon_ws/build/nmpc_ddp/doc/doxygen-html/ nmpc_ddp 85 | cp -r ${GITHUB_WORKSPACE}/colcon_ws/build/nmpc_fmpc/doc/doxygen-html/ nmpc_fmpc 86 | cp -r ${GITHUB_WORKSPACE}/colcon_ws/build/nmpc_cgmres/doc/doxygen-html/ nmpc_cgmres 87 | git add nmpc_ddp nmpc_fmpc nmpc_cgmres 88 | git_status=`git status -s` 89 | if test -n "$git_status"; then 90 | git commit --quiet -m "Update Doxygen HTML files from commit ${{ github.sha }}" 91 | git push origin gh-pages 92 | else 93 | echo "Github pages documentation is already up-to-date." 94 | fi 95 | -------------------------------------------------------------------------------- /.github/workflows/ci-standalone.yaml: -------------------------------------------------------------------------------- 1 | name: CI of NMPC (standalone) 2 | 3 | on: 4 | push: 5 | branches: 6 | - '**' 7 | pull_request: 8 | branches: 9 | - '**' 10 | schedule: 11 | - cron: '0 0 * * 0' 12 | 13 | jobs: 14 | build-and-test: 15 | strategy: 16 | fail-fast: false 17 | matrix: 18 | os: [ubuntu-20.04, Ubuntu-22.04, Ubuntu-24.04] 19 | build-type: [Debug, RelWithDebInfo, Release] 20 | compiler: [gcc, clang] 21 | include: 22 | - build-type: Debug 23 | optimize-for-native: OFF 24 | - build-type: RelWithDebInfo 25 | optimize-for-native: OFF 26 | - build-type: Release 27 | optimize-for-native: ON 28 | runs-on: ${{ matrix.os }} 29 | steps: 30 | - uses: actions/checkout@v3 31 | with: 32 | submodules: recursive 33 | - name: Install dependencies 34 | uses: jrl-umi3218/github-actions/install-dependencies@master 35 | with: 36 | compiler: ${{ matrix.compiler }} 37 | build-type: ${{ matrix.build-type }} 38 | ubuntu: | 39 | apt: cmake libeigen3-dev libgtest-dev 40 | - name: Build and test 41 | uses: jrl-umi3218/github-actions/build-cmake-project@master 42 | with: 43 | compiler: ${{ matrix.compiler }} 44 | build-type: ${{ matrix.build-type }} 45 | options: -DOPTIMIZE_FOR_NATIVE=${{ matrix.optimize-for-native }} 46 | -------------------------------------------------------------------------------- /.github/workflows/package.yaml: -------------------------------------------------------------------------------- 1 | name: Package NMPC 2 | on: 3 | repository_dispatch: 4 | types: 5 | - package-master 6 | - package-release 7 | pull_request: 8 | branches: 9 | - "**" 10 | push: 11 | paths-ignore: 12 | - doc/** 13 | - README.md 14 | - ".github/workflows/ci-catkin.yaml" 15 | - ".github/workflows/ci-standalone.yaml" 16 | branches: 17 | - "**" 18 | tags: 19 | - v* 20 | jobs: 21 | package: 22 | uses: jrl-umi3218/github-actions/.github/workflows/package-project.yml@master 23 | with: 24 | latest-cmake: true 25 | deps: '["isri-aist/CentroidalControlCollection"]' 26 | main-repo: isri-aist/NMPC 27 | matrix: | 28 | { 29 | "dist": ["focal", "jammy", "noble"], 30 | "arch": ["amd64"], 31 | "include": 32 | [ 33 | {"dist": "bionic", "arch": "i386" } 34 | ] 35 | } 36 | secrets: 37 | CLOUDSMITH_API_KEY: ${{ secrets.CLOUDSMITH_API_KEY }} 38 | GH_TOKEN: ${{ secrets.GH_PAGES_TOKEN }} 39 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | 3 | GPATH 4 | GRTAGS 5 | GTAGS 6 | .vscode -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake"] 2 | path = cmake 3 | url = https://github.com/jrl-umi3218/jrl-cmakemodules 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # >=3.14 required for CMP0087 2 | cmake_minimum_required(VERSION 3.14) 3 | 4 | set(PROJECT_VERSION 0.1.0) 5 | project(NMPC) 6 | 7 | option(BUILD_SHARED_LIBS "Build shared libraries" ON) 8 | option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) 9 | 10 | # Set a default build type to 'RelwithDebInfo' if none was specified 11 | IF(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES AND NOT ENV{CMAKE_BUILD_TYPE}) 12 | set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "Choose the type of build." FORCE) 13 | # Set the possible values of build type for cmake-gui 14 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 15 | ENDIF() 16 | 17 | include(CTest) 18 | enable_testing() 19 | 20 | # Check if the install prefix is writable by the current user and set SUDO_CMD accordingly 21 | set(SUDO_CMD) 22 | if(NOT EXISTS "${CMAKE_INSTALL_PREFIX}") 23 | execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_INSTALL_PREFIX} OUTPUT_QUIET ERROR_QUIET) 24 | endif() 25 | set(TEST_FILE "${CMAKE_INSTALL_PREFIX}/${PROJECT_NAME}.writable") 26 | # Same as file(TOUCH ...) but ignores failure 27 | execute_process(COMMAND ${CMAKE_COMMAND} -E touch ${TEST_FILE} OUTPUT_QUIET ERROR_QUIET) 28 | if(NOT EXISTS ${TEST_FILE}) 29 | find_program(SUDO_CMD sudo) 30 | if(NOT SUDO_CMD) 31 | message(FATAL_ERROR "sudo is not installed on this system and the install prefix (${CMAKE_INSTALL_PREFIX}) is not writable by the current user. 32 | You can try the following solutions: 33 | - Choose a different installation prefix; 34 | - Install sudo; 35 | - Fix the permissions and try again;") 36 | endif() 37 | message("-- Use sudo for install: ${SUDO_CMD}") 38 | set(SUDO_CMD ${SUDO_CMD} -E) 39 | else() 40 | file(REMOVE ${TEST_FILE}) 41 | endif() 42 | 43 | include(ExternalProject) 44 | 45 | function(add_nmpc_project NAME) 46 | file(GLOB_RECURSE SOURCES CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/${NAME}/*") 47 | ExternalProject_Add(${NAME} 48 | SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/${NAME}" 49 | BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/${NAME}" 50 | CMAKE_ARGS 51 | -DBUILD_SHARED_LIBS=${BUILD_SHARED_LIBS} 52 | -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} 53 | -DCMAKE_EXPORT_COMPILE_COMMANDS=ON 54 | -DINSTALL_DOCUMENTATION=${INSTALL_DOCUMENTATION} 55 | -DBUILD_TESTING=${BUILD_TESTING} 56 | -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} 57 | -DNMPC_STANDALONE=ON 58 | TEST_COMMAND ctest -C $ 59 | INSTALL_COMMAND ${SUDO_CMD} ${CMAKE_COMMAND} --build ${CMAKE_CURRENT_BINARY_DIR}/${NAME} --target install --config $ 60 | USES_TERMINAL_INSTALL TRUE 61 | STEP_TARGETS configure install 62 | ${ARGN} 63 | ) 64 | ExternalProject_Add_Step(${NAME} check-sources 65 | DEPENDERS configure 66 | DEPENDS ${SOURCES} 67 | ) 68 | add_test( 69 | NAME test-${NAME} 70 | COMMAND ctest -C $ 71 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/${NAME}" 72 | ) 73 | install(CODE "execute_process(COMMAND ${CMAKE_COMMAND} --build ${CMAKE_CURRENT_BINARY_DIR}/${NAME} --target install --config $)") 74 | endfunction() 75 | add_nmpc_project(nmpc_cgmres) 76 | add_nmpc_project(nmpc_ddp) 77 | add_nmpc_project(nmpc_fmpc DEPENDS nmpc_ddp) 78 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2022, AIST-CNRS JRL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | **This is the branch for ROS2; use [the ros1 branch](https://github.com/isri-aist/NMPC/tree/ros1) for ROS1.** 2 | 3 | # [NMPC](https://github.com/isri-aist/NMPC) 4 | Non-linear model predictive control (NMPC) library 5 | 6 | [![CI-standalone](https://github.com/isri-aist/NMPC/actions/workflows/ci-standalone.yaml/badge.svg)](https://github.com/isri-aist/NMPC/actions/workflows/ci-standalone.yaml) 7 | [![CI-colcon](https://github.com/isri-aist/NMPC/actions/workflows/ci-colcon.yaml/badge.svg)](https://github.com/isri-aist/NMPC/actions/workflows/ci-colcon.yaml) 8 | 9 | This repository provides ROS2 packages for the following NMPC methods 10 | 11 | ## [nmpc_ddp](./nmpc_ddp) 12 | 13 | [![Documentation](https://img.shields.io/badge/doxygen-online-brightgreen?logo=read-the-docs&style=flat)](https://isri-aist.github.io/NMPC/nmpc_ddp/index.html) 14 | 15 | NMPC with differential dynamic drogramming (DDP) 16 | 17 | ## [nmpc_fmpc](./nmpc_fmpc) 18 | 19 | [![Documentation](https://img.shields.io/badge/doxygen-online-brightgreen?logo=read-the-docs&style=flat)](https://isri-aist.github.io/NMPC/nmpc_fmpc/index.html) 20 | 21 | FMPC: Fast NMPC combining the direct multiple shooting (DMS) method, the primal-dual interior point (PDIP) method, and Riccati recursion (RR) 22 | 23 | ## [nmpc_cgmres](./nmpc_cgmres) 24 | 25 | [![Documentation](https://img.shields.io/badge/doxygen-online-brightgreen?logo=read-the-docs&style=flat)](https://isri-aist.github.io/NMPC/nmpc_cgmres/index.html) 26 | 27 | NMPC with continuation/GMRES method (C/GMRES) 28 | -------------------------------------------------------------------------------- /debian/changelog: -------------------------------------------------------------------------------- 1 | nmpc (0.1.0-1debian1) unstable; urgency=medium 2 | 3 | * Initial release. 4 | 5 | -- Pierre Gergondet Fri, 02 Dec 2022 23:06:23 +0900 6 | -------------------------------------------------------------------------------- /debian/compat: -------------------------------------------------------------------------------- 1 | 9 2 | -------------------------------------------------------------------------------- /debian/control: -------------------------------------------------------------------------------- 1 | Source: nmpc 2 | Priority: optional 3 | Maintainer: Pierre Gergondet 4 | Standards-Version: 3.9.5 5 | Section: libs 6 | Homepage: https://github.com/isri-aist/nmpc 7 | Build-Depends: debhelper (>= 9), 8 | pkg-config, 9 | cmake, 10 | doxygen, 11 | graphviz, 12 | libeigen3-dev (>= 3.2), 13 | libgtest-dev 14 | 15 | Package: libnmpc-dev 16 | Section: libdevel 17 | Architecture: any 18 | Depends: cmake, 19 | libeigen3-dev (>= 3.2), 20 | ${shlibs:Depends}, 21 | ${misc:Depends} 22 | Description: Non-linear model predictive control (NMPC) library 23 | -------------------------------------------------------------------------------- /debian/copyright: -------------------------------------------------------------------------------- 1 | Format: http://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ 2 | Upstream-Name: nmpc 3 | Source: https://github.com/isri-aist/nmpc 4 | 5 | Files: * 6 | Copyright: 2022 CNRS-AIST JRL 7 | License: BSD-2-clause 8 | BSD 2-Clause License 9 | . 10 | Copyright (c) 2022, AIST-CNRS JRL 11 | All rights reserved. 12 | . 13 | Redistribution and use in source and binary forms, with or without 14 | modification, are permitted provided that the following conditions are met: 15 | . 16 | 1. Redistributions of source code must retain the above copyright notice, this 17 | list of conditions and the following disclaimer. 18 | . 19 | 2. Redistributions in binary form must reproduce the above copyright notice, 20 | this list of conditions and the following disclaimer in the documentation 21 | and/or other materials provided with the distribution. 22 | . 23 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 24 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 25 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 26 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 27 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 28 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 29 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 31 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 32 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | -------------------------------------------------------------------------------- /debian/rules: -------------------------------------------------------------------------------- 1 | #!/usr/bin/make -f 2 | # -*- makefile -*- 3 | 4 | # Uncomment this to turn on verbose mode. 5 | #export DH_VERBOSE=1 6 | 7 | export BUILD_TESTING:=ON 8 | export VERSION_CODENAME:=$(shell . /etc/os-release && echo $$VERSION_CODENAME) 9 | # We disable tests in Bionic because gtest does not come in a usable way 10 | ifeq (${VERSION_CODENAME}, bionic) 11 | export BUILD_TESTING:=OFF 12 | endif 13 | 14 | %: 15 | dh $@ --parallel 16 | 17 | override_dh_auto_configure: 18 | dh_auto_configure -- -DBUILD_TESTING:BOOL=${BUILD_TESTING} 19 | -------------------------------------------------------------------------------- /doc/Install.md: -------------------------------------------------------------------------------- 1 | ## Install 2 | 3 | ### Requirements 4 | - Compiler supporting C++17 5 | - Tested on `Ubuntu 20.04 / ROS Noetic` and `Ubuntu 18.04 / ROS Melodic` 6 | 7 | ### Installation procedure 8 | It is assumed that ROS is installed. 9 | 10 | 1. Setup catkin workspace. 11 | ```bash 12 | $ mkdir -p ~/ros/ws_nmpc/src 13 | $ cd ~/ros/ws_nmpc 14 | $ wstool init src 15 | $ wstool set -t src isri-aist/NMPC git@github.com:isri-aist/NMPC.git --git -y 16 | $ wstool update -t src 17 | ``` 18 | 19 | 2. Install dependent packages. 20 | ```bash 21 | $ source /opt/ros/${ROS_DISTRO}/setup.bash 22 | $ rosdep install -y -r --from-paths src --ignore-src 23 | ``` 24 | 25 | 3. Build a package. 26 | ```bash 27 | $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --catkin-make-args all tests 28 | # For best performance 29 | $ catkin build -DOPTIMIZE_FOR_NATIVE=ON -DCMAKE_BUILD_TYPE=Release --catkin-make-args all tests 30 | ``` 31 | -------------------------------------------------------------------------------- /nmpc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(nmpc) 3 | 4 | ament_package() 5 | -------------------------------------------------------------------------------- /nmpc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | nmpc 3 | 0.1.0 4 | 5 | Metapackage for non-linear model predictive control (NMPC) 6 | 7 | Masaki Murooka 8 | BSD 9 | 10 | http://ros.org/wiki/nmpc 11 | Masaki Murooka 12 | 13 | ament_cmake 14 | 15 | nmpc_ddp 16 | nmpc_fmpc 17 | nmpc_cgmres 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /nmpc_cgmres/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | set(PROJECT_NAME nmpc_cgmres) 4 | set(PROJECT_GENERATED_HEADERS_SKIP_DEPRECATED ON) 5 | set(PROJECT_GENERATED_HEADERS_SKIP_CONFIG ON) 6 | set(PROJECT_GENERATED_HEADERS_SKIP_WARNING ON) 7 | set(PROJECT_URL https://github.com/isri-aist/NMPC) 8 | set(PROJECT_DESCRIPTION "") 9 | set(CMAKE_CXX_STANDARD 17) 10 | set(PROJECT_USE_CMAKE_EXPORT TRUE) 11 | set(CXX_DISABLE_WERROR ON) 12 | 13 | option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) 14 | option(USE_ROS2 "Use ROS2" OFF) 15 | 16 | include(../cmake/base.cmake) 17 | project(nmpc_cgmres LANGUAGES CXX) 18 | 19 | # Options 20 | option(OPTIMIZE_FOR_NATIVE "Enable -march=native" OFF) 21 | 22 | if(NOT DEFINED NMPC_STANDALONE) 23 | set(NMPC_STANDALONE OFF) 24 | endif() 25 | 26 | if(NOT NMPC_STANDALONE) 27 | find_package(rclcpp REQUIRED) 28 | endif() 29 | 30 | # Eigen 31 | find_package(Eigen3 QUIET NO_CMAKE_PACKAGE_REGISTRY) 32 | if(Eigen3_FOUND) 33 | add_project_dependency(Eigen3 REQUIRED NO_CMAKE_PACKAGE_REGISTRY) 34 | else() 35 | add_project_dependency(Eigen3 MODULE REQUIRED) 36 | endif() 37 | 38 | if(NOT NMPC_STANDALONE) 39 | ament_package() 40 | else() 41 | set(CATKIN_ENABLE_TESTING OFF) 42 | endif() 43 | 44 | add_library(nmpc_cgmres 45 | src/CgmresSolver.cpp 46 | ) 47 | target_compile_features(nmpc_cgmres PUBLIC cxx_std_17) 48 | target_include_directories(nmpc_cgmres PUBLIC $ $) 49 | 50 | if(TARGET Eigen3::Eigen) 51 | target_link_libraries(nmpc_cgmres PUBLIC Eigen3::Eigen) 52 | else() 53 | target_include_directories(nmpc_cgmres SYSTEM PUBLIC "${EIGEN3_INCLUDE_DIR}") 54 | endif() 55 | 56 | install(TARGETS nmpc_cgmres EXPORT "${TARGETS_EXPORT_NAME}") 57 | install(DIRECTORY include/nmpc_cgmres DESTINATION "${INCLUDE_INSTALL_DIR}") 58 | 59 | 60 | if(OPTIMIZE_FOR_NATIVE) 61 | # TODO fix segmentation faults 62 | message(WARNING "The -march=native option is disabled for nmpc_cgmres because it causes segmentation faults in tests.") 63 | # target_compile_options(nmpc_cgmres INTERFACE -march=native) 64 | endif() 65 | 66 | if(BUILD_TESTING OR CATKIN_ENABLE_TESTING) 67 | add_subdirectory(tests) 68 | endif() 69 | 70 | if(INSTALL_DOCUMENTATION) 71 | add_subdirectory(doc) 72 | endif() 73 | -------------------------------------------------------------------------------- /nmpc_cgmres/README.md: -------------------------------------------------------------------------------- 1 | # nmpc_cgmres 2 | Non-linear model predictive control (NMPC) with continuation/GMRES method (C/GMRES) 3 | 4 | [![CI-standalone](https://github.com/isri-aist/NMPC/actions/workflows/ci-standalone.yaml/badge.svg)](https://github.com/isri-aist/NMPC/actions/workflows/ci-standalone.yaml) 5 | [![CI-catkin](https://github.com/isri-aist/NMPC/actions/workflows/ci-catkin.yaml/badge.svg)](https://github.com/isri-aist/NMPC/actions/workflows/ci-catkin.yaml) 6 | [![Documentation](https://img.shields.io/badge/doxygen-online-brightgreen?logo=read-the-docs&style=flat)](https://isri-aist.github.io/NMPC/nmpc_cgmres/index.html) 7 | 8 | ## Install 9 | See [here](https://isri-aist.github.io/NMPC/doc/Install). 10 | 11 | ## Technical details 12 | See the following for a detailed algorithm. 13 | - T Ohtsuka. Continuation/GMRES method for fast computation of nonlinear receding horizon control. Automatica, 2004. 14 | - https://www.coronasha.co.jp/np/isbn/9784339033182/ 15 | - https://www.coronasha.co.jp/np/isbn/9784339032109/ 16 | 17 | The source code implementation is based on the following. 18 | - https://www.maizuru-ct.ac.jp/control/kawata/iscie2/vol56_no6_09_ohtsuka.html 19 | 20 | ## Examples 21 | Make sure that it is built with `--catkin-make-args tests` option. 22 | 23 | ### Semiactive damper 24 | 25 | ```bash 26 | $ rosrun nmpc_cgmres TestCgmresSolver --gtest_filter=*SemiactiveDamperProblem 27 | $ rosrun nmpc_cgmres plotCgmresData.py 28 | ``` 29 | ![TestSemiactiveDamperProblem](doc/images/TestSemiactiveDamperProblem.png) 30 | 31 | ### Cart-pole 32 | 33 | ```bash 34 | $ rosrun nmpc_cgmres TestCgmresSolver --gtest_filter=*CartPoleProblem 35 | $ rosrun nmpc_cgmres plotCgmresData.py 36 | ``` 37 | ![TestCartPoleProblem](doc/images/TestCartPoleProblem.png) 38 | -------------------------------------------------------------------------------- /nmpc_cgmres/doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Doxygen REQUIRED) 2 | 3 | if(DOXYGEN_FOUND) 4 | set(DOXYFILE_PATH ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) 5 | 6 | add_custom_target(nmpc_cgmres_doc ALL 7 | ${DOXYGEN_EXECUTABLE} ${DOXYFILE_PATH} 8 | DEPENDS ${DOXYFILE_PATH} 9 | COMMENT "Generating Doxygen documentation" 10 | ) 11 | endif() 12 | -------------------------------------------------------------------------------- /nmpc_cgmres/doc/images/TestCartPoleProblem.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isri-aist/NMPC/80bbe5777487f8c4ec44e0256c4e2a77a2be82ea/nmpc_cgmres/doc/images/TestCartPoleProblem.png -------------------------------------------------------------------------------- /nmpc_cgmres/doc/images/TestSemiactiveDamperProblem.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isri-aist/NMPC/80bbe5777487f8c4ec44e0256c4e2a77a2be82ea/nmpc_cgmres/doc/images/TestSemiactiveDamperProblem.png -------------------------------------------------------------------------------- /nmpc_cgmres/include/nmpc_cgmres/CgmresProblem.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace nmpc_cgmres 12 | { 13 | /** \brief C/GMRES problem. */ 14 | class CgmresProblem 15 | { 16 | public: 17 | /** \brief Type of function to return reference state. */ 18 | using RefFunc = std::function)>; 19 | 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | /** \brief Constructor. */ 24 | CgmresProblem() {} 25 | 26 | /** \brief Calculate the state equation. */ 27 | virtual void stateEquation(double t, 28 | const Eigen::Ref & x, 29 | const Eigen::Ref & u, 30 | Eigen::Ref dotx) = 0; 31 | 32 | /** \brief Calculate the costate equation. */ 33 | virtual void costateEquation(double t, 34 | const Eigen::Ref & lmd, 35 | const Eigen::Ref & xu, 36 | Eigen::Ref dotlmd) = 0; 37 | 38 | /** \brief Calculate \f$ \frac{\partial \phi}{\partial x} \f$. */ 39 | virtual void calcDphiDx(double t, 40 | const Eigen::Ref & x, 41 | Eigen::Ref DphiDx) = 0; 42 | 43 | /** \brief Calculate \f$ \frac{\partial h}{\partial u} \f$. */ 44 | virtual void calcDhDu(double t, 45 | const Eigen::Ref & x, 46 | const Eigen::Ref & u, 47 | const Eigen::Ref & lmd, 48 | Eigen::Ref DhDu) = 0; 49 | 50 | /** \brief Dump model parameters. */ 51 | virtual void dumpData(std::ofstream & ofs) 52 | { 53 | ofs << "\"state_eq_param\": [" << state_eq_param_.format(vecfmt_dump_) << "]," << std::endl; 54 | } 55 | 56 | public: 57 | int dim_x_; 58 | int dim_u_; 59 | int dim_c_; 60 | int dim_uc_; 61 | 62 | Eigen::VectorXd state_eq_param_; 63 | 64 | Eigen::VectorXd x_initial_; 65 | Eigen::VectorXd u_initial_; 66 | 67 | const Eigen::IOFormat vecfmt_dump_ = Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "", ""); 68 | }; 69 | } // namespace nmpc_cgmres 70 | -------------------------------------------------------------------------------- /nmpc_cgmres/include/nmpc_cgmres/CgmresSolver.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace nmpc_cgmres 13 | { 14 | /** \brief C/GMRES solver. 15 | 16 | See the following articles about the C/GMRES method: 17 | - T Ohtsuka. Continuation/GMRES method for fast computation of nonlinear receding horizon control. Automatica. 18 | 2004. 19 | - https://www.coronasha.co.jp/np/isbn/9784339033182/ 20 | - https://www.coronasha.co.jp/np/isbn/9784339032109/ 21 | */ 22 | class CgmresSolver 23 | { 24 | public: 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | 27 | /** \brief Constructor. */ 28 | CgmresSolver(std::shared_ptr problem, 29 | std::shared_ptr ode_solver, 30 | std::shared_ptr sim_ode_solver = nullptr) 31 | : problem_(problem), ode_solver_(ode_solver), sim_ode_solver_(sim_ode_solver) 32 | { 33 | if(!sim_ode_solver_) 34 | { 35 | sim_ode_solver_ = ode_solver_; 36 | } 37 | } 38 | 39 | /** \brief Setup. */ 40 | void setup(); 41 | 42 | /** \brief Run NMPC. */ 43 | void run(); 44 | 45 | /** \brief Calculate the control input. */ 46 | void calcControlInput(double t, 47 | const Eigen::Ref & x, 48 | const Eigen::Ref & next_x, 49 | Eigen::Ref u); 50 | 51 | /** \brief Calculate the \f$ \frac{\partial h}{\partial u} \f$ list in the horizon. */ 52 | void calcDhDuList(double t, 53 | const Eigen::Ref & x, 54 | const Eigen::Ref & u_list, 55 | Eigen::Ref DhDu_list); 56 | 57 | /** \brief Function to return \f$ A * v \f$ where \f$ v \f$ is given. */ 58 | Eigen::VectorXd eqAmulFunc(const Eigen::Ref & vec); 59 | 60 | public: 61 | std::shared_ptr problem_; 62 | std::shared_ptr ode_solver_; 63 | std::shared_ptr sim_ode_solver_; 64 | 65 | //////// parameters of C/GMRES method //////// 66 | double sim_duration_ = 10; 67 | 68 | double steady_horizon_duration_ = 1.0; 69 | int horizon_divide_num_ = 25; 70 | double horizon_increase_ratio_ = 0.5; 71 | 72 | double dt_ = 0.001; 73 | 74 | double eq_zeta_ = 1000.0; 75 | int k_max_ = 5; 76 | 77 | double finite_diff_delta_ = 0.002; 78 | 79 | int dump_step_ = 5; 80 | 81 | //////// variables that are set during processing //////// 82 | Eigen::VectorXd x_; 83 | Eigen::VectorXd u_; 84 | 85 | double t_with_delta_; 86 | Eigen::VectorXd x_with_delta_; 87 | 88 | Eigen::MatrixXd x_list_; 89 | Eigen::MatrixXd lmd_list_; 90 | 91 | Eigen::MatrixXd u_list_; 92 | Eigen::MatrixXd u_list_Amul_func_; 93 | 94 | Eigen::MatrixXd DhDu_list_; 95 | Eigen::MatrixXd DhDu_list_with_delta_; 96 | Eigen::MatrixXd DhDu_list_Amul_func_; 97 | // Eigen::Map does not have a default constructor 98 | std::shared_ptr> DhDu_vec_; 99 | std::shared_ptr> DhDu_vec_with_delta_; 100 | std::shared_ptr> DhDu_vec_Amul_func_; 101 | 102 | Eigen::VectorXd delta_u_vec_; 103 | 104 | //////// variables for utility //////// 105 | std::ofstream ofs_x_; 106 | std::ofstream ofs_u_; 107 | std::ofstream ofs_err_; 108 | const Eigen::IOFormat vecfmt_dump_ = Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "", ""); 109 | }; 110 | } // namespace nmpc_cgmres 111 | -------------------------------------------------------------------------------- /nmpc_cgmres/include/nmpc_cgmres/Gmres.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | namespace nmpc_cgmres 14 | { 15 | /** \brief GMRES method to solve a linear equation. 16 | 17 | See the following articles about the GMRES method: 18 | - C T Kelley. Iterative methods for linear and nonlinear equations. 1995. 19 | - https://www.coronasha.co.jp/np/isbn/9784339032109/ 20 | */ 21 | class Gmres 22 | { 23 | public: 24 | /** \brief Type of function that returns x multiplied by A. */ 25 | using AmulFunc = std::function &)>; 26 | 27 | public: 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | 30 | /** \brief Constructor. */ 31 | Gmres() {} 32 | 33 | /** \brief Solve. 34 | \param A the matrix of linear equation 35 | \param b the vector of linear equation 36 | \param x the initial guess of solution, which is overwritten by the final solution 37 | \param k_max the maximum number of GMRES iteration 38 | \param eps the required solution tolerance 39 | 40 | Solve the linear equation: \f$ A x = b \f$. 41 | */ 42 | inline void solve(const Eigen::Ref & A, 43 | const Eigen::Ref & b, 44 | Eigen::Ref x, 45 | int k_max = 100, 46 | double eps = 1e-10) 47 | { 48 | assert(A.rows() == b.size()); 49 | AmulFunc Amul_func = [&](const Eigen::Ref & vec) { return A * vec; }; 50 | solve(Amul_func, b, x, k_max, eps); 51 | } 52 | 53 | /** \brief Solve. 54 | \param Amul_func the function to return \f$ A * v \f$ where \f$ v \f$ is given 55 | \param b the vector of linear equation 56 | \param x the initial guess of solution, which is overwritten by the final solution 57 | \param k_max the maximum number of GMRES iteration 58 | \param eps the required solution tolerance 59 | 60 | Solve the linear equation: \f$ A x = b \f$. 61 | 62 | Refer to the Algorithm 3.5.1. in [1] for the case that make_triangular_ is true. 63 | Refer to the Algorithm 3.4.2. in [1] for the case that make_triangular_ is false. 64 | [1] Kelley, Carl T. Iterative methods for linear and nonlinear equations. Society for Industrial and Applied 65 | Mathematics, 1995. 66 | */ 67 | inline void solve(const AmulFunc & Amul_func, 68 | const Eigen::Ref & b, 69 | Eigen::Ref x, 70 | int k_max = 100, 71 | double eps = 1e-10) 72 | { 73 | k_max = std::min(k_max, static_cast(x.size())); 74 | 75 | err_list_.clear(); 76 | basis_.clear(); 77 | 78 | // 1. 79 | Eigen::VectorXd r = b - Amul_func(x); 80 | basis_.push_back(r.normalized()); 81 | double rho = r.norm(); 82 | int k = 0; 83 | g_.setZero(k_max + 1); 84 | g_(0) = rho; 85 | 86 | double b_norm = b.norm(); 87 | H_.setZero(k_max + 1, k_max); 88 | err_list_.push_back(rho); 89 | 90 | // 2. 91 | Eigen::VectorXd y_k; 92 | std::vector c_list; 93 | std::vector s_list; 94 | while(rho > eps * b_norm && k < k_max) 95 | { 96 | // (a). 97 | // note that k is 1 in the first iteration 98 | k++; 99 | 100 | // (b). 101 | // new_basis corresponds to $v_{k+1}$ in the paper 102 | Eigen::VectorXd Avk = Amul_func(basis_.back()); 103 | Eigen::VectorXd new_basis = Avk; 104 | for(int j = 0; j < k; j++) 105 | { 106 | // i. 107 | H_(j, k - 1) = new_basis.dot(basis_[j]); 108 | // ii. 109 | new_basis = new_basis - H_(j, k - 1) * basis_[j]; 110 | } 111 | 112 | // (c). 113 | double new_basis_norm = new_basis.norm(); 114 | H_(k, k - 1) = new_basis_norm; 115 | 116 | // (d). 117 | if(apply_reorth_) 118 | { 119 | double Avk_norm = Avk.norm(); 120 | if(Avk_norm + 1e-3 * new_basis_norm == Avk_norm) 121 | { 122 | // std::cout << "apply reorthogonalization. (loop: " << k << ")" << std::endl; 123 | for(int j = 0; j < k; j++) 124 | { 125 | double h_tmp = new_basis.dot(basis_[j]); 126 | H_(j, k - 1) += h_tmp; 127 | new_basis -= h_tmp * basis_[j]; 128 | } 129 | } 130 | } 131 | 132 | // (e). 133 | basis_.push_back(new_basis.normalized()); 134 | 135 | if(make_triangular_) 136 | { 137 | // (f). 138 | // i. 139 | for(int i = 0; i < k - 1; i++) 140 | { 141 | double h0 = H_(i, k - 1); 142 | double h1 = H_(i + 1, k - 1); 143 | double c = c_list[i]; 144 | double s = s_list[i]; 145 | H_(i, k - 1) = c * h0 - s * h1; 146 | H_(i + 1, k - 1) = s * h0 + c * h1; 147 | } 148 | 149 | // ii. 150 | double nu = std::sqrt(std::pow(H_(k - 1, k - 1), 2) + std::pow(H_(k, k - 1), 2)); 151 | 152 | // iii. 153 | double c_k = H_(k - 1, k - 1) / nu; 154 | double s_k = -H_(k, k - 1) / nu; 155 | c_list.push_back(c_k); 156 | s_list.push_back(s_k); 157 | H_(k - 1, k - 1) = c_k * H_(k - 1, k - 1) - s_k * H_(k, k - 1); 158 | H_(k, k - 1) = 0; 159 | 160 | // iv. 161 | double g0 = g_(k - 1); 162 | double g1 = g_(k); 163 | g_(k - 1) = c_k * g0 - s_k * g1; 164 | g_(k) = s_k * g0 + c_k * g1; 165 | 166 | // (g). 167 | rho = std::abs(g_(k)); 168 | } 169 | else 170 | { 171 | // (f). 172 | y_k = H_.topLeftCorner(k + 1, k).householderQr().solve(g_.head(k + 1)); 173 | 174 | // (g). 175 | rho = (g_.head(k + 1) - H_.topLeftCorner(k + 1, k) * y_k).norm(); 176 | } 177 | 178 | err_list_.push_back(rho); 179 | } 180 | 181 | if(make_triangular_) 182 | { 183 | // 3. 184 | y_k = H_.topLeftCorner(k, k).triangularView().solve(g_.head(k)); 185 | } 186 | 187 | // 4. 188 | for(int i = 0; i < k; i++) 189 | { 190 | x += y_k(i) * basis_[i]; 191 | } 192 | } 193 | 194 | public: 195 | bool make_triangular_ = true; 196 | bool apply_reorth_ = true; 197 | 198 | Eigen::MatrixXd H_; 199 | Eigen::VectorXd g_; 200 | 201 | std::vector err_list_; 202 | 203 | std::vector basis_; 204 | }; 205 | } // namespace nmpc_cgmres 206 | -------------------------------------------------------------------------------- /nmpc_cgmres/include/nmpc_cgmres/OdeSolver.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace nmpc_cgmres 12 | { 13 | /** \brief Virtual class to solve Ordinaly Diferential Equation. */ 14 | class OdeSolver 15 | { 16 | public: 17 | using StateEquation = std::function &, 19 | const Eigen::Ref &, 20 | Eigen::Ref)>; 21 | 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | 25 | virtual void solve(const StateEquation & state_eq, 26 | double t, 27 | const Eigen::Ref & x, 28 | const Eigen::Ref & u, 29 | double dt, 30 | Eigen::Ref ret) = 0; 31 | }; 32 | 33 | /** \brief Class to solve Ordinaly Diferential Equation by Euler method. */ 34 | class EulerOdeSolver : public OdeSolver 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | 39 | virtual void solve(const StateEquation & state_eq, 40 | double t, 41 | const Eigen::Ref & x, 42 | const Eigen::Ref & u, 43 | double dt, 44 | Eigen::Ref ret) override 45 | { 46 | Eigen::VectorXd dotx(x.size()); 47 | state_eq(t, x, u, dotx); 48 | ret = x + dt * dotx; 49 | } 50 | }; 51 | 52 | /** \brief Class to solve Ordinaly Diferential Equation by Runge-Kutta method. */ 53 | class RungeKuttaOdeSolver : public OdeSolver 54 | { 55 | public: 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 57 | 58 | virtual void solve(const StateEquation & state_eq, 59 | double t, 60 | const Eigen::Ref & x, 61 | const Eigen::Ref & u, 62 | double dt, 63 | Eigen::Ref ret) override 64 | { 65 | double dt_half = dt / 2; 66 | Eigen::VectorXd k1(x.size()), k2(x.size()), k3(x.size()), k4(x.size()); 67 | state_eq(t, x, u, k1); 68 | state_eq(t + dt_half, x + dt_half * k1, u, k2); 69 | state_eq(t + dt_half, x + dt_half * k2, u, k3); 70 | state_eq(t + dt, x + dt * k3, u, k4); 71 | ret = x + (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4); 72 | } 73 | }; 74 | } // namespace nmpc_cgmres 75 | -------------------------------------------------------------------------------- /nmpc_cgmres/package.xml: -------------------------------------------------------------------------------- 1 | 2 | nmpc_cgmres 3 | 0.1.0 4 | 5 | Non-linear model predictive control (NMPC) with continuation/GMRES method (C/GMRES) 6 | 7 | Masaki Murooka 8 | BSD 9 | 10 | http://ros.org/wiki/nmpc_cgmres 11 | Masaki Murooka 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | 17 | eigen 18 | 19 | ament_cmake_gtest 20 | 21 | doxygen 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /nmpc_cgmres/scripts/plotCgmresData.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | class CgmresDataPlotter(object): 8 | def __init__(self): 9 | self.loadData() 10 | self.setupPlot() 11 | 12 | def loadData(self, file_dir="/tmp"): 13 | print("load temporary files in " + file_dir + " directory") 14 | 15 | # set traj 16 | self.x_traj = np.genfromtxt(file_dir+"/cgmres_x.dat", delimiter=",") 17 | self.u_traj = np.genfromtxt(file_dir+"/cgmres_u.dat", delimiter=",") 18 | self.err_traj = np.genfromtxt(file_dir+"/cgmres_err.dat", delimiter=",") 19 | 20 | def setupPlot(self): 21 | self.fig = plt.figure(figsize=plt.figaspect(1)) 22 | self.fig.canvas.set_window_title("CgmresDataPlotter") 23 | 24 | def plot(self): 25 | ax_num = np.max([self.x_traj.shape[1], self.u_traj.shape[1]]) - 1 26 | first_ax = None 27 | 28 | # x 29 | for i in range(self.x_traj.shape[1] - 1): 30 | ax = self.fig.add_subplot(3, ax_num, i + 1, 31 | xlabel="time", ylabel="x[{}]".format(i+1), 32 | **{} if first_ax is None else {"sharex": first_ax}) 33 | ax.plot(self.x_traj[:,0], self.x_traj[:,i+1]) 34 | if first_ax is None: 35 | first_ax = ax 36 | 37 | # u 38 | for i in range(self.u_traj.shape[1] - 1): 39 | ax = self.fig.add_subplot(3, ax_num, ax_num + i + 1, sharex=first_ax, 40 | xlabel="time", ylabel="u[{}]".format(i+1)) 41 | ax.plot(self.u_traj[:,0], self.u_traj[:,i+1]) 42 | 43 | # err 44 | for i in range(self.err_traj.shape[1] - 1): 45 | ax = self.fig.add_subplot(3, ax_num, 2 * ax_num + i + 1, sharex=first_ax, 46 | xlabel="time", ylabel="err[{}]".format(i+1)) 47 | ax.plot(self.err_traj[:,0], self.err_traj[:,i+1]) 48 | 49 | plt.pause(0.01) 50 | 51 | 52 | if __name__ == "__main__": 53 | plotter = CgmresDataPlotter() 54 | plotter.plot() 55 | plt.show() 56 | -------------------------------------------------------------------------------- /nmpc_cgmres/src/CgmresSolver.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #include 4 | #include 5 | 6 | using namespace nmpc_cgmres; 7 | 8 | void CgmresSolver::setup() 9 | { 10 | double t_initial = 0; 11 | x_ = problem_->x_initial_; 12 | u_ = problem_->u_initial_; 13 | Eigen::VectorXd lmd_initial(problem_->dim_x_); 14 | Eigen::VectorXd DhDu(problem_->dim_uc_); 15 | 16 | // calc lmd_initial 17 | lmd_initial.resize(problem_->dim_x_); 18 | problem_->calcDphiDx(t_initial, x_, lmd_initial); 19 | 20 | // calc u by GMRES method 21 | Gmres gmres; 22 | Eigen::VectorXd DhDu_finite_diff(problem_->dim_uc_); 23 | Gmres::AmulFunc Amul_func = [&](const Eigen::Ref & vec) 24 | { 25 | problem_->calcDhDu(t_initial, x_, u_ + finite_diff_delta_ * vec, lmd_initial, DhDu_finite_diff); 26 | return (DhDu_finite_diff - DhDu) / finite_diff_delta_; 27 | }; 28 | 29 | Eigen::VectorXd delta_u = Eigen::VectorXd::Zero(problem_->dim_uc_); 30 | double DhDu_tol = 1e-6; 31 | for(int i = 0; i < 100; i++) 32 | { 33 | problem_->calcDhDu(t_initial, x_, u_, lmd_initial, DhDu); 34 | if(DhDu.norm() <= DhDu_tol) 35 | { 36 | break; 37 | } 38 | 39 | gmres.solve(Amul_func, -DhDu, delta_u, problem_->dim_uc_, 1e-10); 40 | u_ += delta_u; 41 | } 42 | if(DhDu.norm() > DhDu_tol) 43 | { 44 | std::cout << "failed to converge u in setup." << std::endl; 45 | } 46 | 47 | // setup variables 48 | x_with_delta_.resize(problem_->dim_x_); 49 | // since Eigen's matrix is column major order, it is better for rows to correspond 50 | // to time series in order to efficiently reshape a matrix into a vector. 51 | x_list_.resize(problem_->dim_x_, horizon_divide_num_ + 1); 52 | lmd_list_.resize(problem_->dim_x_, horizon_divide_num_ + 1); 53 | u_list_.resize(problem_->dim_uc_, horizon_divide_num_); 54 | u_list_Amul_func_.resize(problem_->dim_uc_, horizon_divide_num_); 55 | DhDu_list_.resize(problem_->dim_uc_, horizon_divide_num_); 56 | for(int i = 0; i < horizon_divide_num_; i++) 57 | { 58 | u_list_.col(i) = u_; 59 | DhDu_list_.col(i) = DhDu; 60 | } 61 | DhDu_list_with_delta_.resize(problem_->dim_uc_, horizon_divide_num_); 62 | DhDu_list_Amul_func_.resize(problem_->dim_uc_, horizon_divide_num_); 63 | delta_u_vec_.setZero(horizon_divide_num_ * problem_->dim_uc_); 64 | } 65 | 66 | void CgmresSolver::run() 67 | { 68 | ofs_x_.open("/tmp/cgmres_x.dat"); 69 | ofs_u_.open("/tmp/cgmres_u.dat"); 70 | ofs_err_.open("/tmp/cgmres_err.dat"); 71 | { 72 | std::ofstream ofs_param("/tmp/cgmres_param.dat"); 73 | ofs_param << "{" << std::endl; 74 | ofs_param << "\"log_dt\": " << dt_ * dump_step_ << "," << std::endl; 75 | problem_->dumpData(ofs_param); 76 | ofs_param << "}" << std::endl; 77 | } 78 | 79 | setup(); 80 | 81 | Eigen::VectorXd next_x(problem_->dim_x_); 82 | 83 | int i = 0; 84 | for(double t = 0; t <= sim_duration_; t += dt_) 85 | { 86 | // 1. simulate 87 | sim_ode_solver_->solve(std::bind(&CgmresProblem::stateEquation, problem_.get(), std::placeholders::_1, 88 | std::placeholders::_2, std::placeholders::_3, std::placeholders::_4), 89 | t, x_, u_, dt_, next_x); 90 | 91 | // 2. calculate the control input 92 | calcControlInput(t, x_, next_x, u_); 93 | 94 | x_ = next_x; 95 | 96 | // 3. dump data 97 | if(i % dump_step_ == 0) 98 | { 99 | ofs_x_ << t << ", " << x_.format(vecfmt_dump_) << std::endl; 100 | ofs_u_ << t << ", " << u_.format(vecfmt_dump_) << std::endl; 101 | ofs_err_ << t << ", " << DhDu_vec_->norm() << std::endl; 102 | } 103 | i++; 104 | } 105 | 106 | ofs_x_.close(); 107 | ofs_u_.close(); 108 | ofs_err_.close(); 109 | } 110 | 111 | void CgmresSolver::calcControlInput(double t, 112 | const Eigen::Ref & x, 113 | const Eigen::Ref & next_x, 114 | Eigen::Ref u) 115 | { 116 | // 1.1 calculate DhDu_list_ 117 | calcDhDuList(t, x, u_list_, DhDu_list_); 118 | 119 | // 1.2 calculate DhDu_list_with_delta_ 120 | t_with_delta_ = t + finite_diff_delta_; 121 | x_with_delta_ = (1 - finite_diff_delta_ / dt_) * x + (finite_diff_delta_ / dt_) * next_x; 122 | calcDhDuList(t_with_delta_, x_with_delta_, u_list_, DhDu_list_with_delta_); 123 | 124 | // 2.1 calculate a vector of the linear equation 125 | // assume that the matrix is column major order, which is the default setting of Eigen 126 | DhDu_vec_ = std::make_shared>(DhDu_list_.data(), DhDu_list_.size()); 127 | DhDu_vec_with_delta_ = 128 | std::make_shared>(DhDu_list_with_delta_.data(), DhDu_list_with_delta_.size()); 129 | const Eigen::VectorXd & eq_b = 130 | ((1 - eq_zeta_ * finite_diff_delta_) * (*DhDu_vec_) - (*DhDu_vec_with_delta_)) / finite_diff_delta_; 131 | 132 | // 2.2 solve the linear equation by GMRES method 133 | Gmres gmres; 134 | gmres.solve(std::bind(&CgmresSolver::eqAmulFunc, this, std::placeholders::_1), eq_b, delta_u_vec_, k_max_, 1e-10); 135 | 136 | // 2.3 update u_list_ from delta_u_vec_ 137 | for(int i = 0; i < horizon_divide_num_; i++) 138 | { 139 | u_list_.col(i) += dt_ * delta_u_vec_.segment(i * problem_->dim_uc_, problem_->dim_uc_); 140 | } 141 | 142 | // 3. set u_ 143 | u = u_list_.col(0); 144 | } 145 | 146 | void CgmresSolver::calcDhDuList(double t, 147 | const Eigen::Ref & x, 148 | const Eigen::Ref & u_list, 149 | Eigen::Ref DhDu_list) 150 | { 151 | double horizon_duration = steady_horizon_duration_ * (1.0 - std::exp(-horizon_increase_ratio_ * t)); 152 | double horizon_divide_step = horizon_duration / horizon_divide_num_; 153 | 154 | // 1.1 calculate x_list_[0] 155 | x_list_.col(0) = x; 156 | 157 | double tau = t; 158 | for(int i = 0; i < horizon_divide_num_; i++) 159 | { 160 | // 1.2 calculate x_list_[1, ..., horizon_divide_num_] 161 | ode_solver_->solve(std::bind(&CgmresProblem::stateEquation, problem_.get(), std::placeholders::_1, 162 | std::placeholders::_2, std::placeholders::_3, std::placeholders::_4), 163 | tau, x_list_.col(i), u_list.col(i), horizon_divide_step, x_list_.col(i + 1)); 164 | tau += horizon_divide_step; 165 | } 166 | 167 | // 2.1 calculate lmd_list_[horizon_divide_num_] 168 | problem_->calcDphiDx(tau, x_list_.col(horizon_divide_num_), lmd_list_.col(horizon_divide_num_)); 169 | 170 | Eigen::VectorXd xu(problem_->dim_x_ + problem_->dim_uc_); 171 | for(int i = horizon_divide_num_ - 1; i >= 0; i--) 172 | { 173 | // 2.2 calculate lmd_list_[horizon_divide_num_-1, ..., 0] 174 | xu.head(problem_->dim_x_) = x_list_.col(i); 175 | xu.tail(problem_->dim_uc_) = u_list.col(i); 176 | ode_solver_->solve(std::bind(&CgmresProblem::costateEquation, problem_.get(), std::placeholders::_1, 177 | std::placeholders::_2, std::placeholders::_3, std::placeholders::_4), 178 | tau, lmd_list_.col(i + 1), xu, -horizon_divide_step, lmd_list_.col(i)); 179 | tau -= horizon_divide_step; 180 | 181 | // 3. DhDu_list[horizon_divide_num_-1, ..., 0] 182 | problem_->calcDhDu(tau, x_list_.col(i), u_list.col(i), lmd_list_.col(i + 1), DhDu_list.col(i)); 183 | } 184 | } 185 | 186 | Eigen::VectorXd CgmresSolver::eqAmulFunc(const Eigen::Ref & vec) 187 | { 188 | // 1. calculate u_list_Amul_func_ 189 | for(int i = 0; i < horizon_divide_num_; i++) 190 | { 191 | u_list_Amul_func_.col(i) = 192 | u_list_.col(i) + finite_diff_delta_ * vec.segment(i * problem_->dim_uc_, problem_->dim_uc_); 193 | } 194 | 195 | // 2. calculate DhDu_list_Amul_func_ 196 | calcDhDuList(t_with_delta_, x_with_delta_, u_list_Amul_func_, DhDu_list_Amul_func_); 197 | 198 | // 3. calculate the finite difference 199 | DhDu_vec_Amul_func_ = 200 | std::make_shared>(DhDu_list_Amul_func_.data(), DhDu_list_Amul_func_.size()); 201 | return ((*DhDu_vec_Amul_func_) - (*DhDu_vec_with_delta_)) / finite_diff_delta_; 202 | } 203 | -------------------------------------------------------------------------------- /nmpc_cgmres/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT NMPC_STANDALONE) 2 | find_package(rostest REQUIRED) 3 | endif() 4 | 5 | set(nmpc_cgmres_gtest_list 6 | TestGmres 7 | TestCgmresSolver 8 | ) 9 | 10 | if(NMPC_STANDALONE) 11 | find_package(GTest REQUIRED) 12 | include(GoogleTest) 13 | function(add_nmpc_cgmres_test NAME) 14 | add_executable(${NAME} src/${NAME}.cpp) 15 | target_link_libraries(${NAME} PUBLIC GTest::gtest nmpc_cgmres) 16 | gtest_discover_tests(${NAME}) 17 | endfunction() 18 | else() 19 | function(add_nmpc_cgmres_test NAME) 20 | catkin_add_gtest(${NAME} src/${NAME}.cpp) 21 | target_link_libraries(${NAME} nmpc_cgmres) 22 | endfunction() 23 | endif() 24 | 25 | foreach(NAME IN LISTS nmpc_cgmres_gtest_list) 26 | add_nmpc_cgmres_test(${NAME}) 27 | endforeach() 28 | -------------------------------------------------------------------------------- /nmpc_cgmres/tests/src/CartPoleProblem.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | /** \brief Problem of Cart-Pole. */ 11 | class CartPoleProblem : public nmpc_cgmres::CgmresProblem 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | 16 | /** \brief Constructor. */ 17 | CartPoleProblem(const RefFunc & ref_func = nullptr, bool with_input_bound = false) 18 | : ref_func_(ref_func), with_input_bound_(with_input_bound) 19 | { 20 | if(with_input_bound_) 21 | { 22 | dim_x_ = 4; 23 | dim_u_ = 2; 24 | dim_c_ = 1; 25 | dim_uc_ = dim_u_ + dim_c_; 26 | 27 | u_initial_.resize(dim_uc_); 28 | u_initial_ << 0, 1.0, 0.01; 29 | } 30 | else 31 | { 32 | dim_x_ = 4; 33 | dim_u_ = 1; 34 | dim_c_ = 0; 35 | dim_uc_ = dim_u_ + dim_c_; 36 | 37 | u_initial_.resize(dim_uc_); 38 | u_initial_ << 0; 39 | } 40 | 41 | x_initial_.resize(dim_x_); 42 | x_initial_ << 0, M_PI, 0, 0; 43 | 44 | // \f$ (m1, m2, l, f_max) \f$ 45 | state_eq_param_.resize(4); 46 | state_eq_param_ << 1.0, 1.0, 1.0, 100.0; 47 | 48 | // \f$ (q_1, q_2, q_3, q_4, r_1, r_2) \f$ 49 | obj_weight_.resize(6); 50 | obj_weight_ << 10, 100, 1, 10, 10, 0.01; 51 | 52 | // \f$ (s_{f1}, s_{f2}, s_{f3}, s_{f4}) \f$ 53 | terminal_obj_weight_.resize(4); 54 | terminal_obj_weight_ << 100, 300, 1, 10; 55 | 56 | if(ref_func_ == nullptr) 57 | { 58 | ref_func_ = [&](double, // t 59 | Eigen::Ref ref) { ref.setZero(); }; 60 | } 61 | ref_.resize(dim_x_); 62 | } 63 | 64 | /** \brief Calculate the state equation. */ 65 | virtual void stateEquation(double, // t 66 | const Eigen::Ref & xvec, 67 | const Eigen::Ref & uvec, 68 | Eigen::Ref dotxvec) override 69 | { 70 | assert(dotxvec.size() == dim_x_); 71 | 72 | const double & m1 = state_eq_param_(0); 73 | const double & m2 = state_eq_param_(1); 74 | const double & l = state_eq_param_(2); 75 | 76 | // const double & x = xvec(0); 77 | const double & theta = xvec(1); 78 | const double & dx = xvec(2); 79 | const double & dtheta = xvec(3); 80 | const double & f = uvec(0); 81 | 82 | double sin_theta = std::sin(theta); 83 | double cos_theta = std::cos(theta); 84 | double denom = m1 + m2 * std::pow(sin_theta, 2); 85 | dotxvec(0) = dx; 86 | dotxvec(1) = dtheta; 87 | dotxvec(2) = (f - m2 * l * std::pow(dtheta, 2) * sin_theta + m2 * g_ * sin_theta * cos_theta) / denom; 88 | dotxvec(3) = (f * cos_theta - m2 * l * std::pow(dtheta, 2) * sin_theta * cos_theta + g_ * (m1 + m2) * sin_theta) 89 | / (l * denom); 90 | } 91 | 92 | /** \brief Calculate the costate equation. */ 93 | virtual void costateEquation(double t, 94 | const Eigen::Ref & lmd, 95 | const Eigen::Ref & xuvec, 96 | Eigen::Ref dotlmd) override 97 | { 98 | assert(dotlmd.size() == dim_x_); 99 | 100 | const Eigen::Ref & xvec = xuvec.head(dim_x_); 101 | const Eigen::Ref & uvec = xuvec.tail(dim_uc_); 102 | 103 | const double & m1 = state_eq_param_(0); 104 | const double & m2 = state_eq_param_(1); 105 | const double & l = state_eq_param_(2); 106 | 107 | // const double & x = xvec(0); 108 | const double & theta = xvec(1); 109 | // const double & dx = xvec(2); 110 | const double & dtheta = xvec(3); 111 | const double & f = uvec(0); 112 | 113 | double sin_theta = std::sin(theta); 114 | double cos_theta = std::cos(theta); 115 | double dtheta2 = std::pow(dtheta, 2); 116 | double sin_theta2 = std::pow(sin_theta, 2); 117 | double cos_theta2 = std::pow(cos_theta, 2); 118 | double denom = m1 + m2 * sin_theta2; 119 | double denom_square = std::pow(denom, 2); 120 | 121 | ref_func_(t, ref_); 122 | 123 | dotlmd(0) = -(obj_weight_(0) * (xvec(0) - ref_(0))); 124 | dotlmd(1) = -(obj_weight_(1) * (xvec(1) - ref_(1)) 125 | + (lmd(2) / denom_square) 126 | * (((-m2 * l * dtheta2 * cos_theta + m2 * g_ * (cos_theta2 - sin_theta2)) * denom) 127 | - ((f - m2 * l * dtheta2 * sin_theta + m2 * g_ * sin_theta * cos_theta) 128 | * (2 * m2 * sin_theta * cos_theta))) 129 | + (lmd(3) / (std::pow(l, 2) * denom_square)) 130 | * (((-f * sin_theta - m2 * l * dtheta2 * (cos_theta2 - sin_theta2) + g_ * (m1 + m2) * cos_theta) 131 | * l * denom) 132 | - ((f * cos_theta - m2 * l * dtheta2 * sin_theta * cos_theta + g_ * (m1 + m2) * sin_theta) 133 | * (2 * l * m2 * sin_theta * cos_theta)))); 134 | dotlmd(2) = -(obj_weight_(2) * (xvec(2) - ref_(2)) + lmd(0)); 135 | dotlmd(3) = -(obj_weight_(3) * (xvec(3) - ref_(3)) + lmd(1) + lmd(2) * (-2 * m2 * l * dtheta * sin_theta) / denom 136 | + lmd(3) * (-2 * m2 * l * dtheta * sin_theta * cos_theta) / (l * denom)); 137 | } 138 | 139 | /** \brief Calculate \f$ \frac{\partial \phi}{\partial x} \f$. */ 140 | virtual void calcDphiDx(double t, 141 | const Eigen::Ref & xvec, 142 | Eigen::Ref DphiDx) override 143 | { 144 | assert(DphiDx.size() == dim_x_); 145 | 146 | ref_func_(t, ref_); 147 | 148 | for(int i = 0; i < dim_x_; i++) 149 | { 150 | DphiDx(i) = terminal_obj_weight_(i) * (xvec(i) - ref_(i)); 151 | } 152 | } 153 | 154 | /** \brief Calculate \f$ \frac{\partial h}{\partial u} \f$. */ 155 | virtual void calcDhDu(double, // t 156 | const Eigen::Ref & xvec, 157 | const Eigen::Ref & uvec, 158 | const Eigen::Ref & lmd, 159 | Eigen::Ref DhDu) override 160 | { 161 | const double & m1 = state_eq_param_(0); 162 | const double & m2 = state_eq_param_(1); 163 | const double & l = state_eq_param_(2); 164 | 165 | // const double & x = xvec(0); 166 | const double & theta = xvec(1); 167 | const double & f = uvec(0); 168 | const double & r1 = obj_weight_(4); 169 | 170 | double sin_theta = std::sin(theta); 171 | double cos_theta = std::cos(theta); 172 | double denom = m1 + m2 * std::pow(sin_theta, 2); 173 | 174 | assert(DhDu.size() == dim_uc_); 175 | DhDu(0) = (r1 * f + lmd(2) * (1.0 / denom) + lmd(3) * (cos_theta / (l * denom))); 176 | 177 | if(with_input_bound_) 178 | { 179 | const double & f_max = state_eq_param_(3); 180 | const double & r2 = obj_weight_(5); 181 | 182 | const double & f_dummy = uvec(1); 183 | const double & mu = uvec(2); 184 | 185 | DhDu(0) += 2 * mu * f; 186 | DhDu(1) = -r2 + 2 * mu * f_dummy; 187 | DhDu(2) = std::pow(f, 2) + std::pow(f_dummy, 2) - std::pow(f_max, 2); 188 | } 189 | } 190 | 191 | public: 192 | // \f$ (q_1, q_2, q_3, q_4, r_1, r_2) \f$ 193 | Eigen::VectorXd obj_weight_; 194 | 195 | // \f$ (s_{f1}, s_{f2}, s_{f3}, s_{f4}) \f$ 196 | Eigen::VectorXd terminal_obj_weight_; 197 | 198 | // reference 199 | std::function)> ref_func_; 200 | Eigen::VectorXd ref_; 201 | 202 | double g_ = 9.80665; 203 | 204 | bool with_input_bound_; 205 | }; 206 | -------------------------------------------------------------------------------- /nmpc_cgmres/tests/src/SemiactiveDamperProblem.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | /** \brief Problem of semiactive damper. */ 10 | class SemiactiveDamperProblem : public nmpc_cgmres::CgmresProblem 11 | { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | /** \brief Constructor. */ 16 | SemiactiveDamperProblem() 17 | { 18 | dim_x_ = 2; 19 | dim_u_ = 2; 20 | dim_c_ = 1; 21 | dim_uc_ = dim_u_ + dim_c_; 22 | 23 | x_initial_.resize(dim_x_); 24 | x_initial_ << 2, 0; 25 | u_initial_.resize(dim_uc_); 26 | u_initial_ << 0.01, 0.9, 0.03; 27 | 28 | // \f$ (a, b, u_max) \f$ 29 | state_eq_param_.resize(3); 30 | state_eq_param_ << -1, -1, 1; 31 | 32 | obj_weight_.resize(4); 33 | obj_weight_ << 1, 10, 1, 1e-1; 34 | 35 | terminal_obj_weight_.resize(2); 36 | terminal_obj_weight_ << 1, 10; 37 | } 38 | 39 | /** \brief Calculate the state equation. */ 40 | virtual void stateEquation(double, // t 41 | const Eigen::Ref & x, 42 | const Eigen::Ref & u, 43 | Eigen::Ref dotx) override 44 | { 45 | assert(dotx.size() == dim_x_); 46 | dotx(0) = x(1); 47 | dotx(1) = state_eq_param_(0) * x(0) + state_eq_param_(1) * x(1) * u(0); 48 | } 49 | 50 | /** \brief Calculate the costate equation. */ 51 | virtual void costateEquation(double, // t 52 | const Eigen::Ref & lmd, 53 | const Eigen::Ref & xu, 54 | Eigen::Ref dotlmd) override 55 | { 56 | double a = state_eq_param_(0); 57 | double b = state_eq_param_(1); 58 | double q1 = obj_weight_(0); 59 | double q2 = obj_weight_(1); 60 | 61 | const Eigen::Ref & x = xu.head(dim_x_); 62 | const Eigen::Ref & u = xu.tail(dim_u_); 63 | 64 | assert(dotlmd.size() == dim_x_); 65 | dotlmd(0) = -a * lmd(1) - q1 * x(0); 66 | dotlmd(1) = -b * lmd(1) * u(0) - q2 * x(1) - lmd(0); 67 | } 68 | 69 | /** \brief Calculate \f$ \frac{\partial \phi}{\partial x} \f$. */ 70 | virtual void calcDphiDx(double, // t 71 | const Eigen::Ref & x, 72 | Eigen::Ref DphiDx) override 73 | { 74 | double sf1 = terminal_obj_weight_(0); 75 | double sf2 = terminal_obj_weight_(1); 76 | 77 | assert(DphiDx.size() == dim_x_); 78 | DphiDx(0) = sf1 * x(0); 79 | DphiDx(1) = sf2 * x(1); 80 | } 81 | 82 | /** \brief Calculate \f$ \frac{\partial h}{\partial u} \f$. */ 83 | virtual void calcDhDu(double, // t 84 | const Eigen::Ref & x, 85 | const Eigen::Ref & u, 86 | const Eigen::Ref & lmd, 87 | Eigen::Ref DhDu) override 88 | { 89 | double b = state_eq_param_(1); 90 | double u_max = state_eq_param_(2); 91 | double r1 = obj_weight_(2); 92 | double r2 = obj_weight_(3); 93 | double mu = u(2); 94 | 95 | // note that u includes the Lagrange multiplier of the equality constraints 96 | // i.e. u = (u1, u2, mu) 97 | assert(DhDu.size() == dim_uc_); 98 | DhDu(0) = r1 * u(0) + b * lmd(1) * x(1) + mu * (2 * u(0) - u_max); 99 | DhDu(1) = -r2 + 2 * mu * u(1); 100 | DhDu(2) = std::pow((u(0) - u_max / 2.0), 2) + u(1) * u(1) - u_max * u_max / 4.0; 101 | } 102 | 103 | public: 104 | // \f$ (q_1, q_2, r_1, r_2) \f$ 105 | Eigen::VectorXd obj_weight_; 106 | 107 | // \f$ (s_{f1}, s_{f2}) \f$ 108 | Eigen::VectorXd terminal_obj_weight_; 109 | }; 110 | -------------------------------------------------------------------------------- /nmpc_cgmres/tests/src/TestCgmresSolver.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "CartPoleProblem.h" 8 | #include "SemiactiveDamperProblem.h" 9 | 10 | void testCgmresSolver(const std::shared_ptr & problem, double x_thre) 11 | { 12 | auto ode_solver = std::make_shared(); 13 | auto sim_ode_solver = std::make_shared(); 14 | auto solver = std::make_shared(problem, ode_solver, sim_ode_solver); 15 | solver->sim_duration_ = 20.0; 16 | solver->run(); 17 | EXPECT_LT(solver->x_.norm(), x_thre); 18 | } 19 | 20 | TEST(TestCgmresSolver, SemiactiveDamperProblem) 21 | { 22 | testCgmresSolver(std::make_shared(), 0.1); 23 | } 24 | 25 | TEST(TestCgmresSolver, CartPoleProblem) 26 | { 27 | testCgmresSolver(std::make_shared(nullptr, true), 0.1); 28 | } 29 | 30 | int main(int argc, char ** argv) 31 | { 32 | testing::InitGoogleTest(&argc, argv); 33 | return RUN_ALL_TESTS(); 34 | } 35 | -------------------------------------------------------------------------------- /nmpc_cgmres/tests/src/TestGmres.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | 9 | class LinearSolver 10 | { 11 | public: 12 | virtual void solve(const Eigen::MatrixXd & A, 13 | const Eigen::VectorXd & b, 14 | Eigen::Ref x, 15 | double & tm, 16 | double & err) = 0; 17 | }; 18 | 19 | class GmresLinearSolver : public LinearSolver 20 | { 21 | public: 22 | virtual void solve(const Eigen::MatrixXd & A, 23 | const Eigen::VectorXd & b, 24 | Eigen::Ref x, 25 | double &, // tm 26 | double & // err 27 | ) 28 | { 29 | gmres_ = std::make_shared(); 30 | gmres_->make_triangular_ = make_triangular_; 31 | gmres_->apply_reorth_ = apply_reorth_; 32 | gmres_->solve(static_cast &>(A), b, x, k_max_); 33 | } 34 | 35 | std::shared_ptr gmres_; 36 | int k_max_ = 1000; 37 | bool make_triangular_ = true; 38 | bool apply_reorth_ = true; 39 | }; 40 | 41 | class FullPivLuLinearSolver : public LinearSolver 42 | { 43 | public: 44 | virtual void solve(const Eigen::MatrixXd & A, 45 | const Eigen::VectorXd & b, 46 | Eigen::Ref x, 47 | double &, // tm 48 | double & // err 49 | ) 50 | { 51 | x = A.fullPivLu().solve(b); 52 | } 53 | }; 54 | 55 | class HouseholderQrLinearSolver : public LinearSolver 56 | { 57 | public: 58 | virtual void solve(const Eigen::MatrixXd & A, 59 | const Eigen::VectorXd & b, 60 | Eigen::Ref x, 61 | double &, // tm 62 | double & // err 63 | ) 64 | { 65 | x = A.householderQr().solve(b); 66 | } 67 | }; 68 | 69 | double eval(std::shared_ptr solver, 70 | const std::vector & A_list, 71 | const std::vector & b_list) 72 | { 73 | size_t trial_num = A_list.size(); 74 | double tm = 0; 75 | double err = 0; 76 | 77 | for(size_t i = 0; i < trial_num; i++) 78 | { 79 | const Eigen::MatrixXd & A = A_list[i]; 80 | const Eigen::VectorXd & b = b_list[i]; 81 | Eigen::VectorXd x = Eigen::VectorXd::Zero(b.size()); 82 | 83 | clock_t begin_clock = clock(); 84 | solver->solve(A, b, x, tm, err); 85 | clock_t end_clock = clock(); 86 | 87 | tm += static_cast(end_clock - begin_clock) / CLOCKS_PER_SEC; 88 | err += (A * x - b).norm(); 89 | } 90 | 91 | tm /= static_cast(trial_num); 92 | err /= static_cast(trial_num); 93 | std::cout << "ave time: " << 1e3 * tm << " [msec], ave err: " << err << std::endl; 94 | 95 | return err; 96 | } 97 | 98 | TEST(TestGmres, TestCase1) 99 | { 100 | std::vector eq_size_list = {10, 50, 100, 500}; 101 | int trial_num = 10; 102 | 103 | for(int eq_size : eq_size_list) 104 | { 105 | // setup a problem list 106 | std::vector A_list; 107 | std::vector b_list; 108 | for(int i = 0; i < trial_num; i++) 109 | { 110 | A_list.push_back(Eigen::MatrixXd::Random(eq_size, eq_size)); 111 | b_list.push_back(Eigen::VectorXd::Random(eq_size)); 112 | } 113 | 114 | std::cout << "======== eq_size: " << eq_size << " ========" << std::endl; 115 | 116 | // evaluate each method 117 | { 118 | std::cout << "== Gmres ==" << std::endl; 119 | auto solver = std::make_shared(); 120 | EXPECT_LT(eval(solver, A_list, b_list), 1e-10); 121 | 122 | if(eq_size <= 100) 123 | { 124 | std::cout << "== Gmres (no triangular) ==" << std::endl; 125 | solver->k_max_ = 1000; 126 | solver->make_triangular_ = false; 127 | solver->apply_reorth_ = true; 128 | EXPECT_LT(eval(solver, A_list, b_list), 1e-10); 129 | } 130 | 131 | std::cout << "== Gmres (no reorthogonalization) ==" << std::endl; 132 | solver->k_max_ = 1000; 133 | solver->make_triangular_ = true; 134 | solver->apply_reorth_ = false; 135 | EXPECT_LT(eval(solver, A_list, b_list), 1e-10); 136 | 137 | std::cout << "== Gmres (small iteration) ==" << std::endl; 138 | solver->k_max_ = 20; 139 | solver->make_triangular_ = true; 140 | solver->apply_reorth_ = true; 141 | EXPECT_LT(eval(solver, A_list, b_list), 1e2); 142 | } 143 | 144 | { 145 | auto solver = std::make_shared(); 146 | std::cout << "== FullPivLu ==" << std::endl; 147 | EXPECT_LT(eval(solver, A_list, b_list), 1e-10); 148 | } 149 | 150 | { 151 | auto solver = std::make_shared(); 152 | std::cout << "== HouseholderQr ==" << std::endl; 153 | EXPECT_LT(eval(solver, A_list, b_list), 1e-10); 154 | } 155 | } 156 | } 157 | 158 | int main(int argc, char ** argv) 159 | { 160 | testing::InitGoogleTest(&argc, argv); 161 | return RUN_ALL_TESTS(); 162 | } 163 | -------------------------------------------------------------------------------- /nmpc_ddp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | set(PROJECT_NAME nmpc_ddp) 4 | set(PROJECT_GENERATED_HEADERS_SKIP_DEPRECATED ON) 5 | set(PROJECT_GENERATED_HEADERS_SKIP_CONFIG ON) 6 | set(PROJECT_GENERATED_HEADERS_SKIP_WARNING ON) 7 | set(PROJECT_URL https://github.com/isri-aist/NMPC) 8 | set(PROJECT_DESCRIPTION "") 9 | set(CMAKE_CXX_STANDARD 17) 10 | set(PROJECT_USE_CMAKE_EXPORT TRUE) 11 | set(CXX_DISABLE_WERROR ON) 12 | option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) 13 | 14 | include(../cmake/base.cmake) 15 | project(nmpc_ddp LANGUAGES CXX) 16 | 17 | # Options 18 | option(OPTIMIZE_FOR_NATIVE "Enable -march=native" OFF) 19 | 20 | if(NOT DEFINED NMPC_STANDALONE) 21 | set(NMPC_STANDALONE OFF) 22 | endif() 23 | 24 | if(NOT NMPC_STANDALONE) 25 | find_package(rclcpp REQUIRED) 26 | endif() 27 | 28 | # Eigen 29 | find_package(Eigen3 QUIET NO_CMAKE_PACKAGE_REGISTRY) 30 | if(Eigen3_FOUND) 31 | add_project_dependency(Eigen3 REQUIRED NO_CMAKE_PACKAGE_REGISTRY) 32 | else() 33 | add_project_dependency(Eigen3 MODULE REQUIRED) 34 | endif() 35 | 36 | if(NOT NMPC_STANDALONE) 37 | ament_package() 38 | else() 39 | set(BUILD_TESTING OFF) 40 | endif() 41 | 42 | add_library(nmpc_ddp INTERFACE) 43 | target_compile_features(nmpc_ddp INTERFACE cxx_std_17) 44 | 45 | target_include_directories(nmpc_ddp INTERFACE $ $) 46 | if(OPTIMIZE_FOR_NATIVE) 47 | target_compile_options(nmpc_ddp INTERFACE -march=native) 48 | endif() 49 | if(TARGET Eigen3::Eigen) 50 | target_link_libraries(nmpc_ddp INTERFACE Eigen3::Eigen) 51 | else() 52 | target_include_directories(nmpc_ddp SYSTEM INTERFACE "${EIGEN3_INCLUDE_DIR}") 53 | endif() 54 | 55 | install(TARGETS nmpc_ddp EXPORT "${TARGETS_EXPORT_NAME}") 56 | install(DIRECTORY include/nmpc_ddp DESTINATION "${INCLUDE_INSTALL_DIR}") 57 | 58 | if(BUILD_TESTING) 59 | add_subdirectory(tests) 60 | endif() 61 | 62 | if(INSTALL_DOCUMENTATION) 63 | add_subdirectory(doc) 64 | endif() 65 | -------------------------------------------------------------------------------- /nmpc_ddp/README.md: -------------------------------------------------------------------------------- 1 | # nmpc_ddp 2 | Non-linear model predictive control (NMPC) with differential dynamic drogramming (DDP) 3 | 4 | [![CI-standalone](https://github.com/isri-aist/NMPC/actions/workflows/ci-standalone.yaml/badge.svg)](https://github.com/isri-aist/NMPC/actions/workflows/ci-standalone.yaml) 5 | [![CI-catkin](https://github.com/isri-aist/NMPC/actions/workflows/ci-catkin.yaml/badge.svg)](https://github.com/isri-aist/NMPC/actions/workflows/ci-catkin.yaml) 6 | [![Documentation](https://img.shields.io/badge/doxygen-online-brightgreen?logo=read-the-docs&style=flat)](https://isri-aist.github.io/NMPC/nmpc_ddp/index.html) 7 | 8 | https://github.com/isri-aist/NMPC/assets/6636600/02f64c91-88aa-42d8-abfd-f8062d7406e9 9 | 10 | ## Features 11 | - C++ header-only library 12 | - Treats state and control input dimensions as template parameters 13 | - Supports time-varying control input dimensions 14 | - Supports constrained control input 15 | 16 | ## Install 17 | See [here](https://isri-aist.github.io/NMPC/doc/Install). 18 | 19 | ## Technical details 20 | See the following for a detailed algorithm. 21 | - Y Tassa, T Erez, E Todorov. Synthesis and stabilization of complex behaviors through online trajectory optimization. IROS, 2012. 22 | - Y Tassa, N Mansard, E Todorov. Control-limited differential dynamic programming. ICRA, 2014. 23 | 24 | The source code implementation is based on the following. 25 | - https://www.mathworks.com/matlabcentral/fileexchange/52069-ilqg-ddp-trajectory-optimization 26 | 27 | ## Examples 28 | Make sure that it is built with `--catkin-make-args tests` option. 29 | 30 | ### [Bipedal dynamics](tests/src/TestDDPBipedal.cpp) 31 | Controlling on CoM-ZMP dynamics with time-variant CoM height. 32 | System is linear and time-variant. 33 | ```bash 34 | $ rosrun nmpc_ddp TestDDPBipedal 35 | $ rosrun nmpc_ddp plotTestDDPBipedal.py 36 | ``` 37 | ![TestDDPBipedal](doc/images/TestDDPBipedal.png) 38 | 39 | ### [Vertical motion](tests/src/TestDDPVerticalMotion.cpp) 40 | Controlling vertical motion with time-variant number of contacts (including floating phase). 41 | System is linear and time-variant. 42 | The dimension of the control input changes (there are even time steps with an empty control input). 43 | The upper and lower limits are imposed on the control input. 44 | ```bash 45 | $ rosrun nmpc_ddp TestDDPVerticalMotion --gtest_filter=*.WithConstraint 46 | $ # rosrun nmpc_ddp TestDDPVerticalMotion --gtest_filter=*.WithoutConstraint # Try the unconstrained case 47 | $ rosrun nmpc_ddp plotTestDDPVerticalMotion.py 48 | ``` 49 | ![TestDDPVerticalMotion](doc/images/TestDDPVerticalMotion.png) 50 | 51 | ### [Cart-pole](tests/src/TestDDPCartPole.cpp) 52 | Controlling cart-pole (also known as inverted pendulum). 53 | System is non-linear. 54 | This is an example of a realistic setup where the control and simulation have different time periods. 55 | ```bash 56 | # 10-second simulation 57 | $ rostest nmpc_ddp TestDDPCartPole.test --text 58 | # Endless simulation 59 | $ rostest nmpc_ddp TestDDPCartPole.test no_exit:=true --text 60 | ``` 61 | ![TestDDPCartPole](doc/images/TestDDPCartPole.gif) 62 | You can interactively add disturbances and change the target position via the GUI. See the [video](https://www.dropbox.com/s/c3xf67wiffvoj6q/TestDDPCartPole-20220328.mp4?dl=0). 63 | 64 | ### [Centroidal motion](tests/src/TestDDPCentroidalMotion.cpp) 65 | ```bash 66 | $ rosrun nmpc_ddp TestDDPCentroidalMotion 67 | ``` 68 | -------------------------------------------------------------------------------- /nmpc_ddp/doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Doxygen REQUIRED) 2 | 3 | if(DOXYGEN_FOUND) 4 | set(DOXYFILE_PATH ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) 5 | 6 | add_custom_target(nmpc_ddp_doc ALL 7 | ${DOXYGEN_EXECUTABLE} ${DOXYFILE_PATH} 8 | DEPENDS ${DOXYFILE_PATH} 9 | COMMENT "Generating Doxygen documentation" 10 | ) 11 | endif() 12 | -------------------------------------------------------------------------------- /nmpc_ddp/doc/images/TestDDPBipedal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isri-aist/NMPC/80bbe5777487f8c4ec44e0256c4e2a77a2be82ea/nmpc_ddp/doc/images/TestDDPBipedal.png -------------------------------------------------------------------------------- /nmpc_ddp/doc/images/TestDDPCartPole.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isri-aist/NMPC/80bbe5777487f8c4ec44e0256c4e2a77a2be82ea/nmpc_ddp/doc/images/TestDDPCartPole.gif -------------------------------------------------------------------------------- /nmpc_ddp/doc/images/TestDDPVerticalMotion.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isri-aist/NMPC/80bbe5777487f8c4ec44e0256c4e2a77a2be82ea/nmpc_ddp/doc/images/TestDDPVerticalMotion.png -------------------------------------------------------------------------------- /nmpc_ddp/include/nmpc_ddp/BoxQP.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace nmpc_ddp 10 | { 11 | /** \brief Solver for quadratic programming problems with box constraints (i.e., only upper and lower bounds). 12 | \tparam VarDim dimension of decision variables 13 | 14 | See the following for a detailed algorithm. 15 | - Y Tassa, N Mansard, E Todorov. Control-limited differential dynamic programming. ICRA, 2014. 16 | - https://www.mathworks.com/matlabcentral/fileexchange/52069-ilqg-ddp-trajectory-optimization 17 | */ 18 | template 19 | class BoxQP 20 | { 21 | public: 22 | /** \brief Type of vector of variables dimension. */ 23 | using VarDimVector = Eigen::Matrix; 24 | 25 | /** \brief Type of matrix of variables x variables dimension. */ 26 | using VarVarDimMatrix = Eigen::Matrix; 27 | 28 | /** \brief Type of boolean array of variables dimension. */ 29 | using VarDimArray = Eigen::Array; 30 | 31 | public: 32 | /*! \brief Configuration. */ 33 | struct Configuration 34 | { 35 | //! Print level (0: no print, 1: print only important, 2: print verbose, 3: print very verbose) 36 | int print_level = 1; 37 | 38 | //! Maximum iteration of optimization loop 39 | int max_iter = 500; 40 | 41 | //! Termination threshold of non-fixed gradient 42 | double grad_thre = 1e-8; 43 | 44 | //! Termination threshold of relative improvement 45 | double rel_improve_thre = 1e-8; 46 | 47 | //! Factor for decreasing stepsize 48 | double step_factor = 0.6; 49 | 50 | //! Minimum stepsize for linesearch 51 | double min_step = 1e-22; 52 | 53 | //! Armijo parameter (fraction of linear improvement required) 54 | double armijo_param = 0.1; 55 | }; 56 | 57 | /*! \brief Data to trace optimization loop. */ 58 | struct TraceData 59 | { 60 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | 62 | //! Iteration of optimization loop 63 | int iter = 0; 64 | 65 | //! Decision variables 66 | VarDimVector x; 67 | 68 | //! Objective value 69 | double obj = 0; 70 | 71 | //! Search direction 72 | VarDimVector search_dir; 73 | 74 | //! Flag of clamped variables dimensions 75 | VarDimArray clamped_flag; 76 | 77 | //! Number of factorization 78 | int factorization_num = 0; 79 | 80 | //! Number of linesearch step 81 | int step_num = 0; 82 | 83 | /** \brief Constructor. 84 | \param var_dim dimension of decision variables 85 | */ 86 | TraceData(int var_dim) 87 | { 88 | x.setZero(var_dim); 89 | search_dir.setZero(var_dim); 90 | clamped_flag.setZero(var_dim); 91 | } 92 | }; 93 | 94 | public: 95 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 96 | 97 | /** \brief Constructor. 98 | \param var_dim dimension of decision variables 99 | \note dimensions in parameter can be omitted if a fixed value is given in the template value. 100 | */ 101 | BoxQP(int var_dim = VarDim) : var_dim_(var_dim) 102 | { 103 | // Check dimension is positive 104 | if(var_dim_ <= 0) 105 | { 106 | throw std::runtime_error("var_dim must be positive: " + std::to_string(var_dim_) + " <= 0"); 107 | } 108 | 109 | // Check dimension consistency 110 | if constexpr(VarDim != Eigen::Dynamic) 111 | { 112 | if(var_dim_ != VarDim) 113 | { 114 | throw std::runtime_error("var_dim is inconsistent with template parameter: " + std::to_string(var_dim_) 115 | + " != " + std::to_string(VarDim)); 116 | } 117 | } 118 | } 119 | 120 | /** \brief Solve optimization. 121 | \param H Hessian matrix of objective function 122 | \param g gradient vector of objective function 123 | \param lower lower limit of decision variables 124 | \param upper upper limit of decision variables 125 | */ 126 | inline VarDimVector solve(const VarVarDimMatrix & H, 127 | const VarDimVector & g, 128 | const VarDimVector & lower, 129 | const VarDimVector & upper) 130 | { 131 | return solve(H, g, lower, upper, VarDimVector::Zero(var_dim_)); 132 | } 133 | 134 | /** \brief Solve optimization. 135 | \param H Hessian matrix of objective function 136 | \param g gradient vector of objective function 137 | \param lower lower limit of decision variables 138 | \param upper upper limit of decision variables 139 | \param initial_x initial guess of decision variables 140 | */ 141 | inline VarDimVector solve(const VarVarDimMatrix & H, 142 | const VarDimVector & g, 143 | const VarDimVector & lower, 144 | const VarDimVector & upper, 145 | const VarDimVector & initial_x) 146 | { 147 | // Initialize objective value 148 | VarDimVector x = initial_x.cwiseMin(upper).cwiseMax(lower); 149 | double obj = x.dot(g) + 0.5 * x.dot(H * x); 150 | double old_obj = obj; 151 | 152 | // Initialize trace data 153 | trace_data_list_.clear(); 154 | TraceData initial_trace_data(var_dim_); 155 | initial_trace_data.iter = 0; 156 | initial_trace_data.x = x; 157 | initial_trace_data.obj = obj; 158 | trace_data_list_.push_back(initial_trace_data); 159 | 160 | // Main loop 161 | retval_ = 0; 162 | int factorization_num = 0; 163 | VarDimVector grad = VarDimVector::Zero(var_dim_); 164 | VarDimArray clamped_flag = VarDimArray::Zero(var_dim_); 165 | VarDimArray old_clamped_flag = clamped_flag; 166 | int iter = 1; 167 | free_idxs_.clear(); 168 | for(;; iter++) 169 | { 170 | // Append trace data 171 | trace_data_list_.push_back(TraceData(var_dim_)); 172 | auto & trace_data = trace_data_list_.back(); 173 | trace_data.iter = iter; 174 | 175 | // Check relative improvement 176 | if(iter > 1 && (old_obj - obj) < config_.rel_improve_thre * std::abs(old_obj)) 177 | { 178 | retval_ = 4; 179 | break; 180 | } 181 | old_obj = obj; 182 | 183 | // Calculate gradient 184 | grad = g + H * x; 185 | 186 | // Find clamped dimensions 187 | old_clamped_flag = clamped_flag; 188 | clamped_flag.setConstant(false); 189 | clamped_flag = 190 | ((x.array() == lower.array() && grad.array() > 0) || (x.array() == upper.array() && grad.array() < 0)) 191 | .select(true, clamped_flag); 192 | 193 | // Set clamped and free indices 194 | std::vector clamped_idxs; 195 | free_idxs_.clear(); 196 | for(int i = 0; i < clamped_flag.size(); i++) 197 | { 198 | if(clamped_flag[i]) 199 | { 200 | clamped_idxs.push_back(i); 201 | } 202 | else 203 | { 204 | free_idxs_.push_back(i); 205 | } 206 | } 207 | 208 | // Check for all clamped 209 | if(clamped_flag.all()) 210 | { 211 | retval_ = 6; 212 | break; 213 | } 214 | 215 | // Factorize if clamped has changed 216 | if(iter == 1 || (clamped_flag != old_clamped_flag).any()) 217 | { 218 | // Set H_free 219 | Eigen::MatrixXd H_free(free_idxs_.size(), free_idxs_.size()); 220 | for(size_t i = 0; i < free_idxs_.size(); i++) 221 | { 222 | for(size_t j = 0; j < free_idxs_.size(); j++) 223 | { 224 | H_free(i, j) = H(free_idxs_[i], free_idxs_[j]); 225 | } 226 | } 227 | 228 | // Cholesky decomposition 229 | llt_free_ = std::make_unique>(H_free); 230 | if(llt_free_->info() == Eigen::NumericalIssue) 231 | { 232 | if(config_.print_level >= 1) 233 | { 234 | std::cout << "[BoxQP] H_free is not positive definite in Cholesky decomposition (LLT)." << std::endl; 235 | } 236 | retval_ = -1; 237 | break; 238 | } 239 | 240 | factorization_num++; 241 | } 242 | 243 | // Check gradient norm 244 | double grad_norm = 0; 245 | for(size_t i = 0; i < free_idxs_.size(); i++) 246 | { 247 | grad_norm += std::pow(grad[free_idxs_[i]], 2); 248 | } 249 | if(grad_norm < std::pow(config_.grad_thre, 2)) 250 | { 251 | retval_ = 5; 252 | break; 253 | } 254 | 255 | // Calculate search direction 256 | Eigen::VectorXd x_clamped(clamped_idxs.size()); 257 | Eigen::VectorXd x_free(free_idxs_.size()); 258 | Eigen::VectorXd g_free(free_idxs_.size()); 259 | Eigen::MatrixXd H_free_clamped(free_idxs_.size(), clamped_idxs.size()); 260 | for(size_t i = 0; i < clamped_idxs.size(); i++) 261 | { 262 | x_clamped[i] = x[clamped_idxs[i]]; 263 | } 264 | for(size_t i = 0; i < free_idxs_.size(); i++) 265 | { 266 | x_free[i] = x[free_idxs_[i]]; 267 | g_free[i] = g[free_idxs_[i]]; 268 | for(size_t j = 0; j < clamped_idxs.size(); j++) 269 | { 270 | H_free_clamped(i, j) = H(free_idxs_[i], clamped_idxs[j]); 271 | } 272 | } 273 | Eigen::VectorXd grad_free_clamped = g_free + H_free_clamped * x_clamped; 274 | Eigen::VectorXd search_dir_free = -1 * llt_free_->solve(grad_free_clamped) - x_free; 275 | VarDimVector search_dir = VarDimVector::Zero(var_dim_); 276 | for(size_t i = 0; i < free_idxs_.size(); i++) 277 | { 278 | search_dir[free_idxs_[i]] = search_dir_free[i]; 279 | } 280 | 281 | // Check for descent direction 282 | double search_dir_grad = search_dir.dot(grad); 283 | if(search_dir_grad > 1e-10) // This should not happen 284 | { 285 | if(config_.print_level >= 1) 286 | { 287 | std::cout << "[BoxQP] search_dir_grad is positive: " << search_dir_grad << std::endl; 288 | } 289 | retval_ = -2; 290 | break; 291 | } 292 | 293 | // Armijo linesearch 294 | double step = 1; 295 | int step_num = 0; 296 | VarDimVector x_candidate = (x + step * search_dir).cwiseMin(upper).cwiseMax(lower); 297 | double obj_candidate = x_candidate.dot(g) + 0.5 * x_candidate.dot(H * x_candidate); 298 | while((obj_candidate - old_obj) / (step * search_dir_grad) < config_.armijo_param) 299 | { 300 | step = step * config_.step_factor; 301 | step_num++; 302 | x_candidate = (x + step * search_dir).cwiseMin(upper).cwiseMax(lower); 303 | obj_candidate = x_candidate.dot(g) + 0.5 * x_candidate.dot(H * x_candidate); 304 | if(step < config_.min_step) 305 | { 306 | retval_ = 2; 307 | break; 308 | } 309 | } 310 | 311 | // Print 312 | if(config_.print_level >= 3) 313 | { 314 | std::cout << "[BoxQP] iter: " << iter << ", obj: " << obj << ", grad_norm: " << grad_norm 315 | << ", obj_update, : " << old_obj - obj_candidate << ", step: " << step 316 | << ", clamped_flag_num: " << clamped_idxs.size() << std::endl; 317 | } 318 | 319 | // Set trace data 320 | trace_data.x = x; 321 | trace_data.obj = obj; 322 | trace_data.search_dir = search_dir; 323 | trace_data.clamped_flag = clamped_flag; 324 | trace_data.factorization_num = factorization_num; 325 | trace_data.step_num = step_num; 326 | 327 | // Accept candidate 328 | x = x_candidate; 329 | obj = obj_candidate; 330 | 331 | // Check loop termination 332 | if(iter == config_.max_iter) 333 | { 334 | retval_ = 1; 335 | break; 336 | } 337 | } 338 | 339 | // Print 340 | if(config_.print_level >= 2) 341 | { 342 | std::cout << "[BoxQP] result: " << retval_ << " (" << retstr_.at(retval_) << "), iter: " << iter 343 | << ", obj: " << obj << ", factorization_num: " << factorization_num << std::endl; 344 | } 345 | 346 | return x; 347 | } 348 | 349 | /** \brief Accessor to configuration. */ 350 | inline Configuration & config() 351 | { 352 | return config_; 353 | } 354 | 355 | /** \brief Const accessor to configuration. */ 356 | inline const Configuration & config() const 357 | { 358 | return config_; 359 | } 360 | 361 | /** \brief Const accessor to trace data list. */ 362 | inline const std::vector & traceDataList() const 363 | { 364 | return trace_data_list_; 365 | } 366 | 367 | public: 368 | //! Dimension of decision variables 369 | const int var_dim_ = 0; 370 | 371 | //! Return value 372 | int retval_ = 0; 373 | 374 | //! Return string 375 | const std::unordered_map retstr_ = {{-2, "Gradient of search direction is positive"}, 376 | {-1, "Hessian is not positive definite"}, 377 | {0, "Computation is not finished"}, 378 | {1, "Maximum main iterations exceeded"}, 379 | {2, "Maximum line-search iterations exceeded"}, 380 | {3, "No bounds, returning Newton point"}, 381 | {4, "Improvement smaller than tolerance"}, 382 | {5, "Gradient norm smaller than tolerance"}, 383 | {6, "All dimensions are clamped"}}; 384 | 385 | //! Cholesky decomposition (LLT) of free block of objective Hessian matrix 386 | std::unique_ptr> llt_free_; 387 | 388 | //! Indices of free dimensions in decision variables 389 | std::vector free_idxs_; 390 | 391 | protected: 392 | //! Configuration 393 | Configuration config_; 394 | 395 | //! Sequence of trace data 396 | std::vector trace_data_list_; 397 | }; 398 | } // namespace nmpc_ddp 399 | -------------------------------------------------------------------------------- /nmpc_ddp/include/nmpc_ddp/DDPProblem.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace nmpc_ddp 10 | { 11 | /** \brief DDP problem. 12 | \tparam StateDim state dimension (fixed only) 13 | \tparam InputDim input dimension (fixed or dynamic (i.e., Eigen::Dynamic)) 14 | */ 15 | template 16 | class DDPProblem 17 | { 18 | public: 19 | /** \brief Type of vector of state dimension. */ 20 | using StateDimVector = Eigen::Matrix; 21 | 22 | /** \brief Type of vector of input dimension. */ 23 | using InputDimVector = Eigen::Matrix; 24 | 25 | /** \brief Type of matrix of state x state dimension. */ 26 | using StateStateDimMatrix = Eigen::Matrix; 27 | 28 | /** \brief Type of matrix of input x input dimension. */ 29 | using InputInputDimMatrix = Eigen::Matrix; 30 | 31 | /** \brief Type of matrix of state x input dimension. */ 32 | using StateInputDimMatrix = Eigen::Matrix; 33 | 34 | /** \brief Type of matrix of input x state dimension. */ 35 | using InputStateDimMatrix = Eigen::Matrix; 36 | 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | 40 | /** \brief Constructor. 41 | \param dt discretization timestep [sec] 42 | */ 43 | DDPProblem(double dt) : dt_(dt) 44 | { 45 | // Check dimension 46 | static_assert(StateDim > 0, "[DDP] Template param StateDim should be positive."); 47 | static_assert(InputDim >= 0 || InputDim == Eigen::Dynamic, 48 | "[DDP] Template param InputDim should be non-negative or Eigen::Dynamic."); 49 | } 50 | 51 | /** \brief Gets the state dimension. */ 52 | static inline constexpr int stateDim() 53 | { 54 | return StateDim; 55 | } 56 | 57 | /** \brief Gets the input dimension. 58 | \note If input dimension is dynamic, this must not be called. Instead, inputDim(t) must be called passing time as 59 | a parameter. 60 | */ 61 | inline virtual int inputDim() const 62 | { 63 | if constexpr(InputDim == Eigen::Dynamic) 64 | { 65 | throw std::runtime_error("Since input dimension is dynamic, time must be passed to inputDim()."); 66 | } 67 | return InputDim; 68 | } 69 | 70 | /** \brief Gets the input dimension. 71 | \param t time 72 | \note If input dimension is dynamic, this must be overridden. 73 | */ 74 | inline virtual int inputDim(double // t 75 | ) const 76 | { 77 | if constexpr(InputDim == Eigen::Dynamic) 78 | { 79 | throw std::runtime_error("inputDim(t) must be overridden if input dimension is dynamic."); 80 | } 81 | else 82 | { 83 | return inputDim(); 84 | } 85 | } 86 | 87 | /** \brief Gets the discretization timestep [sec]. */ 88 | inline double dt() const 89 | { 90 | return dt_; 91 | } 92 | 93 | /** \brief Calculate discrete state equation. 94 | \param t time [sec] 95 | \param x current state (x[k]) 96 | \param u current input (u[k]) 97 | \returns next state (x[k+1]) 98 | */ 99 | virtual StateDimVector stateEq(double t, const StateDimVector & x, const InputDimVector & u) const = 0; 100 | 101 | /** \brief Calculate running cost. 102 | \param t time [sec] 103 | \param x current state (x[k]) 104 | \param u current input (u[k]) 105 | \returns running cost (L[k]) 106 | */ 107 | virtual double runningCost(double t, const StateDimVector & x, const InputDimVector & u) const = 0; 108 | 109 | /** \brief Calculate terminal cost. 110 | \param t time [sec] 111 | \param x current state (x[k]) 112 | \returns terminal cost (phi[k]) 113 | */ 114 | virtual double terminalCost(double t, const StateDimVector & x) const = 0; 115 | 116 | /** \brief Calculate first-order derivatives of discrete state equation. 117 | \param t time [sec] 118 | \param x state 119 | \param u input 120 | \param state_eq_deriv_x first-order derivative of state equation w.r.t. state 121 | \param state_eq_deriv_u first-order derivative of state equation w.r.t. input 122 | */ 123 | virtual void calcStateEqDeriv(double t, 124 | const StateDimVector & x, 125 | const InputDimVector & u, 126 | Eigen::Ref state_eq_deriv_x, 127 | Eigen::Ref state_eq_deriv_u) const = 0; 128 | 129 | /** \brief Calculate first-order and second-order derivatives of discrete state equation. 130 | \param t time [sec] 131 | \param x state 132 | \param u input 133 | \param state_eq_deriv_x first-order derivative of state equation w.r.t. state 134 | \param state_eq_deriv_u first-order derivative of state equation w.r.t. input 135 | \param state_eq_deriv_xx second-order derivative of state equation w.r.t. state 136 | \param state_eq_deriv_uu second-order derivative of state equation w.r.t. input 137 | \param state_eq_deriv_xu second-order derivative of state equation w.r.t. state and input 138 | */ 139 | virtual void calcStateEqDeriv(double t, 140 | const StateDimVector & x, 141 | const InputDimVector & u, 142 | Eigen::Ref state_eq_deriv_x, 143 | Eigen::Ref state_eq_deriv_u, 144 | std::vector & state_eq_deriv_xx, 145 | std::vector & state_eq_deriv_uu, 146 | std::vector & state_eq_deriv_xu) const = 0; 147 | 148 | /** \brief Calculate first-order derivatives of running cost. 149 | \param t time [sec] 150 | \param x state 151 | \param u input 152 | \param running_cost_deriv_x first-order derivative of running cost w.r.t. state 153 | \param running_cost_deriv_u first-order derivative of running cost w.r.t. input 154 | */ 155 | virtual void calcRunningCostDeriv(double t, 156 | const StateDimVector & x, 157 | const InputDimVector & u, 158 | Eigen::Ref running_cost_deriv_x, 159 | Eigen::Ref running_cost_deriv_u) const = 0; 160 | 161 | /** \brief Calculate first-order and second-order derivatives of running cost. 162 | \param t time [sec] 163 | \param x state 164 | \param u input 165 | \param running_cost_deriv_x first-order derivative of running cost w.r.t. state 166 | \param running_cost_deriv_u first-order derivative of running cost w.r.t. input 167 | \param running_cost_deriv_xx second-order derivative of running cost w.r.t. state 168 | \param running_cost_deriv_uu second-order derivative of running cost w.r.t. input 169 | \param running_cost_deriv_xu second-order derivative of running cost w.r.t. state and input 170 | */ 171 | virtual void calcRunningCostDeriv(double t, 172 | const StateDimVector & x, 173 | const InputDimVector & u, 174 | Eigen::Ref running_cost_deriv_x, 175 | Eigen::Ref running_cost_deriv_u, 176 | Eigen::Ref running_cost_deriv_xx, 177 | Eigen::Ref running_cost_deriv_uu, 178 | Eigen::Ref running_cost_deriv_xu) const = 0; 179 | 180 | /** \brief Calculate first-order derivatives of terminal cost. 181 | \param t time [sec] 182 | \param x state 183 | \param terminal_cost_deriv_x first-order derivative of terminal cost w.r.t. state 184 | */ 185 | virtual void calcTerminalCostDeriv(double t, 186 | const StateDimVector & x, 187 | Eigen::Ref terminal_cost_deriv_x) const = 0; 188 | 189 | /** \brief Calculate first-order and second-order derivatives of terminal cost. 190 | \param t time [sec] 191 | \param x state 192 | \param terminal_cost_deriv_x first-order derivative of terminal cost w.r.t. state 193 | \param terminal_cost_deriv_xx second-order derivative of terminal cost w.r.t. state 194 | */ 195 | virtual void calcTerminalCostDeriv(double t, 196 | const StateDimVector & x, 197 | Eigen::Ref terminal_cost_deriv_x, 198 | Eigen::Ref terminal_cost_deriv_xx) const = 0; 199 | 200 | protected: 201 | //! Discretization timestep [sec] 202 | const double dt_ = 0; 203 | }; 204 | } // namespace nmpc_ddp 205 | -------------------------------------------------------------------------------- /nmpc_ddp/include/nmpc_ddp/DDPSolver.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace nmpc_ddp 12 | { 13 | /** \brief DDP solver. 14 | \tparam StateDim state dimension 15 | \tparam InputDim input dimension 16 | 17 | See the following for a detailed algorithm. 18 | - Y Tassa, T Erez, E Todorov. Synthesis and stabilization of complex behaviors through online trajectory 19 | optimization. IROS, 2012. 20 | - Y Tassa, N Mansard, E Todorov. Control-limited differential dynamic programming. ICRA, 2014. 21 | - https://www.mathworks.com/matlabcentral/fileexchange/52069-ilqg-ddp-trajectory-optimization 22 | */ 23 | template 24 | class DDPSolver 25 | { 26 | public: 27 | /** \brief Type of vector of state dimension. */ 28 | using StateDimVector = typename DDPProblem::StateDimVector; 29 | 30 | /** \brief Type of vector of input dimension. */ 31 | using InputDimVector = typename DDPProblem::InputDimVector; 32 | 33 | /** \brief Type of matrix of state x state dimension. */ 34 | using StateStateDimMatrix = typename DDPProblem::StateStateDimMatrix; 35 | 36 | /** \brief Type of matrix of input x input dimension. */ 37 | using InputInputDimMatrix = typename DDPProblem::InputInputDimMatrix; 38 | 39 | /** \brief Type of matrix of state x input dimension. */ 40 | using StateInputDimMatrix = typename DDPProblem::StateInputDimMatrix; 41 | 42 | /** \brief Type of matrix of input x state dimension. */ 43 | using InputStateDimMatrix = typename DDPProblem::InputStateDimMatrix; 44 | 45 | public: 46 | /*! \brief Configuration. */ 47 | struct Configuration 48 | { 49 | /** \brief Constructor. */ 50 | Configuration() 51 | { 52 | // Initialize alpha_list 53 | int list_size = 11; 54 | alpha_list.resize(list_size); 55 | Eigen::VectorXd alpha_exponent_list = Eigen::VectorXd::LinSpaced(list_size, 0, -3); 56 | for(int i = 0; i < list_size; i++) 57 | { 58 | alpha_list[i] = std::pow(10, alpha_exponent_list[i]); 59 | } 60 | } 61 | 62 | //! Print level (0: no print, 1: print only important, 2: print verbose, 3: print very verbose) 63 | int print_level = 1; 64 | 65 | // \todo Support use_state_eq_second_derivative 66 | //! Whether to use second-order derivatives of state equation 67 | bool use_state_eq_second_derivative = false; 68 | 69 | //! Whether input has constraints 70 | bool with_input_constraint = false; 71 | 72 | //! Maximum iteration of optimization loop 73 | int max_iter = 500; 74 | 75 | //! Number of steps in horizon 76 | int horizon_steps = 100; 77 | 78 | //! Regularization type (1: Quu + lambda * I, 2: Vxx + lambda * I) 79 | int reg_type = 1; 80 | 81 | //! Initial regularization coefficient 82 | double initial_lambda = 1e-4; // 1.0 83 | 84 | //! Initial scaling factor of regularization coefficient 85 | double initial_dlambda = 1.0; 86 | 87 | //! Increasing/decreasing factor of regularization coefficient scaling 88 | double lambda_factor = 1.6; 89 | 90 | //! Minimum regularization coefficient 91 | double lambda_min = 1e-6; 92 | 93 | //! Maximum regularization coefficient 94 | double lambda_max = 1e10; 95 | 96 | //! Termination threshold of relative norm of k 97 | double k_rel_norm_thre = 1e-4; 98 | 99 | //! Termination threshold of regularization coefficient 100 | double lambda_thre = 1e-5; 101 | 102 | //! List of alpha (scaling factor of k) 103 | Eigen::VectorXd alpha_list; 104 | 105 | //! Allowable threshold of cost update ratio 106 | double cost_update_ratio_thre = 0; 107 | 108 | //! Termination threshold of cost update 109 | double cost_update_thre = 1e-7; 110 | }; 111 | 112 | /*! \brief Control data. */ 113 | struct ControlData 114 | { 115 | //! Sequence of state (x[0], ..., x[N-1], x[N]) 116 | std::vector x_list; 117 | 118 | //! Sequence of input (u[0], ..., u[N-1]) 119 | std::vector u_list; 120 | 121 | //! Sequence of cost (L[0], ..., L[N-1], phi[N]) 122 | Eigen::VectorXd cost_list; 123 | }; 124 | 125 | /*! \brief Derivatives of DDP problem. */ 126 | struct Derivative 127 | { 128 | /** \brief Constructor. 129 | \param state_dim state dimension 130 | \param input_dim input dimension 131 | \param outer_dim outer dimension of tensor 132 | */ 133 | Derivative(int state_dim, int input_dim, int outer_dim) 134 | { 135 | Fx.resize(state_dim, state_dim); 136 | Fu.resize(state_dim, input_dim); 137 | Fxx.assign(outer_dim, StateStateDimMatrix(state_dim, state_dim)); 138 | Fuu.assign(outer_dim, InputInputDimMatrix(input_dim, input_dim)); 139 | Fxu.assign(outer_dim, StateInputDimMatrix(state_dim, input_dim)); 140 | Lx.resize(state_dim); 141 | Lu.resize(input_dim); 142 | Lxx.resize(state_dim, state_dim); 143 | Luu.resize(input_dim, input_dim); 144 | Lxu.resize(state_dim, input_dim); 145 | } 146 | 147 | //! First-order derivative of state equation w.r.t. state 148 | StateStateDimMatrix Fx; 149 | 150 | //! First-order derivative of state equation w.r.t. input 151 | StateInputDimMatrix Fu; 152 | 153 | //! Second-order derivative of state equation w.r.t. state (tensor of rank 3) 154 | std::vector Fxx; 155 | 156 | //! Second-order derivative of state equation w.r.t. input (tensor of rank 3) 157 | std::vector Fuu; 158 | 159 | //! Second-order derivative of state equation w.r.t. state and input (tensor of rank 3) 160 | std::vector Fxu; 161 | 162 | //! First-order derivative of running cost w.r.t. state 163 | StateDimVector Lx; 164 | 165 | //! First-order derivative of running cost w.r.t. input 166 | InputDimVector Lu; 167 | 168 | //! Second-order derivative of running cost w.r.t. state 169 | StateStateDimMatrix Lxx; 170 | 171 | //! Second-order derivative of running cost w.r.t. input 172 | InputInputDimMatrix Luu; 173 | 174 | //! Second-order derivative of running cost w.r.t. state and input 175 | StateInputDimMatrix Lxu; 176 | }; 177 | 178 | /*! \brief Data to trace optimization loop. */ 179 | struct TraceData 180 | { 181 | //! Iteration of optimization loop 182 | int iter = 0; 183 | 184 | //! Total cost 185 | double cost = 0; 186 | 187 | //! Regularization coefficient 188 | double lambda = 0; 189 | 190 | //! Scaling factor of regularization coefficient 191 | double dlambda = 0; 192 | 193 | //! Scaling factor of k 194 | double alpha = 0; 195 | 196 | //! Norm of relative values of k and u 197 | double k_rel_norm = 0; 198 | 199 | //! Actual update value of cost 200 | double cost_update_actual = 0; 201 | 202 | //! Expected update value of cost 203 | double cost_update_expected = 0; 204 | 205 | //! Ratio of actual and expected update values of cost 206 | double cost_update_ratio = 0; 207 | 208 | //! Duration to calculate derivatives [msec] 209 | double duration_derivative = 0; 210 | 211 | //! Duration to process backward pass [msec] 212 | double duration_backward = 0; 213 | 214 | //! Duration to process forward pass [msec] 215 | double duration_forward = 0; 216 | }; 217 | 218 | /*! \brief Data of computation duration. */ 219 | struct ComputationDuration 220 | { 221 | //! Duration to solve [msec] 222 | double solve = 0; 223 | 224 | //! Duration to setup (included in solve) [msec] 225 | double setup = 0; 226 | 227 | //! Duration of optimization loop (included in solve) [msec] 228 | double opt = 0; 229 | 230 | //! Duration to calculate derivatives (included in opt) [msec] 231 | double derivative = 0; 232 | 233 | //! Duration to process backward pass (included in opt) [msec] 234 | double backward = 0; 235 | 236 | //! Duration to process forward pass (included in opt) [msec] 237 | double forward = 0; 238 | 239 | //! Duration to calculate Q (included in backward) [msec] 240 | double Q = 0; 241 | 242 | //! Duration to calculate regularization (included in backward) [msec] 243 | double reg = 0; 244 | 245 | //! Duration to calculate gains (included in backward) [msec] 246 | double gain = 0; 247 | }; 248 | 249 | public: 250 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 251 | 252 | /** \brief Constructor. 253 | \param problem DDP problem 254 | */ 255 | DDPSolver(const std::shared_ptr> & problem); 256 | 257 | /** \brief Accessor to configuration. */ 258 | inline Configuration & config() 259 | { 260 | return config_; 261 | } 262 | 263 | /** \brief Const accessor to configuration. */ 264 | inline const Configuration & config() const 265 | { 266 | return config_; 267 | } 268 | 269 | /** \brief Solve optimization. 270 | \param current_t current time [sec] 271 | \param current_x current state 272 | \param initial_u_list initial sequence of input 273 | \return whether the process is finished successfully 274 | */ 275 | bool solve(double current_t, const StateDimVector & current_x, const std::vector & initial_u_list); 276 | 277 | /** \brief Set function to return input limits. 278 | \param input_limits_func function to return input limits (in the order of lower, upper) 279 | 280 | \note The input limits are only considered when with_input_constraint in configuration is true. 281 | */ 282 | inline void setInputLimitsFunc(const std::function(double)> & input_limits_func) 283 | { 284 | input_limits_func_ = input_limits_func; 285 | } 286 | 287 | /** \brief Const accessor to control data calculated by solve(). */ 288 | inline const ControlData & controlData() const 289 | { 290 | return control_data_; 291 | } 292 | 293 | /** \brief Const accessor to trace data list. */ 294 | inline const std::vector & traceDataList() const 295 | { 296 | return trace_data_list_; 297 | } 298 | 299 | /** \brief Const accessor to computation duration. */ 300 | inline const ComputationDuration & computationDuration() const 301 | { 302 | return computation_duration_; 303 | } 304 | 305 | /** \brief Dump trace data list. 306 | \param file_path path to output file 307 | */ 308 | void dumpTraceDataList(const std::string & file_path) const; 309 | 310 | protected: 311 | /** \brief Process one iteration. 312 | \param iter current iteration 313 | \return 0 for continue, 1 for terminate, -1 for failure 314 | */ 315 | int procOnce(int iter); 316 | 317 | /** \brief Process backward pass. 318 | \return whether the process is finished successfully 319 | */ 320 | bool backwardPass(); 321 | 322 | /** \brief Process forward pass. 323 | \param alpha scaling factor of k 324 | */ 325 | void forwardPass(double alpha); 326 | 327 | protected: 328 | //! Configuration 329 | Configuration config_; 330 | 331 | //! DDP problem 332 | std::shared_ptr> problem_; 333 | 334 | //! Sequence of trace data 335 | std::vector trace_data_list_; 336 | 337 | //! Function to return input limits (in the order of lower, upper) 338 | std::function(double)> input_limits_func_; 339 | 340 | //! Computation duration data 341 | ComputationDuration computation_duration_; 342 | 343 | //! Current time [sec] 344 | double current_t_ = 0; 345 | 346 | //! Regularization coefficient 347 | double lambda_ = 0; 348 | 349 | //! Scaling factor of regularization coefficient 350 | double dlambda_ = 0; 351 | 352 | //! Control data (sequence of state, input, and cost) 353 | ControlData control_data_; 354 | 355 | //! Candidate control data (sequence of state, input, and cost) 356 | ControlData candidate_control_data_; 357 | 358 | //! Sequence of feedforward term for input (k[0], ..., k[N-1]) 359 | std::vector k_list_; 360 | 361 | //! Sequence of feedback gain for input w.r.t. state error (K[0], ..., K[N-1]) 362 | std::vector K_list_; 363 | 364 | //! Sequence of derivatives 365 | std::vector derivative_list_; 366 | 367 | //! First-order derivative of value in last step of horizon 368 | StateDimVector last_Vx_; 369 | 370 | //! Second-order derivative of value in last step of horizon 371 | StateStateDimMatrix last_Vxx_; 372 | 373 | //! Expected update of value 374 | Eigen::Vector2d dV_; 375 | }; 376 | } // namespace nmpc_ddp 377 | 378 | #include 379 | -------------------------------------------------------------------------------- /nmpc_ddp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | nmpc_ddp 3 | 0.1.0 4 | 5 | Non-linear model predictive control (NMPC) with differential dynamic drogramming (DDP) 6 | 7 | Masaki Murooka 8 | BSD 9 | 10 | http://ros.org/wiki/nmpc_ddp 11 | Masaki Murooka 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | 17 | eigen 18 | 19 | ament_cmake_gtest 20 | visualization_msgs 21 | std_srvs 22 | rviz 23 | rqt_service_caller 24 | 25 | doxygen 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /nmpc_ddp/scripts/plotDDPTraceData.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | class PlotDDPTraceData(object): 8 | def __init__(self, trace_file_path): 9 | self.trace_data_list = np.genfromtxt(trace_file_path, dtype=None, delimiter=' ', names=True) 10 | print("[PlotDDPTraceData] Load {}".format(trace_file_path)) 11 | 12 | def __call__(self, key_list, x_axis_iter=True): 13 | if isinstance(key_list, str): 14 | key_list = [key_list] 15 | 16 | # Check key 17 | for key in key_list: 18 | if key not in self.trace_data_list.dtype.names: 19 | print("[PlotDDPTraceData] {} is not contained in key list. Key list is {}".format(key, self.trace_data_list.dtype.names)) 20 | return 21 | 22 | # Plot 23 | for key in key_list: 24 | if x_axis_iter: 25 | plt.plot(self.trace_data_list["iter"], self.trace_data_list[key], marker='o', label=key) 26 | else: 27 | plt.plot(self.trace_data_list[key], marker='o', label=key) 28 | 29 | # Show 30 | if x_axis_iter: 31 | plt.xlabel("iter") 32 | else: 33 | plt.xlabel("index") 34 | plt.grid() 35 | plt.legend() 36 | plt.show() 37 | 38 | 39 | if __name__ == "__main__": 40 | import sys 41 | if len(sys.argv) != 2: 42 | print("usage: {} ".format(sys.argv[0])) 43 | sys.exit(1) 44 | 45 | plot = PlotDDPTraceData(sys.argv[1]) 46 | print("[PlotDDPTraceData] Run \"plot()\" or \"plot([, , ...])\". Select the key from the following:\n{}".format( 47 | plot.trace_data_list.dtype.names)) 48 | 49 | import code 50 | code.interact(local=locals()) 51 | -------------------------------------------------------------------------------- /nmpc_ddp/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT NMPC_STANDALONE) 2 | find_package(rostest REQUIRED) 3 | endif() 4 | 5 | if(NMPC_STANDALONE) 6 | find_package(GTest REQUIRED) 7 | include(GoogleTest) 8 | function(add_nmpc_ddp_test NAME) 9 | add_executable(${NAME} src/${NAME}.cpp) 10 | target_link_libraries(${NAME} PUBLIC GTest::gtest nmpc_ddp) 11 | gtest_discover_tests(${NAME}) 12 | endfunction() 13 | else() 14 | function(add_nmpc_ddp_test NAME) 15 | catkin_add_gtest(${NAME} src/${NAME}.cpp) 16 | target_link_libraries(${NAME} nmpc_ddp) 17 | endfunction() 18 | endif() 19 | 20 | 21 | set(nmpc_ddp_gtest_list 22 | TestBoxQP 23 | TestDDPBipedal 24 | TestDDPVerticalMotion 25 | TestDDPCentroidalMotion 26 | ) 27 | 28 | set(nmpc_ddp_rostest_list 29 | TestDDPCartPole 30 | ) 31 | 32 | foreach(NAME IN LISTS nmpc_ddp_gtest_list) 33 | add_nmpc_ddp_test(${NAME}) 34 | endforeach() 35 | 36 | if(NOT NMPC_STANDALONE) 37 | foreach(NAME IN LISTS nmpc_ddp_rostest_list) 38 | add_rostest_gtest(${NAME} test/${NAME}.test src/${NAME}.cpp) 39 | target_link_libraries(${NAME} nmpc_ddp) 40 | endforeach() 41 | endif() 42 | -------------------------------------------------------------------------------- /nmpc_ddp/tests/rqt/TestDDPCartPole.perspective: -------------------------------------------------------------------------------- 1 | { 2 | "keys": {}, 3 | "groups": { 4 | "pluginmanager": { 5 | "keys": { 6 | "running-plugins": { 7 | "type": "repr", 8 | "repr": "{u'rqt_service_caller/ServiceCaller': [1]}" 9 | } 10 | }, 11 | "groups": { 12 | "plugin__rqt_ez_publisher__EzPublisher__1": { 13 | "keys": {}, 14 | "groups": { 15 | "plugin": { 16 | "keys": { 17 | "texts": { 18 | "type": "repr", 19 | "repr": "[u'/goal/header/frame_id', u'/goal/pose/position/x', u'/goal/pose/position/y', u'/goal/pose/position/z', u'/goal/pose/orientation']" 20 | }, 21 | "publish_interval": { 22 | "type": "repr", 23 | "repr": "u'100'" 24 | }, 25 | "configurable": { 26 | "type": "repr", 27 | "repr": "u'true'" 28 | } 29 | }, 30 | "groups": { 31 | "goal": { 32 | "keys": {}, 33 | "groups": { 34 | "header": { 35 | "keys": { 36 | "frame_id_is_repeat": { 37 | "type": "repr", 38 | "repr": "u'false'" 39 | }, 40 | "frame_id_range": { 41 | "type": "repr", 42 | "repr": "(0, 0)" 43 | } 44 | }, 45 | "groups": {} 46 | }, 47 | "pose": { 48 | "keys": { 49 | "orientation_is_repeat": { 50 | "type": "repr", 51 | "repr": "u'false'" 52 | }, 53 | "orientation_range": { 54 | "type": "repr", 55 | "repr": "(-3.14, 3.14)" 56 | } 57 | }, 58 | "groups": { 59 | "position": { 60 | "keys": { 61 | "x_range": { 62 | "type": "repr", 63 | "repr": "(-1.0, 1.0)" 64 | }, 65 | "z_is_repeat": { 66 | "type": "repr", 67 | "repr": "u'false'" 68 | }, 69 | "y_is_repeat": { 70 | "type": "repr", 71 | "repr": "u'false'" 72 | }, 73 | "x_is_repeat": { 74 | "type": "repr", 75 | "repr": "u'false'" 76 | }, 77 | "y_range": { 78 | "type": "repr", 79 | "repr": "(-1.0, 1.0)" 80 | }, 81 | "z_range": { 82 | "type": "repr", 83 | "repr": "(-1.0, 1.0)" 84 | } 85 | }, 86 | "groups": {} 87 | } 88 | } 89 | } 90 | } 91 | } 92 | } 93 | } 94 | } 95 | }, 96 | "plugin__rqt_service_caller__ServiceCaller__1": { 97 | "keys": {}, 98 | "groups": { 99 | "dock_widget__ServiceCallerWidget": { 100 | "keys": { 101 | "dockable": { 102 | "type": "repr", 103 | "repr": "True" 104 | }, 105 | "parent": { 106 | "type": "repr", 107 | "repr": "None" 108 | }, 109 | "dock_widget_title": { 110 | "type": "repr", 111 | "repr": "u'Service Caller'" 112 | } 113 | }, 114 | "groups": {} 115 | }, 116 | "plugin": { 117 | "keys": { 118 | "splitter_orientation": { 119 | "type": "repr", 120 | "repr": "2" 121 | } 122 | }, 123 | "groups": {} 124 | } 125 | } 126 | }, 127 | "plugin__rqt_publisher__Publisher__1": { 128 | "keys": {}, 129 | "groups": { 130 | "dock_widget__PublisherWidget": { 131 | "keys": { 132 | "dockable": { 133 | "type": "repr", 134 | "repr": "u'true'" 135 | }, 136 | "parent": { 137 | "type": "repr", 138 | "repr": "None" 139 | }, 140 | "dock_widget_title": { 141 | "type": "repr", 142 | "repr": "u'Message Publisher'" 143 | } 144 | }, 145 | "groups": {} 146 | }, 147 | "plugin": { 148 | "keys": { 149 | "publishers": { 150 | "type": "repr", 151 | "repr": "u\"[{'type_name': 'std_msgs/Float64', 'topic_name': '/test', 'enabled': False, 'rate': 1.0, 'expressions': {u'/test/data': '10'}, 'publisher_id': 1, 'counter': 0}]\"" 152 | } 153 | }, 154 | "groups": {} 155 | } 156 | } 157 | } 158 | } 159 | }, 160 | "mainwindow": { 161 | "keys": { 162 | "geometry": { 163 | "type": "repr(QByteArray.hex)", 164 | "repr(QByteArray.hex)": "b'01d9d0cb00020000000000a00000032e0000056a0000054e000000a00000035c0000056a0000054e00000000000000000a00'", 165 | "pretty-print": " . j N \\ j N " 166 | }, 167 | "state": { 168 | "type": "repr(QByteArray.hex)", 169 | "repr(QByteArray.hex)": "b'000000ff00000000fd0000000200000002000004cb000001d2fc0100000001fb00000072007200710074005f0073006500720076006900630065005f00630061006c006c00650072005f005f005300650072007600690063006500430061006c006c00650072005f005f0031005f005f005300650072007600690063006500430061006c006c006500720057006900640067006500740100000000000004cb000002b700ffffff00000003000004cb00000198fc0100000002fb0000006a007200710074005f0065007a005f007000750062006c00690073006800650072005f005f0045007a005000750062006c00690073006800650072005f005f0031005f005f0045007a005000750062006c006900730068006500720050006c007500670069006e005500690100000000000004cb0000000000000000fb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c006900730068006500720057006900640067006500740100000000000004cb0000000000000000000004cb0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000'", 170 | "pretty-print": " rrqt_service_caller__ServiceCaller__1__ServiceCallerWidget jrqt_ez_publisher__EzPublisher__1__EzPublisherPluginUi Xrqt_publisher__Publisher__1__PublisherWidget 6MinimizedDockWidgetsToolbar " 171 | } 172 | }, 173 | "groups": { 174 | "toolbar_areas": { 175 | "keys": { 176 | "MinimizedDockWidgetsToolbar": { 177 | "type": "repr", 178 | "repr": "8" 179 | } 180 | }, 181 | "groups": {} 182 | } 183 | } 184 | } 185 | } 186 | } -------------------------------------------------------------------------------- /nmpc_ddp/tests/rviz/TestDDPCartPole.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 110 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 312 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: "" 28 | Preferences: 29 | PromptSaveOnExit: true 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: false 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 100 51 | Reference Frame: 52 | Value: false 53 | - Alpha: 1 54 | Cell Size: 100 55 | Class: rviz/Grid 56 | Color: 0; 0; 0 57 | Enabled: true 58 | Line Style: 59 | Line Width: 0.029999999329447746 60 | Value: Billboards 61 | Name: HorizontalLine 62 | Normal Cell Count: 0 63 | Offset: 64 | X: 0 65 | Y: 0 66 | Z: -1000 67 | Plane: XZ 68 | Plane Cell Count: 1 69 | Reference Frame: 70 | Value: true 71 | - Class: rviz/MarkerArray 72 | Enabled: true 73 | Marker Topic: /marker_arr 74 | Name: MarkerArray 75 | Namespaces: 76 | cart: true 77 | force: true 78 | mass: true 79 | pole: true 80 | target: true 81 | Queue Size: 100 82 | Value: true 83 | Enabled: true 84 | Global Options: 85 | Background Color: 255; 255; 255 86 | Default Light: true 87 | Fixed Frame: world 88 | Frame Rate: 30 89 | Name: root 90 | Tools: 91 | - Class: rviz/Interact 92 | Hide Inactive Objects: true 93 | - Class: rviz/MoveCamera 94 | - Class: rviz/Select 95 | - Class: rviz/FocusCamera 96 | - Class: rviz/Measure 97 | - Class: rviz/SetInitialPose 98 | Theta std deviation: 0.2617993950843811 99 | Topic: /initialpose 100 | X std deviation: 0.5 101 | Y std deviation: 0.5 102 | - Class: rviz/SetGoal 103 | Topic: /move_base_simple/goal 104 | - Class: rviz/PublishPoint 105 | Single click: true 106 | Topic: /clicked_point 107 | Value: true 108 | Views: 109 | Current: 110 | Angle: 0 111 | Class: rviz/TopDownOrtho 112 | Enable Stereo Rendering: 113 | Stereo Eye Separation: 0.05999999865889549 114 | Stereo Focal Distance: 1 115 | Swap Stereo Eyes: false 116 | Value: false 117 | Invert Z Axis: false 118 | Name: Current View 119 | Near Clip Distance: 0.009999999776482582 120 | Scale: 50.557960510253906 121 | Target Frame: 122 | Value: TopDownOrtho (rviz) 123 | X: 0 124 | Y: 0 125 | Saved: ~ 126 | Window Geometry: 127 | Displays: 128 | collapsed: true 129 | Height: 1401 130 | Hide Left Dock: true 131 | Hide Right Dock: true 132 | QMainWindow State: 000000ff00000000fd0000000400000000000001ab00000480fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003a000004800000013600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000012e000004bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003a000004bd000000f800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000099600000051fc0100000002fc00000000000009960000048000fffffffa000000010200000002fb0000003e0045006d007000740079005300650072007600690063006500430061006c006c0049006e00740065007200660061006300650041006300740069006f006e0000000000ffffffff0000000000000000fb0000000800540069006d00650100000500000000510000004e00fffffffb0000000800540069006d0065010000000000000450000000000000000000000996000004bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 133 | Selection: 134 | collapsed: false 135 | Time: 136 | collapsed: false 137 | Tool Properties: 138 | collapsed: false 139 | Views: 140 | collapsed: true 141 | Width: 2454 142 | X: 106 143 | Y: 39 144 | -------------------------------------------------------------------------------- /nmpc_ddp/tests/scripts/plotTestDDPBipedal.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | class PlotTestDDPBipedal(object): 8 | def __init__(self, result_file_path): 9 | self.result_data_list = np.genfromtxt(result_file_path, dtype=None, delimiter=None, names=True) 10 | print("[PlotTestDDPBipedal] Load {}".format(result_file_path)) 11 | 12 | fig = plt.figure() 13 | plt.rcParams["font.size"] = 16 14 | 15 | ax = fig.add_subplot(211) 16 | ax.plot(self.result_data_list["time"], self.result_data_list["planned_zmp"], 17 | color="red", label="planned ZMP") 18 | ax.plot(self.result_data_list["time"], self.result_data_list["ref_zmp"], 19 | color="blue", linestyle="dashed", label="ref ZMP") 20 | ax.plot(self.result_data_list["time"], self.result_data_list["com_pos"], 21 | color="green", label="CoM") 22 | ax.set_xlabel("time [s]") 23 | ax.set_ylabel("pos [m]") 24 | ax.grid() 25 | ax.legend(loc="upper right") 26 | 27 | ax = fig.add_subplot(212) 28 | ax.plot(self.result_data_list["time"], self.result_data_list["omega2"], color="green", label="omega^2") 29 | ax.set_xlabel("time [s]") 30 | ax.set_ylabel("omega^2 [1/s^2]") 31 | ax.grid() 32 | ax.legend(loc="upper right") 33 | 34 | plt.show() 35 | 36 | 37 | if __name__ == "__main__": 38 | result_file_path = "/tmp/TestDDPBipedalResult.txt" 39 | 40 | import sys 41 | if len(sys.argv) >= 2: 42 | result_file_path = sys.argv[1] 43 | 44 | plot = PlotTestDDPBipedal(result_file_path) 45 | -------------------------------------------------------------------------------- /nmpc_ddp/tests/scripts/plotTestDDPVerticalMotion.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | class PlotTestDDPVerticalMotion(object): 8 | def __init__(self, result_file_path): 9 | self.result_data_list = np.genfromtxt(result_file_path, dtype=None, delimiter=None, names=True) 10 | print("[PlotTestDDPVerticalMotion] Load {}".format(result_file_path)) 11 | 12 | fig = plt.figure() 13 | plt.rcParams["font.size"] = 16 14 | 15 | # Preprocess the number of contacts for setting background color 16 | num_contact_list = self.result_data_list["num_contact"] 17 | num_contact_diff_list = (num_contact_list[1:] - num_contact_list[:-1]).astype(bool) 18 | num_contact_diff_list = np.insert(num_contact_diff_list, 0, False) 19 | num_contact_switch_idx_list = np.where(num_contact_diff_list)[0] 20 | num_contact_switch_idx_list = np.insert(num_contact_switch_idx_list, 0, 0) 21 | num_contact_switch_idx_list = np.insert(num_contact_switch_idx_list, len(num_contact_switch_idx_list), -1) 22 | num_contact_colors = ["blue", "green", "yellow"] 23 | 24 | ax = fig.add_subplot(211) 25 | ax.plot(self.result_data_list["time"], self.result_data_list["pos"], 26 | color="red", label="planned pos") 27 | ax.plot(self.result_data_list["time"], self.result_data_list["ref_pos"], 28 | color="blue", linestyle="dashed", label="ref pos") 29 | for start_idx, end_idx in zip(num_contact_switch_idx_list[:-1], num_contact_switch_idx_list[1:]): 30 | num_contact = num_contact_list[start_idx] 31 | if start_idx != np.where(num_contact_list==num_contact)[0][0]: 32 | label = None 33 | elif num_contact == 0: 34 | label = "no contact" 35 | elif num_contact == 1: 36 | label = "1 contact" 37 | elif num_contact >= 2: 38 | label = "{} contacts".format(num_contact) 39 | ax.axvspan(self.result_data_list["time"][start_idx], self.result_data_list["time"][end_idx], 40 | facecolor=num_contact_colors[num_contact], alpha=0.2, label=label) 41 | ax.set_xlabel("time [s]") 42 | ax.set_ylabel("pos [m]") 43 | ax.grid() 44 | ax.legend(loc="lower left") 45 | 46 | ax = fig.add_subplot(212) 47 | ax.plot(self.result_data_list["time"], self.result_data_list["force"], 48 | color="green", label="force") 49 | for start_idx, end_idx in zip(num_contact_switch_idx_list[:-1], num_contact_switch_idx_list[1:]): 50 | ax.axvspan(self.result_data_list["time"][start_idx], self.result_data_list["time"][end_idx], 51 | facecolor=num_contact_colors[num_contact_list[start_idx]], alpha=0.2) 52 | ax.set_xlabel("time [s]") 53 | ax.set_ylabel("force [N]") 54 | ax.grid() 55 | ax.legend(loc="upper left") 56 | 57 | plt.show() 58 | 59 | 60 | if __name__ == "__main__": 61 | result_file_path = "/tmp/TestDDPVerticalMotionResult.txt" 62 | 63 | import sys 64 | if len(sys.argv) >= 2: 65 | result_file_path = sys.argv[1] 66 | 67 | plot = PlotTestDDPVerticalMotion(result_file_path) 68 | -------------------------------------------------------------------------------- /nmpc_ddp/tests/src/TestBoxQP.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | template 11 | void solveOneQP(const Eigen::Matrix & H, 12 | const Eigen::Matrix & g, 13 | const Eigen::Matrix & lower, 14 | const Eigen::Matrix & upper, 15 | const Eigen::Matrix & x_gt) 16 | { 17 | std::shared_ptr> qp; 18 | if constexpr(VarDim == Eigen::Dynamic) 19 | { 20 | qp = std::make_shared>(g.size()); 21 | } 22 | else 23 | { 24 | qp = std::make_shared>(); 25 | } 26 | qp->config().print_level = 3; 27 | 28 | Eigen::Vector2d x_opt = qp->solve(H, g, lower, upper); 29 | EXPECT_LT((x_opt - x_gt).norm(), 1e-6) << "[TestBoxQP] QP solution is incorrect:\n" 30 | << " solution: " << x_opt.transpose() 31 | << "\n ground truth: " << x_gt.transpose() 32 | << "\n error: " << (x_opt - x_gt).norm() << std::endl; 33 | } 34 | 35 | TEST(TestBoxQP, TestFixedSize) 36 | { 37 | // Some problems are copied from 38 | // https://github.com/coin-or/qpOASES/blob/268b2f2659604df27c82aa6e32aeddb8c1d5cc7f/examples/example1b.cpp#L43-L52 39 | Eigen::Matrix2d H; 40 | H << 1.0, 0.0, 0.0, 0.5; 41 | 42 | solveOneQP<2>(H, Eigen::Vector2d(1.5, 1.0), Eigen::Vector2d(-10, -10), Eigen::Vector2d(10, 10), 43 | Eigen::Vector2d(-1.5, -2.0)); 44 | 45 | solveOneQP<2>(H, Eigen::Vector2d(1.5, 1.0), Eigen::Vector2d(0.5, -2.0), Eigen::Vector2d(5.0, 2.0), 46 | Eigen::Vector2d(0.5, -2.0)); 47 | 48 | solveOneQP<2>(H, Eigen::Vector2d(1.0, 1.5), Eigen::Vector2d(0.0, -1.0), Eigen::Vector2d(5.0, -0.5), 49 | Eigen::Vector2d(0.0, -1.0)); 50 | 51 | solveOneQP<2>(H, Eigen::Vector2d(1.5, 1.0), Eigen::Vector2d(-5.0, -1.0), Eigen::Vector2d(-2.0, 2.0), 52 | Eigen::Vector2d(-2.0, -1.0)); 53 | 54 | solveOneQP<2>(H, Eigen::Vector2d(1.0, 1.5), Eigen::Vector2d(-5.0, -10.0), Eigen::Vector2d(-2.0, 10.0), 55 | Eigen::Vector2d(-2.0, -3.0)); 56 | } 57 | 58 | TEST(TestBoxQP, TestDynamicSize) 59 | { 60 | // Some problems are copied from 61 | // https://github.com/coin-or/qpOASES/blob/268b2f2659604df27c82aa6e32aeddb8c1d5cc7f/examples/example1b.cpp#L43-L52 62 | Eigen::MatrixXd H(2, 2); 63 | H << 1.0, 0.0, 0.0, 0.5; 64 | Eigen::VectorXd g(2); 65 | Eigen::VectorXd lower(2); 66 | Eigen::VectorXd upper(2); 67 | Eigen::VectorXd x_gt(2); 68 | 69 | g << 1.5, 1.0; 70 | lower << -10, -10; 71 | upper << 10, 10; 72 | x_gt << -1.5, -2.0; 73 | solveOneQP(H, g, lower, upper, x_gt); 74 | 75 | g << 1.5, 1.0; 76 | lower << 0.5, -2.0; 77 | upper << 5.0, 2.0; 78 | x_gt << 0.5, -2.0; 79 | solveOneQP(H, g, lower, upper, x_gt); 80 | 81 | g << 1.0, 1.5; 82 | lower << 0.0, -1.0; 83 | upper << 5.0, -0.5; 84 | x_gt << 0.0, -1.0; 85 | solveOneQP(H, g, lower, upper, x_gt); 86 | 87 | g << 1.5, 1.0; 88 | lower << -5.0, -1.0; 89 | upper << -2.0, 2.0; 90 | x_gt << -2.0, -1.0; 91 | solveOneQP(H, g, lower, upper, x_gt); 92 | 93 | g << 1.0, 1.5; 94 | lower << -5.0, -10.0; 95 | upper << -2.0, 10.0; 96 | x_gt << -2.0, -3.0; 97 | solveOneQP(H, g, lower, upper, x_gt); 98 | } 99 | 100 | int main(int argc, char ** argv) 101 | { 102 | testing::InitGoogleTest(&argc, argv); 103 | return RUN_ALL_TESTS(); 104 | } 105 | -------------------------------------------------------------------------------- /nmpc_ddp/tests/src/TestDDPBipedal.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | /** \brief DDP problem for bipedal walking. 11 | 12 | State is [CoM_pos, CoM_vel]. Input is [ZMP]. 13 | Running cost is CoM_vel^2 + (ZMP - ZMP_ref)^2. 14 | Terminal cost is (CoM_pos - ZMP_ref)^2 + CoM_vel^2. 15 | */ 16 | class DDPProblemBipedal : public nmpc_ddp::DDPProblem<2, 1> 17 | { 18 | public: 19 | struct CostWeight 20 | { 21 | CostWeight() {} 22 | 23 | double running_vel = 1e-14; 24 | double running_zmp = 1e-1; 25 | double terminal_pos = 1e2; 26 | double terminal_vel = 1.0; 27 | }; 28 | 29 | public: 30 | DDPProblemBipedal(double dt, 31 | const std::function & ref_zmp_func, 32 | const std::function & omega2_func, 33 | const CostWeight & cost_weight = CostWeight()) 34 | : DDPProblem(dt), ref_zmp_func_(ref_zmp_func), omega2_func_(omega2_func), cost_weight_(cost_weight) 35 | { 36 | } 37 | 38 | virtual StateDimVector stateEq(double t, const StateDimVector & x, const InputDimVector & u) const override 39 | { 40 | return A(t) * x + B(t) * u; 41 | } 42 | 43 | virtual double runningCost(double t, const StateDimVector & x, const InputDimVector & u) const override 44 | { 45 | return cost_weight_.running_vel * 0.5 * std::pow(x[1], 2) 46 | + cost_weight_.running_zmp * 0.5 * std::pow(u[0] - ref_zmp_func_(t), 2); 47 | } 48 | 49 | virtual double terminalCost(double t, const StateDimVector & x) const override 50 | { 51 | return cost_weight_.terminal_pos * 0.5 * std::pow(x[0] - ref_zmp_func_(t), 2) 52 | + cost_weight_.terminal_vel * 0.5 * std::pow(x[1], 2); 53 | } 54 | 55 | virtual void calcStateEqDeriv(double t, 56 | const StateDimVector &, // x 57 | const InputDimVector &, // u 58 | Eigen::Ref state_eq_deriv_x, 59 | Eigen::Ref state_eq_deriv_u) const override 60 | { 61 | state_eq_deriv_x = A(t); 62 | state_eq_deriv_u = B(t); 63 | } 64 | 65 | virtual void calcStateEqDeriv(double t, 66 | const StateDimVector & x, 67 | const InputDimVector & u, 68 | Eigen::Ref state_eq_deriv_x, 69 | Eigen::Ref state_eq_deriv_u, 70 | std::vector & state_eq_deriv_xx, 71 | std::vector & state_eq_deriv_uu, 72 | std::vector & state_eq_deriv_xu) const override 73 | { 74 | calcStateEqDeriv(t, x, u, state_eq_deriv_x, state_eq_deriv_u); 75 | state_eq_deriv_xx.assign(stateDim(), StateStateDimMatrix::Zero()); 76 | state_eq_deriv_uu.assign(stateDim(), InputInputDimMatrix::Zero()); 77 | state_eq_deriv_xu.assign(stateDim(), StateInputDimMatrix::Zero()); 78 | } 79 | 80 | virtual void calcRunningCostDeriv(double t, 81 | const StateDimVector & x, 82 | const InputDimVector & u, 83 | Eigen::Ref running_cost_deriv_x, 84 | Eigen::Ref running_cost_deriv_u) const override 85 | { 86 | running_cost_deriv_x[0] = 0; 87 | running_cost_deriv_x[1] = cost_weight_.running_vel * x[1]; 88 | running_cost_deriv_u[0] = cost_weight_.running_zmp * (u[0] - ref_zmp_func_(t)); 89 | } 90 | 91 | virtual void calcRunningCostDeriv(double t, 92 | const StateDimVector & x, 93 | const InputDimVector & u, 94 | Eigen::Ref running_cost_deriv_x, 95 | Eigen::Ref running_cost_deriv_u, 96 | Eigen::Ref running_cost_deriv_xx, 97 | Eigen::Ref running_cost_deriv_uu, 98 | Eigen::Ref running_cost_deriv_xu) const override 99 | { 100 | calcRunningCostDeriv(t, x, u, running_cost_deriv_x, running_cost_deriv_u); 101 | running_cost_deriv_xx << 0, 0, 0, cost_weight_.running_vel; 102 | running_cost_deriv_uu << cost_weight_.running_zmp; 103 | running_cost_deriv_xu << 0, 0; 104 | } 105 | 106 | virtual void calcTerminalCostDeriv(double t, 107 | const StateDimVector & x, 108 | Eigen::Ref terminal_cost_deriv_x) const override 109 | { 110 | terminal_cost_deriv_x[0] = cost_weight_.terminal_pos * (x[0] - ref_zmp_func_(t)); 111 | terminal_cost_deriv_x[1] = cost_weight_.terminal_vel * x[1]; 112 | } 113 | 114 | virtual void calcTerminalCostDeriv(double t, 115 | const StateDimVector & x, 116 | Eigen::Ref terminal_cost_deriv_x, 117 | Eigen::Ref terminal_cost_deriv_xx) const override 118 | { 119 | calcTerminalCostDeriv(t, x, terminal_cost_deriv_x); 120 | terminal_cost_deriv_xx << cost_weight_.terminal_pos, 0, 0, cost_weight_.terminal_vel; 121 | } 122 | 123 | protected: 124 | StateStateDimMatrix A(double t) const 125 | { 126 | StateStateDimMatrix A; 127 | double omega2 = omega2_func_(t); 128 | A << 1 + 0.5 * dt_ * dt_ * omega2, dt_, dt_ * omega2, 1; 129 | return A; 130 | } 131 | 132 | StateInputDimMatrix B(double t) const 133 | { 134 | StateInputDimMatrix B; 135 | double omega2 = omega2_func_(t); 136 | B << -0.5 * dt_ * dt_ * omega2, -1 * dt_ * omega2; 137 | return B; 138 | } 139 | 140 | protected: 141 | std::function ref_zmp_func_; 142 | std::function omega2_func_; 143 | CostWeight cost_weight_; 144 | }; 145 | 146 | /** \brief Min-jerk function. 147 | 148 | See https://courses.shadmehrlab.org/Shortcourse/minimumjerk.pdf 149 | The function smoothly connects (0, 0) to (1, 1). 150 | Velocity and acceleration at both ends are zero. 151 | */ 152 | double minJerk(double t) 153 | { 154 | return 6 * std::pow(t, 5) + -15 * std::pow(t, 4) + 10 * std::pow(t, 3); 155 | } 156 | 157 | double minJerkSecondDeriv(double t) 158 | { 159 | return 120 * std::pow(t, 3) + -180 * std::pow(t, 2) + 60 * t; 160 | } 161 | 162 | TEST(TestDDPBipedal, TestCase1) 163 | { 164 | double dt = 0.01; // [sec] 165 | double horizon_duration = 3.0; // [sec] 166 | int horizon_steps = static_cast(horizon_duration / dt); 167 | double end_t = 20.0; // [sec] 168 | 169 | // Instantiate problem 170 | constexpr double epsilon_t = 1e-6; 171 | std::function ref_zmp_func = [&](double t) 172 | { 173 | // Add small values to avoid numerical instability at inequality bounds 174 | t += epsilon_t; 175 | if(t <= 1.5 || t >= end_t - 1.5) 176 | { 177 | return 0.0; 178 | } 179 | else 180 | { 181 | if(static_cast(std::floor((t - 1.0) / 1.0)) % 2 == 0) 182 | { 183 | return 0.15; // [m] 184 | } 185 | else 186 | { 187 | return -0.15; // [m] 188 | } 189 | } 190 | }; 191 | std::function omega2_func = [](double t) 192 | { 193 | // Add small values to avoid numerical instability at inequality bounds 194 | t += epsilon_t; 195 | double cog_pos_z_high = 1.0; // [m] 196 | double cog_pos_z_low = 0.3; // [m] 197 | double cog_pos_z = 0.0; 198 | double cog_acc_z = 0.0; 199 | if(t < 7.0) 200 | { 201 | cog_pos_z = cog_pos_z_high; 202 | } 203 | else if(t < 8.0) 204 | { 205 | double scale = cog_pos_z_low - cog_pos_z_high; 206 | cog_pos_z = scale * minJerk(t - 7.0) + cog_pos_z_high; 207 | cog_acc_z = scale * minJerkSecondDeriv(t - 7.0); 208 | } 209 | else if(t < 12.0) 210 | { 211 | cog_pos_z = cog_pos_z_low; 212 | } 213 | else if(t < 13.0) 214 | { 215 | double scale = cog_pos_z_high - cog_pos_z_low; 216 | cog_pos_z = scale * minJerk(t - 12.0) + cog_pos_z_low; 217 | cog_acc_z = scale * minJerkSecondDeriv(t - 12.0); 218 | } 219 | else 220 | { 221 | cog_pos_z = cog_pos_z_high; 222 | } 223 | constexpr double g = 9.80665; 224 | return (cog_acc_z + g) / cog_pos_z; 225 | }; 226 | auto ddp_problem = std::make_shared(dt, ref_zmp_func, omega2_func); 227 | 228 | // Instantiate solver 229 | auto ddp_solver = std::make_shared>(ddp_problem); 230 | ddp_solver->config().horizon_steps = horizon_steps; 231 | 232 | // Initialize MPC 233 | double current_t = 0; 234 | DDPProblemBipedal::StateDimVector current_x = DDPProblemBipedal::StateDimVector(0, 0); 235 | std::vector current_u_list; 236 | current_u_list.assign(horizon_steps, DDPProblemBipedal::InputDimVector::Zero()); 237 | 238 | // Run MPC loop 239 | bool first_iter = true; 240 | std::string file_path = "/tmp/TestDDPBipedalResult.txt"; 241 | std::ofstream ofs(file_path); 242 | ofs << "time com_pos com_vel planned_zmp ref_zmp omega^2 iter" << std::endl; 243 | while(current_t < end_t) 244 | { 245 | // Solve 246 | ddp_solver->solve(current_t, current_x, current_u_list); 247 | if(first_iter) 248 | { 249 | first_iter = false; 250 | ddp_solver->dumpTraceDataList("/tmp/TestDDPBipedalTraceData.txt"); 251 | } 252 | 253 | // Check ZMP 254 | double planned_zmp = ddp_solver->controlData().u_list[0][0]; 255 | double ref_zmp = ref_zmp_func(current_t); 256 | EXPECT_LT(std::abs(planned_zmp - ref_zmp), 1e-2); 257 | 258 | // Dump 259 | ofs << current_t << " " << ddp_solver->controlData().x_list[0].transpose() << " " << planned_zmp << " " << ref_zmp 260 | << " " << omega2_func(current_t) << " " << ddp_solver->traceDataList().back().iter << std::endl; 261 | 262 | // Update to next step 263 | current_t += dt; 264 | current_x = ddp_solver->controlData().x_list[1]; 265 | current_u_list = ddp_solver->controlData().u_list; 266 | current_u_list.erase(current_u_list.begin()); 267 | current_u_list.push_back(current_u_list.back()); 268 | } 269 | 270 | // Check final CoM 271 | double ref_zmp = ref_zmp_func(current_t); 272 | EXPECT_LT(std::abs(current_x[0] - ref_zmp), 1e-2); 273 | EXPECT_LT(std::abs(current_x[1]), 1e-2); 274 | 275 | std::cout << "Run the following commands in gnuplot:\n" 276 | << " set key autotitle columnhead\n" 277 | << " set key noenhanced\n" 278 | << " plot \"" << file_path << "\" u 1:2 w lp, \"\" u 1:4 w lp, \"\" u 1:5 w l lw 3\n"; 279 | } 280 | 281 | int main(int argc, char ** argv) 282 | { 283 | testing::InitGoogleTest(&argc, argv); 284 | return RUN_ALL_TESTS(); 285 | } 286 | -------------------------------------------------------------------------------- /nmpc_ddp/tests/src/TestDDPVerticalMotion.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | /** \brief Smooth absolute function. (also known as Pseudo-Huber) 11 | 12 | See https://en.wikipedia.org/wiki/Huber_loss#Pseudo-Huber_loss_function 13 | */ 14 | Eigen::VectorXd smoothAbs(const Eigen::VectorXd & v, double scale_factor = 1.0) 15 | { 16 | return (v.array().square() + std::pow(scale_factor, 2)).sqrt() - scale_factor; 17 | } 18 | 19 | /** \brief First-order derivative of smooth absolute function. */ 20 | Eigen::VectorXd smoothAbsDeriv(const Eigen::VectorXd & v, double scale_factor = 1.0) 21 | { 22 | return v.cwiseProduct((v.array().square() + std::pow(scale_factor, 2)).rsqrt().matrix()); 23 | } 24 | 25 | /** \brief DDP problem for vertical motion. 26 | 27 | State is [pos_z, vel_z]. Input is [force_z]. 28 | Running cost is sum of the respective quadratic terms of state and input. 29 | Terminal cost is quadratic term of state. 30 | */ 31 | class DDPProblemVerticalMotion : public nmpc_ddp::DDPProblem<2, Eigen::Dynamic> 32 | { 33 | public: 34 | struct CostWeight 35 | { 36 | CostWeight() 37 | { 38 | running_x << 1.0, 1e-3; 39 | running_u = 1e-4; 40 | terminal_x << 1.0, 1e-3; 41 | } 42 | 43 | StateDimVector running_x; 44 | double running_u; 45 | StateDimVector terminal_x; 46 | }; 47 | 48 | public: 49 | DDPProblemVerticalMotion(double dt, 50 | const std::function & ref_pos_func, 51 | const CostWeight & cost_weight = CostWeight()) 52 | : DDPProblem(dt), ref_pos_func_(ref_pos_func), cost_weight_(cost_weight) 53 | { 54 | } 55 | 56 | using DDPProblem::inputDim; 57 | 58 | virtual int inputDim(double t) const override 59 | { 60 | // Add small values to avoid numerical instability at inequality bounds 61 | constexpr double epsilon_t = 1e-6; 62 | t += epsilon_t; 63 | if(2.0 < t && t < 3.0) 64 | { 65 | return 2; 66 | } 67 | else if(4.5 < t && t < 5.0) 68 | { 69 | return 0; 70 | } 71 | else 72 | { 73 | return 1; 74 | } 75 | } 76 | 77 | virtual StateDimVector stateEq(double, // t 78 | const StateDimVector & x, 79 | const InputDimVector & u) const override 80 | { 81 | StateDimVector x_dot; 82 | x_dot << x[1], u.sum() / mass_ - g_; 83 | return x + dt_ * x_dot; 84 | } 85 | 86 | virtual double runningCost(double t, const StateDimVector & x, const InputDimVector & u) const override 87 | { 88 | StateDimVector ref_x; 89 | ref_x << ref_pos_func_(t), 0; 90 | double cost_x = 0.5 * cost_weight_.running_x.dot((x - ref_x).cwiseAbs2()); 91 | double cost_u; 92 | if(use_smooth_abs_) 93 | { 94 | cost_u = 0.5 * cost_weight_.running_u * smoothAbs(u).squaredNorm(); 95 | } 96 | else 97 | { 98 | cost_u = 0.5 * cost_weight_.running_u * u.squaredNorm(); 99 | } 100 | return cost_x + cost_u; 101 | } 102 | 103 | virtual double terminalCost(double t, const StateDimVector & x) const override 104 | { 105 | StateDimVector ref_x; 106 | ref_x << ref_pos_func_(t), 0; 107 | return 0.5 * cost_weight_.terminal_x.dot((x - ref_x).cwiseAbs2()); 108 | } 109 | 110 | virtual void calcStateEqDeriv(double, // t 111 | const StateDimVector &, // x 112 | const InputDimVector &, // u 113 | Eigen::Ref state_eq_deriv_x, 114 | Eigen::Ref state_eq_deriv_u) const override 115 | { 116 | state_eq_deriv_x << 0, 1, 0, 0; 117 | state_eq_deriv_x *= dt_; 118 | state_eq_deriv_x.diagonal().array() += 1.0; 119 | 120 | state_eq_deriv_u.row(0).setZero(); 121 | state_eq_deriv_u.row(1).setConstant(1.0 / mass_); 122 | state_eq_deriv_u *= dt_; 123 | } 124 | 125 | virtual void calcStateEqDeriv(double t, 126 | const StateDimVector & x, 127 | const InputDimVector & u, 128 | Eigen::Ref state_eq_deriv_x, 129 | Eigen::Ref state_eq_deriv_u, 130 | std::vector & state_eq_deriv_xx, 131 | std::vector & state_eq_deriv_uu, 132 | std::vector & state_eq_deriv_xu) const override 133 | { 134 | calcStateEqDeriv(t, x, u, state_eq_deriv_x, state_eq_deriv_u); 135 | 136 | if(state_eq_deriv_xx.size() != stateDim() || state_eq_deriv_uu.size() != stateDim() 137 | || state_eq_deriv_xu.size() != stateDim()) 138 | { 139 | throw std::runtime_error("Vector size should be " + std::to_string(stateDim()) + " but " 140 | + std::to_string(state_eq_deriv_xx.size())); 141 | } 142 | for(int i = 0; i < stateDim(); i++) 143 | { 144 | state_eq_deriv_xx[i].setZero(); 145 | state_eq_deriv_uu[i].setZero(); 146 | state_eq_deriv_xu[i].setZero(); 147 | } 148 | } 149 | 150 | virtual void calcRunningCostDeriv(double t, 151 | const StateDimVector & x, 152 | const InputDimVector & u, 153 | Eigen::Ref running_cost_deriv_x, 154 | Eigen::Ref running_cost_deriv_u) const override 155 | { 156 | StateDimVector ref_x; 157 | ref_x << ref_pos_func_(t), 0; 158 | 159 | running_cost_deriv_x = cost_weight_.running_x.cwiseProduct(x - ref_x); 160 | 161 | if(use_smooth_abs_) 162 | { 163 | running_cost_deriv_u = cost_weight_.running_u * smoothAbsDeriv(u).cwiseProduct(smoothAbs(u)); 164 | } 165 | else 166 | { 167 | running_cost_deriv_u = cost_weight_.running_u * u; 168 | } 169 | } 170 | 171 | virtual void calcRunningCostDeriv(double t, 172 | const StateDimVector & x, 173 | const InputDimVector & u, 174 | Eigen::Ref running_cost_deriv_x, 175 | Eigen::Ref running_cost_deriv_u, 176 | Eigen::Ref running_cost_deriv_xx, 177 | Eigen::Ref running_cost_deriv_uu, 178 | Eigen::Ref running_cost_deriv_xu) const override 179 | { 180 | StateDimVector ref_x; 181 | ref_x << ref_pos_func_(t), 0; 182 | 183 | running_cost_deriv_x = cost_weight_.running_x.cwiseProduct(x - ref_x); 184 | 185 | running_cost_deriv_xx = cost_weight_.running_x.asDiagonal(); 186 | running_cost_deriv_xu.setZero(); 187 | 188 | if(use_smooth_abs_) 189 | { 190 | Eigen::VectorXd smooth_abs_deriv = smoothAbsDeriv(u); 191 | running_cost_deriv_u = cost_weight_.running_u * smooth_abs_deriv.cwiseProduct(smoothAbs(u)); 192 | running_cost_deriv_uu = cost_weight_.running_u * smooth_abs_deriv.cwiseAbs2().asDiagonal(); 193 | } 194 | else 195 | { 196 | running_cost_deriv_u = cost_weight_.running_u * u; 197 | running_cost_deriv_uu.setIdentity(); 198 | running_cost_deriv_uu *= cost_weight_.running_u; 199 | } 200 | } 201 | 202 | virtual void calcTerminalCostDeriv(double t, 203 | const StateDimVector & x, 204 | Eigen::Ref terminal_cost_deriv_x) const override 205 | { 206 | StateDimVector ref_x; 207 | ref_x << ref_pos_func_(t), 0; 208 | 209 | terminal_cost_deriv_x = cost_weight_.terminal_x.cwiseProduct(x - ref_x); 210 | } 211 | 212 | virtual void calcTerminalCostDeriv(double t, 213 | const StateDimVector & x, 214 | Eigen::Ref terminal_cost_deriv_x, 215 | Eigen::Ref terminal_cost_deriv_xx) const override 216 | { 217 | StateDimVector ref_x; 218 | ref_x << ref_pos_func_(t), 0; 219 | 220 | terminal_cost_deriv_x = cost_weight_.terminal_x.cwiseProduct(x - ref_x); 221 | terminal_cost_deriv_xx = cost_weight_.terminal_x.asDiagonal(); 222 | } 223 | 224 | protected: 225 | static constexpr double g_ = 9.80665; // [m/s^2] 226 | std::function ref_pos_func_; 227 | CostWeight cost_weight_; 228 | double mass_ = 1.0; // [kg] 229 | 230 | // I encountered a problem with fluctuating pos when the number of contact points went from one to more than one. 231 | // Considering that this is due to the nonlinear (i.e., quadratic) term of force in running cost, I introduced 232 | // a linear term of force instead of quadratic term as running cost. However, there was no improvement in this problem. 233 | bool use_smooth_abs_ = false; 234 | }; 235 | 236 | void test(bool with_constraint) 237 | { 238 | double dt = 0.01; // [sec] 239 | double horizon_duration = 3.0; // [sec] 240 | int horizon_steps = static_cast(horizon_duration / dt); 241 | double end_t = 10.0; // [sec] 242 | 243 | // Instantiate problem 244 | constexpr double epsilon_t = 1e-6; 245 | std::function ref_pos_func = [&](double t) 246 | { 247 | // Add small values to avoid numerical instability at inequality bounds 248 | t += epsilon_t; 249 | if(t < 8.0) 250 | { 251 | return 1.0; // [m] 252 | } 253 | else 254 | { 255 | return 0.0; // [m] 256 | } 257 | }; 258 | auto ddp_problem = std::make_shared(dt, ref_pos_func); 259 | 260 | // Instantiate solver 261 | auto ddp_solver = std::make_shared>(ddp_problem); 262 | ddp_solver->setInputLimitsFunc( 263 | [&](double t) -> std::array 264 | { 265 | std::array limits; 266 | int input_dim = ddp_problem->inputDim(t); 267 | limits[0].setConstant(input_dim, 0.0); 268 | limits[1].setConstant(input_dim, 30.0); 269 | return limits; 270 | }); 271 | ddp_solver->config().with_input_constraint = with_constraint; 272 | ddp_solver->config().horizon_steps = horizon_steps; 273 | ddp_solver->config().initial_lambda = 1e-6; 274 | 275 | // Initialize MPC 276 | double current_t = 0; 277 | DDPProblemVerticalMotion::StateDimVector current_x = DDPProblemVerticalMotion::StateDimVector(1.2, 0); 278 | std::vector current_u_list; 279 | for(int i = 0; i < horizon_steps; i++) 280 | { 281 | double t = current_t + i * dt; 282 | current_u_list.push_back(DDPProblemVerticalMotion::InputDimVector::Zero(ddp_problem->inputDim(t))); 283 | } 284 | 285 | // Run MPC loop 286 | bool first_iter = true; 287 | std::string file_path = "/tmp/TestDDPVerticalMotionResult.txt"; 288 | std::ofstream ofs(file_path); 289 | ofs << "time pos vel force ref_pos num_contact iter" << std::endl; 290 | while(current_t < end_t) 291 | { 292 | // Solve 293 | ddp_solver->solve(current_t, current_x, current_u_list); 294 | if(first_iter) 295 | { 296 | first_iter = false; 297 | ddp_solver->dumpTraceDataList("/tmp/TestDDPVerticalMotionTraceData.txt"); 298 | } 299 | ddp_solver->config().max_iter = 3; // Set max_iter from second loop iteration 300 | 301 | // Check pos 302 | double planned_pos = ddp_solver->controlData().x_list[0][0]; 303 | double ref_pos = ref_pos_func(current_t); 304 | EXPECT_LT(std::abs(planned_pos - ref_pos), 1.0); 305 | 306 | // Dump 307 | ofs << current_t << " " << ddp_solver->controlData().x_list[0].transpose() << " " 308 | << ddp_solver->controlData().u_list[0].sum() << " " << ref_pos << " " 309 | << ddp_solver->controlData().u_list[0].size() << " " << ddp_solver->traceDataList().back().iter << std::endl; 310 | 311 | // Update to next step 312 | current_x = ddp_solver->controlData().x_list[1]; 313 | current_u_list = ddp_solver->controlData().u_list; 314 | current_u_list.erase(current_u_list.begin()); 315 | double terminal_t = current_t + horizon_steps * dt; 316 | int terminal_input_dim = ddp_problem->inputDim(terminal_t); 317 | if(current_u_list.back().size() == terminal_input_dim) 318 | { 319 | current_u_list.push_back(current_u_list.back()); 320 | } 321 | else 322 | { 323 | current_u_list.push_back(DDPProblemVerticalMotion::InputDimVector::Zero(terminal_input_dim)); 324 | } 325 | current_t += dt; 326 | } 327 | 328 | // Check final pos 329 | double ref_pos = ref_pos_func(current_t); 330 | EXPECT_LT(std::abs(current_x[0] - ref_pos), 1e-2); 331 | EXPECT_LT(std::abs(current_x[1]), 1e-2); 332 | 333 | std::cout << "Run the following commands in gnuplot:\n" 334 | << " set key autotitle columnhead\n" 335 | << " set key noenhanced\n" 336 | << " plot \"" << file_path << "\" u 1:2 w lp, \"\" u 1:5 w l lw 3\n"; 337 | } 338 | 339 | TEST(TestDDPVerticalMotion, WithConstraint) 340 | { 341 | test(true); 342 | } 343 | 344 | TEST(TestDDPVerticalMotion, WithoutConstraint) 345 | { 346 | test(false); 347 | } 348 | 349 | int main(int argc, char ** argv) 350 | { 351 | testing::InitGoogleTest(&argc, argv); 352 | return RUN_ALL_TESTS(); 353 | } 354 | -------------------------------------------------------------------------------- /nmpc_ddp/tests/test/TestDDPCartPole.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | 11 | no_exit: $(arg no_exit) 12 | control: 13 | horizon_dt: 0.01 # [sec] 14 | horizon_duration: 2.0 # [sec] 15 | mpc_dt: 0.004 # [sec] 16 | sim_dt: 0.002 # [sec] 17 | param: 18 | cart_mass: 1.0 19 | pole_mass: 0.5 20 | pole_length: 2.0 21 | cost: 22 | running_x: [0.1, 1.0, 0.01, 0.1] 23 | running_u: [0.01] 24 | terminal_x: [0.1, 1.0, 0.01, 0.1] 25 | 26 | 27 | 28 | 30 | 31 | 33 | 34 | -------------------------------------------------------------------------------- /nmpc_fmpc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | set(PROJECT_NAME nmpc_fmpc) 4 | set(PROJECT_GENERATED_HEADERS_SKIP_DEPRECATED ON) 5 | set(PROJECT_GENERATED_HEADERS_SKIP_CONFIG ON) 6 | set(PROJECT_GENERATED_HEADERS_SKIP_WARNING ON) 7 | set(PROJECT_URL https://github.com/isri-aist/NMPC) 8 | set(PROJECT_DESCRIPTION "") 9 | set(CMAKE_CXX_STANDARD 17) 10 | set(PROJECT_USE_CMAKE_EXPORT TRUE) 11 | set(CXX_DISABLE_WERROR ON) 12 | option(INSTALL_DOCUMENTATION "Generate and install the documentation" OFF) 13 | 14 | include(../cmake/base.cmake) 15 | project(nmpc_fmpc LANGUAGES CXX) 16 | 17 | # Options 18 | option(OPTIMIZE_FOR_NATIVE "Enable -march=native" OFF) 19 | 20 | if(NOT DEFINED NMPC_STANDALONE) 21 | set(NMPC_STANDALONE OFF) 22 | endif() 23 | 24 | if(NOT NMPC_STANDALONE) 25 | find_package(nmpc_ddp REQUIRED) 26 | 27 | # Eigen 28 | find_package(Eigen3 REQUIRED) 29 | include_directories(${EIGEN3_INCLUDE_DIR}) 30 | 31 | ament_package() 32 | else() 33 | set(BUILD_TESTING OFF) 34 | add_project_dependency(nmpc_ddp REQUIRED) 35 | endif() 36 | 37 | add_library(nmpc_fmpc INTERFACE) 38 | target_compile_features(nmpc_fmpc INTERFACE cxx_std_17) 39 | 40 | target_include_directories(nmpc_fmpc INTERFACE $ $) 41 | if(TARGET nmpc_ddp::nmpc_ddp) 42 | target_link_libraries(nmpc_fmpc INTERFACE nmpc_ddp::nmpc_ddp) 43 | endif() 44 | 45 | if(OPTIMIZE_FOR_NATIVE) 46 | target_compile_options(nmpc_fmpc INTERFACE -march=native) 47 | endif() 48 | 49 | install(TARGETS nmpc_fmpc EXPORT "${TARGETS_EXPORT_NAME}") 50 | install(DIRECTORY include/nmpc_fmpc DESTINATION "${INCLUDE_INSTALL_DIR}") 51 | 52 | if(BUILD_TESTING) 53 | add_subdirectory(tests) 54 | endif() 55 | 56 | if(INSTALL_DOCUMENTATION) 57 | add_subdirectory(doc) 58 | endif() 59 | -------------------------------------------------------------------------------- /nmpc_fmpc/README.md: -------------------------------------------------------------------------------- 1 | # nmpc_fmpc 2 | FMPC: Fast non-linear model predictive control (NMPC) combining the direct multiple shooting (DMS) method, the primal-dual interior point (PDIP) method, and Riccati recursion (RR) 3 | 4 | [![CI-standalone](https://github.com/isri-aist/NMPC/actions/workflows/ci-standalone.yaml/badge.svg)](https://github.com/isri-aist/NMPC/actions/workflows/ci-standalone.yaml) 5 | [![CI-catkin](https://github.com/isri-aist/NMPC/actions/workflows/ci-catkin.yaml/badge.svg)](https://github.com/isri-aist/NMPC/actions/workflows/ci-catkin.yaml) 6 | [![Documentation](https://img.shields.io/badge/doxygen-online-brightgreen?logo=read-the-docs&style=flat)](https://isri-aist.github.io/NMPC/nmpc_fmpc/index.html) 7 | 8 | ## Features 9 | - C++ header-only library 10 | - Supports inequality constraints on state and control input 11 | - Treats the dimensions of state, control input, and inequality constraints as template parameters 12 | - Supports time-varying dimensions of control input and inequality constraints 13 | 14 | ## Install 15 | See [here](https://isri-aist.github.io/NMPC/doc/Install). 16 | 17 | ## Technical details 18 | See the following for a detailed algorithm. 19 | - S Katayama. Fast model predictive control of robotic systems with rigid contacts. Ph.D. thesis (section 2.2), Kyoto University, 2022. 20 | 21 | ## Examples 22 | Make sure that it is built with `--catkin-make-args tests` option. 23 | 24 | ### [Van der Pol oscillator](tests/src/TestFmpcOscillator.cpp) 25 | Control the [Van der Pol oscillator](https://web.casadi.org/docs/#a-simple-test-problem). 26 | System is non-linear and time-invariant. Constraints are imposed on the state and control input. 27 | ```bash 28 | $ rosrun nmpc_fmpc TestFmpcOscillator 29 | ``` 30 | ![TestFmpcOscillator](doc/images/TestFmpcOscillator.png) 31 | 32 | 33 | ### [Cart-pole](tests/src/TestFmpcCartPole.cpp) 34 | Controlling cart-pole (also known as inverted pendulum). 35 | System is non-linear. 36 | This is an example of a realistic setup where the control and simulation have different time periods. 37 | ```bash 38 | # 10-second simulation 39 | $ rostest nmpc_fmpc TestFmpcCartPole.test --text 40 | # Endless simulation 41 | $ rostest nmpc_fmpc TestFmpcCartPole.test no_exit:=true --text 42 | ``` 43 | ![TestFmpcCartPole](doc/images/TestFmpcCartPole.png) 44 | -------------------------------------------------------------------------------- /nmpc_fmpc/doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Doxygen REQUIRED) 2 | 3 | if(DOXYGEN_FOUND) 4 | set(DOXYFILE_PATH ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) 5 | 6 | add_custom_target(nmpc_fmpc_doc ALL 7 | ${DOXYGEN_EXECUTABLE} ${DOXYFILE_PATH} 8 | DEPENDS ${DOXYFILE_PATH} 9 | COMMENT "Generating Doxygen documentation" 10 | ) 11 | endif() 12 | -------------------------------------------------------------------------------- /nmpc_fmpc/doc/images/TestFmpcCartPole.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isri-aist/NMPC/80bbe5777487f8c4ec44e0256c4e2a77a2be82ea/nmpc_fmpc/doc/images/TestFmpcCartPole.png -------------------------------------------------------------------------------- /nmpc_fmpc/doc/images/TestFmpcOscillator.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/isri-aist/NMPC/80bbe5777487f8c4ec44e0256c4e2a77a2be82ea/nmpc_fmpc/doc/images/TestFmpcOscillator.png -------------------------------------------------------------------------------- /nmpc_fmpc/include/nmpc_fmpc/FmpcProblem.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace nmpc_fmpc 8 | { 9 | /** \brief Fast MPC problem. 10 | \tparam StateDim state dimension (fixed only) 11 | \tparam InputDim input dimension (fixed or dynamic (i.e., Eigen::Dynamic)) 12 | \tparam IneqDim inequality dimension (fixed or dynamic (i.e., Eigen::Dynamic)) 13 | */ 14 | template 15 | class FmpcProblem : public nmpc_ddp::DDPProblem 16 | { 17 | public: 18 | /** \brief Type of vector of state dimension. */ 19 | using StateDimVector = typename nmpc_ddp::DDPProblem::StateDimVector; 20 | 21 | /** \brief Type of vector of input dimension. */ 22 | using InputDimVector = typename nmpc_ddp::DDPProblem::InputDimVector; 23 | 24 | /** \brief Type of vector of inequality dimension. */ 25 | using IneqDimVector = Eigen::Matrix; 26 | 27 | /** \brief Type of matrix of state x state dimension. */ 28 | using StateStateDimMatrix = typename nmpc_ddp::DDPProblem::StateStateDimMatrix; 29 | 30 | /** \brief Type of matrix of input x input dimension. */ 31 | using InputInputDimMatrix = typename nmpc_ddp::DDPProblem::InputInputDimMatrix; 32 | 33 | /** \brief Type of matrix of state x input dimension. */ 34 | using StateInputDimMatrix = typename nmpc_ddp::DDPProblem::StateInputDimMatrix; 35 | 36 | /** \brief Type of matrix of input x state dimension. */ 37 | using InputStateDimMatrix = typename nmpc_ddp::DDPProblem::InputStateDimMatrix; 38 | 39 | /** \brief Type of matrix of inequality x state dimension. */ 40 | using IneqStateDimMatrix = Eigen::Matrix; 41 | 42 | /** \brief Type of matrix of inequality x input dimension. */ 43 | using IneqInputDimMatrix = Eigen::Matrix; 44 | 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | /** \brief Constructor. 49 | \param dt discretization timestep [sec] 50 | */ 51 | FmpcProblem(double dt) : nmpc_ddp::DDPProblem(dt) 52 | { 53 | // Check dimension 54 | static_assert(IneqDim >= 0 || IneqDim == Eigen::Dynamic, 55 | "[FMPC] Template param IneqDim should be non-negative or Eigen::Dynamic."); 56 | } 57 | 58 | /** \brief Gets the inequality dimension. 59 | \note If inequality dimension is dynamic, this must not be called. Instead, ineqDim(t) must be called passing time 60 | as a parameter. 61 | */ 62 | inline virtual int ineqDim() const 63 | { 64 | if constexpr(IneqDim == Eigen::Dynamic) 65 | { 66 | throw std::runtime_error("Since ineq dimension is dynamic, time must be passed to ineqDim()."); 67 | } 68 | return IneqDim; 69 | } 70 | 71 | /** \brief Gets the inequality dimension. 72 | \param t time 73 | \note If inequality dimension is dynamic, this must be overridden. 74 | */ 75 | inline virtual int ineqDim(double // t 76 | ) const 77 | { 78 | if constexpr(IneqDim == Eigen::Dynamic) 79 | { 80 | throw std::runtime_error("ineqDim(t) must be overridden if ineq dimension is dynamic."); 81 | } 82 | else 83 | { 84 | return ineqDim(); 85 | } 86 | } 87 | 88 | /** \brief Calculate inequality constraints. 89 | \param t time [sec] 90 | \param x current state 91 | \param u current input 92 | \returns inequality constraints that must be less than or equal to zero 93 | */ 94 | virtual IneqDimVector ineqConst(double t, const StateDimVector & x, const InputDimVector & u) const = 0; 95 | 96 | /** \brief Calculate first-order derivatives of inequality constraints. 97 | \param t time [sec] 98 | \param x state 99 | \param u input 100 | \param ineq_const_deriv_x first-order derivative of inequality constraints w.r.t. state 101 | \param ineq_const_deriv_u first-order derivative of inequality constraints w.r.t. input 102 | */ 103 | virtual void calcIneqConstDeriv(double t, 104 | const StateDimVector & x, 105 | const InputDimVector & u, 106 | Eigen::Ref ineq_const_deriv_x, 107 | Eigen::Ref ineq_const_deriv_u) const = 0; 108 | 109 | using nmpc_ddp::DDPProblem::calcStateEqDeriv; 110 | 111 | private: 112 | /** \brief Calculate first-order and second-order derivatives of discrete state equation. 113 | \param t time [sec] 114 | \param x state 115 | \param u input 116 | \param state_eq_deriv_x first-order derivative of state equation w.r.t. state 117 | \param state_eq_deriv_u first-order derivative of state equation w.r.t. input 118 | \param state_eq_deriv_xx second-order derivative of state equation w.r.t. state 119 | \param state_eq_deriv_uu second-order derivative of state equation w.r.t. input 120 | \param state_eq_deriv_xu second-order derivative of state equation w.r.t. state and input 121 | */ 122 | inline virtual void calcStateEqDeriv(double, // t 123 | const StateDimVector &, // x 124 | const InputDimVector &, // u 125 | Eigen::Ref, // state_eq_deriv_x 126 | Eigen::Ref, // state_eq_deriv_u 127 | std::vector &, // state_eq_deriv_xx 128 | std::vector &, // state_eq_deriv_uu 129 | std::vector & // state_eq_deriv_xu 130 | ) const override 131 | { 132 | throw std::runtime_error("[FMPC] Second-order derivatives of state equation is not used."); 133 | } 134 | }; 135 | } // namespace nmpc_fmpc 136 | -------------------------------------------------------------------------------- /nmpc_fmpc/include/nmpc_fmpc/FmpcSolver.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace nmpc_fmpc 12 | { 13 | /** \brief FMPC solver. 14 | \tparam StateDim state dimension 15 | \tparam InputDim input dimension 16 | \tparam IneqDim inequality dimension 17 | 18 | See the following for a detailed algorithm. 19 | - S Katayama. Fast model predictive control of robotic systems with rigid contacts. Ph.D. thesis (section 2.2), 20 | Kyoto University, 2022. 21 | */ 22 | template 23 | class FmpcSolver 24 | { 25 | public: 26 | /** \brief Type of vector of state dimension. */ 27 | using StateDimVector = typename FmpcProblem::StateDimVector; 28 | 29 | /** \brief Type of vector of input dimension. */ 30 | using InputDimVector = typename FmpcProblem::InputDimVector; 31 | 32 | /** \brief Type of vector of inequality dimension. */ 33 | using IneqDimVector = typename FmpcProblem::IneqDimVector; 34 | 35 | /** \brief Type of matrix of state x state dimension. */ 36 | using StateStateDimMatrix = typename FmpcProblem::StateStateDimMatrix; 37 | 38 | /** \brief Type of matrix of input x input dimension. */ 39 | using InputInputDimMatrix = typename FmpcProblem::InputInputDimMatrix; 40 | 41 | /** \brief Type of matrix of state x input dimension. */ 42 | using StateInputDimMatrix = typename FmpcProblem::StateInputDimMatrix; 43 | 44 | /** \brief Type of matrix of input x state dimension. */ 45 | using InputStateDimMatrix = typename FmpcProblem::InputStateDimMatrix; 46 | 47 | /** \brief Type of matrix of inequality x state dimension. */ 48 | using IneqStateDimMatrix = typename FmpcProblem::IneqStateDimMatrix; 49 | 50 | /** \brief Type of matrix of inequality x input dimension. */ 51 | using IneqInputDimMatrix = typename FmpcProblem::IneqInputDimMatrix; 52 | 53 | /** \brief Type of matrix of inequality x inequality dimension. */ 54 | using IneqIneqDimMatrix = Eigen::Matrix; 55 | 56 | public: 57 | /*! \brief Configuration. */ 58 | struct Configuration 59 | { 60 | //! Print level (0: no print, 1: print only important, 2: print verbose, 3: print very verbose) 61 | int print_level = 1; 62 | 63 | //! Number of steps in horizon 64 | int horizon_steps = 100; 65 | 66 | //! Maximum iteration of optimization loop 67 | int max_iter = 10; 68 | 69 | //! Threshold of KKT condition error 70 | double kkt_error_thre = 1e-4; 71 | 72 | //! Whether to check NaN 73 | bool check_nan = true; 74 | 75 | //! Whether to initialize complementarity variables 76 | bool init_complementary_variable = false; 77 | 78 | //! Whether to update barrier parameter 79 | bool update_barrier_eps = true; 80 | 81 | //! Whether to break if LLT decomposition fails 82 | bool break_if_llt_fails = false; 83 | 84 | //! Whether to enable line search 85 | bool enable_line_search = false; 86 | 87 | //! Whether to calculate the scale of constraint errors in the merit function from Lagrange multipliers 88 | bool merit_const_scale_from_lagrange_multipliers = false; 89 | }; 90 | 91 | /*! \brief Result status. */ 92 | enum class Status 93 | { 94 | //! Uninitialized 95 | Uninitialized = 0, 96 | 97 | //! Succeeded 98 | Succeeded = 1, 99 | 100 | //! Error in forward 101 | ErrorInForward = 2, 102 | 103 | //! Error in backward 104 | ErrorInBackward = 3, 105 | 106 | //! Error in update 107 | ErrorInUpdate = 4, 108 | 109 | //! Maximum iteration reached 110 | MaxIterationReached = 5, 111 | 112 | //! Iteration continued (used internally only) 113 | IterationContinued = 6 114 | }; 115 | 116 | /*! \brief Optimization variables. */ 117 | struct Variable 118 | { 119 | /** \brief Constructor. 120 | \param horizon_steps number of steps in horizon 121 | */ 122 | Variable(int _horizon_steps = 0); 123 | 124 | /** \brief Reset variables. 125 | \param _x x 126 | \param _u u 127 | \param _lambda lambda 128 | \param _s s (must be non-negative) 129 | \param _nu nu (must be non-negative) 130 | */ 131 | void reset(double _x, double _u, double _lambda, double _s, double _nu); 132 | 133 | /** \brief Check whether NaN or infinity is containd. 134 | \return whether NaN or infinity is containd 135 | */ 136 | bool containsNaN() const; 137 | 138 | //! Number of steps in horizon 139 | int horizon_steps; 140 | 141 | //! Sequence of state (x[0], ..., x[N-1], x[N]) 142 | std::vector x_list; 143 | 144 | //! Sequence of input (u[0], ..., u[N-1]) 145 | std::vector u_list; 146 | 147 | //! Sequence of Lagrange multipliers of equality constraints (lambda[0], ..., lambda[N-1], lambda[N]) 148 | std::vector lambda_list; 149 | 150 | //! Sequence of slack variables of inequality constraints (s[0], ..., s[N-1]) 151 | std::vector s_list; 152 | 153 | //! Sequence of Lagrange multipliers of inequality constraints (nu[0], ..., nu[N-1]) 154 | std::vector nu_list; 155 | 156 | //! Print level (0: no print, 1: print only important, 2: print verbose, 3: print very verbose) 157 | int print_level = 1; 158 | }; 159 | 160 | /*! \brief Coefficients of linearized KKT condition. */ 161 | struct Coefficient 162 | { 163 | /** \brief Constructor. 164 | \param state_dim state dimension 165 | \param input_dim input dimension 166 | \param ineq_dim inequality dimension 167 | */ 168 | Coefficient(int state_dim, int input_dim, int ineq_dim); 169 | 170 | /** \brief Constructor for terminal coefficient. 171 | \param state_dim state dimension 172 | */ 173 | Coefficient(int state_dim); 174 | 175 | /** \brief Check whether NaN or infinity is containd. 176 | \return whether NaN or infinity is containd 177 | */ 178 | bool containsNaN() const; 179 | 180 | //! First-order derivative of state equation w.r.t. state 181 | StateStateDimMatrix A; 182 | 183 | //! First-order derivative of state equation w.r.t. input 184 | StateInputDimMatrix B; 185 | 186 | //! First-order derivative of inequality constraints w.r.t. state 187 | IneqStateDimMatrix C; 188 | 189 | //! First-order derivative of inequality constraints w.r.t. input 190 | IneqInputDimMatrix D; 191 | 192 | //! First-order derivative of running cost w.r.t. state 193 | StateDimVector Lx; 194 | 195 | //! First-order derivative of running cost w.r.t. input 196 | InputDimVector Lu; 197 | 198 | //! Second-order derivative of running cost w.r.t. state 199 | StateStateDimMatrix Lxx; 200 | 201 | //! Second-order derivative of running cost w.r.t. input 202 | InputInputDimMatrix Luu; 203 | 204 | //! Second-order derivative of running cost w.r.t. state and input 205 | StateInputDimMatrix Lxu; 206 | 207 | //! Linearization offsets 208 | //! @{ 209 | StateDimVector x_bar; 210 | IneqDimVector g_bar; 211 | StateDimVector Lx_bar; 212 | InputDimVector Lu_bar; 213 | //! @} 214 | 215 | //! Feedforward term for input 216 | InputDimVector k; 217 | 218 | //! Feedback gain for input w.r.t. state error 219 | InputStateDimMatrix K; 220 | 221 | //! Offset vector for lambda calculation 222 | StateDimVector s; 223 | 224 | //! Coefficient matrix for lambda calculation 225 | StateStateDimMatrix P; 226 | 227 | //! Print level (0: no print, 1: print only important, 2: print verbose, 3: print very verbose) 228 | int print_level = 1; 229 | }; 230 | 231 | /*! \brief Data to trace optimization loop. */ 232 | struct TraceData 233 | { 234 | //! Iteration of optimization loop 235 | int iter = 0; 236 | 237 | //! KKT condition error 238 | double kkt_error = 0; 239 | 240 | //! Duration to calculate coefficients [msec] 241 | double duration_coeff = 0; 242 | 243 | //! Duration to process backward pass [msec] 244 | double duration_backward = 0; 245 | 246 | //! Duration to process forward pass [msec] 247 | double duration_forward = 0; 248 | 249 | //! Duration to update variables [msec] 250 | double duration_update = 0; 251 | }; 252 | 253 | /*! \brief Data of computation duration. */ 254 | struct ComputationDuration 255 | { 256 | //! Duration to solve [msec] 257 | double solve = 0; 258 | 259 | //! Duration to setup (included in solve) [msec] 260 | double setup = 0; 261 | 262 | //! Duration of optimization loop (included in solve) [msec] 263 | double opt = 0; 264 | 265 | //! Duration to calculate coefficients (included in opt) [msec] 266 | double coeff = 0; 267 | 268 | //! Duration to process backward pass (included in opt) [msec] 269 | double backward = 0; 270 | 271 | //! Duration to process forward pass (included in opt) [msec] 272 | double forward = 0; 273 | 274 | //! Duration to update variables (included in opt) [msec] 275 | double update = 0; 276 | 277 | //! Duration of pre-process for gain calculation (included in backward) [msec] 278 | double gain_pre = 0; 279 | 280 | //! Duration to solve linear equation for gain calculation (included in backward) [msec] 281 | double gain_solve = 0; 282 | 283 | //! Duration of post-process for gain calculation (included in backward) [msec] 284 | double gain_post = 0; 285 | 286 | //! Duration to calculate fraction-to-boundary rule (included in update) [msec] 287 | double fraction = 0; 288 | }; 289 | 290 | public: 291 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 292 | 293 | /** \brief Constructor. 294 | \param problem FMPC problem 295 | */ 296 | FmpcSolver(const std::shared_ptr> & problem) : problem_(problem) {} 297 | 298 | /** \brief Accessor to configuration. */ 299 | inline Configuration & config() 300 | { 301 | return config_; 302 | } 303 | 304 | /** \brief Const accessor to configuration. */ 305 | inline const Configuration & config() const 306 | { 307 | return config_; 308 | } 309 | 310 | /** \brief Solve optimization. 311 | \param current_t current time [sec] 312 | \param current_x current state 313 | \param initial_variable initial guess of optimization variables 314 | \return result status 315 | */ 316 | Status solve(double current_t, const StateDimVector & current_x, const Variable & initial_variable); 317 | 318 | /** \brief Const accessor to optimization variables. */ 319 | inline const Variable & variable() const 320 | { 321 | return variable_; 322 | } 323 | 324 | /** \brief Const accessor to sequence of coefficients of linearized KKT condition. */ 325 | inline const std::vector & coeffList() const 326 | { 327 | return coeff_list_; 328 | } 329 | 330 | /** \brief Const accessor to trace data list. */ 331 | inline const std::vector & traceDataList() const 332 | { 333 | return trace_data_list_; 334 | } 335 | 336 | /** \brief Const accessor to computation duration. */ 337 | inline const ComputationDuration & computationDuration() const 338 | { 339 | return computation_duration_; 340 | } 341 | 342 | /** \brief Dump trace data list. 343 | \param file_path path to output file 344 | */ 345 | void dumpTraceDataList(const std::string & file_path) const; 346 | 347 | protected: 348 | /** \brief Check optimization variables. */ 349 | void checkVariable() const; 350 | 351 | /** \brief Process one iteration. 352 | \param iter current iteration 353 | \return result status 354 | */ 355 | Status procOnce(int iter); 356 | 357 | /** \brief Calculate KKT condition error. 358 | \param barrier_eps barrier parameter 359 | */ 360 | double calcKktError(double barrier_eps) const; 361 | 362 | /** \brief Process backward pass a.k.a backward Riccati recursion. 363 | \return whether the process is finished successfully 364 | */ 365 | bool backwardPass(); 366 | 367 | /** \brief Process forward pass a.k.a forward Riccati recursion. 368 | \return whether the process is finished successfully 369 | */ 370 | bool forwardPass(); 371 | 372 | /** \brief Update optimization variables given Newton-step direction. 373 | \return whether the process is finished successfully 374 | */ 375 | bool updateVariables(); 376 | 377 | /** \brief Setup the merit function and its directional derivative. */ 378 | void setupMeritFunc(); 379 | 380 | /** \brief Calculate merit function. 381 | \param variable variable 382 | */ 383 | double calcMeritFunc(const Variable & variable) const; 384 | 385 | protected: 386 | //! Configuration 387 | Configuration config_; 388 | 389 | //! FMPC problem 390 | std::shared_ptr> problem_; 391 | 392 | //! Optimization variables 393 | Variable variable_; 394 | 395 | //! Update amount of optimization variables 396 | Variable delta_variable_; 397 | 398 | //! Sequence of coefficients of linearized KKT condition 399 | std::vector coeff_list_; 400 | 401 | //! Sequence of trace data 402 | std::vector trace_data_list_; 403 | 404 | //! Computation duration data 405 | ComputationDuration computation_duration_; 406 | 407 | //! Current time [sec] 408 | double current_t_ = 0; 409 | 410 | //! Current state 411 | StateDimVector current_x_ = StateDimVector::Zero(); 412 | 413 | //! Barrier parameter 414 | double barrier_eps_ = 1e-4; 415 | 416 | //! Scale of constraint errors in the merit function 417 | double merit_const_scale_ = 0.0; 418 | 419 | //! Merit function 420 | double merit_func_ = 0.0; 421 | 422 | //! Directional derivative of merit function 423 | double merit_deriv_ = 0.0; 424 | }; 425 | } // namespace nmpc_fmpc 426 | 427 | #include 428 | -------------------------------------------------------------------------------- /nmpc_fmpc/include/nmpc_fmpc/MathUtils.h: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace nmpc_fmpc 8 | { 9 | /** \brief Compute the directional derivative of the L1-norm of the function 10 | \param func function value 11 | \param jac jacobian matrix of function 12 | \param dir direction for directional derivative 13 | 14 | See (A.51) in "J Nocedal, S J Wright. Numerical optimization". 15 | */ 16 | template 17 | double l1NormDirectionalDeriv(const Eigen::Matrix & func, 18 | const Eigen::Matrix & jac, 19 | const Eigen::Matrix & dir) 20 | { 21 | double deriv = 0.0; 22 | for(int i = 0; i < func.size(); i++) 23 | { 24 | if(func(i) > 0) 25 | { 26 | deriv += jac.row(i).transpose().dot(dir); 27 | } 28 | else if(func(i) < 0) 29 | { 30 | deriv += -1 * jac.row(i).transpose().dot(dir); 31 | } 32 | else 33 | { 34 | deriv += std::abs(jac.row(i).transpose().dot(dir)); 35 | } 36 | } 37 | return deriv; 38 | } 39 | } // namespace nmpc_fmpc 40 | -------------------------------------------------------------------------------- /nmpc_fmpc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | nmpc_fmpc 3 | 0.1.0 4 | 5 | FMPC: Fast non-linear model predictive control (NMPC) combining the direct multiple shooting (DMS) method, the primal-dual interior point (PDIP) method, and Riccati recursion (RR) 6 | 7 | Masaki Murooka 8 | BSD 9 | 10 | http://ros.org/wiki/nmpc_fmpc 11 | Masaki Murooka 12 | 13 | ament_cmake 14 | 15 | nmpc_ddp 16 | 17 | eigen 18 | 19 | ament_cmake_gtest 20 | visualization_msgs 21 | std_srvs 22 | rviz 23 | rqt_service_caller 24 | 25 | doxygen 26 | 27 | -------------------------------------------------------------------------------- /nmpc_fmpc/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT NMPC_STANDALONE) 2 | find_package(rostest REQUIRED) 3 | endif() 4 | 5 | if(NMPC_STANDALONE) 6 | find_package(GTest REQUIRED) 7 | include(GoogleTest) 8 | function(add_nmpc_fmpc_test NAME) 9 | add_executable(${NAME} src/${NAME}.cpp) 10 | target_link_libraries(${NAME} PUBLIC GTest::gtest nmpc_fmpc) 11 | gtest_discover_tests(${NAME}) 12 | endfunction() 13 | else() 14 | function(add_nmpc_fmpc_test NAME) 15 | catkin_add_gtest(${NAME} src/${NAME}.cpp) 16 | target_link_libraries(${NAME} nmpc_fmpc) 17 | endfunction() 18 | endif() 19 | 20 | 21 | 22 | set(nmpc_fmpc_gtest_list 23 | TestMathUtils 24 | TestFmpcOscillator 25 | ) 26 | 27 | set(nmpc_fmpc_rostest_list 28 | TestFmpcCartPole 29 | ) 30 | 31 | foreach(NAME IN LISTS nmpc_fmpc_gtest_list) 32 | add_nmpc_fmpc_test(${NAME}) 33 | endforeach() 34 | 35 | if(NOT NMPC_STANDALONE) 36 | foreach(NAME IN LISTS nmpc_fmpc_rostest_list) 37 | add_rostest_gtest(${NAME} test/${NAME}.test src/${NAME}.cpp) 38 | target_link_libraries(${NAME} nmpc_fmpc) 39 | endforeach() 40 | endif() 41 | -------------------------------------------------------------------------------- /nmpc_fmpc/tests/rqt/TestFmpcCartPole.perspective: -------------------------------------------------------------------------------- 1 | { 2 | "keys": {}, 3 | "groups": { 4 | "pluginmanager": { 5 | "keys": { 6 | "running-plugins": { 7 | "type": "repr", 8 | "repr": "{u'rqt_service_caller/ServiceCaller': [1]}" 9 | } 10 | }, 11 | "groups": { 12 | "plugin__rqt_ez_publisher__EzPublisher__1": { 13 | "keys": {}, 14 | "groups": { 15 | "plugin": { 16 | "keys": { 17 | "texts": { 18 | "type": "repr", 19 | "repr": "[u'/goal/header/frame_id', u'/goal/pose/position/x', u'/goal/pose/position/y', u'/goal/pose/position/z', u'/goal/pose/orientation']" 20 | }, 21 | "publish_interval": { 22 | "type": "repr", 23 | "repr": "u'100'" 24 | }, 25 | "configurable": { 26 | "type": "repr", 27 | "repr": "u'true'" 28 | } 29 | }, 30 | "groups": { 31 | "goal": { 32 | "keys": {}, 33 | "groups": { 34 | "header": { 35 | "keys": { 36 | "frame_id_is_repeat": { 37 | "type": "repr", 38 | "repr": "u'false'" 39 | }, 40 | "frame_id_range": { 41 | "type": "repr", 42 | "repr": "(0, 0)" 43 | } 44 | }, 45 | "groups": {} 46 | }, 47 | "pose": { 48 | "keys": { 49 | "orientation_is_repeat": { 50 | "type": "repr", 51 | "repr": "u'false'" 52 | }, 53 | "orientation_range": { 54 | "type": "repr", 55 | "repr": "(-3.14, 3.14)" 56 | } 57 | }, 58 | "groups": { 59 | "position": { 60 | "keys": { 61 | "x_range": { 62 | "type": "repr", 63 | "repr": "(-1.0, 1.0)" 64 | }, 65 | "z_is_repeat": { 66 | "type": "repr", 67 | "repr": "u'false'" 68 | }, 69 | "y_is_repeat": { 70 | "type": "repr", 71 | "repr": "u'false'" 72 | }, 73 | "x_is_repeat": { 74 | "type": "repr", 75 | "repr": "u'false'" 76 | }, 77 | "y_range": { 78 | "type": "repr", 79 | "repr": "(-1.0, 1.0)" 80 | }, 81 | "z_range": { 82 | "type": "repr", 83 | "repr": "(-1.0, 1.0)" 84 | } 85 | }, 86 | "groups": {} 87 | } 88 | } 89 | } 90 | } 91 | } 92 | } 93 | } 94 | } 95 | }, 96 | "plugin__rqt_service_caller__ServiceCaller__1": { 97 | "keys": {}, 98 | "groups": { 99 | "dock_widget__ServiceCallerWidget": { 100 | "keys": { 101 | "dockable": { 102 | "type": "repr", 103 | "repr": "True" 104 | }, 105 | "parent": { 106 | "type": "repr", 107 | "repr": "None" 108 | }, 109 | "dock_widget_title": { 110 | "type": "repr", 111 | "repr": "u'Service Caller'" 112 | } 113 | }, 114 | "groups": {} 115 | }, 116 | "plugin": { 117 | "keys": { 118 | "splitter_orientation": { 119 | "type": "repr", 120 | "repr": "2" 121 | } 122 | }, 123 | "groups": {} 124 | } 125 | } 126 | }, 127 | "plugin__rqt_publisher__Publisher__1": { 128 | "keys": {}, 129 | "groups": { 130 | "dock_widget__PublisherWidget": { 131 | "keys": { 132 | "dockable": { 133 | "type": "repr", 134 | "repr": "u'true'" 135 | }, 136 | "parent": { 137 | "type": "repr", 138 | "repr": "None" 139 | }, 140 | "dock_widget_title": { 141 | "type": "repr", 142 | "repr": "u'Message Publisher'" 143 | } 144 | }, 145 | "groups": {} 146 | }, 147 | "plugin": { 148 | "keys": { 149 | "publishers": { 150 | "type": "repr", 151 | "repr": "u\"[{'type_name': 'std_msgs/Float64', 'topic_name': '/test', 'enabled': False, 'rate': 1.0, 'expressions': {u'/test/data': '10'}, 'publisher_id': 1, 'counter': 0}]\"" 152 | } 153 | }, 154 | "groups": {} 155 | } 156 | } 157 | } 158 | } 159 | }, 160 | "mainwindow": { 161 | "keys": { 162 | "geometry": { 163 | "type": "repr(QByteArray.hex)", 164 | "repr(QByteArray.hex)": "b'01d9d0cb00020000000000a00000032e0000056a0000054e000000a00000035c0000056a0000054e00000000000000000a00'", 165 | "pretty-print": " . j N \\ j N " 166 | }, 167 | "state": { 168 | "type": "repr(QByteArray.hex)", 169 | "repr(QByteArray.hex)": "b'000000ff00000000fd0000000200000002000004cb000001d2fc0100000001fb00000072007200710074005f0073006500720076006900630065005f00630061006c006c00650072005f005f005300650072007600690063006500430061006c006c00650072005f005f0031005f005f005300650072007600690063006500430061006c006c006500720057006900640067006500740100000000000004cb000002b700ffffff00000003000004cb00000198fc0100000002fb0000006a007200710074005f0065007a005f007000750062006c00690073006800650072005f005f0045007a005000750062006c00690073006800650072005f005f0031005f005f0045007a005000750062006c006900730068006500720050006c007500670069006e005500690100000000000004cb0000000000000000fb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c006900730068006500720057006900640067006500740100000000000004cb0000000000000000000004cb0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000'", 170 | "pretty-print": " rrqt_service_caller__ServiceCaller__1__ServiceCallerWidget jrqt_ez_publisher__EzPublisher__1__EzPublisherPluginUi Xrqt_publisher__Publisher__1__PublisherWidget 6MinimizedDockWidgetsToolbar " 171 | } 172 | }, 173 | "groups": { 174 | "toolbar_areas": { 175 | "keys": { 176 | "MinimizedDockWidgetsToolbar": { 177 | "type": "repr", 178 | "repr": "8" 179 | } 180 | }, 181 | "groups": {} 182 | } 183 | } 184 | } 185 | } 186 | } -------------------------------------------------------------------------------- /nmpc_fmpc/tests/rviz/TestFmpcCartPole.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 110 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 312 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: "" 28 | Preferences: 29 | PromptSaveOnExit: true 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: false 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 100 51 | Reference Frame: 52 | Value: false 53 | - Alpha: 1 54 | Cell Size: 100 55 | Class: rviz/Grid 56 | Color: 0; 0; 0 57 | Enabled: true 58 | Line Style: 59 | Line Width: 0.029999999329447746 60 | Value: Billboards 61 | Name: HorizontalLine 62 | Normal Cell Count: 0 63 | Offset: 64 | X: 0 65 | Y: 0 66 | Z: -1000 67 | Plane: XZ 68 | Plane Cell Count: 1 69 | Reference Frame: 70 | Value: true 71 | - Class: rviz/MarkerArray 72 | Enabled: true 73 | Marker Topic: /marker_arr 74 | Name: MarkerArray 75 | Namespaces: 76 | cart: true 77 | force: true 78 | mass: true 79 | pole: true 80 | target: true 81 | Queue Size: 100 82 | Value: true 83 | Enabled: true 84 | Global Options: 85 | Background Color: 255; 255; 255 86 | Default Light: true 87 | Fixed Frame: world 88 | Frame Rate: 30 89 | Name: root 90 | Tools: 91 | - Class: rviz/Interact 92 | Hide Inactive Objects: true 93 | - Class: rviz/MoveCamera 94 | - Class: rviz/Select 95 | - Class: rviz/FocusCamera 96 | - Class: rviz/Measure 97 | - Class: rviz/SetInitialPose 98 | Theta std deviation: 0.2617993950843811 99 | Topic: /initialpose 100 | X std deviation: 0.5 101 | Y std deviation: 0.5 102 | - Class: rviz/SetGoal 103 | Topic: /move_base_simple/goal 104 | - Class: rviz/PublishPoint 105 | Single click: true 106 | Topic: /clicked_point 107 | Value: true 108 | Views: 109 | Current: 110 | Angle: 0 111 | Class: rviz/TopDownOrtho 112 | Enable Stereo Rendering: 113 | Stereo Eye Separation: 0.05999999865889549 114 | Stereo Focal Distance: 1 115 | Swap Stereo Eyes: false 116 | Value: false 117 | Invert Z Axis: false 118 | Name: Current View 119 | Near Clip Distance: 0.009999999776482582 120 | Scale: 50.557960510253906 121 | Target Frame: 122 | Value: TopDownOrtho (rviz) 123 | X: 0 124 | Y: 0 125 | Saved: ~ 126 | Window Geometry: 127 | Displays: 128 | collapsed: true 129 | Height: 1401 130 | Hide Left Dock: true 131 | Hide Right Dock: true 132 | QMainWindow State: 000000ff00000000fd0000000400000000000001ab00000480fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000008e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003a000004800000013600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000012e000004bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003a000004bd000000f800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000099600000051fc0100000002fc00000000000009960000048000fffffffa000000010200000002fb0000003e0045006d007000740079005300650072007600690063006500430061006c006c0049006e00740065007200660061006300650041006300740069006f006e0000000000ffffffff0000000000000000fb0000000800540069006d00650100000500000000510000004e00fffffffb0000000800540069006d0065010000000000000450000000000000000000000996000004bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 133 | Selection: 134 | collapsed: false 135 | Time: 136 | collapsed: false 137 | Tool Properties: 138 | collapsed: false 139 | Views: 140 | collapsed: true 141 | Width: 2454 142 | X: 106 143 | Y: 39 144 | -------------------------------------------------------------------------------- /nmpc_fmpc/tests/src/TestFmpcOscillator.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | using Variable = typename nmpc_fmpc::FmpcSolver<2, 1, 3>::Variable; 11 | 12 | using Status = typename nmpc_fmpc::FmpcSolver<2, 1, 3>::Status; 13 | 14 | /** \brief FMPC problem for Van der Pol oscillator. 15 | 16 | See https://web.casadi.org/docs/#a-simple-test-problem 17 | */ 18 | class FmpcProblemOscillator : public nmpc_fmpc::FmpcProblem<2, 1, 3> 19 | { 20 | public: 21 | FmpcProblemOscillator(double dt) : FmpcProblem(dt) {} 22 | 23 | virtual StateDimVector stateEq(double t, const StateDimVector & x, const InputDimVector & u) const override 24 | { 25 | return stateEq(t, x, u, dt_); 26 | } 27 | 28 | virtual StateDimVector stateEq(double, // t 29 | const StateDimVector & x, 30 | const InputDimVector & u, 31 | double dt) const 32 | { 33 | StateDimVector x_dot; 34 | x_dot << (1.0 - std::pow(x[1], 2)) * x[0] - x[1] + u[0], x[0]; 35 | return x + dt * x_dot; 36 | } 37 | 38 | virtual double runningCost(double, // t 39 | const StateDimVector & x, 40 | const InputDimVector & u) const override 41 | { 42 | return 0.5 * (x.squaredNorm() + u.squaredNorm()); 43 | } 44 | 45 | virtual double terminalCost(double, // t 46 | const StateDimVector & // x 47 | ) const override 48 | { 49 | return 0; 50 | } 51 | 52 | virtual IneqDimVector ineqConst(double, // t 53 | const StateDimVector & x, 54 | const InputDimVector & u) const override 55 | { 56 | IneqDimVector g; 57 | g[0] = -1 * x[1] - 0.05; 58 | g[1] = -1 * u[0] - 1.0; 59 | g[2] = u[0] - 0.9; 60 | return g; 61 | } 62 | 63 | virtual void calcStateEqDeriv(double, // t 64 | const StateDimVector & x, 65 | const InputDimVector &, // u 66 | Eigen::Ref state_eq_deriv_x, 67 | Eigen::Ref state_eq_deriv_u) const override 68 | { 69 | state_eq_deriv_x.setZero(); 70 | state_eq_deriv_x(0, 0) = 1.0 - std::pow(x[1], 2); 71 | state_eq_deriv_x(0, 1) = -2 * x[0] * x[1] - 1.0; 72 | state_eq_deriv_x(1, 0) = 1; 73 | state_eq_deriv_x *= dt_; 74 | state_eq_deriv_x.diagonal().array() += 1; 75 | 76 | state_eq_deriv_u.setZero(); 77 | state_eq_deriv_u(0, 0) = 1; 78 | state_eq_deriv_u *= dt_; 79 | } 80 | 81 | virtual void calcRunningCostDeriv(double, // t 82 | const StateDimVector & x, 83 | const InputDimVector & u, 84 | Eigen::Ref running_cost_deriv_x, 85 | Eigen::Ref running_cost_deriv_u) const override 86 | { 87 | running_cost_deriv_x = x; 88 | running_cost_deriv_u = u; 89 | } 90 | 91 | virtual void calcRunningCostDeriv(double t, 92 | const StateDimVector & x, 93 | const InputDimVector & u, 94 | Eigen::Ref running_cost_deriv_x, 95 | Eigen::Ref running_cost_deriv_u, 96 | Eigen::Ref running_cost_deriv_xx, 97 | Eigen::Ref running_cost_deriv_uu, 98 | Eigen::Ref running_cost_deriv_xu) const override 99 | { 100 | calcRunningCostDeriv(t, x, u, running_cost_deriv_x, running_cost_deriv_u); 101 | running_cost_deriv_xx.setIdentity(); 102 | running_cost_deriv_uu.setIdentity(); 103 | running_cost_deriv_xu.setZero(); 104 | } 105 | 106 | virtual void calcTerminalCostDeriv(double, // t 107 | const StateDimVector &, // x 108 | Eigen::Ref terminal_cost_deriv_x) const override 109 | { 110 | terminal_cost_deriv_x.setZero(); 111 | } 112 | 113 | virtual void calcTerminalCostDeriv(double t, 114 | const StateDimVector & x, 115 | Eigen::Ref terminal_cost_deriv_x, 116 | Eigen::Ref terminal_cost_deriv_xx) const override 117 | { 118 | calcTerminalCostDeriv(t, x, terminal_cost_deriv_x); 119 | terminal_cost_deriv_xx.setZero(); 120 | } 121 | 122 | virtual void calcIneqConstDeriv(double, // t 123 | const StateDimVector &, // x 124 | const InputDimVector &, // u 125 | Eigen::Ref ineq_const_deriv_x, 126 | Eigen::Ref ineq_const_deriv_u) const override 127 | { 128 | ineq_const_deriv_x.setZero(); 129 | ineq_const_deriv_x(0, 1) = -1; 130 | 131 | ineq_const_deriv_u.setZero(); 132 | ineq_const_deriv_u(1, 0) = -1; 133 | ineq_const_deriv_u(2, 0) = 1; 134 | } 135 | }; 136 | 137 | TEST(TestFmpcOscillator, SolveMpc) 138 | { 139 | double horizon_dt = 0.01; // [sec] 140 | double horizon_duration = 4.0; // [sec] 141 | int horizon_steps = static_cast(horizon_duration / horizon_dt); 142 | double end_t = 10.0; // [sec] 143 | 144 | // Instantiate problem 145 | auto fmpc_problem = std::make_shared(horizon_dt); 146 | 147 | // Instantiate solver 148 | auto fmpc_solver = std::make_shared>(fmpc_problem); 149 | fmpc_solver->config().horizon_steps = horizon_steps; 150 | fmpc_solver->config().max_iter = 3; 151 | // The option "init_complementary_variable" is effective for problems in which state constraints are infasible. 152 | // fmpc_solver->config().init_complementary_variable = true; 153 | Variable variable(horizon_steps); 154 | variable.reset(0.0, 0.0, 0.0, 1e0, 1e0); 155 | 156 | // Initialize simulation 157 | double sim_dt = 0.005; // [sec] 158 | double current_t = 0; // [sec] 159 | FmpcProblemOscillator::StateDimVector current_x = FmpcProblemOscillator::StateDimVector(0.0, 1.0); 160 | 161 | // Run MPC loop 162 | bool first_iter = true; 163 | std::string file_path = "/tmp/TestFmpcOscillatorResult.txt"; 164 | std::ofstream ofs(file_path); 165 | ofs << "time x[0] x[1] u[0] mpc_iter computation_time kkt_error" << std::endl; 166 | while(current_t < end_t) 167 | { 168 | // Solve 169 | auto status = fmpc_solver->solve(current_t, current_x, variable); 170 | EXPECT_TRUE(status == Status::Succeeded || status == Status::MaxIterationReached); 171 | if(first_iter) 172 | { 173 | first_iter = false; 174 | fmpc_solver->dumpTraceDataList("/tmp/TestFmpcOscillatorTraceData.txt"); 175 | } 176 | 177 | // Check inequality constraints 178 | FmpcProblemOscillator::InputDimVector current_u = fmpc_solver->variable().u_list[0]; 179 | FmpcProblemOscillator::IneqDimVector current_g = fmpc_problem->ineqConst(current_t, current_x, current_u); 180 | EXPECT_TRUE((current_g.array() <= 0).all()) << "Inequality constraints violated: " << current_g.transpose(); 181 | 182 | // Dump 183 | ofs << current_t << " " << current_x.transpose() << " " << current_u.transpose() << " " 184 | << fmpc_solver->traceDataList().back().iter << " " << fmpc_solver->computationDuration().solve << " " 185 | << fmpc_solver->traceDataList().back().kkt_error << std::endl; 186 | 187 | // Update to next step 188 | current_x = fmpc_problem->stateEq(current_t, current_x, current_u, sim_dt); 189 | current_t += sim_dt; 190 | variable = fmpc_solver->variable(); 191 | } 192 | 193 | // Check final convergence 194 | EXPECT_LT(std::abs(current_x[0]), 1e-2); 195 | EXPECT_LT(std::abs(current_x[1]), 1e-2); 196 | 197 | std::cout << "Run the following commands in gnuplot:\n" 198 | << " set key autotitle columnhead\n" 199 | << " set key noenhanced\n" 200 | << " plot \"" << file_path << "\" u 1:2 w lp, \"\" u 1:3 w lp, \"\" u 1:4 w lp\n"; 201 | } 202 | 203 | TEST(TestFmpcOscillator, CheckDerivative) 204 | { 205 | double horizon_dt = 0.1; // [sec] 206 | auto fmpc_problem = std::make_shared(horizon_dt); 207 | 208 | double t = 0; 209 | FmpcProblemOscillator::StateDimVector x; 210 | x << 0.1, -0.2; 211 | FmpcProblemOscillator::InputDimVector u; 212 | u << 0.3; 213 | constexpr double deriv_eps = 1e-6; 214 | 215 | { 216 | FmpcProblemOscillator::StateStateDimMatrix state_eq_deriv_x_analytical; 217 | FmpcProblemOscillator::StateInputDimMatrix state_eq_deriv_u_analytical; 218 | fmpc_problem->calcStateEqDeriv(t, x, u, state_eq_deriv_x_analytical, state_eq_deriv_u_analytical); 219 | 220 | FmpcProblemOscillator::StateStateDimMatrix state_eq_deriv_x_numerical; 221 | FmpcProblemOscillator::StateInputDimMatrix state_eq_deriv_u_numerical; 222 | for(int i = 0; i < fmpc_problem->stateDim(); i++) 223 | { 224 | state_eq_deriv_x_numerical.col(i) = 225 | (fmpc_problem->stateEq(t, x + deriv_eps * FmpcProblemOscillator::StateDimVector::Unit(i), u) 226 | - fmpc_problem->stateEq(t, x - deriv_eps * FmpcProblemOscillator::StateDimVector::Unit(i), u)) 227 | / (2 * deriv_eps); 228 | } 229 | for(int i = 0; i < fmpc_problem->inputDim(); i++) 230 | { 231 | state_eq_deriv_u_numerical.col(i) = 232 | (fmpc_problem->stateEq(t, x, u + deriv_eps * FmpcProblemOscillator::InputDimVector::Unit(i)) 233 | - fmpc_problem->stateEq(t, x, u - deriv_eps * FmpcProblemOscillator::InputDimVector::Unit(i))) 234 | / (2 * deriv_eps); 235 | } 236 | 237 | EXPECT_LT((state_eq_deriv_x_analytical - state_eq_deriv_x_numerical).norm(), 1e-6); 238 | EXPECT_LT((state_eq_deriv_u_analytical - state_eq_deriv_u_numerical).norm(), 1e-6); 239 | } 240 | 241 | { 242 | FmpcProblemOscillator::IneqStateDimMatrix ineq_const_deriv_x_analytical; 243 | FmpcProblemOscillator::IneqInputDimMatrix ineq_const_deriv_u_analytical; 244 | fmpc_problem->calcIneqConstDeriv(t, x, u, ineq_const_deriv_x_analytical, ineq_const_deriv_u_analytical); 245 | 246 | FmpcProblemOscillator::IneqStateDimMatrix ineq_const_deriv_x_numerical; 247 | FmpcProblemOscillator::IneqInputDimMatrix ineq_const_deriv_u_numerical; 248 | for(int i = 0; i < fmpc_problem->stateDim(); i++) 249 | { 250 | ineq_const_deriv_x_numerical.col(i) = 251 | (fmpc_problem->ineqConst(t, x + deriv_eps * FmpcProblemOscillator::StateDimVector::Unit(i), u) 252 | - fmpc_problem->ineqConst(t, x - deriv_eps * FmpcProblemOscillator::StateDimVector::Unit(i), u)) 253 | / (2 * deriv_eps); 254 | } 255 | for(int i = 0; i < fmpc_problem->inputDim(); i++) 256 | { 257 | ineq_const_deriv_u_numerical.col(i) = 258 | (fmpc_problem->ineqConst(t, x, u + deriv_eps * FmpcProblemOscillator::InputDimVector::Unit(i)) 259 | - fmpc_problem->ineqConst(t, x, u - deriv_eps * FmpcProblemOscillator::InputDimVector::Unit(i))) 260 | / (2 * deriv_eps); 261 | } 262 | 263 | EXPECT_LT((ineq_const_deriv_x_analytical - ineq_const_deriv_x_numerical).norm(), 1e-6); 264 | EXPECT_LT((ineq_const_deriv_u_analytical - ineq_const_deriv_u_numerical).norm(), 1e-6); 265 | } 266 | } 267 | 268 | int main(int argc, char ** argv) 269 | { 270 | testing::InitGoogleTest(&argc, argv); 271 | return RUN_ALL_TESTS(); 272 | } 273 | -------------------------------------------------------------------------------- /nmpc_fmpc/tests/src/TestMathUtils.cpp: -------------------------------------------------------------------------------- 1 | /* Author: Masaki Murooka */ 2 | 3 | #include 4 | 5 | #include 6 | 7 | TEST(TestMathUtils, L1NormDirectionalDeriv) 8 | { 9 | constexpr double deriv_eps = 1e-6; 10 | 11 | // Identity function 12 | { 13 | Eigen::Matrix4d jac = Eigen::Matrix4d::Identity(); 14 | 15 | for(int i = 0; i < 1000; i++) 16 | { 17 | Eigen::Vector4d func = 100.0 * Eigen::Vector4d::Random(); 18 | Eigen::Vector4d dir = Eigen::Vector4d::Random(); 19 | if(i == 0) 20 | { 21 | func.setZero(); 22 | } 23 | 24 | double deriv_analytical = nmpc_fmpc::l1NormDirectionalDeriv(func, jac, dir); 25 | double deriv_numerical = ((func + deriv_eps * dir).cwiseAbs().sum() - func.cwiseAbs().sum()) / deriv_eps; 26 | 27 | EXPECT_LT(std::abs(deriv_analytical - deriv_numerical), 1e-5) 28 | << "analytical: " << deriv_analytical << ", numerical: " << deriv_numerical << std::endl; 29 | } 30 | } 31 | 32 | // Nonlinear function 33 | { 34 | int input_dim = 4; 35 | int output_dim = 3; 36 | auto sample_func = [&](const Eigen::VectorXd & x) 37 | { 38 | Eigen::VectorXd func(output_dim); 39 | func[0] = x.squaredNorm() - 10.0; 40 | func[1] = std::pow(x[1], 3) + -5 * std::pow(x[2], 2) + 10 * x[3] + -20; 41 | func[2] = std::sin(x[0]) + std::cos(x[1]); 42 | return func; 43 | }; 44 | auto sample_jac = [&](const Eigen::VectorXd & x) -> Eigen::MatrixXd 45 | { 46 | Eigen::MatrixXd jac(output_dim, input_dim); 47 | jac.row(0) = 2 * x.transpose(); 48 | jac.row(1) << 0, 3 * std::pow(x[1], 2), -10 * x[2], 10; 49 | jac.row(2) << std::cos(x[0]), -1 * std::sin(x[1]), 0, 0; 50 | return jac; 51 | }; 52 | 53 | for(int i = 0; i < 1000; i++) 54 | { 55 | Eigen::VectorXd x = 100.0 * Eigen::VectorXd::Random(input_dim); 56 | Eigen::VectorXd dir = Eigen::VectorXd::Random(input_dim); 57 | if(i == 0) 58 | { 59 | x.setZero(); 60 | } 61 | 62 | double deriv_analytical = nmpc_fmpc::l1NormDirectionalDeriv(sample_func(x), sample_jac(x), dir); 63 | double deriv_numerical = 64 | (sample_func(x + deriv_eps * dir).cwiseAbs().sum() - sample_func(x).cwiseAbs().sum()) / deriv_eps; 65 | 66 | EXPECT_LT(std::abs(deriv_analytical - deriv_numerical), 1e-3) 67 | << "analytical: " << deriv_analytical << ", numerical: " << deriv_numerical << std::endl; 68 | } 69 | } 70 | } 71 | 72 | int main(int argc, char ** argv) 73 | { 74 | testing::InitGoogleTest(&argc, argv); 75 | return RUN_ALL_TESTS(); 76 | } 77 | -------------------------------------------------------------------------------- /nmpc_fmpc/tests/test/TestFmpcCartPole.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | 11 | no_exit: $(arg no_exit) 12 | control: 13 | horizon_dt: 0.01 # [sec] 14 | horizon_duration: 3.0 # [sec] 15 | mpc_dt: 0.004 # [sec] 16 | sim_dt: 0.002 # [sec] 17 | param: 18 | cart_mass: 1.0 19 | pole_mass: 0.5 20 | pole_length: 2.0 21 | cost: 22 | running_x: [0.1, 1.0, 0.01, 0.1] 23 | running_u: [0.01] 24 | terminal_x: [0.1, 1.0, 0.01, 0.1] 25 | 26 | 27 | 28 | 30 | 31 | 33 | 34 | --------------------------------------------------------------------------------