2.5-simple_upper_limb.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "7908c095",
   "metadata": {},
   "source": [
    "# Creating a very simple dynamic bioMod file\n",
    "For the forward dynamics part, using a seven degrees of freedom model will be much to complicated.\n",
    "So the purpose of this extra file is to create a very simple model of one degree of freedom. \n",
    "For that, we are stripping all the lower part of the full body model we have been using up to now. \n",
    "\n",
    "You will therefore find a lot of similarities between this file and the `2-dynamic_model_creation` file, with the only exception that everything below the pelvis is removed.\n",
    "\n",
    "WARNING: Don't forget to run the `1-inverse_kinematics` file with `use_upper_limb` set to `True` so data for upper limb can be used!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f33f8560-a2f9-4fba-b395-54a242b2ba54",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Input paths\n",
    "kinematic_model_file_path = \"models/SimpleBody.bioMod\"\n",
    "kinematic_chain_file_path = \"models/simple_body_kinematic_chain.kc\"\n",
    "\n",
    "# Output paths\n",
    "dynamic_model_file_path = \"models/SimpleUpperLimbWithInertia.bioMod\"\n",
    "kinematic_chain_save_file_path = \"models/upper_limb_kinematic_chain_with_inertia.kc\""
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9e9109cd",
   "metadata": {},
   "source": [
    "## Reload the kinematic model\n",
    "In order ot load the kinematic chain (saved using pickle), we must define the classes that were used. \n",
    "So the next cell is a copy of the class declaration of `0-kinematic_model_creation`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d948e40f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# First of all, let's make our life easier by defining a class that we will fill for each of the segment\n",
    "# Notice that mass, center_of_mass and inertia_xxyyzz are currently ignored. They will become relevant in \n",
    "# dynamics section below\n",
    "class Segment():\n",
    "    def __init__(\n",
    "        self, \n",
    "        name, \n",
    "        parent_name=\"\", \n",
    "        rt=\"\",\n",
    "        translations=\"\", \n",
    "        rotations=\"\",\n",
    "        mass=0,\n",
    "        center_of_mass=None,\n",
    "        inertia_xxyyzz=None,\n",
    "        mesh: tuple=None,\n",
    "        markers: tuple=None,\n",
    "    ):\n",
    "        self.name = name\n",
    "        self.parent_name = parent_name\n",
    "        self.rt = rt\n",
    "        self.translations = translations\n",
    "        self.rotations = rotations\n",
    "        self.mass = mass\n",
    "        self.center_of_mass=center_of_mass\n",
    "        self.inertia_xxyyzz=inertia_xxyyzz\n",
    "        self.mesh = mesh\n",
    "        self.markers = markers\n",
    "\n",
    "    def __str__(self):\n",
    "        # Define the print function so it automatically format things in the file properly<\n",
    "        out_string = f\"segment {self.name}\\n\"\n",
    "        if (self.parent_name):\n",
    "            out_string += f\"\\tparent {self.parent_name}\\n\"\n",
    "        if (self.rt):\n",
    "            out_string += f\"\\tRT {self.rt}\\n\"\n",
    "        if (self.translations):\n",
    "            out_string += f\"\\ttranslations {self.translations}\\n\"\n",
    "        if (self.rotations):\n",
    "            out_string += f\"\\trotations {self.rotations}\\n\"\n",
    "        if (self.mass):\n",
    "            out_string += f\"\\tmass {self.mass}\\n\"\n",
    "        if (self.center_of_mass):\n",
    "            out_string += f\"\\tcom {self.center_of_mass[0]} {self.center_of_mass[1]} {self.center_of_mass[2]}\\n\"\n",
    "        if (self.inertia_xxyyzz):\n",
    "            out_string += f\"\\tinertia {self.inertia_xxyyzz[0]} 0 0\\n\" + \\\n",
    "                          f\"\\t        0 {self.inertia_xxyyzz[1]} 0\\n\" + \\\n",
    "                          f\"\\t        0 0 {self.inertia_xxyyzz[2]}\\n\"\n",
    "        if (self.mesh):\n",
    "            for m in self.mesh:\n",
    "                out_string += f\"\\tmesh {m[0]} {m[1]} {m[2]}\\n\"\n",
    "        out_string += \"endsegment\\n\"\n",
    "        \n",
    "        # Also print the markers attached to the segment\n",
    "        if (self.markers):\n",
    "            for marker in self.markers:\n",
    "                out_string += str(marker)\n",
    "        return out_string\n",
    "    \n",
    "    \n",
    "# Let's do the same for the markers\n",
    "class Marker():\n",
    "    def __init__(\n",
    "        self, \n",
    "        name, \n",
    "        parent_name, \n",
    "        position,\n",
    "    ):\n",
    "        self.name = name\n",
    "        self.parent_name = parent_name\n",
    "        self.position = position\n",
    "\n",
    "    def __str__(self):\n",
    "        # Define the print function so it automatically format things in the file properly<\n",
    "        out_string = f\"marker {self.name}\\n\"\n",
    "        out_string += f\"\\tparent {self.parent_name}\\n\"\n",
    "        out_string += f\"\\tposition {self.position[0]} {self.position[1]} {self.position[2]}\\n\"\n",
    "        out_string += \"endmarker\\n\"\n",
    "        return out_string\n",
    "\n",
    "\n",
    "# And why not creating a data structure that will collect all the segments and produce the a full\n",
    "# kinematic chain that can easily be printed\n",
    "class KinematicChain():\n",
    "    def __init__(self, segments):\n",
    "        self.segments = segments\n",
    "        \n",
    "    def __str__(self):\n",
    "        out_string = \"version 4\\n\\n\"\n",
    "        for segment in self.segments:\n",
    "            out_string += str(segment)\n",
    "            out_string += \"\\n\\n\\n\"  # Give some space between segments\n",
    "        return out_string\n",
    "    \n",
    "    def write(self, file_path):\n",
    "        # Method to write the current KinematicChain to a file\n",
    "        with open(file_path, \"w\") as file:\n",
    "            file.write(str(self))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d51d9d1b-5635-47a9-8ca9-88eef977c592",
   "metadata": {},
   "outputs": [],
   "source": [
    "# So let's first reload the kinematic chain created in `0-kinematic_model_creation` (saved using pickle)\n",
    "# Meaning that after running this cell, the `kinematic_chain` variable is exactly what it was at the end of `0-kinematic_model_creation` with the lower body part removed\n",
    "import pickle\n",
    "\n",
    "with open(kinematic_chain_file_path, \"rb\") as file:\n",
    "    kinematic_chain = pickle.load(file)\n",
    "\n",
    "# Collect the individual segments in the kinematic chain\n",
    "trunk = kinematic_chain.segments[0]\n",
    "head = kinematic_chain.segments[1]\n",
    "upper_arm = kinematic_chain.segments[2]\n",
    "lower_arm = kinematic_chain.segments[3]\n",
    "hand = kinematic_chain.segments[4]\n",
    "\n",
    "# Recreate the kinematic chain using only upper part\n",
    "kinematic_chain = KinematicChain(segments=(trunk, head, upper_arm, lower_arm, hand))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "434a39f0",
   "metadata": {},
   "source": [
    "## Extend to dynamic model\n",
    "For that second part, we will add the inertia parameters to the model. \n",
    "It will allow us, for instance, to perform inverse dynamics analyses.\n",
    "\n",
    "We are using the De Leva inertia parameter model."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "47a25ba0",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now let's use the De Leva table to produce realistic inertia value for our model\n",
    "# We are using biorbd (the bioMod) file to make some computation. Please note that it is\n",
    "# by no way mandatory, and we could have used the KinematicChain class we created. But\n",
    "# I thought it was a good occasion to introduce how to retreive some information from biorbd\n",
    "import biorbd\n",
    "import numpy\n",
    "\n",
    "# Let's first copy the De Leva table in a dictionary using a simple DeLeva class\n",
    "class DeLeva:\n",
    "    # This is an internal class just to make things well sorted\n",
    "    class Param:    \n",
    "        def __init__(\n",
    "            self,\n",
    "            marker_names,  # The name of the markers medial/lateral\n",
    "            mass,  # Percentage of the total body mass\n",
    "            center_of_mass,  # Position of the center of mass as a percentage of the distance from medial to distal\n",
    "            radii,  # [Sagittal, Transverse, Longitudinal] radii of giration\n",
    "        ):\n",
    "            self.marker_names = marker_names\n",
    "            self.mass = mass\n",
    "            self.center_of_mass = center_of_mass\n",
    "            self.radii = radii\n",
    "\n",
    "    def __init__(self, sex, mass, model):\n",
    "        self.sex = sex  # The sex of the subject\n",
    "        self.mass = mass  # The mass of the subject\n",
    "        self.model = model  # The biorbd model. This is to compute lengths\n",
    "        \n",
    "        # Produce some easy to access variables\n",
    "        self.q_zero = numpy.zeros((model.nbQ()))\n",
    "        self.marker_names = [name.to_string() for name in model.markerNames()]\n",
    "        \n",
    "        # This is the actual copy of the De Leva table\n",
    "        self.table = {}\n",
    "        self.table[\"male\"] = {}\n",
    "        self.table[\"male\"][\"HEAD\"]      = DeLeva.Param(marker_names=(\"TOP_HEAD\", \"SHOULDER\"), mass=0.0694  , center_of_mass=0.5002, radii=(0.303, 0.315, 0.261))\n",
    "        self.table[\"male\"][\"TRUNK\"]     = DeLeva.Param(marker_names=(\"SHOULDER\", \"PELVIS\")  , mass=0.4346  , center_of_mass=0.5138, radii=(0.328, 0.306, 0.169))\n",
    "        self.table[\"male\"][\"UPPER_ARM\"] = DeLeva.Param(marker_names=(\"SHOULDER\", \"ELBOW\")   , mass=0.0271*2, center_of_mass=0.5772, radii=(0.285, 0.269, 0.158))\n",
    "        self.table[\"male\"][\"LOWER_ARM\"] = DeLeva.Param(marker_names=(\"ELBOW\", \"WRIST\")      , mass=0.0162*2, center_of_mass=0.4574, radii=(0.276, 0.265, 0.121))\n",
    "        self.table[\"male\"][\"HAND\"]      = DeLeva.Param(marker_names=(\"WRIST\", \"FINGER\")     , mass=0.0061*2, center_of_mass=0.7900, radii=(0.628, 0.513, 0.401))\n",
    "        self.table[\"male\"][\"THIGH\"]     = DeLeva.Param(marker_names=(\"PELVIS\", \"KNEE\")      , mass=0.1416*2, center_of_mass=0.4095, radii=(0.329, 0.329, 0.149))\n",
    "        self.table[\"male\"][\"SHANK\"]     = DeLeva.Param(marker_names=(\"KNEE\", \"ANKLE\")       , mass=0.0433*2, center_of_mass=0.4459, radii=(0.255, 0.249, 0.103))\n",
    "        self.table[\"male\"][\"FOOT\"]      = DeLeva.Param(marker_names=(\"ANKLE\", \"TOE\")        , mass=0.0137*2, center_of_mass=0.4415, radii=(0.257, 0.245, 0.124))\n",
    "\n",
    "        self.table[\"female\"] = {}\n",
    "        self.table[\"female\"][\"HEAD\"]      = DeLeva.Param(marker_names=(\"TOP_HEAD\", \"SHOULDER\"), mass=0.0669  , center_of_mass=0.4841, radii=(0.271, 0.295, 0.261))\n",
    "        self.table[\"female\"][\"TRUNK\"]     = DeLeva.Param(marker_names=(\"SHOULDER\", \"PELVIS\")  , mass=0.4257  , center_of_mass=0.4964, radii=(0.307, 0.292, 0.147))\n",
    "        self.table[\"female\"][\"UPPER_ARM\"] = DeLeva.Param(marker_names=(\"SHOULDER\", \"ELBOW\")   , mass=0.0255*2, center_of_mass=0.5754, radii=(0.278, 0.260, 0.148))\n",
    "        self.table[\"female\"][\"LOWER_ARM\"] = DeLeva.Param(marker_names=(\"ELBOW\", \"WRIST\")      , mass=0.0138*2, center_of_mass=0.4559, radii=(0.261, 0.257, 0.094))\n",
    "        self.table[\"female\"][\"HAND\"]      = DeLeva.Param(marker_names=(\"WRIST\", \"FINGER\")     , mass=0.0056*2, center_of_mass=0.7474, radii=(0.531, 0.454, 0.335))\n",
    "        self.table[\"female\"][\"THIGH\"]     = DeLeva.Param(marker_names=(\"PELVIS\", \"KNEE\")      , mass=0.1478*2, center_of_mass=0.3612, radii=(0.369, 0.364, 0.162))\n",
    "        self.table[\"female\"][\"SHANK\"]     = DeLeva.Param(marker_names=(\"KNEE\", \"ANKLE\")       , mass=0.0481*2, center_of_mass=0.4416, radii=(0.271, 0.267, 0.093))\n",
    "        self.table[\"female\"][\"FOOT\"]      = DeLeva.Param(marker_names=(\"ANKLE\", \"TOE\")        , mass=0.0129*2, center_of_mass=0.4014, radii=(0.299, 0.279, 0.124))\n",
    "\n",
    "    def segment_mass(self, segment):\n",
    "        return self.table[self.sex][segment].mass * self.mass\n",
    "    \n",
    "    def segment_length(self, segment):\n",
    "        table = self.table[self.sex][segment]\n",
    "        \n",
    "        # Find the position of the markers when the model is in resting position\n",
    "        marker_positions = numpy.array([marker.to_array() for marker in self.model.markers(self.q_zero)]).transpose()\n",
    "        \n",
    "        # Find the index of the markers required to compute the length of the segment\n",
    "        idx_proximal = self.marker_names.index(table.marker_names[0])\n",
    "        idx_distal = self.marker_names.index(table.marker_names[1])\n",
    "        \n",
    "        # Compute the Euclidian distance between the two positions\n",
    "        return numpy.linalg.norm(marker_positions[:, idx_distal] - marker_positions[:, idx_proximal])\n",
    "        \n",
    "    def segment_center_of_mass(self, segment, inverse_proximal=False):\n",
    "        # This method will compute the length of the required segment based on the biorbd model and required markers\n",
    "        # If inverse_proximal is set to True, then the value is returned from the distal position\n",
    "        table = self.table[self.sex][segment]\n",
    "        \n",
    "        # Find the position of the markers when the model is in resting position\n",
    "        marker_positions = numpy.array([marker.to_array() for marker in self.model.markers(self.q_zero)]).transpose()\n",
    "        \n",
    "        # Find the index of the markers required to compute the length of the segment\n",
    "        idx_proximal = self.marker_names.index(table.marker_names[0])\n",
    "        idx_distal = self.marker_names.index(table.marker_names[1])\n",
    "        \n",
    "        # Compute the position of the center of mass\n",
    "        if inverse_proximal:\n",
    "            center_of_mass = (1-table.center_of_mass) * (marker_positions[:, idx_proximal] - marker_positions[:, idx_distal])\n",
    "        else:\n",
    "            center_of_mass = table.center_of_mass * (marker_positions[:, idx_distal] - marker_positions[:, idx_proximal])\n",
    "        return tuple(center_of_mass)  # convert the result to a Tuple which is good practise\n",
    "        \n",
    "    def segment_moment_of_inertia(self, segment):\n",
    "        mass = self.segment_mass(segment)\n",
    "        length = self.segment_length(segment)\n",
    "        radii = self.table[self.sex][segment].radii\n",
    "        \n",
    "        return (mass * (length*radii[0])**2, mass * (length*radii[1])**2, mass * (length*radii[2])**2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21f5387e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Let's suppose we have a male subject of 70kg which dimension are given by the bioMod previously created\n",
    "deleva = DeLeva(sex=\"male\", mass=70, model=biorbd.Model(kinematic_model_file_path))\n",
    "\n",
    "\n",
    "# So let's fill all the field in each segments (and again let's define a convenient function that will do that for us)\n",
    "def fill_segment(segment, deleva, inverse_proximal=False):\n",
    "    segment.mass = deleva.segment_mass(segment.name)\n",
    "    segment.center_of_mass = deleva.segment_center_of_mass(segment.name, inverse_proximal)\n",
    "    segment.inertia_xxyyzz = deleva.segment_moment_of_inertia(segment.name)\n",
    "\n",
    "\n",
    "# Fill all the segments\n",
    "fill_segment(trunk, deleva, True)\n",
    "fill_segment(head, deleva, True)\n",
    "fill_segment(upper_arm, deleva)\n",
    "fill_segment(lower_arm, deleva)\n",
    "fill_segment(hand, deleva)\n",
    "\n",
    "# Remove the translations and rotations at the trunk\n",
    "trunk.translations = None\n",
    "trunk.rotations = None\n",
    "\n",
    "# Print and rewrite the bioMod file\n",
    "print(kinematic_chain)\n",
    "kinematic_chain.write(dynamic_model_file_path)\n",
    "\n",
    "# As we did for the kinematic chain, let's save this kinematic chain so it can be reloaded as is later\n",
    "with open(kinematic_chain_save_file_path, \"wb\") as file:\n",
    "    pickle.dump(kinematic_chain, file)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "486d4842",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Run this cell to show the model you just created. \n",
    "# You should notice that the centers of mass now appear\n",
    "# Please note that due to some jupyter limitations, you may have to restart after running this cell\n",
    "# Please also note that this cell will crash if runned from the binder\n",
    "\n",
    "import bioviz\n",
    "\n",
    "# Send the previously loaded model to the vizualizer\n",
    "viz = bioviz.Viz(dynamic_model_file_path)\n",
    "\n",
    "# Move the model to a recognizable position (arm raised and knee flexed)\n",
    "viz.set_q((1.20,))\n",
    "\n",
    "# Halt the program so you can interact with the vizualiser. Closing the window should allow to continue\n",
    "viz.exec()\n",
    "\n",
    "# If nothing happens, check for background. Sometimes the window loads behind the current window"
   ]
  }
 ],
 "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
}