├── .gitignore
├── CMakeLists.txt
├── LICENSE
├── README.md
├── cmake
    ├── AddInstallRPATHSupport.cmake
    ├── AddUninstallTarget.cmake
    └── InstallBasicPackageFiles.cmake
├── matlab
    ├── casadi
    │   ├── accelerationConsistencyConstraint.m
    │   ├── createDiscretizedFunction.m
    │   ├── doublePendulumVariableHeight.m
    │   ├── doublePendulumVariableHeight3Phases.m
    │   ├── doublePendulumVariableHeightFeedback.m
    │   ├── doublePendulumVariableHeightIntegrator.m
    │   ├── getConstraints.m
    │   ├── getIntegratorDynamics.m
    │   ├── getPhaseDependentDynamics.m
    │   ├── plotOptiSolutionForDoublePendulum.m
    │   ├── plotOptiSolutionForDoublePendulumFeedback.m
    │   ├── plotOptiSolutionForDoublePendulumPlusAccelerations.m
    │   ├── solveVariableHeightDoublePendulum.m
    │   ├── solveVariableHeightDoublePendulumAsIntegrator.m
    │   └── solveVariableHeightDoublePendulumFeedback.m
    └── feedbackLinearization
    │   ├── controller.m
    │   ├── main.m
    │   └── variableHeightPendulum.m
├── package.xml
└── src
    ├── CMakeLists.txt
    ├── core
        ├── CMakeLists.txt
        ├── src
        │   ├── CMakeLists.txt
        │   ├── include
        │   │   └── StepUpPlanner
        │   │   │   ├── Control.h
        │   │   │   ├── CostWeights.h
        │   │   │   ├── Phase.h
        │   │   │   ├── PhaseType.h
        │   │   │   ├── References.h
        │   │   │   ├── Rotation.h
        │   │   │   ├── Settings.h
        │   │   │   ├── SideDependentObject.h
        │   │   │   ├── Solver.h
        │   │   │   ├── State.h
        │   │   │   └── Step.h
        │   └── src
        │   │   ├── Control.cpp
        │   │   ├── CostWeights.cpp
        │   │   ├── Phase.cpp
        │   │   ├── References.cpp
        │   │   ├── Rotation.cpp
        │   │   ├── Settings.cpp
        │   │   ├── Solver.cpp
        │   │   ├── State.cpp
        │   │   └── Step.cpp
        └── test
        │   ├── CMakeLists.txt
        │   ├── FiveAndThreePhasesTest.cpp
        │   └── RotationTest.cpp
    └── interface
        ├── CMakeLists.txt
        ├── src
            ├── CMakeLists.txt
            ├── include
            │   └── Responder.h
            └── src
            │   ├── Responder.cpp
            │   └── ResponderMain.cpp
        └── test
            ├── CMakeLists.txt
            └── ResponderTest.cpp


/.gitignore:
--------------------------------------------------------------------------------
1 | 
2 | *.m~
3 | *build
4 | *.user
5 | *.autosave
6 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 3.5)
  2 | 
  3 | project(step_up_planner
  4 |         LANGUAGES CXX
  5 |         VERSION 0.0.0.0)
  6 | 
  7 | include(GNUInstallDirs)
  8 | 
  9 | # Enable C++14
 10 | set(CMAKE_CXX_EXTENSIONS OFF)
 11 | set(CMAKE_CXX_STANDARD 14)
 12 | set(CMAKE_CXX_STANDARD_REQUIRED ON)
 13 | 
 14 | # Control where libraries and executables are placed during the build.
 15 | # With the following settings executables are placed in <the top level of the
 16 | # build tree>/bin and libraries/archives in <top level of the build tree>/lib.
 17 | # This is particularly useful to run ctests on libraries built on Windows
 18 | # machines: tests, which are executables, are placed in the same folders of
 19 | # dlls, which are treated as executables as well, so that they can properly
 20 | # find the libraries to run. This is a because of missing RPATH on Windows.
 21 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")
 22 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
 23 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
 24 | 
 25 | # To build shared libraries in Windows, we set CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS to TRUE.
 26 | # See https://cmake.org/cmake/help/v3.4/variable/CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS.html
 27 | # See https://blog.kitware.com/create-dlls-on-windows-without-declspec-using-new-cmake-export-all-feature/
 28 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
 29 | 
 30 | # Under MSVC, we set CMAKE_DEBUG_POSTFIX to "d" to add a trailing "d" to library
 31 | # built in debug mode. In this Windows user can compile, build and install the
 32 | # library in both Release and Debug configuration avoiding naming clashes in the
 33 | # installation directories.
 34 | if(MSVC)
 35 |     set(CMAKE_DEBUG_POSTFIX "d")
 36 | endif()
 37 | 
 38 | # Build position independent code.
 39 | # Position Independent Code (PIC) is commonly used for shared libraries so that
 40 | # the same shared library code can be loaded in each program address space in a
 41 | # location where it will not overlap with any other uses of such memory.
 42 | # In particular, this option avoids problems occurring when a process wants to
 43 | # load more than one shared library at the same virtual address.
 44 | # Since shared libraries cannot predict where other shared libraries could be
 45 | # loaded, this is an unavoidable problem with the traditional shared library
 46 | # concept.
 47 | # Generating position-independent code is often the default behavior for most
 48 | # modern compilers.
 49 | # Moreover linking a static library that is not built with PIC from a shared
 50 | # library will fail on some compiler/architecture combinations.
 51 | # Further details on PIC can be found here:
 52 | # https://eli.thegreenplace.net/2011/11/03/position-independent-code-pic-in-shared-libraries/
 53 | set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 54 | 
 55 | # Disable C and C++ compiler extensions.
 56 | # C/CXX_EXTENSIONS are ON by default to allow the compilers to use extended
 57 | # variants of the C/CXX language.
 58 | # However, this could expose cross-platform bugs in user code or in the headers
 59 | # of third-party dependencies and thus it is strongly suggested to turn
 60 | # extensions off.
 61 | set(CMAKE_C_EXTENSIONS OFF)
 62 | set(CMAKE_CXX_EXTENSIONS OFF)
 63 | 
 64 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
 65 | 
 66 | ### Options
 67 | # Shared/Dynamic or Static library?
 68 | option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
 69 | 
 70 | # Build test related commands?
 71 | option(BUILD_TESTING "Create tests using CMake" OFF)
 72 | if(BUILD_TESTING)
 73 |     enable_testing()
 74 | endif()
 75 | 
 76 | # Enable RPATH support for installed binaries and libraries
 77 | include(AddInstallRPATHSupport)
 78 | add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_FULL_BINDIR}"
 79 |                           LIB_DIRS "${CMAKE_INSTALL_FULL_LIBDIR}"
 80 |                           INSTALL_NAME_DIR "${CMAKE_INSTALL_FULL_LIBDIR}"
 81 |                           USE_LINK_PATH)
 82 | 
 83 | # Encourage user to specify a build type (e.g. Release, Debug, etc.), otherwise set it to Release.
 84 | if(NOT CMAKE_CONFIGURATION_TYPES)
 85 |     if(NOT CMAKE_BUILD_TYPE)
 86 |         message(STATUS "Setting build type to 'Release' as none was specified.")
 87 |         set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release")
 88 |     endif()
 89 | endif()
 90 | 
 91 | ### Compile- and install-related commands.
 92 | add_subdirectory(src)
 93 | 
 94 | include(InstallBasicPackageFiles)
 95 | install_basic_package_files(${PROJECT_NAME}
 96 |                             TARGETS ${PROJECT_NAME}_core
 97 |                             VERSION ${${PROJECT_NAME}_VERSION}
 98 |                             COMPATIBILITY AnyNewerVersion
 99 |                             VARS_PREFIX ${PROJECT_NAME}
100 |                             NO_CHECK_REQUIRED_COMPONENTS_MACRO)
101 | 
102 | # Add the uninstall target
103 | include(AddUninstallTarget)
104 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2020, Fondazione Istituto Italiano di Tecnologia
 2 | All rights reserved.
 3 | 
 4 | Redistribution and use in source and binary forms, with or without
 5 | modification, are permitted provided that the following conditions are met:
 6 | 
 7 | 1. Redistributions of source code must retain the above copyright notice, this
 8 |    list of conditions and the following disclaimer.
 9 | 
10 | 2. Redistributions in binary form must reproduce the above copyright notice,
11 |    this list of conditions and the following disclaimer in the documentation
12 |    and/or other materials provided with the distribution.
13 | 
14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
17 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
18 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
20 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
21 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
22 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
23 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # Large Step-Ups Planner
  2 | 
  3 | This repo contains the code used to generate the trajectories for large step-ups. It consists of a library which exploits [``CasADi``](https://web.casadi.org/) to solve an optimal control problem, plus a [``ROS2``](https://index.ros.org/doc/ros2/) node to communicate with the robot.
  4 | 
  5 | ## Citing
  6 | This code is related to the paper "Non-Linear Trajectory Optimization for Large Step-Ups: Application to the Humanoid Robot Atlas".
  7 | Paper: https://ieeexplore.ieee.org/document/9341587
  8 | Arxiv: https://arxiv.org/abs/2004.12083
  9 | 
 10 | To cite this work, please add the following to your publication
 11 | ```
 12 | S. Dafarra, S. Bertrand, R. J. Griffin, G. Metta, D. Pucci and J. Pratt, "Non-Linear Trajectory Optimization for Large Step-Ups: Application to the Humanoid Robot Atlas," 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Las Vegas, NV, USA, 2020, pp. 3884-3891, doi: 10.1109/IROS45743.2020.9341587.
 13 | ```
 14 | Bibtex:
 15 | ```
 16 | @INPROCEEDINGS{9341587,  author={S. {Dafarra} and S. {Bertrand} and R. J. {Griffin} and G. {Metta} and D. {Pucci} and J. {Pratt}},  booktitle={2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},   title={Non-Linear Trajectory Optimization for Large Step-Ups: Application to the Humanoid Robot Atlas},   year={2020},  volume={},  number={},  pages={3884-3891},  doi={10.1109/IROS45743.2020.9341587}}
 17 | ```
 18 | 
 19 | ## Dependencies
 20 | - [``CasADi``](https://web.casadi.org/)
 21 | - [``Ipopt``](https://github.com/coin-or/Ipopt)
 22 | - [``cmake``](https://cmake.org/)
 23 | - A C++ compiler
 24 | 
 25 | If the CMake option ``BUILD_INTERFACE`` is ON (it is ON by default), you also need
 26 | - [``ihmc-open-robotics-software``](https://bitbucket.ihmc.us/projects/LIBS/repos/ihmc-open-robotics-software/browse)
 27 | - [``ROS2``](https://index.ros.org/doc/ros2/) (Tested with Crystal Clemmys)
 28 | 
 29 | ## Linux Installation Instructions
 30 | 
 31 | The following instructions have been tested on Ubuntu 16.04. It is also assumed that no other ``ROS2`` installation is already present on the system.
 32 | 
 33 | ### Install system dependencies
 34 | Run the following command to install ``gcc``, ``cmake`` and ``Ipopt`` from ``apt``.
 35 | ```
 36 | sudo apt install gcc g++ gfortran git cmake liblapack-dev pkg-config coinor-libipopt-dev --install-recommends
 37 | ```
 38 | 
 39 | ### Install CasADi
 40 | (These instructions have been adapted from https://github.com/casadi/casadi/wiki/InstallationLinux#building-casadi-from-sources)
 41 | 
 42 | - Move to the directory where you want to download ``CasADi``, e.g. ``~/dev/large_step_ups``.
 43 | ```
 44 | cd ~/dev/large_step_ups
 45 | ```
 46 | 
 47 | - Clone the repository
 48 | ```
 49 | git clone https://github.com/casadi/casadi.git
 50 | ```
 51 | 
 52 | - Install ``CasADi``.
 53 | In order to avoid polluting system directories, it is suggested to specify an installation directory different from ``/usr/local``. In this case we create an ``install`` folder inside the ``build`` folder.
 54 | ```
 55 | mkdir build
 56 | cd build
 57 | mkdir install
 58 | export NEW_INSTALL_DIR=$(pwd)/install
 59 | cmake -DWITH_IPOPT=ON -DCMAKE_INSTALL_PREFIX=$NEW_INSTALL_DIR ..
 60 | make install -j4
 61 | ```
 62 | 
 63 | - Modify the ``.bashrc`` exporting the ``casadi_DIR`` variable. In this way ``CasADi`` can be easily found by ``cmake`` projects. Add the following line.
 64 | ```
 65 | casadi_DIR=/path/to/casadi/install
 66 | ```
 67 | In our case
 68 | ```
 69 | casadi_DIR=~/dev/large_step_ups/casadi/build/install
 70 | ```
 71 | It is then necessary to source the modified ``.bashrc`` in order to apply the changes.
 72 | 
 73 | 
 74 | ### Install ``ROS2``
 75 | (These instructions have been adapted from https://index.ros.org/doc/ros2/Installation/Crystal/Linux-Development-Setup/)
 76 | 
 77 | - Install ``ROS2`` system dependencies
 78 | ```
 79 | curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
 80 | ```
 81 | ```
 82 | sudo apt update && sudo apt install -y \
 83 |   build-essential \
 84 |   cmake \
 85 |   git \
 86 |   python3-colcon-common-extensions \
 87 |   python3-lark-parser \
 88 |   python3-pip \
 89 |   python-rosdep \
 90 |   python3-vcstool \
 91 |   wget
 92 | ```
 93 | ```
 94 | python3 -m pip install -U \
 95 |   argcomplete \
 96 |   flake8 \
 97 |   flake8-blind-except \
 98 |   flake8-builtins \
 99 |   flake8-class-newline \
100 |   flake8-comprehensions \
101 |   flake8-deprecated \
102 |   flake8-docstrings \
103 |   flake8-import-order \
104 |   flake8-quotes \
105 |   pytest-repeat \
106 |   pytest-rerunfailures \
107 |   pytest \
108 |   pytest-cov \
109 |   pytest-runner \
110 |   setuptools
111 | ```
112 | ```
113 | sudo apt install --no-install-recommends -y \
114 |   libasio-dev \
115 |   libtinyxml2-dev
116 | ```
117 | 
118 | - Create a ``ROS2`` workspace and retrieve the code.
119 | ```
120 | mkdir -p ~/ros2_ws/src
121 | cd ~/ros2_ws
122 | wget https://raw.githubusercontent.com/ros2/ros2/crystal/ros2.repos
123 | vcs import src < ros2.repos
124 | ```
125 | 
126 | - Install ``ROS2`` dependencies via ``rosdep``
127 | ```
128 | sudo rosdep init
129 | rosdep update
130 | rosdep install --from-paths src --ignore-src --rosdistro crystal -y --skip-keys "console_bridge fastcdr fastrtps libopensplice67 libopensplice69 python3-lark-parser rti-connext-dds-5.3.1 urdfdom_headers"
131 | python3 -m pip install -U lark-parser
132 | ```
133 | 
134 | - Compile ``ROS2``
135 | ```
136 | cd ~/ros2_ws
137 | colcon build --symlink-install --packages-ignore qt_gui_cpp rqt_gui_cpp
138 | ```
139 | 
140 | ### Compile the controller messages C++ interface
141 | Here we assume that the following messages are available in the ``ihmc-open-robotics-software/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg`` folder:
142 | - ``StepUpPlannerControlElement.msg``
143 | - ``StepUpPlannerCostWeights.msg``
144 | - ``StepUpPlannerErrorMessage.msg``
145 | - ``StepUpPlannerParametersMessage.msg``
146 | - ``StepUpPlannerPhase.msg``
147 | - ``StepUpPlannerPhaseParameters.msg``
148 | - ``StepUpPlannerPhaseResult.msg``
149 | - ``StepUpPlannerRequestMessage.msg``
150 | - ``StepUpPlannerRespondMessage.msg``
151 | - ``StepUpPlannerStepParameters.msg``
152 | - ``StepUpPlannerVector2.msg``
153 | 
154 | If not, you may checkout the branch ``feature/largeStepUps`` of ``ihmc-open-robotics-software``. Now we assume the ``ihmc-open-robotics-software`` to be in the path ``~/dev/atlas/ihmc-open-robotics-software/``.
155 | 
156 | - Create a ``controller_msgs`` workspace
157 | ```
158 | mkdir -p ~/dev/large_step_ups/controller_msgs_ws/src
159 | ```
160 | 
161 | - Create a symbolic link to the ``controller_msgs`` folder
162 | ```
163 | ln -s ~/dev/atlas/ihmc-open-robotics-software/ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/ ~/dev/large_step_ups/controller_msgs_ws/src/controller_msgs
164 | ```
165 | Alternatively, it is possible to exploit the [``ihmc_interfaces``](https://bitbucket.ihmc.us/projects/ROS/repos/ihmc_interfaces/browse) repo. In this case it would be enough to clone the repo in the ``~/dev/large_step_ups/controller_msgs_ws/src`` folder (make sure that the messages listed above are available).
166 | 
167 | - Compile the message workspace
168 | ```
169 | cd ~/dev/large_step_ups/controller_msgs_ws
170 | source ~/ros2_ws/install/local_setup.sh
171 | colcon build --symlink-install
172 | ```
173 | 
174 | ### Clone and compile this repo
175 | - Clone this repo
176 | ```
177 | cd ~/dev/large_step_ups
178 | git clone https://bitbucket.ihmc.us/scm/icsl/large-step-ups-planner.git
179 | ```
180 | 
181 | - Source the ROS setup files to make sure that all the enviromental variables are set.
182 | ```
183 | source ~/ros2_ws/install/local_setup.sh
184 | source ~/dev/large_step_ups/controller_msgs_ws/install/local_setup.sh
185 | ```
186 | 
187 | - Compile the code
188 | ```
189 | cd large-step-ups-planner/
190 | mkdir build
191 | cmake ../
192 | make
193 | ```
194 | 
195 | ## Run the responder node.
196 | This node waits for a ``StepUpPlannerParametersMessage`` and a  ``StepUpPlannerRequestMessage``. The first is used to setup the planner. It returns a ``StepUpPlannerErrorMessage`` as an acknowledgement. Depending on the error code, the parameters may be have been successfully set or refused. Some info are provided in the message about what is wrong.
197 | After the parameters are set, it is possible to send requests. The node will respond with a ``StepUpPlannerRespondMessage`` containing the solution.
198 | 
199 | In order to run, the node needs all the ROS2 variables set up. To this end, it is suggested to add the following alias to the ``.bashrc``
200 | ```
201 | alias LARGE_STEP_UPS_SETUP='export ROS_DOMAIN_ID=8 && source ~/ros2_ws/install/local_setup.sh && source ~/dev/large_step_ups/controller_msgs_ws/install/local_setup.sh && export PATH=$PATH:~/dev/large_step_ups/large-step-ups-planner/build/bin'
202 | ```
203 | 
204 | In this way, it is possible to run the node with the following commands:
205 | ```
206 | LARGE_STEP_UPS_SETUP
207 | step_up_planner_responder
208 | ```
209 | The output should look like
210 | ```
211 | [INFO] [StepUpPlannerResponder]: Running...
212 | 
213 | ```
214 | ### ``StepUpPlannerParametersMessage``
215 | It is necessary to send a ``StepUpPlannerParametersMessage`` the first time a trajectory has to be computed or in case the following values need to change:
216 | - number of phases
217 | - phase settings
218 |     - phase type
219 |     - foot vertexes
220 |     - foot scale
221 |     - center offset
222 | - number of instants per phase
223 | - solver verbosity
224 | - max leg length parameter
225 | - min leg length parameter
226 | - ``Ipopt`` linear solver
227 | - portion of the final phase used to weight the final error
228 | - static friction coefficient
229 | - torsional friction coefficient
230 | - cost weights
231 | - whether to include or not the `CenterOfMassTrajectoryMessage`, the `PelvisHeightTrajectoryMessage` and/or the `FootstepDataListMessage` into the solution message.
232 | - whether or not to send directly to the walking controller he `CenterOfMassTrajectoryMessage`, the `PelvisHeightTrajectoryMessage` and/or the `FootstepDataListMessage`, and their respective topic name.
233 | - delta to be used when creating a `PelvisHeightTrajectoryMessage` from the CoM height profile.
234 | - number of data points per message.
235 | 
236 | In particular, when sending or including the `CenterOfMassTrajectoryMessage` and the `PelvisHeightTrajectoryMessage` a different message per phase is created. Multiple messages per phase are created if the number of data points per message is lower than the number of instants per phase.
237 | 
238 | If a  ``StepUpPlannerParametersMessage`` has already been sent and successfully set, and none of the above parameters need to change, then it is not necessary to send such message again.
239 | 
240 | ### ``StepUpPlannerRequestMessage``
241 | Is the message to be sent every time a new trajectory has to be computed.
242 | 
243 | # Tests on the robot.
244 | Tests on the robot have been performed by launching the node as specified above and by launching the ``AtlasStepUpPlannerDemo`` defined in ``ihmc-open-robotics-software``.
245 | 


--------------------------------------------------------------------------------
/cmake/AddInstallRPATHSupport.cmake:
--------------------------------------------------------------------------------
  1 | #.rst:
  2 | # AddInstallRPATHSupport
  3 | # ----------------------
  4 | #
  5 | # Add support to RPATH during installation to your project::
  6 | #
  7 | #   add_install_rpath_support([BIN_DIRS dir [dir]]
  8 | #                             [LIB_DIRS dir [dir]]
  9 | #                             [INSTALL_NAME_DIR [dir]]
 10 | #                             [DEPENDS condition [condition]]
 11 | #                             [USE_LINK_PATH])
 12 | #
 13 | # Normally (depending on the platform) when you install a shared
 14 | # library you can either specify its absolute path as the install name,
 15 | # or leave just the library name itself. In the former case the library
 16 | # will be correctly linked during run time by all executables and other
 17 | # shared libraries, but it must not change its install location. This
 18 | # is often the case for libraries installed in the system default
 19 | # library directory (e.g. ``/usr/lib``).
 20 | # In the latter case, instead, the library can be moved anywhere in the
 21 | # file system but at run time the dynamic linker must be able to find
 22 | # it. This is often accomplished by setting environmental variables
 23 | # (i.e. ``LD_LIBRARY_PATH`` on Linux).
 24 | # This procedure is usually not desirable for two main reasons:
 25 | #
 26 | # - by setting the variable you are changing the default behaviour
 27 | #   of the dynamic linker thus potentially breaking executables (not as
 28 | #   destructive as ``LD_PRELOAD``)
 29 | # - the variable will be used only by applications spawned by the shell
 30 | #   and not by other processes.
 31 | #
 32 | # RPATH is aimed to solve the issues introduced by the second
 33 | # installation method. Using run-path dependent libraries you can
 34 | # create a directory structure containing executables and dependent
 35 | # libraries that users can relocate without breaking it.
 36 | # A run-path dependent library is a dependent library whose complete
 37 | # install name is not known when the library is created.
 38 | # Instead, the library specifies that the dynamic loader must resolve
 39 | # the library’s install name when it loads the executable that depends
 40 | # on the library. The executable or the other shared library will
 41 | # hardcode in the binary itself the additional search directories
 42 | # to be passed to the dynamic linker. This works great in conjunction
 43 | # with relative paths.
 44 | # This command will enable support to RPATH to your project.
 45 | # It will enable the following things:
 46 | #
 47 | #  - If the project builds shared libraries it will generate a run-path
 48 | #    enabled shared library, i.e. its install name will be resolved
 49 | #    only at run time.
 50 | #  - In all cases (building executables and/or shared libraries)
 51 | #    dependent shared libraries with RPATH support will be properly
 52 | #
 53 | # The command has the following parameters:
 54 | #
 55 | # Options:
 56 | #  - ``USE_LINK_PATH``: if passed the command will automatically adds to
 57 | #    the RPATH the path to all the dependent libraries.
 58 | #
 59 | # Arguments:
 60 | #  - ``BIN_DIRS`` list of directories when the targets (executable and
 61 | #    plugins) will be installed.
 62 | #  - ``LIB_DIRS`` list of directories to be added to the RPATH. These
 63 | #    directories will be added "relative" w.r.t. the ``BIN_DIRS`` and
 64 | #    ``LIB_DIRS``.
 65 | #  - ``INSTALL_NAME_DIR`` directory where the libraries will be installed.
 66 | #    This variable will be used only if ``CMAKE_SKIP_RPATH`` or
 67 | #    ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the
 68 | #    ``INSTALL_NAME_DIR`` on all targets
 69 | #  - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable
 70 | #    RPATH, for example ``FOO; NOT BAR``.
 71 | #
 72 | # Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further
 73 | # details.
 74 | 
 75 | #=======================================================================
 76 | # Copyright 2014 Istituto Italiano di Tecnologia (IIT)
 77 | # @author Francesco Romano <francesco.romano@iit.it>
 78 | #
 79 | # Distributed under the OSI-approved BSD License (the "License");
 80 | # see accompanying file Copyright.txt for details.
 81 | #
 82 | # This software is distributed WITHOUT ANY WARRANTY; without even the
 83 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 84 | # See the License for more information.
 85 | #=======================================================================
 86 | # (To distribute this file outside of CMake, substitute the full
 87 | # License text for the above reference.)
 88 | 
 89 | 
 90 | include(CMakeParseArguments)
 91 | 
 92 | 
 93 | function(ADD_INSTALL_RPATH_SUPPORT)
 94 | 
 95 |   set(_options USE_LINK_PATH)
 96 |   set(_oneValueArgs INSTALL_NAME_DIR)
 97 |   set(_multiValueArgs BIN_DIRS
 98 |                       LIB_DIRS
 99 |                       DEPENDS)
100 | 
101 |   cmake_parse_arguments(_ARS "${_options}"
102 |                              "${_oneValueArgs}"
103 |                              "${_multiValueArgs}"
104 |                              "${ARGN}")
105 | 
106 |   # if either RPATH or INSTALL_RPATH is disabled
107 |   # and the INSTALL_NAME_DIR variable is set, then hardcode the install name
108 |   if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH)
109 |     if(DEFINED _ARS_INSTALL_NAME_DIR)
110 |       set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE)
111 |     endif()
112 |   endif()
113 | 
114 |   if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH))
115 |     return()
116 |   endif()
117 | 
118 | 
119 |   set(_rpath_available 1)
120 |   if(DEFINED _ARS_DEPENDS)
121 |     foreach(_dep ${_ARS_DEPENDS})
122 |       string(REGEX REPLACE " +" ";" _dep "${_dep}")
123 |       if(NOT (${_dep}))
124 |         set(_rpath_available 0)
125 |       endif()
126 |     endforeach()
127 |   endif()
128 | 
129 |   if(_rpath_available)
130 | 
131 |     # Enable RPATH on OSX.
132 |     set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE)
133 | 
134 |     # Find system implicit lib directories
135 |     set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES})
136 |     if(EXISTS "/etc/debian_version") # is this a debian system ?
137 |         if(CMAKE_LIBRARY_ARCHITECTURE)
138 |             list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}"
139 |                                          "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}")
140 |         endif()
141 |     endif()
142 |     # This is relative RPATH for libraries built in the same project
143 |     foreach(lib_dir ${_ARS_LIB_DIRS})
144 |       list(FIND _system_lib_dirs "${lib_dir}" isSystemDir)
145 |       if("${isSystemDir}" STREQUAL "-1")
146 |         foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS})
147 |           file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir})
148 |           if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
149 |             list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}")
150 |           else()
151 |             list(APPEND CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}")
152 |           endif()
153 |         endforeach()
154 |       endif()
155 |     endforeach()
156 |     if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "")
157 |       list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH)
158 |     endif()
159 |     set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE)
160 | 
161 |     # add the automatically determined parts of the RPATH
162 |     # which point to directories outside the build tree to the install RPATH
163 |     set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE)
164 | 
165 |   endif()
166 | 
167 | endfunction()
168 | 


--------------------------------------------------------------------------------
/cmake/AddUninstallTarget.cmake:
--------------------------------------------------------------------------------
 1 | #.rst:
 2 | # AddUninstallTarget
 3 | # ------------------
 4 | #
 5 | # Add the "uninstall" target for your project::
 6 | #
 7 | #   include(AddUninstallTarget)
 8 | #
 9 | #
