diff --git a/python/four_bar_mechanism/equations.ipynb b/python/four_bar_mechanism/equations.ipynb
index adc9d70bb307ab225e43296a228360ab6a44eb74..5cc71b36d64d86bc9d2419765ef83aace4cc37f5 100644
--- a/python/four_bar_mechanism/equations.ipynb
+++ b/python/four_bar_mechanism/equations.ipynb
@@ -1,8 +1,22 @@
 {
  "cells": [
+  {
+   "attachments": {},
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Jacobian of Four-Bar Mechanism \n",
+    "\n",
+    "The knee joint is actuated but the ankle joint is passive.  \n",
+    "\n",
+    "## Todo's\n",
+    "\n",
+    "  - consider $\\theta_0$"
+   ]
+  },
   {
    "cell_type": "code",
-   "execution_count": 158,
+   "execution_count": 57,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -14,26 +28,25 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 159,
+   "execution_count": 71,
    "metadata": {},
    "outputs": [],
    "source": [
-    "# Actuation\n",
-    "\n",
+    "# Actuation (knee)\n",
     "theta = symbols('theta')\n",
     "\n",
-    "# passive joint \n",
+    "# Passive joint (ankle) \n",
     "psi = symbols('psi')\n"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 160,
+   "execution_count": 59,
    "metadata": {},
    "outputs": [],
    "source": [
     "\n",
-    "# Constants defining geometry.\n",
+    "# Constants defined by geometry.\n",
     "theta0 = symbols('theta0')\n",
     "\n",
     "shank, p1, p2, p3 = symbols('shank p1 p2 p3')\n",
@@ -42,52 +55,80 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 161,
+   "execution_count": 60,
    "metadata": {},
    "outputs": [],
    "source": [
+    "# Manually specified structure of equations. We first introduce variables based on the kinematic structure only.\n",
+    "# These can easily be precomputed in code. Here, they are just mentioned as a reference.\n",
     "\n",
     "k1 = shank / p1\n",
     "k2 = shank / p3\n",
     "k3 = (shank**2 + p1**2 + p3**2 - p2**2 ) / (2 * p1 * p3)\n",
     "\n",
+    "\n",
+    "# directly use these variables\n",
+    "k1, k2, k3 = symbols('k1 k2 k3')\n"
+   ]
+  },
+  {
+   "attachments": {},
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Relationship between ankle ($\\psi$) and knee ($\\theta$)"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 61,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "\n",
+    "# some helpers\n",
     "cT = cos(theta)\n",
     "sT = sin(theta)\n",
     "\n",
-    "# precompute the stuff above\n",
-    "k1, k2, k3 = symbols('k1 k2 k3')\n",
-    "\n",
     "\n",
     "A = k1 * cT + k2 + k3 + cT # C.34\n",
     "B = -2*sT # C.35\n",
     "C = k1 * cT - k2 + k3 - cT # C.36\n",
     "\n",
+    "# ankle\n",
     "psi = 2 * atan((-B + sqrt(B * B - 4 * A * C)) / (2 * A)) #  C.39\n",
     "\n",
-    "psi_inner = (-B + sqrt(B * B - 4 * A * C)) / (2 * A)\n",
-    "\n",
     "psi = sp.simplify(psi)\n",
     "\n",
-    "\n",
-    "from sympy import Function\n",
-    "psi = Function('psi')(theta)"
+    "psi_of_theta = psi\n",
+    "\n"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 162,
+   "execution_count": 62,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/latex": [
-       "$\\displaystyle \\psi{\\left(\\theta \\right)}$"
+       "$\\displaystyle 2 \\operatorname{atan}{\\left(\\frac{\\sqrt{- k_{1}^{2} \\cos^{2}{\\left(\\theta \\right)} - 2 k_{1} k_{3} \\cos{\\left(\\theta \\right)} + k_{2}^{2} + 2 k_{2} \\cos{\\left(\\theta \\right)} - k_{3}^{2} + 1} + \\sin{\\left(\\theta \\right)}}{k_{1} \\cos{\\left(\\theta \\right)} + k_{2} + k_{3} + \\cos{\\left(\\theta \\right)}} \\right)}$"
       ],
       "text/plain": [
-       "ψ(θ)"
+       "      ⎛   ______________________________________________________________      \n",
+       "      ⎜  ╱     2    2                         2                   2           \n",
+       "      ⎜╲╱  - k₁ ⋅cos (θ) - 2⋅k₁⋅k₃⋅cos(θ) + k₂  + 2⋅k₂⋅cos(θ) - k₃  + 1  + sin\n",
+       "2⋅atan⎜───────────────────────────────────────────────────────────────────────\n",
+       "      ⎝                       k₁⋅cos(θ) + k₂ + k₃ + cos(θ)                    \n",
+       "\n",
+       "   ⎞\n",
+       "   ⎟\n",
+       "(θ)⎟\n",
+       "───⎟\n",
+       "   ⎠"
       ]
      },
-     "execution_count": 162,
+     "execution_count": 62,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -97,457 +138,343 @@
    ]
   },
   {
-   "cell_type": "code",
-   "execution_count": 163,
+   "attachments": {},
+   "cell_type": "markdown",
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/latex": [
-       "$\\displaystyle \\operatorname{CoordSys3D}\\left(active_base, \\left( \\left[\\begin{matrix}1 & 0 & 0\\\\0 & 1 & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  (shank)\\mathbf{\\hat{i}_{passive}}\\right), \\operatorname{CoordSys3D}\\left(passive, \\left( \\left[\\begin{matrix}\\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & - \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & 0\\\\\\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right), \\operatorname{CoordSys3D}\\left(base, \\left( \\left[\\begin{matrix}1 & 0 & 0\\\\0 & 1 & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right)\\right)\\right)\\right)$"
-      ],
-      "text/plain": [
-       "active_base"
-      ]
-     },
-     "execution_count": 163,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
    "source": [
-    "\n",
-    "from sympy.vector import CoordSys3D, AxisOrienter\n",
-    "from sympy import Matrix\n",
-    "\n",
-    "B = CoordSys3D('base')\n",
-    "\n",
-    "# zAxis = Vector(0,0, 1)\n",
-    "\n",
-    "orienter_psi = AxisOrienter(-psi, B.k )\n",
-    "\n",
-    "T_passive = B.orient_new('passive', (orienter_psi, ))\n",
-    "\n",
-    "T_active_base = T_passive.locate_new('active_base', shank * T_passive.i )\n",
-    "\n",
-    "T_active_base\n",
-    "\n"
+    "### Derivatives"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 164,
+   "execution_count": 63,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/latex": [
-       "$\\displaystyle \\operatorname{CoordSys3D}\\left(active, \\left( \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & \\sin{\\left(\\theta \\right)} & 0\\\\- \\sin{\\left(\\theta \\right)} & \\cos{\\left(\\theta \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right), \\operatorname{CoordSys3D}\\left(active_base, \\left( \\left[\\begin{matrix}1 & 0 & 0\\\\0 & 1 & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  (shank)\\mathbf{\\hat{i}_{passive}}\\right), \\operatorname{CoordSys3D}\\left(passive, \\left( \\left[\\begin{matrix}\\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & - \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & 0\\\\\\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right), \\operatorname{CoordSys3D}\\left(base, \\left( \\left[\\begin{matrix}1 & 0 & 0\\\\0 & 1 & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right)\\right)\\right)\\right)\\right)$"
+       "$\\displaystyle \\frac{2 \\left(\\frac{\\left(k_{1} \\sin{\\left(\\theta \\right)} + \\sin{\\left(\\theta \\right)}\\right) \\left(\\sqrt{- k_{1}^{2} \\cos^{2}{\\left(\\theta \\right)} - 2 k_{1} k_{3} \\cos{\\left(\\theta \\right)} + k_{2}^{2} + 2 k_{2} \\cos{\\left(\\theta \\right)} - k_{3}^{2} + 1} + \\sin{\\left(\\theta \\right)}\\right)}{\\left(k_{1} \\cos{\\left(\\theta \\right)} + k_{2} + k_{3} + \\cos{\\left(\\theta \\right)}\\right)^{2}} + \\frac{\\frac{k_{1}^{2} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\theta \\right)} + k_{1} k_{3} \\sin{\\left(\\theta \\right)} - k_{2} \\sin{\\left(\\theta \\right)}}{\\sqrt{- k_{1}^{2} \\cos^{2}{\\left(\\theta \\right)} - 2 k_{1} k_{3} \\cos{\\left(\\theta \\right)} + k_{2}^{2} + 2 k_{2} \\cos{\\left(\\theta \\right)} - k_{3}^{2} + 1}} + \\cos{\\left(\\theta \\right)}}{k_{1} \\cos{\\left(\\theta \\right)} + k_{2} + k_{3} + \\cos{\\left(\\theta \\right)}}\\right)}{\\frac{\\left(\\sqrt{- k_{1}^{2} \\cos^{2}{\\left(\\theta \\right)} - 2 k_{1} k_{3} \\cos{\\left(\\theta \\right)} + k_{2}^{2} + 2 k_{2} \\cos{\\left(\\theta \\right)} - k_{3}^{2} + 1} + \\sin{\\left(\\theta \\right)}\\right)^{2}}{\\left(k_{1} \\cos{\\left(\\theta \\right)} + k_{2} + k_{3} + \\cos{\\left(\\theta \\right)}\\right)^{2}} + 1}$"
       ],
       "text/plain": [
-       "active"
+       "  ⎛                                                                           \n",
+       "  ⎜                                                                           \n",
+       "  ⎜                                                                           \n",
+       "  ⎜                     ⎛   __________________________________________________\n",
+       "  ⎜                     ⎜  ╱     2    2                         2             \n",
+       "  ⎜(k₁⋅sin(θ) + sin(θ))⋅⎝╲╱  - k₁ ⋅cos (θ) - 2⋅k₁⋅k₃⋅cos(θ) + k₂  + 2⋅k₂⋅cos(θ\n",
+       "2⋅⎜───────────────────────────────────────────────────────────────────────────\n",
+       "  ⎜                                                               2           \n",
+       "  ⎝                                 (k₁⋅cos(θ) + k₂ + k₃ + cos(θ))            \n",
+       "──────────────────────────────────────────────────────────────────────────────\n",
+       "                                                                              \n",
+       "                                                ⎛   __________________________\n",
+       "                                                ⎜  ╱     2    2               \n",
+       "                                                ⎝╲╱  - k₁ ⋅cos (θ) - 2⋅k₁⋅k₃⋅c\n",
+       "                                                ──────────────────────────────\n",
+       "                                                                              \n",
+       "                                                                       (k₁⋅cos\n",
+       "\n",
+       "                                      2                                       \n",
+       "                                    k₁ ⋅sin(θ)⋅cos(θ) + k₁⋅k₃⋅sin(θ) - k₂⋅sin(\n",
+       "                         ─────────────────────────────────────────────────────\n",
+       "____________         ⎞      __________________________________________________\n",
+       "      2              ⎟     ╱     2    2                         2             \n",
+       ") - k₃  + 1  + sin(θ)⎠   ╲╱  - k₁ ⋅cos (θ) - 2⋅k₁⋅k₃⋅cos(θ) + k₂  + 2⋅k₂⋅cos(θ\n",
+       "────────────────────── + ─────────────────────────────────────────────────────\n",
+       "                                                k₁⋅cos(θ) + k₂ + k₃ + cos(θ)  \n",
+       "                                                                              \n",
+       "──────────────────────────────────────────────────────────────────────────────\n",
+       "                                              2                               \n",
+       "____________________________________         ⎞                                \n",
+       "          2                   2              ⎟                                \n",
+       "os(θ) + k₂  + 2⋅k₂⋅cos(θ) - k₃  + 1  + sin(θ)⎠                                \n",
+       "─────────────────────────────────────────────── + 1                           \n",
+       "                       2                                                      \n",
+       "(θ) + k₂ + k₃ + cos(θ))                                                       \n",
+       "\n",
+       "                     ⎞\n",
+       "θ)                   ⎟\n",
+       "──────────── + cos(θ)⎟\n",
+       "____________         ⎟\n",
+       "      2              ⎟\n",
+       ") - k₃  + 1          ⎟\n",
+       "─────────────────────⎟\n",
+       "                     ⎟\n",
+       "                     ⎠\n",
+       "──────────────────────\n",
+       "                      \n",
+       "                      \n",
+       "                      \n",
+       "                      \n",
+       "                      \n",
+       "                      \n",
+       "                      "
       ]
      },
