3-inverse_dynamics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "233dd361",
   "metadata": {},
   "source": [
    "# Now let's do some inverse dynamics !!\n",
    "While inverse kinematics is the core of biomechanics, inverse dynamics is probably its most useful information. \n",
    "It provides some insight on the internal forces that acted on the biomechanical system. \n",
    "Since the body is mostly actuated from muscles, therefore these internal forces reflect the state of the muscles at each instant."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8d1f6926",
   "metadata": {},
   "source": [
    "## Load a previously created bioMod file\n",
    "This first section prepares Python and load a `.bioMod` file as shown in the `2-dynamic_model_creation` or `2.5-simple_upper_limb` script with the data generated by `1-inverse_kinematics`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21442a02",
   "metadata": {},
   "outputs": [],
   "source": [
    "# So first, let's import all the required modules \n",
    "import biorbd  # This is the core that will do all the calculation\n",
    "\n",
    "from copy import copy  # Allows to copy arrays\n",
    "import numpy  # Numpy is a python module that helps dealing with the mathematics of matrices and vectors\n",
    "numpy.set_printoptions(precision=4, suppress=True)  # Make the printing of numpy matrices prettier\n",
    "\n",
    "from matplotlib import pyplot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6202f513",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import and interpret the bioMod of the full body (created by the script `2-dynamic_model_creation` or `2.5-simple_upper_limb`)\n",
    "# Please note that the dynamic model must be loaded\n",
    "use_upper_limb = False  # So use the upper limb model or the full body\n",
    "\n",
    "if use_upper_limb:\n",
    "    model = biorbd.Model(\"models/SimpleUpperLimbWithInertia.bioMod\")\n",
    "    inverse_kinematics = numpy.load(\"results/inverse_kinematics_upper_limb.npy\")\n",
    "else:\n",
    "    model = biorbd.Model(\"models/SimpleBodyWithInertia.bioMod\")\n",
    "    inverse_kinematics = numpy.load(\"results/inverse_kinematics.npy\")\n",
    "\n",
    "# Get the reconstructed kinematics\n",
    "time_vector = numpy.load(\"results/time_vector.npy\")\n",
    "n_frames = inverse_kinematics.shape[1]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "de6e8d14",
   "metadata": {},
   "source": [
    "## Inverse Dynamics\n",
    "Inverse dynamics in a nutshell is to estimate what are the forces that caused some given movement. \n",
    "Here the movement is the one created and reconstructed in the previous script, but as you may have notice, we are using the data from the reconstructed kinematics.\n",
    "That means that the source of data you be some data acquisition device.\n",
    "\n",
    "Inverse dynamics is useful to estimate the joint torques in a movement. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ad2b844d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# First let's define a function that takes in a position (q), velocities (qdot) and accelerations (qddot)\n",
    "# and spits out the net joint forces (tau) that explains this movement\n",
    "def inverse_dynamics(q, qdot, qddot):\n",
    "    return model.InverseDynamics(q, qdot, qddot).to_array()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a98a6638",
   "metadata": {},
   "outputs": [],
   "source": [
    "# The let's test the functions using the first frame assuming no velocity nor accelerations and print the result to make sure it works as expected\n",
    "q = numpy.array(inverse_kinematics[:, 0])\n",
    "qdot = numpy.zeros(q.shape)\n",
    "qddot = numpy.array(q.shape)\n",
    "tau = inverse_dynamics(q, qdot, qddot)\n",
    "print(f\"The forces necessary to maintain the static posture with no velocity nor acceleration\\n is: {tau}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5393e554",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now we have to compute the actual velocity and acceleration of the model\n",
    "# We are using finite differences for that. \n",
    "# You know the drill, let's first define a function and call it once for \n",
    "# the velocities and again for the accelerations\n",
    "def finite_difference(time_vector, data, window=2):\n",
    "    if (window % 2 != 0):  # If window is not an even number\n",
    "        raise ValueError(\"The window for the finite difference should be an even number\")\n",
    "    \n",
    "    # Let's define a matrix output fill by NaN\n",
    "    data_dot = numpy.ndarray(data.shape)\n",
    "    data_dot[:] = numpy.nan\n",
    "    \n",
    "    # Now use finite difference to compute the derivative \n",
    "    # Data are expected to be derivated accross the row\n",
    "    data_dot[:, int(window/2):-int(window/2)] = (data[:, window:] - data[:, :-window]) / (time_vector[window:] - time_vector[:-window])\n",
    "    return data_dot\n",
    "\n",
    "\n",
    "# Now we can compute the velocity and acceleration\n",
    "q = inverse_kinematics\n",
    "qdot = finite_difference(time_vector, q)\n",
    "qddot = finite_difference(time_vector, qdot)\n",
    "\n",
    "# First find the index of the arm x rotation in the vector of degree of freedom (this is determined by the model)\n",
    "# Since the only movement in on the arm, it make sense to only print that particular dof\n",
    "arm_index = [name.to_string() for name in model.nameDof()].index(\"UPPER_ARM_RotX\")\n",
    "\n",
    "# Just for fun, let's plot the values for the arm\n",
    "pyplot.figure()\n",
    "pyplot.title(\"q\")\n",
    "pyplot.ylabel(\"position (rad)\")\n",
    "pyplot.plot(time_vector, q[arm_index, :])\n",
    "\n",
    "pyplot.figure()\n",
    "pyplot.title(\"velocity (rad/s)\")\n",
    "pyplot.ylabel(\"time (s)\")\n",
    "pyplot.plot(time_vector, qdot[arm_index, :])\n",
    "\n",
    "pyplot.figure()\n",
    "pyplot.title(\"qddot\")\n",
    "pyplot.ylabel(\"acceleration (rad/s/s)\")\n",
    "pyplot.xlabel(\"time (s)\")\n",
    "pyplot.plot(time_vector, qddot[arm_index, :])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c3ae0257",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now that we have all the kinematics (q, qdot, qddot), we can get the net joint forces at each frame\n",
    "tau = []\n",
    "for i in range(n_frames):\n",
    "    tau.append(inverse_dynamics(q[:, i], qdot[:, i], qddot[:, i]))\n",
    "tau = numpy.array(tau).transpose()\n",
    "\n",
    "# And print it for the arm\n",
    "pyplot.figure()\n",
    "pyplot.title(\"tau\")\n",
    "pyplot.ylabel(\"joint moment (N/m)\")\n",
    "pyplot.xlabel(\"time (s)\")\n",
    "pyplot.plot(time_vector, tau[arm_index, :])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c2c7f4b7-0b01-4c06-9482-4e10eec981d2",
   "metadata": {},
   "source": [
    "## Mass matrix and bias (or nonlinear) effect\n",
    "As bonus, we can get intermediate computation of the `tau`, namely the mass matrix ($M$) and/or the bias (or nonlinear) effects ($N$).\n",
    "As reminder tau is computed as such:\n",
    "\\begin{align}\n",
    "\\tau = M(q)\\ddot{q} + N(q, \\dot{q})\n",
    "\\end{align}\n",
    "So the next cell gets these intermediate values from biorbd and prints them.\n",
    "\n",
    "For simplicity, only a single frame is shown, but you should be able to perform this over all frames pretty easily."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c31a3bdd-82ab-4cb9-9540-573098ce1fa7",
   "metadata": {},
   "outputs": [],
   "source": [
    "frame = 50\n",
    "Mqddot = model.massMatrix(q[:, frame]).to_array() @ qddot[:, frame]\n",
    "N = model.NonLinearEffect(q[:, frame], qdot[:, frame]).to_array()\n",
    "print(f\"At the frame {frame}\")\n",
    "print(f\"The M(q)*qddot part is: {Mqddot}\")\n",
    "print(f\"The N(q,qdot) part is: {N}\")\n",
    "print(f\"Obviously their sum ({Mqddot + N}) is the same as tau at the same frame {tau[:, frame]}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7e08f14c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Let's finally save the results so it can be used in the static optimisation script\n",
    "import os\n",
    "if not os.path.isdir(\"results\"):\n",
    "    os.mkdir(\"results\")\n",
    "\n",
    "if use_upper_limb:\n",
    "    numpy.save(\"results/inverse_dynamics_upper_body.npy\", (q, qdot, qddot, tau))\n",
    "else:\n",
    "    numpy.save(\"results/inverse_dynamics.npy\", (q, qdot, qddot, tau))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}