10 | # will create a file cmake_uninstall.cmake in the build directory and add a
11 | # custom target uninstall that will remove the files installed by your package
12 | # (using install_manifest.txt)
13 | 
14 | #=============================================================================
15 | # Copyright 2008-2013 Kitware, Inc.
16 | # Copyright 2013 Istituto Italiano di Tecnologia (IIT)
17 | #   Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
18 | #
19 | # Distributed under the OSI-approved BSD License (the "License");
20 | # see accompanying file Copyright.txt for details.
21 | #
22 | # This software is distributed WITHOUT ANY WARRANTY; without even the
23 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
24 | # See the License for more information.
25 | #=============================================================================
26 | # (To distribute this file outside of CMake, substitute the full
27 | #  License text for the above reference.)
28 | 
29 | 
30 | if(DEFINED __ADD_UNINSTALL_TARGET_INCLUDED)
31 |   return()
32 | endif()
33 | set(__ADD_UNINSTALL_TARGET_INCLUDED TRUE)
34 | 
35 | 
36 | set(_filename ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
37 | 
38 | file(WRITE ${_filename}
39 | "if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\")
40 |     message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\")
41 |     return()
42 | endif()
43 | 
44 | file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files)
45 | string(STRIP \"\${files}\" files)
46 | string(REGEX REPLACE \"\\n\" \";\" files \"\${files}\")
47 | list(REVERSE files)
48 | foreach(file \${files})
49 |     message(STATUS \"Uninstalling: \$ENV{DESTDIR}\${file}\")
50 |     if(EXISTS \"\$ENV{DESTDIR}\${file}\")
51 |         execute_process(
52 |             COMMAND \${CMAKE_COMMAND} -E remove \"\$ENV{DESTDIR}\${file}\"
53 |             OUTPUT_VARIABLE rm_out
54 |             RESULT_VARIABLE rm_retval)
55 |         if(NOT \"\${rm_retval}\" EQUAL 0)
56 |             message(FATAL_ERROR \"Problem when removing \\\"\$ENV{DESTDIR}\${file}\\\"\")
57 |         endif()
58 |     else()
59 |         message(STATUS \"File \\\"\$ENV{DESTDIR}\${file}\\\" does not exist.\")
60 |     endif()
61 | endforeach(file)
62 | ")
63 | 
64 | if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)")
65 |     set(_uninstall "UNINSTALL-ALL")
66 | else()
67 |     set(_uninstall "uninstall-all")
68 | endif()
69 | add_custom_target(${_uninstall} COMMAND "${CMAKE_COMMAND}" -P "${_filename}")
70 | set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets")
71 | 


--------------------------------------------------------------------------------
/cmake/InstallBasicPackageFiles.cmake:
--------------------------------------------------------------------------------
  1 | #.rst:
  2 | # InstallBasicPackageFiles
  3 | # ------------------------
  4 | #
  5 | # A helper module to make your package easier to be found by other
  6 | # projects.
  7 | #
  8 | #
  9 | # .. command:: install_basic_package_files
 10 | #
 11 | # Create and install a basic version of cmake config files for your
 12 | # project::
 13 | #
 14 | #  install_basic_package_files(<Name>
 15 | #                              VERSION <version>
 16 | #                              COMPATIBILITY <compatibility>
 17 | #                              [EXPORT <export>] # (default = "<Name>")
 18 | #                              [FIRST_TARGET <target1>] # (default = "<Name>")
 19 | #                              [TARGETS <target1> <target2> ...]
 20 | #                              [TARGETS_PROPERTY <property_name>]
 21 | #                              [TARGETS_PROPERTIES <property1_name> <property2_name> ...]
 22 | #                              [NO_SET_AND_CHECK_MACRO]
 23 | #                              [NO_CHECK_REQUIRED_COMPONENTS_MACRO]
 24 | #                              [VARS_PREFIX <prefix>] # (default = "<Name>")
 25 | #                              [EXPORT_DESTINATION <destination>]
 26 | #                              [INSTALL_DESTINATION <destination>]
 27 | #                              [NAMESPACE <namespace>] # (default = "<Name>::")
 28 | #                              [EXTRA_PATH_VARS_SUFFIX path1 [path2 ...]]
 29 | #                              [CONFIG_TEMPLATE <file>]
 30 | #                              [UPPERCASE_FILENAMES | LOWERCASE_FILENAMES]
 31 | #                              [DEPENDENCIES <dependency1> "<dependency2> [...]" ...]
 32 | #                              [PRIVATE_DEPENDENCIES <dependency1> "<dependency2> [...]" ...]
 33 | #                              [INCLUDE_FILE <file>]
 34 | #                              [COMPONENT <component>] # (default = "<Name>")
 35 | #                              [NO_COMPATIBILITY_VARS]
 36 | #                             )
 37 | #
 38 | # Depending on UPPERCASE_FILENAMES and LOWERCASE_FILENAMES, this
 39 | # function generates 3 files:
 40 | #
 41 | #  - ``<Name>ConfigVersion.cmake`` or ``<name>-config-version.cmake``
 42 | #  - ``<Name>Config.cmake`` or ``<name>-config.cmake``
 43 | #  - ``<Name>Targets.cmake`` or ``<name>-targets.cmake``
 44 | #
 45 | # If neither ``UPPERCASE_FILENAMES`` nor ``LOWERCASE_FILENAMES`` is
 46 | # set, a file ``<Name>ConfigVersion.cmake.in`` or
 47 | # ``<name>-config-version.cmake.in`` is searched, and the convention
 48 | # is chosed according to the file found. If no file was found, the
 49 | # uppercase convention is used.
 50 | #
 51 | # The ``DEPENDENCIES`` argument can be used to set a list of dependencies
 52 | # that will be searched using the :command:`find_dependency` command
 53 | # from the :module:`CMakeFindDependencyMacro` module.
 54 | # Dependencies can be followed by any of the possible :command:`find_dependency`
 55 | # argument.
 56 | # In this case, all the arguments must be specified within double quotes (e.g.
 57 | # "<dependency> 1.0.0 EXACT", "<dependency> CONFIG").
 58 | # The ``PRIVATE_DEPENDENCIES`` argument is similar to ``DEPENDENCIES``, but
 59 | # these dependencies are included only when libraries are built ``STATIC``, i.e.
 60 | # if ``BUILD_SHARED_LIBS`` is ``OFF`` or if the ``TYPE`` property for one or
 61 | # more of the targets is ``STATIC_LIBRARY``.
 62 | # When using a custom template file, the ``@PACKAGE_DEPENDENCIES@``
 63 | # string is replaced with the code checking for the dependencies
 64 | # specified by these two argument.
 65 | #
 66 | # Each file is generated twice, one for the build directory and one for
 67 | # the installation directory.  The ``INSTALL_DESTINATION`` argument can be
 68 | # passed to install the files in a location different from the default
 69 | # one (``CMake`` on Windows, ``${CMAKE_INSTALL_LIBDIR}/cmake/${Name}``
 70 | # on other platforms.  The ``EXPORT_DESTINATION`` argument can be passed to
 71 | # generate the files in the build tree in a location different from the default
 72 | # one (``CMAKE_BINARY_DIR``).  If this is a relative path, it is considered
 73 | # relative to the ``CMAKE_BINARY_DIR`` directory.
 74 | #
 75 | # The ``<Name>ConfigVersion.cmake`` is generated using
 76 | # ``write_basic_package_version_file``.  The ``VERSION``,
 77 | # ``COMPATIBILITY``, ``NO_SET_AND_CHECK_MACRO``, and
 78 | # ``NO_CHECK_REQUIRED_COMPONENTS_MACRO`` are passed to this function
 79 | # and are used internally by :module:`CMakePackageConfigHelpers` module.
 80 | #
 81 | # ``VERSION`` shall be in the form ``<major>[.<minor>[.<patch>[.<tweak>]]]]``.
 82 | # If no ``VERSION`` is given, the ``PROJECT_VERSION`` variable is used.
 83 | # If this hasn’t been set, it errors out.  The ``VERSION`` argument is also used
 84 | # to replace the ``@PACKAGE_VERSION@`` string in the configuration file.
 85 | #
 86 | # ``COMPATIBILITY`` shall be any of ``<AnyNewerVersion|SameMajorVersion|
 87 | # ExactVersion>``.
 88 | # The ``COMPATIBILITY`` mode ``AnyNewerVersion`` means that the installed
 89 | # package version will be considered compatible if it is newer or exactly the
 90 | # same as the requested version. This mode should be used for packages which are
 91 | # fully backward compatible, also across major versions.
 92 | # If ``SameMajorVersion`` is used instead, then the behaviour differs from
 93 | # ``AnyNewerVersion`` in that the major version number must be the same as
 94 | # requested, e.g. version 2.0 will not be considered compatible if 1.0 is
 95 | # requested. This mode should be used for packages which guarantee backward
 96 | # compatibility within the same major version. If ``ExactVersion`` is used, then
 97 | # the package is only considered compatible if the requested version matches
 98 | # exactly its own version number (not considering the tweak version). For
 99 | # example, version 1.2.3 of a package is only considered compatible to requested
100 | # version 1.2.3. This mode is for packages without compatibility guarantees. If
101 | # your project has more elaborated version matching rules, you will need to
102 | # write your own custom ConfigVersion.cmake file instead of using this macro.
103 | #
104 | # By default ``install_basic_package_files`` also generates the two helper
105 | # macros ``set_and_check()`` and ``check_required_components()`` into the
106 | # ``<Name>Config.cmake`` file. ``set_and_check()`` should be used instead of the
107 | # normal set() command for setting directories and file locations. Additionally
108 | # to setting the variable it also checks that the referenced file or directory
109 | # actually exists and fails with a ``FATAL_ERROR`` otherwise. This makes sure
110 | # that the created ``<Name>Config.cmake`` file does not contain wrong
111 | # references. When using the ``NO_SET_AND_CHECK_MACRO, this macro is not
112 | # generated into the ``<Name>Config.cmake`` file.
113 | #
114 | # By default, ``install_basic_package_files`` append a call to
115 | # ``check_required_components(<Name>)`` in <Name>Config.cmake file if the
116 | # package supports components. This macro checks whether all requested,
117 | # non-optional components have been found, and if this is not the case, sets the
118 | # ``<Name>_FOUND`` variable to ``FALSE``, so that the package is considered to
119 | # be not found. It does that by testing the ``<Name>_<Component>_FOUND``
120 | # variables for all requested required components. When using the
121 | # ``NO_CHECK_REQUIRED_COMPONENTS_MACRO`` option, this macro is not generated
122 | # into the <Name>Config.cmake file.
123 | #
124 | # Finally, the files in the build and install directory are exactly the same.
125 | #
126 | # See the documentation of :module:`CMakePackageConfigHelpers` module for
127 | # further information and references therein.
128 | #
129 | #
130 | # The ``<Name>Config.cmake`` is generated using
131 | # ``configure_package_config_file``.  See the documentation for the
132 | # :module:`CMakePackageConfigHelpers` module for further information.
133 | # If the ``CONFIG_TEMPLATE`` argument is passed, the specified file
134 | # is used as template for generating the configuration file, otherwise
135 | # this module expects to find a ``<Name>Config.cmake.in`` or
136 | # ``<name>-config.cmake.in`` file either in the root directory of the
137 | # project or in current source directory.
138 | # If the file does not exist, a very basic file is created.
139 | #
140 | # A set of variables are checked and passed to
141 | # ``configure_package_config_file`` as ``PATH_VARS``. For each of the
142 | # ``SUFFIX`` considered, if one of the variables::
143 | #
144 | #     <VARS_PREFIX>_(BUILD|INSTALL)_<SUFFIX>
145 | #     (BUILD|INSTALL)_<VARS_PREFIX>_<SUFFIX>
146 | #
147 | # is defined, the ``<VARS_PREFIX>_<SUFFIX>`` variable will be defined
148 | # before configuring the package.  In order to use that variable in the
149 | # config file, you have to add a line::
150 | #
151 | #   set_and_check(<VARS_PREFIX>_<SUFFIX> \"@PACKAGE_<VARS_PREFIX>_<SUFFIX>@\")
152 | #
153 | # if the path must exist or just::
154 | #
155 | #   set(<VARS_PREFIX>_<SUFFIX> \"@PACKAGE_<VARS_PREFIX>_<SUFFIX>@\")
156 | #
157 | # if the path could be missing.
158 | #
159 | # These variable will have different values whether you are using the
160 | # package from the build tree or from the install directory.  Also these
161 | # files will contain only relative paths, meaning that you can move the
162 | # whole installation and the CMake files will still work.
163 | #
164 | # Default ``PATH_VARS`` suffixes are::
165 | #
166 | #   BINDIR          BIN_DIR
167 | #   SBINDIR         SBIN_DIR
168 | #   LIBEXECDIR      LIBEXEC_DIR
169 | #   SYSCONFDIR      SYSCONF_DIR
170 | #   SHAREDSTATEDIR  SHAREDSTATE_DIR
171 | #   LOCALSTATEDIR   LOCALSTATE_DIR
172 | #   LIBDIR          LIB_DIR
173 | #   INCLUDEDIR      INCLUDE_DIR
174 | #   OLDINCLUDEDIR   OLDINCLUDE_DIR
175 | #   DATAROOTDIR     DATAROOT_DIR
176 | #   DATADIR         DATA_DIR
177 | #   INFODIR         INFO_DIR
178 | #   LOCALEDIR       LOCALE_DIR
179 | #   MANDIR          MAN_DIR
180 | #   DOCDIR          DOC_DIR
181 | #
182 | # more suffixes can be added using the ``EXTRA_PATH_VARS_SUFFIX``
183 | # argument.
184 | #
185 | #
186 | # The ``<Name>Targets.cmake`` is generated using
187 | # :command:`export(TARGETS)` (if ``EXPORT`` or no options are used) or
188 | # :command:`export(TARGETS)` (if `EXPORT` is not used and one between
189 | # ``TARGETS``, ``TARGETS_PROPERTY``, or ``TARGETS_PROPERTIES`` is used) in the
190 | # build tree and :command:`install(EXPORT)` in the installation directory.
191 | # The targets are exported using the value for the ``NAMESPACE``
192 | # argument as namespace.
193 | # The export can be passed using the `EXPORT` argument.
194 | # The targets can be passed using the `TARGETS` argument or using one or more
195 | # global properties, that can be passed to the function using the
196 | # ``TARGETS_PROPERTY`` or ``TARGET_PROPERTIES`` arguments.
197 | #
198 | # If the ``NO_COMPATIBILITY_VARS`` argument is not set, the compatibility
199 | # variables ``<VARS_PREFIX>_LIBRARIES`` and ``<VARS_PREFIX>_INCLUDE_DIRS``
200 | # are set, trying to guess their correct values from the variables set or
201 | # from the arguments passed to this command. This argument is ignored if
202 | # the template file is not generated by this command.
203 | #
204 | # If the ``INCLUDE_FILE`` argument is passed, the content of the specified file
205 | # (which might be templated) is appended to the ``<Name>Config.cmake``.
206 | # This allows to inject custom code to this file, useful e.g. to set additional
207 | # variables which are loaded by downstream projects.
208 | #
209 | # If the ``COMPONENT`` argument is passed, it is forwarded to the
210 | # :command:`install` commands, otherwise <Name> is used.
211 | 
212 | #=============================================================================
213 | # Copyright 2013 Istituto Italiano di Tecnologia (IIT)
214 | #   Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
215 | #
216 | # Distributed under the OSI-approved BSD License (the "License");
217 | # see accompanying file Copyright.txt for details.
218 | #
219 | # This software is distributed WITHOUT ANY WARRANTY; without even the
220 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
221 | # See the License for more information.
222 | #=============================================================================
223 | # (To distribute this file outside of CMake, substitute the full
224 | #  License text for the above reference.)
225 | 
226 | 
227 | if(COMMAND install_basic_package_files)
228 |   return()
229 | endif()
230 | 
231 | 
232 | include(GNUInstallDirs)
233 | include(CMakePackageConfigHelpers)
234 | include(CMakeParseArguments)
235 | 
236 | 
237 | function(INSTALL_BASIC_PACKAGE_FILES _Name)
238 | 
239 |   # TODO check that _Name does not contain "-" characters
240 | 
241 |   set(_options NO_SET_AND_CHECK_MACRO
242 |                NO_CHECK_REQUIRED_COMPONENTS_MACRO
243 |                UPPERCASE_FILENAMES
244 |                LOWERCASE_FILENAMES
245 |                NO_COMPATIBILITY_VARS)
246 |   set(_oneValueArgs VERSION
247 |                     COMPATIBILITY
248 |                     EXPORT
249 |                     FIRST_TARGET
250 |                     TARGETS_PROPERTY
251 |                     VARS_PREFIX
252 |                     EXPORT_DESTINATION
253 |                     INSTALL_DESTINATION
254 |                     DESTINATION
255 |                     NAMESPACE
256 |                     CONFIG_TEMPLATE
257 |                     INCLUDE_FILE
258 |                     COMPONENT)
259 |   set(_multiValueArgs EXTRA_PATH_VARS_SUFFIX
260 |                       TARGETS
261 |                       TARGETS_PROPERTIES
262 |                       DEPENDENCIES
263 |                       PRIVATE_DEPENDENCIES)
264 |   cmake_parse_arguments(_IBPF "${_options}" "${_oneValueArgs}" "${_multiValueArgs}" "${ARGN}")
265 | 
266 |   if(NOT DEFINED _IBPF_VARS_PREFIX)
267 |     set(_IBPF_VARS_PREFIX ${_Name})
268 |   endif()
269 | 
270 |   if(NOT DEFINED _IBPF_VERSION)
271 |     message(FATAL_ERROR "VERSION argument is required")
272 |   endif()
273 | 
274 |   if(NOT DEFINED _IBPF_COMPATIBILITY)
275 |     message(FATAL_ERROR "COMPATIBILITY argument is required")
276 |   endif()
277 | 
278 |   if(_IBPF_UPPERCASE_FILENAMES AND _IBPF_LOWERCASE_FILENAMES)
279 |     message(FATAL_ERROR "UPPERCASE_FILENAMES and LOWERCASE_FILENAMES arguments cannot be used together")
280 |   endif()
281 | 
282 |   # Prepare install and export commands
283 |   set(_first_target ${_Name})
284 |   set(_targets ${_Name})
285 |   set(_install_cmd EXPORT ${_Name})
286 |   set(_export_cmd EXPORT ${_Name})
287 | 
288 |   if(DEFINED _IBPF_FIRST_TARGET)
289 |     if(DEFINED _IBPF_TARGETS OR DEFINED _IBPF_TARGETS_PROPERTIES OR DEFINED _IBPF_TARGETS_PROPERTIES)
290 |       message(FATAL_ERROR "EXPORT cannot be used with TARGETS, TARGETS_PROPERTY or TARGETS_PROPERTIES")
291 |     endif()
292 | 
293 |     set(_first_target ${_IBPF_FIRST_TARGET})
294 |     set(_targets ${_IBPF_FIRST_TARGET})
295 |   endif()
296 | 
297 |   if(DEFINED _IBPF_EXPORT)
298 |     if(DEFINED _IBPF_TARGETS OR DEFINED _IBPF_TARGETS_PROPERTIES OR DEFINED _IBPF_TARGETS_PROPERTIES)
299 |       message(FATAL_ERROR "EXPORT cannot be used with TARGETS, TARGETS_PROPERTY or TARGETS_PROPERTIES")
300 |     endif()
301 | 
302 |     set(_export_cmd EXPORT ${_IBPF_EXPORT})
303 |     set(_install_cmd EXPORT ${_IBPF_EXPORT})
304 | 
305 |   elseif(DEFINED _IBPF_TARGETS)
306 |     if(DEFINED _IBPF_TARGETS_PROPERTY OR DEFINED _IBPF_TARGETS_PROPERTIES)
307 |       message(FATAL_ERROR "TARGETS cannot be used with TARGETS_PROPERTY or TARGETS_PROPERTIES")
308 |     endif()
309 | 
310 |     set(_targets ${_IBPF_TARGETS})
311 |     set(_export_cmd TARGETS ${_IBPF_TARGETS})
312 |     list(GET _targets 0 _first_target)
313 | 
314 |   elseif(DEFINED _IBPF_TARGETS_PROPERTY)
315 |     if(DEFINED _IBPF_TARGETS_PROPERTIES)
316 |       message(FATAL_ERROR "TARGETS_PROPERTIES cannot be used with TARGETS_PROPERTIES")
317 |     endif()
318 | 
319 |     get_property(_targets GLOBAL PROPERTY ${_IBPF_TARGETS_PROPERTY})
320 |     set(_export_cmd TARGETS ${_targets})
321 |     list(GET _targets 0 _first_target)
322 | 
323 |   elseif(DEFINED _IBPF_TARGETS_PROPERTIES)
324 | 
325 |     unset(_targets)
326 |     foreach(_prop ${_IBPF_TARGETS_PROPERTIES})
327 |       get_property(_prop_val GLOBAL PROPERTY ${_prop})
328 |       list(APPEND _targets ${_prop_val})
329 |     endforeach()
330 |     set(_export_cmd TARGETS ${_targets})
331 |     list(GET _targets 0 _first_target)
332 | 
333 |   endif()
334 | 
335 |   # Path for installed cmake files
336 |   if(DEFINED _IBPF_DESTINATION)
337 |     message(DEPRECATION "DESTINATION is deprecated. Use INSTALL_DESTINATION instead")
338 |     if(NOT DEFINED _IBPF_INSTALL_DESTINATION)
339 |       set(_IBPF_INSTALL_DESTINATION ${_IBPF_DESTINATION})
340 |     endif()
341 |   endif()
342 | 
343 |   if(NOT DEFINED _IBPF_INSTALL_DESTINATION)
344 |     if(WIN32 AND NOT CYGWIN)
345 |       set(_IBPF_INSTALL_DESTINATION CMake)
346 |     else()
347 |       set(_IBPF_INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${_Name})
348 |     endif()
349 |   endif()
350 | 
351 |   if(NOT DEFINED _IBPF_EXPORT_DESTINATION)
352 |     set(_IBPF_EXPORT_DESTINATION "${CMAKE_BINARY_DIR}")
353 |   elseif(NOT IS_ABSOLUTE _IBPF_EXPORT_DESTINATION)
354 |     set(_IBPF_EXPORT_DESTINATION "${CMAKE_BINARY_DIR}/${_IBPF_EXPORT_DESTINATION}")
355 |   endif()
356 | 
357 |   if(NOT DEFINED _IBPF_NAMESPACE)
358 |     set(_IBPF_NAMESPACE "${_Name}::")
359 |   endif()
360 | 
361 |   if(NOT DEFINED _IBPF_COMPONENT)
362 |     set(_IBPF_COMPONENT "${_Name}")
363 |   endif()
364 | 
365 |   if(_IBPF_NO_SET_AND_CHECK_MACRO)
366 |     list(APPEND configure_package_config_file_extra_args NO_SET_AND_CHECK_MACRO)
367 |   endif()
368 | 
369 |   if(_IBPF_NO_CHECK_REQUIRED_COMPONENTS_MACRO)
370 |     list(APPEND configure_package_config_file_extra_args NO_CHECK_REQUIRED_COMPONENTS_MACRO)
371 |   endif()
372 | 
373 | 
374 | 
375 |   # Set input file for config, and ensure that _IBPF_UPPERCASE_FILENAMES
376 |   # and _IBPF_LOWERCASE_FILENAMES are set correctly
377 |   unset(_config_cmake_in)
378 |   set(_generate_file 0)
379 |   if(DEFINED _IBPF_CONFIG_TEMPLATE)
380 |     if(NOT EXISTS "${_IBPF_CONFIG_TEMPLATE}")
381 |       message(FATAL_ERROR "Config template file \"${_IBPF_CONFIG_TEMPLATE}\" not found")
382 |     endif()
383 |     set(_config_cmake_in "${_IBPF_CONFIG_TEMPLATE}")
384 |     if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES)
385 |       if("${_IBPF_CONFIG_TEMPLATE}" MATCHES "${_Name}Config.cmake.in")
386 |         set(_IBPF_UPPERCASE_FILENAMES 1)
387 |       elseif("${_IBPF_CONFIG_TEMPLATE}" MATCHES "${_name}-config.cmake.in")
388 |         set(_IBPF_LOWERCASE_FILENAMES 1)
389 |       else()
390 |         set(_IBPF_UPPERCASE_FILENAMES 1)
391 |       endif()
392 |     endif()
393 |   else()
394 |     string(TOLOWER "${_Name}" _name)
395 |     if(EXISTS "${CMAKE_SOURCE_DIR}/${_Name}Config.cmake.in")
396 |       set(_config_cmake_in "${CMAKE_SOURCE_DIR}/${_Name}Config.cmake.in")
397 |       if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES)
398 |         set(_IBPF_UPPERCASE_FILENAMES 1)
399 |       endif()
400 |     elseif(EXISTS "${CMAKE_SOURCE_DIR}/${_name}-config.cmake.in")
401 |       set(_config_cmake_in "${CMAKE_SOURCE_DIR}/${_name}-config.cmake.in")
402 |       if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES)
403 |         set(_IBPF_LOWERCASE_FILENAMES 1)
404 |       endif()
405 |     elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_Name}Config.cmake.in")
406 |       set(_config_cmake_in "${CMAKE_CURRENT_SOURCE_DIR}/${_Name}Config.cmake.in")
407 |       if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES)
408 |         set(_IBPF_UPPERCASE_FILENAMES 1)
409 |       endif()
410 |     elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_name}-config.cmake.in")
411 |       set(_config_cmake_in "${CMAKE_CURRENT_SOURCE_DIR}/${_name}-config.cmake.in")
412 |       if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES)
413 |         set(_IBPF_LOWERCASE_FILENAMES 1)
414 |       endif()
415 |     else()
416 |       set(_generate_file 1)
417 |       if(_IBPF_LOWERCASE_FILENAMES)
418 |         set(_config_cmake_in "${CMAKE_CURRENT_BINARY_DIR}/${_name}-config.cmake")
419 |       else()
420 |         set(_config_cmake_in "${CMAKE_CURRENT_BINARY_DIR}/${_Name}Config.cmake.in")
421 |         set(_IBPF_UPPERCASE_FILENAMES 1)
422 |       endif()
423 |     endif()
424 |   endif()
425 | 
426 |   # Set input file containing user variables
427 |   if(DEFINED _IBPF_INCLUDE_FILE)
428 |     if(NOT IS_ABSOLUTE "${_IBPF_INCLUDE_FILE}")
429 |       if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_IBPF_INCLUDE_FILE}")
430 |         set(_IBPF_INCLUDE_FILE "${CMAKE_CURRENT_SOURCE_DIR}/${_IBPF_INCLUDE_FILE}")
431 |       endif()
432 |     endif()
433 |     if(NOT EXISTS "${_IBPF_INCLUDE_FILE}")
434 |         message(FATAL_ERROR "File \"${_IBPF_INCLUDE_FILE}\" not found")
435 |     endif()
436 |     file(READ ${_IBPF_INCLUDE_FILE} _includedfile_user_content_in)
437 |     string(CONFIGURE ${_includedfile_user_content_in} _includedfile_user_content)
438 |     set(INCLUDED_FILE_CONTENT
439 | "#### Expanded from INCLUDE_FILE by install_basic_package_files() ####")
440 |     set(INCLUDED_FILE_CONTENT "${INCLUDED_FILE_CONTENT}\n\n${_includedfile_user_content}")
441 |     set(INCLUDED_FILE_CONTENT
442 | "${INCLUDED_FILE_CONTENT}
443 | #####################################################################")
444 |   endif()
445 | 
446 |   # Select output file names
447 |   if(_IBPF_UPPERCASE_FILENAMES)
448 |     set(_config_filename ${_Name}Config.cmake)
449 |     set(_version_filename ${_Name}ConfigVersion.cmake)
450 |     set(_targets_filename ${_Name}Targets.cmake)
451 |   elseif(_IBPF_LOWERCASE_FILENAMES)
452 |     set(_config_filename ${_name}-config.cmake)
453 |     set(_version_filename ${_name}-config-version.cmake)
454 |     set(_targets_filename ${_name}-targets.cmake)
455 |   endif()
456 | 
457 | 
458 |   # If the template config file does not exist, write a basic one
459 |   if(_generate_file)
460 |     # Generate the compatibility code
461 |     unset(_compatibility_vars)
462 |     if(NOT _IBPF_NO_COMPATIBILITY_VARS)
463 |       unset(_get_include_dir_code)
464 |       unset(_set_include_dir_code)
465 |       unset(_target_list)
466 |       foreach(_target ${_targets})
467 |         list(APPEND _target_list ${_IBPF_NAMESPACE}${_target})
468 |       endforeach()
469 |       if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_INCLUDEDIR OR
470 |          DEFINED BUILD_${_IBPF_VARS_PREFIX}_INCLUDEDIR OR
471 |          DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_INCLUDEDIR OR
472 |          DEFINED INSTALL_${_IBPF_VARS_PREFIX}_INCLUDEDIR)
473 |         set(_get_include_dir "set(${_IBPF_VARS_PREFIX}_INCLUDEDIR \"\@PACKAGE_${_IBPF_VARS_PREFIX}_INCLUDEDIR\@\")\n")
474 |         set(_set_include_dir "set(${_Name}_INCLUDE_DIRS \"\${${_IBPF_VARS_PREFIX}_INCLUDEDIR}\")")
475 |       elseif(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_INCLUDE_DIR OR
476 |              DEFINED BUILD_${_IBPF_VARS_PREFIX}_INCLUDE_DIR OR
477 |              DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_INCLUDE_DIR OR
478 |              DEFINED INSTALL_${_IBPF_VARS_PREFIX}_INCLUDE_DIR)
479 |         set(_get_include_dir "set(${_IBPF_VARS_PREFIX}_INCLUDE_DIR \"\@PACKAGE_${_IBPF_VARS_PREFIX}_INCLUDE_DIR\@\")\n")
480 |         set(_set_include_dir "set(${_Name}_INCLUDE_DIRS \"\${${_IBPF_VARS_PREFIX}_INCLUDE_DIR}\")")
481 |       else()
482 |         unset(_include_dir_list)
483 |         foreach(_target ${_targets})
484 |           set(_get_include_dir "${_get_include_dir}get_property(${_IBPF_VARS_PREFIX}_${_target}_INCLUDE_DIR TARGET ${_IBPF_NAMESPACE}${_target} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)\n")
485 |           list(APPEND _include_dir_list "\"\${${_IBPF_VARS_PREFIX}_${_target}_INCLUDE_DIR}\"")
486 |         endforeach()
487 |         string(REPLACE ";" " " _include_dir_list "${_include_dir_list}")
488 |         string(REPLACE ";" " " _target_list "${_target_list}")
489 |         set(_set_include_dir "set(${_Name}_INCLUDE_DIRS ${_include_dir_list})\nlist(REMOVE_DUPLICATES ${_Name}_INCLUDE_DIRS)")
490 |       endif()
491 |       set(_compatibility_vars "# Compatibility\n${_get_include_dir}\nset(${_Name}_LIBRARIES ${_target_list})\n${_set_include_dir}")
492 |     endif()
493 | 
494 |     # Write the file
495 |     file(WRITE "${_config_cmake_in}"
496 | "set(${_IBPF_VARS_PREFIX}_VERSION \@PACKAGE_VERSION\@)
497 | 
498 | \@PACKAGE_INIT\@
499 | 
500 | \@PACKAGE_DEPENDENCIES\@
501 | 
502 | if(NOT TARGET ${_IBPF_NAMESPACE}${_first_target})
503 |   include(\"\${CMAKE_CURRENT_LIST_DIR}/${_targets_filename}\")
504 | endif()
505 | 
506 | ${_compatibility_vars}
507 | 
508 | \@INCLUDED_FILE_CONTENT\@
509 | ")
510 |   endif()
511 | 
512 |   # Make relative paths absolute (needed later on) and append the
513 |   # defined variables to _(build|install)_path_vars_suffix
514 |   foreach(p BINDIR          BIN_DIR
515 |             SBINDIR         SBIN_DIR
516 |             LIBEXECDIR      LIBEXEC_DIR
517 |             SYSCONFDIR      SYSCONF_DIR
518 |             SHAREDSTATEDIR  SHAREDSTATE_DIR
519 |             LOCALSTATEDIR   LOCALSTATE_DIR
520 |             LIBDIR          LIB_DIR
521 |             INCLUDEDIR      INCLUDE_DIR
522 |             OLDINCLUDEDIR   OLDINCLUDE_DIR
523 |             DATAROOTDIR     DATAROOT_DIR
524 |             DATADIR         DATA_DIR
525 |             INFODIR         INFO_DIR
526 |             LOCALEDIR       LOCALE_DIR
527 |             MANDIR          MAN_DIR
528 |             DOCDIR          DOC_DIR
529 |             ${_IBPF_EXTRA_PATH_VARS_SUFFIX})
530 |     if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p})
531 |       list(APPEND _build_path_vars_suffix ${p})
532 |       list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}")
533 |     endif()
534 |     if(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p})
535 |       list(APPEND _build_path_vars_suffix ${p})
536 |       list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}")
537 |     endif()
538 |     if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p})
539 |       list(APPEND _install_path_vars_suffix ${p})
540 |       list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}")
541 |     endif()
542 |     if(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p})
543 |       list(APPEND _install_path_vars_suffix ${p})
544 |       list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}")
545 |     endif()
546 |   endforeach()
547 | 
548 | 
549 |   # <Name>ConfigVersion.cmake file (same for build tree and intall)
550 |   write_basic_package_version_file("${_IBPF_EXPORT_DESTINATION}/${_version_filename}"
551 |                                    VERSION ${_IBPF_VERSION}
552 |                                    COMPATIBILITY ${_IBPF_COMPATIBILITY})
553 |   install(FILES "${_IBPF_EXPORT_DESTINATION}/${_version_filename}"
554 |           DESTINATION ${_IBPF_INSTALL_DESTINATION}
555 |           COMPONENT ${_IBPF_COMPONENT})
556 | 
557 | 
558 |   # Prepare PACKAGE_DEPENDENCIES variable
559 |   set(_need_private_deps 0)
560 |   if(NOT BUILD_SHARED_LIBS)
561 |     set(_need_private_deps 1)
562 |   else()
563 |     foreach(_target ${_targets})
564 |       get_property(_type TARGET ${_target} PROPERTY TYPE)
565 |       if("${_type}" STREQUAL "STATIC_LIBRARY")
566 |         set(_need_private_deps 1)
567 |         break()
568 |       endif()
569 |     endforeach()
570 |   endif()
571 | 
572 |   unset(PACKAGE_DEPENDENCIES)
573 |   if(DEFINED _IBPF_DEPENDENCIES)
574 |     set(PACKAGE_DEPENDENCIES "#### Expanded from @PACKAGE_DEPENDENCIES@ by install_basic_package_files() ####\n\ninclude(CMakeFindDependencyMacro)\n")
575 | 
576 |     # FIXME When CMake 3.9 or greater is required, remove this madness and just
577 |     #       use find_dependency
578 |     if (CMAKE_VERSION VERSION_LESS 3.9)
579 |       string(APPEND PACKAGE_DEPENDENCIES "
580 | set(_${_Name}_FIND_PARTS_REQUIRED)
581 | if (${_Name}_FIND_REQUIRED)
582 |   set(_${_Name}_FIND_PARTS_REQUIRED REQUIRED)
583 | endif()
584 | set(_${_Name}_FIND_PARTS_QUIET)
585 | if (${_Name}_FIND_QUIETLY)
586 |   set(_${_Name}_FIND_PARTS_QUIET QUIET)
587 | endif()
588 | ")
589 | 
590 |       foreach(_dep ${_IBPF_DEPENDENCIES})
591 |         if("${_dep}" MATCHES ".+ .+")
592 |             string(REPLACE " " ";" _dep_list "${_dep}")
593 |             list(INSERT _dep_list 1 \${_${_Name}_FIND_PARTS_QUIET} \${_${_Name}_FIND_PARTS_REQUIRED})
594 |             string(REPLACE ";" " " _depx "${_dep_list}")
595 |             string(APPEND PACKAGE_DEPENDENCIES "find_package(${_depx})\n")
596 |         else()
597 |           string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n")
598 |         endif()
599 |       endforeach()
600 |       if(_need_private_deps)
601 |         foreach(_dep ${_IBPF_PRIVATE_DEPENDENCIES})
602 |           if("${_dep}" MATCHES ".+ .+")
603 |             string(REPLACE " " ";" _dep_list "${_dep}")
604 |             list(INSERT _dep_list 1 \${_${_Name}_FIND_PARTS_QUIET} \${_${_Name}_FIND_PARTS_REQUIRED})
605 |             string(REPLACE ";" "\n       " _depx "${_dep_list}")
606 |             string(APPEND PACKAGE_DEPENDENCIES "find_package(${_depx})\n")
607 |           else()
608 |             string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n")
609 |           endif()
610 |           endforeach()
611 |       endif()
612 | 
613 |     else()
614 | 
615 |       foreach(_dep ${_IBPF_DEPENDENCIES})
616 |         string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n")
617 |       endforeach()
618 |       if(_need_private_deps)
619 |         foreach(_dep ${_IBPF_PRIVATE_DEPENDENCIES})
620 |           string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n")
621 |         endforeach()
622 |       endif()
623 | 
624 |     endif()
625 | 
626 |     set(PACKAGE_DEPENDENCIES "${PACKAGE_DEPENDENCIES}\n###############################################################################\n")
627 |   endif()
628 | 
629 |   # Prepare PACKAGE_VERSION variable
630 |   set(PACKAGE_VERSION ${_IBPF_VERSION})
631 | 
632 |   # <Name>Config.cmake (build tree)
633 |   foreach(p ${_build_path_vars_suffix})
634 |     if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p})
635 |       set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_BUILD_${p}}")
636 |     elseif(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p})
637 |       set(${_IBPF_VARS_PREFIX}_${p} "${BUILD_${_IBPF_VARS_PREFIX}_${p}}")
638 |     endif()
639 |   endforeach()
640 |   configure_package_config_file("${_config_cmake_in}"
641 |                                 "${_IBPF_EXPORT_DESTINATION}/${_config_filename}"
642 |                                 INSTALL_DESTINATION ${_IBPF_EXPORT_DESTINATION}
643 |                                 PATH_VARS ${_build_path_vars}
644 |                                 ${configure_package_config_file_extra_args}
645 |                                 INSTALL_PREFIX ${CMAKE_BINARY_DIR})
646 | 
647 |   # <Name>Config.cmake (installed)
648 |   foreach(p ${_install_path_vars_suffix})
649 |     if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p})
650 |       set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_INSTALL_${p}}")
651 |     elseif(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p})
652 |       set(${_IBPF_VARS_PREFIX}_${p} "${INSTALL_${_IBPF_VARS_PREFIX}_${p}}")
653 |     endif()
654 |   endforeach()
655 |   configure_package_config_file("${_config_cmake_in}"
656 |                                 "${CMAKE_CURRENT_BINARY_DIR}/${_config_filename}.install"
657 |                                 INSTALL_DESTINATION ${_IBPF_INSTALL_DESTINATION}
658 |                                 PATH_VARS ${_install_path_vars}
659 |                                 ${configure_package_config_file_extra_args})
660 |   install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${_config_filename}.install"
661 |           DESTINATION ${_IBPF_INSTALL_DESTINATION}
662 |           RENAME ${_config_filename}
663 |           COMPONENT ${_IBPF_COMPONENT})
664 | 
665 | 
666 |   # <Name>Targets.cmake (build tree)
667 |   export(${_export_cmd}
668 |          NAMESPACE ${_IBPF_NAMESPACE}
669 |          FILE "${_IBPF_EXPORT_DESTINATION}/${_targets_filename}")
670 | 
671 |   # <Name>Targets.cmake (installed)
672 |   install(${_install_cmd}
673 |           NAMESPACE ${_IBPF_NAMESPACE}
674 |           DESTINATION ${_IBPF_INSTALL_DESTINATION}
675 |           FILE "${_targets_filename}"
676 |           COMPONENT ${_IBPF_COMPONENT})
677 | endfunction()
678 | 


