Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: dfki-ric/movement_primitives
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: 0.3.1
Choose a base ref
...
head repository: dfki-ric/movement_primitives
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: main
Choose a head ref

Commits on Jan 19, 2022

  1. Copy the full SHA
    eab064a View commit details

Commits on Feb 8, 2022

  1. Copy the full SHA
    bddf4d5 View commit details
  2. Add docstrings

    AlexanderFabisch committed Feb 8, 2022
    Copy the full SHA
    ed3840e View commit details
  3. Test UR5 simulation

    AlexanderFabisch committed Feb 8, 2022
    Copy the full SHA
    6036ea7 View commit details
  4. Fix random state

    AlexanderFabisch committed Feb 8, 2022
    Copy the full SHA
    83a97e6 View commit details
  5. Add docstrings

    AlexanderFabisch committed Feb 8, 2022
    Copy the full SHA
    2e7460d View commit details
  6. Merge pull request #9 from dfki-ric/feature/pybullet_ik

    Document and test ByBullet IK
    AlexanderFabisch authored Feb 8, 2022
    Copy the full SHA
    900d19f View commit details

Commits on Feb 15, 2022

  1. Copy the full SHA
    dc99845 View commit details
  2. More detailed test

    AlexanderFabisch committed Feb 15, 2022
    Copy the full SHA
    58fb897 View commit details
  3. Merge pull request #11 from dfki-ric/feature/minimum_jerk_p2p

    Minimum jerk trajectory
    AlexanderFabisch authored Feb 15, 2022
    Copy the full SHA
    182186b View commit details

Commits on Feb 16, 2022

  1. add weight getter and setter to dmp base class; currently overwritten…

    … in cartesian dmp which has two forcing terms
    maotto committed Feb 16, 2022
    Copy the full SHA
    f80bcda View commit details
  2. switch from base class method to mixin; add duck typing reminder for …

    …devs (Type Error); remove subclass info from docstring; add mixin where appropriate
    maotto committed Feb 16, 2022
    Copy the full SHA
    2c1dc27 View commit details
  3. Copy the full SHA
    06c9aad View commit details
  4. Merge pull request #10 from maotto/main

    add weight getter and setter to dmp base class
    AlexanderFabisch authored Feb 16, 2022
    Copy the full SHA
    f63aedb View commit details

Commits on Feb 21, 2022

  1. Copy the full SHA
    884beb1 View commit details
  2. Add docstrings

    AlexanderFabisch committed Feb 21, 2022
    Copy the full SHA
    97e02c1 View commit details
  3. Add module docstring

    AlexanderFabisch committed Feb 21, 2022
    Copy the full SHA
    d7f9867 View commit details
  4. Remove loop

    AlexanderFabisch committed Feb 21, 2022
    Copy the full SHA
    319e6ac View commit details
  5. Copy the full SHA
    93ab682 View commit details
  6. Correct import order

    AlexanderFabisch committed Feb 21, 2022
    Copy the full SHA
    576bb65 View commit details
  7. Copy the full SHA
    b79c78a View commit details

Commits on Feb 22, 2022

  1. Version 0.4.0

    AlexanderFabisch committed Feb 22, 2022
    Copy the full SHA
    5dffffb View commit details

Commits on Mar 20, 2022

  1. Copy the full SHA
    d028998 View commit details

Commits on Mar 21, 2022

  1. Fix external scripts

    AlexanderFabisch committed Mar 21, 2022
    Copy the full SHA
    2115d6c View commit details
  2. Merge pull request #13 from dfki-ric/maint/remove_dataset_loaders

    Remove functions to load datasets
    AlexanderFabisch authored Mar 21, 2022
    Copy the full SHA
    fa872f3 View commit details

Commits on Mar 23, 2022

  1. Remove function to pick closest quaternion

    This function is part of pytransform3d now
    AlexanderFabisch committed Mar 23, 2022
    Copy the full SHA
    51d7899 View commit details
  2. Merge pull request #14 from dfki-ric/maint/remove_pick_closest_quater…

    …nion
    
    Remove function to pick closest quaternion
    AlexanderFabisch authored Mar 23, 2022
    Copy the full SHA
    ca8a85d View commit details
  3. Copy the full SHA
    ce35583 View commit details

Commits on Apr 1, 2022

  1. Example with UR5

    AlexanderFabisch committed Apr 1, 2022
    Copy the full SHA
    892f186 View commit details

Commits on Apr 26, 2022

  1. Version 0.5.0

    AlexanderFabisch committed Apr 26, 2022
    Copy the full SHA
    ec94a58 View commit details
  2. Link to zenodo

    AlexanderFabisch committed Apr 26, 2022
    Copy the full SHA
    95fad7d View commit details
  3. Copy the full SHA
    f749d92 View commit details
  4. Install all deps

    AlexanderFabisch committed Apr 26, 2022
    Copy the full SHA
    9cb8cf3 View commit details

Commits on May 16, 2022

  1. Copy the full SHA
    73c72a6 View commit details
  2. Extract function

    AlexanderFabisch committed May 16, 2022
    Copy the full SHA
    fa48da9 View commit details
  3. Copy the full SHA
    3bf668b View commit details

Commits on May 17, 2022

  1. Validate input

    AlexanderFabisch committed May 17, 2022
    Copy the full SHA
    6906c1a View commit details
  2. Ignore flake8

    AlexanderFabisch committed May 17, 2022
    Copy the full SHA
    f429dbe View commit details
  3. Copy the full SHA
    107e2ff View commit details

Commits on May 19, 2022

  1. Copy the full SHA
    ca7be45 View commit details

Commits on Sep 27, 2022

  1. Copy the full SHA
    51c48fd View commit details
  2. Copy the full SHA
    a3380a4 View commit details
  3. Add unit test

    AlexanderFabisch committed Sep 27, 2022
    Copy the full SHA
    25ffcf5 View commit details
  4. Unify behavior of open loop functions

    Make dmp_open_loop_quternion similar to dmp_open_loop.
    AlexanderFabisch committed Sep 27, 2022
    Copy the full SHA
    aa7ce5f View commit details
  5. Merge pull request #20 from dfki-ric/CodingCatMountain-main

    Fix numerical error in Cartesian DMP (open loop execution)
    AlexanderFabisch authored Sep 27, 2022
    Copy the full SHA
    be115a9 View commit details

Commits on Apr 17, 2023

  1. Fix covariance

    AlexanderFabisch committed Apr 17, 2023
    Copy the full SHA
    6dc9bb2 View commit details
  2. Copy the full SHA
    76bae2d View commit details
  3. Copy the full SHA
    b1b231c View commit details

Commits on May 22, 2023

  1. Copy the full SHA
    577ad30 View commit details
  2. Fix test

    AlexanderFabisch committed May 22, 2023
    Copy the full SHA
    48b7e6c View commit details
Showing with 10,084 additions and 1,363 deletions.
  1. +3 −4 .github/workflows/python-package.yml
  2. +1 −1 LICENSE
  3. +2 −0 MANIFEST.in
  4. +321 −104 README.md
  5. +3 −4 benchmarks/benchmark_dmp_phase.py
  6. +21 −0 citation.cff
  7. +20 −0 doc/Makefile
  8. +35 −0 doc/make.bat
  9. BIN doc/source/_static/DFKI_Logo.jpg
  10. BIN doc/source/_static/contextual_promps_kuka_panel_width_open3d2.png
  11. BIN doc/source/_static/dmp_ur5_minimum_jerk.gif
  12. BIN doc/source/_static/summary.png
  13. +6,048 −0 doc/source/_static/summary.svg
  14. +8 −0 doc/source/_templates/class.rst
  15. +159 −0 doc/source/api.rst
  16. +86 −0 doc/source/conf.py
  17. +1 −0 doc/source/index.md
  18. +2 −2 docker/Dockerfile
  19. +3 −2 examples/external_dependencies/plot_contextual_promp_distribution.py
  20. +1 −1 examples/external_dependencies/plot_imitate_panel_rh5.py
  21. +3 −2 examples/external_dependencies/plot_relative_trajectories_kuka.py
  22. +2 −1 examples/external_dependencies/sim_solar_panel_rh5v2.py
  23. +2 −2 examples/external_dependencies/vis_cartesian_dual_dmp.py
  24. +5 −3 examples/external_dependencies/vis_contextual_dmp_distribution.py
  25. +20 −5 examples/external_dependencies/vis_contextual_promp_distribution.py
  26. +77 −31 examples/external_dependencies/vis_contextual_promp_distribution_screws.py
  27. +1 −1 examples/external_dependencies/vis_dmp_to_state_variance.py
  28. +3 −3 examples/external_dependencies/vis_sample_dmp_distribution_kuka.py
  29. +3 −3 examples/external_dependencies/vis_sample_promp_distribution_kuka.py
  30. +4 −3 examples/external_dependencies/vis_solar_panel.py
  31. +3 −3 examples/external_dependencies/vis_solar_panel_from_exported_dmp.py
  32. +2 −2 examples/plot_cartesian_pose_coupling_dual_dmp.py
  33. +3 −0 examples/plot_conditional_promp.py
  34. +32 −0 examples/plot_critical_damping.py
  35. +59 −0 examples/plot_dmp_2d_gain.py
  36. +4 −4 examples/plot_dmp_potential_field.py
  37. +59 −0 examples/plot_dmp_scaling.py
  38. +59 −0 examples/plot_dmp_stepwise_execution.py
  39. +3 −3 examples/plot_minimum_jerk.py
  40. +1 −1 movement_primitives/_version.py
  41. +111 −9 movement_primitives/base.py
  42. +5 −19 movement_primitives/data/__init__.py
  43. +11 −15 movement_primitives/data/_lasa.py
  44. +13 −13 movement_primitives/data/_minimum_jerk.py
  45. +0 −304 movement_primitives/data/_mocap.py
  46. +1 −1 movement_primitives/data/_toy_1d.py
  47. +30 −8 movement_primitives/dmp/__init__.py
  48. +43 −1 movement_primitives/dmp/_base.py
  49. +44 −20 movement_primitives/dmp/_canonical_system.py
  50. +220 −87 movement_primitives/dmp/_cartesian_dmp.py
  51. +244 −56 movement_primitives/dmp/_coupling_terms.py
  52. +262 −125 movement_primitives/dmp/_dmp.py
  53. +111 −44 movement_primitives/dmp/_dmp_with_final_velocity.py
  54. +121 −67 movement_primitives/dmp/_dual_cartesian_dmp.py
  55. +47 −11 movement_primitives/dmp/_forcing_term.py
  56. +18 −16 movement_primitives/dmp/_state_following_dmp.py
  57. +227 −65 movement_primitives/dmp_fast.pyx
  58. +9 −4 movement_primitives/dmp_potential_field.py
  59. +4 −4 movement_primitives/dmp_to_state_space_distribution.py
  60. +5 −1 movement_primitives/io.py
  61. +10 −4 movement_primitives/kinematics.py
  62. +63 −0 movement_primitives/minimum_jerk_trajectory.py
  63. +7 −9 movement_primitives/plot.py
  64. +56 −36 movement_primitives/promp.py
  65. +124 −32 movement_primitives/spring_damper.py
  66. +245 −39 movement_primitives/testing/simulation.py
  67. +49 −23 movement_primitives/utils.py
  68. +7 −6 movement_primitives/visualization.py
  69. +3 −1 setup.cfg
  70. +2 −2 setup.py
  71. +5 −5 test/test_canonical_system.py
  72. +84 −3 test/test_cartesian_dmp.py
  73. +121 −0 test/test_cartesian_pose_coupling_dual_dmp.py
  74. +22 −4 test/test_conditional_promps.py
  75. +1 −2 test/test_contextual_promps.py
  76. +9 −9 test/test_coupling.py
  77. +23 −24 test/test_dataset.py
  78. +61 −18 test/test_dmp.py
  79. +6 −7 test/test_dmp_canonical_system.py
  80. +3 −3 test/test_dmp_fast.py
  81. +15 −16 test/test_dmp_forcing_term.py
  82. +3 −3 test/test_dmp_potential_field.py
  83. +99 −0 test/test_dmp_smooth_spatial_scaling.py
  84. +39 −2 test/test_dmp_with_final_velocity.py
  85. +33 −3 test/test_dual_cartesian_dmp.py
  86. +11 −3 test/test_io.py
  87. +23 −0 test/test_kinematics.py
  88. +0 −17 test/test_load_kuka.py
  89. +28 −0 test/test_minimum_jerk_trajectory.py
  90. +80 −6 test/test_obstacle_avoidance.py
  91. +29 −5 test/test_promp.py
  92. +58 −0 test/test_simulation.py
  93. +42 −3 test/test_spring_damper.py
  94. +36 −0 test/test_spring_damper_obstacle_avoidance.py
  95. +40 −4 test/test_state_following_dmp.py
  96. +15 −6 test/test_step_dual_cartesian.py
  97. +47 −13 test/test_utils.py