-     "execution_count": 164,
+     "execution_count": 63,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
+    "from sympy import diff\n",
     "\n",
-    "orienter_theta = AxisOrienter(theta, B.k )\n",
-    "\n",
-    "T_active = T_active_base.orient_new('active', (orienter_theta, ))\n",
+    "dpsi_dtheta = diff(psi, theta)\n",
     "\n",
-    "T_active"
+    "dpsi_dtheta"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": []
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 165,
+   "execution_count": 64,
    "metadata": {},
    "outputs": [],
    "source": [
-    "# express the end effector in the active joint coordinate system\n",
+    "# We can implement the term dpsi_dtheta now in code. Therefore, we assume from now on that it is given.\n",
     "\n",
-    "x, y,z , alpha, beta, gamma = symbols(\"x y z alpha beta gamma\")\n",
-    "\n",
-    "from sympy.vector import BodyOrienter, Point\n",
-    "euler = BodyOrienter(alpha, beta, gamma, rot_order=\"XYZ\")\n",
-    "\n",
-    "P = T_active.orient_new(\"EEF\", euler, location=(T_active.i * x + T_active.j * y + T_active.k * z))\n",
-    "\n"
+    "from sympy import Function\n",
+    "psi = Function('psi')(theta)"
    ]
   },
   {
-   "cell_type": "code",
-   "execution_count": 166,
+   "attachments": {},
+   "cell_type": "markdown",
    "metadata": {},
-   "outputs": [],
    "source": [
-    "pos_vect = P.origin.position_wrt(B)\n",
-    "p = pos_vect.to_matrix(B)\n",
-    "\n",
-    "o = P.rotation_matrix(B)"
+    "## Coordinate systems"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 167,
+   "execution_count": 65,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/latex": [
-       "$\\displaystyle \\left[\\begin{matrix}- shank \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + x \\left(\\sin{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - \\sin{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} - \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\cos{\\left(\\theta \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\cos{\\left(\\theta \\right)}\\right) + y \\left(\\sin{\\left(\\theta \\right)} \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - \\sin{\\left(\\theta \\right)} \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} + \\cos{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - \\cos{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)}\\right)\\\\- shank \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + x \\left(- \\sin{\\left(\\theta \\right)} \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + \\sin{\\left(\\theta \\right)} \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} - \\cos{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + \\cos{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)}\\right) + y \\left(\\sin{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - \\sin{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} - \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\cos{\\left(\\theta \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\cos{\\left(\\theta \\right)}\\right)\\\\0\\end{matrix}\\right]$"
+       "$\\displaystyle \\operatorname{CoordSys3D}\\left(knee_base, \\left( \\left[\\begin{matrix}1 & 0 & 0\\\\0 & 1 & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  (shank)\\mathbf{\\hat{i}_{ankle}}\\right), \\operatorname{CoordSys3D}\\left(ankle, \\left( \\left[\\begin{matrix}\\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & - \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & 0\\\\\\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right), \\operatorname{CoordSys3D}\\left(base, \\left( \\left[\\begin{matrix}1 & 0 & 0\\\\0 & 1 & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right)\\right)\\right)\\right)$"
       ],
       "text/plain": [
-       "⎡                   d            ⎛                 d                          \n",
-       "⎢ - shank⋅sin(ψ(θ))⋅──(ψ(θ)) + x⋅⎜sin(θ)⋅cos(ψ(θ))⋅──(ψ(θ)) - sin(θ)⋅cos(ψ(θ))\n",
-       "⎢                   dθ           ⎝                 dθ                         \n",
-       "⎢                                                                             \n",
-       "⎢                  d            ⎛                   d                         \n",
-       "⎢- shank⋅cos(ψ(θ))⋅──(ψ(θ)) + x⋅⎜- sin(θ)⋅sin(ψ(θ))⋅──(ψ(θ)) + sin(θ)⋅sin(ψ(θ)\n",
-       "⎢                  dθ           ⎝                   dθ                        \n",
-       "⎢                                                                             \n",
-       "⎣                                                                             \n",
-       "\n",
-       "                    d                          ⎞     ⎛                 d      \n",
-       " - sin(ψ(θ))⋅cos(θ)⋅──(ψ(θ)) + sin(ψ(θ))⋅cos(θ)⎟ + y⋅⎜sin(θ)⋅sin(ψ(θ))⋅──(ψ(θ)\n",
-       "                    dθ                         ⎠     ⎝                 dθ     \n",
-       "                                                                              \n",
-       "                     d                          ⎞     ⎛                 d     \n",
-       ") - cos(θ)⋅cos(ψ(θ))⋅──(ψ(θ)) + cos(θ)⋅cos(ψ(θ))⎟ + y⋅⎜sin(θ)⋅cos(ψ(θ))⋅──(ψ(θ\n",
-       "                     dθ                         ⎠     ⎝                 dθ    \n",
-       "                                                                              \n",
-       "                                  0                                           \n",
-       "\n",
-       "                                        d                          ⎞ ⎤\n",
-       ") - sin(θ)⋅sin(ψ(θ)) + cos(θ)⋅cos(ψ(θ))⋅──(ψ(θ)) - cos(θ)⋅cos(ψ(θ))⎟ ⎥\n",
-       "                                        dθ                         ⎠ ⎥\n",
-       "                                                                     ⎥\n",
-       "                                         d                          ⎞⎥\n",
-       ")) - sin(θ)⋅cos(ψ(θ)) - sin(ψ(θ))⋅cos(θ)⋅──(ψ(θ)) + sin(ψ(θ))⋅cos(θ)⎟⎥\n",
-       "                                         dθ                         ⎠⎥\n",
-       "                                                                     ⎥\n",
-       "                                                                     ⎦"
+       "knee_base"
       ]
      },