--------------------------------------------------------------------------------
/matlab/casadi/accelerationConsistencyConstraint.m:
--------------------------------------------------------------------------------
 1 | function G = accelerationConsistencyConstraint(name, leftRightActivated, ...
 2 |                                                leftLocation, rightLocation, ...
 3 |                                                X, U, A)
 4 |    
 5 | px = X(1);
 6 | py = X(2);
 7 | pz = X(3);
 8 | x_copL = U(1);
 9 | y_copL = U(2);
10 | ul = U(3);
11 | x_copR = U(4);
12 | y_copR = U(5);
13 | ur = U(6);
14 | 
15 | constraint = A;
16 | 
17 | if (leftRightActivated(1))
18 |   constraint = constraint - [(px - leftLocation(1) - x_copL) * ul;
19 |                              (py - leftLocation(2) - y_copL) * ul;
20 |                              (pz - leftLocation(3)) * ul];
21 | end
22 | 
23 | if (leftRightActivated(2))
24 |   constraint = constraint - [(px - rightLocation(1) - x_copR) * ur;
25 |                              (py - rightLocation(2) - y_copR) * ur;
26 |                              (pz - rightLocation(3)) * ur];  
27 | end
28 | 
29 | G = casadi.Function(name, {X, U, A}, {constraint});
30 |                                            
31 | end


--------------------------------------------------------------------------------
/matlab/casadi/createDiscretizedFunction.m:
--------------------------------------------------------------------------------
 1 | function F = createDiscretizedFunction(name, rhs, x, u, dtInt)
 2 | 
 3 | f = casadi.Function(strcat(name, 'dynamics'), {x, u}, {rhs});
 4 | k1 = f(x, u);
 5 | k2 = f(x + dtInt/2 * k1, u);
 6 | k3 = f(x + dtInt/2 * k2, u);
 7 | k4 = f(x + dtInt * k3, u);
 8 | xf = x+dtInt/6*(k1 +2*k2 +2*k3 +k4);
 9 | 
10 | F = casadi.Function(name, {x, u, dtInt}, {xf});


--------------------------------------------------------------------------------
/matlab/casadi/doublePendulumVariableHeight.m:
--------------------------------------------------------------------------------
 1 | close all 
 2 | clear all
 3 | 
 4 | jump = false;
 5 | 
 6 | xL1 = [0.0; 0.15; 0.0];
 7 | xR1 = [0.0; -0.15; 0.0];
 8 | xL2 = [0.6; 0.15; 0.4];
 9 | xR2 = [0.6; -0.15; 0.4];
10 | 
11 | 
12 | initialState.position = [0.0; 0.0; 1.16];
13 | initialState.velocity = [0.0; 0.0; 0.0];
14 | 
15 | references.state.position = [0.6; 0.0; 1.56];
16 | references.state.velocity = [0.0; 0.0; 0.0];
17 | references.state.anticipation = 0.3;
18 | 
19 | constraints.cop = [-0.05, 0.05;
20 |                    -0.05, 0.05];
21 | constraints.legLength = 1.20;
22 | constraints.staticFriction = 0.5;
23 | constraints.torsionalFriction = 0.03;
24 | 
25 | references.control = [0.0;
26 |                       0.0;
27 |                       9.81/(2*(references.state.position(3) - xL2(3)));
28 |                       0.0;
29 |                       0.0;
30 |                       9.81/(2*(references.state.position(3) - xR2(3)))];
31 | 
32 | references.legLength = 1.18;
33 |                    
34 | references.timings = [0.6; 1.2; 0.8; 1.2; 0.6]; 
35 | 
36 | activeFeet = [true, true;
37 |               false, true;
38 |               true, true;
39 |               true, false;
40 |               true, true;];
41 |           
42 | if (jump)
43 |     activeFeet(3,:) = [false, false];              
44 | end
45 |     
46 | feetLocations = {xL1, xR1;
47 |                  xL1, xR1;
48 |                  xL2, xR1;
49 |                  xL2, xR1;
50 |                  xL2, xR2;};            
51 |              
52 | phase_length = 30;
53 | 
54 | numberOfPhases = size(activeFeet, 1);
55 | N = phase_length * numberOfPhases;
56 | 
57 | constraints.minimumTimings = 0.5 * ones(numberOfPhases,1);
58 | constraints.maximumTimings = 2.0 * ones(numberOfPhases,1);
59 | 
60 | weights.time = 1;
61 | weights.finalState = 10;
62 | weights.u = 0.1/N;
63 | weights.cop = 10/N;
64 | weights.controlVariation = 1/N;
65 | weights.finalControl = 1;
66 | weights.torques = 1/N;
67 | 
68 | tic
69 | [xsol, usol, t_sol] = solveVariableHeightDoublePendulum(initialState,...
70 |                                                         references, ...
71 |                                                         constraints, ...
72 |                                                         activeFeet, ...
73 |                                                         feetLocations, ...
74 |                                                         phase_length, ...
75 |                                                         weights);
76 | toc
77 | 
78 | plotOptiSolutionForDoublePendulum(xsol, usol, t_sol);
79 | 


--------------------------------------------------------------------------------
/matlab/casadi/doublePendulumVariableHeight3Phases.m:
--------------------------------------------------------------------------------
 1 | close all 
 2 | clear all
 3 | 
 4 | jump = false;
 5 | 
 6 | xL1 = [0.0; 0.15; 0.0];
 7 | xR1 = [0.0; -0.15; 0.0];
 8 | xL2 = [0.6; 0.15; 0.4];
 9 | xR2 = [0.6; -0.15; 0.4];
10 | 
11 | 
12 | initialState.position = [0.0; -0.08; 1.16];
13 | initialState.velocity = [0.1; -0.05; 0.0];
14 | 
15 | references.state.position = [0.6; 0.08; 1.56];
16 | references.state.velocity = [0.1; 0.05; 0.0];
17 | references.state.anticipation = 0.3;
18 | 
19 | constraints.cop = [-0.05, 0.05;
20 |                    -0.05, 0.05];
21 | constraints.legLength = 1.20;
22 | constraints.staticFriction = 0.5;
23 | constraints.torsionalFriction = 0.03;
24 | 
25 | references.control = [0.0;
26 |                       0.0;
27 |                       9.81/(references.state.position(3) - xL2(3));
28 |                       0.0;
29 |                       0.0;
30 |                       0.0];
31 | 
32 | references.legLength = 1.18;
33 |                    
34 | references.timings = [0.6; 1.2; 0.8;]; 
35 | 
36 | activeFeet = [false, true;
37 |               true, true;
38 |               true, false;];
39 |           
40 | if (jump)
41 |     activeFeet(2,:) = [false, false];              
42 | end
43 |     
44 | feetLocations = {xL1, xR1;
45 |                  xL2, xR1;
46 |                  xL2, xR1;};            
47 |              
48 | phase_length = 30;
49 | 
50 | numberOfPhases = size(activeFeet, 1);
51 | N = phase_length * numberOfPhases;
52 | 
53 | constraints.minimumTimings = 0.5 * ones(numberOfPhases,1);
54 | constraints.maximumTimings = 2.0 * ones(numberOfPhases,1);
55 | 
56 | weights.time = 1;
57 | weights.finalState = 10;
58 | weights.u = 0.1/N;
59 | weights.uMax = 0.1;
60 | weights.cop = 10/N;
61 | weights.controlVariation = 1/N;
62 | weights.finalControl = 1;
63 | weights.torques = 1/N;
64 | 
65 | tic
66 | [xsol, usol, a_sol, t_sol] = ...
67 | solveVariableHeightDoublePendulumAsIntegrator(initialState,...
68 |                                               references, ...
69 |                                               constraints, ...
70 |                                               activeFeet, ...
71 |                                               feetLocations, ...
72 |                                               phase_length, ...
73 |                                               weights);
74 | toc
75 | 
76 | plotOptiSolutionForDoublePendulumPlusAccelerations(xsol, usol, a_sol, t_sol);
77 | 


--------------------------------------------------------------------------------
/matlab/casadi/doublePendulumVariableHeightFeedback.m:
--------------------------------------------------------------------------------
 1 | close all 
 2 | clear all
 3 | 
 4 | jump = false;
 5 | 
 6 | xL1 = [0.0; 0.15; 0.0];
 7 | xR1 = [0.0; -0.15; 0.0];
 8 | xL2 = [0.6; 0.15; 0.4];
 9 | xR2 = [0.6; -0.15; 0.4];
10 | 
11 | 
12 | initialState.position = [0.0; 0.0; 1.16];
13 | initialState.velocity = [0.0; 0.0; 0.0];
14 | 
15 | references.state.position = [0.6; 0.0; 1.56];
16 | references.state.velocity = [0.0; 0.0; 0.0];
17 | references.state.anticipation = 0.3;
18 | 
19 | constraints.cop = [-0.05, 0.05;
20 |                    -0.05, 0.05];
21 | constraints.legLength = 1.20;
22 | constraints.staticFriction = 0.5;
23 | constraints.torsionalFriction = 0.03;
24 | 
25 | references.control = [0.0;
26 |                       0.0;
27 |                       9.81/(2*(references.state.position(3) - xL2(3)));
28 |                       0.0;
29 |                       0.0;
30 |                       9.81/(2*(references.state.position(3) - xR2(3)))];
31 | 
32 | references.legLength = 1.18;
33 |                    
34 | references.timings = [0.6; 1.2; 0.8; 1.2; 0.6]; 
35 | 
36 | activeFeet = [true, true;
37 |               false, true;
38 |               true, true;
39 |               true, false;
40 |               true, true;];
41 |           
42 | if (jump)
43 |     activeFeet(3,:) = [false, false];              
44 | end
45 |     
46 | feetLocations = {xL1, xR1;
47 |                  xL1, xR1;
48 |                  xL2, xR1;
49 |                  xL2, xR1;
50 |                  xL2, xR2;};            
51 |              
52 | phase_length = 30;
53 | 
54 | numberOfPhases = size(activeFeet, 1);
55 | N = phase_length * numberOfPhases;
56 | 
57 | constraints.minimumTimings = 0.5 * ones(numberOfPhases,1);
58 | constraints.maximumTimings = 2.0 * ones(numberOfPhases,1);
59 | 
60 | weights.time = 1;
61 | weights.finalState = 10;
62 | weights.u = 0.1/N;
63 | weights.cop = 10/N;
64 | weights.controlVariation = 1/N;
65 | weights.finalControl = 1;
66 | weights.torques = 1/N;
67 | weights.simplifiedControl = 100/N;
68 | weights.K = 0.01/N;
69 | weights.computedDesired = 0.01/N;
70 | weights.K_diff = 0.1/N;
71 | weights.computedDesiredDiff = 0.01/N;
72 | 
73 | tic
74 | [xsol, usol, a_sol, t_sol, K_sol, x_des_sol] = ...
75 | solveVariableHeightDoublePendulumFeedback(initialState,...
76 |                                           references, ...
77 |                                           constraints, ...
78 |                                           activeFeet, ...
79 |                                           feetLocations, ...
80 |                                           phase_length, ...
81 |                                           weights);
82 | toc
83 | 
84 | plotOptiSolutionForDoublePendulumFeedback(xsol, usol, a_sol,...
85 |                                           t_sol, K_sol, x_des_sol);
86 | 


--------------------------------------------------------------------------------
/matlab/casadi/doublePendulumVariableHeightIntegrator.m:
--------------------------------------------------------------------------------
 1 | close all 
 2 | clear all
 3 | 
 4 | jump = false;
 5 | 
 6 | xL1 = [0.0; 0.15; 0.0];
 7 | xR1 = [0.0; -0.15; 0.0];
 8 | xL2 = [0.6; 0.15; 0.4];
 9 | xR2 = [0.6; -0.15; 0.4];
10 | 
11 | 
12 | initialState.position = [0.0; 0.0; 1.16];
13 | initialState.velocity = [0.0; 0.0; 0.0];
14 | 
15 | references.state.position = [0.6; 0.0; 1.56];
16 | references.state.velocity = [0.0; 0.0; 0.0];
17 | references.state.anticipation = 0.3;
18 | 
19 | constraints.cop = [-0.05, 0.05;
20 |                    -0.05, 0.05];
21 | constraints.legLength = 1.20;
22 | constraints.staticFriction = 0.5;
23 | constraints.torsionalFriction = 0.1;
24 | 
25 | references.control = [0.0;
26 |                       0.0;
27 |                       9.81/(2*(references.state.position(3) - xL2(3)));
28 |                       0.0;
29 |                       0.0;
30 |                       9.81/(2*(references.state.position(3) - xR2(3)))];
31 | 
32 | references.legLength = 1.18;
33 |                    
34 | references.timings = [0.6; 1.2; 0.8; 1.2; 0.6]; 
35 | 
36 | activeFeet = [true, true;
37 |               false, true;
38 |               true, true;
39 |               true, false;
40 |               true, true;];
41 |           
42 | if (jump)
43 |     activeFeet(3,:) = [false, false];              
44 | end
45 |     
46 | feetLocations = {xL1, xR1;
47 |                  xL1, xR1;
48 |                  xL2, xR1;
49 |                  xL2, xR1;
50 |                  xL2, xR2;};            
51 |              
52 | phase_length = 30;
53 | 
54 | numberOfPhases = size(activeFeet, 1);
55 | N = phase_length * numberOfPhases;
56 | 
57 | constraints.minimumTimings = 0.5 * ones(numberOfPhases,1);
58 | constraints.maximumTimings = 2.0 * ones(numberOfPhases,1);
59 | 
60 | weights.time = 1;
61 | weights.finalState = 10;
62 | weights.u = 0.1/N;
63 | weights.uMax = 0.1;
64 | weights.cop = 10/N;
65 | weights.controlVariation = 1/N;
66 | weights.finalControl = 1;
67 | weights.torques = 1/N;
68 | 
69 | tic
70 | [xsol, usol, a_sol, t_sol] = ...
71 | solveVariableHeightDoublePendulumAsIntegrator(initialState,...
72 |                                               references, ...
73 |                                               constraints, ...
74 |                                               activeFeet, ...
75 |                                               feetLocations, ...
76 |                                               phase_length, ...
77 |                                               weights);
78 | toc
79 | 
80 | plotOptiSolutionForDoublePendulumPlusAccelerations(xsol, usol, a_sol, t_sol);
81 | 


--------------------------------------------------------------------------------
/matlab/casadi/getConstraints.m:
--------------------------------------------------------------------------------
 1 | function [G, upperBounds] = getConstraints(name, copLimits, legLength, ...
 2 |                                            staticFriction, torsionalFriction) 
 3 | 
 4 | footLocation = casadi.MX.sym('pFoot', 3);
 5 | footControl = casadi.MX.sym('foot_control', 3);
 6 | state = casadi.MX.sym('state', 6);
 7 | 
 8 | currentPosition = state(1:3);
 9 | 
10 | xCop = footControl(1);
11 | yCop = footControl(2);
12 | u = footControl(3);
13 | foot_cop = [xCop; yCop; 0];
14 | 
15 | forceDividedByMassAndU = (currentPosition - (footLocation + foot_cop)); %Being the mass and u positive quantities, while the upperbound is 0, they don't play a role
16 | 
17 | frictionBounds = [0;
18 |                   0;
19 |                   0];
20 |               
21 | A = [-yCop xCop 0];
22 | B = [0 0 torsionalFriction];
23 |           
24 | friction_value = [[1 1 -(staticFriction^2)] * (forceDividedByMassAndU).^2;
25 |                   (A-B) * forceDividedByMassAndU;
26 |                   (-A-B) * forceDividedByMassAndU];
27 |                  
28 |                  
29 | controlLimitsVector = [-copLimits(1,1);
30 |                         copLimits(1,2);
31 |                        -copLimits(2,1);
32 |                         copLimits(2,2)
33 |                         0];
34 | 
35 | controlLimitValue = [-xCop;
36 |                       xCop;
37 |                      -yCop;
38 |                       yCop;
39 |                      -u];
40 |                  
41 | leg_length_value = (currentPosition - footLocation)' * (currentPosition - footLocation);
42 |                  
43 | constraints = [friction_value; controlLimitValue; leg_length_value];
44 |               
45 | G = casadi.Function(name, {state, footControl, footLocation}, {constraints});
46 | 
47 | upperBounds = [frictionBounds;
48 |                controlLimitsVector;
49 |                legLength^2];                           
50 |                                                                            
51 | end


--------------------------------------------------------------------------------
/matlab/casadi/getIntegratorDynamics.m:
--------------------------------------------------------------------------------
 1 | function F = getIntegratorDynamics(x, a, dT)
 2 | p = x(1:3);
 3 | v = x(4:6);
 4 | g = zeros(3,1);
 5 | g(3) = -9.81;
 6 | rhs = [p + dT * v + 0.5 * dT^2 * (a + g);
 7 |        v + dT * (a + g)];
 8 |    
 9 | F = casadi.Function('integrator', {x, a, dT}, {rhs});  
10 | end


--------------------------------------------------------------------------------
/matlab/casadi/getPhaseDependentDynamics.m:
--------------------------------------------------------------------------------
 1 | function F = getPhaseDependentDynamics(phaseName, leftRightActivated, ...
 2 |                                        leftLocation, rightLocation, X, U, dT)
 3 | px = X(1);
 4 | py = X(2);
 5 | pz = X(3);
 6 | vx = X(4);
 7 | vy = X(5);
 8 | vz = X(6);
 9 | x_copL = U(1);
10 | y_copL = U(2);
11 | ul = U(3);
12 | x_copR = U(4);
13 | y_copR = U(5);
14 | ur = U(6);
15 | 
16 | rhs = [vx;
17 |        vy;
18 |        vz;
19 |        0.0;
20 |        0.0;
21 |        -9.81];
22 |    
23 | if (leftRightActivated(1))
24 |   rhs = rhs + [0.0;
25 |                0.0;
26 |                0.0;
27 |                (px - leftLocation(1) - x_copL) * ul;
28 |                (py - leftLocation(2) - y_copL) * ul;
29 |                (pz - leftLocation(3)) * ul];  
30 | end
31 | 
32 | if (leftRightActivated(2))
33 |   rhs = rhs + [0.0;
34 |                0.0;
35 |                0.0;
36 |                (px - rightLocation(1) - x_copR) * ur;
37 |                (py - rightLocation(2) - y_copR) * ur;
38 |                (pz - rightLocation(3)) * ur];  
39 | end
40 | 
41 | F = createDiscretizedFunction(phaseName, rhs, X, U, dT);
42 | end


--------------------------------------------------------------------------------
/matlab/casadi/plotOptiSolutionForDoublePendulum.m:
--------------------------------------------------------------------------------
 1 | function plotOptiSolutionForDoublePendulum(xsol, usol, t_sol)
 2 | time = 0.0;
 3 | numberOfPhases = length(t_sol);
 4 | phase_length = size(usol,2)/numberOfPhases;
 5 | for i = 1 : numberOfPhases
 6 |     time = [time, linspace(time(end) + t_sol(i)/phase_length, time(end) + t_sol(i), phase_length)];
 7 | end
 8 | 
 9 | figure
10 | ax = axes;
11 | plot(time, xsol(1:3,:));
12 | t = 0;
13 | for i = 1 : length(t_sol)
14 |     t = t + t_sol(i);
15 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
16 | end
17 | title("CoM Position")
18 | legend(["x", "y", "z"]);
19 | ylabel("[m]");
20 | xlabel("t [s]");
21 | 
22 | figure
23 | ax = axes;
24 | plot(time, xsol(4:6,:));
25 | t = 0;
26 | for i = 1 : length(t_sol)
27 |     t = t + t_sol(i);
28 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
29 | end
30 | title("CoM Velocity")
31 | legend(["x", "y", "z"]);
32 | ylabel("[m/s]");
33 | xlabel("t [s]");
34 | 
35 | figure
36 | ax = axes;
37 | t = 0;
38 | plot(time(2:end), usol(3,:), time(2:end), usol(6,:));
39 | for i = 1 : length(t_sol)
40 |     t = t + t_sol(i);
41 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
42 | end
43 | legend(["uLeft", "uRight"]);
44 | title("u")
45 | ylabel("[1/s^2]");
46 | xlabel("t [s]");
47 | 
48 | figure
49 | ax = axes;
50 | t = 0;
51 | plot(time(2:end), usol(1:2,:));
52 | for i = 1 : length(t_sol)
53 |     t = t + t_sol(i);
54 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
55 | end
56 | title("CoP Left")
57 | legend(["x", "y"]);
58 | ylabel("[m]");
59 | xlabel("t [s]");
60 | 
61 | figure
62 | ax = axes;
63 | t = 0;
64 | plot(time(2:end), usol(4:5,:));
65 | for i = 1 : length(t_sol)
66 |     t = t + t_sol(i);
67 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
68 | end
69 | title("CoP Right")
70 | legend(["x", "y"]);
71 | ylabel("[m]");
72 | xlabel("t [s]");
73 | end


--------------------------------------------------------------------------------
/matlab/casadi/plotOptiSolutionForDoublePendulumFeedback.m:
--------------------------------------------------------------------------------
  1 | function plotOptiSolutionForDoublePendulumFeedback(xsol, usol, a_sol, t_sol, k_sol, x_des_sol)
  2 | time = 0.0;
  3 | numberOfPhases = length(t_sol);
  4 | phase_length = size(usol,2)/numberOfPhases;
  5 | for i = 1 : numberOfPhases
  6 |     time = [time, linspace(time(end) + t_sol(i)/phase_length, time(end) + t_sol(i), phase_length)];
  7 | end
  8 | 
  9 | figure
 10 | ax = axes;
 11 | plot(time, xsol(1:3,:));
 12 | t = 0;
 13 | for i = 1 : length(t_sol)
 14 |     t = t + t_sol(i);
 15 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
 16 | end
 17 | title("CoM Position")
 18 | legend(["x", "y", "z"]);
 19 | ylabel("[m]");
 20 | xlabel("t [s]");
 21 | 
 22 | figure
 23 | ax = axes;
 24 | plot(time, xsol(4:6,:));
 25 | t = 0;
 26 | for i = 1 : length(t_sol)
 27 |     t = t + t_sol(i);
 28 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
 29 | end
 30 | title("CoM Velocity")
 31 | legend(["x", "y", "z"]);
 32 | ylabel("[m/s]");
 33 | xlabel("t [s]");
 34 | 
 35 | figure
 36 | ax = axes;
 37 | plot(time(2:end), a_sol(1:3,:));
 38 | t = 0;
 39 | for i = 1 : length(t_sol)
 40 |     t = t + t_sol(i);
 41 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
 42 | end
 43 | title("CoM Acceleration (without gravity)")
 44 | legend(["x", "y", "z"]);
 45 | ylabel("[m/s^2]");
 46 | xlabel("t [s]");
 47 | 
 48 | figure
 49 | ax = axes;
 50 | t = 0;
 51 | plot(time(2:end), usol(3,:), time(2:end), usol(6,:));
 52 | for i = 1 : length(t_sol)
 53 |     t = t + t_sol(i);
 54 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
 55 | end
 56 | legend(["uLeft", "uRight"]);
 57 | title("u")
 58 | ylabel("[1/s^2]");
 59 | xlabel("t [s]");
 60 | 
 61 | figure
 62 | ax = axes;
 63 | t = 0;
 64 | plot(time(2:end), usol(1:2,:));
 65 | for i = 1 : length(t_sol)
 66 |     t = t + t_sol(i);
 67 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
 68 | end
 69 | title("CoP Left")
 70 | legend(["x", "y"]);
 71 | ylabel("[m]");
 72 | xlabel("t [s]");
 73 | 
 74 | figure
 75 | ax = axes;
 76 | t = 0;
 77 | plot(time(2:end), usol(4:5,:));
 78 | for i = 1 : length(t_sol)
 79 |     t = t + t_sol(i);
 80 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
 81 | end
 82 | title("CoP Right")
 83 | legend(["x", "y"]);
 84 | ylabel("[m]");
 85 | xlabel("t [s]");
 86 | 
 87 | k_timed = zeros(6, length(time) - 1);
 88 | ff_timed = zeros(6, length(time) - 1);
 89 | 
 90 | for i = 1 : length(t_sol)
 91 |     for j = (i-1) * phase_length + 1 : i * phase_length
 92 |         k_timed(:, j) = k_sol(:, i);
 93 |         ff_timed(:, j) = x_des_sol(:, i);
 94 |     end
 95 | end
 96 | 
 97 | figure
 98 | ax = axes;
 99 | plot(time(2:end), k_timed(1:3,:));
100 | t = 0;
101 | for i = 1 : length(t_sol)
102 |     t = t + t_sol(i);
103 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
104 | end
105 | title("Position gains")
106 | legend(["x", "y", "z"]);
107 | ylabel("[1/s^2]");
108 | xlabel("t [s]");
109 | 
110 | figure
111 | ax = axes;
112 | plot(time(2:end), k_timed(4:6,:));
113 | t = 0;
114 | for i = 1 : length(t_sol)
115 |     t = t + t_sol(i);
116 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
117 | end
118 | title("Velocity gains")
119 | legend(["x", "y", "z"]);
120 | ylabel("[1/s]");
121 | xlabel("t [s]");
122 | 
123 | figure
124 | ax = axes;
125 | plot(time(2:end), ff_timed(1:3,:));
126 | t = 0;
127 | for i = 1 : length(t_sol)
128 |     t = t + t_sol(i);
129 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
130 | end
131 | title("Optimized Desired Position")
132 | legend(["x", "y", "z"]);
133 | ylabel("[m]");
134 | xlabel("t [s]");
135 | 
136 | figure
137 | ax = axes;
138 | plot(time(2:end), ff_timed(4:6,:));
139 | t = 0;
140 | for i = 1 : length(t_sol)
141 |     t = t + t_sol(i);
142 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
143 | end
144 | title("Optimized Desired Velocity")
145 | legend(["x", "y", "z"]);
146 | ylabel("[m/s]");
147 | xlabel("t [s]");
148 | 
149 | desiredAcceleration = zeros(3, length(time) - 1);
150 | for i = 1 : length(time) - 1 
151 |     gainMatrix = [diag(k_timed(1:3,i)), diag(k_timed(4:6,i))];
152 |     desiredAcceleration(:,i) = +[0;0;9.81] - gainMatrix * (xsol(:, i) - ff_timed(:, i));
153 | end
154 | 
155 | figure
156 | ax = axes;
157 | plot(time(2:end), desiredAcceleration(1:3,:));
158 | hold on
159 | plot(time(2:end), a_sol(1:3,:),'LineStyle','--');
160 | t = 0;
161 | for i = 1 : length(t_sol)
162 |     t = t + t_sol(i);
163 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
164 | end
165 | title("Optimized Desired Acceleration")
166 | legend(["x_{fb}", "y_{fb}", "z_{fb}", "x", "y", "z"]);
167 | ylabel("[m/s^2]");
168 | xlabel("t [s]");
169 | 
170 | end


--------------------------------------------------------------------------------
/matlab/casadi/plotOptiSolutionForDoublePendulumPlusAccelerations.m:
--------------------------------------------------------------------------------
 1 | function plotOptiSolutionForDoublePendulumPlusAccelerations(xsol, usol, a_sol, t_sol, k_sol, x_des_sol)
 2 | time = 0.0;
 3 | numberOfPhases = length(t_sol);
 4 | phase_length = size(usol,2)/numberOfPhases;
 5 | for i = 1 : numberOfPhases
 6 |     time = [time, linspace(time(end) + t_sol(i)/phase_length, time(end) + t_sol(i), phase_length)];
 7 | end
 8 | 
 9 | figure
10 | ax = axes;
11 | plot(time, xsol(1:3,:));
12 | t = 0;
13 | for i = 1 : length(t_sol)
14 |     t = t + t_sol(i);
15 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
16 | end
17 | title("CoM Position")
18 | legend(["x", "y", "z"]);
19 | ylabel("[m]");
20 | xlabel("t [s]");
21 | 
22 | figure
23 | ax = axes;
24 | plot(time, xsol(4:6,:));
25 | t = 0;
26 | for i = 1 : length(t_sol)
27 |     t = t + t_sol(i);
28 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
29 | end
30 | title("CoM Velocity")
31 | legend(["x", "y", "z"]);
32 | ylabel("[m/s]");
33 | xlabel("t [s]");
34 | 
35 | figure
36 | ax = axes;
37 | plot(time(2:end), a_sol(1:3,:));
38 | t = 0;
39 | for i = 1 : length(t_sol)
40 |     t = t + t_sol(i);
41 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
42 | end
43 | title("CoM Acceleration (without gravity)")
44 | legend(["x", "y", "z"]);
45 | ylabel("[m/s^2]");
46 | xlabel("t [s]");
47 | 
48 | figure
49 | ax = axes;
50 | t = 0;
51 | plot(time(2:end), usol(3,:), time(2:end), usol(6,:));
52 | for i = 1 : length(t_sol)
53 |     t = t + t_sol(i);
54 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
55 | end
56 | legend(["uLeft", "uRight"]);
57 | title("u")
58 | ylabel("[1/s^2]");
59 | xlabel("t [s]");
60 | 
61 | figure
62 | ax = axes;
63 | t = 0;
64 | plot(time(2:end), usol(1:2,:));
65 | for i = 1 : length(t_sol)
66 |     t = t + t_sol(i);
67 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
68 | end
69 | title("CoP Left")
70 | legend(["x", "y"]);
71 | ylabel("[m]");
72 | xlabel("t [s]");
73 | 
74 | figure
75 | ax = axes;
76 | t = 0;
77 | plot(time(2:end), usol(4:5,:));
78 | for i = 1 : length(t_sol)
79 |     t = t + t_sol(i);
80 |     line([t t],get(ax,'YLim'),'Color','k', 'LineStyle','--')
81 | end
82 | title("CoP Right")
83 | legend(["x", "y"]);
84 | ylabel("[m]");
85 | xlabel("t [s]");
86 | 
87 | end


--------------------------------------------------------------------------------
/matlab/casadi/solveVariableHeightDoublePendulum.m:
--------------------------------------------------------------------------------
 1 | function [xsol, usol, t_sol] = solveVariableHeightDoublePendulum(initialState, ...
 2 |                                                                  references, ...
 3 |                                                                  constraints, ...
 4 |                                                                  activeFeet, ...
 5 |                                                                  feetLocations, ...
 6 |                                                                  phase_length, ...
 7 |                                                                  weights)
 8 | 
 9 |                                                         
