joint_arm_estimate.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "## comparing the arm estimate and the value from the imu\n",
    "\n",
    "import numpy as np\n",
    "import collections\n",
    "\n",
    "from pinocchio.visualize import MeshcatVisualizer as Visualizer\n",
    "import time\n",
    "\n",
    "from matplotlib import pyplot as plt\n",
    "from dataclasses import dataclass\n",
    "\n",
    "from arm_model import create_arm\n",
    "\n",
    "from imu_measurements import ArmMeasurement\n",
    "from estimation_problem import solve_estimation_problem\n",
    "from interface import ExoSkeletonUDPInterface\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "interface = ExoSkeletonUDPInterface()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7000/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "rmodel, rdata, gmodel, cmodel = create_arm()\n",
    "\n",
    "viz = Visualizer(rmodel, gmodel, cmodel)\n",
    "viz.initViewer(open=False)\n",
    "viz.loadViewerModel()\n",
    "viz.initializeFrames()\n",
    "viz.display_frames = True\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "calibration ...\n",
      "setting offset ...\n",
      "finished calibration ...\n"
     ]
    }
   ],
   "source": [
    "counter = 0\n",
    "interface.calibrate()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "@dataclass\n",
    "class ArmMeasurementCurrent:\n",
    "    arm_orientation : np.array\n",
    "    palm_orientation : np.array\n",
    "\n",
    "imus = ArmMeasurement(\"/dev/ttyACM0\", \"/dev/ttyACM1\", \"/dev/ttyACM2\")\n",
    "estimate_x0 = np.zeros(rmodel.nq + rmodel.nv)\n",
    "\n",
    "T = 30\n",
    "measurement = collections.deque(maxlen=T)\n",
    "counter = 0\n",
    "\n",
    "print(\"reading measurements\")\n",
    "while True:\n",
    "    imus.update_measurements()  \n",
    "    measurement.append(ArmMeasurementCurrent(imus.arm_orientation, imus.palm_orientation))\n",
    "    # interface.setCommand([0], [0.], [0], [0], [6.0])\n",
    "\n",
    "    if counter > T:\n",
    "        estimate = solve_estimation_problem(measurement, T, rmodel, estimate_x0)\n",
    "        viz.display(estimate.xs[-1][:rmodel.nq])\n",
    "        estimate_x0 = estimate.xs[-1]\n",
    "        interface.setCommand([0], [0.], [0], [0], [0.0])\n",
    "    \n",
    "    # time.sleep(0.01)\n",
    "    counter += 1\n",
    "\n",
    "del imus"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "ename": "KeyboardInterrupt",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[6], line 3\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[38;5;28;01mwhile\u001b[39;00m \u001b[38;5;28;01mTrue\u001b[39;00m:\n\u001b[1;32m      2\u001b[0m     interface\u001b[38;5;241m.\u001b[39msetCommand([\u001b[38;5;241m0\u001b[39m], [\u001b[38;5;241m0.\u001b[39m], [\u001b[38;5;241m0\u001b[39m], [\u001b[38;5;241m0\u001b[39m], [\u001b[38;5;241m5.0\u001b[39m])\n\u001b[0;32m----> 3\u001b[0m     \u001b[43mtime\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msleep\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m0.001\u001b[39;49m\u001b[43m)\u001b[49m\n\u001b[1;32m      4\u001b[0m     state \u001b[38;5;241m=\u001b[39m interface\u001b[38;5;241m.\u001b[39mgetState()\n",
      "\u001b[0;31mKeyboardInterrupt\u001b[0m: "
     ]
    }
   ],
   "source": [
    "while True:\n",
    "    interface.setCommand([0], [0.], [0], [0], [0.0])\n",
    "    time.sleep(0.001)\n",
    "    state = interface.getState()\n",
    "    # print(state[\"motor_q\"], state[\"q\"])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "exoskeleton",
   "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.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}