-     "execution_count": 167,
+     "execution_count": 65,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
-    "from sympy import diff\n",
     "\n",
-    "dp_dtheta = diff(p, theta)\n",
-    "dp_dtheta"
+    "from sympy.vector import CoordSys3D, AxisOrienter\n",
+    "from sympy import Matrix\n",
+    "\n",
+    "# The reference system at the origin of the ankle. It is defined as follows:\n",
+    "# -z is the rotation axis of the joint\n",
+    "# x points upwards (away from the platform, aligned with the global z axis)\n",
+    "B = CoordSys3D('base')\n",
+    "\n",
+    "orienter_psi = AxisOrienter(psi, -B.k )\n",
+    "\n",
+    "# coordinate system of the ankle joint (after applying the joint rotation)\n",
+    "T_ankle = B.orient_new('ankle', (orienter_psi, ))\n",
+    "\n",
+    "# helper coordinate system at the origin of the knee after applying the knee rotation\n",
+    "T_knee_base = T_ankle.locate_new('knee_base', shank * T_ankle.i )\n",
+    "\n",
+    "T_knee_base\n",
+    "\n"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 168,
+   "execution_count": 66,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/latex": [
-       "$\\displaystyle \\left[\\begin{matrix}\\left(- \\left(\\sin{\\left(\\alpha \\right)} \\sin{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)} + \\sin{\\left(\\gamma \\right)} \\cos{\\left(\\alpha \\right)}\\right) \\sin{\\left(\\theta \\right)} + \\cos{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)} \\cos{\\left(\\theta \\right)}\\right) \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} + \\left(\\left(\\sin{\\left(\\alpha \\right)} \\sin{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)} + \\sin{\\left(\\gamma \\right)} \\cos{\\left(\\alpha \\right)}\\right) \\cos{\\left(\\theta \\right)} + \\sin{\\left(\\theta \\right)} \\cos{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)}\\right) \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & - \\left(- \\left(\\sin{\\left(\\alpha \\right)} \\sin{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)} + \\sin{\\left(\\gamma \\right)} \\cos{\\left(\\alpha \\right)}\\right) \\sin{\\left(\\theta \\right)} + \\cos{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)} \\cos{\\left(\\theta \\right)}\\right) \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} + \\left(\\left(\\sin{\\left(\\alpha \\right)} \\sin{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)} + \\sin{\\left(\\gamma \\right)} \\cos{\\left(\\alpha \\right)}\\right) \\cos{\\left(\\theta \\right)} + \\sin{\\left(\\theta \\right)} \\cos{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)}\\right) \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & \\sin{\\left(\\alpha \\right)} \\sin{\\left(\\gamma \\right)} - \\sin{\\left(\\beta \\right)} \\cos{\\left(\\alpha \\right)} \\cos{\\left(\\gamma \\right)}\\\\\\left(- \\left(- \\sin{\\left(\\alpha \\right)} \\sin{\\left(\\beta \\right)} \\sin{\\left(\\gamma \\right)} + \\cos{\\left(\\alpha \\right)} \\cos{\\left(\\gamma \\right)}\\right) \\sin{\\left(\\theta \\right)} - \\sin{\\left(\\gamma \\right)} \\cos{\\left(\\beta \\right)} \\cos{\\left(\\theta \\right)}\\right) \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} + \\left(\\left(- \\sin{\\left(\\alpha \\right)} \\sin{\\left(\\beta \\right)} \\sin{\\left(\\gamma \\right)} + \\cos{\\left(\\alpha \\right)} \\cos{\\left(\\gamma \\right)}\\right) \\cos{\\left(\\theta \\right)} - \\sin{\\left(\\gamma \\right)} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\beta \\right)}\\right) \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & - \\left(- \\left(- \\sin{\\left(\\alpha \\right)} \\sin{\\left(\\beta \\right)} \\sin{\\left(\\gamma \\right)} + \\cos{\\left(\\alpha \\right)} \\cos{\\left(\\gamma \\right)}\\right) \\sin{\\left(\\theta \\right)} - \\sin{\\left(\\gamma \\right)} \\cos{\\left(\\beta \\right)} \\cos{\\left(\\theta \\right)}\\right) \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} + \\left(\\left(- \\sin{\\left(\\alpha \\right)} \\sin{\\left(\\beta \\right)} \\sin{\\left(\\gamma \\right)} + \\cos{\\left(\\alpha \\right)} \\cos{\\left(\\gamma \\right)}\\right) \\cos{\\left(\\theta \\right)} - \\sin{\\left(\\gamma \\right)} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\beta \\right)}\\right) \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & \\sin{\\left(\\alpha \\right)} \\cos{\\left(\\gamma \\right)} + \\sin{\\left(\\beta \\right)} \\sin{\\left(\\gamma \\right)} \\cos{\\left(\\alpha \\right)}\\\\\\left(\\sin{\\left(\\alpha \\right)} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\beta \\right)} + \\sin{\\left(\\beta \\right)} \\cos{\\left(\\theta \\right)}\\right) \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} + \\left(- \\sin{\\left(\\alpha \\right)} \\cos{\\left(\\beta \\right)} \\cos{\\left(\\theta \\right)} + \\sin{\\left(\\beta \\right)} \\sin{\\left(\\theta \\right)}\\right) \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & - \\left(\\sin{\\left(\\alpha \\right)} \\sin{\\left(\\theta \\right)} \\cos{\\left(\\beta \\right)} + \\sin{\\left(\\beta \\right)} \\cos{\\left(\\theta \\right)}\\right) \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} + \\left(- \\sin{\\left(\\alpha \\right)} \\cos{\\left(\\beta \\right)} \\cos{\\left(\\theta \\right)} + \\sin{\\left(\\beta \\right)} \\sin{\\left(\\theta \\right)}\\right) \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & \\cos{\\left(\\alpha \\right)} \\cos{\\left(\\beta \\right)}\\end{matrix}\\right]$"
+       "$\\displaystyle \\operatorname{CoordSys3D}\\left(knee, \\left( \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & \\sin{\\left(\\theta \\right)} & 0\\\\- \\sin{\\left(\\theta \\right)} & \\cos{\\left(\\theta \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right), \\operatorname{CoordSys3D}\\left(knee_base, \\left( \\left[\\begin{matrix}1 & 0 & 0\\\\0 & 1 & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  (shank)\\mathbf{\\hat{i}_{ankle}}\\right), \\operatorname{CoordSys3D}\\left(ankle, \\left( \\left[\\begin{matrix}\\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & - \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & 0\\\\\\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} & \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right), \\operatorname{CoordSys3D}\\left(base, \\left( \\left[\\begin{matrix}1 & 0 & 0\\\\0 & 1 & 0\\\\0 & 0 & 1\\end{matrix}\\right], \\  \\mathbf{\\hat{0}}\\right)\\right)\\right)\\right)\\right)$"
       ],
       "text/plain": [
-       "⎡ (-(sin(α)⋅sin(β)⋅cos(γ) + sin(γ)⋅cos(α))⋅sin(θ) + cos(β)⋅cos(γ)⋅cos(θ))⋅cos(\n",
-       "⎢                                                                             \n",
-       "⎢(-(-sin(α)⋅sin(β)⋅sin(γ) + cos(α)⋅cos(γ))⋅sin(θ) - sin(γ)⋅cos(β)⋅cos(θ))⋅cos(\n",
-       "⎢                                                                             \n",
-       "⎣                                 (sin(α)⋅sin(θ)⋅cos(β) + sin(β)⋅cos(θ))⋅cos(ψ\n",
-       "\n",
-       "ψ(θ)) + ((sin(α)⋅sin(β)⋅cos(γ) + sin(γ)⋅cos(α))⋅cos(θ) + sin(θ)⋅cos(β)⋅cos(γ))\n",
-       "                                                                              \n",
-       "ψ(θ)) + ((-sin(α)⋅sin(β)⋅sin(γ) + cos(α)⋅cos(γ))⋅cos(θ) - sin(γ)⋅sin(θ)⋅cos(β)\n",
-       "                                                                              \n",
-       "(θ)) + (-sin(α)⋅cos(β)⋅cos(θ) + sin(β)⋅sin(θ))⋅sin(ψ(θ))                      \n",
-       "\n",
-       "⋅sin(ψ(θ))    -(-(sin(α)⋅sin(β)⋅cos(γ) + sin(γ)⋅cos(α))⋅sin(θ) + cos(β)⋅cos(γ)\n",
-       "                                                                              \n",
-       ")⋅sin(ψ(θ))  -(-(-sin(α)⋅sin(β)⋅sin(γ) + cos(α)⋅cos(γ))⋅sin(θ) - sin(γ)⋅cos(β)\n",
-       "                                                                              \n",
-       "                                              -(sin(α)⋅sin(θ)⋅cos(β) + sin(β)⋅\n",
-       "\n",
-       "⋅cos(θ))⋅sin(ψ(θ)) + ((sin(α)⋅sin(β)⋅cos(γ) + sin(γ)⋅cos(α))⋅cos(θ) + sin(θ)⋅c\n",
-       "                                                                              \n",
-       "⋅cos(θ))⋅sin(ψ(θ)) + ((-sin(α)⋅sin(β)⋅sin(γ) + cos(α)⋅cos(γ))⋅cos(θ) - sin(γ)⋅\n",
-       "                                                                              \n",
-       "cos(θ))⋅sin(ψ(θ)) + (-sin(α)⋅cos(β)⋅cos(θ) + sin(β)⋅sin(θ))⋅cos(ψ(θ))         \n",
-       "\n",
-       "os(β)⋅cos(γ))⋅cos(ψ(θ))   sin(α)⋅sin(γ) - sin(β)⋅cos(α)⋅cos(γ)⎤\n",
-       "                                                              ⎥\n",
-       "sin(θ)⋅cos(β))⋅cos(ψ(θ))  sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)⎥\n",
-       "                                                              ⎥\n",
-       "                                     cos(α)⋅cos(β)            ⎦"
+       "knee"
       ]
      },