10 | assert(length(references.timings) == size(activeFeet, 1));
11 | assert(size(activeFeet, 1) == size(feetLocations, 1));
12 | assert(size(activeFeet,2) == 2);
13 | assert(size(feetLocations,2) == 2);
14 | 
15 | x = casadi.MX.sym('x', 6);
16 | u = casadi.MX.sym('u', 6);
17 | dtInt = casadi.MX.sym('dt');
18 | 
19 | [constraintsFcn, bounds] = getConstraints('constraints', constraints.cop, ...
20 |                                            constraints.legLength, constraints.staticFriction, ...
21 |                                            constraints.torsionalFriction);
22 | 
23 | numberOfPhases = size(activeFeet,1);
24 | 
25 | for k = 1 : numberOfPhases
26 |    F{k} = getPhaseDependentDynamics(strcat('F',num2str(k)), activeFeet(k,:), ...
27 |                                     feetLocations{k,1},feetLocations{k,2},...
28 |                                     x, u, dtInt);
29 | end
30 | 
31 | opti = casadi.Opti();
32 | 
33 | N = numberOfPhases * phase_length;
34 | 
35 | X = opti.variable(6, N + 1);
36 | U = opti.variable(6, N);
37 | 
38 | T = opti.variable(numberOfPhases);
39 | 
40 | opti.subject_to(X(:,1) == [initialState.position; initialState.velocity]);
41 | 
42 | torquesCost = casadi.MX.zeros(1);
43 | 
44 | for phase = 1 : numberOfPhases
45 |     
46 |     dt = T(phase)/phase_length;
47 | 
48 |     for k = (phase - 1) * phase_length + 1 : phase * phase_length
49 |         opti.subject_to(X(:,k+1)==F{phase}(X(:,k),U(:,k), dt));
50 |         
51 |         if (activeFeet(phase, 1))
52 |             opti.subject_to(constraintsFcn(X(:,k+1), U(1:3,k), feetLocations{phase,1}) <= bounds);
53 |             torquesCost = torquesCost + ((X(3, k+1) - feetLocations{phase,1}(3) - references.legLength)*U(3,k))^2;
54 |         else
55 |             opti.subject_to(U(1:3,k) == zeros(3,1));
56 |         end
57 |         
58 |         if (activeFeet(phase, 2))
59 |             opti.subject_to(constraintsFcn(X(:,k+1), U(4:6,k), feetLocations{phase,2}) <= bounds);
60 |             torquesCost = torquesCost + ((X(3, k+1) - feetLocations{phase,2}(3) - references.legLength)*U(6,k))^2;
61 |         else
62 |             opti.subject_to(U(4:6,k) == zeros(3,1));
63 |         end        
64 |     end
65 |     
66 | end
67 | 
68 | opti.subject_to(T >= constraints.minimumTimings);
69 | opti.subject_to(T <= constraints.maximumTimings);
70 | 
71 | opti.set_initial(T, references.timings);
72 | opti.set_initial(X(1, 2:end), linspace(initialState.position(1), references.state.position(1), N));
73 | opti.set_initial(X(3, 2:end), linspace(initialState.position(3), references.state.position(3), N));
74 | opti.set_initial(X(:,1), [initialState.position; initialState.velocity]);
75 | 
76 | opti.minimize(weights.time * (T - references.timings)' * (T - references.timings) ...
77 |     + weights.finalState * sumsqr(X(:,end - round(phase_length * references.state.anticipation) : end) - [references.state.position; references.state.velocity]) ...
78 |     + weights.u * (sumsqr(U(3,:)) + sumsqr(U(6,:))) ...
79 |     + weights.cop * (sumsqr(U(1:2,:)) + sumsqr(U(4:5,:))) ...
80 |     + weights.controlVariation * sumsqr(U(:,2:end) - U(:,1:end-1)) ...
81 |     + weights.finalControl * ((U(:,end) - references.control)' * (U(:,end) - references.control)) ...
82 |     + weights.torques * torquesCost);
83 | 
84 | options = struct;
85 | options.expand = true;
86 | options.ipopt.print_level = 0;
87 | options.ipopt.linear_solver='ma27';
88 |     
89 | opti.solver('ipopt', options);
90 | 
91 | sol = opti.solve();
92 | 
93 | xsol = sol.value(X);
94 | usol = sol.value(U);
95 | t_sol = sol.value(T);                                                
96 | 
97 | end
98 | 


--------------------------------------------------------------------------------
/matlab/casadi/solveVariableHeightDoublePendulumAsIntegrator.m:
--------------------------------------------------------------------------------
  1 | function [xsol, usol, a_sol, t_sol] = ...
  2 |     solveVariableHeightDoublePendulumAsIntegrator(initialState, ...
  3 |                                                   references, ...
  4 |                                                   constraints, ...
  5 |                                                   activeFeet, ...
  6 |                                                   feetLocations, ...
  7 |                                                   phase_length, ...
  8 |                                                   weights)
  9 | 
 10 |                                                         
 11 | assert(length(references.timings) == size(activeFeet, 1));
 12 | assert(size(activeFeet, 1) == size(feetLocations, 1));
 13 | assert(size(activeFeet,2) == 2);
 14 | assert(size(feetLocations,2) == 2);
 15 | 
 16 | x = casadi.MX.sym('x', 6);
 17 | u = casadi.MX.sym('u', 6);
 18 | a = casadi.MX.sym('a', 3);
 19 | dtInt = casadi.MX.sym('dt');
 20 | 
 21 | [footConstraints, bounds] = getConstraints('constraints', constraints.cop, ...
 22 |                                            constraints.legLength, constraints.staticFriction, ...
 23 |                                            constraints.torsionalFriction);
 24 | numberOfPhases = size(activeFeet,1);
 25 | 
 26 | for k = 1 : numberOfPhases
 27 |    accelerationConstraints{k} = ...
 28 |         accelerationConsistencyConstraint(strcat('accelerationConsistency',num2str(k)),...
 29 |                                           activeFeet(k,:), ...
 30 |                                           feetLocations{k,1}, feetLocations{k,2},...
 31 |                                           x, u, a);
 32 | end
 33 | 
 34 | F = getIntegratorDynamics(x, a, dtInt);
 35 | 
 36 | opti = casadi.Opti();
 37 | 
 38 | N = numberOfPhases * phase_length;
 39 | 
 40 | X = opti.variable(6, N + 1);
 41 | A = opti.variable(3,N);
 42 | U = opti.variable(6, N);
 43 | maxU = opti.variable();
 44 | 
 45 | T = opti.variable(numberOfPhases);
 46 | 
 47 | opti.subject_to(X(:,1) == [initialState.position; initialState.velocity]);
 48 | 
 49 | torquesCost = casadi.MX.zeros(1);
 50 | 
 51 | for phase = 1 : numberOfPhases
 52 |     
 53 |     dt = T(phase)/phase_length;
 54 |     
 55 |     for k = (phase - 1) * phase_length + 1 : phase * phase_length
 56 |         opti.subject_to(X(:,k+1)==F(X(:,k),A(:,k), dt));
 57 |         
 58 |         if (activeFeet(phase, 1))
 59 |             opti.subject_to(footConstraints(X(:,k+1), U(1:3,k), feetLocations{phase,1}) <= bounds);
 60 |             torquesCost = torquesCost + ((X(3, k+1) - feetLocations{phase,1}(3) - references.legLength)*U(3,k))^2;
 61 |             opti.subject_to(maxU - U(3,k) >= 0);
 62 |         else
 63 |             opti.subject_to(U(1:3,k) == zeros(3,1));
 64 |         end
 65 |         
 66 |         if (activeFeet(phase, 2))
 67 |             opti.subject_to(footConstraints(X(:,k+1), U(4:6,k), feetLocations{phase,2}) <= bounds);
 68 |             torquesCost = torquesCost + ((X(3, k+1) - feetLocations{phase,2}(3) - references.legLength)*U(6,k))^2;
 69 |             opti.subject_to(maxU - U(6,k) >= 0);
 70 |         else
 71 |             opti.subject_to(U(4:6,k) == zeros(3,1));
 72 |         end
 73 |         
 74 |         opti.subject_to(accelerationConstraints{phase}(X(:,k + 1), U(:,k), A(:,k)) == 0);
 75 |     end
 76 |     
 77 | end
 78 | 
 79 | opti.subject_to(T >= constraints.minimumTimings);
 80 | opti.subject_to(T <= constraints.maximumTimings);
 81 | 
 82 | opti.set_initial(T, references.timings);
 83 | points = linspace(0,1,N);
 84 | position_guess = initialState.position + points.*(references.state.position - initialState.position);
 85 | 
 86 | opti.set_initial(X(1:3, 2:end), position_guess(1:3,:));
 87 | opti.set_initial(X(:,1), [initialState.position; initialState.velocity]);
 88 | 
 89 | opti.minimize(weights.time * (T - references.timings)' * (T - references.timings) ...
 90 |     + weights.finalState * sumsqr(X(:,end - round(phase_length * references.state.anticipation) : end) - [references.state.position; references.state.velocity]) ...
 91 |     + weights.uMax * maxU ...
 92 |     + weights.u * (sumsqr(U(3,:)) + sumsqr(U(6,:))) ...
 93 |     + weights.cop * (sumsqr(U(1:2,:)) + sumsqr(U(4:5,:))) ...
 94 |     + weights.controlVariation * sumsqr(U(:,2:end) - U(:,1:end-1)) ...
 95 |     + weights.finalControl * ((U(:,end) - references.control)' * (U(:,end) - references.control)) ...
 96 |     + weights.torques * torquesCost);
 97 | 
 98 | options = struct;
 99 | options.expand = true;
100 | options.ipopt.print_level = 0;
101 | options.ipopt.linear_solver='ma27';
102 | options.ipopt.mu_strategy= 'adaptive';
103 |     
104 | opti.solver('ipopt', options);
105 | 
106 | sol = opti.solve();
107 | 
108 | xsol = sol.value(X);
109 | usol = sol.value(U);
110 | t_sol = sol.value(T); 
111 | a_sol = sol.value(A);
112 | end
113 | 


--------------------------------------------------------------------------------
/matlab/casadi/solveVariableHeightDoublePendulumFeedback.m:
--------------------------------------------------------------------------------
  1 | function [xsol, usol, a_sol, t_sol, K_sol, x_des_sol] = ...
  2 |     solveVariableHeightDoublePendulumFeedback(initialState, ...
  3 |                                               references, ...
  4 |                                               constraints, ...
  5 |                                               activeFeet, ...
  6 |                                               feetLocations, ...
  7 |                                               phase_length, ...
  8 |                                               weights)
  9 | 
 10 |                                                         
 11 | assert(length(references.timings) == size(activeFeet, 1));
 12 | assert(size(activeFeet, 1) == size(feetLocations, 1));
 13 | assert(size(activeFeet,2) == 2);
 14 | assert(size(feetLocations,2) == 2);
 15 | 
 16 | x = casadi.MX.sym('x', 6);
 17 | u = casadi.MX.sym('u', 6);
 18 | a = casadi.MX.sym('a', 3);
 19 | dtInt = casadi.MX.sym('dt');
 20 | 
 21 | [footConstraints, bounds] = getConstraints('constraints', constraints.cop, ...
 22 |                                            constraints.legLength, constraints.staticFriction, ...
 23 |                                            constraints.torsionalFriction);
 24 | numberOfPhases = size(activeFeet,1);
 25 | 
 26 | for k = 1 : numberOfPhases
 27 |    accelerationConstraints{k} = ...
 28 |         accelerationConsistencyConstraint(strcat('accelerationConsistency',num2str(k)),...
 29 |                                           activeFeet(k,:), ...
 30 |                                           feetLocations{k,1}, feetLocations{k,2},...
 31 |                                           x, u, a);
 32 | end
 33 | 
 34 | F = getIntegratorDynamics(x, a, dtInt);
 35 | 
 36 | opti = casadi.Opti();
 37 | 
 38 | N = numberOfPhases * phase_length;
 39 | 
 40 | X = opti.variable(6, N + 1);
 41 | A = opti.variable(3,N);
 42 | U = opti.variable(6, N);
 43 | 
 44 | T = opti.variable(numberOfPhases);
 45 | 
 46 | K  = opti.variable(6, numberOfPhases);
 47 | x_des = opti.variable(6, numberOfPhases);
 48 | 
 49 | opti.subject_to(X(:,1) == [initialState.position; initialState.velocity]);
 50 | 
 51 | torquesCost = casadi.MX.zeros(1);
 52 | simplifiedControlCost = casadi.MX.zeros(1);
 53 | 
 54 | g = [0;0;9.81]; 
 55 | 
 56 | for phase = 1 : numberOfPhases
 57 |     
 58 |     dt = T(phase)/phase_length;
 59 |     
 60 |     gainMatrix = [diag(K(1:3,phase)), diag(K(4:6,phase))];
 61 | 
 62 |     for k = (phase - 1) * phase_length + 1 : phase * phase_length
 63 |         opti.subject_to(X(:,k+1)==F(X(:,k),A(:,k), dt));
 64 |         
 65 |         if (activeFeet(phase, 1))
 66 |             opti.subject_to(footConstraints(X(:,k+1), U(1:3,k), feetLocations{phase,1}) <= bounds);
 67 |             torquesCost = torquesCost + ((X(3, k+1) - feetLocations{phase,1}(3) - references.legLength)*U(3,k))^2;
 68 |         else
 69 |             opti.subject_to(U(1:3,k) == zeros(3,1));
 70 |         end
 71 |         
 72 |         if (activeFeet(phase, 2))
 73 |             opti.subject_to(footConstraints(X(:,k+1), U(4:6,k), feetLocations{phase,2}) <= bounds);
 74 |             torquesCost = torquesCost + ((X(3, k+1) - feetLocations{phase,2}(3) - references.legLength)*U(6,k))^2;
 75 |         else
 76 |             opti.subject_to(U(4:6,k) == zeros(3,1));
 77 |         end
 78 |         
 79 |         opti.subject_to(accelerationConstraints{phase}(X(:,k + 1), U(:,k), A(:,k)) == 0);
 80 |         
 81 |         %opti.subject_to(A(:,k) ==  g - gainMatrix * (X(:,k) - x_des(:, phase)));
 82 |         simplifiedControlCost = simplifiedControlCost + ...
 83 |             (A(:,k) - g + gainMatrix * (X(:,k) - x_des(:, phase)))' * ...
 84 |             (A(:,k) - g + gainMatrix * (X(:,k) - x_des(:, phase))); %X(:,k) is supposed to be feedback
 85 |     end
 86 |     
 87 |     opti.subject_to(K(:, phase) >= 0.01);
 88 |     opti.subject_to(K(4:6, phase) >= 2*sqrt(K(1:3, phase)));
 89 | 
 90 |     
 91 | end
 92 | 
 93 | opti.subject_to(x_des(:, numberOfPhases) == [references.state.position; references.state.velocity]);
 94 | 
 95 | opti.subject_to(T >= constraints.minimumTimings);
 96 | opti.subject_to(T <= constraints.maximumTimings);
 97 | 
 98 | opti.set_initial(T, references.timings);
 99 | points = linspace(0,1,N);
100 | position_guess = initialState.position + points.*(references.state.position - initialState.position);
101 | 
102 | opti.set_initial(X(1, 2:end), position_guess(1,:));
103 | opti.set_initial(X(3, 2:end), position_guess(3,:));
104 | opti.set_initial(X(:,1), [initialState.position; initialState.velocity]);
105 | 
106 | for phase = 1 : numberOfPhases
107 |     if (phase ~= numberOfPhases)
108 |         meanPosition = mean(position_guess(1:3,(phase-1) * phase_length + 1 : phase * phase_length), 2);
109 |         opti.set_initial(x_des(1:3, phase),meanPosition);
110 |     end
111 | end
112 | 
113 | opti.set_initial(x_des(:, numberOfPhases), [references.state.position; references.state.velocity]);
114 | opti.set_initial(K, ones(6, numberOfPhases));
115 | 
116 | opti.minimize(weights.time * (T - references.timings)' * (T - references.timings) ...
117 |     + weights.finalState * sumsqr(X(:,end - round(phase_length * references.state.anticipation) : end) - [references.state.position; references.state.velocity]) ...
118 |     + weights.u * (sumsqr(U(3,:)) + sumsqr(U(6,:))) ...
119 |     + weights.cop * (sumsqr(U(1:2,:)) + sumsqr(U(4:5,:))) ...
120 |     + weights.controlVariation * sumsqr(U(:,2:end) - U(:,1:end-1)) ...
121 |     + weights.finalControl * ((U(:,end) - references.control)' * (U(:,end) - references.control)) ...
122 |     + weights.torques * torquesCost ...
123 |     + weights.simplifiedControl * simplifiedControlCost ...
124 |     + weights.K * sumsqr(K) ...
125 |     + weights.computedDesired * sumsqr(x_des) ...
126 |     + weights.K_diff * sumsqr(K(:, 2:end) - K(:, 1:end-1))...
127 |     + weights.computedDesiredDiff * sumsqr(x_des(:, 2:end) - x_des(:, 1:end-1)));
128 | 
129 | options = struct;
130 | options.expand = true;
131 | options.ipopt.print_level = 0;
132 | options.ipopt.linear_solver='ma27';
133 |     
134 | opti.solver('ipopt', options);
135 | 
136 | sol = opti.solve();
137 | 
138 | xsol = sol.value(X);
139 | usol = sol.value(U);
140 | t_sol = sol.value(T); 
141 | a_sol = sol.value(A);
142 | K_sol = sol.value(K);
143 | x_des_sol = sol.value(x_des);
144 | 
145 | end
146 | 


--------------------------------------------------------------------------------
/matlab/feedbackLinearization/controller.m:
--------------------------------------------------------------------------------
 1 | function [u, p] = controller(state, desiredState, Kp, Kd)
 2 |     
 3 | w = [state(1:3), -[1;0;0], -[0;1;0]];
 4 | 
 5 | desiredAcceleration = -[Kp, Kd] * (state - desiredState);
 6 | 
 7 | U = w\(desiredAcceleration + [0;0;9.81]);
 8 | 
 9 | u = U(1);
10 | if (abs(u) < 0.001)
11 |     p = zeros(2,1);
12 | else
13 |     p = U(2:3)/u;
14 | end
15 | end


--------------------------------------------------------------------------------
/matlab/feedbackLinearization/main.m:
--------------------------------------------------------------------------------
 1 | clear all
 2 | close all
 3 | clc
 4 | 
 5 | kp = 2*eye(3);
 6 | kd = 2 * sqrt(kp);
 7 | 
 8 | initialHeight = 1.16;
 9 | desiredHeight = 1.57;
10 | desiredFinalPosition = [0;0;desiredHeight];
11 | 
12 | initialPosition = [-0.3; -0.1; initialHeight];
13 | 
14 | w = [initialPosition, -[1;0;0], -[0;1;0]];
15 | 
16 | initialVelocity = kd^-1 *(- kp * (initialPosition - desiredFinalPosition) ...
17 |     + [0.0; 0.0; 9.81] - w * [0.7*9.81/initialHeight; 0; 0]);
18 | %initialVelocity = [0.57;0.34;0.0];
19 | 
20 | initialState = [initialPosition; initialVelocity];
21 | 
22 | desiredFinalVelocity = [0;0;0];
23 | 
24 | desiredState = [desiredFinalPosition;desiredFinalVelocity];
25 | 
26 | control = zeros(1,3);
27 | 
28 | n = 1000;
29 | 
30 | states = zeros(6, n);
31 | controls = zeros(3, n);
32 | t = zeros(1, n);
33 | 
34 | currentState = initialState;
35 | 
36 | for i = 1 : n
37 |     [u, p] = controller(currentState, desiredState, kp, kd);
38 |     
39 |     [~,x] = ode45(@(t,x)variableHeightPendulum(t, x, u, p), ...
40 |         [0, 0.01], currentState);
41 |     
42 |     currentState = x(end, :)';
43 |     
44 |     controls(:, i) = [u;p];
45 |     states(:, i) = currentState;
46 |     t (i) = 0.01 * i;
47 | end
48 | 
49 | figure
50 | plot(t, states(1:3, :));
51 | title("CoM Position")
52 | legend(["x", "y", "z"]);
53 | ylabel("[m]");
54 | xlabel("t [s]");
55 | 
56 | figure
57 | plot(t, states(4:6, :));
58 | title("CoM Velocity")
59 | legend(["x", "y", "z"]);
60 | ylabel("[m/s]");
61 | xlabel("t [s]");
62 | 
63 | 
64 | figure
65 | plot(t, controls(1,:));
66 | title("u")
67 | ylabel("[1/s^2]");
68 | xlabel("t [s]");
69 | 
70 | figure
71 | plot(t, controls(2:3,:));
72 | title("CoP")
73 | legend(["x", "y"]);
74 | ylabel("[m]");
75 | xlabel("t [s]");
76 | 
77 | 
78 | 


--------------------------------------------------------------------------------
/matlab/feedbackLinearization/variableHeightPendulum.m:
--------------------------------------------------------------------------------
1 | function x_dot = variableHeightPendulum(~, x, u, p)
2 | x_dot = zeros(6,1);
3 | x_dot(1:3) = x(4:6);
4 | x_dot(4:6) = -[0.0;0.0;9.81] + (x(1:3) - [p;0]) * u;
5 | end


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="2">
 4 |   <name>step_up_planner</name>
 5 |   <version>0.0.1</version>
 6 |   <description>Node for the communication with the StepUpPlanner</description>
 7 |   <maintainer email="sdafarra@ihmc.us">Stefano Dafarra</maintainer>
 8 |   <license>Apache License 2.0</license>
 9 |   <author>Stefano Dafarra</author>
10 | 
11 |   <buildtool_depend>ament_cmake</buildtool_depend>
12 | 
13 |   <build_depend>rclcpp</build_depend>
14 |   <build_depend>std_msgs</build_depend>
15 |   <build_depend>controller_msgs</build_depend>
16 | 
17 | 
18 |   <exec_depend>rclcpp</exec_depend>
19 |   <exec_depend>std_msgs</exec_depend>
20 |   <exec_depend>controller_msgs</exec_depend>
21 | 
22 | 
23 |   <export>
24 |     <build_type>ament_cmake</build_type>
25 |   </export>
26 | </package>
27 | 


--------------------------------------------------------------------------------
/src/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | add_subdirectory(core)
2 | 
3 | option(BUILD_INTERFACE "Build ROS2 nodes for communication" ON)
4 | if (BUILD_INTERFACE)
5 |     add_subdirectory(interface)
6 | endif()
7 | 


--------------------------------------------------------------------------------
/src/core/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | find_package(casadi REQUIRED)
2 | 
3 | ### Compile- and install-related commands.
4 | add_subdirectory(src)
5 | 
6 | if(BUILD_TESTING)
7 |     add_subdirectory(test)
8 | endif()
9 | 


--------------------------------------------------------------------------------
/src/core/src/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | 
 2 | set(LIBRARY_TARGET_NAME ${PROJECT_NAME}_core)
 3 | 
 4 | # List of CPP (source) library files.
 5 | set(${LIBRARY_TARGET_NAME}_SRC
 6 |         src/Control.cpp
 7 |         src/CostWeights.cpp
 8 |         src/Phase.cpp
 9 |         src/References.cpp
10 |         src/Settings.cpp
11 |         src/Solver.cpp
12 |         src/State.cpp
13 |         src/Step.cpp
14 |         src/Rotation.cpp
15 | )
16 | 
17 | # List of HPP (header) library files.
18 | set(${LIBRARY_TARGET_NAME}_HDR
19 |         include/StepUpPlanner/Control.h
20 |         include/StepUpPlanner/CostWeights.h
21 |         include/StepUpPlanner/Phase.h
22 |         include/StepUpPlanner/PhaseType.h
23 |         include/StepUpPlanner/References.h
24 |         include/StepUpPlanner/Settings.h
25 |         include/StepUpPlanner/SideDependentObject.h
26 |         include/StepUpPlanner/Solver.h
27 |         include/StepUpPlanner/State.h
28 |         include/StepUpPlanner/Step.h
29 |         include/StepUpPlanner/Rotation.h
30 | )
31 | 
32 | add_library(${LIBRARY_TARGET_NAME} ${${LIBRARY_TARGET_NAME}_SRC} ${${LIBRARY_TARGET_NAME}_HDR})
33 | 
34 | add_library(${PROJECT_NAME}::core ALIAS ${LIBRARY_TARGET_NAME})
35 | 
36 | set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES VERSION       ${${PROJECT_NAME}_VERSION}
37 |                                                         PUBLIC_HEADER "${${LIBRARY_TARGET_NAME}_HDR}")
38 | 
39 | target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
40 |                                                          "$<INSTALL_INTERFACE:$<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
41 | 
42 | target_link_libraries(${LIBRARY_TARGET_NAME} PUBLIC casadi)
43 | # If you used find_package() you need to use target_include_directories() and/or
44 | # target_link_libraries(). As explained previously, depending on the imported
45 | # objects, you may need to call either or both:
46 | # - with imported variable:
47 | #   target_include_directories(${LIBRARY_TARGET_NAME} ${FooPackage_INCLUDE_DIRS})
48 | #   target_link_libraries(${LIBRARY_TARGET_NAME} ${FooPackage_LIBRARIES})
49 | # - with imported target:
50 | #   target_link_libraries(${LIBRARY_TARGET_NAME} FooPackage_LIBRARIES::FooPackage_LIBRARIES)
51 | 
52 | # Specify installation targets, typology and destination folders.
53 | install(TARGETS ${LIBRARY_TARGET_NAME}
54 |         EXPORT  ${PROJECT_NAME}
55 |         LIBRARY       DESTINATION "${CMAKE_INSTALL_LIBDIR}"                            COMPONENT shlib
56 |         ARCHIVE       DESTINATION "${CMAKE_INSTALL_LIBDIR}"                            COMPONENT lib
57 |         RUNTIME       DESTINATION "${CMAKE_INSTALL_BINDIR}"                            COMPONENT bin
58 |         PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/${LIBRARY_TARGET_NAME}" COMPONENT dev)
59 | 
60 | message(STATUS "Created target ${LIBRARY_TARGET_NAME} for export ${PROJECT_NAME}.")
61 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/Control.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_CONTROL_H
 2 | #define STEPUPPLANNER_CONTROL_H
 3 | 
 4 | #include <casadi/casadi.hpp>
 5 | #include <StepUpPlanner/SideDependentObject.h>
 6 | 
 7 | namespace StepUpPlanner {
 8 |     class FootControl;
 9 |     class Control;
10 | }
11 | 
12 | class StepUpPlanner::FootControl {
13 | 
14 |     casadi::DM m_multiplier;
15 |     casadi::DM m_CoP;
16 | 
17 | public:
18 | 
19 |     FootControl();
20 | 
21 |     ~FootControl();
22 | 
23 |     //in foot coordinates
24 |     casadi::DM &cop();
25 | 
26 |     const casadi::DM &cop() const;
27 | 
28 |     double cop(size_t i) const;
29 | 
30 |     void setCoP(double copX, double copY);
31 | 
32 |     casadi::DM &multiplier();
33 | 
34 |     const casadi::DM &multiplier() const;
35 | 
36 |     void setMultiplier(double multiplier);
37 | 
38 |     void zero();
39 | };
40 | 
41 | class StepUpPlanner::Control {
42 | 
43 |     StepUpPlanner::SideDependentObject<StepUpPlanner::FootControl> m_controls;
44 |     casadi::DM m_acceleration;
45 | 
46 | public:
47 | 
48 |     Control();
49 | 
50 |     ~Control();
51 | 
52 |     const casadi::DM& acceleration() const;
53 | 
54 |     casadi::DM& acceleration();
55 | 
56 |     double acceleration(size_t i) const;
57 | 
58 |     StepUpPlanner::FootControl &left();
59 | 
60 |     const StepUpPlanner::FootControl &left() const;
61 | 
62 |     StepUpPlanner::FootControl &right();
63 | 
64 |     const StepUpPlanner::FootControl &right() const;
65 | 
66 |     void zero();
67 | };
68 | 
69 | #endif // STEPUPPLANNER_CONTROL_H
70 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/CostWeights.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_COSTWEIGHTS_H
 2 | #define STEPUPPLANNER_COSTWEIGHTS_H
 3 | 
 4 | namespace StepUpPlanner {
 5 |     class CostWeights;
 6 | }
 7 | 
 8 | class StepUpPlanner::CostWeights {
 9 | public:
10 | 
11 |     double durationsDifference;
12 |     double finalStateError;
13 |     double multipliers;
14 |     double maxMultiplier;
15 |     double cop;
16 |     double controlVariations;
17 |     double finalControl;
18 |     double torques;
19 |     double maxTorques;
20 | 
21 |     CostWeights();
22 | };
23 | 
24 | #endif // STEPUPPLANNER_COSTWEIGHTS_H
25 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/Phase.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_PHASE_H
 2 | #define STEPUPPLANNER_PHASE_H
 3 | 
 4 | #include <casadi/casadi.hpp>
 5 | #include <StepUpPlanner/SideDependentObject.h>
 6 | #include <StepUpPlanner/Step.h>
 7 | #include <StepUpPlanner/PhaseType.h>
 8 | #include <StepUpPlanner/State.h>
 9 | #include <StepUpPlanner/Control.h>
10 | #include <StepUpPlanner/Rotation.h>
11 | #include <vector>
12 | 
13 | namespace StepUpPlanner {
14 |     class Phase;
15 | }
16 | 
17 | class StepUpPlanner::Phase {
18 | 
19 |     casadi::DM m_duration;
20 |     double m_minDuration, m_maxDuration, m_desiredDuration;
21 | 
22 |     StepUpPlanner::SideDependentObject<StepUpPlanner::Step> m_steps;
23 |     StepUpPlanner::PhaseType m_phase;
24 | 
25 |     std::vector<StepUpPlanner::State> m_statesSolution;
26 |     std::vector<StepUpPlanner::Control> m_controlsSolution;
27 | 
28 | public:
29 | 
30 |     Phase();
31 | 
32 |     Phase(StepUpPlanner::PhaseType phase);
33 | 
34 |     //Use null pointer in case the corresponding foot is not on the ground
35 |     //Object are copied
36 |     Phase(const StepUpPlanner::Step *left, const StepUpPlanner::Step *right);
37 | 
38 |     ~Phase();
39 | 
40 |     bool setDurationSettings(double minimumDuration, double maximumDuration, double desiredDuration);
41 | 
42 |     void setLeftPosition(double px, double py, double pz);
43 | 
44 |     void setRightPosition(double px, double py, double pz);
45 | 
46 |     StepUpPlanner::PhaseType getPhaseType() const;
47 | 
48 |     const casadi::DM& duration() const;
49 | 
50 |     casadi::DM& duration();
51 | 
52 |     double minDuration() const;
53 | 
54 |     double maxDuration() const;
55 | 
56 |     double desiredDuration() const;
57 | 
58 |     //Returning only the position. In this way, it should not be possible to change the vertices (thus the number of constraints), once the solver object is created
59 |     casadi::DM &leftPosition();
60 | 
61 |     const casadi::DM &leftPosition() const;
62 | 
63 |     double leftPosition(size_t i) const;
64 | 
65 |     casadi::DM &rightPosition();
66 | 
67 |     const casadi::DM &rightPosition() const;
68 | 
69 |     double rightPosition(size_t i) const;
70 | 
71 |     StepUpPlanner::Rotation &leftRotation();
72 | 
73 |     const StepUpPlanner::Rotation &leftRotation() const;
74 | 
75 |     StepUpPlanner::Rotation &rightRotation();
76 | 
77 |     const StepUpPlanner::Rotation &rightRotation() const;
78 | 
79 |     const StepUpPlanner::Step& getLeftStep() const;
80 | 
81 |     const StepUpPlanner::Step& getRightStep() const;
82 | 
83 |     const std::vector<StepUpPlanner::State>& states() const;
84 | 
85 |     std::vector<StepUpPlanner::State>& states();
86 | 
87 |     const std::vector<StepUpPlanner::Control>& controls() const;
88 | 
89 |     std::vector<StepUpPlanner::Control>& controls();
90 | 
91 | };
92 | 
93 | #endif // STEPUPPLANNER_PHASE_H
94 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/PhaseType.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_PHASETYPE_H
 2 | #define STEPUPPLANNER_PHASETYPE_H
 3 | 
 4 | namespace StepUpPlanner {
 5 |     enum class PhaseType {
 6 |         SINGLE_SUPPORT_LEFT,
 7 |         SINGLE_SUPPORT_RIGHT,
 8 |         DOUBLE_SUPPORT,
 9 |         FLYING,
10 |         UNDEFINED
11 |     };
12 | }
13 | 
14 | #endif // STEPUPPLANNER_PHASETYPE_H
15 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/References.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_REFERENCES_H
 2 | #define STEPUPPLANNER_REFERENCES_H
 3 | 
 4 | #include <StepUpPlanner/State.h>
 5 | #include <StepUpPlanner/Control.h>
 6 | 
 7 | namespace StepUpPlanner {
 8 |     class References;
 9 | }
10 | 
11 | class StepUpPlanner::References {
12 | 
13 |     StepUpPlanner::State m_desiredState;
14 |     StepUpPlanner::Control m_desiredControl;
15 | 
16 |     double m_desiredLegLength;
17 | 
18 | 
19 | public:
20 | 
21 |     References();
22 | 
23 |     ~References();
24 | 
25 |     bool setDesiredLegLength(double desiredLegLength);
26 | 
27 |     double getDesiredLength() const;
28 | 
29 |     StepUpPlanner::State &desiredState();
30 | 
31 |     const StepUpPlanner::State &desiredState() const;
32 | 
33 |     StepUpPlanner::Control &desiredControl();
34 | 
35 |     const StepUpPlanner::Control &desiredControl() const;
36 | 
37 |     void zero();
38 | 
39 | };
40 | 
41 | 
42 | #endif // STEPUPPLANNER_REFERENCES_H
43 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/Rotation.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_ROTATION_H
 2 | #define STEPUPPLANNER_ROTATION_H
 3 | 
 4 | #include <casadi/casadi.hpp>
 5 | 
 6 | namespace StepUpPlanner {
 7 |     class Rotation;
 8 | }
 9 | 
10 | class StepUpPlanner::Rotation {
11 | 
12 |     casadi::DM m_rotation, m_quaternion;
13 |     casadi::DM skew(double i, double j, double k);
14 | 
15 |     void computeQuaternion();
16 | 
17 | public:
18 | 
19 |     Rotation();
20 | 
21 |     Rotation(const casadi::DM& matrix);
22 | 
23 |     void setFromQuaternion(double realPart, double i, double j, double k);
24 | 
25 |     const casadi::DM& asMatrix() const;
26 | 
27 |     const casadi::DM& asQuaternion() const;
28 | 
29 |     double asQuaternion(size_t i) const;
30 | 
31 |     static Rotation Identity();
32 | };
33 | 
34 | #endif // STEPUPPLANNER_ROTATION_H
35 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/Settings.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_SETTINGS_H
 2 | #define STEPUPPLANNER_SETTINGS_H
 3 | 
 4 | #include <StepUpPlanner/CostWeights.h>
 5 | #include <string>
 6 | 
 7 | namespace StepUpPlanner {
 8 |     class Settings;
 9 | }
10 | 
11 | class StepUpPlanner::Settings {
12 | 
13 |     double m_maximumLegLength;
14 |     double m_minimumLegLength;
15 |     double m_staticFrictionCoefficient;
16 |     double m_torsionalFrictionCoefficient;
17 | 
18 |     double m_finalStateAnticipation;
19 | 
20 |     unsigned long m_phaseLength;
21 | 
22 |     std::string m_solverName;
23 |     unsigned long m_solverVerbosity;
24 | 
25 |     StepUpPlanner::CostWeights m_costWeights;
26 | 
27 | public:
28 | 
29 |     Settings();
30 | 
31 |     ~Settings();
32 | 
33 |     bool setMaximumLegLength(double maxLength);
34 | 
35 |     double getMaximumLegLength() const;
36 | 
37 |     bool setMinimumLegLength(double minLength);
38 | 
39 |     double getMinimumLegLength() const;
40 | 
41 |     bool setLegLengthSettings(double minLength, double maxLength);
42 | 
43 |     bool setStaticFrictionCoefficient(double staticFrictionCoeff);
44 | 
45 |     double getStaticFrictionCoefficient() const;
46 | 
47 |     bool setTorsionalFrictionCoefficient(double torsionalFrictionCoeff);
48 | 
49 |     double getTorsionalFrictionCoefficient() const;
50 | 
51 |     unsigned long &phaseLength();
52 | 
53 |     unsigned long phaseLength() const;
54 | 
55 |     bool setFinalStateAnticipation(double finalStateAnticipation); //The percentage of the last phase in which the error from the desired state is considered
56 | 
57 |     double getFinalStateAnticipation() const;
58 | 
59 |     //See https://www.coin-or.org/Ipopt/documentation/node51.html#SECTION0001111010000000000000 for options
60 |     bool setIpoptLinearSolver(const std::string& solverName);
61 | 
62 |     const std::string& getIpoptLinearSolver() const;
63 | 
64 |     unsigned long& solverVerbosity();
65 | 
66 |     unsigned long solverVerbosity() const;
67 | 
68 |     StepUpPlanner::CostWeights& costWeights();
69 | 
70 |     const StepUpPlanner::CostWeights& costWeights() const;
71 | 
72 | };
73 | 
74 | #endif //  STEPUPPLANNER_SETTINGS_H
75 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/SideDependentObject.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_SIDEDEPENDENTOBJECT_H
 2 | #define STEPUPPLANNER_SIDEDEPENDENTOBJECT_H
 3 | 
 4 | namespace StepUpPlanner {
 5 |     template<typename Object>
 6 |     class SideDependentObject;
 7 | }
 8 | 
 9 | template <typename Object>
10 | class StepUpPlanner::SideDependentObject {
11 | public:
12 |     Object left;
13 |     Object right;
14 | 
15 |     SideDependentObject() { }
16 | 
17 |     SideDependentObject(const Object& newLeft, const Object& newRight)
18 |         : left(newLeft)
19 |           ,right(newRight)
20 |     { }
21 | 
22 |     void operator=(const SideDependentObject<Object>& other) {
23 |         left = other.left;
24 |         right = other.right;
25 |     }
26 | 
27 |     ~SideDependentObject() { }
28 | };
29 | 
30 | #endif // STEPUPPLANNER_SIDEDEPENDENTOBJECT_H
31 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/Solver.h:
--------------------------------------------------------------------------------
  1 | #ifndef STEPUPPLANNER_SOLVER_H
  2 | #define STEPUPPLANNER_SOLVER_H
  3 | 
  4 | #include <casadi/casadi.hpp>
  5 | #include <StepUpPlanner/Phase.h>
  6 | #include <StepUpPlanner/Settings.h>
  7 | #include <StepUpPlanner/References.h>
  8 | #include <StepUpPlanner/State.h>
  9 | #include <StepUpPlanner/Control.h>
 10 | #include <StepUpPlanner/Step.h>
 11 | #include <StepUpPlanner/SideDependentObject.h>
 12 | #include <vector>
 13 | #include <string>
 14 | #include <memory>
 15 | 
 16 | namespace StepUpPlanner {
 17 |     class Solver;
 18 | }
 19 | 
 20 | class StepUpPlanner::Solver {
 21 | 
 22 |     struct PhaseData {
 23 |         StepUpPlanner::Phase phase;
 24 |         SideDependentObject<bool> isActive;
 25 |         SideDependentObject<casadi::Function> feetConstraints;
 26 |         SideDependentObject<casadi::DM> feetConstraintsBounds;
 27 |         casadi::Function accelerationConsistencyConstraint;
 28 |         SideDependentObject<casadi::MX> feetLocationParameter;
 29 |         SideDependentObject<casadi::MX> feetOrientationParameter;
 30 |         casadi::MX minDurationParameter, maxDurationParameter;
 31 |     };
 32 | 
 33 |     enum class SolverState {
 34 |         NOT_INITIALIZED,
 35 |         PROBLEM_SET,
 36 |         PROBLEM_SOLVED
 37 |     };
 38 | 
 39 |     SolverState m_solverState;
 40 | 
 41 |     std::vector<PhaseData> m_phases;
 42 | 
 43 |     StepUpPlanner::Settings m_settings;
 44 | 
 45 |     casadi::Function m_integratorDynamics;
 46 | 
 47 |     casadi::MX m_initialStateParameter, m_desiredLegLengthParameter;
 48 |     casadi::MX m_referenceTimings, m_referenceStateParameter, m_referenceControlParameter;
 49 | 
 50 |     casadi::MX m_X, m_U, m_A, m_T, m_uMax, m_tauMax;
 51 |     casadi::DM m_Xsol, m_Usol, m_Asol, m_Tsol;
 52 |     bool m_useMaxU, m_useMaxTau;
 53 | 
 54 |     casadi::Opti m_opti;
 55 | 
 56 |     std::unique_ptr<casadi::OptiSol> m_solution;
 57 | 
 58 |     casadi::DM m_linSpacedPoints;
 59 | 
 60 |     casadi::Function getIntegratorDynamics();
 61 | 
 62 |     void createFeetConstraintsFunction(const std::string& name, const StepUpPlanner::Step& step,
 63 |                                        casadi::Function &outputFunction, casadi::DM &outputBounds);
 64 | 
 65 |     casadi::Function getAccelerationConsistencyConstraintFunction(const std::string &name, const StepUpPlanner::SideDependentObject<bool> &isActive);
 66 | 
 67 |     bool fillPhaseDataVector(const std::vector<StepUpPlanner::Phase>& phases);
 68 | 
 69 |     void setupOpti();
 70 | 
 71 |     bool setupProblem(const std::vector<StepUpPlanner::Phase>& phases, const StepUpPlanner::Settings& settings);
 72 | 
 73 |     void setParametersValue(const StepUpPlanner::State &initialState, const References &references);
 74 | 
 75 |     void setInitialValues(const StepUpPlanner::References& references);
 76 | 
 77 |     void fillSolution();
 78 | 
 79 | public:
 80 | 
 81 |     Solver();
 82 | 
 83 |     Solver(const std::vector<StepUpPlanner::Phase>& phases, const StepUpPlanner::Settings& settings);
 84 | 
 85 |     Solver(const Solver& other) = delete;
 86 | 
 87 |     Solver(Solver&& other) = delete;
 88 | 
 89 |     ~Solver();
 90 | 
 91 |     bool resetProblem(const std::vector<StepUpPlanner::Phase>& phases, const StepUpPlanner::Settings& settings);
 92 | 
 93 |     size_t numberOfPhases() const;
 94 | 
 95 |     StepUpPlanner::Phase& getPhase(size_t i); //Use this to change the feet positions from one iteration to the other
 96 | 
 97 |     const StepUpPlanner::Phase& getPhase(size_t i) const;
 98 | 
 99 |     bool solve(const StepUpPlanner::State& initialState, const StepUpPlanner::References& references);
100 | 
101 |     bool getFullSolution(std::vector<StepUpPlanner::Phase>& phases) const;
102 | 
103 |     void clear();
104 | 
105 |     bool isReady() const;
106 | };
107 | 
108 | #endif // STEPUPPLANNER_SOLVER_H
109 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/State.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_STATE_H
 2 | #define STEPUPPLANNER_STATE_H
 3 | 
 4 | #include <casadi/casadi.hpp>
 5 | 
 6 | namespace StepUpPlanner {
 7 |     class State;
 8 | }
 9 | 
10 | class StepUpPlanner::State {
11 | 
12 |     casadi::DM m_position;
13 |     casadi::DM m_velocity;
14 | 
15 | public:
16 | 
17 |     State();
18 | 
19 |     ~State();
20 | 
21 |     void setPosition(double px, double py, double pz);
22 | 
23 |     void setVelocity(double vx, double vy, double vz);
24 | 
25 |     casadi::DM& position();
26 | 
27 |     double position(size_t i) const;
28 | 
29 |     const casadi::DM& position() const;
30 | 
31 |     casadi::DM& velocity();
32 | 
33 |     double velocity(size_t i) const;
34 | 
35 |     const casadi::DM& velocity() const;
36 | 
37 |     void zero();
38 | };
39 | 
40 | #endif // STEPUPPLANNER_STATE_H
41 | 


--------------------------------------------------------------------------------
/src/core/src/include/StepUpPlanner/Step.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_STEP_H
 2 | #define STEPUPPLANNER_STEP_H
 3 | 
 4 | #include <casadi/casadi.hpp>
 5 | #include <StepUpPlanner/Rotation.h>
 6 | #include <vector>
 7 | 
 8 | namespace StepUpPlanner {
 9 |     struct Vertex;
10 |     class Step;
11 | }
12 | 
13 | struct StepUpPlanner::Vertex {
14 |     double x;
15 |     double y;
16 | };
17 | 
18 | class StepUpPlanner::Step {
19 | 
20 |     casadi::DM m_position;
21 |     StepUpPlanner::Rotation m_rotation;
22 |     std::vector<StepUpPlanner::Vertex> m_footVertices;
23 |     std::vector<StepUpPlanner::Vertex> m_scaledFootVertices;
24 |     double m_scaleFactor;
25 |     double m_xOffset, m_yOffset;
26 |     casadi::MX m_edgeConstraints;
27 | 
28 |     casadi::DM m_copBounds;
29 |     casadi::Function m_copConstraints;
30 | 
31 |     void scaleFootVertices(double scale, const std::vector<StepUpPlanner::Vertex>& vertices, double xOffset, double yOffset);
32 | 
33 |     bool computeCoPConstraints(const std::vector<StepUpPlanner::Vertex>& vertices);
34 | 
35 | public:
36 | 
37 |     Step();
38 | 
39 |     Step(double px, double py, double pz);
40 | 
41 |     Step(double px, double py, double pz, const std::vector<StepUpPlanner::Vertex>& vertices);
42 | 
43 |     ~Step();
44 | 
45 |     void setPosition(double px, double py, double pz);
46 | 
47 |     bool setVertices(const std::vector<StepUpPlanner::Vertex>& vertices, double scale = 1.0, double xOffset = 0.0, double yOffset = 0.0);
48 | 
49 |     const std::vector<StepUpPlanner::Vertex>& getOriginalVertices() const;
50 | 
51 |     const std::vector<StepUpPlanner::Vertex>& getScaledVertices() const;
52 | 
53 |     casadi::DM &position();
54 | 
55 |     const casadi::DM &position() const;
56 | 
57 |     StepUpPlanner::Rotation& rotation();
58 | 
59 |     const StepUpPlanner::Rotation& rotation() const;
60 | 
61 |     casadi::DM getCoPBounds() const;
62 | 
63 |     casadi::Function getCoPConstraintsFunction() const;
64 | 
65 |     void clear();
66 | 
67 | };
68 | 
69 | #endif // STEPUPPLANNER_STEP_H
70 | 


--------------------------------------------------------------------------------
/src/core/src/src/Control.cpp:
--------------------------------------------------------------------------------
  1 | #include <StepUpPlanner/Control.h>
  2 | #include <cassert>
  3 | 
  4 | StepUpPlanner::FootControl::FootControl()
  5 |     : m_multiplier(1,1)
  6 |       , m_CoP(2,1)
  7 | { }
  8 | 
  9 | StepUpPlanner::FootControl::~FootControl()
 10 | { }
 11 | 
 12 | casadi::DM &StepUpPlanner::FootControl::cop()
 13 | {
 14 |     return m_CoP;
 15 | }
 16 | 
 17 | const casadi::DM &StepUpPlanner::FootControl::cop() const
 18 | {
 19 |     return m_CoP;
 20 | }
 21 | 
 22 | double StepUpPlanner::FootControl::cop(size_t i) const
 23 | {
 24 |     assert(i < 2);
 25 |     return static_cast<double>(m_CoP(i));
 26 | }
 27 | 
 28 | void StepUpPlanner::FootControl::setCoP(double copX, double copY)
 29 | {
 30 |     m_CoP(0) = copX;
 31 |     m_CoP(1) = copY;
 32 | }
 33 | 
 34 | casadi::DM &StepUpPlanner::FootControl::multiplier()
 35 | {
 36 |     return m_multiplier;
 37 | }
 38 | 
 39 | const casadi::DM &StepUpPlanner::FootControl::multiplier() const
 40 | {
 41 |     return m_multiplier;
 42 | }
 43 | 
 44 | void StepUpPlanner::FootControl::setMultiplier(double multiplier)
 45 | {
 46 |     m_multiplier = multiplier;
 47 | }
 48 | 
 49 | void StepUpPlanner::FootControl::zero()
 50 | {
 51 |     m_multiplier = 0.0;
 52 |     m_CoP(0) = 0.0;
 53 |     m_CoP(1) = 0.0;
 54 | }
 55 | 
 56 | StepUpPlanner::Control::Control()
 57 |     : m_acceleration(3,1)
 58 | { }
 59 | 
 60 | 
 61 | StepUpPlanner::Control::~Control()
 62 | { }
 63 | 
 64 | const casadi::DM &StepUpPlanner::Control::acceleration() const
 65 | {
 66 |     return m_acceleration;
 67 | }
 68 | 
 69 | casadi::DM &StepUpPlanner::Control::acceleration()
 70 | {
 71 |     return m_acceleration;
 72 | }
 73 | 
 74 | double StepUpPlanner::Control::acceleration(size_t i) const
 75 | {
 76 |     assert(i < 3);
 77 |     return static_cast<double>(m_acceleration(i));
 78 | }
 79 | 
 80 | StepUpPlanner::FootControl &StepUpPlanner::Control::left()
 81 | {
 82 |     return m_controls.left;
 83 | }
 84 | 
 85 | const StepUpPlanner::FootControl &StepUpPlanner::Control::left() const
 86 | {
 87 |     return m_controls.left;
 88 | }
 89 | 
 90 | StepUpPlanner::FootControl &StepUpPlanner::Control::right()
 91 | {
 92 |     return m_controls.right;
 93 | }
 94 | 
 95 | const StepUpPlanner::FootControl &StepUpPlanner::Control::right() const
 96 | {
 97 |     return m_controls.right;
 98 | }
 99 | 
100 | void StepUpPlanner::Control::zero()
101 | {
102 |     m_acceleration(0) = 0.0;
103 |     m_acceleration(1) = 0.0;
104 |     m_acceleration(2) = 0.0;
105 |     m_controls.left.zero();
106 |     m_controls.right.zero();
107 | }
108 | 


--------------------------------------------------------------------------------
/src/core/src/src/CostWeights.cpp:
--------------------------------------------------------------------------------
 1 | #include <StepUpPlanner/CostWeights.h>
 2 | 
 3 | StepUpPlanner::CostWeights::CostWeights()
 4 |     : durationsDifference(0.0)
 5 |       , finalStateError(0.0)
 6 |       , multipliers(0.0)
 7 |       , cop(0.0)
 8 |       , controlVariations(0.0)
 9 |       , finalControl(0.0)
10 |       , torques(0.0)
11 |       , maxTorques(0.0)
12 | {}
13 | 


--------------------------------------------------------------------------------
/src/core/src/src/Phase.cpp:
--------------------------------------------------------------------------------
  1 | #include <StepUpPlanner/Phase.h>
  2 | #include <iostream>
  3 | #include <cassert>
  4 | 
  5 | StepUpPlanner::Phase::Phase()
  6 |     : m_duration(1,1)
  7 |       ,m_minDuration(0.5)
  8 |       , m_maxDuration(2.0)
  9 |       , m_desiredDuration(1.0)
 10 |       , m_phase(StepUpPlanner::PhaseType::UNDEFINED)
 11 | { }
 12 | 
 13 | StepUpPlanner::Phase::Phase(PhaseType phase)
 14 |     : m_duration(1,1)
 15 |       ,m_minDuration(0.5)
 16 |       , m_maxDuration(2.0)
 17 |       , m_desiredDuration(1.0)
 18 |       , m_phase(phase)
 19 | { }
 20 | 
 21 | StepUpPlanner::Phase::Phase(const Step *left, const Step *right)
 22 |     : m_duration(1,1)
 23 |       ,m_minDuration(0.5)
 24 |       , m_maxDuration(2.0)
 25 |       , m_desiredDuration(1.0)
 26 | {
 27 |     if (left && right) {
 28 |         m_phase = StepUpPlanner::PhaseType::DOUBLE_SUPPORT;
 29 |         m_steps.left = *left;
 30 |         m_steps.right = *right;
 31 |     } else if (left) {
 32 |         m_phase = StepUpPlanner::PhaseType::SINGLE_SUPPORT_LEFT;
 33 |         m_steps.left = *left;
 34 |     } else if (right) {
 35 |         m_phase = StepUpPlanner::PhaseType::SINGLE_SUPPORT_RIGHT;
 36 |         m_steps.right = *right;
 37 |     } else {
 38 |         m_phase = StepUpPlanner::PhaseType::FLYING;
 39 |     }
 40 | 
 41 | }
 42 | 
 43 | StepUpPlanner::Phase::~Phase()
 44 | {
 45 | 
 46 | }
 47 | 
 48 | bool StepUpPlanner::Phase::setDurationSettings(double minimumDuration, double maximumDuration, double desiredDuration)
 49 | {
 50 |     if (minimumDuration > maximumDuration) {
 51 |         std::cerr << "[ERROR][StepUpPlanner::Phase::setDurationSettings] The minimumDuration is greater than the maximumDuration." << std::endl;
 52 |         return false;
 53 |     }
 54 | 
 55 |     if ((desiredDuration < minimumDuration) || (desiredDuration > maximumDuration)) {
 56 |         std::cerr << "[ERROR][StepUpPlanner::Phase::setDurationSettings] The desiredDuration is not in the interval [minimumDuration, maximumDuration]." << std::endl;
 57 |         return false;
 58 |     }
 59 | 
 60 |     m_minDuration = minimumDuration;
 61 |     m_maxDuration = maximumDuration;
 62 |     m_desiredDuration = desiredDuration;
 63 | 
 64 |     return true;
 65 | }
 66 | 
 67 | void StepUpPlanner::Phase::setLeftPosition(double px, double py, double pz)
 68 | {
 69 |     m_steps.left.setPosition(px, py, pz);
 70 | }
 71 | 
 72 | void StepUpPlanner::Phase::setRightPosition(double px, double py, double pz)
 73 | {
 74 |     m_steps.right.setPosition(px, py, pz);
 75 | }
 76 | 
 77 | StepUpPlanner::PhaseType StepUpPlanner::Phase::getPhaseType() const
 78 | {
 79 |     return m_phase;
 80 | }
 81 | 
 82 | const casadi::DM &StepUpPlanner::Phase::duration() const
 83 | {
 84 |     return m_duration;
 85 | }
 86 | 
 87 | casadi::DM &StepUpPlanner::Phase::duration()
 88 | {
 89 |     return m_duration;
 90 | }
 91 | 
 92 | double StepUpPlanner::Phase::minDuration() const
 93 | {
 94 |     return m_minDuration;
 95 | }
 96 | 
 97 | double StepUpPlanner::Phase::maxDuration() const
 98 | {
 99 |     return m_maxDuration;
100 | }
101 | 
102 | double StepUpPlanner::Phase::desiredDuration() const
103 | {
104 |     return m_desiredDuration;
105 | }
106 | 
107 | casadi::DM &StepUpPlanner::Phase::leftPosition()
108 | {
109 |     return m_steps.left.position();
110 | }
111 | 
112 | const casadi::DM &StepUpPlanner::Phase::leftPosition() const
113 | {
114 |     return m_steps.left.position();
115 | }
116 | 
117 | double StepUpPlanner::Phase::leftPosition(size_t i) const
118 | {
119 |     assert(i < 3);
120 |     return static_cast<double>(m_steps.left.position()(i));
121 | }
122 | 
123 | casadi::DM &StepUpPlanner::Phase::rightPosition()
124 | {
125 |     return m_steps.right.position();
126 | }
127 | 
128 | const casadi::DM &StepUpPlanner::Phase::rightPosition() const
129 | {
130 |     return m_steps.right.position();
131 | }
132 | 
133 | double StepUpPlanner::Phase::rightPosition(size_t i) const
134 | {
135 |     assert(i < 3);
136 |     return static_cast<double>(m_steps.right.position()(i));
137 | }
138 | 
139 | StepUpPlanner::Rotation &StepUpPlanner::Phase::leftRotation()
140 | {
141 |     return m_steps.left.rotation();
142 | }
143 | 
144 | const StepUpPlanner::Rotation &StepUpPlanner::Phase::leftRotation() const
145 | {
146 |     return m_steps.left.rotation();
147 | }
148 | 
149 | StepUpPlanner::Rotation &StepUpPlanner::Phase::rightRotation()
150 | {
151 |     return m_steps.right.rotation();
152 | }
153 | 
154 | const StepUpPlanner::Rotation &StepUpPlanner::Phase::rightRotation() const
155 | {
156 |     return m_steps.right.rotation();
157 | }
158 | 
159 | const StepUpPlanner::Step &StepUpPlanner::Phase::getLeftStep() const
160 | {
161 |     return m_steps.left;
162 | }
163 | 
164 | const StepUpPlanner::Step &StepUpPlanner::Phase::getRightStep() const
165 | {
166 |     return m_steps.right;
167 | }
168 | 
169 | const std::vector<StepUpPlanner::State> &StepUpPlanner::Phase::states() const
170 | {
171 |     return m_statesSolution;
172 | }
173 | 
174 | std::vector<StepUpPlanner::State> &StepUpPlanner::Phase::states()
175 | {
176 |     return m_statesSolution;
177 | }
178 | 
179 | const std::vector<StepUpPlanner::Control> &StepUpPlanner::Phase::controls() const
180 | {
181 |     return m_controlsSolution;
182 | }
183 | 
184 | std::vector<StepUpPlanner::Control> &StepUpPlanner::Phase::controls()
185 | {
186 |     return m_controlsSolution;
187 | }
188 | 
189 | 


--------------------------------------------------------------------------------
/src/core/src/src/References.cpp:
--------------------------------------------------------------------------------
 1 | #include <StepUpPlanner/References.h>
 2 | #include <iostream>
 3 | 
 4 | 
 5 | StepUpPlanner::References::References()
 6 |       : m_desiredLegLength(1.2)
 7 | { }
 8 | 
 9 | StepUpPlanner::References::~References()
10 | { }
11 | 
12 | bool StepUpPlanner::References::setDesiredLegLength(double desiredLegLength)
13 | {
14 |     if (desiredLegLength < 0.0) {
15 |         std::cerr << "[ERROR][StepUpPlanner::References::setDesiredLegLength] The desired leg length is not supposed to be smaller than 0." << std::endl;
16 |         return false;
17 |     }
18 | 
19 |     m_desiredLegLength = desiredLegLength;
20 | 
21 |     return true;
22 | }
23 | 
24 | double StepUpPlanner::References::getDesiredLength() const
25 | {
26 |     return m_desiredLegLength;
27 | }
28 | 
29 | StepUpPlanner::State &StepUpPlanner::References::desiredState()
30 | {
31 |     return m_desiredState;
32 | }
33 | 
34 | const StepUpPlanner::State &StepUpPlanner::References::desiredState() const
35 | {
36 |     return m_desiredState;
37 | }
38 | 
39 | StepUpPlanner::Control &StepUpPlanner::References::desiredControl()
40 | {
41 |     return m_desiredControl;
42 | }
43 | 
44 | const StepUpPlanner::Control &StepUpPlanner::References::desiredControl() const
45 | {
46 |     return m_desiredControl;
47 | }
48 | 
49 | void StepUpPlanner::References::zero()
50 | {
51 |     m_desiredState.zero();
52 |     m_desiredControl.zero();
53 |     m_desiredLegLength = 0.0;
54 | }
55 | 


--------------------------------------------------------------------------------
/src/core/src/src/Rotation.cpp:
--------------------------------------------------------------------------------
  1 | #include <StepUpPlanner/Rotation.h>
  2 | #include <cmath>
  3 | #include <cassert>
  4 | 
  5 | casadi::DM StepUpPlanner::Rotation::skew(double i, double j, double k)
  6 | {
  7 |     casadi::DM skewBuffer = casadi::DM::zeros(3,3);
  8 |     skewBuffer(0, 1) = -k;
  9 |     skewBuffer(0, 2) =  j;
 10 |     skewBuffer(1, 0) =  k;
 11 |     skewBuffer(1, 2) = -i;
 12 |     skewBuffer(2, 0) = -j;
 13 |     skewBuffer(2, 1) =  i;
 14 | 
 15 |     return skewBuffer;
 16 | }
 17 | 
 18 | void StepUpPlanner::Rotation::computeQuaternion()
 19 | {
 20 |     //Taken from "Contributions au contrôle automatique de véhicules aériens"
 21 |     //PhD thesis of "Minh Duc HUA"
 22 |     //INRIA Sophia Antipolis
 23 |     //www.isir.upmc.fr/files/2009THDR2323.pdf
 24 |     //Equation 3.9 (page 101)
 25 | 
 26 |     //Diagonal elements used only to find the maximum
 27 |     //the furthest value from zero
 28 |     //we use this value as denominator to find the other elements
 29 |     double q0 = (static_cast<double>(m_rotation(0,0))  + static_cast<double>(m_rotation(1,1)) + static_cast<double>(m_rotation(2,2)) + 1.0);
 30 |     double q1 = (static_cast<double>(m_rotation(0,0))  - static_cast<double>(m_rotation(1,1)) - static_cast<double>(m_rotation(2,2)) + 1.0);
 31 |     double q2 = (static_cast<double>(-m_rotation(0,0)) + static_cast<double>(m_rotation(1,1)) - static_cast<double>(m_rotation(2,2)) + 1.0);
 32 |     double q3 = (static_cast<double>(-m_rotation(0,0)) - static_cast<double>(m_rotation(1,1)) + static_cast<double>(m_rotation(2,2)) + 1.0);
 33 | 
 34 |     if (q0 < 0.0) q0 = 0.0;
 35 |     if (q1 < 0.0) q1 = 0.0;
 36 |     if (q2 < 0.0) q2 = 0.0;
 37 |     if (q3 < 0.0) q3 = 0.0;
 38 | 
 39 |     if (q0 >= q1 && q0 >= q2 && q0 >= q3) {
 40 |         q0 = std::sqrt(q0);
 41 |         q1 = (static_cast<double>(m_rotation(2,1)) - static_cast<double>(m_rotation(1,2))) / (2.0 * q0);
 42 |         q2 = (static_cast<double>(m_rotation(0,2)) - static_cast<double>(m_rotation(2,0))) / (2.0 * q0);
 43 |         q3 = (static_cast<double>(m_rotation(1,0)) - static_cast<double>(m_rotation(0,1))) / (2.0 * q0);
 44 |         q0 /= 2.0;
 45 |     } else if (q1 >= q0 && q1 >= q2 && q1 >= q3) {
 46 |         q1 = std::sqrt(q1);
 47 |         q0 = (static_cast<double>(m_rotation(2,1)) - static_cast<double>(m_rotation(1,2))) / (2.0 * q1);
 48 |         q2 = (static_cast<double>(m_rotation(1,0)) + static_cast<double>(m_rotation(0,1))) / (2.0 * q1);
 49 |         q3 = (static_cast<double>(m_rotation(2,0)) + static_cast<double>(m_rotation(0,2))) / (2.0 * q1);
 50 |         q1 /= 2.0;
 51 |     } else if (q2 >= q0 && q2 >= q1 && q2 >= q3) {
 52 |         q2 = std::sqrt(q2);
 53 |         q0 = (static_cast<double>(m_rotation(0,2)) - static_cast<double>(m_rotation(2,0))) / (2.0 * q2);
 54 |         q1 = (static_cast<double>(m_rotation(1,0)) + static_cast<double>(m_rotation(0,1))) / (2.0 * q2);
 55 |         q3 = (static_cast<double>(m_rotation(1,2)) + static_cast<double>(m_rotation(2,1))) / (2.0 * q2);
 56 |         q2 /= 2.0;
 57 |     } else if (q3 >= q0 && q3 >= q1 && q3 >= q2) {
 58 |         q3 = std::sqrt(q3);
 59 |         q0 = (static_cast<double>(m_rotation(1,0)) - static_cast<double>(m_rotation(0,1))) / (2.0 * q3);
 60 |         q1 = (static_cast<double>(m_rotation(2,0)) + static_cast<double>(m_rotation(0,2))) / (2.0 * q3);
 61 |         q2 = (static_cast<double>(m_rotation(1,2)) + static_cast<double>(m_rotation(2,1))) / (2.0 * q3);
 62 |         q3 /= 2.0;
 63 |     } else {
 64 |         assert(false && "[StepUpPlanner::Rotation::computeQuaternion] Quaternion numerically bad conditioned");
 65 |         return;
 66 |     }
 67 | 
 68 |     //Here we impose that the leftmost nonzero element of the quaternion is positive
 69 |     double eps = 1e-7;
 70 |     double sign = 1.0;
 71 |     if (q0 > eps || q0 < -eps) {
 72 |         sign = q0 > 0 ? 1.0 : -1.0;
 73 |     } else if (q1 > eps || q1 < -eps) {
 74 |         sign = q1 > 0 ? 1.0 : -1.0;
 75 |     } else if (q2 > eps || q2 < -eps) {
 76 |         sign = q2 > 0 ? 1.0 : -1.0;
 77 |     } else if (q3 > eps || q3 < -eps) {
 78 |         sign = q3 > 0 ? 1.0 : -1.0;
 79 |     }
 80 | 
 81 |     q0 /= sign;
 82 |     q1 /= sign;
 83 |     q2 /= sign;
 84 |     q3 /= sign;
 85 | 
 86 |     double quaternionNorm = std::sqrt(q0 * q0 +
 87 |                                       q1 * q1 +
 88 |                                       q2 * q2 +
 89 |                                       q3 * q3);
 90 |     m_quaternion(0)  = q0 / quaternionNorm;
 91 |     m_quaternion(1) = q1 / quaternionNorm;
 92 |     m_quaternion(2) = q2 / quaternionNorm;
 93 |     m_quaternion(3) = q3 / quaternionNorm;
 94 | }
 95 | 
 96 | StepUpPlanner::Rotation::Rotation()
 97 |     : m_rotation(3,3)
 98 |       , m_quaternion(4,1)
 99 | {
100 |     m_rotation = casadi::DM::zeros(3,3);
101 |     m_rotation(0,0) = 1.0;
102 |     m_rotation(1,1) = 1.0;
103 |     m_rotation(2,2) = 1.0;
104 | }
105 | 
106 | StepUpPlanner::Rotation::Rotation(const casadi::DM &matrix)
107 |     : m_rotation(matrix)
108 |       , m_quaternion(4,1)
109 | 
110 | {
111 |     computeQuaternion();
112 | }
113 | 
114 | void StepUpPlanner::Rotation::setFromQuaternion(double realPart, double i, double j, double k)
115 | {
116 |     double module = std::sqrt(realPart * realPart + i * i + j * j + k * k);
117 | 
118 |     assert(module > 0.01 &&
119 |            "[StepUpPlanner::Rotation::setFromQuaternion] The module of the specified quaternion appear to be close to zero.");
120 | 
121 |     double normalizedModule = realPart/module;
122 |     double normalized_i = i/module;
123 |     double normalized_j = j/module;
124 |     double normalized_k = k/module;
125 | 
126 |     casadi::DM skewBuffer = skew(normalized_i, normalized_j, normalized_k);
127 | 
128 |     casadi::DM identity = casadi::DM::zeros(3,3);
129 |     identity(0,0) = 1.0;
130 |     identity(1,1) = 1.0;
131 |     identity(2,2) = 1.0;
132 | 
133 |     m_rotation = identity + 2 * normalizedModule * skewBuffer + 2 * casadi::DM::mtimes(skewBuffer, skewBuffer);
134 |     m_quaternion(0) = normalizedModule;
135 |     m_quaternion(1) = normalized_i;
136 |     m_quaternion(2) = normalized_j;
137 |     m_quaternion(3) = normalized_k;
138 | }
139 | 
140 | const casadi::DM &StepUpPlanner::Rotation::asMatrix() const
141 | {
142 |     return m_rotation;
143 | }
144 | 
145 | const casadi::DM &StepUpPlanner::Rotation::asQuaternion() const
146 | {
147 |     return m_quaternion;
148 | }
149 | 
150 | double StepUpPlanner::Rotation::asQuaternion(size_t i) const
151 | {
152 |     assert(i < 4);
153 |     return static_cast<double>(m_quaternion(i));
154 | }
155 | 
156 | StepUpPlanner::Rotation StepUpPlanner::Rotation::Identity()
157 | {
158 |     casadi::DM identity = casadi::DM::zeros(3,3);
159 |     identity(0,0) = 1.0;
160 |     identity(1,1) = 1.0;
161 |     identity(2,2) = 1.0;
162 | 
163 |     return StepUpPlanner::Rotation(identity);
164 | }
165 | 
166 | 


--------------------------------------------------------------------------------
/src/core/src/src/Settings.cpp:
--------------------------------------------------------------------------------
  1 | #include <StepUpPlanner/Settings.h>
  2 | #include <iostream>
  3 | #include <vector>
  4 | #include <algorithm>
  5 | 
  6 | StepUpPlanner::Settings::Settings()
  7 |     : m_maximumLegLength(1.5)
  8 |       , m_minimumLegLength(0.0)
  9 |       , m_staticFrictionCoefficient(0.5)
 10 |       , m_torsionalFrictionCoefficient(0.03)
 11 |       , m_finalStateAnticipation(0.3)
 12 |       , m_phaseLength(30)
 13 |       , m_solverName("mumps")
 14 | { }
 15 | 
 16 | StepUpPlanner::Settings::~Settings()
 17 | { }
 18 | 
 19 | bool StepUpPlanner::Settings::setMaximumLegLength(double maxLength)
 20 | {
 21 |     if (maxLength <= 0) {
 22 |         std::cerr << "[StepUpPlanner::Settings::setMaximumLegLength] The maxLength is supposed to be a positive number." << std::endl;
 23 |         return false;
 24 |     }
 25 | 
 26 |     if (maxLength < m_minimumLegLength) {
 27 |         std::cerr << "[StepUpPlanner::Settings::setMaximumLegLength] The maxLength is supposed to be greater than the minimum leg length." << std::endl;
 28 |         return false;
 29 |     }
 30 | 
 31 | 
 32 |     m_maximumLegLength = maxLength;
 33 | 
 34 |     return true;
 35 | }
 36 | 
 37 | double StepUpPlanner::Settings::getMaximumLegLength() const
 38 | {
 39 |     return m_maximumLegLength;
 40 | }
 41 | 
 42 | bool StepUpPlanner::Settings::setMinimumLegLength(double minLength)
 43 | {
 44 |     if (minLength < 0) {
 45 |         std::cerr << "[StepUpPlanner::Settings::setMinimumLegLength] The minLength is supposed to be a non-negative number." << std::endl;
 46 |         return false;
 47 |     }
 48 | 
 49 |     if (minLength > m_maximumLegLength) {
 50 |         std::cerr << "[StepUpPlanner::Settings::setMinimumLegLength] The minLength is supposed to be lower than the maximum leg length." << std::endl;
 51 |         return false;
 52 |     }
 53 | 
 54 |     m_minimumLegLength = minLength;
 55 | 
 56 |     return true;
 57 | }
 58 | 
 59 | double StepUpPlanner::Settings::getMinimumLegLength() const
 60 | {
 61 |     return m_minimumLegLength;
 62 | }
 63 | 
 64 | bool StepUpPlanner::Settings::setLegLengthSettings(double minLength, double maxLength)
 65 | {
 66 |     if (maxLength <= 0) {
 67 |         std::cerr << "[StepUpPlanner::Settings::setLegLengthSettings] The maxLength is supposed to be a positive number." << std::endl;
 68 |         return false;
 69 |     }
 70 | 
 71 |     if (minLength < 0) {
 72 |         std::cerr << "[StepUpPlanner::Settings::setLegLengthSettings] The minLength is supposed to be a non-negative number." << std::endl;
 73 |         return false;
 74 |     }
 75 | 
 76 |     if (maxLength < minLength) {
 77 |         std::cerr << "[StepUpPlanner::Settings::setLegLengthSettings] The maxLength is supposed to be greater than minLength." << std::endl;
 78 |         return false;
 79 |     }
 80 | 
 81 |     m_maximumLegLength = maxLength;
 82 |     m_minimumLegLength = minLength;
 83 | 
 84 |     return true;
 85 | }
 86 | 
 87 | bool StepUpPlanner::Settings::setStaticFrictionCoefficient(double staticFrictionCoeff)
 88 | {
 89 |     if (staticFrictionCoeff < 0) {
 90 |         std::cerr << "[StepUpPlanner::Settings::setStaticFrictionCoefficient] The staticFrictionCoeff is supposed to be a positive number." << std::endl;
 91 |         return false;
 92 |     }
 93 | 
 94 |     m_staticFrictionCoefficient = staticFrictionCoeff;
 95 | 
 96 |     return true;
 97 | }
 98 | 
 99 | double StepUpPlanner::Settings::getStaticFrictionCoefficient() const
100 | {
101 |     return m_staticFrictionCoefficient;
102 | }
103 | 
104 | bool StepUpPlanner::Settings::setTorsionalFrictionCoefficient(double torsionalFrictionCoeff)
105 | {
106 |     if (torsionalFrictionCoeff < 0) {
107 |         std::cerr << "[StepUpPlanner::Settings::setTorsionalFrictionCoefficient] The torsionalFrictionCoeff is supposed to be a positive number." << std::endl;
108 |         return false;
109 |     }
110 | 
111 |     m_torsionalFrictionCoefficient = torsionalFrictionCoeff;
112 | 
113 |     return true;
114 | }
115 | 
116 | double StepUpPlanner::Settings::getTorsionalFrictionCoefficient() const
117 | {
118 |     return m_torsionalFrictionCoefficient;
119 | }
120 | 
121 | unsigned long &StepUpPlanner::Settings::phaseLength()
122 | {
123 |     return m_phaseLength;
124 | }
125 | 
126 | unsigned long StepUpPlanner::Settings::phaseLength() const
127 | {
128 |     return m_phaseLength;
129 | }
130 | 
131 | bool StepUpPlanner::Settings::setFinalStateAnticipation(double finalStateAnticipation)
132 | {
133 |     if ((finalStateAnticipation > 1.0) || (finalStateAnticipation < 0.0))
134 |     {
135 |         std::cerr << "[ERROR][StepUpPlanner::References::setFinalStateAnticipation] The final state anticipation is supposed to be in the interval [0.0, 1.0]." << std::endl;
136 |         return false;
137 |     }
138 | 
139 |     m_finalStateAnticipation = finalStateAnticipation;
140 | 
141 |     return true;
142 | }
143 | 
144 | double StepUpPlanner::Settings::getFinalStateAnticipation() const
145 | {
146 |     return m_finalStateAnticipation;
147 | }
148 | 
149 | bool StepUpPlanner::Settings::setIpoptLinearSolver(const std::string &solverName)
150 | {
151 |     std::vector<std::string> availableSolvers = {"ma27", "ma57", "ma77", "ma86", "ma97", "pardiso", "wsmp", "mumps", "custom"};
152 | 
153 |     if (std::find(availableSolvers.begin(), availableSolvers.end(), solverName) == availableSolvers.end()) {
154 |         std::cerr << "[ERROR][StepUpPlanner::References::setIpoptLinearSolver] Unknown solver name. ";
155 |         std::cerr << "See options at https://www.coin-or.org/Ipopt/documentation/node51.html#SECTION0001111010000000000000." <<std::endl;
156 |         return false;
157 |     }
158 | 
159 |     m_solverName = solverName;
160 | 
161 |     return true;
162 | }
163 | 
164 | const std::string &StepUpPlanner::Settings::getIpoptLinearSolver() const
165 | {
166 |     return m_solverName;
167 | }
168 | 
169 | unsigned long &StepUpPlanner::Settings::solverVerbosity()
170 | {
171 |     return m_solverVerbosity;
172 | }
173 | 
174 | unsigned long StepUpPlanner::Settings::solverVerbosity() const
175 | {
176 |     return m_solverVerbosity;
177 | }
178 | 
179 | StepUpPlanner::CostWeights &StepUpPlanner::Settings::costWeights()
180 | {
181 |     return m_costWeights;
182 | }
183 | 
184 | const StepUpPlanner::CostWeights &StepUpPlanner::Settings::costWeights() const
185 | {
186 |     return m_costWeights;
187 | }
188 | 
189 | 


--------------------------------------------------------------------------------
/src/core/src/src/Solver.cpp:
--------------------------------------------------------------------------------
  1 | #include <StepUpPlanner/Solver.h>
  2 | #include <iostream>
  3 | #include <cmath>
  4 | #include <cassert>
  5 | 
  6 | casadi::Function StepUpPlanner::Solver::getIntegratorDynamics()
  7 | {
  8 |     casadi::MX x = casadi::MX::sym("x", 6);
  9 |     casadi::MX a = casadi::MX::sym("a", 3);
 10 |     casadi::MX dT = casadi::MX::sym("dt");
 11 | 
 12 |     casadi::MX p = x(casadi::Slice(0, 3));
 13 |     casadi::MX v = x(casadi::Slice(3, 6));
 14 |     casadi::MX rhs = casadi::MX::vertcat({p + dT * (v + 0.5 * dT * (a)),
 15 |                                           v + dT * (a)});
 16 | 
 17 |     return casadi::Function("Integrator", {x, a, dT}, {rhs});
 18 | }
 19 | 
 20 | void StepUpPlanner::Solver::createFeetConstraintsFunction(const std::string &name, const Step &step, casadi::Function& outputFunction, casadi::DM &outputBounds)
 21 | {
 22 |     casadi::MX footLocation = casadi::MX::sym("pFoot", 3);
 23 |     casadi::MX footControl = casadi::MX::sym("foot_control", 3);
 24 |     casadi::MX state = casadi::MX::sym("state", 6);
 25 |     casadi::MX footOrientation = casadi::MX::sym("Rfoot", 3, 3);
 26 | 
 27 |     casadi::MX currentPosition = state(casadi::Slice(0,3));
 28 |     casadi::MX footCoP = footControl(casadi::Slice(0,2));
 29 |     casadi::MX u = footControl(2);
 30 |     casadi::MX footCoPInWorld = casadi::MX::mtimes(footOrientation, casadi::MX::vertcat({footCoP, 0}));
 31 |     casadi::MX forceDividedByMassAndU = casadi::MX::mtimes(footOrientation.T(), (currentPosition - (footLocation + footCoPInWorld)));
 32 |     //Being the mass and u positive quantities, they don't play a role
 33 | 
 34 | 
 35 |     casadi::DM frictionBounds = casadi::DM::zeros(3,1);
 36 |     casadi::MX A = casadi::MX::horzcat({-footCoP(1), footCoP(0), 0});
 37 |     casadi::MX B = casadi::MX::horzcat({0, 0, m_settings.getTorsionalFrictionCoefficient()});
 38 |     double staticFriction = m_settings.getStaticFrictionCoefficient();
 39 |     casadi::DM staticFrictionMultiplier = casadi::DM::horzcat({1.0, 1.0, -(staticFriction * staticFriction)});
 40 | 
 41 |     casadi::MX frictionExpressions(3, 1);
 42 |     frictionExpressions(0) = casadi::MX::mtimes(staticFrictionMultiplier, casadi::MX::pow(forceDividedByMassAndU, 2));
 43 |     frictionExpressions(1) = casadi::MX::mtimes(A-B, forceDividedByMassAndU);
 44 |     frictionExpressions(2) = casadi::MX::mtimes(-A-B, forceDividedByMassAndU);
 45 | 
 46 |     casadi::Function copConstraintsFcn = step.getCoPConstraintsFunction();
 47 | 
 48 |     double maxLegLength = m_settings.getMaximumLegLength();
 49 |     double minLegLength = m_settings.getMinimumLegLength();
 50 |     casadi::MX legLengthExpression = casadi::MX::mtimes((currentPosition - footLocation).T(), (currentPosition - footLocation));
 51 | 
 52 |     casadi::MX multiplierPositivityExpression = -u;
 53 | 
 54 |     if (copConstraintsFcn.is_null()) {
 55 | 
 56 |         outputBounds = casadi::DM::vertcat({frictionBounds, 0, maxLegLength * maxLegLength, -minLegLength * minLegLength});
 57 | 
 58 |         outputFunction = casadi::Function(name, {state, footControl, footLocation}, {frictionExpressions,
 59 |                                                                                      multiplierPositivityExpression,
 60 |                                                                                      legLengthExpression,
 61 |                                                                                      -legLengthExpression});
 62 | 
 63 |     } else {
 64 |         casadi::MX copConstraintsExpressions = casadi::MX::vertcat(copConstraintsFcn(footCoP));
 65 |         outputBounds = casadi::DM::vertcat({frictionBounds, step.getCoPBounds(), 0,
 66 |                                             maxLegLength * maxLegLength, -minLegLength * minLegLength});
 67 |         outputFunction = casadi::Function(name, {state, footControl, footLocation, footOrientation},
 68 |                                           {frictionExpressions, copConstraintsExpressions,
 69 |                                            multiplierPositivityExpression, legLengthExpression, -legLengthExpression});
 70 |     }
 71 | }
 72 | 
 73 | casadi::Function StepUpPlanner::Solver::getAccelerationConsistencyConstraintFunction(const std::string &name,
 74 |                                                                                      const StepUpPlanner::SideDependentObject<bool>& isActive)
 75 | {
 76 |     casadi::MX X = casadi::MX::sym("x", 6);
 77 |     casadi::MX U = casadi::MX::sym("u", 6);
 78 |     casadi::MX A = casadi::MX::sym("a", 3);
 79 |     casadi::MX currentPosition = X(casadi::Slice(0,3));
 80 |     casadi::MX leftLocation = casadi::MX::sym("leftLocation", 3);
 81 |     casadi::MX leftRotation = casadi::MX::sym("leftRotation", 3, 3);
 82 |     casadi::MX rightLocation = casadi::MX::sym("rightLocation", 3);
 83 |     casadi::MX rightRotation = casadi::MX::sym("rightRotation", 3, 3);
 84 | 
 85 |     casadi::MX footCoPL = U(casadi::Slice(0,2));
 86 |     casadi::MX ul = U(2);
 87 |     casadi::MX leftCoPInWorld = casadi::MX::mtimes(leftRotation, casadi::MX::vertcat({footCoPL, 0}));
 88 | 
 89 |     casadi::MX footCoPR = U(casadi::Slice(3,5));
 90 |     casadi::MX ur = U(5);
 91 |     casadi::MX rightCoPInWorld = casadi::MX::mtimes(rightRotation, casadi::MX::vertcat({footCoPR, 0}));
 92 | 
 93 |     casadi::DM g = casadi::DM::zeros(3,1);
 94 |     g(2) = 9.81;
 95 | 
 96 |     casadi::MX constraint = A + g;
 97 | 
 98 |     if (isActive.left) {
 99 |         constraint -= (currentPosition - (leftLocation + leftCoPInWorld)) * ul;
100 |     }
101 | 
102 |     if (isActive.right) {
103 |         constraint -= (currentPosition - (rightLocation + rightCoPInWorld)) * ur;
104 |     }
105 | 
106 |     return casadi::Function(name, {X, U, A, leftLocation, leftRotation, rightLocation, rightRotation}, {constraint});
107 | }
108 | 
109 | bool StepUpPlanner::Solver::fillPhaseDataVector(const std::vector<Phase> &phases)
110 | {
111 |     m_phases.resize(phases.size());
112 | 
113 |     for (size_t i = 0; i < m_phases.size(); ++i) {
114 | 
115 |         m_phases[i].phase = phases[i];
116 |         StepUpPlanner::PhaseType phaseType = phases[i].getPhaseType();
117 | 
118 |         if (phaseType == StepUpPlanner::PhaseType::UNDEFINED) {
119 |             std::cerr << "[StepUpPlanner::Solver::fillPhaseDataVector] Undefined phase type at position " << i << "." <<std::endl;
120 |             return false;
121 |         }
122 | 
123 |         m_phases[i].isActive.left = (phaseType == StepUpPlanner::PhaseType::DOUBLE_SUPPORT) ||
124 |             (phaseType == StepUpPlanner::PhaseType::SINGLE_SUPPORT_LEFT);
125 | 
126 |         m_phases[i].isActive.right = (phaseType == StepUpPlanner::PhaseType::DOUBLE_SUPPORT) ||
127 |             (phaseType == StepUpPlanner::PhaseType::SINGLE_SUPPORT_RIGHT);
128 | 
129 |         m_phases[i].accelerationConsistencyConstraint = getAccelerationConsistencyConstraintFunction("Consinstency" + std::to_string(i),
130 |                                                                                                      m_phases[i].isActive);
131 | 
132 |         if (m_phases[i].isActive.left) {
133 |             createFeetConstraintsFunction("LeftFootConstraints" + std::to_string(i), phases[i].getLeftStep(),
134 |                                           m_phases[i].feetConstraints.left, m_phases[i].feetConstraintsBounds.left);
135 |         } else {
136 |             m_phases[i].feetConstraints.left = casadi::Function();
137 |         }
138 | 
139 |         if (m_phases[i].isActive.right) {
140 |             createFeetConstraintsFunction("RightFootConstraints" + std::to_string(i), phases[i].getRightStep(),
141 |                                           m_phases[i].feetConstraints.right, m_phases[i].feetConstraintsBounds.right);
142 |         } else {
143 |             m_phases[i].feetConstraints.right = casadi::Function();
144 |         }
145 | 
146 |         m_phases[i].feetLocationParameter.left  = m_opti.parameter(3);
147 |         m_phases[i].feetOrientationParameter.left = m_opti.parameter(3, 3);
148 |         m_phases[i].feetLocationParameter.right = m_opti.parameter(3);
149 |         m_phases[i].feetOrientationParameter.right = m_opti.parameter(3, 3);
150 |         m_phases[i].minDurationParameter = m_opti.parameter();
151 |         m_phases[i].maxDurationParameter = m_opti.parameter();
152 | 
153 |         m_phases[i].phase.states().resize(m_settings.phaseLength());
154 |         m_phases[i].phase.controls().resize(m_settings.phaseLength());
155 |     }
156 | 
157 |     m_referenceTimings = m_opti.parameter(static_cast<casadi_int>(m_phases.size()));
158 | 
159 |     return true;
160 | }
161 | 
162 | void StepUpPlanner::Solver::setupOpti()
163 | {
164 |     using Sl = casadi::Slice;
165 | 
166 |     casadi_int numberOfPhases = static_cast<casadi_int>(m_phases.size());
167 |     casadi_int phaseLength = static_cast<casadi_int>(m_settings.phaseLength());
168 |     casadi_int N = numberOfPhases * phaseLength;
169 | 
170 |     m_useMaxU = m_settings.costWeights().maxMultiplier > 0;
171 |     m_useMaxTau = m_settings.costWeights().maxTorques > 0;
172 | 
173 |     m_X = m_opti.variable(6, N + 1);
174 |     m_A = m_opti.variable(3,N);
175 |     m_U = m_opti.variable(6, N);
176 |     m_T = m_opti.variable(numberOfPhases);
177 |     m_uMax = m_opti.variable();
178 |     m_tauMax = m_opti.variable();
179 | 
180 |     if (!m_useMaxU) {
181 |         m_opti.subject_to(m_uMax == 0);
182 |     }
183 | 
184 |     if (!m_useMaxTau) {
185 |         m_opti.subject_to(m_tauMax == 0);
186 |     }
187 | 
188 |     m_opti.subject_to(m_X(Sl(), 0) == m_initialStateParameter);
189 | 
190 |     casadi::MX torquesCost = 0, leftTorques, rightTorques;
191 | 
192 |     for (casadi_int phase = 0; phase < numberOfPhases; ++phase) {
193 | 
194 |         size_t castedPhase = static_cast<size_t>(phase);
195 | 
196 |         casadi::MX dT = m_T(phase)/phaseLength;
197 | 
198 |         m_opti.subject_to(m_phases[castedPhase].minDurationParameter <= m_T(phase) <= m_phases[castedPhase].maxDurationParameter);
199 | 
200 |         bool leftIsActive = m_phases[castedPhase].isActive.left;
201 |         bool rightIsActive = m_phases[castedPhase].isActive.right;
202 | 
203 |         casadi::Function& leftFootConstraints = m_phases[castedPhase].feetConstraints.left;
204 |         casadi::DM& leftFootBounds = m_phases[castedPhase].feetConstraintsBounds.left;
205 |         casadi::MX& leftPosition = m_phases[castedPhase].feetLocationParameter.left;
206 |         casadi::MX& leftRotation = m_phases[castedPhase].feetOrientationParameter.left;
207 | 
208 |         casadi::Function& rightFootConstraints = m_phases[castedPhase].feetConstraints.right;
209 |         casadi::DM& rightFootBounds = m_phases[castedPhase].feetConstraintsBounds.right;
210 |         casadi::MX& rightPosition = m_phases[castedPhase].feetLocationParameter.right;
211 |         casadi::MX& rightRotation = m_phases[castedPhase].feetOrientationParameter.right;
212 | 
213 | 
214 |         casadi::Function accelerationConstraint = m_phases[castedPhase].accelerationConsistencyConstraint;
215 | 
216 | 
217 |         for (casadi_int k = 0 + phaseLength * phase; k < phaseLength * (phase + 1); ++k) {
218 |             m_opti.subject_to(m_X(Sl(), k + 1) == casadi::MX::vertcat(m_integratorDynamics({m_X(Sl(), k), m_A(Sl(), k), dT})));
219 | 
220 |             if (leftIsActive) {
221 |                 m_opti.subject_to(casadi::MX::vertcat(leftFootConstraints({m_X(Sl(), k + 1), m_U(Sl(0,3), k), leftPosition, leftRotation})) <= leftFootBounds);
222 | 
223 |                 if (m_useMaxU) {
224 |                     m_opti.subject_to(m_U(2, k) - m_uMax <= 0);
225 |                 }
226 | 
227 |                 leftTorques = casadi::MX::pow(((m_X(2, k + 1) - leftPosition(2) - m_desiredLegLengthParameter) * m_U(2, k)), 2);
228 |                 torquesCost += leftTorques;
229 | 
230 |                 if (m_useMaxTau) {
231 |                     m_opti.subject_to(leftTorques  - m_tauMax <= 0);
232 |                 }
233 | 
234 |             } else {
235 |                 m_opti.subject_to(m_U(Sl(0,3), k) == casadi::MX::zeros(3,1));
236 |             }
237 | 
238 |             if (rightIsActive) {
239 |                 m_opti.subject_to(casadi::MX::vertcat(rightFootConstraints({m_X(Sl(), k + 1), m_U(Sl(3,6), k), rightPosition, rightRotation})) <= rightFootBounds);
240 | 
241 |                 if (m_useMaxU) {
242 |                     m_opti.subject_to(m_U(5, k) - m_uMax <= 0);
243 |                 }
244 | 
245 |                 rightTorques = casadi::MX::pow(((m_X(2, k + 1) - rightPosition(2) - m_desiredLegLengthParameter) * m_U(5, k)), 2);
246 |                 torquesCost += rightTorques;
247 | 
248 |                 if (m_useMaxTau) {
249 |                     m_opti.subject_to(rightTorques  - m_tauMax <= 0);
250 |                 }
251 | 
252 |             } else {
253 |                 m_opti.subject_to(m_U(Sl(3,6), k) == casadi::MX::zeros(3,1));
254 |             }
255 | 
256 |             m_opti.subject_to(casadi::MX::vertcat(accelerationConstraint({m_X(Sl(), k + 1), m_U(Sl(), k), m_A(Sl(), k),
257 |                                                                           leftPosition, leftRotation,
258 |                                                                           rightPosition, rightRotation})) == casadi::DM::zeros(3,1));
259 | 
260 |         }
261 |     }
262 | 
263 |     StepUpPlanner::CostWeights w = m_settings.costWeights();
264 |     Sl lastStates(static_cast<casadi_int>(N - std::round(phaseLength * m_settings.getFinalStateAnticipation())), N+1);
265 | 
266 |     casadi::MX costFunction = w.durationsDifference * casadi::MX::sumsqr(m_T - m_referenceTimings); //Timing error
267 |     costFunction += w.finalStateError * casadi::MX::sumsqr(m_X(Sl(), lastStates) - m_referenceStateParameter); //Final state error
268 |     costFunction += w.multipliers * (casadi::MX::sumsqr(m_U(2, Sl())) + casadi::MX::sumsqr(m_U(5, Sl())));
269 |     costFunction += w.maxMultiplier * m_uMax;
270 |     costFunction += w.cop * (casadi::MX::sumsqr(m_U(Sl(0,2), Sl())) + casadi::MX::sumsqr(m_U(Sl(3,5), Sl())));
271 |     costFunction += w.controlVariations * (casadi::MX::sumsqr(m_U(Sl(), Sl(1, N)) - m_U(Sl(), Sl(0, N-1))));
272 |     costFunction += w.finalControl * casadi::MX::sumsqr(m_U(Sl(), N-1) - m_referenceControlParameter);
273 |     costFunction += w.torques * torquesCost;
274 |     costFunction += w.maxTorques * m_tauMax;
275 | 
276 |     m_opti.minimize(costFunction);
277 | }
278 | 
279 | bool StepUpPlanner::Solver::setupProblem(const std::vector<StepUpPlanner::Phase> &phases, const StepUpPlanner::Settings &settings)
280 | {
281 |     if (!phases.size()) {
282 |         std::cerr << "[StepUpPlanner::Solver::setupProblem] No phases provided." <<std::endl;
283 |         return false;
284 |     }
285 | 
286 |     m_settings = settings;
287 | 
288 |     m_initialStateParameter = m_opti.parameter(6);
289 |     m_desiredLegLengthParameter = m_opti.parameter();
290 |     m_referenceStateParameter = m_opti.parameter(6);
291 |     m_referenceControlParameter = m_opti.parameter(6);
292 |     m_integratorDynamics = getIntegratorDynamics();
293 | 
294 |     bool ok = fillPhaseDataVector(phases);
295 | 
296 |     if (!ok) {
297 |         std::cerr << "[StepUpPlanner::Solver::setupProblem] Problem when specifying the phases." <<std::endl;
298 |         return false;
299 |     }
300 | 
301 |     casadi_int npoints = static_cast<casadi_int>(m_phases.size() * settings.phaseLength());
302 |     m_linSpacedPoints = casadi::DM::linspace(0, 1, npoints + 1);
303 | 
304 |     m_Xsol.resize(6, npoints +1);
305 |     m_Usol.resize(6, npoints);
306 |     m_Asol.resize(3, npoints);
307 |     m_Tsol.resize(static_cast<casadi_int>(m_phases.size()), 1);
308 | 
309 |     setupOpti();
310 | 
311 |     casadi::Dict casadiOptions;
312 |     casadi::Dict ipoptOptions;
313 | 
314 |     casadiOptions["expand"] = true;
315 |     unsigned long solverVerbosity = m_settings.solverVerbosity();
316 |     if (solverVerbosity) {
317 |         casadi_int ipoptVerbosity = static_cast<long long>(solverVerbosity - 1);
318 |         ipoptOptions["print_level"] = ipoptVerbosity;
319 |         casadiOptions["print_time"] = true;
320 |     } else {
321 |         ipoptOptions["print_level"] = 0;
322 |         casadiOptions["print_time"] = false;
323 |     }
324 |     ipoptOptions["linear_solver"] = m_settings.getIpoptLinearSolver();
325 | 
326 |     m_opti.solver("ipopt", casadiOptions, ipoptOptions);
327 | 
328 |     m_solverState = SolverState::PROBLEM_SET;
329 | 
330 |     return true;
331 | }
332 | 
333 | void StepUpPlanner::Solver::setParametersValue(const StepUpPlanner::State &initialState, const References &references)
334 | {
335 |     m_opti.set_value(m_initialStateParameter(casadi::Slice(0,3)), initialState.position());
336 |     m_opti.set_value(m_initialStateParameter(casadi::Slice(3,6)), initialState.velocity());
337 |     m_opti.set_value(m_desiredLegLengthParameter, references.getDesiredLength());
338 |     m_opti.set_value(m_referenceStateParameter(casadi::Slice(0,3)), references.desiredState().position());
339 |     m_opti.set_value(m_referenceStateParameter(casadi::Slice(3,6)), references.desiredState().velocity());
340 |     m_opti.set_value(m_referenceControlParameter(casadi::Slice(0,2)), references.desiredControl().left().cop());
341 |     m_opti.set_value(m_referenceControlParameter(2), references.desiredControl().left().multiplier());
342 |     m_opti.set_value(m_referenceControlParameter(casadi::Slice(3,5)), references.desiredControl().right().cop());
343 |     m_opti.set_value(m_referenceControlParameter(5), references.desiredControl().right().multiplier());
344 | 
345 |     for (size_t i = 0; i < m_phases.size(); ++i) {
346 |         m_opti.set_value(m_phases[i].feetLocationParameter.left, m_phases[i].phase.leftPosition());
347 |         m_opti.set_value(m_phases[i].feetOrientationParameter.left, m_phases[i].phase.leftRotation().asMatrix());
348 |         m_opti.set_value(m_phases[i].feetLocationParameter.right, m_phases[i].phase.rightPosition());
349 |         m_opti.set_value(m_phases[i].feetOrientationParameter.right, m_phases[i].phase.rightRotation().asMatrix());
350 |         m_opti.set_value(m_phases[i].minDurationParameter, m_phases[i].phase.minDuration());
351 |         m_opti.set_value(m_phases[i].maxDurationParameter, m_phases[i].phase.maxDuration());
352 |         m_opti.set_value(m_referenceTimings(i), m_phases[i].phase.desiredDuration());
353 |     }
354 | }
355 | 
356 | void StepUpPlanner::Solver::fillSolution()
357 | {
358 |     m_Xsol = m_solution->value(m_X);
359 |     m_Usol = m_solution->value(m_U);
360 |     m_Asol = m_solution->value(m_A);
361 |     m_Tsol = m_solution->value(m_T);
362 | 
363 |     casadi_int currentInstant = 0;
364 | 
365 |     for (size_t p = 0; p < m_phases.size(); ++p) {
366 |         StepUpPlanner::Phase& phase = m_phases[p].phase;
367 | 
368 |         std::vector<StepUpPlanner::State>& states = phase.states();
369 |         states.resize(m_settings.phaseLength()); //Note that memory should be already allocated when setting the phases
370 |         std::vector<StepUpPlanner::Control>& controls = phase.controls();
371 |         controls.resize(m_settings.phaseLength());
372 | 
373 |         phase.duration() = m_Tsol(p);
374 | 
375 |         for (size_t k = 0; k < m_settings.phaseLength(); ++k) {
376 |             states[k].position() = m_Xsol(casadi::Slice(0,3), currentInstant + 1);
377 |             states[k].velocity() = m_Xsol(casadi::Slice(3,6), currentInstant + 1);
378 |             controls[k].acceleration() = m_Asol(casadi::Slice(), currentInstant);
379 |             controls[k].left().cop() = m_Usol(casadi::Slice(0,2), currentInstant);
380 |             controls[k].left().multiplier() = m_Usol(2, currentInstant);
381 |             controls[k].right().cop() = m_Usol(casadi::Slice(3,5), currentInstant);
382 |             controls[k].right().multiplier() = m_Usol(5, currentInstant);
383 | 
384 |             currentInstant++;
385 |         }
386 |     }
387 | }
388 | 
389 | StepUpPlanner::Solver::Solver()
390 |     : m_solverState(SolverState::NOT_INITIALIZED)
391 |       , m_solution(nullptr)
392 | { }
393 | 
394 | StepUpPlanner::Solver::Solver(const std::vector<StepUpPlanner::Phase> &phases, const StepUpPlanner::Settings &settings)
395 |     : m_solverState(SolverState::NOT_INITIALIZED)
396 |       , m_solution(nullptr)
397 | 
398 | {
399 |     setupProblem(phases, settings);
400 | }
401 | 
402 | StepUpPlanner::Solver::~Solver()
403 | { }
404 | 
405 | bool StepUpPlanner::Solver::resetProblem(const std::vector<StepUpPlanner::Phase> &phases, const StepUpPlanner::Settings &settings)
406 | {
407 |     clear();
408 |     return setupProblem(phases, settings);
409 | }
410 | 
411 | size_t StepUpPlanner::Solver::numberOfPhases() const
412 | {
413 |     return m_phases.size();
414 | }
415 | 
416 | StepUpPlanner::Phase &StepUpPlanner::Solver::getPhase(size_t i)
417 | {
418 |     assert(i < m_phases.size() && "[ERROR][StepUpPlanner::Solver::getPhase] Index out of bounds.");
419 |     assert(((m_solverState == SolverState::PROBLEM_SET) || (m_solverState == SolverState::PROBLEM_SOLVED)) &&
420 |            "[ERROR][StepUpPlanner::Solver::getPhase] First you have to set the problem.");
421 |     return m_phases[i].phase;
422 | }
423 | 
424 | const StepUpPlanner::Phase &StepUpPlanner::Solver::getPhase(size_t i) const
425 | {
426 |     assert(i < m_phases.size() && "[ERROR][StepUpPlanner::Solver::getPhase] Index out of bounds.");
427 |     assert(((m_solverState == SolverState::PROBLEM_SET) || (m_solverState == SolverState::PROBLEM_SOLVED)) &&
428 |            "[ERROR][StepUpPlanner::Solver::getPhase] First you have to set the problem.");
429 |     return m_phases[i].phase;
430 | }
431 | 
432 | bool StepUpPlanner::Solver::solve(const StepUpPlanner::State &initialState, const References &references)
433 | {
434 |     if (m_solverState == SolverState::NOT_INITIALIZED) {
435 |         std::cerr << "[StepUpPlanner::Solver::solve] The problem is not set correctly" << std::endl;
436 |         return false;
437 |     }
438 | 
439 |     if (m_solverState == SolverState::PROBLEM_SOLVED) {
440 |         assert(m_solution);
441 |         m_opti.set_initial(m_solution->value_variables());
442 |         m_opti.set_initial(m_opti.lam_g(), m_solution->value(m_opti.lam_g()));
443 |     } else {
444 |         //Generic initialization
445 |         for (size_t i = 0; i < m_phases.size(); ++i) {
446 |             m_opti.set_initial(m_T(i), m_phases[i].phase.desiredDuration());
447 |         }
448 | 
449 |         casadi_int npoints = static_cast<casadi_int>(m_phases.size() * m_settings.phaseLength());
450 | 
451 |         const casadi::DM& initPos = initialState.position();
452 |         const casadi::DM& initVel = initialState.velocity();
453 |         const casadi::DM& refPos = references.desiredState().position();
454 | 
455 |         m_opti.set_initial(m_X(casadi::Slice(), 0), casadi::DM::vertcat({initPos, initVel}));
456 |         casadi::DM interpolatedPosition(3, 1);
457 | 
458 |         for (casadi_int k = 1; k < npoints + 1; ++k) {
459 |             interpolatedPosition = initPos + m_linSpacedPoints(k) * (refPos - initPos);
460 |             m_opti.set_initial(m_X(casadi::Slice(0,3), k), interpolatedPosition);
461 |         }
462 | 
463 |         casadi::DM g = casadi::DM::zeros(3,1);
464 |         g(2) = -9.81;
465 | 
466 |         for (casadi_int k = 0; k < npoints; ++k) {
467 |             m_opti.set_initial(m_A(casadi::Slice(), k), g);
468 |         }
469 | 
470 |         m_opti.set_initial(m_uMax, m_useMaxU ? 30.0 : 0.0);
471 |         m_opti.set_initial(m_tauMax, m_useMaxTau ? 100.0 : 0.0);
472 |     }
473 | 
474 |     m_solverState = SolverState::PROBLEM_SET;
475 |     m_solution = nullptr;
476 | 
477 |     setParametersValue(initialState, references);
478 | 
479 |     try {
480 |         m_solution = std::make_unique<casadi::OptiSol>(m_opti.solve());
481 |     } catch (std::exception &e) {
482 |         m_opti.debug().show_infeasibilities(1e-5);
483 |         std::cerr << "[StepUpPlanner::Solver::solve] Error while solving the optimization problem." << std::endl;
484 |         std::cerr << "Details: " << e.what() << std::endl;
485 |         return false;
486 |     }
487 | 
488 |     fillSolution();
489 | 
490 |     m_solverState = SolverState::PROBLEM_SOLVED;
491 |     return true;
492 | }
493 | 
494 | bool StepUpPlanner::Solver::getFullSolution(std::vector<StepUpPlanner::Phase> &phases) const
495 | {
496 |     if (m_solverState != SolverState::PROBLEM_SOLVED) {
497 |         std::cerr << "[StepUpPlanner::Solver::getFullSolution] No solution available." << std::endl;
498 |         return false;
499 |     }
500 | 
501 |     phases.resize(m_phases.size());
502 | 
503 |     for (size_t i = 0; i < m_phases.size(); ++i) {
504 |         phases[i] = m_phases[i].phase;
505 |     }
506 | 
507 |     return true;
508 | }
509 | 
510 | void StepUpPlanner::Solver::clear()
511 | {
512 |     m_solverState = SolverState::NOT_INITIALIZED;
513 |     m_opti = casadi::Opti();
514 |     m_solution = nullptr;
515 |     m_phases.clear();
516 |     m_Xsol.clear();
517 |     m_Usol.clear();
518 |     m_Tsol.clear();
519 |     m_Asol.clear();
520 | }
521 | 
522 | bool StepUpPlanner::Solver::isReady() const
523 | {
524 |     return (m_solverState == SolverState::PROBLEM_SET) || (m_solverState == SolverState::PROBLEM_SOLVED);
525 | }
526 | 
527 | 


--------------------------------------------------------------------------------
/src/core/src/src/State.cpp:
--------------------------------------------------------------------------------
 1 | #include <StepUpPlanner/State.h>
 2 | #include <cassert>
 3 | 
 4 | StepUpPlanner::State::State()
 5 |     : m_position(3, 1)
 6 |       , m_velocity(3, 1)
 7 | { }
 8 | 
 9 | StepUpPlanner::State::~State()
10 | { }
11 | 
12 | void StepUpPlanner::State::setPosition(double px, double py, double pz)
13 | {
14 |     m_position(0) = px;
15 |     m_position(1) = py;
16 |     m_position(2) = pz;
17 | }
18 | 
19 | void StepUpPlanner::State::setVelocity(double vx, double vy, double vz)
20 | {
21 |     m_velocity(0) = vx;
22 |     m_velocity(1) = vy;
23 |     m_velocity(2) = vz;
24 | }
25 | 
26 | casadi::DM &StepUpPlanner::State::position()
27 | {
28 |     return m_position;
29 | }
30 | 
31 | double StepUpPlanner::State::position(size_t i) const
32 | {
33 |     assert(i < 3);
34 |     return static_cast<double>(m_position(i));
35 | }
36 | 
37 | const casadi::DM &StepUpPlanner::State::position() const
38 | {
39 |     return m_position;
40 | }
41 | 
42 | casadi::DM &StepUpPlanner::State::velocity()
43 | {
44 |     return  m_velocity;
45 | }
46 | 
47 | double StepUpPlanner::State::velocity(size_t i) const
48 | {
49 |     assert(i < 3);
50 |     return static_cast<double>(m_velocity(i));
51 | }
52 | 
53 | const casadi::DM &StepUpPlanner::State::velocity() const
54 | {
55 |     return  m_velocity;
56 | }
57 | 
58 | void StepUpPlanner::State::zero()
59 | {
60 |     setPosition(0.0, 0.0, 0.0);
61 |     setVelocity(0.0, 0.0, 0.0);
62 | }
63 | 


--------------------------------------------------------------------------------
/src/core/src/src/Step.cpp:
--------------------------------------------------------------------------------
  1 | #include <StepUpPlanner/Step.h>
  2 | #include <cassert>
  3 | #include <iostream>
  4 | #include <cmath>
  5 | 
  6 | void StepUpPlanner::Step::scaleFootVertices(double scale, const std::vector<StepUpPlanner::Vertex> &vertices,
  7 |                                             double xOffset, double yOffset)
  8 | {
  9 |     if (vertices.empty()) {
 10 |         return;
 11 |     }
 12 | 
 13 |     m_scaledFootVertices.resize(vertices.size());
 14 | 
 15 |     StepUpPlanner::Vertex footCenter;
 16 | 
 17 |     footCenter.x = 0.0;
 18 |     footCenter.y = 0.0;
 19 | 
 20 |     for (auto& vertex : vertices) {
 21 |         footCenter.x += vertex.x;
 22 |         footCenter.y += vertex.y;
 23 |     }
 24 |     footCenter.x /= vertices.size();
 25 |     footCenter.y /= vertices.size();
 26 | 
 27 |     double k_x = footCenter.x * (1.0 - scale);
 28 |     double k_y = footCenter.y * (1.0 - scale);
 29 | 
 30 |     for (size_t i = 0; i < vertices.size(); ++i) {
 31 |         m_scaledFootVertices[i].x = k_x + scale * vertices[i].x + xOffset;
 32 |         m_scaledFootVertices[i].y = k_y + scale * vertices[i].y + yOffset;
 33 |     }
 34 | 
 35 | }
 36 | 
 37 | bool StepUpPlanner::Step::computeCoPConstraints(const std::vector<Vertex> &vertices)
 38 | {
 39 |     if (vertices.size() < 3) {
 40 |         // Right now we consider only feet with at least three vertices. When only two vertices are available, the CoP should be constrained on
 41 |         // a line, thus the inequality should become an equality. A possibility could be to write two inequalities,
 42 |         // but this could break the LICQ condition if not correctly handled by the solver. For the time being, this special case is not considered.
 43 |         std::cerr << "[StepUpPlanner::Step::computeCoPConstraints] The vertices are supposed to be at least three." << std::endl;
 44 |         m_copConstraints = casadi::Function();
 45 |         return false;
 46 |     }
 47 | 
 48 |     m_edgeConstraints = casadi::MX(static_cast<casadi_int>(vertices.size()), 1);
 49 |     m_copBounds = casadi::DM(static_cast<casadi_int>(vertices.size()), 1);
 50 | 
 51 |     casadi::MX cop = casadi::MX::sym("cop", 2, 1);
 52 |     double xMultiplier, yMultiplier;
 53 |     double bound;
 54 | 
 55 |     size_t previousIndex = vertices.size() - 1;
 56 |     size_t nextIndex = 1;
 57 |     for (size_t i = 0; i < vertices.size(); ++i) {
 58 | 
 59 |         if (std::abs(vertices[i].y - vertices[previousIndex].y) < 1e-8 && std::abs(vertices[i].x - vertices[previousIndex].x) < 1e-8) {
 60 |             std::cerr << "[StepUpPlanner::Step::computeCoPConstraints] Vertex " << i << " and " << previousIndex;
 61 |             std::cerr << "seem to be coincident." << std::endl;
 62 |             m_copConstraints = casadi::Function();
 63 |             return false;
 64 |         }
 65 | 
 66 |         if (std::abs(vertices[i].x - vertices[previousIndex].x) > std::abs(vertices[i].y - vertices[previousIndex].y)) {
 67 |             xMultiplier = (vertices[i].y - vertices[previousIndex].y) / (vertices[i].x - vertices[previousIndex].x);
 68 |             yMultiplier = -1;
 69 |             bound = -vertices[previousIndex].y + vertices[previousIndex].x * (vertices[i].y - vertices[previousIndex].y) /
 70 |                     (vertices[i].x - vertices[previousIndex].x);
 71 |         } else {
 72 |             xMultiplier = -1;
 73 |             yMultiplier = (vertices[i].x - vertices[previousIndex].x) / (vertices[i].y - vertices[previousIndex].y);
 74 |             bound = -vertices[previousIndex].x + vertices[previousIndex].y * (vertices[i].x - vertices[previousIndex].x) /
 75 |                     (vertices[i].y - vertices[previousIndex].y);
 76 |         }
 77 | 
 78 | 
 79 |         double nextPointValue = xMultiplier * vertices [nextIndex].x + yMultiplier * vertices [nextIndex].y;
 80 |         if (std::abs(nextPointValue - bound) < 1e-8) {
 81 |             std::cerr << "[StepUpPlanner::Step::computeCoPConstraints] The vertices with index ";
 82 |             std::cerr << previousIndex << ", " << i << ", " << nextIndex << " are aligned." << std::endl;
 83 |             m_copConstraints = casadi::Function();
 84 |             return false;
 85 |         }
 86 | 
 87 |         //To understand the sign of the inequality, check in which half plane the next vertex lies.
 88 |         if (nextPointValue < bound) {
 89 |             m_edgeConstraints(i) = xMultiplier * cop(0) + yMultiplier * cop(1);
 90 |             m_copBounds(i) = bound;
 91 |         } else {
 92 |             m_edgeConstraints(i) = -xMultiplier * cop(0) - yMultiplier * cop(1);
 93 |             m_copBounds(i) = -bound;
 94 |         }
 95 | 
 96 |         previousIndex = i;
 97 |         nextIndex++;
 98 |         if (nextIndex >= vertices.size()) {
 99 |             nextIndex = 0;
100 |         }
101 |     }
102 | 
103 |     m_copConstraints = casadi::Function("CoPConstraints", {cop}, {m_edgeConstraints});
104 | 
105 |     assert(!m_copConstraints.is_null());
106 | 
107 |     return true;
108 | }
109 | 
110 | StepUpPlanner::Step::Step()
111 |     : m_position(3,1)
112 |       , m_scaleFactor(1.0)
113 |       , m_xOffset(0.0)
114 |       , m_yOffset(0.0)
115 | { }
116 | 
117 | StepUpPlanner::Step::Step(double px, double py, double pz)
118 |     : m_position(3,1)
119 |       , m_scaleFactor(1.0)
120 |       , m_xOffset(0.0)
121 |       , m_yOffset(0.0)
122 | {
123 |     setPosition(px, py, pz);
124 | }
125 | 
126 | StepUpPlanner::Step::Step(double px, double py, double pz, const std::vector<Vertex> &vertices)
127 |     : m_position(3,1)
128 |       , m_scaleFactor(1.0)
129 |       , m_xOffset(0.0)
130 |       , m_yOffset(0.0)
131 | {
132 |     setPosition(px, py, pz);
133 |     bool ok = setVertices(vertices);
134 |     assert(ok);
135 | }
136 | 
137 | StepUpPlanner::Step::~Step()
138 | { }
139 | 
140 | void StepUpPlanner::Step::setPosition(double px, double py, double pz)
141 | {
142 |     m_position(0) = px;
143 |     m_position(1) = py;
144 |     m_position(2) = pz;
145 | }
146 | 
147 | bool StepUpPlanner::Step::setVertices(const std::vector<Vertex> &vertices, double scale, double xOffset, double yOffset)
148 | {
149 |     if (scale < 0.0) {
150 |         std::cerr << "[StepUpPlanner::Step::Step] The scale is supposed to be positive." << std::endl;
151 |         return false;
152 |     }
153 | 
154 |     scaleFootVertices(scale, vertices, xOffset, yOffset);
155 | 
156 |     bool ok = computeCoPConstraints(m_scaledFootVertices);
157 |     if (!ok) {
158 |         std::cerr << "[StepUpPlanner::Step::Step] Failed to compute the CoP constraint function." << std::endl;
159 |         scaleFootVertices(m_scaleFactor, vertices, m_xOffset, m_yOffset);
160 |         return false;
161 |     }
162 |     m_scaleFactor = scale;
163 |     m_xOffset = xOffset;
164 |     m_yOffset = yOffset;
165 |     m_footVertices = vertices;
166 | 
167 |     return true;
168 | }
169 | 
170 | const std::vector<StepUpPlanner::Vertex> &StepUpPlanner::Step::getOriginalVertices() const
171 | {
172 |     return m_footVertices;
173 | }
174 | 
175 | const std::vector<StepUpPlanner::Vertex> &StepUpPlanner::Step::getScaledVertices() const
176 | {
177 |     return m_scaledFootVertices;
178 | }
179 | 
180 | casadi::DM &StepUpPlanner::Step::position()
181 | {
182 |     return m_position;
183 | }
184 | 
185 | const casadi::DM &StepUpPlanner::Step::position() const
186 | {
187 |     return m_position;
188 | }
189 | 
190 | StepUpPlanner::Rotation &StepUpPlanner::Step::rotation()
191 | {
192 |     return m_rotation;
193 | }
194 | 
195 | const StepUpPlanner::Rotation &StepUpPlanner::Step::rotation() const
196 | {
197 |     return m_rotation;
198 | }
199 | 
200 | casadi::DM StepUpPlanner::Step::getCoPBounds() const
201 | {
202 |     return m_copBounds;
203 | }
204 | 
205 | casadi::Function StepUpPlanner::Step::getCoPConstraintsFunction() const
206 | {
207 |     return m_copConstraints;
208 | }
209 | 
210 | void StepUpPlanner::Step::clear()
211 | {
212 |     setPosition(0.0, 0.0, 0.0);
213 |     m_footVertices.clear();
214 |     m_scaledFootVertices.clear();
215 |     m_scaleFactor = 1.0;
216 |     m_xOffset = 0.0;
217 |     m_yOffset = 0.0;
218 |     m_edgeConstraints = casadi::MX();
219 |     m_copBounds = casadi::DM();
220 |     m_copConstraints = casadi::Function();
221 | }
222 | 


--------------------------------------------------------------------------------
/src/core/test/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # adding support for checking the tests with valgrind
 2 | if(RUN_VALGRIND_TESTS)
 3 |      find_package(Valgrind REQUIRED)
 4 |      if(VALGRIND_FOUND)
 5 |          set(CTEST_MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM})
 6 |          set(MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM})
 7 |          if (APPLE)
 8 |              set(MEMORYCHECK_SUPPRESSIONS "--suppressions=${PROJECT_SOURCE_DIR}/cmake/valgrind-macos.supp")
 9 |          else ()
10 |              set(MEMORYCHECK_SUPPRESSIONS "")
11 |          endif ()
12 |          set(MEMORYCHECK_COMMAND_OPTIONS "--leak-check=full --error-exitcode=1 ${MEMORYCHECK_SUPPRESSIONS}"  CACHE STRING "Options to pass to the memory checker")
13 |          mark_as_advanced(MEMORYCHECK_COMMAND_OPTIONS)
14 |          set(MEMCHECK_COMMAND_COMPLETE "${MEMORYCHECK_COMMAND} ${MEMORYCHECK_COMMAND_OPTIONS}")
15 |          separate_arguments(MEMCHECK_COMMAND_COMPLETE)
16 |      endif()
17 | endif()
18 | 
19 | macro(add_sup_test classname)
20 |     set(testsrc ${classname}Test.cpp)
21 |     set(testbinary ${classname}UnitTest)
22 |     set(testname   UnitTest${classname})
23 |     add_executable(${testbinary} ${testsrc})
24 |     target_link_libraries(${testbinary} PRIVATE step_up_planner::core)
25 |     add_test(NAME ${testname} COMMAND ${testbinary})
26 | 
27 |     if(RUN_VALGRIND_TESTS)
28 |         add_test(NAME memcheck_${testname} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $<TARGET_FILE:${testbinary}>)
29 |     endif()
30 | endmacro()
31 | 
32 | add_sup_test(FiveAndThreePhases)
33 | add_sup_test(Rotation)
34 | 
35 | 


--------------------------------------------------------------------------------
/src/core/test/FiveAndThreePhasesTest.cpp:
--------------------------------------------------------------------------------
  1 | #include <StepUpPlanner/Solver.h>
  2 | #include <cassert>
  3 | 
  4 | #define ASSERT_IS_TRUE(prop) assertTrue(prop,__FILE__,__LINE__)
  5 | 
  6 | void assertTrue(bool prop, std::string file, int line)
  7 | {
  8 |     if( !prop )
  9 |     {
 10 |         std::cerr << file << ":" << line << " : assertTrue failure" << std::endl;
 11 |         assert(false);
 12 |         exit(EXIT_FAILURE);
 13 |     }
 14 | }
 15 | 
 16 | int main() {
 17 | 
 18 |     StepUpPlanner::Step l1, r1, l2, r2;
 19 | 
 20 |     l1.setPosition(0.0, 0.15, 0.0);
 21 |     l1.setVertices({{0.05, 0.05}, {0.05, -0.05}, {-0.05, -0.05}, {-0.05, 0.05}}, 0.9, 0.0, -0.01);
 22 |     ASSERT_IS_TRUE(!l1.getCoPConstraintsFunction().is_null());
 23 | 
 24 |     r1.setPosition(0.0, -0.15, 0.0);
 25 |     r1.setVertices({{0.05, 0.05}, {0.05, -0.05}, {-0.05, -0.05}, {-0.05, 0.05}}, 0.9, 0.0, 0.01);
 26 |     ASSERT_IS_TRUE(!r1.getCoPConstraintsFunction().is_null());
 27 | 
 28 | 
 29 |     l2.setPosition(0.6, 0.15, 0.4);
 30 |     l2.setVertices({{0.05, 0.05}, {0.05, -0.05}, {-0.05, -0.05}, {-0.05, 0.05}}, 0.9, 0.0, -0.01);
 31 |     ASSERT_IS_TRUE(!l2.getCoPConstraintsFunction().is_null());
 32 | 
 33 | 
 34 |     r2.setPosition(0.6, -0.15, 0.4);
 35 |     r2.setVertices({{0.05, 0.05}, {0.05, -0.05}, {-0.05, -0.05}, {-0.05, 0.05}}, 0.9, 0.0, 0.01);
 36 |     ASSERT_IS_TRUE(!r2.getCoPConstraintsFunction().is_null());
 37 | 
 38 | 
 39 |     std::vector<StepUpPlanner::Phase> phases = {StepUpPlanner::Phase(&l1, &r1),
 40 |                                                 StepUpPlanner::Phase(nullptr, &r1),
 41 |                                                 StepUpPlanner::Phase(&l2, &r1),
 42 |                                                 StepUpPlanner::Phase(&l2, nullptr),
 43 |                                                 StepUpPlanner::Phase(&l2, &r2)};
 44 | 
 45 |     phases[0].setDurationSettings(0.5, 2.0, 0.8);
 46 |     phases[1].setDurationSettings(0.5, 2.0, 1.2);
 47 |     phases[2].setDurationSettings(0.5, 2.0, 0.8);
 48 |     phases[3].setDurationSettings(0.5, 2.0, 1.2);
 49 |     phases[4].setDurationSettings(0.5, 2.0, 0.8);
 50 | 
 51 |     StepUpPlanner::Settings settings;
 52 | 
 53 |     settings.phaseLength() = 30;
 54 |     settings.solverVerbosity() = 1;
 55 |     settings.setMaximumLegLength(1.2);
 56 |     settings.setMinimumLegLength(0.8);
 57 |     settings.setIpoptLinearSolver("mumps");
 58 |     settings.setFinalStateAnticipation(0.3);
 59 |     settings.setStaticFrictionCoefficient(0.5);
 60 |     settings.setTorsionalFrictionCoefficient(0.1);
 61 | 
 62 |     double N = settings.phaseLength() * phases.size();
 63 | 
 64 |     StepUpPlanner::CostWeights& weights = settings.costWeights();
 65 |     weights.cop = 10.0/N;
 66 |     weights.torques = 1.0/N;
 67 |     weights.multipliers = 0.1/N;
 68 |     weights.finalControl = 1.0;
 69 |     weights.maxMultiplier = 0.1;
 70 |     weights.finalStateError = 10;
 71 |     weights.controlVariations = 1.0/N;
 72 |     weights.durationsDifference = 5.0/phases.size();
 73 |     weights.maxTorques = 0.1;
 74 | 
 75 |     StepUpPlanner::Solver solver(phases, settings);
 76 | 
 77 |     StepUpPlanner::State initialState;
 78 |     initialState.setPosition(0.0, 0.0, 1.16);
 79 |     initialState.setVelocity(0.0, 0.0, 0.0);
 80 | 
 81 |     StepUpPlanner::References references;
 82 |     references.zero();
 83 |     references.desiredState().setPosition(0.6, 0.0, 1.56);
 84 |     references.desiredState().setVelocity(0.0, 0.0, 0.0);
 85 | 
 86 |     references.desiredControl().zero();
 87 |     double desiredLeftMultiplier = static_cast<double>(9.81/(2.0*(references.desiredState().position()(2) - l2.position()(2))));
 88 |     references.desiredControl().left().setMultiplier(desiredLeftMultiplier);
 89 |     double desiredRightMultiplier = static_cast<double>(9.81/(2.0*(references.desiredState().position()(2) - r2.position()(2))));
 90 |     references.desiredControl().right().setMultiplier(desiredRightMultiplier);
 91 | 
 92 |     bool ok = references.setDesiredLegLength(1.18);
 93 |     ASSERT_IS_TRUE(ok);
 94 | 
 95 |     ok = solver.solve(initialState, references);
 96 |     ASSERT_IS_TRUE(ok);
 97 | 
 98 |     ok = solver.getFullSolution(phases);
 99 |     ASSERT_IS_TRUE(ok);
100 | 
101 |     ok = solver.solve(initialState, references);
102 |     ASSERT_IS_TRUE(ok);
103 | 
104 |     ok = solver.getFullSolution(phases);
105 |     ASSERT_IS_TRUE(ok);
106 | 
107 |     //reset and test the three phases
108 | 
109 |     phases = {StepUpPlanner::Phase(nullptr, &r1),
110 |               StepUpPlanner::Phase(&l2, &r1),
111 |               StepUpPlanner::Phase(&l2, nullptr)};
112 | 
113 |     phases[0].setDurationSettings(0.5, 2.0, 1.2);
114 |     phases[1].setDurationSettings(0.5, 2.0, 0.8);
115 |     phases[2].setDurationSettings(0.5, 2.0, 1.2);
116 | 
117 |     initialState.zero();
118 |     initialState.setPosition(0.0, -0.08, 1.16);
119 |     initialState.setVelocity(0.1, -0.05, 0.0);
120 |     references.desiredState().setPosition(0.6, 0.0, 1.56);
121 |     references.desiredState().setVelocity(0.0, 0.0, 0.0);
122 | 
123 |     references.desiredControl().zero();
124 |     desiredLeftMultiplier = static_cast<double>(9.81/((references.desiredState().position()(2) - l2.position()(2))));
125 |     references.desiredControl().left().setMultiplier(desiredLeftMultiplier);
126 | 
127 |     ok = solver.resetProblem(phases, settings);
128 |     ASSERT_IS_TRUE(ok);
129 | 
130 |     ok = solver.solve(initialState, references);
131 |     ASSERT_IS_TRUE(ok);
132 | 
133 |     ok = solver.getFullSolution(phases);
134 |     ASSERT_IS_TRUE(ok);
135 | 
136 |     ok = solver.solve(initialState, references);
137 |     ASSERT_IS_TRUE(ok);
138 | 
139 |     ok = solver.getFullSolution(phases);
140 |     ASSERT_IS_TRUE(ok);
141 | 
142 | 
143 |     return EXIT_SUCCESS;
144 | }
145 | 


--------------------------------------------------------------------------------
/src/core/test/RotationTest.cpp:
--------------------------------------------------------------------------------
  1 | #include <StepUpPlanner/Rotation.h>
  2 | #include <stdlib.h>
  3 | #include <cassert>
  4 | #include <cmath>
  5 | 
  6 | #define ASSERT_IS_TRUE(prop) assertTrue(prop,__FILE__,__LINE__)
  7 | 
  8 | void assertTrue(bool prop, std::string file, int line)
  9 | {
 10 |     if( !prop )
 11 |     {
 12 |         std::cerr << file << ":" << line << " : assertTrue failure" << std::endl;
 13 |         assert(false);
 14 |         exit(EXIT_FAILURE);
 15 |     }
 16 | }
 17 | 
 18 | double getRandomDouble(double min = 0.0, double max = 1.0)
 19 | {
 20 |     return min + (max-min)*(static_cast<double>(rand()))/(static_cast<double>(RAND_MAX));
 21 | }
 22 | 
 23 | bool matricesAreEqual(const casadi::DM& left, const casadi::DM& right, double tol = 1e-15) {
 24 |     if ((left.rows() != right.rows()) || (left.columns() != right.columns())) {
 25 |         return false;
 26 |     }
 27 | 
 28 |     for (int i = 0; i < left.rows(); ++i) {
 29 |         for (int j = 0; j < left.columns(); ++j) {
 30 |             if (std::abs(static_cast<double>(left(i,j) - right(i,j))) > tol) {
 31 |                 return false;
 32 |             }
 33 |         }
 34 |     }
 35 | 
 36 |     return true;
 37 | }
 38 | 
 39 | bool vectorsAreEqual(const casadi::DM& left, const casadi::DM& right, double tol = 1e-15) {
 40 |     if (left.rows() != right.rows()) {
 41 |         return false;
 42 |     }
 43 | 
 44 |     for (int i = 0; i < left.rows(); ++i) {
 45 |         if (std::abs(static_cast<double>(left(i) - right(i))) > tol) {
 46 |             return false;
 47 |         }
 48 |     }
 49 | 
 50 |     return true;
 51 | }
 52 | 
 53 | int main() {
 54 | 
 55 |     casadi::DM identity = casadi::DM::zeros(3,3);
 56 |     identity(0,0) = 1.0;
 57 |     identity(1,1) = 1.0;
 58 |     identity(2,2) = 1.0;
 59 | 
 60 |     StepUpPlanner::Rotation testRotation;
 61 | 
 62 |     ASSERT_IS_TRUE(matricesAreEqual(testRotation.asMatrix(), identity));
 63 | 
 64 |     ASSERT_IS_TRUE(matricesAreEqual(StepUpPlanner::Rotation::Identity().asMatrix(), identity));
 65 | 
 66 |     testRotation.setFromQuaternion(1.0, 0.0, 0.0, 0.0);
 67 | 
 68 |     ASSERT_IS_TRUE(matricesAreEqual(testRotation.asMatrix(), identity));
 69 | 
 70 |     casadi::DM quaternion(4,1);
 71 |     quaternion(0) = getRandomDouble();
 72 |     quaternion(1) = getRandomDouble(-1.0, 1.0);
 73 |     quaternion(2) = getRandomDouble(-1.0, 1.0);
 74 |     quaternion(3) = getRandomDouble(-1.0, 1.0);
 75 | 
 76 |     testRotation.setFromQuaternion(static_cast<double>(quaternion(0)), static_cast<double>(quaternion(1)),
 77 |                                    static_cast<double>(quaternion(2)), static_cast<double>(quaternion(3)));
 78 | 
 79 |     casadi::DM expectedIdentity = casadi::DM::mtimes(testRotation.asMatrix(), testRotation.asMatrix().T());
 80 | 
 81 |     ASSERT_IS_TRUE(matricesAreEqual(expectedIdentity, identity));
 82 | 
 83 |     StepUpPlanner::Rotation invertedRotation;
 84 | 
 85 |     invertedRotation.setFromQuaternion(-static_cast<double>(quaternion(0)), static_cast<double>(quaternion(1)),
 86 |                                    static_cast<double>(quaternion(2)), static_cast<double>(quaternion(3)));
 87 | 
 88 |     expectedIdentity = casadi::DM::mtimes(testRotation.asMatrix(), invertedRotation.asMatrix());
 89 | 
 90 |     ASSERT_IS_TRUE(matricesAreEqual(expectedIdentity, identity));
 91 | 
 92 |     quaternion(0) = getRandomDouble();
 93 |     quaternion(1) = getRandomDouble(-1.0, 1.0);
 94 |     quaternion(2) = getRandomDouble(-1.0, 1.0);
 95 |     quaternion(3) = getRandomDouble(-1.0, 1.0);
 96 | 
 97 |     testRotation.setFromQuaternion(static_cast<double>(quaternion(0)), static_cast<double>(quaternion(1)),
 98 |                                    static_cast<double>(quaternion(2)), static_cast<double>(quaternion(3)));
 99 | 
100 |     StepUpPlanner::Rotation testQuaternion(testRotation.asMatrix());
101 | 
102 |     ASSERT_IS_TRUE(vectorsAreEqual(testRotation.asQuaternion(), testQuaternion.asQuaternion()));
103 | 
104 |     return EXIT_SUCCESS;
105 | }
106 | 


--------------------------------------------------------------------------------
/src/interface/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | find_package(ament_cmake REQUIRED)
 2 | find_package(rclcpp REQUIRED)
 3 | find_package(std_msgs REQUIRED)
 4 | find_package(controller_msgs REQUIRED)
 5 | 
 6 | add_subdirectory(src)
 7 | 
 8 | if(BUILD_TESTING)
 9 |     add_subdirectory(test)
10 | endif()
11 | 


--------------------------------------------------------------------------------
/src/interface/src/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | 
 2 | add_library(${PROJECT_NAME}_responderLibrary include/Responder.h src/Responder.cpp)
 3 | 
 4 | #TODO this is probably not the ROS way to link a library
 5 | 
 6 | target_link_libraries(${PROJECT_NAME}_responderLibrary step_up_planner::core)
 7 | 
 8 | target_include_directories(${PROJECT_NAME}_responderLibrary PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
 9 |                                                             "$<INSTALL_INTERFACE:$<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
10 | 
11 | ament_target_dependencies(${PROJECT_NAME}_responderLibrary rclcpp controller_msgs)
12 | 
13 | 
14 | add_executable(${PROJECT_NAME}_responder src/ResponderMain.cpp)
15 | 
16 | target_link_libraries(${PROJECT_NAME}_responder ${PROJECT_NAME}_responderLibrary)
17 | 
18 | install(TARGETS
19 |   ${PROJECT_NAME}_responder
20 |   DESTINATION bin/${PROJECT_NAME}
21 | )
22 | 
23 | install(TARGETS
24 |   ${PROJECT_NAME}_responderLibrary
25 |   DESTINATION lib/${PROJECT_NAME}
26 | )
27 | 
28 | ament_package()
29 | 


--------------------------------------------------------------------------------
/src/interface/src/include/Responder.h:
--------------------------------------------------------------------------------
 1 | #ifndef STEPUPPLANNER_IF_RESPONDER_H
 2 | #define STEPUPPLANNER_IF_RESPONDER_H
 3 | 
 4 | #include <rclcpp/rclcpp.hpp>
 5 | #include <StepUpPlanner/Solver.h>
 6 | #include <controller_msgs/msg/step_up_planner_parameters_message.hpp>
 7 | #include <controller_msgs/msg/step_up_planner_request_message.hpp>
 8 | #include <controller_msgs/msg/step_up_planner_respond_message.hpp>
 9 | #include <controller_msgs/msg/step_up_planner_error_message.hpp>
10 | #include <controller_msgs/msg/center_of_mass_trajectory_message.hpp>
11 | #include <controller_msgs/msg/pelvis_height_trajectory_message.hpp>
12 | #include <controller_msgs/msg/footstep_data_list_message.hpp>
13 | #include <vector>
14 | #include <string>
15 | 
16 | #define STEPUPPLANNER_REQUEST_TOPIC    "/us/ihmc/stepUpPlanner/request"
17 | #define STEPUPPLANNER_RESPOND_TOPIC    "/us/ihmc/stepUpPlanner/respond"
18 | #define STEPUPPLANNER_PARAMETERS_TOPIC "/us/ihmc/stepUpPlanner/parameters"
19 | #define STEPUPPLANNER_ERRORS_TOPIC     "/us/ihmc/stepUpPlanner/errors"
20 | 
21 | namespace StepUpPlanner {
22 |     class Responder;
23 | }
24 | 
25 | class StepUpPlanner::Responder : public rclcpp::Node {
26 | 
27 |     enum class Errors {
28 |         PARAMETERS_ERROR,
29 |         PARAMETERS_NOT_SET,
30 |         REQUEST_ERROR,
31 |         SOLVER_ERROR
32 |     };
33 | 
34 |     rclcpp::Publisher<controller_msgs::msg::StepUpPlannerRespondMessage>::SharedPtr m_respondPublisher;
35 |     rclcpp::Publisher<controller_msgs::msg::StepUpPlannerErrorMessage>::SharedPtr m_errorPublisher;
36 |     rclcpp::Subscription<controller_msgs::msg::StepUpPlannerParametersMessage>::SharedPtr m_parametersSubscriber;
37 |     rclcpp::Subscription<controller_msgs::msg::StepUpPlannerRequestMessage>::SharedPtr m_requestSubscriber;
38 |     controller_msgs::msg::StepUpPlannerRespondMessage::SharedPtr m_respondMessage;
39 |     rclcpp::Publisher<controller_msgs::msg::CenterOfMassTrajectoryMessage>::SharedPtr m_CoMMessagePublisher;
40 |     rclcpp::Publisher<controller_msgs::msg::PelvisHeightTrajectoryMessage>::SharedPtr m_pelvisMessagePublisher;
41 |     rclcpp::Publisher<controller_msgs::msg::FootstepDataListMessage>::SharedPtr m_feetMessagePublisher;
42 |     std::vector<controller_msgs::msg::CenterOfMassTrajectoryMessage::SharedPtr> m_CoMMessages;
43 |     size_t m_CoMMessagesPerPhase;
44 |     std::vector<controller_msgs::msg::PelvisHeightTrajectoryMessage::SharedPtr> m_pelvisMessages;
45 |     size_t m_pelvisMessagesPerPhase;
46 |     double m_pelvisHeightDelta;
47 |     controller_msgs::msg::FootstepDataListMessage::SharedPtr m_feetMessage;
48 | 
49 |     std::vector<StepUpPlanner::Phase> m_phases;
50 |     StepUpPlanner::Solver m_solver;
51 | 
52 |     bool prepareSolver(const controller_msgs::msg::StepUpPlannerRequestMessage::SharedPtr msg, State &initialState, References &references);
53 | 
54 |     void respond(const controller_msgs::msg::StepUpPlannerRequestMessage::SharedPtr msg);
55 | 
56 |     bool processPhaseSettings(const controller_msgs::msg::StepUpPlannerParametersMessage::SharedPtr msg);
57 | 
58 |     bool processCoMMessagesSettings(const controller_msgs::msg::StepUpPlannerParametersMessage::SharedPtr msg, const Settings &settings);
59 | 
60 |     bool processPelvisHeightMessagesSettings(const controller_msgs::msg::StepUpPlannerParametersMessage::SharedPtr msg,
61 |                                              const Settings &settings);
62 | 
63 |     bool processFootStepMessagesSettings(const controller_msgs::msg::StepUpPlannerParametersMessage::SharedPtr msg);
64 | 
65 |     bool processPlannerSettings(const controller_msgs::msg::StepUpPlannerParametersMessage::SharedPtr msg, Settings &settings);
66 | 
67 |     void processParameters(const controller_msgs::msg::StepUpPlannerParametersMessage::SharedPtr msg);
68 | 
69 |     void sendErrorMessage(Errors errorType, const std::string& errorMessage);
70 | 
71 |     void prepareRespondMessage();
72 | 
73 |     void sendCenterOfMassTrajectoryMessages();
74 | 
75 |     void sendPelvisHeightTrajectoryMessages();
76 | 
77 |     void sendFootStepDataListMessage();
78 | 
79 |     void ackReceivedParameters(unsigned int message_id);
80 | 
81 | public:
82 | 
83 |     Responder();
84 | 
85 | };
86 | 
87 | 
88 | 
89 | 
90 | #endif // STEPUPPLANNER_IF_RESPONDER_H
91 | 


--------------------------------------------------------------------------------
/src/interface/src/src/ResponderMain.cpp:
--------------------------------------------------------------------------------
 1 | #include <Responder.h>
 2 | 
 3 | int main(int argc, char * argv[])
 4 | {
 5 |     rclcpp::init(argc, argv);
 6 |     auto responder = std::make_shared<StepUpPlanner::Responder>();
 7 |     RCLCPP_INFO(responder->get_logger(), "Running...");
 8 |     rclcpp::spin(responder);
 9 |     rclcpp::shutdown();
10 |     return EXIT_SUCCESS;
11 | }
12 | 


--------------------------------------------------------------------------------
/src/interface/test/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # adding support for checking the tests with valgrind
 2 | if(RUN_VALGRIND_TESTS)
 3 |      find_package(Valgrind REQUIRED)
 4 |      if(VALGRIND_FOUND)
 5 |          set(CTEST_MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM})
 6 |          set(MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM})
 7 |          if (APPLE)
 8 |              set(MEMORYCHECK_SUPPRESSIONS "--suppressions=${PROJECT_SOURCE_DIR}/cmake/valgrind-macos.supp")
 9 |          else ()
