├── .github └── workflows │ └── cmake.yml ├── .gitignore ├── CMakeLists.txt ├── Config.cmake.in ├── colcon.pkg ├── demos ├── demo_dg_solo12_step_adjustment_walk.ipynb ├── demo_dgh_real_solo12_step_adjustment.py ├── demo_dgh_sim_solo12_step_adjustment.py ├── demo_reactive_planners_bolt_step_adjustment.py ├── demo_reactive_planners_dcm_reactive_stepper.py ├── demo_reactive_planners_end_effector_trajectory3d.py ├── demo_reactive_planners_re_split_dcm.py ├── demo_reactive_planners_solo12_step_adjustment_walk.ipynb ├── demo_reactive_planners_split_dcm_uneven_terrain.py ├── demo_reactive_planners_step_adaptation_timing_planner.py ├── demo_reactive_planners_step_adjustment.py ├── demo_reactive_planners_stepper_head.py └── demo_reactive_planners_walking.py ├── doc ├── bipedal_walking_control_usingv_ariable_horizon_MPC.pdf └── walking_control_based_on_step_timing_adaptation.pdf ├── include └── reactive_planners │ ├── dcm_reactive_stepper.hpp │ ├── dcm_vrp_planner.hpp │ ├── dynamic_graph │ ├── dcm_reactive_stepper.hpp │ ├── quadruped_dcm_reactive_stepper.hpp │ └── stepper_head.hpp │ ├── dynamically_consistent_end_effector_trajectory.hpp │ ├── mathematics │ ├── polynome.hpp │ └── polynome.hxx │ ├── polynomial_end_effector_trajectory.hpp │ ├── quadruped_dcm_reactive_stepper.hpp │ └── stepper_head.hpp ├── license.txt ├── python └── reactive_planners │ ├── __init__.py │ ├── centroidal_controller │ ├── __init__.py │ └── lipm_centroidal_controller.py │ ├── dcm_reactive_stepper.py │ ├── dcm_vrp_planner │ ├── __init__.py │ ├── planner.py │ ├── re_split_dcm_planner.py │ ├── solo_step_planner.py │ └── uneven_terrain_planner.py │ ├── demos │ └── bolt_step_adjustment.py │ ├── dynamic_graph │ └── quadruped_stepper.py │ ├── lipm_simulator.py │ └── utils │ ├── __init__.py │ ├── qp_solver.py │ ├── solo_state_estimator.py │ ├── terrains │ └── stairs.urdf │ ├── trajectory_generator.py │ └── utils.py ├── readme.md ├── src ├── dcm_reactive_stepper.cpp ├── dcm_vrp_planner.cpp ├── dynamic_graph │ ├── dcm_reactive_stepper.cpp │ ├── quadruped_dcm_reactive_stepper.cpp │ └── stepper_head.cpp ├── dynamically_consistent_end_effector_trajectory.cpp ├── mathematics │ └── polynome.cpp ├── polynomial_end_effector_trajectory.cpp ├── quadruped_dcm_reactive_stepper.cpp └── stepper_head.cpp ├── srcpy ├── dcm_reactive_stepper.cpp ├── dcm_vrp_planner.cpp ├── polynomial_end_effector_trajectory.cpp ├── quadruped_dcm_reactive_stepper.cpp ├── reactive_planners.cpp ├── stepper_head.cpp └── walking_dg_python_module.cpp └── tests ├── dcm_vrp_planner_ut.cpp └── main.cpp /.github/workflows/cmake.yml: -------------------------------------------------------------------------------- 1 | name: CMake 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | env: 10 | BUILD_TYPE: Release 11 | BUILD_FOLDER: ${{github.workspace}}/../build 12 | DEVEL_FOLDER: ${{github.workspace}}/../devel 13 | 14 | jobs: 15 | build-linux: 16 | # The CMake configure and build commands are platform agnostic and should work equally 17 | # well on Windows or Mac. You can convert this to a matrix build if you need 18 | # cross-platform coverage. 19 | # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix 20 | runs-on: ubuntu-18.04 21 | strategy: 22 | matrix: 23 | build_type: [Release, Debug] 24 | steps: 25 | # 26 | # Setup the machines and build environment 27 | # 28 | - name: Setup Ubuntu18.04. 29 | uses: machines-in-motion/mim_github_actions/setup_ubuntu18_04@main 30 | 31 | # 32 | # Setup the machines and build environment 33 | # 34 | - name: Setup laas packages. 35 | uses: machines-in-motion/mim_github_actions/setup_laas_packages@main 36 | 37 | # 38 | # Checkout the current package locally 39 | # 40 | - name: Checkout current repo. 41 | uses: actions/checkout@v2 42 | 43 | # 44 | # Activate the secret ssh key 45 | # 46 | - name: Spawn ssh-agent 47 | uses: webfactory/ssh-agent@v0.5.2 48 | with: 49 | ssh-private-key: ${{ secrets.GA_SSH_PRIVATE_KEY }} 50 | 51 | # 52 | # Clone the dependencies that needs to be built. 53 | # 54 | - name: Clone dependencies and build them. 55 | id: clone_and_build_dep 56 | uses: machines-in-motion/mim_github_actions/treep_clone_and_build@main 57 | with: 58 | treep_configurations: | 59 | git@github.com:machines-in-motion/treep_machines_in_motion.git 60 | projects_or_repos: | 61 | REACTIVE_PLANNERS_DEPENDENCIES 62 | 63 | # 64 | # Build and test the current package. 65 | # 66 | - name: Build and run CTest. 67 | shell: bash 68 | run: | 69 | source /opt/openrobots/setup.bash 70 | mkdir -p $BUILD_FOLDER 71 | cd $BUILD_FOLDER 72 | echo Source environment. 73 | source ${{ steps.clone_and_build_dep.outputs.setup_file }} 74 | cmake ${{github.workspace}} -DCMAKE_BUILD_TYPE=${{matrix.build_type}} -DGENERATE_DOCUMENTATION=ON 75 | cmake --build . --config ${{matrix.build_type}} 76 | env CTEST_OUTPUT_ON_FAILURE=1 ctest -C $BUILD_TYPE 77 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.txt~ 2 | 3 | # No big files in the repo please. 4 | *.mp4 5 | *.mov 6 | *.dat 7 | 8 | ### 9 | # Based on: 10 | ## Core latex/pdflatex auxiliary files: https://github.com/github/gitignore/blob/master/TeX.gitignore 11 | 12 | *.aux 13 | *.lof 14 | *.log 15 | *.lot 16 | *.fls 17 | *.out 18 | *.toc 19 | *.fmt 20 | *.fot 21 | *.cb 22 | *.cb2 23 | 24 | ## Intermediate documents: 25 | *.dvi 26 | *-converted-to.* 27 | # these rules might exclude image files for figures etc. 28 | # *.ps 29 | # *.eps 30 | # *.pdf 31 | 32 | ## Generated if empty string is given at "Please type another file name for output:" 33 | .pdf 34 | 35 | ## Bibliography auxiliary files (bibtex/biblatex/biber): 36 | *.bbl 37 | *.bcf 38 | *.blg 39 | *-blx.aux 40 | *-blx.bib 41 | *.run.xml 42 | 43 | ## Build tool auxiliary files: 44 | *.fdb_latexmk 45 | *.synctex 46 | *.synctex(busy) 47 | *.synctex.gz 48 | *.synctex.gz(busy) 49 | *.pdfsync 50 | 51 | ## Auxiliary and intermediate files from other packages: 52 | # algorithms 53 | *.alg 54 | *.loa 55 | 56 | # achemso 57 | acs-*.bib 58 | 59 | # amsthm 60 | *.thm 61 | 62 | # beamer 63 | *.nav 64 | *.pre 65 | *.snm 66 | *.vrb 67 | 68 | # changes 69 | *.soc 70 | 71 | # cprotect 72 | *.cpt 73 | 74 | # elsarticle (documentclass of Elsevier journals) 75 | *.spl 76 | 77 | # endnotes 78 | *.ent 79 | 80 | # fixme 81 | *.lox 82 | 83 | # feynmf/feynmp 84 | *.mf 85 | *.mp 86 | *.t[1-9] 87 | *.t[1-9][0-9] 88 | *.tfm 89 | 90 | #(r)(e)ledmac/(r)(e)ledpar 91 | *.end 92 | *.?end 93 | *.[1-9] 94 | *.[1-9][0-9] 95 | *.[1-9][0-9][0-9] 96 | *.[1-9]R 97 | *.[1-9][0-9]R 98 | *.[1-9][0-9][0-9]R 99 | *.eledsec[1-9] 100 | *.eledsec[1-9]R 101 | *.eledsec[1-9][0-9] 102 | *.eledsec[1-9][0-9]R 103 | *.eledsec[1-9][0-9][0-9] 104 | *.eledsec[1-9][0-9][0-9]R 105 | 106 | # glossaries 107 | *.acn 108 | *.acr 109 | *.glg 110 | *.glo 111 | *.gls 112 | *.glsdefs 113 | 114 | # gnuplottex 115 | *-gnuplottex-* 116 | 117 | # gregoriotex 118 | *.gaux 119 | *.gtex 120 | 121 | # hyperref 122 | *.brf 123 | 124 | # knitr 125 | *-concordance.tex 126 | # TODO Comment the next line if you want to keep your tikz graphics files 127 | *.tikz 128 | *-tikzDictionary 129 | 130 | # listings 131 | *.lol 132 | 133 | # makeidx 134 | *.idx 135 | *.ilg 136 | *.ind 137 | *.ist 138 | 139 | # minitoc 140 | *.maf 141 | *.mlf 142 | *.mlt 143 | *.mtc[0-9]* 144 | *.slf[0-9]* 145 | *.slt[0-9]* 146 | *.stc[0-9]* 147 | 148 | # minted 149 | _minted* 150 | *.pyg 151 | 152 | # morewrites 153 | *.mw 154 | 155 | # nomencl 156 | *.nlo 157 | 158 | # pax 159 | *.pax 160 | 161 | # pdfpcnotes 162 | *.pdfpc 163 | 164 | # sagetex 165 | *.sagetex.sage 166 | *.sagetex.py 167 | *.sagetex.scmd 168 | 169 | # scrwfile 170 | *.wrt 171 | 172 | # sympy 173 | *.sout 174 | *.sympy 175 | sympy-plots-for-*.tex/ 176 | 177 | # pdfcomment 178 | *.upa 179 | *.upb 180 | 181 | # pythontex 182 | *.pytxcode 183 | pythontex-files-*/ 184 | 185 | # thmtools 186 | *.loe 187 | 188 | # TikZ & PGF 189 | *.dpth 190 | *.md5 191 | *.auxlock 192 | 193 | # todonotes 194 | *.tdo 195 | 196 | # easy-todo 197 | *.lod 198 | 199 | # xindy 200 | *.xdy 201 | 202 | # xypic precompiled matrices 203 | *.xyc 204 | 205 | # endfloat 206 | *.ttt 207 | *.fff 208 | 209 | # Latexian 210 | TSWLatexianTemp* 211 | 212 | ## Editors: 213 | # WinEdt 214 | *.bak 215 | *.sav 216 | 217 | # Texpad 218 | .texpadtmp 219 | 220 | # Kile 221 | *.backup 222 | 223 | # KBibTeX 224 | *~[0-9]* 225 | 226 | # auto folder when using emacs and auctex 227 | /auto/* 228 | 229 | # expex forward references with \gathertags 230 | *-tags.tex 231 | 232 | 233 | ### 234 | # Based on: https://github.com/github/gitignore/blob/master/Global/macOS.gitignore 235 | 236 | *.DS_Store 237 | .AppleDouble 238 | .LSOverride 239 | 240 | # Icon must end with two \r 241 | Icon 242 | 243 | 244 | # Thumbnails 245 | ._* 246 | 247 | # Files that might appear in the root of a volume 248 | .DocumentRevisions-V100 249 | .fseventsd 250 | .Spotlight-V100 251 | .TemporaryItems 252 | .Trashes 253 | .VolumeIcon.icns 254 | .com.apple.timemachine.donotpresent 255 | 256 | # Directories potentially created on remote AFP share 257 | .AppleDB 258 | .AppleDesktop 259 | Network Trash Folder 260 | Temporary Items 261 | .apdisk 262 | 263 | 264 | ### 265 | # Based on: https://github.com/github/gitignore/blob/master/Python.gitignore 266 | 267 | # Byte-compiled / optimized / DLL files 268 | __pycache__/ 269 | *.py[cod] 270 | *$py.class 271 | 272 | # C extensions 273 | *.so 274 | 275 | # Distribution / packaging 276 | .Python 277 | env/ 278 | *build*/ 279 | develop-eggs/ 280 | dist/ 281 | downloads/ 282 | eggs/ 283 | .eggs/ 284 | lib/ 285 | lib64/ 286 | parts/ 287 | sdist/ 288 | var/ 289 | *.egg-info/ 290 | .installed.cfg 291 | *.egg 292 | 293 | # PyInstaller 294 | # Usually these files are written by a python script from a template 295 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 296 | *.manifest 297 | *.spec 298 | 299 | # Installer logs 300 | pip-log.txt 301 | pip-delete-this-directory.txt 302 | 303 | # Unit test / coverage reports 304 | htmlcov/ 305 | .tox/ 306 | .coverage 307 | .coverage.* 308 | .cache 309 | nosetests.xml 310 | coverage.xml 311 | *,cover 312 | .hypothesis/ 313 | 314 | # Translations 315 | *.mo 316 | *.pot 317 | 318 | # Django stuff: 319 | *.log 320 | local_settings.py 321 | 322 | # Flask stuff: 323 | instance/ 324 | .webassets-cache 325 | 326 | # Scrapy stuff: 327 | .scrapy 328 | 329 | # Sphinx documentation 330 | docs/_build/ 331 | 332 | # PyBuilder 333 | target/ 334 | 335 | # IPython Notebook 336 | .ipynb_checkpoints 337 | 338 | # pyenv 339 | .python-version 340 | 341 | # celery beat schedule file 342 | celerybeat-schedule 343 | 344 | # dotenv 345 | .env 346 | 347 | # virtualenv 348 | venv/ 349 | ENV/ 350 | 351 | # Spyder project settings 352 | .spyderproject 353 | 354 | # Rope project settings 355 | .ropeproject 356 | 357 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019, New York University and Max Planck Gesellschaft. 3 | # 4 | # License BSD-3 clause 5 | # 6 | 7 | # 8 | # set up the project 9 | # 10 | cmake_minimum_required(VERSION 3.10.2) 11 | 12 | project(reactive_planners) 13 | 14 | # specify the C++ 17 standard 15 | set(CMAKE_CXX_STANDARD 17) 16 | set(CMAKE_CXX_STANDARD_REQUIRED True) 17 | 18 | # 19 | # Dependencies. 20 | # 21 | 22 | # Build. 23 | find_package(mpi_cmake_modules REQUIRED) 24 | 25 | # Internal. 26 | find_package(yaml_utils REQUIRED) 27 | 28 | # External. 29 | find_package(eigen-quadprog REQUIRED) 30 | find_package(pinocchio REQUIRED) 31 | find_package(pybind11 REQUIRED) 32 | find_package(Eigen3 REQUIRED) 33 | find_package(Python ${PYTHON_VERSION_STRING} REQUIRED) 34 | # Specific search of boost python as it is yet not trivial. 35 | if(NOT ${Python_FOUND}) 36 | message(FATAL_ERROR "PYTHON not FOUND by FindPython") 37 | endif() 38 | search_for_boost_python() 39 | 40 | # Optionnal 41 | find_package(dynamic-graph QUIET) 42 | find_package(dynamic-graph-python QUIET) 43 | find_package(real_time_tools QUIET) 44 | find_package(eigenpy QUIET) 45 | 46 | set(build_dynamic_graph_plugins FALSE) 47 | if(${dynamic-graph_FOUND} 48 | AND ${dynamic-graph-python_FOUND} 49 | AND ${real_time_tools_FOUND} 50 | AND ${eigenpy_FOUND}) 51 | set(build_dynamic_graph_plugins TRUE) 52 | endif() 53 | 54 | # 55 | # Main library 56 | # 57 | 58 | # Source files. 59 | set(main_lib_src 60 | src/stepper_head.cpp 61 | src/dcm_vrp_planner.cpp 62 | src/polynomial_end_effector_trajectory.cpp 63 | src/dynamically_consistent_end_effector_trajectory.cpp 64 | src/dcm_reactive_stepper.cpp 65 | src/quadruped_dcm_reactive_stepper.cpp) 66 | # Create library target. 67 | add_library(${PROJECT_NAME} SHARED ${main_lib_src}) 68 | # Includes. 69 | target_include_directories( 70 | ${PROJECT_NAME} PUBLIC $ 71 | $) 72 | # Dependencies. 73 | target_link_libraries(${PROJECT_NAME} yaml_utils::yaml_utils) 74 | target_link_libraries(${PROJECT_NAME} pinocchio::pinocchio) 75 | target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) 76 | target_link_libraries(${PROJECT_NAME} eigen-quadprog::eigen-quadprog) 77 | # Export. 78 | list(APPEND all_targets ${PROJECT_NAME}) 79 | 80 | # 81 | # Stores the path to the config folder in the CONFIG_PATH variable. 82 | # 83 | get_filename_component(CONFIG_PATH config ABSOLUTE) 84 | 85 | # 86 | # Demo executables. 87 | # 88 | 89 | install_scripts( 90 | demos/demo_reactive_planners_bolt_step_adjustment.py 91 | demos/demo_reactive_planners_dcm_reactive_stepper.py 92 | demos/demo_reactive_planners_end_effector_trajectory3d.py 93 | demos/demo_reactive_planners_stepper_head.py 94 | demos/demo_reactive_planners_walking.py 95 | demos/demo_reactive_planners_re_split_dcm.py 96 | demos/demo_reactive_planners_split_dcm_uneven_terrain.py 97 | demos/demo_reactive_planners_step_adaptation_timing_planner.py 98 | demos/demo_reactive_planners_step_adjustment.py 99 | DESTINATION 100 | bin) 101 | 102 | # 103 | # Python bindings with pybind11. 104 | # 105 | # cmake-format: off 106 | set(py_lib_src 107 | srcpy/reactive_planners 108 | srcpy/stepper_head.cpp 109 | srcpy/dcm_vrp_planner.cpp 110 | srcpy/polynomial_end_effector_trajectory.cpp 111 | srcpy/dcm_reactive_stepper.cpp 112 | srcpy/quadruped_dcm_reactive_stepper.cpp) 113 | # cmake-format: on 114 | pybind11_add_module(${PROJECT_NAME}_python_binding MODULE ${py_lib_src}) 115 | target_include_directories( 116 | ${PROJECT_NAME}_python_binding 117 | PUBLIC $ 118 | $) 119 | target_link_libraries(${PROJECT_NAME}_python_binding PRIVATE pybind11::module) 120 | target_link_libraries(${PROJECT_NAME}_python_binding PRIVATE ${PROJECT_NAME}) 121 | target_link_boost_python(${PROJECT_NAME}_python_binding PRIVATE) 122 | get_python_install_dir(python_install_dir) 123 | install(TARGETS ${PROJECT_NAME}_python_binding 124 | DESTINATION ${python_install_dir}) 125 | set_target_properties(${PROJECT_NAME}_python_binding 126 | PROPERTIES OUTPUT_NAME ${PROJECT_NAME}_cpp) 127 | 128 | # 129 | # Entities 130 | # 131 | if(${build_dynamic_graph_plugins}) 132 | # plugin name 133 | set(plugin_name walking) 134 | # Create the library 135 | add_library( 136 | ${plugin_name} SHARED 137 | src/dynamic_graph/stepper_head.cpp 138 | src/dynamic_graph/dcm_reactive_stepper.cpp 139 | src/dynamic_graph/quadruped_dcm_reactive_stepper.cpp) 140 | # Add the include dependencies. 141 | target_include_directories( 142 | ${plugin_name} PUBLIC $ 143 | $) 144 | # Link the dependencies. 145 | target_link_libraries(${plugin_name} ${PROJECT_NAME}) 146 | target_link_libraries(${plugin_name} dynamic-graph::dynamic-graph) 147 | target_link_libraries(${plugin_name} eigenpy::eigenpy) 148 | target_link_libraries(${plugin_name} 149 | dynamic-graph-python::dynamic-graph-python) 150 | target_link_libraries(${plugin_name} real_time_tools::real_time_tools) 151 | # Install the target and it's python bindings. 152 | install_dynamic_graph_plugin_python_bindings(${plugin_name}) 153 | # Install the plugin. 154 | get_dynamic_graph_plugin_install_path(plugin_install_path) 155 | install( 156 | TARGETS ${plugin_name} 157 | EXPORT ${PROJECT_NAME}Targets 158 | LIBRARY DESTINATION ${plugin_install_path} 159 | ARCHIVE DESTINATION ${plugin_install_path} 160 | RUNTIME DESTINATION ${plugin_install_path} 161 | INCLUDES 162 | DESTINATION include) 163 | 164 | endif() 165 | 166 | # 167 | # Unit tests 168 | # 169 | include(CTest) 170 | if(BUILD_TESTING) 171 | find_package(GTest CONFIG REQUIRED) 172 | include(GoogleTest) 173 | 174 | macro(ADD_UNIT_TEST src_file) 175 | set(test_name ${PROJECT_NAME}_${src_file}) 176 | add_executable(${test_name} tests/main.cpp tests/${src_file}.cpp) 177 | target_link_libraries(${test_name} ${PROJECT_NAME}) 178 | target_link_libraries(${test_name} GTest::gtest) 179 | set_target_properties( 180 | ${test_name} PROPERTIES COMPILE_DEFINITIONS 181 | CONFIG_FOLDER_PATH="${CONFIG_PATH}") 182 | gtest_add_tests(TARGET ${test_name}) 183 | endmacro() 184 | 185 | # add_unit_test(dcm_vrp_planner_ut) 186 | endif() 187 | 188 | # 189 | # Install all and export 190 | # 191 | 192 | # Python files. 193 | get_python_install_dir(python_install_dir) 194 | install( 195 | DIRECTORY python/ 196 | DESTINATION "${python_install_dir}" 197 | PATTERN "*.pyc" EXCLUDE 198 | PATTERN "__pycache__" EXCLUDE) 199 | 200 | # Command to install the library and binaries. 201 | install( 202 | TARGETS ${all_targets} 203 | EXPORT ${PROJECT_NAME}Targets 204 | LIBRARY DESTINATION lib 205 | ARCHIVE DESTINATION lib 206 | RUNTIME DESTINATION bin 207 | INCLUDES 208 | DESTINATION include) 209 | # Put the cmake files at the right place. 210 | generate_cmake_package(SKIP_TARGET_EXPORT) 211 | 212 | # 213 | # building documentation 214 | # 215 | add_documentation() 216 | -------------------------------------------------------------------------------- /Config.cmake.in: -------------------------------------------------------------------------------- 1 | 2 | @PACKAGE_INIT@ 3 | 4 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") 5 | 6 | include(CMakeFindDependencyMacro) 7 | 8 | # we do not add the other dependencies because these are header files lib 9 | find_dependency(yaml-cpp CONFIG REQUIRED) 10 | find_dependency(Eigen3 REQUIRED) 11 | find_dependency(pinocchio REQUIRED) 12 | find_dependency(eigen-quadprog REQUIRED) 13 | 14 | check_required_components(@PROJECT_NAME@) 15 | -------------------------------------------------------------------------------- /colcon.pkg: -------------------------------------------------------------------------------- 1 | { 2 | "test-dependencies": ["googletest-distribution"] 3 | } -------------------------------------------------------------------------------- /demos/demo_dg_solo12_step_adjustment_walk.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "\"\"\"This file is a demo for using the DG whole body controller.\n", 10 | "\n", 11 | "License BSD-3-Clause\n", 12 | "Copyright (c) 2021, New York University and Max Planck Gesellschaft.\n", 13 | "\n", 14 | "Author: Julian Viereck\n", 15 | "Date: Feb 24, 2021\n", 16 | "\"\"\"\n", 17 | "\n", 18 | "import numpy as np\n", 19 | "np.set_printoptions(suppress=True, precision=3)\n", 20 | "\n", 21 | "import pinocchio as pin\n", 22 | "\n", 23 | "import dynamic_graph as dg\n", 24 | "\n", 25 | "from robot_properties_solo.config import Solo12Config\n", 26 | "from dg_blmc_robots.solo.solo12_bullet import get_solo12_robot\n", 27 | "\n", 28 | "from reactive_planners.dynamic_graph.solo12_stepper import Solo12WBCStepper\n", 29 | "\n", 30 | "from dg_tools.dynamic_graph.dg_tools_entities import CreateWorldFrame" 31 | ] 32 | }, 33 | { 34 | "cell_type": "code", 35 | "execution_count": 3, 36 | "metadata": {}, 37 | "outputs": [], 38 | "source": [ 39 | "###\n", 40 | "# Create the simulated robot\n", 41 | "robot = get_solo12_robot()" 42 | ] 43 | }, 44 | { 45 | "cell_type": "code", 46 | "execution_count": 4, 47 | "metadata": {}, 48 | "outputs": [], 49 | "source": [ 50 | "q0 = Solo12Config.q0.copy()\n", 51 | "q0[0] = 0.2\n", 52 | "\n", 53 | "# Rotate the base if desired.\n", 54 | "des_yaw = 0.\n", 55 | "\n", 56 | "q0[3:7] = pin.Quaternion(pin.rpy.rpyToMatrix(0., 0., des_yaw)).coeffs() # \n", 57 | "\n", 58 | "robot.reset_state(q0, Solo12Config.v0)" 59 | ] 60 | }, 61 | { 62 | "cell_type": "code", 63 | "execution_count": 5, 64 | "metadata": {}, 65 | "outputs": [], 66 | "source": [ 67 | "ctrl = Solo12WBCStepper('solo12_wbc_stepper', 0.2)" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": 6, 73 | "metadata": {}, 74 | "outputs": [], 75 | "source": [ 76 | "# Zero the initial position from the vicon signal.\n", 77 | "base_posture_sin, base_velocity_sin = robot.base_signals()\n", 78 | "\n", 79 | "op = CreateWorldFrame('wf')\n", 80 | "dg.plug(base_posture_sin, op.frame_sin)\n", 81 | "op.update()\n", 82 | "op.set_which_dofs(np.array([1., 1., 0., 0., 0., 0.]))\n", 83 | "\n", 84 | "base_posture_local_sin = stack_two_vectors(\n", 85 | " selec_vector(subtract_vec_vec(base_posture_sin, op.world_frame_sout), 0, 3),\n", 86 | " selec_vector(base_posture_sin, 3, 7), 3, 4)" 87 | ] 88 | }, 89 | { 90 | "cell_type": "code", 91 | "execution_count": 7, 92 | "metadata": {}, 93 | "outputs": [], 94 | "source": [ 95 | "ctrl.plug(robot, base_posture_local_sin, base_velocity_sin)" 96 | ] 97 | }, 98 | { 99 | "cell_type": "code", 100 | "execution_count": 8, 101 | "metadata": {}, 102 | "outputs": [], 103 | "source": [ 104 | "# Set desired base rotation.\n", 105 | "ctrl.des_ori_pos_rpy_sin.value = np.array([0., 0., des_yaw])\n", 106 | "ctrl.des_com_vel_sin.value = np.array([0.0, 0.0, 0.])" 107 | ] 108 | }, 109 | { 110 | "cell_type": "code", 111 | "execution_count": 9, 112 | "metadata": {}, 113 | "outputs": [], 114 | "source": [ 115 | "# Simulate for 1 seconds.\n", 116 | "robot.run(1000, sleep=True)" 117 | ] 118 | }, 119 | { 120 | "cell_type": "code", 121 | "execution_count": 10, 122 | "metadata": {}, 123 | "outputs": [], 124 | "source": [ 125 | "# Start the stepper.\n", 126 | "ctrl.start()" 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": 12, 132 | "metadata": {}, 133 | "outputs": [], 134 | "source": [ 135 | "# Simulate for 4000 seconds.\n", 136 | "robot.run(4000, sleep=True)" 137 | ] 138 | }, 139 | { 140 | "cell_type": "code", 141 | "execution_count": null, 142 | "metadata": {}, 143 | "outputs": [], 144 | "source": [] 145 | }, 146 | { 147 | "cell_type": "code", 148 | "execution_count": null, 149 | "metadata": {}, 150 | "outputs": [], 151 | "source": [] 152 | } 153 | ], 154 | "metadata": { 155 | "kernelspec": { 156 | "display_name": "Python 3", 157 | "language": "python", 158 | "name": "python3" 159 | }, 160 | "language_info": { 161 | "codemirror_mode": { 162 | "name": "ipython", 163 | "version": 3 164 | }, 165 | "file_extension": ".py", 166 | "mimetype": "text/x-python", 167 | "name": "python", 168 | "nbconvert_exporter": "python", 169 | "pygments_lexer": "ipython3", 170 | "version": "3.6.9" 171 | } 172 | }, 173 | "nbformat": 4, 174 | "nbformat_minor": 4 175 | } 176 | -------------------------------------------------------------------------------- /demos/demo_reactive_planners_bolt_step_adjustment.py: -------------------------------------------------------------------------------- 1 | """ @namespace Demos of Bolt step adjustment 2 | @file 3 | @copyright Copyright (c) 2017-2019, 4 | New York University and Max Planck Gesellschaft, 5 | License BSD-3-Clause 6 | @example 7 | """ 8 | import numpy as np 9 | import pybullet as p 10 | from robot_properties_bolt.config import BoltConfig 11 | from robot_properties_bolt.bolt_wrapper import BoltRobot 12 | from mim_control.robot_centroidal_controller import RobotCentroidalController 13 | from mim_control.robot_impedance_controller import RobotImpedanceController 14 | from reactive_planners.lipm_simulator import LipmSimpulator 15 | from reactive_planners_cpp import DcmReactiveStepper 16 | import pinocchio as se3 17 | from scipy.spatial.transform import Rotation as R 18 | from bullet_utils.env import BulletEnvWithGround 19 | 20 | 21 | def zero_cnt_gain(kp, cnt_array): 22 | gain = np.array(kp).copy() 23 | for i, v in enumerate(cnt_array): 24 | if v == 1: 25 | gain[3 * i : 3 * (i + 1)] = 0.0 26 | return gain 27 | 28 | 29 | def yaw(q): 30 | return np.array( 31 | R.from_quat([np.array(q)[3:7]]).as_euler("xyz", degrees=False) 32 | )[0, 2] 33 | 34 | 35 | if __name__ == "__main__": 36 | # Create a robot instance. This initializes the simulator as well. 37 | env = BulletEnvWithGround() 38 | robot = env.add_robot(BoltRobot()) 39 | tau = np.zeros(6) 40 | 41 | time = 0 42 | sim_freq = 10000 # Hz 43 | ctrl_freq = 1000 44 | plan_freq = 1000 45 | 46 | p.resetDebugVisualizerCamera(1.6, 50, -35, (0.0, 0.0, 0.0)) 47 | p.setTimeStep(1.0 / sim_freq) 48 | p.setRealTimeSimulation(0) 49 | for ji in range(8): 50 | p.changeDynamics( 51 | robot.robotId, 52 | ji, 53 | linearDamping=0.04, 54 | angularDamping=0.04, 55 | restitution=0.0, 56 | lateralFriction=4.0, 57 | spinningFriction=5.6, 58 | ) 59 | 60 | q = np.matrix(BoltConfig.initial_configuration).T 61 | qdot = np.matrix(BoltConfig.initial_velocity).T 62 | robot.reset_state(q, qdot) 63 | total_mass = sum([i.mass for i in robot.pin_robot.model.inertias[1:]]) 64 | warmup = 5 65 | kp = np.array([150.0, 150.0, 150.0, 150.0, 150.0, 150.0]) 66 | kd = [5.0, 5.0, 5.0, 5.0, 5.0, 5.0] 67 | robot_config = BoltConfig() 68 | config_file = robot_config.ctrl_path 69 | bolt_leg_ctrl = RobotImpedanceController(robot, config_file) 70 | centr_controller = RobotCentroidalController( 71 | robot_config, 72 | mu=1, 73 | kc=[0, 0, 100], 74 | dc=[0, 0, 10], 75 | kb=[100, 100, 100], 76 | db=[10.0, 10, 10], 77 | qp_penalty_lin=[1, 1, 1e6], 78 | qp_penalty_ang=[1e6, 1e6, 1], 79 | ) 80 | is_left_leg_in_contact = True 81 | l_min = -0.1 82 | l_max = 0.1 83 | w_min = -0.08 84 | w_max = 0.2 85 | t_min = 0.1 86 | t_max = 0.8 87 | l_p = 0.1035 # Pelvis width 88 | com_height = 0.36487417 89 | weight = [1, 1, 5, 1000, 1000, 100000, 100000, 100000, 100000] 90 | mid_air_foot_height = 0.05 91 | control_period = 0.001 92 | planner_loop = 0.010 93 | x_des_local = [ 94 | q[0].item(), 95 | q[1].item() + 0.02, 96 | 0.0, 97 | q[0].item(), 98 | q[1].item() - 0.02, 99 | 0.0, 100 | ] 101 | past_x = x_des_local.copy() 102 | v_des = [0.0, 0.0, 0.0] 103 | sim = LipmSimpulator(com_height) 104 | dcm_reactive_stepper = DcmReactiveStepper() 105 | dcm_reactive_stepper.initialize( 106 | is_left_leg_in_contact, 107 | l_min, 108 | l_max, 109 | w_min, 110 | w_max, 111 | t_min, 112 | t_max, 113 | l_p, 114 | com_height, 115 | weight, 116 | mid_air_foot_height, 117 | control_period, 118 | planner_loop, 119 | x_des_local[:3], 120 | x_des_local[3:], 121 | ) 122 | 123 | dcm_reactive_stepper.set_desired_com_velocity(v_des) 124 | 125 | x_com = [[0.0], [0.0], [com_height]] 126 | cnt_array = [1, 1] 127 | time = 0 128 | control_time = 0 129 | open_loop = True 130 | dcm_force = [0.0, 0.0, 0.0] 131 | offset = 0.0171 # foot radius 132 | dcm_reactive_stepper.start() 133 | 134 | for i in range(5005): 135 | last_qdot = qdot 136 | q, qdot = robot.get_state() 137 | robot.pin_robot.com(q, qdot) 138 | x_com = robot.pin_robot.com(q, qdot)[0] 139 | xd_com = robot.pin_robot.com(q, qdot)[1] 140 | 141 | if warmup <= i: 142 | left = bolt_leg_ctrl.imp_ctrl_array[0] 143 | right = bolt_leg_ctrl.imp_ctrl_array[1] 144 | left_foot_location = np.array( 145 | left.pin_robot.data.oMf[left.frame_end_idx].translation 146 | ).reshape(-1) 147 | right_foot_location = np.array( 148 | right.pin_robot.data.oMf[right.frame_end_idx].translation 149 | ).reshape(-1) 150 | left_foot_vel = np.array( 151 | se3.SE3( 152 | left.pin_robot.data.oMf[left.frame_end_idx].rotation, 153 | np.zeros((3, 1)), 154 | ) 155 | * se3.computeFrameJacobian( 156 | robot.pin_robot.model, 157 | robot.pin_robot.data, 158 | q, 159 | left.frame_end_idx, 160 | ).dot(qdot)[0:3] 161 | ) 162 | right_foot_vel = np.array( 163 | se3.SE3( 164 | right.pin_robot.data.oMf[right.frame_end_idx].rotation, 165 | np.zeros((3, 1)), 166 | ) 167 | * se3.computeFrameJacobian( 168 | robot.pin_robot.model, 169 | robot.pin_robot.data, 170 | q, 171 | right.frame_end_idx, 172 | ).dot(qdot)[0:3] 173 | ) 174 | if dcm_reactive_stepper.get_is_left_leg_in_contact(): 175 | pos_for_plotter = ( 176 | dcm_reactive_stepper.get_right_foot_position().copy() 177 | ) 178 | vel_for_plotter = ( 179 | dcm_reactive_stepper.get_right_foot_velocity().copy() 180 | ) 181 | else: 182 | pos_for_plotter = ( 183 | dcm_reactive_stepper.get_left_foot_position().copy() 184 | ) 185 | vel_for_plotter = ( 186 | dcm_reactive_stepper.get_left_foot_velocity().copy() 187 | ) 188 | 189 | dcm_reactive_stepper.run( 190 | time, 191 | [ 192 | left_foot_location[0], 193 | left_foot_location[1], 194 | left_foot_location[2] - offset, 195 | ], 196 | [ 197 | right_foot_location[0], 198 | right_foot_location[1], 199 | right_foot_location[2] - offset, 200 | ], 201 | left_foot_vel, 202 | right_foot_vel, 203 | x_com, 204 | xd_com, 205 | yaw(q), 206 | not open_loop, 207 | ) 208 | dcm_force = dcm_reactive_stepper.get_forces().copy() 209 | 210 | x_des_local = [] 211 | x_des_local.extend( 212 | dcm_reactive_stepper.get_left_foot_position().copy() 213 | ) 214 | x_des_local.extend( 215 | dcm_reactive_stepper.get_right_foot_position().copy() 216 | ) 217 | 218 | x_des_local[2] += offset 219 | x_des_local[5] += offset 220 | 221 | if dcm_reactive_stepper.get_is_left_leg_in_contact(): 222 | cnt_array = [1, 0] 223 | else: 224 | cnt_array = [0, 1] 225 | time += 0.001 226 | 227 | for j in range(2): 228 | imp = bolt_leg_ctrl.imp_ctrl_array[j] 229 | x_des_local[3 * j : 3 * (j + 1)] -= imp.pin_robot.data.oMf[ 230 | imp.frame_root_idx 231 | ].translation 232 | w_com = centr_controller.compute_com_wrench( 233 | q.copy(), 234 | qdot.copy(), 235 | [0.0, 0.0, com_height], 236 | [0.0, 0.0, 0.0], 237 | [0, 0.0, 0, 1.0], 238 | [0.0, 0.0, 0.0], 239 | ) 240 | w_com[0] = 0.0 241 | w_com[1] = 0.0 242 | 243 | F = centr_controller.compute_force_qp(q, qdot, cnt_array, w_com) 244 | 245 | des_vel = np.concatenate( 246 | ( 247 | dcm_reactive_stepper.get_left_foot_velocity() 248 | - [qdot[0].item(), qdot[1].item(), qdot[2].item()], 249 | dcm_reactive_stepper.get_right_foot_velocity() 250 | - [qdot[0].item(), qdot[1].item(), qdot[2].item()], 251 | ) 252 | ) 253 | 254 | if cnt_array[0] == 1 and cnt_array[1] == 0: 255 | F[3:] = -dcm_force[:3] 256 | elif cnt_array[0] == 0 and cnt_array[1] == 1: 257 | F[:3] = -dcm_force[:3] 258 | tau = bolt_leg_ctrl.return_joint_torques( 259 | q.copy(), 260 | qdot.copy(), 261 | zero_cnt_gain(kp, cnt_array), 262 | zero_cnt_gain(kd, cnt_array), 263 | x_des_local, 264 | des_vel, 265 | F, 266 | ) 267 | control_time += 0.001 268 | 269 | for j in range(10): 270 | robot.send_joint_command(tau) 271 | p.stepSimulation() 272 | 273 | dcm_reactive_stepper.stop() 274 | -------------------------------------------------------------------------------- /demos/demo_reactive_planners_dcm_reactive_stepper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ @namespace Demos of the reactive_planners.dcm_reactive_planner. 4 | @file 5 | @copyright Copyright (c) 2017-2019, 6 | New York University and Max Planck Gesellschaft, 7 | License BSD-3-Clause 8 | @example 9 | """ 10 | 11 | from matplotlib import pyplot as plt 12 | from reactive_planners_cpp import DcmReactiveStepper 13 | from reactive_planners.lipm_simulator import LipmSimpulator 14 | import numpy as np 15 | 16 | if __name__ == "__main__": 17 | # Create the simulator. 18 | sim = LipmSimpulator(0.2) 19 | 20 | # Create the controller. 21 | dcm_reactive_stepper = DcmReactiveStepper() 22 | 23 | # Default parameters. 24 | v_des = np.zeros((3, 1)) 25 | x_com = np.array([[0], [0], [0.2]]) 26 | xd_com = np.zeros((3, 1)) 27 | left_foot_position = np.zeros((3, 1)) 28 | right_foot_position = np.zeros((3, 1)) 29 | time = 0 30 | 31 | # controller initialization. 32 | is_left_leg_in_contact = True 33 | l_min = -0.5 34 | l_max = 0.5 35 | w_min = -0.5 36 | w_max = 0.5 37 | t_min = 0.1 38 | t_max = 0.2 39 | l_p = 0.0 # 0.1235 * 2 40 | com_height = 0.26487417 41 | weight = [1, 1, 5, 100, 100, 100, 100, 100, 100] 42 | mid_air_foot_height = 0.05 43 | control_period = 0.001 44 | loop_period = 0.001 45 | dcm_reactive_stepper.initialize( 46 | is_left_leg_in_contact, 47 | l_min, 48 | l_max, 49 | w_min, 50 | w_max, 51 | t_min, 52 | t_max, 53 | l_p, 54 | com_height, 55 | weight, 56 | mid_air_foot_height, 57 | loop_period, 58 | control_period, 59 | left_foot_position, 60 | right_foot_position, 61 | ) 62 | 63 | # Plot lists. 64 | plt_time = [] 65 | plt_x_com = [] 66 | plt_xd_com = [] 67 | plt_right_foot_position = [] 68 | plt_right_foot_velocity = [] 69 | plt_right_foot_acceleration = [] 70 | plt_left_foot_position = [] 71 | plt_left_foot_velocity = [] 72 | plt_left_foot_acceleration = [] 73 | plt_time_from_last_step_touchdown = [] 74 | plt_step_duration = [] 75 | plt_current_support_foot = [] 76 | 77 | # Control loop. 78 | dt = 0.001 79 | N = 10000 80 | dcm_reactive_stepper.set_desired_com_velocity([0, 0, 0]) 81 | for i in range(N): 82 | # update the time 83 | time += dt 84 | 85 | if time > 0.2 * N * dt: 86 | dcm_reactive_stepper.start() 87 | 88 | if time > 0.4 * N * dt: 89 | dcm_reactive_stepper.stop() 90 | 91 | if time > 0.6 * N * dt: 92 | dcm_reactive_stepper.start() 93 | 94 | if time > 0.8 * N * dt: 95 | dcm_reactive_stepper.stop() 96 | 97 | # Compute the next foot location 98 | dcm_reactive_stepper.run( 99 | time, 100 | dcm_reactive_stepper.get_left_foot_position(), 101 | dcm_reactive_stepper.get_right_foot_position(), 102 | dcm_reactive_stepper.get_left_foot_velocity(), 103 | dcm_reactive_stepper.get_right_foot_velocity(), 104 | x_com, 105 | xd_com, 106 | 0.0, 107 | False, 108 | ) 109 | # simulate a linearized inverted pendulum 110 | x_com, xd_com, _ = sim.step( 111 | dcm_reactive_stepper.get_time_from_last_step_touchdown(), 112 | dcm_reactive_stepper.get_current_support_foot_position().reshape( 113 | (3, 1) 114 | ), 115 | x_com, 116 | xd_com, 117 | ) 118 | 119 | # plot collection 120 | plt_time.append(time) 121 | plt_x_com.append(x_com.copy()) 122 | plt_xd_com.append(xd_com.copy()) 123 | plt_right_foot_position.append( 124 | dcm_reactive_stepper.get_right_foot_position() 125 | ) 126 | plt_right_foot_velocity.append( 127 | dcm_reactive_stepper.get_right_foot_velocity() 128 | ) 129 | plt_right_foot_acceleration.append( 130 | dcm_reactive_stepper.get_right_foot_acceleration() 131 | ) 132 | plt_left_foot_position.append( 133 | dcm_reactive_stepper.get_left_foot_position() 134 | ) 135 | plt_left_foot_velocity.append( 136 | dcm_reactive_stepper.get_left_foot_velocity() 137 | ) 138 | plt_left_foot_acceleration.append( 139 | dcm_reactive_stepper.get_left_foot_acceleration() 140 | ) 141 | plt_time_from_last_step_touchdown.append( 142 | dcm_reactive_stepper.get_time_from_last_step_touchdown() 143 | ) 144 | plt_step_duration.append(dcm_reactive_stepper.get_step_duration()) 145 | plt_current_support_foot.append( 146 | dcm_reactive_stepper.get_current_support_foot_position() 147 | ) 148 | 149 | # Plots. 150 | plt.figure("com") 151 | plt.plot(plt_time, np.array(plt_x_com)[:, 0], label="com x") 152 | plt.plot(plt_time, np.array(plt_x_com)[:, 1], label="com y") 153 | plt.plot(plt_time, np.array(plt_x_com)[:, 2], label="com z") 154 | plt.legend() 155 | 156 | plt.figure("xd") 157 | plt.plot(plt_time, np.array(plt_xd_com)[:, 0], label="dcom x") 158 | plt.plot(plt_time, np.array(plt_xd_com)[:, 1], label="dcom y") 159 | plt.plot(plt_time, np.array(plt_xd_com)[:, 2], label="dcom z") 160 | plt.legend() 161 | 162 | plt.figure("right_foot_pos") 163 | plt.plot(plt_time, np.array(plt_right_foot_position)[:, 0], label="rf x") 164 | plt.plot(plt_time, np.array(plt_right_foot_position)[:, 1], label="rf y") 165 | plt.plot(plt_time, np.array(plt_right_foot_position)[:, 2], label="rf z") 166 | plt.legend() 167 | 168 | plt.figure("left_foot_pos") 169 | plt.plot(plt_time, np.array(plt_left_foot_position)[:, 0], label="lf x") 170 | plt.plot(plt_time, np.array(plt_left_foot_position)[:, 1], label="lf y") 171 | plt.plot(plt_time, np.array(plt_left_foot_position)[:, 2], label="lf z") 172 | plt.legend() 173 | 174 | plt.figure("foot_pos_z") 175 | plt.plot(plt_time, np.array(plt_right_foot_position)[:, 2], label="rf z") 176 | plt.plot(plt_time, np.array(plt_left_foot_position)[:, 2], label="lf z") 177 | plt.legend() 178 | 179 | plt.figure("last_step_touchdown") 180 | plt.plot( 181 | plt_time, 182 | np.array(plt_time_from_last_step_touchdown), 183 | label="time_from_last_step_touchdown", 184 | ) 185 | plt.plot(plt_time, np.array(plt_step_duration), label="step_duration") 186 | plt.legend() 187 | 188 | plt.figure("support_foot") 189 | plt.plot( 190 | plt_time, 191 | np.array(plt_current_support_foot)[:, 0], 192 | label="current support x", 193 | ) 194 | plt.plot( 195 | plt_time, 196 | np.array(plt_current_support_foot)[:, 1], 197 | label="current support y", 198 | ) 199 | plt.plot( 200 | plt_time, 201 | np.array(plt_current_support_foot)[:, 2], 202 | label="current support z", 203 | ) 204 | plt.legend() 205 | 206 | plt.show() 207 | -------------------------------------------------------------------------------- /demos/demo_reactive_planners_end_effector_trajectory3d.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ @namespace Demos of the reactive_planners.end_effector_trajectory. 4 | @file 5 | @copyright Copyright (c) 2017-2019, 6 | New York University and Max Planck Gesellschaft, 7 | License BSD-3-Clause 8 | @example 9 | """ 10 | 11 | import numpy as np 12 | from matplotlib import pyplot as plt 13 | from reactive_planners_cpp import EndEffectorTrajectory3D 14 | 15 | if __name__ == "__main__": 16 | 17 | end_eff_traj3d = EndEffectorTrajectory3D() 18 | 19 | end_eff_traj3d.set_mid_air_height(0.05) 20 | 21 | previous_support_location = np.zeros((3, 1)) 22 | foot_position = np.zeros((3, 1)) 23 | foot_velocity = np.zeros((3, 1)) 24 | foot_acceleration = np.zeros((3, 1)) 25 | next_support_location = np.zeros((3, 1)) 26 | 27 | start_time = 0.0 28 | end_time = 1.0 29 | control_period = 0.001 30 | 31 | plt_foot_position = [] 32 | 33 | for i in range(int(1 / control_period)): 34 | current_time = i * control_period 35 | 36 | end_eff_traj3d.compute( 37 | previous_support_location, 38 | foot_position, 39 | foot_velocity, 40 | foot_acceleration, 41 | next_support_location, 42 | start_time, 43 | current_time, 44 | end_time, 45 | ) 46 | 47 | end_eff_traj3d.get_next_state( 48 | current_time + control_period, 49 | foot_position, 50 | foot_velocity, 51 | foot_acceleration, 52 | ) 53 | 54 | plt_foot_position += [foot_position.copy()] 55 | 56 | plt.plot(np.array(plt_foot_position)[:, 2]) 57 | plt.show() 58 | -------------------------------------------------------------------------------- /demos/demo_reactive_planners_split_dcm_uneven_terrain.py: -------------------------------------------------------------------------------- 1 | ### Author : Avadesh Meduri 2 | ### Date : 11/11/2019 3 | ### Implementation of the Split DCM planner for uneven terrain using MIP 4 | 5 | import numpy as np 6 | from matplotlib import pyplot as plt 7 | 8 | import time 9 | 10 | import os 11 | import os.path 12 | import rospkg 13 | import pybullet as p 14 | import pinocchio as se3 15 | from pinocchio.utils import se3ToXYZQUAT 16 | 17 | from robot_properties_solo.config import Solo12Config 18 | from robot_properties_solo.quadruped12wrapper import Quadruped12Robot 19 | 20 | from mim_control.solo_impedance_controller import ( 21 | SoloImpedanceController, 22 | ) 23 | from mim_control.solo_centroidal_controller import ( 24 | SoloCentroidalController, 25 | ) 26 | 27 | 28 | from pinocchio.utils import zero 29 | from matplotlib import pyplot as plt 30 | 31 | from py_dcm_vrp_planner.uneven_terrain_planner import SplitDcmContactPlanner 32 | from py_utils.utils import create_terrain_constraints 33 | 34 | import py_utils.utils 35 | 36 | # Create a robot instance. This initializes the simulator as well. 37 | robot = Quadruped12Robot() 38 | tau = np.zeros(12) 39 | 40 | 41 | # Reset the robot to some initial state. 42 | initial_configuration = [0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 1.0] + 4 * [ 43 | 0.0, 44 | 0.9, 45 | -1.8, 46 | ] 47 | 48 | q0 = np.matrix(initial_configuration).T 49 | dq0 = np.matrix(Solo12Config.initial_velocity).T 50 | robot.reset_state(q0, dq0) 51 | 52 | arr = lambda a: np.array(a).reshape(-1) 53 | mat = lambda a: np.matrix(a).reshape((-1, 1)) 54 | total_mass = sum([i.mass for i in robot.pin_robot.model.inertias[1:]]) 55 | ##################################################### 56 | 57 | uneven_terrain = os.path.join( 58 | os.path.dirname(py_utils.utils.__file__), "terrains/stairs.urdf" 59 | ) 60 | uneven_terrain_id = p.loadURDF(uneven_terrain) 61 | b = create_terrain_constraints(uneven_terrain) 62 | 63 | 64 | ####################################################### 65 | 66 | x_des = 4 * [0.0, 0.0, -0.2] 67 | xd_des = 4 * [0, 0, 0] 68 | kp = 4 * [400, 400, 300] 69 | kd = 4 * [10.0, 10.0, 10.0] 70 | f = 4 * [0.0, 0.0, (2.2 * 9.8) / 4] 71 | 72 | 73 | ########################################################### 74 | 75 | ht = 0.2 76 | ht_foot = 0.15 77 | l_min = -0.15 78 | l_max = 0.15 79 | 80 | w_min = -0.05 81 | w_max = 0.15 82 | h_min = -0.1 83 | h_max = 0.1 84 | t_min = 0.002 85 | t_max = 0.3 86 | v_des = [2.5, 0.0, 0.0] 87 | l_p = 0 88 | dcm_contact_planner = SplitDcmContactPlanner( 89 | l_min, l_max, w_min, w_max, h_min, h_max, t_min, t_max, v_des, l_p, ht, b 90 | ) 91 | 92 | x_com_cent = [0.0, 0.0, ht + 0.1] 93 | xd_com_cent = [0.0, 0.0, 0.0] 94 | 95 | x_ori = [0.0, 0.0, 0.0, 1.0] 96 | x_angvel = [0.0, 0.0, 0.0] 97 | cnt_array = [1, 1, 1, 1] 98 | 99 | solo_leg_ctrl = SoloImpedanceController(robot) 100 | centr_controller = SoloCentroidalController( 101 | robot.pin_robot, 102 | total_mass, 103 | mu=0.6, 104 | kc=[0, 0, 100], 105 | dc=[0, 0, 30], 106 | kb=[200, 200, 200], 107 | db=[20.0, 20.0, 20.0], 108 | eff_ids=robot.pinocchio_endeff_ids, 109 | ) 110 | 111 | 112 | # weight on [step length_x , step_length_y, step_length_z, step time, dcm_offeset_x, dcm_offeset_y, dcm_offeset_z] 113 | W = 2 * [100, 100, 100, 10, 1000, 1000, 1000] 114 | ######################################################################## 115 | 116 | x_com = np.array([0.0, 0.0, 0.0]) 117 | xd_com = np.array([0.0, 0.0, 0.0]) 118 | 119 | ## Bring the vales from the config file and remove hardcoding when refactoring 120 | FL_HFE_idx = robot.pin_robot.model.getFrameId("FL_HFE") 121 | FR_HFE_idx = robot.pin_robot.model.getFrameId("FR_HFE") 122 | HL_HFE_idx = robot.pin_robot.model.getFrameId("HL_HFE") 123 | HR_HFE_idx = robot.pin_robot.model.getFrameId("HR_HFE") 124 | 125 | 126 | q, dq = robot.get_state() 127 | robot.pin_robot.framesForwardKinematics(q) 128 | FL_HFE = np.reshape( 129 | np.array(robot.pin_robot.data.oMf[FL_HFE_idx].translation), (3,) 130 | ) 131 | FR_HFE = np.reshape( 132 | np.array(robot.pin_robot.data.oMf[FR_HFE_idx].translation), (3,) 133 | ) 134 | HL_HFE = np.reshape( 135 | np.array(robot.pin_robot.data.oMf[HL_HFE_idx].translation), (3,) 136 | ) 137 | HR_HFE = np.reshape( 138 | np.array(robot.pin_robot.data.oMf[HR_HFE_idx].translation), (3,) 139 | ) 140 | FL_FOOT_idx = robot.pin_robot.model.getFrameId("FL_FOOT") 141 | FR_FOOT_idx = robot.pin_robot.model.getFrameId("FR_FOOT") 142 | HL_FOOT_idx = robot.pin_robot.model.getFrameId("HL_FOOT") 143 | HR_FOOT_idx = robot.pin_robot.model.getFrameId("HR_FOOT") 144 | 145 | solo_leg_ctrl = SoloImpedanceController(robot) 146 | 147 | t1 = 0 148 | t2 = 0 149 | t_gap = 0.05 150 | t1_end = t_max 151 | t2_end = t_max 152 | n1 = 1 153 | n2 = 2 154 | vrp1_current = 0.5 * np.add(FL_HFE, FR_HFE) 155 | vrp2_current = 0.5 * np.add(HL_HFE, HR_HFE) 156 | u1_current_step = np.subtract(vrp1_current, [0, 0, ht]) 157 | u2_current_step = np.subtract(vrp2_current, [0, 0, ht]) 158 | u1_old = u1_current_step 159 | u2_old = u2_current_step 160 | 161 | 162 | ############################### Data plotting 163 | 164 | plt_ut = [] 165 | plt_opt = [] 166 | plt_foot = [] 167 | plt_com = [] 168 | plt_force = [] 169 | 170 | ### test 171 | 172 | for i in range(6000): 173 | p.stepSimulation() 174 | time.sleep(0.0001) 175 | 176 | # if i > 1000 and i < 2200: 177 | # force = np.array([0,8,0]) 178 | # p.applyExternalForce(objectUniqueId=robot.robotId, linkIndex=-1, forceObj=force, \ 179 | # posObj=[0.25,0.,0], flags = p.WORLD_FRAME) 180 | 181 | q, dq = robot.get_state() 182 | x_com[0] = float(q[0]) 183 | x_com[1] = float(q[1]) 184 | x_com[2] = float(q[2]) 185 | xd_com[0] = float(dq[0]) 186 | xd_com[1] = float(dq[1]) 187 | xd_com[2] = float(dq[2]) 188 | 189 | robot.pin_robot.framesForwardKinematics(q) 190 | FL_HFE = np.reshape( 191 | np.array(robot.pin_robot.data.oMf[FL_HFE_idx].translation), (3,) 192 | ) 193 | FR_HFE = np.reshape( 194 | np.array(robot.pin_robot.data.oMf[FR_HFE_idx].translation), (3,) 195 | ) 196 | HL_HFE = np.reshape( 197 | np.array(robot.pin_robot.data.oMf[HL_HFE_idx].translation), (3,) 198 | ) 199 | HR_HFE = np.reshape( 200 | np.array(robot.pin_robot.data.oMf[HR_HFE_idx].translation), (3,) 201 | ) 202 | FL_FOOT = np.reshape( 203 | np.array(robot.pin_robot.data.oMf[FL_FOOT_idx].translation), (3,) 204 | ) 205 | FR_FOOT = np.reshape( 206 | np.array(robot.pin_robot.data.oMf[FR_FOOT_idx].translation), (3,) 207 | ) 208 | HL_FOOT = np.reshape( 209 | np.array(robot.pin_robot.data.oMf[HL_FOOT_idx].translation), (3,) 210 | ) 211 | HR_FOOT = np.reshape( 212 | np.array(robot.pin_robot.data.oMf[HR_FOOT_idx].translation), (3,) 213 | ) 214 | 215 | plt_foot.append(FL_HFE) 216 | 217 | if t1 > t1_end: 218 | t1 = 0 219 | n1 += 1 220 | t1_end = t_max 221 | u1_old = np.subtract(vrp1_current, [0, 0, ht]) 222 | vrp1_current = x_opt[0:3] 223 | u1_current_step = np.subtract(vrp1_current, [0, 0, ht]) 224 | if t2 > t2_end: 225 | t2 = 0 226 | n2 += 1 227 | t2_end = t_max 228 | u2_old = np.subtract(vrp2_current, [0, 0, ht]) 229 | vrp2_current = x_opt[4:7] 230 | u2_current_step = np.subtract(vrp2_current, [0, 0, ht]) 231 | 232 | vrp1_current_eff = np.add(vrp1_current, np.subtract(vrp2_current, x_com)) 233 | vrp2_current_eff = np.add(vrp2_current, np.subtract(vrp1_current, x_com)) 234 | offset_1 = np.subtract(vrp2_current, x_com) 235 | offset_2 = np.subtract(vrp1_current, x_com) 236 | 237 | ### This if statement prevents adaptation near the end of the step to prevents jumps in desrired location. 238 | dcm_t = dcm_contact_planner.compute_dcm_current(x_com, xd_com) 239 | alpha = dcm_contact_planner.compute_alpha(xd_com, v_des) 240 | x_opt = dcm_contact_planner.compute_adapted_step_locations( 241 | vrp1_current_eff, 242 | vrp2_current_eff, 243 | t1, 244 | t2, 245 | n1, 246 | n2, 247 | dcm_t, 248 | alpha, 249 | W, 250 | offset_1, 251 | offset_2, 252 | ) 253 | t1_end = x_opt[3] 254 | t2_end = x_opt[7] 255 | ### bringing the effective next step to the inertial frame 256 | x_opt = np.array(x_opt) 257 | x_opt[0:3] = np.add(x_opt[0:3], np.subtract(x_com, vrp2_current)) 258 | x_opt[4:7] = np.add(x_opt[4:7], np.subtract(x_com, vrp1_current)) 259 | 260 | u1_t_end = np.subtract(x_opt[0:3], [0, 0, ht]) 261 | u2_t_end = np.subtract(x_opt[4:7], [0, 0, ht]) 262 | 263 | if np.power(-1, n1) > 0: 264 | x_des_fl, x_des_fr = dcm_contact_planner.generate_foot_trajectory( 265 | u1_t_end, 266 | u1_current_step, 267 | u1_old, 268 | t1_end, 269 | t1, 270 | ht_foot, 271 | -ht, 272 | FR_HFE, 273 | FL_HFE, 274 | n1, 275 | ) 276 | cnt_array[0:2] = [0, 1] 277 | elif np.power(-1, n1) < 0: 278 | x_des_fr, x_des_fl = dcm_contact_planner.generate_foot_trajectory( 279 | u1_t_end, 280 | u1_current_step, 281 | u1_old, 282 | t1_end, 283 | t1, 284 | ht_foot, 285 | -ht, 286 | FL_HFE, 287 | FR_HFE, 288 | n1, 289 | ) 290 | cnt_array[0:2] = [1, 0] 291 | if np.power(-1, n2) > 0: 292 | x_des_hl, x_des_hr = dcm_contact_planner.generate_foot_trajectory( 293 | u2_t_end, 294 | u2_current_step, 295 | u2_old, 296 | t2_end, 297 | t2, 298 | ht_foot, 299 | -ht, 300 | HR_HFE, 301 | HL_HFE, 302 | n2, 303 | ) 304 | cnt_array[2:4] = [0, 1] 305 | elif np.power(-1, n2) < 0: 306 | x_des_hr, x_des_hl = dcm_contact_planner.generate_foot_trajectory( 307 | u2_t_end, 308 | u2_current_step, 309 | u2_old, 310 | t2_end, 311 | t2, 312 | ht_foot, 313 | -ht, 314 | HL_HFE, 315 | HR_HFE, 316 | n2, 317 | ) 318 | cnt_array[2:4] = [1, 0] 319 | 320 | x_des[0:3] = np.reshape(x_des_fl, (3,)) 321 | x_des[3:6] = np.reshape(x_des_fr, (3,)) 322 | x_des[6:9] = np.reshape(x_des_hl, (3,)) 323 | x_des[9:12] = np.reshape(x_des_hr, (3,)) 324 | 325 | # plt_foot.append([x for x in x_des]) 326 | plt_ut.append([x for x in u1_t_end]) 327 | 328 | t1 += 0.001 329 | t2 += 0.001 330 | w_com = centr_controller.compute_com_wrench( 331 | i, q, dq, x_com_cent, xd_com_cent, x_ori, x_angvel 332 | ) 333 | w_com[2] += total_mass * 9.81 334 | F = centr_controller.compute_force_qp(i, q, dq, cnt_array, w_com) 335 | plt_force.append([F[0], F[1], F[2], F[3], F[4], F[5]]) 336 | tau = solo_leg_ctrl.return_joint_torques(q, dq, kp, kd, x_des, xd_des, F) 337 | robot.send_joint_command(tau) 338 | 339 | # #### plotting 340 | 341 | plt_foot = np.array(plt_foot) 342 | plt_ut = np.array(plt_ut) 343 | plt_com = np.array(plt_com) 344 | 345 | fig, (ax1, ax2, ax3) = plt.subplots(3, 1, sharex=True) 346 | 347 | ax1.plot(plt_ut[:, 0], label="fl_x") 348 | ax1.plot(plt_foot[:, 0], label="fl_z") 349 | ax1.grid() 350 | ax1.legend() 351 | ax2.plot(plt_foot[:, 2], label="fl_z") 352 | # ax2.plot(plt_foot[:,5], label = 'fr_z') 353 | ax2.grid() 354 | ax2.legend() 355 | # ax3.plot(plt_foot[:,6], label = 'hl_x') 356 | # ax3.plot(plt_foot[:,8], label = 'hl_x') 357 | ax3.grid() 358 | ax3.legend() 359 | 360 | print("----------------------------------------------------- \n") 361 | for i in range(int(len(b) / 6)): 362 | print(b[6 * i + 0]) 363 | print("\n") 364 | print(b[6 * i + 1]) 365 | 366 | plt.show() 367 | -------------------------------------------------------------------------------- /demos/demo_reactive_planners_step_adaptation_timing_planner.py: -------------------------------------------------------------------------------- 1 | ### Author : Avadesh Meduri 2 | ### Date : 27/09/2019 3 | ### This is the demo of the paper "walking control based on step timing Adaption" by Majid Et al. 4 | ### for the quadruped robot solo 5 | 6 | import numpy as np 7 | 8 | import time 9 | 10 | import os 11 | import rospkg 12 | import pybullet as p 13 | import pinocchio as se3 14 | from pinocchio.utils import se3ToXYZQUAT 15 | 16 | from robot_properties_solo.config import Solo12Config 17 | from robot_properties_solo.quadruped12wrapper import Quadruped12Robot 18 | 19 | from mim_control.solo_centroidal_controller import ( 20 | SoloCentroidalController, 21 | ) 22 | from mim_control.solo_impedance_controller import ( 23 | SoloImpedanceController, 24 | ) 25 | 26 | from pinocchio.utils import zero 27 | from matplotlib import pyplot as plt 28 | 29 | from reactive_planners.dcm_vrp_planner.planner import DcmVrpPlanner 30 | 31 | 32 | ####################################################################################### 33 | 34 | 35 | # Create a robot instance. This initializes the simulator as well. 36 | robot = Quadruped12Robot() 37 | tau = np.zeros(12) 38 | 39 | # Reset the robot to some initial state. 40 | q0 = np.matrix(Solo12Config.initial_configuration).T 41 | dq0 = np.matrix(Solo12Config.initial_velocity).T 42 | robot.reset_state(q0, dq0) 43 | 44 | arr = lambda a: np.array(a).reshape(-1) 45 | mat = lambda a: np.matrix(a).reshape((-1, 1)) 46 | total_mass = sum([i.mass for i in robot.pin_robot.model.inertias[1:]]) 47 | 48 | 49 | ####################################################### 50 | 51 | x_des = 4 * [0.0, 0.0, -0.25] 52 | xd_des = 4 * [0, 0, 0] 53 | kp = 4 * [400, 400, 400] 54 | kd = 4 * [20.0, 20.0, 20.0] 55 | ################################################################################## 56 | 57 | x_com_cent = [0.0, 0.0, 0.25] 58 | xd_com_cent = [0.0, 0.0, 0.0] 59 | 60 | x_ori = [0.0, 0.0, 0.0, 1.0] 61 | x_angvel = [0.0, 0.0, 0.0] 62 | cnt_array = [1, 1, 1, 1] 63 | 64 | solo_leg_ctrl = SoloImpedanceController(robot) 65 | centr_controller = SoloCentroidalController( 66 | robot.pin_robot, 67 | total_mass, 68 | mu=0.6, 69 | kc=[0, 0, 0], 70 | dc=[0, 0, 10], 71 | kb=[100, 100, 100], 72 | db=[10.0, 10.0, 10.0], 73 | eff_ids=robot.pinocchio_endeff_ids, 74 | ) 75 | 76 | 77 | ################################################################################# 78 | 79 | l_min = -0.2 80 | l_max = 0.2 81 | w_min = -0.1 82 | w_max = 0.1 83 | t_min = 0.1 84 | t_max = 0.3 85 | v_des = [0.1, 0.0] 86 | l_p = 0 87 | ht = 0.23 88 | 89 | dcm_vrp_planner = DcmVrpPlanner( 90 | l_min, l_max, w_min, w_max, t_min, t_max, v_des, l_p, ht 91 | ) 92 | 93 | W = [ 94 | 10, 95 | 10, 96 | 1.0, 97 | 1000, 98 | 1000, 99 | 100, 100 | ] # weight on [step length_x , step_length_y, step time, dcm_offeset_x, dcm_offeset_y] 101 | ####################################################################################### 102 | 103 | solo_leg_ctrl = SoloImpedanceController(robot) 104 | 105 | # Run the simulator for 100 steps 106 | t = 0 107 | t_gap = 0.05 108 | t_end = t_max 109 | n = 1 110 | u_current_step = [0.0, 0.0] 111 | u_old = [0.0, 0.0] 112 | x_com = np.array([0.0, 0.0]) 113 | xd_com = np.array([0.0, 0.0]) 114 | 115 | ## for plotting 116 | plt_opt = [] 117 | plt_foot = [] 118 | plt_com = [] 119 | plt_force = [] 120 | for i in range(5000): 121 | p.stepSimulation() 122 | time.sleep(0.0001) 123 | 124 | # if i > 1000 and i < 1200: 125 | # force = np.array([0,10,0]) 126 | # p.applyExternalForce(objectUniqueId=robot.robotId, linkIndex=-1, forceObj=force, \ 127 | # posObj=[0.5,0.,0], flags = p.WORLD_FRAME) 128 | 129 | q, dq = robot.get_state() 130 | x_com[0] = float(q[0]) 131 | x_com[1] = float(q[1]) 132 | xd_com[0] = float(dq[0]) 133 | xd_com[1] = float(dq[1]) 134 | plt_com.append((x_com[0], x_com[1])) 135 | 136 | if t < t_end: 137 | if t_end - t > t_gap: 138 | ### This if statement prevents adaptation near the end of the step to prevents jumps in desrired location. 139 | dcm_t = dcm_vrp_planner.compute_dcm_current(x_com, xd_com) 140 | alpha = dcm_vrp_planner.compute_alpha(xd_com, v_des) 141 | x_opt = dcm_vrp_planner.compute_adapted_step_locations( 142 | u_current_step, t, n, dcm_t, alpha, W 143 | ) 144 | # x_opt = dcm_vrp_planner.compute_adapted_step_locations_gurobi(u_current_step, t, n, dcm_t, alpha, W) 145 | t_end = x_opt[2] 146 | 147 | if np.power(-1, n) > 0: 148 | ( 149 | x_des_fl_hr, 150 | x_des_fr_hl, 151 | ) = dcm_vrp_planner.generate_foot_trajectory( 152 | x_opt[0:2], u_current_step, u_old, t_end, t, 0.2, -0.25 153 | ) 154 | x_des[0:3] = np.reshape(x_des_fl_hr, (3,)) 155 | x_des[3:6] = np.reshape(x_des_fr_hl, (3,)) 156 | x_des[6:9] = np.reshape(x_des_fr_hl, (3,)) 157 | x_des[9:12] = np.reshape(x_des_fl_hr, (3,)) 158 | cnt_array = [0, 1, 0, 1] 159 | else: 160 | ( 161 | x_des_fr_hl, 162 | x_des_fl_hr, 163 | ) = dcm_vrp_planner.generate_foot_trajectory( 164 | x_opt[0:2], u_current_step, u_old, t_end, t, 0.2, -0.25 165 | ) 166 | x_des[0:3] = np.reshape(x_des_fl_hr, (3,)) 167 | x_des[3:6] = np.reshape(x_des_fr_hl, (3,)) 168 | x_des[6:9] = np.reshape(x_des_fr_hl, (3,)) 169 | x_des[9:12] = np.reshape(x_des_fl_hr, (3,)) 170 | cnt_array = [1, 0, 1, 0] 171 | t += 0.001 172 | # print(x_des) 173 | 174 | else: 175 | t = 0 176 | # n_old = n 177 | n += 1 178 | t_end = t_max 179 | u_old = u_current_step 180 | u_current_step = [x_opt[0], x_opt[1]] 181 | 182 | plt_opt.append(x_opt) 183 | plt_foot.append( 184 | [x_des[0], x_des[1], x_des[2], x_des[3], x_des[4], x_des[5]] 185 | ) 186 | w_com = centr_controller.compute_com_wrench( 187 | t, q, dq, x_com_cent, xd_com_cent, x_ori, x_angvel 188 | ) 189 | w_com[2] += total_mass * 9.81 190 | F = centr_controller.compute_force_qp(i, q, dq, cnt_array, w_com) 191 | plt_force.append([F[0], F[1], F[2], F[3], F[4], F[5]]) 192 | tau = solo_leg_ctrl.return_joint_torques(q, dq, kp, kd, x_des, xd_des, F) 193 | robot.send_joint_command(tau) 194 | 195 | 196 | # #### plotting 197 | 198 | plt_opt = np.array(plt_opt) 199 | plt_foot = np.array(plt_foot) 200 | plt_com = np.array(plt_com) 201 | plt_force = np.array(plt_force) 202 | t = np.arange(0, 5, 0.001) 203 | 204 | fig, (ax1, ax2, ax3) = plt.subplots(3, 1, sharex=True) 205 | 206 | ax1.plot(t, plt_opt[:, 0], label="ut_x") 207 | ax1.plot(t, np.add(plt_com[:, 0], plt_foot[:, 0]), label="fl") 208 | ax1.plot(t, np.add(plt_com[:, 0], plt_foot[:, 3]), label="fr") 209 | ax1.grid() 210 | ax1.legend() 211 | ax2.plot(t, plt_foot[:, 2], label="fl") 212 | ax2.plot(t, plt_foot[:, 5], label="fr") 213 | ax2.grid() 214 | ax2.legend() 215 | ax3.plot(t, plt_foot[:, 0], label="fl") 216 | ax3.plot(t, plt_foot[:, 3], label="fr") 217 | ax3.grid() 218 | ax3.legend() 219 | 220 | plt.show() 221 | -------------------------------------------------------------------------------- /demos/demo_reactive_planners_step_adjustment.py: -------------------------------------------------------------------------------- 1 | ## @namespace reactive_planners.step_adjustment 2 | """ This module find the step adjustment for biped robots. 3 | @author Elham Daneshmand (exledh@gmail.com) 4 | @copyright Copyright (c) 2020, 5 | New York University and Max Planck Gesellschaft, 6 | License BSD-3-Clause 7 | """ 8 | 9 | import numpy as np 10 | from matplotlib import pyplot as plt 11 | from reactive_planners_cpp import DcmVrpPlanner 12 | 13 | 14 | class StepAdjustment: 15 | def __init__( 16 | self, 17 | is_left_leg_in_contact, 18 | l_min, 19 | l_max, 20 | w_min, 21 | w_max, 22 | t_min, 23 | t_max, 24 | l_p, 25 | com_height, 26 | weight, 27 | ): 28 | self.kp = np.array(6 * [200.0]) 29 | self.kd = 6 * [5.0] 30 | self.com_height = com_height 31 | self.dcm_vrp_planner = DcmVrpPlanner() 32 | self.dcm_vrp_planner.initialize( 33 | l_min, 34 | l_max, 35 | w_min, 36 | w_max, 37 | t_min, 38 | t_max, 39 | l_p, 40 | self.com_height, 41 | weight, 42 | ) 43 | self.is_left_leg_in_contact = is_left_leg_in_contact 44 | self.omega = np.sqrt(9.8 / self.com_height) 45 | 46 | # for plotting 47 | self.x_com_history = [] 48 | self.xd_com_history = [] 49 | self.t_nom = [] 50 | self.tau_nom = [] 51 | self.l_nom = [] 52 | self.w_nom = [] 53 | self.bx_nom = [] 54 | self.by_nom = [] 55 | self.dcm_local = [] 56 | self.current_step_location_local = [] 57 | self.dcm_nominal = [] 58 | self.next_step_location = [] 59 | self.current_step_location = [] 60 | self.duration_before_step_landing = [] 61 | self.xPhHistory = [] 62 | self.xdPhHistory = [] 63 | self.last_x_com_history = [] 64 | self.last_xd_com_history = [] 65 | self.time_step = [] 66 | 67 | def simulator(self, u_current_step, v_des, x_com, xd_com): 68 | t = 0 69 | last_x_com = x_com 70 | last_xd_com = xd_com 71 | 72 | for i in range(1000): 73 | print(i) 74 | t += 0.001 75 | if ( 76 | i is not 0 77 | and t > self.dcm_vrp_planner.get_duration_before_step_landing() 78 | ): 79 | self.time_step.append(i) 80 | last_x_com = x_com 81 | last_xd_com = xd_com 82 | u_current_step = ( 83 | self.dcm_vrp_planner.get_next_step_location().copy() 84 | ) 85 | t = 0.0 86 | self.is_left_leg_in_contact = not self.is_left_leg_in_contact 87 | 88 | # Analytical solution of x and xd (just for testing) 89 | xddPh = (self.omega ** 2) * (last_x_com - u_current_step) 90 | xPh = 0.5 * xddPh * (0.001 ** 2) + xd_com * 0.001 + x_com 91 | xdPh = xddPh * 0.001 + xd_com 92 | 93 | # Numerical solution of x and xd 94 | x_com = ( 95 | ( 96 | 0.5 97 | * ( 98 | last_x_com 99 | - u_current_step 100 | + (last_xd_com / self.omega) 101 | ) 102 | * pow(np.e, (self.omega * t)) 103 | ) 104 | + ( 105 | 0.5 106 | * ( 107 | last_x_com 108 | - u_current_step 109 | - (last_xd_com / self.omega) 110 | ) 111 | * pow(np.e, (-self.omega * t)) 112 | ) 113 | + u_current_step 114 | ) 115 | xd_com = ( 116 | self.omega 117 | * 0.5 118 | * (last_x_com - u_current_step + (last_xd_com / self.omega)) 119 | * pow(np.e, (self.omega * t)) 120 | ) - ( 121 | self.omega 122 | * 0.5 123 | * (last_x_com - u_current_step - (last_xd_com / self.omega)) 124 | * pow(np.e, (-self.omega * t)) 125 | ) 126 | 127 | self.update_planner( 128 | u_current_step, t * 1.0, v_des, x_com, xd_com, 0 129 | ) 130 | 131 | self.xPhHistory.append(xPh.copy()) 132 | self.xdPhHistory.append(xdPh.copy()) 133 | self.x_com_history.append(x_com.copy()) 134 | self.xd_com_history.append(xd_com.copy()) 135 | self.t_nom.append(self.dcm_vrp_planner.get_t_nom()) 136 | self.tau_nom.append(self.dcm_vrp_planner.get_tau_nom()) 137 | self.l_nom.append(self.dcm_vrp_planner.get_l_nom()) 138 | self.w_nom.append(self.dcm_vrp_planner.get_w_nom()) 139 | self.bx_nom.append(self.dcm_vrp_planner.get_bx_nom()) 140 | self.by_nom.append(self.dcm_vrp_planner.get_by_nom()) 141 | self.dcm_local.append(self.dcm_vrp_planner.get_dcm_local().copy()) 142 | self.current_step_location_local.append( 143 | self.dcm_vrp_planner.get_current_step_location_local().copy() 144 | ) 145 | self.dcm_nominal.append( 146 | self.dcm_vrp_planner.get_dcm_nominal().copy() 147 | ) 148 | self.next_step_location.append( 149 | self.dcm_vrp_planner.get_next_step_location().copy() 150 | ) 151 | self.current_step_location.append(u_current_step.copy()) 152 | self.duration_before_step_landing.append( 153 | self.dcm_vrp_planner.get_duration_before_step_landing() 154 | ) 155 | self.last_x_com_history.append(last_x_com) 156 | self.last_xd_com_history.append(last_xd_com) 157 | 158 | self.plot() 159 | 160 | def update_planner(self, u_current_step, t, v_des, x_com, xd_com, yaw): 161 | self.dcm_vrp_planner.update( 162 | u_current_step, 163 | t, 164 | self.is_left_leg_in_contact, 165 | v_des, 166 | x_com, 167 | xd_com, 168 | yaw, 169 | ) 170 | self.dcm_vrp_planner.solve() 171 | 172 | def plot(self): 173 | fig1, ax1 = plt.subplots(1, 1) 174 | ax1.plot(np.array(self.x_com_history)[:, 1], label="x_com_history") 175 | ax1.plot(self.by_nom, label="by_nom") 176 | ax1.plot(np.array(self.dcm_local)[:, 1], label="dcm_local") 177 | ax1.plot( 178 | np.array(self.current_step_location_local)[:, 1], 179 | label="current_step_location_local", 180 | ) 181 | ax1.plot( 182 | np.array(self.next_step_location)[:, 1], label="next_step_location" 183 | ) 184 | ax1.plot( 185 | np.array(self.current_step_location)[:, 1], 186 | label="current_step_location", 187 | ) 188 | ax1.plot( 189 | self.duration_before_step_landing, 190 | label="duration_before_step_landing", 191 | ) 192 | for t in self.time_step: 193 | ax1.axvline(t) 194 | ax1.grid() 195 | ax1.legend() 196 | 197 | fig1, ax1 = plt.subplots(1, 1) 198 | ax1.plot(np.array(self.x_com_history)[:, 1], label="x_com_history") 199 | ax1.plot(np.array(self.xd_com_history)[:, 1], label="xd_com_history") 200 | ax1.plot(np.array(self.xPhHistory)[:, 1], label="x") 201 | ax1.plot(np.array(self.xdPhHistory)[:, 1], label="xd") 202 | # ax1.plot(np.array(self.last_x_com_history)[:, 1], label = "last_X") 203 | # ax1.plot(np.array(self.last_xd_com_history)[:, 1], label = "last_xd") 204 | ax1.grid() 205 | ax1.legend() 206 | 207 | fig1, ax1 = plt.subplots(1, 1) 208 | ax1.plot(self.t_nom, label="t_nom") 209 | # ax1.plot(self.tau_nom, label = "tau_nom") 210 | ax1.plot(self.l_nom, label="l_nom") 211 | ax1.plot(self.w_nom, label="w_nom") 212 | ax1.plot(self.bx_nom, label="bx_nom") 213 | ax1.plot(self.by_nom, label="by_nom") 214 | ax1.grid() 215 | ax1.legend() 216 | plt.show() 217 | 218 | def zero_cnt_gain(self, kp, cnt_array): 219 | gain = np.array(kp).copy() 220 | for i, v in enumerate(cnt_array): 221 | if v == 1: 222 | gain[3 * i : 3 * (i + 1)] = 0.0 223 | return gain 224 | 225 | 226 | if __name__ == "__main__": 227 | planner = StepAdjustment( 228 | is_left_leg_in_contact=True, 229 | l_min=-0.5, 230 | l_max=0.5, 231 | w_min=-0.5, 232 | w_max=0.5, 233 | t_min=0.1, 234 | t_max=0.2, 235 | l_p=0.1235 * 2, 236 | com_height=0.26487417, 237 | weight=[1, 1, 5, 100, 100, 100, 100, 100, 100], 238 | ) 239 | planner.simulator( 240 | u_current_step=np.array([0.0, 0.1235, 0.0]), 241 | v_des=np.array([0.0, 0.0, 0.0]), 242 | x_com=np.array([0.0, 0.0, 0.2]), 243 | xd_com=np.array([0.0, 0.0, 0.0]), 244 | ) 245 | -------------------------------------------------------------------------------- /demos/demo_reactive_planners_stepper_head.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ @namespace Demos of the reactive_planners.stepper_head controller. 4 | @file 5 | @copyright Copyright (c) 2017-2019, 6 | New York University and Max Planck Gesellschaft, 7 | License BSD-3-Clause 8 | @example 9 | """ 10 | 11 | # Python 3 compatibility, has to be called just after the hashbang. 12 | from __future__ import print_function, division 13 | 14 | import numpy as np 15 | 16 | # Import the class to demo from. 17 | from reactive_planners_cpp import StepperHead 18 | 19 | 20 | def log_traj(data): 21 | import csv 22 | 23 | with open("/tmp/demo_stepper_head_py.dat", "w") as csvfile: 24 | spamwriter = csv.writer( 25 | csvfile, delimiter="\t", quotechar="|", quoting=csv.QUOTE_MINIMAL 26 | ) 27 | for row in data: 28 | spamwriter.writerow(row) 29 | 30 | 31 | if __name__ == "__main__": 32 | 33 | # 34 | # Parameters. 35 | # 36 | time = 0.0 37 | dt = 0.001 38 | duration_before_step_landing = 0.1 39 | next_support_foot = [0, 0, 0] 40 | 41 | # 42 | # Initialization 43 | # 44 | # Create the stepper head object 45 | stepper_head = StepperHead() 46 | # Collect the data. 47 | data = [] 48 | 49 | # 50 | # Compute the trajectory. 51 | # 52 | data += [ 53 | [ 54 | time, 55 | stepper_head.get_time_from_last_step_touchdown(), 56 | stepper_head.get_is_left_leg_in_contact(), 57 | ] 58 | + stepper_head.get_previous_support_location().tolist() 59 | + stepper_head.get_current_support_location().tolist() 60 | ] 61 | 62 | for i in range(10000): 63 | stepper_head.run(duration_before_step_landing, next_support_foot, time) 64 | 65 | next_support_foot = np.array([0.5 * time, time, 2 * time]) 66 | time += dt 67 | 68 | data += [ 69 | [ 70 | time, 71 | stepper_head.get_time_from_last_step_touchdown(), 72 | stepper_head.get_is_left_leg_in_contact(), 73 | ] 74 | + stepper_head.get_previous_support_location().tolist() 75 | + stepper_head.get_current_support_location().tolist() 76 | ] 77 | 78 | log_traj(data) 79 | -------------------------------------------------------------------------------- /demos/demo_reactive_planners_walking.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ @namespace Demos of the reactive_planners.dcm_reactive_planner. 4 | @file 5 | @copyright Copyright (c) 2017-2019, 6 | New York University and Max Planck Gesellschaft, 7 | License BSD-3-Clause 8 | @example 9 | """ 10 | 11 | from matplotlib import pyplot as plt 12 | from reactive_planners.dcm_reactive_stepper import DcmReactiveStepper 13 | from reactive_planners.lipm_simulator import LipmSimpulator 14 | import numpy as np 15 | 16 | if __name__ == "__main__": 17 | sim = LipmSimpulator(0.2) 18 | 19 | dcm_reactive_stepper = DcmReactiveStepper( 20 | is_left_leg_in_contact=True, 21 | l_min=-0.5, 22 | l_max=0.5, 23 | w_min=-0.5, 24 | w_max=0.5, 25 | t_min=0.1, 26 | t_max=0.2, 27 | l_p=0.1235 * 2, 28 | com_height=0.26487417, 29 | weight=[1, 1, 5, 1000, 1000, 100000, 100000, 100000, 100000], 30 | mid_air_foot_height=0.05, 31 | control_period=0.001, 32 | previous_support_foot=[[0.0], [-0.075], [0.0]], 33 | current_support_foot=[[0.0], [0.075], [0.0]], 34 | ) 35 | dcm_reactive_stepper.set_end_eff_traj_costs(1e1, 1e1, 1e0, 1e-6) 36 | v_des = np.zeros((3, 1)) 37 | v_des[:] = [[0.0], [0.0], [0.0]] 38 | dcm_reactive_stepper.set_des_com_vel(v_des) 39 | x_com = np.zeros((3, 1)) 40 | x_com[:] = [[0.0], [0.0], [0.26487417]] 41 | xd_com = np.zeros((3, 1)) 42 | time = 0 43 | plt_time = [] 44 | plt_x_com = [] 45 | plt_xd_com = [] 46 | plt_right_foot_position = [] 47 | plt_right_foot_velocity = [] 48 | plt_right_foot_acceleration = [] 49 | plt_left_foot_position = [] 50 | plt_left_foot_velocity = [] 51 | plt_left_foot_acceleration = [] 52 | plt_time_from_last_step_touchdown = [] 53 | plt_duration_before_step_landing = [] 54 | plt_current_support_foot = [] 55 | plt_support_foot = [] 56 | plt_step_time = [] 57 | plt_dcm_local = [] 58 | plt_is_left_leg_in_contact = [] 59 | t = 0 60 | omega = np.sqrt(9.8 / 0.2) 61 | for i in range(1000): 62 | time += 0.001 63 | if dcm_reactive_stepper.is_left_leg_in_contact: 64 | support_foot = dcm_reactive_stepper.left_foot_position.copy() 65 | swing_foot = dcm_reactive_stepper.right_foot_position.copy() 66 | else: 67 | support_foot = dcm_reactive_stepper.right_foot_position.copy() 68 | swing_foot = dcm_reactive_stepper.left_foot_position.copy() 69 | dcm_reactive_stepper.run( 70 | time, swing_foot, support_foot, x_com, xd_com, 0, [0, 0] 71 | ) 72 | if i == 760: 73 | xd_com[1] = xd_com[1] - 0.2 74 | t = dcm_reactive_stepper.time_from_last_step_touchdown 75 | if dcm_reactive_stepper.time_from_last_step_touchdown == 0: 76 | t = 0 77 | x_com, xd_com, _ = sim.step( 78 | dcm_reactive_stepper.time_from_last_step_touchdown - t, 79 | dcm_reactive_stepper.current_support_foot, 80 | x_com, 81 | xd_com, 82 | ) 83 | plt_time.append(time) 84 | plt_x_com.append(x_com.copy()) 85 | plt_xd_com.append(xd_com.copy()) 86 | plt_right_foot_position.append( 87 | dcm_reactive_stepper.right_foot_position.copy() 88 | ) 89 | plt_right_foot_velocity.append( 90 | dcm_reactive_stepper.right_foot_velocity.copy() 91 | ) 92 | plt_right_foot_acceleration.append( 93 | dcm_reactive_stepper.right_foot_acceleration.copy() 94 | ) 95 | plt_left_foot_position.append( 96 | dcm_reactive_stepper.left_foot_position.copy() 97 | ) 98 | plt_left_foot_velocity.append( 99 | dcm_reactive_stepper.left_foot_velocity.copy() 100 | ) 101 | plt_left_foot_acceleration.append( 102 | dcm_reactive_stepper.left_foot_acceleration.copy() 103 | ) 104 | plt_time_from_last_step_touchdown.append( 105 | dcm_reactive_stepper.time_from_last_step_touchdown 106 | ) 107 | plt_duration_before_step_landing.append( 108 | dcm_reactive_stepper.duration_before_step_landing 109 | ) 110 | plt_current_support_foot.append( 111 | dcm_reactive_stepper.current_support_foot.copy() 112 | ) 113 | plt_support_foot.append(support_foot) 114 | plt_dcm_local.append(x_com + xd_com / omega) 115 | plt_is_left_leg_in_contact.append( 116 | dcm_reactive_stepper.is_left_leg_in_contact 117 | ) 118 | if dcm_reactive_stepper.time_from_last_step_touchdown == 0: 119 | plt_step_time.append(i) 120 | 121 | plt.figure("y") 122 | plt.plot(plt_time, np.array(plt_right_foot_position)[:, 1], label="right") 123 | plt.plot(plt_time, np.array(plt_left_foot_position)[:, 1], label="left") 124 | plt.plot(plt_time, np.array(plt_x_com)[:, 1], label="com") 125 | plt.plot(plt_time, np.array(plt_xd_com)[:, 1], label="xd_com") 126 | plt.plot(plt_time, np.array(plt_dcm_local)[:, 1], label="dcm_local") 127 | plt.plot( 128 | plt_time, plt_is_left_leg_in_contact, label="is_left_leg_in_contact" 129 | ) 130 | plt.plot(plt_time, np.array(plt_support_foot)[:, 1], label="support_foot") 131 | for time in plt_step_time: 132 | plt.axvline(time / 1000) 133 | plt.legend() 134 | print(plt_step_time) 135 | 136 | plt.figure("x") 137 | plt.plot(plt_time, np.array(plt_right_foot_position)[:, 0], label="right") 138 | plt.plot(plt_time, np.array(plt_left_foot_position)[:, 0], label="left") 139 | plt.plot(plt_time, np.array(plt_x_com)[:, 0], label="com") 140 | plt.plot(plt_time, np.array(plt_xd_com)[:, 0], label="xd_com") 141 | plt.plot(plt_time, np.array(plt_dcm_local)[:, 0], label="dcm_local") 142 | for time in plt_step_time: 143 | plt.axvline(time / 1000) 144 | plt.legend() 145 | 146 | plt.show() 147 | -------------------------------------------------------------------------------- /doc/bipedal_walking_control_usingv_ariable_horizon_MPC.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/machines-in-motion/reactive_planners/d6d72a71ab6d71836eb5700b81f7071fac668ec5/doc/bipedal_walking_control_usingv_ariable_horizon_MPC.pdf -------------------------------------------------------------------------------- /doc/walking_control_based_on_step_timing_adaptation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/machines-in-motion/reactive_planners/d6d72a71ab6d71836eb5700b81f7071fac668ec5/doc/walking_control_based_on_step_timing_adaptation.pdf -------------------------------------------------------------------------------- /include/reactive_planners/dynamic_graph/stepper_head.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Implements an entity to coordinate the stepping for the 8 | * dcm_vrp_planner. 9 | */ 10 | 11 | #pragma once 12 | 13 | #include "reactive_planners/dcm_vrp_planner.hpp" 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | namespace reactive_planners 20 | { 21 | namespace dynamic_graph 22 | { 23 | /** 24 | * @brief Main entity controlling the stepping phase and keeps track of 25 | * previously computed variables. 26 | */ 27 | class StepperHead : public dynamicgraph::Entity 28 | { 29 | DYNAMIC_GRAPH_ENTITY_DECL(); 30 | 31 | public: 32 | /** 33 | * @brief Construct a new StepperHead object using its Dynamic Graph name. 34 | * 35 | * @param name 36 | */ 37 | StepperHead(const std::string& name); 38 | 39 | /* 40 | * Input Signals. 41 | */ 42 | 43 | /** @brief The u as computed by the dcm_vrp_planner. */ 44 | dynamicgraph::SignalPtr next_step_location_sin_; 45 | 46 | /** @brief The t_end as computed by the dcm_vrp_planner. */ 47 | dynamicgraph::SignalPtr duration_before_step_landing_sin_; 48 | 49 | /* 50 | * Output Signals. 51 | */ 52 | 53 | /** @brief The time [s] of the current stepping phase. */ 54 | dynamicgraph::SignalTimeDependent 55 | time_from_last_step_touchdown_sout_; 56 | 57 | /** @brief The stepping phase. */ 58 | dynamicgraph::SignalTimeDependent is_left_leg_in_contact_sout_; 59 | 60 | /** @brief The old u. */ 61 | dynamicgraph::SignalTimeDependent 62 | old_step_location_sout_; 63 | 64 | /** @brief The current u. */ 65 | dynamicgraph::SignalTimeDependent 66 | current_step_location_sout_; 67 | 68 | /** 69 | * @brief Helper to define the name of the signals. 70 | * 71 | * @param isInputSignal 72 | * @param signalType 73 | * @param signalName 74 | * @return std::string 75 | */ 76 | std::string make_signal_string(const bool& is_input_signal, 77 | const std::string& signal_type, 78 | const std::string& signal_name); 79 | 80 | protected: 81 | // Use these variables to determine the current outputs. 82 | int dg_time_phase_switch_; 83 | double last_duration_before_step_landing_; 84 | bool is_left_leg_in_contact_; 85 | Eigen::Vector3d last_next_step_location_; 86 | Eigen::Vector3d old_step_location_; 87 | Eigen::Vector3d current_step_location_; 88 | 89 | /** 90 | * @brief Callback for time_from_last_step_touchdown_sout_ signal. 91 | * 92 | * @param time_last_step 93 | * @param t 94 | * @return double& 95 | */ 96 | double& time_from_last_step_touchdown(double& time_last_step, int time); 97 | 98 | /** 99 | * @brief Callback for is_left_leg_in_contact_sout_ signal. 100 | * 101 | * @param left_leg_in_contact 102 | * @param t 103 | * @return double& 104 | */ 105 | int& is_left_leg_in_contact(int& left_leg_in_contact, int time); 106 | 107 | /** 108 | * @brief Callback for old_step_location_sout_ signal. 109 | * 110 | * @param step_location 111 | * @param t 112 | * @return double& 113 | */ 114 | dynamicgraph::Vector& old_step_location(dynamicgraph::Vector& step_location, 115 | int time); 116 | 117 | /** 118 | * @brief Callback for current_step_location_sout_ signal. 119 | * 120 | * @param step_location 121 | * @param t 122 | * @return double& 123 | */ 124 | dynamicgraph::Vector& current_step_location( 125 | dynamicgraph::Vector& step_location, int time); 126 | }; 127 | 128 | } // namespace dynamic_graph 129 | } // namespace reactive_planners 130 | -------------------------------------------------------------------------------- /include/reactive_planners/dynamically_consistent_end_effector_trajectory.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Implements the "Walking Control Based on Step Timing Adaptation" foot 8 | * trajectory QP. The pdf can be found in https://arxiv.org/abs/1704.01271, and 9 | * in the `doc/` folder in this repository. 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #define RESET "\033[0m" 20 | #define RED "\033[31m" /* Red */ 21 | #define BLUE "\033[34m" /* Blue */ 22 | #define MAX_VAR 500 23 | #define EPSILON 1e-9 24 | 25 | namespace reactive_planners 26 | { 27 | /** 28 | * @brief 29 | */ 30 | class DynamicallyConsistentEndEffectorTrajectory 31 | { 32 | /* 33 | * Private methods 34 | */ 35 | public: 36 | /** @brief Constructor. */ 37 | DynamicallyConsistentEndEffectorTrajectory(); 38 | 39 | /** @brief Destructor. */ 40 | ~DynamicallyConsistentEndEffectorTrajectory(); 41 | 42 | bool compute(const Eigen::Ref& start_pose, 43 | const Eigen::Ref& current_pose, 44 | const Eigen::Ref& current_velocity, 45 | const Eigen::Ref& target_pose, 46 | const double& start_time, 47 | const double& current_time, 48 | const double& end_time, 49 | const bool& is_left_leg_in_contact); 50 | 51 | void update_robot_status(Eigen::Ref next_pose, 52 | Eigen::Ref next_velocity, 53 | Eigen::Ref next_acceleration); 54 | 55 | /** @brief Get all the forces until landing foot. Returns the number of 56 | * forces.*/ 57 | int get_forces(Eigen::Ref forces, 58 | Eigen::Ref next_pose, 59 | Eigen::Ref next_velocity, 60 | Eigen::Ref next_acceleration); 61 | 62 | /** @brief Display the matrices of the Problem. */ 63 | void print_solver() const; 64 | 65 | /** @brief Convert the inner data to a string format. */ 66 | std::string to_string() const; 67 | 68 | /* 69 | * Getters 70 | */ 71 | 72 | /** @brief Get the height of the flying foot. */ 73 | double get_mid_air_height() const 74 | { 75 | return mid_air_height_; 76 | } 77 | 78 | /* 79 | * Setters 80 | */ 81 | /** @brief Set the height of the flying foot. 82 | * 83 | * @param mid_air_height 84 | */ 85 | void set_planner_loop(double planner_loop) 86 | { 87 | planner_loop_ = planner_loop; 88 | init_acceleration_velocity_terms(); 89 | } 90 | 91 | /** @brief Set the height of the flying foot. 92 | * 93 | * @param mid_air_height 94 | */ 95 | void set_mid_air_height(double mid_air_height) 96 | { 97 | mid_air_height_ = mid_air_height; 98 | } 99 | 100 | /** 101 | * @brief return cost. 102 | * 103 | * @return double 104 | */ 105 | double cost() 106 | { 107 | x_opt_.resize(nb_var_); 108 | x_opt_ = qp_solver_.result(); 109 | return (0.5 * x_opt_.transpose() * Q_ * x_opt_ + 110 | q_.transpose() * x_opt_)(0, 0); 111 | } 112 | 113 | /** 114 | * @brief Return the slack values from the last solution. 115 | **/ 116 | const Eigen::Vector3d& get_slack_variables() const 117 | { 118 | return slack_variables_; 119 | } 120 | 121 | double calculate_t_min( 122 | const Eigen::Ref& current_pose, 123 | const Eigen::Ref& current_velocity, 124 | const bool& is_left_leg_in_contact); 125 | /* 126 | * Private methods 127 | */ 128 | private: 129 | /** @brief resize QP variable at each step. 130 | */ 131 | void resize_matrices() 132 | { 133 | Q_.resize(nb_var_, nb_var_); 134 | Q_.setZero(); 135 | q_.resize(nb_var_); 136 | q_.setZero(); 137 | 138 | A_eq_.resize(nb_eq_, nb_var_); 139 | A_eq_.setZero(); 140 | B_eq_.resize(nb_eq_); 141 | B_eq_.setZero(); 142 | 143 | A_ineq_.resize(nb_ineq_, nb_var_); 144 | A_ineq_.setZero(); 145 | B_ineq_.resize(nb_ineq_); 146 | B_ineq_.setZero(); 147 | qp_solver_.problem(nb_var_, nb_eq_, nb_ineq_); 148 | } 149 | /** @brief resize QP variable at each step. 150 | */ 151 | void resize_matrices_t_min(int index) 152 | { 153 | Q_t_min_.resize(3 * index, 3 * index); 154 | Q_t_min_ = Eigen::MatrixXd::Identity(3 * index, 3 * index); 155 | q_t_min_.resize(3 * index); 156 | q_t_min_.setZero(); 157 | 158 | A_eq_t_min_.resize(2, 3 * index); 159 | A_eq_t_min_.setZero(); 160 | B_eq_t_min_.resize(2); 161 | B_eq_t_min_.setZero(); 162 | 163 | A_ineq_t_min_.resize(2 * 3 * index, 3 * index); 164 | A_ineq_t_min_.setZero(); 165 | B_ineq_t_min_.resize(2 * 3 * index); 166 | B_ineq_t_min_.setZero(); 167 | qp_solver_t_min_.problem(3 * index, 2, 2 * 3 * index); 168 | } 169 | 170 | /** @brief initialie acceleration_terms and velocity_terms. 171 | */ 172 | void init_acceleration_velocity_terms(); 173 | 174 | /* 175 | * Constant problem parameters. 176 | */ 177 | 178 | /** @brief Flying foot apex to be reach mid-air. */ 179 | double mid_air_height_; 180 | 181 | /** @brief Number of force variables in the optimization problem on the 182 | * one of the axes. */ 183 | int nb_local_sampling_time_; 184 | 185 | /** @brief non_linear_terms. 186 | */ 187 | Eigen::Vector3d h_c; 188 | 189 | /* 190 | * Variable problem parameters. 191 | */ 192 | 193 | /** @brief affect of forces_x to the positions. 194 | * There are two separate matrices for each leg. 195 | */ 196 | Eigen::MatrixXd* position_terms_F_x_; 197 | 198 | /** @brief affect of forces_y to the positions. 199 | * There are two separate matrices for each leg. 200 | */ 201 | Eigen::MatrixXd* position_terms_F_y_; 202 | 203 | /** @brief affect of forces_z to the positions. 204 | * There are two separate matrices for each leg. 205 | */ 206 | Eigen::MatrixXd* position_terms_F_z_; 207 | 208 | /** @brief Desired velocity. */ 209 | Eigen::Vector3d v_des_; 210 | 211 | /** @brief Slack variable values corresponding to last solution. */ 212 | Eigen::Vector3d slack_variables_; 213 | 214 | /** @brief affect of non_linear_terms in position. */ 215 | Eigen::MatrixXd* non_linear_terms; 216 | 217 | /** @brief Initial position before the motion. */ 218 | Eigen::Vector3d start_pose_; 219 | 220 | /** @brief Current position. */ 221 | Eigen::Vector3d current_pose_; 222 | 223 | /** @brief Previous computed position from the QP. */ 224 | Eigen::Vector3d previous_solution_pose_; 225 | 226 | /** @brief Current velocity. */ 227 | Eigen::Vector3d current_velocity_; 228 | 229 | /** @brief Current acceleration. */ 230 | Eigen::Vector3d current_acceleration_; 231 | 232 | /** @brief Target pose after the motion. */ 233 | Eigen::Vector3d target_pose_; 234 | 235 | /** @brief Initial time. */ 236 | double start_time_; 237 | 238 | /** @brief Current time. */ 239 | double current_time_; 240 | 241 | /** @brief Last end time register when we computed the QP. */ 242 | double last_end_time_seen_; 243 | 244 | /** @brief Control loop. */ 245 | double control_loop_; 246 | 247 | /** @brief planner loop. */ 248 | double planner_loop_; 249 | 250 | /* 251 | * QP variables 252 | */ 253 | 254 | /** @brief Number of variables in the optimization problem. */ 255 | int nb_var_; 256 | 257 | /** @brief Number of equality constraints in the optimization problem. */ 258 | int nb_eq_; 259 | 260 | /** @brief Number of inequality in the optimization problem. */ 261 | int nb_ineq_; 262 | 263 | /** @brief Quadratic program solver. 264 | * 265 | * This is an eigen wrapper around the quad_prog fortran solver. 266 | */ 267 | Eigen::QuadProgDense qp_solver_; 268 | 269 | /** @brief Quadratic program solver. 270 | * 271 | * This is an eigen wrapper around the quad_prog fortran solver. 272 | */ 273 | Eigen::QuadProgDense qp_solver_t_min_; 274 | 275 | /** @brief Solution of the optimization problem. 276 | * @see DynamicallyConsistentEndEffectorTrajectory */ 277 | Eigen::VectorXd x_opt_; 278 | 279 | /** @brief Quadratic term of the quadratic cost. 280 | * @see DynamicallyConsistentEndEffectorTrajectory */ 281 | Eigen::MatrixXd Q_; 282 | /** @brief Quadratic term of the quadratic cost. 283 | * @see DynamicallyConsistentEndEffectorTrajectory */ 284 | Eigen::MatrixXd Q_t_min_; 285 | 286 | /** @brief inverse estimation of mass matrix. 287 | * @see DynamicallyConsistentEndEffectorTrajectory */ 288 | Eigen::MatrixXd* M_inv_; 289 | 290 | /** @brief Is the left foot in contact? otherwise the right foot is. */ 291 | bool is_left_leg_in_contact_; 292 | 293 | /** @brief Quadratic term added to the quadratic cost in order regularize 294 | * the system. 295 | * @see DynamicallyConsistentEndEffectorTrajectory */ 296 | Eigen::MatrixXd Q_regul_; 297 | 298 | /** @brief Cost weights for forces. */ 299 | double cost_; 300 | 301 | /** @brief Cost weights for the epsilon_z_mid. */ 302 | double cost_epsilon_z_mid_; 303 | 304 | /** @brief Cost weights for the epsilon_x. */ 305 | double cost_epsilon_x_; 306 | 307 | /** @brief Cost weights for the epsilon_y. */ 308 | double cost_epsilon_y_; 309 | 310 | /** @brief Cost weights for the epsilon_z. */ 311 | double cost_epsilon_z_; 312 | 313 | /** @brief Cost weights for the epsilon_z. */ 314 | double cost_epsilon_vel_; 315 | 316 | /** @brief Linear term of the quadratic cost. 317 | * @see DynamicallyConsistentEndEffectorTrajectory */ 318 | Eigen::VectorXd q_; 319 | 320 | /** @brief Linear equality matrix. 321 | * @see DynamicallyConsistentEndEffectorTrajectory */ 322 | Eigen::MatrixXd A_eq_; 323 | 324 | /** @brief Linear equality vector. 325 | * @see DynamicallyConsistentEndEffectorTrajectory */ 326 | Eigen::VectorXd B_eq_; 327 | 328 | /** @brief Linear inequality matrix. 329 | * @see DynamicallyConsistentEndEffectorTrajectory */ 330 | Eigen::MatrixXd A_ineq_; 331 | 332 | /** @brief Linear inequality vector. 333 | * @see DynamicallyConsistentEndEffectorTrajectory */ 334 | Eigen::VectorXd B_ineq_; 335 | 336 | /** @brief Linear term of the quadratic cost. 337 | * @see DynamicallyConsistentEndEffectorTrajectory */ 338 | Eigen::VectorXd q_t_min_; 339 | 340 | /** @brief Linear equality matrix. 341 | * @see DynamicallyConsistentEndEffectorTrajectory */ 342 | Eigen::MatrixXd A_eq_t_min_; 343 | 344 | /** @brief Linear equality vector. 345 | * @see DynamicallyConsistentEndEffectorTrajectory */ 346 | Eigen::VectorXd B_eq_t_min_; 347 | 348 | /** @brief Linear inequality matrix. 349 | * @see DynamicallyConsistentEndEffectorTrajectory */ 350 | Eigen::MatrixXd A_ineq_t_min_; 351 | 352 | /** @brief Linear inequality vector. 353 | * @see DynamicallyConsistentEndEffectorTrajectory */ 354 | Eigen::VectorXd B_ineq_t_min_; 355 | }; 356 | 357 | } // namespace reactive_planners 358 | -------------------------------------------------------------------------------- /include/reactive_planners/mathematics/polynome.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file polynome.hpp 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @brief Polynomes object for trajectories. 5 | * @version 0.1 6 | * @date 2019-11-06 7 | * 8 | * @copyright Copyright (c) 2019 9 | * 10 | */ 11 | #pragma once 12 | 13 | #include 14 | 15 | namespace blmc_robots 16 | { 17 | /** 18 | * @brief Simple class that defines \f$ P(x) \f$ a polynome of order ORDER. 19 | * It provide simple methods to compute \f$ P(x) \f$, 20 | * \f$ \frac{dP}{dx}(x) \f$, and \f$ \frac{dP^2}{dx^2}(x) \f$ 21 | * 22 | * @tparam ORDER is the order of the polynome 23 | */ 24 | template 25 | class Polynome 26 | { 27 | /*! Type of the container for the poynome coefficients */ 28 | typedef std::array Coefficients; 29 | 30 | public: 31 | /*! Constructor */ 32 | Polynome(); 33 | 34 | /*! Destructor */ 35 | ~Polynome(); 36 | 37 | /*! Compute the value. */ 38 | double compute(double x); 39 | 40 | /*! Compute the value of the derivative. */ 41 | double compute_derivative(double x); 42 | 43 | /*! Compute the value of the second derivative. */ 44 | double compute_sec_derivative(double x); 45 | 46 | /*! Get the coefficients. */ 47 | void get_coefficients(Coefficients &coefficients) const; 48 | 49 | /*! Set the coefficients. */ 50 | void set_coefficients(const Coefficients &coefficients); 51 | 52 | inline int degree() 53 | { 54 | return ORDER; 55 | }; 56 | 57 | /*! Print the coefficient. */ 58 | void print() const; 59 | 60 | protected: 61 | /*! Vector of coefficients. */ 62 | std::array coefficients_; 63 | }; 64 | 65 | /** 66 | * @brief Simple class that defines \f$ P(t) \f$ a polynome of order ORDER. 67 | * With \f$ t \f$ being the time in any units. 68 | * It provide simple methods to compute safely \f$ P(time) \f$, 69 | * \f$ \frac{dP}{dt}(t) \f$, and \f$ \frac{dP^2}{dt^2}(t) \f$ 70 | * 71 | * @tparam ORDER 72 | */ 73 | template 74 | class TimePolynome : public Polynome 75 | { 76 | public: 77 | /*! Constructor */ 78 | TimePolynome() 79 | { 80 | final_time_ = 0.0; 81 | init_pose_ = 0.0; 82 | init_speed_ = 0.0; 83 | init_acc_ = 0.0; 84 | final_pose_ = 0.0; 85 | final_speed_ = 0.0; 86 | final_acc_ = 0.0; 87 | }; 88 | 89 | /*! Destructor */ 90 | ~TimePolynome(){}; 91 | 92 | /*! Compute the value. */ 93 | double compute(double t); 94 | 95 | /*! Compute the value of the derivative. */ 96 | double compute_derivative(double t); 97 | 98 | /*! Compute the value of the second derivative. */ 99 | double compute_sec_derivative(double t); 100 | 101 | double get_final_time() const 102 | { 103 | return final_time_; 104 | } 105 | double get_init_pose() const 106 | { 107 | return init_pose_; 108 | } 109 | double get_init_speed() const 110 | { 111 | return init_speed_; 112 | } 113 | double get_init_acc() const 114 | { 115 | return init_acc_; 116 | } 117 | double get_final_pose() const 118 | { 119 | return final_pose_; 120 | } 121 | double get_final_speed() const 122 | { 123 | return final_speed_; 124 | } 125 | double get_final_acc() const 126 | { 127 | return final_acc_; 128 | } 129 | 130 | /** 131 | * @brief Computes a polynome trajectory according to the following 132 | * constraints: 133 | * \f{eqnarray*}{ 134 | * P(0) &=& init_pose \\ 135 | * P(0) &=& init_speed \\ 136 | * P(0) &=& init_acc \\ 137 | * P(final_time_) &=& final_pose \\ 138 | * P(final_time_) &=& final_speed \\ 139 | * P(final_time_) &=& final_acc 140 | * \f} 141 | * 142 | * @param final_time 143 | * @param init_pos 144 | * @param init_speed 145 | * @param init_acc 146 | * @param middle_pose 147 | * @param final_pos 148 | * @param final_speed 149 | * @param final_acc 150 | */ 151 | void set_parameters(double final_time, 152 | double init_pos, 153 | double init_speed, 154 | double init_acc, 155 | double final_pos, 156 | double final_speed, 157 | double final_acc); 158 | 159 | protected: 160 | double final_time_; /**< store the inputs for later access */ 161 | double init_pose_; /**< store the inputs for later access */ 162 | double init_speed_; /**< store the inputs for later access */ 163 | double init_acc_; /**< store the inputs for later access */ 164 | double final_pose_; /**< store the inputs for later access */ 165 | double final_speed_; /**< store the inputs for later access */ 166 | double final_acc_; /**< store the inputs for later access */ 167 | }; 168 | 169 | } // namespace blmc_robots 170 | 171 | #include "blmc_robots/mathematics/polynome.hxx" 172 | -------------------------------------------------------------------------------- /include/reactive_planners/mathematics/polynome.hxx: -------------------------------------------------------------------------------- 1 | /** 2 | * @file polynome_impl.hh 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @brief Polynomes object for trajectories. 5 | * @version 0.1 6 | * @date 2019-11-06 7 | * 8 | * @copyright Copyright (c) 2019 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include "blmc_robots/mathematics/polynome.hpp" 17 | 18 | namespace blmc_robots 19 | { 20 | /** 21 | * Polynome definitions 22 | */ 23 | 24 | template 25 | Polynome::Polynome() 26 | { 27 | coefficients_.fill(0.0); 28 | } 29 | 30 | template 31 | Polynome::~Polynome() 32 | { 33 | } 34 | 35 | template 36 | double Polynome::compute(double x) 37 | { 38 | double res = 0.0; 39 | double pt = 1.0; 40 | for (size_t i = 0; i < coefficients_.size(); ++i) 41 | { 42 | res += coefficients_[i] * pt; 43 | pt *= x; 44 | } 45 | return res; 46 | } 47 | 48 | template 49 | double Polynome::compute_derivative(double x) 50 | { 51 | double res = 0.0; 52 | double pt = 1.0; 53 | for (size_t i = 1; i < coefficients_.size(); ++i) 54 | { 55 | res += i * coefficients_[i] * pt; 56 | pt *= x; 57 | } 58 | return res; 59 | } 60 | 61 | template 62 | double Polynome::compute_sec_derivative(double x) 63 | { 64 | double res = 0.0; 65 | double pt = 1.0; 66 | for (size_t i = 2; i < coefficients_.size(); ++i) 67 | { 68 | res += i * (i - 1) * coefficients_[i] * pt; 69 | pt *= x; 70 | } 71 | return res; 72 | } 73 | 74 | template 75 | void Polynome::get_coefficients(Coefficients &coefficients) const 76 | { 77 | coefficients = coefficients_; 78 | } 79 | 80 | template 81 | void Polynome::set_coefficients(const Coefficients &coefficients) 82 | { 83 | coefficients_ = coefficients; 84 | } 85 | 86 | template 87 | void Polynome::print() const 88 | { 89 | for (size_t i = 0; i < ORDER; ++i) 90 | { 91 | std::cout << coefficients_[i] << " "; 92 | } 93 | std::cout << std::endl; 94 | } 95 | 96 | /** 97 | * TimePolynome definitions 98 | */ 99 | 100 | template 101 | double TimePolynome::compute(double t) 102 | { 103 | if (t <= 0.0) 104 | { 105 | return init_pose_; 106 | } 107 | else if (t >= final_time_) 108 | { 109 | return final_pose_; 110 | } 111 | else 112 | { 113 | return Polynome::compute(t); 114 | } 115 | } 116 | 117 | template 118 | double TimePolynome::compute_derivative(double t) 119 | { 120 | if (t <= 0.0) 121 | { 122 | return init_speed_; 123 | } 124 | else if (t >= final_time_) 125 | { 126 | return final_speed_; 127 | } 128 | else 129 | { 130 | return Polynome::compute_derivative(t); 131 | } 132 | } 133 | 134 | template 135 | double TimePolynome::compute_sec_derivative(double t) 136 | { 137 | if (t <= 0.0) 138 | { 139 | return init_acc_; 140 | } 141 | else if (t >= final_time_) 142 | { 143 | return final_acc_; 144 | } 145 | else 146 | { 147 | return Polynome::compute_sec_derivative(t); 148 | } 149 | } 150 | 151 | } // namespace blmc_robots 152 | -------------------------------------------------------------------------------- /include/reactive_planners/polynomial_end_effector_trajectory.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Implements the "Walking Control Based on Step Timing Adaptation" foot 8 | * trajectory QP. The pdf can be found in https://arxiv.org/abs/1704.01271, and 9 | * in the `doc/` folder in this repository. 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | 17 | namespace reactive_planners 18 | { 19 | /** 20 | * @brief 21 | */ 22 | class PolynomialEndEffectorTrajectory 23 | { 24 | /* 25 | * Private methods 26 | */ 27 | public: 28 | /** @brief Constructor. */ 29 | PolynomialEndEffectorTrajectory(); 30 | 31 | /** @brief Destructor. */ 32 | ~PolynomialEndEffectorTrajectory(); 33 | 34 | bool compute(const Eigen::Ref &start_pose, 35 | const Eigen::Ref ¤t_pose, 36 | const Eigen::Ref ¤t_velocity, 37 | const Eigen::Ref ¤t_acceleration, 38 | const Eigen::Ref &target_pose, 39 | const double &start_time, 40 | const double ¤t_time, 41 | const double &end_time); 42 | 43 | void get_next_state(const double &next_time, 44 | Eigen::Ref next_pose, 45 | Eigen::Ref next_velocity, 46 | Eigen::Ref next_acceleration); 47 | 48 | /** @brief Display the matrices of the Problem. */ 49 | void print_solver() const; 50 | 51 | /** @brief Convert the inner data to a string format. */ 52 | std::string to_string() const; 53 | 54 | /* 55 | * Getters 56 | */ 57 | 58 | /** @brief Get the height of the flying foot. */ 59 | double get_mid_air_height() 60 | { 61 | return mid_air_height_; 62 | } 63 | 64 | /** @brief Get the last end time taken into account during the foot 65 | * trajectory computation. */ 66 | double get_last_end_time_taken_into_account() 67 | { 68 | return last_end_time_seen_; 69 | } 70 | 71 | /* 72 | * Setters 73 | */ 74 | 75 | /** @brief Set the height of the flying foot. 76 | * 77 | * @param mid_air_height 78 | */ 79 | void set_mid_air_height(double mid_air_height) 80 | { 81 | mid_air_height_ = mid_air_height; 82 | } 83 | /** @brief Set the costs of x, y, z axes, and hessian regularization. 84 | * 85 | * @param cost_x 86 | * @param cost_y 87 | * @param cost_z 88 | * @param hess_regularization 89 | */ 90 | void set_costs(double cost_x, 91 | double cost_y, 92 | double cost_z, 93 | double hess_regularization) 94 | { 95 | cost_x_ = cost_x; 96 | cost_y_ = cost_y; 97 | cost_z_ = cost_z; 98 | Q_regul_ = 99 | Eigen::MatrixXd::Identity(nb_var_, nb_var_) * hess_regularization; 100 | } 101 | /* 102 | * Private methods 103 | */ 104 | private: 105 | /** 106 | * @brief Compute the time vector: \f$ [1, t, ..., t^{ORDER}] \f$. 107 | * 108 | * @param time 109 | * @param time_vec 110 | */ 111 | void t_vec(const double &time, Eigen::VectorXd &time_vec) 112 | { 113 | time_vec(0) = 1.0; 114 | for (int i = 1; i < time_vec.size(); ++i) 115 | { 116 | time_vec(i) = std::pow(time, i); 117 | } 118 | } 119 | 120 | /** 121 | * @brief Compute the time vector first derivative. 122 | * 123 | * \f[ t_vec = [0, 1, 2*t, ..., ORDER * t^{ORDER-1}] \f] 124 | * 125 | * @param time 126 | * @param time_vec 127 | */ 128 | void dt_vec(const double &time, Eigen::VectorXd &time_vec) 129 | { 130 | time_vec(0) = 0.0; 131 | time_vec(1) = 1.0; 132 | for (int i = 2; i < time_vec.size(); ++i) 133 | { 134 | double id = i; 135 | time_vec(i) = id * std::pow(time, i - 1); 136 | } 137 | } 138 | 139 | /** 140 | * @brief Compute the time vector second derivative: 141 | * 142 | * \f[ t_vec = [0, 0, 2, 3*2*t..., ORDER * (ORDER-1) * t^{ORDER-2}] \f] 143 | * 144 | * @param time 145 | * @param time_vec 146 | */ 147 | void ddt_vec(const double &time, Eigen::VectorXd &time_vec) 148 | { 149 | time_vec(0) = 0.0; 150 | time_vec(1) = 0.0; 151 | time_vec(2) = 2.0; 152 | for (int i = 3; i < time_vec.size(); ++i) 153 | { 154 | double id = i; 155 | time_vec(i) = id * (id - 1.0) * std::pow(time, i - 2); 156 | } 157 | } 158 | 159 | /* 160 | * Attributes 161 | */ 162 | private: 163 | /* 164 | * Constant problem parameters. 165 | */ 166 | 167 | /** @brief Flying foot apex to be reach mid-air. */ 168 | double mid_air_height_; 169 | 170 | /** @brief Number of the polynome coefficient for the trajectory on the 171 | * X-axis. */ 172 | int nb_var_x_; 173 | 174 | /** @brief Number of the polynome coefficient for the trajectory on the 175 | * Y-axis. */ 176 | int nb_var_y_; 177 | 178 | /** @brief Number of the polynome coefficient for the trajectory on the 179 | * Z-axis. */ 180 | int nb_var_z_; 181 | 182 | /* 183 | * Variable problem parameters. 184 | */ 185 | 186 | /** @brief Time vector used to compute X(t). */ 187 | Eigen::VectorXd time_vec_x_; 188 | 189 | /** @brief Time vector used to compute Y(t). */ 190 | Eigen::VectorXd time_vec_y_; 191 | 192 | /** @brief Time vector used to compute Z(t). */ 193 | Eigen::VectorXd time_vec_z_; 194 | 195 | /** @brief Initial position before the motion. */ 196 | Eigen::Vector3d start_pose_; 197 | 198 | /** @brief Current position. */ 199 | Eigen::Vector3d current_pose_; 200 | 201 | /** @brief Previous computed position from the QP. */ 202 | Eigen::Vector3d previous_solution_pose_; 203 | 204 | /** @brief Current velocity. */ 205 | Eigen::Vector3d current_velocity_; 206 | 207 | /** @brief Current acceleration. */ 208 | Eigen::Vector3d current_acceleration_; 209 | 210 | /** @brief Target pose after the motion. */ 211 | Eigen::Vector3d target_pose_; 212 | 213 | /** @brief Initial time. */ 214 | double start_time_; 215 | 216 | /** @brief Current time. */ 217 | double current_time_; 218 | 219 | /** @brief Final time and the end of the motion. */ 220 | double end_time_; 221 | 222 | /** @brief Last end time register when we computed the QP. */ 223 | double last_end_time_seen_; 224 | 225 | /* 226 | * QP variables 227 | */ 228 | 229 | /** @brief Number of variabes in the optimization problem. */ 230 | int nb_var_; 231 | 232 | /** @brief Number of equality constraints in the optimization problem. */ 233 | int nb_eq_; 234 | 235 | /** @brief Number of inequality in the optimization problem. */ 236 | int nb_ineq_; 237 | 238 | /** @brief Quadratic program solver. 239 | * 240 | * This is an eigen wrapper around the quad_prog fortran solver. 241 | */ 242 | Eigen::QuadProgDense qp_solver_; 243 | 244 | /** @brief Solution of the optimization problem. 245 | * @see PolynomialEndEffectorTrajectory */ 246 | Eigen::VectorXd x_opt_; 247 | 248 | /** @brief Lower Bound on the solution of the optimization problem. 249 | * @see PolynomialEndEffectorTrajectory */ 250 | Eigen::VectorXd x_opt_lb_; 251 | 252 | /** @brief Upper Bound on the solution of the optimization problem. 253 | * @see PolynomialEndEffectorTrajectory */ 254 | Eigen::VectorXd x_opt_ub_; 255 | 256 | /** @brief Quadratic term of the quadratic cost. 257 | * @see PolynomialEndEffectorTrajectory */ 258 | Eigen::MatrixXd Q_; 259 | 260 | /** @brief Quadratic term added to the quadratic cost in order regularize 261 | * the system. 262 | * @see PolynomialEndEffectorTrajectory */ 263 | Eigen::MatrixXd Q_regul_; 264 | 265 | /** @brief Cost weights for the X-axis. */ 266 | double cost_x_; 267 | 268 | /** @brief Cost weights for the Y-axis. */ 269 | double cost_y_; 270 | 271 | /** @brief Cost weights for the Z-axis. */ 272 | double cost_z_; 273 | 274 | /** @brief Linear term of the quadratic cost. 275 | * @see PolynomialEndEffectorTrajectory */ 276 | Eigen::VectorXd q_; 277 | 278 | /** @brief Linear equality matrix. 279 | * @see PolynomialEndEffectorTrajectory */ 280 | Eigen::MatrixXd A_eq_; 281 | 282 | /** @brief Linear equality vector. 283 | * @see PolynomialEndEffectorTrajectory */ 284 | Eigen::VectorXd B_eq_; 285 | 286 | /** @brief Linear inequality matrix. 287 | * @see PolynomialEndEffectorTrajectory */ 288 | Eigen::MatrixXd A_ineq_; 289 | 290 | /** @brief Linear inequality vector. 291 | * @see PolynomialEndEffectorTrajectory */ 292 | Eigen::VectorXd B_ineq_; 293 | }; 294 | 295 | } // namespace reactive_planners 296 | -------------------------------------------------------------------------------- /include/reactive_planners/stepper_head.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Implements a class to coordinate the stepping for the dcm_vrp_planner. 8 | */ 9 | 10 | #pragma once 11 | 12 | #include "Eigen/Eigen" 13 | 14 | namespace reactive_planners 15 | { 16 | /** 17 | * @brief Main entity controlling the stepping phase and keeps track of 18 | * previously computed variables. 19 | */ 20 | class StepperHead 21 | { 22 | /* 23 | * Public methods. 24 | */ 25 | public: 26 | /** @brief Construct a new StepperHead object with simple default 27 | * parameters. Please call init() in order to setup this class properly. */ 28 | StepperHead(); 29 | 30 | void set_support_feet_pos( 31 | const Eigen::Ref &previous_support_location, 32 | const Eigen::Ref ¤t_support_location) 33 | { 34 | previous_support_location_ = previous_support_location; 35 | current_support_location_ = current_support_location; 36 | } 37 | void set_support_foot_pos( 38 | const Eigen::Ref ¤t_support_location) 39 | { 40 | current_support_location_ = current_support_location; 41 | } 42 | 43 | void run(const double &duration_before_foot_landing, 44 | const Eigen::Vector3d &next_support_location, 45 | const double ¤t_time); 46 | 47 | /* 48 | * Getters. 49 | */ 50 | 51 | /** @brief Get the time from last foot touchdown. 52 | * @return const double& 53 | */ 54 | const double &get_time_from_last_step_touchdown() const 55 | { 56 | return time_from_last_step_touchdown_; 57 | } 58 | 59 | /** @brief Get if the left foot is in contact. If not then it is the right 60 | * foot. 61 | * @return const double& 62 | */ 63 | bool &get_is_left_leg_in_contact() 64 | { 65 | return is_left_leg_in_contact_; 66 | } 67 | 68 | /** @brief Get the previous foot step location in the world frame. The 69 | * previous foot is therefore currently a flying foot. 70 | * @return const Eigen::Vector3d& 71 | */ 72 | const Eigen::Vector3d &get_previous_support_location() 73 | { 74 | return previous_support_location_; 75 | } 76 | 77 | /** @brief Get the current step location. The current foot is the one in 78 | * contact. 79 | * @return const Eigen::Vector3d& 80 | */ 81 | const Eigen::Vector3d &get_current_support_location() 82 | { 83 | return current_support_location_; 84 | } 85 | 86 | /* 87 | * Private methods. 88 | */ 89 | protected: 90 | /* 91 | * Private attributes. 92 | */ 93 | protected: 94 | /* 95 | * Inputs 96 | */ 97 | 98 | /** @brief This is the duration before the current flying foot needs to 99 | * land. */ 100 | double duration_before_foot_landing_; 101 | 102 | /** @brief Next support location in absolute coordinates. The 103 | * corresponding foot is currently **flying**. */ 104 | Eigen::Vector3d next_support_location_; 105 | 106 | /** @brief Current absolute time; */ 107 | double current_time_; 108 | 109 | /* 110 | * Outputs 111 | */ 112 | 113 | /** @brief Is the duration from the last flying foot touchdown. */ 114 | double time_from_last_step_touchdown_; 115 | 116 | /** @brief Left (true) or right (false) foot is in contact. */ 117 | bool is_left_leg_in_contact_; 118 | 119 | /** @brief Previous support location in absolute coordinates. The 120 | * corresponding foot is currently **flying**. */ 121 | Eigen::Vector3d previous_support_location_; 122 | 123 | /** @brief Current support location in absolute coordinates. The 124 | * corresponding foot is currently in **contact**. */ 125 | Eigen::Vector3d current_support_location_; 126 | 127 | /* 128 | * Internal 129 | */ 130 | 131 | /** @brief Time to switch the support foot from left to right. */ 132 | double time_support_switch_; 133 | }; 134 | 135 | } // namespace reactive_planners 136 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, New York University and Max Planck Gesellshaft. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /python/reactive_planners/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/machines-in-motion/reactive_planners/d6d72a71ab6d71836eb5700b81f7071fac668ec5/python/reactive_planners/__init__.py -------------------------------------------------------------------------------- /python/reactive_planners/centroidal_controller/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/machines-in-motion/reactive_planners/d6d72a71ab6d71836eb5700b81f7071fac668ec5/python/reactive_planners/centroidal_controller/__init__.py -------------------------------------------------------------------------------- /python/reactive_planners/centroidal_controller/lipm_centroidal_controller.py: -------------------------------------------------------------------------------- 1 | ## This file contains the implementation of com controller that also contains 2 | ## constratins to keep COP in a certain desired location 3 | ## only works for walking 4 | ## Author: Avadesh Meduri 5 | ## Date: 9/12/2019 6 | 7 | import numpy as np 8 | import pinocchio as pin 9 | 10 | from reactive_planners.utils.qp_solver import quadprog_solve_qp 11 | from reactive_planners.utils.solo_state_estimator import SoloStateEstimator 12 | 13 | arr = lambda a: np.array(a).reshape(-1) 14 | mat = lambda a: np.matrix(a).reshape((-1, 1)) 15 | 16 | 17 | class LipmCentroidalController: 18 | def __init__(self, robot, m, mu, kc, dc, kb, db, eff_ids): 19 | self._robot = robot 20 | self._m = m # Total robot mass. 21 | self._mu = mu # Friction coefficient 22 | self._kc = kc 23 | self._dc = dc 24 | self._kb = kb 25 | self._db = db 26 | self._w_com = self._m * 9.81 ## weight of the robot 27 | self._eff_ids = eff_ids 28 | self.sse = SoloStateEstimator(robot) 29 | 30 | def compute_com_wrench( 31 | self, t, q, dq, des_pos, des_vel, des_ori, des_angvel 32 | ): 33 | """Compute the desired COM wrench (equation 1). 34 | 35 | Args: 36 | t: Timestep to compute w_com 37 | des_pos: desired center of mass position at time t 38 | des_vel: desired center of mass velocity at time t 39 | des_ori: desired base orientation at time t (quaternions) 40 | des_angvel: desired base angular velocity at time t 41 | Returns: 42 | Computed w_com 43 | """ 44 | m = self._m 45 | robot = self._robot 46 | 47 | com = arr(robot.com(q, dq)[0]) 48 | vcom = arr(robot.vcom(q, dq)) 49 | Ib = robot.mass(q)[3:6, 3:6] 50 | 51 | quat_diff = self.quaternion_difference(arr(q[3:7]), arr(des_ori)) 52 | 53 | w_com = np.hstack( 54 | [ 55 | m * np.multiply(self._kc, (des_pos - com)) 56 | + m * np.multiply(self._dc, (des_vel - vcom)), 57 | arr(Ib * mat(np.multiply(self._kb, quat_diff))) 58 | + np.multiply(self._db, (des_angvel - arr(dq[3:6]))), 59 | ] 60 | ) 61 | 62 | return w_com 63 | 64 | def compute_force_qp(self, t, q, dq, cnt_array, u, u_min, u_max, w_com): 65 | """Computes the forces needed to generated a desired centroidal wrench. 66 | It has additional constraints that try to keep cop at desired location 67 | 68 | Args: 69 | t: Timestep. 70 | q: Generalized robot position configuration. 71 | q: Generalized robot velocity configuration. 72 | cnt_array: Array with {0, 1} of #endeffector size indicating if 73 | an endeffector is in contact with the ground or not. Forces are 74 | only computed for active endeffectors. 75 | u: desired COP/ZMP location 76 | u_min: minimum COP location to remove viable 77 | u_max: maximum COP location to remove viable 78 | w_com: com control wrench 79 | Returns: 80 | Computed forces as a plain array of size 3 * num_endeffectors. 81 | """ 82 | robot = self._robot 83 | com = robot.com(q, dq)[0] 84 | foot_loc = [robot.data.oMf[i].translation for i in self._eff_ids] 85 | r = [robot.data.oMf[i].translation - com for i in self._eff_ids] 86 | 87 | # Use the contact activation from the plan to determine which of the forces 88 | # should be active. 89 | no_slack_variables = 8 ## (slack on Fx, Fy, Fz, Tx, Ty, Tz, ux, uy) 90 | N = (int)(np.sum(cnt_array)) 91 | Q = 2.0 * np.eye(3 * N + no_slack_variables) 92 | Q[-no_slack_variables:, -no_slack_variables:] = 1e4 * np.eye( 93 | no_slack_variables 94 | ) 95 | Q[-2, -2] = 1e8 96 | Q[-1, -1] = 1e8 97 | p = np.zeros(3 * N + no_slack_variables) 98 | A = np.zeros((no_slack_variables, 3 * N + no_slack_variables)) 99 | b = np.zeros(no_slack_variables) 100 | b[0:6] = w_com 101 | # b[6:] = u 102 | 103 | G = np.zeros((5 * N + 4, 3 * N + no_slack_variables)) 104 | h = np.zeros((5 * N + 4)) 105 | # h[-4] = u_max[0] 106 | # h[-3] = u_max[1] 107 | # h[-2] = -u_min[0] 108 | # h[-1] = -u_min[1] 109 | 110 | j = 0 111 | for i in range(4): 112 | if cnt_array[i] == 0: 113 | continue 114 | 115 | A[:3, 3 * j : 3 * (j + 1)] = np.eye(3) 116 | A[3:6, 3 * j : 3 * (j + 1)] = pin.utils.skew(r[i]) 117 | 118 | ##Constraints to keep COP close to the desired value 119 | # A[-2][3*i + 0] = -float(com[2])/self._w_com 120 | # A[-2][3*i + 2] = float(foot_loc[i][0])/self._w_com 121 | # A[-1][3*i + 1] = -float(com[2])/self._w_com 122 | # A[-1][3*i + 2] = float(foot_loc[i][1])/self._w_com 123 | 124 | G[5 * j + 0, 3 * j + 0] = 1 # mu Fz - Fx >= 0 125 | G[5 * j + 0, 3 * j + 2] = -self._mu 126 | G[5 * j + 1, 3 * j + 0] = -1 # mu Fz + Fx >= 0 127 | G[5 * j + 1, 3 * j + 2] = -self._mu 128 | G[5 * j + 2, 3 * j + 1] = 1 # mu Fz - Fy >= 0 129 | G[5 * j + 2, 3 * j + 2] = -self._mu 130 | G[5 * j + 3, 3 * j + 1] = -1 # mu Fz + Fy >= 0 131 | G[5 * j + 3, 3 * j + 2] = -self._mu 132 | G[5 * j + 4, 3 * j + 2] = -1 # Fz >= 0 133 | 134 | ## Constraints to keep COP inside viability boundary 135 | # G[-4][3*i + 0] = -float(com[2])/self._w_com 136 | # G[-4][3*i + 2] = float(foot_loc[i][0])/self._w_com 137 | # G[-3][3*i + 1] = -float(com[2])/self._w_com 138 | # G[-3][3*i + 2] = float(foot_loc[i][1])/self._w_com 139 | # G[-2][3*i + 0] = float(com[2])/self._w_com ## minima constraint 140 | # G[-2][3*i + 2] = -float(foot_loc[i][0])/self._w_com 141 | # G[-1][3*i + 1] = float(com[2])/self._w_com ## minima constraint 142 | # G[-1][3*i + 2] = -float(foot_loc[i][1])/self._w_com 143 | j += 1 144 | 145 | A[:, -no_slack_variables:] = np.eye(no_slack_variables) 146 | 147 | solx = quadprog_solve_qp(Q, p, G, h, A, b) 148 | F = np.zeros(12) 149 | j = 0 150 | for i in range(4): 151 | if cnt_array[i] == 0: 152 | continue 153 | F[3 * i : 3 * (i + 1)] = solx[3 * j : 3 * (j + 1)] 154 | j += 1 155 | 156 | return F 157 | 158 | #### quaternion stuff 159 | def skew(self, v): 160 | """converts vector v to skew symmetric matrix""" 161 | assert v.shape[0] == 3, "vector dimension is not 3 in skew method" 162 | return np.array( 163 | [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]] 164 | ) 165 | 166 | def quaternion_to_rotation(self, q): 167 | """ converts quaternion to rotation matrix """ 168 | return ( 169 | (q[3] ** 2 - q[:3].dot(q[:3])) * np.eye(3) 170 | + 2.0 * np.outer(q[:3], q[:3]) 171 | + 2.0 * q[3] * self.skew(q[:3]) 172 | ) 173 | 174 | def exp_quaternion(self, w): 175 | """ converts angular velocity to quaternion """ 176 | qexp = np.zeros(4) 177 | th = np.linalg.norm(w) 178 | if th ** 2 <= 1.0e-6: 179 | """small norm causes closed form to diverge, 180 | use taylor expansion to approximate""" 181 | qexp[:3] = (1 - (th ** 2) / 6) * w 182 | qexp[3] = 1 - (th ** 2) / 2 183 | else: 184 | u = w / th 185 | qexp[:3] = np.sin(th) * u 186 | qexp[3] = np.cos(th) 187 | return qexp 188 | 189 | def log_quaternion(self, q): 190 | """ lives on the tangent space of SO(3) """ 191 | v = q[:3] 192 | w = q[3] 193 | vnorm = np.linalg.norm(v) 194 | if vnorm <= 1.0e-6: 195 | return 2 * v / w * (1 - vnorm ** 2 / (3 * w ** 2)) 196 | else: 197 | return 2 * np.arctan2(vnorm, w) * v / vnorm 198 | 199 | def quaternion_product(self, q1, q2): 200 | """ computes quaternion product of q1 x q2 """ 201 | p = np.zeros(4) 202 | p[:3] = np.cross(q1[:3], q2[:3]) + q2[3] * q1[:3] + q1[3] * q2[:3] 203 | p[3] = q1[3] * q2[3] - q1[:3].dot(q2[:3]) 204 | return p 205 | 206 | def integrate_quaternion(self, q, w): 207 | """ updates quaternion with tangent vector w """ 208 | dq = self.exp_quaternion(0.5 * w) 209 | return self.quaternion_product(dq, q) 210 | 211 | def quaternion_difference(self, q1, q2): 212 | """computes the tangent vector from q1 to q2 at Identity 213 | returns vecotr w 214 | s.t. q2 = exp(.5 * w)*q1 215 | """ 216 | # first compute dq s.t. q2 = q1*dq 217 | q1conjugate = np.array([-q1[0], -q1[1], -q1[2], q1[3]]) 218 | # order of multiplication is very essential here 219 | dq = self.quaternion_product(q2, q1conjugate) 220 | # increment is log of dq 221 | return self.log_quaternion(dq) 222 | -------------------------------------------------------------------------------- /python/reactive_planners/dcm_reactive_stepper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ @namespace Controller using the dcm_vrp_planner. 4 | @file 5 | @copyright Copyright (c) 2017-2019, 6 | New York University and Max Planck Gesellschaft, 7 | License BSD-3-Clause 8 | """ 9 | 10 | import numpy as np 11 | from reactive_planners_cpp import ( 12 | StepperHead, 13 | DcmVrpPlanner, 14 | EndEffectorTrajectory3D, 15 | ) 16 | 17 | 18 | class DcmReactiveStepper(object): 19 | def __init__( 20 | self, 21 | is_left_leg_in_contact, 22 | l_min, 23 | l_max, 24 | w_min, 25 | w_max, 26 | t_min, 27 | t_max, 28 | l_p, 29 | com_height, 30 | weight, 31 | mid_air_foot_height, 32 | control_period, 33 | previous_support_foot=None, 34 | current_support_foot=None, 35 | ): 36 | if previous_support_foot is None: 37 | previous_support_foot = np.zeros((3, 1)) 38 | previous_support_foot[:] = [[0.0], [0.0], [0.0]] 39 | if current_support_foot is None: 40 | current_support_foot = np.zeros((3, 1)) 41 | current_support_foot[:] = [[0.0], [0.0], [0.0]] 42 | self.control_period = control_period 43 | # Create the stepper head. 44 | self.stepper_head = StepperHead() 45 | self.previous_support_foot = np.zeros((3, 1)) 46 | self.current_support_foot = np.zeros((3, 1)) 47 | self.previous_support_foot[:] = previous_support_foot 48 | self.current_support_foot[:] = current_support_foot 49 | self.stepper_head.set_support_feet_pos( 50 | self.previous_support_foot, self.current_support_foot 51 | ) 52 | # Create the dcm vrp planner and initialize it. 53 | self.dcm_vrp_planner = DcmVrpPlanner() 54 | self.dcm_vrp_planner.initialize( 55 | l_min, l_max, w_min, w_max, t_min, t_max, l_p, com_height, weight 56 | ) 57 | # Create the end-effector trajecotry generator. 58 | self.end_eff_traj3d = EndEffectorTrajectory3D() 59 | self.end_eff_traj3d.set_mid_air_height(mid_air_foot_height) 60 | 61 | # Parameters 62 | self.is_left_leg_in_contact = is_left_leg_in_contact 63 | self.duration_before_step_landing = 0.0 64 | self.time_from_last_step_touchdown = 0.0 65 | self.des_com_vel = np.zeros((3, 1)) 66 | self.right_foot_position = np.zeros((3, 1)) 67 | self.right_foot_position[:] = previous_support_foot 68 | self.right_foot_velocity = np.zeros((3, 1)) 69 | self.right_foot_acceleration = np.zeros((3, 1)) 70 | self.flying_foot_position = np.zeros((3, 1)) 71 | self.flying_foot_position[:] = previous_support_foot 72 | self.left_foot_position = np.zeros((3, 1)) 73 | self.left_foot_position[:] = current_support_foot 74 | self.left_foot_velocity = np.zeros((3, 1)) 75 | self.left_foot_acceleration = np.zeros((3, 1)) 76 | self.feasible_velocity = np.zeros((3, 1)) 77 | 78 | def set_end_eff_traj_costs( 79 | self, cost_x, cost_y, cost_z, hess_regularization 80 | ): 81 | self.end_eff_traj3d.set_costs( 82 | cost_x, cost_y, cost_z, hess_regularization 83 | ) 84 | 85 | def set_des_com_vel(self, des_com_vel): 86 | self.des_com_vel = des_com_vel 87 | 88 | def run( 89 | self, 90 | time, 91 | current_flying_foot_position, 92 | current_support_foot_position, 93 | com_position, 94 | com_velocity, 95 | base_yaw, 96 | contact, 97 | ): 98 | if current_support_foot_position is not None: 99 | self.stepper_head.set_support_feet_pos( 100 | self.stepper_head.get_previous_support_location(), 101 | current_support_foot_position, 102 | ) 103 | if not contact[0] and not contact[1]: 104 | self.stepper_head.run( 105 | self.duration_before_step_landing, 106 | current_flying_foot_position, 107 | time, 108 | ) 109 | 110 | elif self.is_left_leg_in_contact: 111 | self.stepper_head.run( 112 | self.duration_before_step_landing, 113 | current_flying_foot_position, 114 | time, 115 | contact[1], 116 | ) 117 | else: 118 | self.stepper_head.run( 119 | self.duration_before_step_landing, 120 | current_flying_foot_position, 121 | time, 122 | contact[0], 123 | ) 124 | 125 | self.time_from_last_step_touchdown = ( 126 | self.stepper_head.get_time_from_last_step_touchdown() 127 | ) 128 | self.current_support_foot[ 129 | : 130 | ] = self.stepper_head.get_current_support_location().reshape((3, 1)) 131 | self.previous_support_foot[ 132 | : 133 | ] = self.stepper_head.get_previous_support_location().reshape((3, 1)) 134 | self.dcm_vrp_planner.update( 135 | self.stepper_head.get_current_support_location(), 136 | self.stepper_head.get_time_from_last_step_touchdown(), 137 | self.stepper_head.get_is_left_leg_in_contact(), 138 | self.des_com_vel, 139 | com_position, 140 | com_velocity, 141 | base_yaw, 142 | ) 143 | assert self.dcm_vrp_planner.solve() 144 | 145 | self.duration_before_step_landing = ( 146 | self.dcm_vrp_planner.get_duration_before_step_landing() 147 | ) 148 | start_time = 0.0 149 | current_time = self.stepper_head.get_time_from_last_step_touchdown() 150 | end_time = self.dcm_vrp_planner.get_duration_before_step_landing() 151 | self.is_left_leg_in_contact = ( 152 | self.stepper_head.get_is_left_leg_in_contact() 153 | ) 154 | # check which foot is in contact 155 | if self.stepper_head.get_is_left_leg_in_contact(): 156 | # flying foot is the right foot 157 | self.end_eff_traj3d.compute( 158 | self.stepper_head.get_previous_support_location(), 159 | self.right_foot_position, 160 | self.right_foot_velocity, 161 | self.right_foot_acceleration, 162 | self.dcm_vrp_planner.get_next_step_location(), 163 | start_time, 164 | current_time, 165 | end_time, 166 | ) 167 | 168 | self.end_eff_traj3d.get_next_state( 169 | current_time + self.control_period, 170 | self.right_foot_position, 171 | self.right_foot_velocity, 172 | self.right_foot_acceleration, 173 | ) 174 | self.flying_foot_position = self.right_foot_position 175 | # The current support foot does not move 176 | self.left_foot_position[:] = ( 177 | self.stepper_head.get_current_support_location() 178 | ).reshape((3, 1)) 179 | self.left_foot_velocity = np.zeros((3, 1)) 180 | self.left_foot_acceleration = np.zeros((3, 1)) 181 | else: 182 | # flying foot is the left foot 183 | self.end_eff_traj3d.compute( 184 | self.stepper_head.get_previous_support_location(), 185 | self.left_foot_position, 186 | self.left_foot_velocity, 187 | self.left_foot_acceleration, 188 | self.dcm_vrp_planner.get_next_step_location(), 189 | start_time, 190 | current_time, 191 | end_time, 192 | ) 193 | 194 | self.end_eff_traj3d.get_next_state( 195 | current_time + self.control_period, 196 | self.left_foot_position, 197 | self.left_foot_velocity, 198 | self.left_foot_acceleration, 199 | ) 200 | self.flying_foot_position = self.left_foot_position 201 | # The current support foot does not move 202 | self.right_foot_position[:] = ( 203 | self.stepper_head.get_current_support_location() 204 | ).reshape((3, 1)) 205 | self.right_foot_velocity = np.zeros((3, 1)) 206 | self.right_foot_acceleration = np.zeros((3, 1)) 207 | 208 | # Compute the feasible velocity. 209 | self.feasible_velocity = ( 210 | self.dcm_vrp_planner.get_next_step_location() 211 | - self.stepper_head.get_previous_support_location() 212 | ) 213 | self.feasible_velocity[2] = 0.0 214 | 215 | return 216 | 217 | 218 | if __name__ == "__main__": 219 | 220 | is_left_leg_in_contact = True 221 | l_min = -0.5 222 | l_max = 0.5 223 | w_min = -0.5 224 | w_max = 0.5 225 | t_min = 0.1 226 | t_max = 0.2 227 | l_p = 0.1235 * 2 228 | com_height = 0.26487417 229 | weight = [1, 1, 5, 100, 100, 100, 100, 100, 100] 230 | mid_air_foot_height = 0.05 231 | control_period = 0.001 232 | dcm_reactive_stepper = DcmReactiveStepper( 233 | is_left_leg_in_contact, 234 | l_min, 235 | l_max, 236 | w_min, 237 | w_max, 238 | t_min, 239 | t_max, 240 | l_p, 241 | com_height, 242 | weight, 243 | mid_air_foot_height, 244 | control_period, 245 | ) 246 | 247 | time = 0.0 248 | current_support_foot = np.array([0, 0, 0]) 249 | com_position = np.array([0, 0, com_height]) 250 | com_velocity = np.array([0, 0, 0]) 251 | base_yaw = 0.0 252 | 253 | dcm_reactive_stepper.run( 254 | time, current_support_foot, com_position, com_velocity, base_yaw 255 | ) 256 | 257 | dcm_reactive_stepper.right_foot_position 258 | dcm_reactive_stepper.right_foot_velocity 259 | dcm_reactive_stepper.right_foot_acceleration 260 | dcm_reactive_stepper.left_foot_position 261 | dcm_reactive_stepper.left_foot_velocity 262 | dcm_reactive_stepper.left_foot_acceleration 263 | -------------------------------------------------------------------------------- /python/reactive_planners/dcm_vrp_planner/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/machines-in-motion/reactive_planners/d6d72a71ab6d71836eb5700b81f7071fac668ec5/python/reactive_planners/dcm_vrp_planner/__init__.py -------------------------------------------------------------------------------- /python/reactive_planners/dcm_vrp_planner/re_split_dcm_planner.py: -------------------------------------------------------------------------------- 1 | ## Implementation of the dcm planner 2 | ## Author : Avadesh Meduri 3 | ## Date : 20/11/ 2019 4 | 5 | import numpy as np 6 | from gurobipy import * 7 | from reactive_planners.utils.qp_solver import quadprog_solve_qp 8 | 9 | 10 | class DCMStepPlanner: 11 | def __init__(self, l_min, l_max, w_min, w_max, t_min, t_max, l_p, ht): 12 | 13 | self.l_min = l_min 14 | self.l_max = l_max 15 | self.w_min = w_min 16 | self.w_max = w_max 17 | self.t_min = t_min 18 | self.t_max = t_max 19 | self.ht = ht 20 | self.l_p = l_p 21 | self.omega = np.sqrt(9.81 / self.ht) 22 | self.bx_max = self.l_max / ( 23 | np.power(np.e, self.omega * self.t_min) - 1 24 | ) 25 | self.bx_min = self.l_min / ( 26 | np.power(np.e, self.omega * self.t_max) - 1 27 | ) 28 | self.by_max_out = ( 29 | self.l_p / (1 + np.power(np.e, self.omega * t_min)) 30 | ) + ( 31 | self.w_max - self.w_min * np.power(np.e, self.omega * self.t_min) 32 | ) / ( 33 | 1 - np.power(np.e, 2 * self.omega * self.t_min) 34 | ) 35 | 36 | self.by_max_in = ( 37 | self.l_p / (1 + np.power(np.e, self.omega * t_min)) 38 | ) + ( 39 | self.w_min - self.w_max * np.power(np.e, self.omega * self.t_min) 40 | ) / ( 41 | 1 - np.power(np.e, 2 * self.omega * self.t_min) 42 | ) 43 | 44 | def compute_nominal_values(self, n, v_des): 45 | 46 | if v_des[0] == 0 or v_des[1] == 0: 47 | B_l = self.t_min 48 | B_u = self.t_max 49 | else: 50 | B_l = np.max( 51 | [ 52 | self.l_min / abs(v_des[0]), 53 | self.w_min / abs(v_des[1]), 54 | self.t_min, 55 | ] 56 | ) 57 | B_u = np.min( 58 | [ 59 | self.l_max / abs(v_des[0]), 60 | self.w_max / abs(v_des[1]), 61 | self.t_max, 62 | ] 63 | ) 64 | 65 | t_nom = (B_l + B_u) / 2.0 66 | t_nom = np.power( 67 | np.e, self.omega * t_nom 68 | ) ### take exp as T is considered as e^wt in qp 69 | 70 | l_nom = v_des[0] * t_nom 71 | w_nom = v_des[1] * t_nom 72 | 73 | bx_nom = l_nom / (np.power(np.e, self.omega * t_nom) - 1) 74 | by_nom = ( 75 | np.power(-1, n) 76 | * (self.l_p / (1 + np.power(np.e, self.omega * t_nom))) 77 | ) - w_nom / (1 - np.power(np.e, self.omega * t_nom)) 78 | 79 | return l_nom, w_nom, t_nom, bx_nom, by_nom 80 | 81 | def compute_adapted_step_location(self, u, t, n, psi, W, v_des): 82 | 83 | l_nom, w_nom, t_nom, bx_nom, by_nom = self.compute_nominal_values( 84 | n, v_des 85 | ) 86 | 87 | P = np.identity(5) ## quadratic cost matrix 88 | P[0][0], P[1][1], P[2][2], P[3][3], P[4][4] = W 89 | q = np.array( 90 | [ 91 | -W[0] * (l_nom), 92 | -W[1] * (w_nom), 93 | -W[2] * t_nom, 94 | -W[3] * bx_nom, 95 | -W[4] * by_nom, 96 | ] 97 | ) ## 98 | 99 | G = np.matrix( 100 | [ 101 | [1, 0, 0, 0, 0], 102 | [0, 1, 0, 0, 0], 103 | [-1, 0, 0, 0, 0], 104 | [0, -1, 0, 0, 0], 105 | [0, 0, 1, 0, 0], 106 | [0, 0, -1, 0, 0], 107 | [0, 0, 0, 1, 0], 108 | [0, 0, 0, -1, 0], 109 | [0, 0, 0, 0, 1], 110 | [0, 0, 0, 0, -1], 111 | ] 112 | ) 113 | 114 | h = np.array( 115 | [ 116 | self.l_max, 117 | self.w_max, 118 | -1 * (self.l_min), 119 | -1 * (self.w_min), 120 | np.power(np.e, self.omega * self.t_max), 121 | -1 * np.power(np.e, self.omega * self.t_min), 122 | self.bx_max, 123 | -1 * self.bx_min, 124 | self.by_max_in, 125 | -1 * self.by_max_out, 126 | ] 127 | ) 128 | 129 | tmp = [0.0, 0.0] 130 | tmp[0] = (psi[0] - u[0]) * np.power(np.e, -1 * self.omega * t) 131 | tmp[1] = (psi[1] - u[1]) * np.power(np.e, -1 * self.omega * t) 132 | A = np.matrix([[1, 0, -1 * tmp[0], 1, 0], [0, 1, -1 * tmp[1], 0, 1]]) 133 | 134 | b = np.array([0.0, 0.0]) 135 | # b = np.array([u[0], u[1]]) 136 | 137 | P = P.astype(float) 138 | q = q.astype(float) 139 | G = G.astype(float) 140 | h = h.astype(float) 141 | A = A.astype(float) 142 | b = b.astype(float) 143 | 144 | try: 145 | self.x_opt = quadprog_solve_qp(P, q, G, h, A, b) 146 | except ValueError as err: 147 | import pdb 148 | 149 | pdb.set_trace() 150 | 151 | t_end = np.log(self.x_opt[2]) / self.omega 152 | 153 | return self.x_opt[0] + u[0], self.x_opt[1] + u[1], t_end 154 | 155 | def compute_adapted_step_location_gurobi(self, u1, t1, n1, psi, W, v_des): 156 | 157 | l_nom1, w_nom1, t_nom1, bx_nom1, by_nom1 = self.compute_nominal_values( 158 | n1, v_des 159 | ) 160 | 161 | m = Model("split_dcm") 162 | 163 | # Creating Variables 164 | ut1_x = m.addVar( 165 | lb=self.l_min + u1[0], ub=self.l_max + u1[0], name="ut1_x" 166 | ) 167 | ut1_y = m.addVar( 168 | lb=self.w_min + u1[1], ub=self.w_max + u1[1], name="ut1_y" 169 | ) 170 | t1_step = m.addVar( 171 | lb=np.power(np.e, self.omega * self.t_min), 172 | ub=np.power(np.e, self.omega * self.t_max), 173 | name="t1_step", 174 | ) 175 | b1_x = m.addVar(lb=self.bx_min, ub=self.bx_max, name="b1_x") 176 | b1_y = m.addVar(lb=self.by_max_out, ub=self.by_max_in, name="b1_y") 177 | 178 | # Creating Cost 179 | c1 = ( 180 | W[0] * (ut1_x - u1[0] - l_nom1) * (ut1_x - u1[0] - l_nom1) 181 | + W[1] * (ut1_y - u1[1] - w_nom1) * (ut1_y - u1[1] - w_nom1) 182 | + W[2] * (t1_step - t_nom1) * (t1_step - t_nom1) 183 | + W[3] * (b1_x - bx_nom1) * (b1_x - bx_nom1) 184 | + W[4] * (b1_y - by_nom1) * (b1_y - by_nom1) 185 | ) 186 | 187 | m.setObjective(c1) 188 | 189 | ## Constraints 190 | tmp1 = np.subtract(psi[0:2], u1) * np.power(np.e, -1 * self.omega * t1) 191 | m.addConstr(ut1_x == tmp1[0] * t1_step + u1[0] - b1_x, "dyn_1_x") 192 | m.addConstr(ut1_y == tmp1[1] * t1_step + u1[1] - b1_y, "dyn_1_y") 193 | 194 | m.optimize() 195 | 196 | x_opt = m.getVars() 197 | 198 | t1_end = np.log(x_opt[2].x) / self.omega 199 | 200 | return x_opt[0].x, x_opt[1].x, t1_end 201 | 202 | def compute_viability_boundary(self, u, t, psi): 203 | """ 204 | copmute the maximum and minimum step location considering ll above constraints 205 | Input: 206 | u: current step location 207 | t: current step time 208 | psi: current dcm location 209 | """ 210 | 211 | u_max = np.subtract(psi[0:2], u) * np.power( 212 | np.e, -1 * self.omega * t 213 | ) * np.power(np.e, self.omega * self.t_max) + np.subtract( 214 | u[0:2], np.array([self.bx_min, self.by_max_out]) 215 | ) 216 | u_min = np.subtract(psi[0:2], u) * np.power( 217 | np.e, -1 * self.omega * t 218 | ) * np.power(np.e, self.omega * self.t_min) + np.subtract( 219 | u[0:2], np.array([self.bx_max, self.by_max_in]) 220 | ) 221 | 222 | if u_max[0] > self.l_max + u[0]: 223 | u_max[0] = self.l_max + u[0] 224 | elif u_max[1] > self.w_max + u[1]: 225 | u_max[1] = self.w_max + u[1] 226 | 227 | if u_min[0] < self.l_min + u[0]: 228 | u_min[0] = self.l_min + u[0] 229 | elif u_min[1] < self.w_min + u[1]: 230 | u_min[1] = self.w_min + u[1] 231 | 232 | return u_max, u_min 233 | -------------------------------------------------------------------------------- /python/reactive_planners/dcm_vrp_planner/solo_step_planner.py: -------------------------------------------------------------------------------- 1 | ## This file contains code to decide where the end effectors should land to satisfy ZMP constraint 2 | ##from DCM step planner 3 | ## Author: Avadesh Meduri 4 | ## Date: 12/12/2019 5 | 6 | import numpy as np 7 | from gurobipy import * 8 | from reactive_planners.utils.solo_state_estimator import SoloStateEstimator 9 | 10 | 11 | class SoloStepPlanner: 12 | def __init__(self, robot, kin_min, kin_max, ht): 13 | """ 14 | Input: 15 | kin_min = kinematic constraint min offset from hip 16 | kin_max = kinematic constraint max offset from hip 17 | ht: the height of the base above the ground 18 | """ 19 | assert np.shape(kin_min) == (2,) 20 | assert np.shape(kin_max) == (2,) 21 | 22 | self.robot = robot 23 | self.sse = SoloStateEstimator(robot) 24 | self.kin_min = kin_min 25 | self.kin_max = kin_max 26 | self.ht = ht 27 | 28 | def compute_kinematic_constraints(self, q, dq, t, t_end, v_des, ht): 29 | """ 30 | Computes the kinematic constraints at the end of the step. 31 | Input: 32 | t: current time 33 | t_end: step time 34 | v_des: desired velocity 35 | ht: the height of the base above the ground 36 | """ 37 | hip_loc = np.array(self.sse.return_hip_locations(q, dq)) 38 | offset = np.array(v_des) * (t_end - t) 39 | for i in range(4): 40 | hip_loc[i][0:2] = np.add(hip_loc[i][0:2], offset) 41 | hip_loc[i][2] = ht 42 | 43 | return hip_loc[0], hip_loc[1], hip_loc[2], hip_loc[3] 44 | 45 | def compute_next_step_locations( 46 | self, q, dq, t, u_t, u_t_min, u_t_max, t_end, cnt_array, v_des, W 47 | ): 48 | """ 49 | Computes the next step locations depending on the decided contact plan 50 | Input: 51 | t: currrent step time 52 | u_t: next step COP location from DCM planner 53 | u_t_min: minimum value of viability boundary 54 | u_t_max: max value of viabilty boundary 55 | t_end: step time 56 | cnt_array: contact array 57 | v_des: desired velocity 58 | W: weight array 59 | """ 60 | 61 | m = Model("solo_step_planner") 62 | 63 | FL_hip, FR_hip, HL_hip, HR_hip = self.compute_kinematic_constraints( 64 | q, dq, t, t_end, v_des, self.ht 65 | ) 66 | 67 | ## Creating Variables 68 | fl_x = m.addVar( 69 | lb=FL_hip[0] + self.kin_min[0], 70 | ub=FL_hip[0] + self.kin_max[0], 71 | name="fl_x", 72 | ) 73 | fl_y = m.addVar( 74 | lb=FL_hip[1] + self.kin_min[1], 75 | ub=FL_hip[1] + self.kin_max[1], 76 | name="fl_y", 77 | ) 78 | 79 | fr_x = m.addVar( 80 | lb=FR_hip[0] + self.kin_min[0], 81 | ub=FR_hip[0] + self.kin_max[0], 82 | name="fr_x", 83 | ) 84 | fr_y = m.addVar( 85 | lb=FR_hip[1] + self.kin_min[1], 86 | ub=FR_hip[1] + self.kin_max[1], 87 | name="fr_y", 88 | ) 89 | 90 | hl_x = m.addVar( 91 | lb=HL_hip[0] + self.kin_min[0], 92 | ub=HL_hip[0] + self.kin_max[0], 93 | name="hl_x", 94 | ) 95 | hl_y = m.addVar( 96 | lb=HL_hip[1] + self.kin_min[1], 97 | ub=HL_hip[1] + self.kin_max[1], 98 | name="hl_y", 99 | ) 100 | 101 | hr_x = m.addVar( 102 | lb=HR_hip[0] + self.kin_min[0], 103 | ub=HR_hip[0] + self.kin_max[0], 104 | name="hr_x", 105 | ) 106 | hr_y = m.addVar( 107 | lb=HR_hip[1] + self.kin_min[1], 108 | ub=HR_hip[1] + self.kin_max[1], 109 | name="hr_y", 110 | ) 111 | 112 | sl_1 = m.addVar(name="slack_1") 113 | sl_2 = m.addVar(name="slack_2") 114 | sl_3 = m.addVar(name="slack_3") 115 | sl_4 = m.addVar(name="slack_4") 116 | sl_5 = m.addVar(name="slack_5") 117 | sl_6 = m.addVar(name="slack_6") 118 | sl_7 = m.addVar(name="slack_7") 119 | sl_8 = m.addVar(name="slack_8") 120 | sl_9 = m.addVar(name="slack_9") 121 | sl_10 = m.addVar(name="slack_10") 122 | 123 | # creating cost 124 | c1 = ( 125 | W[0] * sl_1 * sl_1 126 | + W[1] * sl_2 * sl_2 127 | + W[2] * sl_3 * sl_3 128 | + W[3] * sl_4 * sl_4 129 | + W[4] * sl_5 * sl_5 130 | + W[5] * sl_6 * sl_6 131 | + W[6] * sl_7 * sl_7 132 | + W[7] * sl_8 * sl_8 133 | + W[8] * sl_9 * sl_9 134 | + W[9] * sl_10 * sl_10 135 | ) 136 | 137 | m.setObjective(c1) 138 | 139 | ## constraints 140 | if cnt_array[0] == 1: 141 | # m.addConstr(fl_x >= u_t_max[0], "viability_max_constr_fl_x") 142 | # m.addConstr(fl_y >= u_t_max[1], "viability_max_constr_fl_y") 143 | # m.addConstr(fl_x <= u_t_min[0], "viability_min_constr_fl_x") 144 | # m.addConstr(fl_y <= u_t_min[1], "viability_min_constr_fl_y") 145 | 146 | m.addConstr(fl_x == FL_hip[0] + sl_3) 147 | m.addConstr(fl_y == FL_hip[1] + sl_4) 148 | 149 | if cnt_array[1] == 1: 150 | # m.addConstr(fr_x >= u_t_max[0], "viability_max_constr_fr_x") 151 | # m.addConstr(fr_y >= u_t_max[1], "viability_max_constr_fr_y") 152 | # m.addConstr(fr_x <= u_t_min[0], "viability_min_constr_fr_x") 153 | # m.addConstr(fr_y <= u_t_min[1], "viability_min_constr_fr_y") 154 | 155 | m.addConstr(fr_x == FR_hip[0] + sl_5) 156 | m.addConstr(fr_y == FR_hip[1] + sl_6) 157 | 158 | if cnt_array[2] == 1: 159 | # m.addConstr(hl_x >= u_t_max[0], "viability_max_constr_hl_x") 160 | # m.addConstr(hl_y >= u_t_max[1], "viability_max_constr_hl_y") 161 | # m.addConstr(hl_x <= u_t_min[0], "viability_min_constr_hl_x") 162 | # m.addConstr(hl_y <= u_t_min[1], "viability_min_constr_hl_y") 163 | 164 | m.addConstr(hl_x == HL_hip[0] + sl_7) 165 | m.addConstr(hl_y == HL_hip[1] + sl_8) 166 | 167 | if cnt_array[3] == 1: 168 | # m.addConstr(hr_x >= u_t_max[0], "viability_max_constr_hr_x") 169 | # m.addConstr(hr_y >= u_t_max[1], "viability_max_constr_hr_y") 170 | # m.addConstr(hr_x <= u_t_min[0], "viability_min_constr_hr_x") 171 | # m.addConstr(hr_y <= u_t_min[1], "viability_min_constr_hr_y") 172 | 173 | m.addConstr(hr_x == HR_hip[0] + sl_9) 174 | m.addConstr(hr_x == HR_hip[1] + sl_10) 175 | 176 | m.addConstr( 177 | cnt_array[0] * fl_x 178 | + cnt_array[1] * fr_x 179 | + cnt_array[2] * hl_x 180 | + cnt_array[3] * hr_x 181 | == np.sum(cnt_array) * (u_t[0]) + sl_1, 182 | "sl_constr_1", 183 | ) 184 | m.addConstr( 185 | cnt_array[0] * fl_y 186 | + cnt_array[1] * fr_y 187 | + cnt_array[2] * hl_y 188 | + cnt_array[3] * hr_y 189 | == np.sum(cnt_array) * (u_t[1]) + sl_2, 190 | "sl_constr_2", 191 | ) 192 | 193 | m.optimize() 194 | x_opt = m.getVars() 195 | 196 | return ( 197 | x_opt[0].x, 198 | x_opt[1].x, 199 | x_opt[2].x, 200 | x_opt[3].x, 201 | x_opt[4].x, 202 | x_opt[5].x, 203 | x_opt[6].x, 204 | x_opt[7].x, 205 | ) 206 | -------------------------------------------------------------------------------- /python/reactive_planners/dynamic_graph/quadruped_stepper.py: -------------------------------------------------------------------------------- 1 | """This file is a demo for using the DG whole body controller. 2 | 3 | License BSD-3-Clause 4 | Copyright (c) 2021, New York University and Max Planck Gesellschaft. 5 | 6 | Author: Julian Viereck 7 | Date: Feb 26, 2021 8 | """ 9 | 10 | import numpy as np 11 | 12 | np.set_printoptions(suppress=True, precision=3) 13 | 14 | import pinocchio as pin 15 | import dynamic_graph as dg 16 | 17 | import dynamic_graph.sot.dynamic_pinocchio as dp 18 | from dynamic_graph.sot.core.math_small_entities import ( 19 | Selec_of_matrix, 20 | Stack_of_vector, 21 | ) 22 | 23 | from mim_control.dynamic_graph.wbc_graph import WholeBodyController 24 | 25 | from reactive_planners.dynamic_graph.walking import QuadrupedDcmReactiveStepper 26 | 27 | from dg_tools.dynamic_graph.dg_tools_entities import PoseRPYToPoseQuaternion 28 | from dg_tools.utils import ( 29 | constVectorOp, 30 | subtract_vec_vec, 31 | hom2pos, 32 | add_vec_vec, 33 | stack_two_vectors, 34 | selec_vector, 35 | zero_vec, 36 | basePoseQuat2PoseRPY, 37 | multiply_mat_vec, 38 | ) 39 | 40 | from robot_properties_solo.config import Solo12Config 41 | 42 | 43 | class QuadrupedStepper: 44 | def __init__(self, prefix, pin_robot, endeff_names): 45 | self.prefix = prefix 46 | self.pin_robot = pin_robot 47 | self.endeff_names = endeff_names 48 | self.nv = pin_robot.model.nv 49 | 50 | self.dg_robot = dp.DynamicPinocchio(prefix + "_pinocchio") 51 | self.dg_robot.setModel(self.pin_robot.model) 52 | self.dg_robot.setData(self.pin_robot.data) 53 | 54 | self.sig_eff_pos = [] 55 | self.sig_eff_vel = [] 56 | 57 | # Create the objects of interests from pinocchio. 58 | self.com = self.dg_robot.signal("com") 59 | self.vcom = multiply_mat_vec( 60 | self.dg_robot.signal("Jcom"), self.dg_robot.signal("velocity") 61 | ) 62 | 63 | for endeff_name in endeff_names: 64 | self.dg_robot.createPosition("pos_" + endeff_name, endeff_name) 65 | self.dg_robot.createJacobianEndEffWorld( 66 | "jac_" + endeff_name, endeff_name 67 | ) 68 | 69 | # Store the endeffector position signal. 70 | self.sig_eff_pos.append( 71 | hom2pos(self.dg_robot.signal("pos_" + endeff_name)) 72 | ) 73 | 74 | # Compute the endeffector velocity signal. 75 | sel_linear = Selec_of_matrix(prefix + "_pinocchio_" + endeff_name) 76 | sel_linear.selecRows(0, 3) 77 | sel_linear.selecCols(0, self.nv + 6) 78 | dg.plug(self.dg_robot.signal("jac_" + endeff_name), sel_linear.sin) 79 | 80 | self.sig_eff_vel.append( 81 | multiply_mat_vec( 82 | sel_linear.sout, self.dg_robot.signal("velocity") 83 | ) 84 | ) 85 | 86 | ### 87 | # Create the actual stepper object. 88 | self.stepper = QuadrupedDcmReactiveStepper( 89 | prefix + "_quadruped_stepper" 90 | ) 91 | 92 | # Setup the pinocchio input quantities for the stepper. 93 | dg.plug(self.com, self.stepper.com_position_sin) 94 | dg.plug(self.vcom, self.stepper.com_velocity_sin) 95 | 96 | dg.plug( 97 | self.sig_eff_pos[0], 98 | self.stepper.current_front_left_foot_position_sin, 99 | ) 100 | dg.plug( 101 | self.sig_eff_vel[0], 102 | self.stepper.current_front_left_foot_velocity_sin, 103 | ) 104 | dg.plug( 105 | self.sig_eff_pos[1], 106 | self.stepper.current_front_right_foot_position_sin, 107 | ) 108 | dg.plug( 109 | self.sig_eff_vel[1], 110 | self.stepper.current_front_right_foot_velocity_sin, 111 | ) 112 | dg.plug( 113 | self.sig_eff_pos[2], 114 | self.stepper.current_hind_left_foot_position_sin, 115 | ) 116 | dg.plug( 117 | self.sig_eff_vel[2], 118 | self.stepper.current_hind_left_foot_velocity_sin, 119 | ) 120 | dg.plug( 121 | self.sig_eff_pos[3], 122 | self.stepper.current_hind_right_foot_position_sin, 123 | ) 124 | dg.plug( 125 | self.sig_eff_vel[3], 126 | self.stepper.current_hind_right_foot_velocity_sin, 127 | ) 128 | 129 | self.stepper.is_closed_loop_sin.value = 0.0 130 | 131 | def start(self): 132 | self.stepper.start() 133 | 134 | def stop(self): 135 | self.stepper.stop() 136 | 137 | def plug(self, robot, base_position, base_velocity): 138 | # Args: 139 | # robot; DGM robot device 140 | # base_position: The base position as a 7 dim vector signal 141 | # base_velocity: The base velocity as a 6 dim vector signal 142 | 143 | ### 144 | # Plug the pinocchio entity. 145 | base_pose_rpy = basePoseQuat2PoseRPY(base_position) 146 | position = stack_two_vectors( 147 | base_pose_rpy, robot.device.joint_positions, 6, self.nv - 6 148 | ) 149 | velocity = stack_two_vectors( 150 | base_velocity, robot.device.joint_velocities, 6, self.nv - 6 151 | ) 152 | 153 | dg.plug(position, self.dg_robot.signal("position")) 154 | dg.plug(velocity, self.dg_robot.signal("velocity")) 155 | self.dg_robot.signal("acceleration").value = np.array( 156 | self.nv 157 | * [ 158 | 0.0, 159 | ] 160 | ) 161 | 162 | ### 163 | # Plug the stepper base position. 164 | dg.plug(base_position, self.stepper.xyzquat_base_sin) 165 | 166 | 167 | -------------------------------------------------------------------------------- /python/reactive_planners/lipm_simulator.py: -------------------------------------------------------------------------------- 1 | ## @namespace reactive_planners.LipmSimulator 2 | """ 3 | @author Elham Daneshmand (exledh@gmail.com) 4 | @copyright Copyright (c) 2020, 5 | New York University and Max Planck Gesellschaft, 6 | License BSD-3-Clause 7 | """ 8 | 9 | import numpy as np 10 | 11 | 12 | class LipmSimpulator: 13 | def __init__(self, com_height): 14 | self.omega = np.sqrt(9.8 / com_height) 15 | self.last_x_com = np.zeros((3, 1)) 16 | self.last_x_com[:] = [[0], [0], [com_height]] 17 | self.last_xd_com = np.zeros((3, 1)) 18 | self.x_com = np.zeros((3, 1)) 19 | self.xd_com = np.zeros((3, 1)) 20 | self.xdd_com = np.zeros((3, 1)) 21 | self.u_current_step = np.zeros((3, 1)) 22 | self.xdd_com = np.zeros((3, 1)) 23 | 24 | def step(self, time, u_current_step, previous_x_com, previous_xd_com): 25 | if time == 0.0: 26 | self.last_x_com[:] = previous_x_com 27 | self.last_xd_com[:] = previous_xd_com 28 | self.u_current_step[:] = u_current_step 29 | 30 | self.x_com[:] = ( 31 | ( 32 | 0.5 33 | * ( 34 | self.last_x_com 35 | - self.u_current_step 36 | + (self.last_xd_com / self.omega) 37 | ) 38 | * pow(np.e, (self.omega * time)) 39 | ) 40 | + ( 41 | 0.5 42 | * ( 43 | self.last_x_com 44 | - self.u_current_step 45 | - (self.last_xd_com / self.omega) 46 | ) 47 | * pow(np.e, (-self.omega * time)) 48 | ) 49 | + self.u_current_step 50 | ) 51 | self.xd_com[:] = ( 52 | self.omega 53 | * 0.5 54 | * ( 55 | self.last_x_com 56 | - self.u_current_step 57 | + (self.last_xd_com / self.omega) 58 | ) 59 | * pow(np.e, (self.omega * time)) 60 | ) - ( 61 | self.omega 62 | * 0.5 63 | * ( 64 | self.last_x_com 65 | - self.u_current_step 66 | - (self.last_xd_com / self.omega) 67 | ) 68 | * pow(np.e, (-self.omega * time)) 69 | ) 70 | self.xdd_com[:] = (self.omega ** 2) * ( 71 | self.last_x_com - self.u_current_step 72 | ) 73 | 74 | self.x_com[2] = self.last_x_com[2] 75 | self.xd_com[2] = 0 76 | self.xdd_com[2] = 0 77 | return self.x_com, self.xd_com, self.xdd_com 78 | -------------------------------------------------------------------------------- /python/reactive_planners/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/machines-in-motion/reactive_planners/d6d72a71ab6d71836eb5700b81f7071fac668ec5/python/reactive_planners/utils/__init__.py -------------------------------------------------------------------------------- /python/reactive_planners/utils/qp_solver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2016-2018 Stephane Caron 5 | # 6 | # This file is part of qpsolvers. 7 | # 8 | # qpsolvers is free software: you can redistribute it and/or modify it under 9 | # the terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # qpsolvers is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with qpsolvers. If not, see . 20 | 21 | from numpy import hstack, vstack 22 | from quadprog import solve_qp 23 | 24 | 25 | def quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None): 26 | """ 27 | Solve a Quadratic Program defined as: 28 | minimize 29 | (1/2) * x.T * P * x + q.T * x 30 | subject to 31 | G * x <= h 32 | A * x == b 33 | using quadprog . 34 | Parameters 35 | ---------- 36 | P : numpy.array 37 | Symmetric quadratic-cost matrix. 38 | q : numpy.array 39 | Quadratic-cost vector. 40 | G : numpy.array 41 | Linear inequality constraint matrix. 42 | h : numpy.array 43 | Linear inequality constraint vector. 44 | A : numpy.array, optional 45 | Linear equality constraint matrix. 46 | b : numpy.array, optional 47 | Linear equality constraint vector. 48 | initvals : numpy.array, optional 49 | Warm-start guess vector (not used). 50 | Returns 51 | ------- 52 | x : numpy.array 53 | Solution to the QP, if found, otherwise ``None``. 54 | Note 55 | ---- 56 | The quadprog solver only considers the lower entries of `P`, therefore it 57 | will use a wrong cost function if a non-symmetric matrix is provided. 58 | """ 59 | if initvals is not None: 60 | print("quadprog: note that warm-start values ignored by wrapper") 61 | qp_G = P 62 | qp_a = -q 63 | if A is not None: 64 | qp_C = -vstack([A, G]).T 65 | qp_b = -hstack([b, h]) 66 | meq = A.shape[0] 67 | else: # no equality constraint 68 | qp_C = -G.T 69 | qp_b = -h 70 | meq = 0 71 | return solve_qp(qp_G, qp_a, qp_C, qp_b, meq)[0] 72 | -------------------------------------------------------------------------------- /python/reactive_planners/utils/solo_state_estimator.py: -------------------------------------------------------------------------------- 1 | ## Contains functions that determine the current location of various frames 2 | ## of solo robot 3 | ## Author: Avadesh Meduri 4 | ## Date: 9/12/2019 5 | 6 | import pinocchio as pin 7 | import numpy as np 8 | 9 | 10 | class SoloStateEstimator: 11 | def __init__(self, robot): 12 | self.robot = robot 13 | self.x = np.zeros(3) 14 | self.xd = np.zeros(3) 15 | self.robot_mass = sum([i.mass for i in robot.model.inertias[1:]]) 16 | 17 | def get_frame_location(self, q, dq, frame_idx): 18 | """ 19 | returns the global location of the frame 20 | """ 21 | self.robot.framesForwardKinematics(q) 22 | return np.reshape( 23 | np.array(self.robot.data.oMf[frame_idx].translation), (3,) 24 | ) 25 | 26 | def return_com_location(self, q, dq): 27 | """ 28 | returns com location 29 | """ 30 | return np.reshape(np.array(q[0:3]), (3,)) 31 | 32 | def return_com_velocity(self, q, dq): 33 | """ 34 | returns com velocity 35 | """ 36 | return np.reshape(np.array(dq[0:3]), (3,)) 37 | 38 | def return_foot_locations(self, q, dq): 39 | """ 40 | returns current foot location of the solo. 41 | """ 42 | 43 | fl_foot = self.get_frame_location( 44 | q, dq, self.robot.model.getFrameId("FL_FOOT") 45 | ) 46 | fr_foot = self.get_frame_location( 47 | q, dq, self.robot.model.getFrameId("FR_FOOT") 48 | ) 49 | hl_foot = self.get_frame_location( 50 | q, dq, self.robot.model.getFrameId("HL_FOOT") 51 | ) 52 | hr_foot = self.get_frame_location( 53 | q, dq, self.robot.model.getFrameId("HR_FOOT") 54 | ) 55 | 56 | return fl_foot, fr_foot, hl_foot, hr_foot 57 | 58 | def return_hip_locations(self, q, dq): 59 | """ 60 | returns current hip locations of solo 61 | """ 62 | fl_hip = self.get_frame_location( 63 | q, dq, self.robot.model.getFrameId("FL_HFE") 64 | ) 65 | fr_hip = self.get_frame_location( 66 | q, dq, self.robot.model.getFrameId("FR_HFE") 67 | ) 68 | hl_hip = self.get_frame_location( 69 | q, dq, self.robot.model.getFrameId("HL_HFE") 70 | ) 71 | hr_hip = self.get_frame_location( 72 | q, dq, self.robot.model.getFrameId("HR_HFE") 73 | ) 74 | 75 | return fl_hip, fr_hip, hl_hip, hr_hip 76 | 77 | def return_dcm_location(self, q, dq, omega): 78 | """ 79 | returns current divergent component of motion location of solo 80 | """ 81 | self.x = np.reshape(np.array(q[0:3]), (3,)) 82 | self.xd = np.reshape(np.array(dq[0:3]), (3,)) 83 | return self.x + self.xd / omega 84 | 85 | def return_hip_offset(self, q, dq): 86 | """ 87 | Returns the distance between the hip and the center of mass 88 | """ 89 | fl_hip, fr_hip, hl_hip, hr_hip = self.return_hip_locations(q, dq) 90 | com = np.reshape(np.array(q[0:3]), (3,)) 91 | return ( 92 | np.subtract(fl_hip, com), 93 | np.subtract(fr_hip, com), 94 | np.subtract(hl_hip, com), 95 | np.subtract(hr_hip, com), 96 | ) 97 | 98 | def return_zmp_location(self, q, dq, cnt_array): 99 | """ 100 | returns the current location of the zmp 101 | """ 102 | foot_loc = self.return_foot_locations( 103 | q, dq 104 | ) ## computing current location of the feet 105 | zmp = np.zeros(2) 106 | for i in range(len(cnt_array)): 107 | if cnt_array[i] == 0: 108 | continue 109 | zmp = np.add(zmp, foot_loc[i][0:2]) 110 | 111 | zmp = np.divide(zmp, sum(cnt_array)) 112 | 113 | return zmp 114 | -------------------------------------------------------------------------------- /python/reactive_planners/utils/terrains/stairs.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 120 | -------------------------------------------------------------------------------- /python/reactive_planners/utils/trajectory_generator.py: -------------------------------------------------------------------------------- 1 | ## Contains functions to generate trajectories. Not specific to any robot. 2 | ## Author: Avadesh Meduri 3 | ## Date: 9/12/2019 4 | 5 | import numpy as np 6 | 7 | 8 | class TrajGenerator: 9 | def __init__(self, robot): 10 | 11 | self.robot = robot 12 | 13 | def get_frame_location(self, q, dq, frame_idx): 14 | """ 15 | returns the global location of the frame by computing forward kinematics 16 | Input: 17 | q: current joint configuration of the robot 18 | dq: current joint velocity of the robot 19 | """ 20 | 21 | self.robot.framesForwardKinematics(q) 22 | return np.reshape( 23 | np.array(self.robot.model.oMf[frame_idx].translation), (3,) 24 | ) 25 | 26 | def generate_traj(self, start, end, traj_time, t): 27 | """ 28 | returns desired location at a given time t, 29 | Generates a straight line start to end point(uniform velocity motion) 30 | Input: 31 | start: Start point of the trajectory. 32 | end: End point of the trajecory. 33 | traj_time: Total trajectory time 34 | t: Current time step 35 | """ 36 | 37 | slope = np.divide(np.subtract(end, start), traj_time) 38 | 39 | return np.add(start, slope * t) 40 | 41 | def generate_sin_traj(self, start, end, via_point, traj_time, t): 42 | """ 43 | returns desired location at a given time t, 44 | psi = sse.return_dcm_location(q, dq, step_planner.omega) 45 | Generates sine trajectory from start to end point through a via point 46 | The trajectory passes through the via point at mid duration 47 | Input: 48 | start: Start point of the trajectory. 49 | end: End point of the trajecory. 50 | via_point: The point through which trajectory will go through mid trajecory 51 | traj_time: Total trajectory time 52 | t: Current time step 53 | """ 54 | assert np.shape(start) == np.shape(end) 55 | assert np.shape(start) == np.shape(via_point) 56 | 57 | if t < traj_time / 2.0: 58 | amplitude = np.subtract(via_point, start) 59 | omega = (np.pi) / traj_time 60 | return np.add(start, amplitude * np.sin(omega * t)) 61 | elif t == traj_time / 2.0: 62 | return via_point 63 | else: 64 | amplitude = np.subtract(via_point, end) 65 | omega = (np.pi) / traj_time 66 | return np.add(end, amplitude * np.sin(omega * t)) 67 | 68 | def generate_foot_traj(self, start, end, via_point, traj_time, t): 69 | """ 70 | Generates foot trajectory for walking. A uniform velocity is tracked in x,y direction. 71 | A sine trajectory is generated in the z direction. 72 | Input: 73 | Start: Current location of the foot 3d 74 | end: desired location of the of the foot at the end of the step 75 | via_point: the hieght in the z axis the leg has to rise 76 | traj_time: step time 77 | t: current time 78 | """ 79 | 80 | assert np.shape(start) == (3,) 81 | assert np.shape(end) == (3,) 82 | 83 | x_des = np.zeros(3) 84 | x_des[0:2] = self.generate_traj(start[0:2], end[0:2], traj_time, t) 85 | x_des[2] = self.generate_sin_traj( 86 | start[2], end[2], via_point[2], traj_time, t 87 | ) 88 | 89 | return x_des 90 | -------------------------------------------------------------------------------- /python/reactive_planners/utils/utils.py: -------------------------------------------------------------------------------- 1 | ### This file contains utility functions for the planner 2 | ## Author : Avadesh Meduri 3 | ## Date : 7/11/2019 4 | 5 | import numpy as np 6 | 7 | from urdf_parser_py.urdf import URDF 8 | 9 | 10 | def create_terrain_constraints(file_dir): 11 | """ 12 | creates the contraints for the stepping QP based on the terrain details in the urdf 13 | Gives details of where the next step location can exist 14 | returns the constraints as numpy matrix. 15 | 16 | Input : 17 | file_name : name of the urdf file that describes the terrain 18 | """ 19 | 20 | terrain = URDF.from_xml_file(file_dir) 21 | # print(terrain.links[0].visuals[0].geometry) 22 | 23 | b = [] 24 | link_no = 0 25 | for link in terrain.links: 26 | name = link.name 27 | # Note: The assumption with origin is that everything is in the global frame. 28 | origin_xyz = np.array(link.visual.origin.xyz) 29 | geometry = np.array(link.visual.geometry.size) 30 | 31 | for i in range(2): 32 | b.append(origin_xyz[i] + geometry[i] / 2.0 - 0.05) 33 | b.append(origin_xyz[i] - geometry[i] / 2.0 + 0.05) 34 | 35 | b.append( 36 | origin_xyz[2] + 0.5 * geometry[2] + 0.001 37 | ) ## ground constraint 38 | b.append( 39 | origin_xyz[2] + 0.5 * geometry[2] - 0.001 40 | ) ## ground constraint 41 | 42 | link_no += 1 43 | 44 | return b 45 | 46 | 47 | # terrain_dir = './terrains/stairs.urdf' 48 | # G, b = create_terrain_constraints(terrain_dir) 49 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | [![CodeFactor](https://www.codefactor.io/repository/github/machines-in-motion/reactive_planners/badge/master?s=2265cf35a56607421790341030c3b894f59b1c28)](https://www.codefactor.io/repository/github/machines-in-motion/reactive_planners/overview/master) 2 | 3 | Readme 4 | ------ 5 | 6 | Contains a list of reactive planners specialized in locomotion of legged robots. The reactive planner adapts the step location and timing of the gait based on feedbck from the CoM states and sends the desired swing foot trajectories to an instantanous controller for tracking. 7 | 8 | ### Installation 9 | 10 | #### Standard dependencies 11 | 12 | *Here all the pip and apt install-able stuff* 13 | 14 | #### Download the package 15 | 16 | Install 17 | [treep](https://gitlab.is.tue.mpg.de/amd-clmc/treep) 18 | and 19 | [colcon](https://github.com/machines-in-motion/machines-in-motion.github.io/wiki/use_colcon) 20 | . 21 | 22 | 23 | #### Build the package 24 | 25 | 26 | Then follow the instructions below: 27 | ```bash 28 | # install treep and colcon 29 | pip install -U treep colcon-common-extensions 30 | # change directory to your devel folder 31 | mkdir devel 32 | cd devel 33 | # Clone the treep configuration 34 | git clone https://github.com/machines-in-motion/treep_machines_in_motion.git 35 | # Clone the code base 36 | treep --clone REACTIVE_PLANNERS 37 | # go and build the code 38 | cd workspace 39 | colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release 40 | # source the environment for using the code 41 | source install/setup.bash 42 | ``` 43 | 44 | ### Usage 45 | 46 | #### Demos/Examples 47 | 48 | To run Bolt walking in simulation: 49 | `python3 src/reactive_planners/demos/demo_reactive_planners_bolt_step_adjustment.py` 50 | 51 | To run Solo12 walking in simulation: 52 | `python3 src/reactive_planners/demos/demo_dgh_sim_solo12_step_adjustment.py` 53 | 54 | ### Reference 55 | 56 | This package contains the implementation of the algorithms depicted in: 57 | 58 | - Elham Daneshmand, Majid Khadiv , Felix Grimminger and Ludovic Righetti. 59 | “Variable Horizon MPC With Swing Foot Dynamicsfor Bipedal Walking Control.”, 60 | IEEE Robotics and Automation Letters, 6(2). 61 | https://arxiv.org/abs/2010.08198 (2021) 62 | 63 | - Majid Khadiv, Alexander Herzog, S. Ali A. Moosavian and Ludovic Righetti. 64 | “Walking Control Based on Step Timing Adaptation.”, 65 | IEEE Transactions on Robotics, 36(3). 66 | https://arxiv.org/abs/1704.01271 (2020) 67 | 68 | ### License and Copyrights 69 | 70 | License BSD-3-Clause 71 | Copyright (c) 2020, New York University and Max Planck Gesellschaft. 72 | -------------------------------------------------------------------------------- /src/dynamic_graph/stepper_head.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Definition of the StepperHead class. 8 | */ 9 | 10 | #include "reactive_planners/dynamic_graph/stepper_head.hpp" 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace reactive_planners 18 | { 19 | namespace dynamic_graph 20 | { 21 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(StepperHead, "StepperHead"); 22 | 23 | /* 24 | * The entity. 25 | */ 26 | StepperHead::StepperHead(const std::string& name) 27 | : dynamicgraph::Entity(name), 28 | // Input signals. 29 | next_step_location_sin_( 30 | NULL, make_signal_string(true, "Vector3d", "next_step_location_sin")), 31 | duration_before_step_landing_sin_( 32 | NULL, 33 | make_signal_string( 34 | true, "double", "duration_before_step_landing_sin")), 35 | 36 | // Output signals. 37 | // 38 | // The value for the output signals is computed central in the 39 | // time_from_last_step_touchdown method. 40 | 41 | time_from_last_step_touchdown_sout_( 42 | boost::bind( 43 | &StepperHead::time_from_last_step_touchdown, this, _1, _2), 44 | next_step_location_sin_ << duration_before_step_landing_sin_, 45 | make_signal_string( 46 | false, "double", "time_from_last_step_touchdown_sout")), 47 | is_left_leg_in_contact_sout_( 48 | boost::bind(&StepperHead::is_left_leg_in_contact, this, _1, _2), 49 | time_from_last_step_touchdown_sout_, 50 | make_signal_string(false, "int", "is_left_leg_in_contact_sout")), 51 | old_step_location_sout_( 52 | boost::bind(&StepperHead::old_step_location, this, _1, _2), 53 | time_from_last_step_touchdown_sout_, 54 | make_signal_string(false, "Vector3d", "old_step_location_sout")), 55 | current_step_location_sout_( 56 | boost::bind(&StepperHead::current_step_location, this, _1, _2), 57 | time_from_last_step_touchdown_sout_, 58 | make_signal_string(false, "Vector3d", "current_step_location_sout")), 59 | is_left_leg_in_contact_(true) 60 | { 61 | Entity::signalRegistration( 62 | next_step_location_sin_ 63 | << duration_before_step_landing_sin_ 64 | << time_from_last_step_touchdown_sout_ << is_left_leg_in_contact_sout_ 65 | << old_step_location_sout_ << current_step_location_sout_); 66 | } 67 | 68 | double& StepperHead::time_from_last_step_touchdown(double& time_last_step, 69 | int time) 70 | { 71 | static int init_time = -1; 72 | double t_end; 73 | 74 | if (init_time == -1 || init_time == time) 75 | { 76 | init_time = time; 77 | dg_time_phase_switch_ = time; 78 | t_end = 1; 79 | time_last_step = 0; 80 | old_step_location_.setZero(); 81 | current_step_location_.setZero(); 82 | } 83 | else 84 | { 85 | t_end = duration_before_step_landing_sin_.access(time); 86 | } 87 | 88 | // TODO: Remove assumption of running controller at 1 kHz. 89 | time_last_step = 0.001 * (double)(time - dg_time_phase_switch_); 90 | 91 | if (time_last_step > t_end) 92 | { 93 | // Switch the contact phase. 94 | is_left_leg_in_contact_ = !is_left_leg_in_contact_; 95 | dg_time_phase_switch_ = time; 96 | time_last_step = 0.; 97 | 98 | old_step_location_ = current_step_location_; 99 | current_step_location_ = next_step_location_sin_.access(time); 100 | } 101 | 102 | return time_last_step; 103 | } 104 | 105 | int& StepperHead::is_left_leg_in_contact(int& left_leg_in_contact, int time) 106 | { 107 | // Ensure internals are computed. 108 | time_from_last_step_touchdown_sout_.access(time); 109 | 110 | left_leg_in_contact = is_left_leg_in_contact_; 111 | return left_leg_in_contact; 112 | } 113 | 114 | dynamicgraph::Vector& StepperHead::old_step_location( 115 | dynamicgraph::Vector& step_location, int time) 116 | { 117 | // Ensure internals are computed. 118 | time_from_last_step_touchdown_sout_.access(time); 119 | 120 | step_location = old_step_location_; 121 | return step_location; 122 | } 123 | 124 | dynamicgraph::Vector& StepperHead::current_step_location( 125 | dynamicgraph::Vector& step_location, int time) 126 | { 127 | // Ensure internals are computed. 128 | time_from_last_step_touchdown_sout_.access(time); 129 | 130 | step_location = current_step_location_; 131 | return step_location; 132 | } 133 | 134 | std::string StepperHead::make_signal_string(const bool& is_input_signal, 135 | const std::string& signal_type, 136 | const std::string& signal_name) 137 | { 138 | std::ostringstream oss; 139 | oss << CLASS_NAME << "(" << name 140 | << ")::" << (is_input_signal ? "input" : "output") << "(" << signal_type 141 | << ")::" << signal_name; 142 | return oss.str(); 143 | } 144 | 145 | } // namespace dynamic_graph 146 | } // namespace reactive_planners 147 | -------------------------------------------------------------------------------- /src/mathematics/polynome.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file polynome.cpp 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @brief Polynomes object for trajectories. 5 | * 6 | * @version 0.1 7 | * @date 2019-11-07 8 | * 9 | * @copyright Copyright (c) 2019 10 | * 11 | * See 12 | * https://github.com/jrl-umi3218/jrl-walkgen/blob/master/src/Mathematics/PolynomeFoot.cpp 13 | * for further enhancement. 14 | */ 15 | 16 | #include 17 | #include 18 | 19 | namespace blmc_robots 20 | { 21 | template <> 22 | void TimePolynome<5>::SetParameters(double final_time, 23 | double init_pos, 24 | double init_speed, 25 | double init_acc, 26 | double final_pos, 27 | double final_speed, 28 | double final_acc) 29 | { 30 | double tmp; 31 | m_Coefficients[0] = InitPos_ = InitPos; 32 | m_Coefficients[1] = InitSpeed_ = InitSpeed; 33 | m_Coefficients[2] = InitAcc / 2.0; 34 | InitAcc_ = InitAcc; 35 | FT_ = FT; 36 | FinalPos_ = FinalPos; 37 | tmp = FT * FT * FT; 38 | if (tmp == 0.0) 39 | { 40 | m_Coefficients[3] = 0.0; 41 | m_Coefficients[4] = 0.0; 42 | m_Coefficients[5] = 0.0; 43 | } 44 | else 45 | { 46 | m_Coefficients[3] = 47 | -(1.5 * InitAcc * FT * FT - 0.5 * FinalAcc * FT * FT + 48 | 6.0 * InitSpeed * FT + 4.0 * FinalSpeed * FT + 10.0 * InitPos - 49 | 10.0 * FinalPos) / 50 | tmp; 51 | tmp = tmp * FT; 52 | m_Coefficients[4] = (1.5 * InitAcc * FT * FT - FinalAcc * FT * FT + 53 | 8.0 * InitSpeed * FT + 7.0 * FinalSpeed * FT + 54 | 15.0 * InitPos - 15.0 * FinalPos) / 55 | tmp; 56 | tmp = tmp * FT; 57 | m_Coefficients[5] = 58 | -(0.5 * InitAcc * FT * FT - 0.5 * FinalAcc * FT * FT + 59 | 3.0 * InitSpeed * FT + 3.0 * FinalSpeed * FT + 6.0 * InitPos - 60 | 6.0 * FinalPos) / 61 | tmp; 62 | } 63 | } 64 | 65 | } // namespace blmc_robots -------------------------------------------------------------------------------- /src/quadruped_dcm_reactive_stepper.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Define the QuadrupedDcmReactiveStepper class 8 | */ 9 | 10 | #include "reactive_planners/quadruped_dcm_reactive_stepper.hpp" 11 | 12 | #include "pinocchio/math/rpy.hpp" 13 | 14 | namespace reactive_planners 15 | { 16 | QuadrupedDcmReactiveStepper::QuadrupedDcmReactiveStepper() 17 | { 18 | front_left_foot_position_.setZero(); 19 | front_left_foot_velocity_.setZero(); 20 | front_left_foot_acceleration_.setZero(); 21 | front_right_foot_position_.setZero(); 22 | front_right_foot_velocity_.setZero(); 23 | front_right_foot_acceleration_.setZero(); 24 | hind_left_foot_position_.setZero(); 25 | hind_left_foot_velocity_.setZero(); 26 | hind_left_foot_acceleration_.setZero(); 27 | hind_right_foot_position_.setZero(); 28 | hind_right_foot_velocity_.setZero(); 29 | hind_right_foot_acceleration_.setZero(); 30 | // biped_stepper_ nothing to be done. 31 | fr_offset_.setZero(); 32 | fl_offset_.setZero(); 33 | hr_offset_.setZero(); 34 | hl_offset_.setZero(); 35 | foot_height_offset_ = 0.0; 36 | forces_.setZero(); 37 | contact_array_.fill(1); 38 | } 39 | 40 | QuadrupedDcmReactiveStepper::~QuadrupedDcmReactiveStepper() = default; 41 | 42 | void QuadrupedDcmReactiveStepper::set_steptime_nominal(double t_nom) 43 | { 44 | biped_stepper_.set_steptime_nominal(t_nom); 45 | } 46 | 47 | void QuadrupedDcmReactiveStepper::initialize( 48 | const bool& is_left_leg_in_contact, 49 | const double& l_min, 50 | const double& l_max, 51 | const double& w_min, 52 | const double& w_max, 53 | const double& t_min, 54 | const double& t_max, 55 | const double& l_p, 56 | const double& com_height, 57 | const Eigen::Vector9d& weight, 58 | const double& mid_air_foot_height, 59 | const double& control_period, 60 | const double& planner_loop, 61 | const Eigen::Ref& base_placement, 62 | const Eigen::Ref& front_left_foot_position, 63 | const Eigen::Ref& front_right_foot_position, 64 | const Eigen::Ref& hind_left_foot_position, 65 | const Eigen::Ref& hind_right_foot_position) 66 | { 67 | Eigen::Map quat( 68 | base_placement.tail<4>().data()); 69 | Eigen::Vector3d rpy = pinocchio::rpy::matrixToRpy(quat.matrix()); 70 | rpy.head<2>().setZero(); 71 | Eigen::Matrix3d rot_mat = pinocchio::rpy::rpyToMatrix(rpy).transpose(); 72 | fr_offset_ = 73 | rot_mat * (front_right_foot_position - base_placement.head<3>()); 74 | fl_offset_ = 75 | rot_mat * (front_left_foot_position - base_placement.head<3>()); 76 | hr_offset_ = 77 | rot_mat * (hind_right_foot_position - base_placement.head<3>()); 78 | hl_offset_ = rot_mat * (hind_left_foot_position - base_placement.head<3>()); 79 | 80 | foot_height_offset_ = 81 | (front_right_foot_position(2) + front_left_foot_position(2) + 82 | hind_right_foot_position(2) + hind_left_foot_position(2)) / 83 | 4.0; 84 | 85 | fr_offset_(2) = foot_height_offset_; 86 | fl_offset_(2) = foot_height_offset_; 87 | hr_offset_(2) = foot_height_offset_; 88 | hl_offset_(2) = foot_height_offset_; 89 | 90 | Eigen::Vector3d virtual_left_foot_position = 91 | (front_left_foot_position + hind_right_foot_position) * 0.5; 92 | virtual_left_foot_position[2] = 0.0; 93 | 94 | Eigen::Vector3d virtual_right_foot_position = 95 | (front_right_foot_position + hind_left_foot_position) * 0.5; 96 | virtual_right_foot_position[2] = 0.0; 97 | 98 | // Initialize the dcm vrp planner and initialize it. 99 | biped_stepper_.initialize(is_left_leg_in_contact, 100 | l_min, 101 | l_max, 102 | w_min, 103 | w_max, 104 | t_min, 105 | t_max, 106 | l_p, 107 | com_height, 108 | weight, 109 | mid_air_foot_height, 110 | control_period, 111 | planner_loop, 112 | virtual_left_foot_position, 113 | virtual_right_foot_position); 114 | } 115 | 116 | bool QuadrupedDcmReactiveStepper::run( 117 | double time, 118 | const Eigen::Ref& front_left_foot_position, 119 | const Eigen::Ref& front_right_foot_position, 120 | const Eigen::Ref& hind_left_foot_position, 121 | const Eigen::Ref& hind_right_foot_position, 122 | const Eigen::Ref& front_left_foot_velocity, 123 | const Eigen::Ref& front_right_foot_velocity, 124 | const Eigen::Ref& hind_left_foot_velocity, 125 | const Eigen::Ref& hind_right_foot_velocity, 126 | const Eigen::Ref& com_position, 127 | const Eigen::Ref& com_velocity, 128 | const double& base_yaw, 129 | const bool& is_closed_loop) 130 | { 131 | bool succeed = true; 132 | Eigen::Matrix stepper_forces; 133 | 134 | Eigen::Vector3d virtual_left_foot_position = 135 | (front_left_foot_position + hind_right_foot_position) * 0.5; 136 | virtual_left_foot_position(2) -= foot_height_offset_; 137 | Eigen::Vector3d virtual_left_foot_velocity = 138 | (front_left_foot_velocity + hind_right_foot_velocity) * 0.5; 139 | 140 | Eigen::Vector3d virtual_right_foot_position = 141 | (front_right_foot_position + hind_left_foot_position) * 0.5; 142 | virtual_right_foot_position(2) -= foot_height_offset_; 143 | Eigen::Vector3d virtual_right_foot_velocity = 144 | (front_right_foot_velocity + hind_left_foot_velocity) * 0.5; 145 | 146 | biped_stepper_.run(time, 147 | virtual_left_foot_position, 148 | virtual_right_foot_position, 149 | virtual_left_foot_velocity, 150 | virtual_right_foot_velocity, 151 | com_position, 152 | com_velocity, 153 | base_yaw, 154 | is_closed_loop); 155 | 156 | Eigen::Matrix3d base_yaw_rot = 157 | pinocchio::rpy::rpyToMatrix(0.0, 0.0, base_yaw); 158 | front_left_foot_position_ = 159 | biped_stepper_.get_left_foot_position() + base_yaw_rot * fl_offset_; 160 | hind_right_foot_position_ = 161 | biped_stepper_.get_left_foot_position() + base_yaw_rot * hr_offset_; 162 | front_right_foot_position_ = 163 | biped_stepper_.get_right_foot_position() + base_yaw_rot * fr_offset_; 164 | hind_left_foot_position_ = 165 | biped_stepper_.get_right_foot_position() + base_yaw_rot * hl_offset_; 166 | 167 | front_left_foot_velocity_ = biped_stepper_.get_left_foot_velocity(); 168 | hind_right_foot_velocity_ = biped_stepper_.get_left_foot_velocity(); 169 | front_right_foot_velocity_ = biped_stepper_.get_right_foot_velocity(); 170 | hind_left_foot_velocity_ = biped_stepper_.get_right_foot_velocity(); 171 | 172 | front_left_foot_acceleration_ = biped_stepper_.get_left_foot_acceleration(); 173 | hind_right_foot_acceleration_ = biped_stepper_.get_left_foot_acceleration(); 174 | front_right_foot_acceleration_ = 175 | biped_stepper_.get_right_foot_acceleration(); 176 | hind_left_foot_acceleration_ = biped_stepper_.get_right_foot_acceleration(); 177 | 178 | forces_.setZero(); 179 | if (biped_stepper_.is_running()) 180 | { 181 | stepper_forces = biped_stepper_.get_force(); 182 | if (biped_stepper_.get_is_left_leg_in_contact()) 183 | { 184 | contact_array_ << 1.0, 0.0, 0.0, 1.0; 185 | forces_.block(6, 0, 6, 1) = stepper_forces.block(6, 0, 6, 1); 186 | forces_.block(12, 0, 6, 1) = stepper_forces.block(6, 0, 6, 1); 187 | } 188 | else 189 | { 190 | contact_array_ << 0.0, 1.0, 1.0, 0.0; 191 | forces_.block(0, 0, 6, 1) = stepper_forces.block(0, 0, 6, 1); 192 | forces_.block(18, 0, 6, 1) = stepper_forces.block(0, 0, 6, 1); 193 | } 194 | } 195 | else 196 | { 197 | contact_array_.fill(1.0); 198 | } 199 | return succeed; 200 | } 201 | 202 | bool QuadrupedDcmReactiveStepper::run( 203 | double time, 204 | const Eigen::Ref& front_left_foot_position, 205 | const Eigen::Ref& front_right_foot_position, 206 | const Eigen::Ref& hind_left_foot_position, 207 | const Eigen::Ref& hind_right_foot_position, 208 | const Eigen::Ref& front_left_foot_velocity, 209 | const Eigen::Ref& front_right_foot_velocity, 210 | const Eigen::Ref& hind_left_foot_velocity, 211 | const Eigen::Ref& hind_right_foot_velocity, 212 | const Eigen::Ref& com_position, 213 | const Eigen::Ref& com_velocity, 214 | const pinocchio::SE3& world_M_base, 215 | const bool& is_closed_loop) 216 | { 217 | Eigen::Vector3d rpy = pinocchio::rpy::matrixToRpy(world_M_base.rotation()); 218 | 219 | return run(time, 220 | front_left_foot_position, 221 | front_right_foot_position, 222 | hind_left_foot_position, 223 | hind_right_foot_position, 224 | front_left_foot_velocity, 225 | front_right_foot_velocity, 226 | hind_left_foot_velocity, 227 | hind_right_foot_velocity, 228 | com_position, 229 | com_velocity, 230 | rpy(2), 231 | is_closed_loop); 232 | } 233 | 234 | } // namespace reactive_planners 235 | -------------------------------------------------------------------------------- /src/stepper_head.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Definition of the StepperHead class. 8 | */ 9 | 10 | #include "reactive_planners/stepper_head.hpp" 11 | 12 | namespace reactive_planners 13 | { 14 | StepperHead::StepperHead() 15 | { 16 | // inputs 17 | duration_before_foot_landing_ = 0.0; 18 | next_support_location_.setZero(); 19 | current_time_ = 0.0; 20 | 21 | // outputs 22 | time_from_last_step_touchdown_ = 0.0; 23 | is_left_leg_in_contact_ = true; 24 | previous_support_location_.setZero(); 25 | current_support_location_.setZero(); 26 | 27 | // internals 28 | time_support_switch_ = 0.0; 29 | } 30 | 31 | void StepperHead::run(const double& duration_before_foot_landing, 32 | const Eigen::Vector3d& next_support_location, 33 | const double& current_time) 34 | { 35 | // copy the argument 36 | duration_before_foot_landing_ = duration_before_foot_landing; 37 | next_support_location_ = next_support_location; 38 | current_time_ = current_time; 39 | 40 | // Compute the time_from_last_step_touchdown_ 41 | time_from_last_step_touchdown_ = current_time_ - time_support_switch_; 42 | if (time_from_last_step_touchdown_ > duration_before_foot_landing_) 43 | { 44 | // Switch the contact phase. 45 | is_left_leg_in_contact_ = !is_left_leg_in_contact_; 46 | time_support_switch_ = current_time; 47 | time_from_last_step_touchdown_ = 0.0; 48 | 49 | previous_support_location_ = current_support_location_; 50 | current_support_location_ = next_support_location_; 51 | } 52 | } 53 | 54 | } // namespace reactive_planners -------------------------------------------------------------------------------- /srcpy/dcm_reactive_stepper.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Python bindings for the StepperHead class 8 | */ 9 | 10 | #include "reactive_planners/dcm_reactive_stepper.hpp" 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | using namespace reactive_planners; 18 | 19 | void bind_dcm_reactive_stepper(pybind11::module &module) 20 | { 21 | pybind11::class_(module, "DcmReactiveStepper") 22 | .def(pybind11::init<>()) 23 | 24 | // Public methods. 25 | .def("initialize", &DcmReactiveStepper::initialize) 26 | .def("run", 27 | (bool (DcmReactiveStepper::*)( 28 | double, 29 | const Eigen::Ref &, 30 | const Eigen::Ref &, 31 | const Eigen::Ref &, 32 | const Eigen::Ref &, 33 | const Eigen::Ref &, 34 | const Eigen::Ref &, 35 | const double &, 36 | const bool &)) & 37 | DcmReactiveStepper::run) 38 | .def("run", 39 | [](DcmReactiveStepper &obj, 40 | double time, 41 | const Eigen::Ref &left_foot_position, 42 | const Eigen::Ref &right_foot_position, 43 | const Eigen::Ref &left_foot_vel, 44 | const Eigen::Ref &right_foot_vel, 45 | const Eigen::Ref &com_position, 46 | const Eigen::Ref &com_velocity, 47 | const pybind11::object &py_world_M_base, 48 | const bool &is_closed_loop) { 49 | const pinocchio::SE3 &world_M_base = 50 | boost::python::extract( 51 | py_world_M_base.ptr()); 52 | bool ret = obj.run(time, 53 | left_foot_position, 54 | right_foot_position, 55 | left_foot_vel, 56 | right_foot_vel, 57 | com_position, 58 | com_velocity, 59 | world_M_base, 60 | is_closed_loop); 61 | return ret; 62 | }) 63 | .def("start", &DcmReactiveStepper::start) 64 | .def("stop", &DcmReactiveStepper::stop) 65 | // Setters. 66 | .def("set_desired_com_velocity", 67 | &DcmReactiveStepper::set_desired_com_velocity) 68 | .def("set_right_foot_position", 69 | &DcmReactiveStepper::set_right_foot_position) 70 | .def("set_right_foot_velocity", 71 | &DcmReactiveStepper::set_right_foot_velocity) 72 | .def("set_left_foot_position", 73 | &DcmReactiveStepper::set_left_foot_position) 74 | .def("set_left_foot_velocity", 75 | &DcmReactiveStepper::set_left_foot_velocity) 76 | .def("dcm_vrp_planner_initialization", 77 | &DcmReactiveStepper::dcm_vrp_planner_initialization) 78 | 79 | // Getters. 80 | .def("get_right_foot_position", 81 | &DcmReactiveStepper::get_right_foot_position) 82 | .def("get_right_foot_velocity", 83 | &DcmReactiveStepper::get_right_foot_velocity) 84 | .def("get_right_foot_acceleration", 85 | &DcmReactiveStepper::get_right_foot_acceleration) 86 | .def("get_left_foot_position", 87 | &DcmReactiveStepper::get_left_foot_position) 88 | .def("get_left_foot_velocity", 89 | &DcmReactiveStepper::get_left_foot_velocity) 90 | .def("get_left_foot_acceleration", 91 | &DcmReactiveStepper::get_left_foot_acceleration) 92 | .def("get_previous_support_foot_position", 93 | &DcmReactiveStepper::get_previous_support_foot_position) 94 | .def("get_current_support_foot_position", 95 | &DcmReactiveStepper::get_current_support_foot_position) 96 | .def("get_next_support_foot_position", 97 | &DcmReactiveStepper::get_next_support_foot_position) 98 | .def("get_step_duration", &DcmReactiveStepper::get_step_duration) 99 | .def("get_time_from_last_step_touchdown", 100 | &DcmReactiveStepper::get_time_from_last_step_touchdown) 101 | .def("get_flying_foot_position", 102 | &DcmReactiveStepper::get_flying_foot_position) 103 | .def("get_is_left_leg_in_contact", 104 | &DcmReactiveStepper::get_is_left_leg_in_contact) 105 | .def("get_forces", &DcmReactiveStepper::get_forces); 106 | } -------------------------------------------------------------------------------- /srcpy/dcm_vrp_planner.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Python bindings for the DcmVrpPlanner class 8 | */ 9 | 10 | #include "reactive_planners/dcm_vrp_planner.hpp" 11 | #include 12 | #include 13 | 14 | using namespace reactive_planners; 15 | 16 | void bind_dcm_vrp_planner(pybind11::module &module) 17 | { 18 | pybind11::class_(module, "DcmVrpPlanner") 19 | .def(pybind11::init<>()) 20 | // public methods 21 | .def("initialize", &DcmVrpPlanner::initialize) 22 | .def("solve", &DcmVrpPlanner::solve) 23 | .def("internal_checks", &DcmVrpPlanner::internal_checks) 24 | .def("print_solver", &DcmVrpPlanner::print_solver) 25 | .def("update", 26 | (void (DcmVrpPlanner::*)(const Eigen::Ref &, 27 | const double &, 28 | const bool &, 29 | const Eigen::Ref &, 30 | const Eigen::Ref &, 31 | const Eigen::Ref &, 32 | const pinocchio::SE3 &, 33 | const double &)) & 34 | DcmVrpPlanner::update) 35 | .def("update", 36 | (void (DcmVrpPlanner::*)(const Eigen::Ref &, 37 | const double &, 38 | const bool &, 39 | const Eigen::Ref &, 40 | const Eigen::Ref &, 41 | const Eigen::Ref &, 42 | const double &, 43 | const double &)) & 44 | DcmVrpPlanner::update) 45 | 46 | // getters 47 | .def("get_t_nom", &DcmVrpPlanner::get_t_nom) 48 | .def("get_tau_nom", &DcmVrpPlanner::get_tau_nom) 49 | .def("get_l_nom", &DcmVrpPlanner::get_l_nom) 50 | .def("get_w_nom", &DcmVrpPlanner::get_w_nom) 51 | .def("get_bx_nom", &DcmVrpPlanner::get_bx_nom) 52 | .def("get_by_nom", &DcmVrpPlanner::get_by_nom) 53 | .def("get_world_M_local", &DcmVrpPlanner::get_world_M_local) 54 | .def("get_dcm_local", &DcmVrpPlanner::get_dcm_local) 55 | .def("get_current_step_location_local", 56 | &DcmVrpPlanner::get_current_step_location_local) 57 | .def("get_v_des_local", &DcmVrpPlanner::get_v_des_local) 58 | .def("get_dcm_nominal", &DcmVrpPlanner::get_dcm_nominal) 59 | .def("get_next_step_location", &DcmVrpPlanner::get_next_step_location) 60 | .def("get_duration_before_step_landing", 61 | &DcmVrpPlanner::get_duration_before_step_landing) 62 | 63 | // String representation of the sovler. 64 | .def("__repr__", 65 | [](const DcmVrpPlanner &planner) { return planner.to_string(); }); 66 | } -------------------------------------------------------------------------------- /srcpy/polynomial_end_effector_trajectory.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Python bindings for the EndEffectorTrajectory3D class 8 | */ 9 | 10 | #include "reactive_planners/polynomial_end_effector_trajectory.hpp" 11 | #include 12 | #include 13 | 14 | using namespace reactive_planners; 15 | 16 | void bind_end_effector_trajectory_3d(pybind11::module &module) 17 | { 18 | pybind11::class_(module, 19 | "EndEffectorTrajectory3D") 20 | .def(pybind11::init<>()) 21 | 22 | // public methods 23 | .def("compute", &PolynomialEndEffectorTrajectory::compute) 24 | .def("get_next_state", &PolynomialEndEffectorTrajectory::get_next_state) 25 | .def("get_mid_air_height", 26 | &PolynomialEndEffectorTrajectory::get_mid_air_height) 27 | .def("print_solver", &PolynomialEndEffectorTrajectory::print_solver) 28 | .def("set_mid_air_height", 29 | &PolynomialEndEffectorTrajectory::set_mid_air_height) 30 | .def("set_costs", &PolynomialEndEffectorTrajectory::set_costs) 31 | 32 | // String representation of the sovler. 33 | .def("__repr__", [](const PolynomialEndEffectorTrajectory &planner) { 34 | return planner.to_string(); 35 | }); 36 | } -------------------------------------------------------------------------------- /srcpy/quadruped_dcm_reactive_stepper.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Python bindings for the StepperHead class 8 | */ 9 | 10 | #include "reactive_planners/quadruped_dcm_reactive_stepper.hpp" 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | using namespace reactive_planners; 18 | 19 | void bind_quadruped_dcm_reactive_stepper(pybind11::module &module) 20 | { 21 | pybind11::class_(module, 22 | "QuadrupedDcmReactiveStepper") 23 | .def(pybind11::init<>()) 24 | 25 | // Public methods. 26 | .def("initialize", &QuadrupedDcmReactiveStepper::initialize) 27 | .def("run", 28 | (bool (QuadrupedDcmReactiveStepper::*)( 29 | double, 30 | const Eigen::Ref &, 31 | const Eigen::Ref &, 32 | const Eigen::Ref &, 33 | const Eigen::Ref &, 34 | const Eigen::Ref &, 35 | const Eigen::Ref &, 36 | const Eigen::Ref &, 37 | const Eigen::Ref &, 38 | const Eigen::Ref &, 39 | const Eigen::Ref &, 40 | const double &, 41 | const bool &)) & 42 | QuadrupedDcmReactiveStepper::run) 43 | .def( 44 | "run", 45 | [](QuadrupedDcmReactiveStepper &obj, 46 | double time, 47 | const Eigen::Ref 48 | &front_left_foot_position, 49 | const Eigen::Ref 50 | &front_right_foot_position, 51 | const Eigen::Ref &hind_left_foot_position, 52 | const Eigen::Ref 53 | &hind_right_foot_position, 54 | const Eigen::Ref 55 | &front_left_foot_velocity, 56 | const Eigen::Ref 57 | &front_right_foot_velocity, 58 | const Eigen::Ref &hind_left_foot_velocity, 59 | const Eigen::Ref 60 | &hind_right_foot_velocity, 61 | const Eigen::Ref &com_position, 62 | const Eigen::Ref &com_velocity, 63 | const pybind11::object &py_world_M_base, 64 | const bool &is_closed_loop) { 65 | const pinocchio::SE3 &world_M_base = 66 | boost::python::extract( 67 | py_world_M_base.ptr()); 68 | bool ret = obj.run(time, 69 | front_left_foot_position, 70 | front_right_foot_position, 71 | hind_left_foot_position, 72 | hind_right_foot_position, 73 | front_left_foot_velocity, 74 | front_right_foot_velocity, 75 | hind_left_foot_velocity, 76 | hind_right_foot_velocity, 77 | com_position, 78 | com_velocity, 79 | world_M_base, 80 | is_closed_loop); 81 | return ret; 82 | }) 83 | .def("start", &QuadrupedDcmReactiveStepper::start) 84 | .def("stop", &QuadrupedDcmReactiveStepper::stop) 85 | 86 | // Setters. 87 | .def("set_desired_com_velocity", 88 | &QuadrupedDcmReactiveStepper::set_desired_com_velocity) 89 | .def("set_feet_pos", &QuadrupedDcmReactiveStepper::set_feet_pos) 90 | .def("set_polynomial_end_effector_trajectory", 91 | &QuadrupedDcmReactiveStepper:: 92 | set_polynomial_end_effector_trajectory) 93 | .def( 94 | "set_dynamical_end_effector_trajectory", 95 | &QuadrupedDcmReactiveStepper::set_dynamical_end_effector_trajectory) 96 | .def( 97 | "set_steptime_nominal", 98 | &QuadrupedDcmReactiveStepper::set_steptime_nominal) 99 | 100 | // Getters. 101 | .def("get_front_left_foot_position", 102 | &QuadrupedDcmReactiveStepper::get_front_left_foot_position) 103 | .def("get_front_left_foot_velocity", 104 | &QuadrupedDcmReactiveStepper::get_front_left_foot_velocity) 105 | .def("get_front_right_foot_position", 106 | &QuadrupedDcmReactiveStepper::get_front_right_foot_position) 107 | .def("get_front_right_foot_velocity", 108 | &QuadrupedDcmReactiveStepper::get_front_right_foot_velocity) 109 | .def("get_hind_left_foot_position", 110 | &QuadrupedDcmReactiveStepper::get_hind_left_foot_position) 111 | .def("get_hind_left_foot_velocity", 112 | &QuadrupedDcmReactiveStepper::get_hind_left_foot_velocity) 113 | .def("get_hind_right_foot_position", 114 | &QuadrupedDcmReactiveStepper::get_hind_right_foot_position) 115 | .def("get_hind_right_foot_velocity", 116 | &QuadrupedDcmReactiveStepper::get_hind_right_foot_velocity) 117 | .def("get_feasible_com_velocity", 118 | &QuadrupedDcmReactiveStepper::get_feasible_com_velocity) 119 | .def("get_contact_array", 120 | &QuadrupedDcmReactiveStepper::get_contact_array) 121 | .def("get_forces", 122 | &QuadrupedDcmReactiveStepper::get_forces); 123 | } -------------------------------------------------------------------------------- /srcpy/reactive_planners.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Python bindings for the DcmVrpPlanner class 8 | */ 9 | 10 | #include 11 | 12 | void bind_stepper_head(pybind11::module &module); 13 | void bind_dcm_vrp_planner(pybind11::module &module); 14 | void bind_end_effector_trajectory_3d(pybind11::module &module); 15 | void bind_dcm_reactive_stepper(pybind11::module &module); 16 | void bind_quadruped_dcm_reactive_stepper(pybind11::module &module); 17 | 18 | PYBIND11_MODULE(reactive_planners_cpp, m) 19 | { 20 | m.doc() = R"pbdoc( 21 | reactive_planners python bindings 22 | --------------------------------- 23 | .. currentmodule:: reactive_planners 24 | .. autosummary:: 25 | :toctree: _generate 26 | add 27 | subtract 28 | )pbdoc"; 29 | 30 | bind_stepper_head(m); 31 | bind_dcm_vrp_planner(m); 32 | bind_end_effector_trajectory_3d(m); 33 | bind_dcm_reactive_stepper(m); 34 | bind_quadruped_dcm_reactive_stepper(m); 35 | } -------------------------------------------------------------------------------- /srcpy/stepper_head.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Python bindings for the StepperHead class 8 | */ 9 | 10 | #include "reactive_planners/stepper_head.hpp" 11 | #include 12 | #include 13 | 14 | using namespace reactive_planners; 15 | 16 | void bind_stepper_head(pybind11::module &module) 17 | { 18 | pybind11::class_(module, "StepperHead") 19 | .def(pybind11::init<>()) 20 | 21 | // public methods 22 | .def("run", 23 | (void (StepperHead::*)(const double &, 24 | const Eigen::Vector3d &, 25 | const double &, 26 | bool)) & 27 | StepperHead::run) 28 | 29 | .def("run", 30 | (void (StepperHead::*)( 31 | const double &, const Eigen::Vector3d &, const double &)) & 32 | StepperHead::run) 33 | 34 | .def("get_time_from_last_step_touchdown", 35 | &StepperHead::get_time_from_last_step_touchdown) 36 | .def("get_is_left_leg_in_contact", 37 | &StepperHead::get_is_left_leg_in_contact) 38 | .def("get_previous_support_location", 39 | &StepperHead::get_previous_support_location) 40 | .def("get_current_support_location", 41 | &StepperHead::get_current_support_location) 42 | .def("set_support_feet_pos", &StepperHead::set_support_feet_pos); 43 | } -------------------------------------------------------------------------------- /srcpy/walking_dg_python_module.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Expose the Device and the periodic call to python. 8 | */ 9 | 10 | #include 11 | 12 | #include "dynamic-graph/python/module.hh" 13 | #include "dynamic-graph/python/signal.hh" 14 | 15 | #include "reactive_planners/dynamic_graph/dcm_reactive_stepper.hpp" 16 | #include "reactive_planners/dynamic_graph/quadruped_dcm_reactive_stepper.hpp" 17 | #include "reactive_planners/dynamic_graph/stepper_head.hpp" 18 | 19 | namespace dg = dynamicgraph; 20 | 21 | typedef bp::return_value_policy 22 | reference_existing_object; 23 | 24 | BOOST_PYTHON_MODULE(walking) 25 | { 26 | bp::import("dynamic_graph"); 27 | eigenpy::enableEigenPy(); 28 | eigenpy::enableEigenPySpecific(); 29 | eigenpy::enableEigenPySpecific(); 30 | eigenpy::enableEigenPySpecific(); 31 | 32 | using reactive_planners::dynamic_graph::DcmReactiveStepper; 33 | using reactive_planners::dynamic_graph::StepperHead; 34 | dynamicgraph::python::exposeEntity(); 35 | dynamicgraph::python::exposeEntity(); 36 | 37 | using reactive_planners::dynamic_graph::QuadrupedDcmReactiveStepper; 38 | dynamicgraph::python::exposeEntity() 39 | // NOTE: The `initialize` method has too many args for boost python. 40 | // Therefore we use two initialize functions. 41 | // .def("initialize", &QuadrupedDcmReactiveStepper::initialize) 42 | .def("initialize_placement", 43 | &QuadrupedDcmReactiveStepper::initialize_placement) 44 | .def("initialize_stepper", 45 | &QuadrupedDcmReactiveStepper::initialize_stepper) 46 | .def("set_steptime_nominal", 47 | &QuadrupedDcmReactiveStepper::set_steptime_nominal) 48 | .def("set_polynomial_end_effector_trajectory", 49 | &QuadrupedDcmReactiveStepper:: 50 | set_polynomial_end_effector_trajectory) 51 | .def( 52 | "set_dynamical_end_effector_trajectory", 53 | &QuadrupedDcmReactiveStepper::set_dynamical_end_effector_trajectory) 54 | .def("start", &QuadrupedDcmReactiveStepper::start) 55 | .def("stop", &QuadrupedDcmReactiveStepper::stop); 56 | } 57 | -------------------------------------------------------------------------------- /tests/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Standard main for the google unittests framework. 8 | */ 9 | #include "gtest/gtest.h" 10 | 11 | int main(int argc, char **argv) 12 | { 13 | ::testing::InitGoogleTest(&argc, argv); 14 | return RUN_ALL_TESTS(); 15 | } 16 | --------------------------------------------------------------------------------