Torque driven predictive simulation example

docs/conf.py

@@ -9,6 +9,10 @@
 import algait2de
 import pygait2d
 
+DOCS_CONF_PATH = os.path.realpath(__file__)
+DOCS_DIR = os.path.dirname(DOCS_CONF_PATH)
+REPO_DIR = os.path.realpath(os.path.join(DOCS_DIR, '..'))
+
 # -- Project information -----------------------------------------------------
 # https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information
 
@@ -27,7 +31,7 @@
     'sphinx.ext.mathjax',
     'sphinx.ext.napoleon',
     'sphinx.ext.viewcode',
-    #'sphinx_gallery.gen_gallery',
+    'sphinx_gallery.gen_gallery',
 ]
 
 templates_path = ['_templates']
@@ -43,5 +47,24 @@
 html_theme = 'alabaster'
 html_static_path = ['_static']
 
+# Theme options are theme-specific and customize the look and feel of a theme
+# further.  For a list of options available for each theme, see the
+# documentation.
+#
+html_theme_options = {
+    'github_repo': 'gait2d',
+    'github_type': 'star',
+    'github_user': 'csu-hmc',
+    'page_width': '1080px',  # 960 doesn't show 79 linewidth examples
+}
+
 # Display long function signatures better.
 maximum_signature_line_length = 50
+
+# -- sphinx-gallery settings --------------------------------------------------
+sphinx_gallery_conf = {
+    'examples_dirs': os.path.join(REPO_DIR, 'examples'),
+    'gallery_dirs': 'examples',
+    'matplotlib_animations': True,
+    'remove_config_comments': True,
+}

docs/index.rst

@@ -10,6 +10,7 @@ Gait2D documentation
    :maxdepth: 2
    :caption: Contents:
 
+   examples/index.rst
    api.rst
 
 .. include:: ../README.rst

examples/GALLERY_HEADER.rst

@@ -0,0 +1,3 @@
+========
+Examples
+========

examples/animate.ipynb

@@ -10,6 +10,21 @@
     "run run.py"
    ]
   },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "56e6819d-3a31-44e9-a135-79f35f04c6cf",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from pydy.viz import Scene\n",
+    "scene = Scene(symbolics.inertial_frame, symbolics.origin, *symbolics.viz_frames)\n",
+    "scene.states_symbols = symbolics.states\n",
+    "scene.constants = constant_values\n",
+    "scene.states_trajectories = trajectories\n",
+    "scene.times = time_vector"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": null,
@@ -37,7 +52,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.12.5"
+   "version": "3.14.2"
   }
  },
  "nbformat": 4,

examples/plot_forward_sim.py

@@ -0,0 +1,101 @@
+"""
+Forward Simulation
+==================
+
+This example simulates and visualizes the uncontrolled motion of the model such
+that the model falls down on the treadmill. It also compares the evaluation
+speed of PyDy's and Autolev's models.
+"""
+import timeit
+
+from algait2de.gait2de import evaluate_autolev_rhs
+from pydy.codegen.ode_function_generators import generate_ode_function
+from pygait2d import derive, simulate
+from pygait2d import utils
+from scipy.integrate import odeint
+import matplotlib.pyplot as plt
+import numpy as np
+import sympy as sm
+import yaml
+
+# %%
+# Derive the equations of motion, including a constant treadmill motion.
+symbolics = derive.derive_equations_of_motion(treadmill=True)
+
+# %%
+# Load a parameter mapping from pygait2d symbol to numerical value, as well as
+# a mappig of the symbol string to numerical value.
+try:
+    par_map = simulate.load_constants(symbolics.constants,
+                                      'example_constants.yml')
+    with open('example_constants.yml', 'r') as f:
+        constants_dict = yaml.load(f, Loader=yaml.SafeLoader)
+except FileNotFoundError:
+    par_map = simulate.load_constants(symbolics.constants,
+                                      'examples/example_constants.yml')
+    with open('examples/example_constants.yml', 'r') as f:
+        constants_dict = yaml.load(f, Loader=yaml.SafeLoader)
+
+# %%
+# Use PyDy to generate a function that can evaluate the right hand side of the
+# ordinary differential equations of the multibody system. This uses PyDy's
+# code geenration settings that result in the fastest numerical evaluation
+# times at the cost of a slower code generation and compilation time.
+rhs = generate_ode_function(
+    symbolics.kanes_method.forcing,
+    symbolics.coordinates,
+    symbolics.speeds,
+    constants=list(par_map.keys()),
+    mass_matrix=symbolics.kanes_method.mass_matrix,
+    coordinate_derivatives=sm.Matrix(symbolics.speeds),
+    specifieds=symbolics.specifieds,
+    generator='cython',
+    linear_sys_solver='sympy',
+    constants_arg_type='array',
+    specifieds_arg_type='array',
+)
+
+# %%
+# Prepare numerical arrays to be passed to the ODE functions.
+specifieds_vals = np.zeros(len(symbolics.specifieds))
+specifieds_vals[-1] = 1.0
+
+args = (specifieds_vals, np.array(list(par_map.values())))
+
+initial_conditions = np.zeros(len(symbolics.states))
+initial_conditions[1] = 1.0  # set hip above ground
+initial_conditions[3] = np.deg2rad(5.0)  # right hip angle
+initial_conditions[6] = -np.deg2rad(5.0)  # left hip angle
+
+# %%
+# Time the average execution of PyDy's ODE function evaluation.
+print('PyDy evaluation time:',
+      timeit.timeit(lambda: rhs(initial_conditions, 0.0, *args), number=1000))
+
+# %%
+# Time the average execution of Autolev s ODE function evaluation.
+autolev_constants_dict = simulate.map_values_to_autolev_symbols(constants_dict)
+print('Autolev evaluation time:',
+      timeit.timeit(lambda: evaluate_autolev_rhs(initial_conditions[:9],
+                                                 initial_conditions[9:],
+                                                 specifieds_vals,
+                                                 autolev_constants_dict),
+                    number=1000))
+
+# %%
+# Simulate the model for two seconds using the LSODA integrator (switches
+# between stiff and non-stiff modes).
+time_vector = np.linspace(0.0, 2.0, num=61)
+trajectories = odeint(rhs, initial_conditions, time_vector, args=args)
+
+# %%
+# Visualization of the resulting motion from the forward simulation.
+scene, fig, ax = utils.plot(symbolics, time_vector, initial_conditions,
+                            args[0], args[1])
+ax.set_xlim((-0.8, 0.8))
+ax.set_ylim((-0.2, 1.4))
+ani = utils.animate(scene, fig, time_vector, trajectories,
+                    np.zeros((len(time_vector), len(symbolics.specifieds))),
+                    args[1])
+
+plt.show()