10 |              set(MEMORYCHECK_SUPPRESSIONS "")
11 |          endif ()
12 |          set(MEMORYCHECK_COMMAND_OPTIONS "--leak-check=full --error-exitcode=1 ${MEMORYCHECK_SUPPRESSIONS}"  CACHE STRING "Options to pass to the memory checker")
13 |          mark_as_advanced(MEMORYCHECK_COMMAND_OPTIONS)
14 |          set(MEMCHECK_COMMAND_COMPLETE "${MEMORYCHECK_COMMAND} ${MEMORYCHECK_COMMAND_OPTIONS}")
15 |          separate_arguments(MEMCHECK_COMMAND_COMPLETE)
16 |      endif()
17 | endif()
18 | 
19 | # TODO avoid using directly the source files to test it
20 | set(testsrc ResponderTest.cpp)
21 | set(testbinary ResponderUnitTest)
22 | set(testname   UnitTestResponder)
23 | add_executable(${testbinary} ${testsrc})
24 | target_link_libraries(${testbinary} ${PROJECT_NAME}_responderLibrary)
25 | target_include_directories(${testbinary} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../src/include>")
26 | ament_target_dependencies(${testbinary} rclcpp controller_msgs)
27 | add_test(NAME ${testname} COMMAND ${testbinary})
28 | 
29 | if(RUN_VALGRIND_TESTS)
30 |     add_test(NAME memcheck_${testname} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $<TARGET_FILE:${testbinary}>)
31 | endif()
32 | 


--------------------------------------------------------------------------------
/src/interface/test/ResponderTest.cpp:
--------------------------------------------------------------------------------
  1 | #include <rclcpp/rclcpp.hpp>
  2 | #include <Responder.h>
  3 | #include <controller_msgs/msg/center_of_mass_trajectory_message.hpp>
  4 | #include <controller_msgs/msg/footstep_data_list_message.hpp>
  5 | #include <controller_msgs/msg/step_up_planner_parameters_message.hpp>
  6 | #include <controller_msgs/msg/step_up_planner_request_message.hpp>
  7 | #include <controller_msgs/msg/step_up_planner_respond_message.hpp>
  8 | #include <controller_msgs/msg/step_up_planner_error_message.hpp>
  9 | #include <cassert>
 10 | #include <iostream>
 11 | 
 12 | #include <chrono>
 13 | using namespace std::chrono_literals;
 14 | 
 15 | #define ASSERT_IS_TRUE(prop) assertTrue(prop,__FILE__,__LINE__)
 16 | 
 17 | void assertTrue(bool prop, std::string file, int line)
 18 | {
 19 |     if( !prop )
 20 |     {
 21 |         std::cerr << file << ":" << line << " : assertTrue failure" << std::endl;
 22 |         assert(false);
 23 |         exit(EXIT_FAILURE);
 24 |     }
 25 | }
 26 | 
 27 | class ErrorSubscriber : public rclcpp::Node
 28 | {
 29 | public:
 30 |     ErrorSubscriber()
 31 |         : Node("error_subscriber")
 32 |     {
 33 |         subscription_ = this->create_subscription<controller_msgs::msg::StepUpPlannerErrorMessage>(
 34 |             STEPUPPLANNER_ERRORS_TOPIC,
 35 |             [this](controller_msgs::msg::StepUpPlannerErrorMessage::UniquePtr errorMessage) {
 36 |                 if (errorMessage->error_code) {
 37 |                     std::ostringstream asString;
 38 |                     asString << "Received error with code: " << std::to_string(errorMessage->error_code) << std::endl;
 39 |                     asString << "Error description: " << errorMessage->error_description << std::endl;
 40 |                     RCLCPP_INFO(this->get_logger(), asString.str());
 41 |                     ASSERT_IS_TRUE(false);
 42 |                 } else {
 43 |                     index = errorMessage->sequence_id_received;
 44 |                     RCLCPP_INFO(this->get_logger(), "Received ack for message with ID: "
 45 |                                     + std::to_string(errorMessage->sequence_id_received));
 46 |                 }
 47 |             });
 48 |     }
 49 |     ~ErrorSubscriber();
 50 | 
 51 |     unsigned int index = 0;
 52 |     unsigned char error_code = 0;
 53 | 
 54 | private:
 55 |     rclcpp::Subscription<controller_msgs::msg::StepUpPlannerErrorMessage>::SharedPtr subscription_;
 56 | };
 57 | ErrorSubscriber::~ErrorSubscriber(){}
 58 | 
 59 | class RespondSubscriber : public rclcpp::Node
 60 | {
 61 | public:
 62 |     RespondSubscriber()
 63 |         : Node("respond_subscriber")
 64 |     {
 65 |         m_subscription = this->create_subscription<controller_msgs::msg::StepUpPlannerRespondMessage>(
 66 |             STEPUPPLANNER_RESPOND_TOPIC,
 67 |             [this](controller_msgs::msg::StepUpPlannerRespondMessage::UniquePtr message) {
 68 |                 std::ostringstream asString;
 69 |                 asString << "Received message" << std::endl;
 70 | 
 71 |                 messageReceived = true;
 72 | 
 73 |                 ASSERT_IS_TRUE(message->com_messages.size() == 5);
 74 |                 ASSERT_IS_TRUE(message->pelvis_height_messages.size() == 5);
 75 |                 ASSERT_IS_TRUE(message->foostep_messages.size() == 1);
 76 | 
 77 | 
 78 |                 for (size_t i = 0; i < message->com_messages.size(); ++i) {
 79 |                     ASSERT_IS_TRUE(message->com_messages[i].euclidean_trajectory.taskspace_trajectory_points.size());
 80 |                 }
 81 | 
 82 |                 for (size_t i = 0; i < message->pelvis_height_messages.size(); ++i) {
 83 |                     ASSERT_IS_TRUE(message->com_messages[i].euclidean_trajectory.taskspace_trajectory_points.size());
 84 |                 }
 85 | 
 86 |                 RCLCPP_INFO(this->get_logger(), asString.str());
 87 |             });
 88 |     }
 89 |     ~RespondSubscriber();
 90 | 
 91 |     bool messageReceived = false;
 92 | 
 93 | private:
 94 |     rclcpp::Subscription<controller_msgs::msg::StepUpPlannerRespondMessage>::SharedPtr m_subscription;
 95 | };
 96 | RespondSubscriber::~RespondSubscriber(){}
 97 | 
 98 | class CoMSubscriber : public rclcpp::Node
 99 | {
100 | public:
101 |     CoMSubscriber(const std::string& topicName)
102 |         : Node("com_subscriber")
103 |     {
104 |         m_subscription = this->create_subscription<controller_msgs::msg::CenterOfMassTrajectoryMessage>(
105 |             topicName, [this](controller_msgs::msg::CenterOfMassTrajectoryMessage::UniquePtr message) {
106 |                 std::ostringstream asString;
107 |                 asString << "Received CoM message (ID " << message->euclidean_trajectory.queueing_properties.message_id <<")" << std::endl;
108 |                 ASSERT_IS_TRUE(message->euclidean_trajectory.taskspace_trajectory_points.size());
109 | 
110 |                 messagesReceived++;
111 | 
112 |                 RCLCPP_INFO(this->get_logger(), asString.str());
113 |             });
114 |     }
115 |     ~CoMSubscriber();
116 | 
117 |     size_t messagesReceived = 0;
118 | 
119 | private:
120 |     rclcpp::Subscription<controller_msgs::msg::CenterOfMassTrajectoryMessage>::SharedPtr m_subscription;
121 | };
122 | CoMSubscriber::~CoMSubscriber(){}
123 | 
124 | class PelvisHeightSubscriber : public rclcpp::Node
125 | {
126 | public:
127 |     PelvisHeightSubscriber(const std::string& topicName)
128 |         : Node("pelvis_height_subscriber")
129 |     {
130 |         m_subscription = this->create_subscription<controller_msgs::msg::PelvisHeightTrajectoryMessage>(
131 |             topicName, [this](controller_msgs::msg::PelvisHeightTrajectoryMessage::UniquePtr message) {
132 |                 std::ostringstream asString;
133 |                 asString << "Received Pelvis message (ID " << message->euclidean_trajectory.queueing_properties.message_id <<")" << std::endl;
134 |                 ASSERT_IS_TRUE(message->euclidean_trajectory.taskspace_trajectory_points.size());
135 | 
136 |                 messagesReceived++;
137 | 
138 |                 RCLCPP_INFO(this->get_logger(), asString.str());
139 |             });
140 |     }
141 |     ~PelvisHeightSubscriber();
142 | 
143 |     size_t messagesReceived = 0;
144 | 
145 | private:
146 |     rclcpp::Subscription<controller_msgs::msg::PelvisHeightTrajectoryMessage>::SharedPtr m_subscription;
147 | };
148 | PelvisHeightSubscriber::~PelvisHeightSubscriber(){}
149 | 
150 | class FootstepsSubscriber : public rclcpp::Node
151 | {
152 | public:
153 |     FootstepsSubscriber(const std::string& topicName)
154 |         : Node("footsteps_subscriber")
155 |     {
156 |         m_subscription = this->create_subscription<controller_msgs::msg::FootstepDataListMessage>(
157 |             topicName, [this](controller_msgs::msg::FootstepDataListMessage::UniquePtr message) {
158 |                 std::ostringstream asString;
159 |                 asString << "Received feet message: " << message->footstep_data_list.size() << " steps." << std::endl;
160 | 
161 |                 messageReceived = true;
162 | 
163 |                 RCLCPP_INFO(this->get_logger(), asString.str());
164 |             });
165 |     }
166 |     ~FootstepsSubscriber();
167 | 
168 |     bool messageReceived = false;
169 | 
170 | private:
171 |     rclcpp::Subscription<controller_msgs::msg::FootstepDataListMessage>::SharedPtr m_subscription;
172 | };
173 | FootstepsSubscriber::~FootstepsSubscriber(){}
174 | 
175 | controller_msgs::msg::StepUpPlannerParametersMessage::SharedPtr fillParametersMessage(unsigned int id,
176 |                                                                                       const std::string& comMessageTopic,
177 |                                                                                       const std::string& pelvisMessageTopic,
178 |                                                                                       const std::string& feetMessageTopic) {
179 |     controller_msgs::msg::StepUpPlannerParametersMessage::SharedPtr msg =
180 |         std::make_shared<controller_msgs::msg::StepUpPlannerParametersMessage>();
181 | 
182 |     std::vector<controller_msgs::msg::StepUpPlannerStepParameters> leftSteps;
183 |     std::vector<controller_msgs::msg::StepUpPlannerStepParameters> rightSteps;
184 | 
185 |     leftSteps.resize(5);
186 |     rightSteps.resize(5);
187 | 
188 |     for (size_t i = 0; i < leftSteps.size(); ++i) {
189 |         leftSteps[i].foot_vertices.resize(4);
190 |         leftSteps[i].foot_vertices[0].x = 0.05;
191 |         leftSteps[i].foot_vertices[0].y = 0.05;
192 |         leftSteps[i].foot_vertices[1].x = 0.05;
193 |         leftSteps[i].foot_vertices[1].y = -0.05;
194 |         leftSteps[i].foot_vertices[2].x = -0.05;
195 |         leftSteps[i].foot_vertices[2].y = -0.05;
196 |         leftSteps[i].foot_vertices[3].x = -0.05;
197 |         leftSteps[i].foot_vertices[3].y = 0.05;
198 | 
199 |         leftSteps[i].scale = 0.9;
200 | 
201 |         rightSteps[i].set__foot_vertices(leftSteps[i].foot_vertices);
202 |         rightSteps[i].scale = 0.9;
203 |     }
204 | 
205 |     auto stand = leftSteps[0].STAND;
206 |     auto swing = leftSteps[0].SWING;
207 | 
208 |     leftSteps[0].set__state(stand);
209 |     rightSteps[0].set__state(stand);
210 | 
211 |     leftSteps[1].set__state(swing);
212 |     rightSteps[1].set__state(stand);
213 | 
214 |     leftSteps[2].set__state(stand);
215 |     rightSteps[2].set__state(stand);
216 | 
217 |     leftSteps[3].set__state(stand);
218 |     rightSteps[3].set__state(swing);
219 | 
220 |     leftSteps[4].set__state(stand);
221 |     rightSteps[4].set__state(stand);
222 | 
223 |     msg->phases_parameters.resize(5);
224 |     for (size_t p = 0; p < msg->phases_parameters.size(); ++p) {
225 |         msg->phases_parameters[p].set__left_step_parameters(leftSteps[p]);
226 |         msg->phases_parameters[p].set__right_step_parameters(rightSteps[p]);
227 |     }
228 | 
229 |     msg->phase_length = 30;
230 |     msg->solver_verbosity = 1;
231 |     msg->max_leg_length = 1.2;
232 | //    msg->ipopt_linear_solver = "ma27";
233 |     msg->final_state_anticipation = 0.3;
234 |     msg->static_friction_coefficient = 0.5;
235 |     msg->torsional_friction_coefficient = 0.1;
236 | 
237 |     double N = msg->phase_length * msg->phases_parameters.size();
238 | 
239 |     controller_msgs::msg::StepUpPlannerCostWeights weights;
240 | 
241 |     weights.cop = 10.0/N;
242 |     weights.torques = 1.0/N;
243 |     weights.max_torques = 0.1;
244 |     weights.control_multipliers = 0.1/N;
245 |     weights.final_control = 1.0;
246 |     weights.max_control_multiplier = 0.1;
247 |     weights.final_state = 10.0;
248 |     weights.control_variations = 1.0/N;
249 |     weights.durations_difference = 5.0/msg->phases_parameters.size();
250 | 
251 |     msg->set__cost_weights(weights);
252 | 
253 |     msg->sequence_id = id;
254 | 
255 |     msg->send_com_messages = true;
256 |     msg->com_messages_topic = comMessageTopic;
257 |     msg->max_com_message_length = 50;
258 |     msg->include_com_messages = true;
259 | 
260 |     msg->send_pelvis_height_messages = true;
261 |     msg->pelvis_height_messages_topic = pelvisMessageTopic;
262 |     msg->max_pelvis_height_message_length = 50;
263 |     msg->include_pelvis_height_messages = true;
264 |     msg->pelvis_height_delta = -0.25;
265 | 
266 |     msg->send_footstep_messages = true;
267 |     msg->footstep_messages_topic = feetMessageTopic;
268 |     msg->include_footstep_messages = true;
269 | 
270 |     return msg;
271 | }
272 | 
273 | controller_msgs::msg::StepUpPlannerRequestMessage::SharedPtr fillRequestMessage() {
274 | 
275 |     controller_msgs::msg::StepUpPlannerRequestMessage::SharedPtr msg =
276 |         std::make_shared<controller_msgs::msg::StepUpPlannerRequestMessage>();
277 | 
278 |     msg->initial_com_position.x = 0.0;
279 |     msg->initial_com_position.y = 0.0;
280 |     msg->initial_com_position.z = 1.16;
281 | 
282 |     msg->initial_com_velocity.x = 0.0;
283 |     msg->initial_com_velocity.y = 0.0;
284 |     msg->initial_com_velocity.z = 0.0;
285 | 
286 |     msg->desired_com_position.x = 0.6;
287 |     msg->desired_com_position.y = 0.0;
288 |     msg->desired_com_position.z = 1.56;
289 | 
290 |     msg->desired_com_velocity.x = 0.0;
291 |     msg->desired_com_velocity.y = 0.0;
292 |     msg->desired_com_velocity.z = 0.0;
293 | 
294 |     msg->phases.resize(5);
295 |     geometry_msgs::msg::Quaternion identityQuaternion;
296 |     identityQuaternion.w = 1.0;
297 |     identityQuaternion.x = 0.0;
298 |     identityQuaternion.y = 0.0;
299 |     identityQuaternion.z = 0.0;
300 | 
301 |     msg->phases[0].left_foot_pose.position.x = 0.0;
302 |     msg->phases[0].left_foot_pose.position.y = 0.15;
303 |     msg->phases[0].left_foot_pose.position.z = 0.0;
304 |     msg->phases[0].left_foot_pose.set__orientation(identityQuaternion);
305 | 
306 |     msg->phases[0].right_foot_pose.position.x = 0.0;
307 |     msg->phases[0].right_foot_pose.position.y = -0.15;
308 |     msg->phases[0].right_foot_pose.position.z = 0.0;
309 |     msg->phases[0].right_foot_pose.set__orientation(identityQuaternion);
310 | 
311 |     msg->phases[1].set__right_foot_pose(msg->phases[0].right_foot_pose);
312 | 
313 |     msg->phases[2].left_foot_pose.position.x = 0.6;
314 |     msg->phases[2].left_foot_pose.position.y = 0.15;
315 |     msg->phases[2].left_foot_pose.position.z = 0.4;
316 |     msg->phases[2].left_foot_pose.set__orientation(identityQuaternion);
317 |     msg->phases[2].set__right_foot_pose(msg->phases[0].right_foot_pose);
318 | 
319 |     msg->phases[3].set__left_foot_pose(msg->phases[2].left_foot_pose);
320 | 
321 |     msg->phases[4].set__left_foot_pose(msg->phases[2].left_foot_pose);
322 |     msg->phases[4].right_foot_pose.position.x =   0.6;
323 |     msg->phases[4].right_foot_pose.position.y = -0.15;
324 |     msg->phases[4].right_foot_pose.position.z =   0.4;
325 |     msg->phases[4].right_foot_pose.set__orientation(identityQuaternion);
326 | 
327 | 
328 |     msg->desired_leg_length = 1.18;
329 |     msg->left_desired_final_control.cop.x = 0.0;
330 |     msg->left_desired_final_control.cop.y = 0.0;
331 |     msg->right_desired_final_control.set__cop(msg->left_desired_final_control.cop);
332 | 
333 |     msg->phases[0].minimum_duration = 0.5;
334 |     msg->phases[0].maximum_duration = 2.0;
335 |     msg->phases[0].desired_duration = 0.8;
336 | 
337 |     msg->phases[1].minimum_duration = 0.5;
338 |     msg->phases[1].maximum_duration = 2.0;
339 |     msg->phases[1].desired_duration = 1.2;
340 | 
341 |     msg->phases[2].minimum_duration = 0.5;
342 |     msg->phases[2].maximum_duration = 2.0;
343 |     msg->phases[2].desired_duration = 0.8;
344 | 
345 |     msg->phases[3].minimum_duration = 0.5;
346 |     msg->phases[3].maximum_duration = 2.0;
347 |     msg->phases[3].desired_duration = 1.2;
348 | 
349 |     msg->phases[4].minimum_duration = 0.5;
350 |     msg->phases[4].maximum_duration = 2.0;
351 |     msg->phases[4].desired_duration = 0.8;
352 | 
353 |     double desiredLeftMultiplier = static_cast<double>(9.81/(2.0*(msg->desired_com_position.z -
354 |                                                                       msg->phases[4].left_foot_pose.position.z)));
355 | 
356 |     msg->left_desired_final_control.set__multiplier(desiredLeftMultiplier);
357 | 
358 |     double desiredRightMultiplier = static_cast<double>(9.81/(2.0*(msg->desired_com_position.z -
359 |                                                                        msg->phases[4].right_foot_pose.position.z)));
360 | 
361 |     msg->right_desired_final_control.set__multiplier(desiredRightMultiplier);
362 | 
363 |     return msg;
364 | }
365 | 
366 | int main(int argc, char * argv[])
367 | {
368 |     rclcpp::init(argc, argv);
369 |     rclcpp::WallRate loop_rate(500ms);
370 | 
371 |     auto responderPublisher = std::make_shared<StepUpPlanner::Responder>();
372 |     auto errorSubscriber = std::make_shared<ErrorSubscriber>();
373 |     auto respondSubscriber = std::make_shared<RespondSubscriber>();
374 | 
375 |     std::string comTopic = "/us/ihmc/CoMtest";
376 |     std::string pelvisTopic = "/us/ihmc/Pelvistest";
377 |     std::string feetTopic = "/us/ihmc/FeetTest";
378 | 
379 |     auto comSubscriber = std::make_shared<CoMSubscriber>(comTopic);
380 |     auto pelvisSubscriber = std::make_shared<PelvisHeightSubscriber>(pelvisTopic);
381 |     auto footStepsSubscriber = std::make_shared<FootstepsSubscriber>(feetTopic);
382 | 
383 |     auto parametersPublisherNode = rclcpp::Node::make_shared("parameters_publisher");
384 |     auto parametersPublisher = parametersPublisherNode->create_publisher<controller_msgs::msg::StepUpPlannerParametersMessage>
385 |                                (STEPUPPLANNER_PARAMETERS_TOPIC);
386 | 
387 |     auto requestPublisherNode = rclcpp::Node::make_shared("request_publisher");
388 |     auto requestPublisher = parametersPublisherNode->create_publisher<controller_msgs::msg::StepUpPlannerRequestMessage>
389 |                             (STEPUPPLANNER_REQUEST_TOPIC);
390 | 
391 |     auto parametersMessage = fillParametersMessage(1, comTopic, pelvisTopic, feetTopic);
392 |     loop_rate.sleep(); //This sleep is necessary to make sure that the nodes are registered
393 |     parametersPublisher->publish(parametersMessage);
394 | 
395 |     int i = 0;
396 |     while (!(errorSubscriber->index) && i < 10) {
397 |         std::cout << "Loop" << std::endl;
398 |         rclcpp::spin_some(parametersPublisherNode);
399 |         rclcpp::spin_some(responderPublisher);
400 |         rclcpp::spin_some(errorSubscriber);
401 | 
402 |         loop_rate.sleep();
403 |         ++i;
404 | 
405 |         if (i > 4) {
406 |             std::cout << "Sending again..." << std::endl;
407 |             parametersPublisher->publish(parametersMessage);
408 |         }
409 |     }
410 | 
411 |     auto requestMessage = fillRequestMessage();
412 |     requestPublisher->publish(requestMessage);
413 |     std::cout << "Request sent..." << std::endl;
414 |     i = 0;
415 |     respondSubscriber->messageReceived = false;
416 | 
417 |     while (!((respondSubscriber->messageReceived) &&
418 |              (comSubscriber->messagesReceived == 5) &&
419 |              (pelvisSubscriber->messagesReceived == 5) &&
420 |              (footStepsSubscriber->messageReceived))
421 |            && (i < 100)) {
422 |         std::cout << "Loop " << i << std::endl;
423 |         rclcpp::spin_some(requestPublisherNode);
424 |         rclcpp::spin_some(responderPublisher);
425 |         rclcpp::spin_some(errorSubscriber);
426 |         rclcpp::spin_some(respondSubscriber);
427 |         rclcpp::spin_some(comSubscriber);
428 |         rclcpp::spin_some(pelvisSubscriber);
429 |         rclcpp::spin_some(footStepsSubscriber);
430 |         ++i;
431 |     }
432 |     ASSERT_IS_TRUE(i != 100);
433 | 
434 |     rclcpp::shutdown();
435 |     errorSubscriber = nullptr;
436 |     return EXIT_SUCCESS;
437 | }
438 | 


--------------------------------------------------------------------------------