-     "execution_count": 168,
+     "execution_count": 66,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
-    "o"
+    "\n",
+    "orienter_theta = AxisOrienter(theta, B.k )\n",
+    "\n",
+    "# coordinate system of the knee after applying the rotation\n",
+    "T_knee = T_knee_base.orient_new('knee', (orienter_theta, ))\n",
+    "\n",
+    "T_knee"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 169,
+   "execution_count": null,
    "metadata": {},
    "outputs": [],
-   "source": [
-    "# dp_dtheta_simpl = dp_dtheta.simplify()\n",
-    "\n",
-    "# dx_dtheta = sp.simplify(dp_dtheta[0])"
-   ]
+   "source": []
   },
   {
    "cell_type": "code",
-   "execution_count": 176,
+   "execution_count": 67,
    "metadata": {},
    "outputs": [],
    "source": [
-    "from sympy import ccode #, cpp_generator\n",
-    "\n",
-    "\n",
+    "# express the end effector in the knee joint coordinate system (T_knee). This captures the forward kinematics of child joints.\n",
+    "x, y, z, alpha, beta, gamma = symbols(\"x y z alpha beta gamma\")\n",
     "\n",
-    "cpp_code = ccode(dp_dtheta[0], assign_to=\"dx_dtheta\", standard=\"c11\")\n",
+    "from sympy.vector import BodyOrienter, Point\n",
+    "euler = BodyOrienter(alpha, beta, gamma, rot_order=\"XYZ\")\n",
     "\n",
-    "with open(\"/tmp/expressions.cpp\", \"w\") as f:\n",
-    "  f.write(cpp_code)"
+    "P_eef = T_knee.orient_new(\"EEF\", euler, location=(T_knee.i * x + T_knee.j * y + T_knee.k * z))\n",
+    "\n"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 179,
+   "execution_count": 68,
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/latex": [
-       "$\\displaystyle - shank \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + x \\left(\\sin{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - \\sin{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} - \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\cos{\\left(\\theta \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\cos{\\left(\\theta \\right)}\\right) + y \\left(\\sin{\\left(\\theta \\right)} \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - \\sin{\\left(\\theta \\right)} \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} + \\cos{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - \\cos{\\left(\\theta \\right)} \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)}\\right)$"
-      ],
-      "text/plain": [
-       "                  d            ⎛                 d                            \n",
-       "- shank⋅sin(ψ(θ))⋅──(ψ(θ)) + x⋅⎜sin(θ)⋅cos(ψ(θ))⋅──(ψ(θ)) - sin(θ)⋅cos(ψ(θ)) -\n",
-       "                  dθ           ⎝                 dθ                           \n",
-       "\n",
-       "                  d                          ⎞     ⎛                 d        \n",
-       " sin(ψ(θ))⋅cos(θ)⋅──(ψ(θ)) + sin(ψ(θ))⋅cos(θ)⎟ + y⋅⎜sin(θ)⋅sin(ψ(θ))⋅──(ψ(θ)) \n",
-       "                  dθ                         ⎠     ⎝                 dθ       \n",
-       "\n",
-       "                                      d                          ⎞\n",
-       "- sin(θ)⋅sin(ψ(θ)) + cos(θ)⋅cos(ψ(θ))⋅──(ψ(θ)) - cos(θ)⋅cos(ψ(θ))⎟\n",
-       "                                      dθ                         ⎠"
-      ]
-     },
-     "execution_count": 179,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
+   "outputs": [],
    "source": [
-    "# from sympy.codegen.rewriting import optimize, optims_c99\n",
+    "pos_vect = P_eef.origin.position_wrt(B)\n",
     "\n",
-    "# f = optimize(dp_dtheta[0], optims_c99)\n",
+    "# express the EEF in the base coordinate system (ankle base)\n",
+    "p_eef = pos_vect.to_matrix(B)\n",
     "\n",
-    "# f\n",
-    "\n",
-    "dp_dtheta[0]\n",
-    "\n"
+    "# o = P_eef.rotation_matrix(B)"
    ]
   },
   {
-   "cell_type": "code",
-   "execution_count": 180,
+   "attachments": {},
+   "cell_type": "markdown",
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/latex": [
-       "$\\displaystyle - shank \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + x \\left(\\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - 1\\right) \\sin{\\left(\\theta - \\psi{\\left(\\theta \\right)} \\right)} + y \\left(\\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - 1\\right) \\cos{\\left(\\theta - \\psi{\\left(\\theta \\right)} \\right)}$"
-      ],
-      "text/plain": [
-       "                  d            ⎛d           ⎞                   ⎛d           ⎞\n",
-       "- shank⋅sin(ψ(θ))⋅──(ψ(θ)) + x⋅⎜──(ψ(θ)) - 1⎟⋅sin(θ - ψ(θ)) + y⋅⎜──(ψ(θ)) - 1⎟\n",
-       "                  dθ           ⎝dθ          ⎠                   ⎝dθ          ⎠\n",
-       "\n",
-       "              \n",
-       "⋅cos(θ - ψ(θ))\n",
-       "              "
-      ]
-     },
-     "execution_count": 180,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
    "source": [
-    "dp_dtheta[0].simplify()"
+    "# Jacobian\n",
+    "\n",
+    "Pose of the end-effector\n",
+    "$$\n",
+    "p_{eef} = (x,y,z,\\alpha, \\beta, \\gamma )^T\n",
+    "$$\n",
+    "\n",
+    "The Jacobian $J \\in \\mathrm{R}^{6x1}$\n",
+    "$$\n",
+    "J = \\frac{\\partial p_{eef}}{\\partial \\theta}\n",
+    "$$\n"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 181,
+   "execution_count": 69,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/latex": [
-       "$\\displaystyle - shank \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - x \\left(\\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - 1\\right) \\cos{\\left(\\theta - \\psi{\\left(\\theta \\right)} \\right)} + y \\left(\\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - 1\\right) \\sin{\\left(\\theta - \\psi{\\left(\\theta \\right)} \\right)}$"
+       "$\\displaystyle \\left[\\begin{matrix}- shank \\sin{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} + x \\left(\\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - 1\\right) \\sin{\\left(\\theta - \\psi{\\left(\\theta \\right)} \\right)} + y \\left(\\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - 1\\right) \\cos{\\left(\\theta - \\psi{\\left(\\theta \\right)} \\right)}\\\\- shank \\cos{\\left(\\psi{\\left(\\theta \\right)} \\right)} \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - x \\left(\\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - 1\\right) \\cos{\\left(\\theta - \\psi{\\left(\\theta \\right)} \\right)} + y \\left(\\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)} - 1\\right) \\sin{\\left(\\theta - \\psi{\\left(\\theta \\right)} \\right)}\\\\0\\end{matrix}\\right]$"
       ],
       "text/plain": [
-       "                  d            ⎛d           ⎞                   ⎛d           ⎞\n",
-       "- shank⋅cos(ψ(θ))⋅──(ψ(θ)) - x⋅⎜──(ψ(θ)) - 1⎟⋅cos(θ - ψ(θ)) + y⋅⎜──(ψ(θ)) - 1⎟\n",
-       "                  dθ           ⎝dθ          ⎠                   ⎝dθ          ⎠\n",
+       "⎡                  d            ⎛d           ⎞                   ⎛d           \n",
+       "⎢- shank⋅sin(ψ(θ))⋅──(ψ(θ)) + x⋅⎜──(ψ(θ)) - 1⎟⋅sin(θ - ψ(θ)) + y⋅⎜──(ψ(θ)) - 1\n",
+       "⎢                  dθ           ⎝dθ          ⎠                   ⎝dθ          \n",
+       "⎢                                                                             \n",
+       "⎢                  d            ⎛d           ⎞                   ⎛d           \n",
+       "⎢- shank⋅cos(ψ(θ))⋅──(ψ(θ)) - x⋅⎜──(ψ(θ)) - 1⎟⋅cos(θ - ψ(θ)) + y⋅⎜──(ψ(θ)) - 1\n",
+       "⎢                  dθ           ⎝dθ          ⎠                   ⎝dθ          \n",
+       "⎢                                                                             \n",
+       "⎣                                             0                               \n",
        "\n",
-       "              \n",
-       "⋅sin(θ - ψ(θ))\n",
-       "              "
-      ]
-     },
-     "execution_count": 181,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "dp_dtheta[1].simplify()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 182,
-   "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/latex": [
-       "$\\displaystyle 0$"
-      ],
-      "text/plain": [
-       "0"
+       "⎞              ⎤\n",
+       "⎟⋅cos(θ - ψ(θ))⎥\n",
+       "⎠              ⎥\n",
+       "               ⎥\n",
+       "⎞              ⎥\n",
+       "⎟⋅sin(θ - ψ(θ))⎥\n",
+       "⎠              ⎥\n",
+       "               ⎥\n",
+       "               ⎦"
       ]
      },
