├── .gitignore ├── CMakeLists.txt ├── Config.cmake.in ├── README.md ├── colcon.pkg ├── demos ├── demo_dg_whole_body_controller.py ├── demo_impedance_ctrl_bouncing_cpp.py ├── demo_impedance_ctrl_cpp.py ├── demo_impedance_ctrl_python.py ├── demo_robot_com_ctrl.py ├── demo_robot_com_ctrl_cpp.py ├── demo_robot_com_imp_ctrl_cpp.py ├── demo_robot_impedance.py └── solo_impedance.yaml ├── include └── mim_control │ ├── centroidal_force_qp_controller.hpp │ ├── centroidal_impedance_controller.hpp │ ├── centroidal_pd_controller.hpp │ ├── dynamic_graph │ ├── centroidal_force_qp_controller.hpp │ ├── centroidal_pd_controller.hpp │ ├── impedance_controller.hpp │ └── signal_utils.hpp │ └── impedance_controller.hpp ├── license.txt ├── python └── mim_control │ ├── __init__.py │ ├── dynamic_graph │ ├── __init__.py │ ├── go_to.py │ └── wbc_graph.py │ ├── impedance_controller.py │ ├── qp_solver.py │ ├── robot_centroidal_controller.py │ └── robot_impedance_controller.py ├── src ├── centroidal_force_qp_controller.cpp ├── centroidal_impedance_controller.cpp ├── centroidal_pd_controller.cpp ├── dynamic_graph │ ├── centroidal_force_qp_controller.cpp │ ├── centroidal_pd_controller.cpp │ ├── impedance_controller.cpp │ └── signal_utils.cpp └── impedance_controller.cpp └── srcpy ├── centroidal_force_qp_controller.cpp ├── centroidal_impedance_controller.cpp ├── centroidal_pd_controller.cpp ├── impedance_controller.cpp ├── mim_control.cpp └── wbc_dg_python_module.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.txt~ 2 | *.dat 3 | 4 | ### 5 | # Based on: 6 | ## Core latex/pdflatex auxiliary files: https://github.com/github/gitignore/blob/master/TeX.gitignore 7 | 8 | *.aux 9 | *.lof 10 | *.log 11 | *.lot 12 | *.fls 13 | *.out 14 | *.toc 15 | *.fmt 16 | *.fot 17 | *.cb 18 | *.cb2 19 | 20 | ## Intermediate documents: 21 | *.dvi 22 | *-converted-to.* 23 | # these rules might exclude image files for figures etc. 24 | # *.ps 25 | # *.eps 26 | # *.pdf 27 | 28 | ## Generated if empty string is given at "Please type another file name for output:" 29 | .pdf 30 | 31 | ## Bibliography auxiliary files (bibtex/biblatex/biber): 32 | *.bbl 33 | *.bcf 34 | *.blg 35 | *-blx.aux 36 | *-blx.bib 37 | *.run.xml 38 | 39 | ## Build tool auxiliary files: 40 | *.fdb_latexmk 41 | *.synctex 42 | *.synctex(busy) 43 | *.synctex.gz 44 | *.synctex.gz(busy) 45 | *.pdfsync 46 | 47 | ## Auxiliary and intermediate files from other packages: 48 | # algorithms 49 | *.alg 50 | *.loa 51 | 52 | # achemso 53 | acs-*.bib 54 | 55 | # amsthm 56 | *.thm 57 | 58 | # beamer 59 | *.nav 60 | *.pre 61 | *.snm 62 | *.vrb 63 | 64 | # changes 65 | *.soc 66 | 67 | # cprotect 68 | *.cpt 69 | 70 | # elsarticle (documentclass of Elsevier journals) 71 | *.spl 72 | 73 | # endnotes 74 | *.ent 75 | # fixme 76 | *.lox 77 | 78 | # feynmf/feynmp 79 | *.mf 80 | *.mp 81 | *.t[1-9] 82 | *.t[1-9][0-9] 83 | *.tfm 84 | 85 | #(r)(e)ledmac/(r)(e)ledpar 86 | *.end 87 | *.?end 88 | *.[1-9] 89 | *.[1-9][0-9] 90 | *.[1-9][0-9][0-9] 91 | *.[1-9]R 92 | *.[1-9][0-9]R 93 | *.[1-9][0-9][0-9]R 94 | *.eledsec[1-9] 95 | *.eledsec[1-9]R 96 | *.eledsec[1-9][0-9] 97 | *.eledsec[1-9][0-9]R 98 | *.eledsec[1-9][0-9][0-9] 99 | *.eledsec[1-9][0-9][0-9]R 100 | 101 | # glossaries 102 | *.acn 103 | *.acr 104 | *.glg 105 | *.glo 106 | *.gls 107 | *.glsdefs 108 | 109 | # gnuplottex 110 | *-gnuplottex-* 111 | 112 | # gregoriotex 113 | *.gaux 114 | *.gtex 115 | 116 | # hyperref 117 | *.brf 118 | 119 | # knitr 120 | *-concordance.tex 121 | # TODO Comment the next line if you want to keep your tikz graphics files 122 | *.tikz 123 | *-tikzDictionary 124 | 125 | # listings 126 | *.lol 127 | 128 | # makeidx 129 | *.idx 130 | *.ilg 131 | *.ind 132 | *.ist 133 | 134 | # minitoc 135 | *.maf 136 | *.mlf 137 | *.mlt 138 | *.mtc[0-9]* 139 | *.slf[0-9]* 140 | *.slt[0-9]* 141 | *.stc[0-9]* 142 | 143 | # minted 144 | _minted* 145 | *.pyg 146 | 147 | # morewrites 148 | *.mw 149 | 150 | # nomencl 151 | *.nlo 152 | 153 | # pax 154 | *.pax 155 | 156 | # pdfpcnotes 157 | *.pdfpc 158 | 159 | # sagetex 160 | *.sagetex.sage 161 | *.sagetex.py 162 | *.sagetex.scmd 163 | 164 | # scrwfile 165 | *.wrt 166 | 167 | # sympy 168 | *.sout 169 | *.sympy 170 | sympy-plots-for-*.tex/ 171 | 172 | # pdfcomment 173 | *.upa 174 | *.upb 175 | 176 | # pythontex 177 | *.pytxcode 178 | pythontex-files-*/ 179 | 180 | # thmtools 181 | *.loe 182 | 183 | # TikZ & PGF 184 | *.dpth 185 | *.md5 186 | *.auxlock 187 | 188 | # todonotes 189 | *.tdo 190 | 191 | # easy-todo 192 | *.lod 193 | 194 | # xindy 195 | *.xdy 196 | 197 | # xypic precompiled matrices 198 | *.xyc 199 | 200 | # endfloat 201 | *.ttt 202 | *.fff 203 | 204 | # Latexian 205 | TSWLatexianTemp* 206 | 207 | ## Editors: 208 | # WinEdt 209 | *.bak 210 | *.sav 211 | 212 | # Texpad 213 | .texpadtmp 214 | 215 | # Kile 216 | *.backup 217 | 218 | # KBibTeX 219 | *~[0-9]* 220 | 221 | # auto folder when using emacs and auctex 222 | /auto/* 223 | 224 | # expex forward references with \gathertags 225 | *-tags.tex 226 | 227 | 228 | ### 229 | # Based on: https://github.com/github/gitignore/blob/master/Global/macOS.gitignore 230 | 231 | *.DS_Store 232 | .AppleDouble 233 | .LSOverride 234 | 235 | # Icon must end with two \r 236 | Icon 237 | 238 | 239 | # Thumbnails 240 | ._* 241 | 242 | # Files that might appear in the root of a volume 243 | .DocumentRevisions-V100 244 | .fseventsd 245 | .Spotlight-V100 246 | .TemporaryItems 247 | .Trashes 248 | .VolumeIcon.icns 249 | .com.apple.timemachine.donotpresent 250 | 251 | # Directories potentially created on remote AFP share 252 | .AppleDB 253 | .AppleDesktop 254 | Network Trash Folder 255 | Temporary Items 256 | .apdisk 257 | 258 | 259 | ### 260 | # Based on: https://github.com/github/gitignore/blob/master/Python.gitignore 261 | 262 | # Byte-compiled / optimized / DLL files 263 | __pycache__/ 264 | *.py[cod] 265 | *$py.class 266 | 267 | # C extensions 268 | *.so 269 | 270 | # Distribution / packaging 271 | .Python 272 | env/ 273 | build/ 274 | develop-eggs/ 275 | dist/ 276 | downloads/ 277 | eggs/ 278 | .eggs/ 279 | lib/ 280 | lib64/ 281 | parts/ 282 | sdist/ 283 | var/ 284 | *.egg-info/ 285 | .installed.cfg 286 | *.egg 287 | 288 | # PyInstaller 289 | # Usually these files are written by a python script from a template 290 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 291 | *.manifest 292 | *.spec 293 | 294 | # Installer logs 295 | pip-log.txt 296 | pip-delete-this-directory.txt 297 | 298 | # Unit test / coverage reports 299 | htmlcov/ 300 | .tox/ 301 | .coverage 302 | .coverage.* 303 | .cache 304 | nosetests.xml 305 | coverage.xml 306 | *,cover 307 | .hypothesis/ 308 | 309 | # Translations 310 | *.mo 311 | *.pot 312 | 313 | # Django stuff: 314 | *.log 315 | local_settings.py 316 | 317 | # Flask stuff: 318 | instance/ 319 | .webassets-cache 320 | 321 | # Scrapy stuff: 322 | .scrapy 323 | 324 | # Sphinx documentation 325 | docs/_build/ 326 | 327 | # PyBuilder 328 | target/ 329 | 330 | # IPython Notebook 331 | .ipynb_checkpoints 332 | 333 | # pyenv 334 | .python-version 335 | 336 | # celery beat schedule file 337 | celerybeat-schedule 338 | 339 | # dotenv 340 | .env 341 | 342 | # virtualenv 343 | venv/ 344 | ENV/ 345 | 346 | # Spyder project settings 347 | .spyderproject 348 | 349 | # Rope project settings 350 | .ropeproject 351 | 352 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # BSD 3-clause Copyright (c) 2021, New York University and Max Planck 2 | # Gesellschaft 3 | 4 | # 5 | # set up the project 6 | # 7 | cmake_minimum_required(VERSION 3.10.2) 8 | 9 | project(mim_control) 10 | 11 | # specify the C++ 17 standard 12 | set(CMAKE_CXX_STANDARD 17) 13 | set(CMAKE_CXX_STANDARD_REQUIRED True) 14 | 15 | # 16 | # Search for dependencies. 17 | # 18 | 19 | # Local depends 20 | find_package(yaml_utils REQUIRED) 21 | find_package(mpi_cmake_modules REQUIRED) 22 | 23 | # External dependencies Extract major/minor python version 24 | find_package(Eigen3 REQUIRED) 25 | find_package(pinocchio REQUIRED) 26 | find_package(eiquadprog REQUIRED) 27 | find_package(pybind11 CONFIG REQUIRED) 28 | find_package(Python ${PYTHON_VERSION_STRING} REQUIRED) 29 | # Specific search of boost python as it is yet not trivial. 30 | search_for_boost_python() 31 | 32 | # Optionnal 33 | find_package(dynamic-graph QUIET) 34 | find_package(dynamic-graph-python QUIET) 35 | 36 | set(build_dynamic_graph_plugins FALSE) 37 | if(${dynamic-graph_FOUND} AND ${dynamic-graph-python_FOUND}) 38 | set(build_dynamic_graph_plugins TRUE) 39 | endif() 40 | 41 | # 42 | # Create the main library 43 | # 44 | 45 | # cmake-format: off 46 | add_library( 47 | ${PROJECT_NAME} SHARED 48 | src/impedance_controller.cpp 49 | src/centroidal_pd_controller.cpp 50 | src/centroidal_force_qp_controller.cpp 51 | src/centroidal_impedance_controller.cpp) 52 | # cmake-format: on 53 | target_link_libraries(${PROJECT_NAME} pinocchio::pinocchio) 54 | target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) 55 | target_link_libraries(${PROJECT_NAME} eiquadprog::eiquadprog) 56 | target_link_libraries(${PROJECT_NAME} yaml_utils::yaml_utils) 57 | 58 | # Includes. Add the include dependencies 59 | target_include_directories( 60 | ${PROJECT_NAME} PUBLIC $ 61 | $) 62 | 63 | # 64 | # Build the dynamic graph plugins if the dependencies are found. 65 | # 66 | if(${build_dynamic_graph_plugins}) 67 | # plugin name 68 | set(plugin_name wbc) 69 | # Create the library 70 | add_library( 71 | ${plugin_name} SHARED 72 | src/dynamic_graph/signal_utils.cpp 73 | src/dynamic_graph/impedance_controller.cpp 74 | src/dynamic_graph/centroidal_pd_controller.cpp 75 | src/dynamic_graph/centroidal_force_qp_controller.cpp) 76 | # Add the include dependencies. 77 | target_include_directories( 78 | ${plugin_name} PUBLIC $ 79 | $ 80 | ${pybind11_INCLUDE_DIRS}) 81 | # Link the dependencies. 82 | target_link_libraries(${plugin_name} ${PROJECT_NAME}) 83 | target_link_libraries(${plugin_name} dynamic-graph::dynamic-graph) 84 | target_link_libraries(${plugin_name} 85 | dynamic-graph-python::dynamic-graph-python) 86 | # Install the target and it's python bindings. 87 | install_dynamic_graph_plugin_python_bindings(${plugin_name}) 88 | # Install the plugin. 89 | get_dynamic_graph_plugin_install_path(plugin_install_path) 90 | install( 91 | TARGETS ${plugin_name} 92 | EXPORT ${PROJECT_NAME}Targets 93 | LIBRARY DESTINATION ${plugin_install_path} 94 | ARCHIVE DESTINATION ${plugin_install_path} 95 | RUNTIME DESTINATION ${plugin_install_path} 96 | INCLUDES 97 | DESTINATION include) 98 | endif() 99 | 100 | # 101 | # Install the package 102 | # 103 | 104 | # command to install the library and binaries 105 | install( 106 | TARGETS ${PROJECT_NAME} 107 | EXPORT ${PROJECT_NAME}Targets 108 | LIBRARY DESTINATION lib 109 | ARCHIVE DESTINATION lib 110 | RUNTIME DESTINATION bin 111 | INCLUDES 112 | DESTINATION include) 113 | 114 | # we also need to install the header files 115 | install(DIRECTORY include/ DESTINATION include) 116 | 117 | # 118 | # python bindings with pybind11# 119 | # 120 | 121 | # cmake-format: off 122 | pybind11_add_module(${PROJECT_NAME}_cpp MODULE 123 | srcpy/${PROJECT_NAME}.cpp 124 | srcpy/impedance_controller.cpp 125 | srcpy/centroidal_pd_controller.cpp 126 | srcpy/centroidal_force_qp_controller.cpp 127 | srcpy/centroidal_impedance_controller.cpp) 128 | # cmake-format: on 129 | target_link_libraries(${PROJECT_NAME}_cpp PRIVATE pybind11::module) 130 | target_link_libraries(${PROJECT_NAME}_cpp PRIVATE ${PROJECT_NAME}) 131 | target_link_boost_python(${PROJECT_NAME}_cpp PRIVATE) 132 | 133 | # install the bindings 134 | get_python_install_dir(PYTHON_INSTALL_DIR) 135 | install(TARGETS ${PROJECT_NAME}_cpp DESTINATION ${PYTHON_INSTALL_DIR}) 136 | 137 | # 138 | # install python packages 139 | # 140 | 141 | # install the python package too 142 | install( 143 | DIRECTORY python/${PROJECT_NAME} 144 | DESTINATION "${PYTHON_INSTALL_DIR}" 145 | PATTERN "*.pyc" EXCLUDE 146 | PATTERN "__pycache__" EXCLUDE) 147 | 148 | # 149 | # building documentation 150 | # 151 | add_documentation() 152 | 153 | # 154 | # create the cmake package 155 | # 156 | generate_cmake_package(INSTALL_EXPORT) 157 | -------------------------------------------------------------------------------- /Config.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include("${CMAKE_CURRENT_LIST_DIR}/mim_controlTargets.cmake") 4 | 5 | include(CMakeFindDependencyMacro) 6 | 7 | # we do not add the other dependencies because these are header files lib 8 | find_dependency(pybind11 CONFIG REQUIRED) 9 | find_dependency(Eigen3 REQUIRED) 10 | find_dependency(pinocchio REQUIRED) 11 | find_dependency(eiquadprog REQUIRED) 12 | find_dependency(yaml_utils REQUIRED) 13 | 14 | check_required_components(mim_control) 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | mim_control 2 | ----------- 3 | 4 | This packages contains generic controllers that can be used for a biped, quadruped, hopper and any other robot. The controllers are created based on the nature of the robot and its properties such as number of end effectors etc.. 5 | 6 | ### Installation 7 | 8 | #### Dependencies 9 | ``` 10 | - Pinocchio 11 | - PyBullet 12 | - Quadprog 13 | - Matplotlib (Optional, needed to run demos) 14 | - BulletUtils (Optional, needed to run demos) 15 | - Robot_Properties_Solo (Optional, needed to run demos) 16 | - Robot_Properties_Bolt (Optional, needed to run demos) 17 | ``` 18 | #### Download the package 19 | 20 | External dependencies: 21 | See [this tutorial](https://github.com/machines-in-motion/machines-in-motion.github.io/wiki/laas_package_from_binaries) 22 | in order to install `eiquadprog` and `pinocchio`. 23 | See [treep](https://gitlab.is.tue.mpg.de/amd-clmc/treep) and [colcon](https://github.com/machines-in-motion/machines-in-motion.github.io/wiki/use_colcon) for their usage. 24 | 25 | Local and specific dependencies and the actual repo we need to compile: 26 | ``` 27 | mkdir devel 28 | cd devel 29 | git clone git@github.com:machines-in-motion/treep_machines_in_motion 30 | pip install -U treep 31 | treep --clone MIM_CONTROL 32 | ``` 33 | 34 | #### Build the package 35 | 36 | We use [colcon](https://github.com/machines-in-motion/machines-in-motion.github.io/wiki/use_colcon) 37 | to build this package: 38 | 39 | ``` 40 | cd devel/workspace 41 | colcon build 42 | ``` 43 | 44 | ### Usage 45 | 46 | #### Running Demos 47 | 48 | To run the demos, please install the following additinal packages: 49 | 50 | ``` 51 | treep --clone bullet_utils 52 | treep --clone robot_properties_solo 53 | treep --clone robot_properties_bolt 54 | ``` 55 | 56 | rebuild your workspace using `colcon build` and source it. 57 | 58 | To run the impedance controller on Solo12 follow the below mentioned steps 59 | ``` 60 | source /opt/openrobots/setup.bash (source open robots) 61 | cd demo 62 | python3 demo_robot_impedance.py 63 | ``` 64 | 65 | To run the Center of Mass controller on Solo12 follow the below mentioned steps 66 | ``` 67 | source /opt/openrobots/setup.bash (source open robots) 68 | cd demo 69 | python3 demo_robot_com_ctrl.py 70 | ``` 71 | 72 | ### Cite 73 | ``` 74 | @article{grimminger2020open, 75 | title={An open torque-controlled modular robot architecture for legged locomotion research}, 76 | author={Grimminger, Felix and Meduri, Avadesh and Khadiv, Majid and Viereck, Julian and W{\"u}thrich, Manuel and Naveau, Maximilien and Berenz, Vincent and Heim, Steve and Widmaier, Felix and Flayols, Thomas and others}, 77 | journal={IEEE Robotics and Automation Letters}, 78 | volume={5}, 79 | number={2}, 80 | pages={3650--3657}, 81 | year={2020}, 82 | publisher={IEEE} 83 | } 84 | ``` 85 | 86 | ### License and Copyrights 87 | 88 | BSD 3-Clause License 89 | 90 | Copyright(c) 2019-2020 New York University, Max Planck Gesellschaft 91 | -------------------------------------------------------------------------------- /colcon.pkg: -------------------------------------------------------------------------------- 1 | { 2 | "test-dependencies": ["googletest-distribution", "robot_properties_solo", "robot_properties_bolt", "robot_properties_teststand"] 3 | } 4 | -------------------------------------------------------------------------------- /demos/demo_dg_whole_body_controller.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 16, 2021 8 | """ 9 | 10 | import numpy as np 11 | 12 | np.set_printoptions(suppress=True, precision=3) 13 | 14 | from robot_properties_solo.config import Solo12Config 15 | from dg_blmc_robots.solo.solo12_bullet import get_solo12_robot 16 | 17 | from mim_control.dynamic_graph.wbc_graph import WholeBodyController 18 | 19 | if __name__ == "__main__": 20 | pin_robot = Solo12Config.buildRobotWrapper() 21 | 22 | qp_penalty_weights = np.array([5e5, 5e5, 5e5, 1e6, 1e6, 1e6]) 23 | 24 | ### 25 | # Create the whole body controller. 26 | wbc = WholeBodyController( 27 | "test_wbc", 28 | pin_robot, 29 | Solo12Config.end_effector_names, 30 | 0.2, 31 | qp_penalty_weights, 32 | ) 33 | 34 | ### 35 | # Specify gains for the controller. 36 | x_des = np.array( 37 | [ 38 | 0.195, 39 | 0.147, 40 | 0.015, 41 | 0.195, 42 | -0.147, 43 | 0.015, 44 | -0.195, 45 | 0.147, 46 | 0.015, 47 | -0.195, 48 | -0.147, 49 | 0.015, 50 | ] 51 | ).reshape(4, 3) 52 | 53 | # For the centroidal controllers. 54 | wbc.kc_sin.value = np.array([100.0, 100.0, 100.0]) 55 | wbc.dc_sin.value = np.array([15.0, 15.0, 15.0]) 56 | wbc.kb_sin.value = np.array([25.0, 50.0, 25.0]) 57 | wbc.db_sin.value = np.array([10.0, 10.0, 10.0]) 58 | 59 | wbc.des_com_pos_sin.value = np.array([0.0, 0.0, 0.20]) 60 | wbc.des_com_vel_sin.value = np.zeros(3) 61 | wbc.des_ori_pos_sin.value = np.array([0.0, 0.0, 0.0, 1.0]) 62 | wbc.des_ori_vel_sin.value = np.zeros(3) 63 | 64 | wbc.cnt_array_sin.value = np.array([1.0, 1.0, 1.0, 1.0]) 65 | 66 | # Impedance controllers. 67 | for i, imp in enumerate(wbc.imps): 68 | imp.gain_proportional_sin.value = np.array( 69 | [50.0, 50.0, 50.0, 0.0, 0.0, 0.0] 70 | ) 71 | imp.gain_derivative_sin.value = np.array( 72 | [0.7, 0.7, 0.7, 0.0, 0.0, 0.0] 73 | ) 74 | imp.desired_end_frame_placement_sin.value = np.hstack( 75 | [x_des[i], np.zeros(4)] 76 | ) 77 | imp.desired_end_frame_velocity_sin.value = np.zeros(6) 78 | imp.gain_feed_forward_force_sin.value = 1.0 79 | 80 | wbc.w_com_ff_sin.value = np.array([0.0, 0.0, 9.81 * 2.5, 0.0, 0.0, 0.0]) 81 | 82 | ### 83 | # Create the simulated robot 84 | robot = get_solo12_robot() 85 | 86 | # Change the position of the robot. 87 | q0 = Solo12Config.q0 88 | q0[0] = 0.0 89 | robot.reset_state(q0, Solo12Config.v0) 90 | 91 | # Plug the simulated robot to the controller. 92 | base_signals = robot.base_signals() 93 | wbc.plug(robot, base_signals[0], base_signals[1]) 94 | 95 | # Simulate for 4 seconds. 96 | robot.run(4000, sleep=True) 97 | -------------------------------------------------------------------------------- /demos/demo_impedance_ctrl_bouncing_cpp.py: -------------------------------------------------------------------------------- 1 | """demo_impedance_ctrl_cpp 2 | 3 | This file is the impedance controller demo for solo12 using the C++ code. 4 | 5 | License BSD-3-Clause 6 | Copyright (c) 2021, New York University and Max Planck Gesellschaft. 7 | 8 | Author: Maximilien Naveau 9 | """ 10 | 11 | import numpy as np 12 | 13 | np.set_printoptions(precision=2, suppress=True) 14 | import pinocchio 15 | from bullet_utils.env import BulletEnvWithGround 16 | import pybullet 17 | from robot_properties_solo.solo12wrapper import Solo12Robot, Solo12Config 18 | from mim_control_cpp import ImpedanceController 19 | 20 | 21 | if __name__ == "__main__": 22 | 23 | # Create a Pybullet simulation environment 24 | env = BulletEnvWithGround() 25 | robot = Solo12Robot() 26 | robot = env.add_robot(robot) 27 | pybullet.resetDebugVisualizerCamera(1.3, 100, -35, (0.0, 0.0, 0.0)) 28 | RobotConfig = Solo12Config 29 | pin_robot = robot.pin_robot 30 | 31 | # Create the control vector 32 | q_null = np.zeros(pin_robot.nq) 33 | tau = np.zeros(robot.nb_dof) 34 | tau_imp = np.zeros(robot.nb_dof) 35 | tau_pd = np.zeros(robot.nb_dof) 36 | 37 | # ###################### impedance controller demo ######################## 38 | root_names = ["base_link", "base_link", "base_link", "base_link"] 39 | endeff_names = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"] 40 | ctrls = [ImpedanceController() for _ in endeff_names] 41 | for i, ctrl in enumerate(ctrls): 42 | ctrl.initialize(pin_robot.model, root_names[i], endeff_names[i]) 43 | 44 | # Des state and gains 45 | x_des = [ 46 | pinocchio.SE3(np.identity(3), np.array([0.195, 0.147, -0.2])), 47 | pinocchio.SE3(np.identity(3), np.array([0.195, -0.147, -0.2])), 48 | pinocchio.SE3(np.identity(3), np.array([-0.195, 0.147, -0.2])), 49 | pinocchio.SE3(np.identity(3), np.array([-0.195, -0.147, -0.2])), 50 | ] 51 | xd_des = pinocchio.Motion.Zero() 52 | kp = np.array([200, 200, 200, 0, 0, 0]) 53 | kd = np.array([5.0, 5.0, 5.0, 0, 0, 0]) 54 | f = pinocchio.Force.Zero() 55 | 56 | end_eff_ids = [ 57 | pin_robot.model.getFrameId(endeff_name) for endeff_name in endeff_names 58 | ] 59 | 60 | # Reset the robot to some initial state. 61 | q0 = np.array(RobotConfig.initial_configuration) 62 | dq0 = np.array(RobotConfig.initial_velocity) 63 | q0[0] += 0.0 64 | q0[1] += 0.0 65 | q0[2] += 0.5 66 | q0[3:7] = np.array([0.0, 0.0, 0.0, 1.0]) 67 | robot.reset_state(q0, dq0) 68 | 69 | time = 0 70 | for i in range(2000): 71 | q, dq = robot.get_state() 72 | tau.fill(0) 73 | tau_imp.fill(0) 74 | tau_pd.fill(0) 75 | 76 | tau_pd[:] = -2.0 * q[7:] - 0.1 * dq[6:] 77 | 78 | for i, ctrl in enumerate(ctrls): 79 | ctrl.run( 80 | q, 81 | dq, 82 | kp, 83 | kd, 84 | 0.0, 85 | x_des[i], 86 | xd_des, 87 | f, 88 | ) 89 | tau_imp += ctrl.get_joint_torques() 90 | 91 | for i in range(robot.nb_dof): 92 | if tau_imp[i] != 0.0: 93 | tau[i] = tau_imp[i] 94 | else: 95 | tau[i] = tau_pd[i] 96 | 97 | time += 1 98 | robot.send_joint_command(tau) 99 | env.step() 100 | -------------------------------------------------------------------------------- /demos/demo_impedance_ctrl_cpp.py: -------------------------------------------------------------------------------- 1 | """demo_impedance_ctrl_cpp 2 | 3 | This file is the impedance controller demo for solo12 using the C++ code. 4 | 5 | Author: Julian 6 | Date: 01/21/2021 7 | """ 8 | 9 | import numpy as np 10 | 11 | np.set_printoptions(precision=2, suppress=True) 12 | import pinocchio 13 | from bullet_utils.env import BulletEnvWithGround 14 | import pybullet 15 | from robot_properties_solo.solo12wrapper import Solo12Robot, Solo12Config 16 | from mim_control_cpp import ImpedanceController 17 | 18 | 19 | if __name__ == "__main__": 20 | 21 | # Create a Pybullet simulation environment 22 | env = BulletEnvWithGround() 23 | robot = Solo12Robot(useFixedBase=True) 24 | robot = env.add_robot(robot) 25 | pybullet.resetDebugVisualizerCamera(1.3, 100, -35, (0.0, 0.0, 0.0)) 26 | RobotConfig = Solo12Config 27 | pin_robot = robot.pin_robot 28 | 29 | # Create the control vector 30 | q_null = np.zeros(pin_robot.nq) 31 | tau = np.zeros(robot.nb_dof) 32 | tau_imp = np.zeros(robot.nb_dof) 33 | tau_pd = np.zeros(robot.nb_dof) 34 | 35 | # ###################### impedance controller demo ######################## 36 | root_names = ["base_link", "base_link", "base_link", "base_link"] 37 | endeff_names = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"] 38 | ctrls = [ImpedanceController() for _ in endeff_names] 39 | for i, ctrl in enumerate(ctrls): 40 | ctrl.initialize(pin_robot.model, root_names[i], endeff_names[i]) 41 | 42 | # Des state and gains 43 | x_des = [ 44 | pinocchio.SE3(np.identity(3), np.array([0.195, 0.147, -0.2])), 45 | pinocchio.SE3(np.identity(3), np.array([0.195, -0.147, -0.2])), 46 | pinocchio.SE3(np.identity(3), np.array([-0.195, 0.147, -0.2])), 47 | pinocchio.SE3(np.identity(3), np.array([-0.195, -0.147, -0.2])), 48 | ] 49 | xd_des = pinocchio.Motion.Zero() 50 | kp = np.array([70, 70, 70, 0, 0, 0]) 51 | kd = np.array([20.0, 20.0, 20.0, 0, 0, 0]) 52 | f = pinocchio.Force.Zero() 53 | 54 | end_eff_ids = [ 55 | pin_robot.model.getFrameId(endeff_name) for endeff_name in endeff_names 56 | ] 57 | 58 | time = 0 59 | N = 10 60 | for n in range(N + 1): 61 | # Reset the robot to some initial state. 62 | q0 = np.array(RobotConfig.initial_configuration) 63 | dq0 = np.array(RobotConfig.initial_velocity) 64 | 65 | q0[0] += 0.0 66 | q0[1] += 0.2 67 | q0[2] += 0.2 68 | q0[3:7] = pinocchio.Quaternion( 69 | pinocchio.rpy.rpyToMatrix( 70 | np.array([float(n) / float(N) * 2 * np.pi, 0, 0]) 71 | ) 72 | ).coeffs() 73 | 74 | robot.reset_state(q0, dq0) 75 | 76 | for i in range(300): 77 | q, dq = robot.get_state() 78 | tau.fill(0) 79 | tau_imp.fill(0) 80 | tau_pd.fill(0) 81 | 82 | tau_pd[:] = -2.0 * q[7:] - 0.1 * dq[6:] 83 | 84 | for i, ctrl in enumerate(ctrls): 85 | ctrl.run( 86 | q, 87 | dq, 88 | kp, 89 | kd, 90 | 0.0, 91 | x_des[i], 92 | xd_des, 93 | f, 94 | ) 95 | tau_imp += ctrl.get_joint_torques() 96 | 97 | for i in range(robot.nb_dof): 98 | if tau_imp[i] != 0.0: 99 | tau[i] = tau_imp[i] 100 | else: 101 | tau[i] = tau_pd[i] 102 | 103 | time += 1 104 | robot.send_joint_command(tau) 105 | env.step() 106 | -------------------------------------------------------------------------------- /demos/demo_impedance_ctrl_python.py: -------------------------------------------------------------------------------- 1 | """demo_impedance_ctrl_python.py 2 | 3 | This file is the centroidal controller demo for solo12 using the C++ code. 4 | 5 | License BSD-3-Clause 6 | Copyright (c) 2021, New York University and Max Planck Gesellschaft. 7 | 8 | Author: Maximilien Naveau 9 | """ 10 | 11 | import numpy as np 12 | import pinocchio as pin 13 | from bullet_utils.env import BulletEnvWithGround 14 | from robot_properties_solo.solo12wrapper import Solo12Robot, Solo12Config 15 | from mim_control.impedance_controller import ImpedanceController 16 | 17 | 18 | if __name__ == "__main__": 19 | 20 | # Create a Pybullet simulation environment 21 | env = BulletEnvWithGround() 22 | robot = Solo12Robot() 23 | robot = env.add_robot(robot) 24 | RobotConfig = Solo12Config 25 | pin_robot = robot.pin_robot 26 | 27 | # Create the control vector 28 | tau = np.zeros(robot.nb_dof) 29 | 30 | # ###################### impedance controller demo ################################# 31 | root_names = ["base_link", "base_link", "base_link", "base_link"] 32 | endeff_names = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"] 33 | start_column = [6, 9, 12, 15] 34 | ctrls = [ 35 | ImpedanceController( 36 | endeff_names[i], 37 | pin_robot, 38 | root_names[i], 39 | endeff_names[i], 40 | start_column[i], 41 | [True, True, True], 42 | ) 43 | for i, _ in enumerate(endeff_names) 44 | ] 45 | 46 | # Des state and gains 47 | z_des = -0.2 48 | x_des = np.array( 49 | [ 50 | 0.195, 51 | 0.147, 52 | z_des, 53 | 0.195, 54 | -0.147, 55 | z_des, 56 | -0.195, 57 | 0.147, 58 | z_des, 59 | -0.195, 60 | -0.147, 61 | z_des, 62 | ] 63 | ) 64 | xd_des = np.zeros(3) 65 | kp = [100, 100, 100] 66 | kd = [10, 10, 10] 67 | f = np.zeros(3) 68 | 69 | end_eff_ids = [ 70 | pin_robot.model.getFrameId(endeff_name) for endeff_name in endeff_names 71 | ] 72 | 73 | q0 = np.array(RobotConfig.initial_configuration) 74 | dq0 = np.array(RobotConfig.initial_velocity) 75 | 76 | q0[0] = 0.0 77 | q0[1] = 0.0 78 | q0[2] = -z_des 79 | q0[3:7] = np.array([0, 0, 0, 1]) 80 | 81 | robot.reset_state(q0, dq0) 82 | for i in range(10000): 83 | q, dq = robot.get_state() 84 | tau.fill(0) 85 | # 86 | for i, ctrl in enumerate(ctrls): 87 | tau_out = np.array( 88 | ctrl.compute_impedance_torques_world( 89 | q, 90 | dq, 91 | kp, 92 | kd, 93 | x_des[3 * i : 3 * (i + 1)], 94 | xd_des, 95 | f, 96 | ) 97 | ) 98 | tau[start_column[i] - 6 : start_column[i] - 6 + 3] = tau_out 99 | # 100 | robot.send_joint_command(tau) 101 | env.step() 102 | -------------------------------------------------------------------------------- /demos/demo_robot_com_ctrl.py: -------------------------------------------------------------------------------- 1 | """Centroidal controller demo for a given robot (solo in this case) 2 | 3 | License BSD-3-Clause 4 | Copyright (c) 2020, New York University and Max Planck Gesellschaft. 5 | 6 | Author: Avadesh Meduri 7 | """ 8 | 9 | import argparse 10 | import numpy as np 11 | from mim_control.robot_impedance_controller import RobotImpedanceController 12 | from mim_control.robot_centroidal_controller import RobotCentroidalController 13 | from bullet_utils.env import BulletEnvWithGround 14 | from robot_properties_solo.solo12wrapper import Solo12Robot, Solo12Config 15 | from robot_properties_bolt.bolt_wrapper import BoltRobot, BoltConfig 16 | 17 | 18 | def demo(robot_name): 19 | 20 | # Create a Pybullet simulation environment 21 | env = BulletEnvWithGround() 22 | 23 | # Create a robot instance in the simulator. 24 | if robot_name == "solo": 25 | robot = Solo12Robot() 26 | robot = env.add_robot(robot) 27 | robot_config = Solo12Config() 28 | mu = 0.2 29 | kc = [200, 200, 200] 30 | dc = [5, 5, 5] 31 | kb = [200, 200, 200] 32 | db = [1.0, 1.0, 1.0] 33 | qp_penalty_lin = 3 * [1e6] 34 | qp_penalty_ang = 3 * [1e6] 35 | elif robot_name == "bolt": 36 | robot = env.add_robot(BoltRobot) 37 | robot_config = BoltConfig() 38 | mu = 0.2 39 | kc = [0, 0, 100] 40 | dc = [0, 0, 10] 41 | kb = [100, 100, 100] 42 | db = [10.0, 10.0, 10.0] 43 | qp_penalty_lin = [1, 1, 1e6] 44 | qp_penalty_ang = [1e6, 1e6, 1] 45 | else: 46 | raise RuntimeError( 47 | "Robot name [" + str(robot_name) + "] unknown. " 48 | "Try 'solo' or 'bolt'" 49 | ) 50 | 51 | # Initialize control 52 | tau = np.zeros(robot.nb_dof) 53 | 54 | # Reset the robot to some initial state. 55 | q0 = np.matrix(robot_config.initial_configuration).T 56 | q0[0] = 0.0 57 | dq0 = np.matrix(robot_config.initial_velocity).T 58 | robot.reset_state(q0, dq0) 59 | 60 | # Desired center of mass position and velocity. 61 | x_com = [0.0, 0.0, 0.18] 62 | xd_com = [0.0, 0.0, 0.0] 63 | # The base should be flat. 64 | x_ori = [0.0, 0.0, 0.0, 1.0] 65 | x_angvel = [0.0, 0.0, 0.0] 66 | # Alle end-effectors are in contact. 67 | cnt_array = robot.nb_ee * [1] 68 | 69 | # Impedance controller gains 70 | kp = robot.nb_ee * [0.0, 0.0, 0.0] # Disable for now 71 | kd = robot.nb_ee * [0.0, 0.0, 0.0] 72 | x_des = robot.nb_ee * [0.0, 0.0, -q0[2].item()] # Desired leg length 73 | xd_des = robot.nb_ee * [0.0, 0.0, 0.0] 74 | 75 | config_file = robot_config.ctrl_path 76 | robot_cent_ctrl = RobotCentroidalController( 77 | robot_config, 78 | mu=mu, 79 | kc=kc, 80 | dc=dc, 81 | kb=kb, 82 | db=db, 83 | qp_penalty_lin=qp_penalty_lin, 84 | qp_penalty_ang=qp_penalty_ang, 85 | ) 86 | robot_leg_ctrl = RobotImpedanceController(robot, config_file) 87 | 88 | # Run the simulator for 100 steps 89 | for _ in range(4000): 90 | # Step the simulator. 91 | env.step( 92 | sleep=True 93 | ) # You can sleep here if you want to slow down the replay 94 | # Read the final state and forces after the stepping. 95 | q, dq = robot.get_state() 96 | # computing forces to be applied in the centroidal space 97 | w_com = robot_cent_ctrl.compute_com_wrench( 98 | q, dq, x_com, xd_com, x_ori, x_angvel 99 | ) 100 | # distributing forces to the active end effectors 101 | F = robot_cent_ctrl.compute_force_qp(q, dq, cnt_array, w_com) 102 | # passing forces to the impedance controller 103 | tau = robot_leg_ctrl.return_joint_torques( 104 | q, dq, kp, kd, x_des, xd_des, F 105 | ) 106 | # passing torques to the robot 107 | robot.send_joint_command(tau) 108 | 109 | 110 | if __name__ == "__main__": 111 | 112 | parser = argparse.ArgumentParser() 113 | parser.add_argument( 114 | "--solo", help="Demonstrate Solo.", action="store_true" 115 | ) 116 | parser.add_argument( 117 | "--bolt", help="Demonstrate Bolt.", action="store_true" 118 | ) 119 | args = parser.parse_args() 120 | if args.solo: 121 | robot_name = "solo" 122 | elif args.bolt: 123 | robot_name = "bolt" 124 | else: 125 | robot_name = "solo" 126 | 127 | demo(robot_name) 128 | -------------------------------------------------------------------------------- /demos/demo_robot_com_ctrl_cpp.py: -------------------------------------------------------------------------------- 1 | """This file is the centroidal controller demo for solo12 using the C++ code. 2 | 3 | License BSD-3-Clause 4 | Copyright (c) 2020, New York University and Max Planck Gesellschaft. 5 | 6 | Author: Julian Viereck 7 | """ 8 | 9 | import argparse 10 | import numpy as np 11 | import pinocchio 12 | from mim_control_cpp import ( 13 | ImpedanceController, 14 | CentroidalPDController, 15 | CentroidalForceQPController, 16 | ) 17 | from bullet_utils.env import BulletEnvWithGround 18 | from robot_properties_solo.solo12wrapper import Solo12Robot, Solo12Config 19 | # from robot_properties_bolt.bolt_wrapper import BoltRobot, BoltConfig 20 | 21 | import time 22 | 23 | def demo(robot_name): 24 | 25 | # Create a Pybullet simulation environment 26 | env = BulletEnvWithGround() 27 | 28 | # Create a robot instance in the simulator. 29 | if robot_name == "solo": 30 | robot = Solo12Robot() 31 | robot = env.add_robot(robot) 32 | robot_config = Solo12Config() 33 | mu = 0.2 34 | kc = [200, 200, 200] 35 | dc = [50, 50, 50] 36 | kb = [100, 100, 200] 37 | db = [50.0, 50.0, 200.0] 38 | qp_penalty_weights = [5e5, 5e5, 5e5, 1e6, 1e6, 1e6] 39 | elif robot_name == "bolt": 40 | robot = BoltRobot() 41 | robot = env.add_robot(robot) 42 | robot_config = BoltConfig() 43 | mu = 0.2 44 | kc = [0, 0, 100] 45 | dc = [0, 0, 10] 46 | kb = [100, 100, 100] 47 | db = [10.0, 10.0, 10.0] 48 | qp_penalty_weights = [1, 1, 1e6, 1e6, 1e6, 1] 49 | else: 50 | raise RuntimeError( 51 | "Robot name [" + str(robot_name) + "] unknown. " 52 | "Try 'solo' or 'bolt'" 53 | ) 54 | 55 | # alias 56 | pin_robot = robot.pin_robot 57 | 58 | # Create impedance controllers 59 | root_name = "universe" 60 | endeff_names = robot_config.end_effector_names 61 | end_eff_ids = [ 62 | pin_robot.model.getFrameId(ee_name) for ee_name in endeff_names 63 | ] 64 | imp_ctrls = [ImpedanceController() for _ in endeff_names] 65 | for i, c in enumerate(imp_ctrls): 66 | c.initialize(robot.pin_robot.model, root_name, endeff_names[i]) 67 | 68 | # create centroidal controller 69 | q_init = np.zeros(pin_robot.nq) 70 | q_init[7] = 1 71 | centrl_pd_ctrl = CentroidalPDController() 72 | centrl_pd_ctrl.initialize( 73 | 2.5, np.diag(robot.pin_robot.mass(q_init)[3:6, 3:6]) 74 | ) 75 | force_qp = CentroidalForceQPController() 76 | force_qp.initialize(robot.nb_ee, mu, qp_penalty_weights) 77 | 78 | # Reset the robot to some initial state. 79 | q0 = np.matrix(robot_config.initial_configuration).T 80 | q0[0] = 0.0 81 | dq0 = np.matrix(robot_config.initial_velocity).T 82 | robot.reset_state(q0, dq0) 83 | 84 | # Desired center of mass position and velocity. 85 | x_com = [0.0, 0.0, 0.25] 86 | xd_com = [0.0, 0.0, 0.0] 87 | # The base should be flat. 88 | x_ori = [0.0, 0.0, 0.0, 1.0] 89 | x_angvel = [0.0, 0.0, 0.0] 90 | # Alle end-effectors are in contact. 91 | cnt_array = robot.nb_ee * [1] 92 | 93 | # impedance gains 94 | kp = np.array([200, 200, 200, 0, 0, 0]) 95 | kd = np.array([10.0, 10.0, 10.0, 0, 0, 0]) 96 | 97 | # Desired leg length 98 | x_des = [ 99 | 0.195, 100 | 0.147, 101 | 0.015, 102 | 0.195, 103 | -0.147, 104 | 0.015, 105 | -0.195, 106 | 0.147, 107 | 0.015, 108 | -0.195, 109 | -0.147, 110 | 0.015, 111 | ] 112 | xd_des = pinocchio.Motion(np.zeros(3), np.zeros(3)) 113 | 114 | # Run the simulator for N steps 115 | N = 4000 116 | dur = 0. 117 | for _ in range(N): 118 | 119 | # Read the final state and forces after the stepping. 120 | q, dq = robot.get_state() 121 | 122 | quat = pinocchio.Quaternion(q[6], q[3], q[4], q[5]) 123 | quat.normalize() 124 | 125 | start = time.time() 126 | 127 | # computing forces to be applied in the centroidal space 128 | centrl_pd_ctrl.run( 129 | kc, 130 | dc, 131 | kb, 132 | db, 133 | q[:3], 134 | x_com, 135 | quat.toRotationMatrix().dot(dq[:3]), # local to world frame 136 | xd_com, 137 | q[3:7], 138 | x_ori, 139 | dq[3:6], 140 | x_angvel, 141 | ) 142 | 143 | w_com = np.zeros(6) 144 | w_com[2] += 9.81 * 2.5 145 | w_com += centrl_pd_ctrl.get_wrench() 146 | 147 | # distributing forces to the active end effectors 148 | pin_robot.framesForwardKinematics(q) 149 | com = pin_robot.com(q) 150 | rel_eff = np.array( 151 | [pin_robot.data.oMf[i].translation - com for i in end_eff_ids] 152 | ).reshape(-1) 153 | force_qp.run(w_com, rel_eff, cnt_array) 154 | ee_forces = force_qp.get_forces() 155 | 156 | # passing forces to the impedance controller 157 | tau = np.zeros(robot.nb_dof) 158 | for i, imp_ctrl in enumerate(imp_ctrls): 159 | imp_ctrl.run( 160 | q, 161 | dq, 162 | kp, 163 | kd, 164 | 1.0, 165 | pinocchio.SE3(np.eye(3), np.array(x_des[3 * i : 3 * (i + 1)])), 166 | pinocchio.Motion(xd_des), 167 | pinocchio.Force( 168 | np.array(ee_forces[3 * i : 3 * (i + 1)]), np.zeros(3) 169 | ), 170 | ) 171 | tau += imp_ctrl.get_joint_torques() 172 | 173 | dur += time.time() - start 174 | 175 | # passing torques to the robot 176 | robot.send_joint_command(tau) 177 | # Step the simulator. 178 | env.step( 179 | sleep=False 180 | ) # You can sleep here if you want to slow down the replay 181 | 182 | print('Control path: %0.3f ms' % (dur * 1000. / N)) 183 | 184 | 185 | if __name__ == "__main__": 186 | 187 | parser = argparse.ArgumentParser() 188 | parser.add_argument( 189 | "--solo", help="Demonstrate Solo.", action="store_true" 190 | ) 191 | parser.add_argument( 192 | "--bolt", help="Demonstrate Bolt.", action="store_true" 193 | ) 194 | args = parser.parse_args() 195 | if args.solo: 196 | robot_name = "solo" 197 | elif args.bolt: 198 | robot_name = "bolt" 199 | else: 200 | robot_name = "solo" 201 | 202 | demo(robot_name) 203 | -------------------------------------------------------------------------------- /demos/demo_robot_com_imp_ctrl_cpp.py: -------------------------------------------------------------------------------- 1 | """This file is the centroidal controller demo for solo12 using the C++ code. 2 | 3 | License BSD-3-Clause 4 | Copyright (c) 2020, New York University and Max Planck Gesellschaft. 5 | 6 | Author: Julian Viereck 7 | """ 8 | 9 | import argparse 10 | import numpy as np 11 | import pinocchio 12 | from mim_control_cpp import ( 13 | CentroidalImpedanceController 14 | ) 15 | from bullet_utils.env import BulletEnvWithGround 16 | from robot_properties_solo.solo12wrapper import Solo12Robot, Solo12Config 17 | 18 | import time 19 | 20 | def demo(robot_name): 21 | # Create a Pybullet simulation environment 22 | env = BulletEnvWithGround() 23 | 24 | # Create a robot instance in the simulator. 25 | robot = Solo12Robot() 26 | robot = env.add_robot(robot) 27 | robot_config = Solo12Config() 28 | mu = 0.2 29 | kc = np.array([200, 200, 200]) 30 | dc = np.array([50, 50, 50]) 31 | kb = np.array([100, 100, 200]) 32 | db = np.array([50.0, 50.0, 200.0]) 33 | qp_penalty_weights = np.array([5e5, 5e5, 5e5, 1e6, 1e6, 1e6]) 34 | 35 | # impedance gains 36 | kp = np.array([200, 200, 200, 0, 0, 0]) 37 | kd = np.array([10.0, 10.0, 10.0, 0, 0, 0]) 38 | 39 | pin_robot = robot.pin_robot 40 | 41 | q_init = robot_config.q0.copy() 42 | q_init[0] = 0. 43 | 44 | robot.reset_state(q_init, robot_config.v0) 45 | 46 | ctrl = CentroidalImpedanceController() 47 | ctrl.initialize( 48 | 2.5, 49 | np.diag(robot.pin_robot.mass(q_init)[3:6, 3:6]), 50 | pin_robot.model, 51 | "universe", 52 | robot_config.end_effector_names, 53 | mu, 54 | qp_penalty_weights, 55 | kc, dc, kb, db, 56 | kp, kd 57 | ) 58 | 59 | # Desired center of mass position and velocity. 60 | x_com = [0.0, 0.0, 0.25] 61 | xd_com = [0.0, 0.0, 0.0] 62 | # The base should be flat. 63 | x_ori = [0.0, 0.0, 0.0, 1.0] 64 | x_angvel = [0.0, 0.0, 0.0] 65 | 66 | # Desired leg length 67 | x_des = [ 68 | 0.195, 0.147, 0.015, 0, 0, 0, 1., 69 | 0.195, -0.147, 0.015, 0, 0, 0, 1., 70 | -0.195, 0.147, 0.015, 0, 0, 0, 1., 71 | -0.195, -0.147, 0.015, 0, 0, 0, 1. 72 | ] 73 | xd_des = np.zeros(4 * 6) 74 | 75 | dur = 0. 76 | 77 | # Run the simulator for N steps 78 | N = 4000 79 | for _ in range(N): 80 | # Read the final state and forces after the stepping. 81 | q, dq = robot.get_state() 82 | 83 | quat = pinocchio.Quaternion(q[6], q[3], q[4], q[5]) 84 | quat.normalize() 85 | 86 | start = time.time() 87 | ctrl.run( 88 | q, dq, 89 | np.array([1., 1., 1., 1.]), 90 | q[:3], 91 | x_com, 92 | quat.toRotationMatrix().dot(dq[:3]), # local to world frame 93 | xd_com, 94 | q[3:7], 95 | x_ori, 96 | dq[3:6], 97 | x_angvel, 98 | x_des, xd_des 99 | ) 100 | 101 | tau = ctrl.get_joint_torques() 102 | dur += time.time() - start 103 | 104 | # passing torques to the robot 105 | robot.send_joint_command(tau) 106 | 107 | # Step the simulator. 108 | env.step( 109 | sleep=False 110 | ) # You can sleep here if you want to slow down the replay 111 | 112 | print('Control path: %0.3f ms' % (dur * 1000. / N)) 113 | 114 | if __name__ == "__main__": 115 | demo('solo12') 116 | -------------------------------------------------------------------------------- /demos/demo_robot_impedance.py: -------------------------------------------------------------------------------- 1 | """Demo of the impedance controllers between different frames based on the 2 | input yaml file 3 | 4 | License BSD-3-Clause 5 | Copyright (c) 2020, New York University and Max Planck Gesellschaft. 6 | 7 | Author: Avadesh Meduri & Paarth Shah 8 | """ 9 | 10 | import argparse 11 | import numpy as np 12 | from mim_control.robot_impedance_controller import RobotImpedanceController 13 | from bullet_utils.env import BulletEnvWithGround 14 | from robot_properties_solo.solo12wrapper import Solo12Robot, Solo12Config 15 | from robot_properties_bolt.bolt_wrapper import BoltRobot, BoltConfig 16 | 17 | 18 | def demo(robot_name): 19 | 20 | # Create a Pybullet simulation environment 21 | env = BulletEnvWithGround() 22 | 23 | # Create a robot instance in the simulator. 24 | if robot_name == "solo": 25 | robot = Solo12Robot() 26 | robot = env.add_robot(robot) 27 | robot_config = Solo12Config() 28 | elif robot_name == "bolt": 29 | robot = BoltRobot() 30 | robot = env.add_robot(robot) 31 | robot_config = BoltConfig() 32 | else: 33 | raise RuntimeError( 34 | "Robot name [" + str(robot_name) + "] unknown. " 35 | "Try 'solo' or 'bolt'" 36 | ) 37 | 38 | # Initialize control 39 | tau = np.zeros(robot.nb_dof) 40 | 41 | # Reset the robot to some initial state. 42 | q0 = np.matrix(robot_config.initial_configuration).T 43 | dq0 = np.matrix(robot_config.initial_velocity).T 44 | robot.reset_state(q0, dq0) 45 | 46 | # Impedance controller gains 47 | kp = robot.nb_ee * [200, 200, 200] 48 | kd = robot.nb_ee * [10.0, 10.0, 10.0] 49 | 50 | # Desired leg length. 51 | x_des = robot.nb_ee * [0.0, 0.0, -q0[2].item()] 52 | xd_des = robot.nb_ee * [0.0, 0.0, 0.0] 53 | 54 | # distributing forces to the active end-effectors 55 | f = np.zeros(robot.nb_ee * 3) 56 | f = robot.nb_ee * [0.0, 0.0, (robot_config.mass * 9.8) / 4] 57 | 58 | config_file = robot_config.ctrl_path 59 | robot_leg_ctrl = RobotImpedanceController(robot, config_file) 60 | 61 | # Run the simulator for 100 steps 62 | for _ in range(4000): 63 | # Step the simulator. 64 | env.step( 65 | sleep=True 66 | ) # You can sleep here if you want to slow down the replay 67 | # Read the final state and forces after the stepping. 68 | q, dq = robot.get_state() 69 | # passing forces to the impedance controller 70 | tau = robot_leg_ctrl.return_joint_torques( 71 | q, dq, kp, kd, x_des, xd_des, f 72 | ) 73 | # passing torques to the robot 74 | robot.send_joint_command(tau) 75 | 76 | 77 | if __name__ == "__main__": 78 | 79 | parser = argparse.ArgumentParser() 80 | parser.add_argument( 81 | "--solo", help="Demonstrate Solo.", action="store_true" 82 | ) 83 | parser.add_argument( 84 | "--bolt", help="Demonstrate Bolt.", action="store_true" 85 | ) 86 | args = parser.parse_args() 87 | if args.solo: 88 | robot_name = "solo" 89 | elif args.bolt: 90 | robot_name = "bolt" 91 | else: 92 | robot_name = "solo" 93 | 94 | demo(robot_name) 95 | -------------------------------------------------------------------------------- /demos/solo_impedance.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # License BSD-3-Clause 3 | # Copyright (c) 2020, New York University and Max Planck Gesellschaft. 4 | # 5 | 6 | impedance_controllers: 7 | FL_imp: 8 | frame_root_name: FL_HFE 9 | frame_end_name: FL_FOOT 10 | # Whether or not an impedance controller is associated with a 11 | # specific end-effector 12 | is_eeff: 1.0 13 | # Start column of jacobian in pinocchio. 14 | # In the future this should be found when setting up the controllers 15 | # instead of hardcoded. 16 | start_column: 6 17 | active_joints: [True, True, True] 18 | FR_imp: 19 | frame_root_name: FR_HFE 20 | frame_end_name: FR_FOOT 21 | is_eeff: 1.0 22 | start_column: 9 23 | HL_imp: 24 | frame_root_name: HL_HFE 25 | frame_end_name: HL_FOOT 26 | is_eeff: 1.0 27 | start_column: 12 28 | active_joints: [True, True, True] 29 | HR_imp: 30 | frame_root_name: HR_HFE 31 | frame_end_name: HR_FOOT 32 | is_eeff: 1.0 33 | start_column: 15 34 | active_joints: [True, True, True] 35 | 36 | -------------------------------------------------------------------------------- /include/mim_control/centroidal_force_qp_controller.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 qp allocating forces to the contact points to track a 8 | * desired centroidal wrench. 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | 17 | namespace mim_control 18 | { 19 | typedef Eigen::Array Array6d; 20 | typedef Eigen::Matrix Vector6d; 21 | 22 | /** 23 | * @brief Allocates forces to the contact points to track a desired centroidal 24 | * wrench. 25 | * 26 | * @todo write here the math or point to the doc. 27 | */ 28 | class CentroidalForceQPController 29 | { 30 | public: 31 | /** 32 | * @brief Construct a new CentroidalForceQPController object. 33 | */ 34 | CentroidalForceQPController(); 35 | 36 | /** 37 | * @brief Initialize the internal data. None real-time safe method. 38 | * 39 | * @param number_endeffectors Maximum number of endeffectors in the problem. 40 | * @param friction_coeff Floor friction coefficient to use. 41 | * @param qp_penalty_weights The penalty weight for the linear and angular 42 | * wcom violation. 43 | */ 44 | void initialize(int number_endeffectors, 45 | double friction_coeff, 46 | Eigen::Ref qp_penalty_weights); 47 | 48 | /** 49 | * @brief Computes the centroidal wrench using a PD controller. 50 | * 51 | * @param w_com The desired centroidal wrench to track. 52 | * @param relative_position_endeff The relative position of the endeffectors 53 | * with respect to the center of mass. 54 | */ 55 | void run(Eigen::Ref w_com, 56 | Eigen::Ref relative_position_endeff, 57 | Eigen::Ref cnt_array); 58 | 59 | /** 60 | * @brief Get the computed desired forces 61 | * 62 | * @return Eigen::VectorXd& 63 | */ 64 | Eigen::VectorXd& get_forces(); 65 | 66 | /** 67 | * @brief Get nb_eff, i.e. the number of end-effectors used. 68 | * 69 | * @return int 70 | */ 71 | int get_nb_eff() 72 | { 73 | return nb_eff_; 74 | } 75 | 76 | private: // attributes 77 | /** @brief Output forces */ 78 | Eigen::VectorXd forces_; 79 | Eigen::VectorXd sol_; 80 | 81 | Eigen::MatrixXd hess_; 82 | Eigen::MatrixXd ce_; 83 | Eigen::MatrixXd ce_new_; 84 | Eigen::MatrixXd ci_; 85 | 86 | Eigen::VectorXd g0_; 87 | Eigen::VectorXd ci0_; 88 | 89 | int nb_eff_; 90 | 91 | double mu_; 92 | 93 | eiquadprog::solvers::EiquadprogFast qp_; 94 | }; 95 | 96 | } // namespace mim_control 97 | -------------------------------------------------------------------------------- /include/mim_control/centroidal_impedance_controller.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 This is the implementation for impedance controller between any two 8 | * frames of the robot. 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | #include "pinocchio/multibody/data.hpp" 15 | #include "pinocchio/multibody/model.hpp" 16 | 17 | #include "mim_control/centroidal_force_qp_controller.hpp" 18 | #include "mim_control/centroidal_pd_controller.hpp" 19 | #include "mim_control/impedance_controller.hpp" 20 | 21 | 22 | namespace mim_control 23 | { 24 | /** 25 | * @brief Controller for running centroidal pd, force qp and impedance at once. 26 | */ 27 | class CentroidalImpedanceController 28 | { 29 | public: 30 | typedef Eigen::Array Array6d; 31 | typedef Eigen::Matrix Vector6d; 32 | 33 | /** 34 | * @brief Construct a new CentroidalImpedanceController object. 35 | */ 36 | CentroidalImpedanceController(); 37 | 38 | /** 39 | * @brief Initialize the internal data. None real-time safe method. 40 | * 41 | * @param pinocchio_model rigid body model of the robot 42 | * @param root_frame_name root frame name where the spring starts(Ex. Hip) 43 | * @param end_frame_name frame name where the spring ends(Ex. end effector) 44 | */ 45 | void initialize(const double& mass, 46 | Eigen::Ref inertia, 47 | const pinocchio::Model& pinocchio_model, 48 | const std::string& root_frame_name, 49 | const std::vector& end_frame_names, 50 | double friction_coeff, 51 | Eigen::Ref qp_penalty_weights, 52 | Eigen::Ref kc, 53 | Eigen::Ref dc, 54 | Eigen::Ref kb, 55 | Eigen::Ref db, 56 | Eigen::Ref frame_placement_error_gain, 57 | Eigen::Ref frame_velocity_error_gain); 58 | 59 | void update_centroidal_gains(Eigen::Ref kc, 60 | Eigen::Ref dc, 61 | Eigen::Ref kb, 62 | Eigen::Ref db); 63 | 64 | void update_endeff_gains(Eigen::Ref frame_placement_error_gain, 65 | Eigen::Ref frame_velocity_error_gain); 66 | 67 | void run(Eigen::Ref robot_configuration, 68 | Eigen::Ref robot_velocity, 69 | Eigen::Ref cnt_array, 70 | Eigen::Ref com, 71 | Eigen::Ref com_des, 72 | Eigen::Ref vcom, 73 | Eigen::Ref vcom_des, 74 | Eigen::Ref ori, 75 | Eigen::Ref ori_des, 76 | Eigen::Ref angvel, 77 | Eigen::Ref angvel_des, 78 | Eigen::Ref desired_end_frame_placement, 79 | Eigen::Ref desired_end_frame_velocity); 80 | 81 | /** 82 | * @brief Get the computed joint torques from the impedance controller. 83 | * 84 | * @return Eigen::VectorXd& 85 | */ 86 | const Eigen::VectorXd& get_joint_torques(); 87 | 88 | private: 89 | /** @brief Rigid body dynamics model. */ 90 | pinocchio::Model pinocchio_model_; 91 | 92 | /** @brief Cache of the rigid body dynamics algorithms. */ 93 | pinocchio::Data pinocchio_data_; 94 | 95 | double mass_; 96 | Eigen::Vector3d kc_; 97 | Eigen::Vector3d dc_; 98 | Eigen::Vector3d kb_; 99 | Eigen::Vector3d db_; 100 | Array6d frame_placement_error_gain_; 101 | Array6d frame_velocity_error_gain_; 102 | 103 | Eigen::VectorXd joint_torques_; 104 | Eigen::VectorXd relative_position_endeff_; 105 | 106 | CentroidalPDController centroidal_pd_controller_; 107 | CentroidalForceQPController centroidal_force_qp_controller_; 108 | std::vector impedance_controllers_; 109 | }; 110 | 111 | } // namespace mim_control 112 | -------------------------------------------------------------------------------- /include/mim_control/centroidal_pd_controller.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 PD controller at the center of mass. 8 | * 9 | */ 10 | 11 | #pragma once 12 | 13 | #include 14 | 15 | namespace mim_control 16 | { 17 | typedef Eigen::Array Array6d; 18 | typedef Eigen::Matrix Vector6d; 19 | 20 | /** 21 | * @brief Impedance controller between any two frames of the robot. 22 | */ 23 | class CentroidalPDController 24 | { 25 | public: 26 | /** 27 | * @brief Construct a new ImpedanceController object. 28 | */ 29 | CentroidalPDController(); 30 | 31 | /** 32 | * @brief Initialize the internal data. None real-time safe method. 33 | * 34 | * @param inertia Inertia of the base. 35 | */ 36 | void initialize(const double& mass, 37 | Eigen::Ref inertia); 38 | 39 | /** 40 | * Computes the centroidal wrench using a PD controller. 41 | */ 42 | void run(Eigen::Ref kc, 43 | Eigen::Ref dc, 44 | Eigen::Ref kb, 45 | Eigen::Ref db, 46 | Eigen::Ref com, 47 | Eigen::Ref com_des, 48 | Eigen::Ref vcom, 49 | Eigen::Ref vcom_des, 50 | Eigen::Ref ori, 51 | Eigen::Ref ori_des, 52 | Eigen::Ref angvel, 53 | Eigen::Ref angvel_des); 54 | 55 | /** 56 | * @brief Get the computed wrench from the PD controller. 57 | * 58 | * @return Eigen::VectorXd& 59 | */ 60 | Vector6d& get_wrench(); 61 | 62 | private: // attributes 63 | /** @brief Output wrench */ 64 | Vector6d wrench_; 65 | 66 | double mass_; 67 | Eigen::Vector3d inertia_; 68 | 69 | Eigen::Vector3d pos_error_; 70 | Eigen::Vector3d vel_error_; 71 | Eigen::Vector3d ori_error_; 72 | 73 | Eigen::Vector3d angvel_world_error_; 74 | Eigen::Vector3d des_angvel_world_error_; 75 | 76 | Eigen::Quaternion ori_quat_; 77 | Eigen::Quaternion des_ori_quat_; 78 | Eigen::Quaternion ori_error_quat_; 79 | 80 | Eigen::Matrix ori_se3_; 81 | Eigen::Matrix des_ori_se3_; 82 | Eigen::Matrix 83 | ori_error_se3_; // refer to christian ott paper for definitions (Rdb) 84 | }; 85 | 86 | } // namespace mim_control 87 | -------------------------------------------------------------------------------- /include/mim_control/dynamic_graph/centroidal_force_qp_controller.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 Dynamic graph wrapper around the CentroidalForceQPController class. 8 | * 9 | */ 10 | 11 | #pragma once 12 | 13 | // clang-format off 14 | #include "pinocchio/fwd.hpp" 15 | // clang-format on 16 | #include "dynamic-graph/all-signals.h" 17 | #include "dynamic-graph/entity.h" 18 | #include "mim_control/centroidal_force_qp_controller.hpp" 19 | 20 | namespace mim_control 21 | { 22 | namespace dynamic_graph 23 | { 24 | /** 25 | * @brief Entity around the CentroidalForceQPController class of this package. 26 | */ 27 | class CentroidalForceQPController : public dynamicgraph::Entity 28 | { 29 | DYNAMIC_GRAPH_ENTITY_DECL(); 30 | 31 | public: 32 | /** 33 | * @brief Construct a new CentroidalForceQPController object using its 34 | * Dynamic Graph name. 35 | * 36 | * @param name 37 | */ 38 | CentroidalForceQPController(const std::string& name); 39 | 40 | /** 41 | * @brief Initialize the internal data. None real-time safe method. 42 | * 43 | * @param number_endeffectors Maximum number of endeffectors in the problem. 44 | * @param friction_coeff Floor friction coefficient to use. 45 | * @param qp_penalty_weights The penalty weight for the linear and angular 46 | * wcom violation. 47 | */ 48 | void initialize(int number_endeffectors, 49 | double friction_coeff, 50 | Eigen::Ref qp_penalty_weights); 51 | 52 | /* 53 | * Input Signals. 54 | */ 55 | 56 | /** @brief Robot generalized coordinates (q). */ 57 | dynamicgraph::SignalPtr w_com_sin_; 58 | 59 | /** @brief Robot generalized velocity (dq). */ 60 | dynamicgraph::SignalPtr 61 | relative_position_endeff_sin_; 62 | 63 | /** @brief Proportional gain from the cartesian PD. */ 64 | dynamicgraph::SignalPtr cnt_array_sin_; 65 | 66 | /* 67 | * Output Signals. 68 | */ 69 | 70 | /** @brief Output joint torques. */ 71 | dynamicgraph::SignalTimeDependent forces_sout_; 72 | 73 | protected: 74 | /** 75 | * @brief Callback function of the forces_sout_ signal. 76 | * 77 | * @param torque 78 | * @param time 79 | * @return dynamicgraph::Vector& 80 | */ 81 | dynamicgraph::Vector& forces_callback(dynamicgraph::Vector& torque, 82 | int time); 83 | 84 | /** @brief Actual controller class we wrap around. */ 85 | mim_control::CentroidalForceQPController force_ctrl_; 86 | }; 87 | 88 | } // namespace dynamic_graph 89 | } // namespace mim_control 90 | -------------------------------------------------------------------------------- /include/mim_control/dynamic_graph/centroidal_pd_controller.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 Dynamic graph wrapper around the CentroidalPDController class. 8 | * 9 | */ 10 | 11 | #pragma once 12 | 13 | // clang-format off 14 | #include "pinocchio/fwd.hpp" 15 | // clang-format on 16 | #include "dynamic-graph/all-signals.h" 17 | #include "dynamic-graph/entity.h" 18 | #include "mim_control/centroidal_pd_controller.hpp" 19 | #include "mim_control/dynamic_graph/signal_utils.hpp" 20 | 21 | namespace mim_control 22 | { 23 | namespace dynamic_graph 24 | { 25 | /** 26 | * @brief Entity around the CentroidalPDController class of this package. 27 | */ 28 | class CentroidalPDController : public dynamicgraph::Entity 29 | { 30 | DYNAMIC_GRAPH_ENTITY_DECL(); 31 | 32 | public: 33 | /** 34 | * @brief Construct a new CentroidalPDController object using its Dynamic 35 | * Graph name. 36 | * 37 | * @param name 38 | */ 39 | CentroidalPDController(const std::string& name); 40 | 41 | /** 42 | * @brief Initialize the internal data. None real-time safe method. 43 | * 44 | * @param pinocchio_model rigid body model of the robot 45 | * @param root_frame_name root frame name where the spring starts(Ex. Hip) 46 | * @param end_frame_name frame name where the spring ends(Ex. end effector) 47 | */ 48 | void initialize(const double& mass, const dynamicgraph::Vector& inertia); 49 | 50 | /* 51 | * Input Signals. 52 | */ 53 | 54 | /** @brief Proportional gain on the center of mass tracking error. */ 55 | dynamicgraph::SignalPtr kp_com_sin_; 56 | 57 | /** @brief Derivative gain on the center of mass tracking error. */ 58 | dynamicgraph::SignalPtr kd_com_sin_; 59 | 60 | /** @brief Proportional gain on the base orientation tracking error. */ 61 | dynamicgraph::SignalPtr kp_base_sin_; 62 | 63 | /** @brief Derivative gain on the base orientation tracking error. */ 64 | dynamicgraph::SignalPtr kd_base_sin_; 65 | 66 | /** @brief Current position of the center of mass. */ 67 | dynamicgraph::SignalPtr actual_com_position_sin_; 68 | 69 | /** @brief */ 70 | dynamicgraph::SignalPtr 71 | desired_com_position_sin_; 72 | 73 | /** @brief */ 74 | dynamicgraph::SignalPtr actual_com_velocity_sin_; 75 | 76 | /** @brief */ 77 | dynamicgraph::SignalPtr 78 | desired_com_velocity_sin_; 79 | 80 | /** @brief */ 81 | dynamicgraph::SignalPtr 82 | actual_base_orientation_sin_; 83 | 84 | /** @brief */ 85 | dynamicgraph::SignalPtr 86 | desired_base_orientation_sin_; 87 | 88 | /** @brief */ 89 | dynamicgraph::SignalPtr 90 | actual_base_angular_velocity_sin_; 91 | 92 | /** @brief */ 93 | dynamicgraph::SignalPtr 94 | desired_base_angular_velocity_sin_; 95 | 96 | /* 97 | * Output Signals. 98 | */ 99 | 100 | /** @brief Output joint torques. */ 101 | dynamicgraph::SignalTimeDependent wrench_sout_; 102 | 103 | protected: 104 | /** 105 | * @brief Callback function of the torque_sout_ signal. 106 | * 107 | * @param torque 108 | * @param time 109 | * @return dynamicgraph::Vector& 110 | */ 111 | dynamicgraph::Vector& wrench_callback(dynamicgraph::Vector& signal_data, 112 | int time); 113 | 114 | /** @brief Actual controller class we wrap around. */ 115 | mim_control::CentroidalPDController centroidal_pd_controller_; 116 | }; 117 | 118 | } // namespace dynamic_graph 119 | } // namespace mim_control 120 | -------------------------------------------------------------------------------- /include/mim_control/dynamic_graph/impedance_controller.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 Dynamic graph wrapper around the ImpedanceController class. 8 | * 9 | */ 10 | 11 | #pragma once 12 | 13 | // clang-format off 14 | #include "pinocchio/fwd.hpp" 15 | // clang-format on 16 | #include "dynamic-graph/all-signals.h" 17 | #include "dynamic-graph/entity.h" 18 | #include "mim_control/impedance_controller.hpp" 19 | 20 | namespace mim_control 21 | { 22 | namespace dynamic_graph 23 | { 24 | /** 25 | * @brief Entity around the ImpedanceController class of this package. 26 | */ 27 | class ImpedanceController : public dynamicgraph::Entity 28 | { 29 | DYNAMIC_GRAPH_ENTITY_DECL(); 30 | 31 | public: 32 | /** 33 | * @brief Construct a new ImpedanceController object using its Dynamic Graph 34 | * name. 35 | * 36 | * @param name 37 | */ 38 | ImpedanceController(const std::string& name); 39 | 40 | /** 41 | * @brief Initialize the internal data. None real-time safe method. 42 | * 43 | * @param pinocchio_model rigid body model of the robot 44 | * @param root_frame_name root frame name where the spring starts(Ex. Hip) 45 | * @param end_frame_name frame name where the spring ends(Ex. end effector) 46 | */ 47 | void initialize(const pinocchio::Model& pinocchio_model, 48 | const std::string& root_frame_name, 49 | const std::string& end_frame_name); 50 | 51 | /* 52 | * Input Signals. 53 | */ 54 | 55 | /** @brief Robot generalized coordinates (q). */ 56 | dynamicgraph::SignalPtr robot_configuration_sin_; 57 | 58 | /** @brief Robot generalized velocity (dq). */ 59 | dynamicgraph::SignalPtr robot_velocity_sin_; 60 | 61 | /** @brief Proportional gain from the cartesian PD. */ 62 | dynamicgraph::SignalPtr gain_proportional_sin_; 63 | 64 | /** @brief Proportional gain from the cartesian PD. */ 65 | dynamicgraph::SignalPtr gain_derivative_sin_; 66 | 67 | /** @brief Gain multiplying the feedforward force. */ 68 | dynamicgraph::SignalPtr gain_feed_forward_force_sin_; 69 | 70 | /** @brief */ 71 | dynamicgraph::SignalPtr 72 | desired_end_frame_placement_sin_; 73 | 74 | /** @brief */ 75 | dynamicgraph::SignalPtr 76 | desired_end_frame_velocity_sin_; 77 | 78 | /** @brief */ 79 | dynamicgraph::SignalPtr feed_forward_force_sin_; 80 | 81 | /* 82 | * Output Signals. 83 | */ 84 | 85 | /** @brief Output joint torques. */ 86 | dynamicgraph::SignalTimeDependent torque_sout_; 87 | 88 | /** @brief Output generalized torques. */ 89 | dynamicgraph::SignalTimeDependent 90 | joint_torque_sout_; 91 | 92 | /** @brief Impedance forces computed. */ 93 | dynamicgraph::SignalTimeDependent 94 | impedance_force_; 95 | 96 | /** @brief Internal transition signal. */ 97 | dynamicgraph::SignalTimeDependent one_iteration_sout_; 98 | 99 | protected: 100 | /** 101 | * @brief Callback function of the torque_sout_ signal. 102 | * 103 | * @param torque 104 | * @param time 105 | * @return dynamicgraph::Vector& 106 | */ 107 | dynamicgraph::Vector& torque_callback(dynamicgraph::Vector& torque, 108 | int time); 109 | 110 | /** 111 | * @brief Callback function of the joint_torque_sout_ signal. 112 | * 113 | * @param signal_data 114 | * @param time 115 | * @return dynamicgraph::Vector& 116 | */ 117 | dynamicgraph::Vector& joint_torque_callback( 118 | dynamicgraph::Vector& signal_data, int time); 119 | 120 | /** 121 | * @brief Callback function of the impedance_force_sout_ signal. 122 | * 123 | * @param signal_data 124 | * @param time 125 | * @return dynamicgraph::Vector& 126 | */ 127 | dynamicgraph::Vector& impedance_force_callback( 128 | dynamicgraph::Vector& signal_data, int time); 129 | 130 | /** 131 | * @brief Internally calls the ImpedanceController class. 132 | * 133 | * @param signal_data 134 | * @param time 135 | * @return true 136 | * @return false 137 | */ 138 | bool& one_iteration_callback(bool& signal_data, int time); 139 | 140 | /** @brief Actual controller class we wrap around. */ 141 | mim_control::ImpedanceController impedance_controller_; 142 | 143 | /** @brief Used for type conversion. */ 144 | pinocchio::SE3 desired_end_frame_placement_; 145 | 146 | /** @brief Used for type conversion. */ 147 | pinocchio::Motion desired_end_frame_velocity_; 148 | 149 | /** @brief Used for type conversion. */ 150 | pinocchio::Force feed_forward_force_; 151 | }; 152 | 153 | } // namespace dynamic_graph 154 | } // namespace mim_control 155 | -------------------------------------------------------------------------------- /include/mim_control/dynamic_graph/signal_utils.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 This is the implementation for impedance controller between any two 8 | * frames of the robot. 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | 16 | namespace mim_control 17 | { 18 | namespace dynamic_graph 19 | { 20 | std::string make_signal_string(const bool& is_input_signal, 21 | const std::string& class_name, 22 | const std::string& object_name, 23 | const std::string& signal_type, 24 | const std::string& signal_name); 25 | 26 | // clang-format off 27 | #define crop_underscore(var_name) \ 28 | std::string(#var_name).substr(0, std::string(#var_name).size() - 1) 29 | 30 | #define define_input_signal(signal_var_name, signal_type) \ 31 | signal_var_name( \ 32 | NULL, \ 33 | make_signal_string(true, CLASS_NAME, name, signal_type, \ 34 | crop_underscore(signal_var_name))) 35 | 36 | #define define_output_signal( \ 37 | signal_var_name, signal_type, signal_dep, callback) \ 38 | signal_var_name( \ 39 | boost::bind(callback, this, _1, _2), signal_dep, \ 40 | make_signal_string(false, CLASS_NAME, name, signal_type, \ 41 | crop_underscore(signal_var_name))) 42 | // clang-format on 43 | 44 | } // namespace dynamic_graph 45 | } // namespace mim_control 46 | -------------------------------------------------------------------------------- /include/mim_control/impedance_controller.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 This is the implementation for impedance controller between any two 8 | * frames of the robot. 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | #include "pinocchio/multibody/data.hpp" 15 | #include "pinocchio/multibody/model.hpp" 16 | 17 | namespace mim_control 18 | { 19 | /** 20 | * @brief Impedance controller between any two frames of the robot. 21 | */ 22 | class ImpedanceController 23 | { 24 | public: 25 | typedef Eigen::Array Array6d; 26 | typedef Eigen::Matrix Vector6d; 27 | 28 | /** 29 | * @brief Construct a new ImpedanceController object. 30 | */ 31 | ImpedanceController(); 32 | 33 | /** 34 | * @brief Initialize the internal data. None real-time safe method. 35 | * 36 | * @param pinocchio_model rigid body model of the robot 37 | * @param root_frame_name root frame name where the spring starts(Ex. Hip) 38 | * @param end_frame_name frame name where the spring ends(Ex. end effector) 39 | */ 40 | void initialize(const pinocchio::Model& pinocchio_model, 41 | const std::string& root_frame_name, 42 | const std::string& end_frame_name); 43 | 44 | /** 45 | * @brief Computes the desired joint torques 46 | * 47 | * \f$ 48 | * \tau = J^T (k_p (x^{des} - x) + 49 | * k_d (\dot{x}^{des} - \dot{x}) -k_f f) 50 | * \f$ 51 | * 52 | * with: 53 | * - \f$ \tau \f$ the joint torques, 54 | * - \f$ J \f$ the Jacobian of the sub-kinematic-tree between the root and 55 | * the end frame, 56 | * - \f$ k_p \f$ the gain proportional to the frame placement error, 57 | * - \f$ x_{des} \f$ desired end frame placement with respect to the 58 | * desired root frame. 59 | * - \f$ x \f$ measured end frame placement with respect to the 60 | * measured root frame. 61 | * - \f$ k_d \f$ is the derivative gain applied to the time derivative of 62 | * the error. 63 | * - \f$ \dot{x}^{des} \f$ desired end frame velocity with respect to the 64 | * desired root frame. 65 | * - \f$ \dot{x} \f$ measured end frame velocity with respect to the 66 | * measured root frame. 67 | * - \f$ k_f \f$ the gain over the feed forward force, 68 | * - \f$ f \f$ the feed forward force, 69 | * 70 | * @param robot_configuration robot generalized coordinates configuration. 71 | * @param robot_velocity robot generalized coordinates velocity. 72 | * @param gain_proportional 6d vector for the proportional gains on {x, y, 73 | * z, roll, pitch, yaw}. 74 | * @param gain_derivative 6d vector for the proportional gains on {x, y, z, 75 | * roll, pitch, yaw}. 76 | * @param gain_feed_forward_force gain multiplying the feed forward force. 77 | * @param desired_end_frame_placement desired end frame placement relative 78 | * to the desired root joint. 79 | * @param desired_end_frame_velocity desired end frame velocity relative to 80 | * the desired root joint. 81 | * @param feed_forward_force feed forward force applied to the foot by the 82 | * environment. 83 | */ 84 | void run(Eigen::Ref robot_configuration, 85 | Eigen::Ref robot_velocity, 86 | Eigen::Ref gain_proportional, 87 | Eigen::Ref gain_derivative, 88 | const double& gain_feed_forward_force, 89 | const pinocchio::SE3& desired_end_frame_placement, 90 | const pinocchio::Motion& desired_end_frame_velocity, 91 | const pinocchio::Force& feed_forward_force); 92 | 93 | 94 | /** 95 | * @brief Similar to `run()` but with the data already precomputed. 96 | * 97 | * @param pinocchio_data The data object to use for the computation. 98 | * @param gain_proportional 6d vector for the proportional gains on {x, y, 99 | * z, roll, pitch, yaw}. 100 | * @param gain_derivative 6d vector for the proportional gains on {x, y, z, 101 | * roll, pitch, yaw}. 102 | * @param gain_feed_forward_force gain multiplying the feed forward force. 103 | * @param desired_end_frame_placement desired end frame placement relative 104 | * to the desired root joint. 105 | * @param desired_end_frame_velocity desired end frame velocity relative to 106 | * the desired root joint. 107 | * @param feed_forward_force feed forward force applied to the foot by the 108 | * environment. 109 | */ 110 | void run_precomputed_data(pinocchio::Data& pinocchio_data, 111 | Eigen::Ref gain_proportional, 112 | Eigen::Ref gain_derivative, 113 | const double& gain_feed_forward_force, 114 | const pinocchio::SE3& desired_end_frame_placement, 115 | const pinocchio::Motion& desired_end_frame_velocity, 116 | const pinocchio::Force& feed_forward_force); 117 | 118 | /** 119 | * @brief Get the computed torques from the impedance controller. 120 | * 121 | * @return Eigen::VectorXd& 122 | */ 123 | const Eigen::VectorXd& get_torques(); 124 | 125 | /** 126 | * @brief Get the computed joint torques from the impedance controller. 127 | * 128 | * @return Eigen::VectorXd& 129 | */ 130 | const Eigen::VectorXd& get_joint_torques(); 131 | 132 | /** 133 | * @brief Get the impedance force \f$ f_i \f$ with \f$ \tau = J^T f_i \f$. 134 | * 135 | * @return Vector6d& 136 | */ 137 | const Vector6d& get_impedance_force(); 138 | 139 | /** 140 | * @brief Returns the index of the endeffector frame. 141 | * 142 | * @return pinocchio::FrameIndex 143 | */ 144 | const pinocchio::FrameIndex& get_endframe_index(); 145 | 146 | private: // attributes 147 | /** @brief Rigid body dynamics model. */ 148 | pinocchio::Model pinocchio_model_; 149 | 150 | /** @brief Cache of the rigid body dynamics algorithms. */ 151 | pinocchio::Data pinocchio_data_; 152 | 153 | /** @brief (urdf) name of the root frame. The impedance controller is 154 | * computed with respect to this frame. */ 155 | std::string root_frame_name_; 156 | 157 | /** @brief Index of the root frame in the pinocchio model. */ 158 | pinocchio::FrameIndex root_frame_index_; 159 | 160 | /** @brief Measured root frame placement. */ 161 | pinocchio::SE3 root_placement_; 162 | 163 | /** @brief Measured root frame orientation. */ 164 | pinocchio::SE3 root_orientation_; 165 | 166 | /** @brief Measured root frame velocity. */ 167 | pinocchio::Motion root_velocity_; 168 | 169 | /** @brief (urdf) name of the end frame. This is the controlled frame. */ 170 | std::string end_frame_name_; 171 | 172 | /** @brief Index of the end frame in the pinocchio model. */ 173 | pinocchio::FrameIndex end_frame_index_; 174 | 175 | /** @brief Jacobian of the end frame. */ 176 | pinocchio::Data::Matrix6x end_jacobian_; 177 | 178 | /** @brief Measured end frame placement. */ 179 | pinocchio::SE3 end_placement_; 180 | 181 | /** @brief Measured end frame orientation. */ 182 | pinocchio::SE3 end_orientation_; 183 | 184 | /** @brief Measured end frame velocity. */ 185 | pinocchio::Motion end_velocity_; 186 | 187 | /** @brief Impedance force. Accessible for machine learning purposes. */ 188 | Vector6d impedance_force_; 189 | 190 | /** @brief Measured end frame placement in root frame. */ 191 | pinocchio::SE3 actual_end_frame_placement_; 192 | 193 | /** @brief Measured end frame placement in root frame. */ 194 | pinocchio::Motion actual_end_frame_velocity_; 195 | 196 | /** @brief SE3 placement error. */ 197 | Eigen::Matrix err_se3_; 198 | 199 | /** @brief SE3 velocity error. */ 200 | pinocchio::Motion err_vel_; 201 | 202 | /** @brief Jacobian used in the computation of the impedance. */ 203 | pinocchio::Data::Matrix6x impedance_jacobian_; 204 | 205 | /** @brief Output torques. */ 206 | Eigen::VectorXd torques_; 207 | 208 | /** @brief Output torques. */ 209 | Eigen::VectorXd joint_torques_; 210 | 211 | /** @brief Checks out if the Pinocchio rigid body model of the robot 212 | * contains a free-flyer. This is used to return the command i.e. the 213 | * torques to be applied to the joints only. */ 214 | bool pinocchio_model_has_free_flyer_; 215 | }; 216 | 217 | } // namespace mim_control 218 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, 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/mim_control/__init__.py: -------------------------------------------------------------------------------- 1 | """mim_control pure python API""" -------------------------------------------------------------------------------- /python/mim_control/dynamic_graph/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/machines-in-motion/mim_control/c1e0cb538ee90bc4dfcfa334a2bb8d399b0af204/python/mim_control/dynamic_graph/__init__.py -------------------------------------------------------------------------------- /python/mim_control/dynamic_graph/go_to.py: -------------------------------------------------------------------------------- 1 | """go_to 2 | 3 | Dynamic graph sub-graph implementing a "go to" controller at the joint level. 4 | 5 | License BSD-3-Clause 6 | Copyright (c) 2021, New York University and Max Planck Gesellschaft. 7 | """ 8 | 9 | import numpy as np 10 | import dynamic_graph as dg 11 | from dynamic_graph.sot.core.smooth_reach import SmoothReach 12 | from dynamic_graph.sot.core.control_pd import ControlPD 13 | 14 | 15 | class GoTo(object): 16 | """ 17 | This class allow you to specify a configuration that the robot will reach 18 | in a certain amount of iteration (time = iteration * sampling_period). 19 | """ 20 | 21 | def __init__(self, nb_dof, prefix=""): 22 | self._nb_dof = nb_dof 23 | self._prefix = prefix 24 | 25 | # Euclidean interpolation from 2 vectors. 26 | self._smooth_reach = SmoothReach(self._prefix + "go_to_smooth_reach") 27 | 28 | # PD controller. 29 | self._pd_ctrl = ControlPD(self._prefix + "go_to_pd_controller") 30 | dg.plug(self._smooth_reach.goal, self._pd_ctrl.desired_position) 31 | self._pd_ctrl.desired_velocity.value = np.array(nb_dof * [0.0]) 32 | 33 | def set_pd_gains(self, Kp, Kd): 34 | """ 35 | Set the PD controller gains 36 | """ 37 | self._pd_ctrl.Kp.value = np.array(self._nb_dof * [Kp]) 38 | self._pd_ctrl.Kd.value = np.array(self._nb_dof * [Kd]) 39 | 40 | def go_to(self, desired_joint_position_rad, nb_iteration): 41 | """ Set the curve goal. set(vector (goal), int (duration)) """ 42 | if np.array(desired_joint_position_rad).size != self._nb_dof: 43 | print( 44 | "Warning: Wrong number of input desired joint positions, ", 45 | "nothing to be done", 46 | ) 47 | return 48 | else: 49 | return self._smooth_reach.set( 50 | np.array(desired_joint_position_rad), nb_iteration 51 | ) 52 | 53 | def freeze(self): 54 | self.go_to(self._pd_ctrl.position.value, 1) 55 | 56 | def record_data(self, robot): 57 | # Adding logging traces. 58 | robot.add_trace(self._pd_ctrl.name, "desired_position") 59 | robot.add_trace(self._pd_ctrl.name, "desired_velocity") 60 | robot.add_trace(self._pd_ctrl.name, "position") 61 | robot.add_trace(self._pd_ctrl.name, "velocity") 62 | robot.add_trace(self._pd_ctrl.name, "control") 63 | robot.add_trace(self._smooth_reach.name, "start") 64 | robot.add_trace(self._smooth_reach.name, "goal") 65 | 66 | def plug(self, joint_positions_sout, joint_velocities_sout, torques_sin): 67 | # plug the inputs. 68 | dg.plug(joint_positions_sout, self._pd_ctrl.position) 69 | dg.plug(joint_velocities_sout, self._pd_ctrl.velocity) 70 | dg.plug(joint_positions_sout, self._smooth_reach.start) 71 | 72 | # plug the outins 73 | dg.plug(self._pd_ctrl.control, torques_sin) 74 | -------------------------------------------------------------------------------- /python/mim_control/dynamic_graph/wbc_graph.py: -------------------------------------------------------------------------------- 1 | """End-effector impedance and centroidal dynamics based 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 16, 2021 8 | """ 9 | 10 | import numpy as np 11 | 12 | from dg_tools.utils import ( 13 | constVectorOp, 14 | subtract_vec_vec, 15 | hom2pos, 16 | add_vec_vec, 17 | stack_two_vectors, 18 | selec_vector, 19 | zero_vec, 20 | basePoseQuat2PoseRPY, 21 | multiply_mat_vec 22 | ) 23 | import dynamic_graph as dg 24 | import dynamic_graph.sot.dynamic_pinocchio as dp 25 | 26 | from dynamic_graph.sot.core.math_small_entities import Add_of_vector 27 | 28 | import mim_control.dynamic_graph.wbc as mim_control_dg 29 | 30 | 31 | class WholeBodyController: 32 | def __init__( 33 | self, 34 | prefix, 35 | pin_robot, 36 | endeff_names, 37 | friction_coeff, 38 | qp_penalty_weights, 39 | ): 40 | self.prefix = prefix 41 | self.pin_robot = pin_robot 42 | self.nv = pin_robot.model.nv 43 | self.ne = len(endeff_names) 44 | 45 | self.dg_robot = dp.DynamicPinocchio(prefix + "_pinocchio") 46 | self.dg_robot.setModel(self.pin_robot.model) 47 | self.dg_robot.setData(self.pin_robot.data) 48 | 49 | self.w_com_ff_sin, self.w_com_ff_sout = constVectorOp(np.zeros(6)) 50 | 51 | ### 52 | # CentroidalPDController 53 | self.wcom_pd_ctrl = mim_control_dg.CentroidalPDController( 54 | prefix + "_wcom_pd_controller" 55 | ) 56 | 57 | q_init = np.zeros(pin_robot.nq) 58 | q_init[7] = 1 59 | 60 | self.wcom_pd_ctrl.initialize( 61 | np.sum([i.mass for i in pin_robot.model.inertias]), 62 | np.diag(pin_robot.mass(q_init)[3:6, 3:6]), 63 | ) 64 | 65 | dg.plug( 66 | self.dg_robot.signal('com'), 67 | self.wcom_pd_ctrl.actual_com_position_sin 68 | ) 69 | 70 | ### 71 | # CentroidalForceQPController 72 | self.f_ctrl = mim_control_dg.CentroidalForceQPController( 73 | prefix + "_f_controller" 74 | ) 75 | self.f_ctrl.initialize( 76 | len(endeff_names), friction_coeff, qp_penalty_weights 77 | ) 78 | 79 | self.endeff_names = endeff_names 80 | for endeff_name in self.endeff_names: 81 | self.dg_robot.createPosition("pos_" + endeff_name, endeff_name) 82 | 83 | # Create the relative leg positions. 84 | # TODO: Handle feet offset here as well. 85 | self.com_signal = self.dg_robot.signal("com") 86 | 87 | self.rel_eff_pos = subtract_vec_vec( 88 | hom2pos( 89 | self.dg_robot.signal("pos_" + self.endeff_names[0]), 90 | self.prefix + '_pos_' + self.endeff_names[0] 91 | ), 92 | self.com_signal, 93 | ) 94 | 95 | for i in range(1, len(self.endeff_names)): 96 | self.rel_eff_pos = stack_two_vectors( 97 | self.rel_eff_pos, 98 | subtract_vec_vec( 99 | hom2pos( 100 | self.dg_robot.signal("pos_" + self.endeff_names[i]), 101 | self.prefix + '_pos_' + self.endeff_names[i] 102 | ), 103 | self.com_signal, 104 | ), 105 | 3 * i, 106 | 3, 107 | ) 108 | 109 | dg.plug( 110 | add_vec_vec(self.wcom_pd_ctrl.wrench_sout, self.w_com_ff_sout), 111 | self.f_ctrl.w_com_sin, 112 | ) 113 | dg.plug(self.rel_eff_pos, self.f_ctrl.relative_position_endeff_sin) 114 | 115 | ### 116 | # Impedance controllers. 117 | self.imps = [] 118 | self.imps_feedforward = [] 119 | 120 | # Create plugable vector signals to control the impedance controller. 121 | for i, endeff_name in enumerate(self.endeff_names): 122 | imp = mim_control_dg.ImpedanceController( 123 | prefix + "_imp_" + endeff_name 124 | ) 125 | imp.initialize(pin_robot.model, "universe", endeff_name) 126 | 127 | # Create a way to specify a feedforward force. 128 | op = Add_of_vector(prefix + "_imp_offset_" + endeff_name) 129 | op.sin(0).value = np.zeros(6) 130 | self.imps_feedforward.append(op.sin(0)) 131 | 132 | dg.plug( 133 | stack_two_vectors( 134 | selec_vector(self.f_ctrl.forces_sout, 3 * i, 3 * (i + 1)), 135 | zero_vec(3, ""), 136 | 3, 137 | 3, 138 | ), 139 | op.sin(1) 140 | ) 141 | 142 | dg.plug( 143 | op.sout, 144 | imp.feed_forward_force_sin 145 | ) 146 | 147 | self.imps.append(imp) 148 | 149 | # The final computed control. 150 | self.joint_torques_sout = self.imps[0].joint_torque_sout 151 | for i in range(1, self.ne): 152 | self.joint_torques_sout = add_vec_vec( 153 | self.joint_torques_sout, self.imps[i].joint_torque_sout 154 | ) 155 | 156 | ### 157 | # Export all the signals for the user of the PyEntity. 158 | self.kc_sin = self.wcom_pd_ctrl.kp_com_sin 159 | self.dc_sin = self.wcom_pd_ctrl.kd_com_sin 160 | self.kb_sin = self.wcom_pd_ctrl.kp_base_sin 161 | self.db_sin = self.wcom_pd_ctrl.kd_base_sin 162 | 163 | self.des_com_pos_sin = self.wcom_pd_ctrl.desired_com_position_sin 164 | self.des_com_vel_sin = self.wcom_pd_ctrl.desired_com_velocity_sin 165 | self.des_ori_pos_sin = self.wcom_pd_ctrl.desired_base_orientation_sin 166 | self.des_ori_vel_sin = ( 167 | self.wcom_pd_ctrl.desired_base_angular_velocity_sin 168 | ) 169 | 170 | self.cnt_array_sin = self.f_ctrl.cnt_array_sin 171 | 172 | def trace(self, robot=None): 173 | if robot is None: 174 | try: 175 | robot = self.robot 176 | except: 177 | print("WholeBodyController.trace(): No robot given, cannot trace the data.") 178 | return 179 | 180 | robot.add_trace(self.prefix + '_q', 'sout') 181 | robot.add_trace(self.prefix + '_dq', 'sout') 182 | 183 | for eff_name, imp in zip(self.endeff_names, self.imps): 184 | # Actual position of the endeffector. 185 | robot.add_trace(self.prefix + '_pos_' + eff_name, 'sout') 186 | 187 | # Desired position of the endeffector. 188 | robot.add_trace(imp.name, 'desired_end_frame_placement_sin') 189 | 190 | for imp in self.imps: 191 | robot.add_trace(imp.name, "torque_sout") 192 | robot.add_trace(imp.name, "joint_torque_sout") 193 | robot.add_trace(imp.name, "impedance_force") 194 | robot.add_trace(imp.name, "robot_configuration_sin") 195 | robot.add_trace(imp.name, "robot_velocity_sin") 196 | robot.add_trace(imp.name, "gain_proportional_sin") 197 | robot.add_trace(imp.name, "gain_derivative_sin") 198 | robot.add_trace(imp.name, "gain_feed_forward_force_sin") 199 | robot.add_trace(imp.name, "desired_end_frame_placement_sin") 200 | robot.add_trace(imp.name, "desired_end_frame_velocity_sin") 201 | robot.add_trace(imp.name, "feed_forward_force_sin") 202 | 203 | def plug(self, robot, base_position, base_velocity): 204 | self.plug_all_signals( 205 | robot.device.joint_positions, 206 | robot.device.joint_velocities, 207 | base_position, 208 | base_velocity, 209 | robot.device.ctrl_joint_torques 210 | ) 211 | 212 | def plug_all_signals( 213 | self, 214 | joint_positions_sout, 215 | joint_velocities_sout, 216 | base_position_sout, 217 | base_velocity_sout, 218 | ctrl_joint_torque_sin 219 | ): 220 | # Args: 221 | # robot; DGM robot device 222 | # base_position: The base position as a 7 dim vector signal 223 | # base_velocity: The base velocity as a 6 dim vector signal 224 | 225 | # Create the input to the dg_robot. 226 | base_pose_rpy = basePoseQuat2PoseRPY(base_position_sout) 227 | 228 | position = stack_two_vectors( 229 | base_pose_rpy, joint_positions_sout, 6, self.nv - 6 230 | ) 231 | velocity = stack_two_vectors( 232 | base_velocity_sout, joint_velocities_sout, 6, self.nv - 6, 233 | self.prefix + '_dq' 234 | ) 235 | 236 | dg.plug(position, self.dg_robot.signal("position")) 237 | dg.plug(velocity, self.dg_robot.signal("velocity")) 238 | self.dg_robot.signal("acceleration").value = np.array( 239 | self.nv 240 | * [ 241 | 0.0, 242 | ] 243 | ) 244 | 245 | # Create the input to the impedance controllers. 246 | position = stack_two_vectors( 247 | base_position_sout, joint_positions_sout, 7, self.nv - 6, 248 | self.prefix + '_q' 249 | ) 250 | for imp in self.imps: 251 | dg.plug(position, imp.robot_configuration_sin) 252 | dg.plug(velocity, imp.robot_velocity_sin) 253 | 254 | # Setting the actual quantities for the centroidal-pd controller. 255 | dg.plug( 256 | multiply_mat_vec( 257 | self.dg_robot.signal('Jcom'), 258 | velocity 259 | ), 260 | self.wcom_pd_ctrl.actual_com_velocity_sin 261 | ) 262 | 263 | dg.plug( 264 | selec_vector(base_position_sout, 3, 7), 265 | self.wcom_pd_ctrl.actual_base_orientation_sin, 266 | ) 267 | dg.plug( 268 | selec_vector(base_velocity_sout, 3, 6), 269 | self.wcom_pd_ctrl.actual_base_angular_velocity_sin, 270 | ) 271 | 272 | # Finally, plug the computed torques to the output. 273 | dg.plug(self.joint_torques_sout, ctrl_joint_torque_sin) 274 | 275 | def plug_base_as_com(self, base_position, base_velocity_world): 276 | """ Instead of the COM use the base as com. """ 277 | dg.plug( 278 | selec_vector(base_position, 0, 3), 279 | self.wcom_pd_ctrl.actual_com_position_sin 280 | ) 281 | 282 | dg.plug( 283 | selec_vector(base_velocity_world, 0, 3), 284 | self.wcom_pd_ctrl.actual_com_velocity_sin 285 | ) 286 | 287 | 288 | -------------------------------------------------------------------------------- /python/mim_control/impedance_controller.py: -------------------------------------------------------------------------------- 1 | ################################################################################################################## 2 | ## This is the implementation for impedance controller between any two frames of the robot. 3 | ## This code works with just pybullet and does not depend on dynamic graph. 4 | ## Primarily designed for designing and debuggin controllers 5 | ################################################################################################################# 6 | ## Author: Avadesh Meduri 7 | ## Date: 20/09/2019 8 | ################################################################################################################# 9 | 10 | 11 | import numpy as np 12 | import pinocchio as pin 13 | from pinocchio.utils import zero, eye 14 | 15 | 16 | class ImpedanceController(object): 17 | def __init__( 18 | self, 19 | name, 20 | pin_robot, 21 | frame_root_name, 22 | frame_end_name, 23 | start_column, 24 | active_joints, 25 | ): 26 | 27 | """ 28 | Input : 29 | name : Name of the impedance controller (Ex. Front left leg impedance) 30 | pinocchio_robot : pinocchio wrapper instance. 31 | frame_root_name : The root frame name where the spring starts(Ex. Hip) 32 | frame_end_name : the second frame name where the spring ends(Ex. end effector) 33 | start_column : the column from where 3 columns from the jacobian are selected 34 | """ 35 | 36 | self.name = name 37 | self.pin_robot = pin_robot 38 | self.frame_root_name = frame_root_name 39 | self.frame_end_name = frame_end_name 40 | self.frame_root_idx = self.pin_robot.model.getFrameId( 41 | self.frame_root_name 42 | ) 43 | self.frame_end_idx = self.pin_robot.model.getFrameId( 44 | self.frame_end_name 45 | ) 46 | self.start_column = start_column 47 | self.active_joints = active_joints 48 | 49 | def compute_forward_kinematics(self, q): 50 | """ 51 | Computes forward kinematics of all the frames and stores in data 52 | """ 53 | pin.framesForwardKinematics( 54 | self.pin_robot.model, self.pin_robot.data, q 55 | ) 56 | 57 | def compute_distance_between_frames(self, q): 58 | """ 59 | Computes the distance between the two frames or computes the location 60 | of frame_end with respect to frame_root 61 | """ 62 | return ( 63 | self.pin_robot.data.oMf[self.frame_end_idx].translation 64 | - self.pin_robot.data.oMf[self.frame_root_idx].translation 65 | ) 66 | 67 | def compute_relative_velocity_between_frames(self, q, dq): 68 | """ 69 | computes the velocity of the end_frame with respect to a frame 70 | whose origin aligns with the root frame but is oriented as the world frame 71 | """ 72 | # TODO: define relative vel with respect to frame oriented as the base frame but located at root frame 73 | ## will be a problem in case of a back flip with current implementation. 74 | 75 | frame_config_root = pin.SE3( 76 | self.pin_robot.data.oMf[self.frame_root_idx].rotation, 77 | np.zeros((3, 1)), 78 | ) 79 | frame_config_end = pin.SE3( 80 | self.pin_robot.data.oMf[self.frame_end_idx].rotation, 81 | np.zeros((3, 1)), 82 | ) 83 | 84 | vel_root_in_world_frame = frame_config_root.action.dot( 85 | pin.computeFrameJacobian( 86 | self.pin_robot.model, 87 | self.pin_robot.data, 88 | q, 89 | self.frame_root_idx, 90 | ) 91 | ).dot(dq)[0:3] 92 | vel_end_in_world_frame = frame_config_end.action.dot( 93 | pin.computeFrameJacobian( 94 | self.pin_robot.model, 95 | self.pin_robot.data, 96 | q, 97 | self.frame_end_idx, 98 | ) 99 | ).dot(dq)[0:3] 100 | 101 | return np.subtract(vel_end_in_world_frame, vel_root_in_world_frame).T 102 | 103 | def compute_jacobian(self, q): 104 | """ 105 | computes the jacobian in the world frame 106 | Math : J = R(World,Foot) * J_(Foot frame) 107 | Selection of the required portion of the jacobian is also done here 108 | """ 109 | self.compute_forward_kinematics(q) 110 | jac = pin.computeFrameJacobian( 111 | self.pin_robot.model, self.pin_robot.data, q, self.frame_end_idx 112 | ) 113 | jac = self.pin_robot.data.oMf[self.frame_end_idx].rotation.dot( 114 | jac[0:3] 115 | ) 116 | return jac 117 | 118 | def compute_impedance_torques(self, q, dq, kp, kd, x_des, xd_des, f): 119 | """ 120 | Computes the desired joint torques tau = -Jt * (F + kp(x-x_des) + kd(xd-xd_des)) 121 | Inputs: 122 | q = joint angles 123 | dq = joint velocities 124 | Kp = proportional gain 125 | Kd = derivative gain 126 | x_des = desired [x,y,z] at time t (in the root joint frame) 127 | xd_des = desired velocity of end effector at time t (in the root joint frame) 128 | 129 | """ 130 | assert np.shape(x_des) == (3,) 131 | assert np.shape(xd_des) == (3,) 132 | assert np.shape(f) == (3,) 133 | assert np.shape(kp) == (3,) 134 | assert np.shape(kd) == (3,) 135 | 136 | #### Reshaping values to desired shapes 137 | 138 | x_des = np.array(x_des) 139 | xd_des = np.array(xd_des) 140 | f = np.array(f) 141 | kp = np.array([[kp[0], 0, 0], [0, kp[1], 0], [0, 0, kp[2]]]) 142 | 143 | kd = np.array([[kd[0], 0, 0], [0, kd[1], 0], [0, 0, kd[2]]]) 144 | 145 | ####################################### 146 | 147 | self.compute_forward_kinematics(q) 148 | x = self.compute_distance_between_frames(q) 149 | xd = self.compute_relative_velocity_between_frames(q, dq) 150 | jac = self.compute_jacobian(q)[ 151 | :, 152 | self.start_column : self.start_column + len(self.active_joints), 153 | ] 154 | jac = jac[:, self.active_joints] 155 | 156 | # Store force for learning project. 157 | self.F_ = ( 158 | f + np.matmul(kp, (x - x_des)) + np.matmul(kd, (xd - xd_des).T).T 159 | ) 160 | tau = -jac.T.dot(self.F_.T) 161 | final_tau = [] 162 | j = 0 163 | for i in range(len(self.active_joints)): 164 | if self.active_joints[i] == False: 165 | final_tau.append(0) 166 | else: 167 | final_tau.append(tau[j]) 168 | j += 1 169 | return final_tau 170 | 171 | def compute_impedance_torques_world(self, q, dq, kp, kd, x_des, xd_des, f): 172 | """Computes the leg impedance using world coordinate x_des and xd_des. 173 | 174 | Args: 175 | q: pinocchio generalized coordinates of robot 176 | dq: pinocchio generalized velocity of robot 177 | kp: (list size 3) P gains for position error. 178 | kd: (list size 3) D gains for velocity error. 179 | x_des: (list size 3) desired endeffector position size 3. 180 | xd_des: (list size 3) desired endeffector velocity size 3. 181 | f: (list size 3) feedforward force to apply at endeffector. 182 | """ 183 | assert np.shape(x_des) == (3,) 184 | assert np.shape(xd_des) == (3,) 185 | assert np.shape(f) == (3,) 186 | assert np.shape(kp) == (3,) 187 | assert np.shape(kd) == (3,) 188 | 189 | #### Reshaping values to desired shapes 190 | 191 | x_des = np.array(x_des) 192 | xd_des = np.array(xd_des) 193 | f = np.array(f) 194 | kp = np.array(kp) 195 | kd = np.array(kd) 196 | 197 | ####################################### 198 | 199 | self.compute_forward_kinematics(q) 200 | jac = self.compute_jacobian(q) 201 | 202 | x = self.pin_robot.data.oMf[self.frame_end_idx].translation 203 | xd = jac.dot(dq) 204 | 205 | jac = jac[ 206 | :, 207 | self.start_column : self.start_column + len(self.active_joints), 208 | ] 209 | jac = jac[:, self.active_joints] 210 | 211 | # Store force for learning project. 212 | self.F_ = f + kp * (x - x_des) + kd * (xd - xd_des) 213 | tau = -jac.T.dot(self.F_) 214 | final_tau = [] 215 | j = 0 216 | for i in range(len(self.active_joints)): 217 | if self.active_joints[i] == False: 218 | final_tau.append(0) 219 | else: 220 | final_tau.append(tau[j]) 221 | j += 1 222 | return final_tau 223 | 224 | 225 | class ImpedanceControllerSolo8(ImpedanceController): 226 | def compute_impedance_torques(self, q, dq, kp, kd, x_des, xd_des, f): 227 | """ 228 | Computes the desired joint torques tau = -Jt * (F + kp(x-x_des) + kd(xd-xd_des)) 229 | Inputs: 230 | q = joint angles 231 | dq = joint velocities 232 | Kp = proportional gain 233 | Kd = derivative gain 234 | x_des = desired [x,y,z] at time t (in the root joint frame) 235 | xd_des = desired velocity of end effector at time t (in the root joint frame) 236 | 237 | """ 238 | assert np.shape(x_des) == (3,) 239 | assert np.shape(xd_des) == (3,) 240 | assert np.shape(f) == (3,) 241 | assert np.shape(kp) == (3,) 242 | assert np.shape(kd) == (3,) 243 | 244 | #### Reshaping values to desired shapes 245 | 246 | x_des = np.array(x_des) 247 | xd_des = np.array(xd_des) 248 | f = np.array(f) 249 | kp = np.array(kp) 250 | kd = np.array(kd) 251 | 252 | ####################################### 253 | 254 | self.compute_forward_kinematics(q) 255 | x = self.compute_distance_between_frames(q) 256 | xd = self.compute_relative_velocity_between_frames(q, dq) 257 | 258 | jac = self.compute_jacobian(q) 259 | 260 | # Store force for learning project. 261 | self.F_ = f + kp * (x - x_des) + kd * (xd - xd_des) 262 | tau = -jac.T.dot(self.F_) 263 | return tau 264 | 265 | def compute_impedance_torques_world(self, q, dq, kp, kd, x_des, xd_des, f): 266 | """Computes the leg impedance using world coordinate x_des and xd_des. 267 | 268 | Args: 269 | q: pinocchio generalized coordinates of robot 270 | dq: pinocchio generalized velocity of robot 271 | kp: (list size 3) P gains for position error. 272 | kd: (list size 3) D gains for velocity error. 273 | x_des: (list size 3) desired endeffector position size 3. 274 | xd_des: (list size 3) desired endeffector velocity size 3. 275 | f: (list size 3) feedforward force to apply at endeffector. 276 | """ 277 | assert np.shape(x_des) == (3,) 278 | assert np.shape(xd_des) == (3,) 279 | assert np.shape(f) == (3,) 280 | assert np.shape(kp) == (3,) 281 | assert np.shape(kd) == (3,) 282 | 283 | #### Reshaping values to desired shapes 284 | 285 | x_des = np.array(x_des) 286 | xd_des = np.array(xd_des) 287 | f = np.array(f) 288 | kp = np.array(kp) 289 | kd = np.array(kd) 290 | 291 | ####################################### 292 | 293 | self.compute_forward_kinematics(q) 294 | jac = self.compute_jacobian(q) 295 | 296 | x = self.pin_robot.data.oMf[self.frame_end_idx].translation 297 | xd = jac.dot(dq) 298 | 299 | # Store force for learning project. 300 | self.F_ = f + kp * (x - x_des) + kd * (xd - xd_des) 301 | tau = -jac.T.dot(self.F_) 302 | 303 | return tau 304 | -------------------------------------------------------------------------------- /python/mim_control/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/mim_control/robot_centroidal_controller.py: -------------------------------------------------------------------------------- 1 | ################################################################################################################## 2 | ## This file is the centroidal controller 3 | ################################################################################################################# 4 | ## Author: Avadesh Meduri & Julian Viereck 5 | ## Date: 9/12/2020 6 | ################################################################################################################# 7 | 8 | import numpy as np 9 | import pinocchio as pin 10 | 11 | from .qp_solver import quadprog_solve_qp 12 | 13 | 14 | def arr(a): 15 | return np.array(a).reshape(-1) 16 | 17 | 18 | def mat(a): 19 | return np.matrix(a).reshape((-1, 1)) 20 | 21 | 22 | class RobotCentroidalController: 23 | def __init__( 24 | self, 25 | robot_config, 26 | mu, 27 | kc, 28 | dc, 29 | kb, 30 | db, 31 | qp_penalty_lin=3 * [1e6], 32 | qp_penalty_ang=3 * [1e6], 33 | ): 34 | """ 35 | Input: 36 | robot : pinocchio returned robot object 37 | mu : friction coefficient of the ground 38 | kc : proportional gain on CoM position 39 | kd : derivative gain on CoM position 40 | kb : proportional gain on base orientation 41 | db : derivative gain on base velocity 42 | """ 43 | self.robot_config = robot_config 44 | self.pin_robot_wrapper = robot_config.buildRobotWrapper() 45 | self.robot_mass = robot_config.mass 46 | self.mu = mu # Friction coefficient 47 | self.kc = kc 48 | self.dc = dc 49 | self.kb = kb 50 | self.db = db 51 | self.eff_ids = self.robot_config.end_eff_ids 52 | self.qp_penalty_lin = qp_penalty_lin 53 | self.qp_penalty_ang = qp_penalty_ang 54 | 55 | def compute_com_wrench(self, q, dq, des_pos, des_vel, des_ori, des_angvel): 56 | """Compute the desired COM wrench (equation 1). 57 | 58 | Args: 59 | des_pos: desired center of mass position at time t 60 | des_vel: desired center of mass velocity at time t 61 | des_ori: desired base orientation at time t (quaternions) 62 | des_angvel: desired base angular velocity at time t 63 | Returns: 64 | Computed w_com 65 | """ 66 | m = self.robot_mass 67 | robot = self.pin_robot_wrapper 68 | 69 | com = np.reshape(np.array(q[0:3]), (3,)) 70 | vcom = np.reshape(np.array(dq[0:3]), (3,)) 71 | Ib = robot.mass(q)[3:6, 3:6] 72 | 73 | quat_diff = self.quaternion_difference(arr(q[3:7]), arr(des_ori)) 74 | 75 | cur_angvel = arr(dq[3:6]) 76 | 77 | # Rotate the des and current angular velocity into the world frame. 78 | quat_des = pin.Quaternion( 79 | des_ori[3], des_ori[0], des_ori[1], des_ori[2] 80 | ).matrix() 81 | des_angvel = quat_des.dot(des_angvel) 82 | 83 | quat_cur = pin.Quaternion(q[6], q[3], q[4], q[5]).matrix() 84 | cur_angvel = quat_cur.dot(cur_angvel) 85 | 86 | # Rotate the base velocity into global frame as well. 87 | vcom = quat_cur.dot(vcom) 88 | 89 | w_com = np.hstack( 90 | [ 91 | m * np.multiply(self.kc, des_pos - com) 92 | + m * np.multiply(self.dc, des_vel - vcom), 93 | arr( 94 | arr(np.multiply(self.kb, quat_diff)) 95 | + ( 96 | Ib * mat(np.multiply(self.db, des_angvel - cur_angvel)) 97 | ).T 98 | ), 99 | ] 100 | ) 101 | 102 | # adding weight 103 | w_com[2] += m * 9.81 104 | 105 | return w_com 106 | 107 | def compute_force_qp(self, q, dq, cnt_array, w_com): 108 | """Computes the forces needed to generated a desired centroidal wrench. 109 | Args: 110 | q: Generalized robot position configuration. 111 | dq: Generalized robot velocity configuration. 112 | cnt_array: Array with {0, 1} of #endeffector size indicating if 113 | an endeffector is in contact with the ground or not. Forces are 114 | only computed for active endeffectors. 115 | w_com: Desired centroidal wrench to achieve given forces. 116 | Returns: 117 | Computed forces as a plain array of size 3 * num_endeffectors. 118 | """ 119 | 120 | robot = self.pin_robot_wrapper 121 | com = np.reshape(np.array(q[0:3]), (3,)) 122 | robot.framesForwardKinematics(q) 123 | r = [robot.data.oMf[i].translation - com for i in self.eff_ids] 124 | nb_ee = self.robot_config.nb_ee 125 | # Use the contact activation from the plan to determine which of the 126 | # forces should be active. 127 | 128 | assert len(cnt_array) == nb_ee 129 | # Setup the QP problem. 130 | Q = 2.0 * np.eye(3 * nb_ee + 6) 131 | Q[-6:-3, -6:-3] = np.diag(self.qp_penalty_lin) 132 | Q[-3:, -3:] = np.diag(self.qp_penalty_ang) 133 | p = np.zeros(3 * nb_ee + 6) 134 | A = np.zeros((6, 3 * nb_ee + 6)) 135 | b = w_com 136 | 137 | G = np.zeros((5 * nb_ee, 3 * nb_ee + 6)) 138 | h = np.zeros((5 * nb_ee)) 139 | 140 | j = 0 141 | for i in range(nb_ee): 142 | if cnt_array[i] == 0: 143 | continue 144 | 145 | A[:3, 3 * j : 3 * (j + 1)] = np.eye(3) 146 | A[3:, 3 * j : 3 * (j + 1)] = pin.skew(r[i]) 147 | 148 | G[5 * j + 0, 3 * j + 0] = 1 # mu Fz - Fx >= 0 149 | G[5 * j + 0, 3 * j + 2] = -self.mu 150 | G[5 * j + 1, 3 * j + 0] = -1 # mu Fz + Fx >= 0 151 | G[5 * j + 1, 3 * j + 2] = -self.mu 152 | G[5 * j + 2, 3 * j + 1] = 1 # mu Fz - Fy >= 0 153 | G[5 * j + 2, 3 * j + 2] = -self.mu 154 | G[5 * j + 3, 3 * j + 1] = -1 # mu Fz + Fy >= 0 155 | G[5 * j + 3, 3 * j + 2] = -self.mu 156 | G[5 * j + 4, 3 * j + 2] = -1 # Fz >= 0 157 | 158 | j += 1 159 | 160 | A[:, -6:] = np.eye(6) 161 | 162 | solx = quadprog_solve_qp(Q, p, G, h, A, b) 163 | 164 | F = np.zeros(3 * len(cnt_array)) 165 | j = 0 166 | for i in range(len(cnt_array)): 167 | if cnt_array[i] == 0: 168 | continue 169 | F[3 * i : 3 * (i + 1)] = solx[3 * j : 3 * (j + 1)] 170 | j += 1 171 | 172 | return F 173 | 174 | #### quaternion stuff 175 | def skew(self, v): 176 | """converts vector v to skew symmetric matrix""" 177 | assert v.shape[0] == 3, "vector dimension is not 3 in skew method" 178 | return np.array( 179 | [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]] 180 | ) 181 | 182 | def quaternion_to_rotation(self, q): 183 | """ converts quaternion to rotation matrix """ 184 | return ( 185 | (q[3] ** 2 - q[:3].dot(q[:3])) * np.eye(3) 186 | + 2.0 * np.outer(q[:3], q[:3]) 187 | + 2.0 * q[3] * self.skew(q[:3]) 188 | ) 189 | 190 | def exp_quaternion(self, w): 191 | """ converts angular velocity to quaternion """ 192 | qexp = np.zeros(4) 193 | th = np.linalg.norm(w) 194 | if th ** 2 <= 1.0e-6: 195 | """small norm causes closed form to diverge, 196 | use taylor expansion to approximate""" 197 | qexp[:3] = (1 - (th ** 2) / 6) * w 198 | qexp[3] = 1 - (th ** 2) / 2 199 | else: 200 | u = w / th 201 | qexp[:3] = np.sin(th) * u 202 | qexp[3] = np.cos(th) 203 | return qexp 204 | 205 | def log_quaternion(self, q): 206 | """ lives on the tangent space of SO(3) """ 207 | v = q[:3] 208 | w = q[3] 209 | vnorm = np.linalg.norm(v) 210 | if vnorm <= 1.0e-6: 211 | return 2 * v / w * (1 - vnorm ** 2 / (3 * w ** 2)) 212 | else: 213 | return 2 * np.arctan2(vnorm, w) * v / vnorm 214 | 215 | def quaternion_product(self, q1, q2): 216 | """ computes quaternion product of q1 x q2 """ 217 | p = np.zeros(4) 218 | p[:3] = np.cross(q1[:3], q2[:3]) + q2[3] * q1[:3] + q1[3] * q2[:3] 219 | p[3] = q1[3] * q2[3] - q1[:3].dot(q2[:3]) 220 | return p 221 | 222 | def integrate_quaternion(self, q, w): 223 | """ updates quaternion with tangent vector w """ 224 | dq = self.exp_quaternion(0.5 * w) 225 | return self.quaternion_product(dq, q) 226 | 227 | def quaternion_difference(self, q1, q2): 228 | """computes the tangent vector from q1 to q2 at Identity 229 | returns vecotr w 230 | s.t. q2 = exp(.5 * w)*q1 231 | """ 232 | # first compute dq s.t. q2 = q1*dq 233 | q1conjugate = np.array([-q1[0], -q1[1], -q1[2], q1[3]]) 234 | # order of multiplication is very essential here 235 | dq = self.quaternion_product(q2, q1conjugate) 236 | # increment is log of dq 237 | return self.log_quaternion(dq) 238 | -------------------------------------------------------------------------------- /python/mim_control/robot_impedance_controller.py: -------------------------------------------------------------------------------- 1 | ################################################################################################################## 2 | ## This file creates impedance controllers between different frames 3 | ## based on the input yaml file 4 | ################################################################################################################# 5 | ## Author: Avadesh Meduri & Paarth Shah 6 | ## Date: 9/12/2020 7 | ################################################################################################################# 8 | 9 | import numpy as np 10 | import yaml 11 | from .impedance_controller import ImpedanceController 12 | 13 | 14 | class RobotImpedanceController(ImpedanceController): 15 | def __init__(self, robot, config_file): 16 | """ 17 | Input: 18 | robot : robot object returned by pinocchio wrapper 19 | config_file : file that describes the desired frames to create 20 | springs in 21 | """ 22 | 23 | self.robot = robot 24 | self.num_eef = 0 25 | self.imp_ctrl_array = [] 26 | self.initialise_impedance_controllers(config_file) 27 | 28 | def initialise_impedance_controllers(self, config_file): 29 | """ 30 | Reads the config file and initializes the impedance controllers 31 | 32 | Input: 33 | config_file : file that describes the desired frames to create 34 | springs in 35 | """ 36 | with open(config_file) as config: 37 | data_in = yaml.safe_load(config) 38 | for ctrls in data_in["impedance_controllers"]: 39 | if int(data_in["impedance_controllers"][ctrls]["is_eeff"]): 40 | self.num_eef += 1 41 | 42 | # TODO: 43 | # Check here to make sure frame names exist in pinocchio robot_model/data structure 44 | 45 | # Append to list of impedance controllers 46 | self.imp_ctrl_array.append( 47 | ImpedanceController( 48 | ctrls, 49 | self.robot.pin_robot, 50 | data_in["impedance_controllers"][ctrls]["frame_root_name"], 51 | data_in["impedance_controllers"][ctrls]["frame_end_name"], 52 | int( 53 | data_in["impedance_controllers"][ctrls]["start_column"] 54 | ), 55 | data_in["impedance_controllers"][ctrls]["active_joints"], 56 | ) 57 | ) 58 | 59 | def return_joint_torques(self, q, dq, kp, kd, x_des, xd_des, f): 60 | """ 61 | Returns the joint torques at the current timestep 62 | 63 | Input: 64 | q : current joint positions 65 | dq : current joint velocities 66 | kp : Proportional gain 67 | kd : derivative gain 68 | x_des : desired lengths with respect to the root frame for each 69 | controller (3*number_of_springs) 70 | xd_des : desired velocities with respect to the root frame 71 | f : feed forward forces 72 | """ 73 | tau = np.zeros( 74 | np.sum(len(leg.active_joints) for leg in self.imp_ctrl_array[:]) 75 | ) 76 | s1 = slice(0, 0) 77 | for k in range(len(self.imp_ctrl_array)): 78 | s = slice(3 * k, 3 * (k + 1)) 79 | s1 = slice( 80 | s1.stop, s1.stop + len(self.imp_ctrl_array[k].active_joints) 81 | ) 82 | tau[s1] = self.imp_ctrl_array[k].compute_impedance_torques( 83 | q, dq, kp[s], kd[s], x_des[s], xd_des[s], f[s] 84 | ) 85 | 86 | return tau 87 | 88 | def return_joint_torques_world(self, q, dq, kp, kd, x_des, xd_des, f): 89 | """ 90 | Returns the joint torques at the current timestep, in 91 | world co-ordinates. 92 | 93 | Input: 94 | q : current joint positions 95 | dq : current joint velocities 96 | kp : Proportional gain 97 | kd : derivative gain 98 | x_des : desired lenghts (3*number_of_springs) 99 | xd_des : desired velocities 100 | f : feed forward forces 101 | """ 102 | tau = np.zeros(len(self.imp_ctrl_array) * 3) 103 | for k in range(len(self.imp_ctrl_array)): 104 | s = slice(3 * k, 3 * (k + 1)) 105 | tau[s] = self.imp_ctrl_array[k].compute_impedance_torques_world( 106 | q, dq, kp[s], kd[s], x_des[s], xd_des[s], f[s] 107 | ) 108 | 109 | return tau 110 | -------------------------------------------------------------------------------- /src/centroidal_force_qp_controller.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 Implements a PD controller at the center of mass. 8 | * 9 | */ 10 | 11 | #include "mim_control/centroidal_force_qp_controller.hpp" 12 | 13 | #include 14 | 15 | namespace mim_control 16 | { 17 | CentroidalForceQPController::CentroidalForceQPController() 18 | { 19 | } 20 | 21 | void CentroidalForceQPController::initialize(int number_endeffectors, 22 | double friction_coeff, 23 | Eigen::Ref weights) 24 | { 25 | nb_eff_ = number_endeffectors; 26 | mu_ = friction_coeff; 27 | 28 | // Resize the problem. 29 | qp_.reset(3 * nb_eff_ + 6, 9, 5 * nb_eff_); 30 | sol_.resize(3 * nb_eff_ + 6); 31 | forces_.resize(3 * nb_eff_); 32 | sol_.fill(0.); 33 | forces_.fill(0.); 34 | 35 | // The QP solves the following problem. 36 | // 37 | // min. 0.5 * x' Hess x + g0' x 38 | // s.t. CE x + ce0 = 0 39 | // CI x + ci0 >= 0 40 | 41 | hess_.resize(3 * nb_eff_ + 6, 3 * nb_eff_ + 6); 42 | g0_.resize(3 * nb_eff_ + 6); 43 | 44 | ce_.resize(6, 3 * nb_eff_ + 6); 45 | ce_new_.resize(6, 3 * nb_eff_ + 6); 46 | 47 | ci_.resize(5 * nb_eff_, 3 * nb_eff_ + 6); 48 | ci0_.resize(5 * nb_eff_); 49 | 50 | // Simple initializations. 51 | g0_.fill(0.); 52 | ci_.fill(0.); 53 | ci0_.fill(0.); 54 | ce_.fill(0.); 55 | hess_.setIdentity(); 56 | hess_ *= 2.; 57 | 58 | // Slack variables weights. 59 | hess_.block<6, 6>(3 * nb_eff_, 3 * nb_eff_) = weights.asDiagonal(); 60 | 61 | // Setup centroidal wrench equality. 62 | for (int i = 0; i < nb_eff_; i++) 63 | { 64 | // Setup the linear part. The angular part with the cross product is 65 | // setup in the run() method. 66 | ce_.block<3, 3>(0, 3 * i) << -1, 0, 0, 0, -1, 0, 0, 0, -1; 67 | } 68 | 69 | // Part of the slack variables. 70 | ce_.block<6, 6>(0, 3 * nb_eff_).setIdentity(); 71 | 72 | // Setup the friction cone constraints. 73 | for (int j = 0; j < nb_eff_; j++) 74 | { 75 | ci_(5 * j + 0, 3 * j + 0) = -1; // mu Fz - Fx >= 0 76 | ci_(5 * j + 0, 3 * j + 2) = mu_; 77 | ci_(5 * j + 1, 3 * j + 0) = 1; // mu Fz + Fx >= 0 78 | ci_(5 * j + 1, 3 * j + 2) = mu_; 79 | ci_(5 * j + 2, 3 * j + 1) = -1; // mu Fz - Fy >= 0 80 | ci_(5 * j + 2, 3 * j + 2) = mu_; 81 | ci_(5 * j + 3, 3 * j + 1) = 1; // mu Fz + Fy >= 0 82 | ci_(5 * j + 3, 3 * j + 2) = mu_; 83 | ci_(5 * j + 4, 3 * j + 2) = 1; // Fz >= 0 84 | } 85 | } 86 | 87 | void CentroidalForceQPController::run( 88 | Eigen::Ref w_com, 89 | Eigen::Ref relative_position_endeff, 90 | Eigen::Ref cnt_array) 91 | { 92 | // Copy the linear part for the centroidal wrench equality. 93 | ce_new_ = ce_; 94 | 95 | // Setup cross product at the endeffectors. 96 | for (int i = 0; i < nb_eff_; i++) 97 | { 98 | double x = relative_position_endeff(3 * i); 99 | double y = relative_position_endeff(3 * i + 1); 100 | double z = relative_position_endeff(3 * i + 2); 101 | ce_new_.block<3, 3>(3, 3 * i) << 0, z, -y, -z, 0, x, y, -x, 0; 102 | } 103 | 104 | // Deactivate forces not in contact with the ground. 105 | for (int i = 0; i < nb_eff_; i++) 106 | { 107 | if (cnt_array(i) < 0.2) 108 | { 109 | ce_new_.col(3 * i + 0).setZero(); 110 | ce_new_.col(3 * i + 1).setZero(); 111 | ce_new_.col(3 * i + 2).setZero(); 112 | } 113 | } 114 | 115 | // Solve the QP. 116 | // std::cout << "Solution result: "; 117 | // auto status = 118 | qp_.solve_quadprog(hess_, g0_, ce_new_, w_com, ci_, ci0_, sol_); 119 | // std::cout << status << std::endl; 120 | forces_ = sol_.head(3 * nb_eff_); 121 | } 122 | 123 | Eigen::VectorXd& CentroidalForceQPController::get_forces() 124 | { 125 | return forces_; 126 | } 127 | 128 | } // namespace mim_control 129 | -------------------------------------------------------------------------------- /src/centroidal_impedance_controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2021, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Implements the CentroidalImpedance controller. 8 | */ 9 | 10 | #include "mim_control/centroidal_impedance_controller.hpp" 11 | 12 | #include "pinocchio/algorithm/frames.hpp" 13 | 14 | namespace mim_control 15 | { 16 | CentroidalImpedanceController::CentroidalImpedanceController() 17 | { 18 | } 19 | 20 | void CentroidalImpedanceController::initialize( 21 | const double& mass, 22 | Eigen::Ref inertia, 23 | const pinocchio::Model& pinocchio_model, 24 | const std::string& root_frame_name, 25 | const std::vector& end_frame_names, 26 | double friction_coeff, 27 | Eigen::Ref qp_penalty_weights, 28 | Eigen::Ref kc, 29 | Eigen::Ref dc, 30 | Eigen::Ref kb, 31 | Eigen::Ref db, 32 | Eigen::Ref frame_placement_error_gain, 33 | Eigen::Ref frame_velocity_error_gain 34 | ) 35 | { 36 | bool is_free_flyer = 37 | pinocchio_model.joints[1].shortname() == "JointModelFreeFlyer"; 38 | 39 | mass_ = mass; 40 | kc_ = kc; 41 | dc_ = dc; 42 | kb_ = kb; 43 | db_ = db; 44 | frame_placement_error_gain_ = frame_placement_error_gain; 45 | frame_velocity_error_gain_ = frame_velocity_error_gain; 46 | 47 | pinocchio_model_ = pinocchio_model; 48 | 49 | if (is_free_flyer) { 50 | joint_torques_.resize(pinocchio_model.nv - 6); 51 | } else { 52 | joint_torques_.resize(pinocchio_model.nv); 53 | } 54 | 55 | // Create the cache of the rigid body dynamics algorithms 56 | pinocchio_data_ = pinocchio::Data(pinocchio_model_); 57 | 58 | int num_endeff = end_frame_names.size(); 59 | 60 | relative_position_endeff_.resize(3 * num_endeff); 61 | 62 | // Initialize the other controllers. 63 | centroidal_pd_controller_.initialize(mass, inertia); 64 | 65 | centroidal_force_qp_controller_.initialize( 66 | num_endeff, 67 | friction_coeff, 68 | qp_penalty_weights 69 | ); 70 | 71 | impedance_controllers_.resize(num_endeff); 72 | for (int i = 0; i < num_endeff; i++) 73 | { 74 | impedance_controllers_[i].initialize( 75 | pinocchio_model_, 76 | root_frame_name, end_frame_names[i]); 77 | } 78 | } 79 | 80 | void CentroidalImpedanceController::update_centroidal_gains( 81 | Eigen::Ref kc, 82 | Eigen::Ref dc, 83 | Eigen::Ref kb, 84 | Eigen::Ref db) 85 | { 86 | kc_ = kc; 87 | dc_ = dc; 88 | kb_ = kb; 89 | db_ = db; 90 | } 91 | 92 | void CentroidalImpedanceController::update_endeff_gains( 93 | Eigen::Ref frame_placement_error_gain, 94 | Eigen::Ref frame_velocity_error_gain) 95 | { 96 | frame_placement_error_gain_ = frame_placement_error_gain; 97 | frame_velocity_error_gain_ = frame_velocity_error_gain; 98 | } 99 | 100 | void CentroidalImpedanceController::run( 101 | Eigen::Ref robot_configuration, 102 | Eigen::Ref robot_velocity, 103 | Eigen::Ref cnt_array, 104 | Eigen::Ref com, 105 | Eigen::Ref com_des, 106 | Eigen::Ref vcom, 107 | Eigen::Ref vcom_des, 108 | Eigen::Ref ori, 109 | Eigen::Ref ori_des, 110 | Eigen::Ref angvel, 111 | Eigen::Ref angvel_des, 112 | Eigen::Ref desired_end_frame_placement, 113 | Eigen::Ref desired_end_frame_velocity 114 | ) 115 | { 116 | ////// 117 | // Compute the desired centroidal wrench. 118 | centroidal_pd_controller_.run( 119 | kc_, dc_, kb_, db_, com, com_des, vcom, vcom_des, ori, ori_des, 120 | angvel, angvel_des); 121 | 122 | Vector6d& w_com = centroidal_pd_controller_.get_wrench(); 123 | w_com(2) += 9.81*mass_; 124 | ////// 125 | // Compute the forces at the endeffectors. 126 | 127 | // Compute the relative positions of the endffectors. 128 | int num_endeff = impedance_controllers_.size(); 129 | for (int i = 0; i < num_endeff; i++) 130 | { 131 | auto idx = impedance_controllers_[i].get_endframe_index(); 132 | relative_position_endeff_.block<3, 1>(3 * i, 0) = ( 133 | pinocchio_data_.oMf[idx].translation() - com 134 | ); 135 | } 136 | 137 | centroidal_force_qp_controller_.run( 138 | w_com, relative_position_endeff_, cnt_array); 139 | 140 | Eigen::VectorXd& forces = centroidal_force_qp_controller_.get_forces(); 141 | 142 | ////// 143 | // Update the pinocchio data. 144 | pinocchio::forwardKinematics( 145 | pinocchio_model_, pinocchio_data_, robot_configuration, robot_velocity); 146 | pinocchio::updateFramePlacements( 147 | pinocchio_model_, pinocchio_data_); 148 | pinocchio::computeJointJacobians( 149 | pinocchio_model_, pinocchio_data_, robot_configuration); 150 | 151 | ////// 152 | // Compute the joint torques. 153 | joint_torques_.fill(0); 154 | 155 | for (int i = 0; i < num_endeff; i++) 156 | { 157 | pinocchio::SE3::Quaternion q ( 158 | desired_end_frame_placement[i * 7 + 6], 159 | desired_end_frame_placement[i * 7 + 3], 160 | desired_end_frame_placement[i * 7 + 4], 161 | desired_end_frame_placement[i * 7 + 5]); 162 | pinocchio::SE3::Vector3 t ( 163 | desired_end_frame_placement[i * 7 + 0], 164 | desired_end_frame_placement[i * 7 + 1], 165 | desired_end_frame_placement[i * 7 + 2]); 166 | 167 | pinocchio::SE3 pos(q.matrix(), t); 168 | pinocchio::Motion vel(desired_end_frame_velocity.block<6, 1>(i * 6, 0)); 169 | 170 | Vector6d f; 171 | f.fill(0); 172 | f.head<3>() = forces.block<3, 1>(3 * i, 0); 173 | pinocchio::Force F(f); 174 | 175 | impedance_controllers_[i].run_precomputed_data( 176 | pinocchio_data_, 177 | frame_placement_error_gain_, frame_velocity_error_gain_, 178 | 1., 179 | pos, vel, F 180 | ); 181 | 182 | joint_torques_ += impedance_controllers_[i].get_joint_torques(); 183 | } 184 | } 185 | 186 | const Eigen::VectorXd& CentroidalImpedanceController::get_joint_torques() 187 | { 188 | return joint_torques_; 189 | } 190 | 191 | } 192 | -------------------------------------------------------------------------------- /src/centroidal_pd_controller.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 Implements a PD controller at the center of mass. 8 | * 9 | */ 10 | 11 | #include "mim_control/centroidal_pd_controller.hpp" 12 | #include "pinocchio/spatial/explog.hpp" 13 | 14 | namespace mim_control 15 | { 16 | CentroidalPDController::CentroidalPDController() 17 | { 18 | } 19 | 20 | void CentroidalPDController::initialize( 21 | const double& mass, Eigen::Ref inertia) 22 | { 23 | mass_ = mass; 24 | inertia_ = inertia; 25 | } 26 | 27 | void CentroidalPDController::run(Eigen::Ref kc, 28 | Eigen::Ref dc, 29 | Eigen::Ref kb, 30 | Eigen::Ref db, 31 | Eigen::Ref com, 32 | Eigen::Ref com_des, 33 | Eigen::Ref vcom, 34 | Eigen::Ref vcom_des, 35 | Eigen::Ref ori, 36 | Eigen::Ref ori_des, 37 | Eigen::Ref angvel, 38 | Eigen::Ref angvel_des) 39 | { 40 | des_ori_quat_.w() = ori_des[3]; 41 | des_ori_quat_.vec()[0] = ori_des[0]; 42 | des_ori_quat_.vec()[1] = ori_des[1]; 43 | des_ori_quat_.vec()[2] = ori_des[2]; 44 | 45 | ori_quat_.w() = ori[3]; 46 | ori_quat_.vec()[0] = ori[0]; 47 | ori_quat_.vec()[1] = ori[1]; 48 | ori_quat_.vec()[2] = ori[2]; 49 | 50 | des_ori_se3_ = des_ori_quat_.toRotationMatrix(); 51 | ori_se3_ = ori_quat_.toRotationMatrix(); 52 | 53 | /*************************************************************************/ 54 | // Compute the linear part of the wrench. 55 | 56 | /*---------- computing position error ----*/ 57 | pos_error_.array() = com_des.array() - com.array(); 58 | vel_error_.array() = vcom_des.array() - vcom.array(); 59 | /*---------- computing tourques ----*/ 60 | 61 | wrench_.head<3>().array() = mass_ * (pos_error_.array() * kc.array() + 62 | vel_error_.array() * dc.array()); 63 | 64 | /*************************************************************************/ 65 | // Compute the angular part of the wrench. 66 | ori_error_se3_ = des_ori_se3_.transpose() * ori_se3_; 67 | ori_error_quat_ = ori_error_se3_; 68 | 69 | ori_error_ = pinocchio::quaternion::log3(des_ori_quat_ * ori_quat_.conjugate()); 70 | 71 | /*---------- computing ang error ----*/ 72 | 73 | // Rotate the des and current angular velocity into the world frame. 74 | angvel_world_error_ = ori_se3_ * angvel; 75 | des_angvel_world_error_ = des_ori_se3_ * angvel_des; 76 | 77 | wrench_.tail<3>().array() = 78 | db.array() * inertia_.array() * ( 79 | des_angvel_world_error_.array() - angvel_world_error_.array() 80 | ) + 81 | kb.array() * ori_error_.array(); 82 | } 83 | 84 | Vector6d& CentroidalPDController::get_wrench() 85 | { 86 | return wrench_; 87 | } 88 | 89 | } // namespace mim_control 90 | -------------------------------------------------------------------------------- /src/dynamic_graph/centroidal_force_qp_controller.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 Dynamic graph wrapper around the CentroidalForceQPController class. 8 | */ 9 | 10 | #include "mim_control/dynamic_graph/centroidal_force_qp_controller.hpp" 11 | #include "dynamic-graph/all-commands.h" 12 | #include "dynamic-graph/factory.h" 13 | #include "mim_control/dynamic_graph/signal_utils.hpp" 14 | 15 | namespace mim_control 16 | { 17 | namespace dynamic_graph 18 | { 19 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(CentroidalForceQPController, 20 | "CentroidalForceQPController"); 21 | 22 | CentroidalForceQPController::CentroidalForceQPController( 23 | const std::string& name) 24 | : // Inheritance. 25 | dynamicgraph::Entity(name), 26 | // Input signals. 27 | define_input_signal(w_com_sin_, "Vector3d"), 28 | define_input_signal(relative_position_endeff_sin_, "Vector(nb_ee_x_3)d"), 29 | define_input_signal(cnt_array_sin_, "Vector(nb_ee)d"), 30 | // Output signals. 31 | define_output_signal( 32 | forces_sout_, 33 | "Vector(nb_ee_x_3)d", 34 | w_com_sin_ << relative_position_endeff_sin_ << cnt_array_sin_, 35 | &CentroidalForceQPController::forces_callback) 36 | { 37 | signalRegistration(w_com_sin_ << relative_position_endeff_sin_ 38 | << cnt_array_sin_ << forces_sout_); 39 | } 40 | 41 | void CentroidalForceQPController::initialize( 42 | int number_endeffectors, 43 | double friction_coeff, 44 | Eigen::Ref qp_penalty_weights) 45 | { 46 | assert(qp_penalty_weights.size() == 6 && 47 | "Wrong size for qp_penalty_weights, expected dimension 6."); 48 | 49 | force_ctrl_.initialize( 50 | number_endeffectors, friction_coeff, qp_penalty_weights); 51 | } 52 | 53 | dynamicgraph::Vector& CentroidalForceQPController::forces_callback( 54 | dynamicgraph::Vector& signal_data, int time) 55 | { 56 | const dynamicgraph::Vector& w_com = w_com_sin_.access(time); 57 | const dynamicgraph::Vector& relative_position_endeff = 58 | relative_position_endeff_sin_.access(time); 59 | const dynamicgraph::Vector& cnt_array = cnt_array_sin_.access(time); 60 | 61 | assert(w_com.size() == 6 && "Wrong size of CoM wrench, expected 6."); 62 | assert(relative_position_endeff.size() == force_ctrl_.get_nb_eff() * 3 && 63 | "Wrong size of EE position, expected nb_eff * 3."); 64 | assert(cnt_array.size() == force_ctrl_.get_nb_eff() && 65 | "Wrong size of contact array."); 66 | 67 | force_ctrl_.run(w_com, relative_position_endeff, cnt_array); 68 | 69 | signal_data = force_ctrl_.get_forces(); 70 | return signal_data; 71 | } 72 | 73 | } // namespace dynamic_graph 74 | } // namespace mim_control 75 | -------------------------------------------------------------------------------- /src/dynamic_graph/centroidal_pd_controller.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 Dynamic graph wrapper around the CentroidalPDController class. 8 | */ 9 | 10 | #include "mim_control/dynamic_graph/centroidal_pd_controller.hpp" 11 | 12 | #include "dynamic-graph/all-commands.h" 13 | #include "dynamic-graph/factory.h" 14 | 15 | namespace mim_control 16 | { 17 | namespace dynamic_graph 18 | { 19 | using ::dynamicgraph::command::docCommandVoid2; 20 | using ::dynamicgraph::command::makeCommandVoid2; 21 | 22 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(CentroidalPDController, 23 | "CentroidalPDController"); 24 | 25 | CentroidalPDController::CentroidalPDController(const std::string& name) 26 | : // Inheritance. 27 | dynamicgraph::Entity(name), 28 | // Input signals. 29 | define_input_signal(kp_com_sin_, "Vector3d"), 30 | define_input_signal(kd_com_sin_, "Vector3d"), 31 | define_input_signal(kp_base_sin_, "Vector3d"), 32 | define_input_signal(kd_base_sin_, "Vector3d"), 33 | define_input_signal(actual_com_position_sin_, "Vector3d"), 34 | define_input_signal(desired_com_position_sin_, "Vector3d"), 35 | define_input_signal(actual_com_velocity_sin_, "Vector3d"), 36 | define_input_signal(desired_com_velocity_sin_, "Vector3d"), 37 | define_input_signal(actual_base_orientation_sin_, "Vector4d_quat"), 38 | define_input_signal(desired_base_orientation_sin_, "Vector4d_quat"), 39 | define_input_signal(actual_base_angular_velocity_sin_, "Vector3d"), 40 | define_input_signal(desired_base_angular_velocity_sin_, "Vector3d"), 41 | // Output signals. 42 | define_output_signal( 43 | wrench_sout_, 44 | "inner", 45 | kp_com_sin_ << kd_com_sin_ << kp_base_sin_ << kd_base_sin_ 46 | << actual_com_position_sin_ << desired_com_position_sin_ 47 | << actual_com_velocity_sin_ << desired_com_velocity_sin_ 48 | << actual_base_orientation_sin_ 49 | << desired_base_orientation_sin_ 50 | << actual_base_angular_velocity_sin_ 51 | << desired_base_angular_velocity_sin_, 52 | &CentroidalPDController::wrench_callback) 53 | { 54 | signalRegistration(kp_com_sin_ 55 | << kd_com_sin_ << kp_base_sin_ << kd_base_sin_ 56 | << actual_com_position_sin_ << desired_com_position_sin_ 57 | << actual_com_velocity_sin_ << desired_com_velocity_sin_ 58 | << actual_base_orientation_sin_ 59 | << desired_base_orientation_sin_ 60 | << actual_base_angular_velocity_sin_ 61 | << desired_base_angular_velocity_sin_ << wrench_sout_); 62 | 63 | addCommand("initialize", 64 | makeCommandVoid2( 65 | *this, 66 | &CentroidalPDController::initialize, 67 | docCommandVoid2("Initialize the CentroidalPDController.", 68 | "Robot total mass.", 69 | "Base inertia."))); 70 | } 71 | 72 | void CentroidalPDController::initialize(const double& mass, 73 | const dynamicgraph::Vector& inertia) 74 | { 75 | centroidal_pd_controller_.initialize(mass, inertia); 76 | } 77 | 78 | dynamicgraph::Vector& CentroidalPDController::wrench_callback( 79 | dynamicgraph::Vector& signal_data, int time) 80 | { 81 | const dynamicgraph::Vector& kp_com = kp_com_sin_.access(time); 82 | const dynamicgraph::Vector& kd_com = kd_com_sin_.access(time); 83 | const dynamicgraph::Vector& kp_base = kp_base_sin_.access(time); 84 | const dynamicgraph::Vector& kd_base = kd_base_sin_.access(time); 85 | const dynamicgraph::Vector& actual_com_position = 86 | actual_com_position_sin_.access(time); 87 | const dynamicgraph::Vector& desired_com_position = 88 | desired_com_position_sin_.access(time); 89 | const dynamicgraph::Vector& actual_com_velocity = 90 | actual_com_velocity_sin_.access(time); 91 | const dynamicgraph::Vector& desired_com_velocity = 92 | desired_com_velocity_sin_.access(time); 93 | const dynamicgraph::Vector& actual_base_orientation = 94 | actual_base_orientation_sin_.access(time); 95 | const dynamicgraph::Vector& desired_base_orientation = 96 | desired_base_orientation_sin_.access(time); 97 | const dynamicgraph::Vector& actual_base_angular_velocity = 98 | actual_base_angular_velocity_sin_.access(time); 99 | const dynamicgraph::Vector& desired_base_angular_velocity = 100 | desired_base_angular_velocity_sin_.access(time); 101 | 102 | centroidal_pd_controller_.run(kp_com, 103 | kd_com, 104 | kp_base, 105 | kd_base, 106 | actual_com_position, 107 | desired_com_position, 108 | actual_com_velocity, 109 | desired_com_velocity, 110 | actual_base_orientation, 111 | desired_base_orientation, 112 | actual_base_angular_velocity, 113 | desired_base_angular_velocity); 114 | 115 | signal_data = centroidal_pd_controller_.get_wrench(); 116 | return signal_data; 117 | } 118 | 119 | } // namespace dynamic_graph 120 | } // namespace mim_control 121 | -------------------------------------------------------------------------------- /src/dynamic_graph/impedance_controller.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 Dynamic graph wrapper around the ImpedanceController class. 8 | */ 9 | 10 | #include "mim_control/dynamic_graph/impedance_controller.hpp" 11 | 12 | #include "Eigen/Eigen" 13 | #include "dynamic-graph/factory.h" 14 | #include "mim_control/dynamic_graph/signal_utils.hpp" 15 | 16 | namespace mim_control 17 | { 18 | namespace dynamic_graph 19 | { 20 | typedef Eigen::Map QuatConstMap; 21 | 22 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ImpedanceController, "ImpedanceController"); 23 | 24 | ImpedanceController::ImpedanceController(const std::string& name) 25 | : // Inheritance. 26 | dynamicgraph::Entity(name), 27 | // Input signals. 28 | define_input_signal(robot_configuration_sin_, "VectorXd"), 29 | define_input_signal(robot_velocity_sin_, "VectorXd"), 30 | define_input_signal(gain_proportional_sin_, "Vector6d"), 31 | define_input_signal(gain_derivative_sin_, "Vector6d"), 32 | define_input_signal(gain_feed_forward_force_sin_, "double"), 33 | define_input_signal(desired_end_frame_placement_sin_, "Vector7d_XYZQuat"), 34 | define_input_signal(desired_end_frame_velocity_sin_, "Vector6d_Motion"), 35 | define_input_signal(feed_forward_force_sin_, "Vector6d_Force"), 36 | // Output signals. 37 | define_output_signal(torque_sout_, 38 | "inner", 39 | one_iteration_sout_, 40 | &ImpedanceController::torque_callback), 41 | define_output_signal(joint_torque_sout_, 42 | "inner", 43 | one_iteration_sout_, 44 | &ImpedanceController::joint_torque_callback), 45 | define_output_signal(impedance_force_, 46 | "inner", 47 | one_iteration_sout_, 48 | &ImpedanceController::impedance_force_callback), 49 | // Inner signal. 50 | one_iteration_sout_( 51 | boost::bind( 52 | &ImpedanceController::one_iteration_callback, this, _1, _2), 53 | robot_configuration_sin_ 54 | << robot_velocity_sin_ << gain_proportional_sin_ 55 | << gain_derivative_sin_ << gain_feed_forward_force_sin_ 56 | << desired_end_frame_placement_sin_ 57 | << desired_end_frame_velocity_sin_ << feed_forward_force_sin_, 58 | make_signal_string( 59 | false, CLASS_NAME, name, "bool", "one_iteration_sout")) 60 | { 61 | signalRegistration( 62 | robot_configuration_sin_ 63 | << robot_velocity_sin_ << gain_proportional_sin_ << gain_derivative_sin_ 64 | << gain_feed_forward_force_sin_ << desired_end_frame_placement_sin_ 65 | << desired_end_frame_velocity_sin_ << feed_forward_force_sin_ 66 | << torque_sout_ << joint_torque_sout_ << impedance_force_); 67 | } 68 | 69 | void ImpedanceController::initialize(const pinocchio::Model& pinocchio_model, 70 | const std::string& root_frame_name, 71 | const std::string& end_frame_name) 72 | { 73 | impedance_controller_.initialize( 74 | pinocchio_model, root_frame_name, end_frame_name); 75 | } 76 | 77 | dynamicgraph::Vector& ImpedanceController::torque_callback( 78 | dynamicgraph::Vector& signal_data, int time) 79 | { 80 | one_iteration_sout_.access(time); 81 | signal_data = impedance_controller_.get_torques(); 82 | return signal_data; 83 | } 84 | 85 | dynamicgraph::Vector& ImpedanceController::joint_torque_callback( 86 | dynamicgraph::Vector& signal_data, int time) 87 | { 88 | one_iteration_sout_.access(time); 89 | signal_data = impedance_controller_.get_joint_torques(); 90 | return signal_data; 91 | } 92 | 93 | dynamicgraph::Vector& ImpedanceController::impedance_force_callback( 94 | dynamicgraph::Vector& signal_data, int time) 95 | { 96 | one_iteration_sout_.access(time); 97 | signal_data = impedance_controller_.get_impedance_force(); 98 | return signal_data; 99 | } 100 | 101 | bool& ImpedanceController::one_iteration_callback(bool& signal_data, int time) 102 | { 103 | const dynamicgraph::Vector& robot_configuration = 104 | robot_configuration_sin_.access(time); 105 | const dynamicgraph::Vector& robot_velocity = 106 | robot_velocity_sin_.access(time); 107 | const dynamicgraph::Vector& gain_proportional = 108 | gain_proportional_sin_.access(time); 109 | const dynamicgraph::Vector& gain_derivative = 110 | gain_derivative_sin_.access(time); 111 | const double& gain_feed_forward_force = 112 | gain_feed_forward_force_sin_.access(time); 113 | const dynamicgraph::Vector& desired_end_frame_placement = 114 | desired_end_frame_placement_sin_.access(time); 115 | const dynamicgraph::Vector& desired_end_frame_velocity = 116 | desired_end_frame_velocity_sin_.access(time); 117 | const dynamicgraph::Vector& feed_forward_force = 118 | feed_forward_force_sin_.access(time); 119 | 120 | QuatConstMap quat(desired_end_frame_placement.tail<4>().data()); 121 | desired_end_frame_placement_.rotation() = quat.matrix(); 122 | desired_end_frame_placement_.translation() = 123 | desired_end_frame_placement.template head<3>(); 124 | 125 | desired_end_frame_velocity_.toVector() = desired_end_frame_velocity; 126 | feed_forward_force_.toVector() = feed_forward_force; 127 | 128 | impedance_controller_.run(robot_configuration, 129 | robot_velocity, 130 | gain_proportional.array(), 131 | gain_derivative.array(), 132 | gain_feed_forward_force, 133 | desired_end_frame_placement_, 134 | desired_end_frame_velocity_, 135 | feed_forward_force_); 136 | signal_data = true; 137 | return signal_data; 138 | } 139 | 140 | } // namespace dynamic_graph 141 | } // namespace mim_control 142 | -------------------------------------------------------------------------------- /src/dynamic_graph/signal_utils.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 This is the implementation for impedance controller between any two 8 | * frames of the robot. 9 | * 10 | */ 11 | 12 | #include "mim_control/dynamic_graph/signal_utils.hpp" 13 | 14 | namespace mim_control 15 | { 16 | namespace dynamic_graph 17 | { 18 | std::string make_signal_string(const bool& is_input_signal, 19 | const std::string& class_name, 20 | const std::string& object_name, 21 | const std::string& signal_type, 22 | const std::string& signal_name) 23 | { 24 | std::ostringstream oss; 25 | oss << class_name << "(" << object_name 26 | << ")::" << (is_input_signal ? "input" : "output") << "(" << signal_type 27 | << ")::" << signal_name; 28 | return oss.str(); 29 | } 30 | 31 | } // namespace dynamic_graph 32 | } // namespace mim_control 33 | -------------------------------------------------------------------------------- /src/impedance_controller.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 Implementation of the ImpedanceController class. 8 | */ 9 | 10 | #include "mim_control/impedance_controller.hpp" 11 | 12 | #include "pinocchio/algorithm/frames.hpp" 13 | 14 | namespace mim_control 15 | { 16 | ImpedanceController::ImpedanceController() 17 | { 18 | } 19 | 20 | void ImpedanceController::initialize(const pinocchio::Model& pinocchio_model, 21 | const std::string& root_frame_name, 22 | const std::string& end_frame_name) 23 | { 24 | // Copy the arguments internally. 25 | pinocchio_model_ = pinocchio_model; 26 | 27 | // Create the cache of the rigid body dynamics algorithms 28 | pinocchio_data_ = pinocchio::Data(pinocchio_model_); 29 | 30 | // Copy the arguments internally. 31 | root_frame_name_ = root_frame_name; 32 | end_frame_name_ = end_frame_name; 33 | 34 | // Fetch the index of the frame in the robot model. 35 | root_frame_index_ = pinocchio_model_.getFrameId(root_frame_name_); 36 | end_frame_index_ = pinocchio_model_.getFrameId(end_frame_name_); 37 | 38 | // initialize the size of the vectors. 39 | end_jacobian_.resize(6, pinocchio_model_.nv); 40 | end_jacobian_.fill(0.); 41 | impedance_jacobian_.resize(6, pinocchio_model_.nv); 42 | impedance_jacobian_.fill(0.); 43 | 44 | // Defines if the model has a freeflyer. 45 | pinocchio_model_has_free_flyer_ = 46 | pinocchio_model_.joints[1].shortname() == "JointModelFreeFlyer"; 47 | 48 | // output 49 | torques_.resize(pinocchio_model_.nv, 1); 50 | torques_.fill(0.); 51 | if (pinocchio_model_has_free_flyer_) 52 | joint_torques_.resize(pinocchio_model_.nv, 1); 53 | else 54 | { 55 | joint_torques_.resize(pinocchio_model_.nv - 6, 1); 56 | } 57 | 58 | // Intermediate variables. 59 | root_orientation_ = pinocchio::SE3::Identity(); 60 | end_orientation_ = pinocchio::SE3::Identity(); 61 | } 62 | 63 | void ImpedanceController::run( 64 | Eigen::Ref robot_configuration, 65 | Eigen::Ref robot_velocity, 66 | Eigen::Ref gain_proportional, 67 | Eigen::Ref gain_derivative, 68 | const double& gain_feed_forward_force, 69 | const pinocchio::SE3& desired_end_frame_placement, 70 | const pinocchio::Motion& desired_end_frame_velocity, 71 | const pinocchio::Force& feed_forward_force) 72 | { 73 | assert(robot_configuration.size() == pinocchio_model_.nq && 74 | "robot_configuration is not of the good size."); 75 | assert(robot_velocity.size() == pinocchio_model_.nv && 76 | "robot_velocity is not of the good size."); 77 | 78 | // Get the current frame placements and velocity. 79 | pinocchio::forwardKinematics( 80 | pinocchio_model_, pinocchio_data_, robot_configuration, robot_velocity); 81 | pinocchio::updateFramePlacement( 82 | pinocchio_model_, pinocchio_data_, root_frame_index_); 83 | pinocchio::updateFramePlacement( 84 | pinocchio_model_, pinocchio_data_, end_frame_index_); 85 | 86 | // Compute the jacobians 87 | pinocchio::computeJointJacobians( 88 | pinocchio_model_, pinocchio_data_, robot_configuration); 89 | 90 | run_precomputed_data(pinocchio_data_, 91 | gain_proportional, 92 | gain_derivative, 93 | gain_feed_forward_force, 94 | desired_end_frame_placement, 95 | desired_end_frame_velocity, 96 | feed_forward_force 97 | ); 98 | } 99 | 100 | void ImpedanceController::run_precomputed_data( 101 | pinocchio::Data& pinocchio_data, 102 | Eigen::Ref gain_proportional, 103 | Eigen::Ref gain_derivative, 104 | const double& gain_feed_forward_force, 105 | const pinocchio::SE3& desired_end_frame_placement, 106 | const pinocchio::Motion& desired_end_frame_velocity, 107 | const pinocchio::Force& feed_forward_force) 108 | { 109 | root_placement_ = pinocchio_data.oMf[root_frame_index_]; 110 | end_placement_ = pinocchio_data.oMf[end_frame_index_]; 111 | root_velocity_ = pinocchio::getFrameVelocity( 112 | pinocchio_model_, pinocchio_data, root_frame_index_, pinocchio::WORLD); 113 | end_velocity_ = pinocchio::getFrameVelocity( 114 | pinocchio_model_, pinocchio_data, end_frame_index_, pinocchio::WORLD); 115 | 116 | // Orientations 117 | root_orientation_.rotation() = root_placement_.rotation(); 118 | end_orientation_.rotation() = end_placement_.rotation(); 119 | 120 | // Actual end frame placement in root frame. 121 | actual_end_frame_placement_ = root_placement_.actInv(end_placement_); 122 | 123 | // Placement error. 124 | err_se3_.head<3>() = root_orientation_.rotation() * 125 | (desired_end_frame_placement.translation() - 126 | actual_end_frame_placement_.translation()); 127 | err_se3_.tail<3>() = 128 | pinocchio::log3(desired_end_frame_placement.rotation().transpose() * 129 | actual_end_frame_placement_.rotation()); 130 | 131 | // Actual end frame velocity in root frame. 132 | actual_end_frame_velocity_ = 133 | end_placement_.actInv(end_velocity_ - root_velocity_); 134 | 135 | // Velocity error. 136 | err_vel_ = end_orientation_.act(desired_end_frame_velocity - 137 | actual_end_frame_velocity_); 138 | 139 | // Compute the force to be applied to the environment. 140 | impedance_force_ = gain_proportional * err_se3_.array(); 141 | impedance_force_ += 142 | (gain_derivative * err_vel_.toVector().array()).matrix(); 143 | impedance_force_ -= 144 | (gain_feed_forward_force * feed_forward_force.toVector().array()) 145 | .matrix(); 146 | 147 | // Get the jacobian. 148 | pinocchio::getFrameJacobian(pinocchio_model_, 149 | pinocchio_data, 150 | end_frame_index_, 151 | pinocchio::LOCAL_WORLD_ALIGNED, 152 | end_jacobian_); 153 | 154 | impedance_jacobian_ = end_jacobian_; 155 | 156 | // compute the output torques 157 | torques_ = (impedance_jacobian_.transpose() * impedance_force_); 158 | 159 | if (pinocchio_model_has_free_flyer_) 160 | joint_torques_ = torques_.tail(pinocchio_model_.nv - 6); 161 | else 162 | { 163 | joint_torques_ = torques_; 164 | } 165 | return; 166 | } 167 | 168 | const Eigen::VectorXd& ImpedanceController::get_torques() 169 | { 170 | return torques_; 171 | } 172 | 173 | const Eigen::VectorXd& ImpedanceController::get_joint_torques() 174 | { 175 | return joint_torques_; 176 | } 177 | 178 | const ImpedanceController::Vector6d& ImpedanceController::get_impedance_force() 179 | { 180 | return impedance_force_; 181 | } 182 | 183 | const pinocchio::FrameIndex& ImpedanceController::get_endframe_index() 184 | { 185 | return end_frame_index_; 186 | } 187 | 188 | } // namespace mim_control -------------------------------------------------------------------------------- /srcpy/centroidal_force_qp_controller.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 CentroidalForceQPController class. 8 | */ 9 | 10 | #include "mim_control/centroidal_force_qp_controller.hpp" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace py = pybind11; 16 | 17 | namespace mim_control 18 | { 19 | void bind_centroidal_force_qp_controller(py::module& module) 20 | { 21 | py::class_(module, 22 | "CentroidalForceQPController") 23 | .def(py::init<>()) 24 | 25 | // Public methods. 26 | .def("initialize", &CentroidalForceQPController::initialize) 27 | .def("run", &CentroidalForceQPController::run) 28 | .def("get_forces", 29 | &CentroidalForceQPController::get_forces, 30 | py::return_value_policy::reference); 31 | } 32 | 33 | } // namespace mim_control 34 | -------------------------------------------------------------------------------- /srcpy/centroidal_impedance_controller.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 CentroidalPDController class. 8 | */ 9 | 10 | #include "mim_control/centroidal_impedance_controller.hpp" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace py = pybind11; 18 | 19 | namespace mim_control 20 | { 21 | void bind_centroidal_impedance_controller(py::module& module) 22 | { 23 | py::class_(module, "CentroidalImpedanceController") 24 | .def(py::init<>()) 25 | 26 | // Public methods. 27 | // .def("initialize", &CentroidalImpedanceController::initialize) 28 | .def("initialize", 29 | [](CentroidalImpedanceController& obj, 30 | const double& mass, 31 | Eigen::Ref inertia, 32 | py::object model, 33 | const std::string& root_frame_name, 34 | const std::vector& end_frame_names, 35 | double friction_coeff, 36 | Eigen::Ref qp_penalty_weights, 37 | Eigen::Ref kc, 38 | Eigen::Ref dc, 39 | Eigen::Ref kb, 40 | Eigen::Ref db, 41 | Eigen::Ref frame_placement_error_gain, 42 | Eigen::Ref frame_velocity_error_gain) { 43 | const pinocchio::Model& pinocchio_model = 44 | boost::python::extract( 45 | model.ptr()); 46 | obj.initialize( 47 | mass, inertia, pinocchio_model, root_frame_name, 48 | end_frame_names, friction_coeff, qp_penalty_weights, 49 | kc, dc, kb, db, 50 | frame_placement_error_gain, frame_velocity_error_gain); 51 | return; 52 | }) 53 | 54 | .def("update_centroidal_gains", &CentroidalImpedanceController::update_centroidal_gains) 55 | .def("update_endeff_gains", &CentroidalImpedanceController::update_centroidal_gains) 56 | .def("run", &CentroidalImpedanceController::run) 57 | .def("get_joint_torques", 58 | &CentroidalImpedanceController::get_joint_torques, 59 | py::return_value_policy::reference); 60 | } 61 | 62 | } // namespace mim_control 63 | -------------------------------------------------------------------------------- /srcpy/centroidal_pd_controller.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 CentroidalPDController class. 8 | */ 9 | 10 | #include "mim_control/centroidal_pd_controller.hpp" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace py = pybind11; 16 | 17 | namespace mim_control 18 | { 19 | void bind_centroidal_pd_controller(py::module& module) 20 | { 21 | py::class_(module, "CentroidalPDController") 22 | .def(py::init<>()) 23 | 24 | // Public methods. 25 | .def("initialize", &CentroidalPDController::initialize) 26 | .def("run", &CentroidalPDController::run) 27 | .def("get_wrench", 28 | &CentroidalPDController::get_wrench, 29 | py::return_value_policy::reference); 30 | } 31 | 32 | } // namespace mim_control 33 | -------------------------------------------------------------------------------- /srcpy/impedance_controller.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 "mim_control/impedance_controller.hpp" 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace py = pybind11; 17 | 18 | namespace mim_control 19 | { 20 | void bind_impedance_controller(py::module& module) 21 | { 22 | py::class_(module, "ImpedanceController") 23 | .def(py::init<>()) 24 | 25 | // Public methods. 26 | .def("initialize", 27 | [](ImpedanceController& obj, 28 | py::object model, 29 | const std::string& root_frame_name, 30 | const std::string& end_frame_name) { 31 | const pinocchio::Model& pinocchio_model = 32 | boost::python::extract( 33 | model.ptr()); 34 | obj.initialize( 35 | pinocchio_model, root_frame_name, end_frame_name); 36 | return; 37 | }) 38 | .def("run", 39 | [](ImpedanceController& obj, 40 | const Eigen::VectorXd& robot_configuration, 41 | const Eigen::VectorXd& robot_velocity, 42 | const ImpedanceController::Array6d& gain_proportional, 43 | const ImpedanceController::Array6d& gain_derivative, 44 | const double& gain_feed_forward_force, 45 | py::object py_desired_end_frame_placement, 46 | py::object py_desired_end_frame_velocity, 47 | py::object py_feed_forward_force) { 48 | const pinocchio::SE3& desired_end_frame_placement = 49 | boost::python::extract( 50 | py_desired_end_frame_placement.ptr()); 51 | const pinocchio::Motion& desired_end_frame_velocity = 52 | boost::python::extract( 53 | py_desired_end_frame_velocity.ptr()); 54 | const pinocchio::Force& feed_forward_force = 55 | boost::python::extract( 56 | py_feed_forward_force.ptr()); 57 | obj.run(robot_configuration, 58 | robot_velocity, 59 | gain_proportional, 60 | gain_derivative, 61 | gain_feed_forward_force, 62 | desired_end_frame_placement, 63 | desired_end_frame_velocity, 64 | feed_forward_force); 65 | return; 66 | }) 67 | .def("get_torques", 68 | &ImpedanceController::get_torques, 69 | py::return_value_policy::reference) 70 | .def("get_joint_torques", 71 | &ImpedanceController::get_joint_torques, 72 | py::return_value_policy::reference) 73 | .def("get_impedance_force", 74 | &ImpedanceController::get_impedance_force, 75 | py::return_value_policy::reference); 76 | } 77 | 78 | } // namespace mim_control -------------------------------------------------------------------------------- /srcpy/mim_control.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 mim_control package. 8 | */ 9 | 10 | #include 11 | 12 | namespace mim_control 13 | { 14 | void bind_impedance_controller(pybind11::module &module); 15 | void bind_centroidal_pd_controller(pybind11::module &module); 16 | void bind_centroidal_force_qp_controller(pybind11::module &module); 17 | void bind_centroidal_impedance_controller(pybind11::module &module); 18 | 19 | PYBIND11_MODULE(mim_control_cpp, m) 20 | { 21 | m.doc() = R"pbdoc( 22 | mim_control python bindings 23 | --------------------------------- 24 | .. currentmodule:: mim_control 25 | .. autosummary:: 26 | :toctree: _generate 27 | add 28 | subtract 29 | )pbdoc"; 30 | 31 | bind_impedance_controller(m); 32 | bind_centroidal_pd_controller(m); 33 | bind_centroidal_force_qp_controller(m); 34 | bind_centroidal_impedance_controller(m); 35 | } 36 | 37 | } // namespace mim_control 38 | -------------------------------------------------------------------------------- /srcpy/wbc_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 "dynamic-graph/python/module.hh" 11 | #include "dynamic-graph/python/signal.hh" 12 | #include "mim_control/dynamic_graph/centroidal_force_qp_controller.hpp" 13 | #include "mim_control/dynamic_graph/centroidal_pd_controller.hpp" 14 | #include "mim_control/dynamic_graph/impedance_controller.hpp" 15 | 16 | namespace dg = dynamicgraph; 17 | 18 | typedef bp::return_value_policy 19 | reference_existing_object; 20 | 21 | BOOST_PYTHON_MODULE(wbc) 22 | { 23 | bp::import("dynamic_graph"); 24 | 25 | using mim_control::dynamic_graph::ImpedanceController; 26 | dynamicgraph::python::exposeEntity().def( 27 | "initialize", 28 | +[](ImpedanceController& ImpedanceController, 29 | const boost::python::object& pinocchio_model, 30 | const std::string& root_frame_name, 31 | const std::string& end_frame_name) { 32 | const pinocchio::Model& pinocchio_model_ref = 33 | boost::python::extract( 34 | pinocchio_model); 35 | ImpedanceController.initialize( 36 | pinocchio_model_ref, root_frame_name, end_frame_name); 37 | return; 38 | }, 39 | "Initialize the ImpedanceController."); 40 | 41 | using mim_control::dynamic_graph::CentroidalPDController; 42 | dynamicgraph::python::exposeEntity(); 43 | 44 | using mim_control::dynamic_graph::CentroidalForceQPController; 45 | dynamicgraph::python::exposeEntity().def( 46 | "initialize", 47 | &CentroidalForceQPController::initialize, 48 | "Initialize the CentroidalForceQPController."); 49 | } 50 | --------------------------------------------------------------------------------