test-python.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "ac1e55bf",
   "metadata": {},
   "source": [
    "# Imports"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3710d792",
   "metadata": {},
   "source": [
    "## Windows test import"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "350f3f6d",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from ctypes import cdll, c_double, c_int, windll"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f1d6102f",
   "metadata": {},
   "outputs": [],
   "source": [
    "cd build/"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "607e4ef9",
   "metadata": {},
   "outputs": [],
   "source": [
    "cd Release/"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b6cfcc1b",
   "metadata": {},
   "outputs": [],
   "source": [
    "lib = windll.LoadLibrary('Arm_kinematics.dll')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d0eca3a4",
   "metadata": {},
   "source": [
    "## Linux test import"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "ef0748c5",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from ctypes import cdll, c_double, c_int, cdll"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "5d5209e2",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "/home/demo/dev/Reachy2_arm_kinematics/build\n"
     ]
    }
   ],
   "source": [
    "cd build/"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "7f924145",
   "metadata": {},
   "outputs": [],
   "source": [
    "lib = cdll.LoadLibrary(\"libArm_kinematics.so\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b166df6d",
   "metadata": {},
   "source": [
    "# Test"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "0cef46e0",
   "metadata": {},
   "outputs": [],
   "source": [
    "lib.forward.restype = c_int\n",
    "lib.forward.argtypes = [\n",
    "    c_int, np.ctypeslib.ndpointer(dtype=np.double), c_int, \n",
    "    np.ctypeslib.ndpointer(dtype=np.double),\n",
    "]\n",
    "\n",
    "lib.inverse.restype = c_int\n",
    "lib.inverse.argtypes = [\n",
    "    c_int, np.ctypeslib.ndpointer(dtype=np.double), \n",
    "    np.ctypeslib.ndpointer(dtype=np.double),\n",
    "]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "2c9a34d6",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "lib.setup()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "a8191107",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q = np.array((0.05, -0.26, -0.26, -1.57, 0.2, 0.15, -0.1), dtype=np.double)\n",
    "M = np.zeros(16, dtype=np.double)\n",
    "\n",
    "lib.forward(1, q, len(q), M)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "ccc78c51",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.191, -0.194, -0.962,  0.369],\n",
       "       [ 0.028,  0.981, -0.192, -0.204],\n",
       "       [ 0.981,  0.01 ,  0.193, -0.305],\n",
       "       [ 0.   ,  0.   ,  0.   ,  1.   ]])"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.round(M.reshape((4, 4)), 3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "0f575d9b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q2 = np.zeros(7, dtype=np.double)\n",
    "lib.inverse(1, M, q2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "67ec2785",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([  4.45, -22.44,  -7.69, -89.95,   4.82,   7.49, -13.43])"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.round(np.rad2deg(q2), 2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "bdd13c7e",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "1"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "lib.forward(1, q2, len(q2), M)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "31424d3a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.191, -0.194, -0.962,  0.369],\n",
       "       [ 0.028,  0.981, -0.192, -0.204],\n",
       "       [ 0.981,  0.01 ,  0.193, -0.305],\n",
       "       [ 0.   ,  0.   ,  0.   ,  1.   ]])"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.round(M.reshape((4, 4)), 3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "06dd5218",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}