Forward Kinematics of the Human Ankle Joint.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Turmell-Meter: A Device for estimating the Subtalar and Talocrural axis of the Human Ankle joint by Applying the Product of Exponentials Formula.\n",
    "\n",
    "## Óscar Agudelo, Ángel Valera, and Julio Vargas-Riaño\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the following cell the initial values from anthropometric values and statistics are loaded, three points represent a plane parallel to the foot, for the position estimation. The figure shows the normal planes to the ankle joint axes and trajectories mapped, representing circular shapes."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Ankle joint forward kinematics by using Product of Exponentials Formula\n",
    "\n",
    "from sage.plot.plot3d.shapes2 import *\n",
    "from sage.plot.plot3d.tachyon import ParametricPlot\n",
    "\n",
    "# Rotation axis in a unitary vector and angle in degrees\n",
    "def rot_ax_ang(u,a):\n",
    "    #degrees to radians\n",
    "    alpha=a*pi/180\n",
    "    na=(1-cos(alpha))\n",
    "    ca=cos(alpha)\n",
    "    sa=sin(alpha)\n",
    "    #rotation matrix\n",
    "    R=matrix([[u[0]^2*na+ca, u[0]*u[1]*na-u[2]*sa, u[0]*u[2]*na+u[1]*sa],\n",
    "            [u[0]*u[1]*na+u[2]*sa, u[1]^2*na+ca, u[1]*u[2]*na-u[0]*sa],\n",
    "            [u[2]*u[0]*na-u[1]*sa, u[2]*u[1]*na+u[0]*sa, u[2]^2*na+ca]])\n",
    "    return R\n",
    "\n",
    "pvar=0.9 #percentual variation\n",
    "\n",
    "#Talocrural direction\n",
    "TCa1=-6*pvar\n",
    "TCa2=80*pvar\n",
    "vTC=rot_ax_ang([0,0,1], TCa1)*rot_ax_ang([1,0,0], TCa2)*vector([0,0,1])\n",
    "#Subtalar direction\n",
    "STa1=23*pvar\n",
    "STa2=41*pvar\n",
    "vST=rot_ax_ang([0,0,1], STa1)*rot_ax_ang([0,1,0], STa2)*vector([1,0,0])\n",
    "\n",
    "#Anthropometric values\n",
    "#vK=12;vL=11;vO=16;vP=1;vQ=5;vR=0.54 #anthropometric measurements\n",
    "vK=12*pvar;vL=11*pvar;vO=16*pvar;vP=1*pvar;vQ=5*pvar;vR=0.54*pvar #anthropometric measurements\n",
    "#H=1750 #heigth in mm\n",
    "H=1750*pvar #heigth in mm\n",
    "d_m=(0.285-0.039)*H/2 #knee-ankle half distance\n",
    "d_p=0.039*H #ankle-foot\n",
    "z_p=-(d_m+d_p) # z initial position \n",
    "r_p=0.055*H*2/3 #platform radius \n",
    "ae=2*pi/3 #auxiliar angle\n",
    "PO=vector([0,0,0]) #Origin at middle shank\n",
    "\n",
    "#initial platform position\n",
    "A0=vector([r_p,0,z_p]) \n",
    "B0=vector([r_p*cos(ae),r_p*sin(ae),z_p])\n",
    "C0=vector([r_p*cos(-ae),r_p*sin(-ae),z_p])\n",
    "PM0=(A0+B0+C0)/3 #center of the platform\n",
    "\n",
    "#marker representation\n",
    "dot=point3d((0,0,0),size=5,color='red',opacity=.5)\n",
    "dPO=dot.translate(PO)\n",
    "dA0=dot.translate(A0)\n",
    "dB0=dot.translate(B0)\n",
    "dC0=dot.translate(C0)\n",
    "dPM0=dot.translate(PM0)\n",
    "\n",
    "r_1=vector([0,0,-d_m]) #intersection point between the talocrural axis and the sagittal plane\n",
    "ap=0.039*H \n",
    "lp=0.152*H\n",
    "\n",
    "#points on the malleolar medial and lateral\n",
    "M_1=r_1-ap*vR*vTC\n",
    "M_2=r_1+ap*(1-vR)*vTC\n",
    "tht=(STa2)\n",
    "\n",
    "#subtalar axis and the sagittal plane intersection \n",
    "r_2=vQ*vector([-cos(tht),0,-sin(tht)])+r_1\n",
    "\n",
    "#points from the hindfoot to the midfoot\n",
    "N_1=r_2+0.6*lp*vR*vST\n",
    "N_2=r_2-0.3*lp*vR*vST\n",
    "\n",
    "#representation of the kinematic chain\n",
    "lrt=line([PO,r_1])\n",
    "lm12=line([M_1,M_2])\n",
    "ln12=line([N_1,N_2])\n",
    "lrA=line([r_2,A0])\n",
    "lrB=line([r_2,B0])\n",
    "lrC=line([r_2,C0])\n",
    "\n",
    "#markers representation\n",
    "dr1=dot.translate(r_1)\n",
    "dr2=dot.translate(r_2)\n",
    "dM1=dot.translate(M_1)\n",
    "dM2=dot.translate(M_2)\n",
    "dN1=dot.translate(N_1)\n",
    "dN2=dot.translate(N_2)\n",
    "\n",
    "#reference labels\n",
    "kwds = {'color':'black','fontsize':24,'fontweight':'bold','rotation':30}\n",
    "plbl=vector([0,0,8])\n",
    "lbp = text3d('PO',PO+plbl,**kwds)\n",
    "lbp += text3d('A0',A0++plbl,**kwds)\n",
    "lbp += text3d('B0',B0++plbl,**kwds)\n",
    "lbp += text3d('C0',C0++plbl,**kwds)\n",
    "lbp += text3d('PM0',PM0++plbl,**kwds)\n",
    "#axes labels\n",
    "lbf = text3d('M1',M_1++plbl,**kwds)\n",
    "lbf += text3d('M2',M_2++plbl,**kwds)\n",
    "lbf += text3d('N1',N_1++plbl,**kwds)\n",
    "lbf += text3d('N2',N_2++plbl,**kwds)\n",
    "lbf += text3d('r1',r_1++plbl,**kwds)\n",
    "lbf += text3d('r2',r_2++plbl,**kwds)\n",
    "\n",
    "\n",
    "#finding the twist unitary vectors \n",
    "w1=(M_1-M_2)/abs(M_1-M_2)\n",
    "w2=(N_1-N_2)/abs(N_1-N_2)\n",
    "\n",
    "#the perpendicular component\n",
    "v1=-w1.cross_product(r_1)\n",
    "v2=-w2.cross_product(r_2)\n",
    "\n",
    "#angles for the talocrural and subtalar rotations\n",
    "theta1=var('theta_1')\n",
    "theta2=var('theta_2')\n",
    "\n",
    "#six dimensional vector xi mapping \n",
    "xi1=matrix([v1[0],v1[1],v1[2],w1[0],w1[1],w1[2]]).transpose()\n",
    "xi2=matrix([v2[0],v2[1],v2[2],w2[0],w2[1],w2[2]]).transpose()\n",
    "\n",
    "#transformation of exponential matrix of rotation\n",
    "Rexp1=rot_ax_ang(w1,theta1)\n",
    "\n",
    "#rotation matrix component of the homogeneous transformation\n",
    "vexp1=(matrix.identity(3)-Rexp1)*(w1.cross_product(v1))\n",
    "\n",
    "#conformation of the homogeneous transformation matrix\n",
    "MTH1=(Rexp1.augment(vexp1)).stack(vector([0,0,0,1]))\n",
    "\n",
    "#components for the subtalar axis\n",
    "Rexp2=rot_ax_ang(w2,theta2)\n",
    "vexp2=(matrix.identity(3)-Rexp2)*(w2.cross_product(v2))\n",
    "MTH2=(Rexp2.augment(vexp2)).stack(vector([0,0,0,1]))\n",
    "\n",
    "#transformation matrix representing the initial position\n",
    "gst0A=matrix([[1,0,0,A0[0]],[0,1,0,A0[1]],[0,0,1,A0[2]],[0,0,0,1]])\n",
    "gst0B=matrix([[1,0,0,B0[0]],[0,1,0,B0[1]],[0,0,1,B0[2]],[0,0,0,1]])\n",
    "gst0C=matrix([[1,0,0,C0[0]],[0,1,0,C0[1]],[0,0,1,C0[2]],[0,0,0,1]])\n",
    "gst0PM=matrix([[1,0,0,PM0[0]],[0,1,0,PM0[1]],[0,0,1,PM0[2]],[0,0,0,1]])\n",
    "\n",
    "#product of exponential matrices for all the points\n",
    "MTHA=MTH1*MTH2*gst0A\n",
    "MTHB=MTH1*MTH2*gst0B\n",
    "MTHC=MTH1*MTH2*gst0C\n",
    "MTHPM=MTH1*MTH2*gst0PM\n",
    "\n",
    "#components of the group of rigid movements for the central point\n",
    "f_xpm=MTHPM[0][3]\n",
    "f_ypm=MTHPM[1][3]\n",
    "f_zpm=MTHPM[2][3]\n",
    "#orthogonal direction vectors\n",
    "f_spm=vector([MTHPM[0][0],MTHPM[1][0],MTHPM[2][0]])\n",
    "f_npm=vector([MTHPM[0][1],MTHPM[1][1],MTHPM[2][1]])\n",
    "f_apm=vector([MTHPM[0][2],MTHPM[1][2],MTHPM[2][2]])\n",
    "\n",
    "#components of the three vertices of the plattform\n",
    "f_xA=MTHA[0][3]\n",
    "f_yA=MTHA[1][3]\n",
    "f_zA=MTHA[2][3]\n",
    "f_xB=MTHB[0][3]\n",
    "f_yB=MTHB[1][3]\n",
    "f_zB=MTHB[2][3]\n",
    "f_xC=MTHC[0][3]\n",
    "f_yC=MTHC[1][3]\n",
    "f_zC=MTHC[2][3]\n",
    "\n",
    "sc=40 #scale factor\n",
    "ar1=arrow3d(PO,r_1,sc,color='cyan')\n",
    "ar2=arrow3d(PO,r_2,sc,color='magenta')\n",
    "aw1=arrow3d(r_1, r_1 + sc*w1,sc, color ='black')\n",
    "aw2=arrow3d(r_2, r_2 + sc*w2,sc, color = 'black')\n",
    "av1=arrow3d(r_1, r_1 + sc*v1/abs(r_1),sc, color ='black')\n",
    "av2=arrow3d(r_2, r_2 + sc*v2/abs(r_2),sc, color ='black')\n",
    "#orientation frame at PM0    \n",
    "spm=vector([f_spm[0].subs(theta1==0,theta2==0),\n",
    "            f_spm[1].subs(theta1==0,theta2==0),\n",
    "            f_spm[2].subs(theta1==0,theta2==0)]).n()\n",
    "npm=vector([f_npm[0].subs(theta1==0,theta2==0),\n",
    "           f_npm[1].subs(theta1==0,theta2==0),\n",
    "           f_npm[2].subs(theta1==0,theta2==0)]).n()\n",
    "apm=vector([f_apm[0].subs(theta1==0,theta2==0),\n",
    "           f_apm[1].subs(theta1==0,theta2==0),\n",
    "           f_apm[2].subs(theta1==0,theta2==0)])\n",
    "spmv=arrow3d(PM0, PM0 + sc*spm,sc, color ='red')\n",
    "npmv=arrow3d(PM0, PM0 + sc*npm,sc, color ='green')\n",
    "apmv=arrow3d(PM0, PM0 + sc*apm,sc, color ='blue')\n",
    "\n",
    "scrws = ar1 +ar2+ aw1+aw2 + av1 + av2 + spmv + npmv + apmv\n",
    "\n",
    "#noise matrix\n",
    "rdm = random_matrix(ZZ, 3, 3)\n",
    "rdm = (rdm.augment(vector([0,0,0]))).stack(vector([0,0,0,0]))\n",
    "\n",
    "#trajectory holding constant angles\n",
    "dp=30\n",
    "t2kpm=MTHPM.subs(theta2==0)\n",
    "trkt2=parametric_plot3d([t2kpm[0][3],t2kpm[1][3],t2kpm[2][3]],\n",
    "                        (theta_1,-15,15),\n",
    "                        texture= 'red', thickness='10', plot_points=dp,\n",
    "                        radius=1)\n",
    "t1kpm=MTHPM.subs(theta1==0)\n",
    "trkt1=parametric_plot3d([t1kpm[0][3],t1kpm[1][3],t1kpm[2][3]],\n",
    "                        (theta_2,-15,15),\n",
    "                        texture= 'green', thickness='10', plot_points=dp,\n",
    "                        radius=1)\n",
    "\n",
    "#normal plane to TC axis\n",
    "tcpl = implicit_plot3d(lambda x,y,z: w1.dot_product(vector([x,y,z])-PM0), \n",
    "                       (-30+PM0[0],30+PM0[0]), (-30+PM0[1],30+PM0[1]), \n",
    "                       (-30+PM0[2],30+PM0[2]),color='green',opacity=0.5)\n",
    "#normal plane to ST axis\n",
    "stpl = implicit_plot3d(lambda x,y,z: w2.dot_product(vector([x,y,z])-PM0), \n",
    "                       (-30+PM0[0],30+PM0[0]), (-30+PM0[1],30+PM0[1]), \n",
    "                       (-30+PM0[2],30+PM0[2]), color ='red',opacity=0.5)\n",
    "\n",
    "#trajectory angles depending on parameter t\n",
    "dp=30\n",
    "tmax=15\n",
    "var('t')\n",
    "tpm=MTHPM.subs(theta1==t,theta2==t)\n",
    "vectpm = vector([tpm[0][3],tpm[1][3],tpm[2][3]])\n",
    "ttpm=parametric_plot3d(vectpm, (t,0,tmax),\n",
    "                        texture= 'blue', thickness='10', plot_points=dp,\n",
    "                      radius=1)\n",
    "#mapped trajectory\n",
    "prjtr=vectpm - vectpm.dot_product(apm)*(apm)\n",
    "pltrj=parametric_plot3d(prjtr  + PM0, (t,0,tmax),\n",
    "                        texture= 'cyan', thickness='10', plot_points=dp,\n",
    "                       radius=1)\n",
    "#mapped trajectory to w1\n",
    "prjw1=prjtr - prjtr.dot_product(w1)*(w1)\n",
    "pltw1=parametric_plot3d(prjw1  + PM0, (t,0,tmax),\n",
    "                        texture= 'yellow', thickness='10', plot_points=dp,\n",
    "                       radius=1)\n",
    "\n",
    "#mapped trajectory to w2\n",
    "prjw2=prjtr - prjtr.dot_product(w2)*(w2)\n",
    "pltw2=parametric_plot3d(prjw2  + PM0, (t,0,tmax),\n",
    "                        texture= 'yellow', thickness='10', plot_points=dp,\n",
    "                       radius=1)\n",
    "#platform initial position\n",
    "pltf= polygon([A0,B0,C0],color='gray',opacity=1)\n",
    "\n",
    "#show the initial position, planes and trajectory projections\n",
    "gr = dPO+dA0+dB0+dC0+dPM0 + tcpl + stpl + ttpm #+ pltrj + pltw1 + pltw2\n",
    "gr+= lrt+lm12+ln12+lrA+lrB+lrC\n",
    "gr+= dM1+dM2+dr1+dr2+dN1+dN2\n",
    "\n",
    "\n",
    "gr+= lbp+lbf+scrws+pltf\n",
    "gr.show(viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "    figsize=[60,60], # the image resolution is 100*figsize\n",
    "    camera_position=[3, -4, 2], # a distant camera position combined with \n",
    "    zoom=2, # a large zoom factor will decrease perspective distortion.\n",
    "    updir=(0, 0, 2), # the camera is slightly tilted\n",
    "    viewdir=(-2.2,3.,-1.5), # slightly off-center\n",
    "    light_position=(4.0, -4.0, 3.0),\n",
    "    frame=false,axes = false,aspect_ratio=[1,1,1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following cell plot a manifold representation of the platform central point. Also a trajectory mapped to the surface in a constant direction."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#manifold PM \n",
    "dp=100\n",
    "tmin=0\n",
    "tmax=360\n",
    "steps=tmax/20\n",
    "t=var('t')\n",
    "\n",
    "tpm=MTHPM.subs(theta1==t,theta2==t)\n",
    "vectpm = vector([tpm[0][3],tpm[1][3],tpm[2][3]])\n",
    "ttpm=parametric_plot3d(vectpm, (t,0,tmax),\n",
    "                       texture= 'blue', \n",
    "                       thickness='20', \n",
    "                       radius = 3,\n",
    "                       plot_points=dp)\n",
    "for t1 in srange(tmin,tmax,steps):\n",
    "    tpm=MTHPM.subs(theta1==t1,theta2==t)\n",
    "    vectpm = vector([tpm[0][3],tpm[1][3],tpm[2][3]])\n",
    "    ttpm += parametric_plot3d(vectpm, (t,tmin,tmax),\n",
    "                              texture= 'magenta', \n",
    "                              thickness='10',\n",
    "                              radius = 1,\n",
    "                              plot_points=dp)\n",
    "for t1 in srange(tmin,tmax,steps):\n",
    "    tpm=MTHPM.subs(theta1==t,theta2==t1)\n",
    "    vectpm = vector([tpm[0][3],tpm[1][3],tpm[2][3]])\n",
    "    ttpm += parametric_plot3d(vectpm, (t,tmin,tmax),\n",
    "                              texture= 'cyan', \n",
    "                               thickness='10',\n",
    "                              radius = 1,\n",
    "                              plot_points=dp)\n",
    "\n",
    "kwds = {'color':'black','fontsize':24,'fontweight':'bold','rotation':30}\n",
    "lbpo = text3d('A0',A0++plbl,**kwds)\n",
    "lbpo += text3d('B0',B0++plbl,**kwds)\n",
    "lbpo += text3d('C0',C0++plbl,**kwds)\n",
    "lbpo += text3d('PM0',PM0++plbl,**kwds)\n",
    "ttpm+=aw1+aw2 + av1 + av2 + spmv + npmv + apmv + pltf\n",
    "ttpm.save('pmsim2.png',viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "    figsize=[60,60], # the image resolution is 100*figsize\n",
    "    camera_position=[3, -4, 2], # a distant camera position combined with \n",
    "    zoom=2, # a large zoom factor will decrease perspective distortion.\n",
    "    updir=(0, 0, 2), # the camera is slightly tilted\n",
    "    viewdir=(-2.2,3.,-1.5), # slightly off-center\n",
    "    light_position=(4.0, -4.0, 3.0),\n",
    "    frame=false,axes = false,aspect_ratio=[1,1,1])\n",
    "ttpm.show(viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "    figsize=[60,60], # the image resolution is 100*figsize\n",
    "    camera_position=[3, -4, 2], # a distant camera position combined with \n",
    "    zoom=2, # a large zoom factor will decrease perspective distortion.\n",
    "    updir=(0, 0, 2), # the camera is slightly tilted\n",
    "    viewdir=(-2.2,3.,-1.5), # slightly off-center\n",
    "    light_position=(4.0, -4.0, 3.0),\n",
    "    frame=false,axes = false,aspect_ratio=[1,1,1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following cell plots the manifold representation for the attachment point A."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#manifold A\n",
    "dp=100\n",
    "t=var('t')\n",
    "\n",
    "ta=MTHA.subs(theta1==t,theta2==t)\n",
    "vecta = vector([ta[0][3],ta[1][3],ta[2][3]])\n",
    "tta=parametric_plot3d(vecta, (t,0,tmax),\n",
    "                        texture= 'blue', thickness='20', plot_points=dp,\n",
    "                     radius = 3)\n",
    "for t1 in srange(tmin,tmax,steps):\n",
    "    ta=MTHA.subs(theta1==t1,theta2==t)\n",
    "    vecta = vector([ta[0][3],ta[1][3],ta[2][3]])\n",
    "    tta += parametric_plot3d(vecta, (t,tmin,tmax),\n",
    "                        texture= 'magenta', thickness='10', plot_points=dp,\n",
    "                            radius = 1)\n",
    "for t1 in srange(tmin,tmax,steps):\n",
    "    ta=MTHA.subs(theta1==t,theta2==t1)\n",
    "    vecta = vector([ta[0][3],ta[1][3],ta[2][3]])\n",
    "    tta += parametric_plot3d(vecta, (t,tmin,tmax),\n",
    "                        texture= 'cyan', thickness='10', plot_points=dp,\n",
    "                           radius = 1,)\n",
    "#components of the group of rigid movements for the central point\n",
    "f_xa=MTHA[0][3]\n",
    "f_ya=MTHA[1][3]\n",
    "f_za=MTHA[2][3]\n",
    "#orthogonal direction vectors\n",
    "f_sa=vector([MTHA[0][0],MTHA[1][0],MTHA[2][0]])\n",
    "f_na=vector([MTHA[0][1],MTHA[1][1],MTHA[2][1]])\n",
    "f_aa=vector([MTHA[0][2],MTHA[1][2],MTHA[2][2]])\n",
    "#orientation frame at PM0    \n",
    "sa=vector([f_sa[0].subs(theta1==0,theta2==0),\n",
    "            f_sa[1].subs(theta1==0,theta2==0),\n",
    "            f_sa[2].subs(theta1==0,theta2==0)]).n()\n",
    "na=vector([f_na[0].subs(theta1==0,theta2==0),\n",
    "           f_na[1].subs(theta1==0,theta2==0),\n",
    "           f_na[2].subs(theta1==0,theta2==0)]).n()\n",
    "aa=vector([f_aa[0].subs(theta1==0,theta2==0),\n",
    "           f_aa[1].subs(theta1==0,theta2==0),\n",
    "           f_aa[2].subs(theta1==0,theta2==0)])\n",
    "sav=arrow3d(A0, A0 + sc*sa,sc, color ='red')\n",
    "nav=arrow3d(A0, A0 + sc*na,sc, color ='green')\n",
    "aav=arrow3d(A0, A0 + sc*aa,sc, color ='blue')\n",
    "\n",
    "tta+=aw1+aw2 + av1 + av2 + sav + nav + aav + pltf\n",
    "tta.save('asim2.png',viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "    figsize=[60,60], # the image resolution is 100*figsize\n",
    "    camera_position=[3, -4, 2], # a distant camera position combined with \n",
    "    zoom=2, # a large zoom factor will decrease perspective distortion.\n",
    "    updir=(0, 0, 2), # the camera is slightly tilted\n",
    "    viewdir=(-2.2,3.,-1.5), # slightly off-center\n",
    "    light_position=(4.0, -4.0, 3.0),\n",
    "    frame=false,axes = false,aspect_ratio=[1,1,1])\n",
    "tta.show(viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "    figsize=[60,60], # the image resolution is 100*figsize\n",
    "    camera_position=[3, -4, 2], # a distant camera position combined with \n",
    "    zoom=2, # a large zoom factor will decrease perspective distortion.\n",
    "    updir=(0, 0, 2), # the camera is slightly tilted\n",
    "    viewdir=(-2.2,3.,-1.5), # slightly off-center\n",
    "    light_position=(4.0, -4.0, 3.0),\n",
    "    frame=false,axes = false,aspect_ratio=[1,1,1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following cell shows the manifold for the attaching point B"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#manifold B\n",
    "dp=100\n",
    "tmin=0\n",
    "tmax=360\n",
    "t=var('t')\n",
    "\n",
    "tb=MTHB.subs(theta1==t,theta2==t)\n",
    "vectb = vector([tb[0][3],tb[1][3],tb[2][3]])\n",
    "ttb=parametric_plot3d(vectb, (t,0,tmax),\n",
    "                        texture= 'blue', thickness='40', plot_points=dp,\n",
    "                    radius = 3)\n",
    "for t1 in srange(tmin,tmax,steps):\n",
    "    tb=MTHB.subs(theta1==t1,theta2==t)\n",
    "    vectb = vector([tb[0][3],tb[1][3],tb[2][3]])\n",
    "    ttb += parametric_plot3d(vectb, (t,tmin,tmax),\n",
    "                        texture= 'magenta', thickness='20', plot_points=dp,\n",
    "                            radius = 1)\n",
    "for t1 in srange(tmin,tmax,steps):\n",
    "    tb=MTHB.subs(theta1==t,theta2==t1)\n",
    "    vectb = vector([tb[0][3],tb[1][3],tb[2][3]])\n",
    "    ttb += parametric_plot3d(vectb, (t,tmin,tmax),\n",
    "                        texture= 'cyan', thickness='20', plot_points=dp,\n",
    "                           radius = 1)\n",
    "\n",
    "#components of the group of rigid movements \n",
    "f_xb=MTHB[0][3]\n",
    "f_yb=MTHB[1][3]\n",
    "f_zb=MTHB[2][3]\n",
    "#orthogonal direction vectors\n",
    "f_sb=vector([MTHB[0][0],MTHB[1][0],MTHB[2][0]])\n",
    "f_nb=vector([MTHB[0][1],MTHB[1][1],MTHB[2][1]])\n",
    "f_ab=vector([MTHB[0][2],MTHB[1][2],MTHB[2][2]])\n",
    "#orientation frame at B0    \n",
    "sb=vector([f_sb[0].subs(theta1==0,theta2==0),\n",
    "            f_sb[1].subs(theta1==0,theta2==0),\n",
    "            f_sb[2].subs(theta1==0,theta2==0)]).n()\n",
    "nb=vector([f_nb[0].subs(theta1==0,theta2==0),\n",
    "           f_nb[1].subs(theta1==0,theta2==0),\n",
    "           f_nb[2].subs(theta1==0,theta2==0)]).n()\n",
    "ab=vector([f_ab[0].subs(theta1==0,theta2==0),\n",
    "           f_ab[1].subs(theta1==0,theta2==0),\n",
    "           f_ab[2].subs(theta1==0,theta2==0)])\n",
    "sbv=arrow3d(B0, B0 + sc*sa,sc, color ='red')\n",
    "nbv=arrow3d(B0, B0 + sc*na,sc, color ='green')\n",
    "abv=arrow3d(B0, B0 + sc*aa,sc, color ='blue')\n",
    "\n",
    "ttb+=aw1+aw2 + av1 + av2 + sbv + nbv + abv + pltf\n",
    "\n",
    "ttb.save('bsim2.png',viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "    figsize=[60,60], # the image resolution is 100*figsize\n",
    "    camera_position=[3, -4, 2], # a distant camera position combined with \n",
    "    zoom=2, # a large zoom factor will decrease perspective distortion.\n",
    "    updir=(0, 0, 2), # the camera is slightly tilted\n",
    "    viewdir=(-2.2,3.,-1.5), # slightly off-center\n",
    "    light_position=(4.0, -4.0, 3.0),\n",
    "    frame=false,axes = false,aspect_ratio=[1,1,1])\n",
    "ttb.show(viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "    figsize=[60,60], # the image resolution is 100*figsize\n",
    "    camera_position=[3, -4, 2], # a distant camera position combined with \n",
    "    zoom=2, # a large zoom factor will decrease perspective distortion.\n",
    "    updir=(0, 0, 2), # the camera is slightly tilted\n",
    "    viewdir=(-2.2,3.,-1.5), # slightly off-center\n",
    "    light_position=(4.0, -4.0, 3.0),\n",
    "    frame=false,axes = false,aspect_ratio=[1,1,1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following cell plots the manifold for the point C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#manifold c\n",
    "dp=100\n",
    "tmin=0\n",
    "tmax=360\n",
    "t=var('t')\n",
    "\n",
    "tc=MTHC.subs(theta1==t,theta2==t)\n",
    "vectc = vector([tc[0][3],tc[1][3],tc[2][3]])\n",
    "ttc=parametric_plot3d(vectc, (t,0,tmax),\n",
    "                        texture= 'blue', thickness='20', plot_points=dp,\n",
    "                        radius = 3)\n",
    "for t1 in srange(tmin,tmax,steps):\n",
    "    tc=MTHC.subs(theta1==t1,theta2==t)\n",
    "    vectc = vector([tc[0][3],tc[1][3],tc[2][3]])\n",
    "    ttc+= parametric_plot3d(vectc, (t,tmin,tmax),\n",
    "                        texture= 'magenta', thickness='10', plot_points=dp,\n",
    "                        radius = 1)\n",
    "for t1 in srange(tmin,tmax,steps):\n",
    "    tc=MTHC.subs(theta1==t,theta2==t1)\n",
    "    vectc = vector([tc[0][3],tc[1][3],tc[2][3]])\n",
    "    ttc += parametric_plot3d(vectc, (t,tmin,tmax),\n",
    "                        texture= 'cyan', thickness='10', plot_points=dp,\n",
    "                        radius = 1)\n",
    "    \n",
    "#components of the group of rigid movements \n",
    "f_xc=MTHC[0][3]\n",
    "f_yc=MTHC[1][3]\n",
    "f_zc=MTHC[2][3]\n",
    "#orthogonal direction vectors\n",
    "f_sc=vector([MTHC[0][0],MTHC[1][0],MTHC[2][0]])\n",
    "f_nc=vector([MTHC[0][1],MTHC[1][1],MTHC[2][1]])\n",
    "f_ac=vector([MTHC[0][2],MTHC[1][2],MTHC[2][2]])\n",
    "#orientation frame at C0    \n",
    "scp=vector([f_sc[0].subs(theta1==0,theta2==0),\n",
    "            f_sc[1].subs(theta1==0,theta2==0),\n",
    "            f_sc[2].subs(theta1==0,theta2==0)]).n()\n",
    "nc=vector([f_nc[0].subs(theta1==0,theta2==0),\n",
    "           f_nc[1].subs(theta1==0,theta2==0),\n",
    "           f_nc[2].subs(theta1==0,theta2==0)]).n()\n",
    "ac=vector([f_ac[0].subs(theta1==0,theta2==0),\n",
    "           f_ac[1].subs(theta1==0,theta2==0),\n",
    "           f_ac[2].subs(theta1==0,theta2==0)])\n",
    "scv=arrow3d(C0, C0 + sc*scp,sc, color ='red')\n",
    "ncv=arrow3d(C0, C0 + sc*nc,sc, color ='green')\n",
    "acv=arrow3d(C0, C0 + sc*ac,sc, color ='blue')\n",
    "\n",
    "ttc+=aw1+aw2 + av1 + av2 + scv + ncv + acv + pltf\n",
    "\n",
    "ttc.save('csim2.png',viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "    figsize=[60,60], # the image resolution is 100*figsize\n",
    "    camera_position=[3, -4, 2], # a distant camera position combined with \n",
    "    zoom=2, # a large zoom factor will decrease perspective distortion.\n",
    "    updir=(0, 0, 2), # the camera is slightly tilted\n",
    "    viewdir=(-2.2,3.,-1.5), # slightly off-center\n",
    "    light_position=(4.0, -4.0, 3.0),\n",
    "    frame=false,axes = false,aspect_ratio=[1,1,1])\n",
    "ttc.show(viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "    figsize=[60,60], # the image resolution is 100*figsize\n",
    "    camera_position=[3, -4, 2], # a distant camera position combined with \n",
    "    zoom=2, # a large zoom factor will decrease perspective distortion.\n",
    "    updir=(0, 0, 2), # the camera is slightly tilted\n",
    "    viewdir=(-2.2,3.,-1.5), # slightly off-center\n",
    "    light_position=(4.0, -4.0, 3.0),\n",
    "    frame=false,axes = false,aspect_ratio=[1,1,1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This cell export the symbolics expresions for computing the mainfolds in codes like asympote or C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#saving symbolic expressions to Asymptote file output \n",
    "tx, ty =var('t_x' ,'t_y')\n",
    "pmx=f_xpm.subs(theta1==tx, theta2==ty)\n",
    "pmy=f_ypm.subs(theta1==tx, theta2==ty)\n",
    "pmz=f_zpm.subs(theta1==tx, theta2==ty)\n",
    "ax=f_xA.subs(theta1==tx, theta2==ty)\n",
    "ay=f_yA.subs(theta1==tx, theta2==ty)\n",
    "az=f_zA.subs(theta1==tx, theta2==ty)\n",
    "bx=f_xB.subs(theta1==tx, theta2==ty)\n",
    "by=f_yB.subs(theta1==tx, theta2==ty)\n",
    "bz=f_zB.subs(theta1==tx, theta2==ty)\n",
    "cx=f_xC.subs(theta1==tx, theta2==ty)\n",
    "cy=f_yC.subs(theta1==tx, theta2==ty)\n",
    "cz=f_zC.subs(theta1==tx, theta2==ty)\n",
    "#exportg = [[pmx,pmy,pmz],[ax,ay,az],[bx,by,bz],[cx,cy,cz], \n",
    "#           [A0.n(),B0.n(),C0.n(),PM0.n()], \n",
    "#           [r_1.n(),r_2.n(),N_1.n(),N_2.n(),M_1.n(),M_2.n()]]\n",
    "#with  open(\"outasymptote.txt\", \"w\") as f: f.write(str(exportg))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following cell shows interactivelly the angular limits"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#limit values\n",
    "t1min=-15\n",
    "t1max=15\n",
    "t2min=-15\n",
    "t2max=15\n",
    "#changing the angular parameters interactively \n",
    "@interact\n",
    "def _(t1mi=slider(-90,-1,label=r'min $\\theta_1$', default=-15), \n",
    "      t1ma=slider(1,90,label=r'max $\\theta_1$', default=15),\n",
    "      t2mi=slider(-90,-1,label=r'min $\\theta_2$', default=-15), \n",
    "      t2ma=slider(1,90,label=r'max $\\theta_2$', default=15)):\n",
    "    global t1min,t1max,t2min,t2max\n",
    "     #substitute values\n",
    "    t1min=t1mi\n",
    "    t1max=t1ma\n",
    "    t2min=t2mi\n",
    "    t2max=t2ma"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following cell plots the surfaces for different angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#change angle values\n",
    "@interact\n",
    "def _(txi=slider(t1min,t1max, step_size=1, label=r'Value $\\theta_1$', default=0), \n",
    "    tyi=slider(t2min,t2max, step_size=1, label=r'Value $\\theta_2$', default=0)):\n",
    "    pmxs=pmx.subs(tx==txi, ty==tyi)\n",
    "    pmys=pmy.subs(tx==txi, ty==tyi)\n",
    "    pmzs=pmz.subs(tx==txi, ty==tyi)\n",
    "    axs=ax.subs(tx==txi, ty==tyi)\n",
    "    ays=ay.subs(tx==txi, ty==tyi)\n",
    "    azs=az.subs(tx==txi, ty==tyi)\n",
    "    bxs=bx.subs(tx==txi, ty==tyi)\n",
    "    bys=by.subs(tx==txi, ty==tyi)\n",
    "    bzs=bz.subs(tx==txi, ty==tyi)\n",
    "    cxs=cx.subs(tx==txi, ty==tyi)\n",
    "    cys=cy.subs(tx==txi, ty==tyi)\n",
    "    czs=cz.subs(tx==txi, ty==tyi)\n",
    "    #plotting\n",
    "    sc=50\n",
    "    dp=30 #data points\n",
    "    cpm=parametric_plot3d([f_xpm, f_ypm, f_zpm],(theta_1, t1min, t1max), \n",
    "                        (theta_2, t2min, t2max),plot_points=[dp,dp])\n",
    "    cA=parametric_plot3d([f_xA, f_yA, f_zA], (theta_1, t1min, t1max), \n",
    "                       (theta_2, t2min, t2max),plot_points=[dp,dp])\n",
    "    cB=parametric_plot3d([f_xB, f_yB, f_zB], (theta_1, t1min, t1max), \n",
    "                       (theta_2, t2min, t2max),plot_points=[dp,dp])\n",
    "    cC=parametric_plot3d([f_xC, f_yC, f_zC], (theta_1, t1min, t1max), \n",
    "                       (theta_2, t2min, t2max),plot_points=[dp,dp],texture=\"red\")\n",
    "\n",
    "    ptpm=vector([pmxs,pmys,pmzs])\n",
    "    spms=vector([f_spm[0].subs(theta1==txi, theta2==tyi),\n",
    "                 f_spm[1].subs(theta1==txi, theta2==tyi),\n",
    "                 f_spm[2].subs(theta1==txi, theta2==tyi)]).n()\n",
    "    apms=arrow3d(ptpm, ptpm + sc*spms,sc, color ='red')\n",
    "    spmn=vector([f_npm[0].subs(theta1==txi, theta2==tyi),\n",
    "                 f_npm[1].subs(theta1==txi, theta2==tyi),\n",
    "                 f_npm[2].subs(theta1==txi, theta2==tyi)]).n()\n",
    "    apmn=arrow3d(ptpm, ptpm + sc*spmn,sc, color ='green')  \n",
    "    spma=vector([f_apm[0].subs(theta1==txi, theta2==tyi), \n",
    "                 f_apm[1].subs(theta1==txi, theta2==tyi),\n",
    "                 f_apm[2].subs(theta1==txi, theta2==tyi)]).n()\n",
    "    apma=arrow3d(ptpm, ptpm + sc*spma,sc, color ='blue')   \n",
    "    pta=vector([axs,ays,azs]).n()\n",
    "    ptb=vector([bxs,bys,bzs]).n()\n",
    "    ptc=vector([cxs,cys,czs]).n()\n",
    "    ptf=polygon([pta,ptb,ptc],color='green',opacity=0.7) \n",
    "        #plotting\n",
    "    fkin=apma+apmn+apms+ptf+cA+cB+cC+cpm\n",
    "    fkin.show(viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "        figsize=[90,50], # the image resolution is 100*figsize\n",
    "        camera_position=[3, 4, 2], # a distant camera position combined with \n",
    "        zoom=4.5, # a large zoom factor will decrease perspective distortion.\n",
    "        updir=(0, 0, 1), # the camera is slightly tilted\n",
    "        viewdir=(-2.4,-3.,-1.6), # slightly off-center\n",
    "        light_position=(4.0, -4.0, 3.0),\n",
    "        frame=false,axes = false,aspect_ratio=1\n",
    "        )\n",
    "    fkin.save('interactive1.png', viewer='tachyon', antialiasing=True, raydepth=3,\n",
    "        figsize=[90,50], # the image resolution is 100*figsize\n",
    "        camera_position=[3, 4, 2], # a distant camera position combined with \n",
    "        zoom=4.5, # a large zoom factor will decrease perspective distortion.\n",
    "        updir=(0, 0, 1), # the camera is slightly tilted\n",
    "        viewdir=(-2.4,-3.,-1.6), # slightly off-center\n",
    "        light_position=(4.0, -4.0, 3.0),\n",
    "        frame=false,axes = false,aspect_ratio=1\n",
    "        )\n",
    "    print(\"pm in w1 comp:\", [ptpm.dot_product(w1).n(),spmn.dot_product(w1).n(),spma.dot_product(w1).n()])\n",
    "    print(\"pm in w2 comp:\", [ptpm.dot_product(w2).n(),spmn.dot_product(w2).n(),spma.dot_product(w2).n()])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following cell presents numerically a trajectory"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Trajectory t same rate\n",
    "t=var('t')\n",
    "\n",
    "tpm=MTHPM.subs(theta1==t,theta2==t, pi==3.14)\n",
    "\n",
    "f = lambda t: (-24.67*cos(t)^2 - 57.38*cos(t)*sin(t) +\n",
    "               11.97*sin(t)^2 + 22.86*cos(t) - 17.35*sin(t) + 1.81,\n",
    "               -9.55*cos(t)^2 + 16.33*cos(t)*sin(t) + \n",
    "               5.22*sin(t)^2 - 9.61*cos(t) + 30.93*sin(t) + 19.17,\n",
    "               -36.33*cos(t)^2 + 33.37*cos(t)*sin(t) + \n",
    "               19.44*sin(t)^2 - 18.81*cos(t) - 34.39*sin(t) - 199.99)\n",
    "               \n",
    "\n",
    "t = Tachyon(xres=900, yres=600, zoom=1.0, antialiasing=10, \n",
    "            aspectratio=1.0, raydepth=8, camera_position=[100,100,-30], \n",
    "            camera_center=None, updir=[0, 0, 10], look_at=[0, 0, 0], \n",
    "            viewdir=[-0.4,-0.4,-0.6], projection='PERSPECTIVE', focallength='', \n",
    "            aperture='', frustum=''\n",
    "           )\n",
    "\n",
    "t.light((10,10,10), 0.8, (1,1,1))\n",
    "\n",
    "t.texture('t',  ambient=0.5, diffuse=0.9, specular=0.5, opacity=1.0, color=(1,0,0))\n",
    "t.texture('t0',  ambient=0.5, diffuse=0.9, specular=0.5, opacity=1.0, color=(0,1,0))\n",
    "t.parametric_plot(f, 0, 2*pi, 't', r=1, cylinders=True, min_depth=5, max_depth=8, e_rel=0.01, e_abs=0.01)\n",
    "\n",
    "t.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "SageMath 9.4",
   "language": "sage",
   "name": "sagemath"
  },
  "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.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}