├── .gitignore ├── .gitmodules ├── CHANGELOG.md ├── CITATION.cff ├── LICENSE ├── README.md ├── doc ├── .gitignore ├── Makefile └── src │ ├── conf.py │ ├── contact-stability.rst │ ├── examples.rst │ ├── forward-kinematics.rst │ ├── graphical-user-interface.rst │ ├── images │ ├── com_accel_cone.png │ ├── horizontal_walking.png │ ├── inverse_kinematics.png │ ├── logo.png │ ├── multi_contact_walking.png │ ├── pendulum_push.png │ ├── static_equilibrium_polygon.png │ ├── swing_foot.png │ └── zmp_support_area.png │ ├── index.rst │ ├── interpolation.rst │ ├── inverse-kinematics.rst │ ├── numerical-optimization.rst │ ├── references.rst │ ├── simulation-environment.rst │ └── walking-pattern-generation.rst ├── examples ├── README.md ├── contact_stability │ ├── README.md │ ├── com_accel_cone.py │ ├── com_robust_static_polytope.py │ ├── com_static_polygon.py │ ├── stances │ │ ├── double.json │ │ ├── single.json │ │ └── triple.json │ ├── wrench_friction_cone.py │ └── zmp_support_area.py ├── external_force.py ├── horizontal_walking.py ├── inverse_kinematics.py ├── lip_stabilization.py ├── multi_contact_walking.py └── vhip_stabilization.py ├── pymanoid ├── .gitignore ├── __init__.py ├── body.py ├── centroidal.py ├── contact.py ├── gui.py ├── ik.py ├── interp.py ├── misc.py ├── models.py ├── mpc.py ├── nlp.py ├── proc.py ├── robot.py ├── robots │ ├── __init__.py │ ├── hrp4.py │ └── jvrc1.py ├── sim.py ├── stance.py ├── swing_foot.py ├── tasks.py └── transformations.py └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | 5 | # C extensions 6 | *.so 7 | 8 | # Distribution / packaging 9 | .Python 10 | env/ 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | *.egg-info/ 23 | .installed.cfg 24 | *.egg 25 | 26 | # PyInstaller 27 | # Usually these files are written by a python script from a template 28 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 29 | *.manifest 30 | *.spec 31 | 32 | # Installer logs 33 | pip-log.txt 34 | pip-delete-this-directory.txt 35 | 36 | # Unit test / coverage reports 37 | htmlcov/ 38 | .tox/ 39 | .coverage 40 | .coverage.* 41 | .cache 42 | nosetests.xml 43 | coverage.xml 44 | *,cover 45 | 46 | # Translations 47 | *.mo 48 | *.pot 49 | 50 | # Django stuff: 51 | *.log 52 | 53 | # Sphinx documentation 54 | docs/_build/ 55 | 56 | # PyBuilder 57 | target/ 58 | 59 | # Rope 60 | .ropeproject 61 | 62 | # Local copy of pymanoid wiki 63 | wiki/ 64 | 65 | # COLLADA models 66 | *.dae 67 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pymanoid/pypoman"] 2 | path = pymanoid/pypoman 3 | url = https://github.com/stephane-caron/pypoman.git 4 | [submodule "pymanoid/qpsolvers"] 5 | path = pymanoid/qpsolvers 6 | url = https://github.com/stephane-caron/qpsolvers.git 7 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | All notable changes to this project will be documented in this file. 4 | 5 | ## Unreleased 6 | 7 | - Add an example for recording joint angle data to the simulation documentation 8 | - Add the JointRecorder process to record and plot joint trajectories 9 | - Document joint acceleration limits in inverse kinematics 10 | - Examples are now part of the documentation 11 | - Updated VHIP stabilization example to match [ICRA 2020 video](https://scaron.info/videos/icra-2020.mp4) 12 | 13 | ## [1.2.0] - 2019/10/26 14 | 15 | ### Added 16 | 17 | - Body: ``pos``, ``rotation_matrix`` and ``transform`` aliases 18 | - Body: compute the adjoint matrix in ``body.adjoint_matrix`` 19 | - Contact: ``contact.normal`` alias 20 | - Contact: ``set_wrench()`` function that switches a contact to managed mode 21 | - Contact: ``wrench_at()`` function to get wrench at a given point in the world frame 22 | - Contact: ``wrench``, ``force`` and ``moment`` attributes 23 | - Example: [external force](examples/external_force.py) acting on the right hand of the humanoid 24 | - Example: [stabilization with height variations](https://hal.archives-ouvertes.fr/hal-02289919v1/document) 25 | - InvertedPendulum: draw center of pressure by ``draw_point`` 26 | - Manipulator: ``wrench``, ``force`` and ``moment`` attributes 27 | - Robot: stance binding creates a new ``robot.wrench_distributor`` process 28 | - Simulation: can now ``unschedule()`` a process 29 | - StanceWrenchDistributor class to distribute contact wrenches of a stance 30 | 31 | ### Changed 32 | 33 | - CameraRecorder: ``wait_for()`` does not require ``sim`` argument any more 34 | - InvertedPendulum: clamping is enabled/disabled by ``self.clamp`` 35 | - InvertedPendulum: clamping is now the default behavior for ``set_cop`` and ``set_lambda`` 36 | - RobotWrenchDrawer became RobotDiscWrenchDrawer (discrete velocity differentiation) 37 | - RobotWrenchDrawer now fetches results from stance wrench distributor 38 | - License: switched to GPL as cddlib (and thus pypoman) is GPLv2 39 | - Updated qpsolvers submodule to v1.0.7 40 | 41 | ### Fixed 42 | 43 | - GUI: don't draw zero wrenches 44 | - Video recording and conversion script 45 | 46 | ## [1.1.0] - 2019/04/30 47 | 48 | ### Added 49 | 50 | - Example: [horizontal walking](examples/horizontal_walking.py) by linear model predictive control 51 | - IK: can now take joint acceleration limits into account 52 | - IK: upgraded with Levenberg-Marquardt damping 53 | - IK: warm-start parameter to ``solve()`` 54 | - Point: new attribute ``point.pdd`` for the acceleration 55 | - Point: new function ``point.integrate_constant_jerk()`` 56 | - Robot model gets a ``get_link()`` function 57 | - Simulation gets ``set_camera_transform()`` function 58 | - SwingFoot type: a polynomial swing foot interpolator 59 | - This change log 60 | - ZMP support areas can now take optional contact pressure limits 61 | 62 | ### Changed 63 | 64 | - Contact: ``copy()`` now takes optional ``hide`` keyword argument 65 | - GUI: default point size is now 1 cm 66 | - GUI: renamed ``draw_polyhedron()`` to ``draw_polytope()`` 67 | - IK: task strings now print both weight and gain coefficients 68 | - MPC: can now pass optional arguments to the QP solver in ``solve()`` 69 | - MPC: no need to call the ``build()`` function any more 70 | - Removed outdated [Copra](https://github.com/vsamy/copra/) wrapper 71 | - Stance: now bind end-effector links as well 72 | - Stance: simplified prototype of ``compute_zmp_support_area`` 73 | 74 | ### Fixed 75 | 76 | - IK: singularity fix from [Pfeiffer et al.](https://doi.org/10.1109/LRA.2018.2855265) 77 | - Knee joint names in JVRC-1 model 78 | - Python 3 compatibility 79 | - Restore initial settings in IK solve() 80 | 81 | ## [1.0.0] - 2018/10/17 82 | 83 | ### Added 84 | 85 | - Initial release of the project. Let's take it from there. 86 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you found this code helpful, please cite it as below." 3 | authors: 4 | - family-names: "Caron" 5 | given-names: "Stéphane" 6 | orcid: "https://orcid.org/0000-0003-2906-692X" 7 | title: "pymanoid" 8 | version: 1.2.0 9 | doi: 10.15083/00074003 10 | date-released: 2016-03-24 11 | url: "https://github.com/stephane-caron/pymanoid" 12 | license: "gpl-3.0" 13 | preferred-citation: 14 | type: generic 15 | title: "Computational Foundation for Planner-in-the-Loop Multi-Contact Whole-Body Control of Humanoid Robots" 16 | authors: 17 | - family-names: "Caron" 18 | given-names: "Stéphane" 19 | orcid: "https://orcid.org/0000-0003-2906-692X" 20 | doi: "10.15083/00074003" 21 | month: 3 22 | year: 2016 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pymanoid 2 | 3 | [![License](https://img.shields.io/badge/License-GPLv3-green.svg)](https://opensource.org/licenses/GPL-3.0) 4 | [![Documentation](https://img.shields.io/badge/docs-online-brightgreen?logo=read-the-docs&style=flat)](https://scaron.info/doc/pymanoid/) 5 | ![Status](https://img.shields.io/badge/status-archive-lightgrey.svg) 6 | 7 | Humanoid robotics controller prototyping environment based on [OpenRAVE](https://github.com/rdiankov/openrave). 8 | 9 | > ⚠️ This project is **archived**. Feel free to look at the code, but don't expect support to install and run it. 10 | 11 | Most of the project's functionality has been ported to follow-up libraries that are maintained and easier to install. 12 | 13 | ## Follow-up software 14 | 15 | 16 | 17 | 18 | 19 | - [pink](https://github.com/stephane-caron/pink): inverse kinematics in Python based on [Pinocchio](https://github.com/stack-of-tasks/pinocchio/) 20 | - [pypoman](https://github.com/stephane-caron/pypoman): polyhedral projection functions used to compute contact inequality constraints 21 | - [qpmpc](https://github.com/stephane-caron/qpmpc): linear model predictive control in Python 22 | - [qpsolvers](https://github.com/qpsolvers/qpsolvers): interfaces to quadratic programming solvers in Python 23 | - [vhip\_light](https://github.com/stephane-caron/vhip_light): variable-height inverted pendulum balancing in Python 24 | 25 | ## Features 26 | 27 | ### Contact stability 28 | 29 | - [Wrench friction cones](http://www.roboticsproceedings.org/rss11/p28.pdf) for general multi-contact motions 30 | - [Multi-contact ZMP support areas](https://hal.archives-ouvertes.fr/hal-02108589/document) for locomotion 31 | - [CoM acceleration cones](https://hal.archives-ouvertes.fr/hal-01349880/document) for locomotion (conservative) 32 | - [Robust CoM static-equilibrium polytope](https://hal-lirmm.ccsd.cnrs.fr/lirmm-01477362/document) for posture generation (conservative) 33 | 34 | ### Model predictive control 35 | 36 | - [Linear model predictive control](https://hal.archives-ouvertes.fr/hal-01349880/document) (LMPC) for locomotion 37 | - [Nonlinear model predictive control](https://hal.archives-ouvertes.fr/hal-01481052/document) (NMPC) for locomotion 38 | 39 | ### Inverse kinematics 40 | 41 | - Whole-body IK based on the [weight-prioritized multi-task formulation](https://scaron.info/robot-locomotion/inverse-kinematics.html) 42 | - Jacobians and Hessians for center of mass (CoM) and angular momentum tasks 43 | 44 | ### Geometry and optimization toolbox 45 | 46 | - Interfaces to polyhedral geometry: double description, polytope projection 47 | - Interfaces for numerical optimization solvers: LP, QP and NLP 48 | 49 | ## Use cases 50 | 51 | 52 | 53 | - [Walking pattern generation over uneven terrains](https://github.com/stephane-caron/capture-walkgen) based on capturability of the variable-height inverted pendulum model 54 | - [Nonlinear model predictive control](https://github.com/stephane-caron/fip-walkgen) using a direct transcription of centroidal dynamics 55 | - [Linearized model predictive control](https://github.com/stephane-caron/multi-contact-walkgen) using a conservative linearization of CoM acceleration cones 56 | - [Multi-contact ZMP support areas](https://github.com/stephane-caron/multi-contact-zmp) for locomotion in multi-contact scenarios (including hand contacts) 57 | - [Humanoid stair climbing](https://github.com/stephane-caron/quasistatic-stair-climbing) demonstrated on the HRP-4 robot 58 | 59 | ## Getting started 60 | 61 | - [Documentation](https://scaron.info/doc/pymanoid/) 62 | - [FAQ](https://github.com/stephane-caron/pymanoid/wiki/Frequently-Asked-Questions) 63 | - [Examples](https://github.com/stephane-caron/pymanoid/tree/master/examples) 64 | - Tutorial: [Prototyping a walking pattern generator](https://scaron.info/robot-locomotion/prototyping-a-walking-pattern-generator.html) 65 | 66 | ## Citing pymanoid 67 | 68 | I developed pymanoid during my PhD studies and share it in the hope it can be useful to others. If it helped you in your research, please cite it *e.g.* as follows: 69 | 70 | ```bibtex 71 | @phdthesis{caron2016thesis, 72 | title = {Computational Foundation for Planner-in-the-Loop Multi-Contact Whole-Body Control of Humanoid Robots}, 73 | author = {Caron, St{\'e}phane}, 74 | year = {2016}, 75 | month = jan, 76 | school = {The University of Tokyo}, 77 | url = {https://scaron.info/papers/thesis.pdf}, 78 | doi = {10.15083/00074003}, 79 | } 80 | ``` 81 | 82 | ## Installation 83 | 84 | It is not recommended to try to install this library as it is not maintained and relies on deprecated software. Yet, If you are digging into the archives, you can find instructions for Ubuntu 14.04 in [the wiki](https://github.com/stephane-caron/pymanoid/wiki/Installation-instructions). 85 | -------------------------------------------------------------------------------- /doc/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | -------------------------------------------------------------------------------- /doc/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | BUILDDIR = build 8 | ONLINEDIR = scaron.info:public_html/doc/pymanoid 9 | PDFFILE = pymanoid.pdf 10 | 11 | # Internal variables. 12 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(SPHINXOPTS) src 13 | 14 | .PHONY: help 15 | help: 16 | @echo "Please use \`make ' where is one of" 17 | @echo " html to make standalone HTML files" 18 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 19 | @echo " linkcheck to check all external links for integrity" 20 | 21 | .PHONY: clean 22 | clean: 23 | rm -rf $(BUILDDIR)/* 24 | 25 | .PHONY: html 26 | html: 27 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 28 | @echo 29 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 30 | 31 | .PHONY: latexpdf 32 | latexpdf: 33 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 34 | @echo "Running LaTeX files through pdflatex..." 35 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 36 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 37 | 38 | .PHONY: linkcheck 39 | linkcheck: 40 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 41 | @echo 42 | @echo "Link check complete; look for any errors in the above output " \ 43 | "or in $(BUILDDIR)/linkcheck/output.txt." 44 | 45 | .PHONY: pdfopen 46 | pdfopen: latexpdf 47 | zathura $(BUILDDIR)/latex/$(PDFFILE) 48 | 49 | .PHONY: open 50 | open: 51 | firefox $(BUILDDIR)/html/index.html 52 | 53 | .PHONY: upload 54 | upload: html latexpdf 55 | cp $(BUILDDIR)/latex/$(PDFFILE) $(BUILDDIR)/html/$(PDFFILE) 56 | rsync -auvz --delete-after $(BUILDDIR)/html/ $(ONLINEDIR)/ 57 | -------------------------------------------------------------------------------- /doc/src/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # pymanoid documentation build configuration file, created by 4 | # sphinx-quickstart on Fri Jan 13 14:41:18 2017. 5 | 6 | import os 7 | import sys 8 | 9 | sys.path.insert(0, os.path.abspath("../..")) 10 | sys.path.insert(0, os.path.abspath("../../examples")) 11 | sys.path.insert(0, os.path.abspath("../../examples/contact_stability")) 12 | 13 | # -- General configuration ------------------------------------------------ 14 | 15 | # If your documentation needs a minimal Sphinx version, state it here. 16 | # 17 | # needs_sphinx = '1.0' 18 | 19 | # Add any Sphinx extension module names here, as strings. They can be 20 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 21 | # ones. 22 | extensions = [ 23 | "sphinx.ext.autodoc", 24 | "sphinx.ext.coverage", 25 | "sphinx.ext.mathjax", 26 | "sphinx.ext.napoleon", 27 | # 'numpydoc', 28 | ] 29 | 30 | # Add mock imports so that the doc can be built without all dependencies 31 | autodoc_mock_imports = ["casadi", "openravepy", "pyclipper", "stabilipy"] 32 | 33 | # Add any paths that contain templates here, relative to this directory. 34 | templates_path = [] 35 | 36 | # The suffix(es) of source filenames. 37 | # You can specify multiple suffix as a list of string: 38 | # 39 | # source_suffix = ['.rst', '.md'] 40 | source_suffix = ".rst" 41 | 42 | # The encoding of source files. 43 | # 44 | # source_encoding = 'utf-8-sig' 45 | 46 | # The master toctree document. 47 | master_doc = "index" 48 | 49 | # General information about the project. 50 | project = u"pymanoid" 51 | copyright = u"2015–2019, Stéphane Caron" 52 | author = u"Stéphane Caron" 53 | 54 | # The version info for the project you're documenting, acts as replacement for 55 | # |version| and |release|, also used in various other places throughout the 56 | # built documents. 57 | # 58 | # The short X.Y version. 59 | version = "1.2" 60 | # The full version, including alpha/beta/rc tags. 61 | release = "1.2.0" 62 | 63 | # The language for content autogenerated by Sphinx. Refer to documentation 64 | # for a list of supported languages. 65 | # 66 | # This is also used if you do content translation via gettext catalogs. 67 | # Usually you set "language" from the command line for these cases. 68 | language = None 69 | 70 | # There are two options for replacing |today|: either, you set today to some 71 | # non-false value, then it is used: 72 | # 73 | # today = '' 74 | # 75 | # Else, today_fmt is used as the format for a strftime call. 76 | # 77 | # today_fmt = '%B %d, %Y' 78 | 79 | # List of patterns, relative to source directory, that match files and 80 | # directories to ignore when looking for source files. 81 | # This patterns also effect to html_static_path and html_extra_path 82 | exclude_patterns = ["build", "Thumbs.db", ".DS_Store"] 83 | 84 | # The reST default role (used for this markup: `text`) to use for all 85 | # documents. 86 | # 87 | # default_role = None 88 | 89 | # If true, '()' will be appended to :func: etc. cross-reference text. 90 | # 91 | # add_function_parentheses = True 92 | 93 | # If true, the current module name will be prepended to all description 94 | # unit titles (such as .. function::). 95 | # 96 | # add_module_names = True 97 | 98 | # If true, sectionauthor and moduleauthor directives will be shown in the 99 | # output. They are ignored by default. 100 | # 101 | # show_authors = False 102 | 103 | # The name of the Pygments (syntax highlighting) style to use. 104 | pygments_style = "sphinx" 105 | 106 | # A list of ignored prefixes for module index sorting. 107 | # modindex_common_prefix = [] 108 | 109 | # If true, keep warnings as "system message" paragraphs in the built documents. 110 | # keep_warnings = False 111 | 112 | # If true, `todo` and `todoList` produce output, else they produce nothing. 113 | todo_include_todos = False 114 | 115 | 116 | # -- Options for HTML output ---------------------------------------------- 117 | 118 | # The theme to use for HTML and HTML Help pages. See the documentation for 119 | # a list of builtin themes. 120 | # 121 | html_theme = "sphinx_rtd_theme" 122 | # html_theme = 'classic' 123 | 124 | # Theme options are theme-specific and customize the look and feel of a theme 125 | # further. For a list of options available for each theme, see the 126 | # documentation. 127 | # 128 | # html_theme_options = {} 129 | 130 | # Add any paths that contain custom themes here, relative to this directory. 131 | # html_theme_path = [] 132 | 133 | # The name for this set of Sphinx documents. 134 | # " v documentation" by default. 135 | # 136 | # html_title = u'pymanoid v0.6.0' 137 | 138 | # A shorter title for the navigation bar. Default is the same as html_title. 139 | # 140 | # html_short_title = None 141 | 142 | # The name of an image file (relative to this directory) to place at the top 143 | # of the sidebar. 144 | # 145 | # html_logo = None 146 | 147 | # The name of an image file (relative to this directory) to use as a favicon of 148 | # the docs. This file should be a Windows icon file (.ico) being 16x16 or 149 | # 32x32 pixels large. 150 | # 151 | # html_favicon = None 152 | 153 | # Add any paths that contain custom static files (such as style sheets) here, 154 | # relative to this directory. They are copied after the builtin static files, 155 | # so a file named "default.css" will overwrite the builtin "default.css". 156 | html_static_path = ["images"] 157 | 158 | # Add any extra paths that contain custom files (such as robots.txt or 159 | # .htaccess) here, relative to this directory. These files are copied 160 | # directly to the root of the documentation. 161 | # 162 | # html_extra_path = [] 163 | 164 | # If not None, a 'Last updated on:' timestamp is inserted at every page 165 | # bottom, using the given strftime format. 166 | # The empty string is equivalent to '%b %d, %Y'. 167 | # 168 | # html_last_updated_fmt = None 169 | 170 | # If true, SmartyPants will be used to convert quotes and dashes to 171 | # typographically correct entities. 172 | # 173 | # html_use_smartypants = True 174 | 175 | # Custom sidebar templates, maps document names to template names. 176 | # 177 | # html_sidebars = {} 178 | 179 | # Additional templates that should be rendered to pages, maps page names to 180 | # template names. 181 | # 182 | # html_additional_pages = {} 183 | 184 | # If false, no module index is generated. 185 | # 186 | # html_domain_indices = True 187 | 188 | # If false, no index is generated. 189 | # 190 | # html_use_index = True 191 | 192 | # If true, the index is split into individual pages for each letter. 193 | # 194 | # html_split_index = False 195 | 196 | # If true, links to the reST sources are added to the pages. 197 | # 198 | # html_show_sourcelink = True 199 | 200 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 201 | # 202 | # html_show_sphinx = True 203 | 204 | # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. 205 | # 206 | # html_show_copyright = True 207 | 208 | # If true, an OpenSearch description file will be output, and all pages will 209 | # contain a tag referring to it. The value of this option must be the 210 | # base URL from which the finished HTML is served. 211 | # 212 | # html_use_opensearch = '' 213 | 214 | # This is the file name suffix for HTML files (e.g. ".xhtml"). 215 | # html_file_suffix = None 216 | 217 | # Language to be used for generating the HTML full-text search index. 218 | # Sphinx supports the following languages: 219 | # 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' 220 | # 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr', 'zh' 221 | # 222 | # html_search_language = 'en' 223 | 224 | # A dictionary with options for the search language support, empty by default. 225 | # 'ja' uses this config value. 226 | # 'zh' user can custom change `jieba` dictionary path. 227 | # 228 | # html_search_options = {'type': 'default'} 229 | 230 | # The name of a javascript file (relative to the configuration directory) that 231 | # implements a search results scorer. If empty, the default will be used. 232 | # 233 | # html_search_scorer = 'scorer.js' 234 | 235 | # Output file base name for HTML help builder. 236 | htmlhelp_basename = "pymanoiddoc" 237 | 238 | # -- Options for LaTeX output --------------------------------------------- 239 | 240 | latex_elements = { 241 | # The paper size ('letterpaper' or 'a4paper'). 242 | # 243 | "papersize": "a4paper", 244 | # The font size ('10pt', '11pt' or '12pt'). 245 | # 246 | "pointsize": "12pt", 247 | # Additional stuff for the LaTeX preamble. 248 | # 249 | # 'preamble': '', 250 | # Latex figure (float) alignment 251 | # 252 | # 'figure_align': 'htbp', 253 | } 254 | 255 | # Grouping the document tree into LaTeX files. List of tuples 256 | # (source start file, target name, title, 257 | # author, documentclass [howto, manual, or own class]). 258 | latex_documents = [ 259 | ( 260 | master_doc, 261 | project + ".tex", 262 | project + u" Documentation", 263 | author, 264 | "howto", 265 | ), 266 | ] 267 | 268 | # The name of an image file (relative to this directory) to place at the top of 269 | # the title page. 270 | # 271 | # latex_logo = None 272 | 273 | # For "manual" documents, if this is true, then toplevel headings are parts, 274 | # not chapters. 275 | # 276 | # latex_use_parts = False 277 | 278 | # If true, show page references after internal links. 279 | # 280 | # latex_show_pagerefs = False 281 | 282 | # If true, show URL addresses after external links. 283 | # 284 | # latex_show_urls = False 285 | 286 | # Documents to append as an appendix to all manuals. 287 | # 288 | # latex_appendices = [] 289 | 290 | # It false, will not define \strong, \code, itleref, \crossref ... but only 291 | # \sphinxstrong, ..., \sphinxtitleref, ... To help avoid clash with user added 292 | # packages. 293 | # 294 | # latex_keep_old_macro_names = True 295 | 296 | # If false, no module index is generated. 297 | # 298 | # latex_domain_indices = True 299 | -------------------------------------------------------------------------------- /doc/src/contact-stability.rst: -------------------------------------------------------------------------------- 1 | ***************** 2 | Contact stability 3 | ***************** 4 | 5 | Contact 6 | ======= 7 | 8 | .. autoclass:: pymanoid.contact.Contact 9 | :members: 10 | 11 | Multiple contacts 12 | ================= 13 | 14 | .. autoclass:: pymanoid.contact.ContactSet 15 | :members: 16 | 17 | Computing contact forces 18 | ======================== 19 | 20 | Contact wrenches exerted on the robot while it moves can be computed by 21 | quadratic programming in the *wrench distributor* of a stance. This process is 22 | automatically created when binding a :class:`Stance` to the robot model. It 23 | will only be executed if you schedule it to your simulation. Here is a small 24 | example: 25 | 26 | .. code:: 27 | 28 | from pymanoid import robots, Simulation, Stance 29 | 30 | sim = Simulation(dt=0.03) 31 | robot = robots.JVRC1(download_if_needed=True) 32 | stance = Stance( 33 | com=robot.get_com_point_mass(), 34 | left_foot=robot.left_foot.get_contact(pos=[0, 0.3, 0]), 35 | right_foot=robot.right_foot.get_contact(pos=[0, -0.3, 0])) 36 | stance.com.set_z(0.8) 37 | stance.bind(robot) 38 | sim.schedule(robot.ik) 39 | sim.schedule(robot.wrench_distributor) 40 | sim.start() 41 | 42 | You can see the computed wrenches in the GUI by scheduling the corresponding 43 | drawer process: 44 | 45 | .. code:: 46 | 47 | from pymanoid.gui import RobotWrenchDrawer 48 | 49 | sim.set_viewer() 50 | sim.schedule_extra(RobotWrenchDrawer(robot)) 51 | 52 | Once the wrench distributor is scheduled, it will store its outputs in the 53 | contacts of the stance as well as in the robot's manipulators. Therefore, you 54 | can access ``robot.left_foot.wrench`` or ``robot.stance.left_foot.wrench`` 55 | equivalently. Note that wrenches are given in the *world frame* rooted at their 56 | respective contact points. 57 | 58 | .. autoclass:: pymanoid.stance.StanceWrenchDistributor 59 | :members: 60 | -------------------------------------------------------------------------------- /doc/src/examples.rst: -------------------------------------------------------------------------------- 1 | ******** 2 | Examples 3 | ******** 4 | 5 | Linear inverted pendulum stabilization 6 | -------------------------------------- 7 | 8 | .. automodule:: lip_stabilization 9 | :members: 10 | 11 | Inverse kinematics 12 | ------------------ 13 | 14 | .. automodule:: inverse_kinematics 15 | :members: 16 | 17 | Contact stability 18 | ----------------- 19 | 20 | Wrench friction cone 21 | ~~~~~~~~~~~~~~~~~~~~~ 22 | 23 | .. automodule:: wrench_friction_cone 24 | :members: 25 | 26 | CoM static-equilibrium polygon 27 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 28 | 29 | .. automodule:: com_static_polygon 30 | :members: 31 | 32 | Multi-contact ZMP support areas 33 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 34 | 35 | .. automodule:: zmp_support_area 36 | :members: 37 | 38 | CoM acceleration cone 39 | ~~~~~~~~~~~~~~~~~~~~~ 40 | 41 | .. automodule:: com_accel_cone 42 | :members: 43 | 44 | CoM robust static-equilibrium polytope 45 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 46 | 47 | .. automodule:: com_robust_static_polytope 48 | :members: 49 | 50 | Horizontal walking 51 | ------------------ 52 | 53 | .. automodule:: horizontal_walking 54 | :members: 55 | 56 | Multi-contact walking 57 | --------------------- 58 | 59 | .. automodule:: multi_contact_walking 60 | :members: 61 | 62 | VHIP stabilization 63 | ------------------ 64 | 65 | .. automodule:: vhip_stabilization 66 | :members: 67 | -------------------------------------------------------------------------------- /doc/src/forward-kinematics.rst: -------------------------------------------------------------------------------- 1 | ****************** 2 | Forward kinematics 3 | ****************** 4 | 5 | Rigid transformations 6 | ===================== 7 | 8 | .. automodule:: pymanoid.transformations 9 | :members: 10 | 11 | Rigid body model 12 | ================ 13 | 14 | .. automodule:: pymanoid.body 15 | :members: 16 | 17 | Robot model 18 | =========== 19 | 20 | .. automodule:: pymanoid.robot 21 | :members: 22 | -------------------------------------------------------------------------------- /doc/src/graphical-user-interface.rst: -------------------------------------------------------------------------------- 1 | ************************ 2 | Graphical user interface 3 | ************************ 4 | 5 | Primitive functions 6 | =================== 7 | 8 | .. autofunction:: pymanoid.gui.draw_arrow 9 | .. autofunction:: pymanoid.gui.draw_force 10 | .. autofunction:: pymanoid.gui.draw_line 11 | .. autofunction:: pymanoid.gui.draw_point 12 | .. autofunction:: pymanoid.gui.draw_points 13 | .. autofunction:: pymanoid.gui.draw_trajectory 14 | .. autofunction:: pymanoid.gui.draw_wrench 15 | 16 | Convex polyhedra 17 | ================ 18 | 19 | `Polyhedra `_ correspond to the 20 | sets of linear inequality constraints applied to the system, for instance the 21 | support area in which the zero-tilting moment point (ZMP) of a legged robot 22 | should lie to avoid breaking contacts with the ground. 23 | 24 | .. autofunction:: pymanoid.gui.draw_2d_cone 25 | .. autofunction:: pymanoid.gui.draw_cone 26 | .. autofunction:: pymanoid.gui.draw_horizontal_polygon 27 | .. autofunction:: pymanoid.gui.draw_polygon 28 | .. autofunction:: pymanoid.gui.draw_polytope 29 | 30 | Drawers 31 | ======= 32 | 33 | .. autoclass:: pymanoid.gui.WrenchDrawer 34 | :members: 35 | 36 | .. autoclass:: pymanoid.gui.PointMassWrenchDrawer 37 | :members: 38 | 39 | .. autoclass:: pymanoid.gui.RobotWrenchDrawer 40 | :members: 41 | 42 | .. autoclass:: pymanoid.gui.StaticEquilibriumWrenchDrawer 43 | :members: 44 | 45 | .. autoclass:: pymanoid.gui.TrajectoryDrawer 46 | :members: 47 | -------------------------------------------------------------------------------- /doc/src/images/com_accel_cone.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stephane-caron/pymanoid/d4e48c302e68127f4878e02fdc209f607328ee61/doc/src/images/com_accel_cone.png -------------------------------------------------------------------------------- /doc/src/images/horizontal_walking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stephane-caron/pymanoid/d4e48c302e68127f4878e02fdc209f607328ee61/doc/src/images/horizontal_walking.png -------------------------------------------------------------------------------- /doc/src/images/inverse_kinematics.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stephane-caron/pymanoid/d4e48c302e68127f4878e02fdc209f607328ee61/doc/src/images/inverse_kinematics.png -------------------------------------------------------------------------------- /doc/src/images/logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stephane-caron/pymanoid/d4e48c302e68127f4878e02fdc209f607328ee61/doc/src/images/logo.png -------------------------------------------------------------------------------- /doc/src/images/multi_contact_walking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stephane-caron/pymanoid/d4e48c302e68127f4878e02fdc209f607328ee61/doc/src/images/multi_contact_walking.png -------------------------------------------------------------------------------- /doc/src/images/pendulum_push.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stephane-caron/pymanoid/d4e48c302e68127f4878e02fdc209f607328ee61/doc/src/images/pendulum_push.png -------------------------------------------------------------------------------- /doc/src/images/static_equilibrium_polygon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stephane-caron/pymanoid/d4e48c302e68127f4878e02fdc209f607328ee61/doc/src/images/static_equilibrium_polygon.png -------------------------------------------------------------------------------- /doc/src/images/swing_foot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stephane-caron/pymanoid/d4e48c302e68127f4878e02fdc209f607328ee61/doc/src/images/swing_foot.png -------------------------------------------------------------------------------- /doc/src/images/zmp_support_area.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stephane-caron/pymanoid/d4e48c302e68127f4878e02fdc209f607328ee61/doc/src/images/zmp_support_area.png -------------------------------------------------------------------------------- /doc/src/index.rst: -------------------------------------------------------------------------------- 1 | .. title:: Table of Contents 2 | 3 | #################### 4 | Welcome to pymanoid! 5 | #################### 6 | 7 | .. **Release 1.2.0 -- Oct 26, 2019** 8 | 9 | .. figure:: images/logo.png 10 | :align: center 11 | 12 | .. toctree:: 13 | 14 | examples.rst 15 | interpolation.rst 16 | forward-kinematics.rst 17 | inverse-kinematics.rst 18 | contact-stability.rst 19 | walking-pattern-generation.rst 20 | numerical-optimization.rst 21 | simulation-environment.rst 22 | graphical-user-interface.rst 23 | references.rst 24 | -------------------------------------------------------------------------------- /doc/src/interpolation.rst: -------------------------------------------------------------------------------- 1 | ************* 2 | Interpolation 3 | ************* 4 | 5 | Interpolation is the operation of computing trajectories that connect an 6 | initial state (position and its derivatives such as velocity and acceleration) 7 | to a desired one. 8 | 9 | Position 10 | ======== 11 | 12 | In the absence of inequality constraints, interpolation for positions can be 13 | solved by `polynomial interpolation 14 | `_. The following 15 | functions and classes provide simple interfaces for this common operation. 16 | 17 | .. autofunction:: pymanoid.interp.interpolate_cubic_bezier 18 | 19 | .. autofunction:: pymanoid.interp.interpolate_cubic_hermite 20 | 21 | .. autoclass:: pymanoid.interp.LinearPosInterpolator 22 | :members: 23 | 24 | .. autoclass:: pymanoid.interp.CubicPosInterpolator 25 | :members: 26 | 27 | .. autoclass:: pymanoid.interp.QuinticPosInterpolator 28 | :members: 29 | 30 | Orientation 31 | =========== 32 | 33 | In the absence of inequality constraints, interpolation for orientations can be 34 | solved by `spherical linear interpolation 35 | `_. The following functions and classes 36 | provide simple interfaces for this common operation. They compute *poses*: 37 | following OpenRAVE terminology, the pose of a rigid body denotes the 7D vector 38 | of its 4D orientation quaternion followed by its 3D position coordinates. 39 | 40 | .. autofunction:: pymanoid.interp.interpolate_pose_linear 41 | 42 | .. autofunction:: pymanoid.interp.interpolate_pose_quadratic 43 | 44 | .. autoclass:: pymanoid.interp.LinearPoseInterpolator 45 | :members: 46 | 47 | .. autoclass:: pymanoid.interp.CubicPoseInterpolator 48 | :members: 49 | 50 | .. autoclass:: pymanoid.interp.PoseInterpolator 51 | :members: 52 | 53 | .. autoclass:: pymanoid.interp.QuinticPoseInterpolator 54 | :members: 55 | -------------------------------------------------------------------------------- /doc/src/inverse-kinematics.rst: -------------------------------------------------------------------------------- 1 | ****************** 2 | Inverse kinematics 3 | ****************** 4 | 5 | Inverse kinematics (IK) is the problem of computing *motions* (velocities, 6 | accelerations) that make the robot achieve a given set of *tasks*, such as 7 | putting a foot on a surface, moving the center of mass (COM) to a target 8 | location, etc. If you are not familiar with these concepts, check out `this 9 | introduction to inverse kinematics 10 | `_. 11 | 12 | Tasks 13 | ===== 14 | 15 | Tasks are the way to specify objectives to the robot model in a human-readable 16 | way. 17 | 18 | .. automodule:: pymanoid.tasks 19 | :members: 20 | 21 | Solver 22 | ====== 23 | 24 | The IK solver is the numerical optimization program that converts task targets 25 | and the current robot state to joint motions. In pymanoid, joint motions are 26 | computed as velocities that are integrated forward during each simulation cycle 27 | (other IK solvers may compute acceleration or jerk values, which are then 28 | integrated twice or thrice respectively). 29 | 30 | .. autoclass:: pymanoid.ik.IKSolver 31 | :members: 32 | 33 | Acceleration limits 34 | ------------------- 35 | 36 | When the robot model has joint acceleration limits, *i.e.* when a vector of 37 | size ``robot.nb_dofs`` is specified in ``robot.qdd_lim`` at initialization, the 38 | inverse kinematics will include them in its optimization problem. The 39 | formulation is more complex than a finite-difference approximation: joint 40 | velocities will be selected so that (1) the joint does not collide with its 41 | position limit in one iteration, but also (2) despite its acceleration limit, 42 | it can still brake fast enough to avoid colliding with its position limit in 43 | the future. The inverse kinematics in pymanoid implements the solution to this 44 | problem described in Equation (14) of [Flacco15]_. 45 | 46 | Stance 47 | ====== 48 | 49 | The most common IK tasks for humanoid locomotion are the `COM task 50 | <#pymanoid.tasks.COMTask>`_ and `end-effector pose task 51 | <#pymanoid.tasks.PoseTask>`_. The ``Stance`` class avoids the hassle of 52 | creating and adding these tasks one by one. To use it, first create your targets: 53 | 54 | .. code:: 55 | 56 | com_target = robot.get_com_point_mass() 57 | lf_target = robot.left_foot.get_contact(pos=[0, 0.3, 0]) 58 | rf_target = robot.right_foot.get_contact(pos=[0, -0.3, 0]) 59 | 60 | Then create the stance and bind it to the robot: 61 | 62 | .. code:: 63 | 64 | stance = Stance(com=com_target, left_foot=lf_target, right_foot=rf_target) 65 | stance.bind(robot) 66 | 67 | Calling the `bind <#pymanoid.stance.Stance.bind>`_ function will automatically add the corresponding tasks to the robot IK solver. See the `inverse_kinematics.py `_ example for more details. 68 | 69 | .. autoclass:: pymanoid.stance.Stance 70 | :members: 71 | 72 | Example 73 | ======= 74 | 75 | In this example, we will see how to put the humanoid robot model in a desired 76 | configuration. Let us initialize a simulation with a 30 ms timestep and load 77 | the JVRC-1 humanoid: 78 | 79 | .. code:: 80 | 81 | sim = pymanoid.Simulation(dt=0.03) 82 | robot = JVRC1('JVRC-1.dae', download_if_needed=True) 83 | sim.set_viewer() # open GUI window 84 | 85 | We define targets for foot contacts; 86 | 87 | .. code:: 88 | 89 | lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0], visible=True) 90 | rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0], visible=True) 91 | 92 | Next, let us set the altitude of the robot's free-flyer (attached to the waist 93 | link) 80 cm above contacts. This is useful to give the IK solver a good initial 94 | guess and avoid coming out with a valid but weird solution in the end. 95 | 96 | .. code:: 97 | 98 | robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z]) 99 | 100 | This being done, we initialize a point-mass that will serve as center-of-mass 101 | (COM) target for the IK. Its initial position is set to ``robot.com``, which 102 | will be roughly 80 cm above contacts as it is close to the waist link: 103 | 104 | .. code:: 105 | 106 | com_target = PointMass(pos=robot.com, mass=robot.mass) 107 | 108 | All our targets being defined, we initialize IK tasks for the feet and COM 109 | position, as well as a posture task for the (necessary) regularization of the 110 | underlying QP problem: 111 | 112 | .. code:: 113 | 114 | lf_task = ContactTask(robot, robot.left_foot, lf_target, weight=1.) 115 | rf_task = ContactTask(robot, robot.right_foot, rf_target, weight=1.) 116 | com_task = COMTask(robot, com_target, weight=1e-2) 117 | reg_task = PostureTask(robot, robot.q, weight=1e-6) 118 | 119 | We initialize the robot's IK using all DOFs, and insert our tasks: 120 | 121 | .. code:: 122 | 123 | robot.init_ik(active_dofs=robot.whole_body) 124 | robot.ik.add_task(lf_task) 125 | robot.ik.add_task(rf_task) 126 | robot.ik.add_task(com_task) 127 | robot.ik.add_task(reg_task) 128 | 129 | We can also throw in some extra DOF tasks for a nicer posture: 130 | 131 | .. code:: 132 | 133 | robot.ik.add_task(DOFTask(robot, robot.R_SHOULDER_R, -0.5, gain=0.5, weight=1e-5)) 134 | robot.ik.add_task(DOFTask(robot, robot.L_SHOULDER_R, +0.5, gain=0.5, weight=1e-5)) 135 | 136 | Finally, call the IK solver to update the robot posture: 137 | 138 | .. code:: 139 | 140 | robot.ik.solve(max_it=100, impr_stop=1e-4) 141 | 142 | The resulting posture looks like this: 143 | 144 | .. figure:: images/inverse_kinematics.png 145 | :align: center 146 | 147 | Robot posture found by inverse kinematics. 148 | 149 | Alternatively, rather than creating and adding all tasks one by one, we could 150 | have used the `Stance <#pymanoid.stance.Stance>`_ interface: 151 | 152 | .. code:: 153 | 154 | stance = Stance(com=com_target, left_foot=lf_target, right_foot=rf_target) 155 | stance.bind(robot) 156 | robot.ik.add_task(DOFTask(robot, robot.R_SHOULDER_R, -0.5, gain=0.5, weight=1e-5)) 157 | robot.ik.add_task(DOFTask(robot, robot.L_SHOULDER_R, +0.5, gain=0.5, weight=1e-5)) 158 | robot.ik.solve(max_it=100, impr_stop=1e-4) 159 | 160 | This code is more concise and yields the same result. 161 | -------------------------------------------------------------------------------- /doc/src/numerical-optimization.rst: -------------------------------------------------------------------------------- 1 | ********************** 2 | Numerical optimization 3 | ********************** 4 | 5 | Quadratic programming 6 | ===================== 7 | 8 | Quadratic programming is a class of numerical optimization problems that can be 9 | solved efficiently. It is used for instance by the `IK solver 10 | `_ to solve whole-body control by inverse 11 | kinematics. 12 | 13 | .. autofunction:: pymanoid.qpsolvers.solve_qp 14 | 15 | Nonlinear programming 16 | ===================== 17 | 18 | Nonlinear programming is a catch-all expression for numerical optimization 19 | problems that don't have any particular structure, such as *convexity*, 20 | allowing them to be solved more efficiently by dedicated methods. It is used 21 | for instance in `nonlinear model predictive control 22 | `_ to compute 23 | locomotion trajectories over uneven terrains. 24 | 25 | .. autoclass:: pymanoid.nlp.NonlinearProgram 26 | :members: 27 | -------------------------------------------------------------------------------- /doc/src/references.rst: -------------------------------------------------------------------------------- 1 | ********** 2 | References 3 | ********** 4 | 5 | .. [Caron20] `Biped Stabilization by Linear Feedback of the Variable-Height 6 | Inverted Pendulum Model 7 | `_, S. Caron, IEEE 8 | International Conference on Robotics and Automation, May 2020. 9 | 10 | .. [Caron19] `Capturability-based Pattern Generation for Walking with Variable 11 | Height `_, S. Caron, 12 | A. Escande, L. Lanari and B. Mallein, IEEE Transactions on Robotics, July 13 | 2019. 14 | 15 | .. [Audren18] `3D robust stability polyhedron in multi-contact 16 | `_, H. Audren and A. 17 | Kheddar, IEEE Transactions on Robotics, vol., 34, no. 2, February 2018. 18 | 19 | .. [Caron17w] `Dynamic Walking over Rough Terrains by Nonlinear Predictive 20 | Control of the Floating-base Inverted Pendulum 21 | `_, S. Caron and A. 22 | Kheddar, IEEE/RSJ International Conference on Intelligent Robots and 23 | Systems, September 2017. 24 | 25 | .. [Caron17z] `ZMP support areas for multi-contact mobility under frictional 26 | constraints `_, S. 27 | Caron, Q.-C. Pham and Y. Nakamura, IEEE Transactions on Robotics, vol. 33, 28 | no. 1, February 2017. 29 | 30 | .. [Scianca16] `Intrinsically stable MPC for humanoid gait generation 31 | `_, 32 | N. Scianca, M. Cognetti, D. De Simone, L. Lanari and G. Oriolo, IEEE-RAS 33 | International Conference on Humanoid Robots, November 2016. 34 | 35 | .. [Caron16] `Multi-contact Walking Pattern Generation based on Model Preview 36 | Control of 3D COM Accelerations 37 | `_, S. Caron and A. 38 | Kheddar, IEEE-RAS International Conference on Humanoid Robots, November 39 | 2016. 40 | 41 | .. [Nozawa16] `Three-dimensional humanoid motion planning using COM feasible 42 | region and its application to ladder climbing tasks 43 | `_, Nozawa, Shunichi, et 44 | al., IEEE-RAS International Conference on Humanoid Robots, November 2016. 45 | 46 | .. [Caron15] `Stability of surface contacts for humanoid robots: Closed-form 47 | formulae of the contact wrench cone for rectangular support areas 48 | `_, S. Caron, 49 | Q.-C. Pham and Y. Nakamura, IEEE International Conference on Robotics and 50 | Automation, May 2015. 51 | 52 | .. [Englsberger15] `Three-Dimensional Bipedal Walking Control Based on 53 | Divergent Component of Motion `_, 54 | J. Englsberger, C. Ott and A. Albu-Schäffer, IEEE Transactions on Robotics, 55 | vol. 31, no. 2, March 2015. 56 | 57 | .. [Flacco15] `Control of Redundant Robots Under Hard Joint Constraints: 58 | Saturation in the Null Space `_, 59 | F. Flacco, A. De Luca and O. Khatib, IEEE Transactions on Robotics, vol. 31, 60 | no. 3, June 2015. 61 | 62 | .. [Audren14] `Model preview control in multi-contact motion-application to a 63 | humanoid robot `_, 64 | H. Audren et al., IEEE/RSJ International Conference on Intelligent Robots 65 | and Systems, September 2014. 66 | 67 | .. [ElKhoury13] `Planning Optimal Motions for Anthropomorphic Systems 68 | `_, A. El Khoury, June 69 | 2013. 70 | 71 | .. [Kanoun12] `Real-time prioritized kinematic control under inequality 72 | constraints for redundant manipulators 73 | `_, O. Kanoun, Robotics: 74 | Science and Systems, June 2011. 75 | 76 | .. [Sugihara11] `Solvability-Unconcerned Inverse Kinematics by the 77 | Levenberg-Marquardt Method `_, T. 78 | Sugihara, IEEE Transactions on Robotics, vol. 27, no. 5, October 2011. 79 | 80 | .. [Bretl08] `Testing Static Equilibrium for Legged Robots 81 | `_, T. Bretl and S. Lall, IEEE 82 | Transactions on Robotics, vol. 24, no. 4, August 2008. 83 | 84 | .. [Wieber06] `Trajectory free linear model predictive control for stable 85 | walking in the presence of strong perturbations 86 | `_, P.-B. Wieber, IEEE-RAS 87 | International Conference on Humanoid Robots, 2006. 88 | 89 | .. [Diebel06] `Representing Attitude: Euler Angles, Unit Quaternions, and 90 | Rotation Vectors 91 | `_, J. 92 | Diebel, 2006. 93 | 94 | .. [Sardain04] `Forces acting on a biped robot. Center of pressure-Zero moment 95 | point `_, P. Sardain and G. 96 | Bessonnet, IEEE Transactions on Systems, Man and Cybernetics, vol. 34, no. 97 | 5, September 2004. 98 | 99 | .. [Kajita01] `The 3D Linear Inverted Pendulum Mode: A simple modeling for a 100 | biped walking pattern generation 101 | `_, S. 102 | Kajita, F. Kanehiro, K. Kaneko, K. Yokoi and H. Hirukawa, IEEE/RSJ 103 | International Conference on Intelligent Robots and Systems, 2001. 104 | 105 | .. [Walker82] `Efficient dynamic computer simulation of robotic mechanisms 106 | `_, M. Walker and D. Orin, ASME Trans. J. 107 | Dynamics Systems, Measurement and Control, vol. 104, 1982. 108 | -------------------------------------------------------------------------------- /doc/src/simulation-environment.rst: -------------------------------------------------------------------------------- 1 | ********************** 2 | Simulation environment 3 | ********************** 4 | 5 | Processes 6 | ========= 7 | 8 | Simulations schedule calls to a number of "processes" into an infinite loop, 9 | which represents the control loop of the robot. A process is a simple wrapper 10 | around an :func:`on_tick` function, which is called at each iteration of the 11 | simulation loop. 12 | 13 | .. autoclass:: pymanoid.proc.Process 14 | :members: 15 | 16 | Simulation 17 | ========== 18 | 19 | The simulation object is both a global environment and a serial process 20 | scheduler. As an environment, it is passed as argument when calling the 21 | :func:`on_tick` functions of child processes, and also contains a number of 22 | fields, such as ``dt`` (simulation time step) or ``gravity`` (gravity vector in 23 | the world frame). 24 | 25 | .. autoclass:: pymanoid.sim.Simulation 26 | :members: 27 | 28 | Camera recording 29 | ================ 30 | 31 | To record a video of your simulation, schedule a camera recorder as follows: 32 | 33 | .. code:: 34 | 35 | sim = pymanoid.Simulation(dt=0.03) 36 | camera_recorder = pymanoid.CameraRecorder(sim, fname="my_video.mp4") 37 | sim.schedule_extra(camera_recorder) 38 | 39 | Upon scheduling the camera recorder, the following message will appear in your 40 | Python shell: 41 | 42 | .. code:: 43 | 44 | Please click on the OpenRAVE window. 45 | 46 | The mouse pointer changes to a cross. While it is like this, click on the 47 | OpenRAVE window (so that the recorder knows which window to record from). Then, 48 | start or step your simulation as usual. 49 | 50 | When your simulation is over, run the video conversion script created in the 51 | current directory: 52 | 53 | .. code:: 54 | 55 | ./make_pymanoid_video.sh 56 | 57 | After completion, the file ``my_video.mp4`` is created in the current 58 | directory. 59 | 60 | .. autoclass:: pymanoid.proc.CameraRecorder 61 | :members: 62 | 63 | Making a new process 64 | ==================== 65 | 66 | Imagine we want to record knee angles while the robot moves in the `horizontal 67 | walking example 68 | `_. 69 | We can create a new process class: 70 | 71 | .. code:: python 72 | 73 | class KneeAngleRecorder(pymanoid.Process): 74 | 75 | def __init__(self): 76 | """ 77 | Initialize process. Don't forget to call parent constructor. 78 | """ 79 | super(KneeAngleRecorder, self).__init__() 80 | self.left_knee_angles = [] 81 | self.right_knee_angles = [] 82 | self.times = [] 83 | 84 | def on_tick(self, sim): 85 | """ 86 | Update function run at every simulation tick. 87 | 88 | Parameters 89 | ---------- 90 | sim : Simulation 91 | Instance of the current simulation. 92 | """ 93 | self.left_knee_angles.append(robot.q[robot.left_knee]) 94 | self.right_knee_angles.append(robot.q[robot.right_knee]) 95 | self.times.append(sim.time) 96 | 97 | To execute it, we instantiate a process of this class and add it to the 98 | simulation right before the call to ``start_walking()``: 99 | 100 | .. code:: python 101 | 102 | recorder = KneeAngleRecorder() 103 | sim.schedule_extra(recorder) 104 | start_walking() # comment this out to start walking manually 105 | 106 | We can plot the data recorded in this process at any time by: 107 | 108 | .. code:: python 109 | 110 | import pylab 111 | pylab.ion() 112 | pylab.plot(recorder.times, recorder.left_knee_angles) 113 | pylab.plot(recorder.times, recorder.right_knee_angles) 114 | 115 | Note that we scheduled the recorder as an extra process, using 116 | ``sim.schedule_extra`` rather than ``sim.schedule``, so that it does not get 117 | counted in the computation time budget of the control loop. 118 | -------------------------------------------------------------------------------- /doc/src/walking-pattern-generation.rst: -------------------------------------------------------------------------------- 1 | ************************** 2 | Walking pattern generation 3 | ************************** 4 | 5 | Walking pattern generation converts a high-level objective such as "go through 6 | this sequence of footsteps" to a time-continuous joint trajectory. For 7 | position-controlled robots such as those simulated in pymanoid, joint 8 | trajectories are computed by inverse kinematics from intermediate task targets. 9 | The two main targets for walking are the swing foot and center of mass. 10 | 11 | Swing foot trajectory 12 | ===================== 13 | 14 | The foot in the air during a single-support phase is called the *swing foot*. 15 | Its trajectory can be implemented by `spherical linear interpolation 16 | `_ for the orientation and `polynomial 17 | interpolation `_ for 18 | the position of the foot in the air. 19 | 20 | .. autoclass:: pymanoid.swing_foot.SwingFoot 21 | :members: 22 | 23 | Linear model predictive control 24 | =============================== 25 | 26 | Linear model predictive control [Wieber06]_ generates a dynamically-consistent 27 | trajectory for the center of mass (COM) while walking. It applies to walking 28 | over a flat floor, where the assumptions of the linear inverted pendulum mode 29 | (LIPM) [Kajita01]_ can be applied. 30 | 31 | .. autoclass:: pymanoid.mpc.LinearPredictiveControl 32 | :members: 33 | 34 | Nonlinear predictive control 35 | ============================= 36 | 37 | The assumptions of the LIPM are usually too restrictive to walk over uneven 38 | terrains. In this case, one can turn to more general (nonlinear) model 39 | predictive control. A building block for nonlinear predictive control of the 40 | center of mass is implemented in the 41 | :class:`pymanoid.centroidal.COMStepTransit` class, where a COM trajectory is 42 | constructed from one footstep to the next by solving a `nonlinear program 43 | `_ (NLP). 44 | 45 | .. autoclass:: pymanoid.centroidal.COMStepTransit 46 | :members: 47 | -------------------------------------------------------------------------------- /examples/README.md: -------------------------------------------------------------------------------- 1 | # Examples 2 | 3 | ## Contact stability conditions 4 | 5 | 6 | 7 | Contact-stability areas and volumes are conditions used to prevent contacts 8 | from slipping or detaching during motion. The examples in the 9 | [contact\_stability](contact_stability/) folder illustrate these conditions: 10 | static-equilibrium COM polygon, multi-contact ZMP support areas, etc. 11 | 12 | In these examples, you can move contacts by selecting them in the OpenRAVE GUI. 13 | Contact wrenches are computed at each contact to support the robot in 14 | static-equilibrium. Try moving the blue box (in the plane above the robot) 15 | around, and see what happens when it exits the polygon. 16 | 17 | ## Horizontal walking 18 | 19 | 20 | 21 | In this example, we make the JVRC-1 model walk forward on a flat horizontal 22 | floor. The COM trajectory is generated on the fly by [linear model predictive 23 | control](https://scaron.info/doc/pymanoid/walking-pattern-generation.html#pymanoid.mpc.LinearPredictiveControl). 24 | 25 | This examples illustrates the tutorial [Prototyping a walking pattern 26 | generator](https://scaron.info/robot-locomotion/prototyping-a-walking-pattern-generator.html). 27 | The concepts it introduces are the same as those implemented in a [full-fledged 28 | walking controller](https://github.com/stephane-caron/lipm_walking_controller/) 29 | applied for walking and stair climbing with the HRP-4 humanoid. 30 | 31 | ## Inverse kinematics 32 | 33 | 34 | 35 | The [inverse\_kinematics.py](inverse_kinematics.py) script shows how to use 36 | [inverse kinematics (IK)](https://scaron.info/robot-locomotion/inverse-kinematics.html) 37 | to achieve a set of whole-body tasks. It contains two equivalent 38 | implementations of the IK solver setup. The former is best for beginners as it 39 | uses the simpler ``Stance`` interface. The latter is for more advanced users 40 | and shows how to add individual tasks one by one. 41 | 42 | The example loads the 43 | [JVRC-1](https://github.com/stephane-caron/openrave_models/tree/master/JVRC-1) 44 | humanoid model, then generates a posture where the robot has both feet on 45 | pre-defined contact locations. The robot tracks a reference COM position given 46 | by the red box, which you can move around directly by using the interaction 47 | mode of the OpenRAVE GUI. 48 | 49 | ## Linear inverted pendulum stabilization 50 | 51 | 52 | 53 | This example implements a basic stabilizer for the inverted pendulum model 54 | based on proportional feedback of the divergent component of motion. 55 | 56 | Balancing is the action of constantly compensating undesired motions of the 57 | center of mass by regulating interaction forces with the environment. The core 58 | question is: what forces should be applied in response to an undesired center 59 | of mass motion? This example illustrates the solution based on [feedback of the 60 | 3D DCM](https://doi.org/10.1109/TRO.2015.2405592) which is both efficient and 61 | simple to implement. 62 | 63 | ## Multi-contact walking 64 | 65 | 66 | 67 | In this example, we make the JVRC-1 model walk accross a circular staircase 68 | where all contacts are tilted. 69 | 70 | The environment is generated so as to make the locomotion task difficult. We 71 | solve it using a [multi-contact linear 72 | MPC](https://hal.archives-ouvertes.fr/hal-01349880/document) where the 73 | formulation is kept linear thanks to the pendular COM acceleration cone. 74 | 75 | ## VHIP stabilization 76 | 77 | 78 | 79 | This script comes with the research paper [Biped Stabilization by Linear 80 | Feedback of the Variable-Height Inverted Pendulum 81 | Model](https://hal.archives-ouvertes.fr/hal-02289919v1/document). It compares 82 | two stabilizers for the inverted pendulum model. The first one (baseline) is 83 | based on proportional feedback of the [3D 84 | DCM](https://doi.org/10.1109/TRO.2015.2405592). The second one (proposed) 85 | performs proportional feedback of a 4D DCM of the same model; see the paper for 86 | details. 87 | -------------------------------------------------------------------------------- /examples/contact_stability/README.md: -------------------------------------------------------------------------------- 1 | # Contact-stability conditions 2 | 3 | Contact-stability areas and volumes are conditions used to prevent contacts 4 | from slipping or detaching during motion. 5 | 6 | ## Wrench friction cone 7 | 8 | The ``wrench_friction_cone.py`` script computes the wrench friction cone of 9 | the robot's contacts, as described in [this 10 | paper](http://www.roboticsproceedings.org/rss11/p28.pdf). Wrench friction cones 11 | are 6D polyhedral convex cones that characterize feasible contact wrenches, 12 | that is to say, wrenches that can be realized during contact-stable motions. 13 | They are a 6D generalization of Coulomb friction cones, and can also be used to 14 | encode other power limitations such as maximum joint torques. 15 | 16 | ## CoM static-equilibrium polygon 17 | 18 | The ``com_static_polygon.py`` script illustrates the polygon of CoM positions 19 | that the robot can hold in static equilibirum, as derived in [this 20 | paper](https://doi.org/10.1109/TRO.2008.2001360). You can move contacts by 21 | selecting them in the OpenRAVE GUI. Contact wrenches are computed at each 22 | contact to support the robot in static-equilibrium. Try moving the blue box (in 23 | the plane above the robot) around, and see what happens when it exits the 24 | polygon. 25 | 26 | ## Multi-contact ZMP support areas 27 | 28 | Th ``zmp_support_area.py`` script displays the ZMP support area under a given 29 | set of contacts. The derivation of this area is detailed in [this 30 | paper](https://hal.archives-ouvertes.fr/hal-02108589/document). It depends on both contact 31 | locations and the position of the center of mass, so when you move it or its 32 | projection (blue box) you will see the blue area change as well. 33 | 34 | ## CoM acceleration cone 35 | 36 | The ``com_accel_cone.py`` script displays the cone of CoM accelerations that 37 | the robot can execute while keeping contacts, as derived in [this 38 | paper](https://hal.archives-ouvertes.fr/hal-01349880/document). Like ZMP support 39 | areas, this cone depends on both contact locations and the position of the 40 | center of mass, so that when you move it or its projection (blue box) you will 41 | see its shape change as well. 42 | 43 | ## CoM robust static-equilibrium polytope 44 | 45 | The ``com_robust_static_polytope.py`` example generalizes the previous one when 46 | there are additional constraints on the robot, such as external forces applied 47 | on the robot as described in [this 48 | paper](https://hal-lirmm.ccsd.cnrs.fr/lirmm-01477362/document). In this case, 49 | the upright prism of the static-equilibrium polygon generalizes into a 50 | *polytope* of sustainable CoM positions (an intersection of slanted 51 | static-equilibrium prisms). This example illustrates how to compute this 52 | polytope using the [StabiliPy](https://github.com/haudren/stabilipy) library. 53 | Try moving contacts around to see what happens. 54 | -------------------------------------------------------------------------------- /examples/contact_stability/com_accel_cone.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | This example shows the set of 3D CoM accelerations that a given set of contacts 22 | can realize in the pendulum mode of motion (i.e. no angular momentum). See 23 | [Caron16]_ for details. 24 | """ 25 | 26 | import IPython 27 | 28 | from numpy import zeros 29 | 30 | import pymanoid 31 | 32 | from pymanoid import Stance 33 | from pymanoid.gui import PointMassWrenchDrawer 34 | from pymanoid.gui import draw_polytope 35 | from pymanoid.misc import matplotlib_to_rgb, norm 36 | 37 | com_height = 0.9 # [m] 38 | z_polygon = 2. 39 | 40 | 41 | class AccelConeDrawer(pymanoid.Process): 42 | 43 | """ 44 | Draw the COM acceleration cone of a contact set. 45 | 46 | Parameters 47 | ---------- 48 | stance : Stance 49 | Contacts and COM position of the robot. 50 | scale : scalar, optional 51 | Acceleration to distance conversion ratio, in [s]^2. 52 | color : tuple or string, optional 53 | Area color. 54 | """ 55 | 56 | def __init__(self, stance, scale=0.1, color=None): 57 | super(AccelConeDrawer, self).__init__() 58 | self.scale = scale # done before calling parent constructor 59 | if color is None: 60 | color = (0., 0.5, 0., 0.5) 61 | if type(color) is str: 62 | color = matplotlib_to_rgb(color) + [0.5] 63 | self.color = color 64 | self.contact_poses = {} 65 | self.handle = None 66 | self.last_com = stance.com.p 67 | self.stance = stance 68 | # 69 | self.update_contact_poses() 70 | self.update_cone() 71 | 72 | def clear(self): 73 | self.handle = None 74 | 75 | def on_tick(self, sim): 76 | if self.handle is None: 77 | self.update_cone() 78 | for contact in self.stance.contacts: 79 | if norm(contact.pose - self.contact_poses[contact.name]) > 1e-10: 80 | self.update_contact_poses() 81 | self.update_cone() 82 | break 83 | if norm(self.stance.com.p - self.last_com) > 1e-10: 84 | self.update_contact_poses() 85 | self.update_cone() 86 | self.last_com = self.stance.com.p 87 | 88 | def update_contact_poses(self): 89 | for contact in self.stance.contacts: 90 | self.contact_poses[contact.name] = contact.pose 91 | 92 | def update_cone(self): 93 | self.handle = None 94 | try: 95 | vertices = self.stance.compute_pendular_accel_cone() 96 | vscale = [self.stance.com.p + self.scale * acc for acc in vertices] 97 | self.handle = draw_polytope(vscale, 'r.-#') 98 | except Exception as e: 99 | print("AccelConeDrawer: {}".format(e)) 100 | 101 | 102 | class StaticWrenchDrawer(PointMassWrenchDrawer): 103 | 104 | """ 105 | Draw contact wrenches applied to a robot in static-equilibrium. 106 | 107 | Parameters 108 | ---------- 109 | stance : Stance 110 | Contacts and COM position of the robot. 111 | """ 112 | 113 | def __init__(self, stance): 114 | super(StaticWrenchDrawer, self).__init__(stance.com, stance) 115 | stance.com.set_accel(zeros((3,))) 116 | self.stance = stance 117 | 118 | def find_supporting_wrenches(self, sim): 119 | return self.stance.find_static_supporting_wrenches() 120 | 121 | 122 | class COMSync(pymanoid.Process): 123 | 124 | def __init__(self, stance, com_above): 125 | super(COMSync, self).__init__() 126 | self.com_above = com_above 127 | self.stance = stance 128 | 129 | def on_tick(self, sim): 130 | self.stance.com.set_x(self.com_above.x) 131 | self.stance.com.set_y(self.com_above.y) 132 | 133 | 134 | if __name__ == "__main__": 135 | sim = pymanoid.Simulation(dt=0.03) 136 | robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True) 137 | sim.set_viewer() 138 | sim.viewer.SetCamera([ 139 | [0.60587192, -0.36596244, 0.70639274, -2.4904027], 140 | [-0.79126787, -0.36933163, 0.48732874, -1.6965636], 141 | [0.08254916, -0.85420468, -0.51334199, 2.79584694], 142 | [0., 0., 0., 1.]]) 143 | robot.set_transparency(0.25) 144 | 145 | com_above = pymanoid.Cube(0.02, [0.05, 0.04, z_polygon], color='b') 146 | 147 | stance = Stance.from_json('stances/double.json') 148 | stance.bind(robot) 149 | robot.ik.solve() 150 | 151 | com_sync = COMSync(stance, com_above) 152 | cone_drawer = AccelConeDrawer(stance, scale=0.05) 153 | wrench_drawer = StaticWrenchDrawer(stance) 154 | 155 | sim.schedule(robot.ik) 156 | sim.schedule_extra(com_sync) 157 | sim.schedule_extra(cone_drawer) 158 | sim.schedule_extra(wrench_drawer) 159 | sim.start() 160 | 161 | print(""" 162 | COM acceleration cone 163 | ===================== 164 | 165 | Ready to go! The GUI displays the COM pendular acceleration cone in red. You 166 | can move the blue box (in the plane above the robot) around to make the robot 167 | move its center of mass. Contact wrenches are displayed at each contact (green 168 | dot is COP location, arrow is resultant force). When the COM exits the 169 | static-equilibrium polygon, you should see the background turn red as no 170 | feasible contact wrenches can be found. 171 | 172 | Enjoy :) 173 | """) 174 | 175 | if IPython.get_ipython() is None: 176 | IPython.embed() 177 | -------------------------------------------------------------------------------- /examples/contact_stability/com_robust_static_polytope.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2017 Hervé Audren 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | This example computes the robust static-equilibrium CoM polyhedron. See 22 | [Audren18]_ for details. Running this example requires the `StabiliPy 23 | `_ library. 24 | """ 25 | 26 | import IPython 27 | import pymanoid 28 | import sys 29 | 30 | try: 31 | import stabilipy 32 | except Exception: 33 | pymanoid.error("Running this example requires the StabiliPy library") 34 | print("You can get it from: https://github.com/haudren/stabilipy\n") 35 | sys.exit(-1) 36 | 37 | from numpy import array, zeros 38 | from pymanoid import Stance 39 | from pymanoid.gui import PointMassWrenchDrawer 40 | from pymanoid.gui import draw_polytope 41 | from pymanoid.misc import matplotlib_to_rgb, norm 42 | from openravepy import matrixFromPose 43 | 44 | 45 | class SupportPolyhedronDrawer(pymanoid.Process): 46 | 47 | """ 48 | Draw the robust static-equilibrium polyhedron of a contact set. 49 | 50 | Parameters 51 | ---------- 52 | stance : Stance 53 | Contacts and COM position of the robot. 54 | color : tuple or string, optional 55 | Area color. 56 | method: string, optional 57 | Method to compute the static equilibrium polygon. 58 | Choices are cdd, qhull (default) and parma 59 | """ 60 | 61 | def __init__(self, stance, z=0., color=None, method='qhull'): 62 | if color is None: 63 | color = (0., 0.5, 0., 0.5) 64 | if type(color) is str: 65 | color = matplotlib_to_rgb(color) + [0.5] 66 | super(SupportPolyhedronDrawer, self).__init__() 67 | self.color = color 68 | self.contact_poses = {} 69 | self.handle = None 70 | self.max_iter = 50 71 | self.method = method 72 | self.nr_iter = 0 73 | self.polyhedron = None 74 | self.stance = stance 75 | self.z = z 76 | # 77 | self.update_contacts() 78 | self.create_polyhedron(self.stance.contacts) 79 | 80 | def clear(self): 81 | self.handle = None 82 | 83 | def on_tick(self, sim): 84 | if self.handle is None: 85 | self.create_polyhedron(self.stance.contacts) 86 | return 87 | for contact in self.stance.contacts: 88 | if norm(contact.pose - self.contact_poses[contact.name]) > 1e-10: 89 | self.update_contacts() 90 | self.create_polyhedron(self.stance.contacts) 91 | return 92 | if self.nr_iter < self.max_iter: 93 | self.refine_polyhedron() 94 | self.nr_iter += 1 95 | 96 | def update_contacts(self): 97 | for contact in self.stance.contacts: 98 | self.contact_poses[contact.name] = contact.pose 99 | 100 | def create_polyhedron(self, contacts): 101 | self.handle = None 102 | self.nr_iter = 0 103 | try: 104 | self.polyhedron = stabilipy.StabilityPolygon( 105 | robot.mass, dimension=3, radius=1.5) 106 | stabilipy_contacts = [] 107 | for contact in contacts: 108 | hmatrix = matrixFromPose(contact.pose) 109 | X, Y = contact.shape 110 | displacements = [array([[X, Y, 0]]).T, 111 | array([[-X, Y, 0]]).T, 112 | array([[-X, -Y, 0]]).T, 113 | array([[X, -Y, 0]]).T] 114 | for disp in displacements: 115 | stabilipy_contacts.append( 116 | stabilipy.Contact( 117 | contact.friction, 118 | hmatrix[:3, 3:] + hmatrix[:3, :3].dot(disp), 119 | hmatrix[:3, 2:3])) 120 | self.polyhedron.contacts = stabilipy_contacts 121 | self.polyhedron.select_solver(self.method) 122 | self.polyhedron.make_problem() 123 | self.polyhedron.init_algo() 124 | self.polyhedron.build_polys() 125 | vertices = self.polyhedron.polyhedron() 126 | self.handle = draw_polytope( 127 | [(x[0], x[1], x[2]) for x in vertices]) 128 | except Exception as e: 129 | print("SupportPolyhedronDrawer: {}".format(e)) 130 | 131 | def refine_polyhedron(self): 132 | try: 133 | self.polyhedron.next_edge() 134 | vertices = self.polyhedron.polyhedron() 135 | self.handle = draw_polytope( 136 | [(x[0], x[1], x[2]) for x in vertices]) 137 | except Exception as e: 138 | print("SupportPolyhedronDrawer: {}".format(e)) 139 | 140 | def update_z(self, z): 141 | self.z = z 142 | self.update_polygon() 143 | 144 | 145 | class StaticWrenchDrawer(PointMassWrenchDrawer): 146 | 147 | """ 148 | Draw contact wrenches applied to a robot in static-equilibrium. 149 | 150 | Parameters 151 | ---------- 152 | stance : Stance 153 | Contacts and COM position of the robot. 154 | """ 155 | 156 | def __init__(self, stance): 157 | super(StaticWrenchDrawer, self).__init__(stance.com, stance) 158 | stance.com.set_accel(zeros((3,))) 159 | self.stance = stance 160 | 161 | def find_supporting_wrenches(self, sim): 162 | return self.stance.find_static_supporting_wrenches() 163 | 164 | 165 | if __name__ == "__main__": 166 | sim = pymanoid.Simulation(dt=0.03) 167 | robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True) 168 | sim.set_viewer() 169 | sim.viewer.SetCamera([ 170 | [0.60587192, -0.36596244, 0.70639274, -2.4904027], 171 | [-0.79126787, -0.36933163, 0.48732874, -1.6965636], 172 | [0.08254916, -0.85420468, -0.51334199, 2.79584694], 173 | [0., 0., 0., 1.]]) 174 | robot.set_transparency(0.25) 175 | 176 | stance = Stance.from_json('stances/double.json') 177 | stance.bind(robot) 178 | robot.ik.solve() 179 | 180 | polygon_drawer = SupportPolyhedronDrawer(stance) 181 | wrench_drawer = StaticWrenchDrawer(stance) 182 | 183 | sim.schedule(robot.ik) 184 | sim.schedule_extra(polygon_drawer) 185 | sim.schedule_extra(wrench_drawer) 186 | sim.start() 187 | 188 | print(""" 189 | COM robust static-equilibrium polygon 190 | ===================================== 191 | 192 | Ready to go! The GUI displays the COM static-equilibrium polytope in green. You 193 | can move contacts around to see how they affect the shape of the polytope. 194 | Sample contact wrenches are displayed at each contact (green dot is COP 195 | location, arrow is resultant force). 196 | 197 | Enjoy :) 198 | """) 199 | 200 | if IPython.get_ipython() is None: 201 | IPython.embed() 202 | -------------------------------------------------------------------------------- /examples/contact_stability/com_static_polygon.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | This example computes the static-equilibrium CoM polygon, i.e. the set of CoM 22 | positions that can be sustained usingf a given set of contacts. See [Caron16]_ 23 | for details. 24 | """ 25 | 26 | import IPython 27 | import sys 28 | 29 | import pymanoid 30 | 31 | from pymanoid.gui import StaticEquilibriumWrenchDrawer 32 | from pymanoid.gui import draw_polygon 33 | from pymanoid.misc import norm 34 | 35 | 36 | class SupportPolygonDrawer(pymanoid.Process): 37 | 38 | """ 39 | Draw the static-equilibrium polygon of a contact set. 40 | 41 | Parameters 42 | ---------- 43 | stance : pymanoid.Stance 44 | Contacts and COM position of the robot. 45 | method : string 46 | Method to compute the static equilibrium polygon. Choices are: 'bretl', 47 | 'cdd' and 'hull'. 48 | z_polygon : scalar 49 | Height where to draw the CoM static-equilibrium polygon. 50 | color : tuple or string, optional 51 | Area color. 52 | """ 53 | 54 | def __init__(self, stance, method, z_polygon, color='g'): 55 | if color is None: 56 | color = (0., 0.5, 0., 0.5) 57 | if type(color) is str: 58 | from pymanoid.misc import matplotlib_to_rgb 59 | color = matplotlib_to_rgb(color) + [0.5] 60 | super(SupportPolygonDrawer, self).__init__() 61 | self.color = color 62 | self.contact_poses = {} 63 | self.handle = None 64 | self.method = method 65 | self.stance = stance 66 | self.z = z_polygon 67 | # 68 | self.update_contact_poses() 69 | self.update_polygon() 70 | 71 | def clear(self): 72 | self.handle = None 73 | 74 | def on_tick(self, sim): 75 | if self.handle is None: 76 | self.update_polygon() 77 | for contact in self.stance.contacts: 78 | if norm(contact.pose - self.contact_poses[contact.name]) > 1e-10: 79 | self.update_contact_poses() 80 | self.update_polygon() 81 | break 82 | 83 | def update_contact_poses(self): 84 | for contact in self.stance.contacts: 85 | self.contact_poses[contact.name] = contact.pose 86 | 87 | def update_polygon(self): 88 | self.handle = None 89 | try: 90 | vertices = self.stance.compute_static_equilibrium_polygon( 91 | method=self.method) 92 | self.handle = draw_polygon( 93 | [(x[0], x[1], self.z) for x in vertices], 94 | normal=[0, 0, 1], color=self.color) 95 | except Exception as e: 96 | print("SupportPolygonDrawer: {}".format(e)) 97 | 98 | def update_z(self, z): 99 | self.z = z 100 | self.update_polygon() 101 | 102 | 103 | class COMSync(pymanoid.Process): 104 | 105 | """ 106 | Update stance CoM from the GUI handle in polygon above the robot. 107 | 108 | Parameters 109 | ---------- 110 | stance : pymanoid.Stance 111 | Contacts and COM position of the robot. 112 | com_above : pymanoid.Cube 113 | CoM handle in static-equilibrium polygon. 114 | """ 115 | 116 | def __init__(self, stance, com_above): 117 | super(COMSync, self).__init__() 118 | self.com_above = com_above 119 | self.stance = stance 120 | 121 | def on_tick(self, sim): 122 | self.stance.com.set_x(self.com_above.x) 123 | self.stance.com.set_y(self.com_above.y) 124 | 125 | 126 | if __name__ == "__main__": 127 | sim = pymanoid.Simulation(dt=0.03) 128 | robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True) 129 | sim.set_viewer() 130 | sim.set_camera_top(x=0., y=0., z=3.) 131 | robot.set_transparency(0.25) 132 | 133 | z_polygon = 2. # [m], height where to draw CoM polygon 134 | com_above = pymanoid.Cube(0.02, [0.05, 0.04, z_polygon], color='b') 135 | 136 | stance = pymanoid.Stance.from_json('stances/double.json') 137 | stance.com.hide() 138 | stance.bind(robot) 139 | robot.ik.solve() 140 | 141 | method = "hull" 142 | if "bretl" in sys.argv: 143 | method = "bretl" 144 | elif "cdd" in sys.argv: 145 | method = "cdd" 146 | 147 | com_sync = COMSync(stance, com_above) 148 | polygon_drawer = SupportPolygonDrawer(stance, method, z_polygon) 149 | wrench_drawer = StaticEquilibriumWrenchDrawer(stance) 150 | 151 | sim.schedule(robot.ik) 152 | sim.schedule_extra(com_sync) 153 | sim.schedule_extra(polygon_drawer) 154 | sim.schedule_extra(wrench_drawer) 155 | sim.start() 156 | 157 | print(""" 158 | COM static-equilibrium polygon 159 | ============================== 160 | 161 | Method: %s 162 | 163 | Ready to go! The GUI displays the COM static-equilibrium polygon in green. You 164 | can move the blue box (in the plane above the robot) around to make the robot 165 | move its center of mass. Contact wrenches are displayed at each contact (green 166 | dot is COP location, arrow is resultant force). When the COM exits the 167 | static-equilibrium polygon, you should see the background turn red as no 168 | feasible contact wrenches can be found. 169 | 170 | Enjoy :) 171 | """ % method) 172 | 173 | if IPython.get_ipython() is None: 174 | IPython.embed() 175 | -------------------------------------------------------------------------------- /examples/contact_stability/stances/double.json: -------------------------------------------------------------------------------- 1 | { 2 | "com": { 3 | "pos": [0.0, 0.0, 0.9], 4 | "mass": 62.0 5 | }, 6 | "left_foot": { 7 | "shape": [0.12, 0.065], 8 | "friction": 0.5, 9 | "pos": [0.20, 0.15, 0.1], 10 | "robot_link": 20, 11 | "rpy": [0.4, 0, 0] 12 | }, 13 | "right_foot": { 14 | "shape": [0.12, 0.065], 15 | "friction": 0.5, 16 | "pos": [-0.2, -0.195, 0.0], 17 | "robot_link": 13, 18 | "rpy": [-0.4, 0.0, 0.0] 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /examples/contact_stability/stances/single.json: -------------------------------------------------------------------------------- 1 | { 2 | "com": { 3 | "pos": [0.0, 0.0, 0.9], 4 | "mass": 62.0 5 | }, 6 | "left_foot": { 7 | "shape": [0.12, 0.065], 8 | "friction": 1.5, 9 | "pos": [0.20, 0.15, 0.1], 10 | "robot_link": 20, 11 | "rpy": [0.4, 0, 0] 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /examples/contact_stability/stances/triple.json: -------------------------------------------------------------------------------- 1 | { 2 | "com": { 3 | "pos": [0.3, 0.04, 0.75], 4 | "mass": 62.0 5 | }, 6 | "left_foot": { 7 | "shape": [0.12, 0.065], 8 | "friction": 0.5, 9 | "pos": [ 10 | 0.57785779237747192, 11 | 0.38709041476249695, 12 | 0.10231213271617889 13 | ], 14 | "robot_link": 20, 15 | "rpy": [ 16 | 0.47008350104274976, 17 | 5.3997125596541066e-08, 18 | 1.2930581213820404e-08 19 | ] 20 | }, 21 | "left_hand": { 22 | "shape": [0.12, 0.065], 23 | "friction": 0.5, 24 | "pos": [ 25 | 0.65435177087783813, 26 | 0.39171969890594482, 27 | 1.026335597038269 28 | ], 29 | "robot_link": 48, 30 | "rpy": [ 31 | 1.3381710676315686, 32 | -0.23305234353606685, 33 | -0.77264784963182753 34 | ] 35 | }, 36 | "right_foot": { 37 | "shape": [0.12, 0.065], 38 | "friction": 0.5, 39 | "pos": [ 40 | -4.1245326087846479e-07, 41 | -0.31870916485786438, 42 | 0.057613462209701538 43 | ], 44 | "robot_link": 13, 45 | "rpy": [ 46 | -0.71244569943322367, 47 | 0.0017363430578432853, 48 | 0.00012478226610206157 49 | ] 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /examples/contact_stability/wrench_friction_cone.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | This example shows how to compute the contact wrench cone (CWC), a generalized 22 | multi-contact friction cone. See [Caron15]_ for details. 23 | """ 24 | 25 | import IPython 26 | 27 | from numpy import array 28 | 29 | import pymanoid 30 | 31 | from pymanoid import Stance 32 | 33 | 34 | def print_contact(name, contact): 35 | print("%s:" % name) 36 | print("- pos = %s" % repr(contact.p)) 37 | print("- rpy = %s" % repr(contact.rpy)) 38 | print("- half-length = %s" % repr(contact.shape[0])) 39 | print("- half-width = %s" % repr(contact.shape[1])) 40 | print("- friction = %f" % contact.friction) 41 | print("") 42 | 43 | 44 | if __name__ == "__main__": 45 | sim = pymanoid.Simulation(dt=0.03) 46 | robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True) 47 | sim.set_viewer() 48 | sim.viewer.SetCamera([ 49 | [0.60587192, -0.36596244, 0.70639274, -2.4904027], 50 | [-0.79126787, -0.36933163, 0.48732874, -1.6965636], 51 | [0.08254916, -0.85420468, -0.51334199, 2.79584694], 52 | [0., 0., 0., 1.]]) 53 | robot.set_transparency(0.25) 54 | stance = Stance.from_json('stances/triple.json') 55 | stance.bind(robot) 56 | robot.ik.solve() 57 | sim.schedule(robot.ik) 58 | sim.start() 59 | 60 | p = array([0., 0., 0.]) 61 | CWC_O = stance.compute_wrench_inequalities(p) 62 | print_contact("Left foot", stance.left_foot) 63 | print_contact("Right foot", stance.right_foot) 64 | print("Contact Wrench Cone at %s:" % str(p)) 65 | print("- has %d lines" % CWC_O.shape[0]) 66 | 67 | if IPython.get_ipython() is None: 68 | IPython.embed() 69 | -------------------------------------------------------------------------------- /examples/contact_stability/zmp_support_area.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | This example computes the multi-contact ZMP support area for a given robot 22 | stance (contacts and CoM position). See [Caron16]_ for details. 23 | """ 24 | 25 | import IPython 26 | 27 | from numpy import zeros 28 | 29 | import pymanoid 30 | 31 | from pymanoid import Stance 32 | from pymanoid.gui import PointMassWrenchDrawer 33 | from pymanoid.gui import draw_polygon 34 | from pymanoid.misc import matplotlib_to_rgb, norm 35 | 36 | com_height = 0.9 # [m] 37 | z_polygon = 2. 38 | 39 | 40 | class SupportAreaDrawer(pymanoid.Process): 41 | 42 | """ 43 | Draw the pendular ZMP area of a contact set. 44 | 45 | Parameters 46 | ---------- 47 | stance : Stance 48 | Contacts and COM position of the robot. 49 | height : scalar, optional 50 | Height of the ZMP support area in the world frame. 51 | color : tuple or string, optional 52 | Area color. 53 | """ 54 | 55 | def __init__(self, stance, height=0., color=None): 56 | self.stance = stance # before calling parent constructor 57 | if color is None: 58 | color = (0., 0.5, 0., 0.5) 59 | if type(color) is str: 60 | color = matplotlib_to_rgb(color) + [0.5] 61 | super(SupportAreaDrawer, self).__init__() 62 | self.color = color 63 | self.contact_poses = {} 64 | self.handle = None 65 | self.height = height 66 | self.last_com = stance.com.p 67 | self.stance = stance 68 | # 69 | self.update_contact_poses() 70 | self.update_polygon() 71 | 72 | def clear(self): 73 | self.handle = None 74 | 75 | def update_contact_poses(self): 76 | for contact in self.stance.contacts: 77 | self.contact_poses[contact.name] = contact.pose 78 | 79 | def update_height(self, height): 80 | self.height = height 81 | self.update_polygon() 82 | 83 | def update_polygon(self): 84 | self.handle = None 85 | try: 86 | vertices = self.stance.compute_zmp_support_area(self.height) 87 | self.handle = draw_polygon( 88 | [(x[0], x[1], self.height) for x in vertices], 89 | normal=[0, 0, 1], color=(0.0, 0.0, 0.5, 0.5)) 90 | except Exception as e: 91 | print("SupportAreaDrawer: {}".format(e)) 92 | 93 | def on_tick(self, sim): 94 | if self.handle is None: 95 | self.update_polygon() 96 | for contact in self.stance.contacts: 97 | if norm(contact.pose - self.contact_poses[contact.name]) > 1e-10: 98 | self.update_contact_poses() 99 | self.update_polygon() 100 | break 101 | if norm(self.stance.com.p - self.last_com) > 1e-10: 102 | self.update_contact_poses() 103 | self.update_polygon() 104 | self.last_com = self.stance.com.p 105 | 106 | 107 | class StaticWrenchDrawer(PointMassWrenchDrawer): 108 | 109 | """ 110 | Draw contact wrenches applied to a robot in static-equilibrium. 111 | 112 | Parameters 113 | ---------- 114 | stance : Stance 115 | Contacts and COM position of the robot. 116 | """ 117 | 118 | def __init__(self, stance): 119 | super(StaticWrenchDrawer, self).__init__(stance.com, stance) 120 | stance.com.set_accel(zeros((3,))) 121 | self.stance = stance 122 | 123 | def find_supporting_wrenches(self, sim): 124 | return self.stance.find_static_supporting_wrenches() 125 | 126 | 127 | class COMSync(pymanoid.Process): 128 | 129 | def __init__(self, stance, com_above): 130 | super(COMSync, self).__init__() 131 | self.com_above = com_above 132 | self.stance = stance 133 | 134 | def on_tick(self, sim): 135 | self.stance.com.set_x(self.com_above.x) 136 | self.stance.com.set_y(self.com_above.y) 137 | 138 | 139 | if __name__ == "__main__": 140 | sim = pymanoid.Simulation(dt=0.03) 141 | robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True) 142 | sim.set_viewer() 143 | sim.viewer.SetCamera([ 144 | [0.60587192, -0.36596244, 0.70639274, -2.4904027], 145 | [-0.79126787, -0.36933163, 0.48732874, -1.6965636], 146 | [0.08254916, -0.85420468, -0.51334199, 2.79584694], 147 | [0., 0., 0., 1.]]) 148 | robot.set_transparency(0.25) 149 | 150 | com_above = pymanoid.Cube(0.02, [0.05, 0.04, z_polygon], color='b') 151 | 152 | stance = Stance.from_json('stances/double.json') 153 | stance.bind(robot) 154 | robot.ik.solve() 155 | 156 | com_sync = COMSync(stance, com_above) 157 | support_area_drawer = SupportAreaDrawer(stance, z_polygon) 158 | wrench_drawer = StaticWrenchDrawer(stance) 159 | 160 | sim.schedule(robot.ik) 161 | sim.schedule_extra(com_sync) 162 | sim.schedule_extra(support_area_drawer) 163 | sim.schedule_extra(wrench_drawer) 164 | sim.start() 165 | 166 | print(""" 167 | ZMP support area 168 | ================ 169 | 170 | Ready to go! The GUI displays the ZMP pendular support area in blue. You can 171 | move the blue box (in the plane above the robot) around to make the robot move 172 | its center of mass. Contact wrenches are displayed at each contact (green dot 173 | is COP location, arrow is resultant force). When the COM exists the 174 | static-equilibrium polygon, you should see the background turn red as no 175 | feasible contact wrenches can be found. 176 | 177 | Enjoy :) 178 | """) 179 | 180 | if IPython.get_ipython() is None: 181 | IPython.embed() 182 | -------------------------------------------------------------------------------- /examples/external_force.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | This example applies an external force at the right hand of the JVRC-1 humanoid 22 | while the robot is standing on its two feet. It illustrates how to use managed 23 | contacts (external force provided by an independent process, here SoftContact) 24 | in addition to the default supporting contacts. 25 | """ 26 | 27 | import IPython 28 | 29 | import pymanoid 30 | 31 | from pymanoid import Contact, PointMass, Stance 32 | from pymanoid.gui import draw_polygon 33 | from pymanoid.gui import RobotWrenchDrawer 34 | 35 | 36 | class SoftContact(pymanoid.Process): 37 | 38 | """ 39 | Model a simple elastic contact with a vertical surface. 40 | 41 | Parameters 42 | ---------- 43 | contact : Contact 44 | Contact frame of the robot's end-effector. 45 | stiffness : scalar, optional 46 | Stiffness of the elastic surface. 47 | 48 | Attributes 49 | ---------- 50 | contact : Contact 51 | Contact frame of the robot's end-effector. 52 | handle : openravepy.GraphHandle 53 | Polygon drawing to represent the elastic surface. 54 | init_y : scalar 55 | Position of the wall in the world frame. 56 | stiffness : scalar 57 | Stiffness of the elastic surface. 58 | """ 59 | 60 | def __init__(self, contact, stiffness=2000.): 61 | super(SoftContact, self).__init__() 62 | x, y, z = contact.x, contact.y, contact.z 63 | polygon = [ 64 | (x + 0.5, y, z - 0.5), 65 | (x + 0.5, y, z + 0.5), 66 | (x - 0.5, y, z + 0.5), 67 | (x - 0.5, y, z - 0.5)] 68 | self.contact = contact 69 | self.handle = draw_polygon(polygon, [0, 1, 0], color='k') 70 | self.init_y = y + 0.01 71 | self.stiffness = stiffness 72 | 73 | def on_tick(self, sim): 74 | """ 75 | Update function run at every simulation tick. 76 | 77 | Parameters 78 | ---------- 79 | sim : Simulation 80 | Instance of the current simulation. 81 | """ 82 | F_z = max(0., self.stiffness * (self.init_y - self.contact.y)) 83 | self.contact.set_wrench([0., 0., F_z, 0., 0., 0.]) 84 | 85 | 86 | if __name__ == "__main__": 87 | sim = pymanoid.Simulation(dt=0.03) 88 | robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True) 89 | sim.set_viewer() 90 | sim.viewer.SetCamera([ 91 | [0.2753152, 0.29704774, -0.91431077, 2.89042521], 92 | [0.96034717, -0.04146411, 0.27570643, -0.59789598], 93 | [0.04398688, -0.95396193, -0.29668466, 1.65848958], 94 | [0., 0., 0., 1.]]) 95 | robot.set_transparency(0.25) 96 | 97 | stance = Stance( 98 | com=PointMass( 99 | pos=[0.1, 0.2, 0.9], 100 | mass=robot.mass), 101 | left_foot=Contact( 102 | shape=robot.sole_shape, 103 | friction=0.7, 104 | pos=[0.3, 0.39, 0.10], 105 | link=robot.left_foot, 106 | rpy=[0.15, 0., -0.02]), 107 | right_foot=Contact( 108 | shape=robot.sole_shape, 109 | friction=0.7, 110 | pos=[0., -0.15, 0.02], 111 | link=robot.right_foot, 112 | rpy=[-0.3, 0., 0.]), 113 | right_hand=Contact( 114 | shape=(0.04, 0.04), 115 | friction=0.5, 116 | pos=[0.1, -0.5, 1.2], 117 | rpy=[-1.57, 0., 0.])) 118 | stance.bind(robot) 119 | robot.ik.solve() 120 | 121 | soft_contact = SoftContact(stance.right_hand) 122 | wrench_drawer = RobotWrenchDrawer(robot) 123 | 124 | sim.schedule(robot.ik) 125 | sim.schedule(soft_contact) 126 | sim.schedule(robot.wrench_distributor, log_comp_times=True) 127 | sim.schedule_extra(wrench_drawer) 128 | sim.start() 129 | 130 | if IPython.get_ipython() is None: 131 | IPython.embed() 132 | -------------------------------------------------------------------------------- /examples/horizontal_walking.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | This example implements a walking pattern generator for horizontal walking 22 | based on linear model predictive control 23 | . 24 | """ 25 | 26 | import IPython 27 | 28 | from numpy import array 29 | 30 | import pymanoid 31 | 32 | from pymanoid.body import PointMass 33 | from pymanoid.contact import Contact 34 | from pymanoid.gui import RobotWrenchDrawer 35 | from pymanoid.gui import TrajectoryDrawer 36 | from pymanoid.mpc import LinearPredictiveControl 37 | from pymanoid.robots import JVRC1 38 | from pymanoid.stance import Stance 39 | from pymanoid.swing_foot import SwingFoot 40 | 41 | 42 | def generate_footsteps(distance, step_length, foot_spread, friction): 43 | """ 44 | Generate a set of footsteps for walking forward. 45 | 46 | Parameters 47 | ---------- 48 | distance : scalar 49 | Total distance to walk forward in [m]. 50 | step_length : scalar 51 | Distance between right and left heel in double support. 52 | foot_spread : scalar 53 | Lateral distance between left and right foot centers. 54 | friction : scalar 55 | Friction coefficient between a robot foot and a step. 56 | """ 57 | contacts = [] 58 | 59 | def append_contact(x, y): 60 | contacts.append(Contact( 61 | shape=robot.sole_shape, pos=[x, y, 0.], friction=friction)) 62 | 63 | append_contact(0., +foot_spread) 64 | append_contact(0., -foot_spread) 65 | x = 0. 66 | y = foot_spread 67 | while x < distance: 68 | if distance - x <= step_length: 69 | x += min(distance - x, 0.5 * step_length) 70 | else: # still way to go 71 | x += step_length 72 | y *= -1. 73 | append_contact(x, y) 74 | append_contact(x, -y) # here x == distance 75 | return contacts 76 | 77 | 78 | class HorizontalWalkingFSM(pymanoid.Process): 79 | 80 | """ 81 | Finite State Machine for biped walking. 82 | 83 | Parameters 84 | ---------- 85 | ssp_duration : scalar 86 | Duration of single-support phases, in [s]. 87 | dsp_duration : scalar 88 | Duration of double-support phases, in [s]. 89 | """ 90 | 91 | def __init__(self, ssp_duration, dsp_duration): 92 | super(HorizontalWalkingFSM, self).__init__() 93 | self.dsp_duration = dsp_duration 94 | self.mpc_timestep = round(0.1 / dt) * dt # update MPC every ~0.1 [s] 95 | self.next_footstep = 2 96 | self.ssp_duration = ssp_duration 97 | self.state = None 98 | # 99 | self.start_standing() 100 | 101 | def on_tick(self, sim): 102 | """ 103 | Update function run at every simulation tick. 104 | 105 | Parameters 106 | ---------- 107 | sim : Simulation 108 | Instance of the current simulation. 109 | """ 110 | if self.state == "Standing": 111 | return self.run_standing() 112 | elif self.state == "DoubleSupport": 113 | return self.run_double_support() 114 | elif self.state == "SingleSupport": 115 | return self.run_single_support() 116 | raise Exception("Unknown state: " + self.state) 117 | 118 | def start_standing(self): 119 | """ 120 | Switch to standing state. 121 | """ 122 | self.start_walking = False 123 | self.state = "Standing" 124 | return self.run_standing() 125 | 126 | def run_standing(self): 127 | """ 128 | Run standing state. 129 | """ 130 | if self.start_walking: 131 | self.start_walking = False 132 | if self.next_footstep < len(footsteps): 133 | return self.start_double_support() 134 | 135 | def start_double_support(self): 136 | """ 137 | Switch to double-support state. 138 | """ 139 | if self.next_footstep % 2 == 1: # left foot swings 140 | self.stance_foot = stance.right_foot 141 | else: # right foot swings 142 | self.stance_foot = stance.left_foot 143 | dsp_duration = self.dsp_duration 144 | if self.next_footstep == 2 or self.next_footstep == len(footsteps) - 1: 145 | # double support is a bit longer for the first and last steps 146 | dsp_duration = 4 * self.dsp_duration 147 | self.swing_target = footsteps[self.next_footstep] 148 | self.rem_time = dsp_duration 149 | self.state = "DoubleSupport" 150 | self.start_com_mpc_dsp() 151 | return self.run_double_support() 152 | 153 | def start_com_mpc_dsp(self): 154 | self.update_mpc(self.rem_time, self.ssp_duration) 155 | 156 | def run_double_support(self): 157 | """ 158 | Run double-support state. 159 | """ 160 | if self.rem_time <= 0.: 161 | return self.start_single_support() 162 | self.run_com_mpc() 163 | self.rem_time -= dt 164 | 165 | def start_single_support(self): 166 | """ 167 | Switch to single-support state. 168 | """ 169 | if self.next_footstep % 2 == 1: # left foot swings 170 | self.swing_foot = stance.free_contact('left_foot') 171 | else: # right foot swings 172 | self.swing_foot = stance.free_contact('right_foot') 173 | self.next_footstep += 1 174 | self.rem_time = self.ssp_duration 175 | self.state = "SingleSupport" 176 | self.start_swing_foot() 177 | self.start_com_mpc_ssp() 178 | return self.run_single_support() 179 | 180 | def start_swing_foot(self): 181 | """ 182 | Initialize swing foot interpolator for single-support state. 183 | """ 184 | self.swing_start = self.swing_foot.pose 185 | self.swing_interp = SwingFoot( 186 | self.swing_foot, self.swing_target, self.ssp_duration, 187 | takeoff_clearance=0.05, landing_clearance=0.05) 188 | 189 | def start_com_mpc_ssp(self): 190 | self.update_mpc(0., self.rem_time) 191 | 192 | def run_single_support(self): 193 | """ 194 | Run single-support state. 195 | """ 196 | if self.rem_time <= 0.: 197 | stance.set_contact(self.swing_foot) 198 | if self.next_footstep < len(footsteps): 199 | return self.start_double_support() 200 | else: # footstep sequence is over 201 | return self.start_standing() 202 | self.run_swing_foot() 203 | self.run_com_mpc() 204 | self.rem_time -= dt 205 | 206 | def run_swing_foot(self): 207 | """ 208 | Run swing foot interpolator for single-support state. 209 | """ 210 | self.swing_foot.set_pose(self.swing_interp.integrate(dt)) 211 | 212 | def update_mpc(self, dsp_duration, ssp_duration): 213 | nb_preview_steps = 16 214 | T = self.mpc_timestep 215 | nb_init_dsp_steps = int(round(dsp_duration / T)) 216 | nb_init_ssp_steps = int(round(ssp_duration / T)) 217 | nb_dsp_steps = int(round(self.dsp_duration / T)) 218 | A = array([[1., T, T ** 2 / 2.], [0., 1., T], [0., 0., 1.]]) 219 | B = array([T ** 3 / 6., T ** 2 / 2., T]).reshape((3, 1)) 220 | h = stance.com.z 221 | g = -sim.gravity[2] 222 | zmp_from_state = array([1., 0., -h / g]) 223 | C = array([+zmp_from_state, -zmp_from_state]) 224 | D = None 225 | e = [[], []] 226 | cur_vertices = self.stance_foot.get_scaled_contact_area(0.9) 227 | next_vertices = self.swing_target.get_scaled_contact_area(0.9) 228 | for coord in [0, 1]: 229 | cur_max = max(v[coord] for v in cur_vertices) 230 | cur_min = min(v[coord] for v in cur_vertices) 231 | next_max = max(v[coord] for v in next_vertices) 232 | next_min = min(v[coord] for v in next_vertices) 233 | e[coord] = [ 234 | array([+1000., +1000.]) if i < nb_init_dsp_steps 235 | else array([+cur_max, -cur_min]) 236 | if i - nb_init_dsp_steps <= nb_init_ssp_steps 237 | else array([+1000., +1000.]) 238 | if i - nb_init_dsp_steps - nb_init_ssp_steps < nb_dsp_steps 239 | else array([+next_max, -next_min]) 240 | for i in range(nb_preview_steps)] 241 | self.x_mpc = LinearPredictiveControl( 242 | A, B, C, D, e[0], 243 | x_init=array([stance.com.x, stance.com.xd, stance.com.xdd]), 244 | x_goal=array([self.swing_target.x, 0., 0.]), 245 | nb_steps=nb_preview_steps, 246 | wxt=1., wu=0.01) 247 | self.y_mpc = LinearPredictiveControl( 248 | A, B, C, D, e[1], 249 | x_init=array([stance.com.y, stance.com.yd, stance.com.ydd]), 250 | x_goal=array([self.swing_target.y, 0., 0.]), 251 | nb_steps=nb_preview_steps, 252 | wxt=1., wu=0.01) 253 | self.x_mpc.solve() 254 | self.y_mpc.solve() 255 | self.preview_time = 0. 256 | 257 | def plot_mpc_preview(self): 258 | import pylab 259 | T = self.mpc_timestep 260 | h = stance.com.z 261 | g = -sim.gravity[2] 262 | trange = [sim.time + k * T for k in range(len(self.x_mpc.X))] 263 | pylab.ion() 264 | pylab.clf() 265 | pylab.subplot(211) 266 | pylab.plot(trange, [v[0] for v in self.x_mpc.X]) 267 | pylab.plot(trange, [v[0] - v[2] * h / g for v in self.x_mpc.X]) 268 | pylab.subplot(212) 269 | pylab.plot(trange, [v[0] for v in self.y_mpc.X]) 270 | pylab.plot(trange, [v[0] - v[2] * h / g for v in self.y_mpc.X]) 271 | 272 | def run_com_mpc(self): 273 | """ 274 | Run CoM predictive control for single-support state. 275 | """ 276 | if self.preview_time >= self.mpc_timestep: 277 | if self.state == "DoubleSupport": 278 | self.update_mpc(self.rem_time, self.ssp_duration) 279 | else: # self.state == "SingleSupport": 280 | self.update_mpc(0., self.rem_time) 281 | com_jerk = array([self.x_mpc.U[0][0], self.y_mpc.U[0][0], 0.]) 282 | stance.com.integrate_constant_jerk(com_jerk, dt) 283 | self.preview_time += dt 284 | 285 | 286 | if __name__ == "__main__": 287 | dt = 0.03 # [s] 288 | sim = pymanoid.Simulation(dt=dt) 289 | robot = JVRC1(download_if_needed=True) 290 | sim.set_viewer() 291 | sim.set_camera_transform([ 292 | [-0.86825231, 0.13816899, -0.47649476, 2.95342016], 293 | [0.49606811, 0.22750768, -0.8379479, 3.26736617], 294 | [-0.0073722, -0.96392406, -0.2660753, 1.83063173], 295 | [0., 0., 0., 1.]]) 296 | robot.set_transparency(0.3) 297 | 298 | footsteps = generate_footsteps( 299 | distance=2.1, 300 | step_length=0.3, 301 | foot_spread=0.1, 302 | friction=0.7) 303 | stance = Stance( 304 | com=PointMass([0, 0, robot.leg_length], robot.mass), 305 | left_foot=footsteps[0].copy(hide=True), 306 | right_foot=footsteps[1].copy(hide=True)) 307 | stance.bind(robot) 308 | robot.ik.solve(max_it=42) 309 | 310 | ssp_duration = round(0.7 / dt) * dt # close to 0.7 [s] 311 | dsp_duration = round(0.1 / dt) * dt # close to 0.1 [s] 312 | fsm = HorizontalWalkingFSM(ssp_duration, dsp_duration) 313 | 314 | sim.schedule(fsm) 315 | sim.schedule(robot.ik, log_comp_times=True) 316 | sim.schedule(robot.wrench_distributor, log_comp_times=True) 317 | 318 | com_traj_drawer = TrajectoryDrawer(robot.stance.com, 'b-') 319 | lf_traj_drawer = TrajectoryDrawer(robot.left_foot, 'g-') 320 | rf_traj_drawer = TrajectoryDrawer(robot.right_foot, 'r-') 321 | wrench_drawer = RobotWrenchDrawer(robot) 322 | 323 | sim.schedule_extra(com_traj_drawer) 324 | sim.schedule_extra(lf_traj_drawer) 325 | sim.schedule_extra(rf_traj_drawer) 326 | sim.schedule_extra(wrench_drawer) 327 | 328 | sim.start() 329 | 330 | def start_walking(): 331 | fsm.start_walking = True 332 | 333 | start_walking() # comment this out to start walking manually 334 | if IPython.get_ipython() is None: 335 | IPython.embed() 336 | -------------------------------------------------------------------------------- /examples/inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | This example uses inverse kinematics (IK) to achieve a set of whole-body tasks. 22 | It contains two equivalent implementations of the IK solver setup. The former 23 | is best for beginners as it uses the simpler ``Stance`` interface. The latter 24 | is for more advanced users and shows how to add individual tasks one by one. 25 | 26 | The example loads the JVRC-1 humanoid model, then generates a posture where the 27 | robot has both feet on pre-defined contact locations. The robot tracks a 28 | reference COM position given by the red box, which you can move around directly 29 | by using the interaction mode of the OpenRAVE GUI. 30 | """ 31 | 32 | import IPython 33 | import numpy 34 | import random 35 | import sys 36 | 37 | import pymanoid 38 | 39 | from pymanoid import Contact 40 | from pymanoid import PointMass 41 | from pymanoid import Stance 42 | from pymanoid.robots import JVRC1 43 | 44 | 45 | def setup_ik_from_stance(): 46 | """ 47 | Setup inverse kinematics from the simpler stance interface. 48 | 49 | Notes 50 | ----- 51 | This function is equivalent to :func:`setup_ik_from_tasks` below. 52 | """ 53 | robot.set_z(0.8) # hack to start with the robot above contacts 54 | com_target = robot.get_com_point_mass() 55 | lf_target = robot.left_foot.get_contact(pos=[0, 0.3, 0]) 56 | rf_target = robot.right_foot.get_contact(pos=[0, -0.3, 0]) 57 | stance = Stance(com=com_target, left_foot=lf_target, right_foot=rf_target) 58 | stance.dof_tasks[robot.R_SHOULDER_R] = -0.5 59 | stance.dof_tasks[robot.L_SHOULDER_R] = +0.5 60 | stance.bind(robot) 61 | 62 | 63 | def setup_ik_from_tasks(): 64 | """ 65 | Setup the inverse kinematics task by task. 66 | 67 | Note 68 | ----- 69 | This function is equivalent to :func:`setup_ik_from_stance` above. 70 | Beginners should take a look at that one first. 71 | 72 | Notes 73 | ----- 74 | See this `tutorial on inverse kinematics 75 | `_ for 76 | details. 77 | """ 78 | from pymanoid.tasks import COMTask, ContactTask, DOFTask, PostureTask 79 | 80 | # Prepare targets 81 | lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0]) 82 | rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0]) 83 | 84 | # Initial robot pose 85 | robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z]) 86 | com = PointMass(pos=robot.com, mass=robot.mass) 87 | 88 | # Prepare tasks 89 | left_foot_task = ContactTask( 90 | robot, robot.left_foot, lf_target, weight=1., gain=0.85) 91 | right_foot_task = ContactTask( 92 | robot, robot.right_foot, rf_target, weight=1., gain=0.85) 93 | com_task = COMTask(robot, com, weight=1e-2, gain=0.85) 94 | posture_task = PostureTask(robot, robot.q_halfsit, weight=1e-6, gain=0.85) 95 | 96 | # Add tasks to the IK solver 97 | robot.ik.add(left_foot_task) 98 | robot.ik.add(right_foot_task) 99 | robot.ik.add(com_task) 100 | robot.ik.add(posture_task) 101 | 102 | # Add shoulder DOF tasks for a nicer posture 103 | robot.ik.add(DOFTask( 104 | robot, robot.R_SHOULDER_R, -0.5, gain=0.5, weight=1e-5)) 105 | robot.ik.add(DOFTask( 106 | robot, robot.L_SHOULDER_R, +0.5, gain=0.5, weight=1e-5)) 107 | 108 | 109 | if __name__ == '__main__': 110 | sim = pymanoid.Simulation(dt=0.03) 111 | robot = JVRC1('JVRC-1.dae', download_if_needed=True) 112 | robot.set_transparency(0.3) 113 | sim.set_viewer() 114 | sim.viewer.SetCamera([ 115 | [-0.28985317, 0.40434422, -0.86746233, 2.73872042], 116 | [0.95680251, 0.10095043, -0.2726499, 0.86080128], 117 | [-0.02267371, -0.90901857, -0.41613837, 2.06654644], 118 | [0., 0., 0., 1.]]) 119 | 120 | # Optional: add low DOF acceleration limits 121 | robot.qdd_lim = 100. * numpy.ones(robot.q.shape) 122 | 123 | # Setup IK tasks 124 | if random.randint(0, 1) == 0 or "--stance" in sys.argv: 125 | setup_ik_from_stance() 126 | else: # the two functions are equivalent, pick one at random 127 | setup_ik_from_tasks() 128 | 129 | # Generate an initial posture by solving the IK problem 130 | robot.ik.verbosity = 2 131 | robot.ik.solve(max_it=100, impr_stop=1e-4) 132 | robot.ik.verbosity = 0 133 | 134 | # Next, track targets while the simulation runs 135 | sim.schedule(robot.ik) 136 | sim.start() 137 | 138 | print(""" 139 | =============================================================================== 140 | 141 | Robot posture generated. 142 | 143 | From there, you can: 144 | 145 | - Press [ESC] in the GUI to switch to edit mode. 146 | - Click on the red box and drag-and-drop it around. The robot should follow. 147 | - Click on the white background to de-select the box. 148 | - Press [ESC] again to move the camera around. 149 | - Click on a red contact slab under a robot foot and move the contact around. 150 | 151 | =============================================================================== 152 | """) 153 | 154 | if IPython.get_ipython() is None: # give the user a prompt 155 | IPython.embed() 156 | -------------------------------------------------------------------------------- /examples/lip_stabilization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | This example implements a basic stabilizer for the linear inverted pendulum 22 | model based on proportional feedback of the divergent component of motion. 23 | """ 24 | 25 | import IPython 26 | import numpy 27 | 28 | from numpy import dot 29 | 30 | import pymanoid 31 | 32 | from pymanoid.gui import draw_arrow, draw_point 33 | from pymanoid.misc import norm 34 | 35 | 36 | class Stabilizer(pymanoid.Process): 37 | 38 | """ 39 | Inverted pendulum stabilizer based on proportional DCM feedback. 40 | 41 | Parameters 42 | ---------- 43 | pendulum : pymanoid.models.InvertedPendulum 44 | Inverted pendulum to stabilizer. 45 | gain : scalar 46 | DCM feedback gain. 47 | """ 48 | 49 | def __init__(self, pendulum, gain=2.): 50 | super(Stabilizer, self).__init__() 51 | assert gain > 1., "DCM feedback gain needs to be greater than one" 52 | self.desired_dcm = pendulum.com.p 53 | self.desired_vrp = pendulum.com.p 54 | self.gain = gain 55 | self.handles = None 56 | self.omega = numpy.sqrt(pendulum.lambda_) 57 | self.omega2 = self.omega ** 2 58 | self.pendulum = pendulum 59 | 60 | def draw(self, dcm, cop): 61 | """ 62 | Draw extra points to illustrate stabilizer behavior. 63 | 64 | Parameters 65 | ---------- 66 | dcm : (3,) array 67 | Divergent component of motion. 68 | cop : (3,) array 69 | Center of pressure. 70 | """ 71 | n = self.pendulum.contact.n 72 | dcm_ground = dcm - dot(n, dcm) * n 73 | self.handles = [ 74 | draw_point(dcm_ground, color='b'), 75 | draw_point(cop, color='g')] 76 | 77 | def on_tick(self, sim): 78 | """ 79 | Set inverted pendulum CoP and stiffness inputs. 80 | 81 | Parameters 82 | ---------- 83 | sim : pymanoid.Simulation 84 | Simulation instance. 85 | 86 | Notes 87 | ----- 88 | See [Englsberger15]_ for details on the definition of the virtual 89 | repellent point (VRP). Here we differentiate between the constants 90 | lambda and omega: lambda corresponds to the "CoP-based inverted 91 | pendulum" and omega to the "floating-base inverted pendulum" models 92 | described in Section II.B of [Caron17w]_. 93 | 94 | Overall, we can interpret ``omega`` as a normalized stiffness between 95 | the CoM and VRP, while ``lambda`` corresponds to the virtual leg 96 | stiffness between the CoM and ZMP. The reason why the mapping is 97 | nonlinear is that the ZMP is constrained to lie on the contact surface, 98 | while the CoM can move in 3D. 99 | 100 | If we study further the relationship between lambda and omega, we find 101 | that they are actually related by a Riccati equation [Caron19]_. 102 | """ 103 | com = self.pendulum.com.p 104 | comd = self.pendulum.com.pd 105 | dcm = com + comd / self.omega 106 | dcm_error = self.desired_dcm - dcm 107 | vrp = self.desired_vrp - self.gain * dcm_error 108 | n = self.pendulum.contact.n 109 | gravito_inertial_force = self.omega2 * (com - vrp) - sim.gravity 110 | displacement = com - self.pendulum.contact.p 111 | lambda_ = dot(n, gravito_inertial_force) / dot(n, displacement) 112 | cop = com - gravito_inertial_force / lambda_ 113 | self.pendulum.set_cop(cop, clamp=True) 114 | self.pendulum.set_lambda(lambda_) 115 | self.draw(dcm, cop) 116 | 117 | 118 | class Pusher(pymanoid.Process): 119 | 120 | """ 121 | Send impulses to the inverted pendulum every once in a while. 122 | 123 | Parameters 124 | ---------- 125 | pendulum : pymanoid.models.InvertedPendulum 126 | Inverted pendulum to de-stabilize. 127 | gain : scalar 128 | Magnitude of velocity jumps. 129 | 130 | Notes 131 | ----- 132 | You know, I've seen a lot of people walkin' 'round // With tombstones in 133 | their eyes // But the pusher don't care // Ah, if you live or if you die 134 | """ 135 | 136 | def __init__(self, pendulum, gain=0.1): 137 | super(Pusher, self).__init__() 138 | self.gain = gain 139 | self.handle = None 140 | self.nb_ticks = 0 141 | self.pendulum = pendulum 142 | 143 | def on_tick(self, sim): 144 | """ 145 | Apply regular impulses to the inverted pendulum. 146 | 147 | Parameters 148 | ---------- 149 | sim : pymanoid.Simulation 150 | Simulation instance. 151 | """ 152 | self.nb_ticks += 1 153 | if self.handle is not None and self.nb_ticks % 15 == 0: 154 | self.handle = None 155 | if self.nb_ticks % 50 == 0: 156 | com = self.pendulum.com.p 157 | comd = self.pendulum.com.pd 158 | dv = 2. * numpy.random.random(3) - 1. 159 | dv[2] *= 0.5 # push is weaker in vertical direction 160 | dv *= self.gain / norm(dv) 161 | self.pendulum.com.set_vel(comd + dv) 162 | self.handle = draw_arrow(com - dv, com, color='b', linewidth=0.01) 163 | 164 | 165 | if __name__ == '__main__': 166 | sim = pymanoid.Simulation(dt=0.03) 167 | sim.set_viewer() 168 | sim.viewer.SetCamera([ 169 | [-0.28985337, 0.40434395, -0.86746239, 1.40434551], 170 | [0.95680245, 0.1009506, -0.27265003, 0.45636871], 171 | [-0.02267354, -0.90901867, -0.41613816, 1.15192068], 172 | [0., 0., 0., 1.]]) 173 | 174 | contact = pymanoid.Contact((0.1, 0.05), pos=[0., 0., 0.]) 175 | pendulum = pymanoid.models.InvertedPendulum( 176 | pos=[0., 0., 0.8], vel=numpy.zeros(3), contact=contact) 177 | stabilizer = Stabilizer(pendulum) 178 | pusher = Pusher(pendulum) 179 | 180 | sim.schedule(stabilizer) # before pendulum in schedule 181 | sim.schedule(pendulum) 182 | sim.schedule_extra(pusher) 183 | sim.start() 184 | 185 | print(""" 186 | 187 | Linear Inverted Pendulum Stabilization 188 | ====================================== 189 | 190 | Ready to go! You can access all state variables via this IPython shell. 191 | Here is the list of global objects. Use to see what's inside. 192 | 193 | pendulum -- linear inverted pendulum state 194 | pusher -- applies an external impulse on the pendulum at regular intervals 195 | 196 | You can pause/resume processes or the whole simulation by: 197 | 198 | pusher.pause() -- stop pushing the pendulum at regular intervals 199 | pusher.resume() -- start pushing again 200 | sim.stop() -- stop/pause simulation 201 | sim.step(100) -- run simulation in current thread for 100 steps 202 | sim.start() -- resume simulation 203 | 204 | When the simulation is paused, you can edit the position of the center of mass 205 | of the pendulum using the GUI's edit mode ( then click on the blue box). 206 | 207 | Enjoy :) 208 | 209 | """) 210 | 211 | if IPython.get_ipython() is None: # give the user a prompt 212 | IPython.embed() 213 | -------------------------------------------------------------------------------- /pymanoid/.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | -------------------------------------------------------------------------------- /pymanoid/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from .body import Body 21 | from .body import Box 22 | from .body import Cube 23 | from .body import Manipulator 24 | from .body import Point 25 | from .body import PointMass 26 | from .contact import Contact 27 | from .contact import ContactFeed 28 | from .contact import ContactSet 29 | from .misc import error 30 | from .misc import info 31 | from .misc import warn 32 | from .models import InvertedPendulum 33 | from .proc import CameraRecorder 34 | from .proc import JointRecorder 35 | from .proc import Process 36 | from .robot import Humanoid 37 | from .robot import Robot 38 | from .sim import Simulation 39 | from .stance import Stance 40 | from .swing_foot import SwingFoot 41 | 42 | import models # comment out this import when building the docs 43 | import robots # comment out this import when building the docs 44 | 45 | __all__ = [ 46 | 'Body', 47 | 'Box', 48 | 'CameraRecorder', 49 | 'Contact', 50 | 'ContactFeed', 51 | 'ContactSet', 52 | 'Cube', 53 | 'Humanoid', 54 | 'Humanoid', 55 | 'InvertedPendulum', 56 | 'Manipulator', 57 | 'Point', 58 | 'PointMass', 59 | 'Process', 60 | 'Robot', 61 | 'Simulation', 62 | 'Stance', 63 | 'SwingFoot', 64 | 'error', 65 | 'info', 66 | 'models', 67 | 'robots', 68 | 'warn', 69 | ] 70 | 71 | __version__ = '1.2.0' 72 | -------------------------------------------------------------------------------- /pymanoid/interp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from numpy import hstack, linspace 21 | from openravepy import InterpolateQuatSlerp as quat_slerp 22 | 23 | from .gui import draw_trajectory 24 | from .misc import NDPolynomial 25 | 26 | 27 | def interpolate_cubic_bezier(p0, p1, p2, p3): 28 | """ 29 | Compute the cubic Bezier curve corresponding to four control points. 30 | 31 | Parameters 32 | ---------- 33 | p0 : (3,) array 34 | First control point. 35 | p1 : (3,) array 36 | Second control point. 37 | p2 : (3,) array 38 | Third control point. 39 | p3 : (3,) array 40 | Fourth control point. 41 | 42 | Returns 43 | ------- 44 | P : NDPolynomial 45 | Polynomial function of the Bezier curve. 46 | """ 47 | c3 = p3 - 3 * (p2 - p1) - p0 48 | c2 = 3 * (p2 - 2 * p1 + p0) 49 | c1 = 3 * (p1 - p0) 50 | c0 = p0 51 | return NDPolynomial([c0, c1, c2, c3]) 52 | 53 | 54 | def interpolate_cubic_hermite(p0, v0, p1, v1): 55 | """ 56 | Compute the third-order polynomial of the Hermite curve connecting 57 | :math:`(p_0, v_0)` to :math:`(p_1, v_1)`. 58 | 59 | Parameters 60 | ---------- 61 | p0 : (3,) array 62 | Start point. 63 | v0 : (3,) array 64 | Start velocity. 65 | p1 : (3,) array 66 | End point. 67 | v1 : (3,) array 68 | End velocity. 69 | 70 | Returns 71 | ------- 72 | P : NDPolynomial 73 | Polynomial function of the Hermite curve. 74 | 75 | """ 76 | c3 = 2 * p0 - 2 * p1 + v0 + v1 77 | c2 = -3 * p0 + 3 * p1 - 2 * v0 - v1 78 | c1 = v0 79 | c0 = p0 80 | return NDPolynomial([c0, c1, c2, c3]) 81 | 82 | 83 | def interpolate_pose_linear(pose0, pose1, x): 84 | """ 85 | Standalone function for linear pose interpolation. 86 | 87 | Parameters 88 | ---------- 89 | pose0 : (7,) array 90 | First pose. 91 | pose1 : (7,) array 92 | Second pose. 93 | x : scalar 94 | Number between 0 and 1. 95 | 96 | Returns 97 | ------- 98 | pose : (7,) array 99 | Linear interpolation between the first two arguments. 100 | """ 101 | pos = pose0[4:] + x * (pose1[4:] - pose0[4:]) 102 | quat = quat_slerp(pose0[:4], pose1[:4], x) 103 | return hstack([quat, pos]) 104 | 105 | 106 | def interpolate_pose_quadratic(pose0, pose1, x): 107 | """ 108 | Pose interpolation that is linear in orientation (SLERP) and 109 | quadratic (Bezier) in position. 110 | 111 | Parameters 112 | ---------- 113 | pose0 : (7,) array 114 | First pose. 115 | pose1 : (7,) array 116 | Second pose. 117 | x : scalar 118 | Number between 0 and 1. 119 | 120 | Returns 121 | ------- 122 | pose : (7,) array 123 | Linear interpolation between the first two arguments. 124 | 125 | Note 126 | ---- 127 | Initial and final linear velocities on the trajectory are zero. 128 | """ 129 | pos = x ** 2 * (3 - 2 * x) * (pose1[4:] - pose0[4:]) + pose0[4:] 130 | quat = quat_slerp(pose0[:4], pose1[:4], x) 131 | return hstack([quat, pos]) 132 | 133 | 134 | class PoseInterpolator(object): 135 | 136 | """ 137 | Interpolate a body trajectory between two poses. Orientations are 138 | computed by `spherical linear interpolation 139 | `_. 140 | 141 | Parameters 142 | ---------- 143 | start_pose : (7,) array 144 | Initial pose to interpolate from. 145 | end_pose : (7,) array 146 | End pose to interpolate to. 147 | body : Body, optional 148 | Body affected by the update function. 149 | """ 150 | 151 | def __init__(self, start_pose, end_pose, duration, body=None): 152 | self.body = body 153 | self.duration = duration 154 | self.end_quat = end_pose[:4] 155 | self.start_quat = start_pose[:4] 156 | 157 | def eval_quat(self, s): 158 | return quat_slerp(self.start_quat, self.end_quat, s) 159 | 160 | def eval_pos(self, s): 161 | """ 162 | Evaluate position at index s between 0 and 1. 163 | 164 | Parameters 165 | ---------- 166 | s : scalar 167 | Normalized trajectory index between 0 and 1. 168 | """ 169 | raise NotImplementedError() 170 | 171 | def __call__(self, t): 172 | s = 0. if t < 0. else 1. if t > self.duration else t / self.duration 173 | return hstack([self.eval_quat(s), self.eval_pos(s)]) 174 | 175 | def update(self, t): 176 | pose = self.__call__(t) 177 | self.body.set_pose(pose) 178 | 179 | def draw(self, color='b'): 180 | """ 181 | Draw positions of the interpolated trajectory. 182 | 183 | Parameters 184 | ---------- 185 | color : char or triplet, optional 186 | Color letter or RGB values, default is 'b' for blue. 187 | 188 | Returns 189 | ------- 190 | handle : openravepy.GraphHandle 191 | OpenRAVE graphical handle. Must be stored in some variable, 192 | otherwise the drawn object will vanish instantly. 193 | """ 194 | points = [self.eval_pos(s) for s in linspace(0, 1, 10)] 195 | return draw_trajectory(points, color=color) 196 | 197 | 198 | class LinearPoseInterpolator(PoseInterpolator): 199 | 200 | """ 201 | Interpolate a body trajectory between two poses. Intermediate positions are 202 | interpolated linearly, while orientations are computed by `spherical linear 203 | interpolation `_. 204 | 205 | Parameters 206 | ---------- 207 | start_pose : (7,) array 208 | Initial pose to interpolate from. 209 | end_pose : (7,) array 210 | End pose to interpolate to. 211 | body : Body, optional 212 | Body affected by the update function. 213 | """ 214 | 215 | def __init__(self, start_pose, end_pose, duration, body=None): 216 | assert len(start_pose) == len(end_pose) == 7 217 | super(LinearPoseInterpolator, self).__init__( 218 | start_pose, end_pose, duration, body) 219 | self.delta_pos = end_pose[4:] - start_pose[4:] 220 | self.start_pos = start_pose[4:] 221 | 222 | def eval_pos(self, s): 223 | """ 224 | Evaluate position at index s between 0 and 1. 225 | 226 | Parameters 227 | ---------- 228 | s : scalar 229 | Normalized trajectory index between 0 and 1. 230 | """ 231 | return self.start_pos + s * self.delta_pos 232 | 233 | 234 | class CubicPoseInterpolator(PoseInterpolator): 235 | 236 | """ 237 | Interpolate a body trajectory between two poses. Intermediate positions are 238 | interpolated cubically with zero boundary velocities, while orientations 239 | are computed by `spherical linear interpolation 240 | `_. 241 | 242 | Parameters 243 | ---------- 244 | start_pose : (7,) array 245 | Initial pose to interpolate from. 246 | end_pose : (7,) array 247 | End pose to interpolate to. 248 | body : Body, optional 249 | Body affected by the update function. 250 | """ 251 | 252 | def __init__(self, start_pose, end_pose, duration, body=None): 253 | assert len(start_pose) == len(end_pose) == 7 254 | super(CubicPoseInterpolator, self).__init__( 255 | start_pose, end_pose, duration, body) 256 | self.delta_pos = end_pose[4:] - start_pose[4:] 257 | self.start_pos = start_pose[4:] 258 | 259 | def eval_pos(self, s): 260 | """ 261 | Evaluate position at index s between 0 and 1. 262 | 263 | Parameters 264 | ---------- 265 | s : scalar 266 | Normalized trajectory index between 0 and 1. 267 | """ 268 | return self.start_pos + s ** 2 * (3 - 2 * s) * self.delta_pos 269 | 270 | 271 | class QuinticPoseInterpolator(PoseInterpolator): 272 | 273 | """ 274 | Interpolate a body trajectory between two poses. Intermediate positions are 275 | interpolated quintically with zero-velocity and zero-acceleration boundary 276 | conditions, while orientations are computed by `spherical linear 277 | interpolation `_. 278 | 279 | Parameters 280 | ---------- 281 | start_pose : (7,) array 282 | Initial pose to interpolate from. 283 | end_pose : (7,) array 284 | End pose to interpolate to. 285 | body : Body, optional 286 | Body affected by the update function. 287 | """ 288 | 289 | def __init__(self, start_pose, end_pose, duration, body=None): 290 | assert len(start_pose) == len(end_pose) == 7 291 | super(QuinticPoseInterpolator, self).__init__( 292 | start_pose, end_pose, duration, body) 293 | self.delta_pos = end_pose[4:] - start_pose[4:] 294 | self.start_pos = start_pose[4:] 295 | 296 | def eval_pos(self, s): 297 | """ 298 | Evaluate position at index s between 0 and 1. 299 | 300 | Parameters 301 | ---------- 302 | s : scalar 303 | Normalized trajectory index between 0 and 1. 304 | """ 305 | return self.start_pos + \ 306 | s ** 3 * (10 - 15 * s + 6 * s ** 2) * self.delta_pos 307 | 308 | 309 | class LinearPosInterpolator(LinearPoseInterpolator): 310 | 311 | """ 312 | Linear interpolation between two positions. 313 | 314 | Parameters 315 | ---------- 316 | start_pos : (3,) array 317 | Initial position to interpolate from. 318 | end_pos : (3,) array 319 | End position to interpolate to. 320 | body : Body, optional 321 | Body affected by the update function. 322 | """ 323 | 324 | def __init__(self, start_pos, end_pos, duration, body=None): 325 | # parent constructor is not called 326 | assert len(start_pos) == len(end_pos) == 3 327 | self.body = body 328 | self.delta_pos = end_pos - start_pos 329 | self.duration = duration 330 | self.start_pos = start_pos 331 | 332 | def __call__(self, t): 333 | s = t / self.duration 334 | return self.eval_pos(s) 335 | 336 | 337 | class CubicPosInterpolator(CubicPoseInterpolator): 338 | 339 | """ 340 | Cubic interpolation between two positions with zero-velocity boundary 341 | conditions. 342 | 343 | Parameters 344 | ---------- 345 | start_pos : (3,) array 346 | Initial position to interpolate from. 347 | end_pos : (3,) array 348 | End position to interpolate to. 349 | body : Body, optional 350 | Body affected by the update function. 351 | """ 352 | 353 | def __init__(self, start_pos, end_pos, duration, body=None): 354 | # parent constructor is not called 355 | assert len(start_pos) == len(end_pos) == 3 356 | self.body = body 357 | self.delta_pos = end_pos - start_pos 358 | self.duration = duration 359 | self.start_pos = start_pos 360 | 361 | def __call__(self, t): 362 | s = t / self.duration 363 | return self.eval_pos(s) 364 | 365 | 366 | class QuinticPosInterpolator(QuinticPoseInterpolator): 367 | 368 | """ 369 | Quintic interpolation between two positions with zero-velocity and 370 | zero-acceleration boundary conditions. 371 | 372 | Parameters 373 | ---------- 374 | start_pos : (3,) array 375 | Initial position to interpolate from. 376 | end_pos : (3,) array 377 | End position to interpolate to. 378 | body : Body, optional 379 | Body affected by the update function. 380 | """ 381 | 382 | def __init__(self, start_pos, end_pos, duration, body=None): 383 | # parent constructor is not called 384 | assert len(start_pos) == len(end_pos) == 3 385 | self.body = body 386 | self.delta_pos = end_pos - start_pos 387 | self.duration = duration 388 | self.start_pos = start_pos 389 | 390 | def __call__(self, t): 391 | s = t / self.duration 392 | return self.eval_pos(s) 393 | -------------------------------------------------------------------------------- /pymanoid/misc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from datetime import datetime 21 | from numpy import array, dot, sqrt, tensordot, zeros 22 | 23 | 24 | class AvgStdEstimator(object): 25 | 26 | """ 27 | Online estimator for the average and standard deviation of a time series of 28 | scalar values. 29 | """ 30 | 31 | def __init__(self): 32 | self.reset() 33 | 34 | def reset(self): 35 | self.last_value = None 36 | self.n = 0 37 | self.x = 0. 38 | self.x2 = 0. 39 | self.x_max = None 40 | self.x_min = None 41 | 42 | def add(self, x): 43 | """ 44 | Add a new value of the time series. 45 | 46 | Parameters 47 | ---------- 48 | x : scalar 49 | New value. 50 | """ 51 | self.last_value = x 52 | self.n += 1 53 | self.x += x 54 | self.x2 += x ** 2 55 | if self.x_max is None or x > self.x_max: 56 | self.x_max = x 57 | if self.x_min is None or x < self.x_min: 58 | self.x_min = x 59 | 60 | @property 61 | def avg(self): 62 | """ 63 | Average of the time series. 64 | """ 65 | if self.n < 1: 66 | return None 67 | return self.x / self.n 68 | 69 | @property 70 | def std(self): 71 | """ 72 | Standard deviation of the time series. 73 | """ 74 | if self.n < 1: 75 | return None 76 | elif self.n == 1: 77 | return 0. 78 | unbiased = sqrt(self.n * 1. / (self.n - 1)) 79 | return unbiased * sqrt(self.x2 / self.n - self.avg ** 2) 80 | 81 | def __str__(self): 82 | return "%f +/- %f (max: %f, min: %f) over %d items" % ( 83 | self.avg, self.std, self.x_max, self.x_min, self.n) 84 | 85 | 86 | class NDPolynomial(object): 87 | 88 | """ 89 | Polynomial class with vector-valued coefficients. 90 | 91 | Parameters 92 | ---------- 93 | coeffs : list of arrays 94 | Coefficients of the polynomial from weakest to strongest. 95 | """ 96 | 97 | def __init__(self, coeffs): 98 | self.coeffs = coeffs 99 | self.shape = coeffs[0].shape 100 | 101 | @property 102 | def degree(self): 103 | """ 104 | Degree of the polynomial. 105 | """ 106 | return len(self.coeffs) - 1 107 | 108 | def __call__(self, x): 109 | """ 110 | Evaluate the polynomial at `x`. 111 | 112 | Parameters 113 | ---------- 114 | x : scalar 115 | Value to evaluate the polynomial at. 116 | 117 | Returns 118 | ------- 119 | P(x) : array 120 | Value of the polynomial at this point. 121 | """ 122 | value = zeros(self.shape) 123 | for coeff in reversed(self.coeffs): 124 | value *= x 125 | value += coeff 126 | return value 127 | 128 | 129 | class PointWrap(object): 130 | 131 | """ 132 | An object with a ``p`` array field. 133 | 134 | Parameters 135 | ---------- 136 | p : list or array 137 | Point coordinates. 138 | """ 139 | 140 | def __init__(self, p): 141 | assert len(p) == 3, "Argument is not a point" 142 | self.p = array(p) 143 | 144 | 145 | class PoseWrap(object): 146 | 147 | """ 148 | An object with a ``pose`` array field. 149 | 150 | Parameters 151 | ---------- 152 | p : list or array 153 | Pose coordinates. 154 | """ 155 | 156 | def __init__(self, pose): 157 | assert len(pose) == 7, "Argument is not a pose" 158 | self.pose = array(pose) 159 | 160 | 161 | def error(msg): 162 | """ 163 | Log an error message (in red) to stdout. 164 | 165 | Parameters 166 | ---------- 167 | msg : str 168 | Error message. 169 | """ 170 | now = datetime.now().strftime("%Y-%m-%d %H:%M:%S,%f")[:-3] 171 | print("%c[0;%d;48m%s pymanoid [ERROR] %s%c[m" % (0x1B, 31, now, msg, 0x1B)) 172 | 173 | 174 | def info(msg): 175 | """ 176 | Log an information message (in green) to stdout. 177 | 178 | Parameters 179 | ---------- 180 | msg : str 181 | Information message. 182 | """ 183 | now = datetime.now().strftime("%Y-%m-%d %H:%M:%S,%f")[:-3] 184 | print("%c[0;%d;48m%s pymanoid [INFO] %s%c[m" % (0x1B, 32, now, msg, 0x1B)) 185 | 186 | 187 | def matplotlib_to_rgb(color): 188 | """ 189 | Convert matplotlib color string to RGB tuple. 190 | 191 | Parameters 192 | ---------- 193 | color : string 194 | Color code in matplotlib convention ('r' for red, 'b' for blue, etc.). 195 | 196 | Returns 197 | ------- 198 | rgb : tuple 199 | Red-green-blue tuple with values between 0 and 1. 200 | """ 201 | acolor = [0., 0., 0.] 202 | if color == 'k': 203 | return acolor 204 | if color == 'w': 205 | return [1., 1., 1.] 206 | if color in ['r', 'm', 'y', 'w']: 207 | acolor[0] += 0.5 208 | if color in ['g', 'y', 'c', 'w']: 209 | acolor[1] += 0.5 210 | if color in ['b', 'c', 'm', 'w']: 211 | acolor[2] += 0.5 212 | return acolor 213 | 214 | 215 | def matplotlib_to_rgba(color, alpha=0.5): 216 | """ 217 | Convert matplotlib color string to RGBA tuple. 218 | 219 | Parameters 220 | ---------- 221 | color : string 222 | Color code in matplotlib convention ('r' for red, 'g' for green, etc.). 223 | alpha : scalar, optional 224 | Transparency between 0 and 1. 225 | 226 | Returns 227 | ------- 228 | rgba : tuple 229 | Red-green-blue-alpha tuple with values between 0 and 1. 230 | """ 231 | return matplotlib_to_rgb(color) + [alpha] 232 | 233 | 234 | def middot(M, T): 235 | """ 236 | Dot product of a matrix with the mid-coordinate of a 3D tensor. 237 | 238 | Parameters 239 | ---------- 240 | M : array, shape=(n, m) 241 | Matrix to multiply. 242 | T : array, shape=(a, m, b) 243 | Tensor to multiply. 244 | 245 | Returns 246 | ------- 247 | U : array, shape=(a, n, b) 248 | Dot product between `M` and `T`. 249 | """ 250 | return tensordot(M, T, axes=(1, 1)).transpose([1, 0, 2]) 251 | 252 | 253 | def norm(v): 254 | """ 255 | Euclidean norm. 256 | 257 | Parameters 258 | ---------- 259 | v : array 260 | Any vector. 261 | 262 | Returns 263 | ------- 264 | n : scalar 265 | Euclidean norm of `v`. 266 | 267 | Notes 268 | ----- 269 | This straightforward function is 2x faster than :func:`numpy.linalg.norm` 270 | on my machine. 271 | """ 272 | return sqrt(dot(v, v)) 273 | 274 | 275 | def normalize(v): 276 | """ 277 | Normalize a vector. 278 | 279 | Parameters 280 | ---------- 281 | v : array 282 | Any vector. 283 | 284 | Returns 285 | ------- 286 | nv : array 287 | Unit vector directing `v`. 288 | 289 | Notes 290 | ----- 291 | This method doesn't catch ``ZeroDivisionError`` exceptions on purpose. 292 | """ 293 | return v / norm(v) 294 | 295 | 296 | def plot_polygon(points, alpha=.4, color='g', linestyle='solid', fill=True, 297 | linewidth=None): 298 | """ 299 | Plot a polygon in matplotlib. 300 | 301 | Parameters 302 | ---------- 303 | points : list of arrays 304 | List of poitns. 305 | alpha : scalar, optional 306 | Transparency value. 307 | color : string, optional 308 | Color in matplotlib format. 309 | linestyle : scalar, optional 310 | Line style in matplotlib format. 311 | fill : bool, optional 312 | When ``True``, fills the area inside the polygon. 313 | linewidth : scalar, optional 314 | Line width in matplotlib format. 315 | """ 316 | from matplotlib.patches import Polygon 317 | from pylab import axis, gca 318 | from scipy.spatial import ConvexHull 319 | if type(points) is list: 320 | points = array(points) 321 | ax = gca() 322 | hull = ConvexHull(points) 323 | points = points[hull.vertices, :] 324 | xmin1, xmax1, ymin1, ymax1 = axis() 325 | xmin2, ymin2 = 1.5 * points.min(axis=0) 326 | xmax2, ymax2 = 1.5 * points.max(axis=0) 327 | axis((min(xmin1, xmin2), max(xmax1, xmax2), 328 | min(ymin1, ymin2), max(ymax1, ymax2))) 329 | patch = Polygon( 330 | points, alpha=alpha, color=color, linestyle=linestyle, fill=fill, 331 | linewidth=linewidth) 332 | ax.add_patch(patch) 333 | 334 | 335 | def warn(msg): 336 | """ 337 | Log a warning message (in yellow) to stdout. 338 | 339 | Parameters 340 | ---------- 341 | msg : str 342 | Warning message. 343 | """ 344 | now = datetime.now().strftime("%Y-%m-%d %H:%M:%S,%f")[:-3] 345 | print("%c[0;%d;48m%s pymanoid [WARN] %s%c[m" % (0x1B, 33, now, msg, 0x1B)) 346 | -------------------------------------------------------------------------------- /pymanoid/models.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from numpy import cosh, dot, sinh, sqrt 21 | 22 | from .body import Point 23 | from .gui import draw_line, draw_point 24 | from .misc import warn 25 | from .sim import Process, gravity 26 | 27 | 28 | class InvertedPendulum(Process): 29 | 30 | """ 31 | Inverted pendulum model. 32 | 33 | Parameters 34 | ---------- 35 | pos : (3,) array 36 | Initial position in the world frame. 37 | vel : (3,) array 38 | Initial velocity in the world frame. 39 | contact : pymanoid.Contact 40 | Contact surface specification. 41 | lambda_min : scalar 42 | Minimum virtual leg stiffness. 43 | lambda_max : scalar 44 | Maximum virtual leg stiffness. 45 | clamp : bool, optional 46 | Clamp inputs (e.g. CoP) if they exceed constraints (e.g. support area)? 47 | visible : bool, optional 48 | Draw the pendulum model in GUI? 49 | color : char, optional 50 | Color code in matplotlib convention ('r' for red, 'b' for blue, etc.). 51 | size : scalar, optional 52 | Half-length of a side of the CoM cube handle, in [m]. 53 | """ 54 | 55 | def __init__(self, pos, vel, contact, lambda_min=1e-5, lambda_max=None, 56 | clamp=True, visible=True, color='b', size=0.02): 57 | super(InvertedPendulum, self).__init__() 58 | com = Point(pos, vel, size=size, color=color) 59 | self.clamp = clamp 60 | self.color = color 61 | self.com = com 62 | self.contact = contact 63 | self.cop = contact.p 64 | self.handles = None 65 | self.is_visible = visible 66 | self.lambda_ = -gravity[2] / (com.z - contact.z) 67 | self.lambda_max = lambda_max 68 | self.lambda_min = lambda_min 69 | if visible: 70 | self.show() 71 | else: # not visible 72 | self.hide() 73 | 74 | def copy(self, visible=True): 75 | """ 76 | Copy constructor. 77 | 78 | Parameters 79 | ---------- 80 | visible : bool, optional 81 | Should the copy be visible? 82 | """ 83 | return InvertedPendulum( 84 | self.com.p, self.com.pd, self.contact, visible=visible) 85 | 86 | def draw(self): 87 | """Draw inverted pendulum.""" 88 | fulcrum = draw_point(self.cop, pointsize=0.01, color=self.color) 89 | leg = draw_line(self.com.p, self.cop, linewidth=4, color=self.color) 90 | self.handles = [fulcrum, leg] 91 | 92 | def hide(self): 93 | """Hide pendulum from the GUI.""" 94 | self.com.hide() 95 | if self.handles: 96 | for handle in self.handles: 97 | handle.Close() 98 | self.is_visible = False 99 | 100 | def show(self): 101 | """Show pendulum in the GUI.""" 102 | self.com.show() 103 | self.draw() 104 | self.is_visible = True 105 | 106 | def set_contact(self, contact): 107 | """ 108 | Update the contact the pendulum rests upon. 109 | 110 | Parameters 111 | ---------- 112 | contact : pymanoid.Contact 113 | New contact where CoPs can be realized. 114 | """ 115 | self.contact = contact 116 | 117 | def set_cop(self, cop, clamp=None): 118 | """ 119 | Update the CoP location on the contact surface. 120 | 121 | Parameters 122 | ---------- 123 | cop : (3,) array 124 | New CoP location in the world frame. 125 | clamp : bool, optional 126 | Clamp CoP within the contact area if it lies outside. Overrides 127 | ``self.clamp``. 128 | """ 129 | if (self.clamp if clamp is None else clamp): 130 | cop_local = dot(self.contact.R.T, cop - self.contact.p) 131 | if cop_local[0] >= self.contact.shape[0]: 132 | cop_local[0] = self.contact.shape[0] - 1e-5 133 | elif cop_local[0] <= -self.contact.shape[0]: 134 | cop_local[0] = -self.contact.shape[0] + 1e-5 135 | if cop_local[1] >= self.contact.shape[1]: 136 | cop_local[1] = self.contact.shape[1] - 1e-5 137 | elif cop_local[1] <= -self.contact.shape[1]: 138 | cop_local[1] = -self.contact.shape[1] + 1e-5 139 | cop = self.contact.p + dot(self.contact.R, cop_local) 140 | elif __debug__: 141 | cop_check = dot(self.contact.R.T, cop - self.contact.p) 142 | if abs(cop_check[0]) > 1.05 * self.contact.shape[0]: 143 | warn("CoP crosses contact area along sagittal axis") 144 | if abs(cop_check[1]) > 1.05 * self.contact.shape[1]: 145 | warn("CoP crosses contact area along lateral axis") 146 | if abs(cop_check[2]) > 0.01: 147 | warn("CoP does not lie on contact area") 148 | self.cop = cop 149 | 150 | def set_lambda(self, lambda_, clamp=None): 151 | """ 152 | Update the leg stiffness coefficient. 153 | 154 | Parameters 155 | ---------- 156 | lambda_ : scalar 157 | Leg stiffness coefficient (positive). 158 | clamp : bool, optional 159 | Clamp value if it exits the [lambda_min, lambda_max] interval. 160 | Overrides ``self.clamp``. 161 | """ 162 | if (self.clamp if clamp is None else clamp): 163 | if self.lambda_min is not None and lambda_ < self.lambda_min: 164 | lambda_ = self.lambda_min 165 | if self.lambda_max is not None and lambda_ > self.lambda_max: 166 | lambda_ = self.lambda_max 167 | elif __debug__: 168 | if self.lambda_min is not None and lambda_ < self.lambda_min: 169 | warn("Stiffness %f below %f" % (lambda_, self.lambda_min)) 170 | if self.lambda_max is not None and lambda_ > self.lambda_max: 171 | warn("Stiffness %f above %f" % (lambda_, self.lambda_max)) 172 | self.lambda_ = lambda_ 173 | 174 | def integrate(self, duration): 175 | """ 176 | Integrate dynamics forward for a given duration. 177 | 178 | Parameters 179 | ---------- 180 | duration : scalar 181 | Duration of forward integration. 182 | """ 183 | omega = sqrt(self.lambda_) 184 | p0 = self.com.p 185 | pd0 = self.com.pd 186 | ch, sh = cosh(omega * duration), sinh(omega * duration) 187 | vrp = self.cop - gravity / self.lambda_ 188 | p = p0 * ch + pd0 * sh / omega - vrp * (ch - 1.) 189 | pd = pd0 * ch + omega * (p0 - vrp) * sh 190 | self.com.set_pos(p) 191 | self.com.set_vel(pd) 192 | 193 | def on_tick(self, sim): 194 | """ 195 | Integrate dynamics for one simulation step. 196 | 197 | Parameters 198 | ---------- 199 | sim : pymanoid.Simulation 200 | Simulation instance. 201 | """ 202 | self.integrate(sim.dt) 203 | if self.is_visible: 204 | self.draw() 205 | -------------------------------------------------------------------------------- /pymanoid/mpc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from numpy import dot, eye, hstack, vstack, zeros 21 | from time import time 22 | 23 | from .qpsolvers import solve_qp 24 | 25 | 26 | class LinearPredictiveControl(object): 27 | 28 | """ 29 | Predictive control for a system with linear dynamics and linear 30 | constraints. 31 | 32 | The discretized dynamics of a linear system are described by: 33 | 34 | .. math:: 35 | 36 | x_{k+1} = A x_k + B u_k 37 | 38 | where :math:`x` is assumed to be the first-order state of a configuration 39 | variable :math:`p`, i.e., it stacks both the position :math:`p` and its 40 | time-derivative :math:`\\dot{p}`. Meanwhile, the system is linearly 41 | constrained by: 42 | 43 | .. math:: 44 | 45 | x_0 & = x_\\mathrm{init} \\\\ 46 | \\forall k, \\ C_k x_k + D_k u_k & \\leq e_k \\\\ 47 | 48 | The output control law minimizes a weighted combination of two types of 49 | costs: 50 | 51 | - Terminal state error 52 | :math:`\\|x_\\mathrm{nb\\_steps} - x_\\mathrm{goal}\\|^2` 53 | with weight :math:`w_{xt}`. 54 | - Cumulated state error: 55 | :math:`\\sum_k \\|x_k - x_\\mathrm{goal}\\|^2` 56 | with weight :math:`w_{xc}`. 57 | - Cumulated control costs: 58 | :math:`\\sum_k \\|u_k\\|^2` 59 | with weight :math:`w_{u}`. 60 | 61 | Parameters 62 | ---------- 63 | A : array, shape=(n, n) 64 | State linear dynamics matrix. 65 | B : array, shape=(n, dim(u)) 66 | Control linear dynamics matrix. 67 | x_init : array, shape=(n,) 68 | Initial state as stacked position and velocity. 69 | x_goal : array, shape=(n,) 70 | Goal state as stacked position and velocity. 71 | nb_steps : int 72 | Number of discretization steps in the preview window. 73 | C : array, shape=(m, dim(u)), list of arrays, or None 74 | Constraint matrix on state variables. When this argument is an array, 75 | the same matrix `C` is applied at each step `k`. When it is ``None``, 76 | the null matrix is applied. 77 | D : array, shape=(l, n), or list of arrays, or None 78 | Constraint matrix on control variables. When this argument is an array, 79 | the same matrix `D` is applied at each step `k`. When it is ``None``, 80 | the null matrix is applied. 81 | e : array, shape=(m,), list of arrays 82 | Constraint vector. When this argument is an array, the same vector `e` 83 | is applied at each step `k`. 84 | wxt : scalar, optional 85 | Weight on terminal state cost, or ``None`` to disable. 86 | wxc : scalar, optional 87 | Weight on cumulated state costs, or ``None`` to disable (default). 88 | wu : scalar, optional 89 | Weight on cumulated control costs. 90 | 91 | Notes 92 | ----- 93 | In numerical analysis, there are three classes of methods to solve boundary 94 | value problems: single shooting, multiple shooting and collocation. The 95 | solver implemented in this class follows the `single shooting method 96 | `_. 97 | """ 98 | 99 | def __init__(self, A, B, C, D, e, x_init, x_goal, nb_steps, wxt=None, 100 | wxc=None, wu=1e-3): 101 | assert C is not None or D is not None, "use LQR for unconstrained case" 102 | assert wu > 0., "non-negative control weight needed for regularization" 103 | assert wxt is not None or wxc is not None, "set either wxt or wxc" 104 | u_dim = B.shape[1] 105 | x_dim = A.shape[1] 106 | self.A = A 107 | self.B = B 108 | self.C = C 109 | self.D = D 110 | self.G = None 111 | self.P = None 112 | self.U = None 113 | self.U_dim = u_dim * nb_steps 114 | self.__X = None 115 | self.build_time = None 116 | self.e = e 117 | self.h = None 118 | self.nb_steps = nb_steps 119 | self.q = None 120 | self.solve_time = None 121 | self.u_dim = u_dim 122 | self.wu = wu 123 | self.wxc = wxc 124 | self.wxt = wxt 125 | self.x_dim = x_dim 126 | self.x_goal = x_goal 127 | self.x_init = x_init 128 | # 129 | self.build() 130 | 131 | def build(self): 132 | """ 133 | Compute internal matrices defining the preview QP. 134 | 135 | Notes 136 | ----- 137 | See [Audren14]_ for details on the matrices :math:`\\Phi` and 138 | :math:`\\Psi`, as we use similar notations below. 139 | """ 140 | t_build_start = time() 141 | phi = eye(self.x_dim) 142 | psi = zeros((self.x_dim, self.U_dim)) 143 | G_list, h_list = [], [] 144 | phi_list, psi_list = [], [] 145 | for k in range(self.nb_steps): 146 | # Loop invariant: x == psi * U + phi * x_init 147 | if self.wxc is not None: 148 | phi_list.append(phi) 149 | psi_list.append(psi) 150 | C = self.C[k] if type(self.C) is list else self.C 151 | D = self.D[k] if type(self.D) is list else self.D 152 | e = self.e[k] if type(self.e) is list else self.e 153 | G = zeros((e.shape[0], self.U_dim)) 154 | h = e if C is None else e - dot(dot(C, phi), self.x_init) 155 | if D is not None: 156 | # we rely on G == 0 to avoid a slower += 157 | G[:, k * self.u_dim:(k + 1) * self.u_dim] = D 158 | if C is not None: 159 | G += dot(C, psi) 160 | G_list.append(G) 161 | h_list.append(h) 162 | phi = dot(self.A, phi) 163 | psi = dot(self.A, psi) 164 | psi[:, self.u_dim * k:self.u_dim * (k + 1)] = self.B 165 | P = self.wu * eye(self.U_dim) 166 | q = zeros(self.U_dim) 167 | if self.wxt is not None and self.wxt > 1e-10: 168 | c = dot(phi, self.x_init) - self.x_goal 169 | P += self.wxt * dot(psi.T, psi) 170 | q += self.wxt * dot(c.T, psi) 171 | if self.wxc is not None and self.wxc > 1e-10: 172 | Phi = vstack(phi_list) 173 | Psi = vstack(psi_list) 174 | X_goal = hstack([self.x_goal] * self.nb_steps) 175 | c = dot(Phi, self.x_init) - X_goal 176 | P += self.wxc * dot(Psi.T, Psi) 177 | q += self.wxc * dot(c.T, Psi) 178 | self.P = P 179 | self.q = q 180 | self.G = vstack(G_list) 181 | self.h = hstack(h_list) 182 | self.build_time = time() - t_build_start 183 | 184 | def solve(self, **kwargs): 185 | """ 186 | Compute the series of controls that minimizes the preview QP. 187 | 188 | Parameters 189 | ---------- 190 | solver : string, optional 191 | Name of the QP solver in ``qpsolvers.available_solvers``. 192 | initvals : array, optional 193 | Vector of initial `U` values used to warm-start the QP solver. 194 | """ 195 | t_solve_start = time() 196 | kwargs['sym_proj'] = False # self.P is symmetric 197 | U = solve_qp(self.P, self.q, self.G, self.h, **kwargs) 198 | self.U = U.reshape((self.nb_steps, self.u_dim)) 199 | self.solve_time = time() - t_solve_start 200 | 201 | @property 202 | def X(self): 203 | """ 204 | Series of system states over the preview window. 205 | 206 | Note 207 | ---- 208 | This property is only available after ``solve()`` has been called. 209 | """ 210 | if self.__X is not None: 211 | return self.__X 212 | assert self.U is not None, "you need to solve() the MPC problem first" 213 | X = zeros((self.nb_steps + 1, self.x_dim)) 214 | X[0] = self.x_init 215 | for k in range(self.nb_steps): 216 | X[k + 1] = dot(self.A, X[k]) + dot(self.B, self.U[k]) 217 | self.__X = X 218 | return X 219 | 220 | @property 221 | def solve_and_build_time(self): 222 | """ 223 | Total computation time taken by MPC computations. 224 | """ 225 | return self.build_time + self.solve_time 226 | -------------------------------------------------------------------------------- /pymanoid/nlp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from casadi import MX, nlpsol, vertcat 21 | 22 | 23 | class NonlinearProgram(object): 24 | 25 | """ 26 | Wrapper around `CasADi `_ to 27 | formulate and solve nonlinear optimization problems. 28 | 29 | Parameters 30 | ---------- 31 | solver : string, optional 32 | Solver name. Use 'ipopt' (default) for an interior point method or 33 | 'sqpmethod' for sequential quadratic programming. 34 | options : dict, optional 35 | Dictionary of solver options. 36 | """ 37 | 38 | infty = 1e10 39 | 40 | casadi_expand = True 41 | """ 42 | Replace MX with SX expressions in problem formulation. 43 | 44 | Note 45 | ---- 46 | Setting this option to ``True`` seems to significantly improve computation 47 | times when using IPOPT. 48 | """ 49 | 50 | """ 51 | IPOPT options 52 | ============= 53 | """ 54 | 55 | ipopt_fast_step_computation = 'yes' # default: 'no' 56 | """ 57 | If set to yes, the algorithm assumes that the linear system that is solved 58 | to obtain the search direction, is solved sufficiently well. In that case, 59 | no residuals are computed, and the computation of the search direction is a 60 | little faster. 61 | """ 62 | 63 | ipopt_fixed_variable_treatment = 'relax_bounds' 64 | """ 65 | Default is "make_parameter", but "relax_bounds" seems to make computations 66 | faster and numerically stabler. 67 | """ 68 | 69 | ipopt_linear_solver = 'ma27' 70 | """ 71 | Linear solver used for step computations. 72 | """ 73 | 74 | ipopt_max_cpu_time = 10. # [s] 75 | """ 76 | Maximum number of CPU seconds. Note that this parameter corresponds to 77 | processor time, not wall time. For a CPU with N cores, the latter can be as 78 | much as N times lower than the former. 79 | """ 80 | 81 | ipopt_max_iter = 1000 # default is 3000 82 | """ 83 | Maximum number of iterations. 84 | """ 85 | 86 | ipopt_mu_strategy = "adaptive" # default is "monotone" 87 | """ 88 | Update strategy for barrier parameter: "monotone" (default) or "adaptive". 89 | """ 90 | 91 | ipopt_nlp_lower_bound_inf = -0.9 * infty 92 | """ 93 | Any bound below this value will be considered -inf, i.e. not lower bounded. 94 | """ 95 | 96 | ipopt_nlp_upper_bound_inf = +0.9 * infty 97 | """ 98 | Any bound above this value will be considered +inf, i.e. not upper bounded. 99 | """ 100 | 101 | ipopt_print_level = 0 # default: 5 102 | """ 103 | Output verbosity level between 0 and 12. 104 | """ 105 | 106 | ipopt_print_time = False # default: True 107 | """ 108 | Print detailed solver computation times. 109 | """ 110 | 111 | ipopt_warm_start_init_point = 'yes' # default: 'no' 112 | """ 113 | Indicates whether the optimization should use warm start initialization, 114 | where values of primal and dual variables are given (e.g. from a previous 115 | optimization of a related problem). 116 | """ 117 | 118 | def __init__(self, solver='ipopt', options=None): 119 | if options is not None: 120 | for option in options: 121 | self.__setattr__('ipopt_%s' % option, options[option]) 122 | self.cons_exprs = [] 123 | self.cons_index = {} 124 | self.cons_lbounds = [] 125 | self.cons_ubounds = [] 126 | self.cost_function = 0 127 | self.initvals = [] 128 | self.solver = None 129 | self.solver_name = solver 130 | self.var_index = {} 131 | self.var_lbounds = [] 132 | self.var_symbols = [] 133 | self.var_ubounds = [] 134 | 135 | def extend_cost(self, expr): 136 | """ 137 | Add a new expression term to the cost function of the problem. 138 | 139 | Parameters 140 | ---------- 141 | expr : casadi.MX 142 | Python or CasADi symbolic expression. 143 | """ 144 | self.cost_function = self.cost_function + expr 145 | 146 | def warm_start(self, initvals): 147 | """ 148 | Warm-start the problem with a new vector of initial values. 149 | 150 | Parameters 151 | ---------- 152 | initvals : array 153 | Vector of initial values for all problem variables. 154 | """ 155 | assert len(self.initvals) == len(initvals) 156 | self.initvals = list(initvals) 157 | 158 | def new_variable(self, name, dim, init, lb, ub): 159 | """ 160 | Add a new variable to the problem. 161 | 162 | Parameters 163 | ---------- 164 | name : string 165 | Variable name. 166 | dim : int 167 | Number of dimensions. 168 | init : array, shape=(dim,) 169 | Initial values. 170 | lb : array, shape=(dim,) 171 | Vector of lower bounds on variable values. 172 | ub : array, shape=(dim,) 173 | Vector of upper bounds on variable values. 174 | """ 175 | assert len(init) == len(lb) == len(ub) == dim 176 | var = MX.sym(name, dim) 177 | self.var_symbols.append(var) 178 | self.initvals += init 179 | self.var_index[name] = len(self.var_lbounds) 180 | self.var_lbounds += lb 181 | self.var_ubounds += ub 182 | return var 183 | 184 | def new_constant(self, name, dim, value): 185 | """ 186 | Add a new constant to the problem. 187 | 188 | Parameters 189 | ---------- 190 | name : string 191 | Name of the constant. 192 | dim : int 193 | Number of dimensions. 194 | value : array, shape=(dim,) 195 | Value of the constant. 196 | 197 | Note 198 | ---- 199 | Constants are implemented as variables with matching lower and upper 200 | bounds. 201 | """ 202 | return self.new_variable(name, dim, init=value, lb=value, ub=value) 203 | 204 | def update_constant(self, name, value): 205 | """ 206 | Update the value of an existing constant. 207 | 208 | Parameters 209 | ---------- 210 | name : string 211 | Name of the constant. 212 | value : array, shape=(dim,) 213 | Vector of new values for the constant. 214 | """ 215 | i = self.var_index[name] 216 | for j, v in enumerate(value): 217 | self.initvals[i + j] = v 218 | self.var_lbounds[i + j] = v 219 | self.var_ubounds[i + j] = v 220 | 221 | def update_variable_bounds(self, name, lb, ub): 222 | """ 223 | Update the lower- and upper-bounds on an existing variable. 224 | 225 | Parameters 226 | ---------- 227 | name : string 228 | Name of the variable. 229 | lb : array, shape=(dim,) 230 | Vector of lower bounds on variable values. 231 | ub : array, shape=(dim,) 232 | Vector of upper bounds on variable values. 233 | """ 234 | assert len(lb) == len(ub) 235 | i = self.var_index[name] 236 | for j in range(len(lb)): 237 | self.var_lbounds[i + j] = lb[j] 238 | self.var_ubounds[i + j] = ub[j] 239 | 240 | def add_constraint(self, expr, lb, ub, name=None): 241 | """ 242 | Add a new constraint to the problem. 243 | 244 | Parameters 245 | ---------- 246 | expr : casadi.MX 247 | Python or CasADi symbolic expression. 248 | lb : array 249 | Lower-bound on the expression. 250 | ub : array 251 | Upper-bound on the expression. 252 | name : string, optional 253 | If provided, will stored the expression under this name for future 254 | updates. 255 | """ 256 | self.cons_exprs.append(expr) 257 | if name is not None: 258 | self.cons_index[name] = len(self.cons_lbounds) 259 | self.cons_lbounds += lb 260 | self.cons_ubounds += ub 261 | 262 | def add_equality_constraint(self, expr1, expr2, name=None): 263 | """ 264 | Add an equality constraint between two expressions. 265 | 266 | Parameters 267 | ---------- 268 | expr1 : casadi.MX 269 | Expression on problem variables. 270 | expr2 : casadi.MX 271 | Expression on problem variables. 272 | name : string, optional 273 | If provided, will stored the expression under this name for future 274 | updates. 275 | """ 276 | diff = expr1 - expr2 277 | dim = diff.shape[0] 278 | assert diff.shape[1] == 1 279 | zeros = [0.] * dim 280 | self.add_constraint(diff, zeros, zeros, name) 281 | 282 | def has_constraint(self, name): 283 | """ 284 | Check if a given name identifies a problem constraint. 285 | """ 286 | return name in self.cons_index 287 | 288 | def update_constraint_bounds(self, name, lb, ub): 289 | """ 290 | Update lower- and upper-bounds on an existing constraint. 291 | 292 | Parameters 293 | ---------- 294 | name : string 295 | Identifier of the constraint. 296 | lb : array 297 | New lower-bound of the constraint. 298 | ub : array 299 | New upper-bound of the constraint. 300 | """ 301 | i = self.cons_index[name] 302 | for j in range(len(lb)): 303 | self.cons_lbounds[i + j] = lb[j] 304 | self.cons_ubounds[i + j] = ub[j] 305 | 306 | def create_solver(self): 307 | """ 308 | Create a new nonlinear solver. 309 | """ 310 | problem = { 311 | 'f': self.cost_function, 312 | 'x': vertcat(*self.var_symbols), 313 | 'g': vertcat(*self.cons_exprs)} 314 | options = {} 315 | if self.solver_name in ['scpgen', 'sqpmethod']: 316 | options.update({ 317 | 'qpsol': "qpoases", 318 | 'qpsol_options': {"printLevel": "none"}, 319 | }) 320 | elif self.solver_name == 'ipopt': 321 | options.update({ 322 | 'expand': self.casadi_expand, 323 | 'ipopt.fast_step_computation': 324 | self.ipopt_fast_step_computation, 325 | 'ipopt.fixed_variable_treatment': 326 | self.ipopt_fixed_variable_treatment, 327 | 'ipopt.linear_solver': self.ipopt_linear_solver, 328 | 'ipopt.max_cpu_time': self.ipopt_max_cpu_time, 329 | 'ipopt.max_iter': self.ipopt_max_iter, 330 | 'ipopt.mu_strategy': self.ipopt_mu_strategy, 331 | 'ipopt.nlp_lower_bound_inf': self.ipopt_nlp_lower_bound_inf, 332 | 'ipopt.nlp_upper_bound_inf': self.ipopt_nlp_upper_bound_inf, 333 | 'ipopt.print_level': self.ipopt_print_level, 334 | 'ipopt.warm_start_init_point': 335 | self.ipopt_warm_start_init_point, 336 | 'print_time': self.ipopt_print_time, 337 | 'verbose': False, 338 | 'verbose_init': False, 339 | }) 340 | self.solver = nlpsol('solver', self.solver_name, problem, options) 341 | 342 | def solve(self): 343 | """ 344 | Call the nonlinear solver. 345 | 346 | Returns 347 | ------- 348 | x : array 349 | Vector of variable coordinates for the best solution found. 350 | """ 351 | self.res = self.solver( 352 | x0=self.initvals, lbx=self.var_lbounds, ubx=self.var_ubounds, 353 | lbg=self.cons_lbounds, ubg=self.cons_ubounds) 354 | return self.res['x'].full().flatten() 355 | 356 | @property 357 | def iter_count(self): 358 | """ 359 | Number of LP solver iterations applied by the NLP solver. 360 | """ 361 | return self.solver.stats()['iter_count'] 362 | 363 | @property 364 | def optimal_found(self): 365 | """ 366 | `True` if and only if the solution is a local optimum. 367 | """ 368 | return self.return_status == "Solve_Succeeded" 369 | 370 | @property 371 | def return_status(self): 372 | """ 373 | String containing a status message from the NLP solver. 374 | """ 375 | return self.solver.stats()['return_status'] 376 | 377 | @property 378 | def solve_time(self): 379 | """ 380 | Time (in seconds) taken to solve the problem. 381 | """ 382 | return self.solver.stats()['t_wall_mainloop'] 383 | -------------------------------------------------------------------------------- /pymanoid/proc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from numpy import zeros 21 | 22 | 23 | class Process(object): 24 | 25 | """ 26 | Processes implement the ``on_tick`` method called by the simulation. 27 | """ 28 | 29 | def __init__(self): 30 | self.log_comp_times = False 31 | self.is_paused = False 32 | 33 | def on_tick(self, sim): 34 | """ 35 | Main function called by the simulation at each control cycle. 36 | 37 | Parameters 38 | ---------- 39 | sim : Simulation 40 | Current simulation instance. 41 | """ 42 | raise NotImplementedError("should be implemented by child class") 43 | 44 | def pause(self): 45 | """ 46 | Stop calling the process at new clock ticks. 47 | """ 48 | self.is_paused = True 49 | 50 | def resume(self): 51 | """ 52 | Resume calling the process at new clock ticks. 53 | """ 54 | self.is_paused = False 55 | 56 | 57 | class CameraRecorder(Process): 58 | 59 | """ 60 | Video recording process. 61 | 62 | When created, this process will ask the user to click on the OpenRAVE GUI 63 | to get its window ID. Then, it will save a screenshot in the camera folder 64 | at each tick of the simulation (don't expect real-time recording...). When 65 | your simulation is over, go to the camera folder and run the script called 66 | ``make_video.sh``. 67 | 68 | Parameters 69 | ---------- 70 | sim : Simulation 71 | Simulation instance. 72 | fname : string, optional 73 | Video file name. 74 | tmp_folder : string, optional 75 | Temporary folder where screenshots will be recorded. 76 | 77 | Note 78 | ---- 79 | Creating videos requires the following dependencies (here listed for Ubuntu 80 | 14.04): ``sudo apt-get install x11-utils imagemagick libav-tools``. 81 | 82 | Note 83 | ---- 84 | Don't expect the simulation to run real-time while recording. 85 | 86 | Note 87 | ---- 88 | The GUI window should stay visible on your screen for the whole duration of 89 | the recording. Also, don't resize it, otherwise video conversion will fail 90 | later on. 91 | """ 92 | 93 | def __init__(self, sim, fname=None, tmp_folder='pymanoid_rec'): 94 | super(CameraRecorder, self).__init__() 95 | if fname is None: 96 | now = datetime.datetime.now() 97 | fname = now.strftime('pymanoid-%Y-%m-%d-%H%M%S.mp4') 98 | while tmp_folder.endswith('/'): 99 | tmp_folder = tmp_folder[:-1] 100 | sim.get_viewer_window_id() 101 | if not os.path.exists(tmp_folder): 102 | os.makedirs(tmp_folder) 103 | script_name = 'make_pymanoid_video.sh' 104 | with open(script_name, 'w') as script: 105 | frate = int(1. / sim.dt) 106 | script.write( 107 | ("#!/bin/sh\n") + 108 | (("avconv -r %d" % frate) + 109 | (" -i %s/%%05d.png" % tmp_folder) + 110 | (" -vf crop=\"trunc(iw/2)*2:trunc(ih/2)*2:0:0\"") + 111 | (" %s" % fname)) + 112 | (" && rm -rf %s" % tmp_folder) + 113 | (" && rm %s" % script_name)) 114 | st = fstat(script_name) 115 | chmod(script_name, st.st_mode | S_IEXEC) 116 | self.frame_index = 0 117 | self.sim = sim 118 | self.tmp_folder = tmp_folder 119 | 120 | def on_tick(self, sim): 121 | """ 122 | Main function called by the simulation at each control cycle. 123 | 124 | Parameters 125 | ---------- 126 | sim : Simulation 127 | Current simulation instance. 128 | """ 129 | fname = '%s/%05d.png' % (self.tmp_folder, self.frame_index) 130 | sim.take_screenshot(fname) 131 | self.frame_index += 1 132 | 133 | def wait_for(self, wait_time): 134 | """ 135 | Pause the video by repeating the last frame for a certain duration. 136 | 137 | Parameters 138 | ---------- 139 | wait_time : scalar 140 | Duration in [s]. 141 | """ 142 | for _ in range(int(wait_time / self.sim.dt)): 143 | self.on_tick(self.sim) 144 | 145 | 146 | class JointRecorder(Process): 147 | 148 | """ 149 | Record joint trajectories (position, velocity, acceleration, torque) from 150 | the robot model. 151 | 152 | Parameters 153 | ---------- 154 | robot : Robot 155 | Target robot state to record from. 156 | """ 157 | 158 | def __init__(self, robot): 159 | super(JointRecorder, self).__init__() 160 | self.q = [robot.q] 161 | self.qd = [robot.qd] 162 | self.qdd = [zeros(robot.q.shape)] 163 | self.robot = robot 164 | self.tau = [zeros(robot.q.shape)] 165 | self.times = [0.] 166 | 167 | def on_tick(self, sim): 168 | """ 169 | Main function called by the simulation at each control cycle. 170 | 171 | Parameters 172 | ---------- 173 | sim : Simulation 174 | Current simulation instance. 175 | """ 176 | qd_prev = self.qd[-1] 177 | qdd = (self.robot.qd - qd_prev) / sim.dt 178 | tm, tc, tg = self.robot.compute_inverse_dynamics(qdd) 179 | self.q.append(self.robot.q) 180 | self.qd.append(self.robot.qd) 181 | self.qdd.append(qdd) 182 | self.tau.append(tm + tc + tg) 183 | self.times.append(sim.time) 184 | 185 | def plot(self, dofs=None): 186 | """ 187 | Plot recorded joint trajectories. 188 | 189 | Parameters 190 | ---------- 191 | dofs : list of integers 192 | DOF indices to plot trajectories for, e.g. ``robot.left_leg``. 193 | """ 194 | import pylab 195 | if dofs is None: 196 | dofs = range(self.robot.nb_dofs) 197 | plots = [ 198 | (self.q, "Joint angles [rad]"), 199 | (self.qd, "Joint velocities [rad/s]"), 200 | (self.qdd, "Joint accelerations [rad/s^2]"), 201 | (self.tau, "Joint torques [Nm]")] 202 | for i, plot in enumerate(plots): 203 | traj, ylabel = plot 204 | pylab.subplot(411 + i) 205 | pylab.plot(self.times, [v[dofs] for v in traj]) 206 | pylab.grid(True) 207 | pylab.xlabel('Time [s]') 208 | pylab.ylabel(ylabel) 209 | pylab.xlim(0., self.times[-1]) 210 | -------------------------------------------------------------------------------- /pymanoid/robots/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from .hrp4 import HRP4 21 | from .jvrc1 import JVRC1 22 | 23 | __all__ = ['HRP4', 'JVRC1'] 24 | -------------------------------------------------------------------------------- /pymanoid/robots/hrp4.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from numpy import array, pi 21 | 22 | from ..body import Manipulator 23 | from ..robot import Humanoid 24 | from ..tasks import DOFTask, MinCAMTask 25 | 26 | 27 | class HRP4(Humanoid): 28 | 29 | """ 30 | HRP-4 (HRP4R) humanoid robot. 31 | 32 | This file includes information that is publicly released in 33 | or over the Web for the total 34 | mass and kinematic chain. Other information is read from the COLLADA model. 35 | """ 36 | 37 | leg_length = 0.8 # [m] (roughly, for a strechted leg) 38 | mass = 39. # [kg] (includes batteries) 39 | sole_shape = (0.11, 0.05) # (half-length [m], half-width [m]) of foot sole 40 | palm_shape = (0.03, 0.04) # (half-length [m], half-width [m]) of palm surf 41 | 42 | # DOF indexes with respect to COLLADA model 43 | L_F53 = 0 44 | L_F52 = 1 45 | L_F43 = 2 46 | L_F42 = 3 47 | L_F33 = 4 48 | L_F32 = 5 49 | L_F23 = 6 50 | L_F22 = 7 51 | R_F53 = 8 52 | R_F52 = 9 53 | R_F43 = 10 54 | R_F42 = 11 55 | R_F33 = 12 56 | R_F32 = 13 57 | R_F23 = 14 58 | R_F22 = 15 59 | R_HIP_Y = 16 60 | R_HIP_R = 17 61 | R_HIP_P = 18 62 | R_KNEE_P = 19 63 | R_ANKLE_P = 20 64 | R_ANKLE_R = 21 65 | L_HIP_Y = 22 66 | L_HIP_R = 23 67 | L_HIP_P = 24 68 | L_KNEE_P = 25 69 | L_ANKLE_P = 26 70 | L_ANKLE_R = 27 71 | CHEST_P = 28 72 | WAIST_P = 28 # compatibility with JVRC-1 73 | CHEST_Y = 29 74 | WAIST_Y = 29 # compatibility with JVRC-1 75 | NECK_Y = 30 76 | NECK_P = 31 77 | R_SHOULDER_P = 32 78 | R_SHOULDER_R = 33 79 | R_SHOULDER_Y = 34 80 | R_ELBOW_P = 35 81 | R_WRIST_Y = 36 82 | R_WRIST_P = 37 83 | R_WRIST_R = 38 84 | R_HAND_J0 = 39 85 | R_HAND_J1 = 40 86 | L_SHOULDER_P = 41 87 | L_SHOULDER_R = 42 88 | L_SHOULDER_Y = 43 89 | L_ELBOW_P = 44 90 | L_WRIST_Y = 45 91 | L_WRIST_P = 46 92 | L_WRIST_R = 47 93 | L_HAND_J0 = 48 94 | L_HAND_J1 = 49 95 | TRANS_X = 50 96 | TRANS_Y = 51 97 | TRANS_Z = 52 98 | ROT_Y = 53 99 | ROT_P = 54 100 | ROT_R = 55 101 | 102 | # Joints 103 | chest = [CHEST_P, CHEST_Y] 104 | free_pos = [TRANS_X, TRANS_Y, TRANS_Z] 105 | free_rpy = [ROT_R, ROT_P, ROT_Y] 106 | left_ankle = [L_ANKLE_P, L_ANKLE_R] 107 | left_elbow = [L_ELBOW_P] 108 | left_hip = [L_HIP_Y, L_HIP_R, L_HIP_P] 109 | left_knee = [L_KNEE_P] 110 | left_shoulder = [L_SHOULDER_P, L_SHOULDER_R, L_SHOULDER_Y] 111 | left_thumb = [L_HAND_J0, L_HAND_J1] 112 | left_wrist = [L_WRIST_Y, L_WRIST_P, L_WRIST_R] 113 | neck = [NECK_Y, NECK_P] 114 | right_ankle = [R_ANKLE_P, R_ANKLE_R] 115 | right_elbow = [R_ELBOW_P] 116 | right_hip = [R_HIP_Y, R_HIP_R, R_HIP_P] 117 | right_knee = [R_KNEE_P] 118 | right_shoulder = [R_SHOULDER_P, R_SHOULDER_R, R_SHOULDER_Y] 119 | right_thumb = [R_HAND_J0, R_HAND_J1] 120 | right_wrist = [R_WRIST_Y, R_WRIST_P, R_WRIST_R] 121 | 122 | # Limbs 123 | left_arm = left_shoulder + left_elbow + left_wrist 124 | left_leg = left_hip + left_knee + left_ankle 125 | right_arm = right_shoulder + right_elbow + right_wrist 126 | right_leg = right_hip + right_knee + right_ankle 127 | 128 | # Body 129 | arms = left_arm + right_arm 130 | free = free_pos + free_rpy 131 | legs = left_leg + right_leg 132 | upper_body = arms + chest 133 | whole_body = arms + legs + chest + free 134 | 135 | # Half-sitting posture 136 | q_halfsit = pi / 180 * array( 137 | [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 138 | 0., 0., -0.76, -22.02, 41.29, -18.75, -0.45, 0., 1.15, -21.89, 139 | 41.21, -18.74, -1.10, 8., 0., 0., 0., -3., -10., 0., -30., 0., 140 | 0., 0., 0., 0., -3., 10., 0., -30., 0., 0., 0., 0., 0., 0., 0., 0., 141 | 0., 0., 0.]) 142 | 143 | # Compatibility 144 | chest_p_name = 'CHEST_P' 145 | chest_y_name = 'CHEST_Y' 146 | 147 | def __init__(self, path='HRP4R.dae', root_body='BODY'): 148 | """ 149 | Add the HRP4R model to the environment. 150 | 151 | Parameters 152 | ---------- 153 | path : string 154 | Path to the COLLADA model of the robot. 155 | root_body : string 156 | Name of the root body in the kinematic chain. 157 | 158 | Note 159 | ---- 160 | Unfortunately it is unclear whether we can release the COLLADA file 161 | ``HRP4R.dae`` (md5sum: 38c30928b7ae62daa0fc67ed0488b0a1) due to 162 | copyright. 163 | """ 164 | super(HRP4, self).__init__(path, root_body) 165 | self.ik.set_active_dofs(self.whole_body) 166 | self.mass = sum([link.GetMass() for link in self.rave.GetLinks()]) 167 | self.left_foot = Manipulator( 168 | self.rave.GetManipulator("LeftFootCenter"), shape=self.sole_shape, 169 | friction=0.8) 170 | self.left_hand = Manipulator( 171 | self.rave.GetManipulator("LeftHandCenter"), shape=self.palm_shape, 172 | friction=0.8) 173 | self.right_foot = Manipulator( 174 | self.rave.GetManipulator("RightFootCenter"), shape=self.sole_shape, 175 | friction=0.8) 176 | self.right_hand = Manipulator( 177 | self.rave.GetManipulator("RightHandCenter"), shape=self.palm_shape, 178 | friction=0.8) 179 | 180 | def add_shoulder_abduction_task(self, weight=None): 181 | self.ik.add(DOFTask(self, self.R_SHOULDER_R, -0.4, weight)) 182 | self.ik.add(DOFTask(self, self.L_SHOULDER_R, +0.4, weight)) 183 | 184 | def add_shoulder_extension_task(self, weight=None): 185 | self.ik.add(DOFTask(self, self.L_SHOULDER_P, +0.5, weight)) 186 | self.ik.add(DOFTask(self, self.R_SHOULDER_P, +0.5, weight)) 187 | 188 | def add_shoulder_flexion_task(self, weight=None): 189 | self.ik.add(DOFTask(self, self.L_SHOULDER_P, -0.5, weight)) 190 | self.ik.add(DOFTask(self, self.R_SHOULDER_P, -0.5, weight)) 191 | 192 | def add_shoulder_neutral_pitch_task(self, weight=None): 193 | self.ik.add(DOFTask(self, self.L_SHOULDER_P, 0., weight)) 194 | self.ik.add(DOFTask(self, self.R_SHOULDER_P, 0., weight)) 195 | 196 | def add_upright_chest_task(self, weight=None): 197 | self.ik.add(DOFTask(self, self.ROT_P, 0., weight)) 198 | self.ik.add(DOFTask(self, self.CHEST_P, 0.2, weight)) 199 | self.ik.add(DOFTask(self, self.CHEST_Y, 0., weight)) 200 | 201 | def setup_ik_for_walking(self, com_target): 202 | """ 203 | Prepare inverse-kinematic tracking for locomotion. 204 | 205 | Parameters 206 | ---------- 207 | com_target : pymanoid.PointMass 208 | Target for the IK task on the center of mass. 209 | """ 210 | self.ik.tasks['COM'].update_target(com_target) 211 | self.ik.add(MinCAMTask(self)) 212 | self.add_upright_chest_task() 213 | self.add_shoulder_neutral_pitch_task() 214 | self.ik.set_weights({ 215 | self.left_foot.name: 1., 216 | self.right_foot.name: 1., 217 | 'COM': 1e-2, 218 | 'MIN_CAM': 1e-4, 219 | 'ROT_P': 1e-4, 220 | 'CHEST_P': 1e-4, 221 | 'CHEST_Y': 1e-4, 222 | 'L_SHOULDER_P': 1e-5, 223 | 'R_SHOULDER_P': 1e-5, 224 | }) 225 | 226 | def suntan(self, amount=0.3): 227 | """ 228 | Prepare model for screenshots on white background ;) 229 | 230 | Parameters 231 | ---------- 232 | amount : scalar 233 | Number between 0. (no suntan) and 1. (full burn). 234 | """ 235 | ambient, diffuse = 0., 1. - amount 236 | for link in self.rave.GetLinks(): 237 | if len(link.GetGeometries()) > 0: 238 | geom = link.GetGeometries()[0] 239 | geom.SetAmbientColor([ambient] * 3) 240 | geom.SetDiffuseColor([diffuse] * 3) 241 | -------------------------------------------------------------------------------- /pymanoid/robots/jvrc1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from numpy import array 21 | from os import system 22 | from os.path import isfile 23 | 24 | from ..body import Manipulator 25 | from ..robot import Humanoid 26 | 27 | 28 | class JVRC1(Humanoid): 29 | 30 | """ 31 | JVRC-1 robot model. 32 | 33 | See . 34 | """ 35 | 36 | MODEL_URL = 'https://raw.githubusercontent.com/stephane-caron/' \ 37 | 'openrave_models/master/JVRC-1/JVRC-1.dae' 38 | 39 | sole_shape = (0.11, 0.05) # (half-length [m], half-width [m]) of foot sole 40 | leg_length = 0.85 # [m] (for a stretched leg) 41 | mass = 62. # [kg] 42 | 43 | # DOF indices 44 | R_HIP_P = 0 45 | R_HIP_R = 1 46 | R_HIP_Y = 2 47 | R_KNEE_P = 3 48 | R_ANKLE_R = 4 49 | R_ANKLE_P = 5 50 | L_HIP_P = 6 51 | L_HIP_R = 7 52 | L_HIP_Y = 8 53 | L_KNEE_P = 9 54 | L_ANKLE_R = 10 55 | L_ANKLE_P = 11 56 | CHEST_Y = 12 # compatibility with HRP-4 57 | WAIST_Y = 12 58 | CHEST_P = 13 # compatibility with HRP-4 59 | WAIST_P = 13 60 | WAIST_R = 14 61 | NECK_Y = 15 62 | NECK_R = 16 63 | NECK_P = 17 64 | R_SHOULDER_P = 18 65 | R_SHOULDER_R = 19 66 | R_SHOULDER_Y = 20 67 | R_ELBOW_P = 21 68 | R_ELBOW_Y = 22 69 | R_WRIST_R = 23 70 | R_WRIST_Y = 24 71 | R_UTHUMB = 25 72 | R_LTHUMB = 26 73 | R_UINDEX = 27 74 | R_LINDEX = 28 75 | R_ULITTLE = 29 76 | R_LLITTLE = 30 77 | L_SHOULDER_P = 31 78 | L_SHOULDER_R = 32 79 | L_SHOULDER_Y = 33 80 | L_ELBOW_P = 34 81 | L_ELBOW_Y = 35 82 | L_WRIST_R = 36 83 | L_WRIST_Y = 37 84 | L_UTHUMB = 38 85 | L_LTHUMB = 39 86 | L_UINDEX = 40 87 | L_LINDEX = 41 88 | L_ULITTLE = 42 89 | L_LLITTLE = 43 90 | TRANS_X = 44 91 | TRANS_Y = 45 92 | TRANS_Z = 46 93 | ROT_Y = 47 94 | ROT_P = 48 95 | ROT_R = 49 96 | 97 | # Joints 98 | chest = [WAIST_Y, WAIST_P, WAIST_R] 99 | free_pos = [TRANS_X, TRANS_Y, TRANS_Z] 100 | free_rpy = [ROT_R, ROT_P, ROT_Y] 101 | left_ankle = [L_ANKLE_R, L_ANKLE_P] 102 | left_elbow = [L_ELBOW_P, L_ELBOW_Y] 103 | left_hip = [L_HIP_P, L_HIP_R, L_HIP_Y] 104 | left_index = [L_UINDEX, L_LINDEX] 105 | left_knee = [L_KNEE_P] 106 | left_little = [L_ULITTLE, L_LLITTLE] 107 | left_shoulder = [L_SHOULDER_P, L_SHOULDER_R, L_SHOULDER_Y] 108 | left_thumb = [L_UTHUMB, L_LTHUMB] 109 | left_wrist = [L_WRIST_R, L_WRIST_Y] 110 | neck = [NECK_Y, NECK_R, NECK_P] 111 | right_ankle = [R_ANKLE_R, R_ANKLE_P] 112 | right_elbow = [R_ELBOW_P, R_ELBOW_Y] 113 | right_hip = [R_HIP_P, R_HIP_R, R_HIP_Y] 114 | right_index = [R_UINDEX, R_LINDEX] 115 | right_knee = [R_KNEE_P] 116 | right_little = [R_ULITTLE, R_LLITTLE] 117 | right_shoulder = [R_SHOULDER_P, R_SHOULDER_R, R_SHOULDER_Y] 118 | right_thumb = [R_UTHUMB, R_LTHUMB] 119 | right_wrist = [R_WRIST_R, R_WRIST_Y] 120 | 121 | # Limbs 122 | left_arm = left_shoulder + left_elbow + left_wrist 123 | left_hand = left_thumb + left_index + left_little 124 | left_leg = left_hip + left_knee + left_ankle 125 | right_arm = right_shoulder + right_elbow + right_wrist 126 | right_hand = right_thumb + right_index + right_little 127 | right_leg = right_hip + right_knee + right_ankle 128 | 129 | # Body 130 | arms = left_arm + right_arm 131 | free = free_pos + free_rpy 132 | legs = left_leg + right_leg 133 | whole_body = arms + legs + chest + free 134 | 135 | # Half-sitting posture 136 | q_halfsit = array([ 137 | -0.38, -0.01, 0., 0.72, -0.01, -0.33, -0.38, 0.02, 0., 0.72, -0.02, 138 | -0.33, 0., 0.13, 0., 0., 0., 0., -0.052, -0.17, 0., -0.52, 0., 0., 0., 139 | 0., 0., 0., 0., 0., 0., -0.052, 0.17, 0., -0.52, 0., 0., 0., 0., 0., 140 | 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]) 141 | 142 | # Compatibility 143 | chest_p_name = 'WAIST_P' 144 | chest_y_name = 'WAIST_Y' 145 | 146 | def __init__(self, path='JVRC-1.dae', root_body='PELVIS_S', 147 | download_if_needed=True): 148 | """ 149 | Add the JVRC-1 model to the environment. 150 | 151 | Parameters 152 | ---------- 153 | path : string, optional 154 | Path to the COLLADA model of the robot. 155 | root_body : string, optional 156 | Name of the root body in the kinematic chain. 157 | download_if_needed : bool, optional 158 | If True and there is no model file in ``path``, will attempt to 159 | download it from ``JVRC1.MODEL_URL``. 160 | """ 161 | if download_if_needed and not isfile(path): 162 | rc = system('wget %s -O %s' % (JVRC1.MODEL_URL, path)) 163 | assert rc == 0, "Download of model file failed" 164 | super(JVRC1, self).__init__(path, root_body) 165 | self.ik.set_active_dofs(self.whole_body) 166 | self.left_foot = Manipulator( 167 | self.rave.GetManipulator("left_foot_base"), shape=self.sole_shape, 168 | friction=0.8) 169 | self.left_hand = Manipulator( 170 | self.rave.GetManipulator("left_hand_palm"), friction=0.8) 171 | self.mass = sum([link.GetMass() for link in self.rave.GetLinks()]) 172 | self.right_foot = Manipulator( 173 | self.rave.GetManipulator("right_foot_base"), shape=self.sole_shape, 174 | friction=0.8) 175 | self.right_hand = Manipulator( 176 | self.rave.GetManipulator("right_hand_palm"), friction=0.8) 177 | -------------------------------------------------------------------------------- /pymanoid/swing_foot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from numpy import array, dot, eye, hstack, linspace, zeros 21 | from openravepy import InterpolateQuatSlerp as quat_slerp 22 | from qpsolvers import solve_qp 23 | 24 | from .gui import draw_trajectory 25 | from .interp import interpolate_cubic_hermite 26 | 27 | 28 | def factor_cubic_hermite_curve(p0, n0, p1, n1): 29 | """ 30 | Let `H` denote the Hermite curve (parameterized by `\\lambda` and `\\mu`) 31 | such that: 32 | 33 | .. math:: 34 | 35 | \\begin{array}{rcl} 36 | H(0) & = & p_0 \\\\ 37 | H'(0) & = & \\lambda n_0 \\\\ 38 | H(1) & = & p_1 \\\\ 39 | H'(1) & = & \\mu n_1 \\\\ 40 | \\end{array} 41 | 42 | This function returns the three factors of `H` corresponding to the symbols 43 | :math:`\\lambda`, :math:`\\mu` and :math:`1` (constant part). 44 | 45 | Parameters 46 | ---------- 47 | p0 : (3,) array 48 | Initial position. 49 | n0 : (3,) array 50 | Initial tangent vector. 51 | p1 : (3,) array 52 | Final position. 53 | n1 : (3,) array 54 | Final tangent vector. 55 | 56 | Returns 57 | ------- 58 | H_lambda : function 59 | Factor of :math:`\\lambda` in `H`. 60 | H_mu : function 61 | Factor of :math:`\\mu` in `H`. 62 | H_cst : function 63 | Part of `H` that depends neither on :math:`\\lambda` nor :math:`\\mu`. 64 | """ 65 | def H_lambda(s): 66 | return s * (1 + s * (s - 2)) * n0 67 | 68 | def H_mu(s): 69 | return s ** 2 * (s - 1) * n1 70 | 71 | def H_cst(s): 72 | return p0 + s ** 2 * (3 - 2 * s) * (p1 - p0) 73 | 74 | return H_lambda, H_mu, H_cst 75 | 76 | 77 | class SwingFoot(object): 78 | 79 | """ 80 | Polynomial swing foot interpolator. 81 | 82 | Parameters 83 | ---------- 84 | start_contact : pymanoid.Contact 85 | Initial contact. 86 | end_contact : pymanoid.Contact 87 | Target contact. 88 | duration : scalar 89 | Swing duration in [s]. 90 | takeoff_clearance : scalar, optional 91 | Takeoff clearance height at 1/4th of the interpolated trajectory. 92 | landing_clearance : scalar, optional 93 | Landing clearance height at 3/4th of the interpolated trajectory. 94 | """ 95 | 96 | def __init__(self, start_contact, end_contact, duration, 97 | takeoff_clearance=0.05, landing_clearance=0.05): 98 | self.draw_trajectory = False 99 | self.duration = duration 100 | self.end_contact = end_contact.copy(hide=True) 101 | self.foot_vel = zeros(3) 102 | self.landing_clearance = landing_clearance 103 | self.playback_time = 0. 104 | self.start_contact = start_contact.copy(hide=True) 105 | self.takeoff_clearance = takeoff_clearance 106 | # 107 | self.path = self.interpolate() 108 | 109 | def interpolate(self): 110 | """ 111 | Interpolate swing foot path. 112 | 113 | Returns 114 | ------- 115 | path : pymanoid.NDPolynomial 116 | Polynomial path with index between 0 and 1. 117 | 118 | Notes 119 | ----- 120 | A swing foot trajectory comes under two conflicting imperatives: stay 121 | close to the ground while avoiding collisions. We assume here that the 122 | terrain is uneven but free from obstacles. To avoid robot-ground 123 | collisions, we enforce two clearance distances: one at toe level for 124 | the takeoff contact and the other at heel level for the landing 125 | contact. 126 | 127 | .. figure:: images/swing_foot.png 128 | :align: center 129 | 130 | We interpolate swing foot trajectories as cubic Hermite curves with 131 | four boundary constraints: initial and final positions corresponding to 132 | contact centers, and tangent directions parallel to contact normals. 133 | This leaves two free parameters, corresponding to the norms of the 134 | initial and final tangents, that can be optimized upon. It can be shown 135 | that minimizing these two parameters subject to the clearance 136 | conditions ``a > a_min`` and ``b > b_min`` is a small constrained least 137 | squares problem. 138 | """ 139 | n0 = self.start_contact.n 140 | n1 = self.end_contact.n 141 | p0 = self.start_contact.p 142 | p1 = self.end_contact.p 143 | takeoff_clearance = self.takeoff_clearance 144 | landing_clearance = self.landing_clearance 145 | if hasattr(self.start_contact, 'takeoff_clearance'): 146 | takeoff_clearance = self.start_contact.takeoff_clearance 147 | if hasattr(self.end_contact, 'landing_clearance'): 148 | landing_clearance = self.end_contact.landing_clearance 149 | # H(s) = H_lambda(s) * lambda + H_mu(s) * mu + H_cst(s) 150 | H_lambda, H_mu, H_cst = factor_cubic_hermite_curve(p0, n0, p1, n1) 151 | s0 = 1. / 4 152 | a0 = dot(H_lambda(s0), n0) 153 | b0 = dot(H_mu(s0), n0) 154 | c0 = dot(H_cst(s0) - p0, n0) 155 | h0 = takeoff_clearance 156 | # a0 * lambda + b0 * mu + c0 >= h0 157 | s1 = 3. / 4 158 | a1 = dot(H_lambda(s1), n1) 159 | b1 = dot(H_mu(s1), n1) 160 | c1 = dot(H_cst(s1) - p1, n1) 161 | h1 = landing_clearance 162 | # a1 * lambda + b1 * mu + c1 >= h1 163 | P = eye(2) 164 | q = zeros(2) 165 | G = array([[-a0, -b0], [-a1, -b1]]) 166 | h = array([c0 - h0, c1 - h1]) 167 | x = solve_qp(P, q, G, h) 168 | # H = lambda s: H_lambda(s) * x[0] + H_mu(s) * x[1] + H_cst(s) 169 | path = interpolate_cubic_hermite(p0, x[0] * n0, p1, x[1] * n1) 170 | return path 171 | 172 | def draw(self, color='r'): 173 | """ 174 | Draw swing foot trajectory. 175 | 176 | Parameters 177 | ---------- 178 | color : char or triplet, optional 179 | Color letter or RGB values, default is 'b' for blue. 180 | """ 181 | points = [self.path(s) for s in linspace(0, 1, 10)] 182 | return draw_trajectory(points, color=color) 183 | 184 | def integrate(self, dt): 185 | """ 186 | Integrate swing foot motion forward by a given amount of time. 187 | 188 | Parameters 189 | ---------- 190 | dt : scalar 191 | Duration of forward integration, in [s]. 192 | """ 193 | self.playback_time += dt 194 | s = min(1., self.playback_time / self.duration) 195 | quat = quat_slerp(self.start_contact.quat, self.end_contact.quat, s) 196 | pos = self.path(s) 197 | pose = hstack([quat, pos]) 198 | return pose 199 | -------------------------------------------------------------------------------- /pymanoid/transformations.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | """ 21 | Rotations and rigid-body transformations can be represented in many ways. For 22 | rotations, the three main formats used in pymanoid are: 23 | 24 | - **Roll-pitch-yaw angles:** that is to say Euler angles corresponding to the 25 | sequence (1, 2, 3). 26 | - **Quaternions:** 4D vectors `[w x y z]`, with the scalar term `w` coming 27 | first following the OpenRAVE convention. 28 | - **Rotation matrices:** :math:`3 \\times 3` matrices :math:`R` whose inverse 29 | is equal to their transpose. 30 | 31 | Rigid-body transformations can be represented by: 32 | 33 | - **Poses:** 7D vectors consisting of the quaternion of the orientation 34 | followed by its position. 35 | - **Transformation matrices:** :math:`4 \\times 4` matrices :math:`T`. 36 | 37 | Functions are provided to convert between all these representations. Most of 38 | them are adapted from the comprehensive guide [Diebel06]_. 39 | """ 40 | 41 | from math import asin, atan2, cos, sin 42 | from numpy import array, cross, dot, eye, hstack, zeros 43 | from openravepy import quatFromRotationMatrix as __quatFromRotationMatrix 44 | from openravepy import rotationMatrixFromQuat as __rotationMatrixFromQuat 45 | from openravepy import axisAngleFromQuat as axis_angle_from_quat 46 | 47 | from .misc import norm 48 | 49 | 50 | def apply_transform(T, p): 51 | """ 52 | Apply a transformation matrix `T` to point coordinates `p`. 53 | 54 | Parameters 55 | ---------- 56 | T : (4, 4) array 57 | Homogeneous transformation matrix. 58 | p : (7,) or (3,) array 59 | Pose or point coordinates. 60 | 61 | Returns 62 | ------- 63 | Tp : (7,) or (3,) array 64 | Result after applying the transformation. 65 | 66 | Notes 67 | ----- 68 | For a single point, it is faster to apply the matrix multiplication 69 | directly rather than calling the OpenRAVE function: 70 | 71 | .. code:: python 72 | 73 | In [33]: %timeit dot(T, hstack([p, 1]))[:3] 74 | 100000 loops, best of 3: 3.82 µs per loop 75 | 76 | In [34]: %timeit list(transformPoints(T, [p])) 77 | 100000 loops, best of 3: 6.4 µs per loop 78 | """ 79 | if len(p) == 3: 80 | return dot(T, hstack([p, 1]))[:3] 81 | R = dot(T[:3, :3], rotation_matrix_from_quat(p[:4])) 82 | pos = dot(T, hstack([p[4:], 1]))[:3] 83 | return hstack([quat_from_rotation_matrix(R), pos]) 84 | 85 | 86 | def crossmat(x): 87 | """ 88 | Cross-product matrix of a 3D vector. 89 | 90 | Parameters 91 | ---------- 92 | x : (3,) array 93 | Vector on the left-hand side of the cross product. 94 | 95 | Returns 96 | ------- 97 | C : (3, 3) array 98 | Cross-product matrix of `x`. 99 | """ 100 | return array([ 101 | [0., -x[2], x[1]], 102 | [x[2], 0., -x[0]], 103 | [-x[1], x[0], 0.]]) 104 | 105 | 106 | def integrate_angular_acceleration(R, omega, omegad, dt): 107 | """ 108 | Integrate constant angular acceleration :math:`\\dot{\\omega}` for a 109 | duration `dt` starting from the orientation :math:`R` and angular velocity 110 | :math:`\\omega`. 111 | 112 | Parameters 113 | ---------- 114 | R : (3, 3) array 115 | Rotation matrix. 116 | omega : (3,) array 117 | Initial angular velocity. 118 | omegad : (3,) array 119 | Constant angular acceleration. 120 | dt : scalar 121 | Duration. 122 | 123 | Returns 124 | ------- 125 | Ri : (3, 3) array 126 | Rotation matrix after integration. 127 | 128 | Notes 129 | ----- 130 | This function applies `Rodrigues' rotation formula 131 | `_. See also 132 | the following nice post on `integrating rotations 133 | `_. 134 | """ 135 | Omega = magnus_expansion(omega, omegad, dt) 136 | theta = norm(Omega) 137 | if theta < 1e-5: 138 | return eye(3) 139 | K = crossmat(Omega / theta) 140 | exp_Omega = eye(3) + sin(theta) * K + (1. - cos(theta)) * dot(K, K) 141 | return dot(exp_Omega, R) 142 | 143 | 144 | def integrate_body_acceleration(T, v, a, dt): 145 | """ 146 | Integrate constant body acceleration :math:`a` for a duration `dt` starting 147 | from the affine transform :math:`T` and body velocity :math:`v`. 148 | 149 | Parameters 150 | ---------- 151 | T : (4, 4) array 152 | Affine transform of the rigid body. 153 | v : (3,) array 154 | Initial body velocity of the rigid body. 155 | a : (3,) array 156 | Constant body acceleration. 157 | dt : scalar 158 | Duration. 159 | 160 | Returns 161 | ------- 162 | T_f : (3, 3) array 163 | Affine transform after integration. 164 | 165 | Notes 166 | ----- 167 | With full frame notations, the affine transform :math:`T = {}^W T_B` maps 168 | vectors from the rigid body frame :math:`B` to the inertial frame 169 | :math:`W`. Its body velocity :math:`v = \\begin{bmatrix} v_B & \\omega 170 | \\end{bmatrix}` is a twist in the inertial frame, with coordinates taken at 171 | the origin of :math:`B`. The same goes for its body acceleration :math:`a = 172 | \\begin{bmatrix} a_B & \\dot{\\omega} \\end{bmatrix}`. 173 | """ 174 | R = T[0:3, 0:3] 175 | p = T[0:3, 3] 176 | omega = v[3:6] 177 | omegad = a[3:6] 178 | R_f = integrate_angular_acceleration(R, omega, omegad, dt) 179 | v_f = v + a * dt 180 | p_f = p + v_f[0:3] * dt # semi-implicit Euler integration 181 | T_f = eye(4) 182 | T_f[0:3, 0:3] = R_f 183 | T_f[0:3, 3] = p_f 184 | return T_f 185 | 186 | 187 | def magnus_expansion(omega, omegad, dt): 188 | """ 189 | Compute the integral :math:`\\Omega` obtained by integrating 190 | :math:`\\dot{\\omega}` for a duration `dt` starting from :math:`\\omega`. 191 | 192 | Parameters 193 | ---------- 194 | omega : (3,) array 195 | Initial angular velocity. 196 | omegad : (3,) array 197 | Constant angular acceleration. 198 | dt : scalar 199 | Duration. 200 | 201 | Returns 202 | ------- 203 | Omega : (3,) array 204 | Integral of ``omegad`` for ``dt`` starting from ``omega``. 205 | 206 | Notes 207 | ----- 208 | This function only computes the first three terms of the Magnus expansion. 209 | See if you need a more 210 | advanced version. 211 | """ 212 | dt_sq = dt * dt 213 | omega1 = omega + omegad * dt 214 | Omega1 = (omega + omega1) * dt / 2. 215 | Omega2 = cross(omega1, omega) * dt_sq / 12. 216 | Omega3 = cross(omegad, Omega2) * dt_sq / 20. 217 | return Omega1 + Omega2 + Omega3 218 | 219 | 220 | def quat_from_rotation_matrix(R): 221 | """ 222 | Quaternion from rotation matrix. 223 | 224 | Parameters 225 | ---------- 226 | R : (3, 3) array 227 | Rotation matrix. 228 | 229 | Returns 230 | ------- 231 | quat : (4,) array 232 | Quaternion in `[w x y z]` format. 233 | """ 234 | return __quatFromRotationMatrix(R) 235 | 236 | 237 | def quat_from_rpy(rpy): 238 | """ 239 | Quaternion frmo roll-pitch-yaw angles. 240 | 241 | Parameters 242 | ---------- 243 | rpy : (3,) array 244 | Vector of roll-pitch-yaw angles in [rad]. 245 | 246 | Returns 247 | ------- 248 | quat : (4,) array 249 | Quaternion in `[w x y z]` format. 250 | 251 | Notes 252 | ----- 253 | Roll-pitch-yaw are Euler angles corresponding to the sequence (1, 2, 3). 254 | """ 255 | roll, pitch, yaw = rpy 256 | cr, cp, cy = cos(roll / 2.), cos(pitch / 2.), cos(yaw / 2.) 257 | sr, sp, sy = sin(roll / 2.), sin(pitch / 2.), sin(yaw / 2.) 258 | return array([ 259 | cr * cp * cy + sr * sp * sy, 260 | -cr * sp * sy + cp * cy * sr, 261 | cr * cy * sp + sr * cp * sy, 262 | cr * cp * sy - sr * cy * sp]) 263 | 264 | 265 | def pose_from_transform(T): 266 | """ 267 | Pose vector from a homogeneous transformation matrix. 268 | 269 | Parameters 270 | ---------- 271 | T : (4, 4) array 272 | Homogeneous transformation matrix. 273 | 274 | Returns 275 | ------- 276 | pose : (7,) array 277 | Pose vector `[qw qx qy qz x y z]` of the transformation matrix. 278 | """ 279 | quat = quat_from_rotation_matrix(T[:3, :3]) 280 | return hstack([quat, T[:3, 3]]) 281 | 282 | 283 | def rotation_matrix_from_quat(quat): 284 | """ 285 | Rotation matrix from quaternion. 286 | 287 | Parameters 288 | ---------- 289 | quat : (4,) array 290 | Quaternion in `[w x y z]` format. 291 | 292 | Returns 293 | ------- 294 | R : (3, 3) array 295 | Rotation matrix. 296 | """ 297 | return __rotationMatrixFromQuat(quat) 298 | 299 | 300 | def rotation_matrix_from_rpy(rpy): 301 | """ 302 | Rotation matrix from roll-pitch-yaw angles. 303 | 304 | Parameters 305 | ---------- 306 | rpy : (3,) array 307 | Vector of roll-pitch-yaw angles in [rad]. 308 | 309 | Returns 310 | ------- 311 | R : (3, 3) array 312 | Rotation matrix. 313 | """ 314 | return rotation_matrix_from_quat(quat_from_rpy(rpy)) 315 | 316 | 317 | def rpy_from_quat(quat): 318 | """ 319 | Roll-pitch-yaw angles of a quaternion. 320 | 321 | Parameters 322 | ---------- 323 | quat : (4,) array 324 | Quaternion in `[w x y z]` format. 325 | 326 | Returns 327 | ------- 328 | rpy : (3,) array 329 | Array of roll-pitch-yaw angles, in [rad]. 330 | 331 | Notes 332 | ----- 333 | Roll-pitch-yaw are Euler angles corresponding to the sequence (1, 2, 3). 334 | """ 335 | roll = atan2( 336 | 2 * quat[2] * quat[3] + 2 * quat[0] * quat[1], 337 | quat[3] ** 2 - quat[2] ** 2 - quat[1] ** 2 + quat[0] ** 2) 338 | pitch = -asin( 339 | 2 * quat[1] * quat[3] - 2 * quat[0] * quat[2]) 340 | yaw = atan2( 341 | 2 * quat[1] * quat[2] + 2 * quat[0] * quat[3], 342 | quat[1] ** 2 + quat[0] ** 2 - quat[3] ** 2 - quat[2] ** 2) 343 | return array([roll, pitch, yaw]) 344 | 345 | 346 | def rpy_from_rotation_matrix(R): 347 | """ 348 | Roll-pitch-yaw angles of rotation matrix. 349 | 350 | Parameters 351 | ---------- 352 | R : array 353 | Rotation matrix. 354 | 355 | Returns 356 | ------- 357 | rpy : (3,) array 358 | Array of roll-pitch-yaw angles, in [rad]. 359 | 360 | Notes 361 | ----- 362 | Roll-pitch-yaw are Euler angles corresponding to the sequence (1, 2, 3). 363 | """ 364 | return rpy_from_quat(quat_from_rotation_matrix(R)) 365 | 366 | 367 | def transform_from_pose(pose): 368 | """ 369 | Transformation matrix from a pose vector. 370 | 371 | Parameters 372 | ---------- 373 | pose : (7,) array 374 | Pose vector `[qw qx qy qz x y z]`. 375 | 376 | Returns 377 | ------- 378 | T : (4, 4) array 379 | Homogeneous transformation matrix of the pose vector. 380 | """ 381 | T = zeros((4, 4)) 382 | T[:3, :3] = rotation_matrix_from_quat(pose[:4]) 383 | T[:3, 3] = pose[4:] 384 | T[3, 3] = 1. 385 | return T 386 | 387 | 388 | def transform_from_R_p(R, p): 389 | """ 390 | Transformation matrix from a translation and rotation matrix. 391 | 392 | Parameters 393 | ---------- 394 | R : (3, 3) array 395 | Rotation matrix. 396 | p : (3,) array 397 | Point coordinates. 398 | 399 | Returns 400 | ------- 401 | T : (4, 4) array 402 | Homogeneous transformation matrix. 403 | """ 404 | T = eye(4) 405 | T[:3, :3] = R 406 | T[:3, 3] = p 407 | return T 408 | 409 | 410 | def transform_inverse(T): 411 | """ 412 | Inverse of a transformation matrix. Yields the same result but faster than 413 | :func:`numpy.linalg.inv` on such matrices. 414 | 415 | Parameters 416 | ---------- 417 | T : (4, 4) array 418 | Homogeneous transformation matrix. 419 | 420 | Returns 421 | ------- 422 | T_inv : (4, 4) array 423 | Inverse of `T`. 424 | """ 425 | T_inv = zeros((4, 4)) 426 | R_inv = T[:3, :3].T 427 | T_inv[:3, :3] = R_inv 428 | T_inv[:3, 3] = -dot(R_inv, T[:3, 3]) 429 | T_inv[3, 3] = 1. 430 | return T_inv 431 | 432 | 433 | __all__ = [ 434 | 'apply_transform', 435 | 'axis_angle_from_quat', 436 | 'crossmat', 437 | 'integrate_angular_acceleration', 438 | 'integrate_body_acceleration', 439 | 'magnus_expansion', 440 | 'pose_from_transform', 441 | 'quat_from_rotation_matrix', 442 | 'quat_from_rpy', 443 | 'rotation_matrix_from_quat', 444 | 'rotation_matrix_from_rpy', 445 | 'rpy_from_quat', 446 | 'rpy_from_rotation_matrix', 447 | 'transform_from_pose', 448 | 'transform_inverse', 449 | ] 450 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2015-2020 Stephane Caron 5 | # 6 | # This file is part of pymanoid . 7 | # 8 | # pymanoid is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU General Public License as published by the Free Software 10 | # Foundation, either version 3 of the License, or (at your option) any later 11 | # version. 12 | # 13 | # pymanoid 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 General Public License for more details. 16 | # 17 | # You should have received a copy of the GNU General Public License along with 18 | # pymanoid. If not, see . 19 | 20 | from distutils.core import setup 21 | 22 | setup( 23 | name='pymanoid', 24 | version='1.2.0', 25 | description="Python library for humanoid robotics using OpenRAVE", 26 | url="https://github.com/stephane-caron/pymanoid", 27 | author="Stéphane Caron", 28 | author_email="stephane.caron@normalesup.org", 29 | license="GPL", 30 | classifiers=[ 31 | 'Development Status :: 5 - Production/Stable', 32 | 'Intended Audience :: Developers', 33 | 'Intended Audience :: Education', 34 | 'Intended Audience :: Science/Research', 35 | 'License :: OSI Approved :: GNU General Public License v3 (GPLv3)', 36 | 'Operating System :: POSIX :: Linux', 37 | 'Programming Language :: Python :: 2.7', 38 | 'Topic :: Scientific/Engineering :: Mathematics'], 39 | packages=[ 40 | 'pymanoid', 41 | 'pymanoid.pypoman', 42 | 'pymanoid.pypoman.pypoman', 43 | 'pymanoid.qpsolvers', 44 | 'pymanoid.qpsolvers.qpsolvers', 45 | 'pymanoid.robots'] 46 | ) 47 | --------------------------------------------------------------------------------