01_arm_reaching_exercice.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "233dd361",
   "metadata": {},
   "source": [
    "<h1><center>Bioptim Workshop</center></h1>\n",
    "\n",
    "# Let's do some biomechanics !!\n",
    "\n",
    "Now, we are working with an arm model, actuated by muscles and pure joint torques (to compensate for model weakness). \n",
    "The goal is, starting from an initial position, to perform a reaching  task with this arm, by superimposing two markers. \n",
    "The first one (called 'target' in the model), is fixed in the scene.\n",
    "The second one (called 'COM_hand' in the model) is fixed on the arm, in the *ulna* coordinate system.\n",
    "\n",
    "The ocp to solve is as follow:\n",
    "- Movement of $3s$ divided into $50$ shooting points\n",
    "- The state variables are the generalized position (q) and velocities (qdot)\n",
    "- The control variables are the muscle activations (a) \n",
    "- Cost functions: minimize the control effort at all time and minimize the squared Euclidean distance between the two markers at the final node\n",
    "- Initial guess is arbitrarily set to $0.5$ for muscle activations and to $0$ for all other variables"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21442a02",
   "metadata": {},
   "outputs": [],
   "source": [
    "# So first, let's import all the required classes \n",
    "from bioptim import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6202f513",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Let's define the OCP\n",
    "final_time = 0.5\n",
    "n_shooting = 30\n",
    "\n",
    "# Load the model and define some aliases\n",
    "model = BiorbdModel(\"models/arm26.bioMod\")\n",
    "nq = model.nb_q\n",
    "ndq = model.nb_qdot\n",
    "ntau = model.nb_tau\n",
    "nmus = model.nb_muscles\n",
    "\n",
    "# Add objective functions\n",
    "objective_functions = ...\n",
    "objective_functions.add(...) # min torque, weight = 10\n",
    "objective_functions.add(...) # min muscle control, weight = 1\n",
    "objective_functions.add(..., first_marker=..., second_marker=..., ...) # superimpose markers, weight = 1000\n",
    "\n",
    "# Dynamics\n",
    "dynamics = ...\n",
    "dynamics.add(...) # muscle activation driven with residual torque \n",
    "\n",
    "# Path constraint\n",
    "x_bounds = ...\n",
    "x_bounds.add(bounds=model.bounds_from_ranges([\"q\", \"qdot\"]))\n",
    "... # force initial state at (0.07, 1.4, 0, 0)\n",
    "\n",
    "# Initial guess on state\n",
    "x_init = ...\n",
    "x_init.add([1.57] * nq + [0] * ndq)\n",
    "\n",
    "# Define control path constraint\n",
    "muscle_min, muscle_max, muscle_init = 0, 1, 0.5\n",
    "tau_min, tau_max, tau_init = -5, 5, 0\n",
    "u_bounds = BoundsList()\n",
    "u_bounds.add(\n",
    "    ..., # min bound\n",
    "    ...,# max bound\n",
    ")\n",
    "\n",
    "# Initial guess on control\n",
    "u_init = InitialGuessList()\n",
    "u_init.add([tau_init] * ntau + [muscle_init] * nmus)\n",
    "# ------------- #\n",
    "\n",
    "ocp = OptimalControlProgram(...)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d8a4a5a2",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = ocp.solve()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "31bb7c9d",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol.graphs(automatically_organize=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aeeae93c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# sol.animate()\n",
    "# This would work locally on your computer. Unfortunately, it is nor currently working with remote notebooks...\n",
    "# Call one of the workshop organizers, so you can see the motion animation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1f056da2",
   "metadata": {},
   "outputs": [],
   "source": [
    "sol.print()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d414edaf",
   "metadata": {},
   "source": [
    "Let us compute the distance between the two markers at the end of the motion, based on the information printed in the console above. Keep in mind that SUPERIMPOSE_MARKERS is a quadratic cost.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e4c8a8ba",
   "metadata": {},
   "outputs": [],
   "source": [
    "import biorbd_casadi as biorbd\n",
    "import numpy as np\n",
    "\n",
    "# Prepare the casadi function to call\n",
    "markers = biorbd.to_casadi_func(\"mark1\", model.markers, ocp.nlp[0].states[\"q\"].mx)\n",
    "\n",
    "# Extract results\n",
    "q = ...\n",
    "target_idx = biorbd.marker_index(model.model, \"target\")\n",
    "hand_idx = biorbd.marker_index(model.model, \"COM_hand\")\n",
    "\n",
    "dist = np.sqrt(np.sum((markers(q)[:, target_idx] - markers(q)[:, hand_idx])**2))\n",
    "print(f\"At the final node, the distance between the 2 markers is {round(dist, 3) * 1000}mm\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "85ebf0da",
   "metadata": {},
   "source": [
    "You find it too large ? Try to solve the problem again by increasing the weight of the objective on the markers matching !"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "41be5266",
   "metadata": {},
   "outputs": [],
   "source": [
    "..."
   ]
  }
 ],
 "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.11.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}