7 changes: 3 additions & 4 deletions .github/workflows/python-package.yml
Original file line number Diff line number Diff line change
@@ -8,7 +8,7 @@ jobs:
runs-on: ubuntu-latest
strategy:
matrix:
python-version: [3.6, 3.7]
python-version: ["3.7", "3.8", "3.10"]

steps:
- uses: actions/checkout@v2
@@ -19,8 +19,7 @@ jobs:
- name: Install dependencies
run: |
python -m pip install --upgrade pip
pip install cython numpy
pip install -e .[all] flake8 nose coverage
pip install -e .[all,test] flake8
- name: Lint with flake8
run: |
# stop the build if there are Python syntax errors or undefined names
@@ -29,6 +28,6 @@ jobs:
flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics
- name: Test with pytest
run: |
NUMBA_DISABLE_JIT=1 MPLBACKEND=Agg nosetests -sv test/ --with-coverage
NUMBA_DISABLE_JIT=1 MPLBACKEND=Agg pytest
- name: Codecov
uses: codecov/codecov-action@v1.3.2
2 changes: 1 addition & 1 deletion LICENSE
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
Copyright 2020-2021 Alexander Fabisch (DFKI GmbH, Robotics Innovation Center),
Copyright Alexander Fabisch (DFKI GmbH, Robotics Innovation Center),
Jannik Klemm (University of Bremen, Robotics Research Group),
and movement_primitives contributors.

2 changes: 2 additions & 0 deletions MANIFEST.in
Original file line number Diff line number Diff line change
@@ -1 +1,3 @@
include movement_primitives/dmp_fast.pyx
prune tmp/
prune test/
425 changes: 321 additions & 104 deletions README.md

Large diffs are not rendered by default.

7 changes: 3 additions & 4 deletions benchmarks/benchmark_dmp_phase.py
Original file line number Diff line number Diff line change
@@ -8,9 +8,8 @@

goal_t = 1.0
start_t = 0.0
int_dt = 0.001
alpha = canonical_system_alpha(0.01, goal_t, start_t, int_dt)
times = timeit.repeat(partial(phase_python, 0.5, alpha, goal_t, start_t, int_dt), repeat=1000, number=1000)
alpha = canonical_system_alpha(0.01, goal_t, start_t)
times = timeit.repeat(partial(phase_python, 0.5, alpha, goal_t, start_t), repeat=1000, number=1000)
print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times)))
times = timeit.repeat(partial(phase_cython, 0.5, alpha, goal_t, start_t, int_dt), repeat=1000, number=1000)
times = timeit.repeat(partial(phase_cython, 0.5, alpha, goal_t, start_t), repeat=1000, number=1000)
print("Mean: %.5f; Std. dev.: %.5f" % (np.mean(times), np.std(times)))
21 changes: 21 additions & 0 deletions citation.cff
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
cff-version: 1.2.0
message: "If you use this software, please cite it as below."
authors:
- family-names: Fabisch
given-names: Alexander
title: "movement_primitives"
url: https://github.com/dfki-ric/movement_primitives
doi: 10.5281/zenodo.6491361
preferred-citation:
type: article
authors:
- family-names: Fabisch
given-names: Alexander
title: "movement_primitives: Imitation Learning of Cartesian Motion with Movement Primitives"
doi: 10.21105/joss.06695
journal: Journal of Open Source Software
start: 6695
issue: 97
volume: 9
month: 5
year: 2024
20 changes: 20 additions & 0 deletions doc/Makefile
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
# Minimal makefile for Sphinx documentation
#

# You can set these variables from the command line, and also
# from the environment for the first two.
SPHINXOPTS ?=
SPHINXBUILD ?= sphinx-build
SOURCEDIR = source
BUILDDIR = build

# Put it first so that "make" without argument is like "make help".
help:
@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)

.PHONY: help Makefile

# Catch-all target: route all unknown targets to Sphinx using the new
# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS).
%: Makefile
@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
35 changes: 35 additions & 0 deletions doc/make.bat
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
@ECHO OFF

pushd %~dp0

REM Command file for Sphinx documentation

if "%SPHINXBUILD%" == "" (
set SPHINXBUILD=sphinx-build
)
set SOURCEDIR=source
set BUILDDIR=build

%SPHINXBUILD% >NUL 2>NUL
if errorlevel 9009 (
echo.
echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
echo.installed, then set the SPHINXBUILD environment variable to point
echo.to the full path of the 'sphinx-build' executable. Alternatively you
echo.may add the Sphinx directory to PATH.
echo.
echo.If you don't have Sphinx installed, grab it from
echo.https://www.sphinx-doc.org/
exit /b 1
)

if "%1" == "" goto help

%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
goto end

:help
%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%

:end
popd
Binary file modified doc/source/_static/DFKI_Logo.jpg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/source/_static/dmp_ur5_minimum_jerk.gif
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added doc/source/_static/summary.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
6,048 changes: 6,048 additions & 0 deletions doc/source/_static/summary.svg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
8 changes: 8 additions & 0 deletions doc/source/_templates/class.rst
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
{{ fullname | escape | underline}}

.. currentmodule:: {{ module }}

.. autoclass:: {{ objname }}
:members:
:show-inheritance:
:inherited-members:
159 changes: 159 additions & 0 deletions doc/source/api.rst
Original file line number Diff line number Diff line change
@@ -0,0 +1,159 @@
.. _api:

=================
API Documentation
=================

This is the detailed documentation of all public classes and functions.
You can also search for specific modules, classes, or functions in the
:ref:`genindex`.

.. contents:: :local:
:depth: 1


:mod:`movement_primitives.dmp`
==============================

.. automodule:: movement_primitives.dmp

.. autosummary::
:toctree: _apidoc/
:template: class.rst

~movement_primitives.dmp.DMP
~movement_primitives.dmp.DMPWithFinalVelocity
~movement_primitives.dmp.CartesianDMP
~movement_primitives.dmp.DualCartesianDMP

Coupling Terms
--------------

Coupling terms can be used to adapt the movement generated by a DMP online.
Not all coupling terms are compatible with all types of DMPs.

.. autosummary::
:toctree: _apidoc/
:template: class.rst

~movement_primitives.dmp.CouplingTermPos1DToPos1D
~movement_primitives.dmp.CouplingTermObstacleAvoidance2D
~movement_primitives.dmp.CouplingTermObstacleAvoidance3D
~movement_primitives.dmp.CouplingTermPos3DToPos3D
~movement_primitives.dmp.CouplingTermDualCartesianPose
~movement_primitives.dmp.CouplingTermDualCartesianDistance
~movement_primitives.dmp.CouplingTermDualCartesianTrajectory


:mod:`movement_primitives.promp`
================================

.. automodule:: movement_primitives.promp

.. autosummary::
:toctree: _apidoc/
:template: class.rst

~movement_primitives.promp.ProMP


:mod:`movement_primitives.io`
=============================

.. automodule:: movement_primitives.io

.. autosummary::
:toctree: _apidoc/

~movement_primitives.io.write_pickle
~movement_primitives.io.read_pickle
~movement_primitives.io.write_yaml
~movement_primitives.io.read_yaml
~movement_primitives.io.write_json
~movement_primitives.io.read_json


:mod:`movement_primitives.base`
===============================

.. automodule:: movement_primitives.base

.. autosummary::
:toctree: _apidoc/
:template: class.rst

~movement_primitives.base.PointToPointMovement


:mod:`movement_primitives.data`
===============================

.. automodule:: movement_primitives.data

.. autosummary::
:toctree: _apidoc/

~movement_primitives.data.load_lasa
~movement_primitives.data.generate_minimum_jerk
~movement_primitives.data.generate_1d_trajectory_distribution


:mod:`movement_primitives.kinematics`
=====================================

.. automodule:: movement_primitives.kinematics

.. autosummary::
:toctree: _apidoc/
:template: class.rst

~movement_primitives.kinematics.Kinematics
~movement_primitives.kinematics.Chain


:mod:`movement_primitives.plot`
===============================

.. automodule:: movement_primitives.plot

.. autosummary::
:toctree: _apidoc/

~movement_primitives.plot.plot_trajectory_in_rows
~movement_primitives.plot.plot_distribution_in_rows


:mod:`movement_primitives.visualization`
========================================

.. automodule:: movement_primitives.visualization

.. autosummary::
:toctree: _apidoc/

~movement_primitives.visualization.plot_pointcloud
~movement_primitives.visualization.to_ellipsoid


:mod:`movement_primitives.dmp_potential_field`
==============================================

.. automodule:: movement_primitives.dmp_potential_field

.. autosummary::
:toctree: _apidoc/

~movement_primitives.dmp_potential_field.plot_potential_field_2d


:mod:`movement_primitives.spring_damper`
========================================

.. automodule:: movement_primitives.spring_damper

.. autosummary::
:toctree: _apidoc/
:template: class.rst

~movement_primitives.spring_damper.SpringDamper
~movement_primitives.spring_damper.SpringDamperOrientation
86 changes: 86 additions & 0 deletions doc/source/conf.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,86 @@
# Configuration file for the Sphinx documentation builder.
#
# This file only contains a selection of the most common options. For a full
# list see the documentation:
# https://www.sphinx-doc.org/en/master/usage/configuration.html

# -- Path setup --------------------------------------------------------------

# If extensions (or modules to document with autodoc) are in another directory,
# add these directories to sys.path here. If the directory is relative to the
# documentation root, use os.path.abspath to make it absolute, like shown here.
#
import os
import sys
import time


sys.path.insert(0, os.path.abspath('.'))
sys.path.insert(0, os.path.abspath('../..'))


# -- Project information -----------------------------------------------------

project = "movement_primitives"
copyright = "2020-{}, Alexander Fabisch, DFKI GmbH, Robotics Innovation Center".format(time.strftime("%Y"))
author = "Alexander Fabisch"

# The full version, including alpha/beta/rc tags
release = __import__(project).__version__


# -- General configuration ---------------------------------------------------

# Add any Sphinx extension module names here, as strings. They can be
# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
# ones.
extensions = [
"numpydoc",
"myst_parser",
"sphinx.ext.autodoc",
"sphinx.ext.autosummary"
]

# Add any paths that contain templates here, relative to this directory.
templates_path = ['_templates']

# List of patterns, relative to source directory, that match files and
# directories to ignore when looking for source files.
# This pattern also affects html_static_path and html_extra_path.
exclude_patterns = []

autosummary_generate = True


# -- Options for HTML output -------------------------------------------------

# The theme to use for HTML and HTML Help pages. See the documentation for
# a list of builtin themes.
#
html_theme = "pydata_sphinx_theme"
html_theme_options = {
"logo": {
"text": f"movement_primitives {release}"
},
"navbar_start": ["navbar-logo"],
}

root_doc = "index"
source_suffix = {
".rst": "restructuredtext",
".md": "markdown",
}

# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
html_static_path = ["_static"]
html_show_sourcelink = False
html_show_sphinx = False

intersphinx_mapping = {
'python': ('https://docs.python.org/{.major}'.format(sys.version_info), None),
'numpy': ('https://numpy.org/doc/stable', None),
'scipy': ('https://docs.scipy.org/doc/scipy/reference', None),
'matplotlib': ('https://matplotlib.org/', None)
}
1 change: 1 addition & 0 deletions doc/source/index.md
4 changes: 2 additions & 2 deletions docker/Dockerfile
Original file line number Diff line number Diff line change
@@ -2,8 +2,8 @@ FROM continuumio/miniconda3:latest

RUN apt update && apt -y install libusb-1.0-0 libgl1-mesa-glx build-essential
RUN conda install python=3.8.11 # later versions are not compatible to Open3D 0.13
RUN conda install cython numpy scipy matplotlib pandas pyyaml numba sphinx numpydoc pdoc3 nose coverage
RUN pip install gmr pytransform3d[all] tqdm sphinx-gallery sphinx-bootstrap-theme
RUN conda install cython numpy scipy matplotlib pandas pyyaml numba sphinx numpydoc pytest pytest-cov
RUN pip install pytransform3d[all,doc]