-     "execution_count": 182,
+     "execution_count": 69,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
-    "dp_dtheta[2].simplify()"
+    "from sympy import diff\n",
+    "\n",
+    "# position part of the Jacobian\n",
+    "dp_dtheta = diff(p_eef, theta)\n",
+    "dp_dtheta = dp_dtheta.simplify()\n",
+    "\n",
+    "dp_dtheta\n"
    ]
   },
   {
-   "cell_type": "code",
-   "execution_count": 175,
+   "attachments": {},
+   "cell_type": "markdown",
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/latex": [
-       "$\\displaystyle \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)}$"
-      ],
-      "text/plain": [
-       "d       \n",
-       "──(ψ(θ))\n",
-       "dθ      "
-      ]
-     },
-     "execution_count": 175,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
    "source": [
-    "psi_precompute = symbols(\"psi_precompute\")\n",
-    "dpsi_dtheta_precompute = symbols(\"dpsi_dtheta_precompute\")\n",
-    "psi_inner_precompute = symbols(\"psi_inner_precompute\")\n",
+    "the expression $\\frac{\\partial \\mathbf{p}}{\\partial \\theta}$ above yields $\\frac{\\partial x}{\\partial\\theta}$ and $\\frac{\\partial y}{\\partial\\theta}$. $\\frac{\\partial z}{\\partial\\theta}$ is $0$.\n",
     "\n",
-    "dpsi_inner = diff(psi_inner, theta)\n",
-    "dpsi_inner_precompute = symbols(\"dpsi_inner\")\n",
     "\n",
-    "dpsi_dtheta = diff(psi, theta)\n",
+    "To obtain the orientation part, we only need to consider the angle $\\gamma$ as the joint mechanism is 2D (rotatation around z axis). In other words  $\\frac{\\partial \\alpha}{\\partial\\theta} = \\frac{\\partial \\beta}{\\partial\\theta} = 0$\n",
     "\n",
-    "s = dp_dtheta[0].replace(psi, psi_precompute)\n",
-    "s = s.replace(dpsi_dtheta, dpsi_dtheta_precompute)\n",
-    "s = s.replace(psi_inner, psi_inner_precompute)\n",
-    "s = s.replace(dpsi_inner, dpsi_inner_precompute)\n",
     "\n",
-    "dpsi_dtheta"
+    "\n",
+    "It is\n",
+    "\n",
+    "$$ \n",
+    "\\gamma = \\gamma_{0} -\\psi + \\theta\n",
+    "$$\n",
+    "\n",
+    "Partial derivative:\n",
+    "\n",
+    "$$\n",
+    "\\frac{\\partial \\gamma}{\\partial\\theta} = 1 - \\frac{\\partial \\psi}{\\partial\\theta} \n",
+    "$$\n",
+    "\n",
+    "\n"
    ]
   },
   {
-   "cell_type": "code",
-   "execution_count": 173,
+   "attachments": {},
+   "cell_type": "markdown",
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/latex": [
-       "$\\displaystyle \\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)}$"
-      ],
-      "text/plain": [
-       "d       \n",
-       "──(ψ(θ))\n",
-       "dθ      "
-      ]
-     },
-     "execution_count": 173,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
    "source": [
-    "dpsi_dtheta"
+    "# Automatic code generation\n",
+    "\n",
+    "It is advantageous to implement the equations above by hand. Sine and cosine terms can be computed once for improved efficiency.\n",
+    "\n",
+    "*The following is just for reference*"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 174,
+   "execution_count": 70,
    "metadata": {},
-   "outputs": [
-    {
-     "data": {
-      "text/latex": [
-       "$\\displaystyle \\frac{x \\sin{\\left(\\psi_{precompute} - \\theta \\right)} - y \\cos{\\left(\\psi_{precompute} - \\theta \\right)}}{\\frac{d}{d \\theta} \\psi{\\left(\\theta \\right)}}$"
-      ],
-      "text/plain": [
-       "x⋅sin(ψ_precompute - θ) - y⋅cos(ψ_precompute - θ)\n",
-       "─────────────────────────────────────────────────\n",
-       "                     d                           \n",
-       "                     ──(ψ(θ))                    \n",
-       "                     dθ                          "
-      ]
-     },
-     "execution_count": 174,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
+   "outputs": [],
    "source": [
-    "ff = s / dpsi_dtheta\n",
+    "from sympy import ccode #, cpp_generator\n",
+    "\n",
+    "def generate_code():\n",
+    "\n",
+    "  cpp_code = ccode(dp_dtheta[0], assign_to=\"dx_dtheta\", standard=\"c11\")\n",
     "\n",
-    "ff.simplify()"
+    "  with open(\"/tmp/expressions.cpp\", \"w\") as f:\n",
+    "    f.write(cpp_code)"
    ]
   }
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "armarx",
+   "display_name": "simox_control",
    "language": "python",
    "name": "python3"
   },
@@ -561,12 +488,12 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.7.11"
+   "version": "3.7.13"
   },
   "orig_nbformat": 4,
   "vscode": {
    "interpreter": {
-    "hash": "edf8927144584185110aeeacb4ee22a221f99c19957d335f96fa1b4f8f5621f4"
+    "hash": "e4599465d50e71b5ac4fe67b3a4b0cd1869600672e1938bc9720f881263763f9"
    }
   }
  },