h36m_test.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'Skeleton'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[1], line 2\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mpandas\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mpd\u001b[39;00m\n\u001b[0;32m----> 2\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mSkeleton\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m Skeleton,DynamicSkeleton\n\u001b[1;32m      3\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mnimblephysics\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mnimble\u001b[39;00m\n\u001b[1;32m      4\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mnumpy\u001b[39;00m \u001b[38;5;28;01mas\u001b[39;00m \u001b[38;5;21;01mnp\u001b[39;00m\n",
      "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'Skeleton'"
     ]
    }
   ],
   "source": [
    "import pandas as pd\n",
    "from Skeleton import Skeleton,DynamicSkeleton\n",
    "import nimblephysics as nimble\n",
    "import numpy as np\n",
    "import json\n",
    "import time\n",
    "\n",
    "s12 = Skeleton('/home/rmhri/markerless-human-perception/src/hpe/src/Biomechanical-Model/BODY12.xml')\n",
    "# Rajagopal\n",
    "# s = DynamicSkeleton('/home/rmhri/markerless-human-perception/src/hpe/src/Biomechanical-Model/BODY15_constrained_3D.xml')\n",
    "# BSM\n",
    "s = DynamicSkeleton(config='/home/rmhri/markerless-human-perception/src/hpe/src/Biomechanical-Model/BODY15_constrained_3D.xml', \\\n",
    "    osim_file='/media/rmhri/mrom/data/skel_models_v1.1/bsm.osim',\\\n",
    "    max_velocity=2)# \\\n",
    "    # geometry_dir='/home/rmhri/markerless-human-perception/src/hpe/src/Biomechanical-Model/data/skel_models_v1.1/Geometry/')\n",
    "\n",
    "kps =  ['RKnee', 'LWrist', 'RHip', 'RShoulder',  'LElbow', 'LHip', 'RElbow', 'RWrist', 'LKnee', 'LShoulder', 'RAnkle', 'LAnkle']\n",
    "gui = nimble.NimbleGUI()\n",
    "gui.serve(8080)\n",
    "# gui.nativeAPI().renderBasis()\n",
    "# sudo lsof -iTCP:8080 -sTCP:LISTEN\n",
    "with open('tmp/camera-parameters.json') as f:\n",
    "    parameters = json.load(f)\n",
    "\n",
    "R = np.array(parameters[\"extrinsics\"]['S9']['60457274']['R'])\n",
    "T = np.array(parameters[\"extrinsics\"]['S9']['60457274']['t'])\n",
    "RT = np.vstack([np.hstack([R,T]),np.array([0,0,0,1])])\n",
    "RT = np.linalg.inv(RT)\n",
    "R = RT[0:3,0:3]\n",
    "T = RT[0:3,3].reshape(3,1)\n",
    "\n",
    "# gui.nativeAPI().renderSkeleton(s._nimble)\n",
    "\n",
    "# [s._nimble.getJoint(i).getDofName(j) for i in range(s._nimble.getNumJoints()) for j in range(s._nimble.getJoint(i).getNumDofs())]\n",
    "# j = s._nimble.getJoint(2)\n",
    "\n",
    "# j.getNumDofs?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# gui.nativeAPI().renderSkeleton(s15.nimble_skeleton)\n",
    "action = \"Directions\"\n",
    "error = \"trtfcn\"\n",
    "notes = \"_confidence\"\n",
    "\n",
    "df = pd.read_csv('/home/rmhri/mrom/data/S9_' + action + '.60457274_' + error + '.csv')\n",
    "# df = pd.read_csv('/home/rmhri/mrom/data/S9_Walking.60457274_gt.csv')\n",
    "# Directions Walking SittingDown\n",
    "t = 0\n",
    "Q = []\n",
    "P = []\n",
    "P_target = []\n",
    "times = []\n",
    "s.prob = None\n",
    "s.reset()\n",
    "to_delete = np.random.randint(low=0,high=12,size=6)\n",
    "for t in range(0,df.shape[0]):\n",
    "    target = np.array([np.array(df.loc[t,[l+\":X\",l+\":Y\",l+\":Z\"]].to_numpy()) for l in kps]).squeeze()/1000\n",
    "    \n",
    "    # Rotate for beautiful view\n",
    "    res = np.dot(R,target.reshape(-1,3).transpose()*1000) + T\n",
    "    Rx = np.array([\n",
    "        [1,0,0],\n",
    "        [0,0,1],\n",
    "        [0,-1,0]\n",
    "    ])\n",
    "    res = np.dot(Rx,res)\n",
    "    target = res.transpose()/1000\n",
    "    \n",
    "    # Remove a keypoint\n",
    "    # if t % 10 == 0:    \n",
    "    #     to_delete = np.random.randint(low=0,high=12,size=6)\n",
    "    # target[to_delete,:] = np.nan\n",
    "    # target[2,:] = np.nan\n",
    "    \n",
    "    s12.load_from_numpy(target.reshape(-1,3),kps)\n",
    "    s.load_from_BODY12(s12)\n",
    "    # s.estimate_confidence()\n",
    "    s.estimate_scale()\n",
    "    \n",
    "    # s.gdIK(1000)\n",
    "    start = time.time()\n",
    "    # s.qpIK(100,0.02,precision=0.0001)\n",
    "    s.filter(dt=0.02)    \n",
    "    stop = time.time()\n",
    "    \n",
    "    times.append(stop-start)\n",
    "    # break\n",
    "    \n",
    "    # Rendering\n",
    "    gui.nativeAPI().renderSkeleton(s._nimble)\n",
    "        \n",
    "    # Plot the measurement\n",
    "    target = target.reshape(-1,1)\n",
    "    for i in range(0,int(target.shape[0]/3)):\n",
    "        gui.nativeAPI().createSphere(key=kps[i], radii=np.array([0.01,0.01,0.01]),pos=target[3*i:3*i+3],color=np.array([0,255,0,0.8]))\n",
    "\n",
    "    pos = s._nimble.getJointWorldPositions(s.joints)\n",
    "    pos = s.correct(pos)\n",
    "\n",
    "    for i in range(0,int(pos.shape[0]/3)):\n",
    "        gui.nativeAPI().createSphere(key=s.joints[i].getName(), radii=np.array([0.01,0.01,0.01]),pos=pos[3*i:3*i+3],color=np.array([0,0,0,1]))\n",
    "    Q.append(s._nimble.getPositions())\n",
    "    P.append(s.to_numpy())\n",
    "    P_target.append(target)\n",
    "    time.sleep(0.005)\n",
    "    gui.nativeAPI().createSphere(key=\"COM\", radii=np.array([0.01,0.01,0.01]),pos=s._nimble.getCOM(),color=np.array([1,0,0,1]))\n",
    "    # gui.nativeAPI().createBox(key=\"ground\",size=np.array([10,0,10]),color=np.array([0.5,0.5,0.5,0.4]))\n",
    "    # break\n",
    "    # print(t,np.round(s._nimble.getBodyScales().reshape(-1,3)[:,0].transpose(),2))\n",
    "    \n",
    "    \n",
    "    \n",
    "Q = np.array(Q)\n",
    "P = np.array(P)\n",
    "P_target = np.array(P_target)\n",
    "print(np.mean(times))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Debug\n",
    "notes = \"_kf05\"\n",
    "print(np.mean(times))\n",
    "np.savetxt(\"tmp/Q_\" + action + \"_\" + error + notes + \".csv\",Q,delimiter=',')\n",
    "np.savetxt(\"tmp/P_\" + action + \"_\" + error + notes + \".csv\",P,delimiter=',')\n",
    "np.savetxt(\"tmp/P_\" + action + \"_\" + error + \"_target.csv\",P_target.squeeze(),delimiter=',')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "# notes = \"_2rads\"\n",
    "\n",
    "Q_gt = np.loadtxt(\"tmp/Q_\"+ action +\"_gt.csv\", delimiter=',')\n",
    "Q_qp2r = np.loadtxt(\"tmp/Q_\"+ action +\"_\"+ error + notes +\".csv\", delimiter=',')\n",
    "# Q_qp10r = np.loadtxt(\"tmp/Q_\"+ action +\"_\"+ error +\"_10rads.csv\", delimiter=',')\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "angles = []\n",
    "for i in range(s._nimble.getNumJoints()):\n",
    "    j = s._nimble.getJoint(i)\n",
    "    for k in range(j.getNumDofs()):\n",
    "        angles.append(j.getName()+'_'+j.getDofName(k))\n",
    "\n",
    "i = 45\n",
    "plt.title(angles[i]+\" (\"+str(i)+\")\" )\n",
    "plt.plot(np.array(Q_gt)[:,i]*180/np.pi,label=\"gt\")\n",
    "# plt.plot(np.array(Q_qp10r)[:,i]*180/np.pi,label=\"g25 and +-10 rad/s\")\n",
    "plt.plot(np.array(Q_qp2r)[:,i]*180/np.pi,label=\"g25 and +-2 rad/s\")\n",
    "plt.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "# notes = \"_confidence\"\n",
    "\n",
    "P_gt_target =    np.loadtxt(\"tmp/P_\"+ action +\"_gt_target.csv\", delimiter=',')\n",
    "P_g25_target = np.loadtxt(\"tmp/P_\"+ action +\"_\"+ error +\"_target.csv\", delimiter=',')\n",
    "P_gt =    np.loadtxt(\"tmp/P_\"+ action +\"_gt.csv\", delimiter=',')\n",
    "P_qp2r =  np.loadtxt(\"tmp/P_\"+ action +\"_\"+ error +notes+\".csv\", delimiter=',')\n",
    "# P_qp10r = np.loadtxt(\"tmp/P_\"+ action +\"_\"+ error +\"_10rads.csv\", delimiter=',')\n",
    "\n",
    "fig, axs = plt.subplots(3,1)\n",
    "\n",
    "kp = \"LElbow\"\n",
    "\n",
    "\n",
    "plt.suptitle(kp+\" cartesian displacement\")\n",
    "axs[0].plot(np.array(P_g25_target)[:,3*kps.index(kp)],label=\"input\",color=\"black\")\n",
    "axs[0].plot(np.array(P_qp2r)[:,3*kps.index(kp)],label=\"qpIK 2rad/s\",color=\"orange\")\n",
    "axs[0].plot(np.array(P_gt_target)[:,3*kps.index(kp)],label=\"target\",color=\"green\")\n",
    "axs[0].legend()\n",
    "axs[0].set_ylabel(\"X\")\n",
    "\n",
    "axs[1].plot(np.array(P_g25_target)[:,3*kps.index(kp)+1],label=\"input\",color=\"black\")\n",
    "axs[1].plot(np.array(P_qp2r)[:,3*kps.index(kp)+1],label=\"qpIK 2rad/s\",color=\"orange\")\n",
    "axs[1].plot(np.array(P_gt_target)[:,3*kps.index(kp)+1],label=\"target\",color=\"green\")\n",
    "axs[1].legend()\n",
    "axs[1].set_ylabel(\"Y\")\n",
    "\n",
    "axs[2].plot(np.array(P_g25_target)[:,3*kps.index(kp)+2],label=\"input\",color=\"black\")\n",
    "axs[2].plot(np.array(P_qp2r)[:,3*kps.index(kp)+2],label=\"qpIK 2rad/s\",color=\"orange\")\n",
    "axs[2].plot(np.array(P_gt_target)[:,3*kps.index(kp)+2],label=\"target\",color=\"green\")\n",
    "axs[2].legend()\n",
    "axs[2].set_ylabel(\"Z\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "from Skeleton import Skeleton,DynamicSkeleton\n",
    "import nimblephysics as nimble\n",
    "import numpy as np\n",
    "import time\n",
    "kps =  ['RKnee', 'LWrist', 'RHip', 'RShoulder',  'LElbow', 'LHip', 'RElbow', 'RWrist', 'LKnee', 'LShoulder', 'RAnkle', 'LAnkle']\n",
    "s12 = Skeleton('/home/rmhri/markerless-human-perception/src/hpe/src/Biomechanical-Model/BODY12.xml')\n",
    "s = DynamicSkeleton(config='/home/rmhri/markerless-human-perception/src/hpe/src/Biomechanical-Model/BODY15_constrained_3D.xml', \\\n",
    "    osim_file='/media/rmhri/mrom/data/skel_models_v1.1/bsm.osim')\n",
    "# Debug qpIK\n",
    "df = pd.read_csv('/home/rmhri/mrom/data/S9_Directions.60457274_gt.csv')\n",
    "t = 0\n",
    "Q = []\n",
    "P = []\n",
    "print(\"Starting qpIK:\\n\")\n",
    "total_time = 0\n",
    "for t in range(df.shape[0]):\n",
    "    target = np.array([np.array(df.loc[t,[l+\":X\",l+\":Y\",l+\":Z\"]].to_numpy()) for l in kps]).squeeze()/1000\n",
    "        \n",
    "    s12.load_from_numpy(target.reshape(-1,3),kps)\n",
    "    s.load_from_BODY12(s12)\n",
    "    s.scale()\n",
    "    begin = time.time()\n",
    "    # s.qpIK(100)\n",
    "    print(t)\n",
    "    if t == 0:\n",
    "        s.qpIK(100)\n",
    "    else:\n",
    "        s.qpIK(100,0.02)\n",
    "    total_time += time.time()-begin\n",
    "    \n",
    "    Q.append(s._nimble.getPositions())\n",
    "    P.append(s.to_numpy())\n",
    "\n",
    "print(total_time/df.shape[0])\n",
    "    \n",
    "Q = np.array(Q)\n",
    "P = np.array(P)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Decomposing nimble forward kinematics\n",
    "pos = s._nimble.getJointWorldPositions(s.joints)\n",
    "\n",
    "# s._nimble.getPositions()\n",
    "\n",
    "J = s._nimble.getJoint(1)\n",
    "# print(pos[0:3])\n",
    "\n",
    "transform = J.getParentBodyNode()\n",
    "# transform.\n",
    "print(transform.rotation(),transform.translation())\n",
    "# trans = transform.translate()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "\n",
    "angles = []\n",
    "for i in range(s._nimble.getNumJoints()):\n",
    "    j = s._nimble.getJoint(i)\n",
    "    for k in range(j.getNumDofs()):\n",
    "        angles.append(j.getName()+'_'+j.getDofName(k))\n",
    "    \n",
    "i = 20\n",
    "    \n",
    "plt.plot(np.array(Q)[:,i],label=angles[i])\n",
    "plt.legend()\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from Skeleton import Skeleton,DynamicSkeleton\n",
    "import numpy as np\n",
    "s = DynamicSkeleton(config='/home/rmhri/markerless-human-perception/src/hpe/src/Biomechanical-Model/BODY15_constrained_3D.xml', \\\n",
    "    osim_file='/home/rmhri/markerless-human-perception/src/hpe/src/Biomechanical-Model/data/skel_models_v1.1/bsm.osim')\n",
    "\n",
    "RCAJ = np.array([0.015,-0.035,-0.02])\n",
    "RHGT = np.array([-0.05,0,0])\n",
    "LCAJ = np.array([0.015,-0.035,0.02])\n",
    "LHGT = np.array([-0.05,0,0])\n",
    "\n",
    "\n",
    "scapula = s._nimble.getBodyNode('scapula_l')\n",
    "transform = scapula.getWorldTransform()\n",
    "gui.nativeAPI().renderSkeleton(s._nimble)\n",
    "gui.nativeAPI().createSphere(key=\"LCAJ\", radii=np.array([0.005,0.005,0.005]),pos=transform.multiply(LCAJ),color=np.array([255,0,0,1]))\n",
    "gui.nativeAPI().createSphere(key=\"LHGT\", radii=np.array([0.005,0.005,0.005]),pos=transform.multiply(LHGT),color=np.array([255,0,0,1]))\n",
    "scapula = s._nimble.getBodyNode('scapula_r')\n",
    "transform = scapula.getWorldTransform()\n",
    "gui.nativeAPI().createSphere(key=\"RCAJ\", radii=np.array([0.005,0.005,0.005]),pos=transform.multiply(RCAJ),color=np.array([255,0,0,1]))\n",
    "gui.nativeAPI().createSphere(key=\"RHGT\", radii=np.array([0.005,0.005,0.005]),pos=transform.multiply(RHGT),color=np.array([255,0,0,1]))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}