# docker build . -t conda_py38
# docker tag conda_py38 af01/conda_py38
Original file line number Diff line number Diff line change
@@ -3,10 +3,11 @@
import tqdm
from pytransform3d import trajectories as ptr
from pytransform3d import transformations as pt
from mocap.cleaning import smooth_quaternion_trajectory, median_filter
from pytransform3d.batch_rotations import smooth_quaternion_trajectory
from mocap.dataset_loader import load_kuka_dataset, transpose_dataset, smooth_dual_arm_trajectories_pq
from mocap.cleaning import median_filter
from gmr import GMM

from movement_primitives.data import load_kuka_dataset, transpose_dataset, smooth_dual_arm_trajectories_pq
from movement_primitives.promp import ProMP
from movement_primitives.plot import plot_distribution_in_rows, PALETTE

2 changes: 1 addition & 1 deletion examples/external_dependencies/plot_imitate_panel_rh5.py
Original file line number Diff line number Diff line change
@@ -2,7 +2,7 @@
import matplotlib.pyplot as plt
from pytransform3d.plot_utils import make_3d_axis
import pytransform3d.trajectories as ptr
from movement_primitives.data import load_rh5_demo
from mocap.dataset_loader import load_rh5_demo
from movement_primitives.dmp import DualCartesianDMP, CouplingTermDualCartesianTrajectory


Original file line number Diff line number Diff line change
@@ -1,8 +1,9 @@
import numpy as np
import matplotlib.pyplot as plt
from movement_primitives.data import load_kuka_dataset
from mocap.dataset_loader import load_kuka_dataset
from movement_primitives.plot import plot_trajectory_in_rows
from mocap.cleaning import smooth_quaternion_trajectory, median_filter
from mocap.cleaning import median_filter
from pytransform3d.batch_rotations import smooth_quaternion_trajectory
from pytransform3d import trajectories as ptr


3 changes: 2 additions & 1 deletion examples/external_dependencies/sim_solar_panel_rh5v2.py
Original file line number Diff line number Diff line change
@@ -4,10 +4,11 @@
import pytransform3d.rotations as pr
import pytransform3d.transformations as pt
import pytransform3d.trajectories as ptr
from pytransform3d.batch_rotations import smooth_quaternion_trajectory
from movement_primitives.dmp import DualCartesianDMP
from movement_primitives.kinematics import Kinematics
from movement_primitives.testing.simulation import RH5Simulation, draw_transform, _pybullet_pose, get_absolute_path
from mocap.cleaning import smooth_quaternion_trajectory, smooth_exponential_coordinates
from mocap.cleaning import smooth_exponential_coordinates
from movement_primitives.io import write_json, write_yaml, write_pickle


4 changes: 2 additions & 2 deletions examples/external_dependencies/vis_cartesian_dual_dmp.py
Original file line number Diff line number Diff line change
@@ -84,9 +84,9 @@

tm = UrdfTransformManager(check=False)
with open("abstract-urdf-gripper/urdf/GripperLeft.urdf", "r") as f:
tm.load_urdf(f, mesh_path="abstract-urdf-gripper/urdf/")
tm.load_urdf(f.read(), mesh_path="abstract-urdf-gripper/urdf/")
with open("abstract-urdf-gripper/urdf/GripperRight.urdf", "r") as f:
tm.load_urdf(f, mesh_path="abstract-urdf-gripper/urdf/")
tm.load_urdf(f.read(), mesh_path="abstract-urdf-gripper/urdf/")
tm.add_transform("ALWristPitch_Link", "base", np.eye(4))
tm.add_transform("ARWristPitch_Link", "base", np.eye(4))
# assert tm.check_consistency()
Original file line number Diff line number Diff line change
@@ -1,11 +1,12 @@
import numpy as np
import tqdm
from movement_primitives.data import load_kuka_dataset, transpose_dataset
from mocap.dataset_loader import load_kuka_dataset, transpose_dataset
from movement_primitives.dmp import DualCartesianDMP
from movement_primitives.dmp_to_state_space_distribution import propagate_to_state_space, estimate_state_distribution
from movement_primitives.visualization import plot_pointcloud, ToggleGeometry
from mocap.cleaning import smooth_quaternion_trajectory, median_filter
from mocap.cleaning import median_filter
from gmr import GMM
from pytransform3d.batch_rotations import smooth_quaternion_trajectory
import pytransform3d.visualizer as pv
from pytransform3d.urdf import UrdfTransformManager

@@ -96,7 +97,8 @@ def sample_trajectories(mvn, n_samples, n_dims):
context = np.array([panel_width, 0.0, 1.0])
conditional_weight_distribution = gmm.condition(np.arange(len(context)), context).to_mvn()
average_execution_time = np.mean([T[-1] for T in Ts])
trajectories = propagate_to_state_space(conditional_weight_distribution, n_weights_per_dim, average_execution_time, alpha=alpha, kappa=kappa, verbose=1)
trajectories = propagate_to_state_space(conditional_weight_distribution, n_weights_per_dim, average_execution_time,
alpha=alpha, kappa=kappa, dt=0.01, int_dt=0.001, verbose=1)
conditional_state_distribution = estimate_state_distribution(trajectories, alpha=alpha, kappa=kappa, n_weights_per_dim=n_weights_per_dim)

mean = conditional_state_distribution.mean.reshape(-1, 14)
Original file line number Diff line number Diff line change
@@ -1,11 +1,13 @@
import numpy as np
import pytransform3d.visualizer as pv
from pytransform3d.urdf import UrdfTransformManager
import pytransform3d.transformations as pt
from gmr import GMM
from mocap.dataset_loader import load_kuka_dataset, transpose_dataset, smooth_dual_arm_trajectories_pq

from movement_primitives.data import load_kuka_dataset, transpose_dataset, smooth_dual_arm_trajectories_pq
from movement_primitives.promp import ProMP
from movement_primitives.visualization import to_ellipsoid
from movement_primitives.kinematics import Kinematics


def generate_training_data(
@@ -46,17 +48,30 @@ def generate_training_data(
gmm = GMM(n_components=5, random_state=random_state)
gmm.from_samples(X)

with open("kuka_lbr/urdf/kuka_lbr.urdf", "r") as f:
kin = Kinematics(f.read(), mesh_path="kuka_lbr/urdf/")
right_chain = kin.create_chain(
["kuka_lbr_r_joint_%d" % i for i in range(1, 8)],
"kuka_lbr", "kuka_lbr_r_tcp", verbose=0)
left_chain = kin.create_chain(
["kuka_lbr_l_joint_%d" % i for i in range(1, 8)],
"kuka_lbr", "kuka_lbr_l_tcp", verbose=0)
mean_start = np.mean([
Ps[i][0] for i in range(len(Ps)) if contexts[i][0] == 0.5], axis=0)
mean_start_left = pt.transform_from_pq(mean_start[:7])
mean_start_right = pt.transform_from_pq(mean_start[7:])
left_chain.inverse(mean_start_left, initial_joint_angles=np.zeros(7))
right_chain.inverse(mean_start_right, initial_joint_angles=np.zeros(7))

n_validation_samples = 100
n_steps = 100
T_query = np.linspace(0, 1, n_steps)

fig = pv.figure(with_key_callbacks=True)
fig.plot_transform(s=0.1)
tm = UrdfTransformManager()
with open("kuka_lbr/urdf/kuka_lbr.urdf", "r") as f:
tm.load_urdf(f.read(), mesh_path="kuka_lbr/urdf/")
fig.plot_graph(
tm, "kuka_lbr", show_frames=True, show_visuals=True,
kin.tm, "kuka_lbr", show_frames=False, show_visuals=True,
whitelist=["kuka_lbr_l_tcp", "kuka_lbr_r_tcp"], s=0.2)

for panel_width, color, idx in zip([0.3, 0.4, 0.5], ([1.0, 1.0, 0.0], [0.0, 1.0, 1.0], [1.0, 0.0, 1.0]), range(3)):
@@ -77,7 +92,7 @@ def generate_training_data(
for t in range(0, len(mean), 1):
p_left = mean[t, :3]
p_right = mean[t, 7:10]
cov_left = cov[t, 7:10, 7:10, t]
cov_left = cov[t, :3, :3, t]
cov_right = cov[t, 7:10, 7:10, t]
ellipsoid2origin, radii = to_ellipsoid(p_left, cov_left)
fig.plot_ellipsoid(A2B=ellipsoid2origin, radii=0.5 * radii, c=color)
Original file line number Diff line number Diff line change
@@ -1,15 +1,75 @@
import numpy as np
import open3d as o3d
from matplotlib import cbook
import pytransform3d.visualizer as pv
import pytransform3d.transformations as pt
import pytransform3d.trajectories as ptr
import pytransform3d.uncertainty as pu
from pytransform3d.urdf import UrdfTransformManager
from mocap.cleaning import smooth_exponential_coordinates, median_filter
from mocap.dataset_loader import load_kuka_dataset, transpose_dataset

from movement_primitives.visualization import plot_pointcloud, ToggleGeometry
from movement_primitives.data import load_kuka_dataset, transpose_dataset
from movement_primitives.promp import ProMP
from gmr import GMM


class Surface(pv.Artist):
"""Surface.
Parameters
----------
x : array, shape (n_steps, n_steps)
Coordinates on x-axis of grid on surface.
y : array, shape (n_steps, n_steps)
Coordinates on y-axis of grid on surface.
z : array, shape (n_steps, n_steps)
Coordinates on z-axis of grid on surface.
c : array-like, shape (3,), optional (default: None)
Color
"""
def __init__(self, x, y, z, c=None):
self.c = c
self.mesh = o3d.geometry.TriangleMesh()
self.set_data(x, y, z)

def set_data(self, x, y, z):
"""Update data.
Parameters
----------
x : array, shape (n_steps, n_steps)
Coordinates on x-axis of grid on surface.
y : array, shape (n_steps, n_steps)
Coordinates on y-axis of grid on surface.
z : array, shape (n_steps, n_steps)
Coordinates on z-axis of grid on surface.
"""
polys = np.stack([cbook._array_patch_perimeters(a, 1, 1)
for a in (x, y, z)], axis=-1)
vertices = polys.reshape(-1, 3)
triangles = (
[[4 * i + 0, 4 * i + 1, 4 * i + 2] for i in range(len(polys))] +
[[4 * i + 2, 4 * i + 3, 4 * i + 0] for i in range(len(polys))] +
[[4 * i + 0, 4 * i + 3, 4 * i + 2] for i in range(len(polys))] +
[[4 * i + 2, 4 * i + 1, 4 * i + 0] for i in range(len(polys))]
)
self.mesh.vertices = o3d.utility.Vector3dVector(vertices)
self.mesh.triangles = o3d.utility.Vector3iVector(triangles)
if self.c is not None:
self.mesh.paint_uniform_color(self.c)
self.mesh.compute_vertex_normals()

@property
def geometries(self):
"""Expose geometries.
Returns
-------
geometries : list
List of geometries that can be added to the visualizer.
"""
return [self.mesh]


def generate_training_data(
pattern, n_weights_per_dim, context_names, verbose=0):
Ts, Ps, contexts = transpose_dataset(
@@ -78,36 +138,22 @@ def generate_training_data(
promp.from_weight_distribution(
conditional_weight_distribution.mean,
conditional_weight_distribution.covariance)

# mean and covariance in state space
mean = promp.mean_trajectory(T_query)
var = promp.var_trajectory(T_query)
samples = promp.sample_trajectories(T_query, 100, random_state)

c = [0, 0, 0]
c[idx] = 1
fig.plot_trajectory(ptr.pqs_from_transforms(ptr.transforms_from_exponential_coordinates(mean[:, :6])), s=0.05, c=tuple(c))
fig.plot_trajectory(ptr.pqs_from_transforms(ptr.transforms_from_exponential_coordinates(mean[:, 6:])), s=0.05, c=tuple(c))

pcl_points = []
distances = []
stds = []
for E in samples:
P_left = ptr.pqs_from_transforms(ptr.transforms_from_exponential_coordinates(E[:, :6]))
P_right = ptr.pqs_from_transforms(ptr.transforms_from_exponential_coordinates(E[:, 6:]))
left2base_ee_pos = P_left[:, :3]
right2base_ee_pos = P_right[:, :3]
pcl_points.extend(left2base_ee_pos)
pcl_points.extend(right2base_ee_pos)

ee_distances = np.linalg.norm(left2base_ee_pos - right2base_ee_pos, axis=1)
distances.append(np.mean(ee_distances))
stds.append(np.std(ee_distances))
print("Mean average distance of end-effectors = %.2f, mean std. dev. = %.3f"
% (np.mean(distances), np.mean(stds)))

pcl = plot_pointcloud(fig, pcl_points, color)

key = ord(str((idx + 1) % 10))
fig.visualizer.register_key_action_callback(key, ToggleGeometry(fig, pcl))
cov = promp.cov_trajectory(T_query).reshape(
mean.shape[0], mean.shape[1], mean.shape[1], mean.shape[0])

for t in range(0, len(mean), 1):
mean_left = pt.transform_from_exponential_coordinates(mean[t, :6])
mean_right = pt.transform_from_exponential_coordinates(mean[t, 6:12])
cov_left = cov[t, :6, :6, t]
cov_right = cov[t, 6:, 6:, t]
x, y, z = pu.to_projected_ellipsoid(mean_left, cov_left, 0.5, 20)
surface = Surface(x, y, z, c=color)
surface.add_artist(fig)
x, y, z = pu.to_projected_ellipsoid(mean_right, cov_right, 0.5, 20)
surface = Surface(x, y, z, c=color)

if plot_training_data:
for E in Es:
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
import numpy as np
from tqdm import tqdm
from mocap.dataset_loader import load_kuka_dataset

from movement_primitives.data import load_kuka_dataset
from movement_primitives.dmp_to_state_space_distribution import propagate_weight_distribution_to_state_space
from movement_primitives.plot import plot_trajectory_in_rows, plot_distribution_in_rows
from pytransform3d import transformations as pt
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
import numpy as np
from movement_primitives.data import load_kuka_dataset
from mocap.dataset_loader import load_kuka_dataset
import pytransform3d.visualizer as pv
from pytransform3d.urdf import UrdfTransformManager
from movement_primitives.dmp import DualCartesianDMP
@@ -33,8 +33,8 @@ def __call__(self, vis, key, modifier): # sometimes we don't receive the correc

n_weights_per_dim = 10

#pattern = "data/kuka/20200129_peg_in_hole/csv_processed/01_peg_in_hole_both_arms/*.csv"
pattern = "data/kuka/20191213_carry_heavy_load/csv_processed/01_heavy_load_no_tilt_0cm_dual_arm/*.csv"
pattern = "data/kuka/20200129_peg_in_hole/csv_processed/01_peg_in_hole_both_arms/*.csv"
#pattern = "data/kuka/20191213_carry_heavy_load/csv_processed/01_heavy_load_no_tilt_0cm_dual_arm/*.csv"
#pattern = "data/kuka/20191023_rotate_panel_varying_size/csv_processed/panel_450mm_counterclockwise/*.csv"

dataset = load_kuka_dataset(pattern, verbose=1)
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
import numpy as np
import pytransform3d.visualizer as pv
from pytransform3d.urdf import UrdfTransformManager
from movement_primitives.data import load_kuka_dataset
from mocap.dataset_loader import load_kuka_dataset
from movement_primitives.promp import ProMP


@@ -25,8 +25,8 @@ def __call__(self, vis, key, modifier): # sometimes we don't receive the correc

n_weights_per_dim = 10

#pattern = "data/kuka/20200129_peg_in_hole/csv_processed/01_peg_in_hole_both_arms/*.csv"
pattern = "data/kuka/20191213_carry_heavy_load/csv_processed/01_heavy_load_no_tilt_0cm_dual_arm/*.csv"
pattern = "data/kuka/20200129_peg_in_hole/csv_processed/01_peg_in_hole_both_arms/*.csv"
#pattern = "data/kuka/20191213_carry_heavy_load/csv_processed/01_heavy_load_no_tilt_0cm_dual_arm/*.csv"
#pattern = "data/kuka/20191023_rotate_panel_varying_size/csv_processed/panel_450mm_counterclockwise/*.csv"

dataset = load_kuka_dataset(pattern, verbose=1)
7 changes: 4 additions & 3 deletions examples/external_dependencies/vis_solar_panel.py
Original file line number Diff line number Diff line change
@@ -3,7 +3,8 @@
import pytransform3d.rotations as pr
import pytransform3d.transformations as pt
import pytransform3d.trajectories as ptr
from mocap.cleaning import smooth_exponential_coordinates, smooth_quaternion_trajectory
from pytransform3d.batch_rotations import smooth_quaternion_trajectory
from mocap.cleaning import smooth_exponential_coordinates
from movement_primitives.kinematics import Kinematics
from movement_primitives.dmp import DualCartesianDMP
from movement_primitives.io import (write_json, write_yaml, write_pickle)
@@ -188,9 +189,9 @@ def animation_callback(step, graph, left_arm, right_arm, left_joint_trajectory,
fig = pv.figure()
fig.plot_transform(s=0.3)

# "solar_panels/solar_panel_02/meshes/stl/base link.stl"
# "solar_panels/solar_panel_02/meshes/stl/base_link.stl"
# "solar_panels/solar_panel_03/meshes/stl/base link.stl"
panel_mesh = fig.plot_mesh("solar_panels/solar_panel_02/meshes/stl/base link.stl", A2B=panel2base_start)
panel_mesh = fig.plot_mesh("solar_panels/solar_panel_02/meshes/stl/base_link.stl", A2B=panel2base_start)

graph = fig.plot_graph(
kin.tm, "RH5_Root_Link", show_visuals=True, show_collision_objects=False, show_frames=True, s=0.1,
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
import numpy as np
import pytransform3d.visualizer as pv
import pytransform3d.rotations as pr
import pytransform3d.transformations as pt
import pytransform3d.trajectories as ptr
from movement_primitives.kinematics import Kinematics
@@ -61,9 +62,8 @@ def animation_callback(step, graph, left_arm, right_arm, left_joint_trajectory,
# does not move at all, while a DMP that moves from q to -q moves a lot. We
# have to make sure that start_y always contains the quaternion representation
# that is closest to the previous start_y!
from movement_primitives.utils import pick_closest_quaternion
start_y[3:7] = pick_closest_quaternion(start_y[3:7], target_quaternion=dmp.start_y[3:7])
start_y[10:14] = pick_closest_quaternion(start_y[10:14], target_quaternion=dmp.start_y[10:14])
start_y[3:7] = pr.pick_closest_quaternion(start_y[3:7], target_quaternion=dmp.start_y[3:7])
start_y[10:14] = pr.pick_closest_quaternion(start_y[10:14], target_quaternion=dmp.start_y[10:14])
dmp.configure(start_y=start_y)

# Generate trajectory with open loop.
4 changes: 2 additions & 2 deletions examples/plot_cartesian_pose_coupling_dual_dmp.py
Original file line number Diff line number Diff line change
@@ -75,9 +75,9 @@
tm = UrdfTransformManager()
try:
with open("abstract-urdf-gripper/urdf/GripperLeft.urdf", "r") as f:
tm.load_urdf(f, mesh_path="abstract-urdf-gripper/urdf/")
tm.load_urdf(f.read(), mesh_path="abstract-urdf-gripper/urdf/")
with open("abstract-urdf-gripper/urdf/GripperRight.urdf", "r") as f:
tm.load_urdf(f, mesh_path="abstract-urdf-gripper/urdf/")
tm.load_urdf(f.read(), mesh_path="abstract-urdf-gripper/urdf/")
except FileNotFoundError:
warnings.warn("URDF not found")

3 changes: 3 additions & 0 deletions examples/plot_conditional_promp.py
Original file line number Diff line number Diff line change
@@ -52,5 +52,8 @@
ax2.set_ylim((-2.5, 3))
ax2.legend(loc="best")

ax1.set_xlabel("Time $t$ [s]")
ax1.set_ylabel("Position $y$ [m]")
ax2.set_xlabel("Time $t$ [s]")
plt.tight_layout()
plt.show()
32 changes: 32 additions & 0 deletions examples/plot_critical_damping.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
"""
================
Critical Damping
================
The transformation system of a DMP converges to the goal and the convergence is
modeled as a spring-damper system. For an optimal convergence, the constants
defining the spring-damper system (spring constant k and damping coefficient c)
have to be set to critical damping for optimal convergence, that is, as quickly
as possible without overshooting. To illustrate this, we use a standalone
spring-damper system and explore several values for these parameters.
"""
print(__doc__)


import numpy as np
import matplotlib.pyplot as plt
from movement_primitives.spring_damper import SpringDamper


k = 100
start_y = np.zeros(1)
goal_y = np.ones(1)
for c in [10, 20, 40]:
attractor = SpringDamper(n_dims=1, k=k, c=c, dt=0.01)
attractor.configure(start_y=start_y, goal_y=goal_y)
T, Y = attractor.open_loop(run_t=2.0)
plt.plot(T, Y[:, 0], label=f"$k={k}, c={c}$")
plt.scatter(1.0, 1.0, marker="*", s=200, label="Goal")
plt.legend(loc="best")
plt.title(r"Condition for critical damping: $c = 2 \sqrt{k}$")
plt.show()
59 changes: 59 additions & 0 deletions examples/plot_dmp_2d_gain.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
"""
==================
Effect of DMP Gain
==================
Demonstrates how modifying DMP gains (alpha_y, beta_y) affects
the resulting trajectory reproduction.
"""
print(__doc__)


import matplotlib.pyplot as plt
import numpy as np
from movement_primitives.dmp import DMP


dt = 0.01

dmp1 = DMP(
n_dims=2,
execution_time=1.0,
dt=dt,
n_weights_per_dim=10,
int_dt=0.0001,
alpha_y=np.array([25.0, 25.0]),
beta_y=np.array([6.25, 6.25]),
)

dmp2 = DMP(
n_dims=2,
execution_time=1.0,
dt=dt,
n_weights_per_dim=10,
int_dt=0.0001,
alpha_y=np.array([25.0, 10.0]), # note different alpha_y
beta_y=np.array([6.25, 3.0]), # note different beta_y
)

T = np.linspace(0.0, 1.0, 101)
Y = np.empty((101, 2))
Y[:, 0] = np.cos(np.pi * T)
Y[:, 1] = np.sin(np.pi * T)

plt.plot(Y[:, 0], Y[:, 1], label="Demo")
dmps = [dmp1, dmp2]
for i, dmp in enumerate(dmps):

dmp.imitate(T, Y)
dmp.configure(start_y=Y[0], goal_y=Y[-1])
_, Y_ = dmp.open_loop()
plt.plot(Y_[:, 0], Y_[:, 1], label=f"Reproduction {i+1}")

plt.grid()
plt.gca().set_aspect("equal", "box")
plt.legend()
plt.xlabel("x")
plt.ylabel("y")

plt.show()
8 changes: 4 additions & 4 deletions examples/plot_dmp_potential_field.py
Original file line number Diff line number Diff line change
@@ -25,12 +25,12 @@
dmp.configure(start_y=start_y, goal_y=goal_y)

dmp_ft = DMP(n_dims=2, n_weights_per_dim=10, dt=0.01, execution_time=1.0)
dmp_ft.forcing_term.weights[:, :] = random_state.randn(
*dmp_ft.forcing_term.weights.shape) * 500.0
dmp_ft.forcing_term.weights_[:, :] = random_state.randn(
*dmp_ft.forcing_term.weights_.shape) * 500.0
dmp_ft.configure(start_y=start_y, goal_y=goal_y)

dmp_ct = DMP(n_dims=2, n_weights_per_dim=10, dt=0.01, execution_time=1.0)
dmp_ct.forcing_term.weights[:, :] = dmp_ft.forcing_term.weights[:, :]
dmp_ct.forcing_term.weights_[:, :] = dmp_ft.forcing_term.weights_[:, :]
dmp_ct.configure(start_y=start_y, goal_y=goal_y)
coupling_term = CouplingTermObstacleAvoidance2D(obstacle)

@@ -79,7 +79,7 @@
if i == n_subplots - 1:
break

while dmp.t <= dmp.execution_time * (1 + i) / (n_subplots - 1):
while dmp.t <= dmp.execution_time_ * (1 + i) / (n_subplots - 1):
position, velocity = dmp.step(position, velocity)
positions.append(position)
position_ft, velocity_ft = dmp_ft.step(position_ft, velocity_ft)
59 changes: 59 additions & 0 deletions examples/plot_dmp_scaling.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
"""
======================
Spatial Scaling of DMP
======================
The standard DMP definition of Ijspeert et al. (2013) does not scale well,
when the original demonstration has both the start and the goal state close
together: small perturbation in the start and/or goal state often result in
large accelerations. To fix this issue, our implementation of the DMP does
not scale the forcing term by $(g - y_0)$. We also adopted the modification of
P. Pastor, H. Hoffmann, T. Asfour, S. Schaal:
Learning and Generalization of Motor Skills by Learning from Demonstration
Note that this has to be activated explicitly by setting smooth scaling in the
constructor of the DMP. This behavior is implemented for all types of DMPs.
This example demonstrates the behavior of this DMP implementation in these
cases.
"""
import numpy as np
import matplotlib.pyplot as plt
from movement_primitives.dmp import DMP


T = np.linspace(0, 1, 101)
x = np.sin(T ** 2 * 1.99 * np.pi)
y = np.cos(T ** 2 * 1.99 * np.pi)
z = qx = qy = qz = np.zeros_like(x)
qw = np.ones_like(x)
Y = np.column_stack((x, y, z, qw, qx, qy, qz, x, y, z, qw, qx, qy, qz))
start = Y[0]
goal = Y[-1]
new_start = np.array([0.5, 0.5, 0, 1, 0, 0, 0, 0.5, 0.5, 0, 1, 0, 0, 0])
new_goal = np.array([-0.5, 0.5, 0, 1, 0, 0, 0, -0.5, 0.5, 0, 1, 0, 0, 0])
Y_start_shifted = Y - start[np.newaxis] + new_start[np.newaxis]
Y_goal_shifted = Y - goal[np.newaxis] + new_goal[np.newaxis]

dmp = DMP(n_dims=len(start), execution_time=1.0, dt=0.01, n_weights_per_dim=20,
smooth_scaling=True)
dmp.imitate(T, Y)
dmp.configure(start_y=new_start, goal_y=new_goal)
_, Y_dmp = dmp.open_loop()

plt.plot(Y[:, 0], Y[:, 1], label=r"Demonstration, $g \approx y_0$", ls="--")
plt.plot(Y_start_shifted[:, 0], Y_start_shifted[:, 1], label="Original shape with new start", ls="--")
plt.plot(Y_goal_shifted[:, 0], Y_goal_shifted[:, 1], label="Original shape with new goal", ls="--")
plt.plot(Y_dmp[:, 0], Y_dmp[:, 1], label="DMP with new goal", lw=5, alpha=0.5)
plt.scatter(Y_dmp[:, 0], Y_dmp[:, 1], alpha=0.5)
plt.scatter(goal[0], goal[1], c="r", label="Goal of demonstration: $g$")
plt.scatter(start[0], start[1], c="g", label="Start of demonstration: $y_0$")
plt.scatter(new_start[0], new_start[1], c="c", label="New start: $y_0'$")
plt.scatter(new_goal[0], new_goal[1], c="y", label="New goal: $g'$")
plt.xlabel("$y_1$")
plt.ylabel("$y_2$")
plt.legend(loc="best", ncol=2)
plt.xlim((-1.8, 2.1))
plt.ylim((-1.7, 2.2))
plt.tight_layout()
plt.show()
59 changes: 59 additions & 0 deletions examples/plot_dmp_stepwise_execution.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
"""
=========================
Stepwise Execution of DMP
=========================
Since DMPs are executed open loop in most examples, in this case we will
demonstrate how to perform stepwise execution of a DMP, which is necessary to
incorporate sensor data in the execution.
"""
import time
import numpy as np
import matplotlib.pyplot as plt
from movement_primitives.dmp import DMP, CouplingTermObstacleAvoidance2D


execution_time = 1.0
dt = 0.001
start_y = np.zeros(2)
goal_y = np.ones(2)

dmp = DMP(n_dims=2, execution_time=execution_time, dt=dt, n_weights_per_dim=3)
dmp.configure(start_y=start_y, goal_y=goal_y)
dmp.set_weights(np.array([-50.0, 100.0, 300.0, -200.0, -200.0, -200.0]))
obstacle_position = np.array([0.92, 0.5])
coupling_term = CouplingTermObstacleAvoidance2D(obstacle_position, fast=True)

# on a real system, these would be measured values
y = np.copy(dmp.start_y)
yd = np.copy(dmp.start_yd)

T = []
Y = []

total_time = 0.0
for t in np.arange(0.0, 2.0, dt): # on real system, t would be measured time
T.append(t)
Y.append(y)
start_time = time.time()
y, yd = dmp.step(y, yd, coupling_term, step_function="rk4-cython")
total_time += time.time() - start_time
T.append(2.0)
Y.append(y)

T = np.array(T)
Y = np.array(Y)

print("Total runtime of DMP: %g s" % total_time)

fig = plt.figure()
ax = fig.add_subplot(111)
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.plot(Y[:, 0], Y[:, 1], label="Obstacle avoidance")
ax.scatter(start_y[0], start_y[1], c="r", label="Start")
ax.scatter(goal_y[0], goal_y[1], c="g", label="Goal")
ax.scatter(obstacle_position[0], obstacle_position[1], c="y", label="Obstacle")
ax.legend()
plt.tight_layout()
plt.show()
6 changes: 3 additions & 3 deletions examples/plot_minimum_jerk.py
Original file line number Diff line number Diff line change
@@ -14,12 +14,12 @@
plt.figure()
plt.subplot(311)
plt.ylabel("$x$")
plt.plot(X[0, :])
plt.plot(X[:, 0])
plt.subplot(312)
plt.ylabel("$\dot{x}$")
plt.plot(Xd[0, :])
plt.plot(Xd[:, 0])
plt.subplot(313)
plt.xlabel("$t$")
plt.ylabel("$\ddot{x}$")
plt.plot(Xdd[0, :])
plt.plot(Xdd[:, 0])
plt.show()
2 changes: 1 addition & 1 deletion movement_primitives/_version.py
Original file line number Diff line number Diff line change
@@ -1 +1 @@
__version__ = "0.3.1"
__version__ = "0.9.0"
120 changes: 111 additions & 9 deletions movement_primitives/base.py
Original file line number Diff line number Diff line change
@@ -1,8 +1,13 @@
"""Base classes of movement primitives."""
"""Base Classes
============
Base classes of movement primitives."""
import abc
import numpy as np
from .utils import check_1d_array_length


class PointToPointMovement:
class PointToPointMovement(abc.ABC):
"""Base class for point to point movements (discrete motions).
Parameters
@@ -12,6 +17,44 @@ class PointToPointMovement:
n_vel_dims : int
Number of dimensions of the velocity that will be controlled.
Attributes
----------
n_dims : int
Number of state space dimensions.
n_vel_dims : int
Number of velocity dimensions.
t : float
Current time.
last_t : float
Time during last step.
start_y : array, shape (n_dims,)
Initial state.
start_yd : array, shape (n_vel_dims,)
Initial velocity.
start_ydd : array, shape (n_vel_dims,)
Initial acceleration.
goal_y : array, shape (n_dims,)
Goal state.
goal_yd : array, shape (n_vel_dims,)
Goal velocity.
goal_ydd : array, shape (n_vel_dims,)
Goal acceleration.
current_y : array, shape (n_dims,)
Current state.
current_yd : array, shape (n_vel_dims,)
Current velocity.
"""
def __init__(self, n_pos_dims, n_vel_dims):
self.n_dims = n_pos_dims
@@ -39,37 +82,96 @@ def configure(
Parameters
----------
t : float, optional
Time at current step
Time at current step.
start_y : array, shape (n_dims,)
Initial state
Initial state.
start_yd : array, shape (n_vel_dims,)
Initial velocity
Initial velocity.
start_ydd : array, shape (n_vel_dims,)
Initial acceleration
Initial acceleration.
goal_y : array, shape (n_dims,)
Goal state
Goal state.
goal_yd : array, shape (n_vel_dims,)
Goal velocity
Goal velocity.
goal_ydd : array, shape (n_vel_dims,)
Goal acceleration
Goal acceleration.
Raises
------
ValueError
If the length of the configured meta parameter is not correct.
"""
if t is not None:
self.t = t

if start_y is not None:
check_1d_array_length(start_y, "start_y", self.n_dims)
self.start_y = start_y
if start_yd is not None:
check_1d_array_length(start_yd, "start_yd", self.n_vel_dims)
self.start_yd = start_yd
if start_ydd is not None:
check_1d_array_length(start_ydd, "start_ydd", self.n_vel_dims)
self.start_ydd = start_ydd
if goal_y is not None:
check_1d_array_length(goal_y, "goal_y", self.n_dims)
self.goal_y = goal_y
if goal_yd is not None:
check_1d_array_length(goal_yd, "goal_yd", self.n_vel_dims)
self.goal_yd = goal_yd
if goal_ydd is not None:
check_1d_array_length(goal_ydd, "goal_ydd", self.n_vel_dims)
self.goal_ydd = goal_ydd

@abc.abstractmethod
def step(self, last_y, last_yd):
"""Perform step.
Parameters
----------
last_y : array, shape (n_dims,)
Last state.
last_yd : array, shape (n_dims,)
Last time derivative of state (e.g., velocity).
Returns
-------
y : array, shape (n_dims,)
Next state.
yd : array, shape (n_dims,)
Next time derivative of state (e.g., velocity).
"""

def n_steps_open_loop(self, last_y, last_yd, n_steps):
"""Perform 'n_steps' steps.
Parameters
----------
last_y : array, shape (n_dims,)
Last state.
last_yd : array, shape (n_dims,)
Last time derivative of state (e.g., velocity).
n_steps : int
Number of steps.
Returns
-------
y : array, shape (n_dims,)
Next state.
yd : array, shape (n_dims,)
Next time derivative of state (e.g., velocity).
"""
for _ in range(n_steps):
last_y, last_yd = self.step(last_y, last_yd)
return last_y, last_yd
24 changes: 5 additions & 19 deletions movement_primitives/data/__init__.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,8 @@
"""Tools for loading datasets."""
"""Data
====
Tools for loading datasets.
"""
from ._lasa import load_lasa
from ._minimum_jerk import generate_minimum_jerk
from ._toy_1d import generate_1d_trajectory_distribution
@@ -8,21 +12,3 @@
"load_lasa",
"generate_minimum_jerk",
"generate_1d_trajectory_distribution"]


try:
from ._mocap import (
smooth_dual_arm_trajectories_pq, smooth_single_arm_trajectories_pq,
transpose_dataset, load_mia_demo, load_kuka_demo, load_rh5_demo,
load_kuka_dataset)
mocap_available = True
except ImportError:
mocap_available = False
import warnings
warnings.warn("mocap library is not available")


__all__ += [
"smooth_dual_arm_trajectories_pq", "smooth_single_arm_trajectories_pq",
"load_mia_demo", "load_kuka_demo", "load_rh5_demo",
"load_kuka_dataset"]
26 changes: 11 additions & 15 deletions movement_primitives/data/_lasa.py
Original file line number Diff line number Diff line change
@@ -18,22 +18,11 @@ def load_lasa(shape_idx):
The LASA dataset contains 2D handwriting motions recorded from a
Tablet-PC. It can be found `here
<https://bitbucket.org/khansari/lasahandwritingdataset>`_
<https://bitbucket.org/khansari/lasahandwritingdataset>`_.
Take a look at the `detailed explanation
<http://cs.stanford.edu/people/khansari/DSMotions#SEDS_Benchmark_Dataset>`_
for more information.
The following plot shows multiple demonstrations for the same shape.
.. plot::
import matplotlib.pyplot as plt
from movement_primitives.data import load_lasa
X, Xd, Xdd, dt, shape_name = load_lasa(0)
plt.figure()
plt.title(shape_name)
plt.plot(X[:, :, 0].T, X[:, :, 1].T)
plt.show()
<https://cs.stanford.edu/people/khansari/download.html>`_
for more information. It was initially used to evaluate stable estimators
of dynamical systems [1]_.
Parameters
----------
@@ -60,6 +49,13 @@ def load_lasa(shape_idx):
shape_name : string
Name of the Matlab file from which we load the demonstrations
(without suffix).
References
----------
.. [1] Khansari-Zadeh, S. M., Billard, A. (2011).
Learning Stable Nonlinear Dynamical Systems With Gaussian Mixture Models.
IEEE Transactions on Robotics, 27 (5), 943-957. DOI:
10.1109/TRO.2011.2159412
"""
dataset_path = get_common_dataset_path()
if not os.path.isdir(dataset_path + "lasa_data"): # pragma: no cover
26 changes: 13 additions & 13 deletions movement_primitives/data/_minimum_jerk.py
Original file line number Diff line number Diff line change
@@ -35,13 +35,13 @@ def generate_minimum_jerk(start, goal, execution_time=1.0, dt=0.01):
Returns
-------
X : array, shape (n_dims, n_steps)
X : array, shape (n_steps, n_dims)
The positions of the trajectory
Xd : array, shape (n_dims, n_steps)
Xd : array, shape (n_steps, n_dims)
The velocities of the trajectory
Xdd : array, shape (n_task_dims, n_steps)
Xdd : array, shape (n_steps, n_dims)
The accelerations of the trajectory
Raises
@@ -55,18 +55,18 @@ def generate_minimum_jerk(start, goal, execution_time=1.0, dt=0.01):
raise ValueError("Shape of initial state %s and goal %s must be equal"
% (x0.shape, g.shape))

n_task_dims = x0.shape[0]
n_dims = x0.shape[0]
n_steps = 1 + int(execution_time / dt)

X = np.zeros((n_task_dims, n_steps))
Xd = np.zeros((n_task_dims, n_steps))
Xdd = np.zeros((n_task_dims, n_steps))
X = np.zeros((n_steps, n_dims))
Xd = np.zeros((n_steps, n_dims))
Xdd = np.zeros((n_steps, n_dims))

x = x0.copy()
xd = np.zeros(n_task_dims)
xdd = np.zeros(n_task_dims)
xd = np.zeros(n_dims)
xdd = np.zeros(n_dims)

X[:, 0] = x
X[0] = x
for t in range(1, n_steps):
tau = execution_time - t * dt

@@ -97,8 +97,8 @@ def generate_minimum_jerk(start, goal, execution_time=1.0, dt=0.01):
xd = (5. * c1 * t4 + 4 * c2 * t3 + 3 * c3 * t2 + 2 * c4 * t1 + c5)
xdd = (20. * c1 * t3 + 12. * c2 * t2 + 6. * c3 * t1 + 2. * c4)

X[:, t] = x
Xd[:, t] = xd
Xdd[:, t] = xdd
X[t] = x
Xd[t] = xd
Xdd[t] = xdd

return X, Xd, Xdd
304 changes: 0 additions & 304 deletions movement_primitives/data/_mocap.py

This file was deleted.

2 changes: 1 addition & 1 deletion movement_primitives/data/_toy_1d.py
Original file line number Diff line number Diff line change
@@ -71,7 +71,7 @@ def create_finite_differences_matrix_1d(n_steps, dt):
A : array, shape (n_steps + 2, n_steps)
Finite difference matrix.
"""
A = np.zeros((n_steps + 2, n_steps), dtype=np.float)
A = np.zeros((n_steps + 2, n_steps), dtype=float)
super_diagonal = (np.arange(n_steps), np.arange(n_steps))
sub_diagonal = (np.arange(2, n_steps + 2), np.arange(n_steps))
A[super_diagonal] = np.ones(n_steps)
38 changes: 30 additions & 8 deletions movement_primitives/dmp/__init__.py
Original file line number Diff line number Diff line change
@@ -1,23 +1,45 @@
"""Dynamical movement primitive variants."""
r"""Dynamical Movement Primitive (DMP)
==================================
This module provides implementations of various DMP types. DMPs consist of
a goal-directed movement generated by the transformation system and a forcing
term that defines the shape of the trajectory. They are time-dependent and
usually converge to the goal after a constant execution time.
Every implementation is slightly different, but we use a similar notation for
all of them:
* :math:`y,\dot{y},\ddot{y}` - position, velocity, and acceleration; these
might be translation components only, orientation, or both
* :math:`y_0` - start position
* :math:`g` - goal of the movement (attractor of the DMP)
* :math:`\tau` - execution time
* :math:`z` - phase variable, starts at 1 and converges to 0
* :math:`f(z)` - forcing term, learned component that defines the shape of the
movement
* :math:`C_t` - coupling term that is added to the acceleration of the DMP
"""
from ._base import DMPBase, WeightParametersMixin
from ._dmp import DMP, dmp_transformation_system
from ._dmp_with_final_velocity import DMPWithFinalVelocity
from ._cartesian_dmp import CartesianDMP
from ._dual_cartesian_dmp import DualCartesianDMP
from ._coupling_terms import (
CouplingTermObstacleAvoidance2D, CouplingTermObstacleAvoidance3D,
CouplingTermPos1DToPos1D, CouplingTermPos3DToPos3D,
CouplingTermDualCartesianOrientation, CouplingTermDualCartesianPose,
CouplingTermDualCartesianDistance, CouplingTermDualCartesianTrajectory,
obstacle_avoidance_acceleration_2d, obstacle_avoidance_acceleration_3d)
CouplingTermDualCartesianPose, CouplingTermDualCartesianDistance,
CouplingTermDualCartesianTrajectory, obstacle_avoidance_acceleration_2d,
obstacle_avoidance_acceleration_3d)
from ._state_following_dmp import StateFollowingDMP
from ._canonical_system import canonical_system_alpha, phase


__all__ = [
"DMPBase", "WeightParametersMixin",
"DMP", "dmp_transformation_system", "DMPWithFinalVelocity", "CartesianDMP",
"DualCartesianDMP", "CouplingTermPos1DToPos1D",
"CouplingTermObstacleAvoidance2D", "CouplingTermObstacleAvoidance3D",
"CouplingTermPos3DToPos3D", "CouplingTermDualCartesianOrientation",
"CouplingTermDualCartesianPose", "CouplingTermDualCartesianDistance",
"CouplingTermDualCartesianTrajectory", "canonical_system_alpha", "phase",
"obstacle_avoidance_acceleration_2d", "obstacle_avoidance_acceleration_3d"]
"CouplingTermPos3DToPos3D", "CouplingTermDualCartesianPose",
"CouplingTermDualCartesianDistance", "CouplingTermDualCartesianTrajectory",
"canonical_system_alpha", "phase", "obstacle_avoidance_acceleration_2d",
"obstacle_avoidance_acceleration_3d"]
44 changes: 43 additions & 1 deletion movement_primitives/dmp/_base.py
Original file line number Diff line number Diff line change
@@ -3,7 +3,16 @@


class DMPBase(PointToPointMovement):
"""Base class of Dynamical Movement Primitives (DMPs)."""
"""Base class of Dynamical Movement Primitives (DMPs).
Parameters
----------
n_pos_dims : int
Number of dimensions of the position that will be controlled.
n_vel_dims : int
Number of dimensions of the velocity that will be controlled.
"""
def __init__(self, n_pos_dims, n_vel_dims):
super(DMPBase, self).__init__(n_pos_dims, n_vel_dims)

@@ -15,3 +24,36 @@ def reset(self):
self.last_t = None
self.current_y = np.copy(self.start_y)
self.current_yd = np.copy(self.start_yd)


class WeightParametersMixin:
"""Mixin class providing common access methods to forcing term weights.
This can be used, for instance, for black-box optimization of the weights
with respect to some cost / objective function in a reinforcement learning
setting.
"""
def get_weights(self):
"""Get weight vector of DMP.
Returns
-------
weights : array, shape (N * n_weights_per_dim,)
Current weights of the DMP. N depends on the type of DMP
"""
return self.forcing_term.weights_.ravel()

def set_weights(self, weights):
"""Set weight vector of DMP.
Parameters
----------
weights : array, shape (N * n_weights_per_dim,)
New weights of the DMP. N depends on the type of DMP
"""
self.forcing_term.weights_[:, :] = weights.reshape(*self.forcing_term.shape)

@property
def n_weights(self):
"""Total number of weights configuring the forcing term."""
return np.prod(self.forcing_term.shape)
64 changes: 44 additions & 20 deletions movement_primitives/dmp/_canonical_system.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,12 @@
def canonical_system_alpha(goal_z, goal_t, start_t, int_dt=0.001):
"""Compute parameter alpha of canonical system.
import numpy as np


def canonical_system_alpha(goal_z, goal_t, start_t):
r"""Compute parameter alpha of canonical system.
The parameter alpha is computed such that a specific phase value goal_z
is reached at goal_t. The canonical system is defined according to [1]_,
even though we compute a different value for alpha.
Parameters
----------
@@ -13,9 +20,6 @@ def canonical_system_alpha(goal_z, goal_t, start_t, int_dt=0.001):
start_t : float
Time at which the execution should start.
int_dt : float, optional (default: 0.001)
Time delta that is used internally for integration.
Returns
-------
alpha : float
@@ -25,21 +29,40 @@ def canonical_system_alpha(goal_z, goal_t, start_t, int_dt=0.001):
------
ValueError
If input values are invalid.
References
----------
.. [1] Ijspeert, A. J., Nakanishi, J., Hoffmann, H., Pastor, P., Schaal, S.
(2013). Dynamical Movement Primitives: Learning Attractor Models for
Motor Behaviors. Neural Computation 25 (2), 328-373. DOI:
10.1162/NECO_a_00393,
https://homes.cs.washington.edu/~todorov/courses/amath579/reading/DynamicPrimitives.pdf
"""
if goal_z <= 0.0:
raise ValueError("Final phase must be > 0!")
if start_t >= goal_t:
raise ValueError("Goal must be chronologically after start!")

execution_time = goal_t - start_t
n_phases = int(execution_time / int_dt) + 1
# assert that the execution_time is approximately divisible by int_dt
assert abs(((n_phases - 1) * int_dt) - execution_time) < 0.05
return (1.0 - goal_z ** (1.0 / (n_phases - 1))) * (n_phases - 1)
return float(-np.log(goal_z))


def phase(t, alpha, goal_t, start_t):
r"""Map time to phase.
According to [1]_, the differential Equation
def phase(t, alpha, goal_t, start_t, int_dt=0.001, eps=1e-10):
"""Map time to phase.
.. math::
\tau \dot{z} = -\alpha_z z
describes the evolution of the phase variable z. Starting from the initial
position :math:`z_0 = 1`, the phase value converges monotonically to 0.
Instead of using an iterative procedure to calculate the current value of
z, it is computed directly through
.. math::
z(t) = \exp - \frac{\alpha_z}{\tau} t
Parameters
----------
@@ -55,17 +78,18 @@ def phase(t, alpha, goal_t, start_t, int_dt=0.001, eps=1e-10):
start_t : float
Time at which the execution should start.
int_dt : float, optional (default: 0.001)
Time delta that is used internally for integration.
eps : float, optional (default: 1e-10)
Small number used to avoid numerical issues.
Returns
-------
z : float
Value of phase variable.
References
----------
.. [1] Ijspeert, A. J., Nakanishi, J., Hoffmann, H., Pastor, P., Schaal, S.
(2013). Dynamical Movement Primitives: Learning Attractor Models for
Motor Behaviors. Neural Computation 25 (2), 328-373. DOI:
10.1162/NECO_a_00393,
https://homes.cs.washington.edu/~todorov/courses/amath579/reading/DynamicPrimitives.pdf
"""
execution_time = goal_t - start_t
b = max(1.0 - alpha * int_dt / execution_time, eps)
return b ** ((t - start_t) / int_dt)
return np.exp(-alpha * (t - start_t) / execution_time)
307 changes: 220 additions & 87 deletions movement_primitives/dmp/_cartesian_dmp.py

Large diffs are not rendered by default.

300 changes: 244 additions & 56 deletions movement_primitives/dmp/_coupling_terms.py
Original file line number Diff line number Diff line change
@@ -8,18 +8,11 @@

EPSILON = 1e-10


class CouplingTermObstacleAvoidance2D: # for DMP
"""Coupling term for obstacle avoidance in 2D."""
def __init__(self, obstacle_position, gamma=1000.0, beta=20.0 / math.pi):
self.obstacle_position = obstacle_position
self.gamma = gamma
self.beta = beta

def coupling(self, y, yd):
cdd = obstacle_avoidance_acceleration_2d(
y, yd, self.obstacle_position, self.gamma, self.beta)
return np.zeros_like(cdd), cdd
try:
from ..dmp_fast import obstacle_avoidance_acceleration_2d as obstacle_avoidance_acceleration_2d_fast
obstacle_avoidance_acceleration_2d_fast_available = True
except ImportError:
obstacle_avoidance_acceleration_2d_fast_available = False


def obstacle_avoidance_acceleration_2d(
@@ -72,14 +65,131 @@ def obstacle_avoidance_acceleration_2d(
return np.squeeze(cdd)


class CouplingTermObstacleAvoidance2D:
"""Coupling term for obstacle avoidance in 2D.
For :class:`DMP` and :class:`DMPWithFinalVelocity`.
This is the simplified 2D implementation of
:class:`CouplingTermObstacleAvoidance3D`.
Parameters
----------
obstacle_position : array, shape (2,)
Position of the point obstacle.
gamma : float, optional (default: 1000)
Parameter of obstacle avoidance.
beta : float, optional (default: 20 / pi)
Parameter of obstacle avoidance.
"""
def __init__(self, obstacle_position, gamma=1000.0, beta=20.0 / math.pi,
fast=False):
self.obstacle_position = obstacle_position
self.gamma = gamma
self.beta = beta
if fast and obstacle_avoidance_acceleration_2d_fast_available:
self.step_function = obstacle_avoidance_acceleration_2d_fast
else:
self.step_function = obstacle_avoidance_acceleration_2d

def coupling(self, y, yd):
"""Computes coupling term based on current state.
Parameters
----------
y : array, shape (n_dims,)
Current position.
yd : array, shape (n_dims,)
Current velocity.
Returns
-------
cd : array, shape (n_dims,)
Velocity. 0 for this coupling term.
cdd : array, shape (n_dims,)
Acceleration.
"""
cdd = self.step_function(
y, yd, self.obstacle_position, self.gamma, self.beta)
return np.zeros_like(cdd), cdd


class CouplingTermObstacleAvoidance3D: # for DMP
"""Coupling term for obstacle avoidance in 3D."""
r"""Coupling term for obstacle avoidance in 3D.
For :class:`DMP` and :class:`DMPWithFinalVelocity`.
Following [1]_, this coupling term adds an acceleration
.. math::
\boldsymbol{C}_t = \gamma \boldsymbol{R} \dot{\boldsymbol{y}}
\theta \exp(-\beta \theta),
where
.. math::
\theta = \arccos\left( \frac{(\boldsymbol{o} - \boldsymbol{y})^T
\dot{\boldsymbol{y}}}{|\boldsymbol{o} - \boldsymbol{y}|
|\dot{\boldsymbol{y}}|} \right)
and a rotation axis :math:`\boldsymbol{r} =
(\boldsymbol{o} - \boldsymbol{y}) \times \dot{\boldsymbol{y}}` used to
compute the rotation matrix :math:`\boldsymbol{R}` that rotates about it
by 90 degrees for an obstacle at position :math:`\boldsymbol{o}`.
Intuitively, this coupling term adds a movement perpendicular to the
current velocity in the plane defined by the direction to the obstacle and
the current movement direction.
Parameters
----------
obstacle_position : array, shape (3,)
Position of the point obstacle.
gamma : float, optional (default: 1000)
Parameter of obstacle avoidance.
beta : float, optional (default: 20 / pi)
Parameter of obstacle avoidance.
References
----------
.. [1] Ijspeert, A. J., Nakanishi, J., Hoffmann, H., Pastor, P., Schaal, S.
(2013). Dynamical Movement Primitives: Learning Attractor Models for
Motor Behaviors. Neural Computation 25 (2), 328-373. DOI:
10.1162/NECO_a_00393,
https://homes.cs.washington.edu/~todorov/courses/amath579/reading/DynamicPrimitives.pdf
"""
def __init__(self, obstacle_position, gamma=1000.0, beta=20.0 / math.pi):
self.obstacle_position = obstacle_position
self.gamma = gamma
self.beta = beta

def coupling(self, y, yd):
"""Computes coupling term based on current state.
Parameters
----------
y : array, shape (n_dims,)
Current position.
yd : array, shape (n_dims,)
Current velocity.
Returns
-------
cd : array, shape (n_dims,)
Velocity. 0 for this coupling term.
cdd : array, shape (n_dims,)
Acceleration.
"""
cdd = obstacle_avoidance_acceleration_3d(
y, yd, self.obstacle_position, self.gamma, self.beta)
return np.zeros_like(cdd), cdd
@@ -124,10 +234,7 @@ def obstacle_avoidance_acceleration_3d(
class CouplingTermPos1DToPos1D:
"""Couples position components of a 2D DMP with a virtual spring.
A. Gams, B. Nemec, L. Zlajpah, M. Wächter, T. Asfour, A. Ude:
Modulation of Motor Primitives using Force Feedback: Interaction with
the Environment and Bimanual Tasks (2013), IROS,
https://h2t.anthropomatik.kit.edu/pdf/Gams2013.pdf
For :class:`DMP` and :class:`DMPWithFinalVelocity`.
Parameters
----------
@@ -153,6 +260,15 @@ class CouplingTermPos1DToPos1D:
c2 : float, optional (default: 30)
Scaling factor for spring forces in the acceleration component.
References
----------
.. [1] Gams, A., Nemec, B., Zlajpah, L., Wächter, M., Asfour, T., Ude, A.
(2013). Modulation of Motor Primitives using Force Feedback: Interaction
with the Environment and Bimanual Tasks (2013), In 2013 IEEE/RSJ
International Conference on Intelligent Robots and Systems (pp.
5629-5635). DOI: 10.1109/IROS.2013.6697172,
https://h2t.anthropomatik.kit.edu/pdf/Gams2013.pdf
"""
def __init__(self, desired_distance, lf, k=1.0, c1=100.0, c2=30.0):
self.desired_distance = desired_distance
@@ -172,13 +288,10 @@ def coupling(self, y, yd=None):
return np.array([C12, C21]), np.array([C12dot, C21dot])


class CouplingTermPos3DToPos3D: # for DMP
class CouplingTermPos3DToPos3D:
"""Couples position components of a 6D DMP with a virtual spring in 3D.
A. Gams, B. Nemec, L. Zlajpah, M. Wächter, T. Asfour, A. Ude:
Modulation of Motor Primitives using Force Feedback: Interaction with
the Environment and Bimanual Tasks (2013), IROS,
https://h2t.anthropomatik.kit.edu/pdf/Gams2013.pdf
For :class:`DMP` and :class:`DMPWithFinalVelocity`.
Parameters
----------
@@ -204,6 +317,15 @@ class CouplingTermPos3DToPos3D: # for DMP
c2 : float, optional (default: 30)
Scaling factor for spring forces in the acceleration component.
References
----------
.. [1] Gams, A., Nemec, B., Zlajpah, L., Wächter, M., Asfour, T., Ude, A.
(2013). Modulation of Motor Primitives using Force Feedback: Interaction
with the Environment and Bimanual Tasks (2013), In 2013 IEEE/RSJ
International Conference on Intelligent Robots and Systems (pp.
5629-5635). DOI: 10.1109/IROS.2013.6697172,
https://h2t.anthropomatik.kit.edu/pdf/Gams2013.pdf
"""
def __init__(self, desired_distance, lf, k=1.0, c1=1.0, c2=30.0):
self.desired_distance = desired_distance
@@ -226,8 +348,36 @@ def coupling(self, y, yd=None):
return np.hstack([C12, C21]), np.hstack([C12dot, C21dot])


class CouplingTermDualCartesianDistance: # for DualCartesianDMP
"""Couples distance between 3D positions of a dual Cartesian DMP."""
class CouplingTermDualCartesianDistance:
"""Couples distance between 3D positions of a dual Cartesian DMP.
For :class:`DualCartesianDMP`.
Parameters
----------
desired_distance : float
Desired distance between components.
lf : array-like, shape (2,)
Binary values that indicate which DMP(s) will be adapted.
The variable lf defines the relation leader-follower. If lf[0] = lf[1],
then both robots will adapt their trajectories to follow average
trajectories at the defined distance dd between them [..]. On the other
hand, if lf[0] = 0 and lf[1] = 1, only DMP1 will change the trajectory
to match the trajectory of DMP0, again at the distance dd and again
only after learning. Vice versa applies as well. Leader-follower
relation can be determined by a higher-level planner [..].
k : float, optional (default: 1)
Virtual spring constant that couples the positions.
c1 : float, optional (default: 100)
Scaling factor for spring forces in the velocity component and
acceleration component.
c2 : float, optional (default: 30)
Scaling factor for spring forces in the acceleration component.
"""
def __init__(self, desired_distance, lf, k=1.0, c1=1.0, c2=30.0):
self.desired_distance = desired_distance
self.lf = lf
@@ -249,39 +399,42 @@ def coupling(self, y, yd=None):
np.hstack([C12dot, np.zeros(3), C21dot, np.zeros(3)]))


class CouplingTermDualCartesianOrientation: # for DualCartesianDMP
"""Couples orientations of dual Cartesian DMP."""
def __init__(self, desired_distance, lf, k=1.0, c1=1.0, c2=30.0):
self.desired_distance = desired_distance
self.lf = lf
self.k = k
self.c1 = c1
self.c2 = c2
class CouplingTermDualCartesianPose:
"""Couples relative poses of dual Cartesian DMP.
def coupling(self, y, yd=None):
q1 = y[3:7]
q2 = y[10:]
actual_distance = pr.compact_axis_angle_from_quaternion(
pr.concatenate_quaternions(q1, pr.q_conj(q2)))
actual_distance_norm = np.linalg.norm(actual_distance)
if actual_distance_norm < np.finfo("float").eps:
desired_distance = (np.abs(self.desired_distance)
* np.array([0.0, 0.0, 1.0]))
else:
desired_distance = (np.abs(self.desired_distance) * actual_distance
/ actual_distance_norm)
F12 = self.k * (desired_distance - actual_distance)
F21 = -F12
C12 = self.c1 * F12 * self.lf[0]
C21 = self.c1 * F21 * self.lf[1]
C12dot = F12 * self.c2 * self.lf[0]
C21dot = F21 * self.c2 * self.lf[1]
return (np.hstack([np.zeros(3), C12, np.zeros(3), C21]),
np.hstack([np.zeros(3), C12dot, np.zeros(3), C21dot]))
For :class:`DualCartesianDMP`.
Parameters
----------
desired_distance : array, shape (4, 4)
Desired distance between components.
lf : array-like, shape (2,)
Binary values that indicate which DMP(s) will be adapted.
The variable lf defines the relation leader-follower. If lf[0] = lf[1],
then both robots will adapt their trajectories to follow average
trajectories at the defined distance dd between them [..]. On the other
hand, if lf[0] = 0 and lf[1] = 1, only DMP1 will change the trajectory
to match the trajectory of DMP0, again at the distance dd and again
only after learning. Vice versa applies as well. Leader-follower
relation can be determined by a higher-level planner [..].
couple_position : bool, optional (default: True)
Couple position between components.
couple_orientation : bool, optional (default: True)
Couple orientation between components.
k : float, optional (default: 1)
Virtual spring constant that couples the positions.
c1 : float, optional (default: 100)
Scaling factor for spring forces in the velocity component and
acceleration component.
class CouplingTermDualCartesianPose: # for DualCartesianDMP
"""Couples relative poses of dual Cartesian DMP."""
c2 : float, optional (default: 30)
Scaling factor for spring forces in the acceleration component.
"""
def __init__(self, desired_distance, lf, couple_position=True,
couple_orientation=True, k=1.0, c1=1.0, c2=30.0, verbose=0):
self.desired_distance = desired_distance
@@ -372,8 +525,43 @@ def _right2left_pq(self, y):
return right2left_pq


class CouplingTermDualCartesianTrajectory(CouplingTermDualCartesianPose): # for DualCartesianDMP
"""Couples relative pose in dual Cartesian DMP with a given trajectory."""
class CouplingTermDualCartesianTrajectory(CouplingTermDualCartesianPose):
"""Couples relative pose in dual Cartesian DMP with a given trajectory.
For :class:`DualCartesianDMP`.
Parameters
----------
offset : array, shape (7,)
Offset for desired distance between components as position and
quaternion.
lf : array-like, shape (2,)
Binary values that indicate which DMP(s) will be adapted.
The variable lf defines the relation leader-follower. If lf[0] = lf[1],
then both robots will adapt their trajectories to follow average
trajectories at the defined distance dd between them [..]. On the other
hand, if lf[0] = 0 and lf[1] = 1, only DMP1 will change the trajectory
to match the trajectory of DMP0, again at the distance dd and again
only after learning. Vice versa applies as well. Leader-follower
relation can be determined by a higher-level planner [..].
couple_position : bool, optional (default: True)
Couple position between components.
couple_orientation : bool, optional (default: True)
Couple orientation between components.
k : float, optional (default: 1)
Virtual spring constant that couples the positions.
c1 : float, optional (default: 100)
Scaling factor for spring forces in the velocity component and
acceleration component.
c2 : float, optional (default: 30)
Scaling factor for spring forces in the acceleration component.
"""
def __init__(self, offset, lf, dt, couple_position=True,
couple_orientation=True, k=1.0, c1=1.0, c2=30.0, verbose=1):
self.offset = offset
387 changes: 262 additions & 125 deletions movement_primitives/dmp/_dmp.py

Large diffs are not rendered by default.

155 changes: 111 additions & 44 deletions movement_primitives/dmp/_dmp_with_final_velocity.py
Original file line number Diff line number Diff line change
@@ -1,30 +1,30 @@
import numpy as np
from ._base import DMPBase
from ._forcing_term import ForcingTerm
from ._base import DMPBase, WeightParametersMixin
from ..utils import ensure_1d_array
from ._forcing_term import ForcingTerm, phase
from ._canonical_system import canonical_system_alpha
from ._dmp import dmp_imitate, dmp_open_loop


class DMPWithFinalVelocity(DMPBase):
"""Dynamical movement primitive (DMP) with final velocity.
class DMPWithFinalVelocity(WeightParametersMixin, DMPBase):
r"""Dynamical movement primitive (DMP) with final velocity.
Implementation according to
Equation of transformation system (according to [1]_, Eq. 6):
K. Muelling, J. Kober, O. Kroemer, J. Peters:
Learning to Select and Generalize Striking Movements in Robot Table Tennis
(2013), International Journal of Robotics Research 32(3), pp. 263-279,
https://www.ias.informatik.tu-darmstadt.de/uploads/Publications/Muelling_IJRR_2013.pdf
.. math::
\ddot{y} = (\alpha_y (\beta_y (g - y) + \tau\dot{g} - \tau \dot{y}) + f(z))/\tau^2
Parameters
----------
n_dims : int
State space dimensions.
execution_time : float
Execution time of the DMP.
execution_time : float, optional (default: 1)
Execution time of the DMP: :math:`\tau`.
dt : float, optional (default: 0.01)
Time difference between DMP steps.
Time difference between DMP steps: :math:`\Delta t`.
n_weights_per_dim : int, optional (default: 10)
Number of weights of the function approximator per dimension.
@@ -36,28 +36,60 @@ class DMPWithFinalVelocity(DMPBase):
Gain for proportional controller of DMP tracking error.
The domain is [0, execution_time**2/dt].
alpha_y : float or array-like, shape (n_dims,), optional (default: 25.0)
Parameter of the transformation system.
beta_y : float or array-like, shape (n_dims,), optional (default: 6.25)
Parameter of the transformation system.
Attributes
----------
execution_time_ : float
Execution time of the DMP.
dt_ : float
Time difference between DMP steps. This value can be changed to adapt
the frequency.
References
----------
.. [1] Muelling, K., Kober, J., Kroemer, O., Peters, J. (2013). Learning to
Select and Generalize Striking Movements in Robot Table Tennis.
International Journal of Robotics Research 32 (3), 263-279.
https://www.ias.informatik.tu-darmstadt.de/uploads/Publications/Muelling_IJRR_2013.pdf
"""
def __init__(self, n_dims, execution_time, dt=0.01, n_weights_per_dim=10,
int_dt=0.001, p_gain=0.0):
def __init__(self, n_dims, execution_time=1.0, dt=0.01,
n_weights_per_dim=10, int_dt=0.001, p_gain=0.0,
alpha_y=25.0, beta_y=6.25):
super(DMPWithFinalVelocity, self).__init__(n_dims, n_dims)
self.execution_time = execution_time
self._execution_time = execution_time
self.dt_ = dt
self.n_weights_per_dim = n_weights_per_dim
self.int_dt = int_dt
self.p_gain = p_gain
self.smooth_scaling = False # it is build into this DMP type already

self._init_forcing_term()

alpha_z = canonical_system_alpha(0.01, self.execution_time, 0.0,
self.int_dt)
self.forcing_term = ForcingTerm(self.n_dims, self.n_weights_per_dim,
self.execution_time, 0.0, 0.8, alpha_z)
self.alpha_y = ensure_1d_array(alpha_y, n_dims, "alpha_y")
self.beta_y = ensure_1d_array(beta_y, n_dims, "beta_y")

self.alpha_y = 25.0
self.beta_y = self.alpha_y / 4.0
def _init_forcing_term(self):
alpha_z = canonical_system_alpha(0.01, self.execution_time_, 0.0)
self.forcing_term = ForcingTerm(
self.n_dims, self.n_weights_per_dim, self.execution_time_,
0.0, 0.8, alpha_z)

def get_execution_time_(self):
return self._execution_time

def set_execution_time_(self, execution_time):
self._execution_time = execution_time
weights = self.forcing_term.weights_
self._init_forcing_term()
self.forcing_term.weights_ = weights

execution_time_ = property(get_execution_time_, set_execution_time_)

def step(self, last_y, last_yd, coupling_term=None):
"""DMP step.
@@ -97,13 +129,14 @@ def step(self, last_y, last_yd, coupling_term=None):
self.current_y, self.current_yd,
self.goal_y, self.goal_yd, self.goal_ydd,
self.start_y, self.start_yd, self.start_ydd,
self.execution_time, 0.0,
self.execution_time_, 0.0,
self.alpha_y, self.beta_y,
self.forcing_term,
coupling_term=coupling_term,
int_dt=self.int_dt,
p_gain=self.p_gain,
tracking_error=tracking_error)
tracking_error=tracking_error,
smooth_scaling=self.smooth_scaling)
return np.copy(self.current_y), np.copy(self.current_yd)

def open_loop(self, run_t=None, coupling_term=None):
@@ -126,18 +159,34 @@ def open_loop(self, run_t=None, coupling_term=None):
State at each step.
"""
return dmp_open_loop(
self.execution_time, 0.0, self.dt_,
self.execution_time_, 0.0, self.dt_,
self.start_y, self.goal_y,
self.alpha_y, self.beta_y,
self.forcing_term,
coupling_term,
run_t, self.int_dt,
dmp_step_euler_with_constraints,
start_yd=self.start_yd, start_ydd=self.start_ydd,
goal_yd=self.goal_yd, goal_ydd=self.goal_ydd)
goal_yd=self.goal_yd, goal_ydd=self.goal_ydd,
smooth_scaling=self.smooth_scaling)

def imitate(self, T, Y, regularization_coefficient=0.0):
"""Imitate demonstration.
r"""Imitate demonstration.
Target forces of the forcing term are computed according to
.. math::
f_{target} =
\tau^2 \ddot{y}_{demo}
- \alpha_y(
\beta_y (g-y_{demo})
+ \tau (\dot{g} - \dot{y}_{demo})
)
- \tau^2 \ddot{g},
where :math:`g, \dot{g}, \ddot{g}` are constraints that will be
recomputed in each step.
Parameters
----------
@@ -150,7 +199,8 @@ def imitate(self, T, Y, regularization_coefficient=0.0):
regularization_coefficient : float, optional (default: 0)
Regularization coefficient for regression.
"""
self.forcing_term.weights[:, :], start_y, start_yd, start_ydd, goal_y, goal_yd, goal_ydd = dmp_imitate(
self.forcing_term.weights_[:, :], start_y, start_yd, \
start_ydd, goal_y, goal_yd, goal_ydd = dmp_imitate(
T, Y,
n_weights_per_dim=self.n_weights_per_dim,
regularization_coefficient=regularization_coefficient,
@@ -208,7 +258,8 @@ def apply_constraints(t, goal_y, goal_t, coefficients):
return g, gd, gdd


def determine_forces(T, Y, alpha_y, beta_y, allow_final_velocity):
def determine_forces(T, Y, alpha_y, beta_y, alpha_z, allow_final_velocity,
smooth_scaling=False):
"""Determine forces that the forcing term should generate.
Parameters
@@ -219,16 +270,23 @@ def determine_forces(T, Y, alpha_y, beta_y, allow_final_velocity):
Y : array, shape (n_steps, n_dims)
Position at each step.
alpha_y : float
alpha_y : array, shape (n_dims,)
Parameter of the transformation system.
beta_y : float
beta_y : array, shape (n_dims,)
Parameter of the transformation system.
alpha_z : float
Parameter of the canonical system.
allow_final_velocity : bool
Whether a final velocity is allowed. This should always be True for
this function.
smooth_scaling : bool, optional (default: False)
Not used. This DMP type adapts smoothly to goal changes as constraint
coefficients will be recomputed in each step.
Returns
-------
F : array, shape (n_steps, n_dims)
@@ -272,19 +330,20 @@ def determine_forces(T, Y, alpha_y, beta_y, allow_final_velocity):

execution_time = T[-1] - T[0]
F = np.empty((len(T), n_dims))
for i in range(len(T)):
g, gd, gdd = apply_constraints(T[i], Y[-1], T[-1], coefficients)
F[i, :] = execution_time ** 2 * Ydd[i] - alpha_y * (
beta_y * (g - Y[i]) + gd * execution_time
- Yd[i] * execution_time) - execution_time ** 2 * gdd
for t in range(len(T)):
g, gd, gdd = apply_constraints(T[t], Y[-1], T[-1], coefficients)
F[t, :] = execution_time ** 2 * Ydd[t] - alpha_y * (
beta_y * (g - Y[t])
+ execution_time * (gd - Yd[t])
) - execution_time ** 2 * gdd
return F, Y[0], Yd[0], Ydd[0], Y[-1], Yd[-1], Ydd[-1]


def dmp_step_euler_with_constraints(
last_t, t, current_y, current_yd, goal_y, goal_yd, goal_ydd,
start_y, start_yd, start_ydd, goal_t, start_t, alpha_y, beta_y,
forcing_term, coupling_term=None, coupling_term_precomputed=None,
int_dt=0.001, p_gain=0.0, tracking_error=0.0):
int_dt=0.001, p_gain=0.0, tracking_error=0.0, smooth_scaling=False):
"""Integrate regular DMP for one step with Euler integration.
Parameters
@@ -325,10 +384,10 @@ def dmp_step_euler_with_constraints(
start_t : float
Time at the start.
alpha_y : float
alpha_y : array, shape (n_dims,)
Constant in transformation system.
beta_y : float
beta_y : array, shape (n_dims,)
Constant in transformation system.
forcing_term : ForcingTerm
@@ -350,6 +409,10 @@ def dmp_step_euler_with_constraints(
tracking_error : float, optional (default: 0)
Tracking error from last step.
smooth_scaling : bool, optional (default: False)
Not used. This DMP type adapts smoothly to goal changes as constraint
coefficients will be recomputed in each step.
"""
if start_t >= goal_t:
raise ValueError("Goal must be chronologically after start!")
@@ -378,15 +441,19 @@ def dmp_step_euler_with_constraints(
cd += coupling_term_precomputed[0]
cdd += coupling_term_precomputed[1]

f = forcing_term(current_t).squeeze()
z = forcing_term.phase(current_t)
f = forcing_term.forcing_term(z).squeeze()

g, gd, gdd = apply_constraints(current_t, goal_y, goal_t, coefficients)

coupling_sum = cdd + p_gain * tracking_error / dt
ydd = (alpha_y * (beta_y * (g - current_y)
+ execution_time * gd
- execution_time * current_yd)
+ gdd * execution_time ** 2
+ f + coupling_sum) / execution_time ** 2
ydd = (
alpha_y * (
beta_y * (g - current_y)
+ execution_time * (gd - current_yd)
)
+ f
+ coupling_sum
) / execution_time ** 2 + gdd
current_yd += dt * ydd + cd / execution_time
current_y += dt * current_